aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ModifiedBulletX
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ModifiedBulletX')
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX.sln84
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/BulletDebug.cs136
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs1246
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs136
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs226
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs182
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs102
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs84
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs108
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs72
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs84
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs66
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs318
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs256
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs92
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs118
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs80
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs560
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs326
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs96
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs716
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs314
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs378
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs386
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs260
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs104
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs294
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs608
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs540
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs208
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs200
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs426
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/UnionFind.cs300
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BUSimplex1to4.cs430
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs632
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs166
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs296
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs366
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs108
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs416
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs368
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs282
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs370
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs272
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs200
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs200
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs160
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs110
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs66
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs116
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs198
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs308
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs64
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs586
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs126
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs266
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs232
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs248
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs230
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs134
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs160
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs66
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs272
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs204
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs320
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs374
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs398
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs146
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs234
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs352
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs1266
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs112
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs202
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs684
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/IConvexPenetrationDepthSolver.cs84
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs94
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ManifoldPoint.cs156
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs492
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs544
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs128
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs284
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs230
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs1286
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs976
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactSolverInfo.cs124
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs880
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs492
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/IConstraintSolver.cs64
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs310
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs312
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs1830
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs376
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverBody.cs156
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverConstraint.cs154
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/TypedConstraint.cs176
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs1578
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DynamicsWorld.cs118
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/RigidBody.cs894
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/SimpleDynamicsWorld.cs422
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/RaycastVehicle.cs186
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/VehicleRaycaster.cs98
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/WheelInfo.cs1058
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Exceptions/BulletException.cs106
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/IDebugDraw.cs118
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/DefaultMotionState.cs132
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs1162
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs232
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MotionState.cs88
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/QuadWord.cs176
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs396
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs204
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector3.cs442
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector4.cs220
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj334
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Properties/AssemblyInfo.cs70
-rw-r--r--libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs1366
-rw-r--r--libraries/ModifiedBulletX/MonoXnaCompactMaths/MonoXnaCompactMaths.csproj100
-rw-r--r--libraries/ModifiedBulletX/MonoXnaCompactMaths/Properties/AssemblyInfo.cs66
-rw-r--r--libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs692
-rw-r--r--libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector3.cs1240
-rw-r--r--libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector4.cs1260
121 files changed, 21376 insertions, 21376 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX.sln b/libraries/ModifiedBulletX/ModifiedBulletX.sln
index 0eccfdd..79ae566 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX.sln
+++ b/libraries/ModifiedBulletX/ModifiedBulletX.sln
@@ -1,42 +1,42 @@
1 1
2Microsoft Visual Studio Solution File, Format Version 9.00 2Microsoft Visual Studio Solution File, Format Version 9.00
3# Visual C# Express 2005 3# Visual C# Express 2005
4Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "MonoXnaCompactMaths", "MonoXnaCompactMaths\MonoXnaCompactMaths.csproj", "{121147BC-B06B-406C-84E9-907F268CF0EB}" 4Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "MonoXnaCompactMaths", "MonoXnaCompactMaths\MonoXnaCompactMaths.csproj", "{121147BC-B06B-406C-84E9-907F268CF0EB}"
5EndProject 5EndProject
6Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Modified.XnaDevRu.BulletX", "ModifiedBulletX\Modified.XnaDevRu.BulletX.csproj", "{44270344-ACA7-4875-B585-81D5C06D0489}" 6Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Modified.XnaDevRu.BulletX", "ModifiedBulletX\Modified.XnaDevRu.BulletX.csproj", "{44270344-ACA7-4875-B585-81D5C06D0489}"
7EndProject 7EndProject
8Global 8Global
9 GlobalSection(SolutionConfigurationPlatforms) = preSolution 9 GlobalSection(SolutionConfigurationPlatforms) = preSolution
10 Debug|Any CPU = Debug|Any CPU 10 Debug|Any CPU = Debug|Any CPU
11 Debug|Mixed Platforms = Debug|Mixed Platforms 11 Debug|Mixed Platforms = Debug|Mixed Platforms
12 Debug|x86 = Debug|x86 12 Debug|x86 = Debug|x86
13 Release|Any CPU = Release|Any CPU 13 Release|Any CPU = Release|Any CPU
14 Release|Mixed Platforms = Release|Mixed Platforms 14 Release|Mixed Platforms = Release|Mixed Platforms
15 Release|x86 = Release|x86 15 Release|x86 = Release|x86
16 EndGlobalSection 16 EndGlobalSection
17 GlobalSection(ProjectConfigurationPlatforms) = postSolution 17 GlobalSection(ProjectConfigurationPlatforms) = postSolution
18 {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|Any CPU.ActiveCfg = Debug|Any CPU 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 37 {44270344-ACA7-4875-B585-81D5C06D0489}.Release|x86.ActiveCfg = Release|Any CPU
38 EndGlobalSection 38 EndGlobalSection
39 GlobalSection(SolutionProperties) = preSolution 39 GlobalSection(SolutionProperties) = preSolution
40 HideSolutionNode = FALSE 40 HideSolutionNode = FALSE
41 EndGlobalSection 41 EndGlobalSection
42EndGlobal 42EndGlobal
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/BulletDebug.cs b/libraries/ModifiedBulletX/ModifiedBulletX/BulletDebug.cs
index d0d5845..a4cbfdd 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/BulletDebug.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/BulletDebug.cs
@@ -1,68 +1,68 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using System.Diagnostics; 25using System.Diagnostics;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 internal static class BulletDebug 29 internal static class BulletDebug
30 { 30 {
31 [Conditional("DEBUG")] 31 [Conditional("DEBUG")]
32 public static void Assert(Boolean condition) 32 public static void Assert(Boolean condition)
33 { 33 {
34 //if (!condition) 34 //if (!condition)
35 //{ 35 //{
36 // Throw("No info available"); 36 // Throw("No info available");
37 //} 37 //}
38 Debug.Assert(condition); 38 Debug.Assert(condition);
39 } 39 }
40 40
41 [Conditional("DEBUG")] 41 [Conditional("DEBUG")]
42 public static void Assert(Boolean condition, String message) 42 public static void Assert(Boolean condition, String message)
43 { 43 {
44 //if (!condition) 44 //if (!condition)
45 //{ 45 //{
46 // Throw(message); 46 // Throw(message);
47 //} 47 //}
48 Debug.Assert(condition, message); 48 Debug.Assert(condition, message);
49 } 49 }
50 50
51 [Conditional("DEBUG")] 51 [Conditional("DEBUG")]
52 public static void Assert(Boolean condition, String message, String detailMessage) 52 public static void Assert(Boolean condition, String message, String detailMessage)
53 { 53 {
54 //if (!condition) 54 //if (!condition)
55 //{ 55 //{
56 // Throw(message); 56 // Throw(message);
57 //} 57 //}
58 Debug.Assert(condition, message, detailMessage); 58 Debug.Assert(condition, message, detailMessage);
59 } 59 }
60 60
61 private static void Throw(String message) 61 private static void Throw(String message)
62 { 62 {
63 String msg = String.Format("Assertion Error: {0}", message); 63 String msg = String.Format("Assertion Error: {0}", message);
64 64
65 throw new BulletException(msg); 65 throw new BulletException(msg);
66 } 66 }
67 } 67 }
68} 68}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs
index 12692ea..168d947 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs
@@ -1,623 +1,623 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public class AxisSweep3: OverlappingPairCache 29 public class AxisSweep3: OverlappingPairCache
30 { 30 {
31 Vector3 _worldAabbMin; 31 Vector3 _worldAabbMin;
32 Vector3 _worldAabbMax; 32 Vector3 _worldAabbMax;
33 33
34 Vector3 _quantize; 34 Vector3 _quantize;
35 35
36 int _numHandles; 36 int _numHandles;
37 int _maxHandles; 37 int _maxHandles;
38 38
39 Handle[] _handles; 39 Handle[] _handles;
40 Edge[][] _edges = new Edge[3][]; 40 Edge[][] _edges = new Edge[3][];
41 41
42 ushort _firstFreeHandle; 42 ushort _firstFreeHandle;
43 43
44 int _invalidPair; 44 int _invalidPair;
45 45
46 public AxisSweep3(Vector3 worldAabbMin, Vector3 worldAabbMax, int maxHandles) 46 public AxisSweep3(Vector3 worldAabbMin, Vector3 worldAabbMax, int maxHandles)
47 : base() 47 : base()
48 { 48 {
49 BulletDebug.Assert(maxHandles > 1 && maxHandles < 32767); 49 BulletDebug.Assert(maxHandles > 1 && maxHandles < 32767);
50 50
51 // init bounds 51 // init bounds
52 _worldAabbMin = worldAabbMin; 52 _worldAabbMin = worldAabbMin;
53 _worldAabbMax = worldAabbMax; 53 _worldAabbMax = worldAabbMax;
54 54
55 Vector3 aabbSize = _worldAabbMax - _worldAabbMin; 55 Vector3 aabbSize = _worldAabbMax - _worldAabbMin;
56 _quantize = new Vector3(65535.0f, 65535.0f, 65535.0f) / aabbSize; 56 _quantize = new Vector3(65535.0f, 65535.0f, 65535.0f) / aabbSize;
57 57
58 // allocate handles buffer and put all handles on free list 58 // allocate handles buffer and put all handles on free list
59 _handles = new Handle[maxHandles]; 59 _handles = new Handle[maxHandles];
60 for (int i = 0; i < maxHandles; i++) 60 for (int i = 0; i < maxHandles; i++)
61 _handles[i] = new Handle(); 61 _handles[i] = new Handle();
62 _maxHandles = maxHandles; 62 _maxHandles = maxHandles;
63 _numHandles = 0; 63 _numHandles = 0;
64 64
65 // handle 0 is reserved as the null index, and is also used as the sentinel 65 // handle 0 is reserved as the null index, and is also used as the sentinel
66 _firstFreeHandle = 1; 66 _firstFreeHandle = 1;
67 { 67 {
68 for (int i = _firstFreeHandle; i < maxHandles; i++) 68 for (int i = _firstFreeHandle; i < maxHandles; i++)
69 { 69 {
70 _handles[i].NextFree = (ushort)(i + 1); 70 _handles[i].NextFree = (ushort)(i + 1);
71 } 71 }
72 _handles[maxHandles - 1].NextFree = 0; 72 _handles[maxHandles - 1].NextFree = 0;
73 } 73 }
74 74
75 { 75 {
76 // allocate edge buffers 76 // allocate edge buffers
77 for (int i = 0; i < 3; i++) 77 for (int i = 0; i < 3; i++)
78 { 78 {
79 _edges[i] = new Edge[maxHandles * 2]; 79 _edges[i] = new Edge[maxHandles * 2];
80 for (int j = 0; j < maxHandles * 2; j++) 80 for (int j = 0; j < maxHandles * 2; j++)
81 { 81 {
82 _edges[i][j] = new Edge(); 82 _edges[i][j] = new Edge();
83 } 83 }
84 } 84 }
85 } 85 }
86 //removed overlap management 86 //removed overlap management
87 87
88 // make boundary sentinels 88 // make boundary sentinels
89 89
90 _handles[0].ClientData = 0; 90 _handles[0].ClientData = 0;
91 91
92 for (int axis = 0; axis < 3; axis++) 92 for (int axis = 0; axis < 3; axis++)
93 { 93 {
94 _handles[0].MinEdges[axis] = 0; 94 _handles[0].MinEdges[axis] = 0;
95 _handles[0].MaxEdges[axis] = 1; 95 _handles[0].MaxEdges[axis] = 1;
96 96
97 _edges[axis][0].Position = 0; 97 _edges[axis][0].Position = 0;
98 _edges[axis][0].Handle = 0; 98 _edges[axis][0].Handle = 0;
99 _edges[axis][1].Position = 0xffff; 99 _edges[axis][1].Position = 0xffff;
100 _edges[axis][1].Handle = 0; 100 _edges[axis][1].Handle = 0;
101 } 101 }
102 } 102 }
103 103
104 public ushort AddHandle(Vector3 aabbMin, Vector3 aabbMax, object owner, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask) 104 public ushort AddHandle(Vector3 aabbMin, Vector3 aabbMax, object owner, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask)
105 { 105 {
106 ushort[] min = new ushort[3], max = new ushort[3]; 106 ushort[] min = new ushort[3], max = new ushort[3];
107 Quantize(out min, aabbMin, 0); 107 Quantize(out min, aabbMin, 0);
108 Quantize(out max, aabbMax, 1); 108 Quantize(out max, aabbMax, 1);
109 109
110 ushort handle = AllocateHandle(); 110 ushort handle = AllocateHandle();
111 Handle oHandle = GetHandle(handle); 111 Handle oHandle = GetHandle(handle);
112 112
113 oHandle.HandleID = handle; 113 oHandle.HandleID = handle;
114 oHandle.ClientData = owner; 114 oHandle.ClientData = owner;
115 oHandle.CollisionFilterGroup = collisionFilterGroup; 115 oHandle.CollisionFilterGroup = collisionFilterGroup;
116 oHandle.CollisionFilterMask = collisionFilterMask; 116 oHandle.CollisionFilterMask = collisionFilterMask;
117 117
118 int limit = _numHandles * 2; 118 int limit = _numHandles * 2;
119 119
120 // (Gluk ) 120 // (Gluk )
121 // ( Inside ) 121 // ( Inside )
122 for (int axis = 0; axis < 3; axis++) 122 for (int axis = 0; axis < 3; axis++)
123 { 123 {
124 _handles[0].MaxEdges[axis] += 2; 124 _handles[0].MaxEdges[axis] += 2;
125 125
126 _edges[axis][limit + 1].Position = _edges[axis][limit - 1].Position; 126 _edges[axis][limit + 1].Position = _edges[axis][limit - 1].Position;
127 _edges[axis][limit + 1].Handle = _edges[axis][limit - 1].Handle; 127 _edges[axis][limit + 1].Handle = _edges[axis][limit - 1].Handle;
128 128
129 _edges[axis][limit - 1].Position = min[axis]; 129 _edges[axis][limit - 1].Position = min[axis];
130 _edges[axis][limit - 1].Handle = handle; 130 _edges[axis][limit - 1].Handle = handle;
131 131
132 _edges[axis][limit].Position = max[axis]; 132 _edges[axis][limit].Position = max[axis];
133 _edges[axis][limit].Handle = handle; 133 _edges[axis][limit].Handle = handle;
134 134
135 oHandle.MinEdges[axis] = (ushort)(limit - 1); 135 oHandle.MinEdges[axis] = (ushort)(limit - 1);
136 oHandle.MaxEdges[axis] = (ushort)limit; 136 oHandle.MaxEdges[axis] = (ushort)limit;
137 } 137 }
138 138
139 SortMinDown(0, oHandle.MinEdges[0], false); 139 SortMinDown(0, oHandle.MinEdges[0], false);
140 SortMaxDown(0, oHandle.MaxEdges[0], false); 140 SortMaxDown(0, oHandle.MaxEdges[0], false);
141 SortMinDown(1, oHandle.MinEdges[1], false); 141 SortMinDown(1, oHandle.MinEdges[1], false);
142 SortMaxDown(1, oHandle.MaxEdges[1], false); 142 SortMaxDown(1, oHandle.MaxEdges[1], false);
143 SortMinDown(2, oHandle.MinEdges[2], true); 143 SortMinDown(2, oHandle.MinEdges[2], true);
144 SortMaxDown(2, oHandle.MaxEdges[2], true); 144 SortMaxDown(2, oHandle.MaxEdges[2], true);
145 145
146 return handle; 146 return handle;
147 } 147 }
148 148
149 public void RemoveHandle(ushort handle) 149 public void RemoveHandle(ushort handle)
150 { 150 {
151 Handle pHandle = GetHandle(handle); 151 Handle pHandle = GetHandle(handle);
152 152
153 //explicitly remove the pairs containing the proxy 153 //explicitly remove the pairs containing the proxy
154 //we could do it also in the sortMinUp (passing true) 154 //we could do it also in the sortMinUp (passing true)
155 //todo: compare performance 155 //todo: compare performance
156 RemoveOverlappingPairsContainingProxy(pHandle); 156 RemoveOverlappingPairsContainingProxy(pHandle);
157 157
158 158
159 // compute current limit of edge arrays 159 // compute current limit of edge arrays
160 int limit = _numHandles * 2; 160 int limit = _numHandles * 2;
161 int axis; 161 int axis;
162 162
163 for (axis = 0; axis < 3; axis++) 163 for (axis = 0; axis < 3; axis++)
164 { 164 {
165 _handles[0].MaxEdges[axis] -= 2; 165 _handles[0].MaxEdges[axis] -= 2;
166 } 166 }
167 167
168 // remove the edges by sorting them up to the end of the list 168 // remove the edges by sorting them up to the end of the list
169 for (axis = 0; axis < 3; axis++) 169 for (axis = 0; axis < 3; axis++)
170 { 170 {
171 Edge[] pEdges = _edges[axis]; 171 Edge[] pEdges = _edges[axis];
172 ushort max = pHandle.MaxEdges[axis]; 172 ushort max = pHandle.MaxEdges[axis];
173 pEdges[max].Position = 0xffff; 173 pEdges[max].Position = 0xffff;
174 174
175 SortMaxUp(axis, max, false); 175 SortMaxUp(axis, max, false);
176 176
177 ushort i = pHandle.MinEdges[axis]; 177 ushort i = pHandle.MinEdges[axis];
178 pEdges[i].Position = 0xffff; 178 pEdges[i].Position = 0xffff;
179 179
180 SortMinUp(axis, i, false); 180 SortMinUp(axis, i, false);
181 181
182 pEdges[limit - 1].Handle = 0; 182 pEdges[limit - 1].Handle = 0;
183 pEdges[limit - 1].Position = 0xffff; 183 pEdges[limit - 1].Position = 0xffff;
184 } 184 }
185 185
186 // free the handle 186 // free the handle
187 FreeHandle(handle); 187 FreeHandle(handle);
188 } 188 }
189 189
190 public override void ProcessAllOverlappingPairs(IOverlapCallback callback) 190 public override void ProcessAllOverlappingPairs(IOverlapCallback callback)
191 { 191 {
192 OverlappingPairs.Sort(new Comparison<BroadphasePair>(BroadphasePair.ComparisonSort)); 192 OverlappingPairs.Sort(new Comparison<BroadphasePair>(BroadphasePair.ComparisonSort));
193 193
194 if (_invalidPair != 0) 194 if (_invalidPair != 0)
195 OverlappingPairs.RemoveRange(OverlappingPairs.Count - _invalidPair, _invalidPair); 195 OverlappingPairs.RemoveRange(OverlappingPairs.Count - _invalidPair, _invalidPair);
196 _invalidPair = 0; 196 _invalidPair = 0;
197 197
198 BroadphasePair previousPair = new BroadphasePair(); 198 BroadphasePair previousPair = new BroadphasePair();
199 previousPair.ProxyA = null; 199 previousPair.ProxyA = null;
200 previousPair.ProxyB = null; 200 previousPair.ProxyB = null;
201 previousPair.CollisionAlgorithm = null; 201 previousPair.CollisionAlgorithm = null;
202 202
203 List<BroadphasePair> removal = new List<BroadphasePair>(); 203 List<BroadphasePair> removal = new List<BroadphasePair>();
204 204
205 for (int i = 0; i < OverlappingPairs.Count; i++) 205 for (int i = 0; i < OverlappingPairs.Count; i++)
206 { 206 {
207 bool isDuplicate = (OverlappingPairs[i] == previousPair); 207 bool isDuplicate = (OverlappingPairs[i] == previousPair);
208 previousPair = OverlappingPairs[i]; 208 previousPair = OverlappingPairs[i];
209 bool needsRemoval; 209 bool needsRemoval;
210 if (!isDuplicate) 210 if (!isDuplicate)
211 { 211 {
212 bool hasOverlap = TestOverlap(previousPair.ProxyA, previousPair.ProxyB); 212 bool hasOverlap = TestOverlap(previousPair.ProxyA, previousPair.ProxyB);
213 if (hasOverlap) 213 if (hasOverlap)
214 { 214 {
215 needsRemoval = callback.ProcessOverlap(ref previousPair); 215 needsRemoval = callback.ProcessOverlap(ref previousPair);
216 } 216 }
217 else 217 else
218 { 218 {
219 needsRemoval = true; 219 needsRemoval = true;
220 } 220 }
221 } 221 }
222 else 222 else
223 { 223 {
224 needsRemoval = true; 224 needsRemoval = true;
225 BulletDebug.Assert(previousPair.CollisionAlgorithm == null); 225 BulletDebug.Assert(previousPair.CollisionAlgorithm == null);
226 } 226 }
227 227
228 if (needsRemoval) 228 if (needsRemoval)
229 { 229 {
230 removal.Add(previousPair); 230 removal.Add(previousPair);
231 } 231 }
232 } 232 }
233 233
234 for (int i = 0; i < removal.Count; i++) 234 for (int i = 0; i < removal.Count; i++)
235 { 235 {
236 BroadphasePair pair = removal[i]; 236 BroadphasePair pair = removal[i];
237 CleanOverlappingPair(ref pair); 237 CleanOverlappingPair(ref pair);
238 pair.ProxyA = null; 238 pair.ProxyA = null;
239 pair.ProxyB = null; 239 pair.ProxyB = null;
240 _invalidPair++; 240 _invalidPair++;
241 OverlappingPairCount--; 241 OverlappingPairCount--;
242 } 242 }
243 } 243 }
244 244
245 private bool TestOverlap(BroadphaseProxy proxyA, BroadphaseProxy proxyB) 245 private bool TestOverlap(BroadphaseProxy proxyA, BroadphaseProxy proxyB)
246 { 246 {
247 if (proxyA == null || proxyB == null) 247 if (proxyA == null || proxyB == null)
248 return false; 248 return false;
249 249
250 Handle handleA = proxyA as Handle; 250 Handle handleA = proxyA as Handle;
251 Handle handleB = proxyB as Handle; 251 Handle handleB = proxyB as Handle;
252 252
253 for (int axis = 0; axis < 3; axis++) 253 for (int axis = 0; axis < 3; axis++)
254 { 254 {
255 if (handleA.MaxEdges[axis] < handleB.MinEdges[axis] || 255 if (handleA.MaxEdges[axis] < handleB.MinEdges[axis] ||
256 handleB.MaxEdges[axis] < handleA.MinEdges[axis]) 256 handleB.MaxEdges[axis] < handleA.MinEdges[axis])
257 { 257 {
258 return false; 258 return false;
259 } 259 }
260 } 260 }
261 return true; 261 return true;
262 } 262 }
263 263
264 private bool TestOverlap(int ignoreAxis, Handle pHandleA, Handle pHandleB) 264 private bool TestOverlap(int ignoreAxis, Handle pHandleA, Handle pHandleB)
265 { 265 {
266 for (int axis = 0; axis < 3; axis++) 266 for (int axis = 0; axis < 3; axis++)
267 { 267 {
268 if (axis != ignoreAxis) 268 if (axis != ignoreAxis)
269 { 269 {
270 if (pHandleA.MaxEdges[axis] < pHandleB.MinEdges[axis] || 270 if (pHandleA.MaxEdges[axis] < pHandleB.MinEdges[axis] ||
271 pHandleB.MaxEdges[axis] < pHandleA.MinEdges[axis]) 271 pHandleB.MaxEdges[axis] < pHandleA.MinEdges[axis])
272 { 272 {
273 return false; 273 return false;
274 } 274 }
275 } 275 }
276 } 276 }
277 277
278 return true; 278 return true;
279 } 279 }
280 280
281 private ushort AllocateHandle() 281 private ushort AllocateHandle()
282 { 282 {
283 ushort handle = _firstFreeHandle; 283 ushort handle = _firstFreeHandle;
284 _firstFreeHandle = GetHandle(handle).NextFree; 284 _firstFreeHandle = GetHandle(handle).NextFree;
285 _numHandles++; 285 _numHandles++;
286 286
287 return handle; 287 return handle;
288 } 288 }
289 289
290 private void FreeHandle(ushort handle) 290 private void FreeHandle(ushort handle)
291 { 291 {
292 BulletDebug.Assert(handle > 0 && handle < _maxHandles); 292 BulletDebug.Assert(handle > 0 && handle < _maxHandles);
293 293
294 GetHandle(handle).NextFree = _firstFreeHandle; 294 GetHandle(handle).NextFree = _firstFreeHandle;
295 _firstFreeHandle = handle; 295 _firstFreeHandle = handle;
296 296
297 _numHandles--; 297 _numHandles--;
298 } 298 }
299 299
300 private Handle GetHandle(ushort handle) 300 private Handle GetHandle(ushort handle)
301 { 301 {
302 return _handles[handle]; 302 return _handles[handle];
303 } 303 }
304 304
305 private void UpdateHandle(ushort handle, Vector3 aabbMin, Vector3 aabbMax) 305 private void UpdateHandle(ushort handle, Vector3 aabbMin, Vector3 aabbMax)
306 { 306 {
307 Handle pHandle = GetHandle(handle); 307 Handle pHandle = GetHandle(handle);
308 308
309 // quantize the new bounds 309 // quantize the new bounds
310 ushort[] min = new ushort[3]; 310 ushort[] min = new ushort[3];
311 ushort[] max = new ushort[3]; 311 ushort[] max = new ushort[3];
312 Quantize(out min, aabbMin, 0); 312 Quantize(out min, aabbMin, 0);
313 Quantize(out max, aabbMax, 1); 313 Quantize(out max, aabbMax, 1);
314 314
315 // update changed edges 315 // update changed edges
316 for (int axis = 0; axis < 3; axis++) 316 for (int axis = 0; axis < 3; axis++)
317 { 317 {
318 ushort emin = pHandle.MinEdges[axis]; 318 ushort emin = pHandle.MinEdges[axis];
319 ushort emax = pHandle.MaxEdges[axis]; 319 ushort emax = pHandle.MaxEdges[axis];
320 320
321 int dmin = (int)min[axis] - (int)_edges[axis][emin].Position; 321 int dmin = (int)min[axis] - (int)_edges[axis][emin].Position;
322 int dmax = (int)max[axis] - (int)_edges[axis][emax].Position; 322 int dmax = (int)max[axis] - (int)_edges[axis][emax].Position;
323 323
324 _edges[axis][emin].Position = min[axis]; 324 _edges[axis][emin].Position = min[axis];
325 _edges[axis][emax].Position = max[axis]; 325 _edges[axis][emax].Position = max[axis];
326 326
327 // expand (only adds overlaps) 327 // expand (only adds overlaps)
328 if (dmin < 0) 328 if (dmin < 0)
329 SortMinDown(axis, emin, true); 329 SortMinDown(axis, emin, true);
330 330
331 if (dmax > 0) 331 if (dmax > 0)
332 SortMaxUp(axis, emax, true); 332 SortMaxUp(axis, emax, true);
333 333
334 // shrink (only removes overlaps) 334 // shrink (only removes overlaps)
335 if (dmin > 0) 335 if (dmin > 0)
336 SortMinUp(axis, emin, true); 336 SortMinUp(axis, emin, true);
337 337
338 if (dmax < 0) 338 if (dmax < 0)
339 SortMaxDown(axis, emax, true); 339 SortMaxDown(axis, emax, true);
340 } 340 }
341 } 341 }
342 342
343 private void Quantize(out ushort[] result, Vector3 point, int isMax) 343 private void Quantize(out ushort[] result, Vector3 point, int isMax)
344 { 344 {
345 Vector3 clampedPoint = new Vector3( 345 Vector3 clampedPoint = new Vector3(
346 point.X, 346 point.X,
347 point.Y, 347 point.Y,
348 point.Z 348 point.Z
349 ); 349 );
350 350
351 MathHelper.SetMax(ref clampedPoint, _worldAabbMin); 351 MathHelper.SetMax(ref clampedPoint, _worldAabbMin);
352 MathHelper.SetMin(ref clampedPoint, _worldAabbMax); 352 MathHelper.SetMin(ref clampedPoint, _worldAabbMax);
353 353
354 Vector3 v = (clampedPoint - _worldAabbMin) * _quantize; 354 Vector3 v = (clampedPoint - _worldAabbMin) * _quantize;
355 355
356 result = new ushort[3]; 356 result = new ushort[3];
357 result[0] = (ushort)(((int)v.X & 0xfffe) | isMax); 357 result[0] = (ushort)(((int)v.X & 0xfffe) | isMax);
358 result[1] = (ushort)(((int)v.Y & 0xfffe) | isMax); 358 result[1] = (ushort)(((int)v.Y & 0xfffe) | isMax);
359 result[2] = (ushort)(((int)v.Z & 0xfffe) | isMax); 359 result[2] = (ushort)(((int)v.Z & 0xfffe) | isMax);
360 } 360 }
361 361
362 private void SortMinDown(int axis, ushort edge, bool updateOverlaps) 362 private void SortMinDown(int axis, ushort edge, bool updateOverlaps)
363 { 363 {
364 Edge pEdge = _edges[axis][edge]; 364 Edge pEdge = _edges[axis][edge];
365 Edge pPrev = _edges[axis][edge - 1]; 365 Edge pPrev = _edges[axis][edge - 1];
366 Handle pHandleEdge = GetHandle(pEdge.Handle); 366 Handle pHandleEdge = GetHandle(pEdge.Handle);
367 367
368 while (pEdge.Position < pPrev.Position) 368 while (pEdge.Position < pPrev.Position)
369 { 369 {
370 Handle pHandlePrev = GetHandle(pPrev.Handle); 370 Handle pHandlePrev = GetHandle(pPrev.Handle);
371 371
372 if (pPrev.IsMax()) 372 if (pPrev.IsMax())
373 { 373 {
374 // if previous edge is a maximum check the bounds and add an overlap if necessary 374 // if previous edge is a maximum check the bounds and add an overlap if necessary
375 if (updateOverlaps && TestOverlap(axis, pHandleEdge, pHandlePrev)) 375 if (updateOverlaps && TestOverlap(axis, pHandleEdge, pHandlePrev))
376 { 376 {
377 AddOverlappingPair(pHandleEdge, pHandlePrev); 377 AddOverlappingPair(pHandleEdge, pHandlePrev);
378 } 378 }
379 379
380 // update edge reference in other handle 380 // update edge reference in other handle
381 pHandlePrev.MaxEdges[axis]++; 381 pHandlePrev.MaxEdges[axis]++;
382 } 382 }
383 else 383 else
384 pHandlePrev.MinEdges[axis]++; 384 pHandlePrev.MinEdges[axis]++;
385 385
386 pHandleEdge.MinEdges[axis]--; 386 pHandleEdge.MinEdges[axis]--;
387 387
388 // swap the edges 388 // swap the edges
389 pEdge.Swap(ref pPrev); 389 pEdge.Swap(ref pPrev);
390 390
391 // decrement 391 // decrement
392 edge--; 392 edge--;
393 pEdge = _edges[axis][edge]; 393 pEdge = _edges[axis][edge];
394 pPrev = _edges[axis][edge - 1]; 394 pPrev = _edges[axis][edge - 1];
395 } 395 }
396 } 396 }
397 397
398 private void SortMinUp(int axis, ushort edge, bool updateOverlaps) 398 private void SortMinUp(int axis, ushort edge, bool updateOverlaps)
399 { 399 {
400 Edge pEdge = _edges[axis][edge]; 400 Edge pEdge = _edges[axis][edge];
401 Edge pNext = _edges[axis][edge + 1]; 401 Edge pNext = _edges[axis][edge + 1];
402 Handle pHandleEdge = GetHandle(pEdge.Handle); 402 Handle pHandleEdge = GetHandle(pEdge.Handle);
403 403
404 while ((pNext.Handle != 0) && (pEdge.Position >= pNext.Position)) 404 while ((pNext.Handle != 0) && (pEdge.Position >= pNext.Position))
405 { 405 {
406 Handle pHandleNext = GetHandle(pNext.Handle); 406 Handle pHandleNext = GetHandle(pNext.Handle);
407 407
408 if (pNext.IsMax()) 408 if (pNext.IsMax())
409 { 409 {
410 // if next edge is maximum remove any overlap between the two handles 410 // if next edge is maximum remove any overlap between the two handles
411 if (updateOverlaps) 411 if (updateOverlaps)
412 { 412 {
413 //Handle handle0 = GetHandle(pEdge.Handle); 413 //Handle handle0 = GetHandle(pEdge.Handle);
414 //Handle handle1 = GetHandle(pNext.Handle); 414 //Handle handle1 = GetHandle(pNext.Handle);
415 //BroadphasePair tmpPair = new BroadphasePair(handle0, handle1); 415 //BroadphasePair tmpPair = new BroadphasePair(handle0, handle1);
416 //RemoveOverlappingPair(tmpPair); 416 //RemoveOverlappingPair(tmpPair);
417 } 417 }
418 418
419 // update edge reference in other handle 419 // update edge reference in other handle
420 pHandleNext.MaxEdges[axis]--; 420 pHandleNext.MaxEdges[axis]--;
421 } 421 }
422 else 422 else
423 pHandleNext.MinEdges[axis]--; 423 pHandleNext.MinEdges[axis]--;
424 424
425 pHandleEdge.MinEdges[axis]++; 425 pHandleEdge.MinEdges[axis]++;
426 426
427 // swap the edges 427 // swap the edges
428 pEdge.Swap(ref pNext); 428 pEdge.Swap(ref pNext);
429 429
430 // increment 430 // increment
431 edge++; 431 edge++;
432 pEdge = _edges[axis][edge]; 432 pEdge = _edges[axis][edge];
433 pNext = _edges[axis][edge + 1]; 433 pNext = _edges[axis][edge + 1];
434 } 434 }
435 } 435 }
436 436
437 private void SortMaxDown(int axis, ushort edge, bool updateOverlaps) 437 private void SortMaxDown(int axis, ushort edge, bool updateOverlaps)
438 { 438 {
439 Edge pEdge = _edges[axis][edge]; 439 Edge pEdge = _edges[axis][edge];
440 Edge pPrev = _edges[axis][edge - 1]; 440 Edge pPrev = _edges[axis][edge - 1];
441 Handle pHandleEdge = GetHandle(pEdge.Handle); 441 Handle pHandleEdge = GetHandle(pEdge.Handle);
442 442
443 while (pEdge.Position < pPrev.Position) 443 while (pEdge.Position < pPrev.Position)
444 { 444 {
445 Handle pHandlePrev = GetHandle(pPrev.Handle); 445 Handle pHandlePrev = GetHandle(pPrev.Handle);
446 446
447 if (!pPrev.IsMax()) 447 if (!pPrev.IsMax())
448 { 448 {
449 // if previous edge was a minimum remove any overlap between the two handles 449 // if previous edge was a minimum remove any overlap between the two handles
450 if (updateOverlaps) 450 if (updateOverlaps)
451 { 451 {
452 //this is done during the overlappingpairarray iteration/narrowphase collision 452 //this is done during the overlappingpairarray iteration/narrowphase collision
453 //Handle handle0 = GetHandle(pEdge.Handle); 453 //Handle handle0 = GetHandle(pEdge.Handle);
454 //Handle handle1 = GetHandle(pPrev.Handle); 454 //Handle handle1 = GetHandle(pPrev.Handle);
455 //BroadphasePair pair = FindPair(handle0, handle1); 455 //BroadphasePair pair = FindPair(handle0, handle1);
456 456
457 //if (pair != null) 457 //if (pair != null)
458 //{ 458 //{
459 // RemoveOverlappingPair(pair); 459 // RemoveOverlappingPair(pair);
460 //} 460 //}
461 } 461 }
462 462
463 // update edge reference in other handle 463 // update edge reference in other handle
464 pHandlePrev.MinEdges[axis]++; ; 464 pHandlePrev.MinEdges[axis]++; ;
465 } 465 }
466 else 466 else
467 pHandlePrev.MaxEdges[axis]++; 467 pHandlePrev.MaxEdges[axis]++;
468 468
469 pHandleEdge.MaxEdges[axis]--; 469 pHandleEdge.MaxEdges[axis]--;
470 470
471 // swap the edges 471 // swap the edges
472 pEdge.Swap(ref pPrev); 472 pEdge.Swap(ref pPrev);
473 473
474 // decrement 474 // decrement
475 edge--; 475 edge--;
476 pEdge = _edges[axis][edge]; 476 pEdge = _edges[axis][edge];
477 pPrev = _edges[axis][edge - 1]; 477 pPrev = _edges[axis][edge - 1];
478 } 478 }
479 } 479 }
480 480
481 private void SortMaxUp(int axis, ushort edge, bool updateOverlaps) 481 private void SortMaxUp(int axis, ushort edge, bool updateOverlaps)
482 { 482 {
483 Edge pEdge = _edges[axis][edge]; 483 Edge pEdge = _edges[axis][edge];
484 Edge pNext = _edges[axis][edge + 1]; 484 Edge pNext = _edges[axis][edge + 1];
485 Handle pHandleEdge = GetHandle(pEdge.Handle); 485 Handle pHandleEdge = GetHandle(pEdge.Handle);
486 486
487 while ((pNext.Handle!=0) && (pEdge.Position >= pNext.Position)) 487 while ((pNext.Handle!=0) && (pEdge.Position >= pNext.Position))
488 { 488 {
489 Handle pHandleNext = GetHandle(pNext.Handle); 489 Handle pHandleNext = GetHandle(pNext.Handle);
490 490
491 if (!pNext.IsMax()) 491 if (!pNext.IsMax())
492 { 492 {
493 // if next edge is a minimum check the bounds and add an overlap if necessary 493 // if next edge is a minimum check the bounds and add an overlap if necessary
494 if (updateOverlaps && TestOverlap(axis, pHandleEdge, pHandleNext)) 494 if (updateOverlaps && TestOverlap(axis, pHandleEdge, pHandleNext))
495 { 495 {
496 Handle handle0 = GetHandle(pEdge.Handle); 496 Handle handle0 = GetHandle(pEdge.Handle);
497 Handle handle1 = GetHandle(pNext.Handle); 497 Handle handle1 = GetHandle(pNext.Handle);
498 AddOverlappingPair(handle0, handle1); 498 AddOverlappingPair(handle0, handle1);
499 } 499 }
500 500
501 // update edge reference in other handle 501 // update edge reference in other handle
502 pHandleNext.MinEdges[axis]--; 502 pHandleNext.MinEdges[axis]--;
503 } 503 }
504 else 504 else
505 pHandleNext.MaxEdges[axis]--; 505 pHandleNext.MaxEdges[axis]--;
506 506
507 pHandleEdge.MaxEdges[axis]++; 507 pHandleEdge.MaxEdges[axis]++;
508 508
509 // swap the edges 509 // swap the edges
510 pEdge.Swap(ref pNext); 510 pEdge.Swap(ref pNext);
511 511
512 // increment 512 // increment
513 edge++; 513 edge++;
514 pEdge = _edges[axis][edge]; 514 pEdge = _edges[axis][edge];
515 pNext = _edges[axis][edge + 1]; 515 pNext = _edges[axis][edge + 1];
516 } 516 }
517 } 517 }
518 518
519 #region Abstract 519 #region Abstract
520 520
521 public override void RefreshOverlappingPairs() 521 public override void RefreshOverlappingPairs()
522 { 522 {
523 } 523 }
524 524
525 public override BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask) 525 public override BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask)
526 { 526 {
527 ushort handleId = AddHandle(min, max, userData, collisionFilterGroup, collisionFilterMask); 527 ushort handleId = AddHandle(min, max, userData, collisionFilterGroup, collisionFilterMask);
528 528
529 Handle handle = GetHandle(handleId); 529 Handle handle = GetHandle(handleId);
530 530
531 return handle; 531 return handle;
532 } 532 }
533 533
534 public override void DestroyProxy(BroadphaseProxy proxy) 534 public override void DestroyProxy(BroadphaseProxy proxy)
535 { 535 {
536 Handle handle = proxy as Handle; 536 Handle handle = proxy as Handle;
537 RemoveHandle(handle.HandleID); 537 RemoveHandle(handle.HandleID);
538 } 538 }
539 539
540 public override void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax) 540 public override void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax)
541 { 541 {
542 Handle handle = proxy as Handle; 542 Handle handle = proxy as Handle;
543 UpdateHandle(handle.HandleID, aabbMin, aabbMax); 543 UpdateHandle(handle.HandleID, aabbMin, aabbMax);
544 } 544 }
545 #endregion 545 #endregion
546 } 546 }
547 547
548 public class Edge 548 public class Edge
549 { 549 {
550 ushort position; 550 ushort position;
551 ushort handle; 551 ushort handle;
552 552
553 public ushort Position 553 public ushort Position
554 { 554 {
555 get { return position; } 555 get { return position; }
556 set { position = value; } 556 set { position = value; }
557 } 557 }
558 558
559 public ushort Handle 559 public ushort Handle
560 { 560 {
561 get { return handle; } 561 get { return handle; }
562 set { handle = value; } 562 set { handle = value; }
563 } 563 }
564 564
565 public bool IsMax() 565 public bool IsMax()
566 { 566 {
567 return (position & (ushort)1) == 1; 567 return (position & (ushort)1) == 1;
568 } 568 }
569 569
570 public void Swap(ref Edge e) 570 public void Swap(ref Edge e)
571 { 571 {
572 ushort tmpPosition = this.position; 572 ushort tmpPosition = this.position;
573 ushort tmpHandle = this.handle; 573 ushort tmpHandle = this.handle;
574 this.position = e.position; 574 this.position = e.position;
575 this.handle = e.handle; 575 this.handle = e.handle;
576 e.position = tmpPosition; 576 e.position = tmpPosition;
577 e.handle = tmpHandle; 577 e.handle = tmpHandle;
578 } 578 }
579 } 579 }
580 580
581 public class Handle: BroadphaseProxy 581 public class Handle: BroadphaseProxy
582 { 582 {
583 ushort[] minEdges, maxEdges; 583 ushort[] minEdges, maxEdges;
584 ushort pad; 584 ushort pad;
585 ushort handleID; 585 ushort handleID;
586 586
587 public ushort[] MinEdges 587 public ushort[] MinEdges
588 { 588 {
589 get { return minEdges; } 589 get { return minEdges; }
590 set { minEdges = value; } 590 set { minEdges = value; }
591 } 591 }
592 592
593 public ushort[] MaxEdges 593 public ushort[] MaxEdges
594 { 594 {
595 get { return maxEdges; } 595 get { return maxEdges; }
596 set { maxEdges = value; } 596 set { maxEdges = value; }
597 } 597 }
598 598
599 public ushort HandleID 599 public ushort HandleID
600 { 600 {
601 get { return handleID; } 601 get { return handleID; }
602 set { handleID = value; } 602 set { handleID = value; }
603 } 603 }
604 604
605 public ushort Pad 605 public ushort Pad
606 { 606 {
607 get { return pad; } 607 get { return pad; }
608 set { pad = value; } 608 set { pad = value; }
609 } 609 }
610 610
611 public ushort NextFree 611 public ushort NextFree
612 { 612 {
613 get { return minEdges[0]; } 613 get { return minEdges[0]; }
614 set { minEdges[0] = value;} 614 set { minEdges[0] = value;}
615 } 615 }
616 616
617 public Handle() 617 public Handle()
618 { 618 {
619 minEdges = new ushort[3]; 619 minEdges = new ushort[3];
620 maxEdges = new ushort[3]; 620 maxEdges = new ushort[3];
621 } 621 }
622 } 622 }
623} 623}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs
index dcfd0d8..eb1fd4b 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs
@@ -1,68 +1,68 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25 25
26namespace XnaDevRu.BulletX 26namespace XnaDevRu.BulletX
27{ 27{
28 /// Dispatcher uses these types 28 /// Dispatcher uses these types
29 /// IMPORTANT NOTE:The types are ordered polyhedral, implicit convex and concave 29 /// IMPORTANT NOTE:The types are ordered polyhedral, implicit convex and concave
30 /// to facilitate type checking 30 /// to facilitate type checking
31 public enum BroadphaseNativeTypes 31 public enum BroadphaseNativeTypes
32 { 32 {
33 // polyhedral convex shapes 33 // polyhedral convex shapes
34 Box, 34 Box,
35 Triangle, 35 Triangle,
36 Tetrahedral, 36 Tetrahedral,
37 ConvexTriangleMesh, 37 ConvexTriangleMesh,
38 ConvexHull, 38 ConvexHull,
39 //implicit convex shapes 39 //implicit convex shapes
40 ImplicitConvexShapes, 40 ImplicitConvexShapes,
41 Sphere, 41 Sphere,
42 MultiSphere, 42 MultiSphere,
43 Capsule, 43 Capsule,
44 Cone, 44 Cone,
45 Convex, 45 Convex,
46 Cylinder, 46 Cylinder,
47 MinkowskiSum, 47 MinkowskiSum,
48 MinkowskiDifference, 48 MinkowskiDifference,
49 //concave shapes 49 //concave shapes
50 ConcaveShapesStart, 50 ConcaveShapesStart,
51 //keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy! 51 //keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy!
52 TriangleMesh, 52 TriangleMesh,
53 //used for demo integration FAST/Swift collision library and Bullet 53 //used for demo integration FAST/Swift collision library and Bullet
54 FastConcaveMesh, 54 FastConcaveMesh,
55 //terrain 55 //terrain
56 Terrain, 56 Terrain,
57 //Used for GIMPACT Trimesh integration 57 //Used for GIMPACT Trimesh integration
58 Gimpact, 58 Gimpact,
59 59
60 Empty, 60 Empty,
61 StaticPlane, 61 StaticPlane,
62 ConcaveShapesEnd, 62 ConcaveShapesEnd,
63 63
64 Compound, 64 Compound,
65 65
66 MaxBroadphaseCollisionTypes, 66 MaxBroadphaseCollisionTypes,
67 } 67 }
68} 68}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs
index 2b9e114..eec77ac 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs
@@ -1,113 +1,113 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25 25
26namespace XnaDevRu.BulletX 26namespace XnaDevRu.BulletX
27{ 27{
28 public class BroadphasePair 28 public class BroadphasePair
29 { 29 {
30 private BroadphaseProxy _proxyA; 30 private BroadphaseProxy _proxyA;
31 private BroadphaseProxy _proxyB; 31 private BroadphaseProxy _proxyB;
32 32
33 private CollisionAlgorithm _algorithm; 33 private CollisionAlgorithm _algorithm;
34 private object _userInfo; 34 private object _userInfo;
35 35
36 public BroadphasePair() 36 public BroadphasePair()
37 { 37 {
38 } 38 }
39 39
40 public BroadphasePair(BroadphasePair other) 40 public BroadphasePair(BroadphasePair other)
41 { 41 {
42 _proxyA = other._proxyA; 42 _proxyA = other._proxyA;
43 _proxyB = other._proxyB; 43 _proxyB = other._proxyB;
44 44
45 _algorithm = other._algorithm; 45 _algorithm = other._algorithm;
46 _userInfo = null; 46 _userInfo = null;
47 } 47 }
48 48
49 public BroadphasePair(BroadphaseProxy proxyA, BroadphaseProxy proxyB) 49 public BroadphasePair(BroadphaseProxy proxyA, BroadphaseProxy proxyB)
50 { 50 {
51 _proxyA = proxyA; 51 _proxyA = proxyA;
52 _proxyB = proxyB; 52 _proxyB = proxyB;
53 53
54 _algorithm = null; 54 _algorithm = null;
55 _userInfo = null; 55 _userInfo = null;
56 } 56 }
57 57
58 public BroadphaseProxy ProxyA { get { return _proxyA; } set { _proxyA = value; } } 58 public BroadphaseProxy ProxyA { get { return _proxyA; } set { _proxyA = value; } }
59 public BroadphaseProxy ProxyB { get { return _proxyB; } set { _proxyB = value; } } 59 public BroadphaseProxy ProxyB { get { return _proxyB; } set { _proxyB = value; } }
60 60
61 public CollisionAlgorithm CollisionAlgorithm { get { return _algorithm; } set { _algorithm = value; } } 61 public CollisionAlgorithm CollisionAlgorithm { get { return _algorithm; } set { _algorithm = value; } }
62 public object UserInfo { get { return _userInfo; } set { _userInfo = value; } } 62 public object UserInfo { get { return _userInfo; } set { _userInfo = value; } }
63 63
64 public override int GetHashCode() 64 public override int GetHashCode()
65 { 65 {
66 return _proxyA.GetHashCode() ^ _proxyB.GetHashCode(); 66 return _proxyA.GetHashCode() ^ _proxyB.GetHashCode();
67 } 67 }
68 68
69 public override bool Equals(object obj) 69 public override bool Equals(object obj)
70 { 70 {
71 if (obj is BroadphasePair) 71 if (obj is BroadphasePair)
72 return this == (BroadphasePair)obj; 72 return this == (BroadphasePair)obj;
73 return false; 73 return false;
74 } 74 }
75 75
76 public static int ComparisonSort(BroadphasePair a, BroadphasePair b) 76 public static int ComparisonSort(BroadphasePair a, BroadphasePair b)
77 { 77 {
78 int aAId = a.ProxyA != null ? a.ProxyA.ComparisonID : -1; 78 int aAId = a.ProxyA != null ? a.ProxyA.ComparisonID : -1;
79 int aBId = a.ProxyB != null ? a.ProxyB.ComparisonID : -1; 79 int aBId = a.ProxyB != null ? a.ProxyB.ComparisonID : -1;
80 int aCId = a.CollisionAlgorithm != null ? a.CollisionAlgorithm.ComparisonID : -1; 80 int aCId = a.CollisionAlgorithm != null ? a.CollisionAlgorithm.ComparisonID : -1;
81 int bAId = b.ProxyA != null ? b.ProxyA.ComparisonID : -1; 81 int bAId = b.ProxyA != null ? b.ProxyA.ComparisonID : -1;
82 int bBId = b.ProxyB != null ? b.ProxyB.ComparisonID : -1; 82 int bBId = b.ProxyB != null ? b.ProxyB.ComparisonID : -1;
83 int bCId = b.CollisionAlgorithm != null ? b.CollisionAlgorithm.ComparisonID : -1; 83 int bCId = b.CollisionAlgorithm != null ? b.CollisionAlgorithm.ComparisonID : -1;
84 84
85 if (aAId > bAId || 85 if (aAId > bAId ||
86 (a.ProxyA == b.ProxyA && aBId > bBId) || 86 (a.ProxyA == b.ProxyA && aBId > bBId) ||
87 (a.ProxyA == b.ProxyA && a.ProxyB == b.ProxyB && aCId > bCId)) 87 (a.ProxyA == b.ProxyA && a.ProxyB == b.ProxyB && aCId > bCId))
88 return -1; 88 return -1;
89 else 89 else
90 return 1; 90 return 1;
91 } 91 }
92 92
93 public static bool operator ==(BroadphasePair a, BroadphasePair b) 93 public static bool operator ==(BroadphasePair a, BroadphasePair b)
94 { 94 {
95 if (object.Equals(a, null) && object.Equals(b, null)) 95 if (object.Equals(a, null) && object.Equals(b, null))
96 return true; 96 return true;
97 if (object.Equals(a, null) || object.Equals(b, null)) 97 if (object.Equals(a, null) || object.Equals(b, null))
98 return false; 98 return false;
99 99
100 return (a.ProxyA == b.ProxyA) && (a.ProxyB == b.ProxyB); 100 return (a.ProxyA == b.ProxyA) && (a.ProxyB == b.ProxyB);
101 } 101 }
102 102
103 public static bool operator !=(BroadphasePair a, BroadphasePair b) 103 public static bool operator !=(BroadphasePair a, BroadphasePair b)
104 { 104 {
105 if (object.Equals(a, null) && object.Equals(b, null)) 105 if (object.Equals(a, null) && object.Equals(b, null))
106 return true; 106 return true;
107 if (object.Equals(a, null) || object.Equals(b, null)) 107 if (object.Equals(a, null) || object.Equals(b, null))
108 return false; 108 return false;
109 109
110 return (a.ProxyA != b.ProxyA) || (a.ProxyB != b.ProxyB); 110 return (a.ProxyA != b.ProxyA) || (a.ProxyB != b.ProxyB);
111 } 111 }
112 } 112 }
113} 113}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs
index aed82d5..0abc613 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs
@@ -1,91 +1,91 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25 25
26namespace XnaDevRu.BulletX 26namespace XnaDevRu.BulletX
27{ 27{
28 public class BroadphaseProxy 28 public class BroadphaseProxy
29 { 29 {
30 //Usually the client CollisionObject or Rigidbody class 30 //Usually the client CollisionObject or Rigidbody class
31 private object _clientObject; 31 private object _clientObject;
32 private CollisionFilterGroups _collisionFilterGroup; 32 private CollisionFilterGroups _collisionFilterGroup;
33 private CollisionFilterGroups _collisionFilterMask; 33 private CollisionFilterGroups _collisionFilterMask;
34 private readonly int _comparisonID; 34 private readonly int _comparisonID;
35 35
36 private static int _globalCount = 0; 36 private static int _globalCount = 0;
37 37
38 public BroadphaseProxy() 38 public BroadphaseProxy()
39 { 39 {
40 _comparisonID = _globalCount++; 40 _comparisonID = _globalCount++;
41 } 41 }
42 42
43 public BroadphaseProxy(object userData, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask) 43 public BroadphaseProxy(object userData, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask)
44 : this() 44 : this()
45 { 45 {
46 _clientObject = userData; 46 _clientObject = userData;
47 _collisionFilterGroup = collisionFilterGroup; 47 _collisionFilterGroup = collisionFilterGroup;
48 _collisionFilterMask = collisionFilterMask; 48 _collisionFilterMask = collisionFilterMask;
49 } 49 }
50 50
51 public object ClientData { get { return _clientObject; } set { _clientObject = value; } } 51 public object ClientData { get { return _clientObject; } set { _clientObject = value; } }
52 public CollisionFilterGroups CollisionFilterGroup { get { return _collisionFilterGroup; } set { _collisionFilterGroup = value; } } 52 public CollisionFilterGroups CollisionFilterGroup { get { return _collisionFilterGroup; } set { _collisionFilterGroup = value; } }
53 public CollisionFilterGroups CollisionFilterMask { get { return _collisionFilterMask; } set { _collisionFilterMask = value; } } 53 public CollisionFilterGroups CollisionFilterMask { get { return _collisionFilterMask; } set { _collisionFilterMask = value; } }
54 internal int ComparisonID { get { return _comparisonID; } } 54 internal int ComparisonID { get { return _comparisonID; } }
55 55
56 public static bool IsPolyhedral(BroadphaseNativeTypes proxyType) 56 public static bool IsPolyhedral(BroadphaseNativeTypes proxyType)
57 { 57 {
58 return (proxyType < BroadphaseNativeTypes.ImplicitConvexShapes); 58 return (proxyType < BroadphaseNativeTypes.ImplicitConvexShapes);
59 } 59 }
60 60
61 public static bool IsConvex(BroadphaseNativeTypes proxyType) 61 public static bool IsConvex(BroadphaseNativeTypes proxyType)
62 { 62 {
63 return (proxyType < BroadphaseNativeTypes.ConcaveShapesStart); 63 return (proxyType < BroadphaseNativeTypes.ConcaveShapesStart);
64 } 64 }
65 65
66 public static bool IsConcave(BroadphaseNativeTypes proxyType) 66 public static bool IsConcave(BroadphaseNativeTypes proxyType)
67 { 67 {
68 return ((proxyType > BroadphaseNativeTypes.ConcaveShapesStart) && 68 return ((proxyType > BroadphaseNativeTypes.ConcaveShapesStart) &&
69 (proxyType < BroadphaseNativeTypes.ConcaveShapesEnd)); 69 (proxyType < BroadphaseNativeTypes.ConcaveShapesEnd));
70 } 70 }
71 public static bool IsCompound(BroadphaseNativeTypes proxyType) 71 public static bool IsCompound(BroadphaseNativeTypes proxyType)
72 { 72 {
73 return (proxyType == BroadphaseNativeTypes.Compound); 73 return (proxyType == BroadphaseNativeTypes.Compound);
74 } 74 }
75 public static bool IsInfinite(BroadphaseNativeTypes proxyType) 75 public static bool IsInfinite(BroadphaseNativeTypes proxyType)
76 { 76 {
77 return (proxyType == BroadphaseNativeTypes.StaticPlane); 77 return (proxyType == BroadphaseNativeTypes.StaticPlane);
78 } 78 }
79 79
80 //optional filtering to cull potential collisions 80 //optional filtering to cull potential collisions
81 public enum CollisionFilterGroups 81 public enum CollisionFilterGroups
82 { 82 {
83 Default = 1, 83 Default = 1,
84 Static = 2, 84 Static = 2,
85 Kinematic = 4, 85 Kinematic = 4,
86 Debris = 8, 86 Debris = 8,
87 Sensor = 16, 87 Sensor = 16,
88 All = Default | Static | Kinematic | Debris | Sensor, 88 All = Default | Static | Kinematic | Debris | Sensor,
89 } 89 }
90 } 90 }
91} 91}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs
index 1e59ad8..387340d 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs
@@ -1,51 +1,51 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25 25
26namespace XnaDevRu.BulletX 26namespace XnaDevRu.BulletX
27{ 27{
28 /// <summary> 28 /// <summary>
29 /// CollisionAlgorithm is an collision interface that is compatible with the Broadphase and Dispatcher. 29 /// CollisionAlgorithm is an collision interface that is compatible with the Broadphase and Dispatcher.
30 /// It is persistent over frames 30 /// It is persistent over frames
31 /// </summary> 31 /// </summary>
32 public abstract class CollisionAlgorithm 32 public abstract class CollisionAlgorithm
33 { 33 {
34 private IDispatcher _dispatcher; 34 private IDispatcher _dispatcher;
35 private readonly int _comparisonID = 0; 35 private readonly int _comparisonID = 0;
36 36
37 private static int _globalCount = 0; 37 private static int _globalCount = 0;
38 38
39 public CollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) 39 public CollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo)
40 { 40 {
41 _comparisonID = _globalCount++; 41 _comparisonID = _globalCount++;
42 _dispatcher = collisionAlgorithmConstructionInfo.Dispatcher; 42 _dispatcher = collisionAlgorithmConstructionInfo.Dispatcher;
43 } 43 }
44 44
45 protected IDispatcher Dispatcher { get { return _dispatcher; } set { _dispatcher = value; } } 45 protected IDispatcher Dispatcher { get { return _dispatcher; } set { _dispatcher = value; } }
46 internal int ComparisonID { get { return _comparisonID; } } 46 internal int ComparisonID { get { return _comparisonID; } }
47 47
48 public abstract void ProcessCollision(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut); 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); 49 public abstract float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut);
50 } 50 }
51} 51}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs
index 381752a..de238b9 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs
@@ -1,42 +1,42 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25 25
26namespace XnaDevRu.BulletX 26namespace XnaDevRu.BulletX
27{ 27{
28 public struct CollisionAlgorithmConstructionInfo 28 public struct CollisionAlgorithmConstructionInfo
29 { 29 {
30 private IDispatcher _dispatcher; 30 private IDispatcher _dispatcher;
31 private PersistentManifold _manifold; 31 private PersistentManifold _manifold;
32 32
33 public CollisionAlgorithmConstructionInfo(IDispatcher dispatcher) 33 public CollisionAlgorithmConstructionInfo(IDispatcher dispatcher)
34 { 34 {
35 _dispatcher = dispatcher; 35 _dispatcher = dispatcher;
36 _manifold = null; 36 _manifold = null;
37 } 37 }
38 38
39 public IDispatcher Dispatcher { get { return _dispatcher; } set { _dispatcher = value; } } 39 public IDispatcher Dispatcher { get { return _dispatcher; } set { _dispatcher = value; } }
40 public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } } 40 public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } }
41 } 41 }
42} 42}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs
index 8c0367f..cde3221 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs
@@ -1,54 +1,54 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25 25
26namespace XnaDevRu.BulletX 26namespace XnaDevRu.BulletX
27{ 27{
28 public enum DispatchFunction 28 public enum DispatchFunction
29 { 29 {
30 Discrete = 1, 30 Discrete = 1,
31 Continuous, 31 Continuous,
32 } 32 }
33 33
34 public class DispatcherInfo 34 public class DispatcherInfo
35 { 35 {
36 private float _timeStep; 36 private float _timeStep;
37 private int _stepCount; 37 private int _stepCount;
38 private DispatchFunction _dispatchFunc = DispatchFunction.Discrete; 38 private DispatchFunction _dispatchFunc = DispatchFunction.Discrete;
39 private float _timeOfImpact = 1; 39 private float _timeOfImpact = 1;
40 private bool _useContinuous; 40 private bool _useContinuous;
41 private bool _enableSatConvex; 41 private bool _enableSatConvex;
42 private bool _enableSpu; 42 private bool _enableSpu;
43 private IDebugDraw _debugDraw; 43 private IDebugDraw _debugDraw;
44 44
45 public float TimeStep { get { return _timeStep; } set { _timeStep = value; } } 45 public float TimeStep { get { return _timeStep; } set { _timeStep = value; } }
46 public int StepCount { get { return _stepCount; } set { _stepCount = value; } } 46 public int StepCount { get { return _stepCount; } set { _stepCount = value; } }
47 public DispatchFunction DispatchFunction { get { return _dispatchFunc; } set { _dispatchFunc = value; } } 47 public DispatchFunction DispatchFunction { get { return _dispatchFunc; } set { _dispatchFunc = value; } }
48 public float TimeOfImpact { get { return _timeOfImpact; } set { _timeOfImpact = value; } } 48 public float TimeOfImpact { get { return _timeOfImpact; } set { _timeOfImpact = value; } }
49 public bool UseContinuous { get { return _useContinuous; } set { _useContinuous = value; } } 49 public bool UseContinuous { get { return _useContinuous; } set { _useContinuous = value; } }
50 public bool EnableSatConvex { get { return _enableSatConvex; } set { _enableSatConvex = value; } } 50 public bool EnableSatConvex { get { return _enableSatConvex; } set { _enableSatConvex = value; } }
51 public bool enableSpu { get { return _enableSpu; } set { _enableSpu = value; } } 51 public bool enableSpu { get { return _enableSpu; } set { _enableSpu = value; } }
52 public IDebugDraw DebugDraw { get { return _debugDraw; } set { _debugDraw = value; } } 52 public IDebugDraw DebugDraw { get { return _debugDraw; } set { _debugDraw = value; } }
53 } 53 }
54} 54}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs
index 89618af..fddd8af 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs
@@ -1,36 +1,36 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public interface IBroadphase 29 public interface IBroadphase
30 { 30 {
31 BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask); 31 BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask);
32 void DestroyProxy(BroadphaseProxy proxy); 32 void DestroyProxy(BroadphaseProxy proxy);
33 void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax); 33 void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax);
34 void CleanProxyFromPairs(BroadphaseProxy proxy); 34 void CleanProxyFromPairs(BroadphaseProxy proxy);
35 } 35 }
36} 36}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs
index 52831b7..7677179 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs
@@ -1,42 +1,42 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25 25
26namespace XnaDevRu.BulletX 26namespace XnaDevRu.BulletX
27{ 27{
28 public interface IDispatcher 28 public interface IDispatcher
29 { 29 {
30 CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB, PersistentManifold sharedManifold); 30 CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB, PersistentManifold sharedManifold);
31 CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB); 31 CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB);
32 PersistentManifold GetNewManifold(object bodyA, object bodyB); 32 PersistentManifold GetNewManifold(object bodyA, object bodyB);
33 void ReleaseManifold(PersistentManifold manifold); 33 void ReleaseManifold(PersistentManifold manifold);
34 void ClearManifold(PersistentManifold manifold); 34 void ClearManifold(PersistentManifold manifold);
35 bool NeedsCollision(CollisionObject bodyA, CollisionObject bodyB); 35 bool NeedsCollision(CollisionObject bodyA, CollisionObject bodyB);
36 bool NeedsResponse(CollisionObject bodyA, CollisionObject bodyB); 36 bool NeedsResponse(CollisionObject bodyA, CollisionObject bodyB);
37 void DispatchAllCollisionPairs(OverlappingPairCache pairCache, DispatcherInfo dispatchInfo); 37 void DispatchAllCollisionPairs(OverlappingPairCache pairCache, DispatcherInfo dispatchInfo);
38 PersistentManifold GetManifoldByIndex(int index); 38 PersistentManifold GetManifoldByIndex(int index);
39 39
40 int ManifoldCount { get; } 40 int ManifoldCount { get; }
41 } 41 }
42} 42}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs
index 4e128da..2456911 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs
@@ -1,33 +1,33 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25 25
26namespace XnaDevRu.BulletX 26namespace XnaDevRu.BulletX
27{ 27{
28 public interface IOverlapCallback 28 public interface IOverlapCallback
29 { 29 {
30 //return true for deletion of the pair 30 //return true for deletion of the pair
31 bool ProcessOverlap(ref BroadphasePair pair); 31 bool ProcessOverlap(ref BroadphasePair pair);
32 } 32 }
33} 33}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs
index 165912a..6548560 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs
@@ -1,159 +1,159 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25 25
26namespace XnaDevRu.BulletX 26namespace XnaDevRu.BulletX
27{ 27{
28 public abstract class OverlappingPairCache : IBroadphase 28 public abstract class OverlappingPairCache : IBroadphase
29 { 29 {
30 private static int _overlappingPairCount = 0; 30 private static int _overlappingPairCount = 0;
31 private List<BroadphasePair> _overlappingPairs = new List<BroadphasePair>(); 31 private List<BroadphasePair> _overlappingPairs = new List<BroadphasePair>();
32 //during the dispatch, check that user doesn't destroy/create proxy 32 //during the dispatch, check that user doesn't destroy/create proxy
33 private bool _blockedForChanges; 33 private bool _blockedForChanges;
34 34
35 public List<BroadphasePair> OverlappingPairs { get { return _overlappingPairs; } set { _overlappingPairs = value; } } 35 public List<BroadphasePair> OverlappingPairs { get { return _overlappingPairs; } set { _overlappingPairs = value; } }
36 public bool BlockedForChanges { get { return _blockedForChanges; } set { _blockedForChanges = value; } } 36 public bool BlockedForChanges { get { return _blockedForChanges; } set { _blockedForChanges = value; } }
37 37
38 public static int OverlappingPairCount { get { return _overlappingPairCount; } set { _overlappingPairCount = value; } } 38 public static int OverlappingPairCount { get { return _overlappingPairCount; } set { _overlappingPairCount = value; } }
39 39
40 public void RemoveOverlappingPair(BroadphasePair pair) 40 public void RemoveOverlappingPair(BroadphasePair pair)
41 { 41 {
42 if (!_overlappingPairs.Contains(pair)) 42 if (!_overlappingPairs.Contains(pair))
43 return; 43 return;
44 44
45 CleanOverlappingPair(ref pair); 45 CleanOverlappingPair(ref pair);
46 _overlappingPairs.Remove(pair); 46 _overlappingPairs.Remove(pair);
47 } 47 }
48 48
49 public void AddOverlappingPair(BroadphaseProxy proxyA, BroadphaseProxy proxyB) 49 public void AddOverlappingPair(BroadphaseProxy proxyA, BroadphaseProxy proxyB)
50 { 50 {
51 //don't add overlap with own 51 //don't add overlap with own
52 bool test = proxyA != proxyB; 52 bool test = proxyA != proxyB;
53 BulletDebug.Assert(proxyA != proxyB); 53 BulletDebug.Assert(proxyA != proxyB);
54 54
55 if (!NeedsBroadphaseCollision(proxyA, proxyB)) 55 if (!NeedsBroadphaseCollision(proxyA, proxyB))
56 return; 56 return;
57 57
58 BroadphasePair pair = new BroadphasePair(proxyA, proxyB); 58 BroadphasePair pair = new BroadphasePair(proxyA, proxyB);
59 _overlappingPairs.Add(pair); 59 _overlappingPairs.Add(pair);
60 _overlappingPairCount++; 60 _overlappingPairCount++;
61 } 61 }
62 62
63 //this FindPair becomes really slow. Either sort the list to speedup the query, or 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. 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) 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 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) 67 public BroadphasePair FindPair(BroadphaseProxy proxyA, BroadphaseProxy proxyB)
68 { 68 {
69 if (!NeedsBroadphaseCollision(proxyA, proxyB)) 69 if (!NeedsBroadphaseCollision(proxyA, proxyB))
70 return null; 70 return null;
71 71
72 BroadphasePair pair = new BroadphasePair(proxyA, proxyB); 72 BroadphasePair pair = new BroadphasePair(proxyA, proxyB);
73 for (int i = 0; i < _overlappingPairs.Count; i++) 73 for (int i = 0; i < _overlappingPairs.Count; i++)
74 { 74 {
75 if (_overlappingPairs[i] == pair) 75 if (_overlappingPairs[i] == pair)
76 { 76 {
77 return _overlappingPairs[i]; 77 return _overlappingPairs[i];
78 } 78 }
79 } 79 }
80 80
81 return null; 81 return null;
82 } 82 }
83 83
84 public void CleanProxyFromPairs(BroadphaseProxy proxy) 84 public void CleanProxyFromPairs(BroadphaseProxy proxy)
85 { 85 {
86 for (int i = 0; i < _overlappingPairs.Count; i++) 86 for (int i = 0; i < _overlappingPairs.Count; i++)
87 { 87 {
88 BroadphasePair pair = _overlappingPairs[i]; 88 BroadphasePair pair = _overlappingPairs[i];
89 if (pair.ProxyA == proxy || 89 if (pair.ProxyA == proxy ||
90 pair.ProxyB == proxy) 90 pair.ProxyB == proxy)
91 { 91 {
92 CleanOverlappingPair(ref pair); 92 CleanOverlappingPair(ref pair);
93 _overlappingPairs[i] = pair; 93 _overlappingPairs[i] = pair;
94 } 94 }
95 } 95 }
96 } 96 }
97 97
98 public void RemoveOverlappingPairsContainingProxy(BroadphaseProxy proxy) 98 public void RemoveOverlappingPairsContainingProxy(BroadphaseProxy proxy)
99 { 99 {
100 for (int i = _overlappingPairs.Count - 1; i >= 0; i--) 100 for (int i = _overlappingPairs.Count - 1; i >= 0; i--)
101 { 101 {
102 BroadphasePair pair = _overlappingPairs[i]; 102 BroadphasePair pair = _overlappingPairs[i];
103 if (pair.ProxyA == proxy || 103 if (pair.ProxyA == proxy ||
104 pair.ProxyB == proxy) 104 pair.ProxyB == proxy)
105 { 105 {
106 RemoveOverlappingPair(pair); 106 RemoveOverlappingPair(pair);
107 i++; 107 i++;
108 } 108 }
109 } 109 }
110 } 110 }
111 111
112 public bool NeedsBroadphaseCollision(BroadphaseProxy proxy0, BroadphaseProxy proxy1) 112 public bool NeedsBroadphaseCollision(BroadphaseProxy proxy0, BroadphaseProxy proxy1)
113 { 113 {
114 bool collides = (proxy0.CollisionFilterGroup & proxy1.CollisionFilterMask) != 0; 114 bool collides = (proxy0.CollisionFilterGroup & proxy1.CollisionFilterMask) != 0;
115 collides = collides && ((proxy1.CollisionFilterGroup & proxy0.CollisionFilterMask) != 0); 115 collides = collides && ((proxy1.CollisionFilterGroup & proxy0.CollisionFilterMask) != 0);
116 116
117 return collides; 117 return collides;
118 } 118 }
119 119
120 public virtual void ProcessAllOverlappingPairs(IOverlapCallback callback) 120 public virtual void ProcessAllOverlappingPairs(IOverlapCallback callback)
121 { 121 {
122 List<BroadphasePair> deleting = new List<BroadphasePair>(); 122 List<BroadphasePair> deleting = new List<BroadphasePair>();
123 for (int i = 0; i < _overlappingPairs.Count; i++) 123 for (int i = 0; i < _overlappingPairs.Count; i++)
124 { 124 {
125 BroadphasePair p = _overlappingPairs[i]; 125 BroadphasePair p = _overlappingPairs[i];
126 if (callback.ProcessOverlap(ref p)) 126 if (callback.ProcessOverlap(ref p))
127 { 127 {
128 CleanOverlappingPair(ref p); 128 CleanOverlappingPair(ref p);
129 deleting.Add(p); 129 deleting.Add(p);
130 _overlappingPairCount--; 130 _overlappingPairCount--;
131 } 131 }
132 } 132 }
133 133
134 for (int i = 0; i < deleting.Count; i++) 134 for (int i = 0; i < deleting.Count; i++)
135 _overlappingPairs.Remove(deleting[i]); 135 _overlappingPairs.Remove(deleting[i]);
136 } 136 }
137 137
138 public void CleanOverlappingPair(ref BroadphasePair pair) 138 public void CleanOverlappingPair(ref BroadphasePair pair)
139 { 139 {
140 if (pair.CollisionAlgorithm != null) 140 if (pair.CollisionAlgorithm != null)
141 { 141 {
142 if (pair.CollisionAlgorithm is IDisposable) 142 if (pair.CollisionAlgorithm is IDisposable)
143 (pair.CollisionAlgorithm as IDisposable).Dispose(); 143 (pair.CollisionAlgorithm as IDisposable).Dispose();
144 pair.CollisionAlgorithm = null; 144 pair.CollisionAlgorithm = null;
145 } 145 }
146 } 146 }
147 147
148 public abstract void RefreshOverlappingPairs(); 148 public abstract void RefreshOverlappingPairs();
149 149
150 #region IBroadphase Members 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); 151 public abstract BroadphaseProxy CreateProxy(MonoXnaCompactMaths.Vector3 min, MonoXnaCompactMaths.Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask);
152 152
153 public abstract void DestroyProxy(BroadphaseProxy proxy); 153 public abstract void DestroyProxy(BroadphaseProxy proxy);
154 154
155 public abstract void SetAabb(BroadphaseProxy proxy, MonoXnaCompactMaths.Vector3 aabbMin, MonoXnaCompactMaths.Vector3 aabbMax); 155 public abstract void SetAabb(BroadphaseProxy proxy, MonoXnaCompactMaths.Vector3 aabbMin, MonoXnaCompactMaths.Vector3 aabbMax);
156 156
157 #endregion 157 #endregion
158 } 158 }
159} 159}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs
index 1dc3f34..f37b3f5 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs
@@ -1,128 +1,128 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public class SimpleBroadphase : OverlappingPairCache 29 public class SimpleBroadphase : OverlappingPairCache
30 { 30 {
31 private int _maxProxies; 31 private int _maxProxies;
32 private List<SimpleBroadphaseProxy> _proxies = new List<SimpleBroadphaseProxy>(); 32 private List<SimpleBroadphaseProxy> _proxies = new List<SimpleBroadphaseProxy>();
33 33
34 public SimpleBroadphase() 34 public SimpleBroadphase()
35 : this(16384) { } 35 : this(16384) { }
36 36
37 public SimpleBroadphase(int maxProxies) 37 public SimpleBroadphase(int maxProxies)
38 : base() 38 : base()
39 { 39 {
40 _maxProxies = maxProxies; 40 _maxProxies = maxProxies;
41 } 41 }
42 42
43 public override BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask) 43 public override BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask)
44 { 44 {
45 if (_proxies.Count >= _maxProxies) 45 if (_proxies.Count >= _maxProxies)
46 { 46 {
47 BulletDebug.Assert(false); 47 BulletDebug.Assert(false);
48 return null; //should never happen, but don't let the game crash ;-) 48 return null; //should never happen, but don't let the game crash ;-)
49 } 49 }
50 BulletDebug.Assert(min.X <= max.X && min.Y <= max.Y && min.Z <= max.Z); 50 BulletDebug.Assert(min.X <= max.X && min.Y <= max.Y && min.Z <= max.Z);
51 51
52 SimpleBroadphaseProxy proxy = new SimpleBroadphaseProxy(min, max, shapeType, userData, collisionFilterGroup, collisionFilterMask); 52 SimpleBroadphaseProxy proxy = new SimpleBroadphaseProxy(min, max, shapeType, userData, collisionFilterGroup, collisionFilterMask);
53 _proxies.Add(proxy); 53 _proxies.Add(proxy);
54 54
55 return proxy; 55 return proxy;
56 } 56 }
57 57
58 58
59 public override void DestroyProxy(BroadphaseProxy proxy) 59 public override void DestroyProxy(BroadphaseProxy proxy)
60 { 60 {
61 RemoveOverlappingPairsContainingProxy(proxy); 61 RemoveOverlappingPairsContainingProxy(proxy);
62 _proxies.Remove(proxy as SimpleBroadphaseProxy); 62 _proxies.Remove(proxy as SimpleBroadphaseProxy);
63 } 63 }
64 64
65 public override void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax) 65 public override void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax)
66 { 66 {
67 SimpleBroadphaseProxy simpleProxy = GetSimpleProxyFromProxy(proxy); 67 SimpleBroadphaseProxy simpleProxy = GetSimpleProxyFromProxy(proxy);
68 simpleProxy.Minimum = aabbMin; 68 simpleProxy.Minimum = aabbMin;
69 simpleProxy.Maximum = aabbMax; 69 simpleProxy.Maximum = aabbMax;
70 } 70 }
71 71
72 private SimpleBroadphaseProxy GetSimpleProxyFromProxy(BroadphaseProxy proxy) 72 private SimpleBroadphaseProxy GetSimpleProxyFromProxy(BroadphaseProxy proxy)
73 { 73 {
74 return proxy as SimpleBroadphaseProxy; 74 return proxy as SimpleBroadphaseProxy;
75 } 75 }
76 76
77 public override void RefreshOverlappingPairs() 77 public override void RefreshOverlappingPairs()
78 { 78 {
79 for (int i = 0; i < _proxies.Count; i++) 79 for (int i = 0; i < _proxies.Count; i++)
80 { 80 {
81 SimpleBroadphaseProxy proxyA = _proxies[i]; 81 SimpleBroadphaseProxy proxyA = _proxies[i];
82 82
83 for (int j = i + 1; j < _proxies.Count; j++) 83 for (int j = i + 1; j < _proxies.Count; j++)
84 { 84 {
85 SimpleBroadphaseProxy proxyB = _proxies[j]; 85 SimpleBroadphaseProxy proxyB = _proxies[j];
86 86
87 if (AabbOverlap(proxyA, proxyB)) 87 if (AabbOverlap(proxyA, proxyB))
88 { 88 {
89 if (FindPair(proxyA, proxyB) == null) 89 if (FindPair(proxyA, proxyB) == null)
90 { 90 {
91 AddOverlappingPair(proxyA, proxyB); 91 AddOverlappingPair(proxyA, proxyB);
92 } 92 }
93 } 93 }
94 } 94 }
95 } 95 }
96 96
97 CheckOverlapCallback check = new CheckOverlapCallback(); 97 CheckOverlapCallback check = new CheckOverlapCallback();
98 ProcessAllOverlappingPairs(check); 98 ProcessAllOverlappingPairs(check);
99 } 99 }
100 100
101 public static bool AabbOverlap(SimpleBroadphaseProxy proxyA, SimpleBroadphaseProxy proxyB) 101 public static bool AabbOverlap(SimpleBroadphaseProxy proxyA, SimpleBroadphaseProxy proxyB)
102 { 102 {
103 return proxyA.Minimum.X <= proxyB.Maximum.X && proxyB.Minimum.X <= proxyA.Maximum.X && 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 && 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; 105 proxyA.Minimum.Z <= proxyB.Maximum.Z && proxyB.Minimum.Z <= proxyA.Maximum.Z;
106 } 106 }
107 107
108 private void Validate() 108 private void Validate()
109 { 109 {
110 for (int i = 0; i < _proxies.Count; i++) 110 for (int i = 0; i < _proxies.Count; i++)
111 { 111 {
112 for (int j = i + 1; j < _proxies.Count; j++) 112 for (int j = i + 1; j < _proxies.Count; j++)
113 { 113 {
114 if (_proxies[i] == _proxies[j]) 114 if (_proxies[i] == _proxies[j])
115 throw new BulletException(); 115 throw new BulletException();
116 } 116 }
117 } 117 }
118 } 118 }
119 } 119 }
120 120
121 public class CheckOverlapCallback : IOverlapCallback 121 public class CheckOverlapCallback : IOverlapCallback
122 { 122 {
123 public bool ProcessOverlap(ref BroadphasePair pair) 123 public bool ProcessOverlap(ref BroadphasePair pair)
124 { 124 {
125 return (!SimpleBroadphase.AabbOverlap(pair.ProxyA as SimpleBroadphaseProxy, pair.ProxyB as SimpleBroadphaseProxy)); 125 return (!SimpleBroadphase.AabbOverlap(pair.ProxyA as SimpleBroadphaseProxy, pair.ProxyB as SimpleBroadphaseProxy));
126 } 126 }
127 } 127 }
128} 128}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs
index f473ef2..cfc2150 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs
@@ -1,46 +1,46 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public class SimpleBroadphaseProxy : BroadphaseProxy 29 public class SimpleBroadphaseProxy : BroadphaseProxy
30 { 30 {
31 private Vector3 _min; 31 private Vector3 _min;
32 private Vector3 _max; 32 private Vector3 _max;
33 33
34 public SimpleBroadphaseProxy() { } 34 public SimpleBroadphaseProxy() { }
35 35
36 public SimpleBroadphaseProxy(Vector3 minPoint, Vector3 maxPoint, BroadphaseNativeTypes shapeType, object userData, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask) 36 public SimpleBroadphaseProxy(Vector3 minPoint, Vector3 maxPoint, BroadphaseNativeTypes shapeType, object userData, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask)
37 : base(userData, collisionFilterGroup, collisionFilterMask) 37 : base(userData, collisionFilterGroup, collisionFilterMask)
38 { 38 {
39 _min = minPoint; 39 _min = minPoint;
40 _max = maxPoint; 40 _max = maxPoint;
41 } 41 }
42 42
43 public Vector3 Minimum { get { return _min; } set { _min = value; } } 43 public Vector3 Minimum { get { return _min; } set { _min = value; } }
44 public Vector3 Maximum { get { return _max; } set { _max = value; } } 44 public Vector3 Maximum { get { return _max; } set { _max = value; } }
45 } 45 }
46} 46}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs
index ba638d1..6bebf93 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs
@@ -1,59 +1,59 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 internal class BridgeTriangleRaycastCallback : TriangleRaycastCallback 29 internal class BridgeTriangleRaycastCallback : TriangleRaycastCallback
30 { 30 {
31 CollisionWorld.RayResultCallback _resultCallback; 31 CollisionWorld.RayResultCallback _resultCallback;
32 CollisionObject _collisionObject; 32 CollisionObject _collisionObject;
33 TriangleMeshShape _triangleMesh; 33 TriangleMeshShape _triangleMesh;
34 34
35 public BridgeTriangleRaycastCallback(Vector3 from, Vector3 to, 35 public BridgeTriangleRaycastCallback(Vector3 from, Vector3 to,
36 CollisionWorld.RayResultCallback resultCallback, CollisionObject collisionObject, TriangleMeshShape triangleMesh) 36 CollisionWorld.RayResultCallback resultCallback, CollisionObject collisionObject, TriangleMeshShape triangleMesh)
37 : base(from, to) 37 : base(from, to)
38 { 38 {
39 _resultCallback = resultCallback; 39 _resultCallback = resultCallback;
40 _collisionObject = collisionObject; 40 _collisionObject = collisionObject;
41 _triangleMesh = triangleMesh; 41 _triangleMesh = triangleMesh;
42 } 42 }
43 43
44 public override float ReportHit(Vector3 hitNormalLocal, float hitFraction, int partId, int triangleIndex) 44 public override float ReportHit(Vector3 hitNormalLocal, float hitFraction, int partId, int triangleIndex)
45 { 45 {
46 CollisionWorld.LocalShapeInfo shapeInfo = new CollisionWorld.LocalShapeInfo(); 46 CollisionWorld.LocalShapeInfo shapeInfo = new CollisionWorld.LocalShapeInfo();
47 shapeInfo.ShapePart = partId; 47 shapeInfo.ShapePart = partId;
48 shapeInfo.TriangleIndex = triangleIndex; 48 shapeInfo.TriangleIndex = triangleIndex;
49 49
50 CollisionWorld.LocalRayResult rayResult = new CollisionWorld.LocalRayResult 50 CollisionWorld.LocalRayResult rayResult = new CollisionWorld.LocalRayResult
51 (_collisionObject, 51 (_collisionObject,
52 shapeInfo, 52 shapeInfo,
53 hitNormalLocal, 53 hitNormalLocal,
54 hitFraction); 54 hitFraction);
55 55
56 return _resultCallback.AddSingleResult(rayResult); 56 return _resultCallback.AddSingleResult(rayResult);
57 } 57 }
58 } 58 }
59} 59}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs
index 4e3903b..3bd5601 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs
@@ -1,40 +1,40 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public class CollisionAlgorithmCreateFunction 29 public class CollisionAlgorithmCreateFunction
30 { 30 {
31 private bool _swapped; 31 private bool _swapped;
32 32
33 public bool IsSwapped { get { return _swapped; } set { _swapped = value; } } 33 public bool IsSwapped { get { return _swapped; } set { _swapped = value; } }
34 34
35 public virtual CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) 35 public virtual CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1)
36 { 36 {
37 return null; 37 return null;
38 } 38 }
39 } 39 }
40} 40}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs
index 75d35b0..336defd 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs
@@ -1,280 +1,280 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public delegate void NearCallback(ref BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo); 29 public delegate void NearCallback(ref BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo);
30 30
31 public class CollisionDispatcher : IDispatcher 31 public class CollisionDispatcher : IDispatcher
32 { 32 {
33 private List<PersistentManifold> _manifolds = new List<PersistentManifold>(); 33 private List<PersistentManifold> _manifolds = new List<PersistentManifold>();
34 34
35 //private bool _useIslands; 35 //private bool _useIslands;
36 private NearCallback _nearCallback; 36 private NearCallback _nearCallback;
37 37
38 //private ManifoldResult _defaultManifoldResult; 38 //private ManifoldResult _defaultManifoldResult;
39 39
40 private CollisionAlgorithmCreateFunction[,] _doubleDispatch = new CollisionAlgorithmCreateFunction[(int)BroadphaseNativeTypes.MaxBroadphaseCollisionTypes, (int)BroadphaseNativeTypes.MaxBroadphaseCollisionTypes]; 40 private CollisionAlgorithmCreateFunction[,] _doubleDispatch = new CollisionAlgorithmCreateFunction[(int)BroadphaseNativeTypes.MaxBroadphaseCollisionTypes, (int)BroadphaseNativeTypes.MaxBroadphaseCollisionTypes];
41 41
42 //default CreationFunctions, filling the m_doubleDispatch table 42 //default CreationFunctions, filling the m_doubleDispatch table
43 private CollisionAlgorithmCreateFunction _convexConvexCreateFunc; 43 private CollisionAlgorithmCreateFunction _convexConvexCreateFunc;
44 private CollisionAlgorithmCreateFunction _convexConcaveCreateFunc; 44 private CollisionAlgorithmCreateFunction _convexConcaveCreateFunc;
45 private CollisionAlgorithmCreateFunction _swappedConvexConcaveCreateFunc; 45 private CollisionAlgorithmCreateFunction _swappedConvexConcaveCreateFunc;
46 private CollisionAlgorithmCreateFunction _compoundCreateFunc; 46 private CollisionAlgorithmCreateFunction _compoundCreateFunc;
47 private CollisionAlgorithmCreateFunction _swappedCompoundCreateFunc; 47 private CollisionAlgorithmCreateFunction _swappedCompoundCreateFunc;
48 private CollisionAlgorithmCreateFunction _emptyCreateFunc; 48 private CollisionAlgorithmCreateFunction _emptyCreateFunc;
49 49
50 private int _count; 50 private int _count;
51 private static int _manifoldCount = 0; 51 private static int _manifoldCount = 0;
52 52
53 public CollisionDispatcher() 53 public CollisionDispatcher()
54 { 54 {
55 NearCallback = DefaultNearCallback; 55 NearCallback = DefaultNearCallback;
56 //_useIslands = true; 56 //_useIslands = true;
57 //default CreationFunctions, filling the m_doubleDispatch table 57 //default CreationFunctions, filling the m_doubleDispatch table
58 _convexConvexCreateFunc = new ConvexConvexAlgorithm.CreateFunc(); 58 _convexConvexCreateFunc = new ConvexConvexAlgorithm.CreateFunc();
59 _convexConcaveCreateFunc = new ConvexConcaveCollisionAlgorithm.CreateFunc(); 59 _convexConcaveCreateFunc = new ConvexConcaveCollisionAlgorithm.CreateFunc();
60 _swappedConvexConcaveCreateFunc = new ConvexConcaveCollisionAlgorithm.SwappedCreateFunc(); 60 _swappedConvexConcaveCreateFunc = new ConvexConcaveCollisionAlgorithm.SwappedCreateFunc();
61 _compoundCreateFunc = new CompoundCollisionAlgorithm.CreateFunc(); 61 _compoundCreateFunc = new CompoundCollisionAlgorithm.CreateFunc();
62 _swappedCompoundCreateFunc = new CompoundCollisionAlgorithm.SwappedCreateFunc(); 62 _swappedCompoundCreateFunc = new CompoundCollisionAlgorithm.SwappedCreateFunc();
63 _emptyCreateFunc = new EmptyAlgorithm.CreateFunc(); 63 _emptyCreateFunc = new EmptyAlgorithm.CreateFunc();
64 64
65 for (BroadphaseNativeTypes i = BroadphaseNativeTypes.Box; i < BroadphaseNativeTypes.MaxBroadphaseCollisionTypes; i++) 65 for (BroadphaseNativeTypes i = BroadphaseNativeTypes.Box; i < BroadphaseNativeTypes.MaxBroadphaseCollisionTypes; i++)
66 { 66 {
67 for (BroadphaseNativeTypes j = BroadphaseNativeTypes.Box; j < BroadphaseNativeTypes.MaxBroadphaseCollisionTypes; j++) 67 for (BroadphaseNativeTypes j = BroadphaseNativeTypes.Box; j < BroadphaseNativeTypes.MaxBroadphaseCollisionTypes; j++)
68 { 68 {
69 _doubleDispatch[(int)i, (int)j] = FindCreateFunction(i, j); 69 _doubleDispatch[(int)i, (int)j] = FindCreateFunction(i, j);
70 if (_doubleDispatch[(int)i, (int)j] == null) 70 if (_doubleDispatch[(int)i, (int)j] == null)
71 throw new BulletException(); 71 throw new BulletException();
72 } 72 }
73 } 73 }
74 } 74 }
75 75
76 public int Count { get { return _count; } set { _count = value; } } 76 public int Count { get { return _count; } set { _count = value; } }
77 public int ManifoldCount { get { return _manifolds.Count; } } 77 public int ManifoldCount { get { return _manifolds.Count; } }
78 public List<PersistentManifold> Manifolds { get { return _manifolds; } } 78 public List<PersistentManifold> Manifolds { get { return _manifolds; } }
79 79
80 public static int GlobalManifoldCount { get { return _manifoldCount; } set { _manifoldCount = value; } } 80 public static int GlobalManifoldCount { get { return _manifoldCount; } set { _manifoldCount = value; } }
81 81
82 public PersistentManifold GetManifoldByIndex(int index) 82 public PersistentManifold GetManifoldByIndex(int index)
83 { 83 {
84 return _manifolds[index]; 84 return _manifolds[index];
85 } 85 }
86 86
87 //registerCollisionCreateFunc allows registration of custom/alternative collision create functions 87 //registerCollisionCreateFunc allows registration of custom/alternative collision create functions
88 public void RegisterCollisionCreateFunc(BroadphaseNativeTypes proxyTypeA, BroadphaseNativeTypes proxyTypeB, CollisionAlgorithmCreateFunction createFunc) 88 public void RegisterCollisionCreateFunc(BroadphaseNativeTypes proxyTypeA, BroadphaseNativeTypes proxyTypeB, CollisionAlgorithmCreateFunction createFunc)
89 { 89 {
90 _doubleDispatch[(int)proxyTypeA, (int)proxyTypeB] = createFunc; 90 _doubleDispatch[(int)proxyTypeA, (int)proxyTypeB] = createFunc;
91 } 91 }
92 92
93 public virtual PersistentManifold GetNewManifold(object bodyA, object bodyB) 93 public virtual PersistentManifold GetNewManifold(object bodyA, object bodyB)
94 { 94 {
95 _manifoldCount++; 95 _manifoldCount++;
96 96
97 CollisionObject body0 = bodyA as CollisionObject; 97 CollisionObject body0 = bodyA as CollisionObject;
98 CollisionObject body1 = bodyB as CollisionObject; 98 CollisionObject body1 = bodyB as CollisionObject;
99 99
100 PersistentManifold manifold = new PersistentManifold(body0, body1); 100 PersistentManifold manifold = new PersistentManifold(body0, body1);
101 _manifolds.Add(manifold); 101 _manifolds.Add(manifold);
102 102
103 return manifold; 103 return manifold;
104 } 104 }
105 105
106 public virtual void ReleaseManifold(PersistentManifold manifold) 106 public virtual void ReleaseManifold(PersistentManifold manifold)
107 { 107 {
108 _manifoldCount--; 108 _manifoldCount--;
109 109
110 ClearManifold(manifold); 110 ClearManifold(manifold);
111 _manifolds.Remove(manifold); 111 _manifolds.Remove(manifold);
112 } 112 }
113 113
114 114
115 public virtual void ClearManifold(PersistentManifold manifold) 115 public virtual void ClearManifold(PersistentManifold manifold)
116 { 116 {
117 manifold.ClearManifold(); 117 manifold.ClearManifold();
118 } 118 }
119 119
120 public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB) 120 public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB)
121 { 121 {
122 return FindAlgorithm(bodyA, bodyB, null); 122 return FindAlgorithm(bodyA, bodyB, null);
123 } 123 }
124 124
125 public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB, PersistentManifold sharedManifold) 125 public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB, PersistentManifold sharedManifold)
126 { 126 {
127 CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo = new CollisionAlgorithmConstructionInfo(); 127 CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo = new CollisionAlgorithmConstructionInfo();
128 collisionAlgorithmConstructionInfo.Dispatcher = this; 128 collisionAlgorithmConstructionInfo.Dispatcher = this;
129 collisionAlgorithmConstructionInfo.Manifold = sharedManifold; 129 collisionAlgorithmConstructionInfo.Manifold = sharedManifold;
130 CollisionAlgorithm collisionAlgorithm = _doubleDispatch[(int)bodyA.CollisionShape.ShapeType, (int)bodyB.CollisionShape.ShapeType].CreateCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB); 130 CollisionAlgorithm collisionAlgorithm = _doubleDispatch[(int)bodyA.CollisionShape.ShapeType, (int)bodyB.CollisionShape.ShapeType].CreateCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB);
131 return collisionAlgorithm; 131 return collisionAlgorithm;
132 } 132 }
133 133
134 /*public CollisionAlgorithm internalFindAlgorithm(CollisionObject body0, CollisionObject body1) 134 /*public CollisionAlgorithm internalFindAlgorithm(CollisionObject body0, CollisionObject body1)
135 { 135 {
136 return internalFindAlgorithm(body0, body1, null); 136 return internalFindAlgorithm(body0, body1, null);
137 } 137 }
138 138
139 public CollisionAlgorithm internalFindAlgorithm(CollisionObject body0, CollisionObject body1, PersistentManifold sharedManifold) 139 public CollisionAlgorithm internalFindAlgorithm(CollisionObject body0, CollisionObject body1, PersistentManifold sharedManifold)
140 { 140 {
141 m_count++; 141 m_count++;
142 142
143 CollisionAlgorithmConstructionInfo ci = new CollisionAlgorithmConstructionInfo(); 143 CollisionAlgorithmConstructionInfo ci = new CollisionAlgorithmConstructionInfo();
144 ci.m_dispatcher = this; 144 ci.m_dispatcher = this;
145 145
146 if (body0.getCollisionShape().isConvex() && body1.getCollisionShape().isConvex()) 146 if (body0.getCollisionShape().isConvex() && body1.getCollisionShape().isConvex())
147 { 147 {
148 return new ConvexConvexAlgorithm(sharedManifold, ci, body0, body1); 148 return new ConvexConvexAlgorithm(sharedManifold, ci, body0, body1);
149 } 149 }
150 150
151 if (body0.getCollisionShape().isConvex() && body1.getCollisionShape().isConcave()) 151 if (body0.getCollisionShape().isConvex() && body1.getCollisionShape().isConcave())
152 { 152 {
153 return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, false); 153 return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, false);
154 } 154 }
155 155
156 if (body1.getCollisionShape().isConvex() && body0.getCollisionShape().isConcave()) 156 if (body1.getCollisionShape().isConvex() && body0.getCollisionShape().isConcave())
157 { 157 {
158 return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, true); 158 return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, true);
159 } 159 }
160 160
161 if (body0.getCollisionShape().isCompound()) 161 if (body0.getCollisionShape().isCompound())
162 { 162 {
163 return new CompoundCollisionAlgorithm(ci, body0, body1, false); 163 return new CompoundCollisionAlgorithm(ci, body0, body1, false);
164 } 164 }
165 else 165 else
166 { 166 {
167 if (body1.getCollisionShape().isCompound()) 167 if (body1.getCollisionShape().isCompound())
168 { 168 {
169 return new CompoundCollisionAlgorithm(ci, body0, body1, true); 169 return new CompoundCollisionAlgorithm(ci, body0, body1, true);
170 } 170 }
171 } 171 }
172 172
173 //failed to find an algorithm 173 //failed to find an algorithm
174 return new EmptyAlgorithm(ci); 174 return new EmptyAlgorithm(ci);
175 }*/ 175 }*/
176 176
177 public virtual bool NeedsCollision(CollisionObject bodyA, CollisionObject bodyB) 177 public virtual bool NeedsCollision(CollisionObject bodyA, CollisionObject bodyB)
178 { 178 {
179 if (bodyA == null || bodyB == null) 179 if (bodyA == null || bodyB == null)
180 throw new BulletException(); 180 throw new BulletException();
181 181
182 bool needsCollision = true; 182 bool needsCollision = true;
183 183
184 //broadphase filtering already deals with this 184 //broadphase filtering already deals with this
185 /*if ((body0.isStaticObject() || body0.isKinematicObject()) && 185 /*if ((body0.isStaticObject() || body0.isKinematicObject()) &&
186 (body1.isStaticObject() || body1.isKinematicObject())) 186 (body1.isStaticObject() || body1.isKinematicObject()))
187 { 187 {
188 printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n"); 188 printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n");
189 }*/ 189 }*/
190 190
191 if ((!bodyA.IsActive) && (!bodyB.IsActive)) 191 if ((!bodyA.IsActive) && (!bodyB.IsActive))
192 needsCollision = false; 192 needsCollision = false;
193 193
194 return needsCollision; 194 return needsCollision;
195 } 195 }
196 196
197 public virtual bool NeedsResponse(CollisionObject bodyA, CollisionObject bodyB) 197 public virtual bool NeedsResponse(CollisionObject bodyA, CollisionObject bodyB)
198 { 198 {
199 //here you can do filtering 199 //here you can do filtering
200 bool hasResponse = bodyA.HasContactResponse && bodyB.HasContactResponse; 200 bool hasResponse = bodyA.HasContactResponse && bodyB.HasContactResponse;
201 hasResponse = hasResponse && (!bodyA.IsStaticOrKinematicObject || !bodyB.IsStaticOrKinematicObject); 201 hasResponse = hasResponse && (!bodyA.IsStaticOrKinematicObject || !bodyB.IsStaticOrKinematicObject);
202 return hasResponse; 202 return hasResponse;
203 } 203 }
204 204
205 public virtual void DispatchAllCollisionPairs(OverlappingPairCache pairCache, DispatcherInfo dispatchInfo) 205 public virtual void DispatchAllCollisionPairs(OverlappingPairCache pairCache, DispatcherInfo dispatchInfo)
206 { 206 {
207 CollisionPairCallback collisionCallback = new CollisionPairCallback(dispatchInfo, this); 207 CollisionPairCallback collisionCallback = new CollisionPairCallback(dispatchInfo, this);
208 pairCache.ProcessAllOverlappingPairs(collisionCallback); 208 pairCache.ProcessAllOverlappingPairs(collisionCallback);
209 } 209 }
210 210
211 private CollisionAlgorithmCreateFunction FindCreateFunction(BroadphaseNativeTypes proxyTypeA, BroadphaseNativeTypes proxyTypeB) 211 private CollisionAlgorithmCreateFunction FindCreateFunction(BroadphaseNativeTypes proxyTypeA, BroadphaseNativeTypes proxyTypeB)
212 { 212 {
213 if (BroadphaseProxy.IsConvex(proxyTypeA) && BroadphaseProxy.IsConvex(proxyTypeB)) 213 if (BroadphaseProxy.IsConvex(proxyTypeA) && BroadphaseProxy.IsConvex(proxyTypeB))
214 { 214 {
215 return _convexConvexCreateFunc; 215 return _convexConvexCreateFunc;
216 } 216 }
217 217
218 if (BroadphaseProxy.IsConvex(proxyTypeA) && BroadphaseProxy.IsConcave(proxyTypeB)) 218 if (BroadphaseProxy.IsConvex(proxyTypeA) && BroadphaseProxy.IsConcave(proxyTypeB))
219 { 219 {
220 return _convexConcaveCreateFunc; 220 return _convexConcaveCreateFunc;
221 } 221 }
222 222
223 if (BroadphaseProxy.IsConvex(proxyTypeB) && BroadphaseProxy.IsConcave(proxyTypeA)) 223 if (BroadphaseProxy.IsConvex(proxyTypeB) && BroadphaseProxy.IsConcave(proxyTypeA))
224 { 224 {
225 return _swappedConvexConcaveCreateFunc; 225 return _swappedConvexConcaveCreateFunc;
226 } 226 }
227 227
228 if (BroadphaseProxy.IsCompound(proxyTypeA)) 228 if (BroadphaseProxy.IsCompound(proxyTypeA))
229 { 229 {
230 return _compoundCreateFunc; 230 return _compoundCreateFunc;
231 } 231 }
232 else 232 else
233 { 233 {
234 if (BroadphaseProxy.IsCompound(proxyTypeB)) 234 if (BroadphaseProxy.IsCompound(proxyTypeB))
235 { 235 {
236 return _swappedCompoundCreateFunc; 236 return _swappedCompoundCreateFunc;
237 } 237 }
238 } 238 }
239 239
240 //failed to find an algorithm 240 //failed to find an algorithm
241 return _emptyCreateFunc; 241 return _emptyCreateFunc;
242 } 242 }
243 243
244 public NearCallback NearCallback { get { return _nearCallback; } set { _nearCallback = value; } } 244 public NearCallback NearCallback { get { return _nearCallback; } set { _nearCallback = value; } }
245 245
246 //by default, Bullet will use this near callback 246 //by default, Bullet will use this near callback
247 public static void DefaultNearCallback(ref BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo) 247 public static void DefaultNearCallback(ref BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo)
248 { 248 {
249 CollisionObject collisionObjectA = collisionPair.ProxyA.ClientData as CollisionObject; 249 CollisionObject collisionObjectA = collisionPair.ProxyA.ClientData as CollisionObject;
250 CollisionObject collisionObjectB = collisionPair.ProxyB.ClientData as CollisionObject; 250 CollisionObject collisionObjectB = collisionPair.ProxyB.ClientData as CollisionObject;
251 251
252 if (dispatcher.NeedsCollision(collisionObjectA, collisionObjectB)) 252 if (dispatcher.NeedsCollision(collisionObjectA, collisionObjectB))
253 { 253 {
254 //dispatcher will keep algorithms persistent in the collision pair 254 //dispatcher will keep algorithms persistent in the collision pair
255 if (collisionPair.CollisionAlgorithm == null) 255 if (collisionPair.CollisionAlgorithm == null)
256 { 256 {
257 collisionPair.CollisionAlgorithm = dispatcher.FindAlgorithm(collisionObjectA, collisionObjectB); 257 collisionPair.CollisionAlgorithm = dispatcher.FindAlgorithm(collisionObjectA, collisionObjectB);
258 } 258 }
259 259
260 if (collisionPair.CollisionAlgorithm != null) 260 if (collisionPair.CollisionAlgorithm != null)
261 { 261 {
262 ManifoldResult contactPointResult = new ManifoldResult(collisionObjectA, collisionObjectB); 262 ManifoldResult contactPointResult = new ManifoldResult(collisionObjectA, collisionObjectB);
263 263
264 if (dispatchInfo.DispatchFunction == DispatchFunction.Discrete) 264 if (dispatchInfo.DispatchFunction == DispatchFunction.Discrete)
265 { 265 {
266 //discrete collision detection query 266 //discrete collision detection query
267 collisionPair.CollisionAlgorithm.ProcessCollision(collisionObjectA, collisionObjectB, dispatchInfo, contactPointResult); 267 collisionPair.CollisionAlgorithm.ProcessCollision(collisionObjectA, collisionObjectB, dispatchInfo, contactPointResult);
268 } 268 }
269 else 269 else
270 { 270 {
271 //continuous collision detection query, time of impact (toi) 271 //continuous collision detection query, time of impact (toi)
272 float timeOfImpact = collisionPair.CollisionAlgorithm.CalculateTimeOfImpact(collisionObjectA, collisionObjectB, dispatchInfo, contactPointResult); 272 float timeOfImpact = collisionPair.CollisionAlgorithm.CalculateTimeOfImpact(collisionObjectA, collisionObjectB, dispatchInfo, contactPointResult);
273 if (dispatchInfo.TimeOfImpact > timeOfImpact) 273 if (dispatchInfo.TimeOfImpact > timeOfImpact)
274 dispatchInfo.TimeOfImpact = timeOfImpact; 274 dispatchInfo.TimeOfImpact = timeOfImpact;
275 } 275 }
276 } 276 }
277 } 277 }
278 } 278 }
279 } 279 }
280} 280}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs
index 4e9cf58..3eae564 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs
@@ -1,163 +1,163 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public enum ActivationState 29 public enum ActivationState
30 { 30 {
31 Nothing = 0, 31 Nothing = 0,
32 Active, 32 Active,
33 IslandSleeping, 33 IslandSleeping,
34 WantsDeactivation, 34 WantsDeactivation,
35 DisableDeactivation, 35 DisableDeactivation,
36 DisableSimulation, 36 DisableSimulation,
37 } 37 }
38 38
39 public enum CollisionOptions 39 public enum CollisionOptions
40 { 40 {
41 StaticObject = 1, 41 StaticObject = 1,
42 KinematicObject = 2, 42 KinematicObject = 2,
43 NoContactResponse = 4, 43 NoContactResponse = 4,
44 CustomMaterialCallback = 8,//this allows per-triangle material (friction/restitution) 44 CustomMaterialCallback = 8,//this allows per-triangle material (friction/restitution)
45 } 45 }
46 46
47 /// <summary> 47 /// <summary>
48 /// btCollisionObject can be used to manage collision detection objects. 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. 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. 50 /// They can be added to the btCollisionWorld.
51 /// </summary> 51 /// </summary>
52 public class CollisionObject 52 public class CollisionObject
53 { 53 {
54 protected Matrix _worldTransform; 54 protected Matrix _worldTransform;
55 private BroadphaseProxy _broadphase; 55 private BroadphaseProxy _broadphase;
56 private CollisionShape _collisionShape; 56 private CollisionShape _collisionShape;
57 57
58 //m_interpolationWorldTransform is used for CCD and interpolation 58 //m_interpolationWorldTransform is used for CCD and interpolation
59 //it can be either previous or future (predicted) transform 59 //it can be either previous or future (predicted) transform
60 private Matrix _interpolationWorldTransform; 60 private Matrix _interpolationWorldTransform;
61 61
62 private CollisionOptions _collisionFlags; 62 private CollisionOptions _collisionFlags;
63 63
64 private int _islandTag; 64 private int _islandTag;
65 private ActivationState _activationState; 65 private ActivationState _activationState;
66 private float _deactivationTime; 66 private float _deactivationTime;
67 67
68 private float _friction; 68 private float _friction;
69 private float _restitution; 69 private float _restitution;
70 70
71 //users can point to their objects, m_userPointer is not used by Bullet 71 //users can point to their objects, m_userPointer is not used by Bullet
72 private object _userData; 72 private object _userData;
73 73
74 //m_internalOwner one is used by optional Bullet high level interface 74 //m_internalOwner one is used by optional Bullet high level interface
75 private object _internalOwner; 75 private object _internalOwner;
76 76
77 //time of impact calculation 77 //time of impact calculation
78 private float _hitFraction; 78 private float _hitFraction;
79 79
80 //Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: 80 //Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
81 private float _ccdSweptSphereRadius; 81 private float _ccdSweptSphereRadius;
82 82
83 // Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionTreshold 83 // Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionTreshold
84 private float _ccdSquareMotionThreshold; 84 private float _ccdSquareMotionThreshold;
85 //those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities) 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) 86 //without destroying the continuous interpolated motion (which uses this interpolation velocities)
87 private Vector3 _interpolationLinearVelocity; 87 private Vector3 _interpolationLinearVelocity;
88 private Vector3 _interpolationAngularVelocity; 88 private Vector3 _interpolationAngularVelocity;
89 89
90 private int _companionID; 90 private int _companionID;
91 91
92 public CollisionObject() 92 public CollisionObject()
93 { 93 {
94 _activationState = ActivationState.Active; 94 _activationState = ActivationState.Active;
95 _hitFraction = 1; 95 _hitFraction = 1;
96 } 96 }
97 97
98 public bool IsStaticObject { get { return (_collisionFlags & CollisionOptions.StaticObject) != 0; } } 98 public bool IsStaticObject { get { return (_collisionFlags & CollisionOptions.StaticObject) != 0; } }
99 public bool IsKinematicObject { get { return (_collisionFlags & CollisionOptions.KinematicObject) != 0; } } 99 public bool IsKinematicObject { get { return (_collisionFlags & CollisionOptions.KinematicObject) != 0; } }
100 public bool IsStaticOrKinematicObject { get { return (_collisionFlags & (CollisionOptions.KinematicObject | CollisionOptions.StaticObject)) != 0; } } 100 public bool IsStaticOrKinematicObject { get { return (_collisionFlags & (CollisionOptions.KinematicObject | CollisionOptions.StaticObject)) != 0; } }
101 101
102 public bool HasContactResponse { get { return (_collisionFlags & CollisionOptions.NoContactResponse) == 0; } } 102 public bool HasContactResponse { get { return (_collisionFlags & CollisionOptions.NoContactResponse) == 0; } }
103 public bool MergesSimulationIslands 103 public bool MergesSimulationIslands
104 { 104 {
105 get 105 get
106 { 106 {
107 //static objects, kinematic and object without contact response don't merge islands 107 //static objects, kinematic and object without contact response don't merge islands
108 return (_collisionFlags & (CollisionOptions.StaticObject | CollisionOptions.KinematicObject | CollisionOptions.NoContactResponse)) == 0; 108 return (_collisionFlags & (CollisionOptions.StaticObject | CollisionOptions.KinematicObject | CollisionOptions.NoContactResponse)) == 0;
109 } 109 }
110 } 110 }
111 111
112 public ActivationState ActivationState 112 public ActivationState ActivationState
113 { 113 {
114 get { return _activationState; } 114 get { return _activationState; }
115 set 115 set
116 { 116 {
117 if ((_activationState != ActivationState.DisableDeactivation) && (_activationState != ActivationState.DisableSimulation)) 117 if ((_activationState != ActivationState.DisableDeactivation) && (_activationState != ActivationState.DisableSimulation))
118 _activationState = value; 118 _activationState = value;
119 } 119 }
120 } 120 }
121 121
122 public bool IsActive { get { return ((ActivationState != ActivationState.IslandSleeping) && (ActivationState != ActivationState.DisableSimulation)); } } 122 public bool IsActive { get { return ((ActivationState != ActivationState.IslandSleeping) && (ActivationState != ActivationState.DisableSimulation)); } }
123 public float Restitution { get { return _restitution; } set { _restitution = value; } } 123 public float Restitution { get { return _restitution; } set { _restitution = value; } }
124 public float Friction { get { return _friction; } set { _friction = value; } } 124 public float Friction { get { return _friction; } set { _friction = value; } }
125 public CollisionShape CollisionShape { get { return _collisionShape; } set { _collisionShape = value; } } 125 public CollisionShape CollisionShape { get { return _collisionShape; } set { _collisionShape = value; } }
126 public float DeactivationTime { get { return _deactivationTime; } set { _deactivationTime = value; } } 126 public float DeactivationTime { get { return _deactivationTime; } set { _deactivationTime = value; } }
127 public object Owner { get { return _internalOwner; } protected set { _internalOwner = value; } } 127 public object Owner { get { return _internalOwner; } protected set { _internalOwner = value; } }
128 public Matrix WorldTransform { get { return _worldTransform; } set { _worldTransform = value; } } 128 public Matrix WorldTransform { get { return _worldTransform; } set { _worldTransform = value; } }
129 public BroadphaseProxy Broadphase { get { return _broadphase; } set { _broadphase = value; } } 129 public BroadphaseProxy Broadphase { get { return _broadphase; } set { _broadphase = value; } }
130 public Matrix InterpolationWorldTransform { get { return _interpolationWorldTransform; } set { _interpolationWorldTransform = value; } } 130 public Matrix InterpolationWorldTransform { get { return _interpolationWorldTransform; } set { _interpolationWorldTransform = value; } }
131 public Vector3 InterpolationLinearVelocity { get { return _interpolationLinearVelocity; } protected set { _interpolationLinearVelocity = value; } } 131 public Vector3 InterpolationLinearVelocity { get { return _interpolationLinearVelocity; } protected set { _interpolationLinearVelocity = value; } }
132 public Vector3 InterpolationAngularVelocity { get { return _interpolationAngularVelocity; } protected set { _interpolationAngularVelocity = value; } } 132 public Vector3 InterpolationAngularVelocity { get { return _interpolationAngularVelocity; } protected set { _interpolationAngularVelocity = value; } }
133 public int IslandTag { get { return _islandTag; } set { _islandTag = value; } } 133 public int IslandTag { get { return _islandTag; } set { _islandTag = value; } }
134 public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } 134 public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } }
135 public CollisionOptions CollisionFlags { get { return _collisionFlags; } set { _collisionFlags = value; } } 135 public CollisionOptions CollisionFlags { get { return _collisionFlags; } set { _collisionFlags = value; } }
136 //Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm 136 //Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm
137 public float CcdSweptSphereRadius { get { return _ccdSweptSphereRadius; } set { _ccdSweptSphereRadius = value; } } 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 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; } } 139 public float CcdSquareMotionThreshold { get { return _ccdSquareMotionThreshold; } set { _ccdSquareMotionThreshold = value; } }
140 //users can point to their objects, userPointer is not used by Bullet 140 //users can point to their objects, userPointer is not used by Bullet
141 public object UserData { get { return _userData; } set { _userData = value; } } 141 public object UserData { get { return _userData; } set { _userData = value; } }
142 public int CompanionID { get { return _companionID; } set { _companionID = value; } } 142 public int CompanionID { get { return _companionID; } set { _companionID = value; } }
143 143
144 public void ForceActivationState(ActivationState newState) 144 public void ForceActivationState(ActivationState newState)
145 { 145 {
146 _activationState = newState; 146 _activationState = newState;
147 } 147 }
148 148
149 public void Activate() 149 public void Activate()
150 { 150 {
151 Activate(false); 151 Activate(false);
152 } 152 }
153 153
154 public void Activate(bool forceActivation) 154 public void Activate(bool forceActivation)
155 { 155 {
156 if (forceActivation || (_collisionFlags & (CollisionOptions.StaticObject | CollisionOptions.KinematicObject)) == 0) 156 if (forceActivation || (_collisionFlags & (CollisionOptions.StaticObject | CollisionOptions.KinematicObject)) == 0)
157 { 157 {
158 ActivationState = ActivationState.Active; 158 ActivationState = ActivationState.Active;
159 _deactivationTime = 0; 159 _deactivationTime = 0;
160 } 160 }
161 } 161 }
162 } 162 }
163} 163}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs
index 7a85731..6ba1c87 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs
@@ -1,48 +1,48 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public class CollisionPairCallback : IOverlapCallback 29 public class CollisionPairCallback : IOverlapCallback
30 { 30 {
31 private DispatcherInfo _dispatchInfo; 31 private DispatcherInfo _dispatchInfo;
32 private CollisionDispatcher _dispatcher; 32 private CollisionDispatcher _dispatcher;
33 33
34 public CollisionPairCallback(DispatcherInfo dispatchInfo, CollisionDispatcher dispatcher) 34 public CollisionPairCallback(DispatcherInfo dispatchInfo, CollisionDispatcher dispatcher)
35 { 35 {
36 _dispatchInfo = dispatchInfo; 36 _dispatchInfo = dispatchInfo;
37 _dispatcher = dispatcher; 37 _dispatcher = dispatcher;
38 } 38 }
39 39
40 #region IOverlapCallback Members 40 #region IOverlapCallback Members
41 public bool ProcessOverlap(ref BroadphasePair pair) 41 public bool ProcessOverlap(ref BroadphasePair pair)
42 { 42 {
43 _dispatcher.NearCallback(ref pair, _dispatcher, _dispatchInfo); 43 _dispatcher.NearCallback(ref pair, _dispatcher, _dispatchInfo);
44 return false; 44 return false;
45 } 45 }
46 #endregion 46 #endregion
47 } 47 }
48} 48}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs
index 968e219..cb520bb 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs
@@ -1,358 +1,358 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public class CollisionWorld 29 public class CollisionWorld
30 { 30 {
31 private List<CollisionObject> _collisionObjects = new List<CollisionObject>(); 31 private List<CollisionObject> _collisionObjects = new List<CollisionObject>();
32 private IDispatcher _dispatcher; 32 private IDispatcher _dispatcher;
33 private OverlappingPairCache _broadphasePairCache; 33 private OverlappingPairCache _broadphasePairCache;
34 private bool _ownsDispatcher; 34 private bool _ownsDispatcher;
35 private bool _ownsBroadphasePairCache; 35 private bool _ownsBroadphasePairCache;
36 private DispatcherInfo _dispatchInfo = new DispatcherInfo(); 36 private DispatcherInfo _dispatchInfo = new DispatcherInfo();
37 37
38 /// <summary> 38 /// <summary>
39 /// this constructor doesn't own the dispatcher and paircache/broadphase 39 /// this constructor doesn't own the dispatcher and paircache/broadphase
40 /// </summary> 40 /// </summary>
41 /// <param name="dispatcher"></param> 41 /// <param name="dispatcher"></param>
42 /// <param name="pairCache"></param> 42 /// <param name="pairCache"></param>
43 public CollisionWorld(IDispatcher dispatcher, OverlappingPairCache pairCache) 43 public CollisionWorld(IDispatcher dispatcher, OverlappingPairCache pairCache)
44 { 44 {
45 _dispatcher = dispatcher; 45 _dispatcher = dispatcher;
46 _broadphasePairCache = pairCache; 46 _broadphasePairCache = pairCache;
47 _ownsDispatcher = false; 47 _ownsDispatcher = false;
48 _ownsBroadphasePairCache = false; 48 _ownsBroadphasePairCache = false;
49 } 49 }
50 50
51 public DispatcherInfo DispatchInfo { get { return _dispatchInfo; } protected set { _dispatchInfo = value; } } 51 public DispatcherInfo DispatchInfo { get { return _dispatchInfo; } protected set { _dispatchInfo = value; } }
52 public List<CollisionObject> CollisionObjects { get { return _collisionObjects; } protected set { _collisionObjects = value; } } 52 public List<CollisionObject> CollisionObjects { get { return _collisionObjects; } protected set { _collisionObjects = value; } }
53 public IBroadphase Broadphase { get { return _broadphasePairCache; } } 53 public IBroadphase Broadphase { get { return _broadphasePairCache; } }
54 public OverlappingPairCache BroadphasePairCache { get { return _broadphasePairCache; } protected set { _broadphasePairCache = value; } } 54 public OverlappingPairCache BroadphasePairCache { get { return _broadphasePairCache; } protected set { _broadphasePairCache = value; } }
55 public IDispatcher Dispatcher { get { return _dispatcher; } protected set { _dispatcher = value; } } 55 public IDispatcher Dispatcher { get { return _dispatcher; } protected set { _dispatcher = value; } }
56 public int CollisionObjectsCount { get { return _collisionObjects.Count; } } 56 public int CollisionObjectsCount { get { return _collisionObjects.Count; } }
57 protected bool OwnsDispatcher { get { return _ownsDispatcher; } set { _ownsDispatcher = value; } } 57 protected bool OwnsDispatcher { get { return _ownsDispatcher; } set { _ownsDispatcher = value; } }
58 protected bool OwnsBroadphasePairCache { get { return _ownsBroadphasePairCache; } set { _ownsBroadphasePairCache = value; } } 58 protected bool OwnsBroadphasePairCache { get { return _ownsBroadphasePairCache; } set { _ownsBroadphasePairCache = value; } }
59 59
60 // rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback 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. 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) 62 public void RayTest(Vector3 rayFromWorld, Vector3 rayToWorld, RayResultCallback resultCallback)
63 { 63 {
64 Matrix rayFromTrans, rayToTrans; 64 Matrix rayFromTrans, rayToTrans;
65 65
66 rayFromTrans = Matrix.Identity; 66 rayFromTrans = Matrix.Identity;
67 rayFromTrans.Translation = rayFromWorld; 67 rayFromTrans.Translation = rayFromWorld;
68 68
69 rayToTrans = Matrix.Identity; 69 rayToTrans = Matrix.Identity;
70 rayToTrans.Translation = rayToWorld; 70 rayToTrans.Translation = rayToWorld;
71 71
72 // brute force go over all objects. Once there is a broadphase, use that, or 72 // brute force go over all objects. Once there is a broadphase, use that, or
73 // add a raycast against aabb first. 73 // add a raycast against aabb first.
74 74
75 foreach (CollisionObject collisionObject in _collisionObjects) 75 foreach (CollisionObject collisionObject in _collisionObjects)
76 { 76 {
77 //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); 77 //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
78 Vector3 collisionObjectAabbMin, collisionObjectAabbMax; 78 Vector3 collisionObjectAabbMin, collisionObjectAabbMax;
79 collisionObject.CollisionShape.GetAabb(collisionObject.WorldTransform, out collisionObjectAabbMin, out collisionObjectAabbMax); 79 collisionObject.CollisionShape.GetAabb(collisionObject.WorldTransform, out collisionObjectAabbMin, out collisionObjectAabbMax);
80 80
81 float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing 81 float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing
82 Vector3 hitNormal = new Vector3(); 82 Vector3 hitNormal = new Vector3();
83 83
84 //if (MathHelper.TestAabbAgainstAabb2(rayAabbMin, rayAabbMax, collisionObjectAabbMin, collisionObjectAabbMax)) 84 //if (MathHelper.TestAabbAgainstAabb2(rayAabbMin, rayAabbMax, collisionObjectAabbMin, collisionObjectAabbMax))
85 if (MathHelper.RayAabb(rayFromWorld, rayToWorld, collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal)) 85 if (MathHelper.RayAabb(rayFromWorld, rayToWorld, collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal))
86 { 86 {
87 RayTestSingle(rayFromTrans, rayToTrans, 87 RayTestSingle(rayFromTrans, rayToTrans,
88 collisionObject, collisionObject.CollisionShape, 88 collisionObject, collisionObject.CollisionShape,
89 collisionObject.WorldTransform, resultCallback); 89 collisionObject.WorldTransform, resultCallback);
90 90
91 } 91 }
92 } 92 }
93 } 93 }
94 94
95 // rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest. 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. 96 // In a future implementation, we consider moving the ray test as a virtual method in CollisionShape.
97 // This allows more customization. 97 // This allows more customization.
98 public static void RayTestSingle(Matrix rayFromTrans, Matrix rayToTrans, 98 public static void RayTestSingle(Matrix rayFromTrans, Matrix rayToTrans,
99 CollisionObject collisionObject, 99 CollisionObject collisionObject,
100 CollisionShape collisionShape, 100 CollisionShape collisionShape,
101 Matrix colObjWorldTransform, 101 Matrix colObjWorldTransform,
102 RayResultCallback resultCallback) 102 RayResultCallback resultCallback)
103 { 103 {
104 SphereShape pointShape=new SphereShape(0.0f); 104 SphereShape pointShape=new SphereShape(0.0f);
105 105
106 if (collisionShape.IsConvex) 106 if (collisionShape.IsConvex)
107 { 107 {
108 CastResult castResult = new CastResult(); 108 CastResult castResult = new CastResult();
109 castResult.Fraction = 1f;//?? 109 castResult.Fraction = 1f;//??
110 110
111 ConvexShape convexShape = collisionShape as ConvexShape; 111 ConvexShape convexShape = collisionShape as ConvexShape;
112 VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver(); 112 VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
113 SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, convexShape, simplexSolver); 113 SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, convexShape, simplexSolver);
114 //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); 114 //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
115 //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); 115 //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
116 116
117 if (convexCaster.CalcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult)) 117 if (convexCaster.CalcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult))
118 { 118 {
119 //add hit 119 //add hit
120 if (castResult.Normal.LengthSquared() > 0.0001f) 120 if (castResult.Normal.LengthSquared() > 0.0001f)
121 { 121 {
122 castResult.Normal.Normalize(); 122 castResult.Normal.Normalize();
123 if (castResult.Fraction < resultCallback.ClosestHitFraction) 123 if (castResult.Fraction < resultCallback.ClosestHitFraction)
124 { 124 {
125 125
126 CollisionWorld.LocalRayResult localRayResult = new LocalRayResult 126 CollisionWorld.LocalRayResult localRayResult = new LocalRayResult
127 ( 127 (
128 collisionObject, 128 collisionObject,
129 new LocalShapeInfo(), 129 new LocalShapeInfo(),
130 castResult.Normal, 130 castResult.Normal,
131 castResult.Fraction 131 castResult.Fraction
132 ); 132 );
133 133
134 resultCallback.AddSingleResult(localRayResult); 134 resultCallback.AddSingleResult(localRayResult);
135 } 135 }
136 } 136 }
137 } 137 }
138 else 138 else
139 { 139 {
140 if (collisionShape.IsConcave) 140 if (collisionShape.IsConcave)
141 { 141 {
142 142
143 TriangleMeshShape triangleMesh = collisionShape as TriangleMeshShape; 143 TriangleMeshShape triangleMesh = collisionShape as TriangleMeshShape;
144 144
145 Matrix worldTocollisionObject = MathHelper.InvertMatrix(colObjWorldTransform); 145 Matrix worldTocollisionObject = MathHelper.InvertMatrix(colObjWorldTransform);
146 146
147 Vector3 rayFromLocal = Vector3.TransformNormal(rayFromTrans.Translation, worldTocollisionObject); 147 Vector3 rayFromLocal = Vector3.TransformNormal(rayFromTrans.Translation, worldTocollisionObject);
148 Vector3 rayToLocal = Vector3.TransformNormal(rayToTrans.Translation, worldTocollisionObject); 148 Vector3 rayToLocal = Vector3.TransformNormal(rayToTrans.Translation, worldTocollisionObject);
149 149
150 BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh); 150 BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh);
151 rcb.HitFraction = resultCallback.ClosestHitFraction; 151 rcb.HitFraction = resultCallback.ClosestHitFraction;
152 152
153 Vector3 rayAabbMinLocal = rayFromLocal; 153 Vector3 rayAabbMinLocal = rayFromLocal;
154 MathHelper.SetMin(ref rayAabbMinLocal, rayToLocal); 154 MathHelper.SetMin(ref rayAabbMinLocal, rayToLocal);
155 Vector3 rayAabbMaxLocal = rayFromLocal; 155 Vector3 rayAabbMaxLocal = rayFromLocal;
156 MathHelper.SetMax(ref rayAabbMaxLocal, rayToLocal); 156 MathHelper.SetMax(ref rayAabbMaxLocal, rayToLocal);
157 157
158 triangleMesh.ProcessAllTriangles(rcb, rayAabbMinLocal, rayAabbMaxLocal); 158 triangleMesh.ProcessAllTriangles(rcb, rayAabbMinLocal, rayAabbMaxLocal);
159 } 159 }
160 else 160 else
161 { 161 {
162 //todo: use AABB tree or other BVH acceleration structure! 162 //todo: use AABB tree or other BVH acceleration structure!
163 if (collisionShape.IsCompound) 163 if (collisionShape.IsCompound)
164 { 164 {
165 CompoundShape compoundShape = collisionShape as CompoundShape; 165 CompoundShape compoundShape = collisionShape as CompoundShape;
166 for (int i = 0; i < compoundShape.ChildShapeCount; i++) 166 for (int i = 0; i < compoundShape.ChildShapeCount; i++)
167 { 167 {
168 Matrix childTrans = compoundShape.GetChildTransform(i); 168 Matrix childTrans = compoundShape.GetChildTransform(i);
169 CollisionShape childCollisionShape = compoundShape.GetChildShape(i); 169 CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
170 Matrix childWorldTrans = colObjWorldTransform * childTrans; 170 Matrix childWorldTrans = colObjWorldTransform * childTrans;
171 RayTestSingle(rayFromTrans, rayToTrans, 171 RayTestSingle(rayFromTrans, rayToTrans,
172 collisionObject, 172 collisionObject,
173 childCollisionShape, 173 childCollisionShape,
174 childWorldTrans, 174 childWorldTrans,
175 resultCallback); 175 resultCallback);
176 } 176 }
177 } 177 }
178 } 178 }
179 } 179 }
180 } 180 }
181 } 181 }
182 182
183 public void AddCollisionObject(CollisionObject collisionObject, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask) 183 public void AddCollisionObject(CollisionObject collisionObject, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask)
184 { 184 {
185 //check that the object isn't already added 185 //check that the object isn't already added
186 if (!_collisionObjects.Contains(collisionObject)) 186 if (!_collisionObjects.Contains(collisionObject))
187 { 187 {
188 _collisionObjects.Add(collisionObject); 188 _collisionObjects.Add(collisionObject);
189 189
190 //calculate new AABB 190 //calculate new AABB
191 Matrix trans = collisionObject.WorldTransform; 191 Matrix trans = collisionObject.WorldTransform;
192 192
193 Vector3 minAabb; 193 Vector3 minAabb;
194 Vector3 maxAabb; 194 Vector3 maxAabb;
195 collisionObject.CollisionShape.GetAabb(trans, out minAabb, out maxAabb); 195 collisionObject.CollisionShape.GetAabb(trans, out minAabb, out maxAabb);
196 196
197 BroadphaseNativeTypes type = collisionObject.CollisionShape.ShapeType; 197 BroadphaseNativeTypes type = collisionObject.CollisionShape.ShapeType;
198 collisionObject.Broadphase = Broadphase.CreateProxy( 198 collisionObject.Broadphase = Broadphase.CreateProxy(
199 minAabb, 199 minAabb,
200 maxAabb, 200 maxAabb,
201 type, 201 type,
202 collisionObject, 202 collisionObject,
203 collisionFilterGroup, 203 collisionFilterGroup,
204 collisionFilterMask 204 collisionFilterMask
205 ); 205 );
206 } 206 }
207 } 207 }
208 208
209 public void AddCollisionObject(CollisionObject collisionObject) 209 public void AddCollisionObject(CollisionObject collisionObject)
210 { 210 {
211 AddCollisionObject(collisionObject, BroadphaseProxy.CollisionFilterGroups.Default, BroadphaseProxy.CollisionFilterGroups.Default); 211 AddCollisionObject(collisionObject, BroadphaseProxy.CollisionFilterGroups.Default, BroadphaseProxy.CollisionFilterGroups.Default);
212 } 212 }
213 213
214 public void RemoveCollisionObject(CollisionObject collisionObject) 214 public void RemoveCollisionObject(CollisionObject collisionObject)
215 { 215 {
216 BroadphaseProxy bp = collisionObject.Broadphase; 216 BroadphaseProxy bp = collisionObject.Broadphase;
217 if (bp != null) 217 if (bp != null)
218 { 218 {
219 // 219 //
220 // only clear the cached algorithms 220 // only clear the cached algorithms
221 // 221 //
222 Broadphase.CleanProxyFromPairs(bp); 222 Broadphase.CleanProxyFromPairs(bp);
223 Broadphase.DestroyProxy(bp); 223 Broadphase.DestroyProxy(bp);
224 collisionObject.Broadphase = null; 224 collisionObject.Broadphase = null;
225 } 225 }
226 226
227 _collisionObjects.Remove(collisionObject); 227 _collisionObjects.Remove(collisionObject);
228 } 228 }
229 229
230 public virtual void PerformDiscreteCollisionDetection() 230 public virtual void PerformDiscreteCollisionDetection()
231 { 231 {
232 DispatcherInfo dispatchInfo = DispatchInfo; 232 DispatcherInfo dispatchInfo = DispatchInfo;
233 //update aabb (of all moved objects) 233 //update aabb (of all moved objects)
234 234
235 Vector3 aabbMin, aabbMax; 235 Vector3 aabbMin, aabbMax;
236 for (int i = 0; i < _collisionObjects.Count; i++) 236 for (int i = 0; i < _collisionObjects.Count; i++)
237 { 237 {
238 _collisionObjects[i].CollisionShape.GetAabb(_collisionObjects[i].WorldTransform, out aabbMin, out aabbMax); 238 _collisionObjects[i].CollisionShape.GetAabb(_collisionObjects[i].WorldTransform, out aabbMin, out aabbMax);
239 _broadphasePairCache.SetAabb(_collisionObjects[i].Broadphase, aabbMin, aabbMax); 239 _broadphasePairCache.SetAabb(_collisionObjects[i].Broadphase, aabbMin, aabbMax);
240 } 240 }
241 241
242 _broadphasePairCache.RefreshOverlappingPairs(); 242 _broadphasePairCache.RefreshOverlappingPairs();
243 243
244 IDispatcher dispatcher = Dispatcher; 244 IDispatcher dispatcher = Dispatcher;
245 if (dispatcher != null) 245 if (dispatcher != null)
246 dispatcher.DispatchAllCollisionPairs(_broadphasePairCache, dispatchInfo); 246 dispatcher.DispatchAllCollisionPairs(_broadphasePairCache, dispatchInfo);
247 } 247 }
248 248
249 public void Dispose(bool disposing) 249 public void Dispose(bool disposing)
250 { 250 {
251 if (disposing) 251 if (disposing)
252 { 252 {
253 //clean up remaining objects 253 //clean up remaining objects
254 foreach (CollisionObject collisionObject in _collisionObjects) 254 foreach (CollisionObject collisionObject in _collisionObjects)
255 { 255 {
256 BroadphaseProxy bp = collisionObject.Broadphase; 256 BroadphaseProxy bp = collisionObject.Broadphase;
257 if (bp != null) 257 if (bp != null)
258 { 258 {
259 // 259 //
260 // only clear the cached algorithms 260 // only clear the cached algorithms
261 // 261 //
262 Broadphase.CleanProxyFromPairs(bp); 262 Broadphase.CleanProxyFromPairs(bp);
263 Broadphase.DestroyProxy(bp); 263 Broadphase.DestroyProxy(bp);
264 } 264 }
265 } 265 }
266 } 266 }
267 } 267 }
268 268
269 /// <summary> 269 /// <summary>
270 /// LocalShapeInfo gives extra information for complex shapes 270 /// LocalShapeInfo gives extra information for complex shapes
271 /// Currently, only TriangleMeshShape is available, so it just contains triangleIndex and subpart 271 /// Currently, only TriangleMeshShape is available, so it just contains triangleIndex and subpart
272 /// </summary> 272 /// </summary>
273 public struct LocalShapeInfo 273 public struct LocalShapeInfo
274 { 274 {
275 private int _shapePart; 275 private int _shapePart;
276 private int _triangleIndex; 276 private int _triangleIndex;
277 277
278 public int ShapePart { get { return _shapePart; } set { _shapePart = value; } } 278 public int ShapePart { get { return _shapePart; } set { _shapePart = value; } }
279 public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } } 279 public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } }
280 } 280 }
281 281
282 public struct LocalRayResult 282 public struct LocalRayResult
283 { 283 {
284 private CollisionObject _collisionObject; 284 private CollisionObject _collisionObject;
285 private LocalShapeInfo _localShapeInfo; 285 private LocalShapeInfo _localShapeInfo;
286 private Vector3 _hitNormalLocal; 286 private Vector3 _hitNormalLocal;
287 private float _hitFraction; 287 private float _hitFraction;
288 288
289 public LocalRayResult(CollisionObject collisionObject, 289 public LocalRayResult(CollisionObject collisionObject,
290 LocalShapeInfo localShapeInfo, 290 LocalShapeInfo localShapeInfo,
291 Vector3 hitNormalLocal, 291 Vector3 hitNormalLocal,
292 float hitFraction) 292 float hitFraction)
293 { 293 {
294 _collisionObject = collisionObject; 294 _collisionObject = collisionObject;
295 _localShapeInfo = localShapeInfo; 295 _localShapeInfo = localShapeInfo;
296 _hitNormalLocal = hitNormalLocal; 296 _hitNormalLocal = hitNormalLocal;
297 _hitFraction = hitFraction; 297 _hitFraction = hitFraction;
298 } 298 }
299 299
300 public CollisionObject CollisionObject { get { return _collisionObject; } set { _collisionObject = value; } } 300 public CollisionObject CollisionObject { get { return _collisionObject; } set { _collisionObject = value; } }
301 public LocalShapeInfo LocalShapeInfo { get { return _localShapeInfo; } set { _localShapeInfo = value; } } 301 public LocalShapeInfo LocalShapeInfo { get { return _localShapeInfo; } set { _localShapeInfo = value; } }
302 public Vector3 HitNormalLocal { get { return _hitNormalLocal; } set { _hitNormalLocal = value; } } 302 public Vector3 HitNormalLocal { get { return _hitNormalLocal; } set { _hitNormalLocal = value; } }
303 public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } 303 public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } }
304 } 304 }
305 305
306 /// <summary> 306 /// <summary>
307 /// RayResultCallback is used to report new raycast results 307 /// RayResultCallback is used to report new raycast results
308 /// </summary> 308 /// </summary>
309 public abstract class RayResultCallback 309 public abstract class RayResultCallback
310 { 310 {
311 private float _closestHitFraction; 311 private float _closestHitFraction;
312 312
313 public RayResultCallback() 313 public RayResultCallback()
314 { 314 {
315 _closestHitFraction = 1; 315 _closestHitFraction = 1;
316 } 316 }
317 317
318 public float ClosestHitFraction { get { return _closestHitFraction; } set { _closestHitFraction = value; } } 318 public float ClosestHitFraction { get { return _closestHitFraction; } set { _closestHitFraction = value; } }
319 public bool HasHit { get { return _closestHitFraction < 1; } } 319 public bool HasHit { get { return _closestHitFraction < 1; } }
320 320
321 public abstract float AddSingleResult(LocalRayResult rayResult); 321 public abstract float AddSingleResult(LocalRayResult rayResult);
322 } 322 }
323 323
324 public class ClosestRayResultCallback : RayResultCallback 324 public class ClosestRayResultCallback : RayResultCallback
325 { 325 {
326 private Vector3 _rayFromWorld;//used to calculate hitPointWorld from hitFraction 326 private Vector3 _rayFromWorld;//used to calculate hitPointWorld from hitFraction
327 private Vector3 _rayToWorld; 327 private Vector3 _rayToWorld;
328 328
329 private Vector3 _hitNormalWorld; 329 private Vector3 _hitNormalWorld;
330 private Vector3 _hitPointWorld; 330 private Vector3 _hitPointWorld;
331 private CollisionObject _collisionObject; 331 private CollisionObject _collisionObject;
332 332
333 public ClosestRayResultCallback(Vector3 rayFromWorld, Vector3 rayToWorld) 333 public ClosestRayResultCallback(Vector3 rayFromWorld, Vector3 rayToWorld)
334 { 334 {
335 _rayFromWorld = rayFromWorld; 335 _rayFromWorld = rayFromWorld;
336 _rayToWorld = rayToWorld; 336 _rayToWorld = rayToWorld;
337 _collisionObject = null; 337 _collisionObject = null;
338 } 338 }
339 339
340 public Vector3 RayFromWorld { get { return _rayFromWorld; } set { _rayFromWorld = value; } } 340 public Vector3 RayFromWorld { get { return _rayFromWorld; } set { _rayFromWorld = value; } }
341 public Vector3 RayToWorld { get { return _rayToWorld; } set { _rayToWorld = value; } } 341 public Vector3 RayToWorld { get { return _rayToWorld; } set { _rayToWorld = value; } }
342 public Vector3 HitNormalWorld { get { return _hitNormalWorld; } set { _hitNormalWorld = value; } } 342 public Vector3 HitNormalWorld { get { return _hitNormalWorld; } set { _hitNormalWorld = value; } }
343 public Vector3 HitPointWorld { get { return _hitPointWorld; } set { _hitPointWorld = value; } } 343 public Vector3 HitPointWorld { get { return _hitPointWorld; } set { _hitPointWorld = value; } }
344 public CollisionObject CollisionObject { get { return _collisionObject; } set { _collisionObject = value; } } 344 public CollisionObject CollisionObject { get { return _collisionObject; } set { _collisionObject = value; } }
345 345
346 public override float AddSingleResult(LocalRayResult rayResult) 346 public override float AddSingleResult(LocalRayResult rayResult)
347 { 347 {
348 //caller already does the filter on the m_closestHitFraction 348 //caller already does the filter on the m_closestHitFraction
349 //assert(rayResult.m_hitFraction <= m_closestHitFraction); 349 //assert(rayResult.m_hitFraction <= m_closestHitFraction);
350 ClosestHitFraction = rayResult.HitFraction; 350 ClosestHitFraction = rayResult.HitFraction;
351 _collisionObject = rayResult.CollisionObject; 351 _collisionObject = rayResult.CollisionObject;
352 _hitNormalWorld = Vector3.TransformNormal(rayResult.HitNormalLocal, _collisionObject.WorldTransform); 352 _hitNormalWorld = Vector3.TransformNormal(rayResult.HitNormalLocal, _collisionObject.WorldTransform);
353 MathHelper.SetInterpolate3(_rayFromWorld, _rayToWorld, rayResult.HitFraction, ref _hitPointWorld); 353 MathHelper.SetInterpolate3(_rayFromWorld, _rayToWorld, rayResult.HitFraction, ref _hitPointWorld);
354 return rayResult.HitFraction; 354 return rayResult.HitFraction;
355 } 355 }
356 } 356 }
357 } 357 }
358} 358}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs
index 3fec7d0..6f2278a 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs
@@ -1,157 +1,157 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using System.Diagnostics; 25using System.Diagnostics;
26using MonoXnaCompactMaths; 26using MonoXnaCompactMaths;
27 27
28namespace XnaDevRu.BulletX 28namespace XnaDevRu.BulletX
29{ 29{
30 public class CompoundCollisionAlgorithm : CollisionAlgorithm 30 public class CompoundCollisionAlgorithm : CollisionAlgorithm
31 { 31 {
32 private List<CollisionAlgorithm> _childCollisionAlgorithms; 32 private List<CollisionAlgorithm> _childCollisionAlgorithms;
33 private bool _isSwapped; 33 private bool _isSwapped;
34 34
35 public CompoundCollisionAlgorithm( 35 public CompoundCollisionAlgorithm(
36 CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, 36 CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo,
37 CollisionObject bodyA, 37 CollisionObject bodyA,
38 CollisionObject bodyB, bool isSwapped) 38 CollisionObject bodyB, bool isSwapped)
39 : base(collisionAlgorithmConstructionInfo) 39 : base(collisionAlgorithmConstructionInfo)
40 { 40 {
41 //Begin 41 //Begin
42 _isSwapped = isSwapped; 42 _isSwapped = isSwapped;
43 43
44 CollisionObject collisionObject = isSwapped ? bodyB : bodyA; 44 CollisionObject collisionObject = isSwapped ? bodyB : bodyA;
45 CollisionObject otherObject = isSwapped ? bodyA : bodyB; 45 CollisionObject otherObject = isSwapped ? bodyA : bodyB;
46 46
47 BulletDebug.Assert(collisionObject.CollisionShape.IsCompound); 47 BulletDebug.Assert(collisionObject.CollisionShape.IsCompound);
48 48
49 CompoundShape compoundShape = collisionObject.CollisionShape as CompoundShape; 49 CompoundShape compoundShape = collisionObject.CollisionShape as CompoundShape;
50 int childrenNumber = compoundShape.ChildShapeCount; 50 int childrenNumber = compoundShape.ChildShapeCount;
51 int index = 0; 51 int index = 0;
52 52
53 _childCollisionAlgorithms = new List<CollisionAlgorithm>(childrenNumber); 53 _childCollisionAlgorithms = new List<CollisionAlgorithm>(childrenNumber);
54 54
55 for (index = 0; index < childrenNumber; index++) 55 for (index = 0; index < childrenNumber; index++)
56 { 56 {
57 CollisionShape childShape = compoundShape.GetChildShape(index); 57 CollisionShape childShape = compoundShape.GetChildShape(index);
58 CollisionShape orgShape = collisionObject.CollisionShape; 58 CollisionShape orgShape = collisionObject.CollisionShape;
59 59
60 collisionObject.CollisionShape = childShape; 60 collisionObject.CollisionShape = childShape;
61 _childCollisionAlgorithms[index] = collisionAlgorithmConstructionInfo.Dispatcher.FindAlgorithm(collisionObject, otherObject); 61 _childCollisionAlgorithms[index] = collisionAlgorithmConstructionInfo.Dispatcher.FindAlgorithm(collisionObject, otherObject);
62 collisionObject.CollisionShape = orgShape; 62 collisionObject.CollisionShape = orgShape;
63 } 63 }
64 } 64 }
65 65
66 public override void ProcessCollision( 66 public override void ProcessCollision(
67 CollisionObject bodyA, 67 CollisionObject bodyA,
68 CollisionObject bodyB, 68 CollisionObject bodyB,
69 DispatcherInfo dispatchInfo, ManifoldResult resultOut) 69 DispatcherInfo dispatchInfo, ManifoldResult resultOut)
70 { 70 {
71 //Begin 71 //Begin
72 72
73 CollisionObject collisionObject = _isSwapped ? bodyB : bodyB; 73 CollisionObject collisionObject = _isSwapped ? bodyB : bodyB;
74 CollisionObject otherObject = _isSwapped ? bodyA : bodyB; 74 CollisionObject otherObject = _isSwapped ? bodyA : bodyB;
75 75
76 //Debug.Assert(collisionObject.getCollisionShape().isCompound()); 76 //Debug.Assert(collisionObject.getCollisionShape().isCompound());
77 BulletDebug.Assert(collisionObject.CollisionShape.IsCompound); 77 BulletDebug.Assert(collisionObject.CollisionShape.IsCompound);
78 78
79 CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape; 79 CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape;
80 80
81 int childrenNumber = _childCollisionAlgorithms.Count; 81 int childrenNumber = _childCollisionAlgorithms.Count;
82 82
83 for (int i = 0; i < childrenNumber; i++) 83 for (int i = 0; i < childrenNumber; i++)
84 { 84 {
85 CompoundShape childShape = compoundShape.GetChildShape(i) as CompoundShape; 85 CompoundShape childShape = compoundShape.GetChildShape(i) as CompoundShape;
86 86
87 Matrix orgTransform = collisionObject.WorldTransform; 87 Matrix orgTransform = collisionObject.WorldTransform;
88 CollisionShape orgShape = collisionObject.CollisionShape; 88 CollisionShape orgShape = collisionObject.CollisionShape;
89 89
90 Matrix childTransform = compoundShape.GetChildTransform(i); 90 Matrix childTransform = compoundShape.GetChildTransform(i);
91 Matrix newChildWorld = orgTransform * childTransform; 91 Matrix newChildWorld = orgTransform * childTransform;
92 92
93 collisionObject.WorldTransform = newChildWorld; 93 collisionObject.WorldTransform = newChildWorld;
94 collisionObject.CollisionShape = childShape; 94 collisionObject.CollisionShape = childShape;
95 _childCollisionAlgorithms[i].ProcessCollision(collisionObject, otherObject, dispatchInfo, resultOut); 95 _childCollisionAlgorithms[i].ProcessCollision(collisionObject, otherObject, dispatchInfo, resultOut);
96 96
97 collisionObject.CollisionShape = orgShape; 97 collisionObject.CollisionShape = orgShape;
98 collisionObject.WorldTransform = orgTransform; 98 collisionObject.WorldTransform = orgTransform;
99 } 99 }
100 } 100 }
101 101
102 public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) 102 public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
103 { 103 {
104 CollisionObject collisionObject = _isSwapped ? bodyB : bodyA; 104 CollisionObject collisionObject = _isSwapped ? bodyB : bodyA;
105 CollisionObject otherObject = _isSwapped ? bodyA : bodyB; 105 CollisionObject otherObject = _isSwapped ? bodyA : bodyB;
106 106
107 BulletDebug.Assert(collisionObject.CollisionShape.IsCompound); 107 BulletDebug.Assert(collisionObject.CollisionShape.IsCompound);
108 108
109 CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape; 109 CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape;
110 110
111 float hitFraction = 1.0f; 111 float hitFraction = 1.0f;
112 112
113 for (int i = 0; i < _childCollisionAlgorithms.Count; i++) 113 for (int i = 0; i < _childCollisionAlgorithms.Count; i++)
114 { 114 {
115 CollisionShape childShape = compoundShape.GetChildShape(i); 115 CollisionShape childShape = compoundShape.GetChildShape(i);
116 116
117 Matrix orgTransform = collisionObject.WorldTransform; 117 Matrix orgTransform = collisionObject.WorldTransform;
118 CollisionShape orgShape = collisionObject.CollisionShape; 118 CollisionShape orgShape = collisionObject.CollisionShape;
119 119
120 Matrix childTransform = compoundShape.GetChildTransform(i); 120 Matrix childTransform = compoundShape.GetChildTransform(i);
121 Matrix newChildWorld = orgTransform * childTransform; 121 Matrix newChildWorld = orgTransform * childTransform;
122 collisionObject.WorldTransform = newChildWorld; 122 collisionObject.WorldTransform = newChildWorld;
123 123
124 collisionObject.CollisionShape = childShape; 124 collisionObject.CollisionShape = childShape;
125 float frac = _childCollisionAlgorithms[i].CalculateTimeOfImpact( 125 float frac = _childCollisionAlgorithms[i].CalculateTimeOfImpact(
126 collisionObject, otherObject, dispatchInfo, resultOut 126 collisionObject, otherObject, dispatchInfo, resultOut
127 ); 127 );
128 128
129 if (frac < hitFraction) 129 if (frac < hitFraction)
130 { 130 {
131 hitFraction = frac; 131 hitFraction = frac;
132 } 132 }
133 133
134 collisionObject.CollisionShape = orgShape; 134 collisionObject.CollisionShape = orgShape;
135 collisionObject.WorldTransform = orgTransform; 135 collisionObject.WorldTransform = orgTransform;
136 } 136 }
137 137
138 return hitFraction; 138 return hitFraction;
139 } 139 }
140 140
141 public class CreateFunc : CollisionAlgorithmCreateFunction 141 public class CreateFunc : CollisionAlgorithmCreateFunction
142 { 142 {
143 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) 143 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
144 { 144 {
145 return new CompoundCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, false); 145 return new CompoundCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, false);
146 } 146 }
147 }; 147 };
148 148
149 public class SwappedCreateFunc : CollisionAlgorithmCreateFunction 149 public class SwappedCreateFunc : CollisionAlgorithmCreateFunction
150 { 150 {
151 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) 151 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
152 { 152 {
153 return new CompoundCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, true); 153 return new CompoundCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, true);
154 } 154 }
155 }; 155 };
156 } 156 }
157} 157}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs
index c4949f0..39438e4 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs
@@ -1,189 +1,189 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public class ConvexConcaveCollisionAlgorithm : CollisionAlgorithm 29 public class ConvexConcaveCollisionAlgorithm : CollisionAlgorithm
30 { 30 {
31 private bool _isSwapped; 31 private bool _isSwapped;
32 private ConvexTriangleCallback _convexTriangleCallback; 32 private ConvexTriangleCallback _convexTriangleCallback;
33 33
34 public ConvexConcaveCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped) 34 public ConvexConcaveCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped)
35 : base(collisionAlgorithmConstructionInfo) 35 : base(collisionAlgorithmConstructionInfo)
36 { 36 {
37 _isSwapped = isSwapped; 37 _isSwapped = isSwapped;
38 _convexTriangleCallback = new ConvexTriangleCallback(collisionAlgorithmConstructionInfo.Dispatcher, bodyA, bodyB, isSwapped); 38 _convexTriangleCallback = new ConvexTriangleCallback(collisionAlgorithmConstructionInfo.Dispatcher, bodyA, bodyB, isSwapped);
39 } 39 }
40 40
41 public void ClearCache() 41 public void ClearCache()
42 { 42 {
43 _convexTriangleCallback.ClearCache(); 43 _convexTriangleCallback.ClearCache();
44 } 44 }
45 45
46 public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) 46 public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
47 { 47 {
48 CollisionObject convexBody = _isSwapped ? bodyB : bodyA; 48 CollisionObject convexBody = _isSwapped ? bodyB : bodyA;
49 CollisionObject triBody = _isSwapped ? bodyA : bodyB; 49 CollisionObject triBody = _isSwapped ? bodyA : bodyB;
50 50
51 if (triBody.CollisionShape.IsConcave) 51 if (triBody.CollisionShape.IsConcave)
52 { 52 {
53 CollisionObject triOb = triBody; 53 CollisionObject triOb = triBody;
54 ConcaveShape concaveShape = triOb.CollisionShape as ConcaveShape; 54 ConcaveShape concaveShape = triOb.CollisionShape as ConcaveShape;
55 55
56 if (convexBody.CollisionShape.IsConvex) 56 if (convexBody.CollisionShape.IsConvex)
57 { 57 {
58 float collisionMarginTriangle = concaveShape.Margin; 58 float collisionMarginTriangle = concaveShape.Margin;
59 59
60 resultOut.SetPersistentManifold(_convexTriangleCallback.Manifold); 60 resultOut.SetPersistentManifold(_convexTriangleCallback.Manifold);
61 _convexTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut); 61 _convexTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut);
62 62
63 //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here. 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); 64 //m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr);
65 65
66 _convexTriangleCallback.Manifold.SetBodies(convexBody, triBody); 66 _convexTriangleCallback.Manifold.SetBodies(convexBody, triBody);
67 concaveShape.ProcessAllTriangles(_convexTriangleCallback, _convexTriangleCallback.AabbMin, _convexTriangleCallback.AabbMax); 67 concaveShape.ProcessAllTriangles(_convexTriangleCallback, _convexTriangleCallback.AabbMin, _convexTriangleCallback.AabbMax);
68 } 68 }
69 } 69 }
70 } 70 }
71 71
72 public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) 72 public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
73 { 73 {
74 CollisionObject convexbody = _isSwapped ? bodyB : bodyA; 74 CollisionObject convexbody = _isSwapped ? bodyB : bodyA;
75 CollisionObject triBody = _isSwapped ? bodyA : bodyB; 75 CollisionObject triBody = _isSwapped ? bodyA : bodyB;
76 76
77 77
78 //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast) 78 //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
79 79
80 //only perform CCD above a certain threshold, this prevents blocking on the long run 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... 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(); 82 float squareMot0 = (convexbody.InterpolationWorldTransform.Translation - convexbody.WorldTransform.Translation).LengthSquared();
83 if (squareMot0 < convexbody.CcdSquareMotionThreshold) 83 if (squareMot0 < convexbody.CcdSquareMotionThreshold)
84 { 84 {
85 return 1; 85 return 1;
86 } 86 }
87 87
88 Matrix triInv = MathHelper.InvertMatrix(triBody.WorldTransform); 88 Matrix triInv = MathHelper.InvertMatrix(triBody.WorldTransform);
89 Matrix convexFromLocal = triInv * convexbody.WorldTransform; 89 Matrix convexFromLocal = triInv * convexbody.WorldTransform;
90 Matrix convexToLocal = triInv * convexbody.InterpolationWorldTransform; 90 Matrix convexToLocal = triInv * convexbody.InterpolationWorldTransform;
91 91
92 if (triBody.CollisionShape.IsConcave) 92 if (triBody.CollisionShape.IsConcave)
93 { 93 {
94 Vector3 rayAabbMin = convexFromLocal.Translation; 94 Vector3 rayAabbMin = convexFromLocal.Translation;
95 MathHelper.SetMin(ref rayAabbMin, convexToLocal.Translation); 95 MathHelper.SetMin(ref rayAabbMin, convexToLocal.Translation);
96 Vector3 rayAabbMax = convexFromLocal.Translation; 96 Vector3 rayAabbMax = convexFromLocal.Translation;
97 MathHelper.SetMax(ref rayAabbMax, convexToLocal.Translation); 97 MathHelper.SetMax(ref rayAabbMax, convexToLocal.Translation);
98 float ccdRadius0 = convexbody.CcdSweptSphereRadius; 98 float ccdRadius0 = convexbody.CcdSweptSphereRadius;
99 rayAabbMin -= new Vector3(ccdRadius0, ccdRadius0, ccdRadius0); 99 rayAabbMin -= new Vector3(ccdRadius0, ccdRadius0, ccdRadius0);
100 rayAabbMax += new Vector3(ccdRadius0, ccdRadius0, ccdRadius0); 100 rayAabbMax += new Vector3(ccdRadius0, ccdRadius0, ccdRadius0);
101 101
102 float curHitFraction = 1f; //is this available? 102 float curHitFraction = 1f; //is this available?
103 LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(convexFromLocal, convexToLocal, 103 LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(convexFromLocal, convexToLocal,
104 convexbody.CcdSweptSphereRadius, curHitFraction); 104 convexbody.CcdSweptSphereRadius, curHitFraction);
105 105
106 raycastCallback.HitFraction = convexbody.HitFraction; 106 raycastCallback.HitFraction = convexbody.HitFraction;
107 107
108 CollisionObject concavebody = triBody; 108 CollisionObject concavebody = triBody;
109 109
110 ConcaveShape triangleMesh = concavebody.CollisionShape as ConcaveShape; 110 ConcaveShape triangleMesh = concavebody.CollisionShape as ConcaveShape;
111 111
112 if (triangleMesh != null) 112 if (triangleMesh != null)
113 { 113 {
114 triangleMesh.ProcessAllTriangles(raycastCallback, rayAabbMin, rayAabbMax); 114 triangleMesh.ProcessAllTriangles(raycastCallback, rayAabbMin, rayAabbMax);
115 } 115 }
116 116
117 if (raycastCallback.HitFraction < convexbody.HitFraction) 117 if (raycastCallback.HitFraction < convexbody.HitFraction)
118 { 118 {
119 convexbody.HitFraction = raycastCallback.HitFraction; 119 convexbody.HitFraction = raycastCallback.HitFraction;
120 return raycastCallback.HitFraction; 120 return raycastCallback.HitFraction;
121 } 121 }
122 } 122 }
123 123
124 return 1; 124 return 1;
125 } 125 }
126 126
127 public class CreateFunc : CollisionAlgorithmCreateFunction 127 public class CreateFunc : CollisionAlgorithmCreateFunction
128 { 128 {
129 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) 129 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
130 { 130 {
131 return new ConvexConcaveCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, false); 131 return new ConvexConcaveCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, false);
132 } 132 }
133 } 133 }
134 134
135 public class SwappedCreateFunc : CollisionAlgorithmCreateFunction 135 public class SwappedCreateFunc : CollisionAlgorithmCreateFunction
136 { 136 {
137 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) 137 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
138 { 138 {
139 return new ConvexConcaveCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, true); 139 return new ConvexConcaveCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, true);
140 } 140 }
141 } 141 }
142 142
143 private class LocalTriangleSphereCastCallback : ITriangleCallback 143 private class LocalTriangleSphereCastCallback : ITriangleCallback
144 { 144 {
145 private Matrix _ccdSphereFromTrans; 145 private Matrix _ccdSphereFromTrans;
146 private Matrix _ccdSphereToTrans; 146 private Matrix _ccdSphereToTrans;
147 private Matrix _meshTransform; 147 private Matrix _meshTransform;
148 148
149 private float _ccdSphereRadius; 149 private float _ccdSphereRadius;
150 private float _hitFraction; 150 private float _hitFraction;
151 151
152 public LocalTriangleSphereCastCallback(Matrix from, Matrix to, float ccdSphereRadius, float hitFraction) 152 public LocalTriangleSphereCastCallback(Matrix from, Matrix to, float ccdSphereRadius, float hitFraction)
153 { 153 {
154 _ccdSphereFromTrans = from; 154 _ccdSphereFromTrans = from;
155 _ccdSphereToTrans = to; 155 _ccdSphereToTrans = to;
156 _ccdSphereRadius = ccdSphereRadius; 156 _ccdSphereRadius = ccdSphereRadius;
157 _hitFraction = hitFraction; 157 _hitFraction = hitFraction;
158 } 158 }
159 159
160 public Matrix CcdSphereFromTrans { get { return _ccdSphereFromTrans; } set { _ccdSphereFromTrans = value; } } 160 public Matrix CcdSphereFromTrans { get { return _ccdSphereFromTrans; } set { _ccdSphereFromTrans = value; } }
161 public Matrix CcdSphereToTrans { get { return _ccdSphereToTrans; } set { _ccdSphereToTrans = value; } } 161 public Matrix CcdSphereToTrans { get { return _ccdSphereToTrans; } set { _ccdSphereToTrans = value; } }
162 public Matrix MeshTransform { get { return _meshTransform; } set { _meshTransform = value; } } 162 public Matrix MeshTransform { get { return _meshTransform; } set { _meshTransform = value; } }
163 public float CcdSphereRadius { get { return _ccdSphereRadius; } set { _ccdSphereRadius = value; } } 163 public float CcdSphereRadius { get { return _ccdSphereRadius; } set { _ccdSphereRadius = value; } }
164 public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } 164 public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } }
165 165
166 public void ProcessTriangle(Vector3[] triangle, int partId, int triangleIndex) 166 public void ProcessTriangle(Vector3[] triangle, int partId, int triangleIndex)
167 { 167 {
168 //do a swept sphere for now 168 //do a swept sphere for now
169 Matrix ident = Matrix.Identity; 169 Matrix ident = Matrix.Identity;
170 CastResult castResult = new CastResult(); 170 CastResult castResult = new CastResult();
171 castResult.Fraction = _hitFraction; 171 castResult.Fraction = _hitFraction;
172 SphereShape pointShape = new SphereShape(_ccdSphereRadius); 172 SphereShape pointShape = new SphereShape(_ccdSphereRadius);
173 TriangleShape triShape = new TriangleShape(triangle[0], triangle[1], triangle[2]); 173 TriangleShape triShape = new TriangleShape(triangle[0], triangle[1], triangle[2]);
174 VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver(); 174 VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
175 SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, triShape, simplexSolver); 175 SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, triShape, simplexSolver);
176 //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); 176 //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
177 //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); 177 //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
178 //local space? 178 //local space?
179 179
180 if (convexCaster.CalcTimeOfImpact(_ccdSphereFromTrans, _ccdSphereToTrans, 180 if (convexCaster.CalcTimeOfImpact(_ccdSphereFromTrans, _ccdSphereToTrans,
181 ident, ident, castResult)) 181 ident, ident, castResult))
182 { 182 {
183 if (_hitFraction > castResult.Fraction) 183 if (_hitFraction > castResult.Fraction)
184 _hitFraction = castResult.Fraction; 184 _hitFraction = castResult.Fraction;
185 } 185 }
186 } 186 }
187 } 187 }
188 } 188 }
189} 189}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs
index 8fa4837..01c8fe0 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs
@@ -1,193 +1,193 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25 25
26namespace XnaDevRu.BulletX 26namespace XnaDevRu.BulletX
27{ 27{
28 public class ConvexConvexAlgorithm : CollisionAlgorithm, IDisposable 28 public class ConvexConvexAlgorithm : CollisionAlgorithm, IDisposable
29 { 29 {
30 private const bool DisableCcd = false; 30 private const bool DisableCcd = false;
31 private GjkPairDetector _gjkPairDetector; 31 private GjkPairDetector _gjkPairDetector;
32 private bool _ownManifold; 32 private bool _ownManifold;
33 private PersistentManifold _manifold; 33 private PersistentManifold _manifold;
34 private bool _lowLevelOfDetail; 34 private bool _lowLevelOfDetail;
35 35
36 public ConvexConvexAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver) 36 public ConvexConvexAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
37 : base(collisionAlgorithmConstructionInfo) 37 : base(collisionAlgorithmConstructionInfo)
38 { 38 {
39 _gjkPairDetector = new GjkPairDetector(null, null, simplexSolver, penetrationDepthSolver); 39 _gjkPairDetector = new GjkPairDetector(null, null, simplexSolver, penetrationDepthSolver);
40 _ownManifold = false; 40 _ownManifold = false;
41 _manifold = manifold; 41 _manifold = manifold;
42 _lowLevelOfDetail = false; 42 _lowLevelOfDetail = false;
43 } 43 }
44 44
45 public bool LowLevelOfDetail { get { return _lowLevelOfDetail; } set { _lowLevelOfDetail = value; } } 45 public bool LowLevelOfDetail { get { return _lowLevelOfDetail; } set { _lowLevelOfDetail = value; } }
46 public bool OwnManifold { get { return _ownManifold; } set { _ownManifold = value; } } 46 public bool OwnManifold { get { return _ownManifold; } set { _ownManifold = value; } }
47 public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } } 47 public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } }
48 48
49 public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) 49 public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
50 { 50 {
51 if (_manifold == null) 51 if (_manifold == null)
52 { 52 {
53 //swapped? 53 //swapped?
54 _manifold = Dispatcher.GetNewManifold(bodyA, bodyB); 54 _manifold = Dispatcher.GetNewManifold(bodyA, bodyB);
55 _ownManifold = true; 55 _ownManifold = true;
56 } 56 }
57 resultOut.SetPersistentManifold(_manifold); 57 resultOut.SetPersistentManifold(_manifold);
58 58
59 ConvexShape min0 = bodyA.CollisionShape as ConvexShape; 59 ConvexShape min0 = bodyA.CollisionShape as ConvexShape;
60 ConvexShape min1 = bodyB.CollisionShape as ConvexShape; 60 ConvexShape min1 = bodyB.CollisionShape as ConvexShape;
61 61
62 GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); 62 GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
63 63
64 //TODO: if (dispatchInfo.m_useContinuous) 64 //TODO: if (dispatchInfo.m_useContinuous)
65 _gjkPairDetector.setMinkowskiA(min0); 65 _gjkPairDetector.setMinkowskiA(min0);
66 _gjkPairDetector.setMinkowskiB(min1); 66 _gjkPairDetector.setMinkowskiB(min1);
67 input.MaximumDistanceSquared = min0.Margin + min1.Margin + PersistentManifold.ContactBreakingThreshold; 67 input.MaximumDistanceSquared = min0.Margin + min1.Margin + PersistentManifold.ContactBreakingThreshold;
68 input.MaximumDistanceSquared *= input.MaximumDistanceSquared; 68 input.MaximumDistanceSquared *= input.MaximumDistanceSquared;
69 69
70 // input.m_maximumDistanceSquared = 1e30f; 70 // input.m_maximumDistanceSquared = 1e30f;
71 71
72 input.TransformA = bodyA.WorldTransform; 72 input.TransformA = bodyA.WorldTransform;
73 input.TransformB = bodyB.WorldTransform; 73 input.TransformB = bodyB.WorldTransform;
74 74
75 _gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.DebugDraw); 75 _gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.DebugDraw);
76 } 76 }
77 77
78 public override float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) 78 public override float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
79 { 79 {
80 //Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold 80 //Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
81 81
82 //Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold 82 //Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
83 //col0->m_worldTransform, 83 //col0->m_worldTransform,
84 float resultFraction = 1f; 84 float resultFraction = 1f;
85 85
86 float squareMotA = (colA.InterpolationWorldTransform.Translation - colA.WorldTransform.Translation).LengthSquared(); 86 float squareMotA = (colA.InterpolationWorldTransform.Translation - colA.WorldTransform.Translation).LengthSquared();
87 float squareMotB = (colB.InterpolationWorldTransform.Translation - colB.WorldTransform.Translation).LengthSquared(); 87 float squareMotB = (colB.InterpolationWorldTransform.Translation - colB.WorldTransform.Translation).LengthSquared();
88 88
89 if (squareMotA < colA.CcdSquareMotionThreshold && 89 if (squareMotA < colA.CcdSquareMotionThreshold &&
90 squareMotB < colB.CcdSquareMotionThreshold) 90 squareMotB < colB.CcdSquareMotionThreshold)
91 return resultFraction; 91 return resultFraction;
92 92
93 if (DisableCcd) 93 if (DisableCcd)
94 return 1f; 94 return 1f;
95 95
96 //An adhoc way of testing the Continuous Collision Detection algorithms 96 //An adhoc way of testing the Continuous Collision Detection algorithms
97 //One object is approximated as a sphere, to simplify things 97 //One object is approximated as a sphere, to simplify things
98 //Starting in penetration should report no time of impact 98 //Starting in penetration should report no time of impact
99 //For proper CCD, better accuracy and handling of 'allowed' penetration should be added 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) 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 101
102 // Convex0 against sphere for Convex1 102 // Convex0 against sphere for Convex1
103 { 103 {
104 ConvexShape convexA = colA.CollisionShape as ConvexShape; 104 ConvexShape convexA = colA.CollisionShape as ConvexShape;
105 105
106 SphereShape sphereB = new SphereShape(colB.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation 106 SphereShape sphereB = new SphereShape(colB.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
107 CastResult result = new CastResult(); 107 CastResult result = new CastResult();
108 VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver(); 108 VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
109 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); 109 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
110 //Simplification, one object is simplified as a sphere 110 //Simplification, one object is simplified as a sphere
111 GjkConvexCast ccdB = new GjkConvexCast(convexA, sphereB, voronoiSimplex); 111 GjkConvexCast ccdB = new GjkConvexCast(convexA, sphereB, voronoiSimplex);
112 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); 112 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
113 if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform, 113 if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform,
114 colB.WorldTransform, colB.InterpolationWorldTransform, result)) 114 colB.WorldTransform, colB.InterpolationWorldTransform, result))
115 { 115 {
116 //store result.m_fraction in both bodies 116 //store result.m_fraction in both bodies
117 if (colA.HitFraction > result.Fraction) 117 if (colA.HitFraction > result.Fraction)
118 colA.HitFraction = result.Fraction; 118 colA.HitFraction = result.Fraction;
119 119
120 if (colB.HitFraction > result.Fraction) 120 if (colB.HitFraction > result.Fraction)
121 colB.HitFraction = result.Fraction; 121 colB.HitFraction = result.Fraction;
122 122
123 if (resultFraction > result.Fraction) 123 if (resultFraction > result.Fraction)
124 resultFraction = result.Fraction; 124 resultFraction = result.Fraction;
125 } 125 }
126 } 126 }
127 127
128 // Sphere (for convex0) against Convex1 128 // Sphere (for convex0) against Convex1
129 { 129 {
130 ConvexShape convexB = colB.CollisionShape as ConvexShape; 130 ConvexShape convexB = colB.CollisionShape as ConvexShape;
131 131
132 SphereShape sphereA = new SphereShape(colA.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation 132 SphereShape sphereA = new SphereShape(colA.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
133 CastResult result = new CastResult(); 133 CastResult result = new CastResult();
134 VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver(); 134 VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
135 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); 135 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
136 ///Simplification, one object is simplified as a sphere 136 ///Simplification, one object is simplified as a sphere
137 GjkConvexCast ccdB = new GjkConvexCast(sphereA, convexB, voronoiSimplex); 137 GjkConvexCast ccdB = new GjkConvexCast(sphereA, convexB, voronoiSimplex);
138 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); 138 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
139 if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform, 139 if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform,
140 colB.WorldTransform, colB.InterpolationWorldTransform, result)) 140 colB.WorldTransform, colB.InterpolationWorldTransform, result))
141 { 141 {
142 //store result.m_fraction in both bodies 142 //store result.m_fraction in both bodies
143 if (colA.HitFraction > result.Fraction) 143 if (colA.HitFraction > result.Fraction)
144 colA.HitFraction = result.Fraction; 144 colA.HitFraction = result.Fraction;
145 145
146 if (colB.HitFraction > result.Fraction) 146 if (colB.HitFraction > result.Fraction)
147 colB.HitFraction = result.Fraction; 147 colB.HitFraction = result.Fraction;
148 148
149 if (resultFraction > result.Fraction) 149 if (resultFraction > result.Fraction)
150 resultFraction = result.Fraction; 150 resultFraction = result.Fraction;
151 } 151 }
152 } 152 }
153 return resultFraction; 153 return resultFraction;
154 } 154 }
155 155
156 public class CreateFunc : CollisionAlgorithmCreateFunction 156 public class CreateFunc : CollisionAlgorithmCreateFunction
157 { 157 {
158 private IConvexPenetrationDepthSolver _penetrationDepthSolver; 158 private IConvexPenetrationDepthSolver _penetrationDepthSolver;
159 private ISimplexSolver _simplexSolver; 159 private ISimplexSolver _simplexSolver;
160 //private bool _ownsSolvers; 160 //private bool _ownsSolvers;
161 161
162 public CreateFunc() 162 public CreateFunc()
163 { 163 {
164 //_ownsSolvers = true; 164 //_ownsSolvers = true;
165 _simplexSolver = new VoronoiSimplexSolver(); 165 _simplexSolver = new VoronoiSimplexSolver();
166 _penetrationDepthSolver = new GjkEpaPenetrationDepthSolver(); 166 _penetrationDepthSolver = new GjkEpaPenetrationDepthSolver();
167 } 167 }
168 168
169 public CreateFunc(ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver) 169 public CreateFunc(ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
170 { 170 {
171 //_ownsSolvers = false; 171 //_ownsSolvers = false;
172 _simplexSolver = simplexSolver; 172 _simplexSolver = simplexSolver;
173 _penetrationDepthSolver = penetrationDepthSolver; 173 _penetrationDepthSolver = penetrationDepthSolver;
174 } 174 }
175 175
176 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) 176 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
177 { 177 {
178 return new ConvexConvexAlgorithm(collisionAlgorithmConstructionInfo.Manifold, collisionAlgorithmConstructionInfo, bodyA, bodyB, _simplexSolver, _penetrationDepthSolver); 178 return new ConvexConvexAlgorithm(collisionAlgorithmConstructionInfo.Manifold, collisionAlgorithmConstructionInfo, bodyA, bodyB, _simplexSolver, _penetrationDepthSolver);
179 } 179 }
180 } 180 }
181 181
182 #region IDisposable Members 182 #region IDisposable Members
183 public void Dispose() 183 public void Dispose()
184 { 184 {
185 if (_ownManifold) 185 if (_ownManifold)
186 { 186 {
187 if (_manifold != null) 187 if (_manifold != null)
188 Dispatcher.ReleaseManifold(_manifold); 188 Dispatcher.ReleaseManifold(_manifold);
189 } 189 }
190 } 190 }
191 #endregion 191 #endregion
192 } 192 }
193} 193}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs
index 5355817..ba21c72 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs
@@ -1,130 +1,130 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public class ConvexTriangleCallback : ITriangleCallback, IDisposable 29 public class ConvexTriangleCallback : ITriangleCallback, IDisposable
30 { 30 {
31 private CollisionObject _convexBody; 31 private CollisionObject _convexBody;
32 private CollisionObject _triBody; 32 private CollisionObject _triBody;
33 33
34 private Vector3 _aabbMin; 34 private Vector3 _aabbMin;
35 private Vector3 _aabbMax; 35 private Vector3 _aabbMax;
36 36
37 private ManifoldResult _resultOut; 37 private ManifoldResult _resultOut;
38 38
39 private IDispatcher _dispatcher; 39 private IDispatcher _dispatcher;
40 private DispatcherInfo _dispatchInfo; 40 private DispatcherInfo _dispatchInfo;
41 private float _collisionMarginTriangle; 41 private float _collisionMarginTriangle;
42 42
43 private int _triangleCount; 43 private int _triangleCount;
44 44
45 private PersistentManifold _manifold; 45 private PersistentManifold _manifold;
46 46
47 public ConvexTriangleCallback(IDispatcher dispatcher, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped) 47 public ConvexTriangleCallback(IDispatcher dispatcher, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped)
48 { 48 {
49 _dispatcher = dispatcher; 49 _dispatcher = dispatcher;
50 _dispatchInfo = null; 50 _dispatchInfo = null;
51 _convexBody = isSwapped ? bodyB : bodyA; 51 _convexBody = isSwapped ? bodyB : bodyA;
52 _triBody = isSwapped ? bodyA : bodyB; 52 _triBody = isSwapped ? bodyA : bodyB;
53 53
54 // create the manifold from the dispatcher 'manifold pool' 54 // create the manifold from the dispatcher 'manifold pool'
55 _manifold = _dispatcher.GetNewManifold(_convexBody, _triBody); 55 _manifold = _dispatcher.GetNewManifold(_convexBody, _triBody);
56 ClearCache(); 56 ClearCache();
57 } 57 }
58 58
59 public Vector3 AabbMin { get { return _aabbMin; } } 59 public Vector3 AabbMin { get { return _aabbMin; } }
60 public Vector3 AabbMax { get { return _aabbMax; } } 60 public Vector3 AabbMax { get { return _aabbMax; } }
61 public int TriangleCount { get { return _triangleCount; } set { _triangleCount = value; } } 61 public int TriangleCount { get { return _triangleCount; } set { _triangleCount = value; } }
62 public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } } 62 public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } }
63 63
64 public void SetTimeStepAndCounters(float collisionMarginTriangle, DispatcherInfo dispatchInfo, ManifoldResult resultOut) 64 public void SetTimeStepAndCounters(float collisionMarginTriangle, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
65 { 65 {
66 _dispatchInfo = dispatchInfo; 66 _dispatchInfo = dispatchInfo;
67 _collisionMarginTriangle = collisionMarginTriangle; 67 _collisionMarginTriangle = collisionMarginTriangle;
68 _resultOut = resultOut; 68 _resultOut = resultOut;
69 69
70 //recalc aabbs 70 //recalc aabbs
71 Matrix convexInTriangleSpace = MathHelper.InvertMatrix(_triBody.WorldTransform) * _convexBody.WorldTransform; 71 Matrix convexInTriangleSpace = MathHelper.InvertMatrix(_triBody.WorldTransform) * _convexBody.WorldTransform;
72 CollisionShape convexShape = _convexBody.CollisionShape; 72 CollisionShape convexShape = _convexBody.CollisionShape;
73 //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape); 73 //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
74 convexShape.GetAabb(convexInTriangleSpace, out _aabbMin, out _aabbMax); 74 convexShape.GetAabb(convexInTriangleSpace, out _aabbMin, out _aabbMax);
75 float extraMargin = collisionMarginTriangle; 75 float extraMargin = collisionMarginTriangle;
76 Vector3 extra = new Vector3(extraMargin, extraMargin, extraMargin); 76 Vector3 extra = new Vector3(extraMargin, extraMargin, extraMargin);
77 77
78 _aabbMax += extra; 78 _aabbMax += extra;
79 _aabbMin -= extra; 79 _aabbMin -= extra;
80 } 80 }
81 81
82 public void ClearCache() 82 public void ClearCache()
83 { 83 {
84 _dispatcher.ClearManifold(_manifold); 84 _dispatcher.ClearManifold(_manifold);
85 } 85 }
86 86
87 #region ITriangleCallback Members 87 #region ITriangleCallback Members
88 public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) 88 public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex)
89 { 89 {
90 //aabb filter is already applied! 90 //aabb filter is already applied!
91 CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo = new CollisionAlgorithmConstructionInfo(); 91 CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo = new CollisionAlgorithmConstructionInfo();
92 collisionAlgorithmConstructionInfo.Dispatcher = _dispatcher; 92 collisionAlgorithmConstructionInfo.Dispatcher = _dispatcher;
93 93
94 CollisionObject collisionObject = _triBody; 94 CollisionObject collisionObject = _triBody;
95 95
96 //debug drawing of the overlapping triangles 96 //debug drawing of the overlapping triangles
97 /*if (m_dispatchInfoPtr && m_dispatchInfoPtr.m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0) 97 /*if (m_dispatchInfoPtr && m_dispatchInfoPtr.m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0)
98 { 98 {
99 Vector3 color = new Vector3(255, 255, 0); 99 Vector3 color = new Vector3(255, 255, 0);
100 btTransform & tr = ob->WorldTransform; 100 btTransform & tr = ob->WorldTransform;
101 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]), tr(triangle[1]), color); 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); 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); 103 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]), tr(triangle[0]), color);
104 }*/ 104 }*/
105 105
106 if (_convexBody.CollisionShape.IsConvex) 106 if (_convexBody.CollisionShape.IsConvex)
107 { 107 {
108 TriangleShape triangleShape = new TriangleShape(triangle[0], triangle[1], triangle[2]); 108 TriangleShape triangleShape = new TriangleShape(triangle[0], triangle[1], triangle[2]);
109 triangleShape.Margin=_collisionMarginTriangle; 109 triangleShape.Margin=_collisionMarginTriangle;
110 110
111 CollisionShape tempShape = collisionObject.CollisionShape; 111 CollisionShape tempShape = collisionObject.CollisionShape;
112 collisionObject.CollisionShape = triangleShape; 112 collisionObject.CollisionShape = triangleShape;
113 113
114 CollisionAlgorithm collisionAlgorithm = collisionAlgorithmConstructionInfo.Dispatcher.FindAlgorithm(_convexBody, _triBody, _manifold); 114 CollisionAlgorithm collisionAlgorithm = collisionAlgorithmConstructionInfo.Dispatcher.FindAlgorithm(_convexBody, _triBody, _manifold);
115 115
116 _resultOut.SetShapeIdentifiers(-1, -1, partID, triangleIndex); 116 _resultOut.SetShapeIdentifiers(-1, -1, partID, triangleIndex);
117 collisionAlgorithm.ProcessCollision(_convexBody, _triBody, _dispatchInfo, _resultOut); 117 collisionAlgorithm.ProcessCollision(_convexBody, _triBody, _dispatchInfo, _resultOut);
118 collisionObject.CollisionShape = tempShape; 118 collisionObject.CollisionShape = tempShape;
119 } 119 }
120 } 120 }
121 #endregion 121 #endregion
122 #region IDisposable Members 122 #region IDisposable Members
123 public void Dispose() 123 public void Dispose()
124 { 124 {
125 ClearCache(); 125 ClearCache();
126 _dispatcher.ReleaseManifold(_manifold); 126 _dispatcher.ReleaseManifold(_manifold);
127 } 127 }
128 #endregion 128 #endregion
129 } 129 }
130} 130}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs
index 13cb3d9..5995e2c 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs
@@ -1,52 +1,52 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25 25
26namespace XnaDevRu.BulletX 26namespace XnaDevRu.BulletX
27{ 27{
28 /// <summary> 28 /// <summary>
29 /// EmptyAlgorithm is a stub for unsupported collision pairs. 29 /// EmptyAlgorithm is a stub for unsupported collision pairs.
30 /// The dispatcher can dispatch a persistent btEmptyAlgorithm to avoid a search every frame. 30 /// The dispatcher can dispatch a persistent btEmptyAlgorithm to avoid a search every frame.
31 /// </summary> 31 /// </summary>
32 public class EmptyAlgorithm : CollisionAlgorithm 32 public class EmptyAlgorithm : CollisionAlgorithm
33 { 33 {
34 public EmptyAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) 34 public EmptyAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo)
35 : base(collisionAlgorithmConstructionInfo) { } 35 : base(collisionAlgorithmConstructionInfo) { }
36 36
37 public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { } 37 public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { }
38 38
39 public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) 39 public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
40 { 40 {
41 return 1f; 41 return 1f;
42 } 42 }
43 43
44 public class CreateFunc : CollisionAlgorithmCreateFunction 44 public class CreateFunc : CollisionAlgorithmCreateFunction
45 { 45 {
46 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) 46 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
47 { 47 {
48 return new EmptyAlgorithm(collisionAlgorithmConstructionInfo); 48 return new EmptyAlgorithm(collisionAlgorithmConstructionInfo);
49 } 49 }
50 }; 50 };
51 } 51 }
52} 52}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs
index 63f37ea..69143a1 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs
@@ -1,147 +1,147 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public delegate bool ContactAddedCallback(ManifoldPoint contactPoint, CollisionObject collisionObjectA, int partIdA, int indexA, CollisionObject collisionObjectB, int partIdB, int indexB); 29 public delegate bool ContactAddedCallback(ManifoldPoint contactPoint, CollisionObject collisionObjectA, int partIdA, int indexA, CollisionObject collisionObjectB, int partIdB, int indexB);
30 30
31 public class ManifoldResult : DiscreteCollisionDetectorInterface.Result 31 public class ManifoldResult : DiscreteCollisionDetectorInterface.Result
32 { 32 {
33 private PersistentManifold _manifold; 33 private PersistentManifold _manifold;
34 private static ContactAddedCallback _contactAddedCallback = null; 34 private static ContactAddedCallback _contactAddedCallback = null;
35 35
36 //we need this for compounds 36 //we need this for compounds
37 private Matrix _rootTransA; 37 private Matrix _rootTransA;
38 private Matrix _rootTransB; 38 private Matrix _rootTransB;
39 39
40 private CollisionObject _bodyA; 40 private CollisionObject _bodyA;
41 private CollisionObject _bodyB; 41 private CollisionObject _bodyB;
42 private int _partIdA; 42 private int _partIdA;
43 private int _partIdB; 43 private int _partIdB;
44 private int _indexA; 44 private int _indexA;
45 private int _indexB; 45 private int _indexB;
46 46
47 public ManifoldResult() 47 public ManifoldResult()
48 { 48 {
49 } 49 }
50 50
51 public ManifoldResult(CollisionObject bodyA, CollisionObject bodyB) 51 public ManifoldResult(CollisionObject bodyA, CollisionObject bodyB)
52 { 52 {
53 _bodyA = bodyA; 53 _bodyA = bodyA;
54 _bodyB = bodyB; 54 _bodyB = bodyB;
55 _rootTransA = bodyA.WorldTransform; 55 _rootTransA = bodyA.WorldTransform;
56 _rootTransB = bodyB.WorldTransform; 56 _rootTransB = bodyB.WorldTransform;
57 } 57 }
58 58
59 public static ContactAddedCallback ContactAddedCallback { get { return _contactAddedCallback; } set { _contactAddedCallback = value; } } 59 public static ContactAddedCallback ContactAddedCallback { get { return _contactAddedCallback; } set { _contactAddedCallback = value; } }
60 60
61 public void SetPersistentManifold(PersistentManifold manifold) 61 public void SetPersistentManifold(PersistentManifold manifold)
62 { 62 {
63 _manifold = manifold; 63 _manifold = manifold;
64 } 64 }
65 65
66 public override void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB) 66 public override void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB)
67 { 67 {
68 _partIdA = partIdA; 68 _partIdA = partIdA;
69 _partIdB = partIdB; 69 _partIdB = partIdB;
70 _indexA = indexA; 70 _indexA = indexA;
71 _indexB = indexB; 71 _indexB = indexB;
72 } 72 }
73 73
74 public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) 74 public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth)
75 { 75 {
76 if (_manifold == null) 76 if (_manifold == null)
77 throw new BulletException("Manifold Pointer is null."); 77 throw new BulletException("Manifold Pointer is null.");
78 78
79 //order in manifold needs to match 79 //order in manifold needs to match
80 80
81 if (depth > PersistentManifold.ContactBreakingThreshold) 81 if (depth > PersistentManifold.ContactBreakingThreshold)
82 return; 82 return;
83 83
84 bool isSwapped = _manifold.BodyA != _bodyA; 84 bool isSwapped = _manifold.BodyA != _bodyA;
85 85
86 Vector3 pointA = pointInWorld + normalOnBInWorld * depth; 86 Vector3 pointA = pointInWorld + normalOnBInWorld * depth;
87 Vector3 localA; 87 Vector3 localA;
88 Vector3 localB; 88 Vector3 localB;
89 89
90 if (isSwapped) 90 if (isSwapped)
91 { 91 {
92 localA = MathHelper.InvXForm(_rootTransB, pointA); 92 localA = MathHelper.InvXForm(_rootTransB, pointA);
93 localB = MathHelper.InvXForm(_rootTransA, pointInWorld); 93 localB = MathHelper.InvXForm(_rootTransA, pointInWorld);
94 } 94 }
95 else 95 else
96 { 96 {
97 localA = MathHelper.InvXForm(_rootTransA, pointA); 97 localA = MathHelper.InvXForm(_rootTransA, pointA);
98 localB = MathHelper.InvXForm(_rootTransB, pointInWorld); 98 localB = MathHelper.InvXForm(_rootTransB, pointInWorld);
99 } 99 }
100 100
101 ManifoldPoint newPt = new ManifoldPoint(localA, localB, normalOnBInWorld, depth); 101 ManifoldPoint newPt = new ManifoldPoint(localA, localB, normalOnBInWorld, depth);
102 102
103 int insertIndex = _manifold.GetCacheEntry(newPt); 103 int insertIndex = _manifold.GetCacheEntry(newPt);
104 104
105 newPt.CombinedFriction = CalculateCombinedFriction(_bodyA, _bodyB); 105 newPt.CombinedFriction = CalculateCombinedFriction(_bodyA, _bodyB);
106 newPt.CombinedRestitution = CalculateCombinedRestitution(_bodyA, _bodyB); 106 newPt.CombinedRestitution = CalculateCombinedRestitution(_bodyA, _bodyB);
107 107
108 //User can override friction and/or restitution 108 //User can override friction and/or restitution
109 if (_contactAddedCallback != null && 109 if (_contactAddedCallback != null &&
110 //and if either of the two bodies requires custom material 110 //and if either of the two bodies requires custom material
111 ((_bodyA.CollisionFlags & CollisionOptions.CustomMaterialCallback) != 0 || 111 ((_bodyA.CollisionFlags & CollisionOptions.CustomMaterialCallback) != 0 ||
112 (_bodyB.CollisionFlags & CollisionOptions.CustomMaterialCallback) != 0)) 112 (_bodyB.CollisionFlags & CollisionOptions.CustomMaterialCallback) != 0))
113 { 113 {
114 //experimental feature info, for per-triangle material etc. 114 //experimental feature info, for per-triangle material etc.
115 CollisionObject obj0 = isSwapped ? _bodyB : _bodyA; 115 CollisionObject obj0 = isSwapped ? _bodyB : _bodyA;
116 CollisionObject obj1 = isSwapped ? _bodyA : _bodyB; 116 CollisionObject obj1 = isSwapped ? _bodyA : _bodyB;
117 _contactAddedCallback(newPt, obj0, _partIdA, _indexA, obj1, _partIdB, _indexB); 117 _contactAddedCallback(newPt, obj0, _partIdA, _indexA, obj1, _partIdB, _indexB);
118 } 118 }
119 119
120 if (insertIndex >= 0) 120 if (insertIndex >= 0)
121 { 121 {
122 _manifold.ReplaceContactPoint(newPt, insertIndex); 122 _manifold.ReplaceContactPoint(newPt, insertIndex);
123 } 123 }
124 else 124 else
125 { 125 {
126 _manifold.AddManifoldPoint(newPt); 126 _manifold.AddManifoldPoint(newPt);
127 } 127 }
128 } 128 }
129 129
130 private float CalculateCombinedFriction(CollisionObject bodyA, CollisionObject bodyB) 130 private float CalculateCombinedFriction(CollisionObject bodyA, CollisionObject bodyB)
131 { 131 {
132 float friction = bodyA.Friction * bodyB.Friction; 132 float friction = bodyA.Friction * bodyB.Friction;
133 133
134 float MaxFriction = 10; 134 float MaxFriction = 10;
135 if (friction < -MaxFriction) 135 if (friction < -MaxFriction)
136 friction = -MaxFriction; 136 friction = -MaxFriction;
137 if (friction > MaxFriction) 137 if (friction > MaxFriction)
138 friction = MaxFriction; 138 friction = MaxFriction;
139 return friction; 139 return friction;
140 } 140 }
141 141
142 private float CalculateCombinedRestitution(CollisionObject bodyA, CollisionObject bodyB) 142 private float CalculateCombinedRestitution(CollisionObject bodyA, CollisionObject bodyB)
143 { 143 {
144 return bodyA.Restitution * bodyB.Restitution; 144 return bodyA.Restitution * bodyB.Restitution;
145 } 145 }
146 } 146 }
147} 147}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs
index 957843b..39eb53a 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs
@@ -1,304 +1,304 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public class SimulationIslandManager 29 public class SimulationIslandManager
30 { 30 {
31 private UnionFind _unionFind = new UnionFind(); 31 private UnionFind _unionFind = new UnionFind();
32 32
33 public void InitUnionFind(int n) 33 public void InitUnionFind(int n)
34 { 34 {
35 _unionFind.Reset(n); 35 _unionFind.Reset(n);
36 } 36 }
37 37
38 public UnionFind UnionFind { get { return _unionFind; } } 38 public UnionFind UnionFind { get { return _unionFind; } }
39 39
40 public virtual void UpdateActivationState(CollisionWorld world, IDispatcher dispatcher) 40 public virtual void UpdateActivationState(CollisionWorld world, IDispatcher dispatcher)
41 { 41 {
42 InitUnionFind(world.CollisionObjectsCount); 42 InitUnionFind(world.CollisionObjectsCount);
43 43
44 // put the index into m_controllers into m_tag 44 // put the index into m_controllers into m_tag
45 int index = 0; 45 int index = 0;
46 for (int i = 0; i < world.CollisionObjects.Count; i++) 46 for (int i = 0; i < world.CollisionObjects.Count; i++)
47 { 47 {
48 world.CollisionObjects[i].IslandTag = index; 48 world.CollisionObjects[i].IslandTag = index;
49 world.CollisionObjects[i].HitFraction = 1; 49 world.CollisionObjects[i].HitFraction = 1;
50 world.CollisionObjects[i].CompanionID = -1; 50 world.CollisionObjects[i].CompanionID = -1;
51 index++; 51 index++;
52 } 52 }
53 // do the union find 53 // do the union find
54 FindUnions(dispatcher); 54 FindUnions(dispatcher);
55 } 55 }
56 56
57 public virtual void StoreIslandActivationState(CollisionWorld world) 57 public virtual void StoreIslandActivationState(CollisionWorld world)
58 { 58 {
59 // put the islandId ('find' value) into m_tag 59 // put the islandId ('find' value) into m_tag
60 int index = 0; 60 int index = 0;
61 for (int i = 0; i < world.CollisionObjects.Count; i++) 61 for (int i = 0; i < world.CollisionObjects.Count; i++)
62 { 62 {
63 if (world.CollisionObjects[i].MergesSimulationIslands) 63 if (world.CollisionObjects[i].MergesSimulationIslands)
64 { 64 {
65 world.CollisionObjects[i].IslandTag = _unionFind.Find(index); 65 world.CollisionObjects[i].IslandTag = _unionFind.Find(index);
66 world.CollisionObjects[i].CompanionID = -1; 66 world.CollisionObjects[i].CompanionID = -1;
67 } 67 }
68 else 68 else
69 { 69 {
70 world.CollisionObjects[i].IslandTag = -1; 70 world.CollisionObjects[i].IslandTag = -1;
71 world.CollisionObjects[i].CompanionID = -2; 71 world.CollisionObjects[i].CompanionID = -2;
72 } 72 }
73 index++; 73 index++;
74 } 74 }
75 } 75 }
76 76
77 public void FindUnions(IDispatcher dispatcher) 77 public void FindUnions(IDispatcher dispatcher)
78 { 78 {
79 for (int i = 0; i < dispatcher.ManifoldCount; i++) 79 for (int i = 0; i < dispatcher.ManifoldCount; i++)
80 { 80 {
81 PersistentManifold manifold = dispatcher.GetManifoldByIndex(i); 81 PersistentManifold manifold = dispatcher.GetManifoldByIndex(i);
82 //static objects (invmass 0.f) don't merge ! 82 //static objects (invmass 0.f) don't merge !
83 83
84 CollisionObject colObjA = manifold.BodyA as CollisionObject; 84 CollisionObject colObjA = manifold.BodyA as CollisionObject;
85 CollisionObject colObjB = manifold.BodyB as CollisionObject; 85 CollisionObject colObjB = manifold.BodyB as CollisionObject;
86 86
87 if (((colObjA != null) && (colObjA.MergesSimulationIslands)) && 87 if (((colObjA != null) && (colObjA.MergesSimulationIslands)) &&
88 ((colObjB != null) && (colObjB.MergesSimulationIslands))) 88 ((colObjB != null) && (colObjB.MergesSimulationIslands)))
89 { 89 {
90 _unionFind.Unite(colObjA.IslandTag, colObjB.IslandTag); 90 _unionFind.Unite(colObjA.IslandTag, colObjB.IslandTag);
91 } 91 }
92 } 92 }
93 } 93 }
94 94
95 public void BuildAndProcessIslands(IDispatcher dispatcher, List<CollisionObject> collisionObjects, IIslandCallback callback) 95 public void BuildAndProcessIslands(IDispatcher dispatcher, List<CollisionObject> collisionObjects, IIslandCallback callback)
96 { 96 {
97 //we are going to sort the unionfind array, and store the element id in the size 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 98 //afterwards, we clean unionfind, to make sure no-one uses it anymore
99 UnionFind.SortIslands(); 99 UnionFind.SortIslands();
100 int numElem = UnionFind.ElementCount; 100 int numElem = UnionFind.ElementCount;
101 101
102 int endIslandIndex = 1; 102 int endIslandIndex = 1;
103 int startIslandIndex; 103 int startIslandIndex;
104 104
105 //update the sleeping state for bodies, if all are sleeping 105 //update the sleeping state for bodies, if all are sleeping
106 for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) 106 for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
107 { 107 {
108 int islandId = UnionFind[startIslandIndex].ID; 108 int islandId = UnionFind[startIslandIndex].ID;
109 for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++) 109 for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++)
110 { 110 {
111 } 111 }
112 112
113 //int numSleeping = 0; 113 //int numSleeping = 0;
114 114
115 bool allSleeping = true; 115 bool allSleeping = true;
116 116
117 int idx; 117 int idx;
118 for (idx = startIslandIndex; idx < endIslandIndex; idx++) 118 for (idx = startIslandIndex; idx < endIslandIndex; idx++)
119 { 119 {
120 int i = UnionFind[idx].Size; 120 int i = UnionFind[idx].Size;
121 121
122 CollisionObject colObjA = collisionObjects[i]; 122 CollisionObject colObjA = collisionObjects[i];
123 if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1)) 123 if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1))
124 { 124 {
125 Console.WriteLine("error in island management"); 125 Console.WriteLine("error in island management");
126 } 126 }
127 127
128 BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1)); 128 BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1));
129 if (colObjA.IslandTag == islandId) 129 if (colObjA.IslandTag == islandId)
130 { 130 {
131 if (colObjA.ActivationState == ActivationState.Active) 131 if (colObjA.ActivationState == ActivationState.Active)
132 { 132 {
133 allSleeping = false; 133 allSleeping = false;
134 } 134 }
135 if (colObjA.ActivationState == ActivationState.DisableDeactivation) 135 if (colObjA.ActivationState == ActivationState.DisableDeactivation)
136 { 136 {
137 allSleeping = false; 137 allSleeping = false;
138 } 138 }
139 } 139 }
140 } 140 }
141 141
142 142
143 if (allSleeping) 143 if (allSleeping)
144 { 144 {
145 for (idx = startIslandIndex; idx < endIslandIndex; idx++) 145 for (idx = startIslandIndex; idx < endIslandIndex; idx++)
146 { 146 {
147 int i = UnionFind[idx].Size; 147 int i = UnionFind[idx].Size;
148 CollisionObject colObjA = collisionObjects[i]; 148 CollisionObject colObjA = collisionObjects[i];
149 if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1)) 149 if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1))
150 { 150 {
151 Console.WriteLine("error in island management"); 151 Console.WriteLine("error in island management");
152 } 152 }
153 153
154 BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1)); 154 BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1));
155 155
156 if (colObjA.IslandTag == islandId) 156 if (colObjA.IslandTag == islandId)
157 { 157 {
158 colObjA.ActivationState =ActivationState.IslandSleeping; 158 colObjA.ActivationState =ActivationState.IslandSleeping;
159 } 159 }
160 } 160 }
161 } 161 }
162 else 162 else
163 { 163 {
164 for (idx = startIslandIndex; idx < endIslandIndex; idx++) 164 for (idx = startIslandIndex; idx < endIslandIndex; idx++)
165 { 165 {
166 int i = UnionFind[idx].Size; 166 int i = UnionFind[idx].Size;
167 167
168 CollisionObject colObjA = collisionObjects[i]; 168 CollisionObject colObjA = collisionObjects[i];
169 if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1)) 169 if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1))
170 { 170 {
171 Console.WriteLine("error in island management"); 171 Console.WriteLine("error in island management");
172 } 172 }
173 173
174 BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1)); 174 BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1));
175 175
176 if (colObjA.IslandTag == islandId) 176 if (colObjA.IslandTag == islandId)
177 { 177 {
178 if (colObjA.ActivationState == ActivationState.IslandSleeping) 178 if (colObjA.ActivationState == ActivationState.IslandSleeping)
179 { 179 {
180 colObjA.ActivationState = ActivationState.WantsDeactivation; 180 colObjA.ActivationState = ActivationState.WantsDeactivation;
181 } 181 }
182 } 182 }
183 } 183 }
184 } 184 }
185 } 185 }
186 186
187 //int maxNumManifolds = dispatcher.ManifoldCount; 187 //int maxNumManifolds = dispatcher.ManifoldCount;
188 List<PersistentManifold> islandmanifold = new List<PersistentManifold>(dispatcher.ManifoldCount); 188 List<PersistentManifold> islandmanifold = new List<PersistentManifold>(dispatcher.ManifoldCount);
189 189
190 for (int i = 0; i < dispatcher.ManifoldCount; i++) 190 for (int i = 0; i < dispatcher.ManifoldCount; i++)
191 { 191 {
192 PersistentManifold manifold = dispatcher.GetManifoldByIndex(i); 192 PersistentManifold manifold = dispatcher.GetManifoldByIndex(i);
193 193
194 CollisionObject colObjA = manifold.BodyA as CollisionObject; 194 CollisionObject colObjA = manifold.BodyA as CollisionObject;
195 CollisionObject colObjB = manifold.BodyB as CollisionObject; 195 CollisionObject colObjB = manifold.BodyB as CollisionObject;
196 196
197 //todo: check sleeping conditions! 197 //todo: check sleeping conditions!
198 if (((colObjA != null) && colObjA.ActivationState != ActivationState.IslandSleeping) || 198 if (((colObjA != null) && colObjA.ActivationState != ActivationState.IslandSleeping) ||
199 ((colObjB != null) && colObjB.ActivationState != ActivationState.IslandSleeping)) 199 ((colObjB != null) && colObjB.ActivationState != ActivationState.IslandSleeping))
200 { 200 {
201 201
202 //kinematic objects don't merge islands, but wake up all connected objects 202 //kinematic objects don't merge islands, but wake up all connected objects
203 if (colObjA.IsStaticOrKinematicObject && colObjA.ActivationState != ActivationState.IslandSleeping) 203 if (colObjA.IsStaticOrKinematicObject && colObjA.ActivationState != ActivationState.IslandSleeping)
204 { 204 {
205 colObjB.Activate(); 205 colObjB.Activate();
206 } 206 }
207 if (colObjB.IsStaticOrKinematicObject && colObjB.ActivationState != ActivationState.IslandSleeping) 207 if (colObjB.IsStaticOrKinematicObject && colObjB.ActivationState != ActivationState.IslandSleeping)
208 { 208 {
209 colObjA.Activate(); 209 colObjA.Activate();
210 } 210 }
211 211
212 //filtering for response 212 //filtering for response
213 if (dispatcher.NeedsResponse(colObjA, colObjB)) 213 if (dispatcher.NeedsResponse(colObjA, colObjB))
214 islandmanifold.Add(manifold); 214 islandmanifold.Add(manifold);
215 } 215 }
216 } 216 }
217 217
218 int numManifolds = islandmanifold.Count; 218 int numManifolds = islandmanifold.Count;
219 219
220 // Sort manifolds, based on islands 220 // Sort manifolds, based on islands
221 // Sort the vector using predicate and std::sort 221 // Sort the vector using predicate and std::sort
222 islandmanifold.Sort(new Comparison<PersistentManifold>(PersistentManifoldSortPredicate)); 222 islandmanifold.Sort(new Comparison<PersistentManifold>(PersistentManifoldSortPredicate));
223 223
224 //now process all active islands (sets of manifolds for now) 224 //now process all active islands (sets of manifolds for now)
225 int startManifoldIndex = 0; 225 int startManifoldIndex = 0;
226 int endManifoldIndex = 1; 226 int endManifoldIndex = 1;
227 227
228 List<CollisionObject> islandBodies = new List<CollisionObject>(); 228 List<CollisionObject> islandBodies = new List<CollisionObject>();
229 229
230 for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) 230 for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
231 { 231 {
232 int islandId = UnionFind[startIslandIndex].ID; 232 int islandId = UnionFind[startIslandIndex].ID;
233 bool islandSleeping = false; 233 bool islandSleeping = false;
234 for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++) 234 for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++)
235 { 235 {
236 int i = UnionFind[endIslandIndex].Size; 236 int i = UnionFind[endIslandIndex].Size;
237 CollisionObject colObjA = collisionObjects[i]; 237 CollisionObject colObjA = collisionObjects[i];
238 islandBodies.Add(colObjA); 238 islandBodies.Add(colObjA);
239 if (!colObjA.IsActive) 239 if (!colObjA.IsActive)
240 islandSleeping = true; 240 islandSleeping = true;
241 } 241 }
242 242
243 //find the accompanying contact manifold for this islandId 243 //find the accompanying contact manifold for this islandId
244 int numIslandManifolds = 0; 244 int numIslandManifolds = 0;
245 List<PersistentManifold> startManifold = new List<PersistentManifold>(numIslandManifolds); 245 List<PersistentManifold> startManifold = new List<PersistentManifold>(numIslandManifolds);
246 246
247 if (startManifoldIndex < numManifolds) 247 if (startManifoldIndex < numManifolds)
248 { 248 {
249 int curIslandID = GetIslandId(islandmanifold[startManifoldIndex]); 249 int curIslandID = GetIslandId(islandmanifold[startManifoldIndex]);
250 if (curIslandID == islandId) 250 if (curIslandID == islandId)
251 { 251 {
252 for (int k = startManifoldIndex; k < islandmanifold.Count; k++) 252 for (int k = startManifoldIndex; k < islandmanifold.Count; k++)
253 { 253 {
254 startManifold.Add(islandmanifold[k]); 254 startManifold.Add(islandmanifold[k]);
255 } 255 }
256 for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == GetIslandId(islandmanifold[endManifoldIndex])); endManifoldIndex++) { } 256 for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == GetIslandId(islandmanifold[endManifoldIndex])); endManifoldIndex++) { }
257 257
258 // Process the actual simulation, only if not sleeping/deactivated 258 // Process the actual simulation, only if not sleeping/deactivated
259 numIslandManifolds = endManifoldIndex - startManifoldIndex; 259 numIslandManifolds = endManifoldIndex - startManifoldIndex;
260 } 260 }
261 } 261 }
262 262
263 if (!islandSleeping) 263 if (!islandSleeping)
264 { 264 {
265 callback.ProcessIsland(islandBodies, startManifold, numIslandManifolds, islandId); 265 callback.ProcessIsland(islandBodies, startManifold, numIslandManifolds, islandId);
266 } 266 }
267 267
268 if (numIslandManifolds != 0) 268 if (numIslandManifolds != 0)
269 { 269 {
270 startManifoldIndex = endManifoldIndex; 270 startManifoldIndex = endManifoldIndex;
271 } 271 }
272 272
273 islandBodies.Clear(); 273 islandBodies.Clear();
274 } 274 }
275 } 275 }
276 276
277 private static int GetIslandId(PersistentManifold lhs) 277 private static int GetIslandId(PersistentManifold lhs)
278 { 278 {
279 int islandId; 279 int islandId;
280 CollisionObject rcolObjA = lhs.BodyA as CollisionObject; 280 CollisionObject rcolObjA = lhs.BodyA as CollisionObject;
281 CollisionObject rcolObjB = lhs.BodyB as CollisionObject; 281 CollisionObject rcolObjB = lhs.BodyB as CollisionObject;
282 islandId = rcolObjA.IslandTag >= 0 ? rcolObjA.IslandTag : rcolObjB.IslandTag; 282 islandId = rcolObjA.IslandTag >= 0 ? rcolObjA.IslandTag : rcolObjB.IslandTag;
283 return islandId; 283 return islandId;
284 } 284 }
285 285
286 private static int PersistentManifoldSortPredicate(PersistentManifold lhs, PersistentManifold rhs) 286 private static int PersistentManifoldSortPredicate(PersistentManifold lhs, PersistentManifold rhs)
287 { 287 {
288 int rIslandIdA, lIslandIdB; 288 int rIslandIdA, lIslandIdB;
289 rIslandIdA = GetIslandId(rhs); 289 rIslandIdA = GetIslandId(rhs);
290 lIslandIdB = GetIslandId(lhs); 290 lIslandIdB = GetIslandId(lhs);
291 //return lIslandId0 < rIslandId0; 291 //return lIslandId0 < rIslandId0;
292 if (lIslandIdB < rIslandIdA) 292 if (lIslandIdB < rIslandIdA)
293 return -1; 293 return -1;
294 //else if (lIslandIdB > rIslandIdA) 294 //else if (lIslandIdB > rIslandIdA)
295 // return 1; 295 // return 1;
296 return 1; 296 return 1;
297 } 297 }
298 298
299 public interface IIslandCallback 299 public interface IIslandCallback
300 { 300 {
301 void ProcessIsland(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, int islandID); 301 void ProcessIsland(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, int islandID);
302 } 302 }
303 } 303 }
304} 304}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs
index 8933638..2b4e801 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs
@@ -1,270 +1,270 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// SphereBoxCollisionAlgorithm provides sphere-box collision detection. 30 /// SphereBoxCollisionAlgorithm provides sphere-box collision detection.
31 /// Other features are frame-coherency (persistent data) and collision response. 31 /// Other features are frame-coherency (persistent data) and collision response.
32 /// </summary> 32 /// </summary>
33 public class SphereBoxCollisionAlgorithm : CollisionAlgorithm, IDisposable 33 public class SphereBoxCollisionAlgorithm : CollisionAlgorithm, IDisposable
34 { 34 {
35 private bool _ownManifold; 35 private bool _ownManifold;
36 private PersistentManifold _manifold; 36 private PersistentManifold _manifold;
37 private bool _isSwapped; 37 private bool _isSwapped;
38 38
39 public SphereBoxCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject collisionObjectA, CollisionObject collisionObjectB, bool isSwapped) 39 public SphereBoxCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject collisionObjectA, CollisionObject collisionObjectB, bool isSwapped)
40 : base(collisionAlgorithmConstructionInfo) 40 : base(collisionAlgorithmConstructionInfo)
41 { 41 {
42 _ownManifold = false; 42 _ownManifold = false;
43 _manifold = manifold; 43 _manifold = manifold;
44 _isSwapped = isSwapped; 44 _isSwapped = isSwapped;
45 45
46 CollisionObject sphereObject = _isSwapped ? collisionObjectB : collisionObjectA; 46 CollisionObject sphereObject = _isSwapped ? collisionObjectB : collisionObjectA;
47 CollisionObject boxObject = _isSwapped ? collisionObjectA : collisionObjectB; 47 CollisionObject boxObject = _isSwapped ? collisionObjectA : collisionObjectB;
48 48
49 if (_manifold == null && Dispatcher.NeedsCollision(sphereObject, boxObject)) 49 if (_manifold == null && Dispatcher.NeedsCollision(sphereObject, boxObject))
50 { 50 {
51 _manifold = Dispatcher.GetNewManifold(sphereObject, boxObject); 51 _manifold = Dispatcher.GetNewManifold(sphereObject, boxObject);
52 _ownManifold = true; 52 _ownManifold = true;
53 } 53 }
54 } 54 }
55 55
56 public float GetSphereDistance(CollisionObject boxObject, out Vector3 pointOnBox, out Vector3 pointOnSphere, Vector3 sphereCenter, float radius) 56 public float GetSphereDistance(CollisionObject boxObject, out Vector3 pointOnBox, out Vector3 pointOnSphere, Vector3 sphereCenter, float radius)
57 { 57 {
58 pointOnBox = new Vector3(); 58 pointOnBox = new Vector3();
59 pointOnSphere = new Vector3(); 59 pointOnSphere = new Vector3();
60 60
61 float margins; 61 float margins;
62 Vector3[] bounds = new Vector3[2]; 62 Vector3[] bounds = new Vector3[2];
63 BoxShape boxShape = boxObject.CollisionShape as BoxShape; 63 BoxShape boxShape = boxObject.CollisionShape as BoxShape;
64 64
65 bounds[0] = -boxShape.HalfExtents; 65 bounds[0] = -boxShape.HalfExtents;
66 bounds[1] = boxShape.HalfExtents; 66 bounds[1] = boxShape.HalfExtents;
67 67
68 margins = boxShape.Margin; //also add sphereShape margin? 68 margins = boxShape.Margin; //also add sphereShape margin?
69 69
70 Matrix m44T = boxObject.WorldTransform; 70 Matrix m44T = boxObject.WorldTransform;
71 71
72 Vector3[] boundsVec = new Vector3[2]; 72 Vector3[] boundsVec = new Vector3[2];
73 float penetration; 73 float penetration;
74 74
75 boundsVec[0] = bounds[0]; 75 boundsVec[0] = bounds[0];
76 boundsVec[1] = bounds[1]; 76 boundsVec[1] = bounds[1];
77 77
78 Vector3 marginsVec = new Vector3(margins, margins, margins); 78 Vector3 marginsVec = new Vector3(margins, margins, margins);
79 79
80 // add margins 80 // add margins
81 bounds[0] += marginsVec; 81 bounds[0] += marginsVec;
82 bounds[1] -= marginsVec; 82 bounds[1] -= marginsVec;
83 83
84 ///////////////////////////////////////////////// 84 /////////////////////////////////////////////////
85 85
86 Vector3 tmp, prel, normal, v3P; 86 Vector3 tmp, prel, normal, v3P;
87 Vector3[] n = new Vector3[6]; 87 Vector3[] n = new Vector3[6];
88 float sep = 10000000.0f, sepThis; 88 float sep = 10000000.0f, sepThis;
89 89
90 n[0] = new Vector3(-1.0f, 0.0f, 0.0f); 90 n[0] = new Vector3(-1.0f, 0.0f, 0.0f);
91 n[1] = new Vector3(0.0f, -1.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); 92 n[2] = new Vector3(0.0f, 0.0f, -1.0f);
93 n[3] = new Vector3(1.0f, 0.0f, 0.0f); 93 n[3] = new Vector3(1.0f, 0.0f, 0.0f);
94 n[4] = new Vector3(0.0f, 1.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); 95 n[5] = new Vector3(0.0f, 0.0f, 1.0f);
96 96
97 // convert point in local space 97 // convert point in local space
98 prel = MathHelper.InvXForm(m44T, sphereCenter); 98 prel = MathHelper.InvXForm(m44T, sphereCenter);
99 99
100 bool found = false; 100 bool found = false;
101 101
102 v3P = prel; 102 v3P = prel;
103 103
104 for (int i = 0; i < 6; i++) 104 for (int i = 0; i < 6; i++)
105 { 105 {
106 int j = i < 3 ? 0 : 1; 106 int j = i < 3 ? 0 : 1;
107 if ((sepThis = (Vector3.Dot(v3P - bounds[j], n[i]))) > 0.0f) 107 if ((sepThis = (Vector3.Dot(v3P - bounds[j], n[i]))) > 0.0f)
108 { 108 {
109 v3P = v3P - n[i] * sepThis; 109 v3P = v3P - n[i] * sepThis;
110 found = true; 110 found = true;
111 } 111 }
112 } 112 }
113 113
114 // 114 //
115 115
116 if (found) 116 if (found)
117 { 117 {
118 bounds[0] = boundsVec[0]; 118 bounds[0] = boundsVec[0];
119 bounds[1] = boundsVec[1]; 119 bounds[1] = boundsVec[1];
120 120
121 normal = Vector3.Normalize(prel - v3P); 121 normal = Vector3.Normalize(prel - v3P);
122 pointOnBox = v3P + normal * margins; 122 pointOnBox = v3P + normal * margins;
123 pointOnSphere = prel - normal * radius; 123 pointOnSphere = prel - normal * radius;
124 124
125 if ((Vector3.Dot(pointOnSphere - pointOnBox, normal)) > 0.0f) 125 if ((Vector3.Dot(pointOnSphere - pointOnBox, normal)) > 0.0f)
126 { 126 {
127 return 1.0f; 127 return 1.0f;
128 } 128 }
129 129
130 // transform back in world space 130 // transform back in world space
131 tmp = MathHelper.MatrixToVector(m44T, pointOnBox); 131 tmp = MathHelper.MatrixToVector(m44T, pointOnBox);
132 pointOnBox = tmp; 132 pointOnBox = tmp;
133 tmp = MathHelper.MatrixToVector(m44T, pointOnSphere); 133 tmp = MathHelper.MatrixToVector(m44T, pointOnSphere);
134 pointOnSphere = tmp; 134 pointOnSphere = tmp;
135 float seps2 = (pointOnBox - pointOnSphere).LengthSquared(); 135 float seps2 = (pointOnBox - pointOnSphere).LengthSquared();
136 136
137 //if this fails, fallback into deeper penetration case, below 137 //if this fails, fallback into deeper penetration case, below
138 if (seps2 > MathHelper.Epsilon) 138 if (seps2 > MathHelper.Epsilon)
139 { 139 {
140 sep = -(float)Math.Sqrt(seps2); 140 sep = -(float)Math.Sqrt(seps2);
141 normal = (pointOnBox - pointOnSphere); 141 normal = (pointOnBox - pointOnSphere);
142 normal *= 1f / sep; 142 normal *= 1f / sep;
143 } 143 }
144 return sep; 144 return sep;
145 } 145 }
146 146
147 ////////////////////////////////////////////////// 147 //////////////////////////////////////////////////
148 // Deep penetration case 148 // Deep penetration case
149 149
150 penetration = GetSpherePenetration(boxObject, ref pointOnBox, ref pointOnSphere, sphereCenter, radius, bounds[0], bounds[1]); 150 penetration = GetSpherePenetration(boxObject, ref pointOnBox, ref pointOnSphere, sphereCenter, radius, bounds[0], bounds[1]);
151 151
152 bounds[0] = boundsVec[0]; 152 bounds[0] = boundsVec[0];
153 bounds[1] = boundsVec[1]; 153 bounds[1] = boundsVec[1];
154 154
155 if (penetration <= 0.0f) 155 if (penetration <= 0.0f)
156 return (penetration - margins); 156 return (penetration - margins);
157 else 157 else
158 return 1.0f; 158 return 1.0f;
159 } 159 }
160 160
161 public float GetSpherePenetration(CollisionObject boxObject, ref Vector3 pointOnBox, ref Vector3 pointOnSphere, Vector3 sphereCenter, float radius, Vector3 aabbMin, Vector3 aabbMax) 161 public float GetSpherePenetration(CollisionObject boxObject, ref Vector3 pointOnBox, ref Vector3 pointOnSphere, Vector3 sphereCenter, float radius, Vector3 aabbMin, Vector3 aabbMax)
162 { 162 {
163 Vector3[] bounds = new Vector3[2]; 163 Vector3[] bounds = new Vector3[2];
164 164
165 bounds[0] = aabbMin; 165 bounds[0] = aabbMin;
166 bounds[1] = aabbMax; 166 bounds[1] = aabbMax;
167 167
168 Vector3 p0 = new Vector3(), tmp, prel, normal = new Vector3(); 168 Vector3 p0 = new Vector3(), tmp, prel, normal = new Vector3();
169 Vector3[] n = new Vector3[6]; 169 Vector3[] n = new Vector3[6];
170 float sep = -10000000.0f, sepThis; 170 float sep = -10000000.0f, sepThis;
171 171
172 n[0] = new Vector3(-1.0f, 0.0f, 0.0f); 172 n[0] = new Vector3(-1.0f, 0.0f, 0.0f);
173 n[1] = new Vector3(0.0f, -1.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); 174 n[2] = new Vector3(0.0f, 0.0f, -1.0f);
175 n[3] = new Vector3(1.0f, 0.0f, 0.0f); 175 n[3] = new Vector3(1.0f, 0.0f, 0.0f);
176 n[4] = new Vector3(0.0f, 1.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); 177 n[5] = new Vector3(0.0f, 0.0f, 1.0f);
178 178
179 Matrix m44T = boxObject.WorldTransform; 179 Matrix m44T = boxObject.WorldTransform;
180 180
181 // convert point in local space 181 // convert point in local space
182 prel = MathHelper.InvXForm(m44T, sphereCenter); 182 prel = MathHelper.InvXForm(m44T, sphereCenter);
183 183
184 /////////// 184 ///////////
185 185
186 for (int i = 0; i < 6; i++) 186 for (int i = 0; i < 6; i++)
187 { 187 {
188 int j = i < 3 ? 0 : 1; 188 int j = i < 3 ? 0 : 1;
189 if ((sepThis = (Vector3.Dot(prel - bounds[j], n[i])) - radius) > 0.0f) return 1.0f; 189 if ((sepThis = (Vector3.Dot(prel - bounds[j], n[i])) - radius) > 0.0f) return 1.0f;
190 if (sepThis > sep) 190 if (sepThis > sep)
191 { 191 {
192 p0 = bounds[j]; 192 p0 = bounds[j];
193 normal = n[i]; 193 normal = n[i];
194 sep = sepThis; 194 sep = sepThis;
195 } 195 }
196 } 196 }
197 197
198 pointOnBox = prel - normal * (Vector3.Dot(normal, (prel - p0))); 198 pointOnBox = prel - normal * (Vector3.Dot(normal, (prel - p0)));
199 pointOnSphere = pointOnBox + normal * sep; 199 pointOnSphere = pointOnBox + normal * sep;
200 200
201 // transform back in world space 201 // transform back in world space
202 tmp = MathHelper.MatrixToVector(m44T, pointOnBox); 202 tmp = MathHelper.MatrixToVector(m44T, pointOnBox);
203 pointOnBox = tmp; 203 pointOnBox = tmp;
204 tmp = MathHelper.MatrixToVector(m44T, pointOnSphere); 204 tmp = MathHelper.MatrixToVector(m44T, pointOnSphere);
205 pointOnSphere = tmp; 205 pointOnSphere = tmp;
206 normal = Vector3.Normalize(pointOnBox - pointOnSphere); 206 normal = Vector3.Normalize(pointOnBox - pointOnSphere);
207 207
208 return sep; 208 return sep;
209 } 209 }
210 210
211 public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) 211 public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
212 { 212 {
213 if (_manifold == null) 213 if (_manifold == null)
214 return; 214 return;
215 215
216 CollisionObject sphereObject = _isSwapped ? bodyB : bodyA; 216 CollisionObject sphereObject = _isSwapped ? bodyB : bodyA;
217 CollisionObject boxObject = _isSwapped ? bodyA : bodyB; 217 CollisionObject boxObject = _isSwapped ? bodyA : bodyB;
218 218
219 SphereShape sphereA = sphereObject.CollisionShape as SphereShape; 219 SphereShape sphereA = sphereObject.CollisionShape as SphereShape;
220 220
221 Vector3 pOnBox, pOnSphere; 221 Vector3 pOnBox, pOnSphere;
222 Vector3 sphereCenter = sphereObject.WorldTransform.Translation; 222 Vector3 sphereCenter = sphereObject.WorldTransform.Translation;
223 float radius = sphereA.Radius; 223 float radius = sphereA.Radius;
224 224
225 float dist = GetSphereDistance(boxObject, out pOnBox, out pOnSphere, sphereCenter, radius); 225 float dist = GetSphereDistance(boxObject, out pOnBox, out pOnSphere, sphereCenter, radius);
226 226
227 if (dist < MathHelper.Epsilon) 227 if (dist < MathHelper.Epsilon)
228 { 228 {
229 Vector3 normalOnSurfaceB = Vector3.Normalize(pOnBox - pOnSphere); 229 Vector3 normalOnSurfaceB = Vector3.Normalize(pOnBox - pOnSphere);
230 230
231 // report a contact. internally this will be kept persistent, and contact reduction is done 231 // report a contact. internally this will be kept persistent, and contact reduction is done
232 resultOut.SetPersistentManifold(_manifold); 232 resultOut.SetPersistentManifold(_manifold);
233 resultOut.AddContactPoint(normalOnSurfaceB, pOnBox, dist); 233 resultOut.AddContactPoint(normalOnSurfaceB, pOnBox, dist);
234 } 234 }
235 } 235 }
236 236
237 public override float CalculateTimeOfImpact(CollisionObject collisionObjectA, CollisionObject collisionObjectB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) 237 public override float CalculateTimeOfImpact(CollisionObject collisionObjectA, CollisionObject collisionObjectB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
238 { 238 {
239 //not yet 239 //not yet
240 return 1; 240 return 1;
241 } 241 }
242 242
243 public class CreateFunc : CollisionAlgorithmCreateFunction 243 public class CreateFunc : CollisionAlgorithmCreateFunction
244 { 244 {
245 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) 245 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
246 { 246 {
247 if (!IsSwapped) 247 if (!IsSwapped)
248 return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, false); 248 return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, false);
249 else 249 else
250 return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, true); 250 return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, true);
251 } 251 }
252 } 252 }
253 253
254 #region IDisposable Members 254 #region IDisposable Members
255 public void Dispose() 255 public void Dispose()
256 { 256 {
257 Dispose(true); 257 Dispose(true);
258 } 258 }
259 259
260 public void Dispose(bool disposing) 260 public void Dispose(bool disposing)
261 { 261 {
262 if (disposing && _ownManifold) 262 if (disposing && _ownManifold)
263 { 263 {
264 if (_manifold != null) 264 if (_manifold != null)
265 Dispatcher.ReleaseManifold(_manifold); 265 Dispatcher.ReleaseManifold(_manifold);
266 } 266 }
267 } 267 }
268 #endregion 268 #endregion
269 } 269 }
270} 270}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs
index 7a76d24..3a780c1 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs
@@ -1,104 +1,104 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public class SphereSphereCollisionAlgorithm : CollisionAlgorithm 29 public class SphereSphereCollisionAlgorithm : CollisionAlgorithm
30 { 30 {
31 private bool _ownManifold; 31 private bool _ownManifold;
32 private PersistentManifold _manifold; 32 private PersistentManifold _manifold;
33 33
34 public SphereSphereCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) 34 public SphereSphereCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
35 : base(collisionAlgorithmConstructionInfo) 35 : base(collisionAlgorithmConstructionInfo)
36 { 36 {
37 _ownManifold = false; 37 _ownManifold = false;
38 _manifold = manifold; 38 _manifold = manifold;
39 39
40 if (_manifold == null) 40 if (_manifold == null)
41 { 41 {
42 _manifold = Dispatcher.GetNewManifold(bodyA, bodyB); 42 _manifold = Dispatcher.GetNewManifold(bodyA, bodyB);
43 _ownManifold = true; 43 _ownManifold = true;
44 } 44 }
45 } 45 }
46 46
47 public SphereSphereCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) 47 public SphereSphereCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo)
48 : base(collisionAlgorithmConstructionInfo) { } 48 : base(collisionAlgorithmConstructionInfo) { }
49 49
50 ~SphereSphereCollisionAlgorithm() 50 ~SphereSphereCollisionAlgorithm()
51 { 51 {
52 if (_ownManifold) 52 if (_ownManifold)
53 { 53 {
54 if (_manifold != null) 54 if (_manifold != null)
55 Dispatcher.ReleaseManifold(_manifold); 55 Dispatcher.ReleaseManifold(_manifold);
56 } 56 }
57 } 57 }
58 58
59 public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) 59 public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
60 { 60 {
61 if (_manifold == null) 61 if (_manifold == null)
62 return; 62 return;
63 63
64 SphereShape sphereA = bodyA.CollisionShape as SphereShape; 64 SphereShape sphereA = bodyA.CollisionShape as SphereShape;
65 SphereShape sphereB = bodyB.CollisionShape as SphereShape; 65 SphereShape sphereB = bodyB.CollisionShape as SphereShape;
66 66
67 Vector3 diff = bodyA.WorldTransform.Translation - bodyB.WorldTransform.Translation; 67 Vector3 diff = bodyA.WorldTransform.Translation - bodyB.WorldTransform.Translation;
68 float len = diff.Length(); 68 float len = diff.Length();
69 float radiusA = sphereA.Radius; 69 float radiusA = sphereA.Radius;
70 float radiusB = sphereB.Radius; 70 float radiusB = sphereB.Radius;
71 71
72 //if distance positive, don't generate a new contact 72 //if distance positive, don't generate a new contact
73 if (len > (radiusA + radiusB)) 73 if (len > (radiusA + radiusB))
74 return; 74 return;
75 75
76 //distance (negative means penetration) 76 //distance (negative means penetration)
77 float dist = len - (radiusA + radiusB); 77 float dist = len - (radiusA + radiusB);
78 78
79 Vector3 normalOnSurfaceB = diff / len; 79 Vector3 normalOnSurfaceB = diff / len;
80 //point on A (worldspace) 80 //point on A (worldspace)
81 Vector3 posA = bodyA.WorldTransform.Translation - radiusA * normalOnSurfaceB; 81 Vector3 posA = bodyA.WorldTransform.Translation - radiusA * normalOnSurfaceB;
82 //point on B (worldspace) 82 //point on B (worldspace)
83 Vector3 posB = bodyB.WorldTransform.Translation + radiusB * normalOnSurfaceB; 83 Vector3 posB = bodyB.WorldTransform.Translation + radiusB * normalOnSurfaceB;
84 84
85 // report a contact. internally this will be kept persistent, and contact reduction is done 85 // report a contact. internally this will be kept persistent, and contact reduction is done
86 resultOut.SetPersistentManifold(_manifold); 86 resultOut.SetPersistentManifold(_manifold);
87 resultOut.AddContactPoint(normalOnSurfaceB, posB, dist); 87 resultOut.AddContactPoint(normalOnSurfaceB, posB, dist);
88 } 88 }
89 89
90 public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) 90 public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
91 { 91 {
92 //not yet 92 //not yet
93 return 1f; 93 return 1f;
94 } 94 }
95 95
96 public class CreateFunc : CollisionAlgorithmCreateFunction 96 public class CreateFunc : CollisionAlgorithmCreateFunction
97 { 97 {
98 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) 98 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
99 { 99 {
100 return new SphereSphereCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB); 100 return new SphereSphereCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB);
101 } 101 }
102 } 102 }
103 } 103 }
104} 104}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs
index 1ca5cfb..00dfaf1 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs
@@ -1,100 +1,100 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// SphereSphereCollisionAlgorithm provides sphere-sphere collision detection. 30 /// SphereSphereCollisionAlgorithm provides sphere-sphere collision detection.
31 /// Other features are frame-coherency (persistent data) and collision response. 31 /// Other features are frame-coherency (persistent data) and collision response.
32 /// Also provides the most basic sample for custom/user btCollisionAlgorithm 32 /// Also provides the most basic sample for custom/user btCollisionAlgorithm
33 /// </summary> 33 /// </summary>
34 public class SphereTriangleCollisionAlgorithm : CollisionAlgorithm, IDisposable 34 public class SphereTriangleCollisionAlgorithm : CollisionAlgorithm, IDisposable
35 { 35 {
36 private bool _ownManifold; 36 private bool _ownManifold;
37 private PersistentManifold _manifold; 37 private PersistentManifold _manifold;
38 private bool _isSwapped; 38 private bool _isSwapped;
39 39
40 public SphereTriangleCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped) 40 public SphereTriangleCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped)
41 : base(collisionAlgorithmConstructionInfo) 41 : base(collisionAlgorithmConstructionInfo)
42 { 42 {
43 _ownManifold = false; 43 _ownManifold = false;
44 _manifold = manifold; 44 _manifold = manifold;
45 _isSwapped = isSwapped; 45 _isSwapped = isSwapped;
46 46
47 if (_manifold == null) 47 if (_manifold == null)
48 { 48 {
49 _manifold = Dispatcher.GetNewManifold(bodyA, bodyB); 49 _manifold = Dispatcher.GetNewManifold(bodyA, bodyB);
50 _ownManifold = true; 50 _ownManifold = true;
51 } 51 }
52 } 52 }
53 53
54 public SphereTriangleCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) 54 public SphereTriangleCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo)
55 : base(collisionAlgorithmConstructionInfo) { } 55 : base(collisionAlgorithmConstructionInfo) { }
56 56
57 public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) 57 public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
58 { 58 {
59 if (_manifold == null) 59 if (_manifold == null)
60 return; 60 return;
61 61
62 SphereShape sphere = bodyA.CollisionShape as SphereShape; 62 SphereShape sphere = bodyA.CollisionShape as SphereShape;
63 TriangleShape triangle = bodyB.CollisionShape as TriangleShape; 63 TriangleShape triangle = bodyB.CollisionShape as TriangleShape;
64 64
65 /// report a contact. internally this will be kept persistent, and contact reduction is done 65 /// report a contact. internally this will be kept persistent, and contact reduction is done
66 resultOut.SetPersistentManifold(_manifold); 66 resultOut.SetPersistentManifold(_manifold);
67 SphereTriangleDetector detector = new SphereTriangleDetector(sphere, triangle); 67 SphereTriangleDetector detector = new SphereTriangleDetector(sphere, triangle);
68 68
69 DiscreteCollisionDetectorInterface.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); 69 DiscreteCollisionDetectorInterface.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
70 input.MaximumDistanceSquared = 1e30f;//todo: tighter bounds 70 input.MaximumDistanceSquared = 1e30f;//todo: tighter bounds
71 input.TransformA = bodyA.WorldTransform; 71 input.TransformA = bodyA.WorldTransform;
72 input.TransformB = bodyB.WorldTransform; 72 input.TransformB = bodyB.WorldTransform;
73 73
74 detector.GetClosestPoints(input, resultOut, null); 74 detector.GetClosestPoints(input, resultOut, null);
75 } 75 }
76 76
77 public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) 77 public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
78 { 78 {
79 //not yet 79 //not yet
80 return 1f; 80 return 1f;
81 } 81 }
82 82
83 public class CreateFunc : CollisionAlgorithmCreateFunction 83 public class CreateFunc : CollisionAlgorithmCreateFunction
84 { 84 {
85 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) 85 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
86 { 86 {
87 return new SphereTriangleCollisionAlgorithm(collisionAlgorithmConstructionInfo.Manifold, collisionAlgorithmConstructionInfo, bodyA, bodyB, IsSwapped); 87 return new SphereTriangleCollisionAlgorithm(collisionAlgorithmConstructionInfo.Manifold, collisionAlgorithmConstructionInfo, bodyA, bodyB, IsSwapped);
88 } 88 }
89 } 89 }
90 90
91 #region IDisposable Members 91 #region IDisposable Members
92 public void Dispose() 92 public void Dispose()
93 { 93 {
94 if (_ownManifold) 94 if (_ownManifold)
95 if (_manifold != null) 95 if (_manifold != null)
96 Dispatcher.ReleaseManifold(_manifold); 96 Dispatcher.ReleaseManifold(_manifold);
97 } 97 }
98 #endregion 98 #endregion
99 } 99 }
100} 100}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs
index 865754a..4d578d8 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs
@@ -1,214 +1,214 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public class SphereTriangleDetector : DiscreteCollisionDetectorInterface 29 public class SphereTriangleDetector : DiscreteCollisionDetectorInterface
30 { 30 {
31 private SphereShape _sphere; 31 private SphereShape _sphere;
32 private TriangleShape _triangle; 32 private TriangleShape _triangle;
33 private const int MaxOverlap = 0; 33 private const int MaxOverlap = 0;
34 34
35 public SphereTriangleDetector(SphereShape sphere, TriangleShape triangle) 35 public SphereTriangleDetector(SphereShape sphere, TriangleShape triangle)
36 { 36 {
37 this._sphere = sphere; 37 this._sphere = sphere;
38 this._triangle = triangle; 38 this._triangle = triangle;
39 } 39 }
40 40
41 public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw) 41 public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw)
42 { 42 {
43 Matrix transformA = input.TransformA; 43 Matrix transformA = input.TransformA;
44 Matrix transformB = input.TransformB; 44 Matrix transformB = input.TransformB;
45 45
46 Vector3 point = new Vector3(); 46 Vector3 point = new Vector3();
47 Vector3 normal = new Vector3(); 47 Vector3 normal = new Vector3();
48 Single timeOfImpact = 1.0f; 48 Single timeOfImpact = 1.0f;
49 Single depth = 0.0f; 49 Single depth = 0.0f;
50 50
51 //move sphere into triangle space 51 //move sphere into triangle space
52 Matrix sphereInTr = MathHelper.InverseTimes(transformB, transformA); 52 Matrix sphereInTr = MathHelper.InverseTimes(transformB, transformA);
53 53
54 if (Collide(sphereInTr.Translation, point, normal, depth, timeOfImpact)) 54 if (Collide(sphereInTr.Translation, point, normal, depth, timeOfImpact))
55 output.AddContactPoint(Vector3.TransformNormal(normal, transformB), Vector3.TransformNormal(point, transformB), depth); 55 output.AddContactPoint(Vector3.TransformNormal(normal, transformB), Vector3.TransformNormal(point, transformB), depth);
56 } 56 }
57 57
58 /// <summary> 58 /// <summary>
59 /// See also geometrictools.com 59 /// See also geometrictools.com
60 /// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv 60 /// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv
61 /// </summary> 61 /// </summary>
62 /// <param name="from"></param> 62 /// <param name="from"></param>
63 /// <param name="to"></param> 63 /// <param name="to"></param>
64 /// <param name="p"></param> 64 /// <param name="p"></param>
65 /// <param name="nearest"></param> 65 /// <param name="nearest"></param>
66 /// <returns></returns> 66 /// <returns></returns>
67 private float SegmentSquareDistance(Vector3 from, Vector3 to, Vector3 point, Vector3 nearest) 67 private float SegmentSquareDistance(Vector3 from, Vector3 to, Vector3 point, Vector3 nearest)
68 { 68 {
69 Vector3 diff = point - from; 69 Vector3 diff = point - from;
70 Vector3 v = to - from; 70 Vector3 v = to - from;
71 float t = Vector3.Dot(v, diff); 71 float t = Vector3.Dot(v, diff);
72 72
73 if (t > 0) 73 if (t > 0)
74 { 74 {
75 float dotVV = Vector3.Dot(v, v); 75 float dotVV = Vector3.Dot(v, v);
76 if (t < dotVV) 76 if (t < dotVV)
77 { 77 {
78 t /= dotVV; 78 t /= dotVV;
79 diff -= t * v; 79 diff -= t * v;
80 } 80 }
81 else 81 else
82 { 82 {
83 t = 1; 83 t = 1;
84 diff -= v; 84 diff -= v;
85 } 85 }
86 } 86 }
87 else 87 else
88 t = 0; 88 t = 0;
89 89
90 nearest = from + t * v; 90 nearest = from + t * v;
91 return Vector3.Dot(diff, diff); 91 return Vector3.Dot(diff, diff);
92 } 92 }
93 93
94 private bool Collide(Vector3 sphereCenter, Vector3 point, Vector3 resultNormal, float depth, float timeOfImpact) 94 private bool Collide(Vector3 sphereCenter, Vector3 point, Vector3 resultNormal, float depth, float timeOfImpact)
95 { 95 {
96 Vector3[] vertices = _triangle.Vertices; 96 Vector3[] vertices = _triangle.Vertices;
97 Vector3 c = sphereCenter; 97 Vector3 c = sphereCenter;
98 float r = _sphere.Radius; 98 float r = _sphere.Radius;
99 99
100 Vector3 delta = new Vector3(); 100 Vector3 delta = new Vector3();
101 101
102 Vector3 normal = Vector3.Cross(vertices[1] - vertices[0], vertices[2] - vertices[0]); 102 Vector3 normal = Vector3.Cross(vertices[1] - vertices[0], vertices[2] - vertices[0]);
103 normal = Vector3.Normalize(normal); 103 normal = Vector3.Normalize(normal);
104 Vector3 p1ToCentre = c - vertices[0]; 104 Vector3 p1ToCentre = c - vertices[0];
105 float distanceFromPlane = Vector3.Dot(p1ToCentre, normal); 105 float distanceFromPlane = Vector3.Dot(p1ToCentre, normal);
106 106
107 if (distanceFromPlane < 0) 107 if (distanceFromPlane < 0)
108 { 108 {
109 //triangle facing the other way 109 //triangle facing the other way
110 distanceFromPlane *= -1; 110 distanceFromPlane *= -1;
111 normal *= -1; 111 normal *= -1;
112 } 112 }
113 113
114 float contactMargin = PersistentManifold.ContactBreakingThreshold; 114 float contactMargin = PersistentManifold.ContactBreakingThreshold;
115 bool isInsideContactPlane = distanceFromPlane < r + contactMargin; 115 bool isInsideContactPlane = distanceFromPlane < r + contactMargin;
116 bool isInsideShellPlane = distanceFromPlane < r; 116 bool isInsideShellPlane = distanceFromPlane < r;
117 117
118 float deltaDotNormal = Vector3.Dot(delta, normal); 118 float deltaDotNormal = Vector3.Dot(delta, normal);
119 if (!isInsideShellPlane && deltaDotNormal >= 0.0f) 119 if (!isInsideShellPlane && deltaDotNormal >= 0.0f)
120 return false; 120 return false;
121 121
122 // Check for contact / intersection 122 // Check for contact / intersection
123 bool hasContact = false; 123 bool hasContact = false;
124 Vector3 contactPoint = new Vector3(); 124 Vector3 contactPoint = new Vector3();
125 if (isInsideContactPlane) 125 if (isInsideContactPlane)
126 { 126 {
127 if (FaceContains(c, vertices, normal)) 127 if (FaceContains(c, vertices, normal))
128 { 128 {
129 // Inside the contact wedge - touches a point on the shell plane 129 // Inside the contact wedge - touches a point on the shell plane
130 hasContact = true; 130 hasContact = true;
131 contactPoint = c - normal * distanceFromPlane; 131 contactPoint = c - normal * distanceFromPlane;
132 } 132 }
133 else 133 else
134 { 134 {
135 // Could be inside one of the contact capsules 135 // Could be inside one of the contact capsules
136 float contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin); 136 float contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin);
137 Vector3 nearestOnEdge = new Vector3(); 137 Vector3 nearestOnEdge = new Vector3();
138 for (int i = 0; i < _triangle.EdgeCount; i++) 138 for (int i = 0; i < _triangle.EdgeCount; i++)
139 { 139 {
140 Vector3 pa, pb; 140 Vector3 pa, pb;
141 _triangle.GetEdge(i, out pa, out pb); 141 _triangle.GetEdge(i, out pa, out pb);
142 142
143 float distanceSqr = SegmentSquareDistance(pa, pb, c, nearestOnEdge); 143 float distanceSqr = SegmentSquareDistance(pa, pb, c, nearestOnEdge);
144 if (distanceSqr < contactCapsuleRadiusSqr) 144 if (distanceSqr < contactCapsuleRadiusSqr)
145 { 145 {
146 // Yep, we're inside a capsule 146 // Yep, we're inside a capsule
147 hasContact = true; 147 hasContact = true;
148 contactPoint = nearestOnEdge; 148 contactPoint = nearestOnEdge;
149 } 149 }
150 } 150 }
151 } 151 }
152 } 152 }
153 153
154 if (hasContact) 154 if (hasContact)
155 { 155 {
156 Vector3 contactToCentre = c - contactPoint; 156 Vector3 contactToCentre = c - contactPoint;
157 float distanceSqr = contactToCentre.LengthSquared(); 157 float distanceSqr = contactToCentre.LengthSquared();
158 if (distanceSqr < (r - MaxOverlap) * (r - MaxOverlap)) 158 if (distanceSqr < (r - MaxOverlap) * (r - MaxOverlap))
159 { 159 {
160 float distance = (float)Math.Sqrt(distanceSqr); 160 float distance = (float)Math.Sqrt(distanceSqr);
161 resultNormal = contactToCentre; 161 resultNormal = contactToCentre;
162 resultNormal = Vector3.Normalize(resultNormal); 162 resultNormal = Vector3.Normalize(resultNormal);
163 point = contactPoint; 163 point = contactPoint;
164 depth = -(r - distance); 164 depth = -(r - distance);
165 return true; 165 return true;
166 } 166 }
167 167
168 if (Vector3.Dot(delta, contactToCentre) >= 0.0f) 168 if (Vector3.Dot(delta, contactToCentre) >= 0.0f)
169 return false; 169 return false;
170 170
171 // Moving towards the contact point -> collision 171 // Moving towards the contact point -> collision
172 point = contactPoint; 172 point = contactPoint;
173 timeOfImpact = 0.0f; 173 timeOfImpact = 0.0f;
174 return true; 174 return true;
175 } 175 }
176 return false; 176 return false;
177 } 177 }
178 178
179 private bool PointInTriangle(Vector3[] vertices, Vector3 normal, Vector3 p) 179 private bool PointInTriangle(Vector3[] vertices, Vector3 normal, Vector3 p)
180 { 180 {
181 Vector3 p1 = vertices[0]; 181 Vector3 p1 = vertices[0];
182 Vector3 p2 = vertices[1]; 182 Vector3 p2 = vertices[1];
183 Vector3 p3 = vertices[2]; 183 Vector3 p3 = vertices[2];
184 184
185 Vector3 edge1 = p2 - p1; 185 Vector3 edge1 = p2 - p1;
186 Vector3 edge2 = p3 - p2; 186 Vector3 edge2 = p3 - p2;
187 Vector3 edge3 = p1 - p3; 187 Vector3 edge3 = p1 - p3;
188 188
189 Vector3 p1ToP = p - p1; 189 Vector3 p1ToP = p - p1;
190 Vector3 p2ToP = p - p2; 190 Vector3 p2ToP = p - p2;
191 Vector3 p3ToP = p - p3; 191 Vector3 p3ToP = p - p3;
192 192
193 Vector3 edge1Normal = Vector3.Cross(edge1, normal); 193 Vector3 edge1Normal = Vector3.Cross(edge1, normal);
194 Vector3 edge2Normal = Vector3.Cross(edge2, normal); 194 Vector3 edge2Normal = Vector3.Cross(edge2, normal);
195 Vector3 edge3Normal = Vector3.Cross(edge3, normal); 195 Vector3 edge3Normal = Vector3.Cross(edge3, normal);
196 196
197 float r1, r2, r3; 197 float r1, r2, r3;
198 r1 = Vector3.Dot(edge1Normal, p1ToP); 198 r1 = Vector3.Dot(edge1Normal, p1ToP);
199 r2 = Vector3.Dot(edge2Normal, p2ToP); 199 r2 = Vector3.Dot(edge2Normal, p2ToP);
200 r3 = Vector3.Dot(edge3Normal, p3ToP); 200 r3 = Vector3.Dot(edge3Normal, p3ToP);
201 if ((r1 > 0 && r2 > 0 && r3 > 0) || 201 if ((r1 > 0 && r2 > 0 && r3 > 0) ||
202 (r1 <= 0 && r2 <= 0 && r3 <= 0)) 202 (r1 <= 0 && r2 <= 0 && r3 <= 0))
203 return true; 203 return true;
204 return false; 204 return false;
205 } 205 }
206 206
207 private bool FaceContains(Vector3 p, Vector3[] vertices, Vector3 normal) 207 private bool FaceContains(Vector3 p, Vector3[] vertices, Vector3 normal)
208 { 208 {
209 Vector3 lp = p; 209 Vector3 lp = p;
210 Vector3 lnormal = normal; 210 Vector3 lnormal = normal;
211 return PointInTriangle(vertices, lnormal, lp); 211 return PointInTriangle(vertices, lnormal, lp);
212 } 212 }
213 } 213 }
214} \ No newline at end of file 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
index a825fad..3c7daea 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/UnionFind.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/UnionFind.cs
@@ -1,151 +1,151 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25 25
26namespace XnaDevRu.BulletX 26namespace XnaDevRu.BulletX
27{ 27{
28 public class UnionFind : IDisposable 28 public class UnionFind : IDisposable
29 { 29 {
30 private List<Element> _elements = new List<Element>(); 30 private List<Element> _elements = new List<Element>();
31 31
32 public int ElementCount 32 public int ElementCount
33 { 33 {
34 get { return _elements.Count; } 34 get { return _elements.Count; }
35 } 35 }
36 36
37 public void SortIslands() 37 public void SortIslands()
38 { 38 {
39 for (int i = 0; i < _elements.Count; i++) 39 for (int i = 0; i < _elements.Count; i++)
40 { 40 {
41 _elements[i].ID = Find(i); 41 _elements[i].ID = Find(i);
42 _elements[i].Size = i; 42 _elements[i].Size = i;
43 } 43 }
44 44
45 _elements.Sort(Sort); 45 _elements.Sort(Sort);
46 } 46 }
47 47
48 private static int Sort(Element x, Element y) 48 private static int Sort(Element x, Element y)
49 { 49 {
50 if (x.ID < y.ID) return -1; 50 if (x.ID < y.ID) return -1;
51 //else if (x.ID > y.ID) return 1; 51 //else if (x.ID > y.ID) return 1;
52 else return 0; 52 else return 0;
53 } 53 }
54 54
55 public void Reset(int number) 55 public void Reset(int number)
56 { 56 {
57 Allocate(number); 57 Allocate(number);
58 58
59 for (int i = 0; i < number; i++) 59 for (int i = 0; i < number; i++)
60 { 60 {
61 Element element = new Element(); 61 Element element = new Element();
62 element.ID = i; 62 element.ID = i;
63 element.Size = 1; 63 element.Size = 1;
64 _elements.Insert(i, element); 64 _elements.Insert(i, element);
65 } 65 }
66 } 66 }
67 67
68 public bool IsRoot(int index) 68 public bool IsRoot(int index)
69 { 69 {
70 return (_elements[index].Size == index); 70 return (_elements[index].Size == index);
71 } 71 }
72 72
73 public Element this[int index] 73 public Element this[int index]
74 { 74 {
75 get { return _elements[index]; } 75 get { return _elements[index]; }
76 } 76 }
77 77
78 public void Allocate(int number) 78 public void Allocate(int number)
79 { 79 {
80 //Does nothing 80 //Does nothing
81 _elements = new List<Element>(number); 81 _elements = new List<Element>(number);
82 } 82 }
83 83
84 public bool Find(int i, int j) 84 public bool Find(int i, int j)
85 { 85 {
86 return (Find(i) == Find(j)); 86 return (Find(i) == Find(j));
87 } 87 }
88 88
89 public int Find(int i) 89 public int Find(int i)
90 { 90 {
91 while (i != _elements[i].ID) 91 while (i != _elements[i].ID)
92 { 92 {
93 //Element element = _elements[i]; 93 //Element element = _elements[i];
94 //element.ID = _elements[_elements[i].ID].ID; 94 //element.ID = _elements[_elements[i].ID].ID;
95 _elements[i].ID = _elements[_elements[i].ID].ID; 95 _elements[i].ID = _elements[_elements[i].ID].ID;
96 i = _elements[i].ID; 96 i = _elements[i].ID;
97 } 97 }
98 98
99 return i; 99 return i;
100 } 100 }
101 101
102 public void Unite(int p, int q) 102 public void Unite(int p, int q)
103 { 103 {
104 int i = Find(p), j = Find(q); 104 int i = Find(p), j = Find(q);
105 if (i == j) 105 if (i == j)
106 return; 106 return;
107 107
108 //weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) ) 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) 109 //if (_elements[i].Size < _elements[j].Size)
110 //{ 110 //{
111 // Element element = _elements[i]; 111 // Element element = _elements[i];
112 // element.ID = j; 112 // element.ID = j;
113 // _elements[i] = element; 113 // _elements[i] = element;
114 114
115 // element = _elements[j]; 115 // element = _elements[j];
116 // element.Size += _elements[i].Size; 116 // element.Size += _elements[i].Size;
117 // _elements[j] = element; 117 // _elements[j] = element;
118 //} 118 //}
119 //else 119 //else
120 //{ 120 //{
121 // Element element = _elements[j]; 121 // Element element = _elements[j];
122 // element.ID = i; 122 // element.ID = i;
123 // _elements[j] = element; 123 // _elements[j] = element;
124 124
125 // element = _elements[i]; 125 // element = _elements[i];
126 // element.Size += _elements[j].Size; 126 // element.Size += _elements[j].Size;
127 // _elements[i] = element; 127 // _elements[i] = element;
128 //} 128 //}
129 _elements[i].ID = j; 129 _elements[i].ID = j;
130 _elements[j].Size += _elements[i].Size; 130 _elements[j].Size += _elements[i].Size;
131 } 131 }
132 132
133 #region IDisposable Members 133 #region IDisposable Members
134 134
135 public void Dispose() 135 public void Dispose()
136 { 136 {
137 _elements.Clear(); 137 _elements.Clear();
138 } 138 }
139 139
140 #endregion 140 #endregion
141 } 141 }
142 142
143 public class Element 143 public class Element
144 { 144 {
145 private int _id; 145 private int _id;
146 private int _size; 146 private int _size;
147 147
148 public int ID { get { return _id; } set { _id = value; } } 148 public int ID { get { return _id; } set { _id = value; } }
149 public int Size { get { return _size; } set { _size = value; } } 149 public int Size { get { return _size; } set { _size = value; } }
150 } 150 }
151} \ No newline at end of file 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
index b75c2b6..34f7442 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BUSimplex1to4.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BUSimplex1to4.cs
@@ -1,215 +1,215 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// BUSimplex1to4 implements feature based and implicit simplex of up to 4 vertices (tetrahedron, triangle, line, vertex). 30 /// BUSimplex1to4 implements feature based and implicit simplex of up to 4 vertices (tetrahedron, triangle, line, vertex).
31 /// </summary> 31 /// </summary>
32 public class BUSimplex1to4 : PolyhedralConvexShape 32 public class BUSimplex1to4 : PolyhedralConvexShape
33 { 33 {
34 private int _numVertices = 0; 34 private int _numVertices = 0;
35 private Vector3[] _vertices = new Vector3[4]; 35 private Vector3[] _vertices = new Vector3[4];
36 36
37 public BUSimplex1to4() { } 37 public BUSimplex1to4() { }
38 38
39 public BUSimplex1to4(Vector3 pointA) 39 public BUSimplex1to4(Vector3 pointA)
40 { 40 {
41 AddVertex(pointA); 41 AddVertex(pointA);
42 } 42 }
43 43
44 public BUSimplex1to4(Vector3 pointA, Vector3 pointB) 44 public BUSimplex1to4(Vector3 pointA, Vector3 pointB)
45 { 45 {
46 AddVertex(pointA); 46 AddVertex(pointA);
47 AddVertex(pointB); 47 AddVertex(pointB);
48 } 48 }
49 49
50 public BUSimplex1to4(Vector3 pointA, Vector3 pointB, Vector3 pointC) 50 public BUSimplex1to4(Vector3 pointA, Vector3 pointB, Vector3 pointC)
51 { 51 {
52 AddVertex(pointA); 52 AddVertex(pointA);
53 AddVertex(pointB); 53 AddVertex(pointB);
54 AddVertex(pointC); 54 AddVertex(pointC);
55 } 55 }
56 56
57 public BUSimplex1to4(Vector3 pointA, Vector3 pointB, Vector3 pointC, Vector3 pointD) 57 public BUSimplex1to4(Vector3 pointA, Vector3 pointB, Vector3 pointC, Vector3 pointD)
58 { 58 {
59 AddVertex(pointA); 59 AddVertex(pointA);
60 AddVertex(pointB); 60 AddVertex(pointB);
61 AddVertex(pointC); 61 AddVertex(pointC);
62 AddVertex(pointD); 62 AddVertex(pointD);
63 } 63 }
64 64
65 protected Vector3[] Vertices { get { return _vertices; } set { _vertices = value; } } 65 protected Vector3[] Vertices { get { return _vertices; } set { _vertices = value; } }
66 66
67 public override int VertexCount 67 public override int VertexCount
68 { 68 {
69 get 69 get
70 { 70 {
71 return _numVertices; 71 return _numVertices;
72 } 72 }
73 } 73 }
74 74
75 public override int EdgeCount 75 public override int EdgeCount
76 { 76 {
77 get 77 get
78 { 78 {
79 //euler formula, F-E+V = 2, so E = F+V-2 79 //euler formula, F-E+V = 2, so E = F+V-2
80 switch (_numVertices) 80 switch (_numVertices)
81 { 81 {
82 case 0: return 0; 82 case 0: return 0;
83 case 1: return 0; 83 case 1: return 0;
84 case 2: return 1; 84 case 2: return 1;
85 case 3: return 3; 85 case 3: return 3;
86 case 4: return 6; 86 case 4: return 6;
87 } 87 }
88 return 0; 88 return 0;
89 } 89 }
90 } 90 }
91 91
92 public override int PlaneCount 92 public override int PlaneCount
93 { 93 {
94 get 94 get
95 { 95 {
96 switch (_numVertices) 96 switch (_numVertices)
97 { 97 {
98 case 0: 98 case 0:
99 return 0; 99 return 0;
100 case 1: 100 case 1:
101 return 0; 101 return 0;
102 case 2: 102 case 2:
103 return 0; 103 return 0;
104 case 3: 104 case 3:
105 return 2; 105 return 2;
106 case 4: 106 case 4:
107 return 4; 107 return 4;
108 } 108 }
109 return 0; 109 return 0;
110 } 110 }
111 } 111 }
112 112
113 public override BroadphaseNativeTypes ShapeType 113 public override BroadphaseNativeTypes ShapeType
114 { 114 {
115 get 115 get
116 { 116 {
117 return BroadphaseNativeTypes.Tetrahedral; 117 return BroadphaseNativeTypes.Tetrahedral;
118 } 118 }
119 } 119 }
120 120
121 public override string Name 121 public override string Name
122 { 122 {
123 get 123 get
124 { 124 {
125 return "BUSimplex1to4"; 125 return "BUSimplex1to4";
126 } 126 }
127 } 127 }
128 128
129 public void AddVertex(Vector3 v) 129 public void AddVertex(Vector3 v)
130 { 130 {
131 _vertices[_numVertices++] = v; 131 _vertices[_numVertices++] = v;
132 } 132 }
133 133
134 public void Reset() 134 public void Reset()
135 { 135 {
136 _numVertices = 0; 136 _numVertices = 0;
137 } 137 }
138 138
139 public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) 139 public override void GetEdge(int i, out Vector3 pa, out Vector3 pb)
140 { 140 {
141 switch (_numVertices) 141 switch (_numVertices)
142 { 142 {
143 case 2: 143 case 2:
144 pa = _vertices[0]; 144 pa = _vertices[0];
145 pb = _vertices[1]; 145 pb = _vertices[1];
146 return; 146 return;
147 case 3: 147 case 3:
148 switch (i) 148 switch (i)
149 { 149 {
150 case 0: 150 case 0:
151 pa = _vertices[0]; 151 pa = _vertices[0];
152 pb = _vertices[1]; 152 pb = _vertices[1];
153 return; 153 return;
154 case 1: 154 case 1:
155 pa = _vertices[1]; 155 pa = _vertices[1];
156 pb = _vertices[2]; 156 pb = _vertices[2];
157 return; 157 return;
158 case 2: 158 case 2:
159 pa = _vertices[2]; 159 pa = _vertices[2];
160 pb = _vertices[0]; 160 pb = _vertices[0];
161 return; 161 return;
162 } 162 }
163 break; 163 break;
164 case 4: 164 case 4:
165 switch (i) 165 switch (i)
166 { 166 {
167 case 0: 167 case 0:
168 pa = _vertices[0]; 168 pa = _vertices[0];
169 pb = _vertices[1]; 169 pb = _vertices[1];
170 return; 170 return;
171 case 1: 171 case 1:
172 pa = _vertices[1]; 172 pa = _vertices[1];
173 pb = _vertices[2]; 173 pb = _vertices[2];
174 return; 174 return;
175 case 2: 175 case 2:
176 pa = _vertices[2]; 176 pa = _vertices[2];
177 pb = _vertices[0]; 177 pb = _vertices[0];
178 return; 178 return;
179 case 3: 179 case 3:
180 pa = _vertices[0]; 180 pa = _vertices[0];
181 pb = _vertices[3]; 181 pb = _vertices[3];
182 return; 182 return;
183 case 4: 183 case 4:
184 pa = _vertices[1]; 184 pa = _vertices[1];
185 pb = _vertices[3]; 185 pb = _vertices[3];
186 return; 186 return;
187 case 5: 187 case 5:
188 pa = _vertices[2]; 188 pa = _vertices[2];
189 pb = _vertices[3]; 189 pb = _vertices[3];
190 return; 190 return;
191 } 191 }
192 break; 192 break;
193 } 193 }
194 194
195 pa = new Vector3(); 195 pa = new Vector3();
196 pb = new Vector3(); 196 pb = new Vector3();
197 } 197 }
198 198
199 public override void GetVertex(int i, out Vector3 vtx) 199 public override void GetVertex(int i, out Vector3 vtx)
200 { 200 {
201 vtx = _vertices[i]; 201 vtx = _vertices[i];
202 } 202 }
203 203
204 public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) 204 public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i)
205 { 205 {
206 planeNormal = new Vector3(); 206 planeNormal = new Vector3();
207 planeSupport = new Vector3(); 207 planeSupport = new Vector3();
208 } 208 }
209 209
210 public override bool IsInside(Vector3 pt, float tolerance) 210 public override bool IsInside(Vector3 pt, float tolerance)
211 { 211 {
212 return false; 212 return false;
213 } 213 }
214 } 214 }
215} 215}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs
index ba6f3b7..7fab5c7 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs
@@ -1,316 +1,316 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public class BoxShape : PolyhedralConvexShape 29 public class BoxShape : PolyhedralConvexShape
30 { 30 {
31 public BoxShape(Vector3 boxHalfExtents) 31 public BoxShape(Vector3 boxHalfExtents)
32 { 32 {
33 ImplicitShapeDimensions = boxHalfExtents; 33 ImplicitShapeDimensions = boxHalfExtents;
34 } 34 }
35 35
36 public override int VertexCount 36 public override int VertexCount
37 { 37 {
38 get 38 get
39 { 39 {
40 return 8; 40 return 8;
41 } 41 }
42 } 42 }
43 43
44 public override int EdgeCount 44 public override int EdgeCount
45 { 45 {
46 get 46 get
47 { 47 {
48 return 12; 48 return 12;
49 } 49 }
50 } 50 }
51 51
52 public override BroadphaseNativeTypes ShapeType 52 public override BroadphaseNativeTypes ShapeType
53 { 53 {
54 get 54 get
55 { 55 {
56 return BroadphaseNativeTypes.Box; 56 return BroadphaseNativeTypes.Box;
57 } 57 }
58 } 58 }
59 59
60 public override string Name 60 public override string Name
61 { 61 {
62 get 62 get
63 { 63 {
64 return "Box"; 64 return "Box";
65 } 65 }
66 } 66 }
67 67
68 public override int PreferredPenetrationDirectionsCount 68 public override int PreferredPenetrationDirectionsCount
69 { 69 {
70 get 70 get
71 { 71 {
72 return 6; 72 return 6;
73 } 73 }
74 } 74 }
75 75
76 public override int PlaneCount 76 public override int PlaneCount
77 { 77 {
78 get 78 get
79 { 79 {
80 return 6; 80 return 6;
81 } 81 }
82 } 82 }
83 83
84 public Vector3 HalfExtents { get { return ImplicitShapeDimensions * LocalScaling; } } 84 public Vector3 HalfExtents { get { return ImplicitShapeDimensions * LocalScaling; } }
85 85
86 public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) 86 public override void GetEdge(int i, out Vector3 pa, out Vector3 pb)
87 { 87 {
88 int edgeVert0 = 0; 88 int edgeVert0 = 0;
89 int edgeVert1 = 0; 89 int edgeVert1 = 0;
90 90
91 switch (i) 91 switch (i)
92 { 92 {
93 case 0: 93 case 0:
94 edgeVert0 = 0; 94 edgeVert0 = 0;
95 edgeVert1 = 1; 95 edgeVert1 = 1;
96 break; 96 break;
97 case 1: 97 case 1:
98 edgeVert0 = 0; 98 edgeVert0 = 0;
99 edgeVert1 = 2; 99 edgeVert1 = 2;
100 break; 100 break;
101 case 2: 101 case 2:
102 edgeVert0 = 1; 102 edgeVert0 = 1;
103 edgeVert1 = 3; 103 edgeVert1 = 3;
104 104
105 break; 105 break;
106 case 3: 106 case 3:
107 edgeVert0 = 2; 107 edgeVert0 = 2;
108 edgeVert1 = 3; 108 edgeVert1 = 3;
109 break; 109 break;
110 case 4: 110 case 4:
111 edgeVert0 = 0; 111 edgeVert0 = 0;
112 edgeVert1 = 4; 112 edgeVert1 = 4;
113 break; 113 break;
114 case 5: 114 case 5:
115 edgeVert0 = 1; 115 edgeVert0 = 1;
116 edgeVert1 = 5; 116 edgeVert1 = 5;
117 117
118 break; 118 break;
119 case 6: 119 case 6:
120 edgeVert0 = 2; 120 edgeVert0 = 2;
121 edgeVert1 = 6; 121 edgeVert1 = 6;
122 break; 122 break;
123 case 7: 123 case 7:
124 edgeVert0 = 3; 124 edgeVert0 = 3;
125 edgeVert1 = 7; 125 edgeVert1 = 7;
126 break; 126 break;
127 case 8: 127 case 8:
128 edgeVert0 = 4; 128 edgeVert0 = 4;
129 edgeVert1 = 5; 129 edgeVert1 = 5;
130 break; 130 break;
131 case 9: 131 case 9:
132 edgeVert0 = 4; 132 edgeVert0 = 4;
133 edgeVert1 = 6; 133 edgeVert1 = 6;
134 break; 134 break;
135 case 10: 135 case 10:
136 edgeVert0 = 5; 136 edgeVert0 = 5;
137 edgeVert1 = 7; 137 edgeVert1 = 7;
138 break; 138 break;
139 case 11: 139 case 11:
140 edgeVert0 = 6; 140 edgeVert0 = 6;
141 edgeVert1 = 7; 141 edgeVert1 = 7;
142 break; 142 break;
143 default: 143 default:
144 throw new BulletException(); 144 throw new BulletException();
145 145
146 } 146 }
147 147
148 GetVertex(edgeVert0, out pa); 148 GetVertex(edgeVert0, out pa);
149 GetVertex(edgeVert1, out pb); 149 GetVertex(edgeVert1, out pb);
150 } 150 }
151 151
152 public override void GetVertex(int i, out Vector3 vtx) 152 public override void GetVertex(int i, out Vector3 vtx)
153 { 153 {
154 Vector3 halfExtents = HalfExtents; 154 Vector3 halfExtents = HalfExtents;
155 155
156 vtx = new Vector3( 156 vtx = new Vector3(
157 halfExtents.X * (1 - (i & 1)) - halfExtents.X * (i & 1), 157 halfExtents.X * (1 - (i & 1)) - halfExtents.X * (i & 1),
158 halfExtents.Y * (1 - ((i & 2) >> 1)) - halfExtents.Y * ((i & 2) >> 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)); 159 halfExtents.Z * (1 - ((i & 4) >> 2)) - halfExtents.Z * ((i & 4) >> 2));
160 } 160 }
161 161
162 public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) 162 public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i)
163 { 163 {
164 //this plane might not be aligned... 164 //this plane might not be aligned...
165 Vector4 plane; 165 Vector4 plane;
166 GetPlaneEquation(out plane, i); 166 GetPlaneEquation(out plane, i);
167 planeNormal = new Vector3(plane.X, plane.Y, plane.Z); 167 planeNormal = new Vector3(plane.X, plane.Y, plane.Z);
168 planeSupport = LocalGetSupportingVertex(-planeNormal); 168 planeSupport = LocalGetSupportingVertex(-planeNormal);
169 } 169 }
170 170
171 public override bool IsInside(Vector3 pt, float tolerance) 171 public override bool IsInside(Vector3 pt, float tolerance)
172 { 172 {
173 Vector3 halfExtents = HalfExtents; 173 Vector3 halfExtents = HalfExtents;
174 174
175 //btScalar minDist = 2*tolerance; 175 //btScalar minDist = 2*tolerance;
176 176
177 bool result = (pt.X <= ( halfExtents.X + tolerance)) && 177 bool result = (pt.X <= ( halfExtents.X + tolerance)) &&
178 (pt.X >= (-halfExtents.X - tolerance)) && 178 (pt.X >= (-halfExtents.X - tolerance)) &&
179 (pt.Y <= ( halfExtents.Y + tolerance)) && 179 (pt.Y <= ( halfExtents.Y + tolerance)) &&
180 (pt.Y >= (-halfExtents.Y - tolerance)) && 180 (pt.Y >= (-halfExtents.Y - tolerance)) &&
181 (pt.Z <= ( halfExtents.Z + tolerance)) && 181 (pt.Z <= ( halfExtents.Z + tolerance)) &&
182 (pt.Z >= (-halfExtents.Z - tolerance)); 182 (pt.Z >= (-halfExtents.Z - tolerance));
183 183
184 return result; 184 return result;
185 } 185 }
186 186
187 public override Vector3 LocalGetSupportingVertex(Vector3 vec) 187 public override Vector3 LocalGetSupportingVertex(Vector3 vec)
188 { 188 {
189 Vector3 halfExtents = HalfExtents; 189 Vector3 halfExtents = HalfExtents;
190 190
191 return new Vector3( vec.X < 0.0f ? -halfExtents.X : halfExtents.X, 191 return new Vector3( vec.X < 0.0f ? -halfExtents.X : halfExtents.X,
192 vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y, 192 vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y,
193 vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z); 193 vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z);
194 } 194 }
195 195
196 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) 196 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
197 { 197 {
198 Vector3 halfExtents = HalfExtents; 198 Vector3 halfExtents = HalfExtents;
199 Vector3 margin = new Vector3(Margin, Margin, Margin); 199 Vector3 margin = new Vector3(Margin, Margin, Margin);
200 halfExtents -= margin; 200 halfExtents -= margin;
201 201
202 return new Vector3( vec.X < 0.0f ? -halfExtents.X : halfExtents.X, 202 return new Vector3( vec.X < 0.0f ? -halfExtents.X : halfExtents.X,
203 vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y, 203 vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y,
204 vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z); 204 vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z);
205 } 205 }
206 206
207 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) 207 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
208 { 208 {
209 Vector3 halfExtents = HalfExtents; 209 Vector3 halfExtents = HalfExtents;
210 Vector3 margin = new Vector3(Margin, Margin, Margin); 210 Vector3 margin = new Vector3(Margin, Margin, Margin);
211 halfExtents -= margin; 211 halfExtents -= margin;
212 212
213 for (int i = 0; i < vectors.Length; i++) 213 for (int i = 0; i < vectors.Length; i++)
214 { 214 {
215 Vector3 vec = vectors[i]; 215 Vector3 vec = vectors[i];
216 supportVerticesOut[i] = new Vector3(vec.X < 0.0f ? -halfExtents.X : halfExtents.X, 216 supportVerticesOut[i] = new Vector3(vec.X < 0.0f ? -halfExtents.X : halfExtents.X,
217 vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y, 217 vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y,
218 vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z); 218 vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z);
219 } 219 }
220 } 220 }
221 221
222 public virtual void GetPlaneEquation(out Vector4 plane, int i) 222 public virtual void GetPlaneEquation(out Vector4 plane, int i)
223 { 223 {
224 Vector3 halfExtents = HalfExtents; 224 Vector3 halfExtents = HalfExtents;
225 225
226 switch (i) 226 switch (i)
227 { 227 {
228 case 0: 228 case 0:
229 plane = new Vector4(1, 0, 0, 0); 229 plane = new Vector4(1, 0, 0, 0);
230 plane.W = -halfExtents.X; 230 plane.W = -halfExtents.X;
231 break; 231 break;
232 case 1: 232 case 1:
233 plane = new Vector4(-1, 0, 0, 0); 233 plane = new Vector4(-1, 0, 0, 0);
234 plane.W = -halfExtents.X; 234 plane.W = -halfExtents.X;
235 break; 235 break;
236 case 2: 236 case 2:
237 plane = new Vector4(0, 1, 0, 0); 237 plane = new Vector4(0, 1, 0, 0);
238 plane.W = -halfExtents.Y; 238 plane.W = -halfExtents.Y;
239 break; 239 break;
240 case 3: 240 case 3:
241 plane = new Vector4(0, -1, 0, 0); 241 plane = new Vector4(0, -1, 0, 0);
242 plane.W = -halfExtents.Y; 242 plane.W = -halfExtents.Y;
243 break; 243 break;
244 case 4: 244 case 4:
245 plane = new Vector4(0, 0, 1, 0); 245 plane = new Vector4(0, 0, 1, 0);
246 plane.W = -halfExtents.Z; 246 plane.W = -halfExtents.Z;
247 break; 247 break;
248 case 5: 248 case 5:
249 plane = new Vector4(0, 0, -1, 0); 249 plane = new Vector4(0, 0, -1, 0);
250 plane.W = -halfExtents.Z; 250 plane.W = -halfExtents.Z;
251 break; 251 break;
252 default: 252 default:
253 throw new BulletException(); 253 throw new BulletException();
254 } 254 }
255 } 255 }
256 256
257 public override void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector) 257 public override void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector)
258 { 258 {
259 switch (index) 259 switch (index)
260 { 260 {
261 case 0: 261 case 0:
262 penetrationVector = new Vector3(1, 0, 0); 262 penetrationVector = new Vector3(1, 0, 0);
263 break; 263 break;
264 case 1: 264 case 1:
265 penetrationVector = new Vector3(-1, 0, 0); 265 penetrationVector = new Vector3(-1, 0, 0);
266 break; 266 break;
267 case 2: 267 case 2:
268 penetrationVector = new Vector3(0, 1, 0); 268 penetrationVector = new Vector3(0, 1, 0);
269 break; 269 break;
270 case 3: 270 case 3:
271 penetrationVector = new Vector3(0, -1, 0); 271 penetrationVector = new Vector3(0, -1, 0);
272 break; 272 break;
273 case 4: 273 case 4:
274 penetrationVector = new Vector3(0, 0, 1); 274 penetrationVector = new Vector3(0, 0, 1);
275 break; 275 break;
276 case 5: 276 case 5:
277 penetrationVector = new Vector3(0, 0, -1); 277 penetrationVector = new Vector3(0, 0, -1);
278 break; 278 break;
279 default: 279 default:
280 throw new BulletException(); 280 throw new BulletException();
281 } 281 }
282 } 282 }
283 283
284 public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) 284 public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
285 { 285 {
286 Vector3 halfExtents = HalfExtents; 286 Vector3 halfExtents = HalfExtents;
287 287
288 Matrix abs_b = MathHelper.Absolute(t); 288 Matrix abs_b = MathHelper.Absolute(t);
289 Vector3 center = t.Translation; 289 Vector3 center = t.Translation;
290 Vector3 row1 = new Vector3(abs_b.M11, abs_b.M12, abs_b.M13); 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); 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); 292 Vector3 row3 = new Vector3(abs_b.M31, abs_b.M32, abs_b.M33);
293 Vector3 extent = new Vector3(Vector3.Dot(row1, halfExtents), 293 Vector3 extent = new Vector3(Vector3.Dot(row1, halfExtents),
294 Vector3.Dot(row2, halfExtents), 294 Vector3.Dot(row2, halfExtents),
295 Vector3.Dot(row3, halfExtents)); 295 Vector3.Dot(row3, halfExtents));
296 extent += new Vector3(Margin, Margin, Margin); 296 extent += new Vector3(Margin, Margin, Margin);
297 297
298 aabbMin = center - extent; 298 aabbMin = center - extent;
299 aabbMax = center + extent; 299 aabbMax = center + extent;
300 } 300 }
301 301
302 public override void CalculateLocalInertia(float mass, out Vector3 inertia) 302 public override void CalculateLocalInertia(float mass, out Vector3 inertia)
303 { 303 {
304 Vector3 halfExtents = HalfExtents; 304 Vector3 halfExtents = HalfExtents;
305 305
306 float lx = 2f * (halfExtents.X); 306 float lx = 2f * (halfExtents.X);
307 float ly = 2f * (halfExtents.Y); 307 float ly = 2f * (halfExtents.Y);
308 float lz = 2f * (halfExtents.Z); 308 float lz = 2f * (halfExtents.Z);
309 309
310 inertia = new Vector3(); 310 inertia = new Vector3();
311 inertia.X = mass / (12.0f) * (ly * ly + lz * lz); 311 inertia.X = mass / (12.0f) * (ly * ly + lz * lz);
312 inertia.Y = mass / (12.0f) * (lx * lx + lz * lz); 312 inertia.Y = mass / (12.0f) * (lx * lx + lz * lz);
313 inertia.Z = mass / (12.0f) * (lx * lx + ly * ly); 313 inertia.Z = mass / (12.0f) * (lx * lx + ly * ly);
314 } 314 }
315 } 315 }
316} 316}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs
index 00247a0..03e1e44 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs
@@ -1,83 +1,83 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 class MyNodeOverlapCallback : INodeOverlapCallback 29 class MyNodeOverlapCallback : INodeOverlapCallback
30 { 30 {
31 StridingMeshInterface _meshInterface; 31 StridingMeshInterface _meshInterface;
32 ITriangleCallback _callback; 32 ITriangleCallback _callback;
33 Vector3[] _triangle = new Vector3[3]; 33 Vector3[] _triangle = new Vector3[3];
34 34
35 public MyNodeOverlapCallback(ITriangleCallback callback, StridingMeshInterface meshInterface) 35 public MyNodeOverlapCallback(ITriangleCallback callback, StridingMeshInterface meshInterface)
36 { 36 {
37 _meshInterface = meshInterface; 37 _meshInterface = meshInterface;
38 _callback = callback; 38 _callback = callback;
39 } 39 }
40 40
41 public void ProcessNode(OptimizedBvhNode node) 41 public void ProcessNode(OptimizedBvhNode node)
42 { 42 {
43 List<Vector3> verts; 43 List<Vector3> verts;
44 List<int> indicies; 44 List<int> indicies;
45 int numtriangles; 45 int numtriangles;
46 46
47 _meshInterface.GetLockedReadOnlyVertexIndexBase(out verts, out indicies, out numtriangles, node.SubPart); 47 _meshInterface.GetLockedReadOnlyVertexIndexBase(out verts, out indicies, out numtriangles, node.SubPart);
48 Vector3 meshScaling = _meshInterface.Scaling; 48 Vector3 meshScaling = _meshInterface.Scaling;
49 49
50 for (int j = 0; j < 3; j++) 50 for (int j = 0; j < 3; j++)
51 { 51 {
52 _triangle[j] = verts[indicies[j + node.TriangleIndex * 3]] * meshScaling; 52 _triangle[j] = verts[indicies[j + node.TriangleIndex * 3]] * meshScaling;
53 } 53 }
54 54
55 _callback.ProcessTriangle(_triangle, node.SubPart, node.TriangleIndex); 55 _callback.ProcessTriangle(_triangle, node.SubPart, node.TriangleIndex);
56 _meshInterface.UnLockReadOnlyVertexBase(node.SubPart); 56 _meshInterface.UnLockReadOnlyVertexBase(node.SubPart);
57 } 57 }
58 } 58 }
59 59
60 public class BvhTriangleMeshShape : TriangleMeshShape 60 public class BvhTriangleMeshShape : TriangleMeshShape
61 { 61 {
62 OptimizedBvh _bvh = new OptimizedBvh(); 62 OptimizedBvh _bvh = new OptimizedBvh();
63 63
64 public BvhTriangleMeshShape(StridingMeshInterface meshInterface) : base(meshInterface) 64 public BvhTriangleMeshShape(StridingMeshInterface meshInterface) : base(meshInterface)
65 { 65 {
66 _bvh.Build(meshInterface); 66 _bvh.Build(meshInterface);
67 } 67 }
68 68
69 public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) 69 public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)
70 { 70 {
71 MyNodeOverlapCallback myNodeCallback = new MyNodeOverlapCallback(callback, MeshInterface); 71 MyNodeOverlapCallback myNodeCallback = new MyNodeOverlapCallback(callback, MeshInterface);
72 _bvh.ReportAabbOverlappingNodex(myNodeCallback, aabbMin, aabbMax); 72 _bvh.ReportAabbOverlappingNodex(myNodeCallback, aabbMin, aabbMax);
73 } 73 }
74 74
75 public override string Name 75 public override string Name
76 { 76 {
77 get 77 get
78 { 78 {
79 return "BvhTriangleMesh"; 79 return "BvhTriangleMesh";
80 } 80 }
81 } 81 }
82 } 82 }
83} 83}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs
index a9ce3be..1a4fac2 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs
@@ -1,148 +1,148 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// CollisionShape provides generic interface for collidable objects 30 /// CollisionShape provides generic interface for collidable objects
31 /// </summary> 31 /// </summary>
32 public abstract class CollisionShape 32 public abstract class CollisionShape
33 { 33 {
34 //debugging support 34 //debugging support
35 private string _tempDebug; 35 private string _tempDebug;
36 36
37 public abstract string Name { get; } 37 public abstract string Name { get; }
38 public string ExtraDebugInfo { get { return _tempDebug; } set { _tempDebug = value; } } 38 public string ExtraDebugInfo { get { return _tempDebug; } set { _tempDebug = value; } }
39 39
40 public bool IsPolyhedral 40 public bool IsPolyhedral
41 { 41 {
42 get 42 get
43 { 43 {
44 return BroadphaseProxy.IsPolyhedral(ShapeType); 44 return BroadphaseProxy.IsPolyhedral(ShapeType);
45 } 45 }
46 } 46 }
47 47
48 public bool IsConvex 48 public bool IsConvex
49 { 49 {
50 get 50 get
51 { 51 {
52 return BroadphaseProxy.IsConvex(ShapeType); 52 return BroadphaseProxy.IsConvex(ShapeType);
53 } 53 }
54 } 54 }
55 public bool IsConcave 55 public bool IsConcave
56 { 56 {
57 get 57 get
58 { 58 {
59 return BroadphaseProxy.IsConcave(ShapeType); 59 return BroadphaseProxy.IsConcave(ShapeType);
60 } 60 }
61 } 61 }
62 public bool IsCompound 62 public bool IsCompound
63 { 63 {
64 get 64 get
65 { 65 {
66 return BroadphaseProxy.IsCompound(ShapeType); 66 return BroadphaseProxy.IsCompound(ShapeType);
67 } 67 }
68 } 68 }
69 69
70 //isInfinite is used to catch simulation error (aabb check) 70 //isInfinite is used to catch simulation error (aabb check)
71 public bool IsInfinite 71 public bool IsInfinite
72 { 72 {
73 get 73 get
74 { 74 {
75 return BroadphaseProxy.IsInfinite(ShapeType); 75 return BroadphaseProxy.IsInfinite(ShapeType);
76 } 76 }
77 } 77 }
78 78
79 public abstract float Margin { get; set; } 79 public abstract float Margin { get; set; }
80 public abstract Vector3 LocalScaling { get; set; } 80 public abstract Vector3 LocalScaling { get; set; }
81 public abstract BroadphaseNativeTypes ShapeType { get; } 81 public abstract BroadphaseNativeTypes ShapeType { get; }
82 82
83 83
84 public virtual void GetBoundingSphere(out Vector3 center, out float radius) 84 public virtual void GetBoundingSphere(out Vector3 center, out float radius)
85 { 85 {
86 Matrix tr = Matrix.Identity; 86 Matrix tr = Matrix.Identity;
87 Vector3 aabbMin, aabbMax; 87 Vector3 aabbMin, aabbMax;
88 88
89 GetAabb(tr, out aabbMin, out aabbMax); 89 GetAabb(tr, out aabbMin, out aabbMax);
90 90
91 radius = (aabbMax - aabbMin).Length() * 0.5f; 91 radius = (aabbMax - aabbMin).Length() * 0.5f;
92 center = (aabbMin + aabbMax) * 0.5f; 92 center = (aabbMin + aabbMax) * 0.5f;
93 } 93 }
94 94
95 public virtual float GetAngularMotionDisc() 95 public virtual float GetAngularMotionDisc()
96 { 96 {
97 Vector3 center; 97 Vector3 center;
98 float disc; 98 float disc;
99 GetBoundingSphere(out center, out disc); 99 GetBoundingSphere(out center, out disc);
100 disc += center.Length(); 100 disc += center.Length();
101 return disc; 101 return disc;
102 } 102 }
103 103
104 //calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep) 104 //calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep)
105 //result is conservative 105 //result is conservative
106 public void CalculateTemporalAabb(Matrix currentTransform, Vector3 linearVelocity, Vector3 angularVelocity, float timeStep, out Vector3 temporalAabbMin, out Vector3 temporalAabbMax) 106 public void CalculateTemporalAabb(Matrix currentTransform, Vector3 linearVelocity, Vector3 angularVelocity, float timeStep, out Vector3 temporalAabbMin, out Vector3 temporalAabbMax)
107 { 107 {
108 //start with static aabb 108 //start with static aabb
109 GetAabb(currentTransform, out temporalAabbMin, out temporalAabbMax); 109 GetAabb(currentTransform, out temporalAabbMin, out temporalAabbMax);
110 110
111 float temporalAabbMaxx = temporalAabbMax.X; 111 float temporalAabbMaxx = temporalAabbMax.X;
112 float temporalAabbMaxy = temporalAabbMax.Y; 112 float temporalAabbMaxy = temporalAabbMax.Y;
113 float temporalAabbMaxz = temporalAabbMax.Z; 113 float temporalAabbMaxz = temporalAabbMax.Z;
114 float temporalAabbMinx = temporalAabbMin.X; 114 float temporalAabbMinx = temporalAabbMin.X;
115 float temporalAabbMiny = temporalAabbMin.Y; 115 float temporalAabbMiny = temporalAabbMin.Y;
116 float temporalAabbMinz = temporalAabbMin.Z; 116 float temporalAabbMinz = temporalAabbMin.Z;
117 117
118 // add linear motion 118 // add linear motion
119 Vector3 linMotion = linearVelocity * timeStep; 119 Vector3 linMotion = linearVelocity * timeStep;
120 //todo: simd would have a vector max/min operation, instead of per-element access 120 //todo: simd would have a vector max/min operation, instead of per-element access
121 if (linMotion.X > 0) 121 if (linMotion.X > 0)
122 temporalAabbMaxx += linMotion.X; 122 temporalAabbMaxx += linMotion.X;
123 else 123 else
124 temporalAabbMinx += linMotion.X; 124 temporalAabbMinx += linMotion.X;
125 if (linMotion.Y > 0) 125 if (linMotion.Y > 0)
126 temporalAabbMaxy += linMotion.Y; 126 temporalAabbMaxy += linMotion.Y;
127 else 127 else
128 temporalAabbMiny += linMotion.Y; 128 temporalAabbMiny += linMotion.Y;
129 if (linMotion.Z > 0) 129 if (linMotion.Z > 0)
130 temporalAabbMaxz += linMotion.Z; 130 temporalAabbMaxz += linMotion.Z;
131 else 131 else
132 temporalAabbMinz += linMotion.Z; 132 temporalAabbMinz += linMotion.Z;
133 133
134 //add conservative angular motion 134 //add conservative angular motion
135 float angularMotion = angularVelocity.Length() * GetAngularMotionDisc() * timeStep; 135 float angularMotion = angularVelocity.Length() * GetAngularMotionDisc() * timeStep;
136 Vector3 angularMotion3d = new Vector3(angularMotion, angularMotion, angularMotion); 136 Vector3 angularMotion3d = new Vector3(angularMotion, angularMotion, angularMotion);
137 temporalAabbMin = new Vector3(temporalAabbMinx, temporalAabbMiny, temporalAabbMinz); 137 temporalAabbMin = new Vector3(temporalAabbMinx, temporalAabbMiny, temporalAabbMinz);
138 temporalAabbMax = new Vector3(temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz); 138 temporalAabbMax = new Vector3(temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz);
139 139
140 temporalAabbMin -= angularMotion3d; 140 temporalAabbMin -= angularMotion3d;
141 temporalAabbMax += angularMotion3d; 141 temporalAabbMax += angularMotion3d;
142 } 142 }
143 143
144 public abstract void GetAabb(Matrix transform, out Vector3 aabbMin, out Vector3 aabbMax); 144 public abstract void GetAabb(Matrix transform, out Vector3 aabbMin, out Vector3 aabbMax);
145 145
146 public abstract void CalculateLocalInertia(float mass, out Vector3 inertia); 146 public abstract void CalculateLocalInertia(float mass, out Vector3 inertia);
147 } 147 }
148} 148}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs
index 08aeec8..bd13ffd 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs
@@ -1,183 +1,183 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// CompoundShape allows to store multiple other CollisionShapes 30 /// CompoundShape allows to store multiple other CollisionShapes
31 /// This allows for concave collision objects. This is more general then the Static Concave TriangleMeshShape. 31 /// This allows for concave collision objects. This is more general then the Static Concave TriangleMeshShape.
32 /// </summary> 32 /// </summary>
33 public class CompoundShape : CollisionShape 33 public class CompoundShape : CollisionShape
34 { 34 {
35 private List<Matrix> _childTransforms = new List<Matrix>(); 35 private List<Matrix> _childTransforms = new List<Matrix>();
36 private List<CollisionShape> _childShapes = new List<CollisionShape>(); 36 private List<CollisionShape> _childShapes = new List<CollisionShape>();
37 private Vector3 _localAabbMin; 37 private Vector3 _localAabbMin;
38 private Vector3 _localAabbMax; 38 private Vector3 _localAabbMax;
39 39
40 private OptimizedBvh _aabbTree; 40 private OptimizedBvh _aabbTree;
41 private float _collisionMargin; 41 private float _collisionMargin;
42 private Vector3 _localScaling; 42 private Vector3 _localScaling;
43 43
44 public CompoundShape() 44 public CompoundShape()
45 { 45 {
46 _localAabbMin = new Vector3(1e30f, 1e30f, 1e30f); 46 _localAabbMin = new Vector3(1e30f, 1e30f, 1e30f);
47 _localAabbMax = new Vector3(-1e30f, -1e30f, -1e30f); 47 _localAabbMax = new Vector3(-1e30f, -1e30f, -1e30f);
48 _aabbTree = null; 48 _aabbTree = null;
49 _collisionMargin = 0f; 49 _collisionMargin = 0f;
50 _localScaling = new Vector3(1f, 1f, 1f); 50 _localScaling = new Vector3(1f, 1f, 1f);
51 } 51 }
52 52
53 public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) 53 public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
54 { 54 {
55 Vector3 localHalfExtents = 0.5f * (_localAabbMax - _localAabbMin); 55 Vector3 localHalfExtents = 0.5f * (_localAabbMax - _localAabbMin);
56 Vector3 localCenter = 0.5f * (_localAabbMax + _localAabbMin); 56 Vector3 localCenter = 0.5f * (_localAabbMax + _localAabbMin);
57 57
58 Matrix abs_b = MathHelper.Absolute(t); 58 Matrix abs_b = MathHelper.Absolute(t);
59 59
60 Vector3 row1 = new Vector3(abs_b.M11, abs_b.M12, abs_b.M13); 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); 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); 62 Vector3 row3 = new Vector3(abs_b.M31, abs_b.M32, abs_b.M33);
63 63
64 Vector3 center = new Vector3(Vector3.Dot(row1, localCenter) + t.Translation.X, 64 Vector3 center = new Vector3(Vector3.Dot(row1, localCenter) + t.Translation.X,
65 Vector3.Dot(row2, localCenter) + t.Translation.Y, 65 Vector3.Dot(row2, localCenter) + t.Translation.Y,
66 Vector3.Dot(row3, localCenter) + t.Translation.Z); 66 Vector3.Dot(row3, localCenter) + t.Translation.Z);
67 67
68 Vector3 extent = new Vector3(Vector3.Dot(row1, localHalfExtents), 68 Vector3 extent = new Vector3(Vector3.Dot(row1, localHalfExtents),
69 Vector3.Dot(row2, localHalfExtents), 69 Vector3.Dot(row2, localHalfExtents),
70 Vector3.Dot(row3, localHalfExtents)); 70 Vector3.Dot(row3, localHalfExtents));
71 71
72 aabbMin = center - extent; 72 aabbMin = center - extent;
73 aabbMax = center + extent; 73 aabbMax = center + extent;
74 } 74 }
75 75
76 public override BroadphaseNativeTypes ShapeType 76 public override BroadphaseNativeTypes ShapeType
77 { 77 {
78 get 78 get
79 { 79 {
80 return BroadphaseNativeTypes.Compound; 80 return BroadphaseNativeTypes.Compound;
81 } 81 }
82 } 82 }
83 83
84 public override Vector3 LocalScaling 84 public override Vector3 LocalScaling
85 { 85 {
86 get 86 get
87 { 87 {
88 return _localScaling; 88 return _localScaling;
89 } 89 }
90 set 90 set
91 { 91 {
92 _localScaling = value; 92 _localScaling = value;
93 } 93 }
94 } 94 }
95 95
96 public override string Name 96 public override string Name
97 { 97 {
98 get 98 get
99 { 99 {
100 return "Compound"; 100 return "Compound";
101 } 101 }
102 } 102 }
103 103
104 public override float Margin 104 public override float Margin
105 { 105 {
106 get 106 get
107 { 107 {
108 return _collisionMargin; 108 return _collisionMargin;
109 } 109 }
110 set 110 set
111 { 111 {
112 _collisionMargin = value; 112 _collisionMargin = value;
113 } 113 }
114 } 114 }
115 115
116 public int ChildShapeCount { get { return _childShapes.Count; } } 116 public int ChildShapeCount { get { return _childShapes.Count; } }
117 //this is optional, but should make collision queries faster, by culling non-overlapping nodes 117 //this is optional, but should make collision queries faster, by culling non-overlapping nodes
118 public OptimizedBvh AabbTree { get { return _aabbTree; } } 118 public OptimizedBvh AabbTree { get { return _aabbTree; } }
119 119
120 public CollisionShape GetChildShape(int index) 120 public CollisionShape GetChildShape(int index)
121 { 121 {
122 return _childShapes[index]; 122 return _childShapes[index];
123 } 123 }
124 124
125 public Matrix GetChildTransform(int index) 125 public Matrix GetChildTransform(int index)
126 { 126 {
127 return _childTransforms[index]; 127 return _childTransforms[index];
128 } 128 }
129 129
130 public override void CalculateLocalInertia(float mass, out Vector3 inertia) 130 public override void CalculateLocalInertia(float mass, out Vector3 inertia)
131 { 131 {
132 //approximation: take the inertia from the aabb for now 132 //approximation: take the inertia from the aabb for now
133 Matrix ident = Matrix.Identity; 133 Matrix ident = Matrix.Identity;
134 Vector3 aabbMin, aabbMax; 134 Vector3 aabbMin, aabbMax;
135 GetAabb(ident, out aabbMin, out aabbMax); 135 GetAabb(ident, out aabbMin, out aabbMax);
136 136
137 Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; 137 Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f;
138 138
139 float lx = 2f * (halfExtents.X); 139 float lx = 2f * (halfExtents.X);
140 float ly = 2f * (halfExtents.Y); 140 float ly = 2f * (halfExtents.Y);
141 float lz = 2f * (halfExtents.Z); 141 float lz = 2f * (halfExtents.Z);
142 142
143 inertia = new Vector3(); 143 inertia = new Vector3();
144 inertia.X = mass / (12.0f) * (ly * ly + lz * lz); 144 inertia.X = mass / (12.0f) * (ly * ly + lz * lz);
145 inertia.Y = mass / (12.0f) * (lx * lx + lz * lz); 145 inertia.Y = mass / (12.0f) * (lx * lx + lz * lz);
146 inertia.Z = mass / (12.0f) * (lx * lx + ly * ly); 146 inertia.Z = mass / (12.0f) * (lx * lx + ly * ly);
147 } 147 }
148 148
149 public void AddChildShape(Matrix localTransform, CollisionShape shape) 149 public void AddChildShape(Matrix localTransform, CollisionShape shape)
150 { 150 {
151 _childTransforms.Add(localTransform); 151 _childTransforms.Add(localTransform);
152 _childShapes.Add(shape); 152 _childShapes.Add(shape);
153 153
154 //extend the local aabbMin/aabbMax 154 //extend the local aabbMin/aabbMax
155 Vector3 localAabbMin, localAabbMax; 155 Vector3 localAabbMin, localAabbMax;
156 shape.GetAabb(localTransform, out localAabbMin, out localAabbMax); 156 shape.GetAabb(localTransform, out localAabbMin, out localAabbMax);
157 if (_localAabbMin.X > localAabbMin.X) 157 if (_localAabbMin.X > localAabbMin.X)
158 { 158 {
159 _localAabbMin.X = localAabbMin.X; 159 _localAabbMin.X = localAabbMin.X;
160 } 160 }
161 if (_localAabbMax.X < localAabbMax.X) 161 if (_localAabbMax.X < localAabbMax.X)
162 { 162 {
163 _localAabbMax.X = localAabbMax.X; 163 _localAabbMax.X = localAabbMax.X;
164 } 164 }
165 if (_localAabbMin.Y > localAabbMin.Y) 165 if (_localAabbMin.Y > localAabbMin.Y)
166 { 166 {
167 _localAabbMin.Y = localAabbMin.Y; 167 _localAabbMin.Y = localAabbMin.Y;
168 } 168 }
169 if (_localAabbMax.Y < localAabbMax.Y) 169 if (_localAabbMax.Y < localAabbMax.Y)
170 { 170 {
171 _localAabbMax.Y = localAabbMax.Y; 171 _localAabbMax.Y = localAabbMax.Y;
172 } 172 }
173 if (_localAabbMin.Z > localAabbMin.Z) 173 if (_localAabbMin.Z > localAabbMin.Z)
174 { 174 {
175 _localAabbMin.Z = localAabbMin.Z; 175 _localAabbMin.Z = localAabbMin.Z;
176 } 176 }
177 if (_localAabbMax.Z < localAabbMax.Z) 177 if (_localAabbMax.Z < localAabbMax.Z)
178 { 178 {
179 _localAabbMax.Z = localAabbMax.Z; 179 _localAabbMax.Z = localAabbMax.Z;
180 } 180 }
181 } 181 }
182 } 182 }
183} 183}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs
index 1e773f0..c0b177d 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs
@@ -1,55 +1,55 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public abstract class ConcaveShape : CollisionShape 29 public abstract class ConcaveShape : CollisionShape
30 { 30 {
31 private float _collisionMargin; 31 private float _collisionMargin;
32 32
33 public ConcaveShape() { } 33 public ConcaveShape() { }
34 34
35 public float CollisionMargin 35 public float CollisionMargin
36 { 36 {
37 get { return _collisionMargin; } 37 get { return _collisionMargin; }
38 set { _collisionMargin = value; } 38 set { _collisionMargin = value; }
39 } 39 }
40 40
41 public override float Margin 41 public override float Margin
42 { 42 {
43 get 43 get
44 { 44 {
45 return _collisionMargin; 45 return _collisionMargin;
46 } 46 }
47 set 47 set
48 { 48 {
49 _collisionMargin = value; 49 _collisionMargin = value;
50 } 50 }
51 } 51 }
52 52
53 public abstract void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax); 53 public abstract void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax);
54 } 54 }
55} \ No newline at end of file 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
index 361c277..52084f4 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs
@@ -1,208 +1,208 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// ConeShape implements a Cone shape, around the X axis 30 /// ConeShape implements a Cone shape, around the X axis
31 /// </summary> 31 /// </summary>
32 public class ConeShapeX : ConeShape 32 public class ConeShapeX : ConeShape
33 { 33 {
34 public ConeShapeX(float radius, float height) 34 public ConeShapeX(float radius, float height)
35 : base(radius, height) 35 : base(radius, height)
36 { 36 {
37 ConeUpIndex = 0; 37 ConeUpIndex = 0;
38 } 38 }
39 } 39 }
40 40
41 /// <summary> 41 /// <summary>
42 /// ConeShape implements a Cone shape, around the Z axis 42 /// ConeShape implements a Cone shape, around the Z axis
43 /// </summary> 43 /// </summary>
44 public class ConeShapeZ : ConeShape 44 public class ConeShapeZ : ConeShape
45 { 45 {
46 public ConeShapeZ(float radius, float height) 46 public ConeShapeZ(float radius, float height)
47 : base(radius, height) 47 : base(radius, height)
48 { 48 {
49 ConeUpIndex = 2; 49 ConeUpIndex = 2;
50 } 50 }
51 } 51 }
52 52
53 /// <summary> 53 /// <summary>
54 /// ConeShape implements a Cone shape, around the Y axis 54 /// ConeShape implements a Cone shape, around the Y axis
55 /// </summary> 55 /// </summary>
56 public class ConeShape : ConvexShape 56 public class ConeShape : ConvexShape
57 { 57 {
58 private float _sinAngle; 58 private float _sinAngle;
59 private float _radius; 59 private float _radius;
60 private float _height; 60 private float _height;
61 private int[] _coneIndices = new int[3]; 61 private int[] _coneIndices = new int[3];
62 62
63 public ConeShape(float radius, float height) 63 public ConeShape(float radius, float height)
64 { 64 {
65 _radius = radius; 65 _radius = radius;
66 _height = height; 66 _height = height;
67 ConeUpIndex = 1; 67 ConeUpIndex = 1;
68 _sinAngle = (_radius / (float)Math.Sqrt(_radius * _radius + _height * _height)); 68 _sinAngle = (_radius / (float)Math.Sqrt(_radius * _radius + _height * _height));
69 } 69 }
70 70
71 public float Radius { get { return _radius; } } 71 public float Radius { get { return _radius; } }
72 public float Height { get { return _height; } } 72 public float Height { get { return _height; } }
73 73
74 public override BroadphaseNativeTypes ShapeType 74 public override BroadphaseNativeTypes ShapeType
75 { 75 {
76 get 76 get
77 { 77 {
78 return BroadphaseNativeTypes.Cone; 78 return BroadphaseNativeTypes.Cone;
79 } 79 }
80 } 80 }
81 81
82 public override string Name 82 public override string Name
83 { 83 {
84 get 84 get
85 { 85 {
86 return "Cone"; 86 return "Cone";
87 } 87 }
88 } 88 }
89 89
90 //choose upAxis index 90 //choose upAxis index
91 public int ConeUpIndex 91 public int ConeUpIndex
92 { 92 {
93 get { return _coneIndices[1]; } 93 get { return _coneIndices[1]; }
94 set 94 set
95 { 95 {
96 switch (value) 96 switch (value)
97 { 97 {
98 case 0: 98 case 0:
99 _coneIndices[0] = 1; 99 _coneIndices[0] = 1;
100 _coneIndices[1] = 0; 100 _coneIndices[1] = 0;
101 _coneIndices[2] = 2; 101 _coneIndices[2] = 2;
102 break; 102 break;
103 case 1: 103 case 1:
104 _coneIndices[0] = 0; 104 _coneIndices[0] = 0;
105 _coneIndices[1] = 1; 105 _coneIndices[1] = 1;
106 _coneIndices[2] = 2; 106 _coneIndices[2] = 2;
107 break; 107 break;
108 case 2: 108 case 2:
109 _coneIndices[0] = 0; 109 _coneIndices[0] = 0;
110 _coneIndices[1] = 2; 110 _coneIndices[1] = 2;
111 _coneIndices[2] = 1; 111 _coneIndices[2] = 1;
112 break; 112 break;
113 default: 113 default:
114 BulletDebug.Assert(false); 114 BulletDebug.Assert(false);
115 break; 115 break;
116 } 116 }
117 } 117 }
118 } 118 }
119 119
120 private Vector3 ConeLocalSupport(Vector3 v) 120 private Vector3 ConeLocalSupport(Vector3 v)
121 { 121 {
122 float halfHeight = _height * 0.5f; 122 float halfHeight = _height * 0.5f;
123 bool condition; 123 bool condition;
124 124
125 if (_coneIndices[1] == 0) 125 if (_coneIndices[1] == 0)
126 condition = v.X > v.Length() * _sinAngle; 126 condition = v.X > v.Length() * _sinAngle;
127 else if (_coneIndices[1] == 1) 127 else if (_coneIndices[1] == 1)
128 condition = v.Y > v.Length() * _sinAngle; 128 condition = v.Y > v.Length() * _sinAngle;
129 else 129 else
130 condition = v.Z > v.Length() * _sinAngle; 130 condition = v.Z > v.Length() * _sinAngle;
131 131
132 if (condition) 132 if (condition)
133 { 133 {
134 Vector3 tmp = new Vector3(); 134 Vector3 tmp = new Vector3();
135 MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], halfHeight); 135 MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], halfHeight);
136 return tmp; 136 return tmp;
137 } 137 }
138 else 138 else
139 { 139 {
140 float s = (float)Math.Sqrt(MathHelper.GetValueByIndex(v, _coneIndices[0]) * MathHelper.GetValueByIndex(v, _coneIndices[0]) 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])); 141 + MathHelper.GetValueByIndex(v, _coneIndices[2]) * MathHelper.GetValueByIndex(v, _coneIndices[2]));
142 if (s > MathHelper.Epsilon) 142 if (s > MathHelper.Epsilon)
143 { 143 {
144 float d = _radius / s; 144 float d = _radius / s;
145 Vector3 tmp = new Vector3(); 145 Vector3 tmp = new Vector3();
146 MathHelper.SetValueByIndex(ref tmp, _coneIndices[0], MathHelper.GetValueByIndex(v, _coneIndices[0]) * d); 146 MathHelper.SetValueByIndex(ref tmp, _coneIndices[0], MathHelper.GetValueByIndex(v, _coneIndices[0]) * d);
147 MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], -halfHeight); 147 MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], -halfHeight);
148 MathHelper.SetValueByIndex(ref tmp, _coneIndices[2], MathHelper.GetValueByIndex(v, _coneIndices[2]) * d); 148 MathHelper.SetValueByIndex(ref tmp, _coneIndices[2], MathHelper.GetValueByIndex(v, _coneIndices[2]) * d);
149 return tmp; 149 return tmp;
150 } 150 }
151 else 151 else
152 { 152 {
153 Vector3 tmp = new Vector3(); 153 Vector3 tmp = new Vector3();
154 MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], -halfHeight); 154 MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], -halfHeight);
155 return tmp; 155 return tmp;
156 } 156 }
157 } 157 }
158 } 158 }
159 159
160 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) 160 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
161 { 161 {
162 return ConeLocalSupport(vec); 162 return ConeLocalSupport(vec);
163 } 163 }
164 164
165 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) 165 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
166 { 166 {
167 for (int i = 0; i < vectors.Length; i++) 167 for (int i = 0; i < vectors.Length; i++)
168 supportVerticesOut[i] = ConeLocalSupport(vectors[i]); 168 supportVerticesOut[i] = ConeLocalSupport(vectors[i]);
169 } 169 }
170 170
171 public override void CalculateLocalInertia(float mass, out Vector3 inertia) 171 public override void CalculateLocalInertia(float mass, out Vector3 inertia)
172 { 172 {
173 Matrix identity = Matrix.Identity; 173 Matrix identity = Matrix.Identity;
174 Vector3 aabbMin, aabbMax; 174 Vector3 aabbMin, aabbMax;
175 GetAabb(identity, out aabbMin, out aabbMax); 175 GetAabb(identity, out aabbMin, out aabbMax);
176 176
177 Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; 177 Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f;
178 178
179 float margin = Margin; 179 float margin = Margin;
180 180
181 float lx = 2f * (halfExtents.X + margin); 181 float lx = 2f * (halfExtents.X + margin);
182 float ly = 2f * (halfExtents.Y + margin); 182 float ly = 2f * (halfExtents.Y + margin);
183 float lz = 2f * (halfExtents.Z + margin); 183 float lz = 2f * (halfExtents.Z + margin);
184 float x2 = lx * lx; 184 float x2 = lx * lx;
185 float y2 = ly * ly; 185 float y2 = ly * ly;
186 float z2 = lz * lz; 186 float z2 = lz * lz;
187 float scaledmass = mass * 0.08333333f; 187 float scaledmass = mass * 0.08333333f;
188 188
189 inertia = scaledmass * (new Vector3(y2 + z2, x2 + z2, x2 + y2)); 189 inertia = scaledmass * (new Vector3(y2 + z2, x2 + z2, x2 + y2));
190 } 190 }
191 191
192 public override Vector3 LocalGetSupportingVertex(Vector3 vec) 192 public override Vector3 LocalGetSupportingVertex(Vector3 vec)
193 { 193 {
194 Vector3 supVertex = ConeLocalSupport(vec); 194 Vector3 supVertex = ConeLocalSupport(vec);
195 if (Margin != 0) 195 if (Margin != 0)
196 { 196 {
197 Vector3 vecnorm = vec; 197 Vector3 vecnorm = vec;
198 if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) 198 if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon))
199 { 199 {
200 vecnorm = new Vector3(-1f, -1f, -1f); 200 vecnorm = new Vector3(-1f, -1f, -1f);
201 } 201 }
202 vecnorm = Vector3.Normalize(vecnorm); 202 vecnorm = Vector3.Normalize(vecnorm);
203 supVertex += Margin * vecnorm; 203 supVertex += Margin * vecnorm;
204 } 204 }
205 return supVertex; 205 return supVertex;
206 } 206 }
207 } 207 }
208} 208}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs
index eb78533..8d06c90 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs
@@ -1,184 +1,184 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// ConvexHullShape implements an implicit (getSupportingVertex) Convex Hull of a Point Cloud (vertices) 30 /// ConvexHullShape implements an implicit (getSupportingVertex) Convex Hull of a Point Cloud (vertices)
31 /// No connectivity is needed. localGetSupportingVertex iterates linearly though all 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. 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) 33 /// (memory is much slower then the cpu)
34 /// </summary> 34 /// </summary>
35 public class ConvexHullShape : PolyhedralConvexShape 35 public class ConvexHullShape : PolyhedralConvexShape
36 { 36 {
37 private List<Vector3> _points = new List<Vector3>(); 37 private List<Vector3> _points = new List<Vector3>();
38 38
39 public ConvexHullShape() { } 39 public ConvexHullShape() { }
40 40
41 public override int VertexCount 41 public override int VertexCount
42 { 42 {
43 get 43 get
44 { 44 {
45 return _points.Count; 45 return _points.Count;
46 } 46 }
47 } 47 }
48 48
49 public override int EdgeCount 49 public override int EdgeCount
50 { 50 {
51 get 51 get
52 { 52 {
53 return _points.Count; 53 return _points.Count;
54 } 54 }
55 } 55 }
56 56
57 public override int PlaneCount 57 public override int PlaneCount
58 { 58 {
59 get 59 get
60 { 60 {
61 return 0; 61 return 0;
62 } 62 }
63 } 63 }
64 64
65 public override BroadphaseNativeTypes ShapeType 65 public override BroadphaseNativeTypes ShapeType
66 { 66 {
67 get 67 get
68 { 68 {
69 return BroadphaseNativeTypes.ConvexHull; 69 return BroadphaseNativeTypes.ConvexHull;
70 } 70 }
71 } 71 }
72 72
73 public override string Name 73 public override string Name
74 { 74 {
75 get 75 get
76 { 76 {
77 return "Convex"; 77 return "Convex";
78 } 78 }
79 } 79 }
80 80
81 public override Vector3 LocalGetSupportingVertex(Vector3 vec) 81 public override Vector3 LocalGetSupportingVertex(Vector3 vec)
82 { 82 {
83 Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); 83 Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec);
84 84
85 if (Margin != 0) 85 if (Margin != 0)
86 { 86 {
87 Vector3 vecnorm = vec; 87 Vector3 vecnorm = vec;
88 if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) 88 if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon))
89 { 89 {
90 vecnorm=new Vector3(-1, -1, -1); 90 vecnorm=new Vector3(-1, -1, -1);
91 } 91 }
92 vecnorm = Vector3.Normalize(vecnorm); 92 vecnorm = Vector3.Normalize(vecnorm);
93 supVertex += Margin * vecnorm; 93 supVertex += Margin * vecnorm;
94 } 94 }
95 return supVertex; 95 return supVertex;
96 } 96 }
97 97
98 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec0) 98 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec0)
99 { 99 {
100 Vector3 supVec = new Vector3(); 100 Vector3 supVec = new Vector3();
101 float newDot, maxDot = -1e30f; 101 float newDot, maxDot = -1e30f;
102 102
103 Vector3 vec = vec0; 103 Vector3 vec = vec0;
104 float lenSqr = vec.LengthSquared(); 104 float lenSqr = vec.LengthSquared();
105 if (lenSqr < 0.0001f) 105 if (lenSqr < 0.0001f)
106 { 106 {
107 vec = new Vector3(1, 0, 0); 107 vec = new Vector3(1, 0, 0);
108 } 108 }
109 else 109 else
110 { 110 {
111 float rlen = 1f / (float)Math.Sqrt(lenSqr); 111 float rlen = 1f / (float)Math.Sqrt(lenSqr);
112 vec *= rlen; 112 vec *= rlen;
113 } 113 }
114 114
115 for (int i = 0; i < _points.Count; i++) 115 for (int i = 0; i < _points.Count; i++)
116 { 116 {
117 Vector3 vtx = _points[i] * LocalScaling; 117 Vector3 vtx = _points[i] * LocalScaling;
118 118
119 newDot = Vector3.Dot(vec, vtx); 119 newDot = Vector3.Dot(vec, vtx);
120 if (newDot > maxDot) 120 if (newDot > maxDot)
121 { 121 {
122 maxDot = newDot; 122 maxDot = newDot;
123 supVec = vtx; 123 supVec = vtx;
124 } 124 }
125 } 125 }
126 return supVec; 126 return supVec;
127 } 127 }
128 128
129 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) 129 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
130 { 130 {
131 float newDot; 131 float newDot;
132 //use 'w' component of supportVerticesOut? 132 //use 'w' component of supportVerticesOut?
133 /*{ 133 /*{
134 for (int i = 0; i < numVectors; i++) 134 for (int i = 0; i < numVectors; i++)
135 { 135 {
136 supportVerticesOut[i][3] = -1e30f; 136 supportVerticesOut[i][3] = -1e30f;
137 } 137 }
138 }*/ 138 }*/
139 #warning Warning! 139 #warning Warning!
140 for (int i = 0; i < _points.Count; i++) 140 for (int i = 0; i < _points.Count; i++)
141 { 141 {
142 Vector3 vtx = _points[i] * LocalScaling; 142 Vector3 vtx = _points[i] * LocalScaling;
143 143
144 for (int j = 0; j < vectors.Length; j++) 144 for (int j = 0; j < vectors.Length; j++)
145 { 145 {
146 newDot = Vector3.Dot(vectors[j], vtx); 146 newDot = Vector3.Dot(vectors[j], vtx);
147 if (newDot > -1e30f) 147 if (newDot > -1e30f)
148 { 148 {
149 //WARNING: don't swap next lines, the w component would get overwritten! 149 //WARNING: don't swap next lines, the w component would get overwritten!
150 supportVerticesOut[j] = vtx; 150 supportVerticesOut[j] = vtx;
151 //supportVerticesOut[j][3] = newDot; 151 //supportVerticesOut[j][3] = newDot;
152 #warning Warning! 152 #warning Warning!
153 } 153 }
154 } 154 }
155 } 155 }
156 } 156 }
157 157
158 public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) 158 public override void GetEdge(int i, out Vector3 pa, out Vector3 pb)
159 { 159 {
160 int index0 = i % _points.Count; 160 int index0 = i % _points.Count;
161 int index1 = (i + 1) % _points.Count; 161 int index1 = (i + 1) % _points.Count;
162 pa = _points[index0] * LocalScaling; 162 pa = _points[index0] * LocalScaling;
163 pb = _points[index1] * LocalScaling; 163 pb = _points[index1] * LocalScaling;
164 } 164 }
165 165
166 public override void GetVertex(int i, out Vector3 vtx) 166 public override void GetVertex(int i, out Vector3 vtx)
167 { 167 {
168 vtx = _points[i] * LocalScaling; 168 vtx = _points[i] * LocalScaling;
169 } 169 }
170 170
171 public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) 171 public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i)
172 { 172 {
173 planeNormal = new Vector3(); 173 planeNormal = new Vector3();
174 planeSupport = new Vector3(); 174 planeSupport = new Vector3();
175 BulletDebug.Assert(false); 175 BulletDebug.Assert(false);
176 } 176 }
177 177
178 public override bool IsInside(Vector3 pt, float tolerance) 178 public override bool IsInside(Vector3 pt, float tolerance)
179 { 179 {
180 BulletDebug.Assert(false); 180 BulletDebug.Assert(false);
181 return false; 181 return false;
182 } 182 }
183 } 183 }
184} 184}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs
index 02d678e..d5c2fbd 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs
@@ -1,141 +1,141 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// ConvexShape is an abstract shape interface. 30 /// ConvexShape is an abstract shape interface.
31 /// The explicit part provides plane-equations, the implicit part provides GetClosestPoint interface. 31 /// The explicit part provides plane-equations, the implicit part provides GetClosestPoint interface.
32 /// used in combination with GJK or btConvexCast 32 /// used in combination with GJK or btConvexCast
33 /// </summary> 33 /// </summary>
34 public abstract class ConvexShape : CollisionShape 34 public abstract class ConvexShape : CollisionShape
35 { 35 {
36 private const int _maxPreferredPenetrationDirections = 10; 36 private const int _maxPreferredPenetrationDirections = 10;
37 private const float _convexDistanceMargin = 0.04f; 37 private const float _convexDistanceMargin = 0.04f;
38 38
39 private Vector3 _localScaling; 39 private Vector3 _localScaling;
40 private Vector3 _implicitShapeDimensions; 40 private Vector3 _implicitShapeDimensions;
41 private float _collisionMargin; 41 private float _collisionMargin;
42 42
43 public ConvexShape() 43 public ConvexShape()
44 : base() 44 : base()
45 { 45 {
46 _localScaling = Vector3.One; 46 _localScaling = Vector3.One;
47 _collisionMargin = ConvexDistanceMargin; 47 _collisionMargin = ConvexDistanceMargin;
48 } 48 }
49 49
50 public static int MaxPreferredPenetrationDirections { get { return _maxPreferredPenetrationDirections; } } 50 public static int MaxPreferredPenetrationDirections { get { return _maxPreferredPenetrationDirections; } }
51 public static float ConvexDistanceMargin { get { return _convexDistanceMargin; } } 51 public static float ConvexDistanceMargin { get { return _convexDistanceMargin; } }
52 52
53 public Vector3 ImplicitShapeDimensions { get { return _implicitShapeDimensions; } protected set { _implicitShapeDimensions = value; } } 53 public Vector3 ImplicitShapeDimensions { get { return _implicitShapeDimensions; } protected set { _implicitShapeDimensions = value; } }
54 public virtual int PreferredPenetrationDirectionsCount { get { return 0; } } 54 public virtual int PreferredPenetrationDirectionsCount { get { return 0; } }
55 55
56 protected float CollisionMargin { get { return _collisionMargin; } set { _collisionMargin = value; } } 56 protected float CollisionMargin { get { return _collisionMargin; } set { _collisionMargin = value; } }
57 57
58 public virtual void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector) 58 public virtual void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector)
59 { 59 {
60 penetrationVector = new Vector3(); 60 penetrationVector = new Vector3();
61 BulletDebug.Assert(false); 61 BulletDebug.Assert(false);
62 } 62 }
63 63
64 public abstract Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec); 64 public abstract Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec);
65 //notice that the vectors should be unit length 65 //notice that the vectors should be unit length
66 public abstract void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut); 66 public abstract void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut);
67 67
68 /// <summary> 68 /// <summary>
69 /// getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version 69 /// getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
70 /// </summary> 70 /// </summary>
71 /// <param name="t"></param> 71 /// <param name="t"></param>
72 /// <param name="aabbMin"></param> 72 /// <param name="aabbMin"></param>
73 /// <param name="aabbMax"></param> 73 /// <param name="aabbMax"></param>
74 public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) 74 public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
75 { 75 {
76 GetAabbSlow(t, out aabbMin, out aabbMax); 76 GetAabbSlow(t, out aabbMin, out aabbMax);
77 } 77 }
78 78
79 public override Vector3 LocalScaling 79 public override Vector3 LocalScaling
80 { 80 {
81 get 81 get
82 { 82 {
83 return _localScaling; 83 return _localScaling;
84 } 84 }
85 set 85 set
86 { 86 {
87 _localScaling = value; 87 _localScaling = value;
88 } 88 }
89 } 89 }
90 90
91 public override float Margin 91 public override float Margin
92 { 92 {
93 get 93 get
94 { 94 {
95 return _collisionMargin; 95 return _collisionMargin;
96 } 96 }
97 set 97 set
98 { 98 {
99 _collisionMargin = value; 99 _collisionMargin = value;
100 } 100 }
101 } 101 }
102 102
103 public virtual Vector3 LocalGetSupportingVertex(Vector3 vec) 103 public virtual Vector3 LocalGetSupportingVertex(Vector3 vec)
104 { 104 {
105 Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); 105 Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec);
106 106
107 if (Margin != 0f) 107 if (Margin != 0f)
108 { 108 {
109 Vector3 vecnorm = vec; 109 Vector3 vecnorm = vec;
110 if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) 110 if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon))
111 { 111 {
112 vecnorm = new Vector3(-1f, -1f, -1f); 112 vecnorm = new Vector3(-1f, -1f, -1f);
113 } 113 }
114 vecnorm.Normalize(); 114 vecnorm.Normalize();
115 supVertex += Margin * vecnorm; 115 supVertex += Margin * vecnorm;
116 } 116 }
117 return supVertex; 117 return supVertex;
118 } 118 }
119 119
120 public virtual void GetAabbSlow(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) 120 public virtual void GetAabbSlow(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
121 { 121 {
122 float margin = Margin; 122 float margin = Margin;
123 aabbMax = new Vector3(); 123 aabbMax = new Vector3();
124 aabbMin = new Vector3(); 124 aabbMin = new Vector3();
125 125
126 for (int i = 0; i < 3; i++) 126 for (int i = 0; i < 3; i++)
127 { 127 {
128 Vector3 vec = new Vector3(0f, 0f, 0f); 128 Vector3 vec = new Vector3(0f, 0f, 0f);
129 MathHelper.SetElement(ref vec, i, 1); 129 MathHelper.SetElement(ref vec, i, 1);
130 130
131 Vector3 sv = LocalGetSupportingVertex(Vector3.TransformNormal(vec, t)); 131 Vector3 sv = LocalGetSupportingVertex(Vector3.TransformNormal(vec, t));
132 132
133 Vector3 tmp = MathHelper.MatrixToVector(t, sv); 133 Vector3 tmp = MathHelper.MatrixToVector(t, sv);
134 MathHelper.SetElement(ref aabbMax, i, MathHelper.GetElement(tmp, i) + margin); 134 MathHelper.SetElement(ref aabbMax, i, MathHelper.GetElement(tmp, i) + margin);
135 MathHelper.SetElement(ref vec, i, -1f); 135 MathHelper.SetElement(ref vec, i, -1f);
136 tmp = MathHelper.MatrixToVector(t, LocalGetSupportingVertex(Vector3.TransformNormal(vec, t))); 136 tmp = MathHelper.MatrixToVector(t, LocalGetSupportingVertex(Vector3.TransformNormal(vec, t)));
137 MathHelper.SetElement(ref aabbMin, i, MathHelper.GetElement(tmp, i) - margin); 137 MathHelper.SetElement(ref aabbMin, i, MathHelper.GetElement(tmp, i) - margin);
138 } 138 }
139 } 139 }
140 } 140 }
141} 141}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs
index 0fd3fa7..3e46c66 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs
@@ -1,185 +1,185 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// ConvexTriangleMeshShape is a convex hull of a triangle mesh. If you just have a point cloud, you can use ConvexHullShape instead. 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. 31 /// It uses the StridingMeshInterface instead of a point cloud. This can avoid the duplication of the triangle mesh data.
32 /// </summary> 32 /// </summary>
33 public class ConvexTriangleMeshShape : PolyhedralConvexShape 33 public class ConvexTriangleMeshShape : PolyhedralConvexShape
34 { 34 {
35 private StridingMeshInterface _stridingMesh; 35 private StridingMeshInterface _stridingMesh;
36 36
37 public ConvexTriangleMeshShape(StridingMeshInterface meshInterface) 37 public ConvexTriangleMeshShape(StridingMeshInterface meshInterface)
38 { 38 {
39 _stridingMesh = meshInterface; 39 _stridingMesh = meshInterface;
40 } 40 }
41 41
42 public StridingMeshInterface getStridingMesh() 42 public StridingMeshInterface getStridingMesh()
43 { 43 {
44 return _stridingMesh; 44 return _stridingMesh;
45 } 45 }
46 46
47 public override int VertexCount 47 public override int VertexCount
48 { 48 {
49 get 49 get
50 { 50 {
51 return 0; 51 return 0;
52 } 52 }
53 } 53 }
54 54
55 public override int EdgeCount 55 public override int EdgeCount
56 { 56 {
57 get 57 get
58 { 58 {
59 return 0; 59 return 0;
60 } 60 }
61 } 61 }
62 62
63 public override int PlaneCount 63 public override int PlaneCount
64 { 64 {
65 get 65 get
66 { 66 {
67 return 0; 67 return 0;
68 } 68 }
69 } 69 }
70 70
71 public override Vector3 LocalScaling 71 public override Vector3 LocalScaling
72 { 72 {
73 get 73 get
74 { 74 {
75 return base.LocalScaling; 75 return base.LocalScaling;
76 } 76 }
77 set 77 set
78 { 78 {
79 _stridingMesh.Scaling = value; 79 _stridingMesh.Scaling = value;
80 } 80 }
81 } 81 }
82 82
83 public override BroadphaseNativeTypes ShapeType 83 public override BroadphaseNativeTypes ShapeType
84 { 84 {
85 get 85 get
86 { 86 {
87 return BroadphaseNativeTypes.ConvexTriangleMesh; 87 return BroadphaseNativeTypes.ConvexTriangleMesh;
88 } 88 }
89 } 89 }
90 90
91 public override string Name 91 public override string Name
92 { 92 {
93 get 93 get
94 { 94 {
95 return "ConvexTrimesh"; 95 return "ConvexTrimesh";
96 } 96 }
97 } 97 }
98 98
99 public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) 99 public override void GetEdge(int i, out Vector3 pa, out Vector3 pb)
100 { 100 {
101 pa = new Vector3(); 101 pa = new Vector3();
102 pb = new Vector3(); 102 pb = new Vector3();
103 BulletDebug.Assert(false); 103 BulletDebug.Assert(false);
104 } 104 }
105 105
106 public override void GetVertex(int i, out Vector3 vtx) 106 public override void GetVertex(int i, out Vector3 vtx)
107 { 107 {
108 vtx = new Vector3(); 108 vtx = new Vector3();
109 BulletDebug.Assert(false); 109 BulletDebug.Assert(false);
110 } 110 }
111 111
112 public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) 112 public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i)
113 { 113 {
114 planeNormal = new Vector3(); 114 planeNormal = new Vector3();
115 planeSupport = new Vector3(); 115 planeSupport = new Vector3();
116 BulletDebug.Assert(false); 116 BulletDebug.Assert(false);
117 } 117 }
118 118
119 public override bool IsInside(Vector3 pt, float tolerance) 119 public override bool IsInside(Vector3 pt, float tolerance)
120 { 120 {
121 BulletDebug.Assert(false); 121 BulletDebug.Assert(false);
122 return false; 122 return false;
123 } 123 }
124 124
125 public override Vector3 LocalGetSupportingVertex(Vector3 vec) 125 public override Vector3 LocalGetSupportingVertex(Vector3 vec)
126 { 126 {
127 Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); 127 Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec);
128 128
129 if (Margin != 0) 129 if (Margin != 0)
130 { 130 {
131 Vector3 vecnorm = vec; 131 Vector3 vecnorm = vec;
132 if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) 132 if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon))
133 { 133 {
134 vecnorm = new Vector3(-1, -1, -1); 134 vecnorm = new Vector3(-1, -1, -1);
135 } 135 }
136 vecnorm = Vector3.Normalize(vecnorm); 136 vecnorm = Vector3.Normalize(vecnorm);
137 supVertex += Margin * vecnorm; 137 supVertex += Margin * vecnorm;
138 } 138 }
139 return supVertex; 139 return supVertex;
140 } 140 }
141 141
142 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec0) 142 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec0)
143 { 143 {
144 Vector3 supVec = new Vector3(); 144 Vector3 supVec = new Vector3();
145 145
146 Vector3 vec = vec0; 146 Vector3 vec = vec0;
147 float lenSqr = vec.LengthSquared(); 147 float lenSqr = vec.LengthSquared();
148 if (lenSqr < 0.0001f) 148 if (lenSqr < 0.0001f)
149 { 149 {
150 vec = new Vector3(1, 0, 0); 150 vec = new Vector3(1, 0, 0);
151 } 151 }
152 else 152 else
153 { 153 {
154 float rlen = 1f / (float)Math.Sqrt(lenSqr); 154 float rlen = 1f / (float)Math.Sqrt(lenSqr);
155 vec *= rlen; 155 vec *= rlen;
156 } 156 }
157 157
158 LocalSupportVertexCallback supportCallback = new LocalSupportVertexCallback(vec); 158 LocalSupportVertexCallback supportCallback = new LocalSupportVertexCallback(vec);
159 Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); 159 Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
160 _stridingMesh.InternalProcessAllTriangles(supportCallback, -aabbMax, aabbMax); 160 _stridingMesh.InternalProcessAllTriangles(supportCallback, -aabbMax, aabbMax);
161 supVec = supportCallback.SupportVertexLocal; 161 supVec = supportCallback.SupportVertexLocal;
162 162
163 return supVec; 163 return supVec;
164 } 164 }
165 165
166 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) 166 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
167 { 167 {
168 //use 'w' component of supportVerticesOut? 168 //use 'w' component of supportVerticesOut?
169 /*{ 169 /*{
170 for (int i = 0; i < numVectors; i++) 170 for (int i = 0; i < numVectors; i++)
171 { 171 {
172 supportVerticesOut[i][3] = -1e30f; 172 supportVerticesOut[i][3] = -1e30f;
173 } 173 }
174 }*/ 174 }*/
175 for (int j = 0; j < vectors.Length; j++) 175 for (int j = 0; j < vectors.Length; j++)
176 { 176 {
177 Vector3 vec = vectors[j]; 177 Vector3 vec = vectors[j];
178 LocalSupportVertexCallback supportCallback = new LocalSupportVertexCallback(vec); 178 LocalSupportVertexCallback supportCallback = new LocalSupportVertexCallback(vec);
179 Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); 179 Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
180 _stridingMesh.InternalProcessAllTriangles(supportCallback, -aabbMax, aabbMax); 180 _stridingMesh.InternalProcessAllTriangles(supportCallback, -aabbMax, aabbMax);
181 supportVerticesOut[j] = supportCallback.SupportVertexLocal; 181 supportVerticesOut[j] = supportCallback.SupportVertexLocal;
182 } 182 }
183 } 183 }
184 } 184 }
185} 185}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs
index 9a86eaf..85bf769 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs
@@ -1,136 +1,136 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// implements cylinder shape interface 30 /// implements cylinder shape interface
31 /// </summary> 31 /// </summary>
32 public class CylinderShape : BoxShape 32 public class CylinderShape : BoxShape
33 { 33 {
34 public CylinderShape(Vector3 halfExtents) 34 public CylinderShape(Vector3 halfExtents)
35 : base(halfExtents) 35 : base(halfExtents)
36 { 36 {
37 } 37 }
38 38
39 public override BroadphaseNativeTypes ShapeType 39 public override BroadphaseNativeTypes ShapeType
40 { 40 {
41 get 41 get
42 { 42 {
43 return BroadphaseNativeTypes.Cylinder; 43 return BroadphaseNativeTypes.Cylinder;
44 } 44 }
45 } 45 }
46 46
47 public virtual int UpAxis 47 public virtual int UpAxis
48 { 48 {
49 get 49 get
50 { 50 {
51 return 1; 51 return 1;
52 } 52 }
53 } 53 }
54 54
55 public virtual float Radius 55 public virtual float Radius
56 { 56 {
57 get 57 get
58 { 58 {
59 return HalfExtents.Z; 59 return HalfExtents.Z;
60 } 60 }
61 } 61 }
62 62
63 //debugging 63 //debugging
64 public override string Name 64 public override string Name
65 { 65 {
66 get 66 get
67 { 67 {
68 return "CylinderY"; 68 return "CylinderY";
69 } 69 }
70 } 70 }
71 71
72 //getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version 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) 73 public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
74 { 74 {
75 GetAabbSlow(t, out aabbMin, out aabbMax); 75 GetAabbSlow(t, out aabbMin, out aabbMax);
76 } 76 }
77 77
78 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) 78 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
79 { 79 {
80 return CylinderLocalSupportY(HalfExtents, vec); 80 return CylinderLocalSupportY(HalfExtents, vec);
81 } 81 }
82 82
83 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) 83 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
84 { 84 {
85 for (int i = 0; i < vectors.Length; i++) 85 for (int i = 0; i < vectors.Length; i++)
86 { 86 {
87 supportVerticesOut[i] = CylinderLocalSupportY(HalfExtents, vectors[i]); 87 supportVerticesOut[i] = CylinderLocalSupportY(HalfExtents, vectors[i]);
88 } 88 }
89 } 89 }
90 90
91 public override Vector3 LocalGetSupportingVertex(Vector3 vec) 91 public override Vector3 LocalGetSupportingVertex(Vector3 vec)
92 { 92 {
93 93
94 Vector3 supVertex; 94 Vector3 supVertex;
95 supVertex = LocalGetSupportingVertexWithoutMargin(vec); 95 supVertex = LocalGetSupportingVertexWithoutMargin(vec);
96 96
97 if (Margin != 0) 97 if (Margin != 0)
98 { 98 {
99 Vector3 vecnorm = vec; 99 Vector3 vecnorm = vec;
100 if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) 100 if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon))
101 { 101 {
102 vecnorm=new Vector3(-1, -1, -1); 102 vecnorm=new Vector3(-1, -1, -1);
103 } 103 }
104 vecnorm = Vector3.Normalize(vecnorm); 104 vecnorm = Vector3.Normalize(vecnorm);
105 supVertex += Margin * vecnorm; 105 supVertex += Margin * vecnorm;
106 } 106 }
107 return supVertex; 107 return supVertex;
108 } 108 }
109 109
110 private Vector3 CylinderLocalSupportY(Vector3 halfExtents, Vector3 v) 110 private Vector3 CylinderLocalSupportY(Vector3 halfExtents, Vector3 v)
111 { 111 {
112 float radius = halfExtents.X; 112 float radius = halfExtents.X;
113 float halfHeight = halfExtents.Y; 113 float halfHeight = halfExtents.Y;
114 114
115 Vector3 tmp = new Vector3(); 115 Vector3 tmp = new Vector3();
116 float d; 116 float d;
117 117
118 float s = (float)Math.Sqrt(v.X * v.X + v.Z * v.Z); 118 float s = (float)Math.Sqrt(v.X * v.X + v.Z * v.Z);
119 if (s != 0) 119 if (s != 0)
120 { 120 {
121 d = radius / s; 121 d = radius / s;
122 tmp.X = v.X * d; 122 tmp.X = v.X * d;
123 tmp.Y = v.Y < 0 ? -halfHeight : halfHeight; 123 tmp.Y = v.Y < 0 ? -halfHeight : halfHeight;
124 tmp.Z = v.Z * d; 124 tmp.Z = v.Z * d;
125 return tmp; 125 return tmp;
126 } 126 }
127 else 127 else
128 { 128 {
129 tmp.X = radius; 129 tmp.X = radius;
130 tmp.Y = v.Y < 0 ? -halfHeight : halfHeight; 130 tmp.Y = v.Y < 0 ? -halfHeight : halfHeight;
131 tmp.Z = 0; 131 tmp.Z = 0;
132 return tmp; 132 return tmp;
133 } 133 }
134 } 134 }
135 } 135 }
136} 136}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs
index 9dc8e2d..da84462 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs
@@ -1,100 +1,100 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public class CylinderShapeX : CylinderShape 29 public class CylinderShapeX : CylinderShape
30 { 30 {
31 public CylinderShapeX(Vector3 halfExtents) 31 public CylinderShapeX(Vector3 halfExtents)
32 : base(halfExtents) { } 32 : base(halfExtents) { }
33 33
34 public override int UpAxis 34 public override int UpAxis
35 { 35 {
36 get 36 get
37 { 37 {
38 return 0; 38 return 0;
39 } 39 }
40 } 40 }
41 41
42 public override float Radius 42 public override float Radius
43 { 43 {
44 get 44 get
45 { 45 {
46 return HalfExtents.Y; 46 return HalfExtents.Y;
47 } 47 }
48 } 48 }
49 49
50 //debugging 50 //debugging
51 public override string Name 51 public override string Name
52 { 52 {
53 get 53 get
54 { 54 {
55 return "CylinderX"; 55 return "CylinderX";
56 } 56 }
57 } 57 }
58 58
59 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) 59 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
60 { 60 {
61 return CylinderLocalSupportX(HalfExtents, vec); 61 return CylinderLocalSupportX(HalfExtents, vec);
62 } 62 }
63 63
64 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) 64 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
65 { 65 {
66 for (int i = 0; i < vectors.Length; i++) 66 for (int i = 0; i < vectors.Length; i++)
67 { 67 {
68 supportVerticesOut[i] = CylinderLocalSupportX(HalfExtents, vectors[i]); 68 supportVerticesOut[i] = CylinderLocalSupportX(HalfExtents, vectors[i]);
69 } 69 }
70 } 70 }
71 71
72 private Vector3 CylinderLocalSupportX(Vector3 halfExtents, Vector3 v) 72 private Vector3 CylinderLocalSupportX(Vector3 halfExtents, Vector3 v)
73 { 73 {
74 //mapping depends on how cylinder local orientation is 74 //mapping depends on how cylinder local orientation is
75 // extents of the cylinder is: X,Y is for radius, and Z for height 75 // extents of the cylinder is: X,Y is for radius, and Z for height
76 float radius = halfExtents.Y; 76 float radius = halfExtents.Y;
77 float halfHeight = halfExtents.X; 77 float halfHeight = halfExtents.X;
78 78
79 Vector3 tmp = new Vector3(); 79 Vector3 tmp = new Vector3();
80 float d; 80 float d;
81 81
82 float s = (float)Math.Sqrt(v.Y * v.Y + v.Z * v.Z); 82 float s = (float)Math.Sqrt(v.Y * v.Y + v.Z * v.Z);
83 if (s != 0) 83 if (s != 0)
84 { 84 {
85 d = radius / s; 85 d = radius / s;
86 tmp.Y = v.Y * d; 86 tmp.Y = v.Y * d;
87 tmp.X = v.X < 0 ? -halfHeight : halfHeight; 87 tmp.X = v.X < 0 ? -halfHeight : halfHeight;
88 tmp.Z = v.Z * d; 88 tmp.Z = v.Z * d;
89 return tmp; 89 return tmp;
90 } 90 }
91 else 91 else
92 { 92 {
93 tmp.Y = radius; 93 tmp.Y = radius;
94 tmp.X = v.X < 0 ? -halfHeight : halfHeight; 94 tmp.X = v.X < 0 ? -halfHeight : halfHeight;
95 tmp.Z = 0; 95 tmp.Z = 0;
96 return tmp; 96 return tmp;
97 } 97 }
98 } 98 }
99 } 99 }
100} 100}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs
index 1913e9c..10a1995 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs
@@ -1,100 +1,100 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public class CylinderShapeZ : CylinderShape 29 public class CylinderShapeZ : CylinderShape
30 { 30 {
31 public CylinderShapeZ(Vector3 halfExtents) 31 public CylinderShapeZ(Vector3 halfExtents)
32 : base(halfExtents) { } 32 : base(halfExtents) { }
33 33
34 public override int UpAxis 34 public override int UpAxis
35 { 35 {
36 get 36 get
37 { 37 {
38 return 2; 38 return 2;
39 } 39 }
40 } 40 }
41 41
42 public override float Radius 42 public override float Radius
43 { 43 {
44 get 44 get
45 { 45 {
46 return HalfExtents.X; 46 return HalfExtents.X;
47 } 47 }
48 } 48 }
49 49
50 //debugging 50 //debugging
51 public override string Name 51 public override string Name
52 { 52 {
53 get 53 get
54 { 54 {
55 return "CylinderZ"; 55 return "CylinderZ";
56 } 56 }
57 } 57 }
58 58
59 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) 59 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
60 { 60 {
61 return CylinderLocalSupportZ(HalfExtents, vec); 61 return CylinderLocalSupportZ(HalfExtents, vec);
62 } 62 }
63 63
64 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) 64 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
65 { 65 {
66 for (int i = 0; i < vectors.Length; i++) 66 for (int i = 0; i < vectors.Length; i++)
67 { 67 {
68 supportVerticesOut[i] = CylinderLocalSupportZ(HalfExtents, vectors[i]); 68 supportVerticesOut[i] = CylinderLocalSupportZ(HalfExtents, vectors[i]);
69 } 69 }
70 } 70 }
71 71
72 Vector3 CylinderLocalSupportZ(Vector3 halfExtents, Vector3 v) 72 Vector3 CylinderLocalSupportZ(Vector3 halfExtents, Vector3 v)
73 { 73 {
74 //mapping depends on how cylinder local orientation is 74 //mapping depends on how cylinder local orientation is
75 // extents of the cylinder is: X,Y is for radius, and Z for height 75 // extents of the cylinder is: X,Y is for radius, and Z for height
76 float radius = halfExtents.X; 76 float radius = halfExtents.X;
77 float halfHeight = halfExtents.Z; 77 float halfHeight = halfExtents.Z;
78 78
79 Vector3 tmp = new Vector3(); 79 Vector3 tmp = new Vector3();
80 float d; 80 float d;
81 81
82 float s = (float)Math.Sqrt(v.X * v.X + v.Y * v.Y); 82 float s = (float)Math.Sqrt(v.X * v.X + v.Y * v.Y);
83 if (s != 0) 83 if (s != 0)
84 { 84 {
85 d = radius / s; 85 d = radius / s;
86 tmp.X = v.X * d; 86 tmp.X = v.X * d;
87 tmp.Z = v.Z < 0 ? -halfHeight : halfHeight; 87 tmp.Z = v.Z < 0 ? -halfHeight : halfHeight;
88 tmp.Y = v.Y * d; 88 tmp.Y = v.Y * d;
89 return tmp; 89 return tmp;
90 } 90 }
91 else 91 else
92 { 92 {
93 tmp.X = radius; 93 tmp.X = radius;
94 tmp.Z = v.Z < 0 ? -halfHeight : halfHeight; 94 tmp.Z = v.Z < 0 ? -halfHeight : halfHeight;
95 tmp.Y = 0; 95 tmp.Y = 0;
96 return tmp; 96 return tmp;
97 } 97 }
98 } 98 }
99 } 99 }
100} 100}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs
index b094c39..ef6f134 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs
@@ -1,80 +1,80 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26using System.Diagnostics; 26using System.Diagnostics;
27 27
28namespace XnaDevRu.BulletX 28namespace XnaDevRu.BulletX
29{ 29{
30 public class EmptyShape : ConcaveShape 30 public class EmptyShape : ConcaveShape
31 { 31 {
32 private Vector3 _localScaling; 32 private Vector3 _localScaling;
33 33
34 public override string Name 34 public override string Name
35 { 35 {
36 get 36 get
37 { 37 {
38 return "Empty"; 38 return "Empty";
39 } 39 }
40 } 40 }
41 41
42 public override BroadphaseNativeTypes ShapeType 42 public override BroadphaseNativeTypes ShapeType
43 { 43 {
44 get 44 get
45 { 45 {
46 return BroadphaseNativeTypes.Empty; 46 return BroadphaseNativeTypes.Empty;
47 } 47 }
48 } 48 }
49 49
50 public override Vector3 LocalScaling 50 public override Vector3 LocalScaling
51 { 51 {
52 get 52 get
53 { 53 {
54 return _localScaling; 54 return _localScaling;
55 } 55 }
56 set 56 set
57 { 57 {
58 _localScaling = value; 58 _localScaling = value;
59 } 59 }
60 } 60 }
61 61
62 public override void ProcessAllTriangles(ITriangleCallback callback, MonoXnaCompactMaths.Vector3 aabbMin, MonoXnaCompactMaths.Vector3 aabbMax) 62 public override void ProcessAllTriangles(ITriangleCallback callback, MonoXnaCompactMaths.Vector3 aabbMin, MonoXnaCompactMaths.Vector3 aabbMax)
63 { 63 {
64 throw new Exception("The method or operation is not implemented."); 64 throw new Exception("The method or operation is not implemented.");
65 } 65 }
66 66
67 public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) 67 public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
68 { 68 {
69 Vector3 margin = new Vector3(Margin, Margin, Margin); 69 Vector3 margin = new Vector3(Margin, Margin, Margin);
70 aabbMin = t.Translation - margin; 70 aabbMin = t.Translation - margin;
71 aabbMax = t.Translation + margin; 71 aabbMax = t.Translation + margin;
72 } 72 }
73 73
74 public override void CalculateLocalInertia(float mass, out Vector3 inertia) 74 public override void CalculateLocalInertia(float mass, out Vector3 inertia)
75 { 75 {
76 inertia = new Vector3(); 76 inertia = new Vector3();
77 BulletDebug.Assert(false); 77 BulletDebug.Assert(false);
78 } 78 }
79 } 79 }
80} 80}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs
index 5073096..dffd865 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs
@@ -1,55 +1,55 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public class FilteredCallback : ITriangleIndexCallback 29 public class FilteredCallback : ITriangleIndexCallback
30 { 30 {
31 private ITriangleCallback _callback; 31 private ITriangleCallback _callback;
32 private Vector3 _aabbMin; 32 private Vector3 _aabbMin;
33 private Vector3 _aabbMax; 33 private Vector3 _aabbMax;
34 34
35 public FilteredCallback(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) 35 public FilteredCallback(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)
36 { 36 {
37 _callback = callback; 37 _callback = callback;
38 _aabbMin = aabbMin; 38 _aabbMin = aabbMin;
39 _aabbMax = aabbMax; 39 _aabbMax = aabbMax;
40 } 40 }
41 41
42 public ITriangleCallback TriangleCallback { get { return _callback; } set { _callback = value; } } 42 public ITriangleCallback TriangleCallback { get { return _callback; } set { _callback = value; } }
43 public Vector3 AabbMin { get { return _aabbMin; } set { _aabbMin = value; } } 43 public Vector3 AabbMin { get { return _aabbMin; } set { _aabbMin = value; } }
44 public Vector3 AabbMax { get { return _aabbMax; } set { _aabbMax = value; } } 44 public Vector3 AabbMax { get { return _aabbMax; } set { _aabbMax = value; } }
45 45
46 public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) 46 public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex)
47 { 47 {
48 if (MathHelper.TestTriangleAgainstAabb2(triangle, _aabbMin, _aabbMax)) 48 if (MathHelper.TestTriangleAgainstAabb2(triangle, _aabbMin, _aabbMax))
49 { 49 {
50 //check aabb in triangle-space, before doing this 50 //check aabb in triangle-space, before doing this
51 _callback.ProcessTriangle(triangle, partId, triangleIndex); 51 _callback.ProcessTriangle(triangle, partId, triangleIndex);
52 } 52 }
53 } 53 }
54 } 54 }
55} 55}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs
index b88d804..51c65b7 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs
@@ -1,33 +1,33 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public interface ITriangleIndexCallback 29 public interface ITriangleIndexCallback
30 { 30 {
31 void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex); 31 void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex);
32 } 32 }
33} 33}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs
index 951707d..1e5e141 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs
@@ -1,58 +1,58 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public class LocalSupportVertexCallback : ITriangleIndexCallback 29 public class LocalSupportVertexCallback : ITriangleIndexCallback
30 { 30 {
31 private Vector3 _supportVertexLocal; 31 private Vector3 _supportVertexLocal;
32 private float _maxDot; 32 private float _maxDot;
33 private Vector3 _supportVecLocal; 33 private Vector3 _supportVecLocal;
34 34
35 public LocalSupportVertexCallback(Vector3 supportVecLocal) 35 public LocalSupportVertexCallback(Vector3 supportVecLocal)
36 { 36 {
37 _supportVertexLocal = new Vector3(); 37 _supportVertexLocal = new Vector3();
38 _maxDot = -1e30f; 38 _maxDot = -1e30f;
39 _supportVecLocal = supportVecLocal; 39 _supportVecLocal = supportVecLocal;
40 } 40 }
41 41
42 public float MaxDot { get { return _maxDot; } set { _maxDot = value; } } 42 public float MaxDot { get { return _maxDot; } set { _maxDot = value; } }
43 public Vector3 SupportVertexLocal { get { return _supportVecLocal; } set { _supportVecLocal = value; } } 43 public Vector3 SupportVertexLocal { get { return _supportVecLocal; } set { _supportVecLocal = value; } }
44 44
45 public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) 45 public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex)
46 { 46 {
47 for (int i = 0; i < 3; i++) 47 for (int i = 0; i < 3; i++)
48 { 48 {
49 float dot = Vector3.Dot(_supportVecLocal, triangle[i]); 49 float dot = Vector3.Dot(_supportVecLocal, triangle[i]);
50 if (dot > _maxDot) 50 if (dot > _maxDot)
51 { 51 {
52 _maxDot = dot; 52 _maxDot = dot;
53 _supportVertexLocal = triangle[i]; 53 _supportVertexLocal = triangle[i];
54 } 54 }
55 } 55 }
56 } 56 }
57 } 57 }
58} 58}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs
index 6f70cae..8cdcfbe 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs
@@ -1,99 +1,99 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// MinkowskiSumShape represents implicit (getSupportingVertex) based minkowski sum of two convex implicit shapes. 30 /// MinkowskiSumShape represents implicit (getSupportingVertex) based minkowski sum of two convex implicit shapes.
31 /// </summary> 31 /// </summary>
32 public class MinkowskiSumShape : ConvexShape 32 public class MinkowskiSumShape : ConvexShape
33 { 33 {
34 private Matrix _transformA; 34 private Matrix _transformA;
35 private Matrix _transformB; 35 private Matrix _transformB;
36 private ConvexShape _shapeA; 36 private ConvexShape _shapeA;
37 private ConvexShape _shapeB; 37 private ConvexShape _shapeB;
38 38
39 public MinkowskiSumShape(ConvexShape shapeA, ConvexShape shapeB) 39 public MinkowskiSumShape(ConvexShape shapeA, ConvexShape shapeB)
40 { 40 {
41 _shapeA = shapeA; 41 _shapeA = shapeA;
42 _shapeB = shapeB; 42 _shapeB = shapeB;
43 _transformA = Matrix.Identity; 43 _transformA = Matrix.Identity;
44 _transformB = Matrix.Identity; 44 _transformB = Matrix.Identity;
45 } 45 }
46 46
47 public Matrix TransformA { get { return _transformA; } set { _transformA = value; } } 47 public Matrix TransformA { get { return _transformA; } set { _transformA = value; } }
48 public Matrix TransformB { get { return _transformB; } set { _transformB = value; } } 48 public Matrix TransformB { get { return _transformB; } set { _transformB = value; } }
49 public ConvexShape ShapeA { get { return _shapeA; } } 49 public ConvexShape ShapeA { get { return _shapeA; } }
50 public ConvexShape ShapeB { get { return _shapeB; } } 50 public ConvexShape ShapeB { get { return _shapeB; } }
51 51
52 public override float Margin 52 public override float Margin
53 { 53 {
54 get 54 get
55 { 55 {
56 return _shapeA.Margin + _shapeB.Margin; 56 return _shapeA.Margin + _shapeB.Margin;
57 } 57 }
58 set 58 set
59 { 59 {
60 base.Margin = value; 60 base.Margin = value;
61 } 61 }
62 } 62 }
63 63
64 public override BroadphaseNativeTypes ShapeType 64 public override BroadphaseNativeTypes ShapeType
65 { 65 {
66 get 66 get
67 { 67 {
68 return BroadphaseNativeTypes.MinkowskiDifference; 68 return BroadphaseNativeTypes.MinkowskiDifference;
69 } 69 }
70 } 70 }
71 71
72 public override string Name 72 public override string Name
73 { 73 {
74 get 74 get
75 { 75 {
76 return "MinkowskiSum"; 76 return "MinkowskiSum";
77 } 77 }
78 } 78 }
79 79
80 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) 80 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
81 { 81 {
82 Vector3 supVertexA = MathHelper.MatrixToVector(_transformA, _shapeA.LocalGetSupportingVertexWithoutMargin(Vector3.TransformNormal(vec, _transformA))); 82 Vector3 supVertexA = MathHelper.MatrixToVector(_transformA, _shapeA.LocalGetSupportingVertexWithoutMargin(Vector3.TransformNormal(vec, _transformA)));
83 Vector3 supVertexB = MathHelper.MatrixToVector(_transformB, _shapeB.LocalGetSupportingVertexWithoutMargin(Vector3.TransformNormal(vec, _transformB))); 83 Vector3 supVertexB = MathHelper.MatrixToVector(_transformB, _shapeB.LocalGetSupportingVertexWithoutMargin(Vector3.TransformNormal(vec, _transformB)));
84 return supVertexA + supVertexB; 84 return supVertexA + supVertexB;
85 } 85 }
86 86
87 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) 87 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
88 { 88 {
89 for (int i = 0; i < vectors.Length; i++) 89 for (int i = 0; i < vectors.Length; i++)
90 supportVerticesOut[i] = LocalGetSupportingVertexWithoutMargin(vectors[i]); 90 supportVerticesOut[i] = LocalGetSupportingVertexWithoutMargin(vectors[i]);
91 } 91 }
92 92
93 public override void CalculateLocalInertia(float mass, out Vector3 inertia) 93 public override void CalculateLocalInertia(float mass, out Vector3 inertia)
94 { 94 {
95 inertia = new Vector3(); 95 inertia = new Vector3();
96 BulletDebug.Assert(false); 96 BulletDebug.Assert(false);
97 } 97 }
98 } 98 }
99} 99}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs
index 9959c01..73fd094 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs
@@ -1,154 +1,154 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// MultiSphereShape represents implicit convex hull of a collection of spheres (using getSupportingVertex) 30 /// MultiSphereShape represents implicit convex hull of a collection of spheres (using getSupportingVertex)
31 /// </summary> 31 /// </summary>
32 public class MultiSphereShape : ConvexShape 32 public class MultiSphereShape : ConvexShape
33 { 33 {
34 private const int _maxNumSpheres = 5; 34 private const int _maxNumSpheres = 5;
35 private Vector3[] _localPositions = new Vector3[MaxNumSpheres]; 35 private Vector3[] _localPositions = new Vector3[MaxNumSpheres];
36 private float[] _radi = new float[MaxNumSpheres]; 36 private float[] _radi = new float[MaxNumSpheres];
37 private Vector3 _inertiaHalfExtents; 37 private Vector3 _inertiaHalfExtents;
38 38
39 private int m_numSpheres; 39 private int m_numSpheres;
40 40
41 public MultiSphereShape(Vector3 inertiaHalfExtents, Vector3[] positions, float[] radi, int numSpheres) 41 public MultiSphereShape(Vector3 inertiaHalfExtents, Vector3[] positions, float[] radi, int numSpheres)
42 { 42 {
43 _inertiaHalfExtents = inertiaHalfExtents; 43 _inertiaHalfExtents = inertiaHalfExtents;
44 float startMargin = 1e30f; 44 float startMargin = 1e30f;
45 45
46 m_numSpheres = numSpheres; 46 m_numSpheres = numSpheres;
47 for (int i = 0; i < m_numSpheres; i++) 47 for (int i = 0; i < m_numSpheres; i++)
48 { 48 {
49 _localPositions[i] = positions[i]; 49 _localPositions[i] = positions[i];
50 _radi[i] = radi[i]; 50 _radi[i] = radi[i];
51 if (radi[i] < startMargin) 51 if (radi[i] < startMargin)
52 startMargin = radi[i]; 52 startMargin = radi[i];
53 } 53 }
54 Margin = startMargin; 54 Margin = startMargin;
55 } 55 }
56 56
57 public static int MaxNumSpheres { get { return _maxNumSpheres; } } 57 public static int MaxNumSpheres { get { return _maxNumSpheres; } }
58 58
59 public override BroadphaseNativeTypes ShapeType 59 public override BroadphaseNativeTypes ShapeType
60 { 60 {
61 get 61 get
62 { 62 {
63 return BroadphaseNativeTypes.MultiSphere; 63 return BroadphaseNativeTypes.MultiSphere;
64 } 64 }
65 } 65 }
66 66
67 public override string Name 67 public override string Name
68 { 68 {
69 get 69 get
70 { 70 {
71 return "MultiSphere"; 71 return "MultiSphere";
72 } 72 }
73 } 73 }
74 74
75 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vecA) 75 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vecA)
76 { 76 {
77 Vector3 supVec = new Vector3(); 77 Vector3 supVec = new Vector3();
78 78
79 float maxDot = -1e30f; 79 float maxDot = -1e30f;
80 80
81 81
82 Vector3 vec = vecA; 82 Vector3 vec = vecA;
83 float lenSqr = vec.LengthSquared(); 83 float lenSqr = vec.LengthSquared();
84 if (lenSqr < 0.0001f) 84 if (lenSqr < 0.0001f)
85 { 85 {
86 vec = new Vector3(1, 0, 0); 86 vec = new Vector3(1, 0, 0);
87 } 87 }
88 else 88 else
89 { 89 {
90 float rlen = 1f / (float)Math.Sqrt(lenSqr); 90 float rlen = 1f / (float)Math.Sqrt(lenSqr);
91 vec *= rlen; 91 vec *= rlen;
92 } 92 }
93 93
94 Vector3 vtx; 94 Vector3 vtx;
95 float newDot; 95 float newDot;
96 96
97 for (int i = 0; i < m_numSpheres; i++) 97 for (int i = 0; i < m_numSpheres; i++)
98 { 98 {
99 vtx = _localPositions[i] + vec * LocalScaling * _radi[i] - vec * Margin; 99 vtx = _localPositions[i] + vec * LocalScaling * _radi[i] - vec * Margin;
100 newDot = Vector3.Dot(vec, vtx); 100 newDot = Vector3.Dot(vec, vtx);
101 if (newDot > maxDot) 101 if (newDot > maxDot)
102 { 102 {
103 maxDot = newDot; 103 maxDot = newDot;
104 supVec = vtx; 104 supVec = vtx;
105 } 105 }
106 } 106 }
107 107
108 return supVec; 108 return supVec;
109 } 109 }
110 110
111 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) 111 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
112 { 112 {
113 for (int j = 0; j < vectors.Length; j++) 113 for (int j = 0; j < vectors.Length; j++)
114 { 114 {
115 float maxDot = -1e30f; 115 float maxDot = -1e30f;
116 Vector3 vtx; 116 Vector3 vtx;
117 float newDot; 117 float newDot;
118 118
119 for (int i = 0; i < m_numSpheres; i++) 119 for (int i = 0; i < m_numSpheres; i++)
120 { 120 {
121 vtx = _localPositions[i] + vectors[j] * LocalScaling * _radi[i] - vectors[j] * Margin; 121 vtx = _localPositions[i] + vectors[j] * LocalScaling * _radi[i] - vectors[j] * Margin;
122 newDot = Vector3.Dot(vectors[j], vtx); 122 newDot = Vector3.Dot(vectors[j], vtx);
123 if (newDot > maxDot) 123 if (newDot > maxDot)
124 { 124 {
125 maxDot = newDot; 125 maxDot = newDot;
126 supportVerticesOut[j] = vtx; 126 supportVerticesOut[j] = vtx;
127 } 127 }
128 } 128 }
129 } 129 }
130 } 130 }
131 131
132 public override void CalculateLocalInertia(float mass, out Vector3 inertia) 132 public override void CalculateLocalInertia(float mass, out Vector3 inertia)
133 { 133 {
134 //as an approximation, take the inertia of the box that bounds the spheres 134 //as an approximation, take the inertia of the box that bounds the spheres
135 Matrix ident = Matrix.Identity; 135 Matrix ident = Matrix.Identity;
136 Vector3 halfExtents = _inertiaHalfExtents; 136 Vector3 halfExtents = _inertiaHalfExtents;
137 137
138 float margin = ConvexDistanceMargin; 138 float margin = ConvexDistanceMargin;
139 139
140 float lx = 2f * (halfExtents.X + margin); 140 float lx = 2f * (halfExtents.X + margin);
141 float ly = 2f * (halfExtents.Y + margin); 141 float ly = 2f * (halfExtents.Y + margin);
142 float lz = 2f * (halfExtents.Z + margin); 142 float lz = 2f * (halfExtents.Z + margin);
143 float x2 = lx * lx; 143 float x2 = lx * lx;
144 float y2 = ly * ly; 144 float y2 = ly * ly;
145 float z2 = lz * lz; 145 float z2 = lz * lz;
146 float scaledmass = mass * 0.08333333f; 146 float scaledmass = mass * 0.08333333f;
147 147
148 inertia = new Vector3(); 148 inertia = new Vector3();
149 inertia.X = scaledmass * (y2 + z2); 149 inertia.X = scaledmass * (y2 + z2);
150 inertia.Y = scaledmass * (x2 + z2); 150 inertia.Y = scaledmass * (x2 + z2);
151 inertia.Z = scaledmass * (x2 + y2); 151 inertia.Z = scaledmass * (x2 + y2);
152 } 152 }
153 } 153 }
154} 154}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs
index fac344e..324cdbc 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs
@@ -1,32 +1,32 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25 25
26namespace XnaDevRu.BulletX 26namespace XnaDevRu.BulletX
27{ 27{
28 public interface INodeOverlapCallback 28 public interface INodeOverlapCallback
29 { 29 {
30 void ProcessNode(OptimizedBvhNode node); 30 void ProcessNode(OptimizedBvhNode node);
31 } 31 }
32} 32}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs
index c22c990..5b7a5e4 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs
@@ -1,293 +1,293 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// OptimizedBvh store an AABB tree that can be quickly traversed on CPU (and SPU,GPU in future) 30 /// OptimizedBvh store an AABB tree that can be quickly traversed on CPU (and SPU,GPU in future)
31 /// </summary> 31 /// </summary>
32 public class OptimizedBvh 32 public class OptimizedBvh
33 { 33 {
34 private static int _maxIterations = 0; 34 private static int _maxIterations = 0;
35 private OptimizedBvhNode _rootNode; 35 private OptimizedBvhNode _rootNode;
36 36
37 private OptimizedBvhNode[] _contiguousNodes; 37 private OptimizedBvhNode[] _contiguousNodes;
38 private int _curNodeIndex; 38 private int _curNodeIndex;
39 39
40 private List<OptimizedBvhNode> _leafNodes = new List<OptimizedBvhNode>(); 40 private List<OptimizedBvhNode> _leafNodes = new List<OptimizedBvhNode>();
41 41
42 public OptimizedBvh() { } 42 public OptimizedBvh() { }
43 43
44 public void Build(StridingMeshInterface triangles) 44 public void Build(StridingMeshInterface triangles)
45 { 45 {
46 NodeTriangleCallback callback = new NodeTriangleCallback(_leafNodes); 46 NodeTriangleCallback callback = new NodeTriangleCallback(_leafNodes);
47 47
48 Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); 48 Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f);
49 Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); 49 Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
50 50
51 triangles.InternalProcessAllTriangles(callback, aabbMin, aabbMax); 51 triangles.InternalProcessAllTriangles(callback, aabbMin, aabbMax);
52 52
53 //now we have an array of leafnodes in m_leafNodes 53 //now we have an array of leafnodes in m_leafNodes
54 _contiguousNodes = new OptimizedBvhNode[2 * _leafNodes.Count]; 54 _contiguousNodes = new OptimizedBvhNode[2 * _leafNodes.Count];
55 for (int i = 0; i < _contiguousNodes.Length; i++) 55 for (int i = 0; i < _contiguousNodes.Length; i++)
56 _contiguousNodes[i] = new OptimizedBvhNode(); 56 _contiguousNodes[i] = new OptimizedBvhNode();
57 _curNodeIndex = 0; 57 _curNodeIndex = 0;
58 58
59 _rootNode = BuildTree(_leafNodes, 0, _leafNodes.Count); 59 _rootNode = BuildTree(_leafNodes, 0, _leafNodes.Count);
60 } 60 }
61 61
62 public OptimizedBvhNode BuildTree(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex) 62 public OptimizedBvhNode BuildTree(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex)
63 { 63 {
64 OptimizedBvhNode internalNode; 64 OptimizedBvhNode internalNode;
65 65
66 int splitAxis, splitIndex, i; 66 int splitAxis, splitIndex, i;
67 int numIndices = endIndex - startIndex; 67 int numIndices = endIndex - startIndex;
68 int curIndex = _curNodeIndex; 68 int curIndex = _curNodeIndex;
69 69
70 if (numIndices <= 0) 70 if (numIndices <= 0)
71 throw new BulletException(); 71 throw new BulletException();
72 72
73 if (numIndices == 1) 73 if (numIndices == 1)
74 { 74 {
75 _contiguousNodes[_curNodeIndex++] = leafNodes[startIndex]; 75 _contiguousNodes[_curNodeIndex++] = leafNodes[startIndex];
76 //return new (&m_contiguousNodes[m_curNodeIndex++]) btOptimizedBvhNode(leafNodes[startIndex]); 76 //return new (&m_contiguousNodes[m_curNodeIndex++]) btOptimizedBvhNode(leafNodes[startIndex]);
77 return leafNodes[startIndex]; 77 return leafNodes[startIndex];
78 } 78 }
79 79
80 //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. 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); 81 splitAxis = CalculateSplittingAxis(leafNodes, startIndex, endIndex);
82 82
83 splitIndex = SortAndCalculateSplittingIndex(leafNodes, startIndex, endIndex, splitAxis); 83 splitIndex = SortAndCalculateSplittingIndex(leafNodes, startIndex, endIndex, splitAxis);
84 84
85 internalNode = _contiguousNodes[_curNodeIndex++]; 85 internalNode = _contiguousNodes[_curNodeIndex++];
86 86
87 internalNode.AabbMax = new Vector3(-1e30f, -1e30f, -1e30f); 87 internalNode.AabbMax = new Vector3(-1e30f, -1e30f, -1e30f);
88 internalNode.AabbMin = new Vector3(1e30f, 1e30f, 1e30f); 88 internalNode.AabbMin = new Vector3(1e30f, 1e30f, 1e30f);
89 89
90 for (i = startIndex; i < endIndex; i++) 90 for (i = startIndex; i < endIndex; i++)
91 { 91 {
92 internalNode.AabbMax = MathHelper.SetMax(internalNode.AabbMax, leafNodes[i].AabbMax); 92 internalNode.AabbMax = MathHelper.SetMax(internalNode.AabbMax, leafNodes[i].AabbMax);
93 internalNode.AabbMin = MathHelper.SetMin(internalNode.AabbMin, leafNodes[i].AabbMin); 93 internalNode.AabbMin = MathHelper.SetMin(internalNode.AabbMin, leafNodes[i].AabbMin);
94 } 94 }
95 95
96 //internalNode->m_escapeIndex; 96 //internalNode->m_escapeIndex;
97 internalNode.LeftChild = BuildTree(leafNodes, startIndex, splitIndex); 97 internalNode.LeftChild = BuildTree(leafNodes, startIndex, splitIndex);
98 internalNode.RightChild = BuildTree(leafNodes, splitIndex, endIndex); 98 internalNode.RightChild = BuildTree(leafNodes, splitIndex, endIndex);
99 99
100 internalNode.EscapeIndex = _curNodeIndex - curIndex; 100 internalNode.EscapeIndex = _curNodeIndex - curIndex;
101 return internalNode; 101 return internalNode;
102 } 102 }
103 103
104 public int CalculateSplittingAxis(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex) 104 public int CalculateSplittingAxis(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex)
105 { 105 {
106 Vector3 means = new Vector3(); 106 Vector3 means = new Vector3();
107 Vector3 variance = new Vector3(); 107 Vector3 variance = new Vector3();
108 int numIndices = endIndex - startIndex; 108 int numIndices = endIndex - startIndex;
109 109
110 for (int i = startIndex; i < endIndex; i++) 110 for (int i = startIndex; i < endIndex; i++)
111 { 111 {
112 Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); 112 Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin);
113 means += center; 113 means += center;
114 } 114 }
115 means *= (1f / (float)numIndices); 115 means *= (1f / (float)numIndices);
116 116
117 for (int i = startIndex; i < endIndex; i++) 117 for (int i = startIndex; i < endIndex; i++)
118 { 118 {
119 Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); 119 Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin);
120 Vector3 diff2 = center - means; 120 Vector3 diff2 = center - means;
121 diff2 = diff2 * diff2; 121 diff2 = diff2 * diff2;
122 variance += diff2; 122 variance += diff2;
123 } 123 }
124 variance *= (1f / ((float)numIndices - 1)); 124 variance *= (1f / ((float)numIndices - 1));
125 125
126 return MathHelper.MaxAxis(variance); 126 return MathHelper.MaxAxis(variance);
127 } 127 }
128 128
129 public int SortAndCalculateSplittingIndex(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex, int splitAxis) 129 public int SortAndCalculateSplittingIndex(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex, int splitAxis)
130 { 130 {
131 int splitIndex = startIndex; 131 int splitIndex = startIndex;
132 int numIndices = endIndex - startIndex; 132 int numIndices = endIndex - startIndex;
133 float splitValue; 133 float splitValue;
134 134
135 Vector3 means = new Vector3(); 135 Vector3 means = new Vector3();
136 for (int i = startIndex; i < endIndex; i++) 136 for (int i = startIndex; i < endIndex; i++)
137 { 137 {
138 Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); 138 Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin);
139 means += center; 139 means += center;
140 } 140 }
141 means *= (1f / (float)numIndices); 141 means *= (1f / (float)numIndices);
142 142
143 if (splitAxis == 0) 143 if (splitAxis == 0)
144 splitValue = means.X; 144 splitValue = means.X;
145 else if (splitAxis == 1) 145 else if (splitAxis == 1)
146 splitValue = means.Y; 146 splitValue = means.Y;
147 else if (splitAxis == 2) 147 else if (splitAxis == 2)
148 splitValue = means.Z; 148 splitValue = means.Z;
149 else 149 else
150 throw new ArgumentException(); 150 throw new ArgumentException();
151 151
152 //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'. 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++) 153 for (int i = startIndex; i < endIndex; i++)
154 { 154 {
155 Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); 155 Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin);
156 float centerSplit; 156 float centerSplit;
157 157
158 if (splitAxis == 0) 158 if (splitAxis == 0)
159 centerSplit = means.X; 159 centerSplit = means.X;
160 else if (splitAxis == 1) 160 else if (splitAxis == 1)
161 centerSplit = means.Y; 161 centerSplit = means.Y;
162 else if (splitAxis == 2) 162 else if (splitAxis == 2)
163 centerSplit = means.Z; 163 centerSplit = means.Z;
164 else 164 else
165 throw new ArgumentException(); 165 throw new ArgumentException();
166 166
167 if (centerSplit > splitValue) 167 if (centerSplit > splitValue)
168 { 168 {
169 //swap 169 //swap
170 OptimizedBvhNode tmp = leafNodes[i]; 170 OptimizedBvhNode tmp = leafNodes[i];
171 leafNodes[i] = leafNodes[splitIndex]; 171 leafNodes[i] = leafNodes[splitIndex];
172 leafNodes[splitIndex] = tmp; 172 leafNodes[splitIndex] = tmp;
173 splitIndex++; 173 splitIndex++;
174 } 174 }
175 } 175 }
176 if ((splitIndex == startIndex) || (splitIndex == (endIndex - 1))) 176 if ((splitIndex == startIndex) || (splitIndex == (endIndex - 1)))
177 { 177 {
178 splitIndex = startIndex + (numIndices >> 1); 178 splitIndex = startIndex + (numIndices >> 1);
179 } 179 }
180 return splitIndex; 180 return splitIndex;
181 } 181 }
182 182
183 public void WalkTree(OptimizedBvhNode rootNode, INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) 183 public void WalkTree(OptimizedBvhNode rootNode, INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax)
184 { 184 {
185 bool isLeafNode, aabbOverlap = MathHelper.TestAabbAgainstAabb2(aabbMin, aabbMax, rootNode.AabbMin, rootNode.AabbMax); 185 bool isLeafNode, aabbOverlap = MathHelper.TestAabbAgainstAabb2(aabbMin, aabbMax, rootNode.AabbMin, rootNode.AabbMax);
186 if (aabbOverlap) 186 if (aabbOverlap)
187 { 187 {
188 isLeafNode = (rootNode.LeftChild == null && rootNode.RightChild == null); 188 isLeafNode = (rootNode.LeftChild == null && rootNode.RightChild == null);
189 if (isLeafNode) 189 if (isLeafNode)
190 { 190 {
191 nodeCallback.ProcessNode(rootNode); 191 nodeCallback.ProcessNode(rootNode);
192 } 192 }
193 else 193 else
194 { 194 {
195 WalkTree(rootNode.LeftChild, nodeCallback, aabbMin, aabbMax); 195 WalkTree(rootNode.LeftChild, nodeCallback, aabbMin, aabbMax);
196 WalkTree(rootNode.RightChild, nodeCallback, aabbMin, aabbMax); 196 WalkTree(rootNode.RightChild, nodeCallback, aabbMin, aabbMax);
197 } 197 }
198 } 198 }
199 } 199 }
200 200
201 public void WalkStacklessTree(OptimizedBvhNode[] rootNodeArray, INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) 201 public void WalkStacklessTree(OptimizedBvhNode[] rootNodeArray, INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax)
202 { 202 {
203 int escapeIndex, curIndex = 0; 203 int escapeIndex, curIndex = 0;
204 int walkIterations = 0; 204 int walkIterations = 0;
205 bool aabbOverlap, isLeafNode; 205 bool aabbOverlap, isLeafNode;
206 int rootNodeIndex = 0; 206 int rootNodeIndex = 0;
207 OptimizedBvhNode rootNode = rootNodeArray[rootNodeIndex]; 207 OptimizedBvhNode rootNode = rootNodeArray[rootNodeIndex];
208 208
209 while (curIndex < _curNodeIndex) 209 while (curIndex < _curNodeIndex)
210 { 210 {
211 //catch bugs in tree data 211 //catch bugs in tree data
212 if (walkIterations >= _curNodeIndex) 212 if (walkIterations >= _curNodeIndex)
213 throw new BulletException(); 213 throw new BulletException();
214 214
215 walkIterations++; 215 walkIterations++;
216 aabbOverlap = MathHelper.TestAabbAgainstAabb2(aabbMin, aabbMax, rootNode.AabbMin, rootNode.AabbMax); 216 aabbOverlap = MathHelper.TestAabbAgainstAabb2(aabbMin, aabbMax, rootNode.AabbMin, rootNode.AabbMax);
217 isLeafNode = (rootNode.LeftChild == null && rootNode.RightChild == null); 217 isLeafNode = (rootNode.LeftChild == null && rootNode.RightChild == null);
218 218
219 if (isLeafNode && aabbOverlap) 219 if (isLeafNode && aabbOverlap)
220 { 220 {
221 nodeCallback.ProcessNode(rootNode); 221 nodeCallback.ProcessNode(rootNode);
222 } 222 }
223 223
224 if (aabbOverlap || isLeafNode) 224 if (aabbOverlap || isLeafNode)
225 { 225 {
226 rootNodeIndex++; // this 226 rootNodeIndex++; // this
227 curIndex++; 227 curIndex++;
228 if (rootNodeIndex < rootNodeArray.Length) 228 if (rootNodeIndex < rootNodeArray.Length)
229 rootNode = rootNodeArray[rootNodeIndex]; 229 rootNode = rootNodeArray[rootNodeIndex];
230 } 230 }
231 else 231 else
232 { 232 {
233 escapeIndex = rootNode.EscapeIndex; 233 escapeIndex = rootNode.EscapeIndex;
234 rootNodeIndex += escapeIndex; // and this 234 rootNodeIndex += escapeIndex; // and this
235 curIndex += escapeIndex; 235 curIndex += escapeIndex;
236 if (rootNodeIndex < rootNodeArray.Length) 236 if (rootNodeIndex < rootNodeArray.Length)
237 rootNode = rootNodeArray[rootNodeIndex]; 237 rootNode = rootNodeArray[rootNodeIndex];
238 } 238 }
239 239
240 } 240 }
241 241
242 if (_maxIterations < walkIterations) 242 if (_maxIterations < walkIterations)
243 _maxIterations = walkIterations; 243 _maxIterations = walkIterations;
244 } 244 }
245 245
246 public void ReportAabbOverlappingNodex(INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) 246 public void ReportAabbOverlappingNodex(INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax)
247 { 247 {
248 //either choose recursive traversal (walkTree) or stackless (walkStacklessTree) 248 //either choose recursive traversal (walkTree) or stackless (walkStacklessTree)
249 //walkTree(m_rootNode1,nodeCallback,aabbMin,aabbMax); 249 //walkTree(m_rootNode1,nodeCallback,aabbMin,aabbMax);
250 //WalkStacklessTree(_rootNode, nodeCallback, aabbMin, aabbMax); 250 //WalkStacklessTree(_rootNode, nodeCallback, aabbMin, aabbMax);
251 WalkStacklessTree(_contiguousNodes, nodeCallback, aabbMin, aabbMax); 251 WalkStacklessTree(_contiguousNodes, nodeCallback, aabbMin, aabbMax);
252 } 252 }
253 253
254 public void ReportSphereOverlappingNodex(INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) { } 254 public void ReportSphereOverlappingNodex(INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) { }
255 } 255 }
256 256
257 public class NodeTriangleCallback : ITriangleIndexCallback 257 public class NodeTriangleCallback : ITriangleIndexCallback
258 { 258 {
259 private List<OptimizedBvhNode> _triangleNodes; 259 private List<OptimizedBvhNode> _triangleNodes;
260 260
261 public NodeTriangleCallback(List<OptimizedBvhNode> triangleNodes) 261 public NodeTriangleCallback(List<OptimizedBvhNode> triangleNodes)
262 { 262 {
263 _triangleNodes = triangleNodes; 263 _triangleNodes = triangleNodes;
264 } 264 }
265 265
266 public List<OptimizedBvhNode> TriangleNodes { get { return _triangleNodes; } set { _triangleNodes = value; } } 266 public List<OptimizedBvhNode> TriangleNodes { get { return _triangleNodes; } set { _triangleNodes = value; } }
267 267
268 public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) 268 public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex)
269 { 269 {
270 270
271 OptimizedBvhNode node = new OptimizedBvhNode(); 271 OptimizedBvhNode node = new OptimizedBvhNode();
272 node.AabbMin = new Vector3(1e30f, 1e30f, 1e30f); 272 node.AabbMin = new Vector3(1e30f, 1e30f, 1e30f);
273 node.AabbMax = new Vector3(-1e30f, -1e30f, -1e30f); 273 node.AabbMax = new Vector3(-1e30f, -1e30f, -1e30f);
274 274
275 node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[0]); 275 node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[0]);
276 node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[0]); 276 node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[0]);
277 node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[1]); 277 node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[1]);
278 node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[1]); 278 node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[1]);
279 node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[2]); 279 node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[2]);
280 node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[2]); 280 node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[2]);
281 281
282 node.EscapeIndex = -1; 282 node.EscapeIndex = -1;
283 node.LeftChild = null; 283 node.LeftChild = null;
284 node.RightChild = null; 284 node.RightChild = null;
285 285
286 //for child nodes 286 //for child nodes
287 node.SubPart = partId; 287 node.SubPart = partId;
288 node.TriangleIndex = triangleIndex; 288 node.TriangleIndex = triangleIndex;
289 289
290 _triangleNodes.Add(node); 290 _triangleNodes.Add(node);
291 } 291 }
292 } 292 }
293} 293}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs
index d43f2fb..83cc6e5 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs
@@ -1,63 +1,63 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// OptimizedBvhNode contains both internal and leaf node information. 30 /// OptimizedBvhNode contains both internal and leaf node information.
31 /// It hasn't been optimized yet for storage. Some obvious optimizations are: 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) 32 /// Removal of the pointers (can already be done, they are not used for traversal)
33 /// and storing aabbmin/max as quantized integers. 33 /// and storing aabbmin/max as quantized integers.
34 /// 'subpart' doesn't need an integer either. It allows to re-use graphics triangle 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 35 /// meshes stored in a non-uniform way (like batches/subparts of triangle-fans
36 /// </summary> 36 /// </summary>
37 public class OptimizedBvhNode 37 public class OptimizedBvhNode
38 { 38 {
39 private Vector3 _aabbMin; 39 private Vector3 _aabbMin;
40 private Vector3 _aabbMax; 40 private Vector3 _aabbMax;
41 41
42 //these 2 pointers are obsolete, the stackless traversal just uses the escape index 42 //these 2 pointers are obsolete, the stackless traversal just uses the escape index
43 private OptimizedBvhNode _leftChild; 43 private OptimizedBvhNode _leftChild;
44 private OptimizedBvhNode _rightChild; 44 private OptimizedBvhNode _rightChild;
45 45
46 private int _escapeIndex; 46 private int _escapeIndex;
47 47
48 //for child nodes 48 //for child nodes
49 private int _subPart; 49 private int _subPart;
50 private int _triangleIndex; 50 private int _triangleIndex;
51 51
52 public Vector3 AabbMin { get { return _aabbMin; } set { _aabbMin = value; } } 52 public Vector3 AabbMin { get { return _aabbMin; } set { _aabbMin = value; } }
53 public Vector3 AabbMax { get { return _aabbMax; } set { _aabbMax = value; } } 53 public Vector3 AabbMax { get { return _aabbMax; } set { _aabbMax = value; } }
54 54
55 public OptimizedBvhNode LeftChild { get { return _leftChild; } set { _leftChild = value; } } 55 public OptimizedBvhNode LeftChild { get { return _leftChild; } set { _leftChild = value; } }
56 public OptimizedBvhNode RightChild { get { return _rightChild; } set { _rightChild = value; } } 56 public OptimizedBvhNode RightChild { get { return _rightChild; } set { _rightChild = value; } }
57 57
58 public int EscapeIndex { get { return _escapeIndex; } set { _escapeIndex = value; } } 58 public int EscapeIndex { get { return _escapeIndex; } set { _escapeIndex = value; } }
59 59
60 public int SubPart { get { return _subPart; } set { _subPart = value; } } 60 public int SubPart { get { return _subPart; } set { _subPart = value; } }
61 public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } } 61 public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } }
62 } 62 }
63} 63}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs
index f61371a..1a911b3 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs
@@ -1,133 +1,133 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public abstract class PolyhedralConvexShape : ConvexShape 29 public abstract class PolyhedralConvexShape : ConvexShape
30 { 30 {
31 public PolyhedralConvexShape() 31 public PolyhedralConvexShape()
32 { 32 {
33 //m_optionalHull = null; 33 //m_optionalHull = null;
34 } 34 }
35 35
36 public abstract int VertexCount { get; } 36 public abstract int VertexCount { get; }
37 public abstract int EdgeCount { get; } 37 public abstract int EdgeCount { get; }
38 public abstract int PlaneCount { get; } 38 public abstract int PlaneCount { get; }
39 39
40 public abstract void GetEdge(int i, out Vector3 pointA, out Vector3 pointB); 40 public abstract void GetEdge(int i, out Vector3 pointA, out Vector3 pointB);
41 public abstract void GetVertex(int i, out Vector3 vertex); 41 public abstract void GetVertex(int i, out Vector3 vertex);
42 public abstract void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i); 42 public abstract void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i);
43 // abstract int getIndex(int i); 43 // abstract int getIndex(int i);
44 44
45 public abstract bool IsInside(Vector3 point, float tolerance); 45 public abstract bool IsInside(Vector3 point, float tolerance);
46 46
47 // optional Hull is for optional Separating Axis Test Hull collision detection, see Hull.cpp 47 // optional Hull is for optional Separating Axis Test Hull collision detection, see Hull.cpp
48 //public class Hull m_optionalHull; 48 //public class Hull m_optionalHull;
49 49
50 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) 50 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
51 { 51 {
52 Vector3 supVec = new Vector3(); 52 Vector3 supVec = new Vector3();
53 53
54 float maxDot = -1e30f; 54 float maxDot = -1e30f;
55 55
56 float lenSqr = vec.LengthSquared(); 56 float lenSqr = vec.LengthSquared();
57 if (lenSqr < 0.0001f) 57 if (lenSqr < 0.0001f)
58 { 58 {
59 vec = new Vector3(1, 0, 0); 59 vec = new Vector3(1, 0, 0);
60 } 60 }
61 else 61 else
62 { 62 {
63 float rlen = 1f / (float)Math.Sqrt(lenSqr); 63 float rlen = 1f / (float)Math.Sqrt(lenSqr);
64 vec *= rlen; 64 vec *= rlen;
65 } 65 }
66 66
67 Vector3 vtx; 67 Vector3 vtx;
68 float newDot; 68 float newDot;
69 69
70 for (int i = 0; i < VertexCount; i++) 70 for (int i = 0; i < VertexCount; i++)
71 { 71 {
72 GetVertex(i, out vtx); 72 GetVertex(i, out vtx);
73 newDot = Vector3.Dot(vec, vtx); 73 newDot = Vector3.Dot(vec, vtx);
74 if (newDot > maxDot) 74 if (newDot > maxDot)
75 { 75 {
76 maxDot = newDot; 76 maxDot = newDot;
77 supVec = vtx; 77 supVec = vtx;
78 } 78 }
79 } 79 }
80 return supVec; 80 return supVec;
81 } 81 }
82 82
83 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) 83 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
84 { 84 {
85 #warning Think about this 85 #warning Think about this
86 /*Vector3 vtx; 86 /*Vector3 vtx;
87 float newDot; 87 float newDot;
88 88
89 for (int i = 0; i < vectors.Length; i++) 89 for (int i = 0; i < vectors.Length; i++)
90 { 90 {
91 supportVerticesOut[i][3] = -1e30f; 91 supportVerticesOut[i][3] = -1e30f;
92 } 92 }
93 93
94 for (int j = 0; j < vectors.Length; j++) 94 for (int j = 0; j < vectors.Length; j++)
95 { 95 {
96 Vector3 vec = vectors[j]; 96 Vector3 vec = vectors[j];
97 97
98 for (int i = 0; i < getNumVertices(); i++) 98 for (int i = 0; i < getNumVertices(); i++)
99 { 99 {
100 getVertex(i, out vtx); 100 getVertex(i, out vtx);
101 newDot = Vector3.Dot(vec,vtx); 101 newDot = Vector3.Dot(vec,vtx);
102 if (newDot > supportVerticesOut[j][3]) 102 if (newDot > supportVerticesOut[j][3])
103 { 103 {
104 //WARNING: don't swap next lines, the w component would get overwritten! 104 //WARNING: don't swap next lines, the w component would get overwritten!
105 supportVerticesOut[j] = vtx; 105 supportVerticesOut[j] = vtx;
106 supportVerticesOut[j][3] = newDot; 106 supportVerticesOut[j][3] = newDot;
107 } 107 }
108 } 108 }
109 }*/ 109 }*/
110 } 110 }
111 111
112 public override void CalculateLocalInertia(float mass, out Vector3 inertia) 112 public override void CalculateLocalInertia(float mass, out Vector3 inertia)
113 { 113 {
114 //not yet, return box inertia 114 //not yet, return box inertia
115 float margin = Margin; 115 float margin = Margin;
116 116
117 Matrix ident = Matrix.Identity; 117 Matrix ident = Matrix.Identity;
118 Vector3 aabbMin, aabbMax; 118 Vector3 aabbMin, aabbMax;
119 GetAabb(ident, out aabbMin, out aabbMax); 119 GetAabb(ident, out aabbMin, out aabbMax);
120 Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; 120 Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f;
121 121
122 float lx = 2f * (halfExtents.X + margin); 122 float lx = 2f * (halfExtents.X + margin);
123 float ly = 2f * (halfExtents.Y + margin); 123 float ly = 2f * (halfExtents.Y + margin);
124 float lz = 2f * (halfExtents.Z + margin); 124 float lz = 2f * (halfExtents.Z + margin);
125 float x2 = lx * lx; 125 float x2 = lx * lx;
126 float y2 = ly * ly; 126 float y2 = ly * ly;
127 float z2 = lz * lz; 127 float z2 = lz * lz;
128 float scaledmass = mass * 0.08333333f; 128 float scaledmass = mass * 0.08333333f;
129 129
130 inertia = scaledmass * (new Vector3(y2 + z2, x2 + z2, x2 + y2)); 130 inertia = scaledmass * (new Vector3(y2 + z2, x2 + z2, x2 + y2));
131 } 131 }
132 } 132 }
133} 133}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs
index c660a6f..cc3cea9 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs
@@ -1,116 +1,116 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// btSphereShape implements an implicit (getSupportingVertex) Sphere 30 /// btSphereShape implements an implicit (getSupportingVertex) Sphere
31 /// </summary> 31 /// </summary>
32 public class SphereShape : ConvexShape 32 public class SphereShape : ConvexShape
33 { 33 {
34 public SphereShape(float radius) 34 public SphereShape(float radius)
35 : base() 35 : base()
36 { 36 {
37 Vector3 temp = ImplicitShapeDimensions; 37 Vector3 temp = ImplicitShapeDimensions;
38 temp.X = radius; 38 temp.X = radius;
39 ImplicitShapeDimensions = temp; 39 ImplicitShapeDimensions = temp;
40 } 40 }
41 41
42 public float Radius { get { return ImplicitShapeDimensions.X; } } 42 public float Radius { get { return ImplicitShapeDimensions.X; } }
43 43
44 public override BroadphaseNativeTypes ShapeType 44 public override BroadphaseNativeTypes ShapeType
45 { 45 {
46 get 46 get
47 { 47 {
48 return BroadphaseNativeTypes.Sphere; 48 return BroadphaseNativeTypes.Sphere;
49 } 49 }
50 } 50 }
51 51
52 public override string Name 52 public override string Name
53 { 53 {
54 get 54 get
55 { 55 {
56 return "Sphere"; 56 return "Sphere";
57 } 57 }
58 } 58 }
59 59
60 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) 60 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
61 { 61 {
62 return new Vector3(); 62 return new Vector3();
63 } 63 }
64 64
65 /// <summary> 65 /// <summary>
66 /// to improve gjk behaviour, use radius+margin as the full margin, so never get into the penetration case 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 67 /// this means, non-uniform scaling is not supported anymore
68 /// </summary> 68 /// </summary>
69 public override float Margin 69 public override float Margin
70 { 70 {
71 get 71 get
72 { 72 {
73 return LocalScaling.X * Radius + base.Margin; 73 return LocalScaling.X * Radius + base.Margin;
74 } 74 }
75 set 75 set
76 { 76 {
77 base.Margin = value; 77 base.Margin = value;
78 } 78 }
79 } 79 }
80 80
81 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) 81 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
82 { 82 {
83 if (supportVerticesOut != null) 83 if (supportVerticesOut != null)
84 for (int i = 0; i < supportVerticesOut.Length; i++) 84 for (int i = 0; i < supportVerticesOut.Length; i++)
85 supportVerticesOut[i] = new Vector3(); 85 supportVerticesOut[i] = new Vector3();
86 } 86 }
87 87
88 public override void CalculateLocalInertia(float mass, out Vector3 inertia) 88 public override void CalculateLocalInertia(float mass, out Vector3 inertia)
89 { 89 {
90 float elem = 0.4f * mass * Margin * Margin; 90 float elem = 0.4f * mass * Margin * Margin;
91 inertia = new Vector3(elem, elem, elem); 91 inertia = new Vector3(elem, elem, elem);
92 } 92 }
93 93
94 public override Vector3 LocalGetSupportingVertex(Vector3 vec) 94 public override Vector3 LocalGetSupportingVertex(Vector3 vec)
95 { 95 {
96 Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); 96 Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec);
97 97
98 Vector3 vecnorm = vec; 98 Vector3 vecnorm = vec;
99 if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) 99 if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon))
100 { 100 {
101 vecnorm = new Vector3(-1f, -1f, -1f); 101 vecnorm = new Vector3(-1f, -1f, -1f);
102 } 102 }
103 vecnorm.Normalize(); 103 vecnorm.Normalize();
104 supVertex += Margin * vecnorm; 104 supVertex += Margin * vecnorm;
105 return supVertex; 105 return supVertex;
106 } 106 }
107 107
108 public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) 108 public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
109 { 109 {
110 Vector3 center = t.Translation; 110 Vector3 center = t.Translation;
111 Vector3 extent = new Vector3(Margin, Margin, Margin); 111 Vector3 extent = new Vector3(Margin, Margin, Margin);
112 aabbMin = center - extent; 112 aabbMin = center - extent;
113 aabbMax = center + extent; 113 aabbMax = center + extent;
114 } 114 }
115 } 115 }
116} 116}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs
index 49604a0..a79a145 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs
@@ -1,124 +1,124 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public class StaticPlaneShape : ConcaveShape 29 public class StaticPlaneShape : ConcaveShape
30 { 30 {
31 private Vector3 _localAabbMin; 31 private Vector3 _localAabbMin;
32 private Vector3 _localAabbMax; 32 private Vector3 _localAabbMax;
33 33
34 private Vector3 _planeNormal; 34 private Vector3 _planeNormal;
35 private float _planeConstant; 35 private float _planeConstant;
36 private Vector3 _localScaling; 36 private Vector3 _localScaling;
37 37
38 public StaticPlaneShape(Vector3 planeNormal, float planeConstant) 38 public StaticPlaneShape(Vector3 planeNormal, float planeConstant)
39 { 39 {
40 _planeNormal = planeNormal; 40 _planeNormal = planeNormal;
41 _planeConstant = planeConstant; 41 _planeConstant = planeConstant;
42 _localScaling = new Vector3(); 42 _localScaling = new Vector3();
43 } 43 }
44 44
45 protected Vector3 LocalAabbMin { get { return _localAabbMin; } set { _localAabbMin = value; } } 45 protected Vector3 LocalAabbMin { get { return _localAabbMin; } set { _localAabbMin = value; } }
46 protected Vector3 LocalAabbMax { get { return _localAabbMax; } set { _localAabbMax = value; } } 46 protected Vector3 LocalAabbMax { get { return _localAabbMax; } set { _localAabbMax = value; } }
47 47
48 protected Vector3 PlaneNormal { get { return _planeNormal; } set { _planeNormal = value; } } 48 protected Vector3 PlaneNormal { get { return _planeNormal; } set { _planeNormal = value; } }
49 protected float PlaneConstant { get { return _planeConstant; } set { _planeConstant = value; } } 49 protected float PlaneConstant { get { return _planeConstant; } set { _planeConstant = value; } }
50 50
51 public override BroadphaseNativeTypes ShapeType 51 public override BroadphaseNativeTypes ShapeType
52 { 52 {
53 get 53 get
54 { 54 {
55 return BroadphaseNativeTypes.StaticPlane; 55 return BroadphaseNativeTypes.StaticPlane;
56 } 56 }
57 } 57 }
58 58
59 public override Vector3 LocalScaling 59 public override Vector3 LocalScaling
60 { 60 {
61 get 61 get
62 { 62 {
63 return _localScaling; 63 return _localScaling;
64 } 64 }
65 set 65 set
66 { 66 {
67 _localScaling = value; 67 _localScaling = value;
68 } 68 }
69 } 69 }
70 70
71 public override string Name 71 public override string Name
72 { 72 {
73 get 73 get
74 { 74 {
75 return "StaticPlane"; 75 return "StaticPlane";
76 } 76 }
77 } 77 }
78 78
79 public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) 79 public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
80 { 80 {
81 Vector3 infvec = new Vector3(1e30f, 1e30f, 1e30f); 81 Vector3 infvec = new Vector3(1e30f, 1e30f, 1e30f);
82 82
83 Vector3 center = _planeNormal * _planeConstant; 83 Vector3 center = _planeNormal * _planeConstant;
84 aabbMin = center + infvec * _planeNormal; 84 aabbMin = center + infvec * _planeNormal;
85 aabbMax = aabbMin; 85 aabbMax = aabbMin;
86 MathHelper.SetMin(ref aabbMin, center - infvec * _planeNormal); 86 MathHelper.SetMin(ref aabbMin, center - infvec * _planeNormal);
87 MathHelper.SetMax(ref aabbMax, center - infvec * _planeNormal); 87 MathHelper.SetMax(ref aabbMax, center - infvec * _planeNormal);
88 88
89 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); 89 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f);
90 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); 90 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
91 } 91 }
92 92
93 public override void CalculateLocalInertia(float mass, out Vector3 inertia) 93 public override void CalculateLocalInertia(float mass, out Vector3 inertia)
94 { 94 {
95 //moving concave objects not supported 95 //moving concave objects not supported
96 inertia = new Vector3(); 96 inertia = new Vector3();
97 } 97 }
98 98
99 public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) { 99 public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) {
100 Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; 100 Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f;
101 float radius = halfExtents.Length(); 101 float radius = halfExtents.Length();
102 Vector3 center = (aabbMax + aabbMin) * 0.5f; 102 Vector3 center = (aabbMax + aabbMin) * 0.5f;
103 103
104 //this is where the triangles are generated, given AABB and plane equation (normal/constant) 104 //this is where the triangles are generated, given AABB and plane equation (normal/constant)
105 Vector3 tangentDir0 = new Vector3(), tangentDir1 = new Vector3(); 105 Vector3 tangentDir0 = new Vector3(), tangentDir1 = new Vector3();
106 106
107 //tangentDir0/tangentDir1 can be precalculated 107 //tangentDir0/tangentDir1 can be precalculated
108 MathHelper.PlaneSpace1(_planeNormal, ref tangentDir0, ref tangentDir1); 108 MathHelper.PlaneSpace1(_planeNormal, ref tangentDir0, ref tangentDir1);
109 109
110 Vector3 projectedCenter = center - (Vector3.Dot(_planeNormal, center) - _planeConstant) * _planeNormal; 110 Vector3 projectedCenter = center - (Vector3.Dot(_planeNormal, center) - _planeConstant) * _planeNormal;
111 111
112 Vector3[] triangle = new Vector3[3]; 112 Vector3[] triangle = new Vector3[3];
113 triangle[0] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius; 113 triangle[0] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius;
114 triangle[1] = projectedCenter + tangentDir0 * radius - tangentDir1 * radius; 114 triangle[1] = projectedCenter + tangentDir0 * radius - tangentDir1 * radius;
115 triangle[2] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius; 115 triangle[2] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius;
116 callback.ProcessTriangle(triangle, 0, 0); 116 callback.ProcessTriangle(triangle, 0, 0);
117 117
118 triangle[0] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius; 118 triangle[0] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius;
119 triangle[1] = projectedCenter - tangentDir0 * radius + tangentDir1 * radius; 119 triangle[1] = projectedCenter - tangentDir0 * radius + tangentDir1 * radius;
120 triangle[2] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius; 120 triangle[2] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius;
121 callback.ProcessTriangle(triangle, 0, 1); 121 callback.ProcessTriangle(triangle, 0, 1);
122 } 122 }
123 } 123 }
124} 124}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs
index 2a2ce27..20baccb 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs
@@ -1,115 +1,115 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// PHY_ScalarType enumerates possible scalar types. 30 /// PHY_ScalarType enumerates possible scalar types.
31 /// See the StridingMeshInterface for its use 31 /// See the StridingMeshInterface for its use
32 /// </summary> 32 /// </summary>
33 public enum PHY_ScalarType 33 public enum PHY_ScalarType
34 { 34 {
35 PHY_FLOAT, 35 PHY_FLOAT,
36 PHY_DOUBLE, 36 PHY_DOUBLE,
37 PHY_INTEGER, 37 PHY_INTEGER,
38 PHY_SHORT, 38 PHY_SHORT,
39 PHY_FIXEDPOINT88 39 PHY_FIXEDPOINT88
40 } 40 }
41 41
42 /// <summary> 42 /// <summary>
43 /// StridingMeshInterface is the interface class for high performance access to triangle meshes 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. 44 /// It allows for sharing graphics and collision meshes. Also it provides locking/unlocking of graphics meshes that are in gpu memory.
45 /// </summary> 45 /// </summary>
46 public abstract class StridingMeshInterface 46 public abstract class StridingMeshInterface
47 { 47 {
48 protected Vector3 _scaling; 48 protected Vector3 _scaling;
49 49
50 public StridingMeshInterface() 50 public StridingMeshInterface()
51 { 51 {
52 _scaling = new Vector3(1f,1f,1f); 52 _scaling = new Vector3(1f,1f,1f);
53 } 53 }
54 54
55 public void InternalProcessAllTriangles(ITriangleIndexCallback callback, Vector3 aabbMin, Vector3 aabbMax) 55 public void InternalProcessAllTriangles(ITriangleIndexCallback callback, Vector3 aabbMin, Vector3 aabbMax)
56 { 56 {
57 int numtotalphysicsverts = 0; 57 int numtotalphysicsverts = 0;
58 int numtriangles, gfxindex; 58 int numtriangles, gfxindex;
59 int part, graphicssubparts = SubPartsCount(); 59 int part, graphicssubparts = SubPartsCount();
60 Vector3[] triangle = new Vector3[3]; 60 Vector3[] triangle = new Vector3[3];
61 List<Vector3> verts; 61 List<Vector3> verts;
62 List<int> indicies; 62 List<int> indicies;
63 63
64 Vector3 meshScaling = Scaling; 64 Vector3 meshScaling = Scaling;
65 65
66 //if the number of parts is big, the performance might drop due to the innerloop switch on indextype 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++) 67 for (part = 0; part < graphicssubparts; part++)
68 { 68 {
69 GetLockedReadOnlyVertexIndexBase(out verts, out indicies, out numtriangles, part); 69 GetLockedReadOnlyVertexIndexBase(out verts, out indicies, out numtriangles, part);
70 numtotalphysicsverts += numtriangles * 3; //upper bound 70 numtotalphysicsverts += numtriangles * 3; //upper bound
71 71
72 for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) 72 for (gfxindex = 0; gfxindex < numtriangles; gfxindex++)
73 { 73 {
74 triangle[0] = verts[indicies[gfxindex * 3 + 0]]; 74 triangle[0] = verts[indicies[gfxindex * 3 + 0]];
75 triangle[1] = verts[indicies[gfxindex * 3 + 1]]; 75 triangle[1] = verts[indicies[gfxindex * 3 + 1]];
76 triangle[2] = verts[indicies[gfxindex * 3 + 2]]; 76 triangle[2] = verts[indicies[gfxindex * 3 + 2]];
77 77
78 callback.ProcessTriangleIndex(triangle, part, gfxindex); 78 callback.ProcessTriangleIndex(triangle, part, gfxindex);
79 } 79 }
80 80
81 UnLockReadOnlyVertexBase(part); 81 UnLockReadOnlyVertexBase(part);
82 } 82 }
83 } 83 }
84 84
85 85
86 // get read and write access to a subpart of a triangle mesh 86 // get read and write access to a subpart of a triangle mesh
87 // this subpart has a continuous array of vertices and indices 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 88 // in this way the mesh can be handled as chunks of memory with striding
89 // very similar to OpenGL vertexarray support 89 // very similar to OpenGL vertexarray support
90 // make a call to unLockVertexBase when the read and write access is finished 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); 91 public abstract void GetLockedVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart);
92 92
93 public abstract void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart); 93 public abstract void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart);
94 94
95 // unLockVertexBase finishes the access to a subpart of the triangle mesh 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 96 // make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished
97 public abstract void UnLockVertexBase(int subpart); 97 public abstract void UnLockVertexBase(int subpart);
98 98
99 public abstract void UnLockReadOnlyVertexBase(int subpart); 99 public abstract void UnLockReadOnlyVertexBase(int subpart);
100 100
101 101
102 // getNumSubParts returns the number of seperate subparts 102 // getNumSubParts returns the number of seperate subparts
103 // each subpart has a continuous array of vertices and indices 103 // each subpart has a continuous array of vertices and indices
104 public abstract int SubPartsCount(); 104 public abstract int SubPartsCount();
105 105
106 public abstract void PreallocateVertices(int numverts); 106 public abstract void PreallocateVertices(int numverts);
107 public abstract void PreallocateIndices(int numindices); 107 public abstract void PreallocateIndices(int numindices);
108 108
109 public Vector3 Scaling 109 public Vector3 Scaling
110 { 110 {
111 get { return _scaling; } 111 get { return _scaling; }
112 set { _scaling = value; } 112 set { _scaling = value; }
113 } 113 }
114 } 114 }
115} 115}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs
index ffb5813..eab822e 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs
@@ -1,67 +1,67 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 internal class SupportVertexCallback : ITriangleCallback 29 internal class SupportVertexCallback : ITriangleCallback
30 { 30 {
31 private Vector3 _supportVertexLocal; 31 private Vector3 _supportVertexLocal;
32 32
33 private Matrix _worldTransform; 33 private Matrix _worldTransform;
34 private float _maxDot; 34 private float _maxDot;
35 private Vector3 _supportVecLocal; 35 private Vector3 _supportVecLocal;
36 36
37 public SupportVertexCallback(Vector3 supportVecWorld, Matrix trans) 37 public SupportVertexCallback(Vector3 supportVecWorld, Matrix trans)
38 { 38 {
39 _supportVertexLocal = new Vector3(); 39 _supportVertexLocal = new Vector3();
40 _worldTransform = trans; 40 _worldTransform = trans;
41 _maxDot = -1e30f; 41 _maxDot = -1e30f;
42 _supportVecLocal = Vector3.TransformNormal(supportVecWorld, _worldTransform); 42 _supportVecLocal = Vector3.TransformNormal(supportVecWorld, _worldTransform);
43 } 43 }
44 44
45 public Matrix WorldTransform { get { return _worldTransform; } set { _worldTransform = value; } } 45 public Matrix WorldTransform { get { return _worldTransform; } set { _worldTransform = value; } }
46 public float MaxDot { get { return _maxDot; } set { _maxDot = value; } } 46 public float MaxDot { get { return _maxDot; } set { _maxDot = value; } }
47 public Vector3 SupportVectorLocal { get { return _supportVecLocal; } set { _supportVecLocal = value; } } 47 public Vector3 SupportVectorLocal { get { return _supportVecLocal; } set { _supportVecLocal = value; } }
48 48
49 public Vector3 SupportVertexLocal { get { return _supportVertexLocal; } } 49 public Vector3 SupportVertexLocal { get { return _supportVertexLocal; } }
50 public Vector3 SupportVertexWorldSpace { get { return MathHelper.MatrixToVector(_worldTransform, _supportVertexLocal); } } 50 public Vector3 SupportVertexWorldSpace { get { return MathHelper.MatrixToVector(_worldTransform, _supportVertexLocal); } }
51 51
52 #region ITriangleCallback Members 52 #region ITriangleCallback Members
53 public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) 53 public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex)
54 { 54 {
55 for (int i = 0; i < 3; i++) 55 for (int i = 0; i < 3; i++)
56 { 56 {
57 float dot = Vector3.Dot(_supportVecLocal, triangle[i]); 57 float dot = Vector3.Dot(_supportVecLocal, triangle[i]);
58 if (dot > _maxDot) 58 if (dot > _maxDot)
59 { 59 {
60 _maxDot = dot; 60 _maxDot = dot;
61 _supportVertexLocal = triangle[i]; 61 _supportVertexLocal = triangle[i];
62 } 62 }
63 } 63 }
64 } 64 }
65 #endregion 65 #endregion
66 } 66 }
67} 67}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs
index 158e42a..a472583 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs
@@ -1,80 +1,80 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public class Triangle 29 public class Triangle
30 { 30 {
31 private Vector3 _vertexA; 31 private Vector3 _vertexA;
32 private Vector3 _vertexB; 32 private Vector3 _vertexB;
33 private Vector3 _vertexC; 33 private Vector3 _vertexC;
34 private int _partId; 34 private int _partId;
35 private int _triangleIndex; 35 private int _triangleIndex;
36 36
37 public Vector3 VertexA { get { return _vertexA; } set { _vertexA = value; } } 37 public Vector3 VertexA { get { return _vertexA; } set { _vertexA = value; } }
38 public Vector3 VertexB { get { return _vertexB; } set { _vertexB = value; } } 38 public Vector3 VertexB { get { return _vertexB; } set { _vertexB = value; } }
39 public Vector3 VertexC { get { return _vertexC; } set { _vertexC = value; } } 39 public Vector3 VertexC { get { return _vertexC; } set { _vertexC = value; } }
40 public int PartId { get { return _partId; } set { _partId = value; } } 40 public int PartId { get { return _partId; } set { _partId = value; } }
41 public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } } 41 public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } }
42 } 42 }
43 43
44 /// <summary> 44 /// <summary>
45 /// example usage of this class: 45 /// example usage of this class:
46 /// TriangleBuffer triBuf; 46 /// TriangleBuffer triBuf;
47 /// concaveShape.processAllTriangles(triBuf, out aabbMin, out aabbMax); 47 /// concaveShape.processAllTriangles(triBuf, out aabbMin, out aabbMax);
48 /// for (int i = 0; i < triBuf.getNumTriangles(); i++) 48 /// for (int i = 0; i < triBuf.getNumTriangles(); i++)
49 /// { 49 /// {
50 /// Triangle tri = triBuf.getTriangle(i); 50 /// Triangle tri = triBuf.getTriangle(i);
51 /// //do something useful here with the triangle 51 /// //do something useful here with the triangle
52 /// } 52 /// }
53 /// </summary> 53 /// </summary>
54 public class TriangleBuffer : ITriangleCallback 54 public class TriangleBuffer : ITriangleCallback
55 { 55 {
56 private List<Triangle> _triangleBuffer = new List<Triangle>(); 56 private List<Triangle> _triangleBuffer = new List<Triangle>();
57 57
58 public int TriangleCount { get { return _triangleBuffer.Count; } } 58 public int TriangleCount { get { return _triangleBuffer.Count; } }
59 public Triangle this[int index] { get { return _triangleBuffer[index]; } } 59 public Triangle this[int index] { get { return _triangleBuffer[index]; } }
60 60
61 public void ClearBuffer() 61 public void ClearBuffer()
62 { 62 {
63 _triangleBuffer.Clear(); 63 _triangleBuffer.Clear();
64 } 64 }
65 65
66 #region ITriangleCallback Members 66 #region ITriangleCallback Members
67 public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) 67 public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex)
68 { 68 {
69 Triangle tri = new Triangle(); 69 Triangle tri = new Triangle();
70 tri.VertexA = triangle[0]; 70 tri.VertexA = triangle[0];
71 tri.VertexB = triangle[1]; 71 tri.VertexB = triangle[1];
72 tri.VertexC = triangle[2]; 72 tri.VertexC = triangle[2];
73 tri.PartId = partID; 73 tri.PartId = partID;
74 tri.TriangleIndex = triangleIndex; 74 tri.TriangleIndex = triangleIndex;
75 75
76 _triangleBuffer.Add(tri); 76 _triangleBuffer.Add(tri);
77 } 77 }
78 #endregion 78 #endregion
79 } 79 }
80} 80}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs
index ac892c2..938765b 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs
@@ -1,33 +1,33 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public interface ITriangleCallback 29 public interface ITriangleCallback
30 { 30 {
31 void ProcessTriangle(Vector3[] triangle, int partId, int triangleIndex); 31 void ProcessTriangle(Vector3[] triangle, int partId, int triangleIndex);
32 } 32 }
33} 33}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs
index 6d0d7b5..52c08ed 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs
@@ -1,136 +1,136 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// IndexedMesh indexes into existing vertex and index arrays, in a similar way OpenGL glDrawElements 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 31 /// instead of the number of indices, we pass the number of triangles
32 /// </summary> 32 /// </summary>
33 public struct IndexedMesh 33 public struct IndexedMesh
34 { 34 {
35 private int _numTriangles; 35 private int _numTriangles;
36 private int[] _triangleIndexBase; 36 private int[] _triangleIndexBase;
37 private int _triangleIndexStride; 37 private int _triangleIndexStride;
38 private int _numVertices; 38 private int _numVertices;
39 private Vector3[] _vertexBase; 39 private Vector3[] _vertexBase;
40 private int _vertexStride; 40 private int _vertexStride;
41 41
42 public IndexedMesh(int numTriangleIndices, int[] triangleIndexBase, int triangleIndexStride, int numVertices, Vector3[] vertexBase, int vertexStride) 42 public IndexedMesh(int numTriangleIndices, int[] triangleIndexBase, int triangleIndexStride, int numVertices, Vector3[] vertexBase, int vertexStride)
43 { 43 {
44 _numTriangles = numTriangleIndices; 44 _numTriangles = numTriangleIndices;
45 _triangleIndexBase = triangleIndexBase; 45 _triangleIndexBase = triangleIndexBase;
46 _triangleIndexStride = triangleIndexStride; 46 _triangleIndexStride = triangleIndexStride;
47 _vertexBase = vertexBase; 47 _vertexBase = vertexBase;
48 _numVertices = numVertices; 48 _numVertices = numVertices;
49 _vertexStride = vertexStride; 49 _vertexStride = vertexStride;
50 } 50 }
51 51
52 public IndexedMesh(int[] triangleIndexBase, Vector3[] vertexBase) 52 public IndexedMesh(int[] triangleIndexBase, Vector3[] vertexBase)
53 { 53 {
54 _numTriangles = triangleIndexBase.Length; 54 _numTriangles = triangleIndexBase.Length;
55 _triangleIndexBase = triangleIndexBase; 55 _triangleIndexBase = triangleIndexBase;
56 _triangleIndexStride = 32; 56 _triangleIndexStride = 32;
57 _vertexBase = vertexBase; 57 _vertexBase = vertexBase;
58 _numVertices = vertexBase.Length; 58 _numVertices = vertexBase.Length;
59 _vertexStride = 24; 59 _vertexStride = 24;
60 } 60 }
61 61
62 public int TriangleCount { get { return _numTriangles; } set { _numTriangles = value; } } 62 public int TriangleCount { get { return _numTriangles; } set { _numTriangles = value; } }
63 public int[] TriangleIndexBase { get { return _triangleIndexBase; } set { _triangleIndexBase = value; } } 63 public int[] TriangleIndexBase { get { return _triangleIndexBase; } set { _triangleIndexBase = value; } }
64 public int TriangleIndexStride { get { return _triangleIndexStride; } set { _triangleIndexStride = value; } } 64 public int TriangleIndexStride { get { return _triangleIndexStride; } set { _triangleIndexStride = value; } }
65 public int VertexCount { get { return _numVertices; } set { _numVertices = value; } } 65 public int VertexCount { get { return _numVertices; } set { _numVertices = value; } }
66 public Vector3[] VertexBase { get { return _vertexBase; } set { _vertexBase = value; } } 66 public Vector3[] VertexBase { get { return _vertexBase; } set { _vertexBase = value; } }
67 public int VertexStride { get { return _vertexStride; } set { _vertexStride = value; } } 67 public int VertexStride { get { return _vertexStride; } set { _vertexStride = value; } }
68 } 68 }
69 69
70 /// <summary> 70 /// <summary>
71 /// TriangleIndexVertexArray allows to use multiple meshes, by indexing into existing triangle/index arrays. 71 /// TriangleIndexVertexArray allows to use multiple meshes, by indexing into existing triangle/index arrays.
72 /// Additional meshes can be added using addIndexedMesh 72 /// Additional meshes can be added using addIndexedMesh
73 /// </summary> 73 /// </summary>
74 public class TriangleIndexVertexArray : StridingMeshInterface 74 public class TriangleIndexVertexArray : StridingMeshInterface
75 { 75 {
76 List<IndexedMesh> _indexedMeshes = new List<IndexedMesh>(); 76 List<IndexedMesh> _indexedMeshes = new List<IndexedMesh>();
77 77
78 public TriangleIndexVertexArray() { } 78 public TriangleIndexVertexArray() { }
79 79
80 public TriangleIndexVertexArray(int numTriangleIndices, int[] triangleIndexBase, int triangleIndexStride, int numVertices, Vector3[] vertexBase, int vertexStride) 80 public TriangleIndexVertexArray(int numTriangleIndices, int[] triangleIndexBase, int triangleIndexStride, int numVertices, Vector3[] vertexBase, int vertexStride)
81 { 81 {
82 IndexedMesh mesh = new IndexedMesh(); 82 IndexedMesh mesh = new IndexedMesh();
83 mesh.TriangleCount = numTriangleIndices; 83 mesh.TriangleCount = numTriangleIndices;
84 mesh.TriangleIndexBase = triangleIndexBase; 84 mesh.TriangleIndexBase = triangleIndexBase;
85 mesh.TriangleIndexStride = triangleIndexStride; 85 mesh.TriangleIndexStride = triangleIndexStride;
86 mesh.VertexBase = vertexBase; 86 mesh.VertexBase = vertexBase;
87 mesh.VertexCount = numVertices; 87 mesh.VertexCount = numVertices;
88 mesh.VertexStride = vertexStride; 88 mesh.VertexStride = vertexStride;
89 89
90 AddIndexedMesh(mesh); 90 AddIndexedMesh(mesh);
91 } 91 }
92 92
93 public TriangleIndexVertexArray(int[] triangleIndexBase, Vector3[] vertexBase) 93 public TriangleIndexVertexArray(int[] triangleIndexBase, Vector3[] vertexBase)
94 : this(triangleIndexBase.Length, triangleIndexBase, 32, vertexBase.Length, vertexBase, 24) { } 94 : this(triangleIndexBase.Length, triangleIndexBase, 32, vertexBase.Length, vertexBase, 24) { }
95 95
96 public void AddIndexedMesh(IndexedMesh indexedMesh) 96 public void AddIndexedMesh(IndexedMesh indexedMesh)
97 { 97 {
98 _indexedMeshes.Add(indexedMesh); 98 _indexedMeshes.Add(indexedMesh);
99 } 99 }
100 100
101 public override void GetLockedVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart) 101 public override void GetLockedVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart)
102 { 102 {
103 throw new Exception("The method or operation is not implemented."); 103 throw new Exception("The method or operation is not implemented.");
104 } 104 }
105 105
106 public override void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart) 106 public override void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart)
107 { 107 {
108 throw new Exception("The method or operation is not implemented."); 108 throw new Exception("The method or operation is not implemented.");
109 } 109 }
110 110
111 public override void UnLockVertexBase(int subpart) 111 public override void UnLockVertexBase(int subpart)
112 { 112 {
113 throw new Exception("The method or operation is not implemented."); 113 throw new Exception("The method or operation is not implemented.");
114 } 114 }
115 115
116 public override void UnLockReadOnlyVertexBase(int subpart) 116 public override void UnLockReadOnlyVertexBase(int subpart)
117 { 117 {
118 throw new Exception("The method or operation is not implemented."); 118 throw new Exception("The method or operation is not implemented.");
119 } 119 }
120 120
121 public override int SubPartsCount() 121 public override int SubPartsCount()
122 { 122 {
123 return _indexedMeshes.Count; 123 return _indexedMeshes.Count;
124 } 124 }
125 125
126 public override void PreallocateVertices(int numverts) 126 public override void PreallocateVertices(int numverts)
127 { 127 {
128 throw new Exception("The method or operation is not implemented."); 128 throw new Exception("The method or operation is not implemented.");
129 } 129 }
130 130
131 public override void PreallocateIndices(int numindices) 131 public override void PreallocateIndices(int numindices)
132 { 132 {
133 throw new Exception("The method or operation is not implemented."); 133 throw new Exception("The method or operation is not implemented.");
134 } 134 }
135 } 135 }
136} 136}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs
index 1426355..d5b93ba 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs
@@ -1,102 +1,102 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25 25
26using MonoXnaCompactMaths; 26using MonoXnaCompactMaths;
27 27
28namespace XnaDevRu.BulletX 28namespace XnaDevRu.BulletX
29{ 29{
30 class TriangleMesh : StridingMeshInterface 30 class TriangleMesh : StridingMeshInterface
31 { 31 {
32 int _numTriangles; 32 int _numTriangles;
33 List<Vector3> _verts; 33 List<Vector3> _verts;
34 34
35 public TriangleMesh() 35 public TriangleMesh()
36 { 36 {
37 _numTriangles = 0; 37 _numTriangles = 0;
38 _verts = new List<Vector3>(); 38 _verts = new List<Vector3>();
39 } 39 }
40 40
41 void AddTriangle(Vector3 vertex0, Vector3 vertex1, Vector3 vertex2) 41 void AddTriangle(Vector3 vertex0, Vector3 vertex1, Vector3 vertex2)
42 { 42 {
43 _verts.Add(vertex0); 43 _verts.Add(vertex0);
44 _verts.Add(vertex1); 44 _verts.Add(vertex1);
45 _verts.Add(vertex2); 45 _verts.Add(vertex2);
46 _numTriangles++; 46 _numTriangles++;
47 } 47 }
48 48
49 public override void GetLockedVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart) 49 public override void GetLockedVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart)
50 { 50 {
51 verts = new List<Vector3>(); 51 verts = new List<Vector3>();
52 for (int i = 0; i < 3; i++) 52 for (int i = 0; i < 3; i++)
53 { 53 {
54 verts.Add(_verts[subpart * 3 + i]); 54 verts.Add(_verts[subpart * 3 + i]);
55 } 55 }
56 indicies = new List<int>(); 56 indicies = new List<int>();
57 indicies.Add(0); 57 indicies.Add(0);
58 indicies.Add(1); 58 indicies.Add(1);
59 indicies.Add(2); 59 indicies.Add(2);
60 numfaces = 1; 60 numfaces = 1;
61 } 61 }
62 62
63 public override void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart) 63 public override void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart)
64 { 64 {
65 verts = new List<Vector3>(); 65 verts = new List<Vector3>();
66 for (int i = 0; i < 3; i++) 66 for (int i = 0; i < 3; i++)
67 { 67 {
68 verts.Add(_verts[subpart * 3 + i]); 68 verts.Add(_verts[subpart * 3 + i]);
69 } 69 }
70 indicies = new List<int>(); 70 indicies = new List<int>();
71 indicies.Add(0); 71 indicies.Add(0);
72 indicies.Add(1); 72 indicies.Add(1);
73 indicies.Add(2); 73 indicies.Add(2);
74 numfaces = 1; 74 numfaces = 1;
75 } 75 }
76 76
77 public override void UnLockVertexBase(int subpart) 77 public override void UnLockVertexBase(int subpart)
78 { 78 {
79 79
80 } 80 }
81 81
82 public override void UnLockReadOnlyVertexBase(int subpart) 82 public override void UnLockReadOnlyVertexBase(int subpart)
83 { 83 {
84 84
85 } 85 }
86 86
87 public override int SubPartsCount() 87 public override int SubPartsCount()
88 { 88 {
89 return _numTriangles; 89 return _numTriangles;
90 } 90 }
91 91
92 public override void PreallocateVertices(int numverts) 92 public override void PreallocateVertices(int numverts)
93 { 93 {
94 94
95 } 95 }
96 96
97 public override void PreallocateIndices(int numindices) 97 public override void PreallocateIndices(int numindices)
98 { 98 {
99 99
100 } 100 }
101 } 101 }
102} 102}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs
index bc8f09e..5f5a0cf 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs
@@ -1,160 +1,160 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// Concave triangle mesh. Uses an interface to access the triangles to allow for sharing graphics/physics triangles. 30 /// Concave triangle mesh. Uses an interface to access the triangles to allow for sharing graphics/physics triangles.
31 /// </summary> 31 /// </summary>
32 public class TriangleMeshShape : ConcaveShape 32 public class TriangleMeshShape : ConcaveShape
33 { 33 {
34 private StridingMeshInterface _meshInterface; 34 private StridingMeshInterface _meshInterface;
35 private Vector3 _localAabbMin; 35 private Vector3 _localAabbMin;
36 private Vector3 _localAabbMax; 36 private Vector3 _localAabbMax;
37 37
38 public TriangleMeshShape(StridingMeshInterface meshInterface) 38 public TriangleMeshShape(StridingMeshInterface meshInterface)
39 { 39 {
40 this._meshInterface = meshInterface; 40 this._meshInterface = meshInterface;
41 RecalcLocalAabb(); 41 RecalcLocalAabb();
42 } 42 }
43 43
44 protected StridingMeshInterface MeshInterface { get { return _meshInterface; } set { _meshInterface = value; } } 44 protected StridingMeshInterface MeshInterface { get { return _meshInterface; } set { _meshInterface = value; } }
45 protected Vector3 LocalAabbMin { get { return _localAabbMin; } set { _localAabbMin = value; } } 45 protected Vector3 LocalAabbMin { get { return _localAabbMin; } set { _localAabbMin = value; } }
46 protected Vector3 LocalAabbMax { get { return _localAabbMax; } set { _localAabbMax = value; } } 46 protected Vector3 LocalAabbMax { get { return _localAabbMax; } set { _localAabbMax = value; } }
47 47
48 public override BroadphaseNativeTypes ShapeType 48 public override BroadphaseNativeTypes ShapeType
49 { 49 {
50 get 50 get
51 { 51 {
52 return BroadphaseNativeTypes.TriangleMesh; 52 return BroadphaseNativeTypes.TriangleMesh;
53 } 53 }
54 } 54 }
55 55
56 public override Vector3 LocalScaling 56 public override Vector3 LocalScaling
57 { 57 {
58 get 58 get
59 { 59 {
60 return _meshInterface.Scaling; 60 return _meshInterface.Scaling;
61 } 61 }
62 set 62 set
63 { 63 {
64 _meshInterface.Scaling = value; 64 _meshInterface.Scaling = value;
65 } 65 }
66 } 66 }
67 67
68 public override string Name 68 public override string Name
69 { 69 {
70 get 70 get
71 { 71 {
72 return "TriangleMesh"; 72 return "TriangleMesh";
73 } 73 }
74 } 74 }
75 75
76 public void RecalcLocalAabb() 76 public void RecalcLocalAabb()
77 { 77 {
78 { 78 {
79 Vector3 vec = new Vector3(); 79 Vector3 vec = new Vector3();
80 vec.X = 1f; 80 vec.X = 1f;
81 Vector3 tmp = LocalGetSupportingVertex(vec); 81 Vector3 tmp = LocalGetSupportingVertex(vec);
82 _localAabbMax.X = tmp.X + CollisionMargin; 82 _localAabbMax.X = tmp.X + CollisionMargin;
83 vec.X = -1f; 83 vec.X = -1f;
84 tmp = LocalGetSupportingVertex(vec); 84 tmp = LocalGetSupportingVertex(vec);
85 _localAabbMin.X = tmp.X - CollisionMargin; 85 _localAabbMin.X = tmp.X - CollisionMargin;
86 } 86 }
87 { 87 {
88 Vector3 vec = new Vector3(); 88 Vector3 vec = new Vector3();
89 vec.Y = 1f; 89 vec.Y = 1f;
90 Vector3 tmp = LocalGetSupportingVertex(vec); 90 Vector3 tmp = LocalGetSupportingVertex(vec);
91 _localAabbMax.Y = tmp.Y + CollisionMargin; 91 _localAabbMax.Y = tmp.Y + CollisionMargin;
92 vec.Y = -1f; 92 vec.Y = -1f;
93 tmp = LocalGetSupportingVertex(vec); 93 tmp = LocalGetSupportingVertex(vec);
94 _localAabbMin.Y = tmp.Y - CollisionMargin; 94 _localAabbMin.Y = tmp.Y - CollisionMargin;
95 } 95 }
96 { 96 {
97 Vector3 vec = new Vector3(); 97 Vector3 vec = new Vector3();
98 vec.Z = 1f; 98 vec.Z = 1f;
99 Vector3 tmp = LocalGetSupportingVertex(vec); 99 Vector3 tmp = LocalGetSupportingVertex(vec);
100 _localAabbMax.Z = tmp.Z + CollisionMargin; 100 _localAabbMax.Z = tmp.Z + CollisionMargin;
101 vec.Z = -1f; 101 vec.Z = -1f;
102 tmp = LocalGetSupportingVertex(vec); 102 tmp = LocalGetSupportingVertex(vec);
103 _localAabbMin.Z = tmp.Z - CollisionMargin; 103 _localAabbMin.Z = tmp.Z - CollisionMargin;
104 } 104 }
105 } 105 }
106 106
107 public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) 107 public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)
108 { 108 {
109 LocalProcessAllTriangles(callback, aabbMax, aabbMax); 109 LocalProcessAllTriangles(callback, aabbMax, aabbMax);
110 } 110 }
111 111
112 protected void LocalProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) 112 protected void LocalProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)
113 { 113 {
114 FilteredCallback filterCallback = new FilteredCallback(callback, aabbMin, aabbMax); 114 FilteredCallback filterCallback = new FilteredCallback(callback, aabbMin, aabbMax);
115 _meshInterface.InternalProcessAllTriangles(filterCallback, aabbMin, aabbMax); 115 _meshInterface.InternalProcessAllTriangles(filterCallback, aabbMin, aabbMax);
116 } 116 }
117 117
118 public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) 118 public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
119 { 119 {
120 Vector3 localHalfExtents = 0.5f * (_localAabbMax - _localAabbMin); 120 Vector3 localHalfExtents = 0.5f * (_localAabbMax - _localAabbMin);
121 Vector3 localCenter = 0.5f * (_localAabbMax + _localAabbMin); 121 Vector3 localCenter = 0.5f * (_localAabbMax + _localAabbMin);
122 122
123 Matrix abs_b = MathHelper.Absolute(t); 123 Matrix abs_b = MathHelper.Absolute(t);
124 124
125 Vector3 center = MathHelper.MatrixToVector(t, localCenter); 125 Vector3 center = MathHelper.MatrixToVector(t, localCenter);
126 126
127 Vector3 extent = new Vector3(Vector3.Dot(new Vector3(abs_b.M11, abs_b.M12, abs_b.M13), localHalfExtents), 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), 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)); 129 Vector3.Dot(new Vector3(abs_b.M31, abs_b.M32, abs_b.M33), localHalfExtents));
130 extent += new Vector3(Margin, Margin, Margin); 130 extent += new Vector3(Margin, Margin, Margin);
131 131
132 aabbMin = center - extent; 132 aabbMin = center - extent;
133 aabbMax = center + extent; 133 aabbMax = center + extent;
134 } 134 }
135 135
136 public override void CalculateLocalInertia(float mass, out Vector3 inertia) 136 public override void CalculateLocalInertia(float mass, out Vector3 inertia)
137 { 137 {
138 inertia = new Vector3(); 138 inertia = new Vector3();
139 //moving concave objects not supported 139 //moving concave objects not supported
140 BulletDebug.Assert(false); 140 BulletDebug.Assert(false);
141 } 141 }
142 142
143 public virtual Vector3 LocalGetSupportingVertex(Vector3 vec) 143 public virtual Vector3 LocalGetSupportingVertex(Vector3 vec)
144 { 144 {
145 Vector3 supportVertex; 145 Vector3 supportVertex;
146 Matrix ident = Matrix.Identity; 146 Matrix ident = Matrix.Identity;
147 SupportVertexCallback supportCallback = new SupportVertexCallback(vec, ident); 147 SupportVertexCallback supportCallback = new SupportVertexCallback(vec, ident);
148 Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); 148 Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
149 LocalProcessAllTriangles(supportCallback, -aabbMax, aabbMax); 149 LocalProcessAllTriangles(supportCallback, -aabbMax, aabbMax);
150 supportVertex = supportCallback.SupportVertexLocal; 150 supportVertex = supportCallback.SupportVertexLocal;
151 return supportVertex; 151 return supportVertex;
152 } 152 }
153 153
154 public virtual Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) 154 public virtual Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
155 { 155 {
156 BulletDebug.Assert(false); 156 BulletDebug.Assert(false);
157 return LocalGetSupportingVertex(vec); 157 return LocalGetSupportingVertex(vec);
158 } 158 }
159 } 159 }
160} 160}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs
index 59ffad0..d5a17f3 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs
@@ -1,187 +1,187 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public class TriangleShape : PolyhedralConvexShape 29 public class TriangleShape : PolyhedralConvexShape
30 { 30 {
31 private Vector3[] _vertices = new Vector3[3]; 31 private Vector3[] _vertices = new Vector3[3];
32 32
33 public TriangleShape(Vector3 pointA, Vector3 pointB, Vector3 pointC) 33 public TriangleShape(Vector3 pointA, Vector3 pointB, Vector3 pointC)
34 { 34 {
35 _vertices[0] = pointA; 35 _vertices[0] = pointA;
36 _vertices[1] = pointB; 36 _vertices[1] = pointB;
37 _vertices[2] = pointC; 37 _vertices[2] = pointC;
38 } 38 }
39 39
40 public override int PreferredPenetrationDirectionsCount 40 public override int PreferredPenetrationDirectionsCount
41 { 41 {
42 get 42 get
43 { 43 {
44 return 2; 44 return 2;
45 } 45 }
46 } 46 }
47 47
48 public Vector3[] Vertices 48 public Vector3[] Vertices
49 { 49 {
50 get 50 get
51 { 51 {
52 return _vertices; 52 return _vertices;
53 } 53 }
54 } 54 }
55 55
56 public override int VertexCount 56 public override int VertexCount
57 { 57 {
58 get 58 get
59 { 59 {
60 return 3; 60 return 3;
61 } 61 }
62 } 62 }
63 63
64 public override int EdgeCount 64 public override int EdgeCount
65 { 65 {
66 get 66 get
67 { 67 {
68 return 3; 68 return 3;
69 } 69 }
70 } 70 }
71 71
72 public override int PlaneCount 72 public override int PlaneCount
73 { 73 {
74 get 74 get
75 { 75 {
76 return 1; 76 return 1;
77 } 77 }
78 } 78 }
79 79
80 public override BroadphaseNativeTypes ShapeType 80 public override BroadphaseNativeTypes ShapeType
81 { 81 {
82 get 82 get
83 { 83 {
84 return BroadphaseNativeTypes.Triangle; 84 return BroadphaseNativeTypes.Triangle;
85 } 85 }
86 } 86 }
87 87
88 public override string Name 88 public override string Name
89 { 89 {
90 get 90 get
91 { 91 {
92 return "Triangle"; 92 return "Triangle";
93 } 93 }
94 } 94 }
95 95
96 public override void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector) 96 public override void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector)
97 { 97 {
98 CalculateNormal(out penetrationVector); 98 CalculateNormal(out penetrationVector);
99 if (index != 0) 99 if (index != 0)
100 penetrationVector *= -1f; 100 penetrationVector *= -1f;
101 } 101 }
102 102
103 public virtual void GetPlaneEquation(int i, out Vector3 planeNormal, out Vector3 planeSupport) 103 public virtual void GetPlaneEquation(int i, out Vector3 planeNormal, out Vector3 planeSupport)
104 { 104 {
105 CalculateNormal(out planeNormal); 105 CalculateNormal(out planeNormal);
106 planeSupport = _vertices[0]; 106 planeSupport = _vertices[0];
107 } 107 }
108 108
109 public void CalculateNormal(out Vector3 normal) 109 public void CalculateNormal(out Vector3 normal)
110 { 110 {
111 normal = Vector3.Normalize(Vector3.Cross(_vertices[1] - _vertices[0], _vertices[2] - _vertices[0])); 111 normal = Vector3.Normalize(Vector3.Cross(_vertices[1] - _vertices[0], _vertices[2] - _vertices[0]));
112 } 112 }
113 113
114 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) 114 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
115 { 115 {
116 Vector3 dots = new Vector3(Vector3.Dot(vec, _vertices[0]), Vector3.Dot(vec, _vertices[1]), Vector3.Dot(vec, _vertices[2])); 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)]; 117 return _vertices[MathHelper.MaxAxis(dots)];
118 } 118 }
119 119
120 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) 120 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
121 { 121 {
122 for (int i = 0; i < vectors.Length; i++) 122 for (int i = 0; i < vectors.Length; i++)
123 { 123 {
124 Vector3 dir = vectors[i]; 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])); 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)]; 126 supportVerticesOut[i] = _vertices[MathHelper.MaxAxis(dots)];
127 } 127 }
128 } 128 }
129 129
130 public override void CalculateLocalInertia(float mass, out Vector3 inertia) 130 public override void CalculateLocalInertia(float mass, out Vector3 inertia)
131 { 131 {
132 inertia = new Vector3(); 132 inertia = new Vector3();
133 BulletDebug.Assert(false); 133 BulletDebug.Assert(false);
134 } 134 }
135 135
136 public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) 136 public override void GetEdge(int i, out Vector3 pa, out Vector3 pb)
137 { 137 {
138 GetVertex(i, out pa); 138 GetVertex(i, out pa);
139 GetVertex((i + 1) % 3, out pb); 139 GetVertex((i + 1) % 3, out pb);
140 } 140 }
141 141
142 public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) 142 public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
143 { 143 {
144 GetAabbSlow(t, out aabbMin, out aabbMax); 144 GetAabbSlow(t, out aabbMin, out aabbMax);
145 } 145 }
146 146
147 public override void GetVertex(int i, out Vector3 vtx) 147 public override void GetVertex(int i, out Vector3 vtx)
148 { 148 {
149 vtx = _vertices[i]; 149 vtx = _vertices[i];
150 } 150 }
151 151
152 public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) 152 public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i)
153 { 153 {
154 GetPlaneEquation(i, out planeNormal, out planeSupport); 154 GetPlaneEquation(i, out planeNormal, out planeSupport);
155 } 155 }
156 156
157 public override bool IsInside(Vector3 pt, float tolerance) 157 public override bool IsInside(Vector3 pt, float tolerance)
158 { 158 {
159 Vector3 normal; 159 Vector3 normal;
160 CalculateNormal(out normal); 160 CalculateNormal(out normal);
161 //distance to plane 161 //distance to plane
162 float dist = Vector3.Dot(pt, normal); 162 float dist = Vector3.Dot(pt, normal);
163 float planeconst = Vector3.Dot(_vertices[0], normal); 163 float planeconst = Vector3.Dot(_vertices[0], normal);
164 dist -= planeconst; 164 dist -= planeconst;
165 if (dist >= -tolerance && dist <= tolerance) 165 if (dist >= -tolerance && dist <= tolerance)
166 { 166 {
167 //inside check on edge-planes 167 //inside check on edge-planes
168 int i; 168 int i;
169 for (i = 0; i < 3; i++) 169 for (i = 0; i < 3; i++)
170 { 170 {
171 Vector3 pa, pb; 171 Vector3 pa, pb;
172 GetEdge(i, out pa, out pb); 172 GetEdge(i, out pa, out pb);
173 Vector3 edge = pb - pa; 173 Vector3 edge = pb - pa;
174 Vector3 edgeNormal = Vector3.Cross(edge, normal); 174 Vector3 edgeNormal = Vector3.Cross(edge, normal);
175 edgeNormal = Vector3.Normalize(edgeNormal); 175 edgeNormal = Vector3.Normalize(edgeNormal);
176 float distance = Vector3.Dot(pt, edgeNormal); 176 float distance = Vector3.Dot(pt, edgeNormal);
177 float edgeConst = Vector3.Dot(pa, edgeNormal); 177 float edgeConst = Vector3.Dot(pa, edgeNormal);
178 distance -= edgeConst; 178 distance -= edgeConst;
179 if (distance < -tolerance) 179 if (distance < -tolerance)
180 return false; 180 return false;
181 } 181 }
182 return true; 182 return true;
183 } 183 }
184 return false; 184 return false;
185 } 185 }
186 } 186 }
187} 187}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs
index 497f220..d011ef2 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs
@@ -1,199 +1,199 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// ContinuousConvexCollision implements angular and linear time of impact for convex objects. 30 /// ContinuousConvexCollision implements angular and linear time of impact for convex objects.
31 /// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis). 31 /// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis).
32 /// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent. 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 33 /// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops
34 /// </summary> 34 /// </summary>
35 public class ContinuousConvexCollision : IConvexCast 35 public class ContinuousConvexCollision : IConvexCast
36 { 36 {
37 /// <summary> 37 /// <summary>
38 /// This maximum should not be necessary. It allows for untested/degenerate cases in production code. 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. 39 /// You don't want your game ever to lock-up.
40 /// </summary> 40 /// </summary>
41 private const int MaxIterations = 1000; 41 private const int MaxIterations = 1000;
42 42
43 private ISimplexSolver _simplexSolver; 43 private ISimplexSolver _simplexSolver;
44 private IConvexPenetrationDepthSolver _penetrationDepthSolver; 44 private IConvexPenetrationDepthSolver _penetrationDepthSolver;
45 private ConvexShape _convexA; 45 private ConvexShape _convexA;
46 private ConvexShape _convexB; 46 private ConvexShape _convexB;
47 47
48 public ContinuousConvexCollision(ConvexShape convexA, ConvexShape convexB, 48 public ContinuousConvexCollision(ConvexShape convexA, ConvexShape convexB,
49 ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver) 49 ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
50 { 50 {
51 _simplexSolver = simplexSolver; 51 _simplexSolver = simplexSolver;
52 _penetrationDepthSolver = penetrationDepthSolver; 52 _penetrationDepthSolver = penetrationDepthSolver;
53 _convexA = convexA; 53 _convexA = convexA;
54 _convexB = convexB; 54 _convexB = convexB;
55 } 55 }
56 56
57 public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result) 57 public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
58 { 58 {
59 _simplexSolver.Reset(); 59 _simplexSolver.Reset();
60 60
61 // compute linear and angular velocity for this interval, to interpolate 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(); 62 Vector3 linVelA = new Vector3(), angVelA = new Vector3(), linVelB = new Vector3(), angVelB = new Vector3();
63 TransformUtil.CalculateVelocity(fromA, toA, 1f, ref linVelA, ref angVelA); 63 TransformUtil.CalculateVelocity(fromA, toA, 1f, ref linVelA, ref angVelA);
64 TransformUtil.CalculateVelocity(fromB, toB, 1f, ref linVelB, ref angVelB); 64 TransformUtil.CalculateVelocity(fromB, toB, 1f, ref linVelB, ref angVelB);
65 65
66 float boundingRadiusA = _convexA.GetAngularMotionDisc(); 66 float boundingRadiusA = _convexA.GetAngularMotionDisc();
67 float boundingRadiusB = _convexB.GetAngularMotionDisc(); 67 float boundingRadiusB = _convexB.GetAngularMotionDisc();
68 68
69 float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA + 69 float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA +
70 angVelB.Length() * boundingRadiusB; 70 angVelB.Length() * boundingRadiusB;
71 71
72 float radius = 0.001f; 72 float radius = 0.001f;
73 73
74 float lambda = 0f; 74 float lambda = 0f;
75 Vector3 v = new Vector3(1f, 0f, 0f); 75 Vector3 v = new Vector3(1f, 0f, 0f);
76 76
77 int maxIter = MaxIterations; 77 int maxIter = MaxIterations;
78 78
79 Vector3 n = new Vector3(); 79 Vector3 n = new Vector3();
80 bool hasResult = false; 80 bool hasResult = false;
81 Vector3 c; 81 Vector3 c;
82 82
83 float lastLambda = lambda; 83 float lastLambda = lambda;
84 //float epsilon = 0.001f; 84 //float epsilon = 0.001f;
85 85
86 int numIter = 0; 86 int numIter = 0;
87 //first solution, using GJK 87 //first solution, using GJK
88 88
89 89
90 Matrix identityTrans = Matrix.Identity; 90 Matrix identityTrans = Matrix.Identity;
91 91
92 SphereShape raySphere = new SphereShape(0f); 92 SphereShape raySphere = new SphereShape(0f);
93 raySphere.Margin=0f; 93 raySphere.Margin=0f;
94 94
95 95
96 //result.drawCoordSystem(sphereTr); 96 //result.drawCoordSystem(sphereTr);
97 97
98 PointCollector pointCollector1 = new PointCollector(); 98 PointCollector pointCollector1 = new PointCollector();
99 99
100 GjkPairDetector gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver); 100 GjkPairDetector gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);
101 GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); 101 GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
102 102
103 //we don't use margins during CCD 103 //we don't use margins during CCD
104 gjk.setIgnoreMargin(true); 104 gjk.setIgnoreMargin(true);
105 105
106 input.TransformA = fromA; 106 input.TransformA = fromA;
107 input.TransformB = fromB; 107 input.TransformB = fromB;
108 108
109 DiscreteCollisionDetectorInterface.Result r = (DiscreteCollisionDetectorInterface.Result)pointCollector1; 109 DiscreteCollisionDetectorInterface.Result r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;
110 gjk.GetClosestPoints(input, r, null); 110 gjk.GetClosestPoints(input, r, null);
111 111
112 hasResult = pointCollector1.HasResult; 112 hasResult = pointCollector1.HasResult;
113 c = pointCollector1.PointInWorld; 113 c = pointCollector1.PointInWorld;
114 114
115 if (hasResult) 115 if (hasResult)
116 { 116 {
117 float dist; 117 float dist;
118 dist = pointCollector1.Distance; 118 dist = pointCollector1.Distance;
119 n = pointCollector1.NormalOnBInWorld; 119 n = pointCollector1.NormalOnBInWorld;
120 120
121 //not close enough 121 //not close enough
122 while (dist > radius) 122 while (dist > radius)
123 { 123 {
124 numIter++; 124 numIter++;
125 if (numIter > maxIter) 125 if (numIter > maxIter)
126 return false; //todo: report a failure 126 return false; //todo: report a failure
127 127
128 float dLambda = 0f; 128 float dLambda = 0f;
129 129
130 //calculate safe moving fraction from distance / (linear+rotational velocity) 130 //calculate safe moving fraction from distance / (linear+rotational velocity)
131 131
132 //float clippedDist = GEN_min(angularConservativeRadius,dist); 132 //float clippedDist = GEN_min(angularConservativeRadius,dist);
133 //float clippedDist = dist; 133 //float clippedDist = dist;
134 134
135 float projectedLinearVelocity = Vector3.Dot(linVelB - linVelA, n); 135 float projectedLinearVelocity = Vector3.Dot(linVelB - linVelA, n);
136 136
137 dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity); 137 dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity);
138 138
139 lambda = lambda + dLambda; 139 lambda = lambda + dLambda;
140 140
141 if (lambda > 1f) return false; 141 if (lambda > 1f) return false;
142 if (lambda < 0f) return false; 142 if (lambda < 0f) return false;
143 143
144 //todo: next check with relative epsilon 144 //todo: next check with relative epsilon
145 if (lambda <= lastLambda) 145 if (lambda <= lastLambda)
146 break; 146 break;
147 lastLambda = lambda; 147 lastLambda = lambda;
148 148
149 149
150 //interpolate to next lambda 150 //interpolate to next lambda
151 Matrix interpolatedTransA = new Matrix(), interpolatedTransB = new Matrix(), relativeTrans; 151 Matrix interpolatedTransA = new Matrix(), interpolatedTransB = new Matrix(), relativeTrans;
152 152
153 TransformUtil.IntegrateTransform(fromA, linVelA, angVelA, lambda, ref interpolatedTransA); 153 TransformUtil.IntegrateTransform(fromA, linVelA, angVelA, lambda, ref interpolatedTransA);
154 TransformUtil.IntegrateTransform(fromB, linVelB, angVelB, lambda, ref interpolatedTransB); 154 TransformUtil.IntegrateTransform(fromB, linVelB, angVelB, lambda, ref interpolatedTransB);
155 155
156 relativeTrans = MathHelper.InverseTimes(interpolatedTransB, interpolatedTransA); 156 relativeTrans = MathHelper.InverseTimes(interpolatedTransB, interpolatedTransA);
157 157
158 result.DebugDraw(lambda); 158 result.DebugDraw(lambda);
159 159
160 PointCollector pointCollector = new PointCollector(); 160 PointCollector pointCollector = new PointCollector();
161 gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver); 161 gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);
162 input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); 162 input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
163 input.TransformA = interpolatedTransA; 163 input.TransformA = interpolatedTransA;
164 input.TransformB = interpolatedTransB; 164 input.TransformB = interpolatedTransB;
165 165
166 // !!!!!!!!!! 166 // !!!!!!!!!!
167 r = (DiscreteCollisionDetectorInterface.Result)pointCollector1; 167 r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;
168 gjk.GetClosestPoints(input, r, null); 168 gjk.GetClosestPoints(input, r, null);
169 169
170 if (pointCollector.HasResult) 170 if (pointCollector.HasResult)
171 { 171 {
172 if (pointCollector.Distance < 0f) 172 if (pointCollector.Distance < 0f)
173 { 173 {
174 //degenerate ?! 174 //degenerate ?!
175 result.Fraction = lastLambda; 175 result.Fraction = lastLambda;
176 result.Normal = n; 176 result.Normal = n;
177 return true; 177 return true;
178 } 178 }
179 c = pointCollector.PointInWorld; 179 c = pointCollector.PointInWorld;
180 180
181 dist = pointCollector.Distance; 181 dist = pointCollector.Distance;
182 } 182 }
183 else 183 else
184 { 184 {
185 //?? 185 //??
186 return false; 186 return false;
187 } 187 }
188 188
189 } 189 }
190 190
191 result.Fraction = lambda; 191 result.Fraction = lambda;
192 result.Normal = n; 192 result.Normal = n;
193 return true; 193 return true;
194 } 194 }
195 195
196 return false; 196 return false;
197 } 197 }
198 } 198 }
199} 199}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs
index 9e88a7e..93501ed 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs
@@ -1,73 +1,73 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 29
30 /// <summary> 30 /// <summary>
31 /// CastResult stores the closest result 31 /// CastResult stores the closest result
32 /// alternatively, add a callback method to decide about closest/all results 32 /// alternatively, add a callback method to decide about closest/all results
33 /// </summary> 33 /// </summary>
34 public class CastResult 34 public class CastResult
35 { 35 {
36 private Vector3 _normal; 36 private Vector3 _normal;
37 private float _fraction; 37 private float _fraction;
38 private Matrix _hitTransformA; 38 private Matrix _hitTransformA;
39 private Matrix _hitTransformB; 39 private Matrix _hitTransformB;
40 private IDebugDraw _debugDrawer; 40 private IDebugDraw _debugDrawer;
41 41
42 public CastResult() 42 public CastResult()
43 { 43 {
44 _fraction = 1e30f; 44 _fraction = 1e30f;
45 } 45 }
46 46
47 public Vector3 Normal { get { return _normal; } set { _normal = value; } } 47 public Vector3 Normal { get { return _normal; } set { _normal = value; } }
48 public float Fraction { get { return _fraction; } set { _fraction = value; } } 48 public float Fraction { get { return _fraction; } set { _fraction = value; } }
49 public Matrix HitTransformA { get { return _hitTransformA; } set { _hitTransformA = value; } } 49 public Matrix HitTransformA { get { return _hitTransformA; } set { _hitTransformA = value; } }
50 public Matrix HitTransformB { get { return _hitTransformB; } set { _hitTransformB = value; } } 50 public Matrix HitTransformB { get { return _hitTransformB; } set { _hitTransformB = value; } }
51 public IDebugDraw DebugDrawer { get { return _debugDrawer; } set { _debugDrawer = value; } } 51 public IDebugDraw DebugDrawer { get { return _debugDrawer; } set { _debugDrawer = value; } }
52 52
53 public virtual void DebugDraw(float fraction) { } 53 public virtual void DebugDraw(float fraction) { }
54 public virtual void DrawCoordSystem(Matrix trans) { } 54 public virtual void DrawCoordSystem(Matrix trans) { }
55 } 55 }
56 56
57 /// <summary> 57 /// <summary>
58 /// ConvexCast is an interface for Casting 58 /// ConvexCast is an interface for Casting
59 /// </summary> 59 /// </summary>
60 public interface IConvexCast 60 public interface IConvexCast
61 { 61 {
62 /// <summary> 62 /// <summary>
63 /// cast a convex against another convex object 63 /// cast a convex against another convex object
64 /// </summary> 64 /// </summary>
65 /// <param name="fromA"></param> 65 /// <param name="fromA"></param>
66 /// <param name="toA"></param> 66 /// <param name="toA"></param>
67 /// <param name="fromB"></param> 67 /// <param name="fromB"></param>
68 /// <param name="toB"></param> 68 /// <param name="toB"></param>
69 /// <param name="result"></param> 69 /// <param name="result"></param>
70 /// <returns></returns> 70 /// <returns></returns>
71 bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result); 71 bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result);
72 } 72 }
73} 73}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs
index fae2557..5b07d9b 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs
@@ -1,117 +1,117 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public abstract class DiscreteCollisionDetectorInterface 29 public abstract class DiscreteCollisionDetectorInterface
30 { 30 {
31 public abstract class Result 31 public abstract class Result
32 { 32 {
33 public abstract void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB); 33 public abstract void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB);
34 public abstract void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth); 34 public abstract void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth);
35 } 35 }
36 36
37 public class ClosestPointInput 37 public class ClosestPointInput
38 { 38 {
39 private float _maximumDistanceSquared; 39 private float _maximumDistanceSquared;
40 private Matrix _transformA, _transformB; 40 private Matrix _transformA, _transformB;
41 41
42 #region Properties 42 #region Properties
43 public Matrix TransformB 43 public Matrix TransformB
44 { 44 {
45 get { return _transformB; } 45 get { return _transformB; }
46 set { _transformB = value; } 46 set { _transformB = value; }
47 } 47 }
48 48
49 public Matrix TransformA 49 public Matrix TransformA
50 { 50 {
51 get { return _transformA; } 51 get { return _transformA; }
52 set { _transformA = value; } 52 set { _transformA = value; }
53 } 53 }
54 54
55 public float MaximumDistanceSquared 55 public float MaximumDistanceSquared
56 { 56 {
57 get { return _maximumDistanceSquared; } 57 get { return _maximumDistanceSquared; }
58 set { _maximumDistanceSquared = value; } 58 set { _maximumDistanceSquared = value; }
59 } 59 }
60 #endregion 60 #endregion
61 61
62 public ClosestPointInput() 62 public ClosestPointInput()
63 { 63 {
64 _maximumDistanceSquared = 1e30f; 64 _maximumDistanceSquared = 1e30f;
65 } 65 }
66 } 66 }
67 67
68 public abstract void GetClosestPoints(ClosestPointInput input, Result output, IDebugDraw debugDraw); 68 public abstract void GetClosestPoints(ClosestPointInput input, Result output, IDebugDraw debugDraw);
69 } 69 }
70 70
71 public class StorageResult : DiscreteCollisionDetectorInterface.Result 71 public class StorageResult : DiscreteCollisionDetectorInterface.Result
72 { 72 {
73 private Vector3 _closestPointInB; 73 private Vector3 _closestPointInB;
74 private Vector3 _normalOnSurfaceB; 74 private Vector3 _normalOnSurfaceB;
75 private float _distance; //negative means penetration ! 75 private float _distance; //negative means penetration !
76 76
77 #region Properties 77 #region Properties
78 78
79 public float Distance 79 public float Distance
80 { 80 {
81 get { return _distance; } 81 get { return _distance; }
82 set { _distance = value; } 82 set { _distance = value; }
83 } 83 }
84 public Vector3 NormalOnSurfaceB 84 public Vector3 NormalOnSurfaceB
85 { 85 {
86 get { return _normalOnSurfaceB; } 86 get { return _normalOnSurfaceB; }
87 set { _normalOnSurfaceB = value; } 87 set { _normalOnSurfaceB = value; }
88 } 88 }
89 public Vector3 ClosestPointInB 89 public Vector3 ClosestPointInB
90 { 90 {
91 get { return _closestPointInB; } 91 get { return _closestPointInB; }
92 set { _closestPointInB = value; } 92 set { _closestPointInB = value; }
93 } 93 }
94 94
95 #endregion 95 #endregion
96 96
97 public StorageResult() 97 public StorageResult()
98 { 98 {
99 _distance = 1e30f; 99 _distance = 1e30f;
100 } 100 }
101 101
102 public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) 102 public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth)
103 { 103 {
104 if (depth < _distance) 104 if (depth < _distance)
105 { 105 {
106 _normalOnSurfaceB = normalOnBInWorld; 106 _normalOnSurfaceB = normalOnBInWorld;
107 _closestPointInB = pointInWorld; 107 _closestPointInB = pointInWorld;
108 _distance = depth; 108 _distance = depth;
109 } 109 }
110 } 110 }
111 111
112 public override void SetShapeIdentifiers(int partId0, int index0, int partId1, int index1) 112 public override void SetShapeIdentifiers(int partId0, int index0, int partId1, int index1)
113 { 113 {
114 throw new Exception("The method or operation is not implemented."); 114 throw new Exception("The method or operation is not implemented.");
115 } 115 }
116 } 116 }
117} 117}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs
index 4ee9599..ea7d335 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs
@@ -1,176 +1,176 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// GjkConvexCast performs a raycast on a convex object using support mapping. 30 /// GjkConvexCast performs a raycast on a convex object using support mapping.
31 /// </summary> 31 /// </summary>
32 public class GjkConvexCast : IConvexCast 32 public class GjkConvexCast : IConvexCast
33 { 33 {
34 private VoronoiSimplexSolver _simplexSolver; 34 private VoronoiSimplexSolver _simplexSolver;
35 private ConvexShape _convexA, _convexB; 35 private ConvexShape _convexA, _convexB;
36 36
37 public GjkConvexCast(ConvexShape convexShapeA, ConvexShape convexShapeB, VoronoiSimplexSolver solver) 37 public GjkConvexCast(ConvexShape convexShapeA, ConvexShape convexShapeB, VoronoiSimplexSolver solver)
38 { 38 {
39 _simplexSolver = solver; 39 _simplexSolver = solver;
40 40
41 _convexA = convexShapeA; 41 _convexA = convexShapeA;
42 _convexB = convexShapeB; 42 _convexB = convexShapeB;
43 } 43 }
44 44
45 #region IConvexCast Members 45 #region IConvexCast Members
46 46
47 /// <summary> 47 /// <summary>
48 /// cast a convex against another convex object 48 /// cast a convex against another convex object
49 /// </summary> 49 /// </summary>
50 /// <param name="fromA"></param> 50 /// <param name="fromA"></param>
51 /// <param name="toA"></param> 51 /// <param name="toA"></param>
52 /// <param name="fromB"></param> 52 /// <param name="fromB"></param>
53 /// <param name="toB"></param> 53 /// <param name="toB"></param>
54 /// <param name="result"></param> 54 /// <param name="result"></param>
55 /// <returns></returns> 55 /// <returns></returns>
56 public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result) 56 public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
57 { 57 {
58 MinkowskiSumShape combined = new MinkowskiSumShape(_convexA, _convexB); 58 MinkowskiSumShape combined = new MinkowskiSumShape(_convexA, _convexB);
59 59
60 Matrix rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB; 60 Matrix rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB;
61 Matrix rayToLocalA = MathHelper.InvertMatrix(toA) * toB; 61 Matrix rayToLocalA = MathHelper.InvertMatrix(toA) * toB;
62 62
63 Matrix transformA = fromA; 63 Matrix transformA = fromA;
64 Matrix transformB = fromB; 64 Matrix transformB = fromB;
65 65
66 transformA.Translation = new Vector3(0, 0, 0); 66 transformA.Translation = new Vector3(0, 0, 0);
67 transformB.Translation = new Vector3(0, 0, 0); 67 transformB.Translation = new Vector3(0, 0, 0);
68 68
69 combined.TransformA = transformA; 69 combined.TransformA = transformA;
70 combined.TransformB = transformB; 70 combined.TransformB = transformB;
71 71
72 float radius = 0.01f; 72 float radius = 0.01f;
73 float lambda = 0; 73 float lambda = 0;
74 74
75 Vector3 s = rayFromLocalA.Translation; 75 Vector3 s = rayFromLocalA.Translation;
76 Vector3 r = rayToLocalA.Translation - rayFromLocalA.Translation; 76 Vector3 r = rayToLocalA.Translation - rayFromLocalA.Translation;
77 Vector3 x = s; 77 Vector3 x = s;
78 Vector3 n = new Vector3(); 78 Vector3 n = new Vector3();
79 Vector3 c = new Vector3(); 79 Vector3 c = new Vector3();
80 80
81 bool hasResult = false; 81 bool hasResult = false;
82 float lastLambda = lambda; 82 float lastLambda = lambda;
83 83
84 IConvexPenetrationDepthSolver penSolver = null; 84 IConvexPenetrationDepthSolver penSolver = null;
85 Matrix identityTransform = Matrix.Identity; 85 Matrix identityTransform = Matrix.Identity;
86 86
87 SphereShape raySphere = new SphereShape(0.0f); 87 SphereShape raySphere = new SphereShape(0.0f);
88 raySphere.Margin=0.0f; 88 raySphere.Margin=0.0f;
89 89
90 Matrix sphereTransform = Matrix.Identity; 90 Matrix sphereTransform = Matrix.Identity;
91 sphereTransform.Translation = rayFromLocalA.Translation; 91 sphereTransform.Translation = rayFromLocalA.Translation;
92 92
93 result.DrawCoordSystem(sphereTransform); 93 result.DrawCoordSystem(sphereTransform);
94 94
95 { 95 {
96 PointCollector pointCollector = new PointCollector(); 96 PointCollector pointCollector = new PointCollector();
97 GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver); 97 GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);
98 98
99 GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); 99 GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
100 input.TransformA = sphereTransform; 100 input.TransformA = sphereTransform;
101 input.TransformB = identityTransform; 101 input.TransformB = identityTransform;
102 102
103 gjk.GetClosestPoints(input, pointCollector, null); 103 gjk.GetClosestPoints(input, pointCollector, null);
104 104
105 hasResult = pointCollector.HasResult; 105 hasResult = pointCollector.HasResult;
106 106
107 c = pointCollector.PointInWorld; 107 c = pointCollector.PointInWorld;
108 n = pointCollector.NormalOnBInWorld; 108 n = pointCollector.NormalOnBInWorld;
109 } 109 }
110 110
111 if (hasResult) 111 if (hasResult)
112 { 112 {
113 float dist = (c - x).Length(); 113 float dist = (c - x).Length();
114 114
115 if (dist < radius) 115 if (dist < radius)
116 { 116 {
117 lastLambda = 1.0f; 117 lastLambda = 1.0f;
118 } 118 }
119 119
120 while (dist > radius) 120 while (dist > radius)
121 { 121 {
122 n = x - c; 122 n = x - c;
123 float dot = Vector3.Dot(n, r); 123 float dot = Vector3.Dot(n, r);
124 124
125 if (dot >= -(MathHelper.Epsilon * MathHelper.Epsilon)) return false; 125 if (dot >= -(MathHelper.Epsilon * MathHelper.Epsilon)) return false;
126 126
127 lambda = lambda - Vector3.Distance(n, n) / dot; 127 lambda = lambda - Vector3.Distance(n, n) / dot;
128 if (lambda <= lastLambda) break; 128 if (lambda <= lastLambda) break;
129 129
130 lastLambda = lambda; 130 lastLambda = lambda;
131 131
132 x = s + lambda * r; 132 x = s + lambda * r;
133 133
134 sphereTransform.Translation = x; 134 sphereTransform.Translation = x;
135 result.DrawCoordSystem(sphereTransform); 135 result.DrawCoordSystem(sphereTransform);
136 PointCollector pointCollector = new PointCollector(); 136 PointCollector pointCollector = new PointCollector();
137 137
138 GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver); 138 GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);
139 GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); 139 GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
140 input.TransformA = sphereTransform; 140 input.TransformA = sphereTransform;
141 input.TransformB = identityTransform; 141 input.TransformB = identityTransform;
142 142
143 gjk.GetClosestPoints(input, pointCollector, null); 143 gjk.GetClosestPoints(input, pointCollector, null);
144 144
145 if (pointCollector.HasResult) 145 if (pointCollector.HasResult)
146 { 146 {
147 if (pointCollector.Distance < 0.0f) 147 if (pointCollector.Distance < 0.0f)
148 { 148 {
149 result.Fraction = lastLambda; 149 result.Fraction = lastLambda;
150 result.Normal = n; 150 result.Normal = n;
151 return true; 151 return true;
152 } 152 }
153 153
154 c = pointCollector.PointInWorld; 154 c = pointCollector.PointInWorld;
155 dist = (c - x).Length(); 155 dist = (c - x).Length();
156 } 156 }
157 else 157 else
158 { 158 {
159 return false; 159 return false;
160 } 160 }
161 } 161 }
162 162
163 if (lastLambda < 1.0f) 163 if (lastLambda < 1.0f)
164 { 164 {
165 result.Fraction = lastLambda; 165 result.Fraction = lastLambda;
166 result.Normal = n; 166 result.Normal = n;
167 return true; 167 return true;
168 } 168 }
169 } 169 }
170 170
171 return false; 171 return false;
172 } 172 }
173 173
174 #endregion 174 #endregion
175 } 175 }
176} 176}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs
index 50ae7ab..a3064db 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs
@@ -1,633 +1,633 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// GJK-EPA collision solver by Nathanael Presson 30 /// GJK-EPA collision solver by Nathanael Presson
31 /// Nov.2006 31 /// Nov.2006
32 /// </summary> 32 /// </summary>
33 public class GjkEpa 33 public class GjkEpa
34 { 34 {
35 //private static readonly int _precision = 1 /* U(sizeof(F) == 4)*/; 35 //private static readonly int _precision = 1 /* U(sizeof(F) == 4)*/;
36 36
37 private static readonly float _infinity = MathHelper.Infinity; 37 private static readonly float _infinity = MathHelper.Infinity;
38 //private static readonly float _pi = (float)Math.PI; 38 //private static readonly float _pi = (float)Math.PI;
39 private static readonly float _twoPi = (float)(Math.PI * 2); 39 private static readonly float _twoPi = (float)(Math.PI * 2);
40 40
41 private static readonly int _gjkMaxIterations = 128; 41 private static readonly int _gjkMaxIterations = 128;
42 private static readonly int _gjkHashSize = 1 << 6; 42 private static readonly int _gjkHashSize = 1 << 6;
43 private static readonly int _gjkHashMask = _gjkHashSize - 1; 43 private static readonly int _gjkHashMask = _gjkHashSize - 1;
44 private static readonly float _gjkInSimplexEpsilon = 0.0001f; 44 private static readonly float _gjkInSimplexEpsilon = 0.0001f;
45 private static readonly float _gjkSquareInSimplexEpsilon = _gjkInSimplexEpsilon * _gjkInSimplexEpsilon; 45 private static readonly float _gjkSquareInSimplexEpsilon = _gjkInSimplexEpsilon * _gjkInSimplexEpsilon;
46 46
47 private static readonly int _epaMaxIterations = 256; 47 private static readonly int _epaMaxIterations = 256;
48 private static readonly float _epaInFaceEpsilon = 0.01f; 48 private static readonly float _epaInFaceEpsilon = 0.01f;
49 private static readonly float _epaAccuracy = 0.001f; 49 private static readonly float _epaAccuracy = 0.001f;
50 50
51 public static float EpaAccuracy { get { return _epaAccuracy; } } 51 public static float EpaAccuracy { get { return _epaAccuracy; } }
52 52
53 private static float Abs(float v) { return (v < 0 ? -v : v); } 53 private static float Abs(float v) { return (v < 0 ? -v : v); }
54 private static float Sign(float v) { return (v < 0 ? -1 : 1); } 54 private static float Sign(float v) { return (v < 0 ? -1 : 1); }
55 55
56 static void Swap<T>(ref T a, ref T b) 56 static void Swap<T>(ref T a, ref T b)
57 { 57 {
58 T t = a; 58 T t = a;
59 a = b; 59 a = b;
60 b = t; 60 b = t;
61 } 61 }
62 62
63 public class Gjk 63 public class Gjk
64 { 64 {
65 public class MinkowskiVertice 65 public class MinkowskiVertice
66 { 66 {
67 private Vector3 _vertice; /* Minkowski vertice */ 67 private Vector3 _vertice; /* Minkowski vertice */
68 private Vector3 _ray; /* Ray */ 68 private Vector3 _ray; /* Ray */
69 69
70 public Vector3 Vertice { get { return _vertice; } set { _vertice = value; } } 70 public Vector3 Vertice { get { return _vertice; } set { _vertice = value; } }
71 public Vector3 Ray { get { return _ray; } set { _ray = value; } } 71 public Vector3 Ray { get { return _ray; } set { _ray = value; } }
72 } 72 }
73 public class He 73 public class He
74 { 74 {
75 private Vector3 _ray; 75 private Vector3 _ray;
76 private He _next; 76 private He _next;
77 77
78 public He Next { get { return _next; } set { _next = value; } } 78 public He Next { get { return _next; } set { _next = value; } }
79 public Vector3 Ray { get { return _ray; } set { _ray = value; } } 79 public Vector3 Ray { get { return _ray; } set { _ray = value; } }
80 } 80 }
81 81
82 private He[] _table = new He[_gjkHashSize]; 82 private He[] _table = new He[_gjkHashSize];
83 private Matrix[] _wrotations = new Matrix[2]; 83 private Matrix[] _wrotations = new Matrix[2];
84 private Vector3[] _positions = new Vector3[2]; 84 private Vector3[] _positions = new Vector3[2];
85 private ConvexShape[] _shapes = new ConvexShape[2]; 85 private ConvexShape[] _shapes = new ConvexShape[2];
86 private MinkowskiVertice[] _simplex = new MinkowskiVertice[5]; 86 private MinkowskiVertice[] _simplex = new MinkowskiVertice[5];
87 private Vector3 _ray; 87 private Vector3 _ray;
88 private int _order; 88 private int _order;
89 private int _iterations; 89 private int _iterations;
90 private float _margin; 90 private float _margin;
91 private bool _failed; 91 private bool _failed;
92 92
93 public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA, 93 public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA,
94 Matrix wrotationB, Vector3 positionB, ConvexShape shapeB) 94 Matrix wrotationB, Vector3 positionB, ConvexShape shapeB)
95 : this(wrotationA, positionA, shapeA, wrotationB, positionB, shapeB, 0) { } 95 : this(wrotationA, positionA, shapeA, wrotationB, positionB, shapeB, 0) { }
96 96
97 public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA, 97 public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA,
98 Matrix wrotationB, Vector3 positionB, ConvexShape shapeB, 98 Matrix wrotationB, Vector3 positionB, ConvexShape shapeB,
99 float pmargin) 99 float pmargin)
100 { 100 {
101 for (int i = 0; i < _simplex.Length; i++) 101 for (int i = 0; i < _simplex.Length; i++)
102 _simplex[i] = new MinkowskiVertice(); 102 _simplex[i] = new MinkowskiVertice();
103 103
104 for (int i = 0; i < _wrotations.Length; i++) 104 for (int i = 0; i < _wrotations.Length; i++)
105 _wrotations[i] = new Matrix(); 105 _wrotations[i] = new Matrix();
106 106
107 for (int i = 0; i < _positions.Length; i++) 107 for (int i = 0; i < _positions.Length; i++)
108 _positions[i] = new Vector3(); 108 _positions[i] = new Vector3();
109 109
110 _wrotations[0] = wrotationA; _positions[0] = positionA; 110 _wrotations[0] = wrotationA; _positions[0] = positionA;
111 _shapes[0] = shapeA; 111 _shapes[0] = shapeA;
112 _wrotations[0].Translation = Vector3.Zero; 112 _wrotations[0].Translation = Vector3.Zero;
113 _wrotations[1] = wrotationB; _positions[1] = positionB; 113 _wrotations[1] = wrotationB; _positions[1] = positionB;
114 _shapes[1] = shapeB; 114 _shapes[1] = shapeB;
115 _wrotations[1].Translation = Vector3.Zero; 115 _wrotations[1].Translation = Vector3.Zero;
116 //sablock = sa->BeginBlock(); 116 //sablock = sa->BeginBlock();
117 _margin = pmargin; 117 _margin = pmargin;
118 _failed = false; 118 _failed = false;
119 } 119 }
120 120
121 public bool Failed { get { return _failed; } } 121 public bool Failed { get { return _failed; } }
122 public int Iterations { get { return _iterations; } } 122 public int Iterations { get { return _iterations; } }
123 public int Order { get { return _order; } } 123 public int Order { get { return _order; } }
124 public MinkowskiVertice[] Simplex { get { return _simplex; } } 124 public MinkowskiVertice[] Simplex { get { return _simplex; } }
125 125
126 public int Hash(Vector3 v) 126 public int Hash(Vector3 v)
127 { 127 {
128 int h = ((int)(v.X * 15461) ^ (int)(v.Y * 83003) ^ (int)(v.Z * 15473)); 128 int h = ((int)(v.X * 15461) ^ (int)(v.Y * 83003) ^ (int)(v.Z * 15473));
129 return (h * 169639) & _gjkHashMask; 129 return (h * 169639) & _gjkHashMask;
130 } 130 }
131 131
132 public bool FetchSupport() 132 public bool FetchSupport()
133 { 133 {
134 int h = Hash(_ray); 134 int h = Hash(_ray);
135 He e = _table[h]; 135 He e = _table[h];
136 while (e != null) 136 while (e != null)
137 { 137 {
138 if (e.Ray == _ray) 138 if (e.Ray == _ray)
139 { 139 {
140 --_order; 140 --_order;
141 return (false); 141 return (false);
142 } 142 }
143 else e = e.Next; 143 else e = e.Next;
144 } 144 }
145 e = new He(); 145 e = new He();
146 e.Ray = _ray; 146 e.Ray = _ray;
147 e.Next = _table[h]; 147 e.Next = _table[h];
148 _table[h] = e; 148 _table[h] = e;
149 Support(_ray, ref _simplex[++_order]); 149 Support(_ray, ref _simplex[++_order]);
150 return (Vector3.Dot(_ray, _simplex[_order].Vertice) > 0); 150 return (Vector3.Dot(_ray, _simplex[_order].Vertice) > 0);
151 } 151 }
152 152
153 public Vector3 LocalSupport(Vector3 d, int i) 153 public Vector3 LocalSupport(Vector3 d, int i)
154 { 154 {
155 Matrix m = _wrotations[i]; 155 Matrix m = _wrotations[i];
156 m.Translation = Vector3.Zero; 156 m.Translation = Vector3.Zero;
157 Vector3 vtx = Vector3.TransformNormal(d, m); 157 Vector3 vtx = Vector3.TransformNormal(d, m);
158 Vector3 result = MathHelper.MatrixToVector(_wrotations[i], _shapes[i].LocalGetSupportingVertex(vtx)); 158 Vector3 result = MathHelper.MatrixToVector(_wrotations[i], _shapes[i].LocalGetSupportingVertex(vtx));
159 return (result + _positions[i]); 159 return (result + _positions[i]);
160 } 160 }
161 161
162 public void Support(Vector3 d, ref MinkowskiVertice v) 162 public void Support(Vector3 d, ref MinkowskiVertice v)
163 { 163 {
164 v.Ray = d; 164 v.Ray = d;
165 v.Vertice = LocalSupport(d, 0) - LocalSupport(-d, 1) + d * _margin; 165 v.Vertice = LocalSupport(d, 0) - LocalSupport(-d, 1) + d * _margin;
166 } 166 }
167 167
168 public bool SolveSimplex2(Vector3 ao, Vector3 ab) 168 public bool SolveSimplex2(Vector3 ao, Vector3 ab)
169 { 169 {
170 if (Vector3.Dot(ab, ao) >= 0) 170 if (Vector3.Dot(ab, ao) >= 0)
171 { 171 {
172 Vector3 cabo = Vector3.Cross(ab, ao); 172 Vector3 cabo = Vector3.Cross(ab, ao);
173 if (cabo.LengthSquared() > _gjkSquareInSimplexEpsilon) 173 if (cabo.LengthSquared() > _gjkSquareInSimplexEpsilon)
174 { _ray = Vector3.Cross(cabo, ab); } 174 { _ray = Vector3.Cross(cabo, ab); }
175 else 175 else
176 { return true; } 176 { return true; }
177 } 177 }
178 else 178 else
179 { 179 {
180 _order = 0; 180 _order = 0;
181 _simplex[0].Ray = _simplex[1].Ray; 181 _simplex[0].Ray = _simplex[1].Ray;
182 _simplex[0].Vertice = _simplex[1].Vertice; 182 _simplex[0].Vertice = _simplex[1].Vertice;
183 183
184 _ray = ao; 184 _ray = ao;
185 } 185 }
186 return false; 186 return false;
187 } 187 }
188 188
189 public bool SolveSimplex3(Vector3 ao, Vector3 ab, Vector3 ac) 189 public bool SolveSimplex3(Vector3 ao, Vector3 ab, Vector3 ac)
190 { 190 {
191 return (SolveSimplex3a(ao, ab, ac, Vector3.Cross(ab, ac))); 191 return (SolveSimplex3a(ao, ab, ac, Vector3.Cross(ab, ac)));
192 } 192 }
193 193
194 public bool SolveSimplex3a(Vector3 ao, Vector3 ab, Vector3 ac, Vector3 cabc) 194 public bool SolveSimplex3a(Vector3 ao, Vector3 ab, Vector3 ac, Vector3 cabc)
195 { 195 {
196 if ((Vector3.Dot(Vector3.Cross(cabc, ab), ao)) < -_gjkInSimplexEpsilon) 196 if ((Vector3.Dot(Vector3.Cross(cabc, ab), ao)) < -_gjkInSimplexEpsilon)
197 { 197 {
198 _order = 1; 198 _order = 1;
199 _simplex[0].Vertice = _simplex[1].Vertice; 199 _simplex[0].Vertice = _simplex[1].Vertice;
200 _simplex[0].Ray = _simplex[1].Ray; 200 _simplex[0].Ray = _simplex[1].Ray;
201 201
202 _simplex[1].Vertice = _simplex[2].Vertice; 202 _simplex[1].Vertice = _simplex[2].Vertice;
203 _simplex[1].Ray = _simplex[2].Ray; 203 _simplex[1].Ray = _simplex[2].Ray;
204 204
205 return (SolveSimplex2(ao, ab)); 205 return (SolveSimplex2(ao, ab));
206 } 206 }
207 else if (Vector3.Dot(Vector3.Cross(cabc, ac), ao) > +_gjkInSimplexEpsilon) 207 else if (Vector3.Dot(Vector3.Cross(cabc, ac), ao) > +_gjkInSimplexEpsilon)
208 { 208 {
209 _order = 1; 209 _order = 1;
210 _simplex[1].Vertice = _simplex[2].Vertice; 210 _simplex[1].Vertice = _simplex[2].Vertice;
211 _simplex[1].Ray = _simplex[2].Ray; 211 _simplex[1].Ray = _simplex[2].Ray;
212 212
213 return (SolveSimplex2(ao, ac)); 213 return (SolveSimplex2(ao, ac));
214 } 214 }
215 else 215 else
216 { 216 {
217 float d = Vector3.Dot(cabc, ao); 217 float d = Vector3.Dot(cabc, ao);
218 if (Abs(d) > _gjkInSimplexEpsilon) 218 if (Abs(d) > _gjkInSimplexEpsilon)
219 { 219 {
220 if (d > 0) 220 if (d > 0)
221 { _ray = cabc; } 221 { _ray = cabc; }
222 else 222 else
223 { _ray = -cabc; Swap<MinkowskiVertice>(ref _simplex[0], ref _simplex[1]); } 223 { _ray = -cabc; Swap<MinkowskiVertice>(ref _simplex[0], ref _simplex[1]); }
224 return (false); 224 return (false);
225 } 225 }
226 else return (true); 226 else return (true);
227 } 227 }
228 } 228 }
229 229
230 public bool SolveSimplex4(Vector3 ao, Vector3 ab, Vector3 ac, Vector3 ad) 230 public bool SolveSimplex4(Vector3 ao, Vector3 ab, Vector3 ac, Vector3 ad)
231 { 231 {
232 Vector3 crs; 232 Vector3 crs;
233 if (Vector3.Dot((crs = Vector3.Cross(ab, ac)), ao) > _gjkInSimplexEpsilon) 233 if (Vector3.Dot((crs = Vector3.Cross(ab, ac)), ao) > _gjkInSimplexEpsilon)
234 { 234 {
235 _order = 2; 235 _order = 2;
236 _simplex[0].Vertice = _simplex[1].Vertice; 236 _simplex[0].Vertice = _simplex[1].Vertice;
237 _simplex[0].Ray = _simplex[1].Ray; 237 _simplex[0].Ray = _simplex[1].Ray;
238 238
239 _simplex[1].Vertice = _simplex[2].Vertice; 239 _simplex[1].Vertice = _simplex[2].Vertice;
240 _simplex[1].Ray = _simplex[2].Ray; 240 _simplex[1].Ray = _simplex[2].Ray;
241 241
242 _simplex[2].Vertice = _simplex[3].Vertice; 242 _simplex[2].Vertice = _simplex[3].Vertice;
243 _simplex[2].Ray = _simplex[3].Ray; 243 _simplex[2].Ray = _simplex[3].Ray;
244 244
245 return (SolveSimplex3a(ao, ab, ac, crs)); 245 return (SolveSimplex3a(ao, ab, ac, crs));
246 } 246 }
247 else if (Vector3.Dot((crs = Vector3.Cross(ac, ad)), ao) > _gjkInSimplexEpsilon) 247 else if (Vector3.Dot((crs = Vector3.Cross(ac, ad)), ao) > _gjkInSimplexEpsilon)
248 { 248 {
249 _order = 2; 249 _order = 2;
250 _simplex[2].Vertice = _simplex[3].Vertice; 250 _simplex[2].Vertice = _simplex[3].Vertice;
251 _simplex[2].Ray = _simplex[3].Ray; 251 _simplex[2].Ray = _simplex[3].Ray;
252 252
253 return (SolveSimplex3a(ao, ac, ad, crs)); 253 return (SolveSimplex3a(ao, ac, ad, crs));
254 } 254 }
255 else if (Vector3.Dot((crs = Vector3.Cross(ad, ab)), ao) > _gjkInSimplexEpsilon) 255 else if (Vector3.Dot((crs = Vector3.Cross(ad, ab)), ao) > _gjkInSimplexEpsilon)
256 { 256 {
257 _order = 2; 257 _order = 2;
258 258
259 _simplex[1].Vertice = _simplex[0].Vertice; 259 _simplex[1].Vertice = _simplex[0].Vertice;
260 _simplex[1].Ray = _simplex[0].Ray; 260 _simplex[1].Ray = _simplex[0].Ray;
261 261
262 _simplex[0].Vertice = _simplex[2].Vertice; 262 _simplex[0].Vertice = _simplex[2].Vertice;
263 _simplex[0].Ray = _simplex[2].Ray; 263 _simplex[0].Ray = _simplex[2].Ray;
264 264
265 _simplex[2].Vertice = _simplex[3].Vertice; 265 _simplex[2].Vertice = _simplex[3].Vertice;
266 _simplex[2].Ray = _simplex[3].Ray; 266 _simplex[2].Ray = _simplex[3].Ray;
267 267
268 return (SolveSimplex3a(ao, ad, ab, crs)); 268 return (SolveSimplex3a(ao, ad, ab, crs));
269 } 269 }
270 else return (true); 270 else return (true);
271 } 271 }
272 272
273 public bool SearchOrigin() 273 public bool SearchOrigin()
274 { 274 {
275 return SearchOrigin(new Vector3(1, 0, 0)); 275 return SearchOrigin(new Vector3(1, 0, 0));
276 } 276 }
277 277
278 public bool SearchOrigin(Vector3 initray) 278 public bool SearchOrigin(Vector3 initray)
279 { 279 {
280 _iterations = 0; 280 _iterations = 0;
281 unchecked 281 unchecked
282 { 282 {
283 _order = (int)(-1); 283 _order = (int)(-1);
284 } 284 }
285 _failed = false; 285 _failed = false;
286 _ray = Vector3.Normalize(initray); 286 _ray = Vector3.Normalize(initray);
287 287
288 //ClearMemory(table, sizeof(void*) * GJK_hashsize); 288 //ClearMemory(table, sizeof(void*) * GJK_hashsize);
289 for (int i = 0; i < _table.Length; i++) 289 for (int i = 0; i < _table.Length; i++)
290 _table[i] = null; 290 _table[i] = null;
291 FetchSupport(); 291 FetchSupport();
292 _ray = -_simplex[0].Vertice; 292 _ray = -_simplex[0].Vertice;
293 for (; _iterations < _gjkMaxIterations; ++_iterations) 293 for (; _iterations < _gjkMaxIterations; ++_iterations)
294 { 294 {
295 float rl = _ray.Length(); 295 float rl = _ray.Length();
296 _ray /= rl > 0 ? rl : 1; 296 _ray /= rl > 0 ? rl : 1;
297 if (FetchSupport()) 297 if (FetchSupport())
298 { 298 {
299 bool found = (false); 299 bool found = (false);
300 switch (_order) 300 switch (_order)
301 { 301 {
302 case 1: found = SolveSimplex2(-_simplex[1].Vertice, _simplex[0].Vertice - _simplex[1].Vertice); break; 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; 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; 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 } 305 }
306 if (found) return (true); 306 if (found) return (true);
307 } 307 }
308 else return (false); 308 else return (false);
309 } 309 }
310 _failed = true; 310 _failed = true;
311 return (false); 311 return (false);
312 } 312 }
313 313
314 public bool EncloseOrigin() 314 public bool EncloseOrigin()
315 { 315 {
316 switch (_order) 316 switch (_order)
317 { 317 {
318 /* Point */ 318 /* Point */
319 case 0: break; 319 case 0: break;
320 /* Line */ 320 /* Line */
321 case 1: 321 case 1:
322 Vector3 ab = _simplex[1].Vertice - _simplex[0].Vertice; 322 Vector3 ab = _simplex[1].Vertice - _simplex[0].Vertice;
323 Vector3[] b ={ Vector3.Cross(ab, new Vector3(1, 0, 0)), 323 Vector3[] b ={ Vector3.Cross(ab, new Vector3(1, 0, 0)),
324 Vector3.Cross(ab, new Vector3(0, 1, 0)), 324 Vector3.Cross(ab, new Vector3(0, 1, 0)),
325 Vector3.Cross(ab, new Vector3(0, 0, 1)) }; 325 Vector3.Cross(ab, new Vector3(0, 0, 1)) };
326 float[] m ={ b[0].LengthSquared(), b[1].LengthSquared(), b[2].LengthSquared() }; 326 float[] m ={ b[0].LengthSquared(), b[1].LengthSquared(), b[2].LengthSquared() };
327 Matrix r = Matrix.CreateFromQuaternion(new Quaternion(Vector3.Normalize(ab), _twoPi / 3)); 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]; 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); 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); 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); 331 Support(Vector3.Normalize(w), ref _simplex[3]); w = Vector3.TransformNormal(w, r);
332 _order = 4; 332 _order = 4;
333 return true; 333 return true;
334 /* Triangle */ 334 /* Triangle */
335 case 2: 335 case 2:
336 Vector3 n = Vector3.Normalize(Vector3.Cross(_simplex[1].Vertice - _simplex[0].Vertice, _simplex[2].Vertice - _simplex[0].Vertice)); 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]); 337 Support(n, ref _simplex[3]);
338 Support(-n, ref _simplex[4]); 338 Support(-n, ref _simplex[4]);
339 _order = 4; 339 _order = 4;
340 return true; 340 return true;
341 /* Tetrahedron */ 341 /* Tetrahedron */
342 case 3: return (true); 342 case 3: return (true);
343 /* Hexahedron */ 343 /* Hexahedron */
344 case 4: return (true); 344 case 4: return (true);
345 } 345 }
346 return (false); 346 return (false);
347 } 347 }
348 } 348 }
349 349
350 public class Epa 350 public class Epa
351 { 351 {
352 public class Face 352 public class Face
353 { 353 {
354 public Gjk.MinkowskiVertice[] _vertices = new Gjk.MinkowskiVertice[3]; 354 public Gjk.MinkowskiVertice[] _vertices = new Gjk.MinkowskiVertice[3];
355 public Face[] _faces = new Face[3]; 355 public Face[] _faces = new Face[3];
356 public int[] _e = new int[3]; 356 public int[] _e = new int[3];
357 public Vector3 _n; 357 public Vector3 _n;
358 public float _d; 358 public float _d;
359 public int _mark; 359 public int _mark;
360 public Face _prev; 360 public Face _prev;
361 public Face _next; 361 public Face _next;
362 } 362 }
363 363
364 private Gjk _gjk; 364 private Gjk _gjk;
365 private Face _root; 365 private Face _root;
366 private int _nfaces; 366 private int _nfaces;
367 private int _iterations; 367 private int _iterations;
368 private Vector3[,] _features = new Vector3[2, 3]; 368 private Vector3[,] _features = new Vector3[2, 3];
369 private Vector3[] _nearest = new Vector3[2]; 369 private Vector3[] _nearest = new Vector3[2];
370 private Vector3 _normal; 370 private Vector3 _normal;
371 private float _depth; 371 private float _depth;
372 private bool _failed; 372 private bool _failed;
373 373
374 public Epa(Gjk gjk) 374 public Epa(Gjk gjk)
375 { 375 {
376 this._gjk = gjk; 376 this._gjk = gjk;
377 } 377 }
378 378
379 public bool Failed { get { return _failed; } } 379 public bool Failed { get { return _failed; } }
380 public int Iterations { get { return _iterations; } } 380 public int Iterations { get { return _iterations; } }
381 public Vector3 Normal { get { return _normal; } } 381 public Vector3 Normal { get { return _normal; } }
382 public Vector3[] Nearest { get { return _nearest; } } 382 public Vector3[] Nearest { get { return _nearest; } }
383 383
384 public Vector3 GetCoordinates(Face face) 384 public Vector3 GetCoordinates(Face face)
385 { 385 {
386 Vector3 o = face._n * -face._d; 386 Vector3 o = face._n * -face._d;
387 float[] a ={ Vector3.Cross(face._vertices[0].Vertice - o, face._vertices[1].Vertice - o).Length(), 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(), 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()}; 389 Vector3.Cross(face._vertices[2].Vertice - o, face._vertices[0].Vertice - o).Length()};
390 float sm = a[0] + a[1] + a[2]; 390 float sm = a[0] + a[1] + a[2];
391 return (new Vector3(a[1], a[2], a[0]) / (sm > 0 ? sm : 1)); 391 return (new Vector3(a[1], a[2], a[0]) / (sm > 0 ? sm : 1));
392 } 392 }
393 393
394 public Face FindBest() 394 public Face FindBest()
395 { 395 {
396 Face bf = null; 396 Face bf = null;
397 if (_root != null) 397 if (_root != null)
398 { 398 {
399 Face cf = _root; 399 Face cf = _root;
400 float bd = _infinity; 400 float bd = _infinity;
401 do 401 do
402 { 402 {
403 if (cf._d < bd) { bd = cf._d; bf = cf; } 403 if (cf._d < bd) { bd = cf._d; bf = cf; }
404 } while (null != (cf = cf._next)); 404 } while (null != (cf = cf._next));
405 } 405 }
406 return bf; 406 return bf;
407 } 407 }
408 408
409 public bool Set(ref Face f, Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c) 409 public bool Set(ref Face f, Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c)
410 { 410 {
411 Vector3 nrm = Vector3.Cross(b.Vertice - a.Vertice, c.Vertice - a.Vertice); 411 Vector3 nrm = Vector3.Cross(b.Vertice - a.Vertice, c.Vertice - a.Vertice);
412 float len = nrm.Length(); 412 float len = nrm.Length();
413 bool valid = (Vector3.Dot(Vector3.Cross(a.Vertice, b.Vertice), nrm) >= -_epaInFaceEpsilon && 413 bool valid = (Vector3.Dot(Vector3.Cross(a.Vertice, b.Vertice), nrm) >= -_epaInFaceEpsilon &&
414 Vector3.Dot(Vector3.Cross(b.Vertice, c.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); 415 Vector3.Dot(Vector3.Cross(c.Vertice, a.Vertice), nrm) >= -_epaInFaceEpsilon);
416 f._vertices[0] = a; 416 f._vertices[0] = a;
417 f._vertices[1] = b; 417 f._vertices[1] = b;
418 f._vertices[2] = c; 418 f._vertices[2] = c;
419 f._mark = 0; 419 f._mark = 0;
420 f._n = nrm / (len > 0 ? len : _infinity); 420 f._n = nrm / (len > 0 ? len : _infinity);
421 f._d = Max(0, -Vector3.Dot(f._n, a.Vertice)); 421 f._d = Max(0, -Vector3.Dot(f._n, a.Vertice));
422 return valid; 422 return valid;
423 } 423 }
424 424
425 public Face NewFace(Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c) 425 public Face NewFace(Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c)
426 { 426 {
427 Face pf = new Face(); 427 Face pf = new Face();
428 if (Set(ref pf, a, b, c)) 428 if (Set(ref pf, a, b, c))
429 { 429 {
430 if (_root != null) _root._prev = pf; 430 if (_root != null) _root._prev = pf;
431 pf._prev = null; 431 pf._prev = null;
432 pf._next = _root; 432 pf._next = _root;
433 _root = pf; 433 _root = pf;
434 ++_nfaces; 434 ++_nfaces;
435 } 435 }
436 else 436 else
437 { 437 {
438 pf._prev = pf._next = null; 438 pf._prev = pf._next = null;
439 } 439 }
440 return (pf); 440 return (pf);
441 } 441 }
442 442
443 public void Detach(ref Face face) 443 public void Detach(ref Face face)
444 { 444 {
445 if (face._prev != null || face._next != null) 445 if (face._prev != null || face._next != null)
446 { 446 {
447 --_nfaces; 447 --_nfaces;
448 if (face == _root) 448 if (face == _root)
449 { 449 {
450 _root = face._next; 450 _root = face._next;
451 _root._prev = null; 451 _root._prev = null;
452 } 452 }
453 else 453 else
454 { 454 {
455 if (face._next == null) 455 if (face._next == null)
456 { 456 {
457 face._prev._next = null; 457 face._prev._next = null;
458 } 458 }
459 else 459 else
460 { 460 {
461 face._prev._next = face._next; 461 face._prev._next = face._next;
462 face._next._prev = face._prev; 462 face._next._prev = face._prev;
463 } 463 }
464 } 464 }
465 face._prev = face._next = null; 465 face._prev = face._next = null;
466 } 466 }
467 } 467 }
468 468
469 public void Link(ref Face f0, int e0, ref Face f1, int e1) 469 public void Link(ref Face f0, int e0, ref Face f1, int e1)
470 { 470 {
471 f0._faces[e0] = f1; f1._e[e1] = e0; 471 f0._faces[e0] = f1; f1._e[e1] = e0;
472 f1._faces[e1] = f0; f0._e[e0] = e1; 472 f1._faces[e1] = f0; f0._e[e0] = e1;
473 } 473 }
474 474
475 public Gjk.MinkowskiVertice Support(Vector3 w) 475 public Gjk.MinkowskiVertice Support(Vector3 w)
476 { 476 {
477 Gjk.MinkowskiVertice v = new Gjk.MinkowskiVertice(); 477 Gjk.MinkowskiVertice v = new Gjk.MinkowskiVertice();
478 _gjk.Support(w, ref v); 478 _gjk.Support(w, ref v);
479 return v; 479 return v;
480 } 480 }
481 481
482 private static int[] mod3 ={ 0, 1, 2, 0, 1 }; 482 private static int[] mod3 ={ 0, 1, 2, 0, 1 };
483 483
484 public int BuildHorizon(int markid, Gjk.MinkowskiVertice w, ref Face f, int e, ref Face cf, ref Face ff) 484 public int BuildHorizon(int markid, Gjk.MinkowskiVertice w, ref Face f, int e, ref Face cf, ref Face ff)
485 { 485 {
486 int ne = (0); 486 int ne = (0);
487 if (f._mark != markid) 487 if (f._mark != markid)
488 { 488 {
489 int e1 = (mod3[e + 1]); 489 int e1 = (mod3[e + 1]);
490 if ((Vector3.Dot(f._n, w.Vertice) + f._d) > 0) 490 if ((Vector3.Dot(f._n, w.Vertice) + f._d) > 0)
491 { 491 {
492 Face nf = NewFace(f._vertices[e1], f._vertices[e], w); 492 Face nf = NewFace(f._vertices[e1], f._vertices[e], w);
493 Link(ref nf, 0, ref f, e); 493 Link(ref nf, 0, ref f, e);
494 if (cf != null) Link(ref cf, 1, ref nf, 2); else ff = nf; 494 if (cf != null) Link(ref cf, 1, ref nf, 2); else ff = nf;
495 cf = nf; ne = 1; 495 cf = nf; ne = 1;
496 } 496 }
497 else 497 else
498 { 498 {
499 int e2 = (mod3[e + 2]); 499 int e2 = (mod3[e + 2]);
500 Detach(ref f); 500 Detach(ref f);
501 f._mark = markid; 501 f._mark = markid;
502 ne += BuildHorizon(markid, w, ref f._faces[e1], f._e[e1], ref cf, ref ff); 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); 503 ne += BuildHorizon(markid, w, ref f._faces[e2], f._e[e2], ref cf, ref ff);
504 } 504 }
505 } 505 }
506 return (ne); 506 return (ne);
507 } 507 }
508 508
509 public float EvaluatePD() 509 public float EvaluatePD()
510 { 510 {
511 return EvaluatePD(_epaAccuracy); 511 return EvaluatePD(_epaAccuracy);
512 } 512 }
513 513
514 private int[,] fidx; 514 private int[,] fidx;
515 private int[,] eidx; 515 private int[,] eidx;
516 516
517 public float EvaluatePD(float accuracy) 517 public float EvaluatePD(float accuracy)
518 { 518 {
519 //Block* sablock = sa->BeginBlock(); 519 //Block* sablock = sa->BeginBlock();
520 Face bestface = null; 520 Face bestface = null;
521 int markid = 1; 521 int markid = 1;
522 _depth = -_infinity; 522 _depth = -_infinity;
523 _normal = new Vector3(); 523 _normal = new Vector3();
524 _root = null; 524 _root = null;
525 _nfaces = 0; 525 _nfaces = 0;
526 _iterations = 0; 526 _iterations = 0;
527 _failed = false; 527 _failed = false;
528 /* Prepare hull */ 528 /* Prepare hull */
529 if (_gjk.EncloseOrigin()) 529 if (_gjk.EncloseOrigin())
530 { 530 {
531 int nfidx = 0; 531 int nfidx = 0;
532 int neidx = 0; 532 int neidx = 0;
533 Gjk.MinkowskiVertice[] basemkv = new Gjk.MinkowskiVertice[5]; 533 Gjk.MinkowskiVertice[] basemkv = new Gjk.MinkowskiVertice[5];
534 Face[] basefaces = new Face[6]; 534 Face[] basefaces = new Face[6];
535 switch (_gjk.Order) 535 switch (_gjk.Order)
536 { 536 {
537 /* Tetrahedron */ 537 /* Tetrahedron */
538 case 3: 538 case 3:
539 { 539 {
540 fidx = new int[,] { { 2, 1, 0 }, { 3, 0, 1 }, { 3, 1, 2 }, { 3, 2, 0 } }; 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 } }; 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; 542 nfidx = 4; neidx = 6;
543 } break; 543 } break;
544 /* Hexahedron */ 544 /* Hexahedron */
545 case 4: 545 case 4:
546 { 546 {
547 fidx = new int[,] { { 2, 0, 4 }, { 4, 1, 2 }, { 1, 4, 0 }, { 0, 3, 1 }, { 0, 2, 3 }, { 1, 3, 2 } }; 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 } }; 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; 549 nfidx = 6; neidx = 9;
550 } break; 550 } break;
551 } 551 }
552 int i; 552 int i;
553 553
554 for (i = 0; i <= _gjk.Order; ++i) 554 for (i = 0; i <= _gjk.Order; ++i)
555 { 555 {
556 //basemkv[i] = (GJK::Mkv*)sa->Allocate(sizeof(GJK::Mkv)); 556 //basemkv[i] = (GJK::Mkv*)sa->Allocate(sizeof(GJK::Mkv));
557 basemkv[i] = new Gjk.MinkowskiVertice(); 557 basemkv[i] = new Gjk.MinkowskiVertice();
558 basemkv[i].Vertice = _gjk.Simplex[i].Vertice; 558 basemkv[i].Vertice = _gjk.Simplex[i].Vertice;
559 basemkv[i].Ray = _gjk.Simplex[i].Ray; 559 basemkv[i].Ray = _gjk.Simplex[i].Ray;
560 } 560 }
561 for (i = 0; i < nfidx; ++i) 561 for (i = 0; i < nfidx; ++i)
562 { 562 {
563 basefaces[i] = NewFace(basemkv[fidx[i, 0]], basemkv[fidx[i, 1]], basemkv[fidx[i, 2]]); 563 basefaces[i] = NewFace(basemkv[fidx[i, 0]], basemkv[fidx[i, 1]], basemkv[fidx[i, 2]]);
564 } 564 }
565 for (i = 0; i < neidx; ++i) 565 for (i = 0; i < neidx; ++i)
566 { 566 {
567 Link(ref basefaces[eidx[i, 0]], eidx[i, 1], ref basefaces[eidx[i, 2]], eidx[i, 3]); 567 Link(ref basefaces[eidx[i, 0]], eidx[i, 1], ref basefaces[eidx[i, 2]], eidx[i, 3]);
568 } 568 }
569 } 569 }
570 if (0 == _nfaces) 570 if (0 == _nfaces)
571 { 571 {
572 return _depth; 572 return _depth;
573 } 573 }
574 /* Expand hull */ 574 /* Expand hull */
575 for (; _iterations < _epaMaxIterations; ++_iterations) 575 for (; _iterations < _epaMaxIterations; ++_iterations)
576 { 576 {
577 Face bf = FindBest(); 577 Face bf = FindBest();
578 if (bf != null) 578 if (bf != null)
579 { 579 {
580 Gjk.MinkowskiVertice w = Support(-bf._n); 580 Gjk.MinkowskiVertice w = Support(-bf._n);
581 float d = Vector3.Dot(bf._n, w.Vertice) + bf._d; 581 float d = Vector3.Dot(bf._n, w.Vertice) + bf._d;
582 bestface = bf; 582 bestface = bf;
583 if (d < -accuracy) 583 if (d < -accuracy)
584 { 584 {
585 Face cf = null; 585 Face cf = null;
586 Face ff = null; 586 Face ff = null;
587 int nf = 0; 587 int nf = 0;
588 Detach(ref bf); 588 Detach(ref bf);
589 bf._mark = ++markid; 589 bf._mark = ++markid;
590 for (int i = 0; i < 3; ++i) 590 for (int i = 0; i < 3; ++i)
591 { 591 {
592 nf += BuildHorizon(markid, w, ref bf._faces[i], bf._e[i], ref cf, ref ff); 592 nf += BuildHorizon(markid, w, ref bf._faces[i], bf._e[i], ref cf, ref ff);
593 } 593 }
594 if (nf <= 2) { break; } 594 if (nf <= 2) { break; }
595 Link(ref cf, 1, ref ff, 2); 595 Link(ref cf, 1, ref ff, 2);
596 } 596 }
597 else break; 597 else break;
598 } 598 }
599 else break; 599 else break;
600 } 600 }
601 /* Extract contact */ 601 /* Extract contact */
602 if (bestface != null) 602 if (bestface != null)
603 { 603 {
604 Vector3 b = GetCoordinates(bestface); 604 Vector3 b = GetCoordinates(bestface);
605 _normal = bestface._n; 605 _normal = bestface._n;
606 _depth = Max(0, bestface._d); 606 _depth = Max(0, bestface._d);
607 for (int i = 0; i < 2; ++i) 607 for (int i = 0; i < 2; ++i)
608 { 608 {
609 float s = i != 0 ? -1 : 1; 609 float s = i != 0 ? -1 : 1;
610 for (int j = 0; j < 3; ++j) 610 for (int j = 0; j < 3; ++j)
611 { 611 {
612 _features[i, j] = _gjk.LocalSupport(s * bestface._vertices[j].Ray, i); 612 _features[i, j] = _gjk.LocalSupport(s * bestface._vertices[j].Ray, i);
613 } 613 }
614 } 614 }
615 _nearest[0] = _features[0, 0] * b.X + _features[0, 1] * b.Y + _features[0, 2] * b.Z; 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; 616 _nearest[1] = _features[1, 0] * b.X + _features[1, 1] * b.Y + _features[1, 2] * b.Z;
617 } 617 }
618 else _failed = true; 618 else _failed = true;
619 return _depth; 619 return _depth;
620 } 620 }
621 621
622 private float Max(float a, float b) 622 private float Max(float a, float b)
623 { 623 {
624 return (a > b ? a : b); 624 return (a > b ? a : b);
625 } 625 }
626 626
627 private float Min(float a, float b) 627 private float Min(float a, float b)
628 { 628 {
629 return (a < b ? a : b); 629 return (a < b ? a : b);
630 } 630 }
631 } 631 }
632 } 632 }
633} 633}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs
index 739b4fc..551449f 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs
@@ -1,56 +1,56 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to 30 /// EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to
31 /// calculate the penetration depth between two convex shapes. 31 /// calculate the penetration depth between two convex shapes.
32 /// </summary> 32 /// </summary>
33 public class GjkEpaPenetrationDepthSolver : IConvexPenetrationDepthSolver 33 public class GjkEpaPenetrationDepthSolver : IConvexPenetrationDepthSolver
34 { 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) 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 { 36 {
37 float radialmargin = 0; 37 float radialmargin = 0;
38 38
39 GjkEpaSolver.Results results; 39 GjkEpaSolver.Results results;
40 if (GjkEpaSolver.Collide(convexA, transformA, 40 if (GjkEpaSolver.Collide(convexA, transformA,
41 convexB, transformB, 41 convexB, transformB,
42 radialmargin, out results)) 42 radialmargin, out results))
43 { 43 {
44 // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); 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); 45 //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
46 ptrA = results.Witnesses[0]; 46 ptrA = results.Witnesses[0];
47 ptrB = results.Witnesses[1]; 47 ptrB = results.Witnesses[1];
48 return true; 48 return true;
49 } 49 }
50 ptrA = new Vector3(); 50 ptrA = new Vector3();
51 ptrB = new Vector3(); 51 ptrB = new Vector3();
52 52
53 return false; 53 return false;
54 } 54 }
55 } 55 }
56} 56}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs
index aa9d61e..20af192 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs
@@ -1,101 +1,101 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// GjkEpaSolver contributed under zlib by Nathanael Presson 30 /// GjkEpaSolver contributed under zlib by Nathanael Presson
31 /// </summary> 31 /// </summary>
32 public class GjkEpaSolver 32 public class GjkEpaSolver
33 { 33 {
34 public struct Results 34 public struct Results
35 { 35 {
36 public enum Status 36 public enum Status
37 { 37 {
38 Separated, /* Shapes doesnt penetrate */ 38 Separated, /* Shapes doesnt penetrate */
39 Penetrating, /* Shapes are penetrating */ 39 Penetrating, /* Shapes are penetrating */
40 GjkFailed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ 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 */ 41 EpaFailed, /* EPA phase fail, bigger problem, need to save parameters, and debug */
42 } 42 }
43 43
44 private Vector3[] _witnesses; 44 private Vector3[] _witnesses;
45 private Vector3 _normal; 45 private Vector3 _normal;
46 private float _depth; 46 private float _depth;
47 private int _epaIterations; 47 private int _epaIterations;
48 private int _gjkIterations; 48 private int _gjkIterations;
49 private Status _status; 49 private Status _status;
50 50
51 public Vector3[] Witnesses { get { return _witnesses; } set { _witnesses = value; } } 51 public Vector3[] Witnesses { get { return _witnesses; } set { _witnesses = value; } }
52 public Vector3 Normal { get { return _normal; } set { _normal = value; } } 52 public Vector3 Normal { get { return _normal; } set { _normal = value; } }
53 public float Depth { get { return _depth; } set { _depth = value; } } 53 public float Depth { get { return _depth; } set { _depth = value; } }
54 public int EpaIterations { get { return _epaIterations; } set { _epaIterations = value; } } 54 public int EpaIterations { get { return _epaIterations; } set { _epaIterations = value; } }
55 public int GjkIterations { get { return _gjkIterations; } set { _gjkIterations = value; } } 55 public int GjkIterations { get { return _gjkIterations; } set { _gjkIterations = value; } }
56 public Status ResultStatus { get { return _status; } set { _status = value; } } 56 public Status ResultStatus { get { return _status; } set { _status = value; } }
57 } 57 }
58 58
59 public static bool Collide(ConvexShape shapeA, Matrix wtrsA, 59 public static bool Collide(ConvexShape shapeA, Matrix wtrsA,
60 ConvexShape shapeB, Matrix wtrsB, 60 ConvexShape shapeB, Matrix wtrsB,
61 float radialmargin, 61 float radialmargin,
62 out Results results) 62 out Results results)
63 { 63 {
64 /* Initialize */ 64 /* Initialize */
65 results = new Results(); 65 results = new Results();
66 results.Witnesses = new Vector3[2]; 66 results.Witnesses = new Vector3[2];
67 results.Witnesses[0] = 67 results.Witnesses[0] =
68 results.Witnesses[1] = 68 results.Witnesses[1] =
69 results.Normal = new Vector3(); 69 results.Normal = new Vector3();
70 results.Depth = 0; 70 results.Depth = 0;
71 results.ResultStatus = Results.Status.Separated; 71 results.ResultStatus = Results.Status.Separated;
72 results.EpaIterations = 0; 72 results.EpaIterations = 0;
73 results.GjkIterations = 0; 73 results.GjkIterations = 0;
74 /* Use GJK to locate origin */ 74 /* Use GJK to locate origin */
75 GjkEpa.Gjk gjk = new GjkEpa.Gjk(wtrsA, wtrsA.Translation, shapeA, 75 GjkEpa.Gjk gjk = new GjkEpa.Gjk(wtrsA, wtrsA.Translation, shapeA,
76 wtrsB, wtrsB.Translation, shapeB, 76 wtrsB, wtrsB.Translation, shapeB,
77 radialmargin + GjkEpa.EpaAccuracy); 77 radialmargin + GjkEpa.EpaAccuracy);
78 bool collide = gjk.SearchOrigin(); 78 bool collide = gjk.SearchOrigin();
79 results.GjkIterations = gjk.Iterations + 1; 79 results.GjkIterations = gjk.Iterations + 1;
80 if (collide) 80 if (collide)
81 { 81 {
82 /* Then EPA for penetration depth */ 82 /* Then EPA for penetration depth */
83 GjkEpa.Epa epa = new GjkEpa.Epa(gjk); 83 GjkEpa.Epa epa = new GjkEpa.Epa(gjk);
84 float pd = epa.EvaluatePD(); 84 float pd = epa.EvaluatePD();
85 results.EpaIterations = epa.Iterations + 1; 85 results.EpaIterations = epa.Iterations + 1;
86 if (pd > 0) 86 if (pd > 0)
87 { 87 {
88 results.ResultStatus = Results.Status.Penetrating; 88 results.ResultStatus = Results.Status.Penetrating;
89 results.Normal = epa.Normal; 89 results.Normal = epa.Normal;
90 results.Depth = pd; 90 results.Depth = pd;
91 results.Witnesses[0] = epa.Nearest[0]; 91 results.Witnesses[0] = epa.Nearest[0];
92 results.Witnesses[1] = epa.Nearest[1]; 92 results.Witnesses[1] = epa.Nearest[1];
93 return true; 93 return true;
94 } 94 }
95 else { if (epa.Failed) results.ResultStatus = Results.Status.EpaFailed; } 95 else { if (epa.Failed) results.ResultStatus = Results.Status.EpaFailed; }
96 } 96 }
97 else { if (gjk.Failed) results.ResultStatus = Results.Status.GjkFailed; } 97 else { if (gjk.Failed) results.ResultStatus = Results.Status.GjkFailed; }
98 return false; 98 return false;
99 } 99 }
100 } 100 }
101} 101}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs
index 0831ff5..7557ed9 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs
@@ -1,343 +1,343 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public class GjkPairDetector : DiscreteCollisionDetectorInterface 29 public class GjkPairDetector : DiscreteCollisionDetectorInterface
30 { 30 {
31 private Vector3 _cachedSeparatingAxis; 31 private Vector3 _cachedSeparatingAxis;
32 private IConvexPenetrationDepthSolver _penetrationDepthSolver; 32 private IConvexPenetrationDepthSolver _penetrationDepthSolver;
33 private ISimplexSolver _simplexSolver; 33 private ISimplexSolver _simplexSolver;
34 private ConvexShape _minkowskiA, _minkowskiB; 34 private ConvexShape _minkowskiA, _minkowskiB;
35 private bool _ignoreMargin; 35 private bool _ignoreMargin;
36 36
37 private int _lastUsedMethod; 37 private int _lastUsedMethod;
38 private int _currentIteration; 38 private int _currentIteration;
39 private int _degenerateSimplex; 39 private int _degenerateSimplex;
40 private int _catchDegeneracies; 40 private int _catchDegeneracies;
41 41
42 private static int _numDeepPenetrationChecks = 0; 42 private static int _numDeepPenetrationChecks = 0;
43 private static int _numGjkChecks = 0; 43 private static int _numGjkChecks = 0;
44 44
45 private const float RelativeError2 = 1.0e-6f; 45 private const float RelativeError2 = 1.0e-6f;
46 46
47 #region Properties 47 #region Properties
48 public int LastUsedMethod 48 public int LastUsedMethod
49 { 49 {
50 get { return _lastUsedMethod; } 50 get { return _lastUsedMethod; }
51 set { _lastUsedMethod = value; } 51 set { _lastUsedMethod = value; }
52 } 52 }
53 53
54 public int CurrentIteration 54 public int CurrentIteration
55 { 55 {
56 get { return _currentIteration; } 56 get { return _currentIteration; }
57 set { _currentIteration = value; } 57 set { _currentIteration = value; }
58 } 58 }
59 59
60 public int DegenerateSimplex 60 public int DegenerateSimplex
61 { 61 {
62 get { return _degenerateSimplex; } 62 get { return _degenerateSimplex; }
63 set { _degenerateSimplex = value; } 63 set { _degenerateSimplex = value; }
64 } 64 }
65 65
66 public int CatchDegeneracies 66 public int CatchDegeneracies
67 { 67 {
68 get { return _catchDegeneracies; } 68 get { return _catchDegeneracies; }
69 set { _catchDegeneracies = value; } 69 set { _catchDegeneracies = value; }
70 } 70 }
71 71
72 public static int DeepPenetrationChecksCount { get { return _numDeepPenetrationChecks; } } 72 public static int DeepPenetrationChecksCount { get { return _numDeepPenetrationChecks; } }
73 public static int GjkChecksCount { get { return _numGjkChecks; } } 73 public static int GjkChecksCount { get { return _numGjkChecks; } }
74 #endregion 74 #endregion
75 75
76 public GjkPairDetector(ConvexShape objectA, ConvexShape objectB, 76 public GjkPairDetector(ConvexShape objectA, ConvexShape objectB,
77 ISimplexSolver simplexSolver, 77 ISimplexSolver simplexSolver,
78 IConvexPenetrationDepthSolver penetrationDepthSolver) 78 IConvexPenetrationDepthSolver penetrationDepthSolver)
79 { 79 {
80 _cachedSeparatingAxis = new Vector3(0, 0, 1); 80 _cachedSeparatingAxis = new Vector3(0, 0, 1);
81 81
82 _penetrationDepthSolver = penetrationDepthSolver; 82 _penetrationDepthSolver = penetrationDepthSolver;
83 _simplexSolver = simplexSolver; 83 _simplexSolver = simplexSolver;
84 _minkowskiA = objectA; 84 _minkowskiA = objectA;
85 _minkowskiB = objectB; 85 _minkowskiB = objectB;
86 _ignoreMargin = false; 86 _ignoreMargin = false;
87 _lastUsedMethod = -1; 87 _lastUsedMethod = -1;
88 _catchDegeneracies = 1; 88 _catchDegeneracies = 1;
89 } 89 }
90 90
91 public void setMinkowskiA(ConvexShape minkA) 91 public void setMinkowskiA(ConvexShape minkA)
92 { 92 {
93 _minkowskiA = minkA; 93 _minkowskiA = minkA;
94 } 94 }
95 95
96 public void setMinkowskiB(ConvexShape minkB) 96 public void setMinkowskiB(ConvexShape minkB)
97 { 97 {
98 _minkowskiB = minkB; 98 _minkowskiB = minkB;
99 } 99 }
100 public void setCachedSeperatingAxis(Vector3 seperatingAxis) 100 public void setCachedSeperatingAxis(Vector3 seperatingAxis)
101 { 101 {
102 _cachedSeparatingAxis = seperatingAxis; 102 _cachedSeparatingAxis = seperatingAxis;
103 } 103 }
104 104
105 public void setPenetrationDepthSolver(IConvexPenetrationDepthSolver penetrationDepthSolver) 105 public void setPenetrationDepthSolver(IConvexPenetrationDepthSolver penetrationDepthSolver)
106 { 106 {
107 this._penetrationDepthSolver = penetrationDepthSolver; 107 this._penetrationDepthSolver = penetrationDepthSolver;
108 } 108 }
109 109
110 public void setIgnoreMargin(bool ignoreMargin) 110 public void setIgnoreMargin(bool ignoreMargin)
111 { 111 {
112 this._ignoreMargin = ignoreMargin; 112 this._ignoreMargin = ignoreMargin;
113 } 113 }
114 114
115 public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw) 115 public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw)
116 { 116 {
117 float distance = 0; 117 float distance = 0;
118 118
119 Vector3 normalInB = new Vector3(); 119 Vector3 normalInB = new Vector3();
120 Vector3 pointOnA = new Vector3(), pointOnB = new Vector3(); 120 Vector3 pointOnA = new Vector3(), pointOnB = new Vector3();
121 121
122 Matrix localTransA = input.TransformA; 122 Matrix localTransA = input.TransformA;
123 Matrix localTransB = input.TransformB; 123 Matrix localTransB = input.TransformB;
124 124
125 Vector3 positionOffset = (localTransA.Translation + localTransB.Translation) * 0.5f; 125 Vector3 positionOffset = (localTransA.Translation + localTransB.Translation) * 0.5f;
126 localTransA.Translation -= positionOffset; 126 localTransA.Translation -= positionOffset;
127 localTransB.Translation -= positionOffset; 127 localTransB.Translation -= positionOffset;
128 128
129 float marginA = _minkowskiA.Margin; 129 float marginA = _minkowskiA.Margin;
130 float marginB = _minkowskiB.Margin; 130 float marginB = _minkowskiB.Margin;
131 131
132 _numGjkChecks++; 132 _numGjkChecks++;
133 133
134 if (_ignoreMargin) 134 if (_ignoreMargin)
135 { 135 {
136 marginA = 0; 136 marginA = 0;
137 marginB = 0; 137 marginB = 0;
138 } 138 }
139 139
140 _currentIteration = 0; 140 _currentIteration = 0;
141 141
142 int gjkMaxIter = 1000; 142 int gjkMaxIter = 1000;
143 _cachedSeparatingAxis = new Vector3(0, 1, 0); 143 _cachedSeparatingAxis = new Vector3(0, 1, 0);
144 144
145 bool isValid = false; 145 bool isValid = false;
146 bool checkSimplex = false; 146 bool checkSimplex = false;
147 bool checkPenetration = true; 147 bool checkPenetration = true;
148 _degenerateSimplex = 0; 148 _degenerateSimplex = 0;
149 149
150 _lastUsedMethod = -1; 150 _lastUsedMethod = -1;
151 151
152 { 152 {
153 float squaredDistance = MathHelper.Infinity; 153 float squaredDistance = MathHelper.Infinity;
154 float delta = 0; 154 float delta = 0;
155 155
156 float margin = marginA + marginB; 156 float margin = marginA + marginB;
157 157
158 _simplexSolver.Reset(); 158 _simplexSolver.Reset();
159 159
160 while (true) 160 while (true)
161 { 161 {
162 Matrix transABasis = input.TransformA; 162 Matrix transABasis = input.TransformA;
163 transABasis.Translation = Vector3.Zero; 163 transABasis.Translation = Vector3.Zero;
164 164
165 Matrix transBBasis = input.TransformB; 165 Matrix transBBasis = input.TransformB;
166 transBBasis.Translation = Vector3.Zero; 166 transBBasis.Translation = Vector3.Zero;
167 167
168 Vector3 seperatingAxisInA = Vector3.TransformNormal(-_cachedSeparatingAxis, transABasis); 168 Vector3 seperatingAxisInA = Vector3.TransformNormal(-_cachedSeparatingAxis, transABasis);
169 Vector3 seperatingAxisInB = Vector3.TransformNormal(_cachedSeparatingAxis, transBBasis); 169 Vector3 seperatingAxisInB = Vector3.TransformNormal(_cachedSeparatingAxis, transBBasis);
170 170
171 Vector3 pInA = _minkowskiA.LocalGetSupportingVertexWithoutMargin(seperatingAxisInA); 171 Vector3 pInA = _minkowskiA.LocalGetSupportingVertexWithoutMargin(seperatingAxisInA);
172 Vector3 qInB = _minkowskiB.LocalGetSupportingVertexWithoutMargin(seperatingAxisInB); 172 Vector3 qInB = _minkowskiB.LocalGetSupportingVertexWithoutMargin(seperatingAxisInB);
173 Vector3 pWorld = MathHelper.MatrixToVector(localTransA, pInA); 173 Vector3 pWorld = MathHelper.MatrixToVector(localTransA, pInA);
174 Vector3 qWorld = MathHelper.MatrixToVector(localTransB, qInB); 174 Vector3 qWorld = MathHelper.MatrixToVector(localTransB, qInB);
175 175
176 Vector3 w = pWorld - qWorld; 176 Vector3 w = pWorld - qWorld;
177 delta = Vector3.Dot(_cachedSeparatingAxis, w); 177 delta = Vector3.Dot(_cachedSeparatingAxis, w);
178 178
179 if ((delta > 0.0) && (delta * delta > squaredDistance * input.MaximumDistanceSquared)) 179 if ((delta > 0.0) && (delta * delta > squaredDistance * input.MaximumDistanceSquared))
180 { 180 {
181 checkPenetration = false; 181 checkPenetration = false;
182 break; 182 break;
183 } 183 }
184 184
185 if (_simplexSolver.InSimplex(w)) 185 if (_simplexSolver.InSimplex(w))
186 { 186 {
187 _degenerateSimplex = 1; 187 _degenerateSimplex = 1;
188 checkSimplex = true; 188 checkSimplex = true;
189 break; 189 break;
190 } 190 }
191 191
192 float f0 = squaredDistance - delta; 192 float f0 = squaredDistance - delta;
193 float f1 = squaredDistance * RelativeError2; 193 float f1 = squaredDistance * RelativeError2;
194 194
195 if (f0 <= f1) 195 if (f0 <= f1)
196 { 196 {
197 if (f0 <= 0.0f) 197 if (f0 <= 0.0f)
198 { 198 {
199 _degenerateSimplex = 2; 199 _degenerateSimplex = 2;
200 } 200 }
201 201
202 checkSimplex = true; 202 checkSimplex = true;
203 break; 203 break;
204 } 204 }
205 205
206 _simplexSolver.AddVertex(w, pWorld, qWorld); 206 _simplexSolver.AddVertex(w, pWorld, qWorld);
207 207
208 if (!_simplexSolver.Closest(out _cachedSeparatingAxis)) 208 if (!_simplexSolver.Closest(out _cachedSeparatingAxis))
209 { 209 {
210 _degenerateSimplex = 3; 210 _degenerateSimplex = 3;
211 checkSimplex = true; 211 checkSimplex = true;
212 break; 212 break;
213 } 213 }
214 214
215 float previouseSquaredDistance = squaredDistance; 215 float previouseSquaredDistance = squaredDistance;
216 squaredDistance = _cachedSeparatingAxis.LengthSquared(); 216 squaredDistance = _cachedSeparatingAxis.LengthSquared();
217 217
218 if (previouseSquaredDistance - squaredDistance <= MathHelper.Epsilon * previouseSquaredDistance) 218 if (previouseSquaredDistance - squaredDistance <= MathHelper.Epsilon * previouseSquaredDistance)
219 { 219 {
220 _simplexSolver.BackupClosest(out _cachedSeparatingAxis); 220 _simplexSolver.BackupClosest(out _cachedSeparatingAxis);
221 checkSimplex = true; 221 checkSimplex = true;
222 break; 222 break;
223 } 223 }
224 224
225 if (_currentIteration++ > gjkMaxIter) 225 if (_currentIteration++ > gjkMaxIter)
226 { 226 {
227#if DEBUG 227#if DEBUG
228 Console.WriteLine("GjkPairDetector maxIter exceeded: {0}", _currentIteration); 228 Console.WriteLine("GjkPairDetector maxIter exceeded: {0}", _currentIteration);
229 Console.WriteLine("sepAxis=({0},{1},{2}), squaredDistance = {3}, shapeTypeA={4}, shapeTypeB={5}", 229 Console.WriteLine("sepAxis=({0},{1},{2}), squaredDistance = {3}, shapeTypeA={4}, shapeTypeB={5}",
230 _cachedSeparatingAxis.X, 230 _cachedSeparatingAxis.X,
231 _cachedSeparatingAxis.Y, 231 _cachedSeparatingAxis.Y,
232 _cachedSeparatingAxis.Z, 232 _cachedSeparatingAxis.Z,
233 squaredDistance, 233 squaredDistance,
234 _minkowskiA.ShapeType, 234 _minkowskiA.ShapeType,
235 _minkowskiB.ShapeType 235 _minkowskiB.ShapeType
236 ); 236 );
237#endif 237#endif
238 break; 238 break;
239 } 239 }
240 240
241 bool check = (!_simplexSolver.FullSimplex); 241 bool check = (!_simplexSolver.FullSimplex);
242 242
243 if (!check) 243 if (!check)
244 { 244 {
245 _simplexSolver.BackupClosest(out _cachedSeparatingAxis); 245 _simplexSolver.BackupClosest(out _cachedSeparatingAxis);
246 break; 246 break;
247 } 247 }
248 } 248 }
249 249
250 if (checkSimplex) 250 if (checkSimplex)
251 { 251 {
252 _simplexSolver.ComputePoints(out pointOnA, out pointOnB); 252 _simplexSolver.ComputePoints(out pointOnA, out pointOnB);
253 normalInB = pointOnA - pointOnB; 253 normalInB = pointOnA - pointOnB;
254 float lenSqr = _cachedSeparatingAxis.LengthSquared(); 254 float lenSqr = _cachedSeparatingAxis.LengthSquared();
255 255
256 if (lenSqr < 0.0001f) 256 if (lenSqr < 0.0001f)
257 { 257 {
258 _degenerateSimplex = 5; 258 _degenerateSimplex = 5;
259 } 259 }
260 260
261 if (lenSqr > MathHelper.Epsilon * MathHelper.Epsilon) 261 if (lenSqr > MathHelper.Epsilon * MathHelper.Epsilon)
262 { 262 {
263 float rlen = 1.0f / (float)Math.Sqrt((float)lenSqr); 263 float rlen = 1.0f / (float)Math.Sqrt((float)lenSqr);
264 normalInB *= rlen; 264 normalInB *= rlen;
265 float s = (float)Math.Sqrt((float)squaredDistance); 265 float s = (float)Math.Sqrt((float)squaredDistance);
266 266
267 BulletDebug.Assert(s > 0); 267 BulletDebug.Assert(s > 0);
268 pointOnA -= _cachedSeparatingAxis * (marginA / s); 268 pointOnA -= _cachedSeparatingAxis * (marginA / s);
269 pointOnB += _cachedSeparatingAxis * (marginB / s); 269 pointOnB += _cachedSeparatingAxis * (marginB / s);
270 distance = ((1 / rlen) - margin); 270 distance = ((1 / rlen) - margin);
271 271
272 isValid = true; 272 isValid = true;
273 273
274 _lastUsedMethod = 1; 274 _lastUsedMethod = 1;
275 } 275 }
276 else 276 else
277 { 277 {
278 _lastUsedMethod = 2; 278 _lastUsedMethod = 2;
279 } 279 }
280 } 280 }
281 281
282 bool catchDegeneratePenetrationCase = 282 bool catchDegeneratePenetrationCase =
283 (_catchDegeneracies != 0 && _penetrationDepthSolver != null && _degenerateSimplex != 0 && ((distance + margin) < 0.01f)); 283 (_catchDegeneracies != 0 && _penetrationDepthSolver != null && _degenerateSimplex != 0 && ((distance + margin) < 0.01f));
284 284
285 if (checkPenetration && (!isValid || catchDegeneratePenetrationCase)) 285 if (checkPenetration && (!isValid || catchDegeneratePenetrationCase))
286 { 286 {
287#warning Check this 287#warning Check this
288 if (_penetrationDepthSolver != null) 288 if (_penetrationDepthSolver != null)
289 { 289 {
290 Vector3 tmpPointOnA, tmpPointOnB; 290 Vector3 tmpPointOnA, tmpPointOnB;
291 291
292 _numDeepPenetrationChecks++; 292 _numDeepPenetrationChecks++;
293 293
294 bool isValid2 = _penetrationDepthSolver.CalculatePenetrationDepth( 294 bool isValid2 = _penetrationDepthSolver.CalculatePenetrationDepth(
295 _simplexSolver, _minkowskiA, _minkowskiB, localTransA, localTransB, 295 _simplexSolver, _minkowskiA, _minkowskiB, localTransA, localTransB,
296 _cachedSeparatingAxis, out tmpPointOnA, out tmpPointOnB, 296 _cachedSeparatingAxis, out tmpPointOnA, out tmpPointOnB,
297 debugDraw 297 debugDraw
298 ); 298 );
299 299
300 if (isValid2) 300 if (isValid2)
301 { 301 {
302 Vector3 tmpNormalInB = tmpPointOnB - tmpPointOnA; 302 Vector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
303 float lengSqr = tmpNormalInB.LengthSquared(); 303 float lengSqr = tmpNormalInB.LengthSquared();
304 304
305 if (lengSqr > (MathHelper.Epsilon * MathHelper.Epsilon)) 305 if (lengSqr > (MathHelper.Epsilon * MathHelper.Epsilon))
306 { 306 {
307 tmpNormalInB /= (float)Math.Sqrt((float)lengSqr); 307 tmpNormalInB /= (float)Math.Sqrt((float)lengSqr);
308 float distance2 = -(tmpPointOnA - tmpPointOnB).Length(); 308 float distance2 = -(tmpPointOnA - tmpPointOnB).Length();
309 309
310 if (!isValid || (distance2 < distance)) 310 if (!isValid || (distance2 < distance))
311 { 311 {
312 distance = distance2; 312 distance = distance2;
313 pointOnA = tmpPointOnA; 313 pointOnA = tmpPointOnA;
314 pointOnB = tmpPointOnB; 314 pointOnB = tmpPointOnB;
315 normalInB = tmpNormalInB; 315 normalInB = tmpNormalInB;
316 isValid = true; 316 isValid = true;
317 _lastUsedMethod = 3; 317 _lastUsedMethod = 3;
318 } 318 }
319 else 319 else
320 { 320 {
321 321
322 } 322 }
323 } 323 }
324 else 324 else
325 { 325 {
326 _lastUsedMethod = 4; 326 _lastUsedMethod = 4;
327 } 327 }
328 } 328 }
329 else 329 else
330 { 330 {
331 _lastUsedMethod = 5; 331 _lastUsedMethod = 5;
332 } 332 }
333 } 333 }
334 } 334 }
335 335
336 if (isValid) 336 if (isValid)
337 { 337 {
338 output.AddContactPoint(normalInB, pointOnB + positionOffset, distance); 338 output.AddContactPoint(normalInB, pointOnB + positionOffset, distance);
339 } 339 }
340 } 340 }
341 } 341 }
342 } 342 }
343} \ No newline at end of file 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
index 59fd486..08206c8 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/IConvexPenetrationDepthSolver.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/IConvexPenetrationDepthSolver.cs
@@ -1,42 +1,42 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// IConvexPenetrationDepthSolver provides an interface for penetration depth calculation. 30 /// IConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
31 /// </summary> 31 /// </summary>
32 public interface IConvexPenetrationDepthSolver 32 public interface IConvexPenetrationDepthSolver
33 { 33 {
34 bool CalculatePenetrationDepth( 34 bool CalculatePenetrationDepth(
35 ISimplexSolver simplexSolver, 35 ISimplexSolver simplexSolver,
36 ConvexShape convexA, ConvexShape convexB, 36 ConvexShape convexA, ConvexShape convexB,
37 Matrix transformA, Matrix transformB, 37 Matrix transformA, Matrix transformB,
38 Vector3 vector, out Vector3 ptrA, out Vector3 ptrB, 38 Vector3 vector, out Vector3 ptrA, out Vector3 ptrB,
39 IDebugDraw debugDraw//, StackAlloc stackAlloc 39 IDebugDraw debugDraw//, StackAlloc stackAlloc
40 ); 40 );
41 } 41 }
42} 42}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs
index 71b1eaa..f1b1807 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs
@@ -1,48 +1,48 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// ISimplexSolver can incrementally calculate distance between origin and up to 4 vertices 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 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 31 /// voronoi regions or barycentric coordinates
32 public interface ISimplexSolver 32 public interface ISimplexSolver
33 { 33 {
34 void Reset(); 34 void Reset();
35 void AddVertex(Vector3 w, Vector3 p, Vector3 q); 35 void AddVertex(Vector3 w, Vector3 p, Vector3 q);
36 bool Closest(out Vector3 v); 36 bool Closest(out Vector3 v);
37 37
38 int GetSimplex(out Vector3[] pBuf, out Vector3[] qBuf, out Vector3[] yBuf); 38 int GetSimplex(out Vector3[] pBuf, out Vector3[] qBuf, out Vector3[] yBuf);
39 bool InSimplex(Vector3 w); 39 bool InSimplex(Vector3 w);
40 void BackupClosest(out Vector3 v); 40 void BackupClosest(out Vector3 v);
41 void ComputePoints(out Vector3 pA, out Vector3 pB); 41 void ComputePoints(out Vector3 pA, out Vector3 pB);
42 42
43 int NumVertices { get;} 43 int NumVertices { get;}
44 bool EmptySimplex { get;} 44 bool EmptySimplex { get;}
45 float MaxVertex { get;} 45 float MaxVertex { get;}
46 bool FullSimplex { get;} 46 bool FullSimplex { get;}
47 } 47 }
48} \ No newline at end of file 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
index 751a9df..f5bcedc 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ManifoldPoint.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ManifoldPoint.cs
@@ -1,78 +1,78 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public class ManifoldPoint 29 public class ManifoldPoint
30 { 30 {
31 private Vector3 _localPointA; 31 private Vector3 _localPointA;
32 private Vector3 _localPointB; 32 private Vector3 _localPointB;
33 private Vector3 _positionWorldOnB; 33 private Vector3 _positionWorldOnB;
34 private Vector3 _positionWorldOnA; 34 private Vector3 _positionWorldOnA;
35 private Vector3 _normalWorldOnB; 35 private Vector3 _normalWorldOnB;
36 36
37 private float _distance; 37 private float _distance;
38 private float _combinedFriction; 38 private float _combinedFriction;
39 private float _combinedRestitution; 39 private float _combinedRestitution;
40 40
41 private object _userPersistentData; 41 private object _userPersistentData;
42 42
43 private int _lifeTime;//lifetime of the contactpoint in frames 43 private int _lifeTime;//lifetime of the contactpoint in frames
44 44
45 public ManifoldPoint() 45 public ManifoldPoint()
46 : this(new Vector3(), new Vector3(), new Vector3(), 0f) 46 : this(new Vector3(), new Vector3(), new Vector3(), 0f)
47 { 47 {
48 } 48 }
49 49
50 public ManifoldPoint(Vector3 pointA, Vector3 pointB, 50 public ManifoldPoint(Vector3 pointA, Vector3 pointB,
51 Vector3 normal, 51 Vector3 normal,
52 float distance) 52 float distance)
53 { 53 {
54 _localPointA = pointA; 54 _localPointA = pointA;
55 _localPointB = pointB; 55 _localPointB = pointB;
56 _normalWorldOnB = normal; 56 _normalWorldOnB = normal;
57 _distance = distance; 57 _distance = distance;
58 _positionWorldOnA = new Vector3(); 58 _positionWorldOnA = new Vector3();
59 _positionWorldOnB = new Vector3(); 59 _positionWorldOnB = new Vector3();
60 } 60 }
61 61
62 public float Distance { get { return _distance; } set { _distance = value; } } 62 public float Distance { get { return _distance; } set { _distance = value; } }
63 public int LifeTime { get { return _lifeTime; } set { _lifeTime = value; } } 63 public int LifeTime { get { return _lifeTime; } set { _lifeTime = value; } }
64 64
65 public Vector3 PositionWorldOnA { get { return _positionWorldOnA; } set { _positionWorldOnA = value; } } 65 public Vector3 PositionWorldOnA { get { return _positionWorldOnA; } set { _positionWorldOnA = value; } }
66 public Vector3 PositionWorldOnB { get { return _positionWorldOnB; } set { _positionWorldOnB = value; } } 66 public Vector3 PositionWorldOnB { get { return _positionWorldOnB; } set { _positionWorldOnB = value; } }
67 67
68 public Vector3 LocalPointA { get { return _localPointA; } set { _localPointA = value; } } 68 public Vector3 LocalPointA { get { return _localPointA; } set { _localPointA = value; } }
69 public Vector3 LocalPointB { get { return _localPointB; } set { _localPointB = value; } } 69 public Vector3 LocalPointB { get { return _localPointB; } set { _localPointB = value; } }
70 70
71 public Vector3 NormalWorldOnB { get { return _normalWorldOnB; } } 71 public Vector3 NormalWorldOnB { get { return _normalWorldOnB; } }
72 72
73 public float CombinedFriction { get { return _combinedFriction; } set { _combinedFriction = value; } } 73 public float CombinedFriction { get { return _combinedFriction; } set { _combinedFriction = value; } }
74 public float CombinedRestitution { get { return _combinedRestitution; } set { _combinedRestitution = value; } } 74 public float CombinedRestitution { get { return _combinedRestitution; } set { _combinedRestitution = value; } }
75 75
76 public object UserPersistentData { get { return _userPersistentData; } set { _userPersistentData = value; } } 76 public object UserPersistentData { get { return _userPersistentData; } set { _userPersistentData = value; } }
77 } 77 }
78} 78}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs
index 81b05b8..7363e28 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs
@@ -1,246 +1,246 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation. 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. 31 /// Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points.
32 /// </summary> 32 /// </summary>
33 public class MinkowskiPenetrationDepthSolver : IConvexPenetrationDepthSolver 33 public class MinkowskiPenetrationDepthSolver : IConvexPenetrationDepthSolver
34 { 34 {
35 private const int UnitSpherePointsCount = 42; 35 private const int UnitSpherePointsCount = 42;
36 36
37 private static Vector3[] penetrationDirections = 37 private static Vector3[] penetrationDirections =
38 { 38 {
39 new Vector3(0.000000f , -0.000000f,-1.000000f), 39 new Vector3(0.000000f , -0.000000f,-1.000000f),
40 new Vector3(0.723608f , -0.525725f,-0.447219f), 40 new Vector3(0.723608f , -0.525725f,-0.447219f),
41 new Vector3(-0.276388f , -0.850649f,-0.447219f), 41 new Vector3(-0.276388f , -0.850649f,-0.447219f),
42 new Vector3(-0.894426f , -0.000000f,-0.447216f), 42 new Vector3(-0.894426f , -0.000000f,-0.447216f),
43 new Vector3(-0.276388f , 0.850649f,-0.447220f), 43 new Vector3(-0.276388f , 0.850649f,-0.447220f),
44 new Vector3(0.723608f , 0.525725f,-0.447219f), 44 new Vector3(0.723608f , 0.525725f,-0.447219f),
45 new Vector3(0.276388f , -0.850649f,0.447220f), 45 new Vector3(0.276388f , -0.850649f,0.447220f),
46 new Vector3(-0.723608f , -0.525725f,0.447219f), 46 new Vector3(-0.723608f , -0.525725f,0.447219f),
47 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), 48 new Vector3(0.276388f , 0.850649f,0.447219f),
49 new Vector3(0.894426f , 0.000000f,0.447216f), 49 new Vector3(0.894426f , 0.000000f,0.447216f),
50 new Vector3(-0.000000f , 0.000000f,1.000000f), 50 new Vector3(-0.000000f , 0.000000f,1.000000f),
51 new Vector3(0.425323f , -0.309011f,-0.850654f), 51 new Vector3(0.425323f , -0.309011f,-0.850654f),
52 new Vector3(-0.162456f , -0.499995f,-0.850654f), 52 new Vector3(-0.162456f , -0.499995f,-0.850654f),
53 new Vector3(0.262869f , -0.809012f,-0.525738f), 53 new Vector3(0.262869f , -0.809012f,-0.525738f),
54 new Vector3(0.425323f , 0.309011f,-0.850654f), 54 new Vector3(0.425323f , 0.309011f,-0.850654f),
55 new Vector3(0.850648f , -0.000000f,-0.525736f), 55 new Vector3(0.850648f , -0.000000f,-0.525736f),
56 new Vector3(-0.525730f , -0.000000f,-0.850652f), 56 new Vector3(-0.525730f , -0.000000f,-0.850652f),
57 new Vector3(-0.688190f , -0.499997f,-0.525736f), 57 new Vector3(-0.688190f , -0.499997f,-0.525736f),
58 new Vector3(-0.162456f , 0.499995f,-0.850654f), 58 new Vector3(-0.162456f , 0.499995f,-0.850654f),
59 new Vector3(-0.688190f , 0.499997f,-0.525736f), 59 new Vector3(-0.688190f , 0.499997f,-0.525736f),
60 new Vector3(0.262869f , 0.809012f,-0.525738f), 60 new Vector3(0.262869f , 0.809012f,-0.525738f),
61 new Vector3(0.951058f , 0.309013f,0.000000f), 61 new Vector3(0.951058f , 0.309013f,0.000000f),
62 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), 63 new Vector3(0.587786f , -0.809017f,0.000000f),
64 new Vector3(0.000000f , -1.000000f,0.000000f), 64 new Vector3(0.000000f , -1.000000f,0.000000f),
65 new Vector3(-0.587786f , -0.809017f,0.000000f), 65 new Vector3(-0.587786f , -0.809017f,0.000000f),
66 new Vector3(-0.951058f , -0.309013f,-0.000000f), 66 new Vector3(-0.951058f , -0.309013f,-0.000000f),
67 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), 68 new Vector3(-0.587786f , 0.809017f,-0.000000f),
69 new Vector3(-0.000000f , 1.000000f,-0.000000f), 69 new Vector3(-0.000000f , 1.000000f,-0.000000f),
70 new Vector3(0.587786f , 0.809017f,-0.000000f), 70 new Vector3(0.587786f , 0.809017f,-0.000000f),
71 new Vector3(0.688190f , -0.499997f,0.525736f), 71 new Vector3(0.688190f , -0.499997f,0.525736f),
72 new Vector3(-0.262869f , -0.809012f,0.525738f), 72 new Vector3(-0.262869f , -0.809012f,0.525738f),
73 new Vector3(-0.850648f , 0.000000f,0.525736f), 73 new Vector3(-0.850648f , 0.000000f,0.525736f),
74 new Vector3(-0.262869f , 0.809012f,0.525738f), 74 new Vector3(-0.262869f , 0.809012f,0.525738f),
75 new Vector3(0.688190f , 0.499997f,0.525736f), 75 new Vector3(0.688190f , 0.499997f,0.525736f),
76 new Vector3(0.525730f , 0.000000f,0.850652f), 76 new Vector3(0.525730f , 0.000000f,0.850652f),
77 new Vector3(0.162456f , -0.499995f,0.850654f), 77 new Vector3(0.162456f , -0.499995f,0.850654f),
78 new Vector3(-0.425323f , -0.309011f,0.850654f), 78 new Vector3(-0.425323f , -0.309011f,0.850654f),
79 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) 80 new Vector3(0.162456f , 0.499995f,0.850654f)
81 }; 81 };
82 82
83 private class IntermediateResult : DiscreteCollisionDetectorInterface.Result 83 private class IntermediateResult : DiscreteCollisionDetectorInterface.Result
84 { 84 {
85 private Vector3 _normalOnBInWorld; 85 private Vector3 _normalOnBInWorld;
86 private Vector3 _pointInWorld; 86 private Vector3 _pointInWorld;
87 private float _depth; 87 private float _depth;
88 private bool _hasResult; 88 private bool _hasResult;
89 89
90 public IntermediateResult() 90 public IntermediateResult()
91 { 91 {
92 _hasResult = false; 92 _hasResult = false;
93 } 93 }
94 94
95 public bool HasResult { get { return _hasResult; } } 95 public bool HasResult { get { return _hasResult; } }
96 public float Depth { get { return _depth; } } 96 public float Depth { get { return _depth; } }
97 public Vector3 PointInWorld { get { return _pointInWorld; } } 97 public Vector3 PointInWorld { get { return _pointInWorld; } }
98 98
99 public override void SetShapeIdentifiers(int partId0, int index0, int partId1, int index1) 99 public override void SetShapeIdentifiers(int partId0, int index0, int partId1, int index1)
100 { 100 {
101 } 101 }
102 102
103 public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) 103 public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth)
104 { 104 {
105 _normalOnBInWorld = normalOnBInWorld; 105 _normalOnBInWorld = normalOnBInWorld;
106 _pointInWorld = pointInWorld; 106 _pointInWorld = pointInWorld;
107 _depth = depth; 107 _depth = depth;
108 _hasResult = true; 108 _hasResult = true;
109 } 109 }
110 } 110 }
111 111
112 #region IConvexPenetrationDepthSolver Members 112 #region IConvexPenetrationDepthSolver Members
113 public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver, 113 public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver,
114 ConvexShape convexA, ConvexShape convexB, 114 ConvexShape convexA, ConvexShape convexB,
115 Matrix transformA, Matrix transformB, 115 Matrix transformA, Matrix transformB,
116 Vector3 v, out Vector3 pa, out Vector3 pb, IDebugDraw debugDraw) 116 Vector3 v, out Vector3 pa, out Vector3 pb, IDebugDraw debugDraw)
117 { 117 {
118 pa = new Vector3(); 118 pa = new Vector3();
119 pb = new Vector3(); 119 pb = new Vector3();
120 //just take fixed number of orientation, and sample the penetration depth in that direction 120 //just take fixed number of orientation, and sample the penetration depth in that direction
121 float minProj = 1e30f; 121 float minProj = 1e30f;
122 Vector3 minNorm = new Vector3(); 122 Vector3 minNorm = new Vector3();
123 Vector3 minA = new Vector3(), minB = new Vector3(); 123 Vector3 minA = new Vector3(), minB = new Vector3();
124 Vector3 seperatingAxisInA, seperatingAxisInB; 124 Vector3 seperatingAxisInA, seperatingAxisInB;
125 Vector3 pInA, qInB, pWorld, qWorld, w; 125 Vector3 pInA, qInB, pWorld, qWorld, w;
126 126
127 Vector3[] supportVerticesABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; 127 Vector3[] supportVerticesABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
128 Vector3[] supportVerticesBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; 128 Vector3[] supportVerticesBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
129 Vector3[] seperatingAxisInABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; 129 Vector3[] seperatingAxisInABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
130 Vector3[] seperatingAxisInBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; 130 Vector3[] seperatingAxisInBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
131 131
132 int numSampleDirections = UnitSpherePointsCount; 132 int numSampleDirections = UnitSpherePointsCount;
133 133
134 for (int i = 0; i < numSampleDirections; i++) 134 for (int i = 0; i < numSampleDirections; i++)
135 { 135 {
136 Vector3 norm = penetrationDirections[i]; 136 Vector3 norm = penetrationDirections[i];
137 seperatingAxisInABatch[i] = Vector3.TransformNormal((-norm), transformA); 137 seperatingAxisInABatch[i] = Vector3.TransformNormal((-norm), transformA);
138 seperatingAxisInBBatch[i] = Vector3.TransformNormal(norm, transformB); 138 seperatingAxisInBBatch[i] = Vector3.TransformNormal(norm, transformB);
139 } 139 }
140 140
141 { 141 {
142 int numPDA = convexA.PreferredPenetrationDirectionsCount; 142 int numPDA = convexA.PreferredPenetrationDirectionsCount;
143 if (numPDA != 0) 143 if (numPDA != 0)
144 { 144 {
145 for (int i = 0; i < numPDA; i++) 145 for (int i = 0; i < numPDA; i++)
146 { 146 {
147 Vector3 norm; 147 Vector3 norm;
148 convexA.GetPreferredPenetrationDirection(i, out norm); 148 convexA.GetPreferredPenetrationDirection(i, out norm);
149 norm = Vector3.TransformNormal(norm, transformA); 149 norm = Vector3.TransformNormal(norm, transformA);
150 penetrationDirections[numSampleDirections] = norm; 150 penetrationDirections[numSampleDirections] = norm;
151 seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA); 151 seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA);
152 seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB); 152 seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB);
153 numSampleDirections++; 153 numSampleDirections++;
154 } 154 }
155 } 155 }
156 } 156 }
157 157
158 { 158 {
159 int numPDB = convexB.PreferredPenetrationDirectionsCount; 159 int numPDB = convexB.PreferredPenetrationDirectionsCount;
160 if (numPDB != 0) 160 if (numPDB != 0)
161 { 161 {
162 for (int i = 0; i < numPDB; i++) 162 for (int i = 0; i < numPDB; i++)
163 { 163 {
164 Vector3 norm; 164 Vector3 norm;
165 convexB.GetPreferredPenetrationDirection(i, out norm); 165 convexB.GetPreferredPenetrationDirection(i, out norm);
166 norm = Vector3.TransformNormal(norm, transformB); 166 norm = Vector3.TransformNormal(norm, transformB);
167 penetrationDirections[numSampleDirections] = norm; 167 penetrationDirections[numSampleDirections] = norm;
168 seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA); 168 seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA);
169 seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB); 169 seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB);
170 numSampleDirections++; 170 numSampleDirections++;
171 } 171 }
172 } 172 }
173 } 173 }
174 174
175 convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch); //, numSampleDirections); 175 convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch); //, numSampleDirections);
176 convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch); //, numSampleDirections); 176 convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch); //, numSampleDirections);
177 177
178 for (int i = 0; i < numSampleDirections; i++) 178 for (int i = 0; i < numSampleDirections; i++)
179 { 179 {
180 Vector3 norm = penetrationDirections[i]; 180 Vector3 norm = penetrationDirections[i];
181 seperatingAxisInA = seperatingAxisInABatch[i]; 181 seperatingAxisInA = seperatingAxisInABatch[i];
182 seperatingAxisInB = seperatingAxisInBBatch[i]; 182 seperatingAxisInB = seperatingAxisInBBatch[i];
183 183
184 pInA = supportVerticesABatch[i]; 184 pInA = supportVerticesABatch[i];
185 qInB = supportVerticesBBatch[i]; 185 qInB = supportVerticesBBatch[i];
186 186
187 pWorld = MathHelper.MatrixToVector(transformA, pInA); 187 pWorld = MathHelper.MatrixToVector(transformA, pInA);
188 qWorld = MathHelper.MatrixToVector(transformB, qInB); 188 qWorld = MathHelper.MatrixToVector(transformB, qInB);
189 w = qWorld - pWorld; 189 w = qWorld - pWorld;
190 float delta = Vector3.Dot(norm, w); 190 float delta = Vector3.Dot(norm, w);
191 //find smallest delta 191 //find smallest delta
192 if (delta < minProj) 192 if (delta < minProj)
193 { 193 {
194 minProj = delta; 194 minProj = delta;
195 minNorm = norm; 195 minNorm = norm;
196 minA = pWorld; 196 minA = pWorld;
197 minB = qWorld; 197 minB = qWorld;
198 } 198 }
199 } 199 }
200 200
201 //add the margins 201 //add the margins
202 minA += minNorm * convexA.Margin; 202 minA += minNorm * convexA.Margin;
203 minB -= minNorm * convexB.Margin; 203 minB -= minNorm * convexB.Margin;
204 //no penetration 204 //no penetration
205 if (minProj < 0) 205 if (minProj < 0)
206 return false; 206 return false;
207 207
208 minProj += (convexA.Margin + convexB.Margin); 208 minProj += (convexA.Margin + convexB.Margin);
209 209
210 GjkPairDetector gjkdet = new GjkPairDetector(convexA, convexB, simplexSolver, null); 210 GjkPairDetector gjkdet = new GjkPairDetector(convexA, convexB, simplexSolver, null);
211 211
212 float offsetDist = minProj; 212 float offsetDist = minProj;
213 Vector3 offset = minNorm * offsetDist; 213 Vector3 offset = minNorm * offsetDist;
214 214
215 GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); 215 GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
216 216
217 Vector3 newOrg = transformA.Translation + offset; 217 Vector3 newOrg = transformA.Translation + offset;
218 218
219 Matrix displacedTrans = transformA; 219 Matrix displacedTrans = transformA;
220 displacedTrans.Translation = newOrg; 220 displacedTrans.Translation = newOrg;
221 221
222 input.TransformA = displacedTrans; 222 input.TransformA = displacedTrans;
223 input.TransformB = transformB; 223 input.TransformB = transformB;
224 input.MaximumDistanceSquared = 1e30f;//minProj; 224 input.MaximumDistanceSquared = 1e30f;//minProj;
225 225
226 IntermediateResult res = new IntermediateResult(); 226 IntermediateResult res = new IntermediateResult();
227 gjkdet.GetClosestPoints(input, res, debugDraw); 227 gjkdet.GetClosestPoints(input, res, debugDraw);
228 228
229 float correctedMinNorm = minProj - res.Depth; 229 float correctedMinNorm = minProj - res.Depth;
230 230
231 //the penetration depth is over-estimated, relax it 231 //the penetration depth is over-estimated, relax it
232 float penetration_relaxation = 1; 232 float penetration_relaxation = 1;
233 minNorm *= penetration_relaxation; 233 minNorm *= penetration_relaxation;
234 234
235 if (res.HasResult) 235 if (res.HasResult)
236 { 236 {
237 237
238 pa = res.PointInWorld - minNorm * correctedMinNorm; 238 pa = res.PointInWorld - minNorm * correctedMinNorm;
239 pb = res.PointInWorld; 239 pb = res.PointInWorld;
240 } 240 }
241 241
242 return res.HasResult; 242 return res.HasResult;
243 } 243 }
244 #endregion 244 #endregion
245 } 245 }
246} 246}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs
index 47cfe9a..04a533d 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs
@@ -1,272 +1,272 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public delegate bool ContactDestroyedCallback(object userPersistentData); 29 public delegate bool ContactDestroyedCallback(object userPersistentData);
30 30
31 public class PersistentManifold 31 public class PersistentManifold
32 { 32 {
33 private static ContactDestroyedCallback _contactDestroyedCallback = null; 33 private static ContactDestroyedCallback _contactDestroyedCallback = null;
34 private static float _contactBreakingThreshold = 0.02f; 34 private static float _contactBreakingThreshold = 0.02f;
35 35
36 private ManifoldPoint[] _pointCache = new ManifoldPoint[4]; 36 private ManifoldPoint[] _pointCache = new ManifoldPoint[4];
37 37
38 // this two body pointers can point to the physics rigidbody class. 38 // this two body pointers can point to the physics rigidbody class.
39 // object will allow any rigidbody class 39 // object will allow any rigidbody class
40 private object _bodyA; 40 private object _bodyA;
41 private object _bodyB; 41 private object _bodyB;
42 private int _cachedPoints; 42 private int _cachedPoints;
43 43
44 public PersistentManifold(object bodyA, object bodyB) 44 public PersistentManifold(object bodyA, object bodyB)
45 { 45 {
46 _bodyA = bodyA; 46 _bodyA = bodyA;
47 _bodyB = bodyB; 47 _bodyB = bodyB;
48 _cachedPoints = 0; 48 _cachedPoints = 0;
49 } 49 }
50 50
51 public object BodyA { get { return _bodyA; } } 51 public object BodyA { get { return _bodyA; } }
52 public object BodyB { get { return _bodyB; } } 52 public object BodyB { get { return _bodyB; } }
53 53
54 public int ContactsCount { get { return _cachedPoints; } } 54 public int ContactsCount { get { return _cachedPoints; } }
55 55
56 public static ContactDestroyedCallback ContactDestroyedCallback { get { return _contactDestroyedCallback; } set { _contactDestroyedCallback = value; } } 56 public static ContactDestroyedCallback ContactDestroyedCallback { get { return _contactDestroyedCallback; } set { _contactDestroyedCallback = value; } }
57 public static float ContactBreakingThreshold { get { return _contactBreakingThreshold; } } 57 public static float ContactBreakingThreshold { get { return _contactBreakingThreshold; } }
58 58
59 public void SetBodies(object bodyA, object bodyB) 59 public void SetBodies(object bodyA, object bodyB)
60 { 60 {
61 _bodyA = bodyA; 61 _bodyA = bodyA;
62 _bodyB = bodyB; 62 _bodyB = bodyB;
63 } 63 }
64 64
65 public ManifoldPoint GetContactPoint(int index) 65 public ManifoldPoint GetContactPoint(int index)
66 { 66 {
67 if (index >= _cachedPoints) 67 if (index >= _cachedPoints)
68 throw new ArgumentOutOfRangeException("index", "index must be smaller than cachedPoints"); 68 throw new ArgumentOutOfRangeException("index", "index must be smaller than cachedPoints");
69 69
70 return _pointCache[index]; 70 return _pointCache[index];
71 } 71 }
72 72
73 public int GetCacheEntry(ManifoldPoint newPoint) 73 public int GetCacheEntry(ManifoldPoint newPoint)
74 { 74 {
75 float shortestDist = ContactBreakingThreshold * ContactBreakingThreshold; 75 float shortestDist = ContactBreakingThreshold * ContactBreakingThreshold;
76 int size = ContactsCount; 76 int size = ContactsCount;
77 int nearestPoint = -1; 77 int nearestPoint = -1;
78 for (int i = 0; i < size; i++) 78 for (int i = 0; i < size; i++)
79 { 79 {
80 ManifoldPoint mp = _pointCache[i]; 80 ManifoldPoint mp = _pointCache[i];
81 81
82 Vector3 diffA = mp.LocalPointA - newPoint.LocalPointA; 82 Vector3 diffA = mp.LocalPointA - newPoint.LocalPointA;
83 float distToManiPoint = Vector3.Dot(diffA, diffA); 83 float distToManiPoint = Vector3.Dot(diffA, diffA);
84 if (distToManiPoint < shortestDist) 84 if (distToManiPoint < shortestDist)
85 { 85 {
86 shortestDist = distToManiPoint; 86 shortestDist = distToManiPoint;
87 nearestPoint = i; 87 nearestPoint = i;
88 } 88 }
89 } 89 }
90 return nearestPoint; 90 return nearestPoint;
91 } 91 }
92 92
93 public void AddManifoldPoint(ManifoldPoint newPoint) 93 public void AddManifoldPoint(ManifoldPoint newPoint)
94 { 94 {
95 if (!ValidContactDistance(newPoint)) 95 if (!ValidContactDistance(newPoint))
96 throw new BulletException(); 96 throw new BulletException();
97 97
98 int insertIndex = ContactsCount; 98 int insertIndex = ContactsCount;
99 if (insertIndex == 4) 99 if (insertIndex == 4)
100 { 100 {
101 //sort cache so best points come first, based on area 101 //sort cache so best points come first, based on area
102 insertIndex = SortCachedPoints(newPoint); 102 insertIndex = SortCachedPoints(newPoint);
103 } 103 }
104 else 104 else
105 { 105 {
106 _cachedPoints++; 106 _cachedPoints++;
107 } 107 }
108 ReplaceContactPoint(newPoint, insertIndex); 108 ReplaceContactPoint(newPoint, insertIndex);
109 } 109 }
110 110
111 public void RemoveContactPoint(int index) 111 public void RemoveContactPoint(int index)
112 { 112 {
113 ClearUserCache(_pointCache[index]); 113 ClearUserCache(_pointCache[index]);
114 114
115 int lastUsedIndex = ContactsCount - 1; 115 int lastUsedIndex = ContactsCount - 1;
116 _pointCache[index] = _pointCache[lastUsedIndex]; 116 _pointCache[index] = _pointCache[lastUsedIndex];
117 //get rid of duplicated userPersistentData pointer 117 //get rid of duplicated userPersistentData pointer
118 _pointCache[lastUsedIndex].UserPersistentData = null; 118 _pointCache[lastUsedIndex].UserPersistentData = null;
119 _cachedPoints--; 119 _cachedPoints--;
120 } 120 }
121 121
122 public void ReplaceContactPoint(ManifoldPoint newPoint, int insertIndex) 122 public void ReplaceContactPoint(ManifoldPoint newPoint, int insertIndex)
123 { 123 {
124 BulletDebug.Assert(ValidContactDistance(newPoint)); 124 BulletDebug.Assert(ValidContactDistance(newPoint));
125 125
126 if (_pointCache[insertIndex] != null) 126 if (_pointCache[insertIndex] != null)
127 { 127 {
128 int lifeTime = _pointCache[insertIndex].LifeTime; 128 int lifeTime = _pointCache[insertIndex].LifeTime;
129 BulletDebug.Assert(lifeTime >= 0); 129 BulletDebug.Assert(lifeTime >= 0);
130 object cache = _pointCache[insertIndex].UserPersistentData; 130 object cache = _pointCache[insertIndex].UserPersistentData;
131 131
132 _pointCache[insertIndex] = newPoint; 132 _pointCache[insertIndex] = newPoint;
133 133
134 _pointCache[insertIndex].UserPersistentData = cache; 134 _pointCache[insertIndex].UserPersistentData = cache;
135 _pointCache[insertIndex].LifeTime = lifeTime; 135 _pointCache[insertIndex].LifeTime = lifeTime;
136 } 136 }
137 else 137 else
138 { 138 {
139 _pointCache[insertIndex] = newPoint; 139 _pointCache[insertIndex] = newPoint;
140 } 140 }
141 141
142 //ClearUserCache(_pointCache[insertIndex]); 142 //ClearUserCache(_pointCache[insertIndex]);
143 //_pointCache[insertIndex] = newPoint; 143 //_pointCache[insertIndex] = newPoint;
144 } 144 }
145 145
146 public bool ValidContactDistance(ManifoldPoint pt) 146 public bool ValidContactDistance(ManifoldPoint pt)
147 { 147 {
148 return pt.Distance <= ContactBreakingThreshold; 148 return pt.Distance <= ContactBreakingThreshold;
149 } 149 }
150 150
151 // calculated new worldspace coordinates and depth, and reject points that exceed the collision margin 151 // calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
152 public void RefreshContactPoints(Matrix trA, Matrix trB) 152 public void RefreshContactPoints(Matrix trA, Matrix trB)
153 { 153 {
154 // first refresh worldspace positions and distance 154 // first refresh worldspace positions and distance
155 for (int i = ContactsCount - 1; i >= 0; i--) 155 for (int i = ContactsCount - 1; i >= 0; i--)
156 { 156 {
157 ManifoldPoint manifoldPoint = _pointCache[i]; 157 ManifoldPoint manifoldPoint = _pointCache[i];
158 manifoldPoint.PositionWorldOnA = MathHelper.MatrixToVector(trA,manifoldPoint.LocalPointA); 158 manifoldPoint.PositionWorldOnA = MathHelper.MatrixToVector(trA,manifoldPoint.LocalPointA);
159 manifoldPoint.PositionWorldOnB = MathHelper.MatrixToVector(trB, manifoldPoint.LocalPointB); 159 manifoldPoint.PositionWorldOnB = MathHelper.MatrixToVector(trB, manifoldPoint.LocalPointB);
160 manifoldPoint.Distance = Vector3.Dot(manifoldPoint.PositionWorldOnA - manifoldPoint.PositionWorldOnB, manifoldPoint.NormalWorldOnB); 160 manifoldPoint.Distance = Vector3.Dot(manifoldPoint.PositionWorldOnA - manifoldPoint.PositionWorldOnB, manifoldPoint.NormalWorldOnB);
161 manifoldPoint.LifeTime++; 161 manifoldPoint.LifeTime++;
162 } 162 }
163 163
164 // then 164 // then
165 float distance2d; 165 float distance2d;
166 Vector3 projectedDifference, projectedPoint; 166 Vector3 projectedDifference, projectedPoint;
167 for (int i = ContactsCount - 1; i >= 0; i--) 167 for (int i = ContactsCount - 1; i >= 0; i--)
168 { 168 {
169 169
170 ManifoldPoint manifoldPoint = _pointCache[i]; 170 ManifoldPoint manifoldPoint = _pointCache[i];
171 //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction) 171 //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
172 if (!ValidContactDistance(manifoldPoint)) 172 if (!ValidContactDistance(manifoldPoint))
173 { 173 {
174 RemoveContactPoint(i); 174 RemoveContactPoint(i);
175 } 175 }
176 else 176 else
177 { 177 {
178 //contact also becomes invalid when relative movement orthogonal to normal exceeds margin 178 //contact also becomes invalid when relative movement orthogonal to normal exceeds margin
179 projectedPoint = manifoldPoint.PositionWorldOnA - manifoldPoint.NormalWorldOnB * manifoldPoint.Distance; 179 projectedPoint = manifoldPoint.PositionWorldOnA - manifoldPoint.NormalWorldOnB * manifoldPoint.Distance;
180 projectedDifference = manifoldPoint.PositionWorldOnB - projectedPoint; 180 projectedDifference = manifoldPoint.PositionWorldOnB - projectedPoint;
181 distance2d = Vector3.Dot(projectedDifference, projectedDifference); 181 distance2d = Vector3.Dot(projectedDifference, projectedDifference);
182 if (distance2d > ContactBreakingThreshold * ContactBreakingThreshold) 182 if (distance2d > ContactBreakingThreshold * ContactBreakingThreshold)
183 { 183 {
184 RemoveContactPoint(i); 184 RemoveContactPoint(i);
185 } 185 }
186 } 186 }
187 } 187 }
188 } 188 }
189 189
190 public void ClearManifold() 190 public void ClearManifold()
191 { 191 {
192 for (int i = 0; i < _cachedPoints; i++) 192 for (int i = 0; i < _cachedPoints; i++)
193 { 193 {
194 ClearUserCache(_pointCache[i]); 194 ClearUserCache(_pointCache[i]);
195 } 195 }
196 _cachedPoints = 0; 196 _cachedPoints = 0;
197 } 197 }
198 198
199 private void ClearUserCache(ManifoldPoint pt) 199 private void ClearUserCache(ManifoldPoint pt)
200 { 200 {
201 if (pt != null) 201 if (pt != null)
202 { 202 {
203 object oldPtr = pt.UserPersistentData; 203 object oldPtr = pt.UserPersistentData;
204 204
205 if (oldPtr != null) 205 if (oldPtr != null)
206 { 206 {
207 if (pt.UserPersistentData != null && _contactDestroyedCallback != null) 207 if (pt.UserPersistentData != null && _contactDestroyedCallback != null)
208 { 208 {
209 _contactDestroyedCallback(pt.UserPersistentData); 209 _contactDestroyedCallback(pt.UserPersistentData);
210 pt.UserPersistentData = null; 210 pt.UserPersistentData = null;
211 } 211 }
212 } 212 }
213 } 213 }
214 } 214 }
215 215
216 // sort cached points so most isolated points come first 216 // sort cached points so most isolated points come first
217 private int SortCachedPoints(ManifoldPoint pt) 217 private int SortCachedPoints(ManifoldPoint pt)
218 { 218 {
219 //calculate 4 possible cases areas, and take biggest area 219 //calculate 4 possible cases areas, and take biggest area
220 //also need to keep 'deepest' 220 //also need to keep 'deepest'
221 221
222 int maxPenetrationIndex = -1; 222 int maxPenetrationIndex = -1;
223 float maxPenetration = pt.Distance; 223 float maxPenetration = pt.Distance;
224 for (int i = 0; i < 4; i++) 224 for (int i = 0; i < 4; i++)
225 { 225 {
226 if (_pointCache[i].Distance < maxPenetration) 226 if (_pointCache[i].Distance < maxPenetration)
227 { 227 {
228 maxPenetrationIndex = i; 228 maxPenetrationIndex = i;
229 maxPenetration = _pointCache[i].Distance; 229 maxPenetration = _pointCache[i].Distance;
230 } 230 }
231 } 231 }
232 232
233 float res0 = 0, res1 = 0, res2 = 0, res3 = 0; 233 float res0 = 0, res1 = 0, res2 = 0, res3 = 0;
234 if (maxPenetrationIndex != 0) 234 if (maxPenetrationIndex != 0)
235 { 235 {
236 Vector3 a0 = pt.LocalPointA - _pointCache[1].LocalPointA; 236 Vector3 a0 = pt.LocalPointA - _pointCache[1].LocalPointA;
237 Vector3 b0 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA; 237 Vector3 b0 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA;
238 Vector3 cross = Vector3.Cross(a0, b0); 238 Vector3 cross = Vector3.Cross(a0, b0);
239 res0 = cross.LengthSquared(); 239 res0 = cross.LengthSquared();
240 } 240 }
241 if (maxPenetrationIndex != 1) 241 if (maxPenetrationIndex != 1)
242 { 242 {
243 Vector3 a1 = pt.LocalPointA - _pointCache[0].LocalPointA; 243 Vector3 a1 = pt.LocalPointA - _pointCache[0].LocalPointA;
244 Vector3 b1 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA; 244 Vector3 b1 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA;
245 Vector3 cross = Vector3.Cross(a1, b1); 245 Vector3 cross = Vector3.Cross(a1, b1);
246 res1 = cross.LengthSquared(); 246 res1 = cross.LengthSquared();
247 } 247 }
248 248
249 if (maxPenetrationIndex != 2) 249 if (maxPenetrationIndex != 2)
250 { 250 {
251 Vector3 a2 = pt.LocalPointA - _pointCache[0].LocalPointA; 251 Vector3 a2 = pt.LocalPointA - _pointCache[0].LocalPointA;
252 Vector3 b2 = _pointCache[3].LocalPointA - _pointCache[1].LocalPointA; 252 Vector3 b2 = _pointCache[3].LocalPointA - _pointCache[1].LocalPointA;
253 Vector3 cross = Vector3.Cross(a2, b2); 253 Vector3 cross = Vector3.Cross(a2, b2);
254 res2 = cross.LengthSquared(); 254 res2 = cross.LengthSquared();
255 } 255 }
256 256
257 if (maxPenetrationIndex != 3) 257 if (maxPenetrationIndex != 3)
258 { 258 {
259 Vector3 a3 = pt.LocalPointA - _pointCache[0].LocalPointA; 259 Vector3 a3 = pt.LocalPointA - _pointCache[0].LocalPointA;
260 Vector3 b3 = _pointCache[2].LocalPointA - _pointCache[1].LocalPointA; 260 Vector3 b3 = _pointCache[2].LocalPointA - _pointCache[1].LocalPointA;
261 Vector3 cross = Vector3.Cross(a3, b3); 261 Vector3 cross = Vector3.Cross(a3, b3);
262 res3 = cross.LengthSquared(); 262 res3 = cross.LengthSquared();
263 } 263 }
264 264
265 Vector4 maxvec = new Vector4(res0, res1, res2, res3); 265 Vector4 maxvec = new Vector4(res0, res1, res2, res3);
266 int biggestarea = MathHelper.ClosestAxis(maxvec); 266 int biggestarea = MathHelper.ClosestAxis(maxvec);
267 return biggestarea; 267 return biggestarea;
268 } 268 }
269 269
270 private int FindContactPoint(ManifoldPoint unUsed, int numUnused, ManifoldPoint pt) { return 0; } 270 private int FindContactPoint(ManifoldPoint unUsed, int numUnused, ManifoldPoint pt) { return 0; }
271 } 271 }
272} 272}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs
index 4e0b0a0..c70741e 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs
@@ -1,64 +1,64 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public class PointCollector : DiscreteCollisionDetectorInterface.Result 29 public class PointCollector : DiscreteCollisionDetectorInterface.Result
30 { 30 {
31 private Vector3 _normalOnBInWorld; 31 private Vector3 _normalOnBInWorld;
32 private Vector3 _pointInWorld; 32 private Vector3 _pointInWorld;
33 private float _distance; //negative means penetration 33 private float _distance; //negative means penetration
34 private bool _hasResult; 34 private bool _hasResult;
35 35
36 public PointCollector() 36 public PointCollector()
37 { 37 {
38 _distance = 1e30f; 38 _distance = 1e30f;
39 _hasResult = false; 39 _hasResult = false;
40 } 40 }
41 41
42 public Vector3 NormalOnBInWorld { get { return _normalOnBInWorld; } } 42 public Vector3 NormalOnBInWorld { get { return _normalOnBInWorld; } }
43 public Vector3 PointInWorld { get { return _pointInWorld; } } 43 public Vector3 PointInWorld { get { return _pointInWorld; } }
44 public float Distance { get { return _distance; } } 44 public float Distance { get { return _distance; } }
45 public bool HasResult { get { return _hasResult; } } 45 public bool HasResult { get { return _hasResult; } }
46 46
47 public override void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB) 47 public override void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB)
48 { 48 {
49 //?? 49 //??
50 } 50 }
51 51
52 public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) 52 public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth)
53 { 53 {
54 if (depth < _distance) 54 if (depth < _distance)
55 { 55 {
56 _hasResult = true; 56 _hasResult = true;
57 _normalOnBInWorld = normalOnBInWorld; 57 _normalOnBInWorld = normalOnBInWorld;
58 _pointInWorld = pointInWorld; 58 _pointInWorld = pointInWorld;
59 //negative means penetration 59 //negative means penetration
60 _distance = depth; 60 _distance = depth;
61 } 61 }
62 } 62 }
63 } 63 }
64} 64}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs
index 6d320c4..5c92a0c 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs
@@ -1,142 +1,142 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// SubsimplexConvexCast implements Gino van den Bergens' paper 30 /// SubsimplexConvexCast implements Gino van den Bergens' paper
31 /// "Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection" 31 /// "Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection"
32 /// GJK based Ray Cast, optimized version 32 /// GJK based Ray Cast, optimized version
33 /// Objects should not start in overlap, otherwise results are not defined. 33 /// Objects should not start in overlap, otherwise results are not defined.
34 /// </summary> 34 /// </summary>
35 public class SubsimplexConvexCast : IConvexCast 35 public class SubsimplexConvexCast : IConvexCast
36 { 36 {
37 private ISimplexSolver _simplexSolver; 37 private ISimplexSolver _simplexSolver;
38 private ConvexShape _convexA; 38 private ConvexShape _convexA;
39 private ConvexShape _convexB; 39 private ConvexShape _convexB;
40 40
41 /// <summary> 41 /// <summary>
42 /// Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases. 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 43 /// See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565
44 /// </summary> 44 /// </summary>
45 private const int MaxIterations = 32; 45 private const int MaxIterations = 32;
46 46
47 public SubsimplexConvexCast(ConvexShape shapeA, ConvexShape shapeB, ISimplexSolver simplexSolver) 47 public SubsimplexConvexCast(ConvexShape shapeA, ConvexShape shapeB, ISimplexSolver simplexSolver)
48 { 48 {
49 _simplexSolver = simplexSolver; 49 _simplexSolver = simplexSolver;
50 _convexA = shapeA; 50 _convexA = shapeA;
51 _convexB = shapeB; 51 _convexB = shapeB;
52 } 52 }
53 53
54 #region IConvexCast Members 54 #region IConvexCast Members
55 /// <summary> 55 /// <summary>
56 /// SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects. 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. 57 /// Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using GjkPairDetector.
58 /// </summary> 58 /// </summary>
59 /// <param name="fromA"></param> 59 /// <param name="fromA"></param>
60 /// <param name="toA"></param> 60 /// <param name="toA"></param>
61 /// <param name="fromB"></param> 61 /// <param name="fromB"></param>
62 /// <param name="toB"></param> 62 /// <param name="toB"></param>
63 /// <param name="result"></param> 63 /// <param name="result"></param>
64 /// <returns></returns> 64 /// <returns></returns>
65 public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result) 65 public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
66 { 66 {
67 MinkowskiSumShape convex = new MinkowskiSumShape(_convexA, _convexB); 67 MinkowskiSumShape convex = new MinkowskiSumShape(_convexA, _convexB);
68 68
69 Matrix rayFromLocalA; 69 Matrix rayFromLocalA;
70 Matrix rayToLocalA; 70 Matrix rayToLocalA;
71 71
72 rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB; 72 rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB;
73 rayToLocalA = MathHelper.InvertMatrix(toA) * toB; 73 rayToLocalA = MathHelper.InvertMatrix(toA) * toB;
74 74
75 _simplexSolver.Reset(); 75 _simplexSolver.Reset();
76 76
77 convex.TransformB = rayFromLocalA; 77 convex.TransformB = rayFromLocalA;
78 78
79 float lambda = 0; 79 float lambda = 0;
80 //todo: need to verify this: 80 //todo: need to verify this:
81 //because of minkowski difference, we need the inverse direction 81 //because of minkowski difference, we need the inverse direction
82 82
83 Vector3 s = -rayFromLocalA.Translation; 83 Vector3 s = -rayFromLocalA.Translation;
84 Vector3 r = -(rayToLocalA.Translation - rayFromLocalA.Translation); 84 Vector3 r = -(rayToLocalA.Translation - rayFromLocalA.Translation);
85 Vector3 x = s; 85 Vector3 x = s;
86 Vector3 v; 86 Vector3 v;
87 Vector3 arbitraryPoint = convex.LocalGetSupportingVertex(r); 87 Vector3 arbitraryPoint = convex.LocalGetSupportingVertex(r);
88 88
89 v = x - arbitraryPoint; 89 v = x - arbitraryPoint;
90 90
91 int maxIter = MaxIterations; 91 int maxIter = MaxIterations;
92 92
93 Vector3 n = new Vector3(); 93 Vector3 n = new Vector3();
94 float lastLambda = lambda; 94 float lastLambda = lambda;
95 95
96 float dist2 = v.LengthSquared(); 96 float dist2 = v.LengthSquared();
97 float epsilon = 0.0001f; 97 float epsilon = 0.0001f;
98 98
99 Vector3 w, p; 99 Vector3 w, p;
100 float VdotR; 100 float VdotR;
101 101
102 while ((dist2 > epsilon) && (maxIter-- != 0)) 102 while ((dist2 > epsilon) && (maxIter-- != 0))
103 { 103 {
104 p = convex.LocalGetSupportingVertex(v); 104 p = convex.LocalGetSupportingVertex(v);
105 w = x - p; 105 w = x - p;
106 106
107 float VdotW = Vector3.Dot(v, w); 107 float VdotW = Vector3.Dot(v, w);
108 108
109 if (VdotW > 0) 109 if (VdotW > 0)
110 { 110 {
111 VdotR = Vector3.Dot(v, r); 111 VdotR = Vector3.Dot(v, r);
112 112
113 if (VdotR >= -(MathHelper.Epsilon * MathHelper.Epsilon)) 113 if (VdotR >= -(MathHelper.Epsilon * MathHelper.Epsilon))
114 return false; 114 return false;
115 else 115 else
116 { 116 {
117 lambda = lambda - VdotW / VdotR; 117 lambda = lambda - VdotW / VdotR;
118 x = s + lambda * r; 118 x = s + lambda * r;
119 _simplexSolver.Reset(); 119 _simplexSolver.Reset();
120 //check next line 120 //check next line
121 w = x - p; 121 w = x - p;
122 lastLambda = lambda; 122 lastLambda = lambda;
123 n = v; 123 n = v;
124 } 124 }
125 } 125 }
126 _simplexSolver.AddVertex(w, x, p); 126 _simplexSolver.AddVertex(w, x, p);
127 if (_simplexSolver.Closest(out v)) 127 if (_simplexSolver.Closest(out v))
128 { 128 {
129 dist2 = v.LengthSquared(); 129 dist2 = v.LengthSquared();
130 } 130 }
131 else 131 else
132 { 132 {
133 dist2 = 0f; 133 dist2 = 0f;
134 } 134 }
135 } 135 }
136 result.Fraction = lambda; 136 result.Fraction = lambda;
137 result.Normal = n; 137 result.Normal = n;
138 return true; 138 return true;
139 } 139 }
140 #endregion 140 #endregion
141 } 141 }
142} 142}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs
index c127460..1e0fb3b 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs
@@ -1,115 +1,115 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public abstract class TriangleRaycastCallback : ITriangleCallback 29 public abstract class TriangleRaycastCallback : ITriangleCallback
30 { 30 {
31 private Vector3 _from; 31 private Vector3 _from;
32 private Vector3 _to; 32 private Vector3 _to;
33 private float _hitFraction; 33 private float _hitFraction;
34 34
35 public TriangleRaycastCallback(Vector3 from, Vector3 to) 35 public TriangleRaycastCallback(Vector3 from, Vector3 to)
36 { 36 {
37 _from = from; 37 _from = from;
38 _to = to; 38 _to = to;
39 _hitFraction = 1; 39 _hitFraction = 1;
40 } 40 }
41 41
42 public Vector3 From { get { return _from; } set { _from = value; } } 42 public Vector3 From { get { return _from; } set { _from = value; } }
43 public Vector3 To { get { return _to; } set { _to = value; } } 43 public Vector3 To { get { return _to; } set { _to = value; } }
44 public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } 44 public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } }
45 45
46 public abstract float ReportHit(Vector3 hitNormalLocal, float hitFraction, int partId, int triangleIndex); 46 public abstract float ReportHit(Vector3 hitNormalLocal, float hitFraction, int partId, int triangleIndex);
47 47
48 #region ITriangleCallback Members 48 #region ITriangleCallback Members
49 49
50 public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) 50 public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex)
51 { 51 {
52 Vector3 vertA = triangle[0]; 52 Vector3 vertA = triangle[0];
53 Vector3 vertB = triangle[1]; 53 Vector3 vertB = triangle[1];
54 Vector3 vertC = triangle[2]; 54 Vector3 vertC = triangle[2];
55 55
56 Vector3 vBA = vertB - vertA; 56 Vector3 vBA = vertB - vertA;
57 Vector3 vCA = vertC - vertA; 57 Vector3 vCA = vertC - vertA;
58 58
59 Vector3 triangleNormal = Vector3.Cross(vBA, vCA); 59 Vector3 triangleNormal = Vector3.Cross(vBA, vCA);
60 60
61 float dist = Vector3.Dot(vertA, triangleNormal); 61 float dist = Vector3.Dot(vertA, triangleNormal);
62 float distA = Vector3.Dot(triangleNormal, _from); 62 float distA = Vector3.Dot(triangleNormal, _from);
63 distA -= dist; 63 distA -= dist;
64 float distB = Vector3.Dot(triangleNormal, _to); 64 float distB = Vector3.Dot(triangleNormal, _to);
65 distB -= dist; 65 distB -= dist;
66 66
67 if (distA * distB >= 0.0f) 67 if (distA * distB >= 0.0f)
68 { 68 {
69 return; // same sign 69 return; // same sign
70 } 70 }
71 71
72 float projLength = distA - distB; 72 float projLength = distA - distB;
73 float distance = (distA) / (projLength); 73 float distance = (distA) / (projLength);
74 // Now we have the intersection point on the plane, we'll see if it's inside the triangle 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, 75 // Add an epsilon as a tolerance for the raycast,
76 // in case the ray hits exacly on the edge of the triangle. 76 // in case the ray hits exacly on the edge of the triangle.
77 // It must be scaled for the triangle size. 77 // It must be scaled for the triangle size.
78 78
79 if (distance < _hitFraction) 79 if (distance < _hitFraction)
80 { 80 {
81 float edgeTolerance = triangleNormal.LengthSquared(); 81 float edgeTolerance = triangleNormal.LengthSquared();
82 edgeTolerance *= -0.0001f; 82 edgeTolerance *= -0.0001f;
83 Vector3 point = new Vector3(); 83 Vector3 point = new Vector3();
84 MathHelper.SetInterpolate3(_from, _to, distance, ref point); 84 MathHelper.SetInterpolate3(_from, _to, distance, ref point);
85 85
86 Vector3 vertexAPoint = vertA - point; 86 Vector3 vertexAPoint = vertA - point;
87 Vector3 vertexBPoint = vertB - point; 87 Vector3 vertexBPoint = vertB - point;
88 Vector3 contactPointA = Vector3.Cross(vertexAPoint, vertexBPoint); 88 Vector3 contactPointA = Vector3.Cross(vertexAPoint, vertexBPoint);
89 89
90 if (Vector3.Dot(contactPointA, triangleNormal) >= edgeTolerance) 90 if (Vector3.Dot(contactPointA, triangleNormal) >= edgeTolerance)
91 { 91 {
92 Vector3 vertexCPoint = vertC - point; 92 Vector3 vertexCPoint = vertC - point;
93 Vector3 contactPointB = Vector3.Cross(vertexBPoint, vertexCPoint); 93 Vector3 contactPointB = Vector3.Cross(vertexBPoint, vertexCPoint);
94 if (Vector3.Dot(contactPointB, triangleNormal) >= edgeTolerance) 94 if (Vector3.Dot(contactPointB, triangleNormal) >= edgeTolerance)
95 { 95 {
96 Vector3 contactPointC = Vector3.Cross(vertexCPoint, vertexAPoint); 96 Vector3 contactPointC = Vector3.Cross(vertexCPoint, vertexAPoint);
97 97
98 if (Vector3.Dot(contactPointC, triangleNormal) >= edgeTolerance) 98 if (Vector3.Dot(contactPointC, triangleNormal) >= edgeTolerance)
99 { 99 {
100 if (distA > 0) 100 if (distA > 0)
101 { 101 {
102 _hitFraction = ReportHit(triangleNormal, distance, partID, triangleIndex); 102 _hitFraction = ReportHit(triangleNormal, distance, partID, triangleIndex);
103 } 103 }
104 else 104 else
105 { 105 {
106 _hitFraction = ReportHit(-triangleNormal, distance, partID, triangleIndex); 106 _hitFraction = ReportHit(-triangleNormal, distance, partID, triangleIndex);
107 } 107 }
108 } 108 }
109 } 109 }
110 } 110 }
111 } 111 }
112 } 112 }
113 #endregion 113 #endregion
114 } 114 }
115} 115}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs
index 16f3dab..1bc94d3 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs
@@ -1,643 +1,643 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public class UsageBitfield 29 public class UsageBitfield
30 { 30 {
31 private bool _usedVertexA, _usedVertexB, _usedVertexC, _usedVertexD; 31 private bool _usedVertexA, _usedVertexB, _usedVertexC, _usedVertexD;
32 32
33 public bool UsedVertexA { get { return _usedVertexA; } set { _usedVertexA = value; } } 33 public bool UsedVertexA { get { return _usedVertexA; } set { _usedVertexA = value; } }
34 public bool UsedVertexB { get { return _usedVertexB; } set { _usedVertexB = value; } } 34 public bool UsedVertexB { get { return _usedVertexB; } set { _usedVertexB = value; } }
35 public bool UsedVertexC { get { return _usedVertexC; } set { _usedVertexC = value; } } 35 public bool UsedVertexC { get { return _usedVertexC; } set { _usedVertexC = value; } }
36 public bool UsedVertexD { get { return _usedVertexD; } set { _usedVertexD = value; } } 36 public bool UsedVertexD { get { return _usedVertexD; } set { _usedVertexD = value; } }
37 37
38 public void Reset() 38 public void Reset()
39 { 39 {
40 _usedVertexA = _usedVertexB = _usedVertexC = _usedVertexD = false; 40 _usedVertexA = _usedVertexB = _usedVertexC = _usedVertexD = false;
41 } 41 }
42 } 42 }
43 43
44 public class SubSimplexClosestResult 44 public class SubSimplexClosestResult
45 { 45 {
46 private Vector3 _closestPointOnSimplex; 46 private Vector3 _closestPointOnSimplex;
47 47
48 //MASK for m_usedVertices 48 //MASK for m_usedVertices
49 //stores the simplex vertex-usage, using the MASK, 49 //stores the simplex vertex-usage, using the MASK,
50 // if m_usedVertices & MASK then the related vertex is used 50 // if m_usedVertices & MASK then the related vertex is used
51 private UsageBitfield _usedVertices = new UsageBitfield(); 51 private UsageBitfield _usedVertices = new UsageBitfield();
52 private float[] _barycentricCoords = new float[4]; 52 private float[] _barycentricCoords = new float[4];
53 private bool _degenerate; 53 private bool _degenerate;
54 54
55 public Vector3 ClosestPointOnSimplex { get { return _closestPointOnSimplex; } set { _closestPointOnSimplex = value; } } 55 public Vector3 ClosestPointOnSimplex { get { return _closestPointOnSimplex; } set { _closestPointOnSimplex = value; } }
56 public UsageBitfield UsedVertices { get { return _usedVertices; } set { _usedVertices = value; } } 56 public UsageBitfield UsedVertices { get { return _usedVertices; } set { _usedVertices = value; } }
57 public float[] BarycentricCoords { get { return _barycentricCoords; } set { _barycentricCoords = value; } } 57 public float[] BarycentricCoords { get { return _barycentricCoords; } set { _barycentricCoords = value; } }
58 public bool Degenerate { get { return _degenerate; } set { _degenerate = value; } } 58 public bool Degenerate { get { return _degenerate; } set { _degenerate = value; } }
59 59
60 public void Reset() 60 public void Reset()
61 { 61 {
62 _degenerate = false; 62 _degenerate = false;
63 SetBarycentricCoordinates(); 63 SetBarycentricCoordinates();
64 _usedVertices.Reset(); 64 _usedVertices.Reset();
65 } 65 }
66 66
67 public bool IsValid 67 public bool IsValid
68 { 68 {
69 get 69 get
70 { 70 {
71 return (_barycentricCoords[0] >= 0f) && 71 return (_barycentricCoords[0] >= 0f) &&
72 (_barycentricCoords[1] >= 0f) && 72 (_barycentricCoords[1] >= 0f) &&
73 (_barycentricCoords[2] >= 0f) && 73 (_barycentricCoords[2] >= 0f) &&
74 (_barycentricCoords[3] >= 0f); 74 (_barycentricCoords[3] >= 0f);
75 } 75 }
76 } 76 }
77 77
78 public void SetBarycentricCoordinates() 78 public void SetBarycentricCoordinates()
79 { 79 {
80 SetBarycentricCoordinates(0f, 0f, 0f, 0f); 80 SetBarycentricCoordinates(0f, 0f, 0f, 0f);
81 } 81 }
82 82
83 public void SetBarycentricCoordinates(float a, float b, float c, float d) 83 public void SetBarycentricCoordinates(float a, float b, float c, float d)
84 { 84 {
85 _barycentricCoords[0] = a; 85 _barycentricCoords[0] = a;
86 _barycentricCoords[1] = b; 86 _barycentricCoords[1] = b;
87 _barycentricCoords[2] = c; 87 _barycentricCoords[2] = c;
88 _barycentricCoords[3] = d; 88 _barycentricCoords[3] = d;
89 } 89 }
90 } 90 }
91 91
92 /// VoronoiSimplexSolver is an implementation of the closest point distance 92 /// VoronoiSimplexSolver is an implementation of the closest point distance
93 /// algorithm from a 1-4 points simplex to the origin. 93 /// algorithm from a 1-4 points simplex to the origin.
94 /// Can be used with GJK, as an alternative to Johnson distance algorithm. 94 /// Can be used with GJK, as an alternative to Johnson distance algorithm.
95 public class VoronoiSimplexSolver : ISimplexSolver 95 public class VoronoiSimplexSolver : ISimplexSolver
96 { 96 {
97 private const int VertexA = 0, VertexB = 1, VertexC = 2, VertexD = 3; 97 private const int VertexA = 0, VertexB = 1, VertexC = 2, VertexD = 3;
98 98
99 private const int VoronoiSimplexMaxVerts = 5; 99 private const int VoronoiSimplexMaxVerts = 5;
100 private const bool CatchDegenerateTetrahedron = true; 100 private const bool CatchDegenerateTetrahedron = true;
101 101
102 private int _numVertices; 102 private int _numVertices;
103 103
104 private Vector3[] _simplexVectorW = new Vector3[VoronoiSimplexMaxVerts]; 104 private Vector3[] _simplexVectorW = new Vector3[VoronoiSimplexMaxVerts];
105 private Vector3[] _simplexPointsP = new Vector3[VoronoiSimplexMaxVerts]; 105 private Vector3[] _simplexPointsP = new Vector3[VoronoiSimplexMaxVerts];
106 private Vector3[] _simplexPointsQ = new Vector3[VoronoiSimplexMaxVerts]; 106 private Vector3[] _simplexPointsQ = new Vector3[VoronoiSimplexMaxVerts];
107 107
108 private Vector3 _cachedPA; 108 private Vector3 _cachedPA;
109 private Vector3 _cachedPB; 109 private Vector3 _cachedPB;
110 private Vector3 _cachedV; 110 private Vector3 _cachedV;
111 private Vector3 _lastW; 111 private Vector3 _lastW;
112 private bool _cachedValidClosest; 112 private bool _cachedValidClosest;
113 113
114 private SubSimplexClosestResult _cachedBC = new SubSimplexClosestResult(); 114 private SubSimplexClosestResult _cachedBC = new SubSimplexClosestResult();
115 115
116 private bool _needsUpdate; 116 private bool _needsUpdate;
117 117
118 #region ISimplexSolver Members 118 #region ISimplexSolver Members
119 119
120 public bool FullSimplex 120 public bool FullSimplex
121 { 121 {
122 get 122 get
123 { 123 {
124 return _numVertices == 4; 124 return _numVertices == 4;
125 } 125 }
126 } 126 }
127 127
128 public int NumVertices 128 public int NumVertices
129 { 129 {
130 get 130 get
131 { 131 {
132 return _numVertices; 132 return _numVertices;
133 } 133 }
134 } 134 }
135 135
136 public void Reset() 136 public void Reset()
137 { 137 {
138 _cachedValidClosest = false; 138 _cachedValidClosest = false;
139 _numVertices = 0; 139 _numVertices = 0;
140 _needsUpdate = true; 140 _needsUpdate = true;
141 _lastW = new Vector3(1e30f, 1e30f, 1e30f); 141 _lastW = new Vector3(1e30f, 1e30f, 1e30f);
142 _cachedBC.Reset(); 142 _cachedBC.Reset();
143 } 143 }
144 144
145 public void AddVertex(Vector3 w, Vector3 p, Vector3 q) 145 public void AddVertex(Vector3 w, Vector3 p, Vector3 q)
146 { 146 {
147 _lastW = w; 147 _lastW = w;
148 _needsUpdate = true; 148 _needsUpdate = true;
149 149
150 _simplexVectorW[_numVertices] = w; 150 _simplexVectorW[_numVertices] = w;
151 _simplexPointsP[_numVertices] = p; 151 _simplexPointsP[_numVertices] = p;
152 _simplexPointsQ[_numVertices] = q; 152 _simplexPointsQ[_numVertices] = q;
153 153
154 _numVertices++; 154 _numVertices++;
155 } 155 }
156 156
157 //return/calculate the closest vertex 157 //return/calculate the closest vertex
158 public bool Closest(out Vector3 v) 158 public bool Closest(out Vector3 v)
159 { 159 {
160 bool succes = UpdateClosestVectorAndPoints(); 160 bool succes = UpdateClosestVectorAndPoints();
161 v = _cachedV; 161 v = _cachedV;
162 return succes; 162 return succes;
163 } 163 }
164 164
165 public float MaxVertex 165 public float MaxVertex
166 { 166 {
167 get 167 get
168 { 168 {
169 int numverts = NumVertices; 169 int numverts = NumVertices;
170 float maxV = 0f, curLen2; 170 float maxV = 0f, curLen2;
171 for (int i = 0; i < numverts; i++) 171 for (int i = 0; i < numverts; i++)
172 { 172 {
173 curLen2 = _simplexVectorW[i].LengthSquared(); 173 curLen2 = _simplexVectorW[i].LengthSquared();
174 if (maxV < curLen2) maxV = curLen2; 174 if (maxV < curLen2) maxV = curLen2;
175 } 175 }
176 return maxV; 176 return maxV;
177 } 177 }
178 } 178 }
179 179
180 //return the current simplex 180 //return the current simplex
181 public int GetSimplex(out Vector3[] pBuf, out Vector3[] qBuf, out Vector3[] yBuf) 181 public int GetSimplex(out Vector3[] pBuf, out Vector3[] qBuf, out Vector3[] yBuf)
182 { 182 {
183 int numverts = NumVertices; 183 int numverts = NumVertices;
184 pBuf = new Vector3[numverts]; 184 pBuf = new Vector3[numverts];
185 qBuf = new Vector3[numverts]; 185 qBuf = new Vector3[numverts];
186 yBuf = new Vector3[numverts]; 186 yBuf = new Vector3[numverts];
187 for (int i = 0; i < numverts; i++) 187 for (int i = 0; i < numverts; i++)
188 { 188 {
189 yBuf[i] = _simplexVectorW[i]; 189 yBuf[i] = _simplexVectorW[i];
190 pBuf[i] = _simplexPointsP[i]; 190 pBuf[i] = _simplexPointsP[i];
191 qBuf[i] = _simplexPointsQ[i]; 191 qBuf[i] = _simplexPointsQ[i];
192 } 192 }
193 return numverts; 193 return numverts;
194 } 194 }
195 195
196 public bool InSimplex(Vector3 w) 196 public bool InSimplex(Vector3 w)
197 { 197 {
198 //check in case lastW is already removed 198 //check in case lastW is already removed
199 if (w == _lastW) return true; 199 if (w == _lastW) return true;
200 200
201 //w is in the current (reduced) simplex 201 //w is in the current (reduced) simplex
202 int numverts = NumVertices; 202 int numverts = NumVertices;
203 for (int i = 0; i < numverts; i++) 203 for (int i = 0; i < numverts; i++)
204 if (_simplexVectorW[i] == w) return true; 204 if (_simplexVectorW[i] == w) return true;
205 205
206 return false; 206 return false;
207 } 207 }
208 208
209 public void BackupClosest(out Vector3 v) 209 public void BackupClosest(out Vector3 v)
210 { 210 {
211 v = _cachedV; 211 v = _cachedV;
212 } 212 }
213 213
214 public bool EmptySimplex 214 public bool EmptySimplex
215 { 215 {
216 get 216 get
217 { 217 {
218 return NumVertices == 0; 218 return NumVertices == 0;
219 } 219 }
220 } 220 }
221 221
222 public void ComputePoints(out Vector3 p1, out Vector3 p2) 222 public void ComputePoints(out Vector3 p1, out Vector3 p2)
223 { 223 {
224 UpdateClosestVectorAndPoints(); 224 UpdateClosestVectorAndPoints();
225 p1 = _cachedPA; 225 p1 = _cachedPA;
226 p2 = _cachedPB; 226 p2 = _cachedPB;
227 } 227 }
228 228
229 #endregion 229 #endregion
230 230
231 public void RemoveVertex(int index) 231 public void RemoveVertex(int index)
232 { 232 {
233 BulletDebug.Assert(_numVertices > 0); 233 BulletDebug.Assert(_numVertices > 0);
234 _numVertices--; 234 _numVertices--;
235 _simplexVectorW[index] = _simplexVectorW[_numVertices]; 235 _simplexVectorW[index] = _simplexVectorW[_numVertices];
236 _simplexPointsP[index] = _simplexPointsP[_numVertices]; 236 _simplexPointsP[index] = _simplexPointsP[_numVertices];
237 _simplexPointsQ[index] = _simplexPointsQ[_numVertices]; 237 _simplexPointsQ[index] = _simplexPointsQ[_numVertices];
238 } 238 }
239 239
240 public void ReduceVertices(UsageBitfield usedVerts) 240 public void ReduceVertices(UsageBitfield usedVerts)
241 { 241 {
242 if ((NumVertices >= 4) && (!usedVerts.UsedVertexD)) RemoveVertex(3); 242 if ((NumVertices >= 4) && (!usedVerts.UsedVertexD)) RemoveVertex(3);
243 if ((NumVertices >= 3) && (!usedVerts.UsedVertexC)) RemoveVertex(2); 243 if ((NumVertices >= 3) && (!usedVerts.UsedVertexC)) RemoveVertex(2);
244 if ((NumVertices >= 2) && (!usedVerts.UsedVertexB)) RemoveVertex(1); 244 if ((NumVertices >= 2) && (!usedVerts.UsedVertexB)) RemoveVertex(1);
245 if ((NumVertices >= 1) && (!usedVerts.UsedVertexA)) RemoveVertex(0); 245 if ((NumVertices >= 1) && (!usedVerts.UsedVertexA)) RemoveVertex(0);
246 } 246 }
247 247
248 public bool UpdateClosestVectorAndPoints() 248 public bool UpdateClosestVectorAndPoints()
249 { 249 {
250 if (_needsUpdate) 250 if (_needsUpdate)
251 { 251 {
252 _cachedBC.Reset(); 252 _cachedBC.Reset();
253 _needsUpdate = false; 253 _needsUpdate = false;
254 254
255 Vector3 p, a, b, c, d; 255 Vector3 p, a, b, c, d;
256 switch (NumVertices) 256 switch (NumVertices)
257 { 257 {
258 case 0: 258 case 0:
259 _cachedValidClosest = false; 259 _cachedValidClosest = false;
260 break; 260 break;
261 case 1: 261 case 1:
262 _cachedPA = _simplexPointsP[0]; 262 _cachedPA = _simplexPointsP[0];
263 _cachedPB = _simplexPointsQ[0]; 263 _cachedPB = _simplexPointsQ[0];
264 _cachedV = _cachedPA - _cachedPB; 264 _cachedV = _cachedPA - _cachedPB;
265 _cachedBC.Reset(); 265 _cachedBC.Reset();
266 _cachedBC.SetBarycentricCoordinates(1f, 0f, 0f, 0f); 266 _cachedBC.SetBarycentricCoordinates(1f, 0f, 0f, 0f);
267 _cachedValidClosest = _cachedBC.IsValid; 267 _cachedValidClosest = _cachedBC.IsValid;
268 break; 268 break;
269 case 2: 269 case 2:
270 //closest point origin from line segment 270 //closest point origin from line segment
271 Vector3 from = _simplexVectorW[0]; 271 Vector3 from = _simplexVectorW[0];
272 Vector3 to = _simplexVectorW[1]; 272 Vector3 to = _simplexVectorW[1];
273 Vector3 nearest; 273 Vector3 nearest;
274 274
275 Vector3 diff = -from; 275 Vector3 diff = -from;
276 Vector3 v = to - from; 276 Vector3 v = to - from;
277 float t = Vector3.Dot(v, diff); 277 float t = Vector3.Dot(v, diff);
278 278
279 if (t > 0) 279 if (t > 0)
280 { 280 {
281 float dotVV = v.LengthSquared(); 281 float dotVV = v.LengthSquared();
282 if (t < dotVV) 282 if (t < dotVV)
283 { 283 {
284 t /= dotVV; 284 t /= dotVV;
285 diff -= t * v; 285 diff -= t * v;
286 _cachedBC.UsedVertices.UsedVertexA = true; 286 _cachedBC.UsedVertices.UsedVertexA = true;
287 _cachedBC.UsedVertices.UsedVertexB = true; 287 _cachedBC.UsedVertices.UsedVertexB = true;
288 } 288 }
289 else 289 else
290 { 290 {
291 t = 1; 291 t = 1;
292 diff -= v; 292 diff -= v;
293 //reduce to 1 point 293 //reduce to 1 point
294 _cachedBC.UsedVertices.UsedVertexB = true; 294 _cachedBC.UsedVertices.UsedVertexB = true;
295 } 295 }
296 } 296 }
297 else 297 else
298 { 298 {
299 t = 0; 299 t = 0;
300 //reduce to 1 point 300 //reduce to 1 point
301 _cachedBC.UsedVertices.UsedVertexA = true; 301 _cachedBC.UsedVertices.UsedVertexA = true;
302 } 302 }
303 303
304 _cachedBC.SetBarycentricCoordinates(1 - t, t, 0, 0); 304 _cachedBC.SetBarycentricCoordinates(1 - t, t, 0, 0);
305 nearest = from + t * v; 305 nearest = from + t * v;
306 306
307 _cachedPA = _simplexPointsP[0] + t * (_simplexPointsP[1] - _simplexPointsP[0]); 307 _cachedPA = _simplexPointsP[0] + t * (_simplexPointsP[1] - _simplexPointsP[0]);
308 _cachedPB = _simplexPointsQ[0] + t * (_simplexPointsQ[1] - _simplexPointsQ[0]); 308 _cachedPB = _simplexPointsQ[0] + t * (_simplexPointsQ[1] - _simplexPointsQ[0]);
309 _cachedV = _cachedPA - _cachedPB; 309 _cachedV = _cachedPA - _cachedPB;
310 310
311 ReduceVertices(_cachedBC.UsedVertices); 311 ReduceVertices(_cachedBC.UsedVertices);
312 312
313 _cachedValidClosest = _cachedBC.IsValid; 313 _cachedValidClosest = _cachedBC.IsValid;
314 break; 314 break;
315 case 3: 315 case 3:
316 //closest point origin from triangle 316 //closest point origin from triangle
317 p = new Vector3(); 317 p = new Vector3();
318 a = _simplexVectorW[0]; 318 a = _simplexVectorW[0];
319 b = _simplexVectorW[1]; 319 b = _simplexVectorW[1];
320 c = _simplexVectorW[2]; 320 c = _simplexVectorW[2];
321 321
322 ClosestPtPointTriangle(p, a, b, c, ref _cachedBC); 322 ClosestPtPointTriangle(p, a, b, c, ref _cachedBC);
323 _cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] + 323 _cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] +
324 _simplexPointsP[1] * _cachedBC.BarycentricCoords[1] + 324 _simplexPointsP[1] * _cachedBC.BarycentricCoords[1] +
325 _simplexPointsP[2] * _cachedBC.BarycentricCoords[2] + 325 _simplexPointsP[2] * _cachedBC.BarycentricCoords[2] +
326 _simplexPointsP[3] * _cachedBC.BarycentricCoords[3]; 326 _simplexPointsP[3] * _cachedBC.BarycentricCoords[3];
327 327
328 _cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] + 328 _cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] +
329 _simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] + 329 _simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] +
330 _simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] + 330 _simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] +
331 _simplexPointsQ[3] * _cachedBC.BarycentricCoords[3]; 331 _simplexPointsQ[3] * _cachedBC.BarycentricCoords[3];
332 332
333 _cachedV = _cachedPA - _cachedPB; 333 _cachedV = _cachedPA - _cachedPB;
334 334
335 ReduceVertices(_cachedBC.UsedVertices); 335 ReduceVertices(_cachedBC.UsedVertices);
336 _cachedValidClosest = _cachedBC.IsValid; 336 _cachedValidClosest = _cachedBC.IsValid;
337 break; 337 break;
338 case 4: 338 case 4:
339 p = new Vector3(); 339 p = new Vector3();
340 a = _simplexVectorW[0]; 340 a = _simplexVectorW[0];
341 b = _simplexVectorW[1]; 341 b = _simplexVectorW[1];
342 c = _simplexVectorW[2]; 342 c = _simplexVectorW[2];
343 d = _simplexVectorW[3]; 343 d = _simplexVectorW[3];
344 344
345 bool hasSeperation = ClosestPtPointTetrahedron(p, a, b, c, d, ref _cachedBC); 345 bool hasSeperation = ClosestPtPointTetrahedron(p, a, b, c, d, ref _cachedBC);
346 346
347 if (hasSeperation) 347 if (hasSeperation)
348 { 348 {
349 _cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] + 349 _cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] +
350 _simplexPointsP[1] * _cachedBC.BarycentricCoords[1] + 350 _simplexPointsP[1] * _cachedBC.BarycentricCoords[1] +
351 _simplexPointsP[2] * _cachedBC.BarycentricCoords[2] + 351 _simplexPointsP[2] * _cachedBC.BarycentricCoords[2] +
352 _simplexPointsP[3] * _cachedBC.BarycentricCoords[3]; 352 _simplexPointsP[3] * _cachedBC.BarycentricCoords[3];
353 353
354 _cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] + 354 _cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] +
355 _simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] + 355 _simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] +
356 _simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] + 356 _simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] +
357 _simplexPointsQ[3] * _cachedBC.BarycentricCoords[3]; 357 _simplexPointsQ[3] * _cachedBC.BarycentricCoords[3];
358 358
359 _cachedV = _cachedPA - _cachedPB; 359 _cachedV = _cachedPA - _cachedPB;
360 ReduceVertices(_cachedBC.UsedVertices); 360 ReduceVertices(_cachedBC.UsedVertices);
361 } 361 }
362 else 362 else
363 { 363 {
364 if (_cachedBC.Degenerate) 364 if (_cachedBC.Degenerate)
365 { 365 {
366 _cachedValidClosest = false; 366 _cachedValidClosest = false;
367 } 367 }
368 else 368 else
369 { 369 {
370 _cachedValidClosest = true; 370 _cachedValidClosest = true;
371 //degenerate case == false, penetration = true + zero 371 //degenerate case == false, penetration = true + zero
372 _cachedV.X = _cachedV.Y = _cachedV.Z = 0f; 372 _cachedV.X = _cachedV.Y = _cachedV.Z = 0f;
373 } 373 }
374 break; // !!!!!!!!!!!! proverit na vsakiy sluchai 374 break; // !!!!!!!!!!!! proverit na vsakiy sluchai
375 } 375 }
376 376
377 _cachedValidClosest = _cachedBC.IsValid; 377 _cachedValidClosest = _cachedBC.IsValid;
378 378
379 //closest point origin from tetrahedron 379 //closest point origin from tetrahedron
380 break; 380 break;
381 default: 381 default:
382 _cachedValidClosest = false; 382 _cachedValidClosest = false;
383 break; 383 break;
384 } 384 }
385 } 385 }
386 386
387 return _cachedValidClosest; 387 return _cachedValidClosest;
388 } 388 }
389 389
390 public bool ClosestPtPointTriangle(Vector3 p, Vector3 a, Vector3 b, Vector3 c, 390 public bool ClosestPtPointTriangle(Vector3 p, Vector3 a, Vector3 b, Vector3 c,
391 ref SubSimplexClosestResult result) 391 ref SubSimplexClosestResult result)
392 { 392 {
393 result.UsedVertices.Reset(); 393 result.UsedVertices.Reset();
394 394
395 float v, w; 395 float v, w;
396 396
397 // Check if P in vertex region outside A 397 // Check if P in vertex region outside A
398 Vector3 ab = b - a; 398 Vector3 ab = b - a;
399 Vector3 ac = c - a; 399 Vector3 ac = c - a;
400 Vector3 ap = p - a; 400 Vector3 ap = p - a;
401 float d1 = Vector3.Dot(ab, ap); 401 float d1 = Vector3.Dot(ab, ap);
402 float d2 = Vector3.Dot(ac, ap); 402 float d2 = Vector3.Dot(ac, ap);
403 if (d1 <= 0f && d2 <= 0f) 403 if (d1 <= 0f && d2 <= 0f)
404 { 404 {
405 result.ClosestPointOnSimplex = a; 405 result.ClosestPointOnSimplex = a;
406 result.UsedVertices.UsedVertexA = true; 406 result.UsedVertices.UsedVertexA = true;
407 result.SetBarycentricCoordinates(1, 0, 0, 0); 407 result.SetBarycentricCoordinates(1, 0, 0, 0);
408 return true; // a; // barycentric coordinates (1,0,0) 408 return true; // a; // barycentric coordinates (1,0,0)
409 } 409 }
410 410
411 // Check if P in vertex region outside B 411 // Check if P in vertex region outside B
412 Vector3 bp = p - b; 412 Vector3 bp = p - b;
413 float d3 = Vector3.Dot(ab, bp); 413 float d3 = Vector3.Dot(ab, bp);
414 float d4 = Vector3.Dot(ac, bp); 414 float d4 = Vector3.Dot(ac, bp);
415 if (d3 >= 0f && d4 <= d3) 415 if (d3 >= 0f && d4 <= d3)
416 { 416 {
417 result.ClosestPointOnSimplex = b; 417 result.ClosestPointOnSimplex = b;
418 result.UsedVertices.UsedVertexB = true; 418 result.UsedVertices.UsedVertexB = true;
419 result.SetBarycentricCoordinates(0, 1, 0, 0); 419 result.SetBarycentricCoordinates(0, 1, 0, 0);
420 420
421 return true; // b; // barycentric coordinates (0,1,0) 421 return true; // b; // barycentric coordinates (0,1,0)
422 } 422 }
423 // Check if P in edge region of AB, if so return projection of P onto AB 423 // Check if P in edge region of AB, if so return projection of P onto AB
424 float vc = d1 * d4 - d3 * d2; 424 float vc = d1 * d4 - d3 * d2;
425 if (vc <= 0f && d1 >= 0f && d3 <= 0f) 425 if (vc <= 0f && d1 >= 0f && d3 <= 0f)
426 { 426 {
427 v = d1 / (d1 - d3); 427 v = d1 / (d1 - d3);
428 result.ClosestPointOnSimplex = a + v * ab; 428 result.ClosestPointOnSimplex = a + v * ab;
429 result.UsedVertices.UsedVertexA = true; 429 result.UsedVertices.UsedVertexA = true;
430 result.UsedVertices.UsedVertexB = true; 430 result.UsedVertices.UsedVertexB = true;
431 result.SetBarycentricCoordinates(1 - v, v, 0, 0); 431 result.SetBarycentricCoordinates(1 - v, v, 0, 0);
432 return true; 432 return true;
433 //return a + v * ab; // barycentric coordinates (1-v,v,0) 433 //return a + v * ab; // barycentric coordinates (1-v,v,0)
434 } 434 }
435 435
436 // Check if P in vertex region outside C 436 // Check if P in vertex region outside C
437 Vector3 cp = p - c; 437 Vector3 cp = p - c;
438 float d5 = Vector3.Dot(ab, cp); 438 float d5 = Vector3.Dot(ab, cp);
439 float d6 = Vector3.Dot(ac, cp); 439 float d6 = Vector3.Dot(ac, cp);
440 if (d6 >= 0f && d5 <= d6) 440 if (d6 >= 0f && d5 <= d6)
441 { 441 {
442 result.ClosestPointOnSimplex = c; 442 result.ClosestPointOnSimplex = c;
443 result.UsedVertices.UsedVertexC = true; 443 result.UsedVertices.UsedVertexC = true;
444 result.SetBarycentricCoordinates(0, 0, 1, 0); 444 result.SetBarycentricCoordinates(0, 0, 1, 0);
445 return true;//c; // barycentric coordinates (0,0,1) 445 return true;//c; // barycentric coordinates (0,0,1)
446 } 446 }
447 447
448 // Check if P in edge region of AC, if so return projection of P onto AC 448 // Check if P in edge region of AC, if so return projection of P onto AC
449 float vb = d5 * d2 - d1 * d6; 449 float vb = d5 * d2 - d1 * d6;
450 if (vb <= 0f && d2 >= 0f && d6 <= 0f) 450 if (vb <= 0f && d2 >= 0f && d6 <= 0f)
451 { 451 {
452 w = d2 / (d2 - d6); 452 w = d2 / (d2 - d6);
453 result.ClosestPointOnSimplex = a + w * ac; 453 result.ClosestPointOnSimplex = a + w * ac;
454 result.UsedVertices.UsedVertexA = true; 454 result.UsedVertices.UsedVertexA = true;
455 result.UsedVertices.UsedVertexC = true; 455 result.UsedVertices.UsedVertexC = true;
456 result.SetBarycentricCoordinates(1 - w, 0, w, 0); 456 result.SetBarycentricCoordinates(1 - w, 0, w, 0);
457 return true; 457 return true;
458 //return a + w * ac; // barycentric coordinates (1-w,0,w) 458 //return a + w * ac; // barycentric coordinates (1-w,0,w)
459 } 459 }
460 460
461 // Check if P in edge region of BC, if so return projection of P onto BC 461 // Check if P in edge region of BC, if so return projection of P onto BC
462 float va = d3 * d6 - d5 * d4; 462 float va = d3 * d6 - d5 * d4;
463 if (va <= 0f && (d4 - d3) >= 0f && (d5 - d6) >= 0f) 463 if (va <= 0f && (d4 - d3) >= 0f && (d5 - d6) >= 0f)
464 { 464 {
465 w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); 465 w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
466 466
467 result.ClosestPointOnSimplex = b + w * (c - b); 467 result.ClosestPointOnSimplex = b + w * (c - b);
468 result.UsedVertices.UsedVertexB = true; 468 result.UsedVertices.UsedVertexB = true;
469 result.UsedVertices.UsedVertexC = true; 469 result.UsedVertices.UsedVertexC = true;
470 result.SetBarycentricCoordinates(0, 1 - w, w, 0); 470 result.SetBarycentricCoordinates(0, 1 - w, w, 0);
471 return true; 471 return true;
472 // return b + w * (c - b); // barycentric coordinates (0,1-w,w) 472 // return b + w * (c - b); // barycentric coordinates (0,1-w,w)
473 } 473 }
474 474
475 // P inside face region. Compute Q through its barycentric coordinates (u,v,w) 475 // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
476 float denom = 1.0f / (va + vb + vc); 476 float denom = 1.0f / (va + vb + vc);
477 v = vb * denom; 477 v = vb * denom;
478 w = vc * denom; 478 w = vc * denom;
479 479
480 result.ClosestPointOnSimplex = a + ab * v + ac * w; 480 result.ClosestPointOnSimplex = a + ab * v + ac * w;
481 result.UsedVertices.UsedVertexA = true; 481 result.UsedVertices.UsedVertexA = true;
482 result.UsedVertices.UsedVertexB = true; 482 result.UsedVertices.UsedVertexB = true;
483 result.UsedVertices.UsedVertexC = true; 483 result.UsedVertices.UsedVertexC = true;
484 result.SetBarycentricCoordinates(1 - v - w, v, w, 0); 484 result.SetBarycentricCoordinates(1 - v - w, v, w, 0);
485 485
486 return true; 486 return true;
487 } 487 }
488 488
489 /// Test if point p and d lie on opposite sides of plane through abc 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) 490 public int PointOutsideOfPlane(Vector3 p, Vector3 a, Vector3 b, Vector3 c, Vector3 d)
491 { 491 {
492 Vector3 normal = Vector3.Cross(b - a, c - a); 492 Vector3 normal = Vector3.Cross(b - a, c - a);
493 493
494 float signp = Vector3.Dot(p - a, normal); // [AP AB AC] 494 float signp = Vector3.Dot(p - a, normal); // [AP AB AC]
495 float signd = Vector3.Dot(d - a, normal); // [AD AB AC] 495 float signd = Vector3.Dot(d - a, normal); // [AD AB AC]
496 496
497 if (CatchDegenerateTetrahedron) 497 if (CatchDegenerateTetrahedron)
498 if (signd * signd < (1e-4f * 1e-4f)) return -1; 498 if (signd * signd < (1e-4f * 1e-4f)) return -1;
499 499
500 // Points on opposite sides if expression signs are opposite 500 // Points on opposite sides if expression signs are opposite
501 return signp * signd < 0f ? 1 : 0; 501 return signp * signd < 0f ? 1 : 0;
502 } 502 }
503 503
504 public bool ClosestPtPointTetrahedron(Vector3 p, Vector3 a, Vector3 b, Vector3 c, Vector3 d, 504 public bool ClosestPtPointTetrahedron(Vector3 p, Vector3 a, Vector3 b, Vector3 c, Vector3 d,
505 ref SubSimplexClosestResult finalResult) 505 ref SubSimplexClosestResult finalResult)
506 { 506 {
507 SubSimplexClosestResult tempResult = new SubSimplexClosestResult(); 507 SubSimplexClosestResult tempResult = new SubSimplexClosestResult();
508 508
509 // Start out assuming point inside all halfspaces, so closest to itself 509 // Start out assuming point inside all halfspaces, so closest to itself
510 finalResult.ClosestPointOnSimplex = p; 510 finalResult.ClosestPointOnSimplex = p;
511 finalResult.UsedVertices.Reset(); 511 finalResult.UsedVertices.Reset();
512 finalResult.UsedVertices.UsedVertexA = true; 512 finalResult.UsedVertices.UsedVertexA = true;
513 finalResult.UsedVertices.UsedVertexB = true; 513 finalResult.UsedVertices.UsedVertexB = true;
514 finalResult.UsedVertices.UsedVertexC = true; 514 finalResult.UsedVertices.UsedVertexC = true;
515 finalResult.UsedVertices.UsedVertexD = true; 515 finalResult.UsedVertices.UsedVertexD = true;
516 516
517 int pointOutsideABC = PointOutsideOfPlane(p, a, b, c, d); 517 int pointOutsideABC = PointOutsideOfPlane(p, a, b, c, d);
518 int pointOutsideACD = PointOutsideOfPlane(p, a, c, d, b); 518 int pointOutsideACD = PointOutsideOfPlane(p, a, c, d, b);
519 int pointOutsideADB = PointOutsideOfPlane(p, a, d, b, c); 519 int pointOutsideADB = PointOutsideOfPlane(p, a, d, b, c);
520 int pointOutsideBDC = PointOutsideOfPlane(p, b, d, c, a); 520 int pointOutsideBDC = PointOutsideOfPlane(p, b, d, c, a);
521 521
522 if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0) 522 if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
523 { 523 {
524 finalResult.Degenerate = true; 524 finalResult.Degenerate = true;
525 return false; 525 return false;
526 } 526 }
527 527
528 if (pointOutsideABC == 0 && pointOutsideACD == 0 && pointOutsideADB == 0 && pointOutsideBDC == 0) 528 if (pointOutsideABC == 0 && pointOutsideACD == 0 && pointOutsideADB == 0 && pointOutsideBDC == 0)
529 return false; 529 return false;
530 530
531 float bestSqDist = float.MaxValue; 531 float bestSqDist = float.MaxValue;
532 // If point outside face abc then compute closest point on abc 532 // If point outside face abc then compute closest point on abc
533 if (pointOutsideABC != 0) 533 if (pointOutsideABC != 0)
534 { 534 {
535 ClosestPtPointTriangle(p, a, b, c, ref tempResult); 535 ClosestPtPointTriangle(p, a, b, c, ref tempResult);
536 Vector3 q = tempResult.ClosestPointOnSimplex; 536 Vector3 q = tempResult.ClosestPointOnSimplex;
537 537
538 float sqDist = ((Vector3)(q - p)).LengthSquared(); 538 float sqDist = ((Vector3)(q - p)).LengthSquared();
539 // Update best closest point if (squared) distance is less than current best 539 // Update best closest point if (squared) distance is less than current best
540 if (sqDist < bestSqDist) 540 if (sqDist < bestSqDist)
541 { 541 {
542 bestSqDist = sqDist; 542 bestSqDist = sqDist;
543 finalResult.ClosestPointOnSimplex = q; 543 finalResult.ClosestPointOnSimplex = q;
544 //convert result bitmask! 544 //convert result bitmask!
545 finalResult.UsedVertices.Reset(); 545 finalResult.UsedVertices.Reset();
546 finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA; 546 finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA;
547 finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexB; 547 finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexB;
548 finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexC; 548 finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexC;
549 finalResult.SetBarycentricCoordinates( 549 finalResult.SetBarycentricCoordinates(
550 tempResult.BarycentricCoords[VertexA], 550 tempResult.BarycentricCoords[VertexA],
551 tempResult.BarycentricCoords[VertexB], 551 tempResult.BarycentricCoords[VertexB],
552 tempResult.BarycentricCoords[VertexC], 552 tempResult.BarycentricCoords[VertexC],
553 0); 553 0);
554 } 554 }
555 } 555 }
556 556
557 // Repeat test for face acd 557 // Repeat test for face acd
558 if (pointOutsideACD != 0) 558 if (pointOutsideACD != 0)
559 { 559 {
560 ClosestPtPointTriangle(p, a, c, d, ref tempResult); 560 ClosestPtPointTriangle(p, a, c, d, ref tempResult);
561 Vector3 q = tempResult.ClosestPointOnSimplex; 561 Vector3 q = tempResult.ClosestPointOnSimplex;
562 //convert result bitmask! 562 //convert result bitmask!
563 563
564 float sqDist = ((Vector3)(q - p)).LengthSquared(); 564 float sqDist = ((Vector3)(q - p)).LengthSquared();
565 if (sqDist < bestSqDist) 565 if (sqDist < bestSqDist)
566 { 566 {
567 bestSqDist = sqDist; 567 bestSqDist = sqDist;
568 finalResult.ClosestPointOnSimplex = q; 568 finalResult.ClosestPointOnSimplex = q;
569 finalResult.UsedVertices.Reset(); 569 finalResult.UsedVertices.Reset();
570 finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA; 570 finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA;
571 finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexB; 571 finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexB;
572 finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexC; 572 finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexC;
573 finalResult.SetBarycentricCoordinates( 573 finalResult.SetBarycentricCoordinates(
574 tempResult.BarycentricCoords[VertexA], 574 tempResult.BarycentricCoords[VertexA],
575 0, 575 0,
576 tempResult.BarycentricCoords[VertexB], 576 tempResult.BarycentricCoords[VertexB],
577 tempResult.BarycentricCoords[VertexC]); 577 tempResult.BarycentricCoords[VertexC]);
578 } 578 }
579 } 579 }
580 // Repeat test for face adb 580 // Repeat test for face adb
581 581
582 if (pointOutsideADB != 0) 582 if (pointOutsideADB != 0)
583 { 583 {
584 ClosestPtPointTriangle(p, a, d, b, ref tempResult); 584 ClosestPtPointTriangle(p, a, d, b, ref tempResult);
585 Vector3 q = tempResult.ClosestPointOnSimplex; 585 Vector3 q = tempResult.ClosestPointOnSimplex;
586 //convert result bitmask! 586 //convert result bitmask!
587 587
588 float sqDist = ((Vector3)(q - p)).LengthSquared(); 588 float sqDist = ((Vector3)(q - p)).LengthSquared();
589 if (sqDist < bestSqDist) 589 if (sqDist < bestSqDist)
590 { 590 {
591 bestSqDist = sqDist; 591 bestSqDist = sqDist;
592 finalResult.ClosestPointOnSimplex = q; 592 finalResult.ClosestPointOnSimplex = q;
593 finalResult.UsedVertices.Reset(); 593 finalResult.UsedVertices.Reset();
594 finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA; 594 finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA;
595 finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexB; 595 finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexB;
596 finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexC; 596 finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexC;
597 finalResult.SetBarycentricCoordinates( 597 finalResult.SetBarycentricCoordinates(
598 tempResult.BarycentricCoords[VertexA], 598 tempResult.BarycentricCoords[VertexA],
599 tempResult.BarycentricCoords[VertexC], 599 tempResult.BarycentricCoords[VertexC],
600 0, 600 0,
601 tempResult.BarycentricCoords[VertexB]); 601 tempResult.BarycentricCoords[VertexB]);
602 602
603 } 603 }
604 } 604 }
605 // Repeat test for face bdc 605 // Repeat test for face bdc
606 606
607 if (pointOutsideBDC != 0) 607 if (pointOutsideBDC != 0)
608 { 608 {
609 ClosestPtPointTriangle(p, b, d, c, ref tempResult); 609 ClosestPtPointTriangle(p, b, d, c, ref tempResult);
610 Vector3 q = tempResult.ClosestPointOnSimplex; 610 Vector3 q = tempResult.ClosestPointOnSimplex;
611 //convert result bitmask! 611 //convert result bitmask!
612 float sqDist = ((Vector3)(q - p)).LengthSquared(); 612 float sqDist = ((Vector3)(q - p)).LengthSquared();
613 if (sqDist < bestSqDist) 613 if (sqDist < bestSqDist)
614 { 614 {
615 bestSqDist = sqDist; 615 bestSqDist = sqDist;
616 finalResult.ClosestPointOnSimplex = q; 616 finalResult.ClosestPointOnSimplex = q;
617 finalResult.UsedVertices.Reset(); 617 finalResult.UsedVertices.Reset();
618 finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexA; 618 finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexA;
619 finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexB; 619 finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexB;
620 finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexC; 620 finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexC;
621 621
622 finalResult.SetBarycentricCoordinates( 622 finalResult.SetBarycentricCoordinates(
623 0, 623 0,
624 tempResult.BarycentricCoords[VertexA], 624 tempResult.BarycentricCoords[VertexA],
625 tempResult.BarycentricCoords[VertexC], 625 tempResult.BarycentricCoords[VertexC],
626 tempResult.BarycentricCoords[VertexB]); 626 tempResult.BarycentricCoords[VertexB]);
627 } 627 }
628 } 628 }
629 629
630 //help! we ended up full ! 630 //help! we ended up full !
631 631
632 if (finalResult.UsedVertices.UsedVertexA && 632 if (finalResult.UsedVertices.UsedVertexA &&
633 finalResult.UsedVertices.UsedVertexB && 633 finalResult.UsedVertices.UsedVertexB &&
634 finalResult.UsedVertices.UsedVertexC && 634 finalResult.UsedVertices.UsedVertexC &&
635 finalResult.UsedVertices.UsedVertexD) 635 finalResult.UsedVertices.UsedVertexD)
636 { 636 {
637 return true; 637 return true;
638 } 638 }
639 639
640 return true; 640 return true;
641 } 641 }
642 } 642 }
643} 643}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs
index e4a9ae2..e731643 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs
@@ -1,488 +1,488 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX.Dynamics 27namespace XnaDevRu.BulletX.Dynamics
28{ 28{
29 public delegate float ContactSolverFunc (RigidBody bodyA, RigidBody bodyB, ManifoldPoint contactPoint, ContactSolverInfo info); 29 public delegate float ContactSolverFunc (RigidBody bodyA, RigidBody bodyB, ManifoldPoint contactPoint, ContactSolverInfo info);
30 30
31 public enum ContactSolverType 31 public enum ContactSolverType
32 { 32 {
33 Default = 0, 33 Default = 0,
34 TypeA, 34 TypeA,
35 TypeB, 35 TypeB,
36 User, 36 User,
37 MaxContactSolverType, 37 MaxContactSolverType,
38 } 38 }
39 39
40 public class ConstraintPersistentData 40 public class ConstraintPersistentData
41 { 41 {
42 // total applied impulse during most recent frame 42 // total applied impulse during most recent frame
43 private float _appliedImpulse; 43 private float _appliedImpulse;
44 private float _previousAppliedImpulse; 44 private float _previousAppliedImpulse;
45 private float _accumulatedTangentImpulse0; 45 private float _accumulatedTangentImpulse0;
46 private float _accumulatedTangentImpulse1; 46 private float _accumulatedTangentImpulse1;
47 47
48 private float _jacDiagABInv; 48 private float _jacDiagABInv;
49 private float _jacDiagABInvTangentA; 49 private float _jacDiagABInvTangentA;
50 private float _jacDiagABInvTangentB; 50 private float _jacDiagABInvTangentB;
51 private int _persistentLifeTime; 51 private int _persistentLifeTime;
52 private float _restitution; 52 private float _restitution;
53 private float _friction; 53 private float _friction;
54 private float _penetration; 54 private float _penetration;
55 private Vector3 _frictionWorldTangentialA; 55 private Vector3 _frictionWorldTangentialA;
56 private Vector3 _frictionWorldTangentialB; 56 private Vector3 _frictionWorldTangentialB;
57 57
58 private Vector3 _frictionAngularComponent0A; 58 private Vector3 _frictionAngularComponent0A;
59 private Vector3 _frictionAngularComponent0B; 59 private Vector3 _frictionAngularComponent0B;
60 private Vector3 _frictionAngularComponent1A; 60 private Vector3 _frictionAngularComponent1A;
61 private Vector3 _frictionAngularComponent1B; 61 private Vector3 _frictionAngularComponent1B;
62 62
63 //some data doesn't need to be persistent over frames: todo: clean/reuse this 63 //some data doesn't need to be persistent over frames: todo: clean/reuse this
64 private Vector3 _angularComponentA; 64 private Vector3 _angularComponentA;
65 private Vector3 _angularComponentB; 65 private Vector3 _angularComponentB;
66 66
67 private ContactSolverFunc _contactSolverFunc; 67 private ContactSolverFunc _contactSolverFunc;
68 private ContactSolverFunc _frictionSolverFunc; 68 private ContactSolverFunc _frictionSolverFunc;
69 69
70 public float AppliedImpulse { get { return _appliedImpulse; } set { _appliedImpulse = value; } } 70 public float AppliedImpulse { get { return _appliedImpulse; } set { _appliedImpulse = value; } }
71 public float PreviousAppliedImpulse { get { return _previousAppliedImpulse; } set { _previousAppliedImpulse = value; } } 71 public float PreviousAppliedImpulse { get { return _previousAppliedImpulse; } set { _previousAppliedImpulse = value; } }
72 public float AccumulatedTangentImpulseA { get { return _accumulatedTangentImpulse0; } set { _accumulatedTangentImpulse0 = value; } } 72 public float AccumulatedTangentImpulseA { get { return _accumulatedTangentImpulse0; } set { _accumulatedTangentImpulse0 = value; } }
73 public float AccumulatedTangentImpulseB { get { return _accumulatedTangentImpulse1; } set { _accumulatedTangentImpulse1 = value; } } 73 public float AccumulatedTangentImpulseB { get { return _accumulatedTangentImpulse1; } set { _accumulatedTangentImpulse1 = value; } }
74 74
75 public float JacDiagABInv { get { return _jacDiagABInv; } set { _jacDiagABInv = value; } } 75 public float JacDiagABInv { get { return _jacDiagABInv; } set { _jacDiagABInv = value; } }
76 public float JacDiagABInvTangentA { get { return _jacDiagABInvTangentA; } set { _jacDiagABInvTangentA = value; } } 76 public float JacDiagABInvTangentA { get { return _jacDiagABInvTangentA; } set { _jacDiagABInvTangentA = value; } }
77 public float JacDiagABInvTangentB { get { return _jacDiagABInvTangentB; } set { _jacDiagABInvTangentB = value; } } 77 public float JacDiagABInvTangentB { get { return _jacDiagABInvTangentB; } set { _jacDiagABInvTangentB = value; } }
78 public int PersistentLifeTime { get { return _persistentLifeTime; } set { _persistentLifeTime = value; } } 78 public int PersistentLifeTime { get { return _persistentLifeTime; } set { _persistentLifeTime = value; } }
79 public float Restitution { get { return _restitution; } set { _restitution = value; } } 79 public float Restitution { get { return _restitution; } set { _restitution = value; } }
80 public float Friction { get { return _friction; } set { _friction = value; } } 80 public float Friction { get { return _friction; } set { _friction = value; } }
81 public float Penetration { get { return _penetration; } set { _penetration = value; } } 81 public float Penetration { get { return _penetration; } set { _penetration = value; } }
82 public Vector3 FrictionWorldTangentialA { get { return _frictionWorldTangentialA; } set { _frictionWorldTangentialA = value; } } 82 public Vector3 FrictionWorldTangentialA { get { return _frictionWorldTangentialA; } set { _frictionWorldTangentialA = value; } }
83 public Vector3 FrictionWorldTangentialB { get { return _frictionWorldTangentialB; } set { _frictionWorldTangentialB = value; } } 83 public Vector3 FrictionWorldTangentialB { get { return _frictionWorldTangentialB; } set { _frictionWorldTangentialB = value; } }
84 84
85 public Vector3 FrictionAngularComponent0A { get { return _frictionAngularComponent0A; } set { _frictionAngularComponent0A = value; } } 85 public Vector3 FrictionAngularComponent0A { get { return _frictionAngularComponent0A; } set { _frictionAngularComponent0A = value; } }
86 public Vector3 FrictionAngularComponent0B { get { return _frictionAngularComponent0B; } set { _frictionAngularComponent0B = value; } } 86 public Vector3 FrictionAngularComponent0B { get { return _frictionAngularComponent0B; } set { _frictionAngularComponent0B = value; } }
87 public Vector3 FrictionAngularComponent1A { get { return _frictionAngularComponent1A; } set { _frictionAngularComponent1A = value; } } 87 public Vector3 FrictionAngularComponent1A { get { return _frictionAngularComponent1A; } set { _frictionAngularComponent1A = value; } }
88 public Vector3 FrictionAngularComponent1B { get { return _frictionAngularComponent1B; } set { _frictionAngularComponent1B = value; } } 88 public Vector3 FrictionAngularComponent1B { get { return _frictionAngularComponent1B; } set { _frictionAngularComponent1B = value; } }
89 89
90 public Vector3 AngularComponentA { get { return _angularComponentA; } set { _angularComponentA = value; } } 90 public Vector3 AngularComponentA { get { return _angularComponentA; } set { _angularComponentA = value; } }
91 public Vector3 AngularComponentB { get { return _angularComponentB; } set { _angularComponentB = value; } } 91 public Vector3 AngularComponentB { get { return _angularComponentB; } set { _angularComponentB = value; } }
92 92
93 public ContactSolverFunc ContactSolverFunc { get { return _contactSolverFunc; } set { _contactSolverFunc = value; } } 93 public ContactSolverFunc ContactSolverFunc { get { return _contactSolverFunc; } set { _contactSolverFunc = value; } }
94 public ContactSolverFunc FrictionSolverFunc { get { return _frictionSolverFunc; } set { _frictionSolverFunc = value; } } 94 public ContactSolverFunc FrictionSolverFunc { get { return _frictionSolverFunc; } set { _frictionSolverFunc = value; } }
95 } 95 }
96 96
97 public static class ContactConstraint 97 public static class ContactConstraint
98 { 98 {
99 private const int UseInternalApplyImpulse = 1; 99 private const int UseInternalApplyImpulse = 1;
100 100
101 /// <summary> 101 /// <summary>
102 /// bilateral constraint between two dynamic objects 102 /// bilateral constraint between two dynamic objects
103 /// positive distance = separation, negative distance = penetration 103 /// positive distance = separation, negative distance = penetration
104 /// </summary> 104 /// </summary>
105 /// <param name="body1"></param> 105 /// <param name="body1"></param>
106 /// <param name="pos1"></param> 106 /// <param name="pos1"></param>
107 /// <param name="body2"></param> 107 /// <param name="body2"></param>
108 /// <param name="pos2"></param> 108 /// <param name="pos2"></param>
109 /// <param name="distance"></param> 109 /// <param name="distance"></param>
110 /// <param name="normal"></param> 110 /// <param name="normal"></param>
111 /// <param name="impulse"></param> 111 /// <param name="impulse"></param>
112 /// <param name="timeStep"></param> 112 /// <param name="timeStep"></param>
113 public static void ResolveSingleBilateral(RigidBody bodyA, Vector3 posA, 113 public static void ResolveSingleBilateral(RigidBody bodyA, Vector3 posA,
114 RigidBody bodyB, Vector3 posB, 114 RigidBody bodyB, Vector3 posB,
115 float distance, Vector3 normal, out float impulse, float timeStep) 115 float distance, Vector3 normal, out float impulse, float timeStep)
116 { 116 {
117 float normalLenSqr = normal.LengthSquared(); 117 float normalLenSqr = normal.LengthSquared();
118 118
119 if (Math.Abs(normalLenSqr) >= 1.1f) 119 if (Math.Abs(normalLenSqr) >= 1.1f)
120 throw new BulletException(); 120 throw new BulletException();
121 121
122 /*if (normalLenSqr > 1.1f) 122 /*if (normalLenSqr > 1.1f)
123 { 123 {
124 impulse = 0f; 124 impulse = 0f;
125 return; 125 return;
126 }*/ 126 }*/
127 Vector3 rel_pos1 = posA - bodyA.CenterOfMassPosition; 127 Vector3 rel_pos1 = posA - bodyA.CenterOfMassPosition;
128 Vector3 rel_pos2 = posB - bodyB.CenterOfMassPosition; 128 Vector3 rel_pos2 = posB - bodyB.CenterOfMassPosition;
129 //this jacobian entry could be re-used for all iterations 129 //this jacobian entry could be re-used for all iterations
130 130
131 Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1); 131 Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);
132 Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2); 132 Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);
133 Vector3 vel = vel1 - vel2; 133 Vector3 vel = vel1 - vel2;
134 134
135 135
136 JacobianEntry jac = new JacobianEntry(Matrix.Transpose(bodyA.CenterOfMassTransform), 136 JacobianEntry jac = new JacobianEntry(Matrix.Transpose(bodyA.CenterOfMassTransform),
137 Matrix.Transpose(bodyB.CenterOfMassTransform), 137 Matrix.Transpose(bodyB.CenterOfMassTransform),
138 rel_pos1, rel_pos2, normal, bodyA.InvInertiaDiagLocal, bodyA.InverseMass, 138 rel_pos1, rel_pos2, normal, bodyA.InvInertiaDiagLocal, bodyA.InverseMass,
139 bodyB.InvInertiaDiagLocal, bodyB.InverseMass); 139 bodyB.InvInertiaDiagLocal, bodyB.InverseMass);
140 140
141 float jacDiagAB = jac.Diagonal; 141 float jacDiagAB = jac.Diagonal;
142 float jacDiagABInv = 1f / jacDiagAB; 142 float jacDiagABInv = 1f / jacDiagAB;
143 143
144 float rel_vel = jac.GetRelativeVelocity( 144 float rel_vel = jac.GetRelativeVelocity(
145 bodyA.LinearVelocity, 145 bodyA.LinearVelocity,
146 Vector3.TransformNormal(bodyA.AngularVelocity, Matrix.Transpose(bodyA.CenterOfMassTransform)), 146 Vector3.TransformNormal(bodyA.AngularVelocity, Matrix.Transpose(bodyA.CenterOfMassTransform)),
147 bodyB.LinearVelocity, 147 bodyB.LinearVelocity,
148 Vector3.TransformNormal(bodyB.AngularVelocity, Matrix.Transpose(bodyB.CenterOfMassTransform))); 148 Vector3.TransformNormal(bodyB.AngularVelocity, Matrix.Transpose(bodyB.CenterOfMassTransform)));
149 float a; 149 float a;
150 a = jacDiagABInv; 150 a = jacDiagABInv;
151 151
152 152
153 rel_vel = Vector3.Dot(normal, vel); 153 rel_vel = Vector3.Dot(normal, vel);
154 154
155 float contactDamping = 0.2f; 155 float contactDamping = 0.2f;
156 156
157 float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; 157 float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
158 impulse = velocityImpulse; 158 impulse = velocityImpulse;
159 } 159 }
160 160
161 161
162 /// <summary> 162 /// <summary>
163 /// contact constraint resolution: 163 /// contact constraint resolution:
164 /// calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint 164 /// calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint
165 /// positive distance = separation, negative distance = penetration 165 /// positive distance = separation, negative distance = penetration
166 /// </summary> 166 /// </summary>
167 /// <param name="body1"></param> 167 /// <param name="body1"></param>
168 /// <param name="body2"></param> 168 /// <param name="body2"></param>
169 /// <param name="contactPoint"></param> 169 /// <param name="contactPoint"></param>
170 /// <param name="info"></param> 170 /// <param name="info"></param>
171 /// <returns></returns> 171 /// <returns></returns>
172 public static float ResolveSingleCollision(RigidBody bodyA, RigidBody bodyB, 172 public static float ResolveSingleCollision(RigidBody bodyA, RigidBody bodyB,
173 ManifoldPoint contactPoint, ContactSolverInfo solverInfo) 173 ManifoldPoint contactPoint, ContactSolverInfo solverInfo)
174 { 174 {
175 Vector3 pos1 = contactPoint.PositionWorldOnA; 175 Vector3 pos1 = contactPoint.PositionWorldOnA;
176 Vector3 pos2 = contactPoint.PositionWorldOnB; 176 Vector3 pos2 = contactPoint.PositionWorldOnB;
177 177
178 178
179 // printf("distance=%f\n",distance); 179 // printf("distance=%f\n",distance);
180 180
181 Vector3 normal = contactPoint.NormalWorldOnB; 181 Vector3 normal = contactPoint.NormalWorldOnB;
182 182
183 Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition; 183 Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition;
184 Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition; 184 Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition;
185 185
186 Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1); 186 Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);
187 Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2); 187 Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);
188 Vector3 vel = vel1 - vel2; 188 Vector3 vel = vel1 - vel2;
189 float rel_vel; 189 float rel_vel;
190 rel_vel = Vector3.Dot(normal, vel); 190 rel_vel = Vector3.Dot(normal, vel);
191 191
192 192
193 float Kfps = 1f / solverInfo.TimeStep; 193 float Kfps = 1f / solverInfo.TimeStep;
194 194
195 //float damping = solverInfo.m_damping; 195 //float damping = solverInfo.m_damping;
196 float Kerp = solverInfo.Erp; 196 float Kerp = solverInfo.Erp;
197 197
198 float Kcor = Kerp * Kfps; 198 float Kcor = Kerp * Kfps;
199 199
200 //printf("dist=%f\n",distance); 200 //printf("dist=%f\n",distance);
201 201
202 ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; 202 ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
203 if (cpd == null) 203 if (cpd == null)
204 throw new BulletException(); 204 throw new BulletException();
205 205
206 float distance = cpd.Penetration;//contactPoint.getDistance(); 206 float distance = cpd.Penetration;//contactPoint.getDistance();
207 207
208 208
209 //distance = 0.f; 209 //distance = 0.f;
210 float positionalError = Kcor * -distance; 210 float positionalError = Kcor * -distance;
211 //jacDiagABInv; 211 //jacDiagABInv;
212 float velocityError = cpd.Restitution - rel_vel;// * damping; 212 float velocityError = cpd.Restitution - rel_vel;// * damping;
213 213
214 214
215 float penetrationImpulse = positionalError * cpd.JacDiagABInv; 215 float penetrationImpulse = positionalError * cpd.JacDiagABInv;
216 float velocityImpulse = velocityError * cpd.JacDiagABInv; 216 float velocityImpulse = velocityError * cpd.JacDiagABInv;
217 float normalImpulse = penetrationImpulse + velocityImpulse; 217 float normalImpulse = penetrationImpulse + velocityImpulse;
218 218
219 // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse 219 // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
220 float oldNormalImpulse = cpd.AppliedImpulse; 220 float oldNormalImpulse = cpd.AppliedImpulse;
221 float sum = oldNormalImpulse + normalImpulse; 221 float sum = oldNormalImpulse + normalImpulse;
222 cpd.AppliedImpulse = 0f > sum ? 0f : sum; 222 cpd.AppliedImpulse = 0f > sum ? 0f : sum;
223 223
224 normalImpulse = cpd.AppliedImpulse - oldNormalImpulse; 224 normalImpulse = cpd.AppliedImpulse - oldNormalImpulse;
225 225
226 if (bodyA.InverseMass != 0) 226 if (bodyA.InverseMass != 0)
227 { 227 {
228 bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse); 228 bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse);
229 } 229 }
230 if (bodyB.InverseMass != 0) 230 if (bodyB.InverseMass != 0)
231 { 231 {
232 bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse); 232 bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse);
233 } 233 }
234 234
235 /*body1.applyImpulse(normal * (normalImpulse), rel_pos1); 235 /*body1.applyImpulse(normal * (normalImpulse), rel_pos1);
236 body2.applyImpulse(-normal * (normalImpulse), rel_pos2);*/ 236 body2.applyImpulse(-normal * (normalImpulse), rel_pos2);*/
237 237
238 return normalImpulse; 238 return normalImpulse;
239 } 239 }
240 240
241 public static float ResolveSingleFriction(RigidBody bodyA, RigidBody bodyB, 241 public static float ResolveSingleFriction(RigidBody bodyA, RigidBody bodyB,
242 ManifoldPoint contactPoint, ContactSolverInfo solverInfo) 242 ManifoldPoint contactPoint, ContactSolverInfo solverInfo)
243 { 243 {
244 244
245 Vector3 pos1 = contactPoint.PositionWorldOnA; 245 Vector3 pos1 = contactPoint.PositionWorldOnA;
246 Vector3 pos2 = contactPoint.PositionWorldOnB; 246 Vector3 pos2 = contactPoint.PositionWorldOnB;
247 247
248 Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition; 248 Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition;
249 Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition; 249 Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition;
250 250
251 ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; 251 ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
252 if (cpd == null) 252 if (cpd == null)
253 throw new BulletException(); 253 throw new BulletException();
254 254
255 float combinedFriction = cpd.Friction; 255 float combinedFriction = cpd.Friction;
256 256
257 float limit = cpd.AppliedImpulse * combinedFriction; 257 float limit = cpd.AppliedImpulse * combinedFriction;
258 258
259 //friction 259 //friction
260 if (cpd.AppliedImpulse > 0) 260 if (cpd.AppliedImpulse > 0)
261 { 261 {
262 //apply friction in the 2 tangential directions 262 //apply friction in the 2 tangential directions
263 263
264 // 1st tangent 264 // 1st tangent
265 Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1); 265 Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);
266 Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2); 266 Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);
267 Vector3 vel = vel1 - vel2; 267 Vector3 vel = vel1 - vel2;
268 268
269 float j1, j2; 269 float j1, j2;
270 270
271 { 271 {
272 272
273 float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel); 273 float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel);
274 274
275 // calculate j that moves us to zero relative velocity 275 // calculate j that moves us to zero relative velocity
276 j1 = -vrel * cpd.JacDiagABInvTangentA; 276 j1 = -vrel * cpd.JacDiagABInvTangentA;
277 float oldTangentImpulse = cpd.AccumulatedTangentImpulseA; 277 float oldTangentImpulse = cpd.AccumulatedTangentImpulseA;
278 cpd.AccumulatedTangentImpulseA = oldTangentImpulse + j1; 278 cpd.AccumulatedTangentImpulseA = oldTangentImpulse + j1;
279 float atia = cpd.AccumulatedTangentImpulseA; 279 float atia = cpd.AccumulatedTangentImpulseA;
280 MathHelper.SetMin(ref atia, limit); 280 MathHelper.SetMin(ref atia, limit);
281 MathHelper.SetMax(ref atia, -limit); 281 MathHelper.SetMax(ref atia, -limit);
282 cpd.AccumulatedTangentImpulseA = atia; 282 cpd.AccumulatedTangentImpulseA = atia;
283 j1 = cpd.AccumulatedTangentImpulseA - oldTangentImpulse; 283 j1 = cpd.AccumulatedTangentImpulseA - oldTangentImpulse;
284 284
285 } 285 }
286 { 286 {
287 // 2nd tangent 287 // 2nd tangent
288 288
289 float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel); 289 float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel);
290 290
291 // calculate j that moves us to zero relative velocity 291 // calculate j that moves us to zero relative velocity
292 j2 = -vrel * cpd.JacDiagABInvTangentB; 292 j2 = -vrel * cpd.JacDiagABInvTangentB;
293 float oldTangentImpulse = cpd.AccumulatedTangentImpulseB; 293 float oldTangentImpulse = cpd.AccumulatedTangentImpulseB;
294 cpd.AccumulatedTangentImpulseB = oldTangentImpulse + j2; 294 cpd.AccumulatedTangentImpulseB = oldTangentImpulse + j2;
295 float atib = cpd.AccumulatedTangentImpulseB; 295 float atib = cpd.AccumulatedTangentImpulseB;
296 MathHelper.SetMin(ref atib, limit); 296 MathHelper.SetMin(ref atib, limit);
297 MathHelper.SetMax(ref atib, -limit); 297 MathHelper.SetMax(ref atib, -limit);
298 cpd.AccumulatedTangentImpulseB = atib; 298 cpd.AccumulatedTangentImpulseB = atib;
299 j2 = cpd.AccumulatedTangentImpulseB - oldTangentImpulse; 299 j2 = cpd.AccumulatedTangentImpulseB - oldTangentImpulse;
300 } 300 }
301 301
302 if (bodyA.InverseMass != 0) 302 if (bodyA.InverseMass != 0)
303 { 303 {
304 bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyA.InverseMass, cpd.FrictionAngularComponent0A, j1); 304 bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyA.InverseMass, cpd.FrictionAngularComponent0A, j1);
305 bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyA.InverseMass, cpd.FrictionAngularComponent1A, j2); 305 bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyA.InverseMass, cpd.FrictionAngularComponent1A, j2);
306 } 306 }
307 if (bodyB.InverseMass != 0) 307 if (bodyB.InverseMass != 0)
308 { 308 {
309 bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyB.InverseMass, cpd.FrictionAngularComponent0B, -j1); 309 bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyB.InverseMass, cpd.FrictionAngularComponent0B, -j1);
310 bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyB.InverseMass, cpd.FrictionAngularComponent1B, -j2); 310 bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyB.InverseMass, cpd.FrictionAngularComponent1B, -j2);
311 } 311 }
312 312
313 } 313 }
314 return cpd.AppliedImpulse; 314 return cpd.AppliedImpulse;
315 } 315 }
316 316
317 public static float ResolveSingleFrictionOriginal( 317 public static float ResolveSingleFrictionOriginal(
318 RigidBody bodyA, 318 RigidBody bodyA,
319 RigidBody bodyB, 319 RigidBody bodyB,
320 ManifoldPoint contactPoint, 320 ManifoldPoint contactPoint,
321 ContactSolverInfo solverInfo) 321 ContactSolverInfo solverInfo)
322 { 322 {
323 Vector3 posA = contactPoint.PositionWorldOnA; 323 Vector3 posA = contactPoint.PositionWorldOnA;
324 Vector3 posB = contactPoint.PositionWorldOnB; 324 Vector3 posB = contactPoint.PositionWorldOnB;
325 325
326 Vector3 relPosA = posA - bodyA.CenterOfMassPosition; 326 Vector3 relPosA = posA - bodyA.CenterOfMassPosition;
327 Vector3 relPosB = posB - bodyB.CenterOfMassPosition; 327 Vector3 relPosB = posB - bodyB.CenterOfMassPosition;
328 328
329 ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; 329 ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
330 if (cpd == null) 330 if (cpd == null)
331 throw new BulletException(); 331 throw new BulletException();
332 332
333 float combinedFriction = cpd.Friction; 333 float combinedFriction = cpd.Friction;
334 334
335 float limit = cpd.AppliedImpulse * combinedFriction; 335 float limit = cpd.AppliedImpulse * combinedFriction;
336 //if (contactPoint.m_appliedImpulse>0.f) 336 //if (contactPoint.m_appliedImpulse>0.f)
337 //friction 337 //friction
338 { 338 {
339 //apply friction in the 2 tangential directions 339 //apply friction in the 2 tangential directions
340 340
341 { 341 {
342 // 1st tangent 342 // 1st tangent
343 Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); 343 Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
344 Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); 344 Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
345 Vector3 vel = velA - velB; 345 Vector3 vel = velA - velB;
346 346
347 float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel); 347 float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel);
348 348
349 // calculate j that moves us to zero relative velocity 349 // calculate j that moves us to zero relative velocity
350 float j = -vrel * cpd.JacDiagABInvTangentA; 350 float j = -vrel * cpd.JacDiagABInvTangentA;
351 float total = cpd.AccumulatedTangentImpulseA + j; 351 float total = cpd.AccumulatedTangentImpulseA + j;
352 if (limit < total) 352 if (limit < total)
353 total = limit; 353 total = limit;
354 if (total < -limit) 354 if (total < -limit)
355 total = -limit; 355 total = -limit;
356 j = total - cpd.AccumulatedTangentImpulseA; 356 j = total - cpd.AccumulatedTangentImpulseA;
357 cpd.AccumulatedTangentImpulseA = total; 357 cpd.AccumulatedTangentImpulseA = total;
358 bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialA, relPosA); 358 bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialA, relPosA);
359 bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialA, relPosB); 359 bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialA, relPosB);
360 } 360 }
361 361
362 362
363 { 363 {
364 // 2nd tangent 364 // 2nd tangent
365 Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); 365 Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
366 Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); 366 Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
367 Vector3 vel = velA - velB; 367 Vector3 vel = velA - velB;
368 368
369 float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel); 369 float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel);
370 370
371 // calculate j that moves us to zero relative velocity 371 // calculate j that moves us to zero relative velocity
372 float j = -vrel * cpd.JacDiagABInvTangentB; 372 float j = -vrel * cpd.JacDiagABInvTangentB;
373 float total = cpd.AccumulatedTangentImpulseB + j; 373 float total = cpd.AccumulatedTangentImpulseB + j;
374 if (limit < total) 374 if (limit < total)
375 total = limit; 375 total = limit;
376 if (total < -limit) 376 if (total < -limit)
377 total = -limit; 377 total = -limit;
378 j = total - cpd.AccumulatedTangentImpulseB; 378 j = total - cpd.AccumulatedTangentImpulseB;
379 cpd.AccumulatedTangentImpulseB = total; 379 cpd.AccumulatedTangentImpulseB = total;
380 bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialB, relPosA); 380 bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialB, relPosA);
381 bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialB, relPosB); 381 bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialB, relPosB);
382 } 382 }
383 } 383 }
384 384
385 return cpd.AppliedImpulse; 385 return cpd.AppliedImpulse;
386 } 386 }
387 387
388 //velocity + friction 388 //velocity + friction
389 //response between two dynamic objects with friction 389 //response between two dynamic objects with friction
390 public static float ResolveSingleCollisionCombined( 390 public static float ResolveSingleCollisionCombined(
391 RigidBody bodyA, 391 RigidBody bodyA,
392 RigidBody bodyB, 392 RigidBody bodyB,
393 ManifoldPoint contactPoint, 393 ManifoldPoint contactPoint,
394 ContactSolverInfo solverInfo) 394 ContactSolverInfo solverInfo)
395 { 395 {
396 396
397 Vector3 posA = contactPoint.PositionWorldOnA; 397 Vector3 posA = contactPoint.PositionWorldOnA;
398 Vector3 posB = contactPoint.PositionWorldOnB; 398 Vector3 posB = contactPoint.PositionWorldOnB;
399 Vector3 normal = contactPoint.NormalWorldOnB; 399 Vector3 normal = contactPoint.NormalWorldOnB;
400 400
401 Vector3 relPosA = posA - bodyA.CenterOfMassPosition; 401 Vector3 relPosA = posA - bodyA.CenterOfMassPosition;
402 Vector3 relPosB = posB - bodyB.CenterOfMassPosition; 402 Vector3 relPosB = posB - bodyB.CenterOfMassPosition;
403 403
404 Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); 404 Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
405 Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); 405 Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
406 Vector3 vel = velA - velB; 406 Vector3 vel = velA - velB;
407 float relVel; 407 float relVel;
408 relVel = Vector3.Dot(normal, vel); 408 relVel = Vector3.Dot(normal, vel);
409 409
410 float Kfps = 1f / solverInfo.TimeStep; 410 float Kfps = 1f / solverInfo.TimeStep;
411 411
412 //float damping = solverInfo.m_damping; 412 //float damping = solverInfo.m_damping;
413 float Kerp = solverInfo.Erp; 413 float Kerp = solverInfo.Erp;
414 float Kcor = Kerp * Kfps; 414 float Kcor = Kerp * Kfps;
415 415
416 ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; 416 ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
417 if (cpd == null) 417 if (cpd == null)
418 throw new BulletException(); 418 throw new BulletException();
419 419
420 float distance = cpd.Penetration; 420 float distance = cpd.Penetration;
421 float positionalError = Kcor * -distance; 421 float positionalError = Kcor * -distance;
422 float velocityError = cpd.Restitution - relVel;// * damping; 422 float velocityError = cpd.Restitution - relVel;// * damping;
423 423
424 float penetrationImpulse = positionalError * cpd.JacDiagABInv; 424 float penetrationImpulse = positionalError * cpd.JacDiagABInv;
425 425
426 float velocityImpulse = velocityError * cpd.JacDiagABInv; 426 float velocityImpulse = velocityError * cpd.JacDiagABInv;
427 427
428 float normalImpulse = penetrationImpulse + velocityImpulse; 428 float normalImpulse = penetrationImpulse + velocityImpulse;
429 429
430 // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse 430 // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
431 float oldNormalImpulse = cpd.AppliedImpulse; 431 float oldNormalImpulse = cpd.AppliedImpulse;
432 float sum = oldNormalImpulse + normalImpulse; 432 float sum = oldNormalImpulse + normalImpulse;
433 cpd.AppliedImpulse = 0 > sum ? 0 : sum; 433 cpd.AppliedImpulse = 0 > sum ? 0 : sum;
434 434
435 normalImpulse = cpd.AppliedImpulse - oldNormalImpulse; 435 normalImpulse = cpd.AppliedImpulse - oldNormalImpulse;
436 436
437 if (bodyA.InverseMass != 0) 437 if (bodyA.InverseMass != 0)
438 { 438 {
439 bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse); 439 bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse);
440 } 440 }
441 if (bodyB.InverseMass != 0) 441 if (bodyB.InverseMass != 0)
442 { 442 {
443 bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse); 443 bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse);
444 } 444 }
445 445
446 { 446 {
447 //friction 447 //friction
448 Vector3 vel12 = bodyA.GetVelocityInLocalPoint(relPosA); 448 Vector3 vel12 = bodyA.GetVelocityInLocalPoint(relPosA);
449 Vector3 vel22 = bodyB.GetVelocityInLocalPoint(relPosB); 449 Vector3 vel22 = bodyB.GetVelocityInLocalPoint(relPosB);
450 Vector3 vel3 = vel12 - vel22; 450 Vector3 vel3 = vel12 - vel22;
451 451
452 relVel = Vector3.Dot(normal, vel3); 452 relVel = Vector3.Dot(normal, vel3);
453 453
454 454
455 Vector3 latVel = vel3 - normal * relVel; 455 Vector3 latVel = vel3 - normal * relVel;
456 float lat_rel_vel = latVel.Length(); 456 float lat_rel_vel = latVel.Length();
457 457
458 float combinedFriction = cpd.Friction; 458 float combinedFriction = cpd.Friction;
459 459
460 if (cpd.AppliedImpulse > 0) 460 if (cpd.AppliedImpulse > 0)
461 if (lat_rel_vel > float.Epsilon) 461 if (lat_rel_vel > float.Epsilon)
462 { 462 {
463 latVel /= lat_rel_vel; 463 latVel /= lat_rel_vel;
464 Vector3 temp1 = Vector3.TransformNormal(Vector3.Cross(relPosA, latVel), bodyA.InvInertiaTensorWorld); 464 Vector3 temp1 = Vector3.TransformNormal(Vector3.Cross(relPosA, latVel), bodyA.InvInertiaTensorWorld);
465 Vector3 temp2 = Vector3.TransformNormal(Vector3.Cross(relPosB, latVel), bodyB.InvInertiaTensorWorld); 465 Vector3 temp2 = Vector3.TransformNormal(Vector3.Cross(relPosB, latVel), bodyB.InvInertiaTensorWorld);
466 float friction_impulse = lat_rel_vel / 466 float friction_impulse = lat_rel_vel /
467 (bodyA.InverseMass + bodyB.InverseMass + Vector3.Dot(latVel, Vector3.Cross(temp1, relPosA) + Vector3.Cross(temp2, relPosB))); 467 (bodyA.InverseMass + bodyB.InverseMass + Vector3.Dot(latVel, Vector3.Cross(temp1, relPosA) + Vector3.Cross(temp2, relPosB)));
468 float normal_impulse = cpd.AppliedImpulse * combinedFriction; 468 float normal_impulse = cpd.AppliedImpulse * combinedFriction;
469 469
470 MathHelper.SetMin(ref friction_impulse, normal_impulse); 470 MathHelper.SetMin(ref friction_impulse, normal_impulse);
471 MathHelper.SetMin(ref friction_impulse, -normal_impulse); 471 MathHelper.SetMin(ref friction_impulse, -normal_impulse);
472 bodyA.ApplyImpulse(latVel * -friction_impulse, relPosA); 472 bodyA.ApplyImpulse(latVel * -friction_impulse, relPosA);
473 bodyB.ApplyImpulse(latVel * friction_impulse, relPosB); 473 bodyB.ApplyImpulse(latVel * friction_impulse, relPosB);
474 } 474 }
475 } 475 }
476 return normalImpulse; 476 return normalImpulse;
477 } 477 }
478 478
479 public static float ResolveSingleFrictionEmpty( 479 public static float ResolveSingleFrictionEmpty(
480 RigidBody bodyA, 480 RigidBody bodyA,
481 RigidBody bodyB, 481 RigidBody bodyB,
482 ManifoldPoint contactPoint, 482 ManifoldPoint contactPoint,
483 ContactSolverInfo solverInfo) 483 ContactSolverInfo solverInfo)
484 { 484 {
485 return 0; 485 return 0;
486 } 486 }
487 } 487 }
488} 488}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactSolverInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactSolverInfo.cs
index c1763df..d3424f6 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactSolverInfo.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactSolverInfo.cs
@@ -1,62 +1,62 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25 25
26namespace XnaDevRu.BulletX.Dynamics 26namespace XnaDevRu.BulletX.Dynamics
27{ 27{
28 public class ContactSolverInfo 28 public class ContactSolverInfo
29 { 29 {
30 private float _tau; 30 private float _tau;
31 private float _damping; 31 private float _damping;
32 private float _friction; 32 private float _friction;
33 private float _timeStep; 33 private float _timeStep;
34 private float _restitution; 34 private float _restitution;
35 private int _numIterations; 35 private int _numIterations;
36 private float _maxErrorReduction; 36 private float _maxErrorReduction;
37 private float _sor; 37 private float _sor;
38 private float _erp; 38 private float _erp;
39 39
40 public ContactSolverInfo() 40 public ContactSolverInfo()
41 { 41 {
42 _tau = 0.6f; 42 _tau = 0.6f;
43 _damping = 1.0f; 43 _damping = 1.0f;
44 _friction = 0.3f; 44 _friction = 0.3f;
45 _restitution = 0f; 45 _restitution = 0f;
46 _maxErrorReduction = 20f; 46 _maxErrorReduction = 20f;
47 _numIterations = 10; 47 _numIterations = 10;
48 _erp = 0.4f; 48 _erp = 0.4f;
49 _sor = 1.3f; 49 _sor = 1.3f;
50 } 50 }
51 51
52 public float Tau { get { return _tau; } set { _tau = value; } } 52 public float Tau { get { return _tau; } set { _tau = value; } }
53 public float Damping { get { return _damping; } set { _damping = value; } } 53 public float Damping { get { return _damping; } set { _damping = value; } }
54 public float Friction { get { return _friction; } set { _friction = value; } } 54 public float Friction { get { return _friction; } set { _friction = value; } }
55 public float TimeStep { get { return _timeStep; } set { _timeStep = value; } } 55 public float TimeStep { get { return _timeStep; } set { _timeStep = value; } }
56 public float Restitution { get { return _restitution; } set { _restitution = value; } } 56 public float Restitution { get { return _restitution; } set { _restitution = value; } }
57 public int IterationsCount { get { return _numIterations; } set { _numIterations = value; } } 57 public int IterationsCount { get { return _numIterations; } set { _numIterations = value; } }
58 public float MaxErrorReduction { get { return _maxErrorReduction; } set { _maxErrorReduction = value; } } 58 public float MaxErrorReduction { get { return _maxErrorReduction; } set { _maxErrorReduction = value; } }
59 public float Sor { get { return _sor; } set { _sor = value; } } 59 public float Sor { get { return _sor; } set { _sor = value; } }
60 public float Erp { get { return _erp; } set { _erp = value; } } 60 public float Erp { get { return _erp; } set { _erp = value; } }
61 } 61 }
62} 62}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs
index 479c863..f1e0b1e 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs
@@ -1,440 +1,440 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX.Dynamics 27namespace XnaDevRu.BulletX.Dynamics
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space 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' 31 /// Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'
32 /// Work in progress (is still a Hinge actually) 32 /// Work in progress (is still a Hinge actually)
33 /// </summary> 33 /// </summary>
34 public class Generic6DofConstraint : TypedConstraint 34 public class Generic6DofConstraint : TypedConstraint
35 { 35 {
36 private static readonly float[] _sign = { 1.0f, -1.0f, 1.0f }; 36 private static readonly float[] _sign = { 1.0f, -1.0f, 1.0f };
37 private static readonly int[] _axisA = { 1, 0, 0 }; 37 private static readonly int[] _axisA = { 1, 0, 0 };
38 private static readonly int[] _axisB = { 2, 2, 1 }; 38 private static readonly int[] _axisB = { 2, 2, 1 };
39 39
40 private JacobianEntry[] _jacLinear = new JacobianEntry[3]; // 3 orthogonal linear constraints 40 private JacobianEntry[] _jacLinear = new JacobianEntry[3]; // 3 orthogonal linear constraints
41 private JacobianEntry[] _jacAng = new JacobianEntry[3]; // 3 orthogonal angular constraints 41 private JacobianEntry[] _jacAng = new JacobianEntry[3]; // 3 orthogonal angular constraints
42 42
43 private Matrix _frameInA; // the constraint space w.r.t body A 43 private Matrix _frameInA; // the constraint space w.r.t body A
44 private Matrix _frameInB; // the constraint space w.r.t body B 44 private Matrix _frameInB; // the constraint space w.r.t body B
45 45
46 private float[] _lowerLimit = new float[6]; // the constraint lower limits 46 private float[] _lowerLimit = new float[6]; // the constraint lower limits
47 private float[] _upperLimit = new float[6]; // the constraint upper limits 47 private float[] _upperLimit = new float[6]; // the constraint upper limits
48 48
49 private float[] _accumulatedImpulse = new float[6]; 49 private float[] _accumulatedImpulse = new float[6];
50 50
51 public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, Matrix frameInA, Matrix frameInB) 51 public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, Matrix frameInA, Matrix frameInB)
52 : base(rbA, rbB) 52 : base(rbA, rbB)
53 { 53 {
54 _frameInA = frameInA; 54 _frameInA = frameInA;
55 _frameInB = frameInB; 55 _frameInB = frameInB;
56 //free means upper < lower, 56 //free means upper < lower,
57 //locked means upper == lower 57 //locked means upper == lower
58 //limited means upper > lower 58 //limited means upper > lower
59 //so start all locked 59 //so start all locked
60 for (int i = 0; i < 6; ++i) 60 for (int i = 0; i < 6; ++i)
61 { 61 {
62 _lowerLimit[i] = 0.0f; 62 _lowerLimit[i] = 0.0f;
63 _upperLimit[i] = 0.0f; 63 _upperLimit[i] = 0.0f;
64 _accumulatedImpulse[i] = 0.0f; 64 _accumulatedImpulse[i] = 0.0f;
65 } 65 }
66 } 66 }
67 67
68 public Generic6DofConstraint() { } 68 public Generic6DofConstraint() { }
69 69
70 public void UpdateRHS(float timeStep) { } 70 public void UpdateRHS(float timeStep) { }
71 71
72 public float ComputeAngle(int axis) 72 public float ComputeAngle(int axis)
73 { 73 {
74 float angle = 0; 74 float angle = 0;
75 75
76 switch (axis) 76 switch (axis)
77 { 77 {
78 case 0: 78 case 0:
79 { 79 {
80 Vector3 v1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 1), RigidBodyA.CenterOfMassTransform); 80 Vector3 v1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 1), RigidBodyA.CenterOfMassTransform);
81 Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform); 81 Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform);
82 Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform); 82 Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform);
83 83
84 float s = Vector3.Dot(v1, w2); 84 float s = Vector3.Dot(v1, w2);
85 float c = Vector3.Dot(v1, v2); 85 float c = Vector3.Dot(v1, v2);
86 86
87 angle = (float)Math.Atan2(s, c); 87 angle = (float)Math.Atan2(s, c);
88 break; 88 break;
89 } 89 }
90 case 1: 90 case 1:
91 { 91 {
92 Vector3 w1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 2), RigidBodyA.CenterOfMassTransform); 92 Vector3 w1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 2), RigidBodyA.CenterOfMassTransform);
93 Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform); 93 Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform);
94 Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform); 94 Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform);
95 95
96 float s = Vector3.Dot(w1, u2); 96 float s = Vector3.Dot(w1, u2);
97 float c = Vector3.Dot(w1, w2); 97 float c = Vector3.Dot(w1, w2);
98 98
99 angle = (float)Math.Atan2(s, c); 99 angle = (float)Math.Atan2(s, c);
100 break; 100 break;
101 } 101 }
102 case 2: 102 case 2:
103 { 103 {
104 Vector3 u1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 0), RigidBodyA.CenterOfMassTransform); 104 Vector3 u1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 0), RigidBodyA.CenterOfMassTransform);
105 Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform); 105 Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform);
106 Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform); 106 Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform);
107 107
108 float s = Vector3.Dot(u1, v2); 108 float s = Vector3.Dot(u1, v2);
109 float c = Vector3.Dot(u1, u2); 109 float c = Vector3.Dot(u1, u2);
110 110
111 angle = (float)Math.Atan2(s, c); 111 angle = (float)Math.Atan2(s, c);
112 break; 112 break;
113 } 113 }
114 default: BulletDebug.Assert(false); break; 114 default: BulletDebug.Assert(false); break;
115 } 115 }
116 116
117 return angle; 117 return angle;
118 } 118 }
119 119
120 public void SetLinearLowerLimit(Vector3 linearLower) 120 public void SetLinearLowerLimit(Vector3 linearLower)
121 { 121 {
122 _lowerLimit[0] = linearLower.X; 122 _lowerLimit[0] = linearLower.X;
123 _lowerLimit[1] = linearLower.Y; 123 _lowerLimit[1] = linearLower.Y;
124 _lowerLimit[2] = linearLower.Z; 124 _lowerLimit[2] = linearLower.Z;
125 } 125 }
126 126
127 public void SetLinearUpperLimit(Vector3 linearUpper) 127 public void SetLinearUpperLimit(Vector3 linearUpper)
128 { 128 {
129 _upperLimit[0] = linearUpper.X; 129 _upperLimit[0] = linearUpper.X;
130 _upperLimit[1] = linearUpper.Y; 130 _upperLimit[1] = linearUpper.Y;
131 _upperLimit[2] = linearUpper.Z; 131 _upperLimit[2] = linearUpper.Z;
132 } 132 }
133 133
134 public void SetAngularLowerLimit(Vector3 angularLower) 134 public void SetAngularLowerLimit(Vector3 angularLower)
135 { 135 {
136 _lowerLimit[3] = angularLower.X; 136 _lowerLimit[3] = angularLower.X;
137 _lowerLimit[4] = angularLower.Y; 137 _lowerLimit[4] = angularLower.Y;
138 _lowerLimit[5] = angularLower.Z; 138 _lowerLimit[5] = angularLower.Z;
139 } 139 }
140 140
141 public void SetAngularUpperLimit(Vector3 angularUpper) 141 public void SetAngularUpperLimit(Vector3 angularUpper)
142 { 142 {
143 _upperLimit[3] = angularUpper.X; 143 _upperLimit[3] = angularUpper.X;
144 _upperLimit[4] = angularUpper.Y; 144 _upperLimit[4] = angularUpper.Y;
145 _upperLimit[5] = angularUpper.Z; 145 _upperLimit[5] = angularUpper.Z;
146 } 146 }
147 147
148 //first 3 are linear, next 3 are angular 148 //first 3 are linear, next 3 are angular
149 public void SetLimit(int axis, float lo, float hi) 149 public void SetLimit(int axis, float lo, float hi)
150 { 150 {
151 _lowerLimit[axis] = lo; 151 _lowerLimit[axis] = lo;
152 _upperLimit[axis] = hi; 152 _upperLimit[axis] = hi;
153 } 153 }
154 154
155 //free means upper < lower, 155 //free means upper < lower,
156 //locked means upper == lower 156 //locked means upper == lower
157 //limited means upper > lower 157 //limited means upper > lower
158 //limitIndex: first 3 are linear, next 3 are angular 158 //limitIndex: first 3 are linear, next 3 are angular
159 public bool IsLimited(int limitIndex) 159 public bool IsLimited(int limitIndex)
160 { 160 {
161 return (_upperLimit[limitIndex] >= _lowerLimit[limitIndex]); 161 return (_upperLimit[limitIndex] >= _lowerLimit[limitIndex]);
162 } 162 }
163 163
164 public override void BuildJacobian() 164 public override void BuildJacobian()
165 { 165 {
166 Vector3 localNormalInA = new Vector3(0, 0, 0); 166 Vector3 localNormalInA = new Vector3(0, 0, 0);
167 167
168 Vector3 pivotInA = _frameInA.Translation; 168 Vector3 pivotInA = _frameInA.Translation;
169 Vector3 pivotInB = _frameInB.Translation; 169 Vector3 pivotInB = _frameInB.Translation;
170 170
171 Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform); 171 Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform);
172 Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform); 172 Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform);
173 173
174 Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; 174 Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
175 Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; 175 Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;
176 176
177 //linear part 177 //linear part
178 for (int i = 0; i < 3; i++) 178 for (int i = 0; i < 3; i++)
179 { 179 {
180 if (IsLimited(i)) 180 if (IsLimited(i))
181 { 181 {
182 if (i == 0) 182 if (i == 0)
183 localNormalInA = new Vector3(1, 0, 0); 183 localNormalInA = new Vector3(1, 0, 0);
184 else if (i == 1) 184 else if (i == 1)
185 localNormalInA = new Vector3(0, 1, 0); 185 localNormalInA = new Vector3(0, 1, 0);
186 else 186 else
187 localNormalInA = new Vector3(0, 0, 1); 187 localNormalInA = new Vector3(0, 0, 1);
188 188
189 Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform); 189 Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform);
190 190
191 // Create linear atom 191 // Create linear atom
192 _jacLinear[i] = new JacobianEntry( 192 _jacLinear[i] = new JacobianEntry(
193 MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), 193 MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
194 MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), 194 MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
195 MathHelper.Transform(pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, 195 MathHelper.Transform(pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition,
196 MathHelper.Transform(pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, 196 MathHelper.Transform(pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition,
197 normalWorld, 197 normalWorld,
198 RigidBodyA.InvInertiaDiagLocal, 198 RigidBodyA.InvInertiaDiagLocal,
199 RigidBodyA.InverseMass, 199 RigidBodyA.InverseMass,
200 RigidBodyB.InvInertiaDiagLocal, 200 RigidBodyB.InvInertiaDiagLocal,
201 RigidBodyB.InverseMass); 201 RigidBodyB.InverseMass);
202 202
203 //optionally disable warmstarting 203 //optionally disable warmstarting
204 _accumulatedImpulse[i] = 0f; 204 _accumulatedImpulse[i] = 0f;
205 205
206 // Apply accumulated impulse 206 // Apply accumulated impulse
207 Vector3 impulse_vector = _accumulatedImpulse[i] * normalWorld; 207 Vector3 impulse_vector = _accumulatedImpulse[i] * normalWorld;
208 208
209 RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1); 209 RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1);
210 RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2); 210 RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2);
211 } 211 }
212 } 212 }
213 213
214 // angular part 214 // angular part
215 for (int i = 0; i < 3; i++) 215 for (int i = 0; i < 3; i++)
216 { 216 {
217 if (IsLimited(i + 3)) 217 if (IsLimited(i + 3))
218 { 218 {
219 Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform); 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); 220 Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform);
221 221
222 Vector3 axis = _sign[i] * Vector3.Cross(axisA, axisB); 222 Vector3 axis = _sign[i] * Vector3.Cross(axisA, axisB);
223 223
224 // Create angular atom 224 // Create angular atom
225 _jacAng[i] = new JacobianEntry(axis, 225 _jacAng[i] = new JacobianEntry(axis,
226 MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), 226 MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
227 MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), 227 MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
228 RigidBodyA.InvInertiaDiagLocal, 228 RigidBodyA.InvInertiaDiagLocal,
229 RigidBodyB.InvInertiaDiagLocal); 229 RigidBodyB.InvInertiaDiagLocal);
230 230
231 _accumulatedImpulse[i + 3] = 0f; 231 _accumulatedImpulse[i + 3] = 0f;
232 232
233 // Apply accumulated impulse 233 // Apply accumulated impulse
234 Vector3 impulse_vector = _accumulatedImpulse[i + 3] * axis; 234 Vector3 impulse_vector = _accumulatedImpulse[i + 3] * axis;
235 235
236 RigidBodyA.ApplyTorqueImpulse(impulse_vector); 236 RigidBodyA.ApplyTorqueImpulse(impulse_vector);
237 RigidBodyB.ApplyTorqueImpulse(-impulse_vector); 237 RigidBodyB.ApplyTorqueImpulse(-impulse_vector);
238 } 238 }
239 } 239 }
240 } 240 }
241 241
242 public override void SolveConstraint(float timeStep) 242 public override void SolveConstraint(float timeStep)
243 { 243 {
244 float tau = 0.1f; 244 float tau = 0.1f;
245 float damping = 1.0f; 245 float damping = 1.0f;
246 246
247 Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform); 247 Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform);
248 Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform); 248 Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform);
249 249
250 Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; 250 Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
251 Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; 251 Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;
252 252
253 Vector3 localNormalInA = new Vector3(); 253 Vector3 localNormalInA = new Vector3();
254 254
255 // linear 255 // linear
256 for (int i = 0; i < 3; i++) 256 for (int i = 0; i < 3; i++)
257 { 257 {
258 if (IsLimited(i)) 258 if (IsLimited(i))
259 { 259 {
260 Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform)); 260 Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform));
261 Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform)); 261 Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform));
262 262
263 if (i == 0) 263 if (i == 0)
264 localNormalInA = new Vector3(1, 0, 0); 264 localNormalInA = new Vector3(1, 0, 0);
265 else if (i == 1) 265 else if (i == 1)
266 localNormalInA = new Vector3(0, 1, 0); 266 localNormalInA = new Vector3(0, 1, 0);
267 else 267 else
268 localNormalInA = new Vector3(0, 0, 1); 268 localNormalInA = new Vector3(0, 0, 1);
269 269
270 Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform); 270 Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform);
271 271
272 float jacDiagABInv = 1f / _jacLinear[i].Diagonal; 272 float jacDiagABInv = 1f / _jacLinear[i].Diagonal;
273 273
274 //velocity error (first order error) 274 //velocity error (first order error)
275 float rel_vel = _jacLinear[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA, 275 float rel_vel = _jacLinear[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA,
276 RigidBodyB.LinearVelocity, angvelB); 276 RigidBodyB.LinearVelocity, angvelB);
277 277
278 //positional error (zeroth order error) 278 //positional error (zeroth order error)
279 float depth = -Vector3.Dot(pivotAInW - pivotBInW, normalWorld); 279 float depth = -Vector3.Dot(pivotAInW - pivotBInW, normalWorld);
280 float lo = -1e30f; 280 float lo = -1e30f;
281 float hi = 1e30f; 281 float hi = 1e30f;
282 282
283 //handle the limits 283 //handle the limits
284 if (_lowerLimit[i] < _upperLimit[i]) 284 if (_lowerLimit[i] < _upperLimit[i])
285 { 285 {
286 if (depth > _upperLimit[i]) 286 if (depth > _upperLimit[i])
287 { 287 {
288 depth -= _upperLimit[i]; 288 depth -= _upperLimit[i];
289 lo = 0f; 289 lo = 0f;
290 } 290 }
291 else 291 else
292 { 292 {
293 if (depth < _lowerLimit[i]) 293 if (depth < _lowerLimit[i])
294 { 294 {
295 depth -= _lowerLimit[i]; 295 depth -= _lowerLimit[i];
296 hi = 0f; 296 hi = 0f;
297 } 297 }
298 else 298 else
299 { 299 {
300 continue; 300 continue;
301 } 301 }
302 } 302 }
303 } 303 }
304 304
305 float normalImpulse = (tau * depth / timeStep - damping * rel_vel) * jacDiagABInv; 305 float normalImpulse = (tau * depth / timeStep - damping * rel_vel) * jacDiagABInv;
306 float oldNormalImpulse = _accumulatedImpulse[i]; 306 float oldNormalImpulse = _accumulatedImpulse[i];
307 float sum = oldNormalImpulse + normalImpulse; 307 float sum = oldNormalImpulse + normalImpulse;
308 _accumulatedImpulse[i] = sum > hi ? 0f : sum < lo ? 0f : sum; 308 _accumulatedImpulse[i] = sum > hi ? 0f : sum < lo ? 0f : sum;
309 normalImpulse = _accumulatedImpulse[i] - oldNormalImpulse; 309 normalImpulse = _accumulatedImpulse[i] - oldNormalImpulse;
310 310
311 Vector3 impulse_vector = normalWorld * normalImpulse; 311 Vector3 impulse_vector = normalWorld * normalImpulse;
312 RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1); 312 RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1);
313 RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2); 313 RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2);
314 } 314 }
315 } 315 }
316 316
317 Vector3 axis; 317 Vector3 axis;
318 float angle; 318 float angle;
319 Matrix frameAWorld = RigidBodyA.CenterOfMassTransform * _frameInA; 319 Matrix frameAWorld = RigidBodyA.CenterOfMassTransform * _frameInA;
320 Matrix frameBWorld = RigidBodyB.CenterOfMassTransform * _frameInB; 320 Matrix frameBWorld = RigidBodyB.CenterOfMassTransform * _frameInB;
321 321
322 TransformUtil.CalculateDiffAxisAngle(frameAWorld, frameBWorld, out axis, out angle); 322 TransformUtil.CalculateDiffAxisAngle(frameAWorld, frameBWorld, out axis, out angle);
323 Quaternion diff = new Quaternion(axis, angle); 323 Quaternion diff = new Quaternion(axis, angle);
324 Matrix diffMat = Matrix.CreateFromQuaternion(diff); 324 Matrix diffMat = Matrix.CreateFromQuaternion(diff);
325 Vector3 xyz; 325 Vector3 xyz;
326 // this is not perfect, we can first check which axis are limited, and choose a more appropriate order 326 // this is not perfect, we can first check which axis are limited, and choose a more appropriate order
327 MatrixToEulerXYZ(diffMat, out xyz); 327 MatrixToEulerXYZ(diffMat, out xyz);
328 328
329 // angular 329 // angular
330 for (int i = 0; i < 3; i++) 330 for (int i = 0; i < 3; i++)
331 { 331 {
332 if (IsLimited(i + 3)) 332 if (IsLimited(i + 3))
333 { 333 {
334 Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform)); 334 Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform));
335 Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform)); 335 Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform));
336 336
337 float jacDiagABInv = 1f / _jacAng[i].Diagonal; 337 float jacDiagABInv = 1f / _jacAng[i].Diagonal;
338 338
339 //velocity error (first order error) 339 //velocity error (first order error)
340 float rel_vel = _jacAng[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA, 340 float rel_vel = _jacAng[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA,
341 RigidBodyB.LinearVelocity, angvelB); 341 RigidBodyB.LinearVelocity, angvelB);
342 342
343 //positional error (zeroth order error) 343 //positional error (zeroth order error)
344 Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform); 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); 345 Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform);
346 346
347 float rel_pos = _sign[i] * Vector3.Dot(axisA, axisB); 347 float rel_pos = _sign[i] * Vector3.Dot(axisA, axisB);
348 348
349 float lo = -1e30f; 349 float lo = -1e30f;
350 float hi = 1e30f; 350 float hi = 1e30f;
351 351
352 //handle the twist limit 352 //handle the twist limit
353 if (_lowerLimit[i + 3] < _upperLimit[i + 3]) 353 if (_lowerLimit[i + 3] < _upperLimit[i + 3])
354 { 354 {
355 //clamp the values 355 //clamp the values
356 float loLimit = _upperLimit[i + 3] > -3.1415 ? _lowerLimit[i + 3] : -1e30f; 356 float loLimit = _upperLimit[i + 3] > -3.1415 ? _lowerLimit[i + 3] : -1e30f;
357 float hiLimit = _upperLimit[i + 3] < 3.1415 ? _upperLimit[i + 3] : 1e30f; 357 float hiLimit = _upperLimit[i + 3] < 3.1415 ? _upperLimit[i + 3] : 1e30f;
358 358
359 float projAngle; 359 float projAngle;
360 360
361 if (i == 0) 361 if (i == 0)
362 projAngle = -2f * xyz.Z; 362 projAngle = -2f * xyz.Z;
363 else if (i == 1) 363 else if (i == 1)
364 projAngle = -2f * xyz.Y; 364 projAngle = -2f * xyz.Y;
365 else 365 else
366 projAngle = -2f * xyz.Z; 366 projAngle = -2f * xyz.Z;
367 367
368 if (projAngle < loLimit) 368 if (projAngle < loLimit)
369 { 369 {
370 hi = 0f; 370 hi = 0f;
371 rel_pos = loLimit - projAngle; 371 rel_pos = loLimit - projAngle;
372 } 372 }
373 else 373 else
374 { 374 {
375 if (projAngle > hiLimit) 375 if (projAngle > hiLimit)
376 { 376 {
377 lo = 0f; 377 lo = 0f;
378 rel_pos = (hiLimit - projAngle); 378 rel_pos = (hiLimit - projAngle);
379 } 379 }
380 else 380 else
381 { 381 {
382 continue; 382 continue;
383 } 383 }
384 } 384 }
385 } 385 }
386 386
387 //impulse 387 //impulse
388 388
389 float normalImpulse = -(tau * rel_pos / timeStep + damping * rel_vel) * jacDiagABInv; 389 float normalImpulse = -(tau * rel_pos / timeStep + damping * rel_vel) * jacDiagABInv;
390 float oldNormalImpulse = _accumulatedImpulse[i + 3]; 390 float oldNormalImpulse = _accumulatedImpulse[i + 3];
391 float sum = oldNormalImpulse + normalImpulse; 391 float sum = oldNormalImpulse + normalImpulse;
392 _accumulatedImpulse[i + 3] = sum > hi ? 0f : sum < lo ? 0f : sum; 392 _accumulatedImpulse[i + 3] = sum > hi ? 0f : sum < lo ? 0f : sum;
393 normalImpulse = _accumulatedImpulse[i + 3] - oldNormalImpulse; 393 normalImpulse = _accumulatedImpulse[i + 3] - oldNormalImpulse;
394 394
395 Vector3 axis2 = _sign[i] * Vector3.Cross(axisA, axisB); 395 Vector3 axis2 = _sign[i] * Vector3.Cross(axisA, axisB);
396 Vector3 impulse_vector = axis2 * normalImpulse; 396 Vector3 impulse_vector = axis2 * normalImpulse;
397 397
398 RigidBodyA.ApplyTorqueImpulse(impulse_vector); 398 RigidBodyA.ApplyTorqueImpulse(impulse_vector);
399 RigidBodyB.ApplyTorqueImpulse(-impulse_vector); 399 RigidBodyB.ApplyTorqueImpulse(-impulse_vector);
400 } 400 }
401 } 401 }
402 } 402 }
403 403
404 //MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html 404 //MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
405 private bool MatrixToEulerXYZ(Matrix mat, out Vector3 xyz) 405 private bool MatrixToEulerXYZ(Matrix mat, out Vector3 xyz)
406 { 406 {
407 // rot = cy*cz -cy*sz sy 407 // rot = cy*cz -cy*sz sy
408 // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx 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 409 // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
410 xyz = new Vector3(); 410 xyz = new Vector3();
411 411
412 if (MathHelper.GetElement(mat, 2) < 1.0f) 412 if (MathHelper.GetElement(mat, 2) < 1.0f)
413 { 413 {
414 if (MathHelper.GetElement(mat, 2) > -1.0f) 414 if (MathHelper.GetElement(mat, 2) > -1.0f)
415 { 415 {
416 xyz.X = (float)Math.Atan2(-MathHelper.GetElement(mat, 5), MathHelper.GetElement(mat, 8)); 416 xyz.X = (float)Math.Atan2(-MathHelper.GetElement(mat, 5), MathHelper.GetElement(mat, 8));
417 xyz.Y = (float)Math.Asin(MathHelper.GetElement(mat, 2)); 417 xyz.Y = (float)Math.Asin(MathHelper.GetElement(mat, 2));
418 xyz.Z = (float)Math.Atan2(-MathHelper.GetElement(mat, 1), MathHelper.GetElement(mat, 0)); 418 xyz.Z = (float)Math.Atan2(-MathHelper.GetElement(mat, 1), MathHelper.GetElement(mat, 0));
419 return true; 419 return true;
420 } 420 }
421 else 421 else
422 { 422 {
423 // WARNING. Not unique. XA - ZA = -atan2(r10,r11) 423 // WARNING. Not unique. XA - ZA = -atan2(r10,r11)
424 xyz.X = -(float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4)); 424 xyz.X = -(float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4));
425 xyz.Y = -(float)Math.PI / 2; 425 xyz.Y = -(float)Math.PI / 2;
426 xyz.Z = 0.0f; 426 xyz.Z = 0.0f;
427 return false; 427 return false;
428 } 428 }
429 } 429 }
430 else 430 else
431 { 431 {
432 // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) 432 // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
433 xyz.X = (float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4)); 433 xyz.X = (float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4));
434 xyz.Y = (float)Math.PI / 2; 434 xyz.Y = (float)Math.PI / 2;
435 xyz.Z = 0.0f; 435 xyz.Z = 0.0f;
436 return false; 436 return false;
437 } 437 }
438 } 438 }
439 } 439 }
440} 440}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs
index 3e35550..02bcf8a 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs
@@ -1,246 +1,246 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX.Dynamics 27namespace XnaDevRu.BulletX.Dynamics
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space 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 31 /// axis defines the orientation of the hinge axis
32 /// </summary> 32 /// </summary>
33 public class HingeConstraint : TypedConstraint 33 public class HingeConstraint : TypedConstraint
34 { 34 {
35 private JacobianEntry[] _jac = new JacobianEntry[3]; //3 orthogonal linear constraints 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 36 private JacobianEntry[] _jacAng = new JacobianEntry[3]; //2 orthogonal angular constraints + 1 for limit/motor
37 37
38 private Vector3 _pivotInA; 38 private Vector3 _pivotInA;
39 private Vector3 _pivotInB; 39 private Vector3 _pivotInB;
40 private Vector3 _axisInA; 40 private Vector3 _axisInA;
41 private Vector3 _axisInB; 41 private Vector3 _axisInB;
42 42
43 private bool _angularOnly; 43 private bool _angularOnly;
44 44
45 private float _motorTargetVelocity; 45 private float _motorTargetVelocity;
46 private float _maxMotorImpulse; 46 private float _maxMotorImpulse;
47 private bool _enableAngularMotor; 47 private bool _enableAngularMotor;
48 48
49 public HingeConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB, Vector3 axisInA, Vector3 axisInB) 49 public HingeConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB, Vector3 axisInA, Vector3 axisInB)
50 : base(rbA, rbB) 50 : base(rbA, rbB)
51 { 51 {
52 _pivotInA = pivotInA; 52 _pivotInA = pivotInA;
53 _pivotInB = pivotInB; 53 _pivotInB = pivotInB;
54 _axisInA = axisInA; 54 _axisInA = axisInA;
55 _axisInB = -axisInB; 55 _axisInB = -axisInB;
56 _angularOnly = false; 56 _angularOnly = false;
57 } 57 }
58 58
59 public HingeConstraint(RigidBody rbA, Vector3 pivotInA, Vector3 axisInA) 59 public HingeConstraint(RigidBody rbA, Vector3 pivotInA, Vector3 axisInA)
60 : base(rbA) 60 : base(rbA)
61 { 61 {
62 _pivotInA = pivotInA; 62 _pivotInA = pivotInA;
63 _pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, pivotInA); 63 _pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, pivotInA);
64 _axisInA = axisInA; 64 _axisInA = axisInA;
65 //fixed axis in worldspace 65 //fixed axis in worldspace
66 _axisInB = MathHelper.TransformNormal(-axisInA, rbA.CenterOfMassTransform); 66 _axisInB = MathHelper.TransformNormal(-axisInA, rbA.CenterOfMassTransform);
67 _angularOnly = false; 67 _angularOnly = false;
68 } 68 }
69 69
70 public HingeConstraint() { } 70 public HingeConstraint() { }
71 71
72 public bool AngularOnly { set { _angularOnly = value; } } 72 public bool AngularOnly { set { _angularOnly = value; } }
73 73
74 public override void BuildJacobian() 74 public override void BuildJacobian()
75 { 75 {
76 AppliedImpulse = 0f; 76 AppliedImpulse = 0f;
77 77
78 Vector3 normal = new Vector3(); 78 Vector3 normal = new Vector3();
79 79
80 if (!_angularOnly) 80 if (!_angularOnly)
81 { 81 {
82 for (int i = 0; i < 3; i++) 82 for (int i = 0; i < 3; i++)
83 { 83 {
84 MathHelper.SetElement(ref normal, i, 1); 84 MathHelper.SetElement(ref normal, i, 1);
85 _jac[i] = new JacobianEntry( 85 _jac[i] = new JacobianEntry(
86 MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), 86 MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
87 MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), 87 MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
88 MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, 88 MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition,
89 MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, 89 MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition,
90 normal, 90 normal,
91 RigidBodyA.InvInertiaDiagLocal, 91 RigidBodyA.InvInertiaDiagLocal,
92 RigidBodyA.InverseMass, 92 RigidBodyA.InverseMass,
93 RigidBodyB.InvInertiaDiagLocal, 93 RigidBodyB.InvInertiaDiagLocal,
94 RigidBodyB.InverseMass); 94 RigidBodyB.InverseMass);
95 MathHelper.SetElement(ref normal, i, 0); 95 MathHelper.SetElement(ref normal, i, 0);
96 } 96 }
97 } 97 }
98 98
99 //calculate two perpendicular jointAxis, orthogonal to hingeAxis 99 //calculate two perpendicular jointAxis, orthogonal to hingeAxis
100 //these two jointAxis require equal angular velocities for both bodies 100 //these two jointAxis require equal angular velocities for both bodies
101 //this is unused for now, it's a todo 101 //this is unused for now, it's a todo
102 Vector3 jointAxisALocal = new Vector3(); 102 Vector3 jointAxisALocal = new Vector3();
103 Vector3 jointAxisBLocal = new Vector3(); 103 Vector3 jointAxisBLocal = new Vector3();
104 MathHelper.PlaneSpace1(_axisInA, ref jointAxisALocal, ref jointAxisBLocal); 104 MathHelper.PlaneSpace1(_axisInA, ref jointAxisALocal, ref jointAxisBLocal);
105 105
106 Vector3 jointAxisA = MathHelper.TransformNormal(jointAxisALocal, RigidBodyA.CenterOfMassTransform); 106 Vector3 jointAxisA = MathHelper.TransformNormal(jointAxisALocal, RigidBodyA.CenterOfMassTransform);
107 Vector3 jointAxisB = MathHelper.TransformNormal(jointAxisBLocal, RigidBodyA.CenterOfMassTransform); 107 Vector3 jointAxisB = MathHelper.TransformNormal(jointAxisBLocal, RigidBodyA.CenterOfMassTransform);
108 Vector3 hingeAxisWorld = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform); 108 Vector3 hingeAxisWorld = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform);
109 109
110 _jacAng[0] = new JacobianEntry(jointAxisA, 110 _jacAng[0] = new JacobianEntry(jointAxisA,
111 MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), 111 MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
112 MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), 112 MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
113 RigidBodyA.InvInertiaDiagLocal, 113 RigidBodyA.InvInertiaDiagLocal,
114 RigidBodyB.InvInertiaDiagLocal); 114 RigidBodyB.InvInertiaDiagLocal);
115 115
116 _jacAng[1] = new JacobianEntry(jointAxisB, 116 _jacAng[1] = new JacobianEntry(jointAxisB,
117 MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), 117 MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
118 MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), 118 MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
119 RigidBodyA.InvInertiaDiagLocal, 119 RigidBodyA.InvInertiaDiagLocal,
120 RigidBodyB.InvInertiaDiagLocal); 120 RigidBodyB.InvInertiaDiagLocal);
121 121
122 _jacAng[2] = new JacobianEntry(hingeAxisWorld, 122 _jacAng[2] = new JacobianEntry(hingeAxisWorld,
123 MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), 123 MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
124 MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), 124 MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
125 RigidBodyA.InvInertiaDiagLocal, 125 RigidBodyA.InvInertiaDiagLocal,
126 RigidBodyB.InvInertiaDiagLocal); 126 RigidBodyB.InvInertiaDiagLocal);
127 } 127 }
128 128
129 public override void SolveConstraint(float timeStep) 129 public override void SolveConstraint(float timeStep)
130 { 130 {
131 Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform); 131 Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform);
132 Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform); 132 Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform);
133 133
134 Vector3 normal = new Vector3(0, 0, 0); 134 Vector3 normal = new Vector3(0, 0, 0);
135 float tau = 0.3f; 135 float tau = 0.3f;
136 float damping = 1f; 136 float damping = 1f;
137 137
138 //linear part 138 //linear part
139 if (!_angularOnly) 139 if (!_angularOnly)
140 { 140 {
141 for (int i = 0; i < 3; i++) 141 for (int i = 0; i < 3; i++)
142 { 142 {
143 if (i == 0) 143 if (i == 0)
144 normal = new Vector3(1, 0, 0); 144 normal = new Vector3(1, 0, 0);
145 else if (i == 1) 145 else if (i == 1)
146 normal = new Vector3(0, 1, 0); 146 normal = new Vector3(0, 1, 0);
147 else 147 else
148 normal = new Vector3(0, 0, 1); 148 normal = new Vector3(0, 0, 1);
149 149
150 float jacDiagABInv = 1f / _jac[i].Diagonal; 150 float jacDiagABInv = 1f / _jac[i].Diagonal;
151 151
152 Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; 152 Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
153 Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; 153 Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;
154 154
155 Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1); 155 Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1);
156 Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2); 156 Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2);
157 Vector3 vel = vel1 - vel2; 157 Vector3 vel = vel1 - vel2;
158 float rel_vel; 158 float rel_vel;
159 rel_vel = Vector3.Dot(normal, vel); 159 rel_vel = Vector3.Dot(normal, vel);
160 //positional error (zeroth order error) 160 //positional error (zeroth order error)
161 float depth = -Vector3.Dot(pivotAInW - pivotBInW, normal); //this is the error projected on the normal 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; 162 float impulse = depth * tau / timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv * damping;
163 AppliedImpulse += impulse; 163 AppliedImpulse += impulse;
164 Vector3 impulse_vector = normal * impulse; 164 Vector3 impulse_vector = normal * impulse;
165 RigidBodyA.ApplyImpulse(impulse_vector, pivotAInW - RigidBodyA.CenterOfMassPosition); 165 RigidBodyA.ApplyImpulse(impulse_vector, pivotAInW - RigidBodyA.CenterOfMassPosition);
166 RigidBodyB.ApplyImpulse(-impulse_vector, pivotBInW - RigidBodyB.CenterOfMassPosition); 166 RigidBodyB.ApplyImpulse(-impulse_vector, pivotBInW - RigidBodyB.CenterOfMassPosition);
167 } 167 }
168 } 168 }
169 //solve angular part 169 //solve angular part
170 // get axes in world space 170 // get axes in world space
171 Vector3 axisA = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform); 171 Vector3 axisA = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform);
172 Vector3 axisB = MathHelper.TransformNormal(_axisInB, RigidBodyB.CenterOfMassTransform); 172 Vector3 axisB = MathHelper.TransformNormal(_axisInB, RigidBodyB.CenterOfMassTransform);
173 173
174 Vector3 angVelA = RigidBodyA.AngularVelocity; 174 Vector3 angVelA = RigidBodyA.AngularVelocity;
175 Vector3 angVelB = RigidBodyB.AngularVelocity; 175 Vector3 angVelB = RigidBodyB.AngularVelocity;
176 Vector3 angVelAroundHingeAxisA = axisA * Vector3.Dot(axisA, angVelA); 176 Vector3 angVelAroundHingeAxisA = axisA * Vector3.Dot(axisA, angVelA);
177 Vector3 angVelAroundHingeAxisB = axisB * Vector3.Dot(axisB, angVelB); 177 Vector3 angVelAroundHingeAxisB = axisB * Vector3.Dot(axisB, angVelB);
178 178
179 Vector3 angAOrthog = angVelA - angVelAroundHingeAxisA; 179 Vector3 angAOrthog = angVelA - angVelAroundHingeAxisA;
180 Vector3 angBOrthog = angVelB - angVelAroundHingeAxisB; 180 Vector3 angBOrthog = angVelB - angVelAroundHingeAxisB;
181 Vector3 velrelOrthog = angAOrthog - angBOrthog; 181 Vector3 velrelOrthog = angAOrthog - angBOrthog;
182 182
183 //solve angular velocity correction 183 //solve angular velocity correction
184 float relaxation = 1f; 184 float relaxation = 1f;
185 float len = velrelOrthog.Length(); 185 float len = velrelOrthog.Length();
186 if (len > 0.00001f) 186 if (len > 0.00001f)
187 { 187 {
188 Vector3 normal2 = Vector3.Normalize(velrelOrthog); 188 Vector3 normal2 = Vector3.Normalize(velrelOrthog);
189 float denom = RigidBodyA.ComputeAngularImpulseDenominator(normal2) + 189 float denom = RigidBodyA.ComputeAngularImpulseDenominator(normal2) +
190 RigidBodyB.ComputeAngularImpulseDenominator(normal2); 190 RigidBodyB.ComputeAngularImpulseDenominator(normal2);
191 // scale for mass and relaxation 191 // scale for mass and relaxation
192 velrelOrthog *= (1f / denom) * 0.9f; 192 velrelOrthog *= (1f / denom) * 0.9f;
193 } 193 }
194 194
195 //solve angular positional correction 195 //solve angular positional correction
196 Vector3 angularError = -Vector3.Cross(axisA, axisB) * (1f / timeStep); 196 Vector3 angularError = -Vector3.Cross(axisA, axisB) * (1f / timeStep);
197 float len2 = angularError.Length(); 197 float len2 = angularError.Length();
198 if (len2 > 0.00001f) 198 if (len2 > 0.00001f)
199 { 199 {
200 Vector3 normal2 = Vector3.Normalize(angularError); 200 Vector3 normal2 = Vector3.Normalize(angularError);
201 float denom2 = RigidBodyA.ComputeAngularImpulseDenominator(normal2) + 201 float denom2 = RigidBodyA.ComputeAngularImpulseDenominator(normal2) +
202 RigidBodyB.ComputeAngularImpulseDenominator(normal2); 202 RigidBodyB.ComputeAngularImpulseDenominator(normal2);
203 angularError *= (1f / denom2) * relaxation; 203 angularError *= (1f / denom2) * relaxation;
204 } 204 }
205 205
206 RigidBodyA.ApplyTorqueImpulse(-velrelOrthog + angularError); 206 RigidBodyA.ApplyTorqueImpulse(-velrelOrthog + angularError);
207 RigidBodyB.ApplyTorqueImpulse(velrelOrthog - angularError); 207 RigidBodyB.ApplyTorqueImpulse(velrelOrthog - angularError);
208 208
209 //apply motor 209 //apply motor
210 if (_enableAngularMotor) 210 if (_enableAngularMotor)
211 { 211 {
212 //todo: add limits too 212 //todo: add limits too
213 Vector3 angularLimit = Vector3.Zero; 213 Vector3 angularLimit = Vector3.Zero;
214 214
215 Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; 215 Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
216 float projRelVel = Vector3.Dot(velrel, axisA); 216 float projRelVel = Vector3.Dot(velrel, axisA);
217 217
218 float desiredMotorVel = _motorTargetVelocity; 218 float desiredMotorVel = _motorTargetVelocity;
219 float motorRelvel = desiredMotorVel - projRelVel; 219 float motorRelvel = desiredMotorVel - projRelVel;
220 220
221 float denom3 = RigidBodyA.ComputeAngularImpulseDenominator(axisA) + 221 float denom3 = RigidBodyA.ComputeAngularImpulseDenominator(axisA) +
222 RigidBodyB.ComputeAngularImpulseDenominator(axisA); 222 RigidBodyB.ComputeAngularImpulseDenominator(axisA);
223 223
224 float unclippedMotorImpulse = (1f / denom3) * motorRelvel; 224 float unclippedMotorImpulse = (1f / denom3) * motorRelvel;
225 //todo: should clip against accumulated impulse 225 //todo: should clip against accumulated impulse
226 float clippedMotorImpulse = unclippedMotorImpulse > _maxMotorImpulse ? _maxMotorImpulse : unclippedMotorImpulse; 226 float clippedMotorImpulse = unclippedMotorImpulse > _maxMotorImpulse ? _maxMotorImpulse : unclippedMotorImpulse;
227 clippedMotorImpulse = clippedMotorImpulse < -_maxMotorImpulse ? -_maxMotorImpulse : clippedMotorImpulse; 227 clippedMotorImpulse = clippedMotorImpulse < -_maxMotorImpulse ? -_maxMotorImpulse : clippedMotorImpulse;
228 Vector3 motorImp = clippedMotorImpulse * axisA; 228 Vector3 motorImp = clippedMotorImpulse * axisA;
229 229
230 RigidBodyA.ApplyTorqueImpulse(motorImp + angularLimit); 230 RigidBodyA.ApplyTorqueImpulse(motorImp + angularLimit);
231 RigidBodyB.ApplyTorqueImpulse(-motorImp - angularLimit); 231 RigidBodyB.ApplyTorqueImpulse(-motorImp - angularLimit);
232 } 232 }
233 } 233 }
234 234
235 public void EnableAngularMotor(bool enableMotor, float targetVelocity, float maxMotorImpulse) 235 public void EnableAngularMotor(bool enableMotor, float targetVelocity, float maxMotorImpulse)
236 { 236 {
237 _enableAngularMotor = enableMotor; 237 _enableAngularMotor = enableMotor;
238 _motorTargetVelocity = targetVelocity; 238 _motorTargetVelocity = targetVelocity;
239 _maxMotorImpulse = maxMotorImpulse; 239 _maxMotorImpulse = maxMotorImpulse;
240 } 240 }
241 241
242 public void UpdateRHS(float timeStep) 242 public void UpdateRHS(float timeStep)
243 { 243 {
244 } 244 }
245 } 245 }
246} 246}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/IConstraintSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/IConstraintSolver.cs
index de25225..a83eaa5 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/IConstraintSolver.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/IConstraintSolver.cs
@@ -1,32 +1,32 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25 25
26namespace XnaDevRu.BulletX.Dynamics 26namespace XnaDevRu.BulletX.Dynamics
27{ 27{
28 public interface IConstraintSolver 28 public interface IConstraintSolver
29 { 29 {
30 float SolveGroup(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo info, IDebugDraw debugDrawer); 30 float SolveGroup(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo info, IDebugDraw debugDrawer);
31 } 31 }
32} 32}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs
index 3b8d8d8..6249998 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs
@@ -1,155 +1,155 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX.Dynamics 27namespace XnaDevRu.BulletX.Dynamics
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// Jacobian entry is an abstraction that allows to describe constraints 30 /// Jacobian entry is an abstraction that allows to describe constraints
31 /// it can be used in combination with a constraint solver 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 32 /// Can be used to relate the effect of an impulse to the constraint error
33 /// </summary> 33 /// </summary>
34 public class JacobianEntry 34 public class JacobianEntry
35 { 35 {
36 private Vector3 _linearJointAxis; 36 private Vector3 _linearJointAxis;
37 private Vector3 _aJ; 37 private Vector3 _aJ;
38 private Vector3 _bJ; 38 private Vector3 _bJ;
39 private Vector3 _0MinvJt; 39 private Vector3 _0MinvJt;
40 private Vector3 _1MinvJt; 40 private Vector3 _1MinvJt;
41 private float _adiag; 41 private float _adiag;
42 42
43 public JacobianEntry() { } 43 public JacobianEntry() { }
44 44
45 //constraint between two different rigidbodies 45 //constraint between two different rigidbodies
46 public JacobianEntry( 46 public JacobianEntry(
47 Matrix world2A, 47 Matrix world2A,
48 Matrix world2B, 48 Matrix world2B,
49 Vector3 relPosA, Vector3 relPosB, 49 Vector3 relPosA, Vector3 relPosB,
50 Vector3 jointAxis, 50 Vector3 jointAxis,
51 Vector3 inertiaInvA, 51 Vector3 inertiaInvA,
52 float massInvA, 52 float massInvA,
53 Vector3 inertiaInvB, 53 Vector3 inertiaInvB,
54 float massInvB) 54 float massInvB)
55 { 55 {
56 _linearJointAxis = jointAxis; 56 _linearJointAxis = jointAxis;
57 _aJ = Vector3.TransformNormal(Vector3.Cross(relPosA, _linearJointAxis), world2A); 57 _aJ = Vector3.TransformNormal(Vector3.Cross(relPosA, _linearJointAxis), world2A);
58 _bJ = Vector3.TransformNormal(Vector3.Cross(relPosB, -_linearJointAxis), world2B); 58 _bJ = Vector3.TransformNormal(Vector3.Cross(relPosB, -_linearJointAxis), world2B);
59 _0MinvJt = inertiaInvA * _aJ; 59 _0MinvJt = inertiaInvA * _aJ;
60 _1MinvJt = inertiaInvB * _bJ; 60 _1MinvJt = inertiaInvB * _bJ;
61 _adiag = massInvA + Vector3.Dot(_0MinvJt, _aJ) + massInvB + Vector3.Dot(_1MinvJt, _bJ); 61 _adiag = massInvA + Vector3.Dot(_0MinvJt, _aJ) + massInvB + Vector3.Dot(_1MinvJt, _bJ);
62 62
63 if (_adiag <= 0.0f) 63 if (_adiag <= 0.0f)
64 throw new BulletException(); 64 throw new BulletException();
65 } 65 }
66 66
67 //angular constraint between two different rigidbodies 67 //angular constraint between two different rigidbodies
68 public JacobianEntry(Vector3 jointAxis, 68 public JacobianEntry(Vector3 jointAxis,
69 Matrix world2A, 69 Matrix world2A,
70 Matrix world2B, 70 Matrix world2B,
71 Vector3 inertiaInvA, 71 Vector3 inertiaInvA,
72 Vector3 inertiaInvB) 72 Vector3 inertiaInvB)
73 { 73 {
74 _linearJointAxis = new Vector3(); 74 _linearJointAxis = new Vector3();
75 _aJ = Vector3.TransformNormal(jointAxis, world2A); 75 _aJ = Vector3.TransformNormal(jointAxis, world2A);
76 _bJ = Vector3.TransformNormal(-jointAxis, world2B); 76 _bJ = Vector3.TransformNormal(-jointAxis, world2B);
77 _0MinvJt = inertiaInvA * _aJ; 77 _0MinvJt = inertiaInvA * _aJ;
78 _1MinvJt = inertiaInvB * _bJ; 78 _1MinvJt = inertiaInvB * _bJ;
79 _adiag = Vector3.Dot(_0MinvJt, _aJ) + Vector3.Dot(_1MinvJt, _bJ); 79 _adiag = Vector3.Dot(_0MinvJt, _aJ) + Vector3.Dot(_1MinvJt, _bJ);
80 80
81 if (_adiag <= 0.0f) 81 if (_adiag <= 0.0f)
82 throw new BulletException(); 82 throw new BulletException();
83 } 83 }
84 84
85 //angular constraint between two different rigidbodies 85 //angular constraint between two different rigidbodies
86 public JacobianEntry(Vector3 axisInA, 86 public JacobianEntry(Vector3 axisInA,
87 Vector3 axisInB, 87 Vector3 axisInB,
88 Vector3 inertiaInvA, 88 Vector3 inertiaInvA,
89 Vector3 inertiaInvB) 89 Vector3 inertiaInvB)
90 { 90 {
91 _linearJointAxis = new Vector3(); 91 _linearJointAxis = new Vector3();
92 _aJ = axisInA; 92 _aJ = axisInA;
93 _bJ = -axisInB; 93 _bJ = -axisInB;
94 _0MinvJt = inertiaInvA * _aJ; 94 _0MinvJt = inertiaInvA * _aJ;
95 _1MinvJt = inertiaInvB * _bJ; 95 _1MinvJt = inertiaInvB * _bJ;
96 _adiag = Vector3.Dot(_0MinvJt, _aJ) + Vector3.Dot(_1MinvJt, _bJ); 96 _adiag = Vector3.Dot(_0MinvJt, _aJ) + Vector3.Dot(_1MinvJt, _bJ);
97 97
98 if (_adiag <= 0.0f) 98 if (_adiag <= 0.0f)
99 throw new BulletException(); 99 throw new BulletException();
100 } 100 }
101 101
102 //constraint on one rigidbody 102 //constraint on one rigidbody
103 public JacobianEntry( 103 public JacobianEntry(
104 Matrix world2A, 104 Matrix world2A,
105 Vector3 rel_pos1, Vector3 rel_pos2, 105 Vector3 rel_pos1, Vector3 rel_pos2,
106 Vector3 jointAxis, 106 Vector3 jointAxis,
107 Vector3 inertiaInvA, 107 Vector3 inertiaInvA,
108 float massInvA) 108 float massInvA)
109 { 109 {
110 _linearJointAxis = jointAxis; 110 _linearJointAxis = jointAxis;
111 _aJ = Vector3.TransformNormal(Vector3.Cross(rel_pos1, jointAxis), world2A); 111 _aJ = Vector3.TransformNormal(Vector3.Cross(rel_pos1, jointAxis), world2A);
112 _bJ = Vector3.TransformNormal(Vector3.Cross(rel_pos2, -jointAxis), world2A); 112 _bJ = Vector3.TransformNormal(Vector3.Cross(rel_pos2, -jointAxis), world2A);
113 _0MinvJt = inertiaInvA * _aJ; 113 _0MinvJt = inertiaInvA * _aJ;
114 _1MinvJt = new Vector3(); 114 _1MinvJt = new Vector3();
115 _adiag = massInvA + Vector3.Dot(_0MinvJt, _aJ); 115 _adiag = massInvA + Vector3.Dot(_0MinvJt, _aJ);
116 116
117 if (_adiag <= 0.0f) 117 if (_adiag <= 0.0f)
118 throw new BulletException(); 118 throw new BulletException();
119 } 119 }
120 120
121 public float Diagonal { get { return _adiag; } } 121 public float Diagonal { get { return _adiag; } }
122 122
123 // for two constraints on the same rigidbody (for example vehicle friction) 123 // for two constraints on the same rigidbody (for example vehicle friction)
124 public float GetNonDiagonal(JacobianEntry jacB, float massInvA) 124 public float GetNonDiagonal(JacobianEntry jacB, float massInvA)
125 { 125 {
126 float lin = massInvA * Vector3.Dot(_linearJointAxis, jacB._linearJointAxis); 126 float lin = massInvA * Vector3.Dot(_linearJointAxis, jacB._linearJointAxis);
127 float ang = Vector3.Dot(_0MinvJt, jacB._aJ); 127 float ang = Vector3.Dot(_0MinvJt, jacB._aJ);
128 return lin + ang; 128 return lin + ang;
129 } 129 }
130 130
131 // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies) 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) 132 public float GetNonDiagonal(JacobianEntry jacB, float massInvA, float massInvB)
133 { 133 {
134 Vector3 lin = _linearJointAxis * jacB._linearJointAxis; 134 Vector3 lin = _linearJointAxis * jacB._linearJointAxis;
135 Vector3 ang0 = _0MinvJt * jacB._aJ; 135 Vector3 ang0 = _0MinvJt * jacB._aJ;
136 Vector3 ang1 = _1MinvJt * jacB._bJ; 136 Vector3 ang1 = _1MinvJt * jacB._bJ;
137 Vector3 lin0 = massInvA * lin; 137 Vector3 lin0 = massInvA * lin;
138 Vector3 lin1 = massInvB * lin; 138 Vector3 lin1 = massInvB * lin;
139 Vector3 sum = ang0 + ang1 + lin0 + lin1; 139 Vector3 sum = ang0 + ang1 + lin0 + lin1;
140 return sum.X + sum.Y + sum.Z; 140 return sum.X + sum.Y + sum.Z;
141 } 141 }
142 142
143 public float GetRelativeVelocity(Vector3 linvelA, Vector3 angvelA, Vector3 linvelB, Vector3 angvelB) 143 public float GetRelativeVelocity(Vector3 linvelA, Vector3 angvelA, Vector3 linvelB, Vector3 angvelB)
144 { 144 {
145 Vector3 linrel = linvelA - linvelB; 145 Vector3 linrel = linvelA - linvelB;
146 Vector3 angvela = angvelA * _aJ; 146 Vector3 angvela = angvelA * _aJ;
147 Vector3 angvelb = angvelB * _bJ; 147 Vector3 angvelb = angvelB * _bJ;
148 linrel *= _linearJointAxis; 148 linrel *= _linearJointAxis;
149 angvela += angvelb; 149 angvela += angvelb;
150 angvela += linrel; 150 angvela += linrel;
151 float rel_vel2 = angvela.X + angvela.Y + angvela.Z; 151 float rel_vel2 = angvela.X + angvela.Y + angvela.Z;
152 return rel_vel2 + float.Epsilon; 152 return rel_vel2 + float.Epsilon;
153 } 153 }
154 } 154 }
155} 155}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs
index d4206ea..8ffd3d5 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs
@@ -1,157 +1,157 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX.Dynamics 27namespace XnaDevRu.BulletX.Dynamics
28{ 28{
29 public class ConstraintSetting 29 public class ConstraintSetting
30 { 30 {
31 private float _tau, _damping; 31 private float _tau, _damping;
32 32
33 public ConstraintSetting() 33 public ConstraintSetting()
34 { 34 {
35 _tau = 0.3f; 35 _tau = 0.3f;
36 _damping = 1.0f; 36 _damping = 1.0f;
37 } 37 }
38 38
39 public float Damping 39 public float Damping
40 { 40 {
41 get { return _damping; } 41 get { return _damping; }
42 set { _damping = value; } 42 set { _damping = value; }
43 } 43 }
44 44
45 public float Tau 45 public float Tau
46 { 46 {
47 get { return _tau; } 47 get { return _tau; }
48 set { _tau = value; } 48 set { _tau = value; }
49 } 49 }
50 } 50 }
51 51
52 public class Point2PointConstraint : TypedConstraint 52 public class Point2PointConstraint : TypedConstraint
53 { 53 {
54 private JacobianEntry[] _jacobian; 54 private JacobianEntry[] _jacobian;
55 private Vector3 _pivotInA, _pivotInB; 55 private Vector3 _pivotInA, _pivotInB;
56 56
57 private ConstraintSetting _setting = new ConstraintSetting(); 57 private ConstraintSetting _setting = new ConstraintSetting();
58 58
59 public Point2PointConstraint() 59 public Point2PointConstraint()
60 { 60 {
61 _jacobian = new JacobianEntry[3]; 61 _jacobian = new JacobianEntry[3];
62 } 62 }
63 63
64 public Point2PointConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB) 64 public Point2PointConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB)
65 : base(rbA, rbB) 65 : base(rbA, rbB)
66 { 66 {
67 _jacobian = new JacobianEntry[3]; 67 _jacobian = new JacobianEntry[3];
68 68
69 _pivotInA = pivotInA; 69 _pivotInA = pivotInA;
70 _pivotInB = pivotInB; 70 _pivotInB = pivotInB;
71 } 71 }
72 72
73 public Point2PointConstraint(RigidBody rbA, Vector3 pivotInA) 73 public Point2PointConstraint(RigidBody rbA, Vector3 pivotInA)
74 : base(rbA) 74 : base(rbA)
75 { 75 {
76 _jacobian = new JacobianEntry[3]; 76 _jacobian = new JacobianEntry[3];
77 77
78 _pivotInA = pivotInA; 78 _pivotInA = pivotInA;
79 _pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, _pivotInA); 79 _pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, _pivotInA);
80 } 80 }
81 81
82 public ConstraintSetting Settings { get { return _setting; } set { _setting = value; } } 82 public ConstraintSetting Settings { get { return _setting; } set { _setting = value; } }
83 83
84 public Vector3 PivotInA 84 public Vector3 PivotInA
85 { 85 {
86 set 86 set
87 { 87 {
88 _pivotInA = value; 88 _pivotInA = value;
89 } 89 }
90 } 90 }
91 91
92 public Vector3 PivotInB 92 public Vector3 PivotInB
93 { 93 {
94 set 94 set
95 { 95 {
96 _pivotInB = value; 96 _pivotInB = value;
97 } 97 }
98 } 98 }
99 99
100 public override void BuildJacobian() 100 public override void BuildJacobian()
101 { 101 {
102 Vector3 normal = new Vector3(); 102 Vector3 normal = new Vector3();
103 103
104 for (int i = 0; i < 3; i++) 104 for (int i = 0; i < 3; i++)
105 { 105 {
106 MathHelper.SetElement(ref normal, i, 1); 106 MathHelper.SetElement(ref normal, i, 1);
107 _jacobian[i] = new JacobianEntry( 107 _jacobian[i] = new JacobianEntry(
108 MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), 108 MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
109 MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), 109 MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
110 MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, 110 MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition,
111 MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, 111 MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition,
112 normal, 112 normal,
113 RigidBodyA.InvInertiaDiagLocal, 113 RigidBodyA.InvInertiaDiagLocal,
114 RigidBodyA.InverseMass, 114 RigidBodyA.InverseMass,
115 RigidBodyB.InvInertiaDiagLocal, 115 RigidBodyB.InvInertiaDiagLocal,
116 RigidBodyB.InverseMass 116 RigidBodyB.InverseMass
117 ); 117 );
118 MathHelper.SetElement(ref normal, i, 0); 118 MathHelper.SetElement(ref normal, i, 0);
119 } 119 }
120 } 120 }
121 121
122 public override void SolveConstraint(float timeStep) 122 public override void SolveConstraint(float timeStep)
123 { 123 {
124 Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform); 124 Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform);
125 Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform); 125 Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform);
126 126
127 Vector3 normal = new Vector3(); 127 Vector3 normal = new Vector3();
128 128
129 for (int i = 0; i < 3; i++) 129 for (int i = 0; i < 3; i++)
130 { 130 {
131 MathHelper.SetElement(ref normal, i, 1); 131 MathHelper.SetElement(ref normal, i, 1);
132 132
133 float jacDiagABInv = 1.0f / _jacobian[i].Diagonal; 133 float jacDiagABInv = 1.0f / _jacobian[i].Diagonal;
134 134
135 Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; 135 Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
136 Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; 136 Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;
137 137
138 Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1); 138 Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1);
139 Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2); 139 Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2);
140 140
141 Vector3 vel = vel1 - vel2; 141 Vector3 vel = vel1 - vel2;
142 142
143 float rel_vel = Vector3.Dot(normal, vel); 143 float rel_vel = Vector3.Dot(normal, vel);
144 float depth = -Vector3.Dot((pivotAInW - pivotBInW), normal); 144 float depth = -Vector3.Dot((pivotAInW - pivotBInW), normal);
145 145
146 float impulse = depth * _setting.Tau / timeStep * jacDiagABInv - _setting.Damping * rel_vel * jacDiagABInv; 146 float impulse = depth * _setting.Tau / timeStep * jacDiagABInv - _setting.Damping * rel_vel * jacDiagABInv;
147 AppliedImpulse += impulse; 147 AppliedImpulse += impulse;
148 Vector3 impulseVector = normal * impulse; 148 Vector3 impulseVector = normal * impulse;
149 149
150 RigidBodyA.ApplyImpulse(impulseVector, pivotAInW - RigidBodyA.CenterOfMassPosition); 150 RigidBodyA.ApplyImpulse(impulseVector, pivotAInW - RigidBodyA.CenterOfMassPosition);
151 RigidBodyB.ApplyImpulse(-impulseVector, pivotBInW - RigidBodyB.CenterOfMassPosition); 151 RigidBodyB.ApplyImpulse(-impulseVector, pivotBInW - RigidBodyB.CenterOfMassPosition);
152 152
153 MathHelper.SetElement(ref normal, i, 0); 153 MathHelper.SetElement(ref normal, i, 0);
154 } 154 }
155 } 155 }
156 } 156 }
157} \ No newline at end of file 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
index 4d5e93d..f275550 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs
@@ -1,915 +1,915 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX.Dynamics 27namespace XnaDevRu.BulletX.Dynamics
28{ 28{
29 [Flags] 29 [Flags]
30 public enum SolverMode 30 public enum SolverMode
31 { 31 {
32 None = 0, 32 None = 0,
33 RandomizeOrder = 1, 33 RandomizeOrder = 1,
34 FrictionSeperate = 2, 34 FrictionSeperate = 2,
35 UseWarmstarting = 4, 35 UseWarmstarting = 4,
36 CacheFriendly = 8, 36 CacheFriendly = 8,
37 } 37 }
38 38
39 public class SequentialImpulseConstraintSolver : IConstraintSolver 39 public class SequentialImpulseConstraintSolver : IConstraintSolver
40 { 40 {
41 private static int _totalContactPoints = 0; 41 private static int _totalContactPoints = 0;
42 42
43 private SolverMode _solverMode; 43 private SolverMode _solverMode;
44 private int _totalCpd = 0; 44 private int _totalCpd = 0;
45 private ContactSolverFunc[,] _contactDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType]; 45 private ContactSolverFunc[,] _contactDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType];
46 private ContactSolverFunc[,] _frictionDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType]; 46 private ContactSolverFunc[,] _frictionDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType];
47 47
48 private float _penetrationResolveFactor = 0.9f; 48 private float _penetrationResolveFactor = 0.9f;
49 private List<SolverBody> _tmpSolverBodyPool = new List<SolverBody>(); 49 private List<SolverBody> _tmpSolverBodyPool = new List<SolverBody>();
50 private List<SolverConstraint> _tmpSolverConstraintPool = new List<SolverConstraint>(); 50 private List<SolverConstraint> _tmpSolverConstraintPool = new List<SolverConstraint>();
51 private List<SolverConstraint> _tmpSolverFrictionConstraintPool = new List<SolverConstraint>(); 51 private List<SolverConstraint> _tmpSolverFrictionConstraintPool = new List<SolverConstraint>();
52 52
53 private const int _sequentialImpulseMaxSolverPoints = 16384; 53 private const int _sequentialImpulseMaxSolverPoints = 16384;
54 private static OrderIndex[] _order = new OrderIndex[SequentialImpulseMaxSolverPoints]; 54 private static OrderIndex[] _order = new OrderIndex[SequentialImpulseMaxSolverPoints];
55 private static long _seed2 = 0; 55 private static long _seed2 = 0;
56 56
57 public SequentialImpulseConstraintSolver() 57 public SequentialImpulseConstraintSolver()
58 { 58 {
59 _solverMode = SolverMode.RandomizeOrder | SolverMode.CacheFriendly; 59 _solverMode = SolverMode.RandomizeOrder | SolverMode.CacheFriendly;
60 PersistentManifold.ContactDestroyedCallback = MyContactDestroyedCallback; 60 PersistentManifold.ContactDestroyedCallback = MyContactDestroyedCallback;
61 61
62 //initialize default friction/contact funcs 62 //initialize default friction/contact funcs
63 int i, j; 63 int i, j;
64 for (i = 0; i < (int)ContactSolverType.MaxContactSolverType; i++) 64 for (i = 0; i < (int)ContactSolverType.MaxContactSolverType; i++)
65 for (j = 0; j < (int)ContactSolverType.MaxContactSolverType; j++) 65 for (j = 0; j < (int)ContactSolverType.MaxContactSolverType; j++)
66 { 66 {
67 67
68 _contactDispatch[i, j] = ContactConstraint.ResolveSingleCollision; 68 _contactDispatch[i, j] = ContactConstraint.ResolveSingleCollision;
69 _frictionDispatch[i, j] = ContactConstraint.ResolveSingleFriction; 69 _frictionDispatch[i, j] = ContactConstraint.ResolveSingleFriction;
70 } 70 }
71 } 71 }
72 72
73 public SolverMode SolverMode { get { return _solverMode; } set { _solverMode = value; } } 73 public SolverMode SolverMode { get { return _solverMode; } set { _solverMode = value; } }
74 public static int SequentialImpulseMaxSolverPoints { get { return _sequentialImpulseMaxSolverPoints; } } 74 public static int SequentialImpulseMaxSolverPoints { get { return _sequentialImpulseMaxSolverPoints; } }
75 protected static OrderIndex[] Order { get { return _order; } set { _order = value; } } 75 protected static OrderIndex[] Order { get { return _order; } set { _order = value; } }
76 public static long RandSeed { get { return _seed2; } set { _seed2 = value; } } 76 public static long RandSeed { get { return _seed2; } set { _seed2 = value; } }
77 77
78 ///<summary> 78 ///<summary>
79 /// Advanced: Override the default contact solving function for contacts, for certain types of rigidbody 79 /// Advanced: Override the default contact solving function for contacts, for certain types of rigidbody
80 /// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType 80 /// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
81 ///</summary> 81 ///</summary>
82 public void SetContactSolverFunc(ContactSolverFunc func, int typeA, int typeB) 82 public void SetContactSolverFunc(ContactSolverFunc func, int typeA, int typeB)
83 { 83 {
84 _contactDispatch[typeA, typeB] = func; 84 _contactDispatch[typeA, typeB] = func;
85 } 85 }
86 86
87 /// <summary> 87 /// <summary>
88 /// Advanced: Override the default friction solving function for contacts, for certain types of rigidbody 88 /// Advanced: Override the default friction solving function for contacts, for certain types of rigidbody
89 /// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType 89 /// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
90 ///</summary> 90 ///</summary>
91 public void SetFrictionSolverFunc(ContactSolverFunc func, int typeA, int typeB) 91 public void SetFrictionSolverFunc(ContactSolverFunc func, int typeA, int typeB)
92 { 92 {
93 _frictionDispatch[typeA, typeB] = func; 93 _frictionDispatch[typeA, typeB] = func;
94 } 94 }
95 95
96 protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) 96 protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
97 { 97 {
98 float maxImpulse = 0; 98 float maxImpulse = 0;
99 99
100 Vector3 color = new Vector3(0, 1, 0); 100 Vector3 color = new Vector3(0, 1, 0);
101 if (cp.Distance <= 0) 101 if (cp.Distance <= 0)
102 { 102 {
103 if (iter == 0) 103 if (iter == 0)
104 if(debugDraw != null) 104 if(debugDraw != null)
105 debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color); 105 debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color);
106 106
107 ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; 107 ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
108 float impulse = cpd.ContactSolverFunc( 108 float impulse = cpd.ContactSolverFunc(
109 bodyA, bodyB, 109 bodyA, bodyB,
110 cp, 110 cp,
111 info); 111 info);
112 112
113 if (maxImpulse < impulse) 113 if (maxImpulse < impulse)
114 maxImpulse = impulse; 114 maxImpulse = impulse;
115 } 115 }
116 return maxImpulse; 116 return maxImpulse;
117 } 117 }
118 118
119 protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter) 119 protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter)
120 { 120 {
121 return Solve(bodyA, bodyB, cp, info, iter, null); 121 return Solve(bodyA, bodyB, cp, info, iter, null);
122 } 122 }
123 123
124 protected float SolveCombinedContactFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) 124 protected float SolveCombinedContactFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
125 { 125 {
126 float maxImpulse = 0; 126 float maxImpulse = 0;
127 127
128 Vector3 color = new Vector3(0, 1, 0); 128 Vector3 color = new Vector3(0, 1, 0);
129 if (cp.Distance <= 0) 129 if (cp.Distance <= 0)
130 { 130 {
131 if (iter == 0) 131 if (iter == 0)
132 if (debugDraw != null) 132 if (debugDraw != null)
133 debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color); 133 debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color);
134 134
135 float impulse = ContactConstraint.ResolveSingleCollisionCombined( 135 float impulse = ContactConstraint.ResolveSingleCollisionCombined(
136 bodyA, bodyB, 136 bodyA, bodyB,
137 cp, 137 cp,
138 info); 138 info);
139 139
140 if (maxImpulse < impulse) 140 if (maxImpulse < impulse)
141 maxImpulse = impulse; 141 maxImpulse = impulse;
142 } 142 }
143 return maxImpulse; 143 return maxImpulse;
144 } 144 }
145 145
146 protected float SolveFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) 146 protected float SolveFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
147 { 147 {
148 Vector3 color = new Vector3(0, 1, 0); 148 Vector3 color = new Vector3(0, 1, 0);
149 149
150 if (cp.Distance <= 0) 150 if (cp.Distance <= 0)
151 { 151 {
152 152
153 ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; 153 ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
154 cpd.FrictionSolverFunc( 154 cpd.FrictionSolverFunc(
155 bodyA, bodyB, 155 bodyA, bodyB,
156 cp, 156 cp,
157 info); 157 info);
158 } 158 }
159 return 0; 159 return 0;
160 } 160 }
161 161
162 protected void PrepareConstraints(PersistentManifold manifold, ContactSolverInfo info) 162 protected void PrepareConstraints(PersistentManifold manifold, ContactSolverInfo info)
163 { 163 {
164 RigidBody body0 = manifold.BodyA as RigidBody; 164 RigidBody body0 = manifold.BodyA as RigidBody;
165 RigidBody body1 = manifold.BodyB as RigidBody; 165 RigidBody body1 = manifold.BodyB as RigidBody;
166 166
167 167
168 //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop 168 //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
169 { 169 {
170 manifold.RefreshContactPoints(body0.CenterOfMassTransform, body1.CenterOfMassTransform); 170 manifold.RefreshContactPoints(body0.CenterOfMassTransform, body1.CenterOfMassTransform);
171 171
172 int numpoints = manifold.ContactsCount; 172 int numpoints = manifold.ContactsCount;
173 173
174 _totalContactPoints += numpoints; 174 _totalContactPoints += numpoints;
175 175
176 Vector3 color = new Vector3(0, 1, 0); 176 Vector3 color = new Vector3(0, 1, 0);
177 for (int i = 0; i < numpoints; i++) 177 for (int i = 0; i < numpoints; i++)
178 { 178 {
179 ManifoldPoint cp = manifold.GetContactPoint(i); 179 ManifoldPoint cp = manifold.GetContactPoint(i);
180 if (cp.Distance <= 0) 180 if (cp.Distance <= 0)
181 { 181 {
182 Vector3 pos1 = cp.PositionWorldOnA; 182 Vector3 pos1 = cp.PositionWorldOnA;
183 Vector3 pos2 = cp.PositionWorldOnB; 183 Vector3 pos2 = cp.PositionWorldOnB;
184 184
185 Vector3 rel_pos1 = pos1 - body0.CenterOfMassPosition; 185 Vector3 rel_pos1 = pos1 - body0.CenterOfMassPosition;
186 Vector3 rel_pos2 = pos2 - body1.CenterOfMassPosition; 186 Vector3 rel_pos2 = pos2 - body1.CenterOfMassPosition;
187 187
188 188
189 //this jacobian entry is re-used for all iterations 189 //this jacobian entry is re-used for all iterations
190 JacobianEntry jac = new JacobianEntry(MatrixOperations.Transpose(body0.CenterOfMassTransform), 190 JacobianEntry jac = new JacobianEntry(MatrixOperations.Transpose(body0.CenterOfMassTransform),
191 MatrixOperations.Transpose(body1.CenterOfMassTransform), 191 MatrixOperations.Transpose(body1.CenterOfMassTransform),
192 rel_pos1, rel_pos2, cp.NormalWorldOnB, body0.InvInertiaDiagLocal, body0.InverseMass, 192 rel_pos1, rel_pos2, cp.NormalWorldOnB, body0.InvInertiaDiagLocal, body0.InverseMass,
193 body1.InvInertiaDiagLocal, body1.InverseMass); 193 body1.InvInertiaDiagLocal, body1.InverseMass);
194 194
195 float jacDiagAB = jac.Diagonal; 195 float jacDiagAB = jac.Diagonal;
196 196
197 ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; 197 ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
198 if (cpd != null) 198 if (cpd != null)
199 { 199 {
200 //might be invalid 200 //might be invalid
201 cpd.PersistentLifeTime++; 201 cpd.PersistentLifeTime++;
202 if (cpd.PersistentLifeTime != cp.LifeTime) 202 if (cpd.PersistentLifeTime != cp.LifeTime)
203 { 203 {
204 //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime()); 204 //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
205 cpd = new ConstraintPersistentData(); 205 cpd = new ConstraintPersistentData();
206 cpd.PersistentLifeTime = cp.LifeTime; 206 cpd.PersistentLifeTime = cp.LifeTime;
207 207
208 } 208 }
209 } 209 }
210 else 210 else
211 { 211 {
212 212
213 cpd = new ConstraintPersistentData(); 213 cpd = new ConstraintPersistentData();
214 _totalCpd++; 214 _totalCpd++;
215 //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd); 215 //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
216 cp.UserPersistentData = cpd; 216 cp.UserPersistentData = cpd;
217 cpd.PersistentLifeTime = cp.LifeTime; 217 cpd.PersistentLifeTime = cp.LifeTime;
218 //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime()); 218 //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
219 219
220 } 220 }
221 if (cpd == null) 221 if (cpd == null)
222 throw new BulletException(); 222 throw new BulletException();
223 223
224 cpd.JacDiagABInv = 1f / jacDiagAB; 224 cpd.JacDiagABInv = 1f / jacDiagAB;
225 225
226 //Dependent on Rigidbody A and B types, fetch the contact/friction response func 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... 227 //perhaps do a similar thing for friction/restutution combiner funcs...
228 228
229 cpd.FrictionSolverFunc = _frictionDispatch[(int)body0.FrictionSolverType, (int)body1.FrictionSolverType]; 229 cpd.FrictionSolverFunc = _frictionDispatch[(int)body0.FrictionSolverType, (int)body1.FrictionSolverType];
230 cpd.ContactSolverFunc = _contactDispatch[(int)body0.ContactSolverType, (int)body1.ContactSolverType]; 230 cpd.ContactSolverFunc = _contactDispatch[(int)body0.ContactSolverType, (int)body1.ContactSolverType];
231 231
232 Vector3 vel1 = body0.GetVelocityInLocalPoint(rel_pos1); 232 Vector3 vel1 = body0.GetVelocityInLocalPoint(rel_pos1);
233 Vector3 vel2 = body1.GetVelocityInLocalPoint(rel_pos2); 233 Vector3 vel2 = body1.GetVelocityInLocalPoint(rel_pos2);
234 Vector3 vel = vel1 - vel2; 234 Vector3 vel = vel1 - vel2;
235 float rel_vel; 235 float rel_vel;
236 rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel); 236 rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel);
237 237
238 float combinedRestitution = cp.CombinedRestitution; 238 float combinedRestitution = cp.CombinedRestitution;
239 239
240 cpd.Penetration = cp.Distance; 240 cpd.Penetration = cp.Distance;
241 cpd.Friction = cp.CombinedFriction; 241 cpd.Friction = cp.CombinedFriction;
242 cpd.Restitution = RestitutionCurve(rel_vel, combinedRestitution); 242 cpd.Restitution = RestitutionCurve(rel_vel, combinedRestitution);
243 if (cpd.Restitution < 0f) 243 if (cpd.Restitution < 0f)
244 { 244 {
245 cpd.Restitution = 0.0f; 245 cpd.Restitution = 0.0f;
246 246
247 }; 247 };
248 248
249 //restitution and penetration work in same direction so 249 //restitution and penetration work in same direction so
250 //rel_vel 250 //rel_vel
251 251
252 float penVel = -cpd.Penetration / info.TimeStep; 252 float penVel = -cpd.Penetration / info.TimeStep;
253 253
254 if (cpd.Restitution > penVel) 254 if (cpd.Restitution > penVel)
255 { 255 {
256 cpd.Penetration = 0; 256 cpd.Penetration = 0;
257 } 257 }
258 258
259 259
260 float relaxation = info.Damping; 260 float relaxation = info.Damping;
261 if ((_solverMode & SolverMode.UseWarmstarting) != 0) 261 if ((_solverMode & SolverMode.UseWarmstarting) != 0)
262 { 262 {
263 cpd.AppliedImpulse *= relaxation; 263 cpd.AppliedImpulse *= relaxation;
264 } 264 }
265 else 265 else
266 { 266 {
267 cpd.AppliedImpulse = 0f; 267 cpd.AppliedImpulse = 0f;
268 } 268 }
269 269
270 //for friction 270 //for friction
271 cpd.PreviousAppliedImpulse = cpd.AppliedImpulse; 271 cpd.PreviousAppliedImpulse = cpd.AppliedImpulse;
272 272
273 //re-calculate friction direction every frame, todo: check if this is really needed 273 //re-calculate friction direction every frame, todo: check if this is really needed
274 Vector3 fwta = cpd.FrictionWorldTangentialA; 274 Vector3 fwta = cpd.FrictionWorldTangentialA;
275 Vector3 fwtb = cpd.FrictionWorldTangentialB; 275 Vector3 fwtb = cpd.FrictionWorldTangentialB;
276 MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref fwta, ref fwtb); 276 MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref fwta, ref fwtb);
277 cpd.FrictionWorldTangentialA = fwta; 277 cpd.FrictionWorldTangentialA = fwta;
278 cpd.FrictionWorldTangentialB = fwtb; 278 cpd.FrictionWorldTangentialB = fwtb;
279 279
280 cpd.AccumulatedTangentImpulseA = 0; 280 cpd.AccumulatedTangentImpulseA = 0;
281 cpd.AccumulatedTangentImpulseB = 0; 281 cpd.AccumulatedTangentImpulseB = 0;
282 float denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialA); 282 float denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialA);
283 float denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialA); 283 float denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialA);
284 float denom = relaxation / (denom0 + denom1); 284 float denom = relaxation / (denom0 + denom1);
285 cpd.JacDiagABInvTangentA = denom; 285 cpd.JacDiagABInvTangentA = denom;
286 286
287 287
288 denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialB); 288 denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialB);
289 denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialB); 289 denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialB);
290 denom = relaxation / (denom0 + denom1); 290 denom = relaxation / (denom0 + denom1);
291 cpd.JacDiagABInvTangentB = denom; 291 cpd.JacDiagABInvTangentB = denom;
292 292
293 Vector3 totalImpulse = cp.NormalWorldOnB * cpd.AppliedImpulse; 293 Vector3 totalImpulse = cp.NormalWorldOnB * cpd.AppliedImpulse;
294 294
295 { 295 {
296 Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); 296 Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB);
297 cpd.AngularComponentA = Vector3.TransformNormal(torqueAxis0, body0.InvInertiaTensorWorld); 297 cpd.AngularComponentA = Vector3.TransformNormal(torqueAxis0, body0.InvInertiaTensorWorld);
298 Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); 298 Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB);
299 cpd.AngularComponentB = Vector3.TransformNormal(torqueAxis1, body1.InvInertiaTensorWorld); 299 cpd.AngularComponentB = Vector3.TransformNormal(torqueAxis1, body1.InvInertiaTensorWorld);
300 } 300 }
301 { 301 {
302 Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialA); 302 Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialA);
303 cpd.FrictionAngularComponent0A = Vector3.TransformNormal(ftorqueAxis0, body0.InvInertiaTensorWorld); 303 cpd.FrictionAngularComponent0A = Vector3.TransformNormal(ftorqueAxis0, body0.InvInertiaTensorWorld);
304 } 304 }
305 { 305 {
306 Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialB); 306 Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialB);
307 cpd.FrictionAngularComponent1A = Vector3.TransformNormal(ftorqueAxis1, body0.InvInertiaTensorWorld); 307 cpd.FrictionAngularComponent1A = Vector3.TransformNormal(ftorqueAxis1, body0.InvInertiaTensorWorld);
308 } 308 }
309 { 309 {
310 Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialA); 310 Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialA);
311 cpd.FrictionAngularComponent0B = Vector3.TransformNormal(ftorqueAxis0, body1.InvInertiaTensorWorld); 311 cpd.FrictionAngularComponent0B = Vector3.TransformNormal(ftorqueAxis0, body1.InvInertiaTensorWorld);
312 } 312 }
313 { 313 {
314 Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialB); 314 Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialB);
315 cpd.FrictionAngularComponent1B = Vector3.TransformNormal(ftorqueAxis1, body1.InvInertiaTensorWorld); 315 cpd.FrictionAngularComponent1B = Vector3.TransformNormal(ftorqueAxis1, body1.InvInertiaTensorWorld);
316 } 316 }
317 317
318 318
319 //apply previous frames impulse on both bodies 319 //apply previous frames impulse on both bodies
320 body0.ApplyImpulse(totalImpulse, rel_pos1); 320 body0.ApplyImpulse(totalImpulse, rel_pos1);
321 body1.ApplyImpulse(-totalImpulse, rel_pos2); 321 body1.ApplyImpulse(-totalImpulse, rel_pos2);
322 } 322 }
323 } 323 }
324 } 324 }
325 } 325 }
326 326
327 private bool MyContactDestroyedCallback(object userPersistentData) 327 private bool MyContactDestroyedCallback(object userPersistentData)
328 { 328 {
329 if (userPersistentData == null) 329 if (userPersistentData == null)
330 throw new BulletException(); 330 throw new BulletException();
331 ConstraintPersistentData cpd = userPersistentData as ConstraintPersistentData; 331 ConstraintPersistentData cpd = userPersistentData as ConstraintPersistentData;
332 _totalCpd--; 332 _totalCpd--;
333 return true; 333 return true;
334 } 334 }
335 335
336 private float RestitutionCurve(float relVel, float restitution) 336 private float RestitutionCurve(float relVel, float restitution)
337 { 337 {
338 float rest = restitution * -relVel; 338 float rest = restitution * -relVel;
339 return rest; 339 return rest;
340 } 340 }
341 341
342 //velocity + friction 342 //velocity + friction
343 //response between two dynamic objects with friction 343 //response between two dynamic objects with friction
344 public virtual float ResolveSingleCollisionCombinedCacheFriendly( 344 public virtual float ResolveSingleCollisionCombinedCacheFriendly(
345 SolverBody bodyA, 345 SolverBody bodyA,
346 SolverBody bodyB, 346 SolverBody bodyB,
347 SolverConstraint contactConstraint, 347 SolverConstraint contactConstraint,
348 ContactSolverInfo solverInfo) 348 ContactSolverInfo solverInfo)
349 { 349 {
350 float normalImpulse = 0; 350 float normalImpulse = 0;
351 351
352 if (contactConstraint.Penetration < 0) 352 if (contactConstraint.Penetration < 0)
353 return 0; 353 return 0;
354 354
355 float relVel; 355 float relVel;
356 float velADotn = Vector3.Dot(contactConstraint.ContactNormal,bodyA.LinearVelocity) 356 float velADotn = Vector3.Dot(contactConstraint.ContactNormal,bodyA.LinearVelocity)
357 + Vector3.Dot(contactConstraint.RelPosACrossNormal,bodyA.AngularVelocity); 357 + Vector3.Dot(contactConstraint.RelPosACrossNormal,bodyA.AngularVelocity);
358 float velBDotn = Vector3.Dot(contactConstraint.ContactNormal,bodyB.LinearVelocity) 358 float velBDotn = Vector3.Dot(contactConstraint.ContactNormal,bodyB.LinearVelocity)
359 + Vector3.Dot(contactConstraint.RelPosBCrossNormal,bodyB.AngularVelocity); 359 + Vector3.Dot(contactConstraint.RelPosBCrossNormal,bodyB.AngularVelocity);
360 360
361 relVel = velADotn - velBDotn; 361 relVel = velADotn - velBDotn;
362 362
363 float positionalError = contactConstraint.Penetration; 363 float positionalError = contactConstraint.Penetration;
364 float velocityError = contactConstraint.Restitution - relVel;// * damping; 364 float velocityError = contactConstraint.Restitution - relVel;// * damping;
365 365
366 float penetrationImpulse = positionalError * contactConstraint.JacDiagABInv; 366 float penetrationImpulse = positionalError * contactConstraint.JacDiagABInv;
367 float velocityImpulse = velocityError * contactConstraint.JacDiagABInv; 367 float velocityImpulse = velocityError * contactConstraint.JacDiagABInv;
368 normalImpulse = penetrationImpulse + velocityImpulse; 368 normalImpulse = penetrationImpulse + velocityImpulse;
369 369
370 // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse 370 // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
371 float oldNormalImpulse = contactConstraint.AppliedImpulse; 371 float oldNormalImpulse = contactConstraint.AppliedImpulse;
372 float sum = oldNormalImpulse + normalImpulse; 372 float sum = oldNormalImpulse + normalImpulse;
373 contactConstraint.AppliedImpulse = 0 > sum ? 0 : sum; 373 contactConstraint.AppliedImpulse = 0 > sum ? 0 : sum;
374 374
375 float oldVelocityImpulse = contactConstraint.AppliedVelocityImpulse; 375 float oldVelocityImpulse = contactConstraint.AppliedVelocityImpulse;
376 float velocitySum = oldVelocityImpulse + velocityImpulse; 376 float velocitySum = oldVelocityImpulse + velocityImpulse;
377 contactConstraint.AppliedVelocityImpulse = 0 > velocitySum ? 0 : velocitySum; 377 contactConstraint.AppliedVelocityImpulse = 0 > velocitySum ? 0 : velocitySum;
378 378
379 normalImpulse = contactConstraint.AppliedImpulse - oldNormalImpulse; 379 normalImpulse = contactConstraint.AppliedImpulse - oldNormalImpulse;
380 380
381 if (bodyA.InvMass != 0) 381 if (bodyA.InvMass != 0)
382 { 382 {
383 bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, 383 bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass,
384 contactConstraint.AngularComponentA, normalImpulse); 384 contactConstraint.AngularComponentA, normalImpulse);
385 } 385 }
386 if (bodyB.InvMass != 0) 386 if (bodyB.InvMass != 0)
387 { 387 {
388 bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, 388 bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass,
389 contactConstraint.AngularComponentB, -normalImpulse); 389 contactConstraint.AngularComponentB, -normalImpulse);
390 } 390 }
391 391
392 return normalImpulse; 392 return normalImpulse;
393 } 393 }
394 394
395 public virtual float ResolveSingleFrictionCacheFriendly( 395 public virtual float ResolveSingleFrictionCacheFriendly(
396 SolverBody bodyA, 396 SolverBody bodyA,
397 SolverBody bodyB, 397 SolverBody bodyB,
398 SolverConstraint contactConstraint, 398 SolverConstraint contactConstraint,
399 ContactSolverInfo solverInfo, 399 ContactSolverInfo solverInfo,
400 float appliedNormalImpulse) 400 float appliedNormalImpulse)
401 { 401 {
402 float combinedFriction = contactConstraint.Friction; 402 float combinedFriction = contactConstraint.Friction;
403 float limit = appliedNormalImpulse * combinedFriction; 403 float limit = appliedNormalImpulse * combinedFriction;
404 404
405 if (appliedNormalImpulse > 0) 405 if (appliedNormalImpulse > 0)
406 //friction 406 //friction
407 { 407 {
408 float j1; 408 float j1;
409 { 409 {
410 float relVel; 410 float relVel;
411 float velADotn = Vector3.Dot(contactConstraint.ContactNormal, bodyA.LinearVelocity) 411 float velADotn = Vector3.Dot(contactConstraint.ContactNormal, bodyA.LinearVelocity)
412 + Vector3.Dot(contactConstraint.RelPosACrossNormal, bodyA.AngularVelocity); 412 + Vector3.Dot(contactConstraint.RelPosACrossNormal, bodyA.AngularVelocity);
413 float velBDotn = Vector3.Dot(contactConstraint.ContactNormal, bodyB.LinearVelocity) 413 float velBDotn = Vector3.Dot(contactConstraint.ContactNormal, bodyB.LinearVelocity)
414 + Vector3.Dot(contactConstraint.RelPosBCrossNormal, bodyB.AngularVelocity); 414 + Vector3.Dot(contactConstraint.RelPosBCrossNormal, bodyB.AngularVelocity);
415 relVel = velADotn - velBDotn; 415 relVel = velADotn - velBDotn;
416 416
417 // calculate j that moves us to zero relative velocity 417 // calculate j that moves us to zero relative velocity
418 j1 = -relVel * contactConstraint.JacDiagABInv; 418 j1 = -relVel * contactConstraint.JacDiagABInv;
419 float oldTangentImpulse = contactConstraint.AppliedImpulse; 419 float oldTangentImpulse = contactConstraint.AppliedImpulse;
420 contactConstraint.AppliedImpulse = oldTangentImpulse + j1; 420 contactConstraint.AppliedImpulse = oldTangentImpulse + j1;
421 421
422 float test = contactConstraint.AppliedImpulse; 422 float test = contactConstraint.AppliedImpulse;
423 MathHelper.SetMin(ref test, limit); 423 MathHelper.SetMin(ref test, limit);
424 MathHelper.SetMax(ref test, -limit); 424 MathHelper.SetMax(ref test, -limit);
425 contactConstraint.AppliedImpulse = test; 425 contactConstraint.AppliedImpulse = test;
426 426
427 j1 = contactConstraint.AppliedImpulse - oldTangentImpulse; 427 j1 = contactConstraint.AppliedImpulse - oldTangentImpulse;
428 } 428 }
429 429
430 if (bodyA.InvMass != 0) 430 if (bodyA.InvMass != 0)
431 { 431 {
432 bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, contactConstraint.AngularComponentA, j1); 432 bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, contactConstraint.AngularComponentA, j1);
433 } 433 }
434 if (bodyB.InvMass != 0) 434 if (bodyB.InvMass != 0)
435 { 435 {
436 bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, contactConstraint.AngularComponentB, -j1); 436 bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, contactConstraint.AngularComponentB, -j1);
437 } 437 }
438 } 438 }
439 return 0; 439 return 0;
440 } 440 }
441 441
442 public virtual float SolveGroupCacheFriendly(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer) 442 public virtual float SolveGroupCacheFriendly(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer)
443 { 443 {
444 if (constraints.Count + numManifolds == 0) 444 if (constraints.Count + numManifolds == 0)
445 { 445 {
446 return 0; 446 return 0;
447 } 447 }
448 448
449 for (int i = 0; i < numManifolds; i++) 449 for (int i = 0; i < numManifolds; i++)
450 { 450 {
451 PersistentManifold manifold = manifolds[i]; 451 PersistentManifold manifold = manifolds[i];
452 RigidBody rbA = (RigidBody)manifold.BodyA; 452 RigidBody rbA = (RigidBody)manifold.BodyA;
453 RigidBody rbB = (RigidBody)manifold.BodyB; 453 RigidBody rbB = (RigidBody)manifold.BodyB;
454 454
455 manifold.RefreshContactPoints(rbA.CenterOfMassTransform, rbB.CenterOfMassTransform); 455 manifold.RefreshContactPoints(rbA.CenterOfMassTransform, rbB.CenterOfMassTransform);
456 } 456 }
457 457
458 int minReservation = manifolds.Count * 2; 458 int minReservation = manifolds.Count * 2;
459 459
460 _tmpSolverBodyPool = new List<SolverBody>(minReservation); 460 _tmpSolverBodyPool = new List<SolverBody>(minReservation);
461 461
462 for (int i = 0; i < bodies.Count; i++) 462 for (int i = 0; i < bodies.Count; i++)
463 { 463 {
464 RigidBody rb = RigidBody.Upcast(bodies[i]); 464 RigidBody rb = RigidBody.Upcast(bodies[i]);
465 if (rb != null && rb.IslandTag >= 0) 465 if (rb != null && rb.IslandTag >= 0)
466 { 466 {
467 BulletDebug.Assert(rb.CompanionID < 0); 467 BulletDebug.Assert(rb.CompanionID < 0);
468 int solverBodyId = _tmpSolverBodyPool.Count; 468 int solverBodyId = _tmpSolverBodyPool.Count;
469 SolverBody solverBody; 469 SolverBody solverBody;
470 InitSolverBody(out solverBody, rb); 470 InitSolverBody(out solverBody, rb);
471 _tmpSolverBodyPool.Add(solverBody); 471 _tmpSolverBodyPool.Add(solverBody);
472 rb.CompanionID = solverBodyId; 472 rb.CompanionID = solverBodyId;
473 } 473 }
474 } 474 }
475 475
476 _tmpSolverConstraintPool = new List<SolverConstraint>(minReservation); 476 _tmpSolverConstraintPool = new List<SolverConstraint>(minReservation);
477 _tmpSolverFrictionConstraintPool = new List<SolverConstraint>(minReservation); 477 _tmpSolverFrictionConstraintPool = new List<SolverConstraint>(minReservation);
478 478
479 for (int i = 0; i < numManifolds; i++) 479 for (int i = 0; i < numManifolds; i++)
480 { 480 {
481 PersistentManifold manifold = manifolds[i]; 481 PersistentManifold manifold = manifolds[i];
482 RigidBody rb0 = (RigidBody)manifold.BodyA; 482 RigidBody rb0 = (RigidBody)manifold.BodyA;
483 RigidBody rb1 = (RigidBody)manifold.BodyB; 483 RigidBody rb1 = (RigidBody)manifold.BodyB;
484 484
485 int solverBodyIdA = -1; 485 int solverBodyIdA = -1;
486 int solverBodyIdB = -1; 486 int solverBodyIdB = -1;
487 487
488 //if (i == 89) 488 //if (i == 89)
489 // System.Diagnostics.Debugger.Break(); 489 // System.Diagnostics.Debugger.Break();
490 490
491 if (manifold.ContactsCount != 0) 491 if (manifold.ContactsCount != 0)
492 { 492 {
493 if (rb0.IslandTag >= 0) 493 if (rb0.IslandTag >= 0)
494 { 494 {
495 solverBodyIdA = rb0.CompanionID; 495 solverBodyIdA = rb0.CompanionID;
496 } 496 }
497 else 497 else
498 { 498 {
499 //create a static body 499 //create a static body
500 solverBodyIdA = _tmpSolverBodyPool.Count; 500 solverBodyIdA = _tmpSolverBodyPool.Count;
501 SolverBody solverBody; 501 SolverBody solverBody;
502 InitSolverBody(out solverBody, rb0); 502 InitSolverBody(out solverBody, rb0);
503 _tmpSolverBodyPool.Add(solverBody); 503 _tmpSolverBodyPool.Add(solverBody);
504 } 504 }
505 505
506 if (rb1.IslandTag >= 0) 506 if (rb1.IslandTag >= 0)
507 { 507 {
508 solverBodyIdB = rb1.CompanionID; 508 solverBodyIdB = rb1.CompanionID;
509 } 509 }
510 else 510 else
511 { 511 {
512 //create a static body 512 //create a static body
513 solverBodyIdB = _tmpSolverBodyPool.Count; 513 solverBodyIdB = _tmpSolverBodyPool.Count;
514 SolverBody solverBody; 514 SolverBody solverBody;
515 InitSolverBody(out solverBody, rb1); 515 InitSolverBody(out solverBody, rb1);
516 _tmpSolverBodyPool.Add(solverBody); 516 _tmpSolverBodyPool.Add(solverBody);
517 } 517 }
518 } 518 }
519 519
520 if (solverBodyIdB == -1 || solverBodyIdA == -1) 520 if (solverBodyIdB == -1 || solverBodyIdA == -1)
521 System.Diagnostics.Debug.WriteLine(string.Format("We're in ass ! {0}", i)); 521 System.Diagnostics.Debug.WriteLine(string.Format("We're in ass ! {0}", i));
522 522
523 for (int j = 0; j < manifold.ContactsCount; j++) 523 for (int j = 0; j < manifold.ContactsCount; j++)
524 { 524 {
525 ManifoldPoint cp = manifold.GetContactPoint(j); 525 ManifoldPoint cp = manifold.GetContactPoint(j);
526 526
527 int frictionIndex = _tmpSolverConstraintPool.Count; 527 int frictionIndex = _tmpSolverConstraintPool.Count;
528 528
529 if (cp.Distance <= 0) 529 if (cp.Distance <= 0)
530 { 530 {
531 531
532 Vector3 pos1 = cp.PositionWorldOnA; 532 Vector3 pos1 = cp.PositionWorldOnA;
533 Vector3 pos2 = cp.PositionWorldOnB; 533 Vector3 pos2 = cp.PositionWorldOnB;
534 534
535 Vector3 rel_pos1 = pos1 - rb0.CenterOfMassPosition; 535 Vector3 rel_pos1 = pos1 - rb0.CenterOfMassPosition;
536 Vector3 rel_pos2 = pos2 - rb1.CenterOfMassPosition; 536 Vector3 rel_pos2 = pos2 - rb1.CenterOfMassPosition;
537 537
538 float relaxation = 1; 538 float relaxation = 1;
539 { 539 {
540 SolverConstraint solverConstraint = new SolverConstraint(); 540 SolverConstraint solverConstraint = new SolverConstraint();
541 _tmpSolverConstraintPool.Add(solverConstraint); 541 _tmpSolverConstraintPool.Add(solverConstraint);
542 542
543 solverConstraint.SolverBodyIdA = solverBodyIdA; 543 solverConstraint.SolverBodyIdA = solverBodyIdA;
544 solverConstraint.SolverBodyIdB = solverBodyIdB; 544 solverConstraint.SolverBodyIdB = solverBodyIdB;
545 solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Contact; 545 solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Contact;
546 546
547 //can be optimized, the cross products are already calculated 547 //can be optimized, the cross products are already calculated
548 float denom0 = rb0.ComputeImpulseDenominator(pos1, cp.NormalWorldOnB); 548 float denom0 = rb0.ComputeImpulseDenominator(pos1, cp.NormalWorldOnB);
549 float denom1 = rb1.ComputeImpulseDenominator(pos2, cp.NormalWorldOnB); 549 float denom1 = rb1.ComputeImpulseDenominator(pos2, cp.NormalWorldOnB);
550 float denom = relaxation / (denom0 + denom1); 550 float denom = relaxation / (denom0 + denom1);
551 solverConstraint.JacDiagABInv = denom; 551 solverConstraint.JacDiagABInv = denom;
552 552
553 solverConstraint.ContactNormal = cp.NormalWorldOnB; 553 solverConstraint.ContactNormal = cp.NormalWorldOnB;
554 solverConstraint.RelPosACrossNormal = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); 554 solverConstraint.RelPosACrossNormal = Vector3.Cross(rel_pos1, cp.NormalWorldOnB);
555 solverConstraint.RelPosBCrossNormal = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); 555 solverConstraint.RelPosBCrossNormal = Vector3.Cross(rel_pos2, cp.NormalWorldOnB);
556 556
557 Vector3 vel1 = rb0.GetVelocityInLocalPoint(rel_pos1); 557 Vector3 vel1 = rb0.GetVelocityInLocalPoint(rel_pos1);
558 Vector3 vel2 = rb1.GetVelocityInLocalPoint(rel_pos2); 558 Vector3 vel2 = rb1.GetVelocityInLocalPoint(rel_pos2);
559 559
560 Vector3 vel = vel1 - vel2; 560 Vector3 vel = vel1 - vel2;
561 float rel_vel; 561 float rel_vel;
562 rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel); 562 rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel);
563 563
564 564
565 solverConstraint.Penetration = cp.Distance;//btScalar(infoGlobal.m_numIterations); 565 solverConstraint.Penetration = cp.Distance;//btScalar(infoGlobal.m_numIterations);
566 solverConstraint.Friction = cp.CombinedFriction; 566 solverConstraint.Friction = cp.CombinedFriction;
567 float rest = RestitutionCurve(rel_vel, cp.CombinedRestitution); 567 float rest = RestitutionCurve(rel_vel, cp.CombinedRestitution);
568 if (rest <= 0) 568 if (rest <= 0)
569 { 569 {
570 rest = 0; 570 rest = 0;
571 } 571 }
572 572
573 float penVel = -solverConstraint.Penetration / infoGlobal.TimeStep; 573 float penVel = -solverConstraint.Penetration / infoGlobal.TimeStep;
574 if (rest > penVel) 574 if (rest > penVel)
575 { 575 {
576 rest = 0; 576 rest = 0;
577 } 577 }
578 solverConstraint.Restitution = rest; 578 solverConstraint.Restitution = rest;
579 579
580 solverConstraint.Penetration *= -(infoGlobal.Erp / infoGlobal.TimeStep); 580 solverConstraint.Penetration *= -(infoGlobal.Erp / infoGlobal.TimeStep);
581 581
582 solverConstraint.AppliedImpulse = 0f; 582 solverConstraint.AppliedImpulse = 0f;
583 solverConstraint.AppliedVelocityImpulse = 0f; 583 solverConstraint.AppliedVelocityImpulse = 0f;
584 584
585#warning Check to see if we need Vector3.Transform 585#warning Check to see if we need Vector3.Transform
586 Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); 586 Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB);
587 solverConstraint.AngularComponentA = Vector3.TransformNormal(torqueAxis0, rb0.InvInertiaTensorWorld); 587 solverConstraint.AngularComponentA = Vector3.TransformNormal(torqueAxis0, rb0.InvInertiaTensorWorld);
588 Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); 588 Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB);
589 solverConstraint.AngularComponentB = Vector3.TransformNormal(torqueAxis1, rb1.InvInertiaTensorWorld); 589 solverConstraint.AngularComponentB = Vector3.TransformNormal(torqueAxis1, rb1.InvInertiaTensorWorld);
590 } 590 }
591 //create 2 '1d axis' constraints for 2 tangential friction directions 591 //create 2 '1d axis' constraints for 2 tangential friction directions
592 592
593 //re-calculate friction direction every frame, todo: check if this is really needed 593 //re-calculate friction direction every frame, todo: check if this is really needed
594 Vector3 frictionTangential0a = new Vector3(), 594 Vector3 frictionTangential0a = new Vector3(),
595 frictionTangential1b = new Vector3(); 595 frictionTangential1b = new Vector3();
596 596
597 MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref frictionTangential0a, ref frictionTangential1b); 597 MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref frictionTangential0a, ref frictionTangential1b);
598 { 598 {
599 SolverConstraint solverConstraint = new SolverConstraint(); 599 SolverConstraint solverConstraint = new SolverConstraint();
600 _tmpSolverFrictionConstraintPool.Add(solverConstraint); 600 _tmpSolverFrictionConstraintPool.Add(solverConstraint);
601 solverConstraint.ContactNormal = frictionTangential0a; 601 solverConstraint.ContactNormal = frictionTangential0a;
602 602
603 solverConstraint.SolverBodyIdA = solverBodyIdA; 603 solverConstraint.SolverBodyIdA = solverBodyIdA;
604 solverConstraint.SolverBodyIdB = solverBodyIdB; 604 solverConstraint.SolverBodyIdB = solverBodyIdB;
605 solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction; 605 solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction;
606 solverConstraint.FrictionIndex = frictionIndex; 606 solverConstraint.FrictionIndex = frictionIndex;
607 607
608 solverConstraint.Friction = cp.CombinedFriction; 608 solverConstraint.Friction = cp.CombinedFriction;
609 609
610 solverConstraint.AppliedImpulse = 0; 610 solverConstraint.AppliedImpulse = 0;
611 solverConstraint.AppliedVelocityImpulse = 0; 611 solverConstraint.AppliedVelocityImpulse = 0;
612 612
613 float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal); 613 float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal);
614 float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal); 614 float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal);
615 float denom = relaxation / (denom0 + denom1); 615 float denom = relaxation / (denom0 + denom1);
616 solverConstraint.JacDiagABInv = denom; 616 solverConstraint.JacDiagABInv = denom;
617 617
618 { 618 {
619 Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal); 619 Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal);
620 solverConstraint.RelPosACrossNormal = ftorqueAxis0; 620 solverConstraint.RelPosACrossNormal = ftorqueAxis0;
621 solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis0, rb0.InvInertiaTensorWorld); 621 solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis0, rb0.InvInertiaTensorWorld);
622 } 622 }
623 { 623 {
624 Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal); 624 Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal);
625 solverConstraint.RelPosBCrossNormal = ftorqueAxis0; 625 solverConstraint.RelPosBCrossNormal = ftorqueAxis0;
626 solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis0, rb1.InvInertiaTensorWorld); 626 solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis0, rb1.InvInertiaTensorWorld);
627 } 627 }
628 } 628 }
629 629
630 630
631 { 631 {
632 632
633 SolverConstraint solverConstraint = new SolverConstraint(); 633 SolverConstraint solverConstraint = new SolverConstraint();
634 _tmpSolverFrictionConstraintPool.Add(solverConstraint); 634 _tmpSolverFrictionConstraintPool.Add(solverConstraint);
635 solverConstraint.ContactNormal = frictionTangential1b; 635 solverConstraint.ContactNormal = frictionTangential1b;
636 636
637 solverConstraint.SolverBodyIdA = solverBodyIdA; 637 solverConstraint.SolverBodyIdA = solverBodyIdA;
638 solverConstraint.SolverBodyIdB = solverBodyIdB; 638 solverConstraint.SolverBodyIdB = solverBodyIdB;
639 solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction; 639 solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction;
640 solverConstraint.FrictionIndex = frictionIndex; 640 solverConstraint.FrictionIndex = frictionIndex;
641 641
642 solverConstraint.Friction = cp.CombinedFriction; 642 solverConstraint.Friction = cp.CombinedFriction;
643 643
644 solverConstraint.AppliedImpulse = 0; 644 solverConstraint.AppliedImpulse = 0;
645 solverConstraint.AppliedVelocityImpulse = 0; 645 solverConstraint.AppliedVelocityImpulse = 0;
646 646
647 float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal); 647 float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal);
648 float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal); 648 float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal);
649 float denom = relaxation / (denom0 + denom1); 649 float denom = relaxation / (denom0 + denom1);
650 solverConstraint.JacDiagABInv = denom; 650 solverConstraint.JacDiagABInv = denom;
651 { 651 {
652 Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal); 652 Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal);
653 solverConstraint.RelPosACrossNormal = ftorqueAxis1; 653 solverConstraint.RelPosACrossNormal = ftorqueAxis1;
654 solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis1, rb0.InvInertiaTensorWorld); 654 solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis1, rb0.InvInertiaTensorWorld);
655 } 655 }
656 { 656 {
657 Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal); 657 Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal);
658 solverConstraint.RelPosBCrossNormal = ftorqueAxis1; 658 solverConstraint.RelPosBCrossNormal = ftorqueAxis1;
659 solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis1, rb1.InvInertiaTensorWorld); 659 solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis1, rb1.InvInertiaTensorWorld);
660 } 660 }
661 } 661 }
662 } 662 }
663 } 663 }
664 } 664 }
665 665
666 ContactSolverInfo info = infoGlobal; 666 ContactSolverInfo info = infoGlobal;
667 { 667 {
668 for (int j = 0; j < constraints.Count; j++) 668 for (int j = 0; j < constraints.Count; j++)
669 { 669 {
670 TypedConstraint constraint = constraints[j]; 670 TypedConstraint constraint = constraints[j];
671 constraint.BuildJacobian(); 671 constraint.BuildJacobian();
672 } 672 }
673 } 673 }
674 674
675 int numConstraintPool = _tmpSolverConstraintPool.Count; 675 int numConstraintPool = _tmpSolverConstraintPool.Count;
676 int numFrictionPool = _tmpSolverFrictionConstraintPool.Count; 676 int numFrictionPool = _tmpSolverFrictionConstraintPool.Count;
677 677
678 //todo: use stack allocator for such temporarily memory, same for solver bodies/constraints 678 //todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
679 List<int> gOrderTmpConstraintPool = new List<int>(numConstraintPool); 679 List<int> gOrderTmpConstraintPool = new List<int>(numConstraintPool);
680 List<int> gOrderFrictionConstraintPool = new List<int>(numFrictionPool); 680 List<int> gOrderFrictionConstraintPool = new List<int>(numFrictionPool);
681 { 681 {
682 for (int i = 0; i < numConstraintPool; i++) 682 for (int i = 0; i < numConstraintPool; i++)
683 { 683 {
684 gOrderTmpConstraintPool.Add(i); 684 gOrderTmpConstraintPool.Add(i);
685 } 685 }
686 for (int i = 0; i < numFrictionPool; i++) 686 for (int i = 0; i < numFrictionPool; i++)
687 { 687 {
688 gOrderFrictionConstraintPool.Add(i); 688 gOrderFrictionConstraintPool.Add(i);
689 } 689 }
690 } 690 }
691 691
692 //should traverse the contacts random order... 692 //should traverse the contacts random order...
693 int iteration; 693 int iteration;
694 { 694 {
695 for (iteration = 0; iteration < info.IterationsCount; iteration++) 695 for (iteration = 0; iteration < info.IterationsCount; iteration++)
696 { 696 {
697 697
698 int j; 698 int j;
699 if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None) 699 if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None)
700 { 700 {
701 if ((iteration & 7) == 0) 701 if ((iteration & 7) == 0)
702 { 702 {
703 for (j = 0; j < numConstraintPool; ++j) 703 for (j = 0; j < numConstraintPool; ++j)
704 { 704 {
705 int tmp = gOrderTmpConstraintPool[j]; 705 int tmp = gOrderTmpConstraintPool[j];
706 int swapi = RandInt2(j + 1); 706 int swapi = RandInt2(j + 1);
707 gOrderTmpConstraintPool[j] = gOrderTmpConstraintPool[swapi]; 707 gOrderTmpConstraintPool[j] = gOrderTmpConstraintPool[swapi];
708 gOrderTmpConstraintPool[swapi] = tmp; 708 gOrderTmpConstraintPool[swapi] = tmp;
709 } 709 }
710 710
711 for (j = 0; j < numFrictionPool; ++j) 711 for (j = 0; j < numFrictionPool; ++j)
712 { 712 {
713 int tmp = gOrderFrictionConstraintPool[j]; 713 int tmp = gOrderFrictionConstraintPool[j];
714 int swapi = RandInt2(j + 1); 714 int swapi = RandInt2(j + 1);
715 gOrderFrictionConstraintPool[j] = gOrderFrictionConstraintPool[swapi]; 715 gOrderFrictionConstraintPool[j] = gOrderFrictionConstraintPool[swapi];
716 gOrderFrictionConstraintPool[swapi] = tmp; 716 gOrderFrictionConstraintPool[swapi] = tmp;
717 } 717 }
718 } 718 }
719 } 719 }
720 720
721 for (j = 0; j < constraints.Count; j++) 721 for (j = 0; j < constraints.Count; j++)
722 { 722 {
723 TypedConstraint constraint = constraints[j]; 723 TypedConstraint constraint = constraints[j];
724 //todo: use solver bodies, so we don't need to copy from/to btRigidBody 724 //todo: use solver bodies, so we don't need to copy from/to btRigidBody
725 725
726 if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0)) 726 if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0))
727 { 727 {
728 _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].WriteBackVelocity(); 728 _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].WriteBackVelocity();
729 } 729 }
730 if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0)) 730 if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0))
731 { 731 {
732 _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].WriteBackVelocity(); 732 _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].WriteBackVelocity();
733 } 733 }
734 734
735 constraint.SolveConstraint(info.TimeStep); 735 constraint.SolveConstraint(info.TimeStep);
736 736
737 if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0)) 737 if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0))
738 { 738 {
739 _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].ReadVelocity(); 739 _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].ReadVelocity();
740 } 740 }
741 if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0)) 741 if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0))
742 { 742 {
743 _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].ReadVelocity(); 743 _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].ReadVelocity();
744 } 744 }
745 745
746 } 746 }
747 747
748 { 748 {
749 int numPoolConstraints = _tmpSolverConstraintPool.Count; 749 int numPoolConstraints = _tmpSolverConstraintPool.Count;
750 for (j = 0; j < numPoolConstraints; j++) 750 for (j = 0; j < numPoolConstraints; j++)
751 { 751 {
752 SolverConstraint solveManifold = _tmpSolverConstraintPool[gOrderTmpConstraintPool[j]]; 752 SolverConstraint solveManifold = _tmpSolverConstraintPool[gOrderTmpConstraintPool[j]];
753 ResolveSingleCollisionCombinedCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA], 753 ResolveSingleCollisionCombinedCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA],
754 _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info); 754 _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info);
755 } 755 }
756 } 756 }
757 757
758 { 758 {
759 int numFrictionPoolConstraints = _tmpSolverFrictionConstraintPool.Count; 759 int numFrictionPoolConstraints = _tmpSolverFrictionConstraintPool.Count;
760 for (j = 0; j < numFrictionPoolConstraints; j++) 760 for (j = 0; j < numFrictionPoolConstraints; j++)
761 { 761 {
762 SolverConstraint solveManifold = _tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]]; 762 SolverConstraint solveManifold = _tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]];
763 float appliedNormalImpulse = _tmpSolverConstraintPool[solveManifold.FrictionIndex].AppliedImpulse; 763 float appliedNormalImpulse = _tmpSolverConstraintPool[solveManifold.FrictionIndex].AppliedImpulse;
764 764
765 ResolveSingleFrictionCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA], 765 ResolveSingleFrictionCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA],
766 _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info, appliedNormalImpulse); 766 _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info, appliedNormalImpulse);
767 } 767 }
768 } 768 }
769 } 769 }
770 } 770 }
771 771
772 for (int i = 0; i < _tmpSolverBodyPool.Count; i++) 772 for (int i = 0; i < _tmpSolverBodyPool.Count; i++)
773 { 773 {
774 _tmpSolverBodyPool[i].WriteBackVelocity(); 774 _tmpSolverBodyPool[i].WriteBackVelocity();
775 } 775 }
776 776
777 _tmpSolverBodyPool.Clear(); 777 _tmpSolverBodyPool.Clear();
778 _tmpSolverConstraintPool.Clear(); 778 _tmpSolverConstraintPool.Clear();
779 _tmpSolverFrictionConstraintPool.Clear(); 779 _tmpSolverFrictionConstraintPool.Clear();
780 780
781 return 0; 781 return 0;
782 } 782 }
783 783
784 public virtual float SolveGroup(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer) 784 public virtual float SolveGroup(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer)
785 { 785 {
786 if ((_solverMode & SolverMode.CacheFriendly) != SolverMode.None) 786 if ((_solverMode & SolverMode.CacheFriendly) != SolverMode.None)
787 { 787 {
788 return SolveGroupCacheFriendly(bodies, manifolds, numManifolds, constraints, infoGlobal, debugDrawer); 788 return SolveGroupCacheFriendly(bodies, manifolds, numManifolds, constraints, infoGlobal, debugDrawer);
789 } 789 }
790 790
791 ContactSolverInfo info = infoGlobal; 791 ContactSolverInfo info = infoGlobal;
792 int totalPoints = 0; 792 int totalPoints = 0;
793 793
794 int numiter = infoGlobal.IterationsCount; 794 int numiter = infoGlobal.IterationsCount;
795 795
796 for (int j = 0; j < manifolds.Count; j++) 796 for (int j = 0; j < manifolds.Count; j++)
797 { 797 {
798 PersistentManifold manifold = manifolds[j]; 798 PersistentManifold manifold = manifolds[j];
799 PrepareConstraints(manifold, info); 799 PrepareConstraints(manifold, info);
800 800
801 for (int p = 0; p < manifolds[j].ContactsCount; p++) 801 for (int p = 0; p < manifolds[j].ContactsCount; p++)
802 { 802 {
803 _order[totalPoints].ManifoldIndex = j; 803 _order[totalPoints].ManifoldIndex = j;
804 _order[totalPoints].PointIndex = p; 804 _order[totalPoints].PointIndex = p;
805 totalPoints++; 805 totalPoints++;
806 } 806 }
807 } 807 }
808 808
809 for (int j = 0; j < constraints.Count; j++) 809 for (int j = 0; j < constraints.Count; j++)
810 { 810 {
811 constraints[j].BuildJacobian(); 811 constraints[j].BuildJacobian();
812 } 812 }
813 813
814 //should traverse the contacts random order... 814 //should traverse the contacts random order...
815 int iteration; 815 int iteration;
816 816
817 for (iteration = 0; iteration < numiter; iteration++) 817 for (iteration = 0; iteration < numiter; iteration++)
818 { 818 {
819 int j; 819 int j;
820 if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None) 820 if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None)
821 { 821 {
822 if ((iteration & 7) == 0) 822 if ((iteration & 7) == 0)
823 { 823 {
824 for (j = 0; j < totalPoints; ++j) 824 for (j = 0; j < totalPoints; ++j)
825 { 825 {
826 OrderIndex tmp = _order[j]; 826 OrderIndex tmp = _order[j];
827 int swapi = RandInt2(j + 1); 827 int swapi = RandInt2(j + 1);
828 _order[j] = _order[swapi]; 828 _order[j] = _order[swapi];
829 _order[swapi] = tmp; 829 _order[swapi] = tmp;
830 } 830 }
831 } 831 }
832 } 832 }
833 833
834 for (j = 0; j < constraints.Count; j++) 834 for (j = 0; j < constraints.Count; j++)
835 { 835 {
836 constraints[j].SolveConstraint(info.TimeStep); 836 constraints[j].SolveConstraint(info.TimeStep);
837 } 837 }
838 838
839 for (j = 0; j < totalPoints; j++) 839 for (j = 0; j < totalPoints; j++)
840 { 840 {
841 PersistentManifold manifold = manifolds[_order[j].ManifoldIndex]; 841 PersistentManifold manifold = manifolds[_order[j].ManifoldIndex];
842 Solve((RigidBody)manifold.BodyA, (RigidBody)manifold.BodyB, 842 Solve((RigidBody)manifold.BodyA, (RigidBody)manifold.BodyB,
843 manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer); 843 manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer);
844 } 844 }
845 845
846 for (j = 0; j < totalPoints; j++) 846 for (j = 0; j < totalPoints; j++)
847 { 847 {
848 PersistentManifold manifold = manifolds[_order[j].ManifoldIndex]; 848 PersistentManifold manifold = manifolds[_order[j].ManifoldIndex];
849 SolveFriction((RigidBody)manifold.BodyA, 849 SolveFriction((RigidBody)manifold.BodyA,
850 (RigidBody)manifold.BodyB, manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer); 850 (RigidBody)manifold.BodyB, manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer);
851 } 851 }
852 } 852 }
853 853
854 return 0; 854 return 0;
855 } 855 }
856 856
857 private void InitSolverBody(out SolverBody solverBody, RigidBody rigidBody) 857 private void InitSolverBody(out SolverBody solverBody, RigidBody rigidBody)
858 { 858 {
859 solverBody = new SolverBody(); 859 solverBody = new SolverBody();
860 solverBody.AngularVelocity = rigidBody.AngularVelocity; 860 solverBody.AngularVelocity = rigidBody.AngularVelocity;
861 solverBody.CenterOfMassPosition = rigidBody.CenterOfMassPosition; 861 solverBody.CenterOfMassPosition = rigidBody.CenterOfMassPosition;
862 solverBody.Friction = rigidBody.Friction; 862 solverBody.Friction = rigidBody.Friction;
863 solverBody.InvMass = rigidBody.InverseMass; 863 solverBody.InvMass = rigidBody.InverseMass;
864 solverBody.LinearVelocity = rigidBody.LinearVelocity; 864 solverBody.LinearVelocity = rigidBody.LinearVelocity;
865 solverBody.OriginalBody = rigidBody; 865 solverBody.OriginalBody = rigidBody;
866 solverBody.AngularFactor = rigidBody.AngularFactor; 866 solverBody.AngularFactor = rigidBody.AngularFactor;
867 } 867 }
868 868
869 private long Rand2() 869 private long Rand2()
870 { 870 {
871 _seed2 = (1664525L * _seed2 + 1013904223L) & 0xffffffff; 871 _seed2 = (1664525L * _seed2 + 1013904223L) & 0xffffffff;
872 return _seed2; 872 return _seed2;
873 } 873 }
874 874
875 private int RandInt2(int n) 875 private int RandInt2(int n)
876 { 876 {
877 // seems good; xor-fold and modulus 877 // seems good; xor-fold and modulus
878 long un = n; 878 long un = n;
879 long r = Rand2(); 879 long r = Rand2();
880 880
881 // note: probably more aggressive than it needs to be -- might be 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. 882 // able to get away without one or two of the innermost branches.
883 if (un <= 0x00010000L) 883 if (un <= 0x00010000L)
884 { 884 {
885 r ^= (r >> 16); 885 r ^= (r >> 16);
886 if (un <= 0x00000100L) 886 if (un <= 0x00000100L)
887 { 887 {
888 r ^= (r >> 8); 888 r ^= (r >> 8);
889 if (un <= 0x00000010L) 889 if (un <= 0x00000010L)
890 { 890 {
891 r ^= (r >> 4); 891 r ^= (r >> 4);
892 if (un <= 0x00000004L) 892 if (un <= 0x00000004L)
893 { 893 {
894 r ^= (r >> 2); 894 r ^= (r >> 2);
895 if (un <= 0x00000002L) 895 if (un <= 0x00000002L)
896 { 896 {
897 r ^= (r >> 1); 897 r ^= (r >> 1);
898 } 898 }
899 } 899 }
900 } 900 }
901 } 901 }
902 } 902 }
903 return (int)(r % un); 903 return (int)(r % un);
904 } 904 }
905 905
906 protected struct OrderIndex 906 protected struct OrderIndex
907 { 907 {
908 private int _manifoldIndex; 908 private int _manifoldIndex;
909 private int _pointIndex; 909 private int _pointIndex;
910 910
911 public int ManifoldIndex { get { return _manifoldIndex; } set { _manifoldIndex = value; } } 911 public int ManifoldIndex { get { return _manifoldIndex; } set { _manifoldIndex = value; } }
912 public int PointIndex { get { return _pointIndex; } set { _pointIndex = value; } } 912 public int PointIndex { get { return _pointIndex; } set { _pointIndex = value; } }
913 } 913 }
914 } 914 }
915} 915}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs
index 6ced783..9d4060d 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs
@@ -1,188 +1,188 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX.Dynamics 27namespace XnaDevRu.BulletX.Dynamics
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// constraint class used for lateral tyre friction 30 /// constraint class used for lateral tyre friction
31 /// </summary> 31 /// </summary>
32 public class Solve2LinearConstraint 32 public class Solve2LinearConstraint
33 { 33 {
34 private float _tau; 34 private float _tau;
35 private float _damping; 35 private float _damping;
36 36
37 public Solve2LinearConstraint(float tau, float damping) 37 public Solve2LinearConstraint(float tau, float damping)
38 { 38 {
39 _tau = tau; 39 _tau = tau;
40 _damping = damping; 40 _damping = damping;
41 } 41 }
42 42
43 // solve unilateral constraint (equality, direct method) 43 // solve unilateral constraint (equality, direct method)
44 public void ResolveUnilateralPairConstraint( 44 public void ResolveUnilateralPairConstraint(
45 RigidBody body1, RigidBody body2, 45 RigidBody body1, RigidBody body2,
46 Matrix world2A, 46 Matrix world2A,
47 Matrix world2B, 47 Matrix world2B,
48 Vector3 invInertiaADiag, 48 Vector3 invInertiaADiag,
49 float invMassA, 49 float invMassA,
50 Vector3 linvelA, Vector3 angvelA, 50 Vector3 linvelA, Vector3 angvelA,
51 Vector3 rel_posA1, 51 Vector3 rel_posA1,
52 Vector3 invInertiaBDiag, 52 Vector3 invInertiaBDiag,
53 float invMassB, 53 float invMassB,
54 Vector3 linvelB, Vector3 angvelB, 54 Vector3 linvelB, Vector3 angvelB,
55 Vector3 rel_posA2, 55 Vector3 rel_posA2,
56 float depthA, Vector3 normalA, 56 float depthA, Vector3 normalA,
57 Vector3 rel_posB1, Vector3 rel_posB2, 57 Vector3 rel_posB1, Vector3 rel_posB2,
58 float depthB, Vector3 normalB, 58 float depthB, Vector3 normalB,
59 out float imp0, out float imp1) 59 out float imp0, out float imp1)
60 { 60 {
61 imp0 = 0; 61 imp0 = 0;
62 imp1 = 0; 62 imp1 = 0;
63 63
64 float len = Math.Abs(normalA.Length()) - 1f; 64 float len = Math.Abs(normalA.Length()) - 1f;
65 if (Math.Abs(len) >= float.Epsilon) 65 if (Math.Abs(len) >= float.Epsilon)
66 return; 66 return;
67 67
68 BulletDebug.Assert(len < float.Epsilon); 68 BulletDebug.Assert(len < float.Epsilon);
69 69
70 //this jacobian entry could be re-used for all iterations 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, 71 JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
72 invInertiaBDiag, invMassB); 72 invInertiaBDiag, invMassB);
73 JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA, 73 JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
74 invInertiaBDiag, invMassB); 74 invInertiaBDiag, invMassB);
75 75
76 float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1)); 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)); 77 float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1));
78 78
79 // btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv 79 // btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv
80 float massTerm = 1f / (invMassA + invMassB); 80 float massTerm = 1f / (invMassA + invMassB);
81 81
82 // calculate rhs (or error) terms 82 // calculate rhs (or error) terms
83 float dv0 = depthA * _tau * massTerm - vel0 * _damping; 83 float dv0 = depthA * _tau * massTerm - vel0 * _damping;
84 float dv1 = depthB * _tau * massTerm - vel1 * _damping; 84 float dv1 = depthB * _tau * massTerm - vel1 * _damping;
85 85
86 float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB); 86 float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB);
87 float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag); 87 float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag);
88 88
89 imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet; 89 imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet;
90 imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet; 90 imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet;
91 } 91 }
92 92
93 // solving 2x2 lcp problem (inequality, direct solution ) 93 // solving 2x2 lcp problem (inequality, direct solution )
94 public void ResolveBilateralPairConstraint( 94 public void ResolveBilateralPairConstraint(
95 RigidBody body1, RigidBody body2, 95 RigidBody body1, RigidBody body2,
96 Matrix world2A, Matrix world2B, 96 Matrix world2A, Matrix world2B,
97 Vector3 invInertiaADiag, 97 Vector3 invInertiaADiag,
98 float invMassA, 98 float invMassA,
99 Vector3 linvelA, Vector3 angvelA, 99 Vector3 linvelA, Vector3 angvelA,
100 Vector3 rel_posA1, 100 Vector3 rel_posA1,
101 Vector3 invInertiaBDiag, 101 Vector3 invInertiaBDiag,
102 float invMassB, 102 float invMassB,
103 Vector3 linvelB, Vector3 angvelB, 103 Vector3 linvelB, Vector3 angvelB,
104 Vector3 rel_posA2, 104 Vector3 rel_posA2,
105 float depthA, Vector3 normalA, 105 float depthA, Vector3 normalA,
106 Vector3 rel_posB1, Vector3 rel_posB2, 106 Vector3 rel_posB1, Vector3 rel_posB2,
107 float depthB, Vector3 normalB, 107 float depthB, Vector3 normalB,
108 out float imp0, out float imp1) 108 out float imp0, out float imp1)
109 { 109 {
110 imp0 = 0f; 110 imp0 = 0f;
111 imp1 = 0f; 111 imp1 = 0f;
112 112
113 float len = Math.Abs(normalA.Length()) - 1f; 113 float len = Math.Abs(normalA.Length()) - 1f;
114 if (Math.Abs(len) >= float.Epsilon) 114 if (Math.Abs(len) >= float.Epsilon)
115 return; 115 return;
116 116
117 BulletDebug.Assert(len < float.Epsilon); 117 BulletDebug.Assert(len < float.Epsilon);
118 118
119 JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA, 119 JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
120 invInertiaBDiag, invMassB); 120 invInertiaBDiag, invMassB);
121 JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA, 121 JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
122 invInertiaBDiag, invMassB); 122 invInertiaBDiag, invMassB);
123 123
124 float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1)); 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)); 125 float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1));
126 126
127 // calculate rhs (or error) terms 127 // calculate rhs (or error) terms
128 float dv0 = depthA * _tau - vel0 * _damping; 128 float dv0 = depthA * _tau - vel0 * _damping;
129 float dv1 = depthB * _tau - vel1 * _damping; 129 float dv1 = depthB * _tau - vel1 * _damping;
130 130
131 float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB); 131 float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB);
132 float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag); 132 float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag);
133 133
134 imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet; 134 imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet;
135 imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet; 135 imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet;
136 136
137 if (imp0 > 0.0f) 137 if (imp0 > 0.0f)
138 { 138 {
139 if (imp1 <= 0.0f) 139 if (imp1 <= 0.0f)
140 { 140 {
141 imp1 = 0f; 141 imp1 = 0f;
142 142
143 // now imp0>0 imp1<0 143 // now imp0>0 imp1<0
144 imp0 = dv0 / jacA.Diagonal; 144 imp0 = dv0 / jacA.Diagonal;
145 if (imp0 < 0.0f) 145 if (imp0 < 0.0f)
146 imp0 = 0f; 146 imp0 = 0f;
147 } 147 }
148 } 148 }
149 else 149 else
150 { 150 {
151 imp0 = 0f; 151 imp0 = 0f;
152 152
153 imp1 = dv1 / jacB.Diagonal; 153 imp1 = dv1 / jacB.Diagonal;
154 if (imp1 <= 0.0f) 154 if (imp1 <= 0.0f)
155 { 155 {
156 imp1 = 0f; 156 imp1 = 0f;
157 // now imp0>0 imp1<0 157 // now imp0>0 imp1<0
158 imp0 = dv0 / jacA.Diagonal; 158 imp0 = dv0 / jacA.Diagonal;
159 if (imp0 > 0.0f) 159 if (imp0 > 0.0f)
160 { 160 {
161 } 161 }
162 else 162 else
163 { 163 {
164 imp0 = 0f; 164 imp0 = 0f;
165 } 165 }
166 } 166 }
167 } 167 }
168 } 168 }
169 169
170 //public void ResolveAngularConstraint( 170 //public void ResolveAngularConstraint(
171 // Matrix invInertiaAWS, 171 // Matrix invInertiaAWS,
172 // float invMassA, 172 // float invMassA,
173 // Vector3 linvelA, Vector3 angvelA, 173 // Vector3 linvelA, Vector3 angvelA,
174 // Vector3 rel_posA1, 174 // Vector3 rel_posA1,
175 // Matrix invInertiaBWS, 175 // Matrix invInertiaBWS,
176 // float invMassB, 176 // float invMassB,
177 // Vector3 linvelB, Vector3 angvelB, 177 // Vector3 linvelB, Vector3 angvelB,
178 // Vector3 rel_posA2, 178 // Vector3 rel_posA2,
179 // float depthA, Vector3 normalA, 179 // float depthA, Vector3 normalA,
180 // Vector3 rel_posB1, Vector3 rel_posB2, 180 // Vector3 rel_posB1, Vector3 rel_posB2,
181 // float depthB, Vector3 normalB, 181 // float depthB, Vector3 normalB,
182 // out float imp0, out float imp1) 182 // out float imp0, out float imp1)
183 //{ 183 //{
184 // imp0 = 0; 184 // imp0 = 0;
185 // imp1 = 0; 185 // imp1 = 0;
186 //} 186 //}
187 } 187 }
188} 188}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverBody.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverBody.cs
index 09b73cc..6996272 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverBody.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverBody.cs
@@ -1,78 +1,78 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26using System.Runtime.InteropServices; 26using System.Runtime.InteropServices;
27 27
28namespace XnaDevRu.BulletX.Dynamics 28namespace XnaDevRu.BulletX.Dynamics
29{ 29{
30 public class SolverBody 30 public class SolverBody
31 { 31 {
32 private Vector3 _centerOfMassPosition = new Vector3(); 32 private Vector3 _centerOfMassPosition = new Vector3();
33 private Vector3 _linearVelocity = new Vector3(); 33 private Vector3 _linearVelocity = new Vector3();
34 private Vector3 _angularVelocity = new Vector3(); 34 private Vector3 _angularVelocity = new Vector3();
35 private RigidBody _originalBody = null; 35 private RigidBody _originalBody = null;
36 private float _invMass; 36 private float _invMass;
37 private float _friction; 37 private float _friction;
38 private float _angularFactor; 38 private float _angularFactor;
39 39
40 public Vector3 CenterOfMassPosition { get { return _centerOfMassPosition; } set { _centerOfMassPosition = value; } } 40 public Vector3 CenterOfMassPosition { get { return _centerOfMassPosition; } set { _centerOfMassPosition = value; } }
41 public Vector3 LinearVelocity { get { return _linearVelocity; } set { _linearVelocity = value; } } 41 public Vector3 LinearVelocity { get { return _linearVelocity; } set { _linearVelocity = value; } }
42 public Vector3 AngularVelocity { get { return _angularVelocity; } set { _angularVelocity = value; } } 42 public Vector3 AngularVelocity { get { return _angularVelocity; } set { _angularVelocity = value; } }
43 public RigidBody OriginalBody { get { return _originalBody; } set { _originalBody = value; } } 43 public RigidBody OriginalBody { get { return _originalBody; } set { _originalBody = value; } }
44 public float InvMass { get { return _invMass; } set { _invMass = value; } } 44 public float InvMass { get { return _invMass; } set { _invMass = value; } }
45 public float Friction { get { return _friction; } set { _friction = value; } } 45 public float Friction { get { return _friction; } set { _friction = value; } }
46 public float AngularFactor { get { return _angularFactor; } set { _angularFactor = value; } } 46 public float AngularFactor { get { return _angularFactor; } set { _angularFactor = value; } }
47 47
48 public void GetVelocityInLocalPoint(Vector3 relPos, out Vector3 velocity) 48 public void GetVelocityInLocalPoint(Vector3 relPos, out Vector3 velocity)
49 { 49 {
50 velocity = _linearVelocity + Vector3.Cross(_angularVelocity, relPos); 50 velocity = _linearVelocity + Vector3.Cross(_angularVelocity, relPos);
51 } 51 }
52 52
53 public void WriteBackVelocity() 53 public void WriteBackVelocity()
54 { 54 {
55 if (_invMass != 0) 55 if (_invMass != 0)
56 { 56 {
57 _originalBody.LinearVelocity = _linearVelocity; 57 _originalBody.LinearVelocity = _linearVelocity;
58 _originalBody.AngularVelocity = _angularVelocity; 58 _originalBody.AngularVelocity = _angularVelocity;
59 } 59 }
60 } 60 }
61 61
62 public void ReadVelocity() 62 public void ReadVelocity()
63 { 63 {
64 if (_invMass != 0) 64 if (_invMass != 0)
65 { 65 {
66 _linearVelocity = _originalBody.LinearVelocity; 66 _linearVelocity = _originalBody.LinearVelocity;
67 _angularVelocity = _originalBody.AngularVelocity; 67 _angularVelocity = _originalBody.AngularVelocity;
68 } 68 }
69 } 69 }
70 70
71 //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position 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) 72 internal void ApplyImpulse(Vector3 linearComponent, Vector3 angularComponent, float impulseMagnitude)
73 { 73 {
74 _linearVelocity += linearComponent * impulseMagnitude; 74 _linearVelocity += linearComponent * impulseMagnitude;
75 _angularVelocity += angularComponent * impulseMagnitude * _angularFactor; 75 _angularVelocity += angularComponent * impulseMagnitude * _angularFactor;
76 } 76 }
77 } 77 }
78} 78}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverConstraint.cs
index 3e3a64a..4fd0361 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverConstraint.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverConstraint.cs
@@ -1,77 +1,77 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26using System.Runtime.InteropServices; 26using System.Runtime.InteropServices;
27 27
28namespace XnaDevRu.BulletX.Dynamics 28namespace XnaDevRu.BulletX.Dynamics
29{ 29{
30 //1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. 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 31 public class SolverConstraint
32 { 32 {
33 private Vector3 _relpos1CrossNormal = new Vector3(); 33 private Vector3 _relpos1CrossNormal = new Vector3();
34 private Vector3 _relpos2CrossNormal = new Vector3(); 34 private Vector3 _relpos2CrossNormal = new Vector3();
35 private Vector3 _contactNormal = new Vector3(); 35 private Vector3 _contactNormal = new Vector3();
36 private Vector3 _angularComponentA = new Vector3(); 36 private Vector3 _angularComponentA = new Vector3();
37 private Vector3 _angularComponentB = new Vector3(); 37 private Vector3 _angularComponentB = new Vector3();
38 38
39 private float _appliedVelocityImpulse; 39 private float _appliedVelocityImpulse;
40 private int _solverBodyIdA; 40 private int _solverBodyIdA;
41 int _solverBodyIdB; 41 int _solverBodyIdB;
42 private float _friction; 42 private float _friction;
43 private float _restitution; 43 private float _restitution;
44 private float _jacDiagABInv; 44 private float _jacDiagABInv;
45 private float _penetration; 45 private float _penetration;
46 private float _appliedImpulse; 46 private float _appliedImpulse;
47 47
48 private SolverConstraintType _constraintType = SolverConstraintType.Contact; 48 private SolverConstraintType _constraintType = SolverConstraintType.Contact;
49 private int _frictionIndex; 49 private int _frictionIndex;
50 private int[] _unusedPadding = new int[2]; 50 private int[] _unusedPadding = new int[2];
51 51
52 public Vector3 RelPosACrossNormal { get { return _relpos1CrossNormal; } set { _relpos1CrossNormal = value; } } 52 public Vector3 RelPosACrossNormal { get { return _relpos1CrossNormal; } set { _relpos1CrossNormal = value; } }
53 public Vector3 RelPosBCrossNormal { get { return _relpos2CrossNormal; } set { _relpos2CrossNormal = value; } } 53 public Vector3 RelPosBCrossNormal { get { return _relpos2CrossNormal; } set { _relpos2CrossNormal = value; } }
54 public Vector3 ContactNormal { get { return _contactNormal; } set { _contactNormal = value; } } 54 public Vector3 ContactNormal { get { return _contactNormal; } set { _contactNormal = value; } }
55 public Vector3 AngularComponentA { get { return _angularComponentA; } set { _angularComponentA = value; } } 55 public Vector3 AngularComponentA { get { return _angularComponentA; } set { _angularComponentA = value; } }
56 public Vector3 AngularComponentB { get { return _angularComponentB; } set { _angularComponentB = value; } } 56 public Vector3 AngularComponentB { get { return _angularComponentB; } set { _angularComponentB = value; } }
57 57
58 public float AppliedVelocityImpulse { get { return _appliedVelocityImpulse; } set { _appliedVelocityImpulse = value; } } 58 public float AppliedVelocityImpulse { get { return _appliedVelocityImpulse; } set { _appliedVelocityImpulse = value; } }
59 public int SolverBodyIdA { get { return _solverBodyIdA; } set { _solverBodyIdA = value; } } 59 public int SolverBodyIdA { get { return _solverBodyIdA; } set { _solverBodyIdA = value; } }
60 public int SolverBodyIdB { get { return _solverBodyIdB; } set { _solverBodyIdB = value; } } 60 public int SolverBodyIdB { get { return _solverBodyIdB; } set { _solverBodyIdB = value; } }
61 public float Friction { get { return _friction; } set { _friction = value; } } 61 public float Friction { get { return _friction; } set { _friction = value; } }
62 public float Restitution { get { return _restitution; } set { _restitution = value; } } 62 public float Restitution { get { return _restitution; } set { _restitution = value; } }
63 public float JacDiagABInv { get { return _jacDiagABInv; } set { _jacDiagABInv = value; } } 63 public float JacDiagABInv { get { return _jacDiagABInv; } set { _jacDiagABInv = value; } }
64 public float Penetration { get { return _penetration; } set { _penetration = value; } } 64 public float Penetration { get { return _penetration; } set { _penetration = value; } }
65 public float AppliedImpulse { get { return _appliedImpulse; } set { _appliedImpulse = value; } } 65 public float AppliedImpulse { get { return _appliedImpulse; } set { _appliedImpulse = value; } }
66 66
67 public SolverConstraintType ConstraintType { get { return _constraintType; } set { _constraintType = value; } } 67 public SolverConstraintType ConstraintType { get { return _constraintType; } set { _constraintType = value; } }
68 public int FrictionIndex { get { return _frictionIndex; } set { _frictionIndex = value; } } 68 public int FrictionIndex { get { return _frictionIndex; } set { _frictionIndex = value; } }
69 public int[] UnusedPadding { get { return _unusedPadding; } set { _unusedPadding = value; } } 69 public int[] UnusedPadding { get { return _unusedPadding; } set { _unusedPadding = value; } }
70 70
71 public enum SolverConstraintType 71 public enum SolverConstraintType
72 { 72 {
73 Contact = 0, 73 Contact = 0,
74 Friction, 74 Friction,
75 } 75 }
76 } 76 }
77} 77}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/TypedConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/TypedConstraint.cs
index abd10f1..8131463 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/TypedConstraint.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/TypedConstraint.cs
@@ -1,88 +1,88 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX.Dynamics 27namespace XnaDevRu.BulletX.Dynamics
28{ 28{
29 public abstract class TypedConstraint 29 public abstract class TypedConstraint
30 { 30 {
31 private static RigidBody _fixed = new RigidBody(0, null, null, new Vector3(), 0, 0, 0.5f, 0); 31 private static RigidBody _fixed = new RigidBody(0, null, null, new Vector3(), 0, 0, 0.5f, 0);
32 private int _userConstraintType; 32 private int _userConstraintType;
33 private int _userConstraintId; 33 private int _userConstraintId;
34 34
35 private RigidBody _rbA; 35 private RigidBody _rbA;
36 private RigidBody _rbB; 36 private RigidBody _rbB;
37 private float _appliedImpulse; 37 private float _appliedImpulse;
38 38
39 public TypedConstraint() 39 public TypedConstraint()
40 : this(_fixed, _fixed) { } 40 : this(_fixed, _fixed) { }
41 41
42 public TypedConstraint(RigidBody rbA) 42 public TypedConstraint(RigidBody rbA)
43 : this(rbA, _fixed) { } 43 : this(rbA, _fixed) { }
44 44
45 public TypedConstraint(RigidBody rbA, RigidBody rbB) 45 public TypedConstraint(RigidBody rbA, RigidBody rbB)
46 { 46 {
47 _userConstraintType = -1; 47 _userConstraintType = -1;
48 _userConstraintId = -1; 48 _userConstraintId = -1;
49 _rbA = rbA; 49 _rbA = rbA;
50 _rbB = rbB; 50 _rbB = rbB;
51 _appliedImpulse = 0; 51 _appliedImpulse = 0;
52 52
53 _fixed.SetMassProps(0, new Vector3()); 53 _fixed.SetMassProps(0, new Vector3());
54 } 54 }
55 55
56 public virtual RigidBody RigidBodyA { get { return _rbA; } protected set { _rbA = value; } } 56 public virtual RigidBody RigidBodyA { get { return _rbA; } protected set { _rbA = value; } }
57 public virtual RigidBody RigidBodyB { get { return _rbB; } protected set { _rbB = value; } } 57 public virtual RigidBody RigidBodyB { get { return _rbB; } protected set { _rbB = value; } }
58 58
59 public float AppliedImpulse { get { return _appliedImpulse; } protected set { _appliedImpulse = value; } } 59 public float AppliedImpulse { get { return _appliedImpulse; } protected set { _appliedImpulse = value; } }
60 public int UserConstraintId { get { return _userConstraintId; } set { _userConstraintId = value; } } 60 public int UserConstraintId { get { return _userConstraintId; } set { _userConstraintId = value; } }
61 public int UserConstraintType { get { return _userConstraintType; } set { _userConstraintType = value; } } 61 public int UserConstraintType { get { return _userConstraintType; } set { _userConstraintType = value; } }
62 62
63 public abstract void BuildJacobian(); 63 public abstract void BuildJacobian();
64 public abstract void SolveConstraint(float timeStep); 64 public abstract void SolveConstraint(float timeStep);
65 65
66 public static int SortConstraintOnIslandPredicate(TypedConstraint left, TypedConstraint right) 66 public static int SortConstraintOnIslandPredicate(TypedConstraint left, TypedConstraint right)
67 { 67 {
68 int rightIslandID, leftIslandID; 68 int rightIslandID, leftIslandID;
69 rightIslandID = GetConstraintIslandId(right); 69 rightIslandID = GetConstraintIslandId(right);
70 leftIslandID = GetConstraintIslandId(left); 70 leftIslandID = GetConstraintIslandId(left);
71 if (leftIslandID < rightIslandID) 71 if (leftIslandID < rightIslandID)
72 return -1; 72 return -1;
73 else 73 else
74 return 1; 74 return 1;
75 return 0; 75 return 0;
76 } 76 }
77 77
78 internal static int GetConstraintIslandId(TypedConstraint lhs) 78 internal static int GetConstraintIslandId(TypedConstraint lhs)
79 { 79 {
80 int islandId; 80 int islandId;
81 81
82 CollisionObject colObjA = lhs.RigidBodyA; 82 CollisionObject colObjA = lhs.RigidBodyA;
83 CollisionObject colObjB = lhs.RigidBodyB; 83 CollisionObject colObjB = lhs.RigidBodyB;
84 islandId = colObjA.IslandTag >= 0 ? colObjA.IslandTag : colObjB.IslandTag; 84 islandId = colObjA.IslandTag >= 0 ? colObjA.IslandTag : colObjB.IslandTag;
85 return islandId; 85 return islandId;
86 } 86 }
87 } 87 }
88} 88}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs
index acceb98..3702c2a 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs
@@ -1,790 +1,790 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX.Dynamics 27namespace XnaDevRu.BulletX.Dynamics
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// DiscreteDynamicsWorld provides discrete rigid body simulation 30 /// DiscreteDynamicsWorld provides discrete rigid body simulation
31 /// those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController 31 /// those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController
32 /// </summary> 32 /// </summary>
33 public class DiscreteDynamicsWorld : DynamicsWorld 33 public class DiscreteDynamicsWorld : DynamicsWorld
34 { 34 {
35 private static bool _reportMe = true; 35 private static bool _reportMe = true;
36 36
37 private IConstraintSolver _constraintSolver; 37 private IConstraintSolver _constraintSolver;
38 private SimulationIslandManager _islandManager; 38 private SimulationIslandManager _islandManager;
39 private List<TypedConstraint> _constraints = new List<TypedConstraint>(); 39 private List<TypedConstraint> _constraints = new List<TypedConstraint>();
40 private IDebugDraw _debugDrawer; 40 private IDebugDraw _debugDrawer;
41 private ContactSolverInfo _solverInfo = new ContactSolverInfo(); 41 private ContactSolverInfo _solverInfo = new ContactSolverInfo();
42 private Vector3 _gravity; 42 private Vector3 _gravity;
43 //for variable timesteps 43 //for variable timesteps
44 private float _localTime; 44 private float _localTime;
45 //for variable timesteps 45 //for variable timesteps
46 private bool _ownsIslandManager; 46 private bool _ownsIslandManager;
47 private bool _ownsConstraintSolver; 47 private bool _ownsConstraintSolver;
48 private List<RaycastVehicle> _vehicles = new List<RaycastVehicle>(); 48 private List<RaycastVehicle> _vehicles = new List<RaycastVehicle>();
49 private int _profileTimings; 49 private int _profileTimings;
50 50
51 public DiscreteDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache) 51 public DiscreteDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache)
52 : this(dispatcher, pairCache, null) { } 52 : this(dispatcher, pairCache, null) { }
53 53
54 //this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those 54 //this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those
55 public DiscreteDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache, IConstraintSolver constraintSolver) 55 public DiscreteDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache, IConstraintSolver constraintSolver)
56 : base(dispatcher, pairCache) 56 : base(dispatcher, pairCache)
57 { 57 {
58 _constraintSolver = constraintSolver != null ? constraintSolver : new SequentialImpulseConstraintSolver(); 58 _constraintSolver = constraintSolver != null ? constraintSolver : new SequentialImpulseConstraintSolver();
59 _debugDrawer = null; 59 _debugDrawer = null;
60 _gravity = new Vector3(0, -10, 0); 60 _gravity = new Vector3(0, -10, 0);
61 _localTime = 1f / 60f; 61 _localTime = 1f / 60f;
62 _profileTimings = 0; 62 _profileTimings = 0;
63 _islandManager = new SimulationIslandManager(); 63 _islandManager = new SimulationIslandManager();
64 _ownsIslandManager = true; 64 _ownsIslandManager = true;
65 _ownsConstraintSolver = constraintSolver == null; 65 _ownsConstraintSolver = constraintSolver == null;
66 } 66 }
67 67
68 public ContactSolverInfo SolverInfo { get { return _solverInfo; } } 68 public ContactSolverInfo SolverInfo { get { return _solverInfo; } }
69 public SimulationIslandManager SimulationIslandManager { get { return _islandManager; } } 69 public SimulationIslandManager SimulationIslandManager { get { return _islandManager; } }
70 public CollisionWorld CollisionWorld { get { return this; } } 70 public CollisionWorld CollisionWorld { get { return this; } }
71 71
72 public override IDebugDraw DebugDrawer 72 public override IDebugDraw DebugDrawer
73 { 73 {
74 get 74 get
75 { 75 {
76 return _debugDrawer; 76 return _debugDrawer;
77 } 77 }
78 set 78 set
79 { 79 {
80 _debugDrawer = value; 80 _debugDrawer = value;
81 } 81 }
82 } 82 }
83 83
84 public override Vector3 Gravity 84 public override Vector3 Gravity
85 { 85 {
86 set 86 set
87 { 87 {
88 _gravity = value; 88 _gravity = value;
89 for (int i = 0; i < CollisionObjects.Count; i++) 89 for (int i = 0; i < CollisionObjects.Count; i++)
90 { 90 {
91 CollisionObject colObj = CollisionObjects[i]; 91 CollisionObject colObj = CollisionObjects[i];
92 RigidBody body = RigidBody.Upcast(colObj); 92 RigidBody body = RigidBody.Upcast(colObj);
93 if (body != null) 93 if (body != null)
94 { 94 {
95 body.Gravity = value; 95 body.Gravity = value;
96 } 96 }
97 } 97 }
98 } 98 }
99 } 99 }
100 100
101 public override IConstraintSolver ConstraintSolver 101 public override IConstraintSolver ConstraintSolver
102 { 102 {
103 set 103 set
104 { 104 {
105 _ownsConstraintSolver = false; 105 _ownsConstraintSolver = false;
106 _constraintSolver = value; 106 _constraintSolver = value;
107 } 107 }
108 } 108 }
109 109
110 public override int ConstraintsCount 110 public override int ConstraintsCount
111 { 111 {
112 get 112 get
113 { 113 {
114 return _constraints.Count; 114 return _constraints.Count;
115 } 115 }
116 } 116 }
117 117
118 //if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's 118 //if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's
119 public override void StepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep) 119 public override void StepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep)
120 { 120 {
121 int numSimulationSubSteps = 0; 121 int numSimulationSubSteps = 0;
122 122
123 if (maxSubSteps != 0) 123 if (maxSubSteps != 0)
124 { 124 {
125 //fixed timestep with interpolation 125 //fixed timestep with interpolation
126 _localTime += timeStep; 126 _localTime += timeStep;
127 if (_localTime >= fixedTimeStep) 127 if (_localTime >= fixedTimeStep)
128 { 128 {
129 numSimulationSubSteps = (int)(_localTime / fixedTimeStep); 129 numSimulationSubSteps = (int)(_localTime / fixedTimeStep);
130 _localTime -= numSimulationSubSteps * fixedTimeStep; 130 _localTime -= numSimulationSubSteps * fixedTimeStep;
131 } 131 }
132 } 132 }
133 else 133 else
134 { 134 {
135 //variable timestep 135 //variable timestep
136 fixedTimeStep = timeStep; 136 fixedTimeStep = timeStep;
137 _localTime = timeStep; 137 _localTime = timeStep;
138 if (Math.Abs(timeStep) < float.Epsilon) 138 if (Math.Abs(timeStep) < float.Epsilon)
139 { 139 {
140 numSimulationSubSteps = 0; 140 numSimulationSubSteps = 0;
141 maxSubSteps = 0; 141 maxSubSteps = 0;
142 } 142 }
143 else 143 else
144 { 144 {
145 numSimulationSubSteps = 1; 145 numSimulationSubSteps = 1;
146 maxSubSteps = 1; 146 maxSubSteps = 1;
147 } 147 }
148 } 148 }
149 149
150 //process some debugging flags 150 //process some debugging flags
151 if (DebugDrawer != null) 151 if (DebugDrawer != null)
152 { 152 {
153 RigidBody.DisableDeactivation = (DebugDrawer.DebugMode & DebugDrawModes.NoDeactivation) != 0; 153 RigidBody.DisableDeactivation = (DebugDrawer.DebugMode & DebugDrawModes.NoDeactivation) != 0;
154 } 154 }
155 if (numSimulationSubSteps != 0) 155 if (numSimulationSubSteps != 0)
156 { 156 {
157 157
158 SaveKinematicState(fixedTimeStep); 158 SaveKinematicState(fixedTimeStep);
159 159
160 //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt 160 //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
161 int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps; 161 int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps;
162 162
163 for (int i = 0; i < clampedSimulationSteps; i++) 163 for (int i = 0; i < clampedSimulationSteps; i++)
164 { 164 {
165 InternalSingleStepSimulation(fixedTimeStep); 165 InternalSingleStepSimulation(fixedTimeStep);
166 SynchronizeMotionStates(); 166 SynchronizeMotionStates();
167 } 167 }
168 168
169 } 169 }
170 170
171 SynchronizeMotionStates(); 171 SynchronizeMotionStates();
172 172
173 //return numSimulationSubSteps; 173 //return numSimulationSubSteps;
174 } 174 }
175 175
176 public void StepSimulation(float timeStep, int maxSubSteps) 176 public void StepSimulation(float timeStep, int maxSubSteps)
177 { 177 {
178 StepSimulation(timeStep, maxSubSteps, 1f / 60f); 178 StepSimulation(timeStep, maxSubSteps, 1f / 60f);
179 } 179 }
180 180
181 public override void UpdateAabbs() 181 public override void UpdateAabbs()
182 { 182 {
183 Vector3 colorvec = new Vector3(1, 0, 0); 183 Vector3 colorvec = new Vector3(1, 0, 0);
184 for (int i = 0; i < CollisionObjects.Count; i++) 184 for (int i = 0; i < CollisionObjects.Count; i++)
185 { 185 {
186 CollisionObject colObj = CollisionObjects[i]; 186 CollisionObject colObj = CollisionObjects[i];
187 RigidBody body = RigidBody.Upcast(colObj); 187 RigidBody body = RigidBody.Upcast(colObj);
188 188
189 if (body != null) 189 if (body != null)
190 { 190 {
191 // if (body->IsActive() && (!body->IsStatic())) 191 // if (body->IsActive() && (!body->IsStatic()))
192 { 192 {
193 Vector3 minAabb, maxAabb; 193 Vector3 minAabb, maxAabb;
194 colObj.CollisionShape.GetAabb(colObj.WorldTransform, out minAabb, out maxAabb); 194 colObj.CollisionShape.GetAabb(colObj.WorldTransform, out minAabb, out maxAabb);
195 OverlappingPairCache bp = BroadphasePairCache; 195 OverlappingPairCache bp = BroadphasePairCache;
196 196
197 //moving objects should be moderately sized, probably something wrong if not 197 //moving objects should be moderately sized, probably something wrong if not
198 if (colObj.IsStaticObject || ((maxAabb - minAabb).LengthSquared() < 1e12f)) 198 if (colObj.IsStaticObject || ((maxAabb - minAabb).LengthSquared() < 1e12f))
199 { 199 {
200 bp.SetAabb(body.Broadphase, minAabb, maxAabb); 200 bp.SetAabb(body.Broadphase, minAabb, maxAabb);
201 } 201 }
202 else 202 else
203 { 203 {
204 //something went wrong, investigate 204 //something went wrong, investigate
205 //this assert is unwanted in 3D modelers (danger of loosing work) 205 //this assert is unwanted in 3D modelers (danger of loosing work)
206 BulletDebug.Assert(false); 206 BulletDebug.Assert(false);
207 body.ActivationState = ActivationState.DisableSimulation; 207 body.ActivationState = ActivationState.DisableSimulation;
208 208
209 if (_reportMe) 209 if (_reportMe)
210 { 210 {
211 _reportMe = false; 211 _reportMe = false;
212 Console.WriteLine("Overflow in AABB, object removed from simulation \n"); 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"); 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"); 214 Console.WriteLine("Please include above information, your Platform, version of OS.\n");
215 Console.WriteLine("Thanks.\n"); 215 Console.WriteLine("Thanks.\n");
216 } 216 }
217 } 217 }
218 if (_debugDrawer != null && (_debugDrawer.DebugMode & DebugDrawModes.DrawAabb) != 0) 218 if (_debugDrawer != null && (_debugDrawer.DebugMode & DebugDrawModes.DrawAabb) != 0)
219 DrawAabb(_debugDrawer, minAabb, maxAabb, colorvec); 219 DrawAabb(_debugDrawer, minAabb, maxAabb, colorvec);
220 } 220 }
221 } 221 }
222 } 222 }
223 } 223 }
224 224
225 public override void AddConstraint(TypedConstraint constraint) 225 public override void AddConstraint(TypedConstraint constraint)
226 { 226 {
227 _constraints.Add(constraint); 227 _constraints.Add(constraint);
228 } 228 }
229 229
230 public override void RemoveConstraint(TypedConstraint constraint) 230 public override void RemoveConstraint(TypedConstraint constraint)
231 { 231 {
232 _constraints.Remove(constraint); 232 _constraints.Remove(constraint);
233 } 233 }
234 234
235 public void AddVehicle(RaycastVehicle vehicle) 235 public void AddVehicle(RaycastVehicle vehicle)
236 { 236 {
237 _vehicles.Add(vehicle); 237 _vehicles.Add(vehicle);
238 } 238 }
239 239
240 public void RemoveVehicle(RaycastVehicle vehicle) 240 public void RemoveVehicle(RaycastVehicle vehicle)
241 { 241 {
242 _vehicles.Remove(vehicle); 242 _vehicles.Remove(vehicle);
243 } 243 }
244 244
245 public override void AddRigidBody(RigidBody body) 245 public override void AddRigidBody(RigidBody body)
246 { 246 {
247 if (!body.IsStaticOrKinematicObject) 247 if (!body.IsStaticOrKinematicObject)
248 { 248 {
249 body.Gravity = _gravity; 249 body.Gravity = _gravity;
250 } 250 }
251 251
252 if (body.CollisionShape != null) 252 if (body.CollisionShape != null)
253 { 253 {
254 bool isDynamic = !(body.IsStaticObject || body.IsKinematicObject); 254 bool isDynamic = !(body.IsStaticObject || body.IsKinematicObject);
255 BroadphaseProxy.CollisionFilterGroups collisionFilterGroup = isDynamic ? BroadphaseProxy.CollisionFilterGroups.Default : BroadphaseProxy.CollisionFilterGroups.Static; 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); 256 BroadphaseProxy.CollisionFilterGroups collisionFilterMask = isDynamic ? BroadphaseProxy.CollisionFilterGroups.All : (BroadphaseProxy.CollisionFilterGroups.All ^ BroadphaseProxy.CollisionFilterGroups.Static);
257 257
258 AddCollisionObject(body, collisionFilterGroup, collisionFilterMask); 258 AddCollisionObject(body, collisionFilterGroup, collisionFilterMask);
259 } 259 }
260 } 260 }
261 261
262 public override void RemoveRigidBody(RigidBody body) 262 public override void RemoveRigidBody(RigidBody body)
263 { 263 {
264 RemoveCollisionObject(body); 264 RemoveCollisionObject(body);
265 } 265 }
266 266
267 public void DebugDrawObject(Matrix worldTransform, CollisionShape shape, Vector3 color) 267 public void DebugDrawObject(Matrix worldTransform, CollisionShape shape, Vector3 color)
268 { 268 {
269 if (shape.ShapeType == BroadphaseNativeTypes.Compound) 269 if (shape.ShapeType == BroadphaseNativeTypes.Compound)
270 { 270 {
271 CompoundShape compoundShape = shape as CompoundShape; 271 CompoundShape compoundShape = shape as CompoundShape;
272 for (int i = compoundShape.ChildShapeCount - 1; i >= 0; i--) 272 for (int i = compoundShape.ChildShapeCount - 1; i >= 0; i--)
273 { 273 {
274 Matrix childTrans = compoundShape.GetChildTransform(i); 274 Matrix childTrans = compoundShape.GetChildTransform(i);
275 CollisionShape colShape = compoundShape.GetChildShape(i); 275 CollisionShape colShape = compoundShape.GetChildShape(i);
276 DebugDrawObject(worldTransform * childTrans, colShape, color); 276 DebugDrawObject(worldTransform * childTrans, colShape, color);
277 } 277 }
278 278
279 } 279 }
280 else 280 else
281 { 281 {
282 switch (shape.ShapeType) 282 switch (shape.ShapeType)
283 { 283 {
284 284
285 case BroadphaseNativeTypes.Sphere: 285 case BroadphaseNativeTypes.Sphere:
286 { 286 {
287 SphereShape sphereShape = shape as SphereShape; 287 SphereShape sphereShape = shape as SphereShape;
288 float radius = sphereShape.Margin;//radius doesn't include the margin, so draw with margin 288 float radius = sphereShape.Margin;//radius doesn't include the margin, so draw with margin
289 Vector3 start = worldTransform.Translation; 289 Vector3 start = worldTransform.Translation;
290 DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(radius, 0, 0), worldTransform), color); 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); 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); 292 DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(0, 0, radius), worldTransform), color);
293 //drawSphere 293 //drawSphere
294 break; 294 break;
295 } 295 }
296 case BroadphaseNativeTypes.MultiSphere: 296 case BroadphaseNativeTypes.MultiSphere:
297 case BroadphaseNativeTypes.Cone: 297 case BroadphaseNativeTypes.Cone:
298 { 298 {
299 ConeShape coneShape = shape as ConeShape; 299 ConeShape coneShape = shape as ConeShape;
300 float radius = coneShape.Radius;//+coneShape->getMargin(); 300 float radius = coneShape.Radius;//+coneShape->getMargin();
301 float height = coneShape.Height;//+coneShape->getMargin(); 301 float height = coneShape.Height;//+coneShape->getMargin();
302 Vector3 start = worldTransform.Translation; 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); 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); 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); 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); 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; 307 break;
308 } 308 }
309 case BroadphaseNativeTypes.Cylinder: 309 case BroadphaseNativeTypes.Cylinder:
310 { 310 {
311 CylinderShape cylinder = shape as CylinderShape; 311 CylinderShape cylinder = shape as CylinderShape;
312 int upAxis = cylinder.UpAxis; 312 int upAxis = cylinder.UpAxis;
313 float radius = cylinder.Radius; 313 float radius = cylinder.Radius;
314 float halfHeight = MathHelper.GetElement(cylinder.HalfExtents, upAxis); 314 float halfHeight = MathHelper.GetElement(cylinder.HalfExtents, upAxis);
315 Vector3 start = worldTransform.Translation; 315 Vector3 start = worldTransform.Translation;
316 Vector3 offsetHeight = new Vector3(); 316 Vector3 offsetHeight = new Vector3();
317 MathHelper.SetElement(ref offsetHeight, upAxis, halfHeight); 317 MathHelper.SetElement(ref offsetHeight, upAxis, halfHeight);
318 Vector3 offsetRadius = new Vector3(); 318 Vector3 offsetRadius = new Vector3();
319 MathHelper.SetElement(ref offsetRadius, (upAxis + 1) % 3, radius); 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); 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); 321 DebugDrawer.DrawLine(start + Vector3.TransformNormal(offsetHeight - offsetRadius, worldTransform), start + Vector3.TransformNormal(-offsetHeight - offsetRadius, worldTransform), color);
322 break; 322 break;
323 } 323 }
324 default: 324 default:
325 { 325 {
326 if (shape.ShapeType == BroadphaseNativeTypes.TriangleMesh) 326 if (shape.ShapeType == BroadphaseNativeTypes.TriangleMesh)
327 { 327 {
328 TriangleMeshShape concaveMesh = shape as TriangleMeshShape; 328 TriangleMeshShape concaveMesh = shape as TriangleMeshShape;
329 //btVector3 aabbMax(1e30f,1e30f,1e30f); 329 //btVector3 aabbMax(1e30f,1e30f,1e30f);
330 //btVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f); 330 //btVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f);
331 331
332 //todo pass camera, for some culling 332 //todo pass camera, for some culling
333 Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); 333 Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
334 Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); 334 Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f);
335 335
336 DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color); 336 DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color);
337 concaveMesh.ProcessAllTriangles(drawCallback, aabbMin, aabbMax); 337 concaveMesh.ProcessAllTriangles(drawCallback, aabbMin, aabbMax);
338 } 338 }
339 339
340 if (shape.ShapeType == BroadphaseNativeTypes.ConvexTriangleMesh) 340 if (shape.ShapeType == BroadphaseNativeTypes.ConvexTriangleMesh)
341 { 341 {
342 ConvexTriangleMeshShape convexMesh = shape as ConvexTriangleMeshShape; 342 ConvexTriangleMeshShape convexMesh = shape as ConvexTriangleMeshShape;
343 //todo: pass camera for some culling 343 //todo: pass camera for some culling
344 Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); 344 Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
345 Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); 345 Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f);
346 //DebugDrawcallback drawCallback; 346 //DebugDrawcallback drawCallback;
347 DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color); 347 DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color);
348 convexMesh.getStridingMesh().InternalProcessAllTriangles(drawCallback, aabbMin, aabbMax); 348 convexMesh.getStridingMesh().InternalProcessAllTriangles(drawCallback, aabbMin, aabbMax);
349 } 349 }
350 350
351 // for polyhedral shapes 351 // for polyhedral shapes
352 if (shape.IsPolyhedral) 352 if (shape.IsPolyhedral)
353 { 353 {
354 PolyhedralConvexShape polyshape = shape as PolyhedralConvexShape; 354 PolyhedralConvexShape polyshape = shape as PolyhedralConvexShape;
355 355
356 for (int i = 0; i < polyshape.EdgeCount; i++) 356 for (int i = 0; i < polyshape.EdgeCount; i++)
357 { 357 {
358 Vector3 a, b; 358 Vector3 a, b;
359 polyshape.GetEdge(i, out a, out b); 359 polyshape.GetEdge(i, out a, out b);
360 a = Vector3.TransformNormal(a, worldTransform); 360 a = Vector3.TransformNormal(a, worldTransform);
361 b = Vector3.TransformNormal(b, worldTransform); 361 b = Vector3.TransformNormal(b, worldTransform);
362 DebugDrawer.DrawLine(a, b, color); 362 DebugDrawer.DrawLine(a, b, color);
363 } 363 }
364 } 364 }
365 break; 365 break;
366 } 366 }
367 } 367 }
368 } 368 }
369 } 369 }
370 370
371 public override TypedConstraint GetConstraint(int index) 371 public override TypedConstraint GetConstraint(int index)
372 { 372 {
373 return _constraints[index]; 373 return _constraints[index];
374 } 374 }
375 375
376 public static void DrawAabb(IDebugDraw debugDrawer, Vector3 from, Vector3 to, Vector3 color) 376 public static void DrawAabb(IDebugDraw debugDrawer, Vector3 from, Vector3 to, Vector3 color)
377 { 377 {
378 Vector3 halfExtents = (to - from) * 0.5f; 378 Vector3 halfExtents = (to - from) * 0.5f;
379 Vector3 center = (to + from) * 0.5f; 379 Vector3 center = (to + from) * 0.5f;
380 380
381 Vector3 edgecoord = new Vector3(1f, 1f, 1f), pa, pb; 381 Vector3 edgecoord = new Vector3(1f, 1f, 1f), pa, pb;
382 for (int i = 0; i < 4; i++) 382 for (int i = 0; i < 4; i++)
383 { 383 {
384 for (int j = 0; j < 3; j++) 384 for (int j = 0; j < 3; j++)
385 { 385 {
386 pa = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y, 386 pa = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y,
387 edgecoord.Z * halfExtents.Z); 387 edgecoord.Z * halfExtents.Z);
388 pa += center; 388 pa += center;
389 389
390 int othercoord = j % 3; 390 int othercoord = j % 3;
391 MathHelper.SetElement(ref edgecoord, othercoord, MathHelper.GetElement(edgecoord, othercoord) * -1f); 391 MathHelper.SetElement(ref edgecoord, othercoord, MathHelper.GetElement(edgecoord, othercoord) * -1f);
392 pb = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y, 392 pb = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y,
393 edgecoord.Z * halfExtents.Z); 393 edgecoord.Z * halfExtents.Z);
394 pb += center; 394 pb += center;
395 395
396 debugDrawer.DrawLine(pa, pb, color); 396 debugDrawer.DrawLine(pa, pb, color);
397 } 397 }
398 edgecoord = new Vector3(-1f, -1f, -1f); 398 edgecoord = new Vector3(-1f, -1f, -1f);
399 if (i < 3) 399 if (i < 3)
400 MathHelper.SetElement(ref edgecoord, i, MathHelper.GetElement(edgecoord, i) * -1f); 400 MathHelper.SetElement(ref edgecoord, i, MathHelper.GetElement(edgecoord, i) * -1f);
401 } 401 }
402 } 402 }
403 403
404 protected void PredictUnconstraintMotion(float timeStep) 404 protected void PredictUnconstraintMotion(float timeStep)
405 { 405 {
406 for (int i = 0; i < CollisionObjects.Count; i++) 406 for (int i = 0; i < CollisionObjects.Count; i++)
407 { 407 {
408 CollisionObject colObj = CollisionObjects[i]; 408 CollisionObject colObj = CollisionObjects[i];
409 RigidBody body = RigidBody.Upcast(colObj); 409 RigidBody body = RigidBody.Upcast(colObj);
410 if (body != null) 410 if (body != null)
411 { 411 {
412 if (!body.IsStaticOrKinematicObject) 412 if (!body.IsStaticOrKinematicObject)
413 { 413 {
414 if (body.IsActive) 414 if (body.IsActive)
415 { 415 {
416 body.ApplyForces(timeStep); 416 body.ApplyForces(timeStep);
417 body.IntegrateVelocities(timeStep); 417 body.IntegrateVelocities(timeStep);
418 Matrix temp = body.InterpolationWorldTransform; 418 Matrix temp = body.InterpolationWorldTransform;
419 body.PredictIntegratedTransform(timeStep, ref temp); 419 body.PredictIntegratedTransform(timeStep, ref temp);
420 body.InterpolationWorldTransform = temp; 420 body.InterpolationWorldTransform = temp;
421 } 421 }
422 } 422 }
423 } 423 }
424 } 424 }
425 } 425 }
426 426
427 protected void IntegrateTransforms(float timeStep) 427 protected void IntegrateTransforms(float timeStep)
428 { 428 {
429 Matrix predictedTrans = new Matrix(); 429 Matrix predictedTrans = new Matrix();
430 for (int i = 0; i < CollisionObjects.Count; i++) 430 for (int i = 0; i < CollisionObjects.Count; i++)
431 { 431 {
432 CollisionObject colObj = CollisionObjects[i]; 432 CollisionObject colObj = CollisionObjects[i];
433 RigidBody body = RigidBody.Upcast(colObj); 433 RigidBody body = RigidBody.Upcast(colObj);
434 if (body != null) 434 if (body != null)
435 { 435 {
436 if (body.IsActive && (!body.IsStaticOrKinematicObject)) 436 if (body.IsActive && (!body.IsStaticOrKinematicObject))
437 { 437 {
438 body.PredictIntegratedTransform(timeStep, ref predictedTrans); 438 body.PredictIntegratedTransform(timeStep, ref predictedTrans);
439 body.ProceedToTransform(predictedTrans); 439 body.ProceedToTransform(predictedTrans);
440 } 440 }
441 } 441 }
442 } 442 }
443 } 443 }
444 444
445 protected void CalculateSimulationIslands() 445 protected void CalculateSimulationIslands()
446 { 446 {
447 SimulationIslandManager.UpdateActivationState(this, Dispatcher); 447 SimulationIslandManager.UpdateActivationState(this, Dispatcher);
448 448
449 for (int i = 0; i < _constraints.Count; i++) 449 for (int i = 0; i < _constraints.Count; i++)
450 { 450 {
451 TypedConstraint constraint = _constraints[i]; 451 TypedConstraint constraint = _constraints[i];
452 452
453 RigidBody colObj0 = constraint.RigidBodyA; 453 RigidBody colObj0 = constraint.RigidBodyA;
454 RigidBody colObj1 = constraint.RigidBodyB; 454 RigidBody colObj1 = constraint.RigidBodyB;
455 455
456 if (((colObj0 != null) && (colObj0.MergesSimulationIslands)) && 456 if (((colObj0 != null) && (colObj0.MergesSimulationIslands)) &&
457 ((colObj1 != null) && (colObj1.MergesSimulationIslands))) 457 ((colObj1 != null) && (colObj1.MergesSimulationIslands)))
458 { 458 {
459 if (colObj0.IsActive || colObj1.IsActive) 459 if (colObj0.IsActive || colObj1.IsActive)
460 { 460 {
461 461
462 SimulationIslandManager.UnionFind.Unite((colObj0).IslandTag, 462 SimulationIslandManager.UnionFind.Unite((colObj0).IslandTag,
463 (colObj1).IslandTag); 463 (colObj1).IslandTag);
464 } 464 }
465 } 465 }
466 } 466 }
467 467
468 //Store the island id in each body 468 //Store the island id in each body
469 SimulationIslandManager.StoreIslandActivationState(this); 469 SimulationIslandManager.StoreIslandActivationState(this);
470 } 470 }
471 471
472 //protected void SolveNonContactConstraints(ContactSolverInfo solverInfo) 472 //protected void SolveNonContactConstraints(ContactSolverInfo solverInfo)
473 //{ 473 //{
474 // //constraint preparation: building jacobians 474 // //constraint preparation: building jacobians
475 // for (int i = 0; i < _constraints.Count; i++) 475 // for (int i = 0; i < _constraints.Count; i++)
476 // { 476 // {
477 // TypedConstraint constraint = _constraints[i]; 477 // TypedConstraint constraint = _constraints[i];
478 // constraint.BuildJacobian(); 478 // constraint.BuildJacobian();
479 // } 479 // }
480 480
481 // //solve the regular non-contact constraints (point 2 point, hinge, generic d6) 481 // //solve the regular non-contact constraints (point 2 point, hinge, generic d6)
482 // for (int g = 0; g < solverInfo.IterationsCount; g++) 482 // for (int g = 0; g < solverInfo.IterationsCount; g++)
483 // { 483 // {
484 // for (int i = 0; i < _constraints.Count; i++) 484 // for (int i = 0; i < _constraints.Count; i++)
485 // { 485 // {
486 // TypedConstraint constraint = _constraints[i]; 486 // TypedConstraint constraint = _constraints[i];
487 // constraint.SolveConstraint(solverInfo.TimeStep); 487 // constraint.SolveConstraint(solverInfo.TimeStep);
488 // } 488 // }
489 // } 489 // }
490 //} 490 //}
491 491
492 //protected void SolveContactConstraints(ContactSolverInfo solverInfo) 492 //protected void SolveContactConstraints(ContactSolverInfo solverInfo)
493 //{ 493 //{
494 // InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, _debugDrawer); 494 // InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, _debugDrawer);
495 495
496 // // solve all the contact points and contact friction 496 // // solve all the contact points and contact friction
497 // _islandManager.BuildAndProcessIslands(Dispatcher, CollisionObjects, solverCallback); 497 // _islandManager.BuildAndProcessIslands(Dispatcher, CollisionObjects, solverCallback);
498 //} 498 //}
499 499
500 protected void SolveConstraints(ContactSolverInfo solverInfo) 500 protected void SolveConstraints(ContactSolverInfo solverInfo)
501 { 501 {
502 //sorted version of all btTypedConstraint, based on islandId 502 //sorted version of all btTypedConstraint, based on islandId
503 List<TypedConstraint> sortedConstraints = new List<TypedConstraint>(ConstraintsCount); 503 List<TypedConstraint> sortedConstraints = new List<TypedConstraint>(ConstraintsCount);
504 504
505 for (int i = 0; i < ConstraintsCount; i++) 505 for (int i = 0; i < ConstraintsCount; i++)
506 { 506 {
507 sortedConstraints.Add(_constraints[i]); 507 sortedConstraints.Add(_constraints[i]);
508 } 508 }
509 509
510 sortedConstraints.Sort(new Comparison<TypedConstraint>(TypedConstraint.SortConstraintOnIslandPredicate)); 510 sortedConstraints.Sort(new Comparison<TypedConstraint>(TypedConstraint.SortConstraintOnIslandPredicate));
511 List<TypedConstraint> constraintsPtr = ConstraintsCount != 0 ? sortedConstraints : new List<TypedConstraint>(); 511 List<TypedConstraint> constraintsPtr = ConstraintsCount != 0 ? sortedConstraints : new List<TypedConstraint>();
512 512
513 InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, constraintsPtr, _debugDrawer); 513 InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, constraintsPtr, _debugDrawer);
514 514
515 // solve all the constraints for this island 515 // solve all the constraints for this island
516 _islandManager.BuildAndProcessIslands(CollisionWorld.Dispatcher, CollisionWorld.CollisionObjects, solverCallback); 516 _islandManager.BuildAndProcessIslands(CollisionWorld.Dispatcher, CollisionWorld.CollisionObjects, solverCallback);
517 } 517 }
518 518
519 protected void UpdateActivationState(float timeStep) 519 protected void UpdateActivationState(float timeStep)
520 { 520 {
521 for (int i = 0; i < CollisionObjects.Count; i++) 521 for (int i = 0; i < CollisionObjects.Count; i++)
522 { 522 {
523 CollisionObject colObj = CollisionObjects[i]; 523 CollisionObject colObj = CollisionObjects[i];
524 RigidBody body = RigidBody.Upcast(colObj); 524 RigidBody body = RigidBody.Upcast(colObj);
525 if (body != null) 525 if (body != null)
526 { 526 {
527 body.UpdateDeactivation(timeStep); 527 body.UpdateDeactivation(timeStep);
528 528
529 if (body.WantsSleeping()) 529 if (body.WantsSleeping())
530 { 530 {
531 if (body.IsStaticOrKinematicObject) 531 if (body.IsStaticOrKinematicObject)
532 { 532 {
533 body.ActivationState = ActivationState.IslandSleeping; 533 body.ActivationState = ActivationState.IslandSleeping;
534 } 534 }
535 else 535 else
536 { 536 {
537 if (body.ActivationState == ActivationState.Active) 537 if (body.ActivationState == ActivationState.Active)
538 body.ActivationState = ActivationState.WantsDeactivation; 538 body.ActivationState = ActivationState.WantsDeactivation;
539 } 539 }
540 } 540 }
541 else 541 else
542 { 542 {
543 if (body.ActivationState != ActivationState.DisableDeactivation) 543 if (body.ActivationState != ActivationState.DisableDeactivation)
544 body.ActivationState = ActivationState.Active; 544 body.ActivationState = ActivationState.Active;
545 } 545 }
546 } 546 }
547 } 547 }
548 } 548 }
549 549
550 protected void UpdateVehicles(float timeStep) 550 protected void UpdateVehicles(float timeStep)
551 { 551 {
552 for (int i = 0; i < _vehicles.Count; i++) 552 for (int i = 0; i < _vehicles.Count; i++)
553 { 553 {
554 RaycastVehicle vehicle = _vehicles[i]; 554 RaycastVehicle vehicle = _vehicles[i];
555 vehicle.updateVehicle(timeStep); 555 vehicle.updateVehicle(timeStep);
556 } 556 }
557 } 557 }
558 558
559 protected void StartProfiling(float timeStep) { } 559 protected void StartProfiling(float timeStep) { }
560 560
561 protected virtual void InternalSingleStepSimulation(float timeStep) 561 protected virtual void InternalSingleStepSimulation(float timeStep)
562 { 562 {
563 StartProfiling(timeStep); 563 StartProfiling(timeStep);
564 564
565 //update aabbs information 565 //update aabbs information
566 UpdateAabbs(); 566 UpdateAabbs();
567 567
568 //apply gravity, predict motion 568 //apply gravity, predict motion
569 PredictUnconstraintMotion(timeStep); 569 PredictUnconstraintMotion(timeStep);
570 570
571 DispatcherInfo dispatchInfo = DispatchInfo; 571 DispatcherInfo dispatchInfo = DispatchInfo;
572 dispatchInfo.TimeStep = timeStep; 572 dispatchInfo.TimeStep = timeStep;
573 dispatchInfo.StepCount = 0; 573 dispatchInfo.StepCount = 0;
574 dispatchInfo.DebugDraw = DebugDrawer; 574 dispatchInfo.DebugDraw = DebugDrawer;
575 575
576 //perform collision detection 576 //perform collision detection
577 PerformDiscreteCollisionDetection(); 577 PerformDiscreteCollisionDetection();
578 578
579 CalculateSimulationIslands(); 579 CalculateSimulationIslands();
580 580
581 SolverInfo.TimeStep = timeStep; 581 SolverInfo.TimeStep = timeStep;
582 582
583 //solve contact and other joint constraints 583 //solve contact and other joint constraints
584 SolveConstraints(SolverInfo); 584 SolveConstraints(SolverInfo);
585 585
586 //CallbackTriggers(); 586 //CallbackTriggers();
587 587
588 //integrate transforms 588 //integrate transforms
589 IntegrateTransforms(timeStep); 589 IntegrateTransforms(timeStep);
590 590
591 //update vehicle simulation 591 //update vehicle simulation
592 UpdateVehicles(timeStep); 592 UpdateVehicles(timeStep);
593 593
594 UpdateActivationState(timeStep); 594 UpdateActivationState(timeStep);
595 } 595 }
596 596
597 protected void SynchronizeMotionStates() 597 protected void SynchronizeMotionStates()
598 { 598 {
599 //debug vehicle wheels 599 //debug vehicle wheels
600 { 600 {
601 //todo: iterate over awake simulation islands! 601 //todo: iterate over awake simulation islands!
602 for (int i = 0; i < CollisionObjects.Count; i++) 602 for (int i = 0; i < CollisionObjects.Count; i++)
603 { 603 {
604 CollisionObject colObj = CollisionObjects[i]; 604 CollisionObject colObj = CollisionObjects[i];
605 if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0) 605 if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0)
606 { 606 {
607 Vector3 color = new Vector3(255f, 255f, 255f); 607 Vector3 color = new Vector3(255f, 255f, 255f);
608 switch (colObj.ActivationState) 608 switch (colObj.ActivationState)
609 { 609 {
610 case ActivationState.Active: 610 case ActivationState.Active:
611 color = new Vector3(255f, 255f, 255f); break; 611 color = new Vector3(255f, 255f, 255f); break;
612 case ActivationState.IslandSleeping: 612 case ActivationState.IslandSleeping:
613 color = new Vector3(0f, 255f, 0f); break; 613 color = new Vector3(0f, 255f, 0f); break;
614 case ActivationState.WantsDeactivation: 614 case ActivationState.WantsDeactivation:
615 color = new Vector3(0f, 255f, 255f); break; 615 color = new Vector3(0f, 255f, 255f); break;
616 case ActivationState.DisableDeactivation: 616 case ActivationState.DisableDeactivation:
617 color = new Vector3(255f, 0f, 0f); break; 617 color = new Vector3(255f, 0f, 0f); break;
618 case ActivationState.DisableSimulation: 618 case ActivationState.DisableSimulation:
619 color = new Vector3(255f, 255f, 0f); break; 619 color = new Vector3(255f, 255f, 0f); break;
620 default: 620 default:
621 color = new Vector3(255f, 0f, 0f); break; 621 color = new Vector3(255f, 0f, 0f); break;
622 } 622 }
623 623
624 DebugDrawObject(colObj.WorldTransform, colObj.CollisionShape, color); 624 DebugDrawObject(colObj.WorldTransform, colObj.CollisionShape, color);
625 } 625 }
626 RigidBody body = RigidBody.Upcast(colObj); 626 RigidBody body = RigidBody.Upcast(colObj);
627 if (body != null && body.MotionState != null && !body.IsStaticOrKinematicObject) 627 if (body != null && body.MotionState != null && !body.IsStaticOrKinematicObject)
628 { 628 {
629 //if (body.ActivationState != ActivationState.IslandSleeping) 629 //if (body.ActivationState != ActivationState.IslandSleeping)
630 { 630 {
631 Matrix interpolatedTransform = new Matrix(); 631 Matrix interpolatedTransform = new Matrix();
632 TransformUtil.IntegrateTransform(body.InterpolationWorldTransform, 632 TransformUtil.IntegrateTransform(body.InterpolationWorldTransform,
633 body.InterpolationLinearVelocity, body.InterpolationAngularVelocity, _localTime, ref interpolatedTransform); 633 body.InterpolationLinearVelocity, body.InterpolationAngularVelocity, _localTime, ref interpolatedTransform);
634 body.MotionState.SetWorldTransform(interpolatedTransform); 634 body.MotionState.SetWorldTransform(interpolatedTransform);
635 } 635 }
636 } 636 }
637 } 637 }
638 } 638 }
639 639
640 if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0) 640 if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0)
641 { 641 {
642 for (int i = 0; i < _vehicles.Count; i++) 642 for (int i = 0; i < _vehicles.Count; i++)
643 { 643 {
644 for (int v = 0; v < _vehicles[i].getNumWheels(); v++) 644 for (int v = 0; v < _vehicles[i].getNumWheels(); v++)
645 { 645 {
646 Vector3 wheelColor = new Vector3(0, 255, 255); 646 Vector3 wheelColor = new Vector3(0, 255, 255);
647 if (_vehicles[i].getWheelInfo(v).RaycastInfo.IsInContact) 647 if (_vehicles[i].getWheelInfo(v).RaycastInfo.IsInContact)
648 { 648 {
649 wheelColor = new Vector3(0, 0, 255); 649 wheelColor = new Vector3(0, 0, 255);
650 } 650 }
651 else 651 else
652 { 652 {
653 wheelColor = new Vector3(255, 0, 255); 653 wheelColor = new Vector3(255, 0, 255);
654 } 654 }
655 655
656 //synchronize the wheels with the (interpolated) chassis worldtransform 656 //synchronize the wheels with the (interpolated) chassis worldtransform
657 _vehicles[i].updateWheelTransform(v, true); 657 _vehicles[i].updateWheelTransform(v, true);
658 658
659 Vector3 wheelPosWS = _vehicles[i].getWheelInfo(v).WorldTransform.Translation; 659 Vector3 wheelPosWS = _vehicles[i].getWheelInfo(v).WorldTransform.Translation;
660 660
661 Vector3 axle = new Vector3( 661 Vector3 axle = new Vector3(
662 MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 0, _vehicles[i].getRightAxis()), 662 MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 0, _vehicles[i].getRightAxis()),
663 MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 1, _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())); 664 MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 2, _vehicles[i].getRightAxis()));
665 665
666 666
667 //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS 667 //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS
668 //debug wheels (cylinders) 668 //debug wheels (cylinders)
669 _debugDrawer.DrawLine(wheelPosWS, wheelPosWS + axle, wheelColor); 669 _debugDrawer.DrawLine(wheelPosWS, wheelPosWS + axle, wheelColor);
670 _debugDrawer.DrawLine(wheelPosWS, _vehicles[i].getWheelInfo(v).RaycastInfo.ContactPointWS, wheelColor); 670 _debugDrawer.DrawLine(wheelPosWS, _vehicles[i].getWheelInfo(v).RaycastInfo.ContactPointWS, wheelColor);
671 } 671 }
672 } 672 }
673 } 673 }
674 } 674 }
675 675
676 protected void SaveKinematicState(float timeStep) 676 protected void SaveKinematicState(float timeStep)
677 { 677 {
678 for (int i = 0; i < CollisionObjects.Count; i++) 678 for (int i = 0; i < CollisionObjects.Count; i++)
679 { 679 {
680 CollisionObject colObj = CollisionObjects[i]; 680 CollisionObject colObj = CollisionObjects[i];
681 RigidBody body = RigidBody.Upcast(colObj); 681 RigidBody body = RigidBody.Upcast(colObj);
682 if (body != null) 682 if (body != null)
683 { 683 {
684 if (body.ActivationState != ActivationState.IslandSleeping) 684 if (body.ActivationState != ActivationState.IslandSleeping)
685 { 685 {
686 if (body.IsKinematicObject) 686 if (body.IsKinematicObject)
687 { 687 {
688 //to calculate velocities next frame 688 //to calculate velocities next frame
689 body.SaveKinematicState(timeStep); 689 body.SaveKinematicState(timeStep);
690 } 690 }
691 } 691 }
692 } 692 }
693 } 693 }
694 } 694 }
695 695
696 internal class InplaceSolverIslandCallback : SimulationIslandManager.IIslandCallback 696 internal class InplaceSolverIslandCallback : SimulationIslandManager.IIslandCallback
697 { 697 {
698 private ContactSolverInfo _solverInfo; 698 private ContactSolverInfo _solverInfo;
699 private IConstraintSolver _solver; 699 private IConstraintSolver _solver;
700 private IDebugDraw _debugDrawer; 700 private IDebugDraw _debugDrawer;
701 private List<TypedConstraint> _sortedConstraints; 701 private List<TypedConstraint> _sortedConstraints;
702 702
703 public InplaceSolverIslandCallback( 703 public InplaceSolverIslandCallback(
704 ContactSolverInfo solverInfo, 704 ContactSolverInfo solverInfo,
705 IConstraintSolver solver, 705 IConstraintSolver solver,
706 List<TypedConstraint> sortedConstraints, 706 List<TypedConstraint> sortedConstraints,
707 IDebugDraw debugDrawer) 707 IDebugDraw debugDrawer)
708 { 708 {
709 _solverInfo = solverInfo; 709 _solverInfo = solverInfo;
710 _solver = solver; 710 _solver = solver;
711 _sortedConstraints = sortedConstraints; 711 _sortedConstraints = sortedConstraints;
712 _debugDrawer = debugDrawer; 712 _debugDrawer = debugDrawer;
713 } 713 }
714 714
715 public ContactSolverInfo SolverInfo { get { return _solverInfo; } set { _solverInfo = value; } } 715 public ContactSolverInfo SolverInfo { get { return _solverInfo; } set { _solverInfo = value; } }
716 public IConstraintSolver Solver { get { return _solver; } set { _solver = value; } } 716 public IConstraintSolver Solver { get { return _solver; } set { _solver = value; } }
717 public List<TypedConstraint> Constraints { get { return _sortedConstraints; } set { _sortedConstraints = value; } } 717 public List<TypedConstraint> Constraints { get { return _sortedConstraints; } set { _sortedConstraints = value; } }
718 public IDebugDraw DebugDrawer { get { return _debugDrawer; } set { _debugDrawer = value; } } 718 public IDebugDraw DebugDrawer { get { return _debugDrawer; } set { _debugDrawer = value; } }
719 719
720 public void ProcessIsland(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, int islandID) 720 public void ProcessIsland(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, int islandID)
721 { 721 {
722 //also add all non-contact constraints/joints for this island 722 //also add all non-contact constraints/joints for this island
723 List<TypedConstraint> startConstraint = new List<TypedConstraint>(); 723 List<TypedConstraint> startConstraint = new List<TypedConstraint>();
724 int numCurConstraints = 0; 724 int numCurConstraints = 0;
725 int startIndex = 0; 725 int startIndex = 0;
726 int i; 726 int i;
727 727
728 //find the first constraint for this island 728 //find the first constraint for this island
729 for (i = 0; i < _sortedConstraints.Count; i++) 729 for (i = 0; i < _sortedConstraints.Count; i++)
730 { 730 {
731 if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID) 731 if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID)
732 { 732 {
733 //startConstraint = &m_sortedConstraints[i]; 733 //startConstraint = &m_sortedConstraints[i];
734 startIndex = i; 734 startIndex = i;
735 break; 735 break;
736 } 736 }
737 } 737 }
738 //count the number of constraints in this island 738 //count the number of constraints in this island
739 for (; i < _sortedConstraints.Count; i++) 739 for (; i < _sortedConstraints.Count; i++)
740 { 740 {
741 if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID) 741 if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID)
742 { 742 {
743 numCurConstraints++; 743 numCurConstraints++;
744 } 744 }
745 } 745 }
746 746
747 for (i = startIndex; i < startIndex + numCurConstraints; i++) 747 for (i = startIndex; i < startIndex + numCurConstraints; i++)
748 { 748 {
749 startConstraint.Add(_sortedConstraints[i]); 749 startConstraint.Add(_sortedConstraints[i]);
750 } 750 }
751 751
752 _solver.SolveGroup(bodies, manifolds, numManifolds, startConstraint, _solverInfo, _debugDrawer); 752 _solver.SolveGroup(bodies, manifolds, numManifolds, startConstraint, _solverInfo, _debugDrawer);
753 } 753 }
754 } 754 }
755 } 755 }
756 756
757 internal class DebugDrawCallback : ITriangleIndexCallback, ITriangleCallback 757 internal class DebugDrawCallback : ITriangleIndexCallback, ITriangleCallback
758 { 758 {
759 private IDebugDraw _debugDrawer; 759 private IDebugDraw _debugDrawer;
760 private Vector3 _color; 760 private Vector3 _color;
761 private Matrix _worldTrans; 761 private Matrix _worldTrans;
762 762
763 public DebugDrawCallback(IDebugDraw debugDrawer, Matrix worldTrans, Vector3 color) 763 public DebugDrawCallback(IDebugDraw debugDrawer, Matrix worldTrans, Vector3 color)
764 { 764 {
765 _debugDrawer = debugDrawer; 765 _debugDrawer = debugDrawer;
766 _worldTrans = worldTrans; 766 _worldTrans = worldTrans;
767 _color = color; 767 _color = color;
768 } 768 }
769 769
770 public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) 770 public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex)
771 { 771 {
772 ProcessTriangle(triangle, partId, triangleIndex); 772 ProcessTriangle(triangle, partId, triangleIndex);
773 } 773 }
774 774
775 #region ITriangleCallback Members 775 #region ITriangleCallback Members
776 776
777 public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) 777 public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex)
778 { 778 {
779 Vector3 wv0, wv1, wv2; 779 Vector3 wv0, wv1, wv2;
780 wv0 = Vector3.TransformNormal(triangle[0], _worldTrans); 780 wv0 = Vector3.TransformNormal(triangle[0], _worldTrans);
781 wv1 = Vector3.TransformNormal(triangle[1], _worldTrans); 781 wv1 = Vector3.TransformNormal(triangle[1], _worldTrans);
782 wv2 = Vector3.TransformNormal(triangle[2], _worldTrans); 782 wv2 = Vector3.TransformNormal(triangle[2], _worldTrans);
783 _debugDrawer.DrawLine(wv0, wv1, _color); 783 _debugDrawer.DrawLine(wv0, wv1, _color);
784 _debugDrawer.DrawLine(wv1, wv2, _color); 784 _debugDrawer.DrawLine(wv1, wv2, _color);
785 _debugDrawer.DrawLine(wv2, wv0, _color); 785 _debugDrawer.DrawLine(wv2, wv0, _color);
786 } 786 }
787 787
788 #endregion 788 #endregion
789 } 789 }
790} \ No newline at end of file 790} \ No newline at end of file
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DynamicsWorld.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DynamicsWorld.cs
index 647c8ec..34ae608 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DynamicsWorld.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DynamicsWorld.cs
@@ -1,59 +1,59 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX.Dynamics 27namespace XnaDevRu.BulletX.Dynamics
28{ 28{
29 public abstract class DynamicsWorld : CollisionWorld 29 public abstract class DynamicsWorld : CollisionWorld
30 { 30 {
31 public DynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache) 31 public DynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache)
32 : base(dispatcher, pairCache) { } 32 : base(dispatcher, pairCache) { }
33 33
34 //once a rigidbody is added to the dynamics world, it will get this gravity assigned 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 35 //existing rigidbodies in the world get gravity assigned too, during this method
36 public abstract Vector3 Gravity { set; } 36 public abstract Vector3 Gravity { set; }
37 public abstract IConstraintSolver ConstraintSolver { set; } 37 public abstract IConstraintSolver ConstraintSolver { set; }
38 public virtual int ConstraintsCount { get { return 0; } } 38 public virtual int ConstraintsCount { get { return 0; } }
39 public abstract IDebugDraw DebugDrawer { get; set; } 39 public abstract IDebugDraw DebugDrawer { get; set; }
40 40
41 //stepSimulation proceeds the simulation over timeStep units 41 //stepSimulation proceeds the simulation over timeStep units
42 public abstract void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep); 42 public abstract void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep);
43 43
44 public void StepSimulation(float timeStep) 44 public void StepSimulation(float timeStep)
45 { 45 {
46 StepSimulation(timeStep, 1, 1f / 60f); 46 StepSimulation(timeStep, 1, 1f / 60f);
47 } 47 }
48 48
49 public abstract void UpdateAabbs(); 49 public abstract void UpdateAabbs();
50 50
51 public virtual void AddConstraint(TypedConstraint constraint) { } 51 public virtual void AddConstraint(TypedConstraint constraint) { }
52 public virtual void RemoveConstraint(TypedConstraint constraint) { } 52 public virtual void RemoveConstraint(TypedConstraint constraint) { }
53 53
54 public abstract void AddRigidBody(RigidBody body); 54 public abstract void AddRigidBody(RigidBody body);
55 public abstract void RemoveRigidBody(RigidBody body); 55 public abstract void RemoveRigidBody(RigidBody body);
56 56
57 public virtual TypedConstraint GetConstraint(int index) { return null; } 57 public virtual TypedConstraint GetConstraint(int index) { return null; }
58 } 58 }
59} 59}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/RigidBody.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/RigidBody.cs
index ec14daf..12a7485 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/RigidBody.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/RigidBody.cs
@@ -1,447 +1,447 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX.Dynamics 27namespace XnaDevRu.BulletX.Dynamics
28{ 28{
29 public class RigidBody : CollisionObject 29 public class RigidBody : CollisionObject
30 { 30 {
31 private static float _linearAirDamping = 1; 31 private static float _linearAirDamping = 1;
32 //'temporarily' global variables 32 //'temporarily' global variables
33 private static float _rigidBodyDeactivationTime = 2; 33 private static float _rigidBodyDeactivationTime = 2;
34 private static bool _disableDeactivation = false; 34 private static bool _disableDeactivation = false;
35 35
36 private static float _linearSleepingThreshold = 0.8f; 36 private static float _linearSleepingThreshold = 0.8f;
37 private static float _angularSleepingThreshold = 1.0f; 37 private static float _angularSleepingThreshold = 1.0f;
38 private static int _uniqueId = 0; 38 private static int _uniqueId = 0;
39 39
40 private Matrix _invInertiaTensorWorld; 40 private Matrix _invInertiaTensorWorld;
41 private Vector3 _linearVelocity; 41 private Vector3 _linearVelocity;
42 private Vector3 _angularVelocity; 42 private Vector3 _angularVelocity;
43 private float _inverseMass; 43 private float _inverseMass;
44 private float _angularFactor; 44 private float _angularFactor;
45 45
46 private Vector3 _gravity; 46 private Vector3 _gravity;
47 private Vector3 _invInertiaLocal; 47 private Vector3 _invInertiaLocal;
48 private Vector3 _totalForce; 48 private Vector3 _totalForce;
49 private Vector3 _totalTorque; 49 private Vector3 _totalTorque;
50 50
51 private float _linearDamping; 51 private float _linearDamping;
52 private float _angularDamping; 52 private float _angularDamping;
53 53
54 //m_optionalMotionState allows to automatic synchronize the world transform for active objects 54 //m_optionalMotionState allows to automatic synchronize the world transform for active objects
55 private MotionState _optionalMotionState; 55 private MotionState _optionalMotionState;
56 56
57 //for experimental overriding of friction/contact solver func 57 //for experimental overriding of friction/contact solver func
58 private ContactSolverType _contactSolverType; 58 private ContactSolverType _contactSolverType;
59 private ContactSolverType _frictionSolverType; 59 private ContactSolverType _frictionSolverType;
60 60
61 private int _debugBodyId; 61 private int _debugBodyId;
62 62
63 //Bullet 2.20b has experimental damping code to reduce jitter just before objects fall asleep/deactivate 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) 64 //doesn't work very well yet (value 0 disabled this damping)
65 //note there this influences deactivation thresholds! 65 //note there this influences deactivation thresholds!
66 private float _clippedAngvelThresholdSqr = 0.01f; 66 private float _clippedAngvelThresholdSqr = 0.01f;
67 private float _clippedLinearThresholdSqr = 0.01f; 67 private float _clippedLinearThresholdSqr = 0.01f;
68 68
69 private float _jitterVelocityDampingFactor = 0.7f; 69 private float _jitterVelocityDampingFactor = 0.7f;
70 70
71 public RigidBody(float mass, MotionState motionState, CollisionShape collisionShape, Vector3 localInertia, float linearDamping, float angularDamping, float friction, float restitution) 71 public RigidBody(float mass, MotionState motionState, CollisionShape collisionShape, Vector3 localInertia, float linearDamping, float angularDamping, float friction, float restitution)
72 { 72 {
73 _optionalMotionState = motionState; 73 _optionalMotionState = motionState;
74 _angularFactor = 1; 74 _angularFactor = 1;
75 _angularDamping = 0.5f; 75 _angularDamping = 0.5f;
76 76
77 if (motionState != null) 77 if (motionState != null)
78 { 78 {
79 motionState.GetWorldTransform(out _worldTransform); 79 motionState.GetWorldTransform(out _worldTransform);
80 } 80 }
81 else 81 else
82 { 82 {
83 WorldTransform = Matrix.Identity; 83 WorldTransform = Matrix.Identity;
84 } 84 }
85 85
86 InterpolationWorldTransform = WorldTransform; 86 InterpolationWorldTransform = WorldTransform;
87 InterpolationLinearVelocity = new Vector3(); 87 InterpolationLinearVelocity = new Vector3();
88 InterpolationAngularVelocity = new Vector3(); 88 InterpolationAngularVelocity = new Vector3();
89 89
90 //moved to btCollisionObject 90 //moved to btCollisionObject
91 Friction = friction; 91 Friction = friction;
92 Restitution = restitution; 92 Restitution = restitution;
93 93
94 CollisionShape = collisionShape; 94 CollisionShape = collisionShape;
95 _debugBodyId = UniqueID++; 95 _debugBodyId = UniqueID++;
96 96
97 //m_internalOwner is to allow upcasting from collision object to rigid body 97 //m_internalOwner is to allow upcasting from collision object to rigid body
98 Owner = this; 98 Owner = this;
99 99
100 SetMassProps(mass, localInertia); 100 SetMassProps(mass, localInertia);
101 SetDamping(linearDamping, angularDamping); 101 SetDamping(linearDamping, angularDamping);
102 UpdateInertiaTensor(); 102 UpdateInertiaTensor();
103 } 103 }
104 104
105 public int DebugBodyID { get { return _debugBodyId; } set { _debugBodyId = value; } } 105 public int DebugBodyID { get { return _debugBodyId; } set { _debugBodyId = value; } }
106 106
107 public ContactSolverType ContactSolverType { get { return _contactSolverType; } set { _contactSolverType = value; } } 107 public ContactSolverType ContactSolverType { get { return _contactSolverType; } set { _contactSolverType = value; } }
108 public ContactSolverType FrictionSolverType { get { return _frictionSolverType; } set { _frictionSolverType = value; } } 108 public ContactSolverType FrictionSolverType { get { return _frictionSolverType; } set { _frictionSolverType = value; } }
109 109
110 public float AngularFactor { get { return _angularFactor; } set { _angularFactor = value; } } 110 public float AngularFactor { get { return _angularFactor; } set { _angularFactor = value; } }
111 111
112 //is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase? 112 //is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase?
113 public bool IsInWorld { get { return Broadphase != null; } } 113 public bool IsInWorld { get { return Broadphase != null; } }
114 114
115 public Vector3 Gravity 115 public Vector3 Gravity
116 { 116 {
117 get { return _gravity; } 117 get { return _gravity; }
118 set 118 set
119 { 119 {
120 if (_inverseMass != 0.0f) 120 if (_inverseMass != 0.0f)
121 { 121 {
122 _gravity = value * (1.0f / _inverseMass); 122 _gravity = value * (1.0f / _inverseMass);
123 } 123 }
124 } 124 }
125 } 125 }
126 public Matrix InvInertiaTensorWorld { get { return _invInertiaTensorWorld; } } 126 public Matrix InvInertiaTensorWorld { get { return _invInertiaTensorWorld; } }
127 public float InverseMass { get { return _inverseMass; } } 127 public float InverseMass { get { return _inverseMass; } }
128 public Vector3 InvInertiaDiagLocal { get { return _invInertiaLocal; } set { _invInertiaLocal = value; } } 128 public Vector3 InvInertiaDiagLocal { get { return _invInertiaLocal; } set { _invInertiaLocal = value; } }
129 public Vector3 CenterOfMassPosition { get { return WorldTransform.Translation; } } 129 public Vector3 CenterOfMassPosition { get { return WorldTransform.Translation; } }
130 public Quaternion Orientation { get { return Quaternion.CreateFromRotationMatrix(WorldTransform); } } 130 public Quaternion Orientation { get { return Quaternion.CreateFromRotationMatrix(WorldTransform); } }
131 public Matrix CenterOfMassTransform 131 public Matrix CenterOfMassTransform
132 { 132 {
133 get { return WorldTransform; } 133 get { return WorldTransform; }
134 set 134 set
135 { 135 {
136 InterpolationWorldTransform = value; 136 InterpolationWorldTransform = value;
137 InterpolationLinearVelocity = LinearVelocity; 137 InterpolationLinearVelocity = LinearVelocity;
138 InterpolationAngularVelocity = AngularVelocity; 138 InterpolationAngularVelocity = AngularVelocity;
139 WorldTransform = value; 139 WorldTransform = value;
140 UpdateInertiaTensor(); 140 UpdateInertiaTensor();
141 } 141 }
142 } 142 }
143 143
144 public Vector3 LinearVelocity 144 public Vector3 LinearVelocity
145 { 145 {
146 get { return _linearVelocity; } 146 get { return _linearVelocity; }
147 set 147 set
148 { 148 {
149 if (CollisionFlags == CollisionOptions.StaticObject) 149 if (CollisionFlags == CollisionOptions.StaticObject)
150 throw new BulletException("Static objects can't have linear velocity!"); 150 throw new BulletException("Static objects can't have linear velocity!");
151 _linearVelocity = value; 151 _linearVelocity = value;
152 } 152 }
153 } 153 }
154 154
155 public Vector3 AngularVelocity 155 public Vector3 AngularVelocity
156 { 156 {
157 get { return _angularVelocity; } 157 get { return _angularVelocity; }
158 set 158 set
159 { 159 {
160 if (CollisionFlags == CollisionOptions.StaticObject) 160 if (CollisionFlags == CollisionOptions.StaticObject)
161 throw new BulletException("Static objects can't have angular velocity!"); 161 throw new BulletException("Static objects can't have angular velocity!");
162 _angularVelocity = value; 162 _angularVelocity = value;
163 } 163 }
164 } 164 }
165 165
166 //MotionState allows to automatic synchronize the world transform for active objects 166 //MotionState allows to automatic synchronize the world transform for active objects
167 public MotionState MotionState 167 public MotionState MotionState
168 { 168 {
169 get { return _optionalMotionState; } 169 get { return _optionalMotionState; }
170 set 170 set
171 { 171 {
172 _optionalMotionState = value; 172 _optionalMotionState = value;
173 if (_optionalMotionState != null) 173 if (_optionalMotionState != null)
174 value.GetWorldTransform(out _worldTransform); 174 value.GetWorldTransform(out _worldTransform);
175 } 175 }
176 } 176 }
177 177
178 public static float LinearAirDamping { get { return _linearAirDamping; } set { _linearAirDamping = value; } } 178 public static float LinearAirDamping { get { return _linearAirDamping; } set { _linearAirDamping = value; } }
179 public static float RigidBodyDeactivationTime { get { return _rigidBodyDeactivationTime; } set { _rigidBodyDeactivationTime = value; } } 179 public static float RigidBodyDeactivationTime { get { return _rigidBodyDeactivationTime; } set { _rigidBodyDeactivationTime = value; } }
180 public static bool DisableDeactivation { get { return _disableDeactivation; } set { _disableDeactivation = value; } } 180 public static bool DisableDeactivation { get { return _disableDeactivation; } set { _disableDeactivation = value; } }
181 public static float LinearSleepingThreshold { get { return _linearSleepingThreshold; } set { _linearSleepingThreshold = value; } } 181 public static float LinearSleepingThreshold { get { return _linearSleepingThreshold; } set { _linearSleepingThreshold = value; } }
182 public static float AngularSleepingThreshold { get { return _angularSleepingThreshold; } set { _angularSleepingThreshold = value; } } 182 public static float AngularSleepingThreshold { get { return _angularSleepingThreshold; } set { _angularSleepingThreshold = value; } }
183 public static int UniqueID { get { return _uniqueId; } set { _uniqueId = value; } } 183 public static int UniqueID { get { return _uniqueId; } set { _uniqueId = value; } }
184 184
185 public void ProceedToTransform(Matrix newTrans) 185 public void ProceedToTransform(Matrix newTrans)
186 { 186 {
187 CenterOfMassTransform = newTrans; 187 CenterOfMassTransform = newTrans;
188 } 188 }
189 189
190 //to keep collision detection and dynamics separate we don't store a rigidbody pointer 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 191 //but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast
192 public static RigidBody Upcast(CollisionObject colObj) 192 public static RigidBody Upcast(CollisionObject colObj)
193 { 193 {
194 return colObj.Owner as RigidBody; 194 return colObj.Owner as RigidBody;
195 } 195 }
196 196
197 // continuous collision detection needs prediction 197 // continuous collision detection needs prediction
198 public void PredictIntegratedTransform(float step, ref Matrix predictedTransform) 198 public void PredictIntegratedTransform(float step, ref Matrix predictedTransform)
199 { 199 {
200 if ((_angularVelocity.LengthSquared() < _clippedAngvelThresholdSqr) && 200 if ((_angularVelocity.LengthSquared() < _clippedAngvelThresholdSqr) &&
201 (_linearVelocity.LengthSquared() < _clippedLinearThresholdSqr)) 201 (_linearVelocity.LengthSquared() < _clippedLinearThresholdSqr))
202 { 202 {
203 _angularVelocity *= _jitterVelocityDampingFactor; 203 _angularVelocity *= _jitterVelocityDampingFactor;
204 _linearVelocity *= _jitterVelocityDampingFactor; 204 _linearVelocity *= _jitterVelocityDampingFactor;
205 } 205 }
206 206
207 TransformUtil.IntegrateTransform(WorldTransform, _linearVelocity, _angularVelocity, step, ref predictedTransform); 207 TransformUtil.IntegrateTransform(WorldTransform, _linearVelocity, _angularVelocity, step, ref predictedTransform);
208 } 208 }
209 209
210 public void SaveKinematicState(float step) 210 public void SaveKinematicState(float step)
211 { 211 {
212 //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities 212 //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
213 if (step != 0) 213 if (step != 0)
214 { 214 {
215 //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform 215 //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
216 if (MotionState != null) 216 if (MotionState != null)
217 MotionState.GetWorldTransform(out _worldTransform); 217 MotionState.GetWorldTransform(out _worldTransform);
218 218
219 TransformUtil.CalculateVelocity(InterpolationWorldTransform, WorldTransform, step, ref _linearVelocity, ref _angularVelocity); 219 TransformUtil.CalculateVelocity(InterpolationWorldTransform, WorldTransform, step, ref _linearVelocity, ref _angularVelocity);
220 InterpolationLinearVelocity = _linearVelocity; 220 InterpolationLinearVelocity = _linearVelocity;
221 InterpolationAngularVelocity = _angularVelocity; 221 InterpolationAngularVelocity = _angularVelocity;
222 InterpolationWorldTransform = WorldTransform; 222 InterpolationWorldTransform = WorldTransform;
223 } 223 }
224 } 224 }
225 225
226 public void ApplyForces(float step) 226 public void ApplyForces(float step)
227 { 227 {
228 if (IsStaticOrKinematicObject) 228 if (IsStaticOrKinematicObject)
229 return; 229 return;
230 230
231 ApplyCentralForce(_gravity); 231 ApplyCentralForce(_gravity);
232 232
233 _linearVelocity *= (1 - step * LinearAirDamping * _linearDamping) < 0.0f ? 0.0f : (1.0f < (1 - step * LinearAirDamping * _linearDamping) ? 1.0f : (1 - step * LinearAirDamping * _linearDamping)); 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)); 234 _angularVelocity *= (1 - step * _angularDamping) < 0.0f ? 0.0f : (1.0f < (1 - step * _angularDamping) ? 1.0f : (1 - step * _angularDamping));
235 235
236 float speed = _linearVelocity.Length(); 236 float speed = _linearVelocity.Length();
237 if (speed < _linearDamping) 237 if (speed < _linearDamping)
238 { 238 {
239 float dampVel = 0.005f; 239 float dampVel = 0.005f;
240 if (speed > dampVel) 240 if (speed > dampVel)
241 { 241 {
242 Vector3 dir = _linearVelocity; 242 Vector3 dir = _linearVelocity;
243 dir.Normalize(); 243 dir.Normalize();
244 _linearVelocity -= dir * dampVel; 244 _linearVelocity -= dir * dampVel;
245 } 245 }
246 else 246 else
247 { 247 {
248 _linearVelocity = new Vector3(); 248 _linearVelocity = new Vector3();
249 } 249 }
250 } 250 }
251 251
252 float angSpeed = _angularVelocity.Length(); 252 float angSpeed = _angularVelocity.Length();
253 if (angSpeed < _angularDamping) 253 if (angSpeed < _angularDamping)
254 { 254 {
255 float angDampVel = 0.005f; 255 float angDampVel = 0.005f;
256 if (angSpeed > angDampVel) 256 if (angSpeed > angDampVel)
257 { 257 {
258 Vector3 dir = _angularVelocity; 258 Vector3 dir = _angularVelocity;
259 dir.Normalize(); 259 dir.Normalize();
260 _angularVelocity -= dir * angDampVel; 260 _angularVelocity -= dir * angDampVel;
261 } 261 }
262 else 262 else
263 { 263 {
264 _angularVelocity = new Vector3(); 264 _angularVelocity = new Vector3();
265 } 265 }
266 } 266 }
267 } 267 }
268 268
269 public void SetDamping(float linDamping, float angDamping) 269 public void SetDamping(float linDamping, float angDamping)
270 { 270 {
271 _linearDamping = linDamping < 0.0f ? 0.0f : (1.0f < linDamping ? 1.0f : linDamping); 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); 272 _angularDamping = angDamping < 0.0f ? 0.0f : (1.0f < angDamping ? 1.0f : angDamping);
273 } 273 }
274 274
275 public void SetMassProps(float mass, Vector3 inertia) 275 public void SetMassProps(float mass, Vector3 inertia)
276 { 276 {
277 if (mass == 0) 277 if (mass == 0)
278 { 278 {
279 CollisionFlags |= CollisionOptions.StaticObject; 279 CollisionFlags |= CollisionOptions.StaticObject;
280 _inverseMass = 0; 280 _inverseMass = 0;
281 } 281 }
282 else 282 else
283 { 283 {
284 CollisionFlags &= (~CollisionOptions.StaticObject); 284 CollisionFlags &= (~CollisionOptions.StaticObject);
285 _inverseMass = 1.0f / mass; 285 _inverseMass = 1.0f / mass;
286 } 286 }
287 287
288 _invInertiaLocal = new Vector3(inertia.X != 0.0f ? 1.0f / inertia.X : 0.0f, 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, 289 inertia.Y != 0.0f ? 1.0f / inertia.Y : 0.0f,
290 inertia.Z != 0.0f ? 1.0f / inertia.Z : 0.0f); 290 inertia.Z != 0.0f ? 1.0f / inertia.Z : 0.0f);
291 } 291 }
292 292
293 public void IntegrateVelocities(float step) 293 public void IntegrateVelocities(float step)
294 { 294 {
295 if (IsStaticOrKinematicObject) 295 if (IsStaticOrKinematicObject)
296 return; 296 return;
297 297
298 _linearVelocity += _totalForce * (_inverseMass * step); 298 _linearVelocity += _totalForce * (_inverseMass * step);
299 _angularVelocity += Vector3.TransformNormal(_totalTorque, _invInertiaTensorWorld) * step; 299 _angularVelocity += Vector3.TransformNormal(_totalTorque, _invInertiaTensorWorld) * step;
300 300
301 //float MAX_ANGVEL = MonoXnaCompactMaths.MathHelper.PiOver2; 301 //float MAX_ANGVEL = MonoXnaCompactMaths.MathHelper.PiOver2;
302 float MAX_ANGVEL = (float)Math.PI / 2.0f; 302 float MAX_ANGVEL = (float)Math.PI / 2.0f;
303 /// clamp angular velocity. collision calculations will fail on higher angular velocities 303 /// clamp angular velocity. collision calculations will fail on higher angular velocities
304 float angvel = _angularVelocity.Length(); 304 float angvel = _angularVelocity.Length();
305 if (angvel * step > MAX_ANGVEL) 305 if (angvel * step > MAX_ANGVEL)
306 { 306 {
307 _angularVelocity *= (MAX_ANGVEL / step) / angvel; 307 _angularVelocity *= (MAX_ANGVEL / step) / angvel;
308 } 308 }
309 309
310 ClearForces(); 310 ClearForces();
311 } 311 }
312 312
313 public void ApplyCentralForce(Vector3 force) 313 public void ApplyCentralForce(Vector3 force)
314 { 314 {
315 _totalForce += force; 315 _totalForce += force;
316 } 316 }
317 317
318 public void ApplyTorque(Vector3 torque) 318 public void ApplyTorque(Vector3 torque)
319 { 319 {
320 _totalTorque += torque; 320 _totalTorque += torque;
321 } 321 }
322 322
323 public void ApplyForce(Vector3 force, Vector3 rel_pos) 323 public void ApplyForce(Vector3 force, Vector3 rel_pos)
324 { 324 {
325 ApplyCentralForce(force); 325 ApplyCentralForce(force);
326 ApplyTorque(Vector3.Cross(rel_pos, force)); 326 ApplyTorque(Vector3.Cross(rel_pos, force));
327 } 327 }
328 328
329 public void ApplyCentralImpulse(Vector3 impulse) 329 public void ApplyCentralImpulse(Vector3 impulse)
330 { 330 {
331 _linearVelocity += impulse * _inverseMass; 331 _linearVelocity += impulse * _inverseMass;
332 } 332 }
333 333
334 public void ApplyTorqueImpulse(Vector3 torque) 334 public void ApplyTorqueImpulse(Vector3 torque)
335 { 335 {
336 _angularVelocity += Vector3.TransformNormal(torque, _invInertiaTensorWorld); 336 _angularVelocity += Vector3.TransformNormal(torque, _invInertiaTensorWorld);
337 } 337 }
338 338
339 public void ApplyImpulse(Vector3 impulse, Vector3 rel_pos) 339 public void ApplyImpulse(Vector3 impulse, Vector3 rel_pos)
340 { 340 {
341 if (_inverseMass != 0) 341 if (_inverseMass != 0)
342 { 342 {
343 ApplyCentralImpulse(impulse); 343 ApplyCentralImpulse(impulse);
344 if (_angularFactor != 0) 344 if (_angularFactor != 0)
345 ApplyTorqueImpulse(Vector3.Cross(rel_pos, impulse) * _angularFactor); 345 ApplyTorqueImpulse(Vector3.Cross(rel_pos, impulse) * _angularFactor);
346 } 346 }
347 } 347 }
348 348
349 public void InternalApplyImpulse(Vector3 linearComponent, Vector3 angularComponent, float impulseMagnitude) 349 public void InternalApplyImpulse(Vector3 linearComponent, Vector3 angularComponent, float impulseMagnitude)
350 { 350 {
351 if (_inverseMass != 0) 351 if (_inverseMass != 0)
352 { 352 {
353 _linearVelocity += linearComponent * impulseMagnitude; 353 _linearVelocity += linearComponent * impulseMagnitude;
354 if (_angularFactor != 0) 354 if (_angularFactor != 0)
355 _angularVelocity += angularComponent * impulseMagnitude * _angularFactor; 355 _angularVelocity += angularComponent * impulseMagnitude * _angularFactor;
356 } 356 }
357 } 357 }
358 358
359 public void ClearForces() 359 public void ClearForces()
360 { 360 {
361 _totalForce = new Vector3(); 361 _totalForce = new Vector3();
362 _totalTorque = new Vector3(); 362 _totalTorque = new Vector3();
363 } 363 }
364 364
365 public void UpdateInertiaTensor() 365 public void UpdateInertiaTensor()
366 { 366 {
367 Matrix temp = WorldTransform; 367 Matrix temp = WorldTransform;
368 temp.Translation = Vector3.Zero; 368 temp.Translation = Vector3.Zero;
369 _invInertiaTensorWorld = MatrixOperations.Multiply(MatrixOperations.Scaled(WorldTransform, _invInertiaLocal), Matrix.Transpose(temp)); 369 _invInertiaTensorWorld = MatrixOperations.Multiply(MatrixOperations.Scaled(WorldTransform, _invInertiaLocal), Matrix.Transpose(temp));
370 } 370 }
371 371
372 public Vector3 GetVelocityInLocalPoint(Vector3 relPos) 372 public Vector3 GetVelocityInLocalPoint(Vector3 relPos)
373 { 373 {
374 //we also calculate lin/ang velocity for kinematic objects 374 //we also calculate lin/ang velocity for kinematic objects
375 return _linearVelocity + Vector3.Cross(_angularVelocity, relPos); 375 return _linearVelocity + Vector3.Cross(_angularVelocity, relPos);
376 376
377 //for kinematic objects, we could also use use: 377 //for kinematic objects, we could also use use:
378 // return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep; 378 // return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep;
379 } 379 }
380 380
381 public void Translate(Vector3 v) 381 public void Translate(Vector3 v)
382 { 382 {
383 Matrix m = WorldTransform; 383 Matrix m = WorldTransform;
384 m.Translation += v; 384 m.Translation += v;
385 WorldTransform = m; 385 WorldTransform = m;
386 } 386 }
387 387
388 public void GetAabb(out Vector3 aabbMin, out Vector3 aabbMax) 388 public void GetAabb(out Vector3 aabbMin, out Vector3 aabbMax)
389 { 389 {
390 CollisionShape.GetAabb(WorldTransform, out aabbMin, out aabbMax); 390 CollisionShape.GetAabb(WorldTransform, out aabbMin, out aabbMax);
391 } 391 }
392 392
393 public float ComputeImpulseDenominator(Vector3 pos, Vector3 normal) 393 public float ComputeImpulseDenominator(Vector3 pos, Vector3 normal)
394 { 394 {
395 Vector3 r0 = pos - CenterOfMassPosition; 395 Vector3 r0 = pos - CenterOfMassPosition;
396 Vector3 c0 = Vector3.Cross(r0, normal); 396 Vector3 c0 = Vector3.Cross(r0, normal);
397 Vector3 vec = Vector3.Cross(Vector3.TransformNormal(c0, InvInertiaTensorWorld), r0); 397 Vector3 vec = Vector3.Cross(Vector3.TransformNormal(c0, InvInertiaTensorWorld), r0);
398 398
399 return _inverseMass + Vector3.Dot(normal, vec); 399 return _inverseMass + Vector3.Dot(normal, vec);
400 400
401 } 401 }
402 402
403 public float ComputeAngularImpulseDenominator(Vector3 axis) 403 public float ComputeAngularImpulseDenominator(Vector3 axis)
404 { 404 {
405 Vector3 vec = Vector3.TransformNormal(axis, InvInertiaTensorWorld); 405 Vector3 vec = Vector3.TransformNormal(axis, InvInertiaTensorWorld);
406 return Vector3.Dot(axis, vec); 406 return Vector3.Dot(axis, vec);
407 } 407 }
408 408
409 public void UpdateDeactivation(float timeStep) 409 public void UpdateDeactivation(float timeStep)
410 { 410 {
411 if ((ActivationState == ActivationState.IslandSleeping) || (ActivationState == ActivationState.DisableDeactivation)) 411 if ((ActivationState == ActivationState.IslandSleeping) || (ActivationState == ActivationState.DisableDeactivation))
412 return; 412 return;
413 413
414 if ((LinearVelocity.LengthSquared() < LinearSleepingThreshold * LinearSleepingThreshold) && 414 if ((LinearVelocity.LengthSquared() < LinearSleepingThreshold * LinearSleepingThreshold) &&
415 (AngularVelocity.LengthSquared() < AngularSleepingThreshold * AngularSleepingThreshold)) 415 (AngularVelocity.LengthSquared() < AngularSleepingThreshold * AngularSleepingThreshold))
416 { 416 {
417 DeactivationTime += timeStep; 417 DeactivationTime += timeStep;
418 } 418 }
419 else 419 else
420 { 420 {
421 DeactivationTime = 0; 421 DeactivationTime = 0;
422 ActivationState = ActivationState.Nothing; 422 ActivationState = ActivationState.Nothing;
423 } 423 }
424 424
425 } 425 }
426 426
427 public bool WantsSleeping() 427 public bool WantsSleeping()
428 { 428 {
429 429
430 if (ActivationState == ActivationState.DisableDeactivation) 430 if (ActivationState == ActivationState.DisableDeactivation)
431 return false; 431 return false;
432 432
433 //disable deactivation 433 //disable deactivation
434 if (DisableDeactivation || (RigidBodyDeactivationTime == 0)) 434 if (DisableDeactivation || (RigidBodyDeactivationTime == 0))
435 return false; 435 return false;
436 436
437 if ((ActivationState == ActivationState.IslandSleeping) || (ActivationState == ActivationState.WantsDeactivation)) 437 if ((ActivationState == ActivationState.IslandSleeping) || (ActivationState == ActivationState.WantsDeactivation))
438 return true; 438 return true;
439 439
440 if (DeactivationTime > RigidBodyDeactivationTime) 440 if (DeactivationTime > RigidBodyDeactivationTime)
441 { 441 {
442 return true; 442 return true;
443 } 443 }
444 return false; 444 return false;
445 } 445 }
446 } 446 }
447} 447}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/SimpleDynamicsWorld.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/SimpleDynamicsWorld.cs
index aa91a03..3430a73 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/SimpleDynamicsWorld.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/SimpleDynamicsWorld.cs
@@ -1,211 +1,211 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX.Dynamics 27namespace XnaDevRu.BulletX.Dynamics
28{ 28{
29 public class SimpleDynamicsWorld : DynamicsWorld 29 public class SimpleDynamicsWorld : DynamicsWorld
30 { 30 {
31 private IConstraintSolver _constraintSolver; 31 private IConstraintSolver _constraintSolver;
32 private bool _ownsConstraintSolver; 32 private bool _ownsConstraintSolver;
33 private Vector3 _gravity; 33 private Vector3 _gravity;
34 private IDebugDraw _debugDrawer; 34 private IDebugDraw _debugDrawer;
35 35
36 /// <summary> 36 /// <summary>
37 /// this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver 37 /// this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver
38 /// </summary> 38 /// </summary>
39 /// <param name="dispatcher"></param> 39 /// <param name="dispatcher"></param>
40 /// <param name="pairCache"></param> 40 /// <param name="pairCache"></param>
41 /// <param name="constraintSolver"></param> 41 /// <param name="constraintSolver"></param>
42 public SimpleDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache, IConstraintSolver constraintSolver) 42 public SimpleDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache, IConstraintSolver constraintSolver)
43 : base(dispatcher, pairCache) 43 : base(dispatcher, pairCache)
44 { 44 {
45 _constraintSolver = constraintSolver; 45 _constraintSolver = constraintSolver;
46 _ownsConstraintSolver = false; 46 _ownsConstraintSolver = false;
47 _gravity = new Vector3(0, 0, -10); 47 _gravity = new Vector3(0, 0, -10);
48 } 48 }
49 49
50 public override Vector3 Gravity 50 public override Vector3 Gravity
51 { 51 {
52 set 52 set
53 { 53 {
54 _gravity = value; 54 _gravity = value;
55 for (int i = 0; i < CollisionObjects.Count; i++) 55 for (int i = 0; i < CollisionObjects.Count; i++)
56 { 56 {
57 CollisionObject colObj = CollisionObjects[i]; 57 CollisionObject colObj = CollisionObjects[i];
58 RigidBody body = RigidBody.Upcast(colObj); 58 RigidBody body = RigidBody.Upcast(colObj);
59 if (body != null) 59 if (body != null)
60 { 60 {
61 body.Gravity = value; 61 body.Gravity = value;
62 } 62 }
63 } 63 }
64 } 64 }
65 } 65 }
66 66
67 public override IConstraintSolver ConstraintSolver 67 public override IConstraintSolver ConstraintSolver
68 { 68 {
69 set 69 set
70 { 70 {
71 _ownsConstraintSolver = false; 71 _ownsConstraintSolver = false;
72 _constraintSolver = value; 72 _constraintSolver = value;
73 } 73 }
74 } 74 }
75 75
76 public override IDebugDraw DebugDrawer 76 public override IDebugDraw DebugDrawer
77 { 77 {
78 get 78 get
79 { 79 {
80 return _debugDrawer; 80 return _debugDrawer;
81 } 81 }
82 set 82 set
83 { 83 {
84 _debugDrawer = value; 84 _debugDrawer = value;
85 } 85 }
86 } 86 }
87 87
88 public override void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep) 88 public override void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep)
89 { 89 {
90 //apply gravity, predict motion 90 //apply gravity, predict motion
91 PredictUnconstraintMotion(timeStep); 91 PredictUnconstraintMotion(timeStep);
92 92
93 DispatcherInfo dispatchInfo = new DispatcherInfo(); 93 DispatcherInfo dispatchInfo = new DispatcherInfo();
94 dispatchInfo.TimeStep = timeStep; 94 dispatchInfo.TimeStep = timeStep;
95 dispatchInfo.StepCount = 0; 95 dispatchInfo.StepCount = 0;
96 dispatchInfo.DebugDraw = DebugDrawer; 96 dispatchInfo.DebugDraw = DebugDrawer;
97 //perform collision detection 97 //perform collision detection
98 PerformDiscreteCollisionDetection(); 98 PerformDiscreteCollisionDetection();
99 99
100 //solve contact constraints 100 //solve contact constraints
101 int numManifolds = Dispatcher.ManifoldCount; 101 int numManifolds = Dispatcher.ManifoldCount;
102 if (numManifolds != 0) 102 if (numManifolds != 0)
103 { 103 {
104 104
105 List<PersistentManifold> manifolds = (Dispatcher as CollisionDispatcher).Manifolds; 105 List<PersistentManifold> manifolds = (Dispatcher as CollisionDispatcher).Manifolds;
106 //int numManifolds = m_dispatcher1.GetNumManifolds(); 106 //int numManifolds = m_dispatcher1.GetNumManifolds();
107 ContactSolverInfo infoGlobal = new ContactSolverInfo(); 107 ContactSolverInfo infoGlobal = new ContactSolverInfo();
108 infoGlobal.TimeStep = timeStep; 108 infoGlobal.TimeStep = timeStep;
109 109
110 _constraintSolver.SolveGroup(new List<CollisionObject>(), manifolds, manifolds.Count, new List<TypedConstraint>(), infoGlobal, _debugDrawer); 110 _constraintSolver.SolveGroup(new List<CollisionObject>(), manifolds, manifolds.Count, new List<TypedConstraint>(), infoGlobal, _debugDrawer);
111 } 111 }
112 //integrate transforms 112 //integrate transforms
113 IntegrateTransforms(timeStep); 113 IntegrateTransforms(timeStep);
114 114
115 UpdateAabbs(); 115 UpdateAabbs();
116 116
117 SynchronizeMotionStates(); 117 SynchronizeMotionStates();
118 } 118 }
119 119
120 public override void UpdateAabbs() 120 public override void UpdateAabbs()
121 { 121 {
122 for (int i = 0; i < CollisionObjects.Count; i++) 122 for (int i = 0; i < CollisionObjects.Count; i++)
123 { 123 {
124 CollisionObject colObj = CollisionObjects[i]; 124 CollisionObject colObj = CollisionObjects[i];
125 RigidBody body = RigidBody.Upcast(colObj); 125 RigidBody body = RigidBody.Upcast(colObj);
126 if (body != null) 126 if (body != null)
127 { 127 {
128 if (body.IsActive && (!body.IsStaticObject)) 128 if (body.IsActive && (!body.IsStaticObject))
129 { 129 {
130 Vector3 minAabb, maxAabb; 130 Vector3 minAabb, maxAabb;
131 colObj.CollisionShape.GetAabb(colObj.WorldTransform, out minAabb, out maxAabb); 131 colObj.CollisionShape.GetAabb(colObj.WorldTransform, out minAabb, out maxAabb);
132 IBroadphase bp = Broadphase; 132 IBroadphase bp = Broadphase;
133 bp.SetAabb(body.Broadphase, minAabb, maxAabb); 133 bp.SetAabb(body.Broadphase, minAabb, maxAabb);
134 } 134 }
135 } 135 }
136 } 136 }
137 } 137 }
138 138
139 public override void AddRigidBody(RigidBody body) 139 public override void AddRigidBody(RigidBody body)
140 { 140 {
141 body.Gravity = _gravity; 141 body.Gravity = _gravity;
142 142
143 if (body.CollisionShape != null) 143 if (body.CollisionShape != null)
144 { 144 {
145 AddCollisionObject(body); 145 AddCollisionObject(body);
146 } 146 }
147 } 147 }
148 148
149 public override void RemoveRigidBody(RigidBody body) 149 public override void RemoveRigidBody(RigidBody body)
150 { 150 {
151 RemoveCollisionObject(body); 151 RemoveCollisionObject(body);
152 } 152 }
153 153
154 public void SynchronizeMotionStates() 154 public void SynchronizeMotionStates()
155 { 155 {
156 for (int i = 0; i < CollisionObjects.Count; i++) 156 for (int i = 0; i < CollisionObjects.Count; i++)
157 { 157 {
158 CollisionObject colObj = CollisionObjects[i]; 158 CollisionObject colObj = CollisionObjects[i];
159 RigidBody body = RigidBody.Upcast(colObj); 159 RigidBody body = RigidBody.Upcast(colObj);
160 if (body != null && body.MotionState != null) 160 if (body != null && body.MotionState != null)
161 { 161 {
162 if (body.ActivationState != ActivationState.IslandSleeping) 162 if (body.ActivationState != ActivationState.IslandSleeping)
163 { 163 {
164 body.MotionState.SetWorldTransform(body.WorldTransform); 164 body.MotionState.SetWorldTransform(body.WorldTransform);
165 } 165 }
166 } 166 }
167 } 167 }
168 } 168 }
169 169
170 protected void PredictUnconstraintMotion(float timeStep) 170 protected void PredictUnconstraintMotion(float timeStep)
171 { 171 {
172 for (int i = 0; i < CollisionObjects.Count; i++) 172 for (int i = 0; i < CollisionObjects.Count; i++)
173 { 173 {
174 CollisionObject colObj = CollisionObjects[i]; 174 CollisionObject colObj = CollisionObjects[i];
175 RigidBody body = RigidBody.Upcast(colObj); 175 RigidBody body = RigidBody.Upcast(colObj);
176 if (body != null) 176 if (body != null)
177 { 177 {
178 if (!body.IsStaticObject) 178 if (!body.IsStaticObject)
179 { 179 {
180 if (body.IsActive) 180 if (body.IsActive)
181 { 181 {
182 body.ApplyForces(timeStep); 182 body.ApplyForces(timeStep);
183 body.IntegrateVelocities(timeStep); 183 body.IntegrateVelocities(timeStep);
184 Matrix temp = body.InterpolationWorldTransform; 184 Matrix temp = body.InterpolationWorldTransform;
185 body.PredictIntegratedTransform(timeStep, ref temp); 185 body.PredictIntegratedTransform(timeStep, ref temp);
186 body.InterpolationWorldTransform = temp; 186 body.InterpolationWorldTransform = temp;
187 } 187 }
188 } 188 }
189 } 189 }
190 } 190 }
191 } 191 }
192 192
193 protected void IntegrateTransforms(float timeStep) 193 protected void IntegrateTransforms(float timeStep)
194 { 194 {
195 Matrix predictedTrans = Matrix.Identity; 195 Matrix predictedTrans = Matrix.Identity;
196 for (int i = 0; i < CollisionObjects.Count; i++) 196 for (int i = 0; i < CollisionObjects.Count; i++)
197 { 197 {
198 CollisionObject colObj = CollisionObjects[i]; 198 CollisionObject colObj = CollisionObjects[i];
199 RigidBody body = RigidBody.Upcast(colObj); 199 RigidBody body = RigidBody.Upcast(colObj);
200 if (body != null) 200 if (body != null)
201 { 201 {
202 if (body.IsActive && (!body.IsStaticObject)) 202 if (body.IsActive && (!body.IsStaticObject))
203 { 203 {
204 body.PredictIntegratedTransform(timeStep, ref predictedTrans); 204 body.PredictIntegratedTransform(timeStep, ref predictedTrans);
205 body.ProceedToTransform(predictedTrans); 205 body.ProceedToTransform(predictedTrans);
206 } 206 }
207 } 207 }
208 } 208 }
209 } 209 }
210 } 210 }
211} 211}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/RaycastVehicle.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/RaycastVehicle.cs
index 90db9ee..4e0e64b 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/RaycastVehicle.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/RaycastVehicle.cs
@@ -1,94 +1,94 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX.Dynamics 27namespace XnaDevRu.BulletX.Dynamics
28{ 28{
29 public class RaycastVehicle : TypedConstraint 29 public class RaycastVehicle : TypedConstraint
30 { 30 {
31 public override void BuildJacobian() 31 public override void BuildJacobian()
32 { 32 {
33 throw new Exception("The method or operation is not implemented."); 33 throw new Exception("The method or operation is not implemented.");
34 } 34 }
35 35
36 public override void SolveConstraint(float timeStep) 36 public override void SolveConstraint(float timeStep)
37 { 37 {
38 throw new Exception("The method or operation is not implemented."); 38 throw new Exception("The method or operation is not implemented.");
39 } 39 }
40 40
41 public int getNumWheels() 41 public int getNumWheels()
42 { 42 {
43 throw new Exception("The method or operation is not implemented."); 43 throw new Exception("The method or operation is not implemented.");
44 } 44 }
45 45
46 public WheelInfo getWheelInfo(int v) 46 public WheelInfo getWheelInfo(int v)
47 { 47 {
48 throw new Exception("The method or operation is not implemented."); 48 throw new Exception("The method or operation is not implemented.");
49 } 49 }
50 50
51 public void updateWheelTransform(int v, bool p) 51 public void updateWheelTransform(int v, bool p)
52 { 52 {
53 throw new Exception("The method or operation is not implemented."); 53 throw new Exception("The method or operation is not implemented.");
54 } 54 }
55 55
56 public int getRightAxis() 56 public int getRightAxis()
57 { 57 {
58 throw new Exception("The method or operation is not implemented."); 58 throw new Exception("The method or operation is not implemented.");
59 } 59 }
60 60
61 public void updateVehicle(float timeStep) 61 public void updateVehicle(float timeStep)
62 { 62 {
63 throw new Exception("The method or operation is not implemented."); 63 throw new Exception("The method or operation is not implemented.");
64 } 64 }
65 } 65 }
66 66
67 public class DefaultVehicleRaycaster : IVehicleRaycaster 67 public class DefaultVehicleRaycaster : IVehicleRaycaster
68 { 68 {
69 DynamicsWorld _dynamicsWorld; 69 DynamicsWorld _dynamicsWorld;
70 70
71 public DefaultVehicleRaycaster(DynamicsWorld world) 71 public DefaultVehicleRaycaster(DynamicsWorld world)
72 { 72 {
73 _dynamicsWorld = world; 73 _dynamicsWorld = world;
74 } 74 }
75 75
76 public object CastRay(Vector3 from, Vector3 to, out VehicleRaycasterResult result) 76 public object CastRay(Vector3 from, Vector3 to, out VehicleRaycasterResult result)
77 { 77 {
78 CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(from, to); 78 CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(from, to);
79 _dynamicsWorld.RayTest(from, to, rayCallback); 79 _dynamicsWorld.RayTest(from, to, rayCallback);
80 80
81 result = new VehicleRaycasterResult(); 81 result = new VehicleRaycasterResult();
82 82
83 if (!rayCallback.HasHit) return 0; 83 if (!rayCallback.HasHit) return 0;
84 RigidBody body = RigidBody.Upcast(rayCallback.CollisionObject); 84 RigidBody body = RigidBody.Upcast(rayCallback.CollisionObject);
85 if (body == null) return 0; 85 if (body == null) return 0;
86 86
87 result.HitPointInWorld = rayCallback.HitPointWorld; 87 result.HitPointInWorld = rayCallback.HitPointWorld;
88 result.HitNormalInWorld = rayCallback.HitNormalWorld; 88 result.HitNormalInWorld = rayCallback.HitNormalWorld;
89 result.HitNormalInWorld.Normalize(); 89 result.HitNormalInWorld.Normalize();
90 result.DistFraction = rayCallback.ClosestHitFraction; 90 result.DistFraction = rayCallback.ClosestHitFraction;
91 return body; 91 return body;
92 } 92 }
93 } 93 }
94} \ No newline at end of file 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
index 86829e3..14c13c6 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/VehicleRaycaster.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/VehicleRaycaster.cs
@@ -1,49 +1,49 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX.Dynamics 27namespace XnaDevRu.BulletX.Dynamics
28{ 28{
29 public interface IVehicleRaycaster 29 public interface IVehicleRaycaster
30 { 30 {
31 object CastRay(Vector3 from, Vector3 to, out VehicleRaycasterResult result); 31 object CastRay(Vector3 from, Vector3 to, out VehicleRaycasterResult result);
32 } 32 }
33 33
34 public class VehicleRaycasterResult 34 public class VehicleRaycasterResult
35 { 35 {
36 private Single _distFraction; 36 private Single _distFraction;
37 private Vector3 _hitNormalInWorld; 37 private Vector3 _hitNormalInWorld;
38 private Vector3 _hitPointInWorld; 38 private Vector3 _hitPointInWorld;
39 39
40 public VehicleRaycasterResult() 40 public VehicleRaycasterResult()
41 { 41 {
42 _distFraction = -1; 42 _distFraction = -1;
43 } 43 }
44 44
45 public float DistFraction { get { return _distFraction; } set { _distFraction = value; } } 45 public float DistFraction { get { return _distFraction; } set { _distFraction = value; } }
46 public Vector3 HitNormalInWorld { get { return _hitNormalInWorld; } set { _hitNormalInWorld = value; } } 46 public Vector3 HitNormalInWorld { get { return _hitNormalInWorld; } set { _hitNormalInWorld = value; } }
47 public Vector3 HitPointInWorld { get { return _hitPointInWorld; } set { _hitPointInWorld = value; } } 47 public Vector3 HitPointInWorld { get { return _hitPointInWorld; } set { _hitPointInWorld = value; } }
48 } 48 }
49} 49}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/WheelInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/WheelInfo.cs
index d4a94f3..516e455 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/WheelInfo.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/WheelInfo.cs
@@ -1,529 +1,529 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX.Dynamics 27namespace XnaDevRu.BulletX.Dynamics
28{ 28{
29 public struct WheelInfoConstructionInfo 29 public struct WheelInfoConstructionInfo
30 { 30 {
31 private Vector3 _chassicConnectionCS; 31 private Vector3 _chassicConnectionCS;
32 private Vector3 _wheelDirectionCS; 32 private Vector3 _wheelDirectionCS;
33 private Vector3 _wheelAxisCS; 33 private Vector3 _wheelAxisCS;
34 34
35 private Single _suspensionRestLength; 35 private Single _suspensionRestLength;
36 private Single _maxSuspensionTravelCm; 36 private Single _maxSuspensionTravelCm;
37 private Single _wheelRadius; 37 private Single _wheelRadius;
38 private Single _suspensionStiffness; 38 private Single _suspensionStiffness;
39 private Single _wheelsDampingCompression; 39 private Single _wheelsDampingCompression;
40 private Single _wheelsDampingRelaxation; 40 private Single _wheelsDampingRelaxation;
41 private Single _frictionSlip; 41 private Single _frictionSlip;
42 42
43 private Boolean _isFrontWheel; 43 private Boolean _isFrontWheel;
44 44
45 #region Basic Properties 45 #region Basic Properties
46 public Vector3 ChassicConnectionCS 46 public Vector3 ChassicConnectionCS
47 { 47 {
48 get { return _chassicConnectionCS; } 48 get { return _chassicConnectionCS; }
49 set { _chassicConnectionCS = value; } 49 set { _chassicConnectionCS = value; }
50 } 50 }
51 51
52 public Vector3 WheelDirectionCS 52 public Vector3 WheelDirectionCS
53 { 53 {
54 get { return _wheelDirectionCS; } 54 get { return _wheelDirectionCS; }
55 set { _wheelDirectionCS = value; } 55 set { _wheelDirectionCS = value; }
56 } 56 }
57 57
58 public Vector3 WheelAxleCS 58 public Vector3 WheelAxleCS
59 { 59 {
60 get { return _wheelAxisCS; } 60 get { return _wheelAxisCS; }
61 set { _wheelAxisCS = value; } 61 set { _wheelAxisCS = value; }
62 } 62 }
63 63
64 64
65 public Single SuspensionRestLength 65 public Single SuspensionRestLength
66 { 66 {
67 get { return _suspensionRestLength; } 67 get { return _suspensionRestLength; }
68 set { _suspensionRestLength = value; } 68 set { _suspensionRestLength = value; }
69 } 69 }
70 70
71 public Single MaxSuspensionTravelCm 71 public Single MaxSuspensionTravelCm
72 { 72 {
73 get { return _maxSuspensionTravelCm; } 73 get { return _maxSuspensionTravelCm; }
74 set { _maxSuspensionTravelCm = value; } 74 set { _maxSuspensionTravelCm = value; }
75 } 75 }
76 76
77 public Single WheelRadius 77 public Single WheelRadius
78 { 78 {
79 get { return _wheelRadius; } 79 get { return _wheelRadius; }
80 set { _wheelRadius = value; } 80 set { _wheelRadius = value; }
81 } 81 }
82 82
83 83
84 public Single SuspensionStiffness 84 public Single SuspensionStiffness
85 { 85 {
86 get { return _suspensionStiffness; } 86 get { return _suspensionStiffness; }
87 set { _suspensionStiffness = value; } 87 set { _suspensionStiffness = value; }
88 } 88 }
89 89
90 public Single WheelsDampingCompression 90 public Single WheelsDampingCompression
91 { 91 {
92 get { return _wheelsDampingCompression; } 92 get { return _wheelsDampingCompression; }
93 set { _wheelsDampingCompression = value; } 93 set { _wheelsDampingCompression = value; }
94 } 94 }
95 95
96 public Single WheelsDampingRelaxation 96 public Single WheelsDampingRelaxation
97 { 97 {
98 get { return _wheelsDampingRelaxation; } 98 get { return _wheelsDampingRelaxation; }
99 set { _wheelsDampingRelaxation = value; } 99 set { _wheelsDampingRelaxation = value; }
100 } 100 }
101 101
102 public Single FrictionSlip 102 public Single FrictionSlip
103 { 103 {
104 get { return _frictionSlip; } 104 get { return _frictionSlip; }
105 set { _frictionSlip = value; } 105 set { _frictionSlip = value; }
106 } 106 }
107 107
108 108
109 public Boolean IsFrontWheel 109 public Boolean IsFrontWheel
110 { 110 {
111 get { return _isFrontWheel; } 111 get { return _isFrontWheel; }
112 set { _isFrontWheel = value; } 112 set { _isFrontWheel = value; }
113 } 113 }
114 #endregion 114 #endregion
115 } 115 }
116 116
117 public struct RaycastInfo 117 public struct RaycastInfo
118 { 118 {
119 private Vector3 _contractNormalWS; 119 private Vector3 _contractNormalWS;
120 private Vector3 _contractPointWS; 120 private Vector3 _contractPointWS;
121 121
122 private Vector3 _hardPointWS; 122 private Vector3 _hardPointWS;
123 private Vector3 _wheelDirectionWS; 123 private Vector3 _wheelDirectionWS;
124 private Vector3 _wheelAxleWS; 124 private Vector3 _wheelAxleWS;
125 125
126 private Single _suspensionLength; 126 private Single _suspensionLength;
127 private Boolean _isInContract; 127 private Boolean _isInContract;
128 128
129 #region Basic Properties 129 #region Basic Properties
130 public Single SuspensionLength 130 public Single SuspensionLength
131 { 131 {
132 get { return _suspensionLength; } 132 get { return _suspensionLength; }
133 set { _suspensionLength = value; } 133 set { _suspensionLength = value; }
134 } 134 }
135 135
136 public Boolean IsInContact 136 public Boolean IsInContact
137 { 137 {
138 get { return _isInContract; } 138 get { return _isInContract; }
139 set { _isInContract = value; } 139 set { _isInContract = value; }
140 } 140 }
141 141
142 public Vector3 ContactNormalWS 142 public Vector3 ContactNormalWS
143 { 143 {
144 get { return _contractNormalWS; } 144 get { return _contractNormalWS; }
145 set { _contractNormalWS = value; } 145 set { _contractNormalWS = value; }
146 } 146 }
147 147
148 public Vector3 ContactPointWS 148 public Vector3 ContactPointWS
149 { 149 {
150 get { return _contractPointWS; } 150 get { return _contractPointWS; }
151 set { _contractPointWS = value; } 151 set { _contractPointWS = value; }
152 } 152 }
153 153
154 public Vector3 HardPointWS 154 public Vector3 HardPointWS
155 { 155 {
156 get { return _hardPointWS; } 156 get { return _hardPointWS; }
157 set { _hardPointWS = value; } 157 set { _hardPointWS = value; }
158 } 158 }
159 159
160 public Vector3 WheelDirectionWS 160 public Vector3 WheelDirectionWS
161 { 161 {
162 get { return _wheelDirectionWS; } 162 get { return _wheelDirectionWS; }
163 set { _wheelDirectionWS = value; } 163 set { _wheelDirectionWS = value; }
164 } 164 }
165 165
166 public Vector3 WheelAxleWS 166 public Vector3 WheelAxleWS
167 { 167 {
168 get { return _wheelAxleWS; } 168 get { return _wheelAxleWS; }
169 set { _wheelAxleWS = value; } 169 set { _wheelAxleWS = value; }
170 } 170 }
171 #endregion 171 #endregion
172 } 172 }
173 173
174 public struct WheelInfo 174 public struct WheelInfo
175 { 175 {
176 private RaycastInfo _raycastInfo; 176 private RaycastInfo _raycastInfo;
177 177
178 private Matrix _worldTransform; 178 private Matrix _worldTransform;
179 179
180 private Vector3 _chassicConnectionPointCS; 180 private Vector3 _chassicConnectionPointCS;
181 private Vector3 _wheelDirectionCS; 181 private Vector3 _wheelDirectionCS;
182 private Vector3 _wheelAxleCS; 182 private Vector3 _wheelAxleCS;
183 183
184 private Single _suspensionRestLength; 184 private Single _suspensionRestLength;
185 private Single _maxSuspensionTravelCm; 185 private Single _maxSuspensionTravelCm;
186 186
187 private Single _wheelsRadius; 187 private Single _wheelsRadius;
188 private Single _rollInfluence; 188 private Single _rollInfluence;
189 private Single _suspensionStiffness; 189 private Single _suspensionStiffness;
190 private Single _wheelsDampingCompression; 190 private Single _wheelsDampingCompression;
191 private Single _wheelsDampingRelaxation; 191 private Single _wheelsDampingRelaxation;
192 private Single _frictionSlip; 192 private Single _frictionSlip;
193 private Single _steering; 193 private Single _steering;
194 private Single _rotation; 194 private Single _rotation;
195 private Single _deltaRotation; 195 private Single _deltaRotation;
196 196
197 private Single _engineForce; 197 private Single _engineForce;
198 private Single _brake; 198 private Single _brake;
199 private Boolean _isFrontWheel; 199 private Boolean _isFrontWheel;
200 200
201 201
202 private Single _clippedInvContactDotSuspension; 202 private Single _clippedInvContactDotSuspension;
203 private Single _skidInfo; 203 private Single _skidInfo;
204 private Single _wheelsSuspensionForce; 204 private Single _wheelsSuspensionForce;
205 private Single _suspensionRelativeVelocity; 205 private Single _suspensionRelativeVelocity;
206 //can be used to store pointer to sync transforms... 206 //can be used to store pointer to sync transforms...
207 private object _clientInfo; 207 private object _clientInfo;
208 208
209 #region Constructor 209 #region Constructor
210 public WheelInfo(WheelInfoConstructionInfo constructionInfo) 210 public WheelInfo(WheelInfoConstructionInfo constructionInfo)
211 { 211 {
212 _suspensionRestLength = constructionInfo.SuspensionRestLength; 212 _suspensionRestLength = constructionInfo.SuspensionRestLength;
213 _maxSuspensionTravelCm = constructionInfo.MaxSuspensionTravelCm; 213 _maxSuspensionTravelCm = constructionInfo.MaxSuspensionTravelCm;
214 214
215 _wheelsRadius = constructionInfo.WheelRadius; 215 _wheelsRadius = constructionInfo.WheelRadius;
216 _wheelsDampingCompression = constructionInfo.WheelsDampingCompression; 216 _wheelsDampingCompression = constructionInfo.WheelsDampingCompression;
217 _wheelsDampingRelaxation = constructionInfo.WheelsDampingRelaxation; 217 _wheelsDampingRelaxation = constructionInfo.WheelsDampingRelaxation;
218 _wheelDirectionCS = constructionInfo.WheelDirectionCS; 218 _wheelDirectionCS = constructionInfo.WheelDirectionCS;
219 219
220 _suspensionStiffness = constructionInfo.SuspensionStiffness; 220 _suspensionStiffness = constructionInfo.SuspensionStiffness;
221 _chassicConnectionPointCS = constructionInfo.ChassicConnectionCS; 221 _chassicConnectionPointCS = constructionInfo.ChassicConnectionCS;
222 222
223 _wheelAxleCS = constructionInfo.WheelAxleCS; 223 _wheelAxleCS = constructionInfo.WheelAxleCS;
224 _frictionSlip = constructionInfo.FrictionSlip; 224 _frictionSlip = constructionInfo.FrictionSlip;
225 225
226 _clippedInvContactDotSuspension = 0; 226 _clippedInvContactDotSuspension = 0;
227 _suspensionRelativeVelocity = 0; 227 _suspensionRelativeVelocity = 0;
228 _wheelsSuspensionForce = 0; 228 _wheelsSuspensionForce = 0;
229 _skidInfo = 0; 229 _skidInfo = 0;
230 230
231 _steering = 0; 231 _steering = 0;
232 _engineForce = 0; 232 _engineForce = 0;
233 _rotation = 0; 233 _rotation = 0;
234 _rotation = 0; 234 _rotation = 0;
235 _deltaRotation = 0; 235 _deltaRotation = 0;
236 _brake = 0; 236 _brake = 0;
237 _rollInfluence = 0.1f; 237 _rollInfluence = 0.1f;
238 _brake = 0; 238 _brake = 0;
239 _rollInfluence = 0.1f; 239 _rollInfluence = 0.1f;
240 240
241 _isFrontWheel = constructionInfo.IsFrontWheel; 241 _isFrontWheel = constructionInfo.IsFrontWheel;
242 242
243 _raycastInfo = default(RaycastInfo); 243 _raycastInfo = default(RaycastInfo);
244 _worldTransform = default(Matrix); 244 _worldTransform = default(Matrix);
245 _clientInfo = null; 245 _clientInfo = null;
246 } 246 }
247 #endregion 247 #endregion
248 248
249 #region BasicProperties 249 #region BasicProperties
250 public object ClientInfo { get { return _clientInfo; } set { _clientInfo = value; } } 250 public object ClientInfo { get { return _clientInfo; } set { _clientInfo = value; } }
251 251
252 public RaycastInfo RaycastInfo 252 public RaycastInfo RaycastInfo
253 { 253 {
254 get { return _raycastInfo; } 254 get { return _raycastInfo; }
255 set { _raycastInfo = value; } 255 set { _raycastInfo = value; }
256 } 256 }
257 257
258 public Matrix WorldTransform 258 public Matrix WorldTransform
259 { 259 {
260 get { return _worldTransform; } 260 get { return _worldTransform; }
261 set { _worldTransform = value; } 261 set { _worldTransform = value; }
262 } 262 }
263 263
264 public Vector3 ChassicConnectionPointCS 264 public Vector3 ChassicConnectionPointCS
265 { 265 {
266 get { return _chassicConnectionPointCS; } 266 get { return _chassicConnectionPointCS; }
267 set { _chassicConnectionPointCS = value; } 267 set { _chassicConnectionPointCS = value; }
268 } 268 }
269 public Vector3 WheelDirectionCS 269 public Vector3 WheelDirectionCS
270 { 270 {
271 get { return _wheelDirectionCS; } 271 get { return _wheelDirectionCS; }
272 set { _wheelDirectionCS = value; } 272 set { _wheelDirectionCS = value; }
273 } 273 }
274 public Vector3 WheelAxleCS 274 public Vector3 WheelAxleCS
275 { 275 {
276 get { return _wheelAxleCS; } 276 get { return _wheelAxleCS; }
277 set { _wheelAxleCS = value; } 277 set { _wheelAxleCS = value; }
278 } 278 }
279 279
280 public Single SuspensionRestLength 280 public Single SuspensionRestLength
281 { 281 {
282 get { return _suspensionRestLength; } 282 get { return _suspensionRestLength; }
283 set { _suspensionRestLength = value; } 283 set { _suspensionRestLength = value; }
284 } 284 }
285 285
286 286
287 public Single MaxSuspensionTravelCm 287 public Single MaxSuspensionTravelCm
288 { 288 {
289 get { return _maxSuspensionTravelCm; } 289 get { return _maxSuspensionTravelCm; }
290 set { _maxSuspensionTravelCm = value; } 290 set { _maxSuspensionTravelCm = value; }
291 } 291 }
292 292
293 public Single WheelsRadius 293 public Single WheelsRadius
294 { 294 {
295 get { return _wheelsRadius; } 295 get { return _wheelsRadius; }
296 set { _wheelsRadius = value; } 296 set { _wheelsRadius = value; }
297 } 297 }
298 298
299 public Single SuspensionStiffness 299 public Single SuspensionStiffness
300 { 300 {
301 get { return _suspensionStiffness; } 301 get { return _suspensionStiffness; }
302 set { _suspensionStiffness = value; } 302 set { _suspensionStiffness = value; }
303 } 303 }
304 304
305 public Single WheelsDampingCompression 305 public Single WheelsDampingCompression
306 { 306 {
307 get { return _wheelsDampingCompression; } 307 get { return _wheelsDampingCompression; }
308 set { _wheelsDampingCompression = value; } 308 set { _wheelsDampingCompression = value; }
309 } 309 }
310 310
311 public Single WheelsDampingRelaxation 311 public Single WheelsDampingRelaxation
312 { 312 {
313 get { return _wheelsDampingRelaxation; } 313 get { return _wheelsDampingRelaxation; }
314 set { _wheelsDampingRelaxation = value; } 314 set { _wheelsDampingRelaxation = value; }
315 } 315 }
316 316
317 public Single FrictionSlip 317 public Single FrictionSlip
318 { 318 {
319 get { return _frictionSlip; } 319 get { return _frictionSlip; }
320 set { _frictionSlip = value; } 320 set { _frictionSlip = value; }
321 } 321 }
322 322
323 public Single Steering 323 public Single Steering
324 { 324 {
325 get { return _steering; } 325 get { return _steering; }
326 set { _steering = value; } 326 set { _steering = value; }
327 } 327 }
328 328
329 public Single Rotation 329 public Single Rotation
330 { 330 {
331 get { return _rotation; } 331 get { return _rotation; }
332 set { _rotation = value; } 332 set { _rotation = value; }
333 } 333 }
334 334
335 public Single DeltaRotation 335 public Single DeltaRotation
336 { 336 {
337 get { return _deltaRotation; } 337 get { return _deltaRotation; }
338 set { _deltaRotation = value; } 338 set { _deltaRotation = value; }
339 } 339 }
340 340
341 public Single RollInfluence 341 public Single RollInfluence
342 { 342 {
343 get { return _rollInfluence; } 343 get { return _rollInfluence; }
344 set { _rollInfluence = value; } 344 set { _rollInfluence = value; }
345 } 345 }
346 346
347 public Single EngineForce 347 public Single EngineForce
348 { 348 {
349 get { return _engineForce; } 349 get { return _engineForce; }
350 set { _engineForce = value; } 350 set { _engineForce = value; }
351 } 351 }
352 352
353 public Single Brake 353 public Single Brake
354 { 354 {
355 get { return _brake; } 355 get { return _brake; }
356 set { _brake = value; } 356 set { _brake = value; }
357 } 357 }
358 358
359 public Boolean IsFrontWheel 359 public Boolean IsFrontWheel
360 { 360 {
361 get { return _isFrontWheel; } 361 get { return _isFrontWheel; }
362 set { _isFrontWheel = value; } 362 set { _isFrontWheel = value; }
363 } 363 }
364 364
365 public Single ClippedInvContactDotSuspension 365 public Single ClippedInvContactDotSuspension
366 { 366 {
367 get { return _clippedInvContactDotSuspension; } 367 get { return _clippedInvContactDotSuspension; }
368 set { _clippedInvContactDotSuspension = value; } 368 set { _clippedInvContactDotSuspension = value; }
369 } 369 }
370 370
371 public Single SuspensionRelativeVelocity 371 public Single SuspensionRelativeVelocity
372 { 372 {
373 get { return _suspensionRelativeVelocity; } 373 get { return _suspensionRelativeVelocity; }
374 set { _suspensionRelativeVelocity = value; } 374 set { _suspensionRelativeVelocity = value; }
375 } 375 }
376 376
377 public Single WheelsSuspensionForce 377 public Single WheelsSuspensionForce
378 { 378 {
379 get { return _wheelsSuspensionForce; } 379 get { return _wheelsSuspensionForce; }
380 set { _wheelsSuspensionForce = value; } 380 set { _wheelsSuspensionForce = value; }
381 } 381 }
382 382
383 public Single SkidInfo 383 public Single SkidInfo
384 { 384 {
385 get { return _skidInfo; } 385 get { return _skidInfo; }
386 set { _skidInfo = value; } 386 set { _skidInfo = value; }
387 } 387 }
388 #endregion 388 #endregion
389 389
390 /// <summary> 390 /// <summary>
391 /// 391 ///
392 /// </summary> 392 /// </summary>
393 /// <param name="chassis"></param> 393 /// <param name="chassis"></param>
394 /// <param name="paramRaycastInfo">Not used!</param> 394 /// <param name="paramRaycastInfo">Not used!</param>
395 public void UpdateWheel(RigidBody chassis, RaycastInfo paramRaycastInfo) 395 public void UpdateWheel(RigidBody chassis, RaycastInfo paramRaycastInfo)
396 { 396 {
397 if (_raycastInfo.IsInContact) 397 if (_raycastInfo.IsInContact)
398 { 398 {
399 float project = Vector3.Dot(_raycastInfo.ContactNormalWS, _raycastInfo.WheelDirectionWS); 399 float project = Vector3.Dot(_raycastInfo.ContactNormalWS, _raycastInfo.WheelDirectionWS);
400 400
401 Vector3 chassisVelocityAtContactPoint = new Vector3(); 401 Vector3 chassisVelocityAtContactPoint = new Vector3();
402 Vector3 relpos = _raycastInfo.ContactPointWS - chassis.CenterOfMassPosition; 402 Vector3 relpos = _raycastInfo.ContactPointWS - chassis.CenterOfMassPosition;
403 chassisVelocityAtContactPoint = chassis.GetVelocityInLocalPoint(relpos); 403 chassisVelocityAtContactPoint = chassis.GetVelocityInLocalPoint(relpos);
404 float projVel = Vector3.Dot(_raycastInfo.ContactNormalWS, chassisVelocityAtContactPoint); 404 float projVel = Vector3.Dot(_raycastInfo.ContactNormalWS, chassisVelocityAtContactPoint);
405 405
406 if (project >= -0.1f) 406 if (project >= -0.1f)
407 { 407 {
408 _suspensionRelativeVelocity = 0; 408 _suspensionRelativeVelocity = 0;
409 _clippedInvContactDotSuspension = 1.0f / 0.1f; 409 _clippedInvContactDotSuspension = 1.0f / 0.1f;
410 } 410 }
411 else 411 else
412 { 412 {
413 float inv = -1 / project; 413 float inv = -1 / project;
414 _suspensionRelativeVelocity = projVel * inv; 414 _suspensionRelativeVelocity = projVel * inv;
415 _clippedInvContactDotSuspension = inv; 415 _clippedInvContactDotSuspension = inv;
416 } 416 }
417 } 417 }
418 else 418 else
419 { 419 {
420 _raycastInfo.SuspensionLength = _suspensionRestLength; 420 _raycastInfo.SuspensionLength = _suspensionRestLength;
421 _suspensionRelativeVelocity = 0.0f; 421 _suspensionRelativeVelocity = 0.0f;
422 _raycastInfo.ContactNormalWS = -_raycastInfo.WheelDirectionWS; 422 _raycastInfo.ContactNormalWS = -_raycastInfo.WheelDirectionWS;
423 _clippedInvContactDotSuspension = 1.0f; 423 _clippedInvContactDotSuspension = 1.0f;
424 } 424 }
425 } 425 }
426 426
427 // if (m_raycastInfo.m_isInContact) 427 // if (m_raycastInfo.m_isInContact)
428 428
429 //{ 429 //{
430 // btScalar project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS ); 430 // btScalar project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS );
431 // btVector3 chassis_velocity_at_contactPoint; 431 // btVector3 chassis_velocity_at_contactPoint;
432 // btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition(); 432 // btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition();
433 // chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos ); 433 // chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos );
434 // btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); 434 // btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );
435 // if ( project >= -0.1f) 435 // if ( project >= -0.1f)
436 // { 436 // {
437 // m_suspensionRelativeVelocity = 0.0f; 437 // m_suspensionRelativeVelocity = 0.0f;
438 // m_clippedInvContactDotSuspension = 1.0f / 0.1f; 438 // m_clippedInvContactDotSuspension = 1.0f / 0.1f;
439 // } 439 // }
440 // else 440 // else
441 // { 441 // {
442 // btScalar inv = -1.f / project; 442 // btScalar inv = -1.f / project;
443 // m_suspensionRelativeVelocity = projVel * inv; 443 // m_suspensionRelativeVelocity = projVel * inv;
444 // m_clippedInvContactDotSuspension = inv; 444 // m_clippedInvContactDotSuspension = inv;
445 // } 445 // }
446 446
447 //} 447 //}
448 448
449 //else // Not in contact : position wheel in a nice (rest length) position 449 //else // Not in contact : position wheel in a nice (rest length) position
450 //{ 450 //{
451 // m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength(); 451 // m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength();
452 // m_suspensionRelativeVelocity = 0.0f; 452 // m_suspensionRelativeVelocity = 0.0f;
453 // m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS; 453 // m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS;
454 // m_clippedInvContactDotSuspension = 1.0f; 454 // m_clippedInvContactDotSuspension = 1.0f;
455 //} 455 //}
456 }; 456 };
457 457
458 //btScalar m_clippedInvContactDotSuspension; 458 //btScalar m_clippedInvContactDotSuspension;
459 //btScalar m_suspensionRelativeVelocity; 459 //btScalar m_suspensionRelativeVelocity;
460 //btScalar m_wheelsSuspensionForce; 460 //btScalar m_wheelsSuspensionForce;
461 //btScalar m_skidInfo; 461 //btScalar m_skidInfo;
462 462
463 //void* m_clientInfo;//can be used to store pointer to sync transforms... 463 //void* m_clientInfo;//can be used to store pointer to sync transforms...
464 464
465 //btWheelInfo(btWheelInfoConstructionInfo& ci) 465 //btWheelInfo(btWheelInfoConstructionInfo& ci)
466 466
467 //{ 467 //{
468 468
469 // m_suspensionRestLength1 = ci.m_suspensionRestLength; 469 // m_suspensionRestLength1 = ci.m_suspensionRestLength;
470 // m_maxSuspensionTravelCm = ci.m_maxSuspensionTravelCm; 470 // m_maxSuspensionTravelCm = ci.m_maxSuspensionTravelCm;
471 471
472 // m_wheelsRadius = ci.m_wheelRadius; 472 // m_wheelsRadius = ci.m_wheelRadius;
473 // m_suspensionStiffness = ci.m_suspensionStiffness; 473 // m_suspensionStiffness = ci.m_suspensionStiffness;
474 // m_wheelsDampingCompression = ci.m_wheelsDampingCompression; 474 // m_wheelsDampingCompression = ci.m_wheelsDampingCompression;
475 // m_wheelsDampingRelaxation = ci.m_wheelsDampingRelaxation; 475 // m_wheelsDampingRelaxation = ci.m_wheelsDampingRelaxation;
476 // m_chassisConnectionPointCS = ci.m_chassisConnectionCS; 476 // m_chassisConnectionPointCS = ci.m_chassisConnectionCS;
477 // m_wheelDirectionCS = ci.m_wheelDirectionCS; 477 // m_wheelDirectionCS = ci.m_wheelDirectionCS;
478 // m_wheelAxleCS = ci.m_wheelAxleCS; 478 // m_wheelAxleCS = ci.m_wheelAxleCS;
479 // m_frictionSlip = ci.m_frictionSlip; 479 // m_frictionSlip = ci.m_frictionSlip;
480 // m_steering = 0.f; 480 // m_steering = 0.f;
481 // m_engineForce = 0.f; 481 // m_engineForce = 0.f;
482 // m_rotation = 0.f; 482 // m_rotation = 0.f;
483 // m_deltaRotation = 0.f; 483 // m_deltaRotation = 0.f;
484 // m_brake = 0.f; 484 // m_brake = 0.f;
485 // m_rollInfluence = 0.1f; 485 // m_rollInfluence = 0.1f;
486 // m_bIsFrontWheel = ci.m_bIsFrontWheel; 486 // m_bIsFrontWheel = ci.m_bIsFrontWheel;
487 487
488 //} 488 //}
489 489
490 //void updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo); 490 //void updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo);
491 491
492 //btScalar m_clippedInvContactDotSuspension; 492 //btScalar m_clippedInvContactDotSuspension;
493 //btScalar m_suspensionRelativeVelocity; 493 //btScalar m_suspensionRelativeVelocity;
494 ////calculated by suspension 494 ////calculated by suspension
495 //btScalar m_wheelsSuspensionForce; 495 //btScalar m_wheelsSuspensionForce;
496 //btScalar m_skidInfo; 496 //btScalar m_skidInfo;
497 497
498 //}; 498 //};
499 499
500 //struct RaycastInfo 500 //struct RaycastInfo
501 //{ 501 //{
502 // //set by raycaster 502 // //set by raycaster
503 // btVector3 m_contactNormalWS;//contactnormal 503 // btVector3 m_contactNormalWS;//contactnormal
504 // btVector3 m_contactPointWS;//raycast hitpoint 504 // btVector3 m_contactPointWS;//raycast hitpoint
505 // btScalar m_suspensionLength; 505 // btScalar m_suspensionLength;
506 // btVector3 m_hardPointWS;//raycast starting point 506 // btVector3 m_hardPointWS;//raycast starting point
507 // btVector3 m_wheelDirectionWS; //direction in worldspace 507 // btVector3 m_wheelDirectionWS; //direction in worldspace
508 // btVector3 m_wheelAxleWS; // axle in worldspace 508 // btVector3 m_wheelAxleWS; // axle in worldspace
509 // bool m_isInContact; 509 // bool m_isInContact;
510 // void* m_groundObject; //could be general void* ptr 510 // void* m_groundObject; //could be general void* ptr
511 //}; 511 //};
512 512
513 //struct btWheelInfoConstructionInfo 513 //struct btWheelInfoConstructionInfo
514 //{ 514 //{
515 // btVector3 m_chassisConnectionCS; 515 // btVector3 m_chassisConnectionCS;
516 // btVector3 m_wheelDirectionCS; 516 // btVector3 m_wheelDirectionCS;
517 // btVector3 m_wheelAxleCS; 517 // btVector3 m_wheelAxleCS;
518 // btScalar m_suspensionRestLength; 518 // btScalar m_suspensionRestLength;
519 // btScalar m_maxSuspensionTravelCm; 519 // btScalar m_maxSuspensionTravelCm;
520 // btScalar m_wheelRadius; 520 // btScalar m_wheelRadius;
521 521
522 // float m_suspensionStiffness; 522 // float m_suspensionStiffness;
523 // float m_wheelsDampingCompression; 523 // float m_wheelsDampingCompression;
524 // float m_wheelsDampingRelaxation; 524 // float m_wheelsDampingRelaxation;
525 // float m_frictionSlip; 525 // float m_frictionSlip;
526 // bool m_bIsFrontWheel; 526 // bool m_bIsFrontWheel;
527 527
528 //}; 528 //};
529} 529}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Exceptions/BulletException.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Exceptions/BulletException.cs
index abd4885..3ae3ad6 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Exceptions/BulletException.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Exceptions/BulletException.cs
@@ -1,53 +1,53 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using System.Runtime.Serialization; 25using System.Runtime.Serialization;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public class BulletException : Exception 29 public class BulletException : Exception
30 { 30 {
31 internal BulletException() 31 internal BulletException()
32 : base("Bullet Physics Library has thrown an exception.") 32 : base("Bullet Physics Library has thrown an exception.")
33 { 33 {
34 } 34 }
35 35
36 internal BulletException(string message) 36 internal BulletException(string message)
37 : base(message) 37 : base(message)
38 { 38 {
39 } 39 }
40 40
41 internal BulletException(string message, Exception innerException) 41 internal BulletException(string message, Exception innerException)
42 : base(message, innerException) 42 : base(message, innerException)
43 { 43 {
44 } 44 }
45 45
46#if !XBOX 46#if !XBOX
47 internal BulletException(SerializationInfo info, StreamingContext context) 47 internal BulletException(SerializationInfo info, StreamingContext context)
48 : base(info, context) 48 : base(info, context)
49 { 49 {
50 } 50 }
51#endif 51#endif
52 } 52 }
53} 53}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/IDebugDraw.cs b/libraries/ModifiedBulletX/ModifiedBulletX/IDebugDraw.cs
index ef5b5dc..5af979c 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/IDebugDraw.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/IDebugDraw.cs
@@ -1,60 +1,60 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public enum DebugDrawModes 29 public enum DebugDrawModes
30 { 30 {
31 NoDebug = 0, 31 NoDebug = 0,
32 DrawWireframe = 1, 32 DrawWireframe = 1,
33 DrawAabb = 2, 33 DrawAabb = 2,
34 DrawFeaturesText = 4, 34 DrawFeaturesText = 4,
35 DrawContactPoints = 8, 35 DrawContactPoints = 8,
36 NoDeactivation = 16, 36 NoDeactivation = 16,
37 NoHelpText = 32, 37 NoHelpText = 32,
38 DrawText = 64, 38 DrawText = 64,
39 ProfileTimings = 128, 39 ProfileTimings = 128,
40 EnableSatComparison = 256, 40 EnableSatComparison = 256,
41 DisableBulletLcp = 512, 41 DisableBulletLcp = 512,
42 EnableCcd = 1024, 42 EnableCcd = 1024,
43 MaxDebugDrawMode 43 MaxDebugDrawMode
44 } 44 }
45 45
46 public interface IDebugDraw 46 public interface IDebugDraw
47 { 47 {
48 void DrawLine(Vector3 from, Vector3 to, Vector3 color); 48 void DrawLine(Vector3 from, Vector3 to, Vector3 color);
49 49
50 void DrawContactPoint( 50 void DrawContactPoint(
51 Vector3 pointOnB, 51 Vector3 pointOnB,
52 Vector3 normalOnB, 52 Vector3 normalOnB,
53 float distance, 53 float distance,
54 int lifeTime, 54 int lifeTime,
55 Vector3 color 55 Vector3 color
56 ); 56 );
57 57
58 DebugDrawModes DebugMode { get; set; } 58 DebugDrawModes DebugMode { get; set; }
59 } 59 }
60} \ No newline at end of file 60} \ No newline at end of file
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/DefaultMotionState.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/DefaultMotionState.cs
index 2a45a20..73dc675 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/DefaultMotionState.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/DefaultMotionState.cs
@@ -1,66 +1,66 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 // DefaultMotionState provides a common implementation to synchronize world transforms with offsets 29 // DefaultMotionState provides a common implementation to synchronize world transforms with offsets
30 public class DefaultMotionState : MotionState 30 public class DefaultMotionState : MotionState
31 { 31 {
32 private Matrix _graphicsWorldTransform; 32 private Matrix _graphicsWorldTransform;
33 private Matrix _centerOfMassOffset; 33 private Matrix _centerOfMassOffset;
34 private Matrix _startWorldTransform; 34 private Matrix _startWorldTransform;
35 private object _userData; 35 private object _userData;
36 36
37 public DefaultMotionState() 37 public DefaultMotionState()
38 : this(Matrix.Identity, Matrix.Identity) { } 38 : this(Matrix.Identity, Matrix.Identity) { }
39 39
40 public DefaultMotionState(Matrix startTransform, Matrix centerOfMassOffset) 40 public DefaultMotionState(Matrix startTransform, Matrix centerOfMassOffset)
41 { 41 {
42 _graphicsWorldTransform = startTransform; 42 _graphicsWorldTransform = startTransform;
43 _centerOfMassOffset = centerOfMassOffset; 43 _centerOfMassOffset = centerOfMassOffset;
44 _startWorldTransform = startTransform; 44 _startWorldTransform = startTransform;
45 } 45 }
46 46
47 public Matrix GraphicsWorldTransform { get { return _graphicsWorldTransform; } set { _graphicsWorldTransform = value; } } 47 public Matrix GraphicsWorldTransform { get { return _graphicsWorldTransform; } set { _graphicsWorldTransform = value; } }
48 public Matrix CenterOfMassOffset { get { return _centerOfMassOffset; } set { _centerOfMassOffset = value; } } 48 public Matrix CenterOfMassOffset { get { return _centerOfMassOffset; } set { _centerOfMassOffset = value; } }
49 public Matrix StartWorldTransform { get { return _startWorldTransform; } set { _startWorldTransform = value; } } 49 public Matrix StartWorldTransform { get { return _startWorldTransform; } set { _startWorldTransform = value; } }
50 public object UserData { get { return _userData; } set { _userData = value; } } 50 public object UserData { get { return _userData; } set { _userData = value; } }
51 51
52 // synchronizes world transform from user to physics 52 // synchronizes world transform from user to physics
53 public override void GetWorldTransform(out Matrix centerOfMassWorldTrans) 53 public override void GetWorldTransform(out Matrix centerOfMassWorldTrans)
54 { 54 {
55 centerOfMassWorldTrans = MathHelper.InvertMatrix(_centerOfMassOffset) * _graphicsWorldTransform; 55 centerOfMassWorldTrans = MathHelper.InvertMatrix(_centerOfMassOffset) * _graphicsWorldTransform;
56 } 56 }
57 57
58 // synchronizes world transform from physics to user 58 // synchronizes world transform from physics to user
59 // Bullet only calls the update of worldtransform for active objects 59 // Bullet only calls the update of worldtransform for active objects
60 public override void SetWorldTransform(Matrix centerOfMassWorldTrans) 60 public override void SetWorldTransform(Matrix centerOfMassWorldTrans)
61 { 61 {
62 _graphicsWorldTransform = MatrixOperations.Multiply(centerOfMassWorldTrans, _centerOfMassOffset); 62 _graphicsWorldTransform = MatrixOperations.Multiply(centerOfMassWorldTrans, _centerOfMassOffset);
63 _graphicsWorldTransform.Translation = centerOfMassWorldTrans.Translation; 63 _graphicsWorldTransform.Translation = centerOfMassWorldTrans.Translation;
64 } 64 }
65 } 65 }
66} 66}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs
index 2e3f258..15c7bb8 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs
@@ -1,581 +1,581 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25 25
26using MonoXnaCompactMaths; 26using MonoXnaCompactMaths;
27 27
28namespace XnaDevRu.BulletX 28namespace XnaDevRu.BulletX
29{ 29{
30 public static class MathHelper 30 public static class MathHelper
31 { 31 {
32 internal const float Sqrt12 = 0.7071067811865475244008443621048490f; 32 internal const float Sqrt12 = 0.7071067811865475244008443621048490f;
33 internal const float Infinity = 3.402823466e+38f; 33 internal const float Infinity = 3.402823466e+38f;
34 internal const float Epsilon = 1.192092896e-07f; 34 internal const float Epsilon = 1.192092896e-07f;
35 35
36 public static Vector3 MatrixToVector(Matrix m, Vector3 v) 36 public static Vector3 MatrixToVector(Matrix m, Vector3 v)
37 { 37 {
38 return new Vector3( 38 return new Vector3(
39 Vector3.Dot(new Vector3(m.M11, m.M12, m.M13), v) + m.Translation.X, 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, 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 41 Vector3.Dot(new Vector3(m.M31, m.M32, m.M33), v) + m.Translation.Z
42 ); 42 );
43 } 43 }
44 44
45 internal static int ClosestAxis(Vector4 v) 45 internal static int ClosestAxis(Vector4 v)
46 { 46 {
47 return MaxAxis(Absolute(v)); 47 return MaxAxis(Absolute(v));
48 } 48 }
49 49
50 internal static Vector4 Absolute(Vector4 v) 50 internal static Vector4 Absolute(Vector4 v)
51 { 51 {
52 return new Vector4(Math.Abs(v.X), Math.Abs(v.Y), Math.Abs(v.Z), Math.Abs(v.W)); 52 return new Vector4(Math.Abs(v.X), Math.Abs(v.Y), Math.Abs(v.Z), Math.Abs(v.W));
53 } 53 }
54 54
55 internal static int MaxAxis(Vector4 v) 55 internal static int MaxAxis(Vector4 v)
56 { 56 {
57 int maxIndex = -1; 57 int maxIndex = -1;
58 float maxVal = float.MinValue; 58 float maxVal = float.MinValue;
59 if (v.X > maxVal) 59 if (v.X > maxVal)
60 { 60 {
61 maxIndex = 0; 61 maxIndex = 0;
62 maxVal = v.X; 62 maxVal = v.X;
63 } 63 }
64 if (v.Y > maxVal) 64 if (v.Y > maxVal)
65 { 65 {
66 maxIndex = 1; 66 maxIndex = 1;
67 maxVal = v.Y; 67 maxVal = v.Y;
68 } 68 }
69 if (v.Z > maxVal) 69 if (v.Z > maxVal)
70 { 70 {
71 maxIndex = 2; 71 maxIndex = 2;
72 maxVal = v.Z; 72 maxVal = v.Z;
73 } 73 }
74 if (v.W > maxVal) 74 if (v.W > maxVal)
75 { 75 {
76 maxIndex = 3; 76 maxIndex = 3;
77 maxVal = v.W; 77 maxVal = v.W;
78 } 78 }
79 79
80 return maxIndex; 80 return maxIndex;
81 } 81 }
82 82
83 internal static int MaxAxis(Vector3 v) 83 internal static int MaxAxis(Vector3 v)
84 { 84 {
85 return v.X < v.Y ? (v.Y < v.Z ? 2 : 1) : (v.X < v.Z ? 2 : 0); 85 return v.X < v.Y ? (v.Y < v.Z ? 2 : 1) : (v.X < v.Z ? 2 : 0);
86 } 86 }
87 87
88 // conservative test for overlap between two aabbs 88 // conservative test for overlap between two aabbs
89 internal static bool TestAabbAgainstAabb2(Vector3 aabbMinA, Vector3 aabbMaxA, Vector3 aabbMinB, Vector3 aabbMaxB) 89 internal static bool TestAabbAgainstAabb2(Vector3 aabbMinA, Vector3 aabbMaxA, Vector3 aabbMinB, Vector3 aabbMaxB)
90 { 90 {
91 bool overlap = true; 91 bool overlap = true;
92 overlap = (aabbMinA.X > aabbMaxB.X || aabbMaxA.X < aabbMinB.X) ? false : overlap; 92 overlap = (aabbMinA.X > aabbMaxB.X || aabbMaxA.X < aabbMinB.X) ? false : overlap;
93 overlap = (aabbMinA.Z > aabbMaxB.Z || aabbMaxA.Z < aabbMinB.Z) ? 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; 94 overlap = (aabbMinA.Y > aabbMaxB.Y || aabbMaxA.Y < aabbMinB.Y) ? false : overlap;
95 return overlap; 95 return overlap;
96 } 96 }
97 97
98 internal static bool TestTriangleAgainstAabb2(Vector3[] vertices, Vector3 aabbMin, Vector3 aabbMax) 98 internal static bool TestTriangleAgainstAabb2(Vector3[] vertices, Vector3 aabbMin, Vector3 aabbMax)
99 { 99 {
100 Vector3 p1 = vertices[0]; 100 Vector3 p1 = vertices[0];
101 Vector3 p2 = vertices[1]; 101 Vector3 p2 = vertices[1];
102 Vector3 p3 = vertices[2]; 102 Vector3 p3 = vertices[2];
103 103
104 if (Math.Min(Math.Min(p1.X, p2.X), p3.X) > aabbMax.X) return false; 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; 105 if (Math.Max(Math.Max(p1.X, p2.X), p3.X) < aabbMin.X) return false;
106 106
107 if (Math.Min(Math.Min(p1.Z, p2.Z), p3.Z) > aabbMax.Z) return false; 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; 108 if (Math.Max(Math.Max(p1.Z, p2.Z), p3.Z) < aabbMin.Z) return false;
109 109
110 if (Math.Min(Math.Min(p1.Y, p2.Y), p3.Y) > aabbMax.Y) return false; 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; 111 if (Math.Max(Math.Max(p1.Y, p2.Y), p3.Y) < aabbMin.Y) return false;
112 return true; 112 return true;
113 } 113 }
114 114
115 internal static void SetInterpolate3(Vector3 vA, Vector3 vB, float rt, ref Vector3 interpolated) 115 internal static void SetInterpolate3(Vector3 vA, Vector3 vB, float rt, ref Vector3 interpolated)
116 { 116 {
117 float s = 1.0f - rt; 117 float s = 1.0f - rt;
118 interpolated.X = s * vA.X + rt * vB.X; 118 interpolated.X = s * vA.X + rt * vB.X;
119 interpolated.Y = s * vA.Y + rt * vB.Y; 119 interpolated.Y = s * vA.Y + rt * vB.Y;
120 interpolated.Z = s * vA.Z + rt * vB.Z; 120 interpolated.Z = s * vA.Z + rt * vB.Z;
121 } 121 }
122 122
123 internal static void PlaneSpace1(Vector3 n, ref Vector3 p, ref Vector3 q) 123 internal static void PlaneSpace1(Vector3 n, ref Vector3 p, ref Vector3 q)
124 { 124 {
125 if (Math.Abs(n.Z) > Sqrt12) 125 if (Math.Abs(n.Z) > Sqrt12)
126 { 126 {
127 // choose p in y-z plane 127 // choose p in y-z plane
128 float a = n.Y * n.Y + n.Z * n.Z; 128 float a = n.Y * n.Y + n.Z * n.Z;
129 float k = 1f / (float)Math.Sqrt(a); 129 float k = 1f / (float)Math.Sqrt(a);
130 p.X = 0; 130 p.X = 0;
131 p.Y = -n.Z * k; 131 p.Y = -n.Z * k;
132 p.Z = n.Y * k; 132 p.Z = n.Y * k;
133 // set q = n x p 133 // set q = n x p
134 q.X = a * k; 134 q.X = a * k;
135 q.Y = -n.X * p.Z; 135 q.Y = -n.X * p.Z;
136 q.Z = n.X * p.Y; 136 q.Z = n.X * p.Y;
137 } 137 }
138 else 138 else
139 { 139 {
140 // choose p in x-y plane 140 // choose p in x-y plane
141 float a = n.X * n.X + n.Y * n.Y; 141 float a = n.X * n.X + n.Y * n.Y;
142 float k = 1f / (float)Math.Sqrt(a); 142 float k = 1f / (float)Math.Sqrt(a);
143 p.X = -n.Y * k; 143 p.X = -n.Y * k;
144 p.Y = n.X * k; 144 p.Y = n.X * k;
145 p.Z = 0; 145 p.Z = 0;
146 // set q = n x p 146 // set q = n x p
147 q.X = -n.Z * p.Y; 147 q.X = -n.Z * p.Y;
148 q.Y = n.Z * p.X; 148 q.Y = n.Z * p.X;
149 q.Z = a * k; 149 q.Z = a * k;
150 } 150 }
151 } 151 }
152 152
153 internal static bool RayAabb(Vector3 rayFrom, 153 internal static bool RayAabb(Vector3 rayFrom,
154 Vector3 rayTo, 154 Vector3 rayTo,
155 Vector3 aabbMin, 155 Vector3 aabbMin,
156 Vector3 aabbMax, 156 Vector3 aabbMax,
157 float param, Vector3 normal) 157 float param, Vector3 normal)
158 { 158 {
159 Vector3 aabbHalfExtent = (aabbMax - aabbMin) * 0.5f; 159 Vector3 aabbHalfExtent = (aabbMax - aabbMin) * 0.5f;
160 Vector3 aabbCenter = (aabbMax + aabbMin) * 0.5f; 160 Vector3 aabbCenter = (aabbMax + aabbMin) * 0.5f;
161 Vector3 source = rayFrom - aabbCenter; 161 Vector3 source = rayFrom - aabbCenter;
162 Vector3 target = rayTo - aabbCenter; 162 Vector3 target = rayTo - aabbCenter;
163 int sourceOutcode = Outcode(source, aabbHalfExtent); 163 int sourceOutcode = Outcode(source, aabbHalfExtent);
164 int targetOutcode = Outcode(target, aabbHalfExtent); 164 int targetOutcode = Outcode(target, aabbHalfExtent);
165 if ((sourceOutcode & targetOutcode) == 0x0) 165 if ((sourceOutcode & targetOutcode) == 0x0)
166 { 166 {
167 float lambda_enter = 0; 167 float lambda_enter = 0;
168 float lambda_exit = param; 168 float lambda_exit = param;
169 Vector3 r = target - source; 169 Vector3 r = target - source;
170 float normSign = 1; 170 float normSign = 1;
171 Vector3 hitNormal = new Vector3(); 171 Vector3 hitNormal = new Vector3();
172 int bit = 1; 172 int bit = 1;
173 173
174 for (int j = 0; j < 2; j++) 174 for (int j = 0; j < 2; j++)
175 { 175 {
176 { 176 {
177 if ((sourceOutcode & bit) != 0) 177 if ((sourceOutcode & bit) != 0)
178 { 178 {
179 float lambda = (-source.X - aabbHalfExtent.X * normSign) / r.X; 179 float lambda = (-source.X - aabbHalfExtent.X * normSign) / r.X;
180 if (lambda_enter <= lambda) 180 if (lambda_enter <= lambda)
181 { 181 {
182 lambda_enter = lambda; 182 lambda_enter = lambda;
183 hitNormal = new Vector3(); 183 hitNormal = new Vector3();
184 hitNormal.X = normSign; 184 hitNormal.X = normSign;
185 } 185 }
186 } 186 }
187 else if ((targetOutcode & bit) != 0) 187 else if ((targetOutcode & bit) != 0)
188 { 188 {
189 float lambda = (-source.X - aabbHalfExtent.X * normSign) / r.X; 189 float lambda = (-source.X - aabbHalfExtent.X * normSign) / r.X;
190 SetMin(ref lambda_exit, lambda); 190 SetMin(ref lambda_exit, lambda);
191 } 191 }
192 bit <<= 1; 192 bit <<= 1;
193 } 193 }
194 { 194 {
195 if ((sourceOutcode & bit) != 0) 195 if ((sourceOutcode & bit) != 0)
196 { 196 {
197 float lambda = (-source.Y - aabbHalfExtent.Y * normSign) / r.Y; 197 float lambda = (-source.Y - aabbHalfExtent.Y * normSign) / r.Y;
198 if (lambda_enter <= lambda) 198 if (lambda_enter <= lambda)
199 { 199 {
200 lambda_enter = lambda; 200 lambda_enter = lambda;
201 hitNormal = new Vector3(); 201 hitNormal = new Vector3();
202 hitNormal.Y = normSign; 202 hitNormal.Y = normSign;
203 } 203 }
204 } 204 }
205 else if ((targetOutcode & bit) != 0) 205 else if ((targetOutcode & bit) != 0)
206 { 206 {
207 float lambda = (-source.Y - aabbHalfExtent.Y * normSign) / r.Y; 207 float lambda = (-source.Y - aabbHalfExtent.Y * normSign) / r.Y;
208 SetMin(ref lambda_exit, lambda); 208 SetMin(ref lambda_exit, lambda);
209 } 209 }
210 bit <<= 1; 210 bit <<= 1;
211 } 211 }
212 { 212 {
213 if ((sourceOutcode & bit) != 0) 213 if ((sourceOutcode & bit) != 0)
214 { 214 {
215 float lambda = (-source.Z - aabbHalfExtent.Z * normSign) / r.Z; 215 float lambda = (-source.Z - aabbHalfExtent.Z * normSign) / r.Z;
216 if (lambda_enter <= lambda) 216 if (lambda_enter <= lambda)
217 { 217 {
218 lambda_enter = lambda; 218 lambda_enter = lambda;
219 hitNormal = new Vector3(); 219 hitNormal = new Vector3();
220 hitNormal.Z = normSign; 220 hitNormal.Z = normSign;
221 } 221 }
222 } 222 }
223 else if ((targetOutcode & bit) != 0) 223 else if ((targetOutcode & bit) != 0)
224 { 224 {
225 float lambda = (-source.Z - aabbHalfExtent.Z * normSign) / r.Z; 225 float lambda = (-source.Z - aabbHalfExtent.Z * normSign) / r.Z;
226 SetMin(ref lambda_exit, lambda); 226 SetMin(ref lambda_exit, lambda);
227 } 227 }
228 bit <<= 1; 228 bit <<= 1;
229 } 229 }
230 normSign = -1; 230 normSign = -1;
231 } 231 }
232 if (lambda_enter <= lambda_exit) 232 if (lambda_enter <= lambda_exit)
233 { 233 {
234 param = lambda_enter; 234 param = lambda_enter;
235 normal = hitNormal; 235 normal = hitNormal;
236 return true; 236 return true;
237 } 237 }
238 } 238 }
239 return false; 239 return false;
240 } 240 }
241 241
242 internal static void SetMin(ref float a, float b) 242 internal static void SetMin(ref float a, float b)
243 { 243 {
244 if (a > b) 244 if (a > b)
245 a = b; 245 a = b;
246 } 246 }
247 247
248 internal static void SetMax(ref float a, float b) 248 internal static void SetMax(ref float a, float b)
249 { 249 {
250 if (a < b) 250 if (a < b)
251 a = b; 251 a = b;
252 } 252 }
253 253
254 internal static void SetMax(ref Vector3 self, Vector3 other) 254 internal static void SetMax(ref Vector3 self, Vector3 other)
255 { 255 {
256 if (other.X > self.X) 256 if (other.X > self.X)
257 self.X = other.X; 257 self.X = other.X;
258 258
259 if (other.Y > self.Y) 259 if (other.Y > self.Y)
260 self.Y = other.Y; 260 self.Y = other.Y;
261 261
262 if (other.Z > self.Z) 262 if (other.Z > self.Z)
263 self.Z = other.Z; 263 self.Z = other.Z;
264 } 264 }
265 265
266 internal static Vector3 SetMax(Vector3 self, Vector3 other) 266 internal static Vector3 SetMax(Vector3 self, Vector3 other)
267 { 267 {
268 if (other.X > self.X) 268 if (other.X > self.X)
269 self.X = other.X; 269 self.X = other.X;
270 270
271 if (other.Y > self.Y) 271 if (other.Y > self.Y)
272 self.Y = other.Y; 272 self.Y = other.Y;
273 273
274 if (other.Z > self.Z) 274 if (other.Z > self.Z)
275 self.Z = other.Z; 275 self.Z = other.Z;
276 276
277 return self; 277 return self;
278 } 278 }
279 279
280 internal static void SetMin(ref Vector3 self, Vector3 other) 280 internal static void SetMin(ref Vector3 self, Vector3 other)
281 { 281 {
282 if (other.X < self.X) 282 if (other.X < self.X)
283 self.X = other.X; 283 self.X = other.X;
284 284
285 if (other.Y < self.Y) 285 if (other.Y < self.Y)
286 self.Y = other.Y; 286 self.Y = other.Y;
287 287
288 if (other.Z < self.Z) 288 if (other.Z < self.Z)
289 self.Z = other.Z; 289 self.Z = other.Z;
290 } 290 }
291 291
292 internal static Vector3 SetMin(Vector3 self, Vector3 other) 292 internal static Vector3 SetMin(Vector3 self, Vector3 other)
293 { 293 {
294 if (other.X < self.X) 294 if (other.X < self.X)
295 self.X = other.X; 295 self.X = other.X;
296 296
297 if (other.Y < self.Y) 297 if (other.Y < self.Y)
298 self.Y = other.Y; 298 self.Y = other.Y;
299 299
300 if (other.Z < self.Z) 300 if (other.Z < self.Z)
301 self.Z = other.Z; 301 self.Z = other.Z;
302 302
303 return self; 303 return self;
304 } 304 }
305 305
306 internal static int Outcode(Vector3 p, Vector3 halfExtent) 306 internal static int Outcode(Vector3 p, Vector3 halfExtent)
307 { 307 {
308 return (p.X < -halfExtent.X ? 0x01 : 0x0) | 308 return (p.X < -halfExtent.X ? 0x01 : 0x0) |
309 (p.X > halfExtent.X ? 0x08 : 0x0) | 309 (p.X > halfExtent.X ? 0x08 : 0x0) |
310 (p.Y < -halfExtent.Y ? 0x02 : 0x0) | 310 (p.Y < -halfExtent.Y ? 0x02 : 0x0) |
311 (p.Y > halfExtent.Y ? 0x10 : 0x0) | 311 (p.Y > halfExtent.Y ? 0x10 : 0x0) |
312 (p.Z < -halfExtent.Z ? 0x4 : 0x0) | 312 (p.Z < -halfExtent.Z ? 0x4 : 0x0) |
313 (p.Z > halfExtent.Z ? 0x20 : 0x0); 313 (p.Z > halfExtent.Z ? 0x20 : 0x0);
314 } 314 }
315 315
316 internal static Matrix Absolute(Matrix m) 316 internal static Matrix Absolute(Matrix m)
317 { 317 {
318 return new Matrix(Math.Abs(m.M11), Math.Abs(m.M12), Math.Abs(m.M13), Math.Abs(m.M14), 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), 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), 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)); 321 Math.Abs(m.M41), Math.Abs(m.M42), Math.Abs(m.M43), Math.Abs(m.M44));
322 } 322 }
323 323
324 internal static void SetValueByIndex(ref Vector3 v, int i, float value) 324 internal static void SetValueByIndex(ref Vector3 v, int i, float value)
325 { 325 {
326 if (i == 0) 326 if (i == 0)
327 v.X = value; 327 v.X = value;
328 else if (i == 1) 328 else if (i == 1)
329 v.Y = value; 329 v.Y = value;
330 else 330 else
331 v.Z = value; 331 v.Z = value;
332 } 332 }
333 333
334 internal static float GetValueByIndex(Vector3 v, int i) 334 internal static float GetValueByIndex(Vector3 v, int i)
335 { 335 {
336 if (i == 0) 336 if (i == 0)
337 return v.X; 337 return v.X;
338 else if (i == 1) 338 else if (i == 1)
339 return v.Y; 339 return v.Y;
340 else 340 else
341 return v.Z; 341 return v.Z;
342 } 342 }
343 343
344 internal static Vector3 InvXForm(Matrix m, Vector3 v) 344 internal static Vector3 InvXForm(Matrix m, Vector3 v)
345 { 345 {
346 v -= m.Translation; 346 v -= m.Translation;
347 m.Translation = new Vector3(); 347 m.Translation = new Vector3();
348 return MathHelper.Transform(v, Matrix.Transpose(m)); 348 return MathHelper.Transform(v, Matrix.Transpose(m));
349 } 349 }
350 350
351 internal static Matrix InverseTimes(Matrix m, Matrix t) 351 internal static Matrix InverseTimes(Matrix m, Matrix t)
352 { 352 {
353 Vector3 v = t.Translation - m.Translation; 353 Vector3 v = t.Translation - m.Translation;
354 354
355 Matrix mat = TransposeTimes(m, t); 355 Matrix mat = TransposeTimes(m, t);
356 mat.Translation = Vector3.Transform(v, m); 356 mat.Translation = Vector3.Transform(v, m);
357 return mat; 357 return mat;
358 } 358 }
359 359
360 internal static Matrix TransposeTimes(Matrix mA, Matrix mB) 360 internal static Matrix TransposeTimes(Matrix mA, Matrix mB)
361 { 361 {
362 return new Matrix( 362 return new Matrix(
363 mA.M11 * mB.M11 + mA.M21 * mB.M21 + mA.M31 * mB.M31, 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, 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, 365 mA.M11 * mB.M13 + mA.M21 * mB.M23 + mA.M31 * mB.M33,
366 0, 366 0,
367 mA.M12 * mB.M11 + mA.M22 * mB.M21 + mA.M32 * mB.M31, 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, 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, 369 mA.M12 * mB.M13 + mA.M22 * mB.M23 + mA.M32 * mB.M33,
370 0, 370 0,
371 mA.M13 * mB.M11 + mA.M23 * mB.M21 + mA.M33 * mB.M31, 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, 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, 373 mA.M13 * mB.M13 + mA.M23 * mB.M23 + mA.M33 * mB.M33,
374 0, 0, 0, 0, 1); 374 0, 0, 0, 0, 1);
375 } 375 }
376 376
377 internal static Vector3 GetColumn(Matrix m, int column) 377 internal static Vector3 GetColumn(Matrix m, int column)
378 { 378 {
379 switch (column) 379 switch (column)
380 { 380 {
381 case 1: 381 case 1:
382 return new Vector3(m.M11, m.M21, m.M31); 382 return new Vector3(m.M11, m.M21, m.M31);
383 case 2: 383 case 2:
384 return new Vector3(m.M12, m.M22, m.M32); 384 return new Vector3(m.M12, m.M22, m.M32);
385 case 3: 385 case 3:
386 return new Vector3(m.M13, m.M23, m.M33); 386 return new Vector3(m.M13, m.M23, m.M33);
387 default: 387 default:
388 throw new ArgumentOutOfRangeException("column"); 388 throw new ArgumentOutOfRangeException("column");
389 } 389 }
390 } 390 }
391 391
392 internal static Vector3 GetRow(Matrix m, int row) 392 internal static Vector3 GetRow(Matrix m, int row)
393 { 393 {
394 switch (row) 394 switch (row)
395 { 395 {
396 case 1: 396 case 1:
397 return new Vector3(m.M11, m.M12, m.M13); 397 return new Vector3(m.M11, m.M12, m.M13);
398 case 2: 398 case 2:
399 return new Vector3(m.M21, m.M22, m.M23); 399 return new Vector3(m.M21, m.M22, m.M23);
400 case 3: 400 case 3:
401 return new Vector3(m.M31, m.M32, m.M33); 401 return new Vector3(m.M31, m.M32, m.M33);
402 default: 402 default:
403 throw new ArgumentOutOfRangeException("row"); 403 throw new ArgumentOutOfRangeException("row");
404 } 404 }
405 } 405 }
406 406
407 internal static Quaternion GetRotation(Matrix m) 407 internal static Quaternion GetRotation(Matrix m)
408 { 408 {
409 float trace = m.M11 + m.M22 + m.M33; 409 float trace = m.M11 + m.M22 + m.M33;
410 Quaternion q = new Quaternion(); 410 Quaternion q = new Quaternion();
411 411
412 if (trace > 0) 412 if (trace > 0)
413 { 413 {
414 float s = (float)Math.Sqrt(trace + 1.0f); 414 float s = (float)Math.Sqrt(trace + 1.0f);
415 q.W = s * 0.5f; 415 q.W = s * 0.5f;
416 s = 0.5f / s; 416 s = 0.5f / s;
417 417
418 q.X = (m.M32 - m.M23) * s; 418 q.X = (m.M32 - m.M23) * s;
419 q.Y = (m.M13 - m.M31) * s; 419 q.Y = (m.M13 - m.M31) * s;
420 q.Z = (m.M21 - m.M12) * s; 420 q.Z = (m.M21 - m.M12) * s;
421 } 421 }
422 else 422 else
423 { 423 {
424 int i = m.M11 < m.M22 ? 424 int i = m.M11 < m.M22 ?
425 (m.M22 < m.M33 ? 2 : 1) : 425 (m.M22 < m.M33 ? 2 : 1) :
426 (m.M11 < m.M33 ? 2 : 0); 426 (m.M11 < m.M33 ? 2 : 0);
427 int j = (i + 1) % 3; 427 int j = (i + 1) % 3;
428 int k = (i + 2) % 3; 428 int k = (i + 2) % 3;
429 429
430 float s = (float)Math.Sqrt(GetElement(m, i, i) - GetElement(m, j, j) - GetElement(m, k, k) + 1.0f); 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); 431 SetElement(ref q, i, s * 0.5f);
432 s = 0.5f / s; 432 s = 0.5f / s;
433 433
434 q.W = (GetElement(m, k, j) - GetElement(m, j, k)) * s; 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); 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); 436 SetElement(ref q, k, (GetElement(m, k, i) + GetElement(m, i, k)) * s);
437 } 437 }
438 return q; 438 return q;
439 } 439 }
440 440
441 internal static float SetElement(ref Quaternion q, int index, float value) 441 internal static float SetElement(ref Quaternion q, int index, float value)
442 { 442 {
443 switch (index) 443 switch (index)
444 { 444 {
445 case 0: 445 case 0:
446 q.X = value; break; 446 q.X = value; break;
447 case 1: 447 case 1:
448 q.Y = value; break; 448 q.Y = value; break;
449 case 2: 449 case 2:
450 q.Z = value; break; 450 q.Z = value; break;
451 case 3: 451 case 3:
452 q.W = value; break; 452 q.W = value; break;
453 } 453 }
454 454
455 return 0; 455 return 0;
456 } 456 }
457 457
458 internal static float GetElement(Quaternion q, int index) 458 internal static float GetElement(Quaternion q, int index)
459 { 459 {
460 switch (index) 460 switch (index)
461 { 461 {
462 case 0: 462 case 0:
463 return q.X; 463 return q.X;
464 case 1: 464 case 1:
465 return q.Y; 465 return q.Y;
466 case 2: 466 case 2:
467 return q.Z; 467 return q.Z;
468 default: 468 default:
469 return q.W; 469 return q.W;
470 } 470 }
471 } 471 }
472 472
473 internal static float GetElement(Matrix mat, int index) 473 internal static float GetElement(Matrix mat, int index)
474 { 474 {
475 int row = index % 3; 475 int row = index % 3;
476 int col = index / 3; 476 int col = index / 3;
477 477
478 return GetElement(mat, row, col); 478 return GetElement(mat, row, col);
479 } 479 }
480 480
481 internal static float GetElement(Matrix mat, int row, int col) 481 internal static float GetElement(Matrix mat, int row, int col)
482 { 482 {
483 switch (row) 483 switch (row)
484 { 484 {
485 case 0: 485 case 0:
486 switch (col) 486 switch (col)
487 { 487 {
488 case 0: 488 case 0:
489 return mat.M11; 489 return mat.M11;
490 case 1: 490 case 1:
491 return mat.M12; 491 return mat.M12;
492 case 2: 492 case 2:
493 return mat.M13; 493 return mat.M13;
494 } break; 494 } break;
495 case 1: 495 case 1:
496 switch (col) 496 switch (col)
497 { 497 {
498 case 0: 498 case 0:
499 return mat.M21; 499 return mat.M21;
500 case 1: 500 case 1:
501 return mat.M22; 501 return mat.M22;
502 case 2: 502 case 2:
503 return mat.M23; 503 return mat.M23;
504 } break; 504 } break;
505 case 2: 505 case 2:
506 switch (col) 506 switch (col)
507 { 507 {
508 case 0: 508 case 0:
509 return mat.M31; 509 return mat.M31;
510 case 1: 510 case 1:
511 return mat.M32; 511 return mat.M32;
512 case 2: 512 case 2:
513 return mat.M33; 513 return mat.M33;
514 } break; 514 } break;
515 } 515 }
516 516
517 return 0; 517 return 0;
518 } 518 }
519 519
520 internal static float GetElement(Vector3 v, int index) 520 internal static float GetElement(Vector3 v, int index)
521 { 521 {
522 if (index == 0) 522 if (index == 0)
523 return v.X; 523 return v.X;
524 if (index == 1) 524 if (index == 1)
525 return v.Y; 525 return v.Y;
526 if (index == 2) 526 if (index == 2)
527 return v.Z; 527 return v.Z;
528 528
529 throw new ArgumentOutOfRangeException("index"); 529 throw new ArgumentOutOfRangeException("index");
530 } 530 }
531 531
532 internal static void SetElement(ref Vector3 v, int index, float value) 532 internal static void SetElement(ref Vector3 v, int index, float value)
533 { 533 {
534 if (index == 0) 534 if (index == 0)
535 v.X = value; 535 v.X = value;
536 else if (index == 1) 536 else if (index == 1)
537 v.Y = value; 537 v.Y = value;
538 else if (index == 2) 538 else if (index == 2)
539 v.Z = value; 539 v.Z = value;
540 else 540 else
541 throw new ArgumentOutOfRangeException("index"); 541 throw new ArgumentOutOfRangeException("index");
542 } 542 }
543 543
544 public static Matrix InvertMatrix(Matrix m) 544 public static Matrix InvertMatrix(Matrix m)
545 { 545 {
546 Vector3 pos = m.Translation; 546 Vector3 pos = m.Translation;
547 m.Translation = Vector3.Zero; 547 m.Translation = Vector3.Zero;
548 Matrix inv = Matrix.Transpose(m); 548 Matrix inv = Matrix.Transpose(m);
549 pos = Vector3.Transform(-pos, m); 549 pos = Vector3.Transform(-pos, m);
550 inv.Translation = pos; 550 inv.Translation = pos;
551 return inv; 551 return inv;
552 } 552 }
553 553
554 public static Matrix GetDisplayMatrix(Matrix m) 554 public static Matrix GetDisplayMatrix(Matrix m)
555 { 555 {
556 Matrix displayMatrix = m; 556 Matrix displayMatrix = m;
557 displayMatrix.Translation = Vector3.Zero; 557 displayMatrix.Translation = Vector3.Zero;
558 displayMatrix = Matrix.Transpose(displayMatrix); 558 displayMatrix = Matrix.Transpose(displayMatrix);
559 displayMatrix.Translation = m.Translation; 559 displayMatrix.Translation = m.Translation;
560 return displayMatrix; 560 return displayMatrix;
561 } 561 }
562 562
563 internal static Vector3 Transform(Vector3 position, Matrix matrix) 563 internal static Vector3 Transform(Vector3 position, Matrix matrix)
564 { 564 {
565 Vector3 vector = new Vector3(); 565 Vector3 vector = new Vector3();
566 vector.X = (((position.X * matrix.M11) + (position.Y * matrix.M12)) + (position.Z * matrix.M13)) + matrix.M41; 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; 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; 568 vector.Z = (((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33)) + matrix.M43;
569 return vector; 569 return vector;
570 } 570 }
571 571
572 internal static Vector3 TransformNormal(Vector3 position, Matrix matrix) 572 internal static Vector3 TransformNormal(Vector3 position, Matrix matrix)
573 { 573 {
574 Vector3 vector = new Vector3(); 574 Vector3 vector = new Vector3();
575 vector.X = (((position.X * matrix.M11) + (position.Y * matrix.M12)) + (position.Z * matrix.M13)); 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)); 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)); 577 vector.Z = (((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33));
578 return vector; 578 return vector;
579 } 579 }
580 } 580 }
581} 581}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs
index 04e42d9..93a99f2 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs
@@ -1,116 +1,116 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 internal static class MatrixOperations 29 internal static class MatrixOperations
30 { 30 {
31 public static void SetRotation(ref Matrix m, Quaternion q) 31 public static void SetRotation(ref Matrix m, Quaternion q)
32 { 32 {
33 float d = q.LengthSquared(); 33 float d = q.LengthSquared();
34 BulletDebug.Assert(d != 0); 34 BulletDebug.Assert(d != 0);
35 float s = 2f / d; 35 float s = 2f / d;
36 float xs = q.X * s, ys = q.Y * s, zs = q.Z * s; 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; 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; 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; 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, 40 m = new Matrix(1 - (yy + zz), xy - wz, xz + wy, 0,
41 xy + wz, 1 - (xx + zz), yz - wx, 0, 41 xy + wz, 1 - (xx + zz), yz - wx, 0,
42 xz - wy, yz + wx, 1 - (xx + yy), 0, 42 xz - wy, yz + wx, 1 - (xx + yy), 0,
43 m.M41, m.M42, m.M43, 1); 43 m.M41, m.M42, m.M43, 1);
44 } 44 }
45 45
46 public static Quaternion GetRotation(Matrix m) 46 public static Quaternion GetRotation(Matrix m)
47 { 47 {
48 Quaternion q = new Quaternion(); 48 Quaternion q = new Quaternion();
49 49
50 float trace = m.M11 + m.M22 + m.M33; 50 float trace = m.M11 + m.M22 + m.M33;
51 51
52 if (trace > 0) 52 if (trace > 0)
53 { 53 {
54 float s = (float)Math.Sqrt(trace + 1); 54 float s = (float)Math.Sqrt(trace + 1);
55 q.W = s * 0.5f; 55 q.W = s * 0.5f;
56 s = 0.5f / s; 56 s = 0.5f / s;
57 57
58 q.X = (m.M32 - m.M23) * s; 58 q.X = (m.M32 - m.M23) * s;
59 q.Y = (m.M13 - m.M31) * s; 59 q.Y = (m.M13 - m.M31) * s;
60 q.Z = (m.M21 - m.M12) * s; 60 q.Z = (m.M21 - m.M12) * s;
61 } 61 }
62 else 62 else
63 { 63 {
64 int i = m.M11 < m.M22 ? 64 int i = m.M11 < m.M22 ?
65 (m.M22 < m.M33 ? 2 : 1) : 65 (m.M22 < m.M33 ? 2 : 1) :
66 (m.M11 < m.M33 ? 2 : 0); 66 (m.M11 < m.M33 ? 2 : 0);
67 int j = (i + 1) % 3; 67 int j = (i + 1) % 3;
68 int k = (i + 2) % 3; 68 int k = (i + 2) % 3;
69 69
70 float s = (float)Math.Sqrt(MathHelper.GetElement(m, i, i) - MathHelper.GetElement(m, j, j) - MathHelper.GetElement(m, k, k) + 1); 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); 71 MathHelper.SetElement(ref q, i, s * 0.5f);
72 s = 0.5f / s; 72 s = 0.5f / s;
73 73
74 q.W = (MathHelper.GetElement(m, k, j) - MathHelper.GetElement(m, j, k)) * s; 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); 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); 76 MathHelper.SetElement(ref q, k, (MathHelper.GetElement(m, k, i) + MathHelper.GetElement(m, i, k)) * s);
77 } 77 }
78 78
79 return q; 79 return q;
80 } 80 }
81 81
82 public static Matrix Scaled(Matrix m, Vector3 v) 82 public static Matrix Scaled(Matrix m, Vector3 v)
83 { 83 {
84 return new Matrix( m.M11 * v.X, m.M12 * v.Y, m.M13 * v.Z, 0, 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, 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, 86 m.M31 * v.X, m.M32 * v.Y, m.M33 * v.Z, 0,
87 0, 0, 0, 1); 87 0, 0, 0, 1);
88 } 88 }
89 89
90 public static Matrix Multiply(Matrix a, Matrix b) 90 public static Matrix Multiply(Matrix a, Matrix b)
91 { 91 {
92 /*return btMatrix3x3( 92 /*return btMatrix3x3(
93 m2.tdot(0, m1[0]), m2.tdot(1, m1[0]), m2.tdot(2, m1[0]), 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]), 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]));*/ 95 m2.tdot(0, m1[2]), m2.tdot(1, m1[2]), m2.tdot(2, m1[2]));*/
96 return new Matrix( 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, 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, 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, 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); 100 0, 0, 0, 1);
101 } 101 }
102 102
103 public static float Dot(Matrix m, int c, Vector3 v) 103 public static float Dot(Matrix m, int c, Vector3 v)
104 { 104 {
105 return MathHelper.GetElement(m, 0, c) * v.X + MathHelper.GetElement(m, 1, c) * v.Y + MathHelper.GetElement(m, 2, c) * v.Z; 105 return MathHelper.GetElement(m, 0, c) * v.X + MathHelper.GetElement(m, 1, c) * v.Y + MathHelper.GetElement(m, 2, c) * v.Z;
106 } 106 }
107 107
108 public static Matrix Transpose(Matrix m) 108 public static Matrix Transpose(Matrix m)
109 { 109 {
110 return new Matrix( m.M11, m.M21, m.M31, 0, 110 return new Matrix( m.M11, m.M21, m.M31, 0,
111 m.M12, m.M22, m.M32, 0, 111 m.M12, m.M22, m.M32, 0,
112 m.M13, m.M23, m.M33, 0, 112 m.M13, m.M23, m.M33, 0,
113 0, 0, 0, 1); 113 0, 0, 0, 1);
114 } 114 }
115 } 115 }
116} 116}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MotionState.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MotionState.cs
index 4039177..407a285 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MotionState.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MotionState.cs
@@ -1,44 +1,44 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public abstract class MotionState 29 public abstract class MotionState
30 { 30 {
31 /*public abstract Vector3 getWorldPosition(); 31 /*public abstract Vector3 getWorldPosition();
32 public abstract Vector3 getWorldScaling(); 32 public abstract Vector3 getWorldScaling();
33 public abstract Quaternion getWorldOrientation(); 33 public abstract Quaternion getWorldOrientation();
34 34
35 public abstract void setWorldPosition(Vector3 worldPos); 35 public abstract void setWorldPosition(Vector3 worldPos);
36 public abstract void setWorldOrientation(Quaternion orn); 36 public abstract void setWorldOrientation(Quaternion orn);
37 37
38 public abstract void calculateWorldTransformations();*/ 38 public abstract void calculateWorldTransformations();*/
39 39
40 public abstract void GetWorldTransform(out Matrix worldTransform); 40 public abstract void GetWorldTransform(out Matrix worldTransform);
41 //Bullet only calls the update of worldtransform for active objects 41 //Bullet only calls the update of worldtransform for active objects
42 public abstract void SetWorldTransform(Matrix worldTransform); 42 public abstract void SetWorldTransform(Matrix worldTransform);
43 } 43 }
44} 44}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/QuadWord.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/QuadWord.cs
index 35c9fed..0f59e87 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/QuadWord.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/QuadWord.cs
@@ -1,88 +1,88 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25 25
26namespace XnaDevRu.BulletX.LinearMath 26namespace XnaDevRu.BulletX.LinearMath
27{ 27{
28 internal abstract class QuadWord 28 internal abstract class QuadWord
29 { 29 {
30 private float x; 30 private float x;
31 private float y; 31 private float y;
32 private float z; 32 private float z;
33 private float w; 33 private float w;
34 34
35 public float X { get { return x; } set { x = value; } } 35 public float X { get { return x; } set { x = value; } }
36 public float Y { get { return y; } set { y = value; } } 36 public float Y { get { return y; } set { y = value; } }
37 public float Z { get { return z; } set { z = value; } } 37 public float Z { get { return z; } set { z = value; } }
38 public float W { get { return w; } set { w = value; } } 38 public float W { get { return w; } set { w = value; } }
39 39
40 public QuadWord() { } 40 public QuadWord() { }
41 41
42 public QuadWord(float x, float y, float z, float w) 42 public QuadWord(float x, float y, float z, float w)
43 { 43 {
44 X = x; 44 X = x;
45 Y = y; 45 Y = y;
46 Z = z; 46 Z = z;
47 W = w; 47 W = w;
48 } 48 }
49 49
50 public QuadWord(float x, float y, float z) 50 public QuadWord(float x, float y, float z)
51 { 51 {
52 X = x; 52 X = x;
53 Y = y; 53 Y = y;
54 Z = z; 54 Z = z;
55 W = 0; 55 W = 0;
56 } 56 }
57 57
58 public void SetMax(QuadWord other) 58 public void SetMax(QuadWord other)
59 { 59 {
60 if (other.X > X) 60 if (other.X > X)
61 X = other.X; 61 X = other.X;
62 62
63 if (other.Y > Y) 63 if (other.Y > Y)
64 Y = other.Y; 64 Y = other.Y;
65 65
66 if (other.Z > Z) 66 if (other.Z > Z)
67 Z = other.Z; 67 Z = other.Z;
68 68
69 if (other.W > W) 69 if (other.W > W)
70 W = other.W; 70 W = other.W;
71 } 71 }
72 72
73 public void SetMin(QuadWord other) 73 public void SetMin(QuadWord other)
74 { 74 {
75 if (other.X < X) 75 if (other.X < X)
76 X = other.X; 76 X = other.X;
77 77
78 if (other.Y < Y) 78 if (other.Y < Y)
79 Y = other.Y; 79 Y = other.Y;
80 80
81 if (other.Z < Z) 81 if (other.Z < Z)
82 Z = other.Z; 82 Z = other.Z;
83 83
84 if (other.W < W) 84 if (other.W < W)
85 W = other.W; 85 W = other.W;
86 } 86 }
87 } 87 }
88} 88}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs
index 5425a05..0685d4a 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs
@@ -1,198 +1,198 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25 25
26namespace XnaDevRu.BulletX.LinearMath 26namespace XnaDevRu.BulletX.LinearMath
27{ 27{
28 internal class Quaternion : QuadWord 28 internal class Quaternion : QuadWord
29 { 29 {
30 public Quaternion() { } 30 public Quaternion() { }
31 31
32 public Quaternion(float x, float y, float z, float w) 32 public Quaternion(float x, float y, float z, float w)
33 : base(x, y, z, w) { } 33 : base(x, y, z, w) { }
34 34
35 public Quaternion(Vector3 axis, float angle) 35 public Quaternion(Vector3 axis, float angle)
36 { 36 {
37 SetRotation(axis, angle); 37 SetRotation(axis, angle);
38 } 38 }
39 39
40 public Quaternion(float yaw, float pitch, float roll) 40 public Quaternion(float yaw, float pitch, float roll)
41 { 41 {
42 SetEuler(yaw, pitch, roll); 42 SetEuler(yaw, pitch, roll);
43 } 43 }
44 44
45 public void SetRotation(Vector3 axis, float angle) 45 public void SetRotation(Vector3 axis, float angle)
46 { 46 {
47 float d = axis.Length(); 47 float d = axis.Length();
48 if (d == 0) throw new DivideByZeroException(); 48 if (d == 0) throw new DivideByZeroException();
49 float s = (float)Math.Sin(angle * 0.5f) / d; 49 float s = (float)Math.Sin(angle * 0.5f) / d;
50 X = axis.X * s; 50 X = axis.X * s;
51 Y = axis.Y * s; 51 Y = axis.Y * s;
52 Z = axis.Z * s; 52 Z = axis.Z * s;
53 W = (float)Math.Cos(angle * 0.5f); 53 W = (float)Math.Cos(angle * 0.5f);
54 } 54 }
55 55
56 public void SetEuler(float yaw, float pitch, float roll) 56 public void SetEuler(float yaw, float pitch, float roll)
57 { 57 {
58 float halfYaw = yaw * 0.5f; 58 float halfYaw = yaw * 0.5f;
59 float halfPitch = pitch * 0.5f; 59 float halfPitch = pitch * 0.5f;
60 float halfRoll = roll * 0.5f; 60 float halfRoll = roll * 0.5f;
61 float cosYaw = (float)Math.Cos(halfYaw); 61 float cosYaw = (float)Math.Cos(halfYaw);
62 float sinYaw = (float)Math.Sin(halfYaw); 62 float sinYaw = (float)Math.Sin(halfYaw);
63 float cosPitch = (float)Math.Cos(halfPitch); 63 float cosPitch = (float)Math.Cos(halfPitch);
64 float sinPitch = (float)Math.Sin(halfPitch); 64 float sinPitch = (float)Math.Sin(halfPitch);
65 float cosRoll = (float)Math.Cos(halfRoll); 65 float cosRoll = (float)Math.Cos(halfRoll);
66 float sinRoll = (float)Math.Sin(halfRoll); 66 float sinRoll = (float)Math.Sin(halfRoll);
67 X = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw; 67 X = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
68 Y = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw; 68 Y = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
69 Z = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw; 69 Z = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
70 W = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw; 70 W = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
71 } 71 }
72 72
73 public float LengthSquared() 73 public float LengthSquared()
74 { 74 {
75 return Dot(this, this); 75 return Dot(this, this);
76 } 76 }
77 77
78 public float Length() 78 public float Length()
79 { 79 {
80 return (float)Math.Sqrt(LengthSquared()); 80 return (float)Math.Sqrt(LengthSquared());
81 } 81 }
82 82
83 public float Angle() 83 public float Angle()
84 { 84 {
85 return 2f * (float)Math.Acos(W); 85 return 2f * (float)Math.Acos(W);
86 } 86 }
87 87
88 public static float Angle(Quaternion a, Quaternion b) 88 public static float Angle(Quaternion a, Quaternion b)
89 { 89 {
90 float s = (float)Math.Sqrt(a.LengthSquared() * b.LengthSquared()); 90 float s = (float)Math.Sqrt(a.LengthSquared() * b.LengthSquared());
91 if (s == 0) throw new DivideByZeroException(); 91 if (s == 0) throw new DivideByZeroException();
92 return (float)Math.Acos(Dot(a, b) / s); 92 return (float)Math.Acos(Dot(a, b) / s);
93 } 93 }
94 94
95 public static Quaternion Farthest(Quaternion a, Quaternion b) 95 public static Quaternion Farthest(Quaternion a, Quaternion b)
96 { 96 {
97 Quaternion diff, sum; 97 Quaternion diff, sum;
98 diff = a - b; 98 diff = a - b;
99 sum = a + b; 99 sum = a + b;
100 if (Dot(diff, diff) > Dot(sum, sum)) 100 if (Dot(diff, diff) > Dot(sum, sum))
101 return b; 101 return b;
102 return -b; 102 return -b;
103 } 103 }
104 104
105 public static Quaternion Slerp(Quaternion a, Quaternion b, float c) 105 public static Quaternion Slerp(Quaternion a, Quaternion b, float c)
106 { 106 {
107 float theta = Angle(a, b); 107 float theta = Angle(a, b);
108 if (theta != 0) 108 if (theta != 0)
109 { 109 {
110 float d = 1f / (float)Math.Sin(theta); 110 float d = 1f / (float)Math.Sin(theta);
111 float s0 = (float)Math.Sin((1f - c) * theta); 111 float s0 = (float)Math.Sin((1f - c) * theta);
112 float s1 = (float)Math.Sin(c * theta); 112 float s1 = (float)Math.Sin(c * theta);
113 return new Quaternion( 113 return new Quaternion(
114 (a.X * s0 + b.X * s1) * d, 114 (a.X * s0 + b.X * s1) * d,
115 (a.Y * s0 + b.Y * s1) * d, 115 (a.Y * s0 + b.Y * s1) * d,
116 (a.Z * s0 + b.Z * s1) * d, 116 (a.Z * s0 + b.Z * s1) * d,
117 (a.W * s0 + b.W * s1) * d); 117 (a.W * s0 + b.W * s1) * d);
118 } 118 }
119 else 119 else
120 { 120 {
121 return a; 121 return a;
122 } 122 }
123 } 123 }
124 124
125 public static Quaternion Inverse(Quaternion a) 125 public static Quaternion Inverse(Quaternion a)
126 { 126 {
127 return new Quaternion(a.X, a.Y, a.Z, -a.W); 127 return new Quaternion(a.X, a.Y, a.Z, -a.W);
128 } 128 }
129 129
130 public static Quaternion Normalize(Quaternion a) 130 public static Quaternion Normalize(Quaternion a)
131 { 131 {
132 return a / a.Length(); 132 return a / a.Length();
133 } 133 }
134 134
135 public static float Dot(Quaternion a, Quaternion b) 135 public static float Dot(Quaternion a, Quaternion b)
136 { 136 {
137 return a.X * b.X + a.Y * b.Y + a.Z * b.Z + a.W * b.W; 137 return a.X * b.X + a.Y * b.Y + a.Z * b.Z + a.W * b.W;
138 } 138 }
139 139
140 public static Quaternion operator +(Quaternion a, Quaternion b) 140 public static Quaternion operator +(Quaternion a, Quaternion b)
141 { 141 {
142 return new Quaternion(a.X + b.X, a.Y + b.Y, a.Z + b.Z, a.W + b.W); 142 return new Quaternion(a.X + b.X, a.Y + b.Y, a.Z + b.Z, a.W + b.W);
143 } 143 }
144 144
145 public static Quaternion operator -(Quaternion a, Quaternion b) 145 public static Quaternion operator -(Quaternion a, Quaternion b)
146 { 146 {
147 return new Quaternion(a.X - b.X, a.Y - b.Y, a.Z - b.Z, a.W - b.W); 147 return new Quaternion(a.X - b.X, a.Y - b.Y, a.Z - b.Z, a.W - b.W);
148 } 148 }
149 149
150 public static Quaternion operator -(Quaternion a) 150 public static Quaternion operator -(Quaternion a)
151 { 151 {
152 return new Quaternion(-a.X, -a.Y, -a.Z, -a.W); 152 return new Quaternion(-a.X, -a.Y, -a.Z, -a.W);
153 } 153 }
154 154
155 public static Quaternion operator *(Quaternion a, float b) 155 public static Quaternion operator *(Quaternion a, float b)
156 { 156 {
157 return new Quaternion(a.X * b, a.Y * b, a.Z * b, a.W * b); 157 return new Quaternion(a.X * b, a.Y * b, a.Z * b, a.W * b);
158 } 158 }
159 159
160 public static Quaternion operator *(Quaternion a, Quaternion b) 160 public static Quaternion operator *(Quaternion a, Quaternion b)
161 { 161 {
162 return new Quaternion( 162 return new Quaternion(
163 a.W * b.X + a.X * b.W + a.Y * b.Z - a.Z * b.Y, 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, 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, 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); 166 a.W * b.W - a.X * b.X - a.Y * b.Y - a.Z * b.Z);
167 } 167 }
168 168
169 public static Quaternion operator *(Quaternion a, Vector3 b) 169 public static Quaternion operator *(Quaternion a, Vector3 b)
170 { 170 {
171 return new Quaternion( 171 return new Quaternion(
172 a.W * b.X + a.Y * b.Z - a.Z * b.Y, 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, 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, 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); 175 -a.X * b.X - a.Y * b.Y - a.Z * b.Z);
176 } 176 }
177 177
178 public static Quaternion operator *(Vector3 w, Quaternion q) 178 public static Quaternion operator *(Vector3 w, Quaternion q)
179 { 179 {
180 return new Quaternion( 180 return new Quaternion(
181 w.X * q.W + w.Y * q.Z - w.Z * q.Y, 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, 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, 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); 184 -w.X * q.X - w.Y * q.Y - w.Z * q.Z);
185 } 185 }
186 186
187 public static Quaternion operator /(Quaternion a, float b) 187 public static Quaternion operator /(Quaternion a, float b)
188 { 188 {
189 if (b == 0) throw new DivideByZeroException(); 189 if (b == 0) throw new DivideByZeroException();
190 return new Quaternion(a.X / b, a.Y / b, a.Z / b, a.W / b); 190 return new Quaternion(a.X / b, a.Y / b, a.Z / b, a.W / b);
191 } 191 }
192 192
193 public static explicit operator MonoXnaCompactMaths.Quaternion(Quaternion a) 193 public static explicit operator MonoXnaCompactMaths.Quaternion(Quaternion a)
194 { 194 {
195 return new MonoXnaCompactMaths.Quaternion(a.X, a.Y, a.Z, a.W); 195 return new MonoXnaCompactMaths.Quaternion(a.X, a.Y, a.Z, a.W);
196 } 196 }
197 } 197 }
198} 198}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs
index 031faab..ccf92a0 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs
@@ -1,102 +1,102 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX 27namespace XnaDevRu.BulletX
28{ 28{
29 public static class TransformUtil 29 public static class TransformUtil
30 { 30 {
31 //const float AngularMotionTreshold = 0.5f * MonoXnaCompactMaths.MathHelper.PiOver2; 31 //const float AngularMotionTreshold = 0.5f * MonoXnaCompactMaths.MathHelper.PiOver2;
32 const float AngularMotionTreshold = 0.5f * (float)Math.PI / 2.0f; 32 const float AngularMotionTreshold = 0.5f * (float)Math.PI / 2.0f;
33 33
34 public static void IntegrateTransform(Matrix currentTransform, Vector3 linearVelocity, Vector3 angularVelocity, float timeStep, ref Matrix predictedTransform) 34 public static void IntegrateTransform(Matrix currentTransform, Vector3 linearVelocity, Vector3 angularVelocity, float timeStep, ref Matrix predictedTransform)
35 { 35 {
36 predictedTransform.Translation = currentTransform.Translation + linearVelocity * timeStep; 36 predictedTransform.Translation = currentTransform.Translation + linearVelocity * timeStep;
37 //exponential map 37 //exponential map
38 Vector3 axis; 38 Vector3 axis;
39 float angle = angularVelocity.Length(); 39 float angle = angularVelocity.Length();
40 //limit the angular motion 40 //limit the angular motion
41 if (angle * timeStep > AngularMotionTreshold) 41 if (angle * timeStep > AngularMotionTreshold)
42 { 42 {
43 angle = AngularMotionTreshold / timeStep; 43 angle = AngularMotionTreshold / timeStep;
44 } 44 }
45 45
46 if (angle < 0.001f) 46 if (angle < 0.001f)
47 { 47 {
48 // use Taylor's expansions of sync function 48 // use Taylor's expansions of sync function
49 axis = angularVelocity * (0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f) * angle * angle); 49 axis = angularVelocity * (0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f) * angle * angle);
50 } 50 }
51 else 51 else
52 { 52 {
53 // sync(fAngle) = sin(c*fAngle)/t 53 // sync(fAngle) = sin(c*fAngle)/t
54 axis = angularVelocity * ((float)Math.Sin(0.5f * angle * timeStep) / angle); 54 axis = angularVelocity * ((float)Math.Sin(0.5f * angle * timeStep) / angle);
55 } 55 }
56 Quaternion dorn = new Quaternion(axis.X, axis.Y, axis.Z, (float)Math.Cos(angle * timeStep * 0.5f)); 56 Quaternion dorn = new Quaternion(axis.X, axis.Y, axis.Z, (float)Math.Cos(angle * timeStep * 0.5f));
57 Quaternion ornA = MatrixOperations.GetRotation(currentTransform); 57 Quaternion ornA = MatrixOperations.GetRotation(currentTransform);
58 58
59 Quaternion predictedOrn = dorn * ornA; 59 Quaternion predictedOrn = dorn * ornA;
60 predictedOrn.Normalize(); 60 predictedOrn.Normalize();
61 61
62 MatrixOperations.SetRotation(ref predictedTransform, predictedOrn); 62 MatrixOperations.SetRotation(ref predictedTransform, predictedOrn);
63 63
64 Matrix test = Matrix.CreateFromQuaternion(predictedOrn); 64 Matrix test = Matrix.CreateFromQuaternion(predictedOrn);
65 } 65 }
66 66
67 public static void CalculateVelocity(Matrix transformA, Matrix transformB, float timeStep, ref Vector3 linearVelocity, ref Vector3 angularVelocity) 67 public static void CalculateVelocity(Matrix transformA, Matrix transformB, float timeStep, ref Vector3 linearVelocity, ref Vector3 angularVelocity)
68 { 68 {
69 linearVelocity = (transformB.Translation - transformA.Translation) / timeStep; 69 linearVelocity = (transformB.Translation - transformA.Translation) / timeStep;
70 Matrix dmat = transformB * MathHelper.InvertMatrix(transformA); 70 Matrix dmat = transformB * MathHelper.InvertMatrix(transformA);
71 Quaternion dorn = Quaternion.CreateFromRotationMatrix(dmat); 71 Quaternion dorn = Quaternion.CreateFromRotationMatrix(dmat);
72 72
73 Vector3 axis; 73 Vector3 axis;
74 float angle = 2 * (float)Math.Acos(dorn.W); 74 float angle = 2 * (float)Math.Acos(dorn.W);
75 axis = new Vector3(dorn.X, dorn.Y, dorn.Z); 75 axis = new Vector3(dorn.X, dorn.Y, dorn.Z);
76 //axis[3] = 0.f; 76 //axis[3] = 0.f;
77 //check for axis length 77 //check for axis length
78 float len = axis.LengthSquared(); 78 float len = axis.LengthSquared();
79 if (len < MathHelper.Epsilon * MathHelper.Epsilon) 79 if (len < MathHelper.Epsilon * MathHelper.Epsilon)
80 axis = new Vector3(1f, 0f, 0f); 80 axis = new Vector3(1f, 0f, 0f);
81 else 81 else
82 axis /= (float)Math.Sqrt(len); 82 axis /= (float)Math.Sqrt(len);
83 83
84 angularVelocity = axis * angle / timeStep; 84 angularVelocity = axis * angle / timeStep;
85 } 85 }
86 86
87 public static void CalculateDiffAxisAngle(Matrix transformA, Matrix transformB, out Vector3 axis, out float angle) 87 public static void CalculateDiffAxisAngle(Matrix transformA, Matrix transformB, out Vector3 axis, out float angle)
88 { 88 {
89 Matrix dmat = transformB * MathHelper.InvertMatrix(transformA); 89 Matrix dmat = transformB * MathHelper.InvertMatrix(transformA);
90 Quaternion dorn = MathHelper.GetRotation(dmat); 90 Quaternion dorn = MathHelper.GetRotation(dmat);
91 91
92 angle = 2f * (float)Math.Acos(dorn.W); 92 angle = 2f * (float)Math.Acos(dorn.W);
93 axis = new Vector3(dorn.X, dorn.Y, dorn.Z); 93 axis = new Vector3(dorn.X, dorn.Y, dorn.Z);
94 //check for axis length 94 //check for axis length
95 float len = axis.LengthSquared(); 95 float len = axis.LengthSquared();
96 if (len < MathHelper.Epsilon * MathHelper.Epsilon) 96 if (len < MathHelper.Epsilon * MathHelper.Epsilon)
97 axis = new Vector3(1f, 0f, 0f); 97 axis = new Vector3(1f, 0f, 0f);
98 else 98 else
99 axis /= (float)Math.Sqrt(len); 99 axis /= (float)Math.Sqrt(len);
100 } 100 }
101 } 101 }
102} 102}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector3.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector3.cs
index 79b1262..adeec25 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector3.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector3.cs
@@ -1,221 +1,221 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25 25
26namespace XnaDevRu.BulletX.LinearMath 26namespace XnaDevRu.BulletX.LinearMath
27{ 27{
28 internal class Vector3 : QuadWord 28 internal class Vector3 : QuadWord
29 { 29 {
30 public Vector3() { } 30 public Vector3() { }
31 31
32 public Vector3(float x, float y, float z) 32 public Vector3(float x, float y, float z)
33 : base(x, y, z) { } 33 : base(x, y, z) { }
34 34
35 public void SetInterpolate3(Vector3 a, Vector3 b, float c) 35 public void SetInterpolate3(Vector3 a, Vector3 b, float c)
36 { 36 {
37 float s = 1.0f - c; 37 float s = 1.0f - c;
38 X = s * a.X + c * b.X; 38 X = s * a.X + c * b.X;
39 Y = s * a.Y + c * b.Y; 39 Y = s * a.Y + c * b.Y;
40 Z = s * a.Z + c * b.Z; 40 Z = s * a.Z + c * b.Z;
41 } 41 }
42 42
43 public float LengthSquared() 43 public float LengthSquared()
44 { 44 {
45 return Dot(this, this); 45 return Dot(this, this);
46 } 46 }
47 47
48 public float Length() 48 public float Length()
49 { 49 {
50 return (float)Math.Sqrt(LengthSquared()); 50 return (float)Math.Sqrt(LengthSquared());
51 } 51 }
52 52
53 public int MinAxis() 53 public int MinAxis()
54 { 54 {
55 return X < Y ? (X < Z ? 0 : 2) : (Y < Z ? 1 : 2); 55 return X < Y ? (X < Z ? 0 : 2) : (Y < Z ? 1 : 2);
56 } 56 }
57 57
58 public int MaxAxis() 58 public int MaxAxis()
59 { 59 {
60 return X < Y ? (Y < Z ? 2 : 1) : (X < Z ? 2 : 0); 60 return X < Y ? (Y < Z ? 2 : 1) : (X < Z ? 2 : 0);
61 } 61 }
62 62
63 public int FurthestAxis() 63 public int FurthestAxis()
64 { 64 {
65 return Absolute(this).MinAxis(); 65 return Absolute(this).MinAxis();
66 } 66 }
67 67
68 public int ClosestAxis() 68 public int ClosestAxis()
69 { 69 {
70 return Absolute(this).MaxAxis(); 70 return Absolute(this).MaxAxis();
71 } 71 }
72 72
73 public Vector3 Rotate(Vector3 axis, float angle) 73 public Vector3 Rotate(Vector3 axis, float angle)
74 { 74 {
75 Vector3 o = axis * Dot(axis, this); 75 Vector3 o = axis * Dot(axis, this);
76 Vector3 x = this - o; 76 Vector3 x = this - o;
77 Vector3 y = Cross(axis, this); 77 Vector3 y = Cross(axis, this);
78 78
79 return (o + x * (float)Math.Cos(angle) + y * (float)Math.Sin(angle)); 79 return (o + x * (float)Math.Cos(angle) + y * (float)Math.Sin(angle));
80 } 80 }
81 81
82 public static Vector3 Lerp(Vector3 a, Vector3 b, float c) 82 public static Vector3 Lerp(Vector3 a, Vector3 b, float c)
83 { 83 {
84 return new Vector3( 84 return new Vector3(
85 a.X + (b.X - a.X) * c, 85 a.X + (b.X - a.X) * c,
86 a.Y + (b.Y - a.Y) * c, 86 a.Y + (b.Y - a.Y) * c,
87 a.Z + (b.Z - a.Z) * c); 87 a.Z + (b.Z - a.Z) * c);
88 } 88 }
89 89
90 public static float Angle(Vector3 a, Vector3 b) 90 public static float Angle(Vector3 a, Vector3 b)
91 { 91 {
92 float s = (float)Math.Sqrt(a.LengthSquared() * b.LengthSquared()); 92 float s = (float)Math.Sqrt(a.LengthSquared() * b.LengthSquared());
93 if (s == 0) throw new DivideByZeroException(); 93 if (s == 0) throw new DivideByZeroException();
94 return (float)Math.Acos(Dot(a, b) / s); 94 return (float)Math.Acos(Dot(a, b) / s);
95 } 95 }
96 96
97 public static Vector3 Absolute(Vector3 a) 97 public static Vector3 Absolute(Vector3 a)
98 { 98 {
99 return new Vector3( 99 return new Vector3(
100 Math.Abs(a.X), 100 Math.Abs(a.X),
101 Math.Abs(a.Y), 101 Math.Abs(a.Y),
102 Math.Abs(a.Z)); 102 Math.Abs(a.Z));
103 } 103 }
104 104
105 public static Vector3 Normalize(Vector3 a) 105 public static Vector3 Normalize(Vector3 a)
106 { 106 {
107 return a / a.Length(); 107 return a / a.Length();
108 } 108 }
109 109
110 public static Vector3 Cross(Vector3 a, Vector3 b) 110 public static Vector3 Cross(Vector3 a, Vector3 b)
111 { 111 {
112 return new Vector3( 112 return new Vector3(
113 a.Y * b.Z - a.Z * b.Y, 113 a.Y * b.Z - a.Z * b.Y,
114 a.Z * b.X - a.X * b.Z, 114 a.Z * b.X - a.X * b.Z,
115 a.X * b.Y - a.Y * b.X); 115 a.X * b.Y - a.Y * b.X);
116 } 116 }
117 117
118 public static float Dot(Vector3 a, Vector3 b) 118 public static float Dot(Vector3 a, Vector3 b)
119 { 119 {
120 return a.X * b.X + a.Y * b.Y + a.Z * b.Z; 120 return a.X * b.X + a.Y * b.Y + a.Z * b.Z;
121 } 121 }
122 122
123 public static float Triple(Vector3 a, Vector3 b, Vector3 c) 123 public static float Triple(Vector3 a, Vector3 b, Vector3 c)
124 { 124 {
125 return a.X * (b.Y * c.Z - b.Z * c.Y) + 125 return a.X * (b.Y * c.Z - b.Z * c.Y) +
126 a.Y * (b.Z * c.X - b.X * c.Z) + 126 a.Y * (b.Z * c.X - b.X * c.Z) +
127 a.Z * (b.X * c.Y - b.Y * c.X); 127 a.Z * (b.X * c.Y - b.Y * c.X);
128 } 128 }
129 129
130 public static float Distance(Vector3 a, Vector3 b) 130 public static float Distance(Vector3 a, Vector3 b)
131 { 131 {
132 return (b - a).Length(); 132 return (b - a).Length();
133 } 133 }
134 134
135 public static float DistanceSquared(Vector3 a, Vector3 b) 135 public static float DistanceSquared(Vector3 a, Vector3 b)
136 { 136 {
137 return (b - a).LengthSquared(); 137 return (b - a).LengthSquared();
138 } 138 }
139 139
140 public static Vector3 Rotate(Vector3 a, Vector3 axis, float angle) 140 public static Vector3 Rotate(Vector3 a, Vector3 axis, float angle)
141 { 141 {
142 Vector3 o = axis * Dot(axis, a); 142 Vector3 o = axis * Dot(axis, a);
143 Vector3 x = a - o; 143 Vector3 x = a - o;
144 Vector3 y = Cross(axis, a); 144 Vector3 y = Cross(axis, a);
145 145
146 return (o + x * (float)Math.Cos(angle) + y * (float)Math.Sin(angle)); 146 return (o + x * (float)Math.Cos(angle) + y * (float)Math.Sin(angle));
147 } 147 }
148 148
149 public static Vector3 operator +(Vector3 a, Vector3 b) 149 public static Vector3 operator +(Vector3 a, Vector3 b)
150 { 150 {
151 return new Vector3(a.X + b.X, a.Y + b.Y, a.Z + b.Z); 151 return new Vector3(a.X + b.X, a.Y + b.Y, a.Z + b.Z);
152 } 152 }
153 153
154 public static Vector3 operator -(Vector3 a, Vector3 b) 154 public static Vector3 operator -(Vector3 a, Vector3 b)
155 { 155 {
156 return new Vector3(a.X - b.X, a.Y - b.Y, a.Z - b.Z); 156 return new Vector3(a.X - b.X, a.Y - b.Y, a.Z - b.Z);
157 } 157 }
158 158
159 public static Vector3 operator -(Vector3 a) 159 public static Vector3 operator -(Vector3 a)
160 { 160 {
161 return new Vector3(-a.X, -a.Y, -a.Z); 161 return new Vector3(-a.X, -a.Y, -a.Z);
162 } 162 }
163 163
164 public static Vector3 operator *(float b, Vector3 a) 164 public static Vector3 operator *(float b, Vector3 a)
165 { 165 {
166 return new Vector3(a.X * b, a.Y * b, a.Z * b); 166 return new Vector3(a.X * b, a.Y * b, a.Z * b);
167 } 167 }
168 168
169 public static Vector3 operator *(Vector3 a, float b) 169 public static Vector3 operator *(Vector3 a, float b)
170 { 170 {
171 return new Vector3(a.X * b, a.Y * b, a.Z * b); 171 return new Vector3(a.X * b, a.Y * b, a.Z * b);
172 } 172 }
173 173
174 public static Vector3 operator *(Vector3 a, Vector3 b) 174 public static Vector3 operator *(Vector3 a, Vector3 b)
175 { 175 {
176 return new Vector3(a.X * b.X, a.Y * b.Y, a.Z * b.Z); 176 return new Vector3(a.X * b.X, a.Y * b.Y, a.Z * b.Z);
177 } 177 }
178 178
179 public static Vector3 operator /(Vector3 a, float b) 179 public static Vector3 operator /(Vector3 a, float b)
180 { 180 {
181 if (b == 0) throw new DivideByZeroException(); 181 if (b == 0) throw new DivideByZeroException();
182 return new Vector3(a.X / b, a.Y / b, a.Z / b); 182 return new Vector3(a.X / b, a.Y / b, a.Z / b);
183 } 183 }
184 184
185 public static Vector3 operator /(Vector3 a, Vector3 b) 185 public static Vector3 operator /(Vector3 a, Vector3 b)
186 { 186 {
187 if (b.X == 0 || b.Y == 0 || b.Z == 0) throw new DivideByZeroException(); 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); 188 return new Vector3(a.X / b.X, a.Y / b.Y, a.Z / b.Z);
189 } 189 }
190 190
191 public static bool operator ==(Vector3 a, Vector3 b) 191 public static bool operator ==(Vector3 a, Vector3 b)
192 { 192 {
193 return a.X == b.X && a.Y == b.Y && a.Z == b.Z; 193 return a.X == b.X && a.Y == b.Y && a.Z == b.Z;
194 } 194 }
195 195
196 public static bool operator !=(Vector3 a, Vector3 b) 196 public static bool operator !=(Vector3 a, Vector3 b)
197 { 197 {
198 return a.X != b.X || a.Y != b.Y || a.Z != b.Z; 198 return a.X != b.X || a.Y != b.Y || a.Z != b.Z;
199 } 199 }
200 200
201 public static explicit operator MonoXnaCompactMaths.Vector3(Vector3 a) 201 public static explicit operator MonoXnaCompactMaths.Vector3(Vector3 a)
202 { 202 {
203 return new MonoXnaCompactMaths.Vector3(a.X, a.Y, a.Z); 203 return new MonoXnaCompactMaths.Vector3(a.X, a.Y, a.Z);
204 } 204 }
205 205
206 public override bool Equals(object obj) 206 public override bool Equals(object obj)
207 { 207 {
208 return object.Equals(this, obj); 208 return object.Equals(this, obj);
209 } 209 }
210 210
211 public override int GetHashCode() 211 public override int GetHashCode()
212 { 212 {
213 return X.GetHashCode() & Y.GetHashCode() & Z.GetHashCode(); 213 return X.GetHashCode() & Y.GetHashCode() & Z.GetHashCode();
214 } 214 }
215 215
216 public override string ToString() 216 public override string ToString()
217 { 217 {
218 return string.Format("{0}, {1}, {2}", X, Y, Z); 218 return string.Format("{0}, {1}, {2}", X, Y, Z);
219 } 219 }
220 } 220 }
221} 221}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector4.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector4.cs
index a64ce97..de98ccd 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector4.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector4.cs
@@ -1,110 +1,110 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 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 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 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 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 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 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 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25 25
26namespace XnaDevRu.BulletX.LinearMath 26namespace XnaDevRu.BulletX.LinearMath
27{ 27{
28 internal class Vector4 : Vector3 28 internal class Vector4 : Vector3
29 { 29 {
30 public Vector4() { } 30 public Vector4() { }
31 31
32 public Vector4(float x, float y, float z, float w) 32 public Vector4(float x, float y, float z, float w)
33 : base(x, y, z) { W = w; } 33 : base(x, y, z) { W = w; }
34 34
35 public static Vector4 Absolute4(Vector4 a) 35 public static Vector4 Absolute4(Vector4 a)
36 { 36 {
37 return new Vector4( 37 return new Vector4(
38 Math.Abs(a.X), 38 Math.Abs(a.X),
39 Math.Abs(a.Y), 39 Math.Abs(a.Y),
40 Math.Abs(a.Z), 40 Math.Abs(a.Z),
41 Math.Abs(a.W)); 41 Math.Abs(a.W));
42 } 42 }
43 43
44 public int MaxAxis4() 44 public int MaxAxis4()
45 { 45 {
46 int maxIndex = -1; 46 int maxIndex = -1;
47 float maxVal = -1e30f; 47 float maxVal = -1e30f;
48 if (X > maxVal) 48 if (X > maxVal)
49 { 49 {
50 maxIndex = 0; 50 maxIndex = 0;
51 maxVal = X; 51 maxVal = X;
52 } 52 }
53 if (Y > maxVal) 53 if (Y > maxVal)
54 { 54 {
55 maxIndex = 1; 55 maxIndex = 1;
56 maxVal = Y; 56 maxVal = Y;
57 } 57 }
58 if (Z > maxVal) 58 if (Z > maxVal)
59 { 59 {
60 maxIndex = 2; 60 maxIndex = 2;
61 maxVal = Z; 61 maxVal = Z;
62 } 62 }
63 if (W > maxVal) 63 if (W > maxVal)
64 { 64 {
65 maxIndex = 3; 65 maxIndex = 3;
66 maxVal = W; 66 maxVal = W;
67 } 67 }
68 68
69 return maxIndex; 69 return maxIndex;
70 } 70 }
71 71
72 public int MinAxis4() 72 public int MinAxis4()
73 { 73 {
74 int minIndex = -1; 74 int minIndex = -1;
75 float minVal = 1e30f; 75 float minVal = 1e30f;
76 if (X < minVal) 76 if (X < minVal)
77 { 77 {
78 minIndex = 0; 78 minIndex = 0;
79 minVal = X; 79 minVal = X;
80 } 80 }
81 if (Y < minVal) 81 if (Y < minVal)
82 { 82 {
83 minIndex = 1; 83 minIndex = 1;
84 minVal = Y; 84 minVal = Y;
85 } 85 }
86 if (Z < minVal) 86 if (Z < minVal)
87 { 87 {
88 minIndex = 2; 88 minIndex = 2;
89 minVal = Z; 89 minVal = Z;
90 } 90 }
91 if (W < minVal) 91 if (W < minVal)
92 { 92 {
93 minIndex = 3; 93 minIndex = 3;
94 minVal = W; 94 minVal = W;
95 } 95 }
96 96
97 return minIndex; 97 return minIndex;
98 } 98 }
99 99
100 public int ClosestAxis4() 100 public int ClosestAxis4()
101 { 101 {
102 return Absolute4(this).MaxAxis4(); 102 return Absolute4(this).MaxAxis4();
103 } 103 }
104 104
105 public static explicit operator MonoXnaCompactMaths.Vector4(Vector4 a) 105 public static explicit operator MonoXnaCompactMaths.Vector4(Vector4 a)
106 { 106 {
107 return new MonoXnaCompactMaths.Vector4(a.X, a.Y, a.Z, a.W); 107 return new MonoXnaCompactMaths.Vector4(a.X, a.Y, a.Z, a.W);
108 } 108 }
109 } 109 }
110} 110}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj b/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj
index bed8d19..06b98bc 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj
@@ -1,168 +1,168 @@
1<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003"> 1<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
2 <PropertyGroup> 2 <PropertyGroup>
3 <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration> 3 <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
4 <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform> 4 <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
5 <ProductVersion>8.0.50727</ProductVersion> 5 <ProductVersion>8.0.50727</ProductVersion>
6 <SchemaVersion>2.0</SchemaVersion> 6 <SchemaVersion>2.0</SchemaVersion>
7 <ProjectGuid>{44270344-ACA7-4875-B585-81D5C06D0489}</ProjectGuid> 7 <ProjectGuid>{44270344-ACA7-4875-B585-81D5C06D0489}</ProjectGuid>
8 <OutputType>Library</OutputType> 8 <OutputType>Library</OutputType>
9 <AppDesignerFolder>Properties</AppDesignerFolder> 9 <AppDesignerFolder>Properties</AppDesignerFolder>
10 <RootNamespace>XnaDevRu.BulletX</RootNamespace> 10 <RootNamespace>XnaDevRu.BulletX</RootNamespace>
11 <AssemblyName>Modified.XnaDevRu.BulletX</AssemblyName> 11 <AssemblyName>Modified.XnaDevRu.BulletX</AssemblyName>
12 </PropertyGroup> 12 </PropertyGroup>
13 <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' "> 13 <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
14 <DebugSymbols>true</DebugSymbols> 14 <DebugSymbols>true</DebugSymbols>
15 <DebugType>full</DebugType> 15 <DebugType>full</DebugType>
16 <Optimize>false</Optimize> 16 <Optimize>false</Optimize>
17 <OutputPath>bin\Debug\</OutputPath> 17 <OutputPath>bin\Debug\</OutputPath>
18 <DefineConstants>DEBUG;TRACE</DefineConstants> 18 <DefineConstants>DEBUG;TRACE</DefineConstants>
19 <ErrorReport>prompt</ErrorReport> 19 <ErrorReport>prompt</ErrorReport>
20 <WarningLevel>4</WarningLevel> 20 <WarningLevel>4</WarningLevel>
21 </PropertyGroup> 21 </PropertyGroup>
22 <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' "> 22 <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
23 <DebugType>pdbonly</DebugType> 23 <DebugType>pdbonly</DebugType>
24 <Optimize>true</Optimize> 24 <Optimize>true</Optimize>
25 <OutputPath>bin\Release\</OutputPath> 25 <OutputPath>bin\Release\</OutputPath>
26 <DefineConstants>TRACE</DefineConstants> 26 <DefineConstants>TRACE</DefineConstants>
27 <ErrorReport>prompt</ErrorReport> 27 <ErrorReport>prompt</ErrorReport>
28 <WarningLevel>4</WarningLevel> 28 <WarningLevel>4</WarningLevel>
29 </PropertyGroup> 29 </PropertyGroup>
30 <ItemGroup> 30 <ItemGroup>
31 <Reference Include="System" /> 31 <Reference Include="System" />
32 </ItemGroup> 32 </ItemGroup>
33 <ItemGroup> 33 <ItemGroup>
34 <Compile Include="BulletDebug.cs" /> 34 <Compile Include="BulletDebug.cs" />
35 <Compile Include="Collision\BroadphaseCollision\AxisSweep3.cs" /> 35 <Compile Include="Collision\BroadphaseCollision\AxisSweep3.cs" />
36 <Compile Include="Collision\BroadphaseCollision\BroadphaseNativeTypes.cs" /> 36 <Compile Include="Collision\BroadphaseCollision\BroadphaseNativeTypes.cs" />
37 <Compile Include="Collision\BroadphaseCollision\BroadphasePair.cs" /> 37 <Compile Include="Collision\BroadphaseCollision\BroadphasePair.cs" />
38 <Compile Include="Collision\BroadphaseCollision\BroadphaseProxy.cs" /> 38 <Compile Include="Collision\BroadphaseCollision\BroadphaseProxy.cs" />
39 <Compile Include="Collision\BroadphaseCollision\CollisionAlgorithm.cs" /> 39 <Compile Include="Collision\BroadphaseCollision\CollisionAlgorithm.cs" />
40 <Compile Include="Collision\BroadphaseCollision\CollisionAlgorithmConstructionInfo.cs" /> 40 <Compile Include="Collision\BroadphaseCollision\CollisionAlgorithmConstructionInfo.cs" />
41 <Compile Include="Collision\BroadphaseCollision\DispatcherInfo.cs" /> 41 <Compile Include="Collision\BroadphaseCollision\DispatcherInfo.cs" />
42 <Compile Include="Collision\BroadphaseCollision\IBroadphase.cs" /> 42 <Compile Include="Collision\BroadphaseCollision\IBroadphase.cs" />
43 <Compile Include="Collision\BroadphaseCollision\IDispatcher.cs" /> 43 <Compile Include="Collision\BroadphaseCollision\IDispatcher.cs" />
44 <Compile Include="Collision\BroadphaseCollision\IOverlapCallback.cs" /> 44 <Compile Include="Collision\BroadphaseCollision\IOverlapCallback.cs" />
45 <Compile Include="Collision\BroadphaseCollision\OverlappingPairCache.cs" /> 45 <Compile Include="Collision\BroadphaseCollision\OverlappingPairCache.cs" />
46 <Compile Include="Collision\BroadphaseCollision\SimpleBroadphase.cs" /> 46 <Compile Include="Collision\BroadphaseCollision\SimpleBroadphase.cs" />
47 <Compile Include="Collision\BroadphaseCollision\SimpleBroadphaseProxy.cs" /> 47 <Compile Include="Collision\BroadphaseCollision\SimpleBroadphaseProxy.cs" />
48 <Compile Include="Collision\CollisionDispatch\BridgeTriangleRaycastCallback.cs" /> 48 <Compile Include="Collision\CollisionDispatch\BridgeTriangleRaycastCallback.cs" />
49 <Compile Include="Collision\CollisionDispatch\CollisionAlgorithmCreateFunc.cs" /> 49 <Compile Include="Collision\CollisionDispatch\CollisionAlgorithmCreateFunc.cs" />
50 <Compile Include="Collision\CollisionDispatch\CollisionDispatcher.cs" /> 50 <Compile Include="Collision\CollisionDispatch\CollisionDispatcher.cs" />
51 <Compile Include="Collision\CollisionDispatch\CollisionObject.cs" /> 51 <Compile Include="Collision\CollisionDispatch\CollisionObject.cs" />
52 <Compile Include="Collision\CollisionDispatch\CollisionPairCallback.cs" /> 52 <Compile Include="Collision\CollisionDispatch\CollisionPairCallback.cs" />
53 <Compile Include="Collision\CollisionDispatch\CollisionWorld.cs" /> 53 <Compile Include="Collision\CollisionDispatch\CollisionWorld.cs" />
54 <Compile Include="Collision\CollisionDispatch\CompoundCollisionAlgorithm.cs" /> 54 <Compile Include="Collision\CollisionDispatch\CompoundCollisionAlgorithm.cs" />
55 <Compile Include="Collision\CollisionDispatch\ConvexConcaveCollisionAlgorithm.cs" /> 55 <Compile Include="Collision\CollisionDispatch\ConvexConcaveCollisionAlgorithm.cs" />
56 <Compile Include="Collision\CollisionDispatch\ConvexConvexCollisionAlgorithm.cs" /> 56 <Compile Include="Collision\CollisionDispatch\ConvexConvexCollisionAlgorithm.cs" />
57 <Compile Include="Collision\CollisionDispatch\ConvexTriangleCallback.cs" /> 57 <Compile Include="Collision\CollisionDispatch\ConvexTriangleCallback.cs" />
58 <Compile Include="Collision\CollisionDispatch\EmptyAlgorithm.cs" /> 58 <Compile Include="Collision\CollisionDispatch\EmptyAlgorithm.cs" />
59 <Compile Include="Collision\CollisionDispatch\ManifoldResult.cs" /> 59 <Compile Include="Collision\CollisionDispatch\ManifoldResult.cs" />
60 <Compile Include="Collision\CollisionDispatch\SimulationIslandManager.cs" /> 60 <Compile Include="Collision\CollisionDispatch\SimulationIslandManager.cs" />
61 <Compile Include="Collision\CollisionDispatch\SphereBoxCollisionAlgorithm.cs" /> 61 <Compile Include="Collision\CollisionDispatch\SphereBoxCollisionAlgorithm.cs" />
62 <Compile Include="Collision\CollisionDispatch\SphereSphereCollisionAlgorithm.cs" /> 62 <Compile Include="Collision\CollisionDispatch\SphereSphereCollisionAlgorithm.cs" />
63 <Compile Include="Collision\CollisionDispatch\SphereTriangleCollisionAlgorithm.cs" /> 63 <Compile Include="Collision\CollisionDispatch\SphereTriangleCollisionAlgorithm.cs" />
64 <Compile Include="Collision\CollisionDispatch\SphereTriangleDetector.cs" /> 64 <Compile Include="Collision\CollisionDispatch\SphereTriangleDetector.cs" />
65 <Compile Include="Collision\CollisionDispatch\UnionFind.cs" /> 65 <Compile Include="Collision\CollisionDispatch\UnionFind.cs" />
66 <Compile Include="Collision\CollisionShapes\BoxShape.cs" /> 66 <Compile Include="Collision\CollisionShapes\BoxShape.cs" />
67 <Compile Include="Collision\CollisionShapes\BUSimplex1to4.cs" /> 67 <Compile Include="Collision\CollisionShapes\BUSimplex1to4.cs" />
68 <Compile Include="Collision\CollisionShapes\BvhTriangleMeshShape.cs" /> 68 <Compile Include="Collision\CollisionShapes\BvhTriangleMeshShape.cs" />
69 <Compile Include="Collision\CollisionShapes\CollisionShape.cs" /> 69 <Compile Include="Collision\CollisionShapes\CollisionShape.cs" />
70 <Compile Include="Collision\CollisionShapes\CompoundShape.cs" /> 70 <Compile Include="Collision\CollisionShapes\CompoundShape.cs" />
71 <Compile Include="Collision\CollisionShapes\ConcaveShape.cs" /> 71 <Compile Include="Collision\CollisionShapes\ConcaveShape.cs" />
72 <Compile Include="Collision\CollisionShapes\ConeShape.cs" /> 72 <Compile Include="Collision\CollisionShapes\ConeShape.cs" />
73 <Compile Include="Collision\CollisionShapes\ConvexHullShape.cs" /> 73 <Compile Include="Collision\CollisionShapes\ConvexHullShape.cs" />
74 <Compile Include="Collision\CollisionShapes\ConvexShape.cs" /> 74 <Compile Include="Collision\CollisionShapes\ConvexShape.cs" />
75 <Compile Include="Collision\CollisionShapes\ConvexTriangleMeshShape.cs" /> 75 <Compile Include="Collision\CollisionShapes\ConvexTriangleMeshShape.cs" />
76 <Compile Include="Collision\CollisionShapes\CylinderShape.cs" /> 76 <Compile Include="Collision\CollisionShapes\CylinderShape.cs" />
77 <Compile Include="Collision\CollisionShapes\CylinderShapeX.cs" /> 77 <Compile Include="Collision\CollisionShapes\CylinderShapeX.cs" />
78 <Compile Include="Collision\CollisionShapes\CylinderShapeZ.cs" /> 78 <Compile Include="Collision\CollisionShapes\CylinderShapeZ.cs" />
79 <Compile Include="Collision\CollisionShapes\EmptyShape.cs" /> 79 <Compile Include="Collision\CollisionShapes\EmptyShape.cs" />
80 <Compile Include="Collision\CollisionShapes\FilteredCallback.cs" /> 80 <Compile Include="Collision\CollisionShapes\FilteredCallback.cs" />
81 <Compile Include="Collision\CollisionShapes\InternalTriangleIndexCallback.cs" /> 81 <Compile Include="Collision\CollisionShapes\InternalTriangleIndexCallback.cs" />
82 <Compile Include="Collision\CollisionShapes\LocalSupportVertexCallback.cs" /> 82 <Compile Include="Collision\CollisionShapes\LocalSupportVertexCallback.cs" />
83 <Compile Include="Collision\CollisionShapes\MinkowskiSumShape.cs" /> 83 <Compile Include="Collision\CollisionShapes\MinkowskiSumShape.cs" />
84 <Compile Include="Collision\CollisionShapes\MultiSphereShape.cs" /> 84 <Compile Include="Collision\CollisionShapes\MultiSphereShape.cs" />
85 <Compile Include="Collision\CollisionShapes\NodeOverlapCallback.cs" /> 85 <Compile Include="Collision\CollisionShapes\NodeOverlapCallback.cs" />
86 <Compile Include="Collision\CollisionShapes\OptimizedBvh.cs" /> 86 <Compile Include="Collision\CollisionShapes\OptimizedBvh.cs" />
87 <Compile Include="Collision\CollisionShapes\OptimizedBvhNode.cs" /> 87 <Compile Include="Collision\CollisionShapes\OptimizedBvhNode.cs" />
88 <Compile Include="Collision\CollisionShapes\PolyhedralConvexShape.cs" /> 88 <Compile Include="Collision\CollisionShapes\PolyhedralConvexShape.cs" />
89 <Compile Include="Collision\CollisionShapes\SphereShape.cs" /> 89 <Compile Include="Collision\CollisionShapes\SphereShape.cs" />
90 <Compile Include="Collision\CollisionShapes\StaticPlaneShape.cs" /> 90 <Compile Include="Collision\CollisionShapes\StaticPlaneShape.cs" />
91 <Compile Include="Collision\CollisionShapes\StridingMeshInterface.cs" /> 91 <Compile Include="Collision\CollisionShapes\StridingMeshInterface.cs" />
92 <Compile Include="Collision\CollisionShapes\SupportVertexCallback.cs" /> 92 <Compile Include="Collision\CollisionShapes\SupportVertexCallback.cs" />
93 <Compile Include="Collision\CollisionShapes\TriangleBuffer.cs" /> 93 <Compile Include="Collision\CollisionShapes\TriangleBuffer.cs" />
94 <Compile Include="Collision\CollisionShapes\TriangleCallback.cs" /> 94 <Compile Include="Collision\CollisionShapes\TriangleCallback.cs" />
95 <Compile Include="Collision\CollisionShapes\TriangleIndexVertexArray.cs" /> 95 <Compile Include="Collision\CollisionShapes\TriangleIndexVertexArray.cs" />
96 <Compile Include="Collision\CollisionShapes\TriangleMesh.cs" /> 96 <Compile Include="Collision\CollisionShapes\TriangleMesh.cs" />
97 <Compile Include="Collision\CollisionShapes\TriangleMeshShape.cs" /> 97 <Compile Include="Collision\CollisionShapes\TriangleMeshShape.cs" />
98 <Compile Include="Collision\CollisionShapes\TriangleShape.cs" /> 98 <Compile Include="Collision\CollisionShapes\TriangleShape.cs" />
99 <Compile Include="Collision\NarrowPhaseCollision\ContinuousConvexCollision.cs" /> 99 <Compile Include="Collision\NarrowPhaseCollision\ContinuousConvexCollision.cs" />
100 <Compile Include="Collision\NarrowPhaseCollision\ConvexCast.cs" /> 100 <Compile Include="Collision\NarrowPhaseCollision\ConvexCast.cs" />
101 <Compile Include="Collision\NarrowPhaseCollision\DiscreteCollisionDetectorInterface.cs" /> 101 <Compile Include="Collision\NarrowPhaseCollision\DiscreteCollisionDetectorInterface.cs" />
102 <Compile Include="Collision\NarrowPhaseCollision\GjkConvexCast.cs" /> 102 <Compile Include="Collision\NarrowPhaseCollision\GjkConvexCast.cs" />
103 <Compile Include="Collision\NarrowPhaseCollision\GjkEpa.cs" /> 103 <Compile Include="Collision\NarrowPhaseCollision\GjkEpa.cs" />
104 <Compile Include="Collision\NarrowPhaseCollision\GjkEpaPenetrationDepthSolver.cs" /> 104 <Compile Include="Collision\NarrowPhaseCollision\GjkEpaPenetrationDepthSolver.cs" />
105 <Compile Include="Collision\NarrowPhaseCollision\GjkEpaSolver.cs" /> 105 <Compile Include="Collision\NarrowPhaseCollision\GjkEpaSolver.cs" />
106 <Compile Include="Collision\NarrowPhaseCollision\GjkPairDetector.cs" /> 106 <Compile Include="Collision\NarrowPhaseCollision\GjkPairDetector.cs" />
107 <Compile Include="Collision\NarrowPhaseCollision\IConvexPenetrationDepthSolver.cs" /> 107 <Compile Include="Collision\NarrowPhaseCollision\IConvexPenetrationDepthSolver.cs" />
108 <Compile Include="Collision\NarrowPhaseCollision\ISimplexSolver.cs" /> 108 <Compile Include="Collision\NarrowPhaseCollision\ISimplexSolver.cs" />
109 <Compile Include="Collision\NarrowPhaseCollision\ManifoldPoint.cs" /> 109 <Compile Include="Collision\NarrowPhaseCollision\ManifoldPoint.cs" />
110 <Compile Include="Collision\NarrowPhaseCollision\MinkowskiPenetrationDepthSolver.cs" /> 110 <Compile Include="Collision\NarrowPhaseCollision\MinkowskiPenetrationDepthSolver.cs" />
111 <Compile Include="Collision\NarrowPhaseCollision\PersistentManifold.cs" /> 111 <Compile Include="Collision\NarrowPhaseCollision\PersistentManifold.cs" />
112 <Compile Include="Collision\NarrowPhaseCollision\PointCollector.cs" /> 112 <Compile Include="Collision\NarrowPhaseCollision\PointCollector.cs" />
113 <Compile Include="Collision\NarrowPhaseCollision\SubsimplexConvexCast.cs" /> 113 <Compile Include="Collision\NarrowPhaseCollision\SubsimplexConvexCast.cs" />
114 <Compile Include="Collision\NarrowPhaseCollision\TriangleRaycastCallback.cs" /> 114 <Compile Include="Collision\NarrowPhaseCollision\TriangleRaycastCallback.cs" />
115 <Compile Include="Collision\NarrowPhaseCollision\VoronoiSimplexSolver.cs" /> 115 <Compile Include="Collision\NarrowPhaseCollision\VoronoiSimplexSolver.cs" />
116 <Compile Include="Dynamics\ConstraintSolver\ContactConstraint.cs" /> 116 <Compile Include="Dynamics\ConstraintSolver\ContactConstraint.cs" />
117 <Compile Include="Dynamics\ConstraintSolver\ContactSolverInfo.cs" /> 117 <Compile Include="Dynamics\ConstraintSolver\ContactSolverInfo.cs" />
118 <Compile Include="Dynamics\ConstraintSolver\Generic6DofConstraint.cs" /> 118 <Compile Include="Dynamics\ConstraintSolver\Generic6DofConstraint.cs" />
119 <Compile Include="Dynamics\ConstraintSolver\HingeConstraint.cs" /> 119 <Compile Include="Dynamics\ConstraintSolver\HingeConstraint.cs" />
120 <Compile Include="Dynamics\ConstraintSolver\IConstraintSolver.cs" /> 120 <Compile Include="Dynamics\ConstraintSolver\IConstraintSolver.cs" />
121 <Compile Include="Dynamics\ConstraintSolver\JacobianEntry.cs" /> 121 <Compile Include="Dynamics\ConstraintSolver\JacobianEntry.cs" />
122 <Compile Include="Dynamics\ConstraintSolver\Point2PointConstraint.cs" /> 122 <Compile Include="Dynamics\ConstraintSolver\Point2PointConstraint.cs" />
123 <Compile Include="Dynamics\ConstraintSolver\SequentialImpulseConstraintSolver.cs" /> 123 <Compile Include="Dynamics\ConstraintSolver\SequentialImpulseConstraintSolver.cs" />
124 <Compile Include="Dynamics\ConstraintSolver\Solve2LinearConstraint.cs" /> 124 <Compile Include="Dynamics\ConstraintSolver\Solve2LinearConstraint.cs" />
125 <Compile Include="Dynamics\ConstraintSolver\SolverBody.cs" /> 125 <Compile Include="Dynamics\ConstraintSolver\SolverBody.cs" />
126 <Compile Include="Dynamics\ConstraintSolver\SolverConstraint.cs" /> 126 <Compile Include="Dynamics\ConstraintSolver\SolverConstraint.cs" />
127 <Compile Include="Dynamics\ConstraintSolver\TypedConstraint.cs" /> 127 <Compile Include="Dynamics\ConstraintSolver\TypedConstraint.cs" />
128 <Compile Include="Dynamics\DiscreteDynamicsWorld.cs" /> 128 <Compile Include="Dynamics\DiscreteDynamicsWorld.cs" />
129 <Compile Include="Dynamics\DynamicsWorld.cs" /> 129 <Compile Include="Dynamics\DynamicsWorld.cs" />
130 <Compile Include="Dynamics\RigidBody.cs" /> 130 <Compile Include="Dynamics\RigidBody.cs" />
131 <Compile Include="Dynamics\SimpleDynamicsWorld.cs" /> 131 <Compile Include="Dynamics\SimpleDynamicsWorld.cs" />
132 <Compile Include="Dynamics\Vehicle\RaycastVehicle.cs" /> 132 <Compile Include="Dynamics\Vehicle\RaycastVehicle.cs" />
133 <Compile Include="Dynamics\Vehicle\VehicleRaycaster.cs" /> 133 <Compile Include="Dynamics\Vehicle\VehicleRaycaster.cs" />
134 <Compile Include="Dynamics\Vehicle\WheelInfo.cs" /> 134 <Compile Include="Dynamics\Vehicle\WheelInfo.cs" />
135 <Compile Include="Exceptions\BulletException.cs" /> 135 <Compile Include="Exceptions\BulletException.cs" />
136 <Compile Include="IDebugDraw.cs" /> 136 <Compile Include="IDebugDraw.cs" />
137 <Compile Include="LinearMath\DefaultMotionState.cs" /> 137 <Compile Include="LinearMath\DefaultMotionState.cs" />
138 <Compile Include="LinearMath\MathHelper.cs" /> 138 <Compile Include="LinearMath\MathHelper.cs" />
139 <Compile Include="LinearMath\MatrixOperations.cs" /> 139 <Compile Include="LinearMath\MatrixOperations.cs" />
140 <Compile Include="LinearMath\MotionState.cs" /> 140 <Compile Include="LinearMath\MotionState.cs" />
141 <Compile Include="LinearMath\QuadWord.cs" /> 141 <Compile Include="LinearMath\QuadWord.cs" />
142 <Compile Include="LinearMath\Quaternion.cs" /> 142 <Compile Include="LinearMath\Quaternion.cs" />
143 <Compile Include="LinearMath\TransformUtil.cs" /> 143 <Compile Include="LinearMath\TransformUtil.cs" />
144 <Compile Include="LinearMath\Vector3.cs" /> 144 <Compile Include="LinearMath\Vector3.cs" />
145 <Compile Include="LinearMath\Vector4.cs" /> 145 <Compile Include="LinearMath\Vector4.cs" />
146 <Compile Include="Properties\AssemblyInfo.cs" /> 146 <Compile Include="Properties\AssemblyInfo.cs" />
147 </ItemGroup> 147 </ItemGroup>
148 <ItemGroup> 148 <ItemGroup>
149 <ProjectReference Include="..\MonoXnaCompactMaths\MonoXnaCompactMaths.csproj"> 149 <ProjectReference Include="..\MonoXnaCompactMaths\MonoXnaCompactMaths.csproj">
150 <Project>{121147BC-B06B-406C-84E9-907F268CF0EB}</Project> 150 <Project>{121147BC-B06B-406C-84E9-907F268CF0EB}</Project>
151 <Name>MonoXnaCompactMaths</Name> 151 <Name>MonoXnaCompactMaths</Name>
152 </ProjectReference> 152 </ProjectReference>
153 </ItemGroup> 153 </ItemGroup>
154 <ItemGroup> 154 <ItemGroup>
155 <None Include="BulletX.snk" /> 155 <None Include="BulletX.snk" />
156 </ItemGroup> 156 </ItemGroup>
157 <ItemGroup> 157 <ItemGroup>
158 <Folder Include="Content\" /> 158 <Folder Include="Content\" />
159 </ItemGroup> 159 </ItemGroup>
160 <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" /> 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. 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. 162 Other similar extension points exist, see Microsoft.Common.targets.
163 <Target Name="BeforeBuild"> 163 <Target Name="BeforeBuild">
164 </Target> 164 </Target>
165 <Target Name="AfterBuild"> 165 <Target Name="AfterBuild">
166 </Target> 166 </Target>
167 --> 167 -->
168</Project> \ No newline at end of file 168</Project> \ No newline at end of file
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Properties/AssemblyInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Properties/AssemblyInfo.cs
index efc8fd6..03d1e3e 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Properties/AssemblyInfo.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Properties/AssemblyInfo.cs
@@ -1,35 +1,35 @@
1using System.Reflection; 1using System.Reflection;
2using System.Runtime.CompilerServices; 2using System.Runtime.CompilerServices;
3using System.Runtime.InteropServices; 3using System.Runtime.InteropServices;
4 4
5// La información general sobre un ensamblado se controla mediante el siguiente 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 6// conjunto de atributos. Cambie estos atributos para modificar la información
7// asociada con un ensamblado. 7// asociada con un ensamblado.
8[assembly: AssemblyTitle("Modified.XnaDevRu.BulletX")] 8[assembly: AssemblyTitle("Modified.XnaDevRu.BulletX")]
9[assembly: AssemblyDescription("Modified Bullet for XNA")] 9[assembly: AssemblyDescription("Modified Bullet for XNA")]
10[assembly: AssemblyConfiguration("")] 10[assembly: AssemblyConfiguration("")]
11[assembly: AssemblyCompany("XNADev.ru")] 11[assembly: AssemblyCompany("XNADev.ru")]
12[assembly: AssemblyProduct("Bullet for XNA")] 12[assembly: AssemblyProduct("Bullet for XNA")]
13[assembly: AssemblyCopyright("Copyright © 2007 XNADev.ru")] 13[assembly: AssemblyCopyright("Copyright © 2007 XNADev.ru")]
14[assembly: AssemblyTrademark("")] 14[assembly: AssemblyTrademark("")]
15[assembly: AssemblyCulture("")] 15[assembly: AssemblyCulture("")]
16 16
17// Si establece ComVisible como false hace que los tipos de este ensamblado no sean visibles 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 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. 19// COM, establezca el atributo ComVisible como true en este tipo.
20[assembly: ComVisible(false)] 20[assembly: ComVisible(false)]
21 21
22// El siguiente GUID sirve como identificador de la biblioteca de tipos si este proyecto se expone a COM 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")] 23[assembly: Guid("a6148224-0271-4cd8-9d0a-5d4955cd2b2c")]
24 24
25// La información de versión de un ensamblado consta de los cuatro valores siguientes: 25// La información de versión de un ensamblado consta de los cuatro valores siguientes:
26// 26//
27// Versión principal 27// Versión principal
28// Versión secundaria 28// Versión secundaria
29// Número de versión de compilación 29// Número de versión de compilación
30// Revisión 30// Revisión
31// 31//
32// Puede especificar todos los valores o puede establecer como valores predeterminados los números de revisión y generación 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: 33// mediante el asterisco ('*'), como se muestra a continuación:
34[assembly: AssemblyVersion("1.0.0.0")] 34[assembly: AssemblyVersion("1.0.0.0")]
35[assembly: AssemblyFileVersion("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
index a104d89..22842ca 100644
--- a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs
+++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs
@@ -1,683 +1,683 @@
1#region License 1#region License
2/* 2/*
3MIT License 3MIT License
4Copyright © 2006 The Mono.Xna Team 4Copyright © 2006 The Mono.Xna Team
5 5
6All rights reserved. 6All rights reserved.
7 7
8Permission is hereby granted, free of charge, to any person obtaining a copy 8Permission is hereby granted, free of charge, to any person obtaining a copy
9of this software and associated documentation files (the "Software"), to deal 9of this software and associated documentation files (the "Software"), to deal
10in the Software without restriction, including without limitation the rights 10in the Software without restriction, including without limitation the rights
11to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12copies of the Software, and to permit persons to whom the Software is 12copies of the Software, and to permit persons to whom the Software is
13furnished to do so, subject to the following conditions: 13furnished to do so, subject to the following conditions:
14 14
15The above copyright notice and this permission notice shall be included in all 15The above copyright notice and this permission notice shall be included in all
16copies or substantial portions of the Software. 16copies or substantial portions of the Software.
17 17
18THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 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 23OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
24SOFTWARE. 24SOFTWARE.
25*/ 25*/
26#endregion License 26#endregion License
27 27
28using System; 28using System;
29using System.ComponentModel; 29using System.ComponentModel;
30using System.Runtime.InteropServices; 30using System.Runtime.InteropServices;
31 31
32namespace MonoXnaCompactMaths 32namespace MonoXnaCompactMaths
33{ 33{
34 [Serializable] 34 [Serializable]
35 [StructLayout(LayoutKind.Sequential)] 35 [StructLayout(LayoutKind.Sequential)]
36 //[TypeConverter(typeof(MatrixConverter))] 36 //[TypeConverter(typeof(MatrixConverter))]
37 public struct Matrix : IEquatable<Matrix> 37 public struct Matrix : IEquatable<Matrix>
38 { 38 {
39 #region Public Constructors 39 #region Public Constructors
40 40
41 public Matrix(float m11, float m12, float m13, float m14, float m21, float m22, float m23, float m24, float m31, 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) 42 float m32, float m33, float m34, float m41, float m42, float m43, float m44)
43 { 43 {
44 this.M11 = m11; 44 this.M11 = m11;
45 this.M12 = m12; 45 this.M12 = m12;
46 this.M13 = m13; 46 this.M13 = m13;
47 this.M14 = m14; 47 this.M14 = m14;
48 this.M21 = m21; 48 this.M21 = m21;
49 this.M22 = m22; 49 this.M22 = m22;
50 this.M23 = m23; 50 this.M23 = m23;
51 this.M24 = m24; 51 this.M24 = m24;
52 this.M31 = m31; 52 this.M31 = m31;
53 this.M32 = m32; 53 this.M32 = m32;
54 this.M33 = m33; 54 this.M33 = m33;
55 this.M34 = m34; 55 this.M34 = m34;
56 this.M41 = m41; 56 this.M41 = m41;
57 this.M42 = m42; 57 this.M42 = m42;
58 this.M43 = m43; 58 this.M43 = m43;
59 this.M44 = m44; 59 this.M44 = m44;
60 } 60 }
61 61
62 #endregion Public Constructors 62 #endregion Public Constructors
63 63
64 64
65 #region Public Fields 65 #region Public Fields
66 66
67 public float M11; 67 public float M11;
68 public float M12; 68 public float M12;
69 public float M13; 69 public float M13;
70 public float M14; 70 public float M14;
71 public float M21; 71 public float M21;
72 public float M22; 72 public float M22;
73 public float M23; 73 public float M23;
74 public float M24; 74 public float M24;
75 public float M31; 75 public float M31;
76 public float M32; 76 public float M32;
77 public float M33; 77 public float M33;
78 public float M34; 78 public float M34;
79 public float M41; 79 public float M41;
80 public float M42; 80 public float M42;
81 public float M43; 81 public float M43;
82 public float M44; 82 public float M44;
83 83
84 #endregion Public Fields 84 #endregion Public Fields
85 85
86 86
87 #region Private Members 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); 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 89 #endregion Private Members
90 90
91 91
92 #region Public Properties 92 #region Public Properties
93 93
94 public Vector3 Backward 94 public Vector3 Backward
95 { 95 {
96 get 96 get
97 { 97 {
98 return new Vector3(this.M31, this.M32, this.M33); 98 return new Vector3(this.M31, this.M32, this.M33);
99 } 99 }
100 set 100 set
101 { 101 {
102 this.M31 = value.X; 102 this.M31 = value.X;
103 this.M32 = value.Y; 103 this.M32 = value.Y;
104 this.M33 = value.Z; 104 this.M33 = value.Z;
105 } 105 }
106 } 106 }
107 107
108 108
109 public Vector3 Down 109 public Vector3 Down
110 { 110 {
111 get 111 get
112 { 112 {
113 return new Vector3(-this.M21, -this.M22, -this.M23); 113 return new Vector3(-this.M21, -this.M22, -this.M23);
114 } 114 }
115 set 115 set
116 { 116 {
117 this.M21 = -value.X; 117 this.M21 = -value.X;
118 this.M22 = -value.Y; 118 this.M22 = -value.Y;
119 this.M23 = -value.Z; 119 this.M23 = -value.Z;
120 } 120 }
121 } 121 }
122 122
123 123
124 public Vector3 Forward 124 public Vector3 Forward
125 { 125 {
126 get 126 get
127 { 127 {
128 return new Vector3(-this.M31, -this.M32, -this.M33); 128 return new Vector3(-this.M31, -this.M32, -this.M33);
129 } 129 }
130 set 130 set
131 { 131 {
132 this.M31 = -value.X; 132 this.M31 = -value.X;
133 this.M32 = -value.Y; 133 this.M32 = -value.Y;
134 this.M33 = -value.Z; 134 this.M33 = -value.Z;
135 } 135 }
136 } 136 }
137 137
138 138
139 public static Matrix Identity 139 public static Matrix Identity
140 { 140 {
141 get { return identity; } 141 get { return identity; }
142 } 142 }
143 143
144 144
145 public Vector3 Left 145 public Vector3 Left
146 { 146 {
147 get 147 get
148 { 148 {
149 return new Vector3(-this.M11, -this.M12, -this.M13); 149 return new Vector3(-this.M11, -this.M12, -this.M13);
150 } 150 }
151 set 151 set
152 { 152 {
153 this.M11 = -value.X; 153 this.M11 = -value.X;
154 this.M12 = -value.Y; 154 this.M12 = -value.Y;
155 this.M13 = -value.Z; 155 this.M13 = -value.Z;
156 } 156 }
157 } 157 }
158 158
159 159
160 public Vector3 Right 160 public Vector3 Right
161 { 161 {
162 get 162 get
163 { 163 {
164 return new Vector3(this.M11, this.M12, this.M13); 164 return new Vector3(this.M11, this.M12, this.M13);
165 } 165 }
166 set 166 set
167 { 167 {
168 this.M11 = value.X; 168 this.M11 = value.X;
169 this.M12 = value.Y; 169 this.M12 = value.Y;
170 this.M13 = value.Z; 170 this.M13 = value.Z;
171 } 171 }
172 } 172 }
173 173
174 174
175 public Vector3 Translation 175 public Vector3 Translation
176 { 176 {
177 get 177 get
178 { 178 {
179 return new Vector3(this.M41, this.M42, this.M43); 179 return new Vector3(this.M41, this.M42, this.M43);
180 } 180 }
181 set 181 set
182 { 182 {
183 this.M41 = value.X; 183 this.M41 = value.X;
184 this.M42 = value.Y; 184 this.M42 = value.Y;
185 this.M43 = value.Z; 185 this.M43 = value.Z;
186 } 186 }
187 } 187 }
188 188
189 189
190 public Vector3 Up 190 public Vector3 Up
191 { 191 {
192 get 192 get
193 { 193 {
194 return new Vector3(this.M21, this.M22, this.M23); 194 return new Vector3(this.M21, this.M22, this.M23);
195 } 195 }
196 set 196 set
197 { 197 {
198 this.M21 = value.X; 198 this.M21 = value.X;
199 this.M22 = value.Y; 199 this.M22 = value.Y;
200 this.M23 = value.Z; 200 this.M23 = value.Z;
201 } 201 }
202 } 202 }
203 #endregion Public Properties 203 #endregion Public Properties
204 204
205 205
206 #region Public Methods 206 #region Public Methods
207 207
208 public static Matrix Add(Matrix matrix1, Matrix matrix2) 208 public static Matrix Add(Matrix matrix1, Matrix matrix2)
209 { 209 {
210 throw new NotImplementedException(); 210 throw new NotImplementedException();
211 } 211 }
212 212
213 213
214 public static void Add(ref Matrix matrix1, ref Matrix matrix2, out Matrix result) 214 public static void Add(ref Matrix matrix1, ref Matrix matrix2, out Matrix result)
215 { 215 {
216 throw new NotImplementedException(); 216 throw new NotImplementedException();
217 } 217 }
218 218
219 219
220 public static Matrix CreateBillboard(Vector3 objectPosition, Vector3 cameraPosition, 220 public static Matrix CreateBillboard(Vector3 objectPosition, Vector3 cameraPosition,
221 Vector3 cameraUpVector, Nullable<Vector3> cameraForwardVector) 221 Vector3 cameraUpVector, Nullable<Vector3> cameraForwardVector)
222 { 222 {
223 throw new NotImplementedException(); 223 throw new NotImplementedException();
224 } 224 }
225 225
226 226
227 public static void CreateBillboard(ref Vector3 objectPosition, ref Vector3 cameraPosition, 227 public static void CreateBillboard(ref Vector3 objectPosition, ref Vector3 cameraPosition,
228 ref Vector3 cameraUpVector, Vector3? cameraForwardVector, out Matrix result) 228 ref Vector3 cameraUpVector, Vector3? cameraForwardVector, out Matrix result)
229 { 229 {
230 throw new NotImplementedException(); 230 throw new NotImplementedException();
231 } 231 }
232 232
233 233
234 public static Matrix CreateConstrainedBillboard(Vector3 objectPosition, Vector3 cameraPosition, 234 public static Matrix CreateConstrainedBillboard(Vector3 objectPosition, Vector3 cameraPosition,
235 Vector3 rotateAxis, Nullable<Vector3> cameraForwardVector, Nullable<Vector3> objectForwardVector) 235 Vector3 rotateAxis, Nullable<Vector3> cameraForwardVector, Nullable<Vector3> objectForwardVector)
236 { 236 {
237 throw new NotImplementedException(); 237 throw new NotImplementedException();
238 } 238 }
239 239
240 240
241 public static void CreateConstrainedBillboard(ref Vector3 objectPosition, ref Vector3 cameraPosition, 241 public static void CreateConstrainedBillboard(ref Vector3 objectPosition, ref Vector3 cameraPosition,
242 ref Vector3 rotateAxis, Vector3? cameraForwardVector, Vector3? objectForwardVector, out Matrix result) 242 ref Vector3 rotateAxis, Vector3? cameraForwardVector, Vector3? objectForwardVector, out Matrix result)
243 { 243 {
244 throw new NotImplementedException(); 244 throw new NotImplementedException();
245 } 245 }
246 246
247 247
248 public static Matrix CreateFromAxisAngle(Vector3 axis, float angle) 248 public static Matrix CreateFromAxisAngle(Vector3 axis, float angle)
249 { 249 {
250 throw new NotImplementedException(); 250 throw new NotImplementedException();
251 } 251 }
252 252
253 253
254 public static void CreateFromAxisAngle(ref Vector3 axis, float angle, out Matrix result) 254 public static void CreateFromAxisAngle(ref Vector3 axis, float angle, out Matrix result)
255 { 255 {
256 throw new NotImplementedException(); 256 throw new NotImplementedException();
257 } 257 }
258 258
259 259
260 public static Matrix CreateFromQuaternion(Quaternion quaternion) 260 public static Matrix CreateFromQuaternion(Quaternion quaternion)
261 { 261 {
262 //--- 262 //---
263 //http://lists.ximian.com/pipermail/mono-patches/2006-December/084667.html 263 //http://lists.ximian.com/pipermail/mono-patches/2006-December/084667.html
264 float xx = quaternion.X * quaternion.X; 264 float xx = quaternion.X * quaternion.X;
265 float xy = quaternion.X * quaternion.Y; 265 float xy = quaternion.X * quaternion.Y;
266 float xw = quaternion.X * quaternion.W; 266 float xw = quaternion.X * quaternion.W;
267 float yy = quaternion.Y * quaternion.Y; 267 float yy = quaternion.Y * quaternion.Y;
268 float yw = quaternion.Y * quaternion.W; 268 float yw = quaternion.Y * quaternion.W;
269 float yz = quaternion.Y * quaternion.Z; 269 float yz = quaternion.Y * quaternion.Z;
270 float zx = quaternion.Z * quaternion.X; 270 float zx = quaternion.Z * quaternion.X;
271 float zw = quaternion.Z * quaternion.W; 271 float zw = quaternion.Z * quaternion.W;
272 float zz = quaternion.Z * quaternion.Z; 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), 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), 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); 275 1f - (2f * (yy + xx)), 0f, 0f, 0f, 0f, 1f);
276 //throw new NotImplementedException(); 276 //throw new NotImplementedException();
277 } 277 }
278 278
279 279
280 public static void CreateFromQuaternion(ref Quaternion quaternion, out Matrix result) 280 public static void CreateFromQuaternion(ref Quaternion quaternion, out Matrix result)
281 { 281 {
282 throw new NotImplementedException(); 282 throw new NotImplementedException();
283 } 283 }
284 284
285 285
286 public static Matrix CreateLookAt(Vector3 cameraPosition, Vector3 cameraTarget, Vector3 cameraUpVector) 286 public static Matrix CreateLookAt(Vector3 cameraPosition, Vector3 cameraTarget, Vector3 cameraUpVector)
287 { 287 {
288 throw new NotImplementedException(); 288 throw new NotImplementedException();
289 } 289 }
290 290
291 291
292 public static void CreateLookAt(ref Vector3 cameraPosition, ref Vector3 cameraTarget, ref Vector3 cameraUpVector, out Matrix result) 292 public static void CreateLookAt(ref Vector3 cameraPosition, ref Vector3 cameraTarget, ref Vector3 cameraUpVector, out Matrix result)
293 { 293 {
294 throw new NotImplementedException(); 294 throw new NotImplementedException();
295 } 295 }
296 296
297 297
298 public static Matrix CreateOrthographic(float width, float height, float zNearPlane, float zFarPlane) 298 public static Matrix CreateOrthographic(float width, float height, float zNearPlane, float zFarPlane)
299 { 299 {
300 throw new NotImplementedException(); 300 throw new NotImplementedException();
301 } 301 }
302 302
303 303
304 public static void CreateOrthographic(float width, float height, float zNearPlane, float zFarPlane, out Matrix result) 304 public static void CreateOrthographic(float width, float height, float zNearPlane, float zFarPlane, out Matrix result)
305 { 305 {
306 throw new NotImplementedException(); 306 throw new NotImplementedException();
307 } 307 }
308 308
309 309
310 public static Matrix CreateOrthographicOffCenter(float left, float right, float bottom, float top, float zNearPlane, float zFarPlane) 310 public static Matrix CreateOrthographicOffCenter(float left, float right, float bottom, float top, float zNearPlane, float zFarPlane)
311 { 311 {
312 throw new NotImplementedException(); 312 throw new NotImplementedException();
313 } 313 }
314 314
315 315
316 public static void CreateOrthographicOffCenter(float left, float right, float bottom, float top, 316 public static void CreateOrthographicOffCenter(float left, float right, float bottom, float top,
317 float zNearPlane, float zFarPlane, out Matrix result) 317 float zNearPlane, float zFarPlane, out Matrix result)
318 { 318 {
319 throw new NotImplementedException(); 319 throw new NotImplementedException();
320 } 320 }
321 321
322 322
323 public static Matrix CreatePerspective(float width, float height, float zNearPlane, float zFarPlane) 323 public static Matrix CreatePerspective(float width, float height, float zNearPlane, float zFarPlane)
324 { 324 {
325 throw new NotImplementedException(); 325 throw new NotImplementedException();
326 } 326 }
327 327
328 328
329 public static void CreatePerspective(float width, float height, float zNearPlane, float zFarPlane, out Matrix result) 329 public static void CreatePerspective(float width, float height, float zNearPlane, float zFarPlane, out Matrix result)
330 { 330 {
331 throw new NotImplementedException(); 331 throw new NotImplementedException();
332 } 332 }
333 333
334 334
335 public static Matrix CreatePerspectiveFieldOfView(float fieldOfView, float aspectRatio, float zNearPlane, float zFarPlane) 335 public static Matrix CreatePerspectiveFieldOfView(float fieldOfView, float aspectRatio, float zNearPlane, float zFarPlane)
336 { 336 {
337 throw new NotImplementedException(); 337 throw new NotImplementedException();
338 } 338 }
339 339
340 340
341 public static void CreatePerspectiveFieldOfView(float fieldOfView, float aspectRatio, float nearPlaneDistance, float farPlaneDistance, out Matrix result) 341 public static void CreatePerspectiveFieldOfView(float fieldOfView, float aspectRatio, float nearPlaneDistance, float farPlaneDistance, out Matrix result)
342 { 342 {
343 throw new NotImplementedException(); 343 throw new NotImplementedException();
344 } 344 }
345 345
346 346
347 public static Matrix CreatePerspectiveOffCenter(float left, float right, float bottom, float top, float zNearPlane, float zFarPlane) 347 public static Matrix CreatePerspectiveOffCenter(float left, float right, float bottom, float top, float zNearPlane, float zFarPlane)
348 { 348 {
349 throw new NotImplementedException(); 349 throw new NotImplementedException();
350 } 350 }
351 351
352 352
353 public static void CreatePerspectiveOffCenter(float left, float right, float bottom, float top, float nearPlaneDistance, float farPlaneDistance, out Matrix result) 353 public static void CreatePerspectiveOffCenter(float left, float right, float bottom, float top, float nearPlaneDistance, float farPlaneDistance, out Matrix result)
354 { 354 {
355 throw new NotImplementedException(); 355 throw new NotImplementedException();
356 } 356 }
357 357
358 358
359 public static Matrix CreateRotationX(float radians) 359 public static Matrix CreateRotationX(float radians)
360 { 360 {
361 throw new NotImplementedException(); 361 throw new NotImplementedException();
362 } 362 }
363 363
364 364
365 public static void CreateRotationX(float radians, out Matrix result) 365 public static void CreateRotationX(float radians, out Matrix result)
366 { 366 {
367 throw new NotImplementedException(); 367 throw new NotImplementedException();
368 } 368 }
369 369
370 370
371 public static Matrix CreateRotationY(float radians) 371 public static Matrix CreateRotationY(float radians)
372 { 372 {
373 throw new NotImplementedException(); 373 throw new NotImplementedException();
374 } 374 }
375 375
376 376
377 public static void CreateRotationY(float radians, out Matrix result) 377 public static void CreateRotationY(float radians, out Matrix result)
378 { 378 {
379 throw new NotImplementedException(); 379 throw new NotImplementedException();
380 } 380 }
381 381
382 382
383 public static Matrix CreateRotationZ(float radians) 383 public static Matrix CreateRotationZ(float radians)
384 { 384 {
385 throw new NotImplementedException(); 385 throw new NotImplementedException();
386 } 386 }
387 387
388 388
389 public static void CreateRotationZ(float radians, out Matrix result) 389 public static void CreateRotationZ(float radians, out Matrix result)
390 { 390 {
391 throw new NotImplementedException(); 391 throw new NotImplementedException();
392 } 392 }
393 393
394 394
395 public static Matrix CreateScale(float scale) 395 public static Matrix CreateScale(float scale)
396 { 396 {
397 throw new NotImplementedException(); 397 throw new NotImplementedException();
398 } 398 }
399 399
400 400
401 public static void CreateScale(float scale, out Matrix result) 401 public static void CreateScale(float scale, out Matrix result)
402 { 402 {
403 throw new NotImplementedException(); 403 throw new NotImplementedException();
404 } 404 }
405 405
406 406
407 public static Matrix CreateScale(float xScale, float yScale, float zScale) 407 public static Matrix CreateScale(float xScale, float yScale, float zScale)
408 { 408 {
409 throw new NotImplementedException(); 409 throw new NotImplementedException();
410 } 410 }
411 411
412 412
413 public static void CreateScale(float xScale, float yScale, float zScale, out Matrix result) 413 public static void CreateScale(float xScale, float yScale, float zScale, out Matrix result)
414 { 414 {
415 throw new NotImplementedException(); 415 throw new NotImplementedException();
416 } 416 }
417 417
418 418
419 public static Matrix CreateScale(Vector3 scales) 419 public static Matrix CreateScale(Vector3 scales)
420 { 420 {
421 throw new NotImplementedException(); 421 throw new NotImplementedException();
422 } 422 }
423 423
424 424
425 public static void CreateScale(ref Vector3 scales, out Matrix result) 425 public static void CreateScale(ref Vector3 scales, out Matrix result)
426 { 426 {
427 throw new NotImplementedException(); 427 throw new NotImplementedException();
428 } 428 }
429 429
430 430
431 public static Matrix CreateTranslation(float xPosition, float yPosition, float zPosition) 431 public static Matrix CreateTranslation(float xPosition, float yPosition, float zPosition)
432 { 432 {
433 throw new NotImplementedException(); 433 throw new NotImplementedException();
434 } 434 }
435 435
436 436
437 public static void CreateTranslation(ref Vector3 position, out Matrix result) 437 public static void CreateTranslation(ref Vector3 position, out Matrix result)
438 { 438 {
439 throw new NotImplementedException(); 439 throw new NotImplementedException();
440 } 440 }
441 441
442 442
443 public static Matrix CreateTranslation(Vector3 position) 443 public static Matrix CreateTranslation(Vector3 position)
444 { 444 {
445 throw new NotImplementedException(); 445 throw new NotImplementedException();
446 } 446 }
447 447
448 448
449 public static void CreateTranslation(float xPosition, float yPosition, float zPosition, out Matrix result) 449 public static void CreateTranslation(float xPosition, float yPosition, float zPosition, out Matrix result)
450 { 450 {
451 throw new NotImplementedException(); 451 throw new NotImplementedException();
452 } 452 }
453 453
454 454
455 public float Determinant() 455 public float Determinant()
456 { 456 {
457 throw new NotImplementedException(); 457 throw new NotImplementedException();
458 } 458 }
459 459
460 460
461 public static Matrix Divide(Matrix matrix1, Matrix matrix2) 461 public static Matrix Divide(Matrix matrix1, Matrix matrix2)
462 { 462 {
463 throw new NotImplementedException(); 463 throw new NotImplementedException();
464 } 464 }
465 465
466 466
467 public static void Divide(ref Matrix matrix1, ref Matrix matrix2, out Matrix result) 467 public static void Divide(ref Matrix matrix1, ref Matrix matrix2, out Matrix result)
468 { 468 {
469 throw new NotImplementedException(); 469 throw new NotImplementedException();
470 } 470 }
471 471
472 472
473 public static Matrix Divide(Matrix matrix1, float divider) 473 public static Matrix Divide(Matrix matrix1, float divider)
474 { 474 {
475 throw new NotImplementedException(); 475 throw new NotImplementedException();
476 } 476 }
477 477
478 478
479 public static void Divide(ref Matrix matrix1, float divider, out Matrix result) 479 public static void Divide(ref Matrix matrix1, float divider, out Matrix result)
480 { 480 {
481 throw new NotImplementedException(); 481 throw new NotImplementedException();
482 } 482 }
483 483
484 484
485 public bool Equals(Matrix other) 485 public bool Equals(Matrix other)
486 { 486 {
487 throw new NotImplementedException(); 487 throw new NotImplementedException();
488 } 488 }
489 489
490 490
491 public override bool Equals(object obj) 491 public override bool Equals(object obj)
492 { 492 {
493 throw new NotImplementedException(); 493 throw new NotImplementedException();
494 } 494 }
495 495
496 496
497 public override int GetHashCode() 497 public override int GetHashCode()
498 { 498 {
499 throw new NotImplementedException(); 499 throw new NotImplementedException();
500 } 500 }
501 501
502 502
503 public static Matrix Invert(Matrix matrix) 503 public static Matrix Invert(Matrix matrix)
504 { 504 {
505 throw new NotImplementedException(); 505 throw new NotImplementedException();
506 } 506 }
507 507
508 508
509 public static void Invert(ref Matrix matrix, out Matrix result) 509 public static void Invert(ref Matrix matrix, out Matrix result)
510 { 510 {
511 throw new NotImplementedException(); 511 throw new NotImplementedException();
512 } 512 }
513 513
514 514
515 public static Matrix Lerp(Matrix matrix1, Matrix matrix2, float amount) 515 public static Matrix Lerp(Matrix matrix1, Matrix matrix2, float amount)
516 { 516 {
517 throw new NotImplementedException(); 517 throw new NotImplementedException();
518 } 518 }
519 519
520 520
521 public static void Lerp(ref Matrix matrix1, ref Matrix matrix2, float amount, out Matrix result) 521 public static void Lerp(ref Matrix matrix1, ref Matrix matrix2, float amount, out Matrix result)
522 { 522 {
523 throw new NotImplementedException(); 523 throw new NotImplementedException();
524 } 524 }
525 525
526 public static Matrix Multiply(Matrix matrix1, Matrix matrix2) 526 public static Matrix Multiply(Matrix matrix1, Matrix matrix2)
527 { 527 {
528 throw new NotImplementedException(); 528 throw new NotImplementedException();
529 } 529 }
530 530
531 531
532 public static void Multiply(ref Matrix matrix1, ref Matrix matrix2, out Matrix result) 532 public static void Multiply(ref Matrix matrix1, ref Matrix matrix2, out Matrix result)
533 { 533 {
534 throw new NotImplementedException(); 534 throw new NotImplementedException();
535 } 535 }
536 536
537 537
538 public static Matrix Multiply(Matrix matrix1, float factor) 538 public static Matrix Multiply(Matrix matrix1, float factor)
539 { 539 {
540 throw new NotImplementedException(); 540 throw new NotImplementedException();
541 } 541 }
542 542
543 543
544 public static void Multiply(ref Matrix matrix1, float factor, out Matrix result) 544 public static void Multiply(ref Matrix matrix1, float factor, out Matrix result)
545 { 545 {
546 throw new NotImplementedException(); 546 throw new NotImplementedException();
547 } 547 }
548 548
549 549
550 public static Matrix Negate(Matrix matrix) 550 public static Matrix Negate(Matrix matrix)
551 { 551 {
552 throw new NotImplementedException(); 552 throw new NotImplementedException();
553 } 553 }
554 554
555 555
556 public static void Negate(ref Matrix matrix, out Matrix result) 556 public static void Negate(ref Matrix matrix, out Matrix result)
557 { 557 {
558 throw new NotImplementedException(); 558 throw new NotImplementedException();
559 } 559 }
560 560
561 561
562 public static Matrix operator +(Matrix matrix1, Matrix matrix2) 562 public static Matrix operator +(Matrix matrix1, Matrix matrix2)
563 { 563 {
564 throw new NotImplementedException(); 564 throw new NotImplementedException();
565 } 565 }
566 566
567 567
568 public static Matrix operator /(Matrix matrix1, Matrix matrix2) 568 public static Matrix operator /(Matrix matrix1, Matrix matrix2)
569 { 569 {
570 throw new NotImplementedException(); 570 throw new NotImplementedException();
571 } 571 }
572 572
573 573
574 public static Matrix operator /(Matrix matrix1, float divider) 574 public static Matrix operator /(Matrix matrix1, float divider)
575 { 575 {
576 throw new NotImplementedException(); 576 throw new NotImplementedException();
577 } 577 }
578 578
579 579
580 public static bool operator ==(Matrix matrix1, Matrix matrix2) 580 public static bool operator ==(Matrix matrix1, Matrix matrix2)
581 { 581 {
582 throw new NotImplementedException(); 582 throw new NotImplementedException();
583 } 583 }
584 584
585 585
586 public static bool operator !=(Matrix matrix1, Matrix matrix2) 586 public static bool operator !=(Matrix matrix1, Matrix matrix2)
587 { 587 {
588 throw new NotImplementedException(); 588 throw new NotImplementedException();
589 } 589 }
590 590
591 591
592 public static Matrix operator *(Matrix matrix1, Matrix matrix2) 592 public static Matrix operator *(Matrix matrix1, Matrix matrix2)
593 { 593 {
594 //--- 594 //---
595 float[, ] arrayMatrix1 = new float[4, 4]; 595 float[, ] arrayMatrix1 = new float[4, 4];
596 float[, ] arrayMatrix2 = new float[4, 4]; 596 float[, ] arrayMatrix2 = new float[4, 4];
597 float[, ] arrayMatrixProduct = 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; 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; 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; 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; 601 arrayMatrix1[3, 0] = matrix1.M41; arrayMatrix1[3, 1] = matrix1.M42; arrayMatrix1[3, 2] = matrix1.M43; arrayMatrix1[3, 3] = matrix1.M44;
602 602
603 arrayMatrix2[0, 0] = matrix2.M11; arrayMatrix2[0, 1] = matrix2.M12; arrayMatrix2[0, 2] = matrix2.M13; arrayMatrix2[0, 3] = matrix2.M14; 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; 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; 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; 606 arrayMatrix2[3, 0] = matrix2.M41; arrayMatrix2[3, 1] = matrix2.M42; arrayMatrix2[3, 2] = matrix2.M43; arrayMatrix2[3, 3] = matrix2.M44;
607 607
608 int n = 4; 608 int n = 4;
609 for (int i = 0; i < n; i++) 609 for (int i = 0; i < n; i++)
610 { 610 {
611 for (int j = 0; j < n; j++) 611 for (int j = 0; j < n; j++)
612 { 612 {
613 // (AB)[i,j] = Sum(k=0; k < 4; k++) { A[i,k] * B[k, j] } 613 // (AB)[i,j] = Sum(k=0; k < 4; k++) { A[i,k] * B[k, j] }
614 for (int k = 0; k < n; k++) 614 for (int k = 0; k < n; k++)
615 { 615 {
616 arrayMatrixProduct[i, j] += arrayMatrix1[i, k] * arrayMatrix2[k, j]; 616 arrayMatrixProduct[i, j] += arrayMatrix1[i, k] * arrayMatrix2[k, j];
617 } 617 }
618 } 618 }
619 } 619 }
620 return new Matrix( arrayMatrixProduct[0, 0], arrayMatrixProduct[0, 1], arrayMatrixProduct[0, 2], arrayMatrixProduct[0, 3], 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], 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], 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]); 623 arrayMatrixProduct[3, 1], arrayMatrixProduct[3, 1], arrayMatrixProduct[3, 2], arrayMatrixProduct[3, 3]);
624 //--- 624 //---
625 //throw new NotImplementedException(); 625 //throw new NotImplementedException();
626 } 626 }
627 627
628 628
629 public static Matrix operator *(Matrix matrix, float scaleFactor) 629 public static Matrix operator *(Matrix matrix, float scaleFactor)
630 { 630 {
631 throw new NotImplementedException(); 631 throw new NotImplementedException();
632 } 632 }
633 633
634 634
635 public static Matrix operator -(Matrix matrix1, Matrix matrix2) 635 public static Matrix operator -(Matrix matrix1, Matrix matrix2)
636 { 636 {
637 throw new NotImplementedException(); 637 throw new NotImplementedException();
638 } 638 }
639 639
640 640
641 public static Matrix operator -(Matrix matrix1) 641 public static Matrix operator -(Matrix matrix1)
642 { 642 {
643 throw new NotImplementedException(); 643 throw new NotImplementedException();
644 } 644 }
645 645
646 646
647 public static Matrix Subtract(Matrix matrix1, Matrix matrix2) 647 public static Matrix Subtract(Matrix matrix1, Matrix matrix2)
648 { 648 {
649 throw new NotImplementedException(); 649 throw new NotImplementedException();
650 } 650 }
651 651
652 652
653 public static void Subtract(ref Matrix matrix1, ref Matrix matrix2, out Matrix result) 653 public static void Subtract(ref Matrix matrix1, ref Matrix matrix2, out Matrix result)
654 { 654 {
655 throw new NotImplementedException(); 655 throw new NotImplementedException();
656 } 656 }
657 657
658 658
659 public override string ToString() 659 public override string ToString()
660 { 660 {
661 throw new NotImplementedException(); 661 throw new NotImplementedException();
662 } 662 }
663 663
664 664
665 public static Matrix Transpose(Matrix matrix) 665 public static Matrix Transpose(Matrix matrix)
666 { 666 {
667 //--- 667 //---
668 return new Matrix( matrix.M11, matrix.M21, matrix.M31, matrix.M41, 668 return new Matrix( matrix.M11, matrix.M21, matrix.M31, matrix.M41,
669 matrix.M12, matrix.M22, matrix.M32, matrix.M42, 669 matrix.M12, matrix.M22, matrix.M32, matrix.M42,
670 matrix.M13, matrix.M23, matrix.M33, matrix.M43, 670 matrix.M13, matrix.M23, matrix.M33, matrix.M43,
671 matrix.M14, matrix.M24, matrix.M34, matrix.M44); 671 matrix.M14, matrix.M24, matrix.M34, matrix.M44);
672 //--- 672 //---
673 //throw new NotImplementedException(); 673 //throw new NotImplementedException();
674 } 674 }
675 675
676 676
677 public static void Transpose(ref Matrix matrix, out Matrix result) 677 public static void Transpose(ref Matrix matrix, out Matrix result)
678 { 678 {
679 throw new NotImplementedException(); 679 throw new NotImplementedException();
680 } 680 }
681 #endregion Public Methods 681 #endregion Public Methods
682 } 682 }
683} 683}
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/MonoXnaCompactMaths.csproj b/libraries/ModifiedBulletX/MonoXnaCompactMaths/MonoXnaCompactMaths.csproj
index c3ec4dd..79c5263 100644
--- a/libraries/ModifiedBulletX/MonoXnaCompactMaths/MonoXnaCompactMaths.csproj
+++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/MonoXnaCompactMaths.csproj
@@ -1,51 +1,51 @@
1<?xml version="1.0" encoding="utf-8"?> 1<?xml version="1.0" encoding="utf-8"?>
2<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003"> 2<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
3 <PropertyGroup> 3 <PropertyGroup>
4 <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration> 4 <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
5 <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform> 5 <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
6 <ProductVersion>8.0.50727</ProductVersion> 6 <ProductVersion>8.0.50727</ProductVersion>
7 <SchemaVersion>2.0</SchemaVersion> 7 <SchemaVersion>2.0</SchemaVersion>
8 <ProjectGuid>{121147BC-B06B-406C-84E9-907F268CF0EB}</ProjectGuid> 8 <ProjectGuid>{121147BC-B06B-406C-84E9-907F268CF0EB}</ProjectGuid>
9 <OutputType>Library</OutputType> 9 <OutputType>Library</OutputType>
10 <AppDesignerFolder>Properties</AppDesignerFolder> 10 <AppDesignerFolder>Properties</AppDesignerFolder>
11 <RootNamespace>MonoXnaCompactMaths</RootNamespace> 11 <RootNamespace>MonoXnaCompactMaths</RootNamespace>
12 <AssemblyName>MonoXnaCompactMaths</AssemblyName> 12 <AssemblyName>MonoXnaCompactMaths</AssemblyName>
13 <StartupObject> 13 <StartupObject>
14 </StartupObject> 14 </StartupObject>
15 </PropertyGroup> 15 </PropertyGroup>
16 <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' "> 16 <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
17 <DebugSymbols>true</DebugSymbols> 17 <DebugSymbols>true</DebugSymbols>
18 <DebugType>full</DebugType> 18 <DebugType>full</DebugType>
19 <Optimize>false</Optimize> 19 <Optimize>false</Optimize>
20 <OutputPath>bin\Debug\</OutputPath> 20 <OutputPath>bin\Debug\</OutputPath>
21 <DefineConstants>DEBUG;TRACE</DefineConstants> 21 <DefineConstants>DEBUG;TRACE</DefineConstants>
22 <ErrorReport>prompt</ErrorReport> 22 <ErrorReport>prompt</ErrorReport>
23 <WarningLevel>4</WarningLevel> 23 <WarningLevel>4</WarningLevel>
24 </PropertyGroup> 24 </PropertyGroup>
25 <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' "> 25 <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
26 <DebugType>pdbonly</DebugType> 26 <DebugType>pdbonly</DebugType>
27 <Optimize>true</Optimize> 27 <Optimize>true</Optimize>
28 <OutputPath>bin\Release\</OutputPath> 28 <OutputPath>bin\Release\</OutputPath>
29 <DefineConstants>TRACE</DefineConstants> 29 <DefineConstants>TRACE</DefineConstants>
30 <ErrorReport>prompt</ErrorReport> 30 <ErrorReport>prompt</ErrorReport>
31 <WarningLevel>4</WarningLevel> 31 <WarningLevel>4</WarningLevel>
32 </PropertyGroup> 32 </PropertyGroup>
33 <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" /> 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. 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. 35 Other similar extension points exist, see Microsoft.Common.targets.
36 <Target Name="BeforeBuild"> 36 <Target Name="BeforeBuild">
37 </Target> 37 </Target>
38 <Target Name="AfterBuild"> 38 <Target Name="AfterBuild">
39 </Target> 39 </Target>
40 --> 40 -->
41 <ItemGroup> 41 <ItemGroup>
42 <Compile Include="Matrix.cs" /> 42 <Compile Include="Matrix.cs" />
43 <Compile Include="Properties\AssemblyInfo.cs" /> 43 <Compile Include="Properties\AssemblyInfo.cs" />
44 <Compile Include="Quaternion.cs" /> 44 <Compile Include="Quaternion.cs" />
45 <Compile Include="Vector3.cs" /> 45 <Compile Include="Vector3.cs" />
46 <Compile Include="Vector4.cs" /> 46 <Compile Include="Vector4.cs" />
47 </ItemGroup> 47 </ItemGroup>
48 <ItemGroup> 48 <ItemGroup>
49 <Reference Include="System" /> 49 <Reference Include="System" />
50 </ItemGroup> 50 </ItemGroup>
51</Project> \ No newline at end of file 51</Project> \ No newline at end of file
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Properties/AssemblyInfo.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Properties/AssemblyInfo.cs
index 748b94a..366f664 100644
--- a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Properties/AssemblyInfo.cs
+++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Properties/AssemblyInfo.cs
@@ -1,33 +1,33 @@
1using System.Reflection; 1using System.Reflection;
2using System.Runtime.CompilerServices; 2using System.Runtime.CompilerServices;
3using System.Runtime.InteropServices; 3using System.Runtime.InteropServices;
4 4
5// La información general sobre un ensamblado se controla mediante el siguiente 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 6// conjunto de atributos. Cambie estos atributos para modificar la información
7// asociada con un ensamblado. 7// asociada con un ensamblado.
8[assembly: AssemblyTitle("MonoXnaCompactMaths")] 8[assembly: AssemblyTitle("MonoXnaCompactMaths")]
9[assembly: AssemblyDescription("MonoXnaCompactMaths (Vector3, Vector4, Quaternion and Matrix)")] 9[assembly: AssemblyDescription("MonoXnaCompactMaths (Vector3, Vector4, Quaternion and Matrix)")]
10[assembly: AssemblyConfiguration("")] 10[assembly: AssemblyConfiguration("")]
11[assembly: AssemblyCompany("")] 11[assembly: AssemblyCompany("")]
12[assembly: AssemblyProduct("")] 12[assembly: AssemblyProduct("")]
13[assembly: AssemblyCopyright("Copyright 2006 - 2007 The Mono.Xna Team")] 13[assembly: AssemblyCopyright("Copyright 2006 - 2007 The Mono.Xna Team")]
14[assembly: AssemblyTrademark("")] 14[assembly: AssemblyTrademark("")]
15[assembly: AssemblyCulture("")] 15[assembly: AssemblyCulture("")]
16 16
17// Si establece ComVisible como false hace que los tipos de este ensamblado no sean visibles 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 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. 19// COM, establezca el atributo ComVisible como true en este tipo.
20[assembly: ComVisible(false)] 20[assembly: ComVisible(false)]
21 21
22// El siguiente GUID sirve como identificador de la biblioteca de tipos si este proyecto se expone a COM 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")] 23[assembly: Guid("79f9dfd1-60f8-44b8-ab08-6f6e341976b7")]
24 24
25// La información de versión de un ensamblado consta de los cuatro valores siguientes: 25// La información de versión de un ensamblado consta de los cuatro valores siguientes:
26// 26//
27// Versión principal 27// Versión principal
28// Versión secundaria 28// Versión secundaria
29// Número de versión de compilación 29// Número de versión de compilación
30// Revisión 30// Revisión
31// 31//
32[assembly: AssemblyVersion("1.0.0.0")] 32[assembly: AssemblyVersion("1.0.0.0")]
33[assembly: AssemblyFileVersion("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
index 6ed1443..b4f1873 100644
--- a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs
+++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs
@@ -1,346 +1,346 @@
1#region License 1#region License
2/* 2/*
3MIT License 3MIT License
4Copyright © 2006 The Mono.Xna Team 4Copyright © 2006 The Mono.Xna Team
5 5
6All rights reserved. 6All rights reserved.
7 7
8Permission is hereby granted, free of charge, to any person obtaining a copy 8Permission is hereby granted, free of charge, to any person obtaining a copy
9of this software and associated documentation files (the "Software"), to deal 9of this software and associated documentation files (the "Software"), to deal
10in the Software without restriction, including without limitation the rights 10in the Software without restriction, including without limitation the rights
11to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12copies of the Software, and to permit persons to whom the Software is 12copies of the Software, and to permit persons to whom the Software is
13furnished to do so, subject to the following conditions: 13furnished to do so, subject to the following conditions:
14 14
15The above copyright notice and this permission notice shall be included in all 15The above copyright notice and this permission notice shall be included in all
16copies or substantial portions of the Software. 16copies or substantial portions of the Software.
17 17
18THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 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 23OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
24SOFTWARE. 24SOFTWARE.
25*/ 25*/
26#endregion License 26#endregion License
27 27
28using System; 28using System;
29using System.ComponentModel; 29using System.ComponentModel;
30 30
31namespace MonoXnaCompactMaths 31namespace MonoXnaCompactMaths
32{ 32{
33 [Serializable] 33 [Serializable]
34 //[TypeConverter(typeof(QuaternionConverter))] 34 //[TypeConverter(typeof(QuaternionConverter))]
35 public struct Quaternion : IEquatable<Quaternion> 35 public struct Quaternion : IEquatable<Quaternion>
36 { 36 {
37 public float X; 37 public float X;
38 public float Y; 38 public float Y;
39 public float Z; 39 public float Z;
40 public float W; 40 public float W;
41 static Quaternion identity = new Quaternion(0, 0, 0, 1); 41 static Quaternion identity = new Quaternion(0, 0, 0, 1);
42 42
43 43
44 public Quaternion(float x, float y, float z, float w) 44 public Quaternion(float x, float y, float z, float w)
45 { 45 {
46 this.X = x; 46 this.X = x;
47 this.Y = y; 47 this.Y = y;
48 this.Z = z; 48 this.Z = z;
49 this.W = w; 49 this.W = w;
50 } 50 }
51 51
52 52
53 public Quaternion(Vector3 vectorPart, float scalarPart) 53 public Quaternion(Vector3 vectorPart, float scalarPart)
54 { 54 {
55 this.X = vectorPart.X; 55 this.X = vectorPart.X;
56 this.Y = vectorPart.Y; 56 this.Y = vectorPart.Y;
57 this.Z = vectorPart.Z; 57 this.Z = vectorPart.Z;
58 this.W = scalarPart; 58 this.W = scalarPart;
59 } 59 }
60 60
61 public static Quaternion Identity 61 public static Quaternion Identity
62 { 62 {
63 get{ return identity; } 63 get{ return identity; }
64 } 64 }
65 65
66 66
67 public static Quaternion Add(Quaternion quaternion1, Quaternion quaternion2) 67 public static Quaternion Add(Quaternion quaternion1, Quaternion quaternion2)
68 { 68 {
69 throw new NotImplementedException(); 69 throw new NotImplementedException();
70 } 70 }
71 71
72 72
73 public static void Add(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result) 73 public static void Add(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)
74 { 74 {
75 throw new NotImplementedException(); 75 throw new NotImplementedException();
76 } 76 }
77 77
78 78
79 public static Quaternion CreateFromAxisAngle(Vector3 axis, float angle) 79 public static Quaternion CreateFromAxisAngle(Vector3 axis, float angle)
80 { 80 {
81 throw new NotImplementedException(); 81 throw new NotImplementedException();
82 } 82 }
83 83
84 84
85 public static void CreateFromAxisAngle(ref Vector3 axis, float angle, out Quaternion result) 85 public static void CreateFromAxisAngle(ref Vector3 axis, float angle, out Quaternion result)
86 { 86 {
87 throw new NotImplementedException(); 87 throw new NotImplementedException();
88 } 88 }
89 89
90 90
91 public static Quaternion CreateFromRotationMatrix(Matrix matrix) 91 public static Quaternion CreateFromRotationMatrix(Matrix matrix)
92 { 92 {
93 throw new NotImplementedException(); 93 throw new NotImplementedException();
94 } 94 }
95 95
96 96
97 public static void CreateFromRotationMatrix(ref Matrix matrix, out Quaternion result) 97 public static void CreateFromRotationMatrix(ref Matrix matrix, out Quaternion result)
98 { 98 {
99 throw new NotImplementedException(); 99 throw new NotImplementedException();
100 } 100 }
101 101
102 102
103 public static Quaternion Divide(Quaternion quaternion1, Quaternion quaternion2) 103 public static Quaternion Divide(Quaternion quaternion1, Quaternion quaternion2)
104 { 104 {
105 throw new NotImplementedException(); 105 throw new NotImplementedException();
106 } 106 }
107 107
108 108
109 public static void Divide(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result) 109 public static void Divide(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)
110 { 110 {
111 throw new NotImplementedException(); 111 throw new NotImplementedException();
112 } 112 }
113 113
114 114
115 public static float Dot(Quaternion quaternion1, Quaternion quaternion2) 115 public static float Dot(Quaternion quaternion1, Quaternion quaternion2)
116 { 116 {
117 throw new NotImplementedException(); 117 throw new NotImplementedException();
118 } 118 }
119 119
120 120
121 public static void Dot(ref Quaternion quaternion1, ref Quaternion quaternion2, out float result) 121 public static void Dot(ref Quaternion quaternion1, ref Quaternion quaternion2, out float result)
122 { 122 {
123 throw new NotImplementedException(); 123 throw new NotImplementedException();
124 } 124 }
125 125
126 126
127 public override bool Equals(object obj) 127 public override bool Equals(object obj)
128 { 128 {
129 throw new NotImplementedException(); 129 throw new NotImplementedException();
130 } 130 }
131 131
132 132
133 public bool Equals(Quaternion other) 133 public bool Equals(Quaternion other)
134 { 134 {
135 throw new NotImplementedException(); 135 throw new NotImplementedException();
136 } 136 }
137 137
138 138
139 public override int GetHashCode() 139 public override int GetHashCode()
140 { 140 {
141 throw new NotImplementedException(); 141 throw new NotImplementedException();
142 } 142 }
143 143
144 144
145 public static Quaternion Inverse(Quaternion quaternion) 145 public static Quaternion Inverse(Quaternion quaternion)
146 { 146 {
147 throw new NotImplementedException(); 147 throw new NotImplementedException();
148 } 148 }
149 149
150 150
151 public static void Inverse(ref Quaternion quaternion, out Quaternion result) 151 public static void Inverse(ref Quaternion quaternion, out Quaternion result)
152 { 152 {
153 throw new NotImplementedException(); 153 throw new NotImplementedException();
154 } 154 }
155 155
156 156
157 public float Length() 157 public float Length()
158 { 158 {
159 //--- 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)); 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 //--- 161 //---
162 //throw new NotImplementedException(); 162 //throw new NotImplementedException();
163 } 163 }
164 164
165 165
166 public float LengthSquared() 166 public float LengthSquared()
167 { 167 {
168 //--- 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)); 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 //--- 170 //---
171 //throw new NotImplementedException(); 171 //throw new NotImplementedException();
172 } 172 }
173 173
174 174
175 public static Quaternion Lerp(Quaternion quaternion1, Quaternion quaternion2, float amount) 175 public static Quaternion Lerp(Quaternion quaternion1, Quaternion quaternion2, float amount)
176 { 176 {
177 throw new NotImplementedException(); 177 throw new NotImplementedException();
178 } 178 }
179 179
180 180
181 public static void Lerp(ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result) 181 public static void Lerp(ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result)
182 { 182 {
183 throw new NotImplementedException(); 183 throw new NotImplementedException();
184 } 184 }
185 185
186 186
187 public static Quaternion Slerp(Quaternion quaternion1, Quaternion quaternion2, float amount) 187 public static Quaternion Slerp(Quaternion quaternion1, Quaternion quaternion2, float amount)
188 { 188 {
189 throw new NotImplementedException(); 189 throw new NotImplementedException();
190 } 190 }
191 191
192 192
193 public static void Slerp(ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result) 193 public static void Slerp(ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result)
194 { 194 {
195 throw new NotImplementedException(); 195 throw new NotImplementedException();
196 } 196 }
197 197
198 198
199 public static Quaternion Subtract(Quaternion quaternion1, Quaternion quaternion2) 199 public static Quaternion Subtract(Quaternion quaternion1, Quaternion quaternion2)
200 { 200 {
201 throw new NotImplementedException(); 201 throw new NotImplementedException();
202 } 202 }
203 203
204 204
205 public static void Subtract(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result) 205 public static void Subtract(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)
206 { 206 {
207 throw new NotImplementedException(); 207 throw new NotImplementedException();
208 } 208 }
209 209
210 210
211 public static Quaternion Multiply(Quaternion quaternion1, Quaternion quaternion2) 211 public static Quaternion Multiply(Quaternion quaternion1, Quaternion quaternion2)
212 { 212 {
213 throw new NotImplementedException(); 213 throw new NotImplementedException();
214 } 214 }
215 215
216 216
217 public static Quaternion Multiply(Quaternion quaternion1, float scaleFactor) 217 public static Quaternion Multiply(Quaternion quaternion1, float scaleFactor)
218 { 218 {
219 throw new NotImplementedException(); 219 throw new NotImplementedException();
220 } 220 }
221 221
222 222
223 public static void Multiply(ref Quaternion quaternion1, float scaleFactor, out Quaternion result) 223 public static void Multiply(ref Quaternion quaternion1, float scaleFactor, out Quaternion result)
224 { 224 {
225 throw new NotImplementedException(); 225 throw new NotImplementedException();
226 } 226 }
227 227
228 228
229 public static void Multiply(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result) 229 public static void Multiply(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)
230 { 230 {
231 throw new NotImplementedException(); 231 throw new NotImplementedException();
232 } 232 }
233 233
234 234
235 public static Quaternion Negate(Quaternion quaternion) 235 public static Quaternion Negate(Quaternion quaternion)
236 { 236 {
237 throw new NotImplementedException(); 237 throw new NotImplementedException();
238 } 238 }
239 239
240 240
241 public static void Negate(ref Quaternion quaternion, out Quaternion result) 241 public static void Negate(ref Quaternion quaternion, out Quaternion result)
242 { 242 {
243 throw new NotImplementedException(); 243 throw new NotImplementedException();
244 } 244 }
245 245
246 246
247 public void Normalize() 247 public void Normalize()
248 { 248 {
249 //--- 249 //---
250 this = Normalize(this); 250 this = Normalize(this);
251 //--- 251 //---
252 //throw new NotImplementedException(); 252 //throw new NotImplementedException();
253 } 253 }
254 254
255 255
256 public static Quaternion Normalize(Quaternion quaternion) 256 public static Quaternion Normalize(Quaternion quaternion)
257 { 257 {
258 //--- 258 //---
259 return quaternion / quaternion.Length(); 259 return quaternion / quaternion.Length();
260 //--- 260 //---
261 //throw new NotImplementedException(); 261 //throw new NotImplementedException();
262 } 262 }
263 263
264 264
265 public static void Normalize(ref Quaternion quaternion, out Quaternion result) 265 public static void Normalize(ref Quaternion quaternion, out Quaternion result)
266 { 266 {
267 throw new NotImplementedException(); 267 throw new NotImplementedException();
268 } 268 }
269 269
270 270
271 public static Quaternion operator +(Quaternion quaternion1, Quaternion quaternion2) 271 public static Quaternion operator +(Quaternion quaternion1, Quaternion quaternion2)
272 { 272 {
273 throw new NotImplementedException(); 273 throw new NotImplementedException();
274 } 274 }
275 275
276 276
277 public static Quaternion operator /(Quaternion quaternion1, Quaternion quaternion2) 277 public static Quaternion operator /(Quaternion quaternion1, Quaternion quaternion2)
278 { 278 {
279 throw new NotImplementedException(); 279 throw new NotImplementedException();
280 } 280 }
281 public static Quaternion operator /(Quaternion quaternion, float factor) 281 public static Quaternion operator /(Quaternion quaternion, float factor)
282 { 282 {
283 quaternion.W /= factor; 283 quaternion.W /= factor;
284 quaternion.X /= factor; 284 quaternion.X /= factor;
285 quaternion.Y /= factor; 285 quaternion.Y /= factor;
286 quaternion.Z /= factor; 286 quaternion.Z /= factor;
287 return quaternion; 287 return quaternion;
288 } 288 }
289 289
290 public static bool operator ==(Quaternion quaternion1, Quaternion quaternion2) 290 public static bool operator ==(Quaternion quaternion1, Quaternion quaternion2)
291 { 291 {
292 throw new NotImplementedException(); 292 throw new NotImplementedException();
293 } 293 }
294 294
295 295
296 public static bool operator !=(Quaternion quaternion1, Quaternion quaternion2) 296 public static bool operator !=(Quaternion quaternion1, Quaternion quaternion2)
297 { 297 {
298 throw new NotImplementedException(); 298 throw new NotImplementedException();
299 } 299 }
300 300
301 301
302 public static Quaternion operator *(Quaternion quaternion1, Quaternion quaternion2) 302 public static Quaternion operator *(Quaternion quaternion1, Quaternion quaternion2)
303 { 303 {
304 //--- 304 //---
305 //Grassmann product 305 //Grassmann product
306 Quaternion quaternionProduct = new Quaternion(); 306 Quaternion quaternionProduct = new Quaternion();
307 307
308 quaternionProduct.W = quaternion1.W * quaternion2.W - quaternion1.X * quaternion2.X - quaternion1.Y * quaternion2.Y - quaternion1.Z * quaternion2.Z; 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; 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; 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; 311 quaternionProduct.Z = quaternion1.W * quaternion2.Z + quaternion1.X * quaternion2.Y - quaternion1.Y * quaternion2.X + quaternion1.Z * quaternion2.W;
312 return quaternionProduct; 312 return quaternionProduct;
313 //--- 313 //---
314 //throw new NotImplementedException(); 314 //throw new NotImplementedException();
315 } 315 }
316 316
317 317
318 public static Quaternion operator *(Quaternion quaternion1, float scaleFactor) 318 public static Quaternion operator *(Quaternion quaternion1, float scaleFactor)
319 { 319 {
320 throw new NotImplementedException(); 320 throw new NotImplementedException();
321 } 321 }
322 322
323 323
324 public static Quaternion operator -(Quaternion quaternion1, Quaternion quaternion2) 324 public static Quaternion operator -(Quaternion quaternion1, Quaternion quaternion2)
325 { 325 {
326 throw new NotImplementedException(); 326 throw new NotImplementedException();
327 } 327 }
328 328
329 329
330 public static Quaternion operator -(Quaternion quaternion) 330 public static Quaternion operator -(Quaternion quaternion)
331 { 331 {
332 throw new NotImplementedException(); 332 throw new NotImplementedException();
333 } 333 }
334 334
335 335
336 public override string ToString() 336 public override string ToString()
337 { 337 {
338 throw new NotImplementedException(); 338 throw new NotImplementedException();
339 } 339 }
340 340
341 private static void Conjugate(ref Quaternion quaternion, out Quaternion result) 341 private static void Conjugate(ref Quaternion quaternion, out Quaternion result)
342 { 342 {
343 throw new NotImplementedException(); 343 throw new NotImplementedException();
344 } 344 }
345 } 345 }
346} 346}
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector3.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector3.cs
index fd4bcd2..f246415 100644
--- a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector3.cs
+++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector3.cs
@@ -1,620 +1,620 @@
1#region License 1#region License
2/* 2/*
3MIT License 3MIT License
4Copyright © 2006 The Mono.Xna Team 4Copyright © 2006 The Mono.Xna Team
5 5
6All rights reserved. 6All rights reserved.
7 7
8Authors: 8Authors:
9 * Alan McGovern 9 * Alan McGovern
10 10
11Permission is hereby granted, free of charge, to any person obtaining a copy 11Permission is hereby granted, free of charge, to any person obtaining a copy
12of this software and associated documentation files (the "Software"), to deal 12of this software and associated documentation files (the "Software"), to deal
13in the Software without restriction, including without limitation the rights 13in the Software without restriction, including without limitation the rights
14to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 14to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
15copies of the Software, and to permit persons to whom the Software is 15copies of the Software, and to permit persons to whom the Software is
16furnished to do so, subject to the following conditions: 16furnished to do so, subject to the following conditions:
17 17
18The above copyright notice and this permission notice shall be included in all 18The above copyright notice and this permission notice shall be included in all
19copies or substantial portions of the Software. 19copies or substantial portions of the Software.
20 20
21THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 21THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
22IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 22IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
23FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 23FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
24AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 24AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
25LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 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 26OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
27SOFTWARE. 27SOFTWARE.
28*/ 28*/
29#endregion License 29#endregion License
30 30
31using System; 31using System;
32using System.ComponentModel; 32using System.ComponentModel;
33using System.Text; 33using System.Text;
34using System.Runtime.InteropServices; 34using System.Runtime.InteropServices;
35 35
36namespace MonoXnaCompactMaths 36namespace MonoXnaCompactMaths
37{ 37{
38 [Serializable] 38 [Serializable]
39 [StructLayout(LayoutKind.Sequential)] 39 [StructLayout(LayoutKind.Sequential)]
40 //[TypeConverter(typeof(Vector3Converter))] 40 //[TypeConverter(typeof(Vector3Converter))]
41 public struct Vector3 : IEquatable<Vector3> 41 public struct Vector3 : IEquatable<Vector3>
42 { 42 {
43 #region Private Fields 43 #region Private Fields
44 44
45 private static Vector3 zero = new Vector3(0f, 0f, 0f); 45 private static Vector3 zero = new Vector3(0f, 0f, 0f);
46 private static Vector3 one = new Vector3(1f, 1f, 1f); 46 private static Vector3 one = new Vector3(1f, 1f, 1f);
47 private static Vector3 unitX = new Vector3(1f, 0f, 0f); 47 private static Vector3 unitX = new Vector3(1f, 0f, 0f);
48 private static Vector3 unitY = new Vector3(0f, 1f, 0f); 48 private static Vector3 unitY = new Vector3(0f, 1f, 0f);
49 private static Vector3 unitZ = new Vector3(0f, 0f, 1f); 49 private static Vector3 unitZ = new Vector3(0f, 0f, 1f);
50 private static Vector3 up = new Vector3(0f, 1f, 0f); 50 private static Vector3 up = new Vector3(0f, 1f, 0f);
51 private static Vector3 down = 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); 52 private static Vector3 right = new Vector3(1f, 0f, 0f);
53 private static Vector3 left = 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); 54 private static Vector3 forward = new Vector3(0f, 0f, -1f);
55 private static Vector3 backward = new Vector3(0f, 0f, 1f); 55 private static Vector3 backward = new Vector3(0f, 0f, 1f);
56 56
57 #endregion Private Fields 57 #endregion Private Fields
58 58
59 59
60 #region Public Fields 60 #region Public Fields
61 61
62 public float X; 62 public float X;
63 public float Y; 63 public float Y;
64 public float Z; 64 public float Z;
65 65
66 #endregion Public Fields 66 #endregion Public Fields
67 67
68 68
69 #region Properties 69 #region Properties
70 70
71 public static Vector3 Zero 71 public static Vector3 Zero
72 { 72 {
73 get { return zero; } 73 get { return zero; }
74 } 74 }
75 75
76 public static Vector3 One 76 public static Vector3 One
77 { 77 {
78 get { return one; } 78 get { return one; }
79 } 79 }
80 80
81 public static Vector3 UnitX 81 public static Vector3 UnitX
82 { 82 {
83 get { return unitX; } 83 get { return unitX; }
84 } 84 }
85 85
86 public static Vector3 UnitY 86 public static Vector3 UnitY
87 { 87 {
88 get { return unitY; } 88 get { return unitY; }
89 } 89 }
90 90
91 public static Vector3 UnitZ 91 public static Vector3 UnitZ
92 { 92 {
93 get { return unitZ; } 93 get { return unitZ; }
94 } 94 }
95 95
96 public static Vector3 Up 96 public static Vector3 Up
97 { 97 {
98 get { return up; } 98 get { return up; }
99 } 99 }
100 100
101 public static Vector3 Down 101 public static Vector3 Down
102 { 102 {
103 get { return down; } 103 get { return down; }
104 } 104 }
105 105
106 public static Vector3 Right 106 public static Vector3 Right
107 { 107 {
108 get { return right; } 108 get { return right; }
109 } 109 }
110 110
111 public static Vector3 Left 111 public static Vector3 Left
112 { 112 {
113 get { return left; } 113 get { return left; }
114 } 114 }
115 115
116 public static Vector3 Forward 116 public static Vector3 Forward
117 { 117 {
118 get { return forward; } 118 get { return forward; }
119 } 119 }
120 120
121 public static Vector3 Backward 121 public static Vector3 Backward
122 { 122 {
123 get { return backward; } 123 get { return backward; }
124 } 124 }
125 125
126 #endregion Properties 126 #endregion Properties
127 127
128 128
129 #region Constructors 129 #region Constructors
130 130
131 public Vector3(float x, float y, float z) 131 public Vector3(float x, float y, float z)
132 { 132 {
133 this.X = x; 133 this.X = x;
134 this.Y = y; 134 this.Y = y;
135 this.Z = z; 135 this.Z = z;
136 } 136 }
137 137
138 138
139 public Vector3(float value) 139 public Vector3(float value)
140 { 140 {
141 this.X = value; 141 this.X = value;
142 this.Y = value; 142 this.Y = value;
143 this.Z = value; 143 this.Z = value;
144 } 144 }
145 145
146 146
147 /*public Vector3(Vector2 value, float z) 147 /*public Vector3(Vector2 value, float z)
148 { 148 {
149 this.X = value.X; 149 this.X = value.X;
150 this.Y = value.Y; 150 this.Y = value.Y;
151 this.Z = z; 151 this.Z = z;
152 }*/ 152 }*/
153 153
154 154
155 #endregion Constructors 155 #endregion Constructors
156 156
157 157
158 #region Public Methods 158 #region Public Methods
159 159
160 public static Vector3 Add(Vector3 value1, Vector3 value2) 160 public static Vector3 Add(Vector3 value1, Vector3 value2)
161 { 161 {
162 value1.X += value2.X; 162 value1.X += value2.X;
163 value1.Y += value2.Y; 163 value1.Y += value2.Y;
164 value1.Z += value2.Z; 164 value1.Z += value2.Z;
165 return value1; 165 return value1;
166 } 166 }
167 167
168 public static void Add(ref Vector3 value1, ref Vector3 value2, out Vector3 result) 168 public static void Add(ref Vector3 value1, ref Vector3 value2, out Vector3 result)
169 { 169 {
170 result.X = value1.X + value2.X; 170 result.X = value1.X + value2.X;
171 result.Y = value1.Y + value2.Y; 171 result.Y = value1.Y + value2.Y;
172 result.Z = value1.Z + value2.Z; 172 result.Z = value1.Z + value2.Z;
173 } 173 }
174 174
175 /*public static Vector3 Barycentric(Vector3 value1, Vector3 value2, Vector3 value3, float amount1, float amount2) 175 /*public static Vector3 Barycentric(Vector3 value1, Vector3 value2, Vector3 value3, float amount1, float amount2)
176 { 176 {
177 return new Vector3( 177 return new Vector3(
178 MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2), 178 MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2),
179 MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2), 179 MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2),
180 MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2)); 180 MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2));
181 }*/ 181 }*/
182 182
183 /*public static void Barycentric(ref Vector3 value1, ref Vector3 value2, ref Vector3 value3, float amount1, float amount2, out Vector3 result) 183 /*public static void Barycentric(ref Vector3 value1, ref Vector3 value2, ref Vector3 value3, float amount1, float amount2, out Vector3 result)
184 { 184 {
185 result = new Vector3( 185 result = new Vector3(
186 MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2), 186 MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2),
187 MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2), 187 MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2),
188 MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2)); 188 MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2));
189 }*/ 189 }*/
190 190
191 /*public static Vector3 CatmullRom(Vector3 value1, Vector3 value2, Vector3 value3, Vector3 value4, float amount) 191 /*public static Vector3 CatmullRom(Vector3 value1, Vector3 value2, Vector3 value3, Vector3 value4, float amount)
192 { 192 {
193 return new Vector3( 193 return new Vector3(
194 MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount), 194 MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount),
195 MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, 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)); 196 MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount));
197 }*/ 197 }*/
198 198
199 /*public static void CatmullRom(ref Vector3 value1, ref Vector3 value2, ref Vector3 value3, ref Vector3 value4, float amount, out Vector3 result) 199 /*public static void CatmullRom(ref Vector3 value1, ref Vector3 value2, ref Vector3 value3, ref Vector3 value4, float amount, out Vector3 result)
200 { 200 {
201 result = new Vector3( 201 result = new Vector3(
202 MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount), 202 MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount),
203 MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, 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)); 204 MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount));
205 }*/ 205 }*/
206 206
207 /*public static Vector3 Clamp(Vector3 value1, Vector3 min, Vector3 max) 207 /*public static Vector3 Clamp(Vector3 value1, Vector3 min, Vector3 max)
208 { 208 {
209 return new Vector3( 209 return new Vector3(
210 MathHelper.Clamp(value1.X, min.X, max.X), 210 MathHelper.Clamp(value1.X, min.X, max.X),
211 MathHelper.Clamp(value1.Y, min.Y, max.Y), 211 MathHelper.Clamp(value1.Y, min.Y, max.Y),
212 MathHelper.Clamp(value1.Z, min.Z, max.Z)); 212 MathHelper.Clamp(value1.Z, min.Z, max.Z));
213 }*/ 213 }*/
214 214
215 /*public static void Clamp(ref Vector3 value1, ref Vector3 min, ref Vector3 max, out Vector3 result) 215 /*public static void Clamp(ref Vector3 value1, ref Vector3 min, ref Vector3 max, out Vector3 result)
216 { 216 {
217 result = new Vector3( 217 result = new Vector3(
218 MathHelper.Clamp(value1.X, min.X, max.X), 218 MathHelper.Clamp(value1.X, min.X, max.X),
219 MathHelper.Clamp(value1.Y, min.Y, max.Y), 219 MathHelper.Clamp(value1.Y, min.Y, max.Y),
220 MathHelper.Clamp(value1.Z, min.Z, max.Z)); 220 MathHelper.Clamp(value1.Z, min.Z, max.Z));
221 }*/ 221 }*/
222 222
223 public static Vector3 Cross(Vector3 vector1, Vector3 vector2) 223 public static Vector3 Cross(Vector3 vector1, Vector3 vector2)
224 { 224 {
225 Cross(ref vector1, ref vector2, out vector1); 225 Cross(ref vector1, ref vector2, out vector1);
226 return vector1; 226 return vector1;
227 } 227 }
228 228
229 public static void Cross(ref Vector3 vector1, ref Vector3 vector2, out Vector3 result) 229 public static void Cross(ref Vector3 vector1, ref Vector3 vector2, out Vector3 result)
230 { 230 {
231 result = new Vector3(vector1.Y * vector2.Z - vector2.Y * vector1.Z, 231 result = new Vector3(vector1.Y * vector2.Z - vector2.Y * vector1.Z,
232 -(vector1.X * vector2.Z - vector2.X * vector1.Z), 232 -(vector1.X * vector2.Z - vector2.X * vector1.Z),
233 vector1.X * vector2.Y - vector2.X * vector1.Y); 233 vector1.X * vector2.Y - vector2.X * vector1.Y);
234 } 234 }
235 235
236 public static float Distance(Vector3 vector1, Vector3 vector2) 236 public static float Distance(Vector3 vector1, Vector3 vector2)
237 { 237 {
238 float result; 238 float result;
239 DistanceSquared(ref vector1, ref vector2, out result); 239 DistanceSquared(ref vector1, ref vector2, out result);
240 return (float)Math.Sqrt(result); 240 return (float)Math.Sqrt(result);
241 } 241 }
242 242
243 public static void Distance(ref Vector3 value1, ref Vector3 value2, out float result) 243 public static void Distance(ref Vector3 value1, ref Vector3 value2, out float result)
244 { 244 {
245 DistanceSquared(ref value1, ref value2, out result); 245 DistanceSquared(ref value1, ref value2, out result);
246 result = (float)Math.Sqrt(result); 246 result = (float)Math.Sqrt(result);
247 } 247 }
248 248
249 public static float DistanceSquared(Vector3 value1, Vector3 value2) 249 public static float DistanceSquared(Vector3 value1, Vector3 value2)
250 { 250 {
251 float result; 251 float result;
252 DistanceSquared(ref value1, ref value2, out result); 252 DistanceSquared(ref value1, ref value2, out result);
253 return result; 253 return result;
254 } 254 }
255 255
256 public static void DistanceSquared(ref Vector3 value1, ref Vector3 value2, out float result) 256 public static void DistanceSquared(ref Vector3 value1, ref Vector3 value2, out float result)
257 { 257 {
258 result = (value1.X - value2.X) * (value1.X - value2.X) + 258 result = (value1.X - value2.X) * (value1.X - value2.X) +
259 (value1.Y - value2.Y) * (value1.Y - value2.Y) + 259 (value1.Y - value2.Y) * (value1.Y - value2.Y) +
260 (value1.Z - value2.Z) * (value1.Z - value2.Z); 260 (value1.Z - value2.Z) * (value1.Z - value2.Z);
261 } 261 }
262 262
263 public static Vector3 Divide(Vector3 value1, Vector3 value2) 263 public static Vector3 Divide(Vector3 value1, Vector3 value2)
264 { 264 {
265 value1.X /= value2.X; 265 value1.X /= value2.X;
266 value1.Y /= value2.Y; 266 value1.Y /= value2.Y;
267 value1.Z /= value2.Z; 267 value1.Z /= value2.Z;
268 return value1; 268 return value1;
269 } 269 }
270 270
271 public static Vector3 Divide(Vector3 value1, float value2) 271 public static Vector3 Divide(Vector3 value1, float value2)
272 { 272 {
273 float factor = 1 / value2; 273 float factor = 1 / value2;
274 value1.X *= factor; 274 value1.X *= factor;
275 value1.Y *= factor; 275 value1.Y *= factor;
276 value1.Z *= factor; 276 value1.Z *= factor;
277 return value1; 277 return value1;
278 } 278 }
279 279
280 public static void Divide(ref Vector3 value1, float divisor, out Vector3 result) 280 public static void Divide(ref Vector3 value1, float divisor, out Vector3 result)
281 { 281 {
282 float factor = 1 / divisor; 282 float factor = 1 / divisor;
283 result.X = value1.X * factor; 283 result.X = value1.X * factor;
284 result.Y = value1.Y * factor; 284 result.Y = value1.Y * factor;
285 result.Z = value1.Z * factor; 285 result.Z = value1.Z * factor;
286 } 286 }
287 287
288 public static void Divide(ref Vector3 value1, ref Vector3 value2, out Vector3 result) 288 public static void Divide(ref Vector3 value1, ref Vector3 value2, out Vector3 result)
289 { 289 {
290 result.X = value1.X / value2.X; 290 result.X = value1.X / value2.X;
291 result.Y = value1.Y / value2.Y; 291 result.Y = value1.Y / value2.Y;
292 result.Z = value1.Z / value2.Z; 292 result.Z = value1.Z / value2.Z;
293 } 293 }
294 294
295 public static float Dot(Vector3 vector1, Vector3 vector2) 295 public static float Dot(Vector3 vector1, Vector3 vector2)
296 { 296 {
297 return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z; 297 return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z;
298 } 298 }
299 299
300 public static void Dot(ref Vector3 vector1, ref Vector3 vector2, out float result) 300 public static void Dot(ref Vector3 vector1, ref Vector3 vector2, out float result)
301 { 301 {
302 result = vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z; 302 result = vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z;
303 } 303 }
304 304
305 public override bool Equals(object obj) 305 public override bool Equals(object obj)
306 { 306 {
307 return (obj is Vector3) ? this == (Vector3)obj : false; 307 return (obj is Vector3) ? this == (Vector3)obj : false;
308 } 308 }
309 309
310 public bool Equals(Vector3 other) 310 public bool Equals(Vector3 other)
311 { 311 {
312 return this == other; 312 return this == other;
313 } 313 }
314 314
315 public override int GetHashCode() 315 public override int GetHashCode()
316 { 316 {
317 return (int)(this.X + this.Y + this.Z); 317 return (int)(this.X + this.Y + this.Z);
318 } 318 }
319 319
320 /*public static Vector3 Hermite(Vector3 value1, Vector3 tangent1, Vector3 value2, Vector3 tangent2, float amount) 320 /*public static Vector3 Hermite(Vector3 value1, Vector3 tangent1, Vector3 value2, Vector3 tangent2, float amount)
321 { 321 {
322 Vector3 result = new Vector3(); 322 Vector3 result = new Vector3();
323 Hermite(ref value1, ref tangent1, ref value2, ref tangent2, amount, out result); 323 Hermite(ref value1, ref tangent1, ref value2, ref tangent2, amount, out result);
324 return result; 324 return result;
325 }*/ 325 }*/
326 326
327 /*public static void Hermite(ref Vector3 value1, ref Vector3 tangent1, ref Vector3 value2, ref Vector3 tangent2, float amount, out Vector3 result) 327 /*public static void Hermite(ref Vector3 value1, ref Vector3 tangent1, ref Vector3 value2, ref Vector3 tangent2, float amount, out Vector3 result)
328 { 328 {
329 result.X = MathHelper.Hermite(value1.X, tangent1.X, value2.X, tangent2.X, amount); 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); 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); 331 result.Z = MathHelper.Hermite(value1.Z, tangent1.Z, value2.Z, tangent2.Z, amount);
332 }*/ 332 }*/
333 333
334 public float Length() 334 public float Length()
335 { 335 {
336 float result; 336 float result;
337 DistanceSquared(ref this, ref zero, out result); 337 DistanceSquared(ref this, ref zero, out result);
338 return (float)Math.Sqrt(result); 338 return (float)Math.Sqrt(result);
339 } 339 }
340 340
341 public float LengthSquared() 341 public float LengthSquared()
342 { 342 {
343 float result; 343 float result;
344 DistanceSquared(ref this, ref zero, out result); 344 DistanceSquared(ref this, ref zero, out result);
345 return result; 345 return result;
346 } 346 }
347 347
348 /*public static Vector3 Lerp(Vector3 value1, Vector3 value2, float amount) 348 /*public static Vector3 Lerp(Vector3 value1, Vector3 value2, float amount)
349 { 349 {
350 return new Vector3( 350 return new Vector3(
351 MathHelper.Lerp(value1.X, value2.X, amount), 351 MathHelper.Lerp(value1.X, value2.X, amount),
352 MathHelper.Lerp(value1.Y, value2.Y, amount), 352 MathHelper.Lerp(value1.Y, value2.Y, amount),
353 MathHelper.Lerp(value1.Z, value2.Z, amount)); 353 MathHelper.Lerp(value1.Z, value2.Z, amount));
354 }*/ 354 }*/
355 355
356 /*public static void Lerp(ref Vector3 value1, ref Vector3 value2, float amount, out Vector3 result) 356 /*public static void Lerp(ref Vector3 value1, ref Vector3 value2, float amount, out Vector3 result)
357 { 357 {
358 result = new Vector3( 358 result = new Vector3(
359 MathHelper.Lerp(value1.X, value2.X, amount), 359 MathHelper.Lerp(value1.X, value2.X, amount),
360 MathHelper.Lerp(value1.Y, value2.Y, amount), 360 MathHelper.Lerp(value1.Y, value2.Y, amount),
361 MathHelper.Lerp(value1.Z, value2.Z, amount)); 361 MathHelper.Lerp(value1.Z, value2.Z, amount));
362 }*/ 362 }*/
363 363
364 /*public static Vector3 Max(Vector3 value1, Vector3 value2) 364 /*public static Vector3 Max(Vector3 value1, Vector3 value2)
365 { 365 {
366 return new Vector3( 366 return new Vector3(
367 MathHelper.Max(value1.X, value2.X), 367 MathHelper.Max(value1.X, value2.X),
368 MathHelper.Max(value1.Y, value2.Y), 368 MathHelper.Max(value1.Y, value2.Y),
369 MathHelper.Max(value1.Z, value2.Z)); 369 MathHelper.Max(value1.Z, value2.Z));
370 }*/ 370 }*/
371 371
372 /*public static void Max(ref Vector3 value1, ref Vector3 value2, out Vector3 result) 372 /*public static void Max(ref Vector3 value1, ref Vector3 value2, out Vector3 result)
373 { 373 {
374 result = new Vector3( 374 result = new Vector3(
375 MathHelper.Max(value1.X, value2.X), 375 MathHelper.Max(value1.X, value2.X),
376 MathHelper.Max(value1.Y, value2.Y), 376 MathHelper.Max(value1.Y, value2.Y),
377 MathHelper.Max(value1.Z, value2.Z)); 377 MathHelper.Max(value1.Z, value2.Z));
378 }*/ 378 }*/
379 379
380 /*public static Vector3 Min(Vector3 value1, Vector3 value2) 380 /*public static Vector3 Min(Vector3 value1, Vector3 value2)
381 { 381 {
382 return new Vector3( 382 return new Vector3(
383 MathHelper.Min(value1.X, value2.X), 383 MathHelper.Min(value1.X, value2.X),
384 MathHelper.Min(value1.Y, value2.Y), 384 MathHelper.Min(value1.Y, value2.Y),
385 MathHelper.Min(value1.Z, value2.Z)); 385 MathHelper.Min(value1.Z, value2.Z));
386 }*/ 386 }*/
387 387
388 /*public static void Min(ref Vector3 value1, ref Vector3 value2, out Vector3 result) 388 /*public static void Min(ref Vector3 value1, ref Vector3 value2, out Vector3 result)
389 { 389 {
390 result = new Vector3( 390 result = new Vector3(
391 MathHelper.Min(value1.X, value2.X), 391 MathHelper.Min(value1.X, value2.X),
392 MathHelper.Min(value1.Y, value2.Y), 392 MathHelper.Min(value1.Y, value2.Y),
393 MathHelper.Min(value1.Z, value2.Z)); 393 MathHelper.Min(value1.Z, value2.Z));
394 }*/ 394 }*/
395 395
396 public static Vector3 Multiply(Vector3 value1, Vector3 value2) 396 public static Vector3 Multiply(Vector3 value1, Vector3 value2)
397 { 397 {
398 value1.X *= value2.X; 398 value1.X *= value2.X;
399 value1.Y *= value2.Y; 399 value1.Y *= value2.Y;
400 value1.Z *= value2.Z; 400 value1.Z *= value2.Z;
401 return value1; 401 return value1;
402 } 402 }
403 403
404 public static Vector3 Multiply(Vector3 value1, float scaleFactor) 404 public static Vector3 Multiply(Vector3 value1, float scaleFactor)
405 { 405 {
406 value1.X *= scaleFactor; 406 value1.X *= scaleFactor;
407 value1.Y *= scaleFactor; 407 value1.Y *= scaleFactor;
408 value1.Z *= scaleFactor; 408 value1.Z *= scaleFactor;
409 return value1; 409 return value1;
410 } 410 }
411 411
412 public static void Multiply(ref Vector3 value1, float scaleFactor, out Vector3 result) 412 public static void Multiply(ref Vector3 value1, float scaleFactor, out Vector3 result)
413 { 413 {
414 result.X = value1.X * scaleFactor; 414 result.X = value1.X * scaleFactor;
415 result.Y = value1.Y * scaleFactor; 415 result.Y = value1.Y * scaleFactor;
416 result.Z = value1.Z * scaleFactor; 416 result.Z = value1.Z * scaleFactor;
417 } 417 }
418 418
419 public static void Multiply(ref Vector3 value1, ref Vector3 value2, out Vector3 result) 419 public static void Multiply(ref Vector3 value1, ref Vector3 value2, out Vector3 result)
420 { 420 {
421 result.X = value1.X * value2.X; 421 result.X = value1.X * value2.X;
422 result.Y = value1.Y * value2.Y; 422 result.Y = value1.Y * value2.Y;
423 result.Z = value1.Z * value2.Z; 423 result.Z = value1.Z * value2.Z;
424 } 424 }
425 425
426 public static Vector3 Negate(Vector3 value) 426 public static Vector3 Negate(Vector3 value)
427 { 427 {
428 value = new Vector3(-value.X, -value.Y, -value.Z); 428 value = new Vector3(-value.X, -value.Y, -value.Z);
429 return value; 429 return value;
430 } 430 }
431 431
432 public static void Negate(ref Vector3 value, out Vector3 result) 432 public static void Negate(ref Vector3 value, out Vector3 result)
433 { 433 {
434 result = new Vector3(-value.X, -value.Y, -value.Z); 434 result = new Vector3(-value.X, -value.Y, -value.Z);
435 } 435 }
436 436
437 public void Normalize() 437 public void Normalize()
438 { 438 {
439 Normalize(ref this, out this); 439 Normalize(ref this, out this);
440 } 440 }
441 441
442 public static Vector3 Normalize(Vector3 vector) 442 public static Vector3 Normalize(Vector3 vector)
443 { 443 {
444 Normalize(ref vector, out vector); 444 Normalize(ref vector, out vector);
445 return vector; 445 return vector;
446 } 446 }
447 447
448 public static void Normalize(ref Vector3 value, out Vector3 result) 448 public static void Normalize(ref Vector3 value, out Vector3 result)
449 { 449 {
450 float factor; 450 float factor;
451 Distance(ref value, ref zero, out factor); 451 Distance(ref value, ref zero, out factor);
452 factor = 1f / factor; 452 factor = 1f / factor;
453 result.X = value.X * factor; 453 result.X = value.X * factor;
454 result.Y = value.Y * factor; 454 result.Y = value.Y * factor;
455 result.Z = value.Z * factor; 455 result.Z = value.Z * factor;
456 } 456 }
457 457
458 public static Vector3 Reflect(Vector3 vector, Vector3 normal) 458 public static Vector3 Reflect(Vector3 vector, Vector3 normal)
459 { 459 {
460 throw new NotImplementedException(); 460 throw new NotImplementedException();
461 } 461 }
462 462
463 public static void Reflect(ref Vector3 vector, ref Vector3 normal, out Vector3 result) 463 public static void Reflect(ref Vector3 vector, ref Vector3 normal, out Vector3 result)
464 { 464 {
465 throw new NotImplementedException(); 465 throw new NotImplementedException();
466 } 466 }
467 467
468 /*public static Vector3 SmoothStep(Vector3 value1, Vector3 value2, float amount) 468 /*public static Vector3 SmoothStep(Vector3 value1, Vector3 value2, float amount)
469 { 469 {
470 return new Vector3( 470 return new Vector3(
471 MathHelper.SmoothStep(value1.X, value2.X, amount), 471 MathHelper.SmoothStep(value1.X, value2.X, amount),
472 MathHelper.SmoothStep(value1.Y, value2.Y, amount), 472 MathHelper.SmoothStep(value1.Y, value2.Y, amount),
473 MathHelper.SmoothStep(value1.Z, value2.Z, amount)); 473 MathHelper.SmoothStep(value1.Z, value2.Z, amount));
474 }*/ 474 }*/
475 475
476 /*public static void SmoothStep(ref Vector3 value1, ref Vector3 value2, float amount, out Vector3 result) 476 /*public static void SmoothStep(ref Vector3 value1, ref Vector3 value2, float amount, out Vector3 result)
477 { 477 {
478 result = new Vector3( 478 result = new Vector3(
479 MathHelper.SmoothStep(value1.X, value2.X, amount), 479 MathHelper.SmoothStep(value1.X, value2.X, amount),
480 MathHelper.SmoothStep(value1.Y, value2.Y, amount), 480 MathHelper.SmoothStep(value1.Y, value2.Y, amount),
481 MathHelper.SmoothStep(value1.Z, value2.Z, amount)); 481 MathHelper.SmoothStep(value1.Z, value2.Z, amount));
482 }*/ 482 }*/
483 483
484 public static Vector3 Subtract(Vector3 value1, Vector3 value2) 484 public static Vector3 Subtract(Vector3 value1, Vector3 value2)
485 { 485 {
486 value1.X -= value2.X; 486 value1.X -= value2.X;
487 value1.Y -= value2.Y; 487 value1.Y -= value2.Y;
488 value1.Z -= value2.Z; 488 value1.Z -= value2.Z;
489 return value1; 489 return value1;
490 } 490 }
491 491
492 public static void Subtract(ref Vector3 value1, ref Vector3 value2, out Vector3 result) 492 public static void Subtract(ref Vector3 value1, ref Vector3 value2, out Vector3 result)
493 { 493 {
494 result.X = value1.X - value2.X; 494 result.X = value1.X - value2.X;
495 result.Y = value1.Y - value2.Y; 495 result.Y = value1.Y - value2.Y;
496 result.Z = value1.Z - value2.Z; 496 result.Z = value1.Z - value2.Z;
497 } 497 }
498 498
499 public override string ToString() 499 public override string ToString()
500 { 500 {
501 StringBuilder sb = new StringBuilder(32); 501 StringBuilder sb = new StringBuilder(32);
502 sb.Append("{X:"); 502 sb.Append("{X:");
503 sb.Append(this.X); 503 sb.Append(this.X);
504 sb.Append(" Y:"); 504 sb.Append(" Y:");
505 sb.Append(this.Y); 505 sb.Append(this.Y);
506 sb.Append(" Z:"); 506 sb.Append(" Z:");
507 sb.Append(this.Z); 507 sb.Append(this.Z);
508 sb.Append("}"); 508 sb.Append("}");
509 return sb.ToString(); 509 return sb.ToString();
510 } 510 }
511 511
512 public static Vector3 Transform(Vector3 position, Matrix matrix) 512 public static Vector3 Transform(Vector3 position, Matrix matrix)
513 { 513 {
514 Transform(ref position, ref matrix, out position); 514 Transform(ref position, ref matrix, out position);
515 return position; 515 return position;
516 } 516 }
517 517
518 public static void Transform(ref Vector3 position, ref Matrix matrix, out Vector3 result) 518 public static void Transform(ref Vector3 position, ref Matrix matrix, out Vector3 result)
519 { 519 {
520 result = new Vector3((position.X * matrix.M11) + (position.Y * matrix.M21) + (position.Z * matrix.M31) + matrix.M41, 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, 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); 522 (position.X * matrix.M13) + (position.Y * matrix.M23) + (position.Z * matrix.M33) + matrix.M43);
523 } 523 }
524 524
525 public static Vector3 TransformNormal(Vector3 normal, Matrix matrix) 525 public static Vector3 TransformNormal(Vector3 normal, Matrix matrix)
526 { 526 {
527 TransformNormal(ref normal, ref matrix, out normal); 527 TransformNormal(ref normal, ref matrix, out normal);
528 return normal; 528 return normal;
529 } 529 }
530 530
531 public static void TransformNormal(ref Vector3 normal, ref Matrix matrix, out Vector3 result) 531 public static void TransformNormal(ref Vector3 normal, ref Matrix matrix, out Vector3 result)
532 { 532 {
533 result = new Vector3((normal.X * matrix.M11) + (normal.Y * matrix.M21) + (normal.Z * matrix.M31), 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), 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)); 535 (normal.X * matrix.M13) + (normal.Y * matrix.M23) + (normal.Z * matrix.M33));
536 } 536 }
537 537
538 #endregion Public methods 538 #endregion Public methods
539 539
540 540
541 #region Operators 541 #region Operators
542 542
543 public static bool operator ==(Vector3 value1, Vector3 value2) 543 public static bool operator ==(Vector3 value1, Vector3 value2)
544 { 544 {
545 return value1.X == value2.X 545 return value1.X == value2.X
546 && value1.Y == value2.Y 546 && value1.Y == value2.Y
547 && value1.Z == value2.Z; 547 && value1.Z == value2.Z;
548 } 548 }
549 549
550 public static bool operator !=(Vector3 value1, Vector3 value2) 550 public static bool operator !=(Vector3 value1, Vector3 value2)
551 { 551 {
552 return !(value1 == value2); 552 return !(value1 == value2);
553 } 553 }
554 554
555 public static Vector3 operator +(Vector3 value1, Vector3 value2) 555 public static Vector3 operator +(Vector3 value1, Vector3 value2)
556 { 556 {
557 value1.X += value2.X; 557 value1.X += value2.X;
558 value1.Y += value2.Y; 558 value1.Y += value2.Y;
559 value1.Z += value2.Z; 559 value1.Z += value2.Z;
560 return value1; 560 return value1;
561 } 561 }
562 562
563 public static Vector3 operator -(Vector3 value) 563 public static Vector3 operator -(Vector3 value)
564 { 564 {
565 value = new Vector3(-value.X, -value.Y, -value.Z); 565 value = new Vector3(-value.X, -value.Y, -value.Z);
566 return value; 566 return value;
567 } 567 }
568 568
569 public static Vector3 operator -(Vector3 value1, Vector3 value2) 569 public static Vector3 operator -(Vector3 value1, Vector3 value2)
570 { 570 {
571 value1.X -= value2.X; 571 value1.X -= value2.X;
572 value1.Y -= value2.Y; 572 value1.Y -= value2.Y;
573 value1.Z -= value2.Z; 573 value1.Z -= value2.Z;
574 return value1; 574 return value1;
575 } 575 }
576 576
577 public static Vector3 operator *(Vector3 value1, Vector3 value2) 577 public static Vector3 operator *(Vector3 value1, Vector3 value2)
578 { 578 {
579 value1.X *= value2.X; 579 value1.X *= value2.X;
580 value1.Y *= value2.Y; 580 value1.Y *= value2.Y;
581 value1.Z *= value2.Z; 581 value1.Z *= value2.Z;
582 return value1; 582 return value1;
583 } 583 }
584 584
585 public static Vector3 operator *(Vector3 value, float scaleFactor) 585 public static Vector3 operator *(Vector3 value, float scaleFactor)
586 { 586 {
587 value.X *= scaleFactor; 587 value.X *= scaleFactor;
588 value.Y *= scaleFactor; 588 value.Y *= scaleFactor;
589 value.Z *= scaleFactor; 589 value.Z *= scaleFactor;
590 return value; 590 return value;
591 } 591 }
592 592
593 public static Vector3 operator *(float scaleFactor, Vector3 value) 593 public static Vector3 operator *(float scaleFactor, Vector3 value)
594 { 594 {
595 value.X *= scaleFactor; 595 value.X *= scaleFactor;
596 value.Y *= scaleFactor; 596 value.Y *= scaleFactor;
597 value.Z *= scaleFactor; 597 value.Z *= scaleFactor;
598 return value; 598 return value;
599 } 599 }
600 600
601 public static Vector3 operator /(Vector3 value1, Vector3 value2) 601 public static Vector3 operator /(Vector3 value1, Vector3 value2)
602 { 602 {
603 value1.X /= value2.X; 603 value1.X /= value2.X;
604 value1.Y /= value2.Y; 604 value1.Y /= value2.Y;
605 value1.Z /= value2.Z; 605 value1.Z /= value2.Z;
606 return value1; 606 return value1;
607 } 607 }
608 608
609 public static Vector3 operator /(Vector3 value, float divider) 609 public static Vector3 operator /(Vector3 value, float divider)
610 { 610 {
611 float factor = 1 / divider; 611 float factor = 1 / divider;
612 value.X *= factor; 612 value.X *= factor;
613 value.Y *= factor; 613 value.Y *= factor;
614 value.Z *= factor; 614 value.Z *= factor;
615 return value; 615 return value;
616 } 616 }
617 617
618 #endregion 618 #endregion
619 } 619 }
620} 620}
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector4.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector4.cs
index c75b7c8..abb30a8 100644
--- a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector4.cs
+++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector4.cs
@@ -1,630 +1,630 @@
1#region License 1#region License
2/* 2/*
3MIT License 3MIT License
4Copyright © 2006 The Mono.Xna Team 4Copyright © 2006 The Mono.Xna Team
5 5
6All rights reserved. 6All rights reserved.
7 7
8Permission is hereby granted, free of charge, to any person obtaining a copy 8Permission is hereby granted, free of charge, to any person obtaining a copy
9of this software and associated documentation files (the "Software"), to deal 9of this software and associated documentation files (the "Software"), to deal
10in the Software without restriction, including without limitation the rights 10in the Software without restriction, including without limitation the rights
11to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12copies of the Software, and to permit persons to whom the Software is 12copies of the Software, and to permit persons to whom the Software is
13furnished to do so, subject to the following conditions: 13furnished to do so, subject to the following conditions:
14 14
15The above copyright notice and this permission notice shall be included in all 15The above copyright notice and this permission notice shall be included in all
16copies or substantial portions of the Software. 16copies or substantial portions of the Software.
17 17
18THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 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 23OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
24SOFTWARE. 24SOFTWARE.
25*/ 25*/
26#endregion License 26#endregion License
27 27
28using System; 28using System;
29using System.ComponentModel; 29using System.ComponentModel;
30using System.Text; 30using System.Text;
31using System.Runtime.InteropServices; 31using System.Runtime.InteropServices;
32 32
33namespace MonoXnaCompactMaths 33namespace MonoXnaCompactMaths
34{ 34{
35 [Serializable] 35 [Serializable]
36 [StructLayout(LayoutKind.Sequential)] 36 [StructLayout(LayoutKind.Sequential)]
37 //[TypeConverter(typeof(Vector4Converter))] 37 //[TypeConverter(typeof(Vector4Converter))]
38 public struct Vector4 : IEquatable<Vector4> 38 public struct Vector4 : IEquatable<Vector4>
39 { 39 {
40 #region Private Fields 40 #region Private Fields
41 41
42 private static Vector4 zeroVector = new Vector4(); 42 private static Vector4 zeroVector = new Vector4();
43 private static Vector4 unitVector = new Vector4(1f, 1f, 1f, 1f); 43 private static Vector4 unitVector = new Vector4(1f, 1f, 1f, 1f);
44 private static Vector4 unitXVector = new Vector4(1f, 0f, 0f, 0f); 44 private static Vector4 unitXVector = new Vector4(1f, 0f, 0f, 0f);
45 private static Vector4 unitYVector = new Vector4(0f, 1f, 0f, 0f); 45 private static Vector4 unitYVector = new Vector4(0f, 1f, 0f, 0f);
46 private static Vector4 unitZVector = new Vector4(0f, 0f, 1f, 0f); 46 private static Vector4 unitZVector = new Vector4(0f, 0f, 1f, 0f);
47 private static Vector4 unitWVector = new Vector4(0f, 0f, 0f, 1f); 47 private static Vector4 unitWVector = new Vector4(0f, 0f, 0f, 1f);
48 48
49 #endregion Private Fields 49 #endregion Private Fields
50 50
51 51
52 #region Public Fields 52 #region Public Fields
53 53
54 public float X; 54 public float X;
55 public float Y; 55 public float Y;
56 public float Z; 56 public float Z;
57 public float W; 57 public float W;
58 58
59 #endregion Public Fields 59 #endregion Public Fields
60 60
61 61
62 #region Properties 62 #region Properties
63 63
64 public static Vector4 Zero 64 public static Vector4 Zero
65 { 65 {
66 get { return zeroVector; } 66 get { return zeroVector; }
67 } 67 }
68 68
69 public static Vector4 One 69 public static Vector4 One
70 { 70 {
71 get { return unitVector; } 71 get { return unitVector; }
72 } 72 }
73 73
74 public static Vector4 UnitX 74 public static Vector4 UnitX
75 { 75 {
76 get { return unitXVector; } 76 get { return unitXVector; }
77 } 77 }
78 78
79 public static Vector4 UnitY 79 public static Vector4 UnitY
80 { 80 {
81 get { return unitYVector; } 81 get { return unitYVector; }
82 } 82 }
83 83
84 public static Vector4 UnitZ 84 public static Vector4 UnitZ
85 { 85 {
86 get { return unitZVector; } 86 get { return unitZVector; }
87 } 87 }
88 88
89 public static Vector4 UnitW 89 public static Vector4 UnitW
90 { 90 {
91 get { return unitWVector; } 91 get { return unitWVector; }
92 } 92 }
93 93
94 #endregion Properties 94 #endregion Properties
95 95
96 96
97 #region Constructors 97 #region Constructors
98 98
99 public Vector4(float x, float y, float z, float w) 99 public Vector4(float x, float y, float z, float w)
100 { 100 {
101 this.X = x; 101 this.X = x;
102 this.Y = y; 102 this.Y = y;
103 this.Z = z; 103 this.Z = z;
104 this.W = w; 104 this.W = w;
105 } 105 }
106 106
107 /*public Vector4(Vector2 value, float z, float w) 107 /*public Vector4(Vector2 value, float z, float w)
108 { 108 {
109 this.X = value.X; 109 this.X = value.X;
110 this.Y = value.Y; 110 this.Y = value.Y;
111 this.Z = z; 111 this.Z = z;
112 this.W = w; 112 this.W = w;
113 }*/ 113 }*/
114 114
115 public Vector4(Vector3 value, float w) 115 public Vector4(Vector3 value, float w)
116 { 116 {
117 this.X = value.X; 117 this.X = value.X;
118 this.Y = value.Y; 118 this.Y = value.Y;
119 this.Z = value.Z; 119 this.Z = value.Z;
120 this.W = w; 120 this.W = w;
121 } 121 }
122 122
123 public Vector4(float value) 123 public Vector4(float value)
124 { 124 {
125 this.X = value; 125 this.X = value;
126 this.Y = value; 126 this.Y = value;
127 this.Z = value; 127 this.Z = value;
128 this.W = value; 128 this.W = value;
129 } 129 }
130 130
131 #endregion 131 #endregion
132 132
133 133
134 #region Public Methods 134 #region Public Methods
135 135
136 public static Vector4 Add(Vector4 value1, Vector4 value2) 136 public static Vector4 Add(Vector4 value1, Vector4 value2)
137 { 137 {
138 value1.W += value2.W; 138 value1.W += value2.W;
139 value1.X += value2.X; 139 value1.X += value2.X;
140 value1.Y += value2.Y; 140 value1.Y += value2.Y;
141 value1.Z += value2.Z; 141 value1.Z += value2.Z;
142 return value1; 142 return value1;
143 } 143 }
144 144
145 public static void Add(ref Vector4 value1, ref Vector4 value2, out Vector4 result) 145 public static void Add(ref Vector4 value1, ref Vector4 value2, out Vector4 result)
146 { 146 {
147 result.W = value1.W + value2.W; 147 result.W = value1.W + value2.W;
148 result.X = value1.X + value2.X; 148 result.X = value1.X + value2.X;
149 result.Y = value1.Y + value2.Y; 149 result.Y = value1.Y + value2.Y;
150 result.Z = value1.Z + value2.Z; 150 result.Z = value1.Z + value2.Z;
151 } 151 }
152 152
153 /*public static Vector4 Barycentric(Vector4 value1, Vector4 value2, Vector4 value3, float amount1, float amount2) 153 /*public static Vector4 Barycentric(Vector4 value1, Vector4 value2, Vector4 value3, float amount1, float amount2)
154 { 154 {
155 return new Vector4( 155 return new Vector4(
156 MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2), 156 MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2),
157 MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2), 157 MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2),
158 MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2), 158 MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2),
159 MathHelper.Barycentric(value1.W, value2.W, value3.W, amount1, amount2)); 159 MathHelper.Barycentric(value1.W, value2.W, value3.W, amount1, amount2));
160 }*/ 160 }*/
161 161
162 /*public static void Barycentric(ref Vector4 value1, ref Vector4 value2, ref Vector4 value3, float amount1, float amount2, out Vector4 result) 162 /*public static void Barycentric(ref Vector4 value1, ref Vector4 value2, ref Vector4 value3, float amount1, float amount2, out Vector4 result)
163 { 163 {
164 result = new Vector4( 164 result = new Vector4(
165 MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2), 165 MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2),
166 MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2), 166 MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2),
167 MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2), 167 MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2),
168 MathHelper.Barycentric(value1.W, value2.W, value3.W, amount1, amount2)); 168 MathHelper.Barycentric(value1.W, value2.W, value3.W, amount1, amount2));
169 }*/ 169 }*/
170 170
171 /*public static Vector4 CatmullRom(Vector4 value1, Vector4 value2, Vector4 value3, Vector4 value4, float amount) 171 /*public static Vector4 CatmullRom(Vector4 value1, Vector4 value2, Vector4 value3, Vector4 value4, float amount)
172 { 172 {
173 return new Vector4( 173 return new Vector4(
174 MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount), 174 MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount),
175 MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, 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), 176 MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount),
177 MathHelper.CatmullRom(value1.W, value2.W, value3.W, value4.W, amount)); 177 MathHelper.CatmullRom(value1.W, value2.W, value3.W, value4.W, amount));
178 }*/ 178 }*/
179 179
180 /*public static void CatmullRom(ref Vector4 value1, ref Vector4 value2, ref Vector4 value3, ref Vector4 value4, float amount, out Vector4 result) 180 /*public static void CatmullRom(ref Vector4 value1, ref Vector4 value2, ref Vector4 value3, ref Vector4 value4, float amount, out Vector4 result)
181 { 181 {
182 result = new Vector4( 182 result = new Vector4(
183 MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount), 183 MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount),
184 MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, 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), 185 MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount),
186 MathHelper.CatmullRom(value1.W, value2.W, value3.W, value4.W, amount)); 186 MathHelper.CatmullRom(value1.W, value2.W, value3.W, value4.W, amount));
187 }*/ 187 }*/
188 188
189 /*public static Vector4 Clamp(Vector4 value1, Vector4 min, Vector4 max) 189 /*public static Vector4 Clamp(Vector4 value1, Vector4 min, Vector4 max)
190 { 190 {
191 return new Vector4( 191 return new Vector4(
192 MathHelper.Clamp(value1.X, min.X, max.X), 192 MathHelper.Clamp(value1.X, min.X, max.X),
193 MathHelper.Clamp(value1.Y, min.Y, max.Y), 193 MathHelper.Clamp(value1.Y, min.Y, max.Y),
194 MathHelper.Clamp(value1.Z, min.Z, max.Z), 194 MathHelper.Clamp(value1.Z, min.Z, max.Z),
195 MathHelper.Clamp(value1.W, min.W, max.W)); 195 MathHelper.Clamp(value1.W, min.W, max.W));
196 }*/ 196 }*/
197 197
198 /*public static void Clamp(ref Vector4 value1, ref Vector4 min, ref Vector4 max, out Vector4 result) 198 /*public static void Clamp(ref Vector4 value1, ref Vector4 min, ref Vector4 max, out Vector4 result)
199 { 199 {
200 result = new Vector4( 200 result = new Vector4(
201 MathHelper.Clamp(value1.X, min.X, max.X), 201 MathHelper.Clamp(value1.X, min.X, max.X),
202 MathHelper.Clamp(value1.Y, min.Y, max.Y), 202 MathHelper.Clamp(value1.Y, min.Y, max.Y),
203 MathHelper.Clamp(value1.Z, min.Z, max.Z), 203 MathHelper.Clamp(value1.Z, min.Z, max.Z),
204 MathHelper.Clamp(value1.W, min.W, max.W)); 204 MathHelper.Clamp(value1.W, min.W, max.W));
205 }*/ 205 }*/
206 206
207 public static float Distance(Vector4 value1, Vector4 value2) 207 public static float Distance(Vector4 value1, Vector4 value2)
208 { 208 {
209 return (float)Math.Sqrt(DistanceSquared(value1, value2)); 209 return (float)Math.Sqrt(DistanceSquared(value1, value2));
210 } 210 }
211 211
212 public static void Distance(ref Vector4 value1, ref Vector4 value2, out float result) 212 public static void Distance(ref Vector4 value1, ref Vector4 value2, out float result)
213 { 213 {
214 result = (float)Math.Sqrt(DistanceSquared(value1, value2)); 214 result = (float)Math.Sqrt(DistanceSquared(value1, value2));
215 } 215 }
216 216
217 public static float DistanceSquared(Vector4 value1, Vector4 value2) 217 public static float DistanceSquared(Vector4 value1, Vector4 value2)
218 { 218 {
219 float result; 219 float result;
220 DistanceSquared(ref value1, ref value2, out result); 220 DistanceSquared(ref value1, ref value2, out result);
221 return result; 221 return result;
222 } 222 }
223 223
224 public static void DistanceSquared(ref Vector4 value1, ref Vector4 value2, out float result) 224 public static void DistanceSquared(ref Vector4 value1, ref Vector4 value2, out float result)
225 { 225 {
226 result = (value1.W - value2.W) * (value1.W - value2.W) + 226 result = (value1.W - value2.W) * (value1.W - value2.W) +
227 (value1.X - value2.X) * (value1.X - value2.X) + 227 (value1.X - value2.X) * (value1.X - value2.X) +
228 (value1.Y - value2.Y) * (value1.Y - value2.Y) + 228 (value1.Y - value2.Y) * (value1.Y - value2.Y) +
229 (value1.Z - value2.Z) * (value1.Z - value2.Z); 229 (value1.Z - value2.Z) * (value1.Z - value2.Z);
230 } 230 }
231 231
232 public static Vector4 Divide(Vector4 value1, Vector4 value2) 232 public static Vector4 Divide(Vector4 value1, Vector4 value2)
233 { 233 {
234 value1.W /= value2.W; 234 value1.W /= value2.W;
235 value1.X /= value2.X; 235 value1.X /= value2.X;
236 value1.Y /= value2.Y; 236 value1.Y /= value2.Y;
237 value1.Z /= value2.Z; 237 value1.Z /= value2.Z;
238 return value1; 238 return value1;
239 } 239 }
240 240
241 public static Vector4 Divide(Vector4 value1, float divider) 241 public static Vector4 Divide(Vector4 value1, float divider)
242 { 242 {
243 float factor = 1f / divider; 243 float factor = 1f / divider;
244 value1.W *= factor; 244 value1.W *= factor;
245 value1.X *= factor; 245 value1.X *= factor;
246 value1.Y *= factor; 246 value1.Y *= factor;
247 value1.Z *= factor; 247 value1.Z *= factor;
248 return value1; 248 return value1;
249 } 249 }
250 250
251 public static void Divide(ref Vector4 value1, float divider, out Vector4 result) 251 public static void Divide(ref Vector4 value1, float divider, out Vector4 result)
252 { 252 {
253 float factor = 1f / divider; 253 float factor = 1f / divider;
254 result.W = value1.W * factor; 254 result.W = value1.W * factor;
255 result.X = value1.X * factor; 255 result.X = value1.X * factor;
256 result.Y = value1.Y * factor; 256 result.Y = value1.Y * factor;
257 result.Z = value1.Z * factor; 257 result.Z = value1.Z * factor;
258 } 258 }
259 259
260 public static void Divide(ref Vector4 value1, ref Vector4 value2, out Vector4 result) 260 public static void Divide(ref Vector4 value1, ref Vector4 value2, out Vector4 result)
261 { 261 {
262 result.W = value1.W / value2.W; 262 result.W = value1.W / value2.W;
263 result.X = value1.X / value2.X; 263 result.X = value1.X / value2.X;
264 result.Y = value1.Y / value2.Y; 264 result.Y = value1.Y / value2.Y;
265 result.Z = value1.Z / value2.Z; 265 result.Z = value1.Z / value2.Z;
266 } 266 }
267 267
268 public static float Dot(Vector4 vector1, Vector4 vector2) 268 public static float Dot(Vector4 vector1, Vector4 vector2)
269 { 269 {
270 return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z + vector1.W * vector2.W; 270 return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z + vector1.W * vector2.W;
271 } 271 }
272 272
273 public static void Dot(ref Vector4 vector1, ref Vector4 vector2, out float result) 273 public static void Dot(ref Vector4 vector1, ref Vector4 vector2, out float result)
274 { 274 {
275 result = vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z + vector1.W * vector2.W; 275 result = vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z + vector1.W * vector2.W;
276 } 276 }
277 277
278 public override bool Equals(object obj) 278 public override bool Equals(object obj)
279 { 279 {
280 return (obj is Vector4) ? this == (Vector4)obj : false; 280 return (obj is Vector4) ? this == (Vector4)obj : false;
281 } 281 }
282 282
283 public bool Equals(Vector4 other) 283 public bool Equals(Vector4 other)
284 { 284 {
285 return this.W == other.W 285 return this.W == other.W
286 && this.X == other.X 286 && this.X == other.X
287 && this.Y == other.Y 287 && this.Y == other.Y
288 && this.Z == other.Z; 288 && this.Z == other.Z;
289 } 289 }
290 290
291 public override int GetHashCode() 291 public override int GetHashCode()
292 { 292 {
293 return (int)(this.W + this.X + this.Y + this.Y); 293 return (int)(this.W + this.X + this.Y + this.Y);
294 } 294 }
295 295
296 /*public static Vector4 Hermite(Vector4 value1, Vector4 tangent1, Vector4 value2, Vector4 tangent2, float amount) 296 /*public static Vector4 Hermite(Vector4 value1, Vector4 tangent1, Vector4 value2, Vector4 tangent2, float amount)
297 { 297 {
298 Vector4 result = new Vector4(); 298 Vector4 result = new Vector4();
299 Hermite(ref value1, ref tangent1, ref value2, ref tangent2, amount, out result); 299 Hermite(ref value1, ref tangent1, ref value2, ref tangent2, amount, out result);
300 return result; 300 return result;
301 }*/ 301 }*/
302 302
303 /*public static void Hermite(ref Vector4 value1, ref Vector4 tangent1, ref Vector4 value2, ref Vector4 tangent2, float amount, out Vector4 result) 303 /*public static void Hermite(ref Vector4 value1, ref Vector4 tangent1, ref Vector4 value2, ref Vector4 tangent2, float amount, out Vector4 result)
304 { 304 {
305 result.W = MathHelper.Hermite(value1.W, tangent1.W, value2.W, tangent2.W, amount); 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); 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); 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); 308 result.Z = MathHelper.Hermite(value1.Z, tangent1.Z, value2.Z, tangent2.Z, amount);
309 }*/ 309 }*/
310 310
311 public float Length() 311 public float Length()
312 { 312 {
313 float result; 313 float result;
314 DistanceSquared(ref this, ref zeroVector, out result); 314 DistanceSquared(ref this, ref zeroVector, out result);
315 return (float)Math.Sqrt(result); 315 return (float)Math.Sqrt(result);
316 } 316 }
317 317
318 public float LengthSquared() 318 public float LengthSquared()
319 { 319 {
320 float result; 320 float result;
321 DistanceSquared(ref this, ref zeroVector, out result); 321 DistanceSquared(ref this, ref zeroVector, out result);
322 return result; 322 return result;
323 } 323 }
324 324
325 /*public static Vector4 Lerp(Vector4 value1, Vector4 value2, float amount) 325 /*public static Vector4 Lerp(Vector4 value1, Vector4 value2, float amount)
326 { 326 {
327 return new Vector4( 327 return new Vector4(
328 MathHelper.Lerp(value1.X, value2.X, amount), 328 MathHelper.Lerp(value1.X, value2.X, amount),
329 MathHelper.Lerp(value1.Y, value2.Y, amount), 329 MathHelper.Lerp(value1.Y, value2.Y, amount),
330 MathHelper.Lerp(value1.Z, value2.Z, amount), 330 MathHelper.Lerp(value1.Z, value2.Z, amount),
331 MathHelper.Lerp(value1.W, value2.W, amount)); 331 MathHelper.Lerp(value1.W, value2.W, amount));
332 }*/ 332 }*/
333 333
334 /*public static void Lerp(ref Vector4 value1, ref Vector4 value2, float amount, out Vector4 result) 334 /*public static void Lerp(ref Vector4 value1, ref Vector4 value2, float amount, out Vector4 result)
335 { 335 {
336 result = new Vector4( 336 result = new Vector4(
337 MathHelper.Lerp(value1.X, value2.X, amount), 337 MathHelper.Lerp(value1.X, value2.X, amount),
338 MathHelper.Lerp(value1.Y, value2.Y, amount), 338 MathHelper.Lerp(value1.Y, value2.Y, amount),
339 MathHelper.Lerp(value1.Z, value2.Z, amount), 339 MathHelper.Lerp(value1.Z, value2.Z, amount),
340 MathHelper.Lerp(value1.W, value2.W, amount)); 340 MathHelper.Lerp(value1.W, value2.W, amount));
341 }*/ 341 }*/
342 342
343 /*public static Vector4 Max(Vector4 value1, Vector4 value2) 343 /*public static Vector4 Max(Vector4 value1, Vector4 value2)
344 { 344 {
345 return new Vector4( 345 return new Vector4(
346 MathHelper.Max(value1.X, value2.X), 346 MathHelper.Max(value1.X, value2.X),
347 MathHelper.Max(value1.Y, value2.Y), 347 MathHelper.Max(value1.Y, value2.Y),
348 MathHelper.Max(value1.Z, value2.Z), 348 MathHelper.Max(value1.Z, value2.Z),
349 MathHelper.Max(value1.W, value2.W)); 349 MathHelper.Max(value1.W, value2.W));
350 }*/ 350 }*/
351 351
352 /*public static void Max(ref Vector4 value1, ref Vector4 value2, out Vector4 result) 352 /*public static void Max(ref Vector4 value1, ref Vector4 value2, out Vector4 result)
353 { 353 {
354 result = new Vector4( 354 result = new Vector4(
355 MathHelper.Max(value1.X, value2.X), 355 MathHelper.Max(value1.X, value2.X),
356 MathHelper.Max(value1.Y, value2.Y), 356 MathHelper.Max(value1.Y, value2.Y),
357 MathHelper.Max(value1.Z, value2.Z), 357 MathHelper.Max(value1.Z, value2.Z),
358 MathHelper.Max(value1.W, value2.W)); 358 MathHelper.Max(value1.W, value2.W));
359 }*/ 359 }*/
360 360
361 /*public static Vector4 Min(Vector4 value1, Vector4 value2) 361 /*public static Vector4 Min(Vector4 value1, Vector4 value2)
362 { 362 {
363 return new Vector4( 363 return new Vector4(
364 MathHelper.Min(value1.X, value2.X), 364 MathHelper.Min(value1.X, value2.X),
365 MathHelper.Min(value1.Y, value2.Y), 365 MathHelper.Min(value1.Y, value2.Y),
366 MathHelper.Min(value1.Z, value2.Z), 366 MathHelper.Min(value1.Z, value2.Z),
367 MathHelper.Min(value1.W, value2.W)); 367 MathHelper.Min(value1.W, value2.W));
368 }*/ 368 }*/
369 369
370 /*public static void Min(ref Vector4 value1, ref Vector4 value2, out Vector4 result) 370 /*public static void Min(ref Vector4 value1, ref Vector4 value2, out Vector4 result)
371 { 371 {
372 result = new Vector4( 372 result = new Vector4(
373 MathHelper.Min(value1.X, value2.X), 373 MathHelper.Min(value1.X, value2.X),
374 MathHelper.Min(value1.Y, value2.Y), 374 MathHelper.Min(value1.Y, value2.Y),
375 MathHelper.Min(value1.Z, value2.Z), 375 MathHelper.Min(value1.Z, value2.Z),
376 MathHelper.Min(value1.W, value2.W)); 376 MathHelper.Min(value1.W, value2.W));
377 }*/ 377 }*/
378 378
379 public static Vector4 Multiply(Vector4 value1, Vector4 value2) 379 public static Vector4 Multiply(Vector4 value1, Vector4 value2)
380 { 380 {
381 value1.W *= value2.W; 381 value1.W *= value2.W;
382 value1.X *= value2.X; 382 value1.X *= value2.X;
383 value1.Y *= value2.Y; 383 value1.Y *= value2.Y;
384 value1.Z *= value2.Z; 384 value1.Z *= value2.Z;
385 return value1; 385 return value1;
386 } 386 }
387 387
388 public static Vector4 Multiply(Vector4 value1, float scaleFactor) 388 public static Vector4 Multiply(Vector4 value1, float scaleFactor)
389 { 389 {
390 value1.W *= scaleFactor; 390 value1.W *= scaleFactor;
391 value1.X *= scaleFactor; 391 value1.X *= scaleFactor;
392 value1.Y *= scaleFactor; 392 value1.Y *= scaleFactor;
393 value1.Z *= scaleFactor; 393 value1.Z *= scaleFactor;
394 return value1; 394 return value1;
395 } 395 }
396 396
397 public static void Multiply(ref Vector4 value1, float scaleFactor, out Vector4 result) 397 public static void Multiply(ref Vector4 value1, float scaleFactor, out Vector4 result)
398 { 398 {
399 result.W = value1.W * scaleFactor; 399 result.W = value1.W * scaleFactor;
400 result.X = value1.X * scaleFactor; 400 result.X = value1.X * scaleFactor;
401 result.Y = value1.Y * scaleFactor; 401 result.Y = value1.Y * scaleFactor;
402 result.Z = value1.Z * scaleFactor; 402 result.Z = value1.Z * scaleFactor;
403 } 403 }
404 404
405 public static void Multiply(ref Vector4 value1, ref Vector4 value2, out Vector4 result) 405 public static void Multiply(ref Vector4 value1, ref Vector4 value2, out Vector4 result)
406 { 406 {
407 result.W = value1.W * value2.W; 407 result.W = value1.W * value2.W;
408 result.X = value1.X * value2.X; 408 result.X = value1.X * value2.X;
409 result.Y = value1.Y * value2.Y; 409 result.Y = value1.Y * value2.Y;
410 result.Z = value1.Z * value2.Z; 410 result.Z = value1.Z * value2.Z;
411 } 411 }
412 412
413 public static Vector4 Negate(Vector4 value) 413 public static Vector4 Negate(Vector4 value)
414 { 414 {
415 value = new Vector4(-value.X, -value.Y, -value.Z, -value.W); 415 value = new Vector4(-value.X, -value.Y, -value.Z, -value.W);
416 return value; 416 return value;
417 } 417 }
418 418
419 public static void Negate(ref Vector4 value, out Vector4 result) 419 public static void Negate(ref Vector4 value, out Vector4 result)
420 { 420 {
421 result = new Vector4(-value.X, -value.Y, -value.Z,-value.W); 421 result = new Vector4(-value.X, -value.Y, -value.Z,-value.W);
422 } 422 }
423 423
424 public void Normalize() 424 public void Normalize()
425 { 425 {
426 Normalize(ref this, out this); 426 Normalize(ref this, out this);
427 } 427 }
428 428
429 public static Vector4 Normalize(Vector4 vector) 429 public static Vector4 Normalize(Vector4 vector)
430 { 430 {
431 Normalize(ref vector, out vector); 431 Normalize(ref vector, out vector);
432 return vector; 432 return vector;
433 } 433 }
434 434
435 public static void Normalize(ref Vector4 vector, out Vector4 result) 435 public static void Normalize(ref Vector4 vector, out Vector4 result)
436 { 436 {
437 float factor; 437 float factor;
438 DistanceSquared(ref vector, ref zeroVector, out factor); 438 DistanceSquared(ref vector, ref zeroVector, out factor);
439 factor = 1f / (float)Math.Sqrt(factor); 439 factor = 1f / (float)Math.Sqrt(factor);
440 440
441 result.W = vector.W * factor; 441 result.W = vector.W * factor;
442 result.X = vector.X * factor; 442 result.X = vector.X * factor;
443 result.Y = vector.Y * factor; 443 result.Y = vector.Y * factor;
444 result.Z = vector.Z * factor; 444 result.Z = vector.Z * factor;
445 } 445 }
446 446
447 /*public static Vector4 SmoothStep(Vector4 value1, Vector4 value2, float amount) 447 /*public static Vector4 SmoothStep(Vector4 value1, Vector4 value2, float amount)
448 { 448 {
449 return new Vector4( 449 return new Vector4(
450 MathHelper.SmoothStep(value1.X, value2.X, amount), 450 MathHelper.SmoothStep(value1.X, value2.X, amount),
451 MathHelper.SmoothStep(value1.Y, value2.Y, amount), 451 MathHelper.SmoothStep(value1.Y, value2.Y, amount),
452 MathHelper.SmoothStep(value1.Z, value2.Z, amount), 452 MathHelper.SmoothStep(value1.Z, value2.Z, amount),
453 MathHelper.SmoothStep(value1.W, value2.W, amount)); 453 MathHelper.SmoothStep(value1.W, value2.W, amount));
454 }*/ 454 }*/
455 455
456 /*public static void SmoothStep(ref Vector4 value1, ref Vector4 value2, float amount, out Vector4 result) 456 /*public static void SmoothStep(ref Vector4 value1, ref Vector4 value2, float amount, out Vector4 result)
457 { 457 {
458 result = new Vector4( 458 result = new Vector4(
459 MathHelper.SmoothStep(value1.X, value2.X, amount), 459 MathHelper.SmoothStep(value1.X, value2.X, amount),
460 MathHelper.SmoothStep(value1.Y, value2.Y, amount), 460 MathHelper.SmoothStep(value1.Y, value2.Y, amount),
461 MathHelper.SmoothStep(value1.Z, value2.Z, amount), 461 MathHelper.SmoothStep(value1.Z, value2.Z, amount),
462 MathHelper.SmoothStep(value1.W, value2.W, amount)); 462 MathHelper.SmoothStep(value1.W, value2.W, amount));
463 }*/ 463 }*/
464 464
465 public static Vector4 Subtract(Vector4 value1, Vector4 value2) 465 public static Vector4 Subtract(Vector4 value1, Vector4 value2)
466 { 466 {
467 value1.W -= value2.W; 467 value1.W -= value2.W;
468 value1.X -= value2.X; 468 value1.X -= value2.X;
469 value1.Y -= value2.Y; 469 value1.Y -= value2.Y;
470 value1.Z -= value2.Z; 470 value1.Z -= value2.Z;
471 return value1; 471 return value1;
472 } 472 }
473 473
474 public static void Subtract(ref Vector4 value1, ref Vector4 value2, out Vector4 result) 474 public static void Subtract(ref Vector4 value1, ref Vector4 value2, out Vector4 result)
475 { 475 {
476 result.W = value1.W - value2.W; 476 result.W = value1.W - value2.W;
477 result.X = value1.X - value2.X; 477 result.X = value1.X - value2.X;
478 result.Y = value1.Y - value2.Y; 478 result.Y = value1.Y - value2.Y;
479 result.Z = value1.Z - value2.Z; 479 result.Z = value1.Z - value2.Z;
480 } 480 }
481 481
482 /*public static Vector4 Transform(Vector2 position, Matrix matrix) 482 /*public static Vector4 Transform(Vector2 position, Matrix matrix)
483 { 483 {
484 Vector4 result; 484 Vector4 result;
485 Transform(ref position, ref matrix, out result); 485 Transform(ref position, ref matrix, out result);
486 return result; 486 return result;
487 }*/ 487 }*/
488 488
489 public static Vector4 Transform(Vector3 position, Matrix matrix) 489 public static Vector4 Transform(Vector3 position, Matrix matrix)
490 { 490 {
491 Vector4 result; 491 Vector4 result;
492 Transform(ref position, ref matrix, out result); 492 Transform(ref position, ref matrix, out result);
493 return result; 493 return result;
494 } 494 }
495 495
496 public static Vector4 Transform(Vector4 vector, Matrix matrix) 496 public static Vector4 Transform(Vector4 vector, Matrix matrix)
497 { 497 {
498 Transform(ref vector, ref matrix, out vector); 498 Transform(ref vector, ref matrix, out vector);
499 return vector; 499 return vector;
500 } 500 }
501 501
502 /*public static void Transform(ref Vector2 position, ref Matrix matrix, out Vector4 result) 502 /*public static void Transform(ref Vector2 position, ref Matrix matrix, out Vector4 result)
503 { 503 {
504 result = new Vector4((position.X * matrix.M11) + (position.Y * matrix.M21) + matrix.M41, 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, 505 (position.X * matrix.M12) + (position.Y * matrix.M22) + matrix.M42,
506 (position.X * matrix.M13) + (position.Y * matrix.M23) + matrix.M43, 506 (position.X * matrix.M13) + (position.Y * matrix.M23) + matrix.M43,
507 (position.X * matrix.M14) + (position.Y * matrix.M24) + matrix.M44); 507 (position.X * matrix.M14) + (position.Y * matrix.M24) + matrix.M44);
508 }*/ 508 }*/
509 509
510 public static void Transform(ref Vector3 position, ref Matrix matrix, out Vector4 result) 510 public static void Transform(ref Vector3 position, ref Matrix matrix, out Vector4 result)
511 { 511 {
512 result = new Vector4((position.X * matrix.M11) + (position.Y * matrix.M21) + (position.Z * matrix.M31) + matrix.M41, 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, 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, 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); 515 (position.X * matrix.M14) + (position.Y * matrix.M24) + (position.Z * matrix.M34) + matrix.M44);
516 } 516 }
517 517
518 public static void Transform(ref Vector4 vector, ref Matrix matrix, out Vector4 result) 518 public static void Transform(ref Vector4 vector, ref Matrix matrix, out Vector4 result)
519 { 519 {
520 result = new Vector4((vector.X * matrix.M11) + (vector.Y * matrix.M21) + (vector.Z * matrix.M31) + (vector.W * matrix.M41), 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), 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), 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)); 523 (vector.X * matrix.M14) + (vector.Y * matrix.M24) + (vector.Z * matrix.M34) + (vector.W * matrix.M44));
524 } 524 }
525 525
526 public override string ToString() 526 public override string ToString()
527 { 527 {
528 StringBuilder sb = new StringBuilder(32); 528 StringBuilder sb = new StringBuilder(32);
529 sb.Append("{X:"); 529 sb.Append("{X:");
530 sb.Append(this.X); 530 sb.Append(this.X);
531 sb.Append(" Y:"); 531 sb.Append(" Y:");
532 sb.Append(this.Y); 532 sb.Append(this.Y);
533 sb.Append(" Z:"); 533 sb.Append(" Z:");
534 sb.Append(this.Z); 534 sb.Append(this.Z);
535 sb.Append(" W:"); 535 sb.Append(" W:");
536 sb.Append(this.W); 536 sb.Append(this.W);
537 sb.Append("}"); 537 sb.Append("}");
538 return sb.ToString(); 538 return sb.ToString();
539 } 539 }
540 540
541 #endregion Public Methods 541 #endregion Public Methods
542 542
543 543
544 #region Operators 544 #region Operators
545 545
546 public static Vector4 operator -(Vector4 value) 546 public static Vector4 operator -(Vector4 value)
547 { 547 {
548 return new Vector4(-value.X, -value.Y, -value.Z, -value.W); 548 return new Vector4(-value.X, -value.Y, -value.Z, -value.W);
549 } 549 }
550 550
551 public static bool operator ==(Vector4 value1, Vector4 value2) 551 public static bool operator ==(Vector4 value1, Vector4 value2)
552 { 552 {
553 return value1.W == value2.W 553 return value1.W == value2.W
554 && value1.X == value2.X 554 && value1.X == value2.X
555 && value1.Y == value2.Y 555 && value1.Y == value2.Y
556 && value1.Z == value2.Z; 556 && value1.Z == value2.Z;
557 } 557 }
558 558
559 public static bool operator !=(Vector4 value1, Vector4 value2) 559 public static bool operator !=(Vector4 value1, Vector4 value2)
560 { 560 {
561 return !(value1 == value2); 561 return !(value1 == value2);
562 } 562 }
563 563
564 public static Vector4 operator +(Vector4 value1, Vector4 value2) 564 public static Vector4 operator +(Vector4 value1, Vector4 value2)
565 { 565 {
566 value1.W += value2.W; 566 value1.W += value2.W;
567 value1.X += value2.X; 567 value1.X += value2.X;
568 value1.Y += value2.Y; 568 value1.Y += value2.Y;
569 value1.Z += value2.Z; 569 value1.Z += value2.Z;
570 return value1; 570 return value1;
571 } 571 }
572 572
573 public static Vector4 operator -(Vector4 value1, Vector4 value2) 573 public static Vector4 operator -(Vector4 value1, Vector4 value2)
574 { 574 {
575 value1.W -= value2.W; 575 value1.W -= value2.W;
576 value1.X -= value2.X; 576 value1.X -= value2.X;
577 value1.Y -= value2.Y; 577 value1.Y -= value2.Y;
578 value1.Z -= value2.Z; 578 value1.Z -= value2.Z;
579 return value1; 579 return value1;
580 } 580 }
581 581
582 public static Vector4 operator *(Vector4 value1, Vector4 value2) 582 public static Vector4 operator *(Vector4 value1, Vector4 value2)
583 { 583 {
584 value1.W *= value2.W; 584 value1.W *= value2.W;
585 value1.X *= value2.X; 585 value1.X *= value2.X;
586 value1.Y *= value2.Y; 586 value1.Y *= value2.Y;
587 value1.Z *= value2.Z; 587 value1.Z *= value2.Z;
588 return value1; 588 return value1;
589 } 589 }
590 590
591 public static Vector4 operator *(Vector4 value1, float scaleFactor) 591 public static Vector4 operator *(Vector4 value1, float scaleFactor)
592 { 592 {
593 value1.W *= scaleFactor; 593 value1.W *= scaleFactor;
594 value1.X *= scaleFactor; 594 value1.X *= scaleFactor;
595 value1.Y *= scaleFactor; 595 value1.Y *= scaleFactor;
596 value1.Z *= scaleFactor; 596 value1.Z *= scaleFactor;
597 return value1; 597 return value1;
598 } 598 }
599 599
600 public static Vector4 operator *(float scaleFactor, Vector4 value1) 600 public static Vector4 operator *(float scaleFactor, Vector4 value1)
601 { 601 {
602 value1.W *= scaleFactor; 602 value1.W *= scaleFactor;
603 value1.X *= scaleFactor; 603 value1.X *= scaleFactor;
604 value1.Y *= scaleFactor; 604 value1.Y *= scaleFactor;
605 value1.Z *= scaleFactor; 605 value1.Z *= scaleFactor;
606 return value1; 606 return value1;
607 } 607 }
608 608
609 public static Vector4 operator /(Vector4 value1, Vector4 value2) 609 public static Vector4 operator /(Vector4 value1, Vector4 value2)
610 { 610 {
611 value1.W /= value2.W; 611 value1.W /= value2.W;
612 value1.X /= value2.X; 612 value1.X /= value2.X;
613 value1.Y /= value2.Y; 613 value1.Y /= value2.Y;
614 value1.Z /= value2.Z; 614 value1.Z /= value2.Z;
615 return value1; 615 return value1;
616 } 616 }
617 617
618 public static Vector4 operator /(Vector4 value1, float divider) 618 public static Vector4 operator /(Vector4 value1, float divider)
619 { 619 {
620 float factor = 1f / divider; 620 float factor = 1f / divider;
621 value1.W *= factor; 621 value1.W *= factor;
622 value1.X *= factor; 622 value1.X *= factor;
623 value1.Y *= factor; 623 value1.Y *= factor;
624 value1.Z *= factor; 624 value1.Z *= factor;
625 return value1; 625 return value1;
626 } 626 }
627 627
628 #endregion Operators 628 #endregion Operators
629 } 629 }
630} 630}