From 2a3c79df83e800d5dfe75a1a3b140ed81da2b1d6 Mon Sep 17 00:00:00 2001 From: Sean Dague Date: Mon, 16 Jul 2007 15:40:11 +0000 Subject: changed to native line ending encoding --- libraries/ModifiedBulletX/ModifiedBulletX.sln | 84 +- .../ModifiedBulletX/ModifiedBulletX/BulletDebug.cs | 136 +- .../Collision/BroadphaseCollision/AxisSweep3.cs | 1246 ++++++------- .../BroadphaseCollision/BroadphaseNativeTypes.cs | 136 +- .../BroadphaseCollision/BroadphasePair.cs | 226 +-- .../BroadphaseCollision/BroadphaseProxy.cs | 182 +- .../BroadphaseCollision/CollisionAlgorithm.cs | 102 +- .../CollisionAlgorithmConstructionInfo.cs | 84 +- .../BroadphaseCollision/DispatcherInfo.cs | 108 +- .../Collision/BroadphaseCollision/IBroadphase.cs | 72 +- .../Collision/BroadphaseCollision/IDispatcher.cs | 84 +- .../BroadphaseCollision/IOverlapCallback.cs | 66 +- .../BroadphaseCollision/OverlappingPairCache.cs | 318 ++-- .../BroadphaseCollision/SimpleBroadphase.cs | 256 +-- .../BroadphaseCollision/SimpleBroadphaseProxy.cs | 92 +- .../BridgeTriangleRaycastCallback.cs | 118 +- .../CollisionAlgorithmCreateFunc.cs | 80 +- .../CollisionDispatch/CollisionDispatcher.cs | 560 +++--- .../Collision/CollisionDispatch/CollisionObject.cs | 326 ++-- .../CollisionDispatch/CollisionPairCallback.cs | 96 +- .../Collision/CollisionDispatch/CollisionWorld.cs | 716 ++++---- .../CompoundCollisionAlgorithm.cs | 314 ++-- .../ConvexConcaveCollisionAlgorithm.cs | 378 ++-- .../ConvexConvexCollisionAlgorithm.cs | 386 ++--- .../CollisionDispatch/ConvexTriangleCallback.cs | 260 +-- .../Collision/CollisionDispatch/EmptyAlgorithm.cs | 104 +- .../Collision/CollisionDispatch/ManifoldResult.cs | 294 ++-- .../CollisionDispatch/SimulationIslandManager.cs | 608 +++---- .../SphereBoxCollisionAlgorithm.cs | 540 +++--- .../SphereSphereCollisionAlgorithm.cs | 208 +-- .../SphereTriangleCollisionAlgorithm.cs | 200 +-- .../CollisionDispatch/SphereTriangleDetector.cs | 426 ++--- .../Collision/CollisionDispatch/UnionFind.cs | 300 ++-- .../Collision/CollisionShapes/BUSimplex1to4.cs | 430 ++--- .../Collision/CollisionShapes/BoxShape.cs | 632 +++---- .../CollisionShapes/BvhTriangleMeshShape.cs | 166 +- .../Collision/CollisionShapes/CollisionShape.cs | 296 ++-- .../Collision/CollisionShapes/CompoundShape.cs | 366 ++-- .../Collision/CollisionShapes/ConcaveShape.cs | 108 +- .../Collision/CollisionShapes/ConeShape.cs | 416 ++--- .../Collision/CollisionShapes/ConvexHullShape.cs | 368 ++-- .../Collision/CollisionShapes/ConvexShape.cs | 282 +-- .../CollisionShapes/ConvexTriangleMeshShape.cs | 370 ++-- .../Collision/CollisionShapes/CylinderShape.cs | 272 +-- .../Collision/CollisionShapes/CylinderShapeX.cs | 200 +-- .../Collision/CollisionShapes/CylinderShapeZ.cs | 200 +-- .../Collision/CollisionShapes/EmptyShape.cs | 160 +- .../Collision/CollisionShapes/FilteredCallback.cs | 110 +- .../InternalTriangleIndexCallback.cs | 66 +- .../CollisionShapes/LocalSupportVertexCallback.cs | 116 +- .../Collision/CollisionShapes/MinkowskiSumShape.cs | 198 +-- .../Collision/CollisionShapes/MultiSphereShape.cs | 308 ++-- .../CollisionShapes/NodeOverlapCallback.cs | 64 +- .../Collision/CollisionShapes/OptimizedBvh.cs | 586 +++---- .../Collision/CollisionShapes/OptimizedBvhNode.cs | 126 +- .../CollisionShapes/PolyhedralConvexShape.cs | 266 +-- .../Collision/CollisionShapes/SphereShape.cs | 232 +-- .../Collision/CollisionShapes/StaticPlaneShape.cs | 248 +-- .../CollisionShapes/StridingMeshInterface.cs | 230 +-- .../CollisionShapes/SupportVertexCallback.cs | 134 +- .../Collision/CollisionShapes/TriangleBuffer.cs | 160 +- .../Collision/CollisionShapes/TriangleCallback.cs | 66 +- .../CollisionShapes/TriangleIndexVertexArray.cs | 272 +-- .../Collision/CollisionShapes/TriangleMesh.cs | 204 +-- .../Collision/CollisionShapes/TriangleMeshShape.cs | 320 ++-- .../Collision/CollisionShapes/TriangleShape.cs | 374 ++-- .../ContinuousConvexCollision.cs | 398 ++--- .../Collision/NarrowPhaseCollision/ConvexCast.cs | 146 +- .../DiscreteCollisionDetectorInterface.cs | 234 +-- .../NarrowPhaseCollision/GjkConvexCast.cs | 352 ++-- .../Collision/NarrowPhaseCollision/GjkEpa.cs | 1266 +++++++------- .../GjkEpaPenetrationDepthSolver.cs | 112 +- .../Collision/NarrowPhaseCollision/GjkEpaSolver.cs | 202 +-- .../NarrowPhaseCollision/GjkPairDetector.cs | 684 ++++---- .../IConvexPenetrationDepthSolver.cs | 84 +- .../NarrowPhaseCollision/ISimplexSolver.cs | 94 +- .../NarrowPhaseCollision/ManifoldPoint.cs | 156 +- .../MinkowskiPenetrationDepthSolver.cs | 492 +++--- .../NarrowPhaseCollision/PersistentManifold.cs | 544 +++--- .../NarrowPhaseCollision/PointCollector.cs | 128 +- .../NarrowPhaseCollision/SubsimplexConvexCast.cs | 284 +-- .../TriangleRaycastCallback.cs | 230 +-- .../NarrowPhaseCollision/VoronoiSimplexSolver.cs | 1286 +++++++------- .../Dynamics/ConstraintSolver/ContactConstraint.cs | 976 +++++------ .../Dynamics/ConstraintSolver/ContactSolverInfo.cs | 124 +- .../ConstraintSolver/Generic6DofConstraint.cs | 880 +++++----- .../Dynamics/ConstraintSolver/HingeConstraint.cs | 492 +++--- .../Dynamics/ConstraintSolver/IConstraintSolver.cs | 64 +- .../Dynamics/ConstraintSolver/JacobianEntry.cs | 310 ++-- .../ConstraintSolver/Point2PointConstraint.cs | 312 ++-- .../SequentialImpulseConstraintSolver.cs | 1830 ++++++++++---------- .../ConstraintSolver/Solve2LinearConstraint.cs | 376 ++-- .../Dynamics/ConstraintSolver/SolverBody.cs | 156 +- .../Dynamics/ConstraintSolver/SolverConstraint.cs | 154 +- .../Dynamics/ConstraintSolver/TypedConstraint.cs | 176 +- .../Dynamics/DiscreteDynamicsWorld.cs | 1578 ++++++++--------- .../ModifiedBulletX/Dynamics/DynamicsWorld.cs | 118 +- .../ModifiedBulletX/Dynamics/RigidBody.cs | 894 +++++----- .../Dynamics/SimpleDynamicsWorld.cs | 422 ++--- .../Dynamics/Vehicle/RaycastVehicle.cs | 186 +- .../Dynamics/Vehicle/VehicleRaycaster.cs | 98 +- .../ModifiedBulletX/Dynamics/Vehicle/WheelInfo.cs | 1058 +++++------ .../ModifiedBulletX/Exceptions/BulletException.cs | 106 +- .../ModifiedBulletX/ModifiedBulletX/IDebugDraw.cs | 118 +- .../LinearMath/DefaultMotionState.cs | 132 +- .../ModifiedBulletX/LinearMath/MathHelper.cs | 1162 ++++++------- .../ModifiedBulletX/LinearMath/MatrixOperations.cs | 232 +-- .../ModifiedBulletX/LinearMath/MotionState.cs | 88 +- .../ModifiedBulletX/LinearMath/QuadWord.cs | 176 +- .../ModifiedBulletX/LinearMath/Quaternion.cs | 396 ++--- .../ModifiedBulletX/LinearMath/TransformUtil.cs | 204 +-- .../ModifiedBulletX/LinearMath/Vector3.cs | 442 ++--- .../ModifiedBulletX/LinearMath/Vector4.cs | 220 +-- .../Modified.XnaDevRu.BulletX.csproj | 334 ++-- .../ModifiedBulletX/Properties/AssemblyInfo.cs | 70 +- .../ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs | 1366 +++++++-------- .../MonoXnaCompactMaths/MonoXnaCompactMaths.csproj | 100 +- .../MonoXnaCompactMaths/Properties/AssemblyInfo.cs | 66 +- .../MonoXnaCompactMaths/Quaternion.cs | 692 ++++---- .../ModifiedBulletX/MonoXnaCompactMaths/Vector3.cs | 1240 ++++++------- .../ModifiedBulletX/MonoXnaCompactMaths/Vector4.cs | 1260 +++++++------- 121 files changed, 21376 insertions(+), 21376 deletions(-) (limited to 'libraries') diff --git a/libraries/ModifiedBulletX/ModifiedBulletX.sln b/libraries/ModifiedBulletX/ModifiedBulletX.sln index 0eccfdd..79ae566 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX.sln +++ b/libraries/ModifiedBulletX/ModifiedBulletX.sln @@ -1,42 +1,42 @@ - -Microsoft Visual Studio Solution File, Format Version 9.00 -# Visual C# Express 2005 -Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "MonoXnaCompactMaths", "MonoXnaCompactMaths\MonoXnaCompactMaths.csproj", "{121147BC-B06B-406C-84E9-907F268CF0EB}" -EndProject -Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Modified.XnaDevRu.BulletX", "ModifiedBulletX\Modified.XnaDevRu.BulletX.csproj", "{44270344-ACA7-4875-B585-81D5C06D0489}" -EndProject -Global - GlobalSection(SolutionConfigurationPlatforms) = preSolution - Debug|Any CPU = Debug|Any CPU - Debug|Mixed Platforms = Debug|Mixed Platforms - Debug|x86 = Debug|x86 - Release|Any CPU = Release|Any CPU - Release|Mixed Platforms = Release|Mixed Platforms - Release|x86 = Release|x86 - EndGlobalSection - GlobalSection(ProjectConfigurationPlatforms) = postSolution - {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|Any CPU.ActiveCfg = Debug|Any CPU - {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|Any CPU.Build.0 = Debug|Any CPU - {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|Mixed Platforms.ActiveCfg = Debug|Any CPU - {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|Mixed Platforms.Build.0 = Debug|Any CPU - {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|x86.ActiveCfg = Debug|Any CPU - {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|Any CPU.ActiveCfg = Release|Any CPU - {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|Any CPU.Build.0 = Release|Any CPU - {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|Mixed Platforms.ActiveCfg = Release|Any CPU - {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|Mixed Platforms.Build.0 = Release|Any CPU - {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|x86.ActiveCfg = Release|Any CPU - {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|Any CPU.ActiveCfg = Debug|Any CPU - {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|Any CPU.Build.0 = Debug|Any CPU - {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|Mixed Platforms.ActiveCfg = Debug|Any CPU - {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|Mixed Platforms.Build.0 = Debug|Any CPU - {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|x86.ActiveCfg = Debug|Any CPU - {44270344-ACA7-4875-B585-81D5C06D0489}.Release|Any CPU.ActiveCfg = Release|Any CPU - {44270344-ACA7-4875-B585-81D5C06D0489}.Release|Any CPU.Build.0 = Release|Any CPU - {44270344-ACA7-4875-B585-81D5C06D0489}.Release|Mixed Platforms.ActiveCfg = Release|Any CPU - {44270344-ACA7-4875-B585-81D5C06D0489}.Release|Mixed Platforms.Build.0 = Release|Any CPU - {44270344-ACA7-4875-B585-81D5C06D0489}.Release|x86.ActiveCfg = Release|Any CPU - EndGlobalSection - GlobalSection(SolutionProperties) = preSolution - HideSolutionNode = FALSE - EndGlobalSection -EndGlobal + +Microsoft Visual Studio Solution File, Format Version 9.00 +# Visual C# Express 2005 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "MonoXnaCompactMaths", "MonoXnaCompactMaths\MonoXnaCompactMaths.csproj", "{121147BC-B06B-406C-84E9-907F268CF0EB}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Modified.XnaDevRu.BulletX", "ModifiedBulletX\Modified.XnaDevRu.BulletX.csproj", "{44270344-ACA7-4875-B585-81D5C06D0489}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|Mixed Platforms = Debug|Mixed Platforms + Debug|x86 = Debug|x86 + Release|Any CPU = Release|Any CPU + Release|Mixed Platforms = Release|Mixed Platforms + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|Any CPU.Build.0 = Debug|Any CPU + {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|Mixed Platforms.ActiveCfg = Debug|Any CPU + {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|Mixed Platforms.Build.0 = Debug|Any CPU + {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|x86.ActiveCfg = Debug|Any CPU + {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|Any CPU.ActiveCfg = Release|Any CPU + {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|Any CPU.Build.0 = Release|Any CPU + {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|Mixed Platforms.ActiveCfg = Release|Any CPU + {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|Mixed Platforms.Build.0 = Release|Any CPU + {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|x86.ActiveCfg = Release|Any CPU + {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|Any CPU.Build.0 = Debug|Any CPU + {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|Mixed Platforms.ActiveCfg = Debug|Any CPU + {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|Mixed Platforms.Build.0 = Debug|Any CPU + {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|x86.ActiveCfg = Debug|Any CPU + {44270344-ACA7-4875-B585-81D5C06D0489}.Release|Any CPU.ActiveCfg = Release|Any CPU + {44270344-ACA7-4875-B585-81D5C06D0489}.Release|Any CPU.Build.0 = Release|Any CPU + {44270344-ACA7-4875-B585-81D5C06D0489}.Release|Mixed Platforms.ActiveCfg = Release|Any CPU + {44270344-ACA7-4875-B585-81D5C06D0489}.Release|Mixed Platforms.Build.0 = Release|Any CPU + {44270344-ACA7-4875-B585-81D5C06D0489}.Release|x86.ActiveCfg = Release|Any CPU + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/BulletDebug.cs b/libraries/ModifiedBulletX/ModifiedBulletX/BulletDebug.cs index d0d5845..a4cbfdd 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/BulletDebug.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/BulletDebug.cs @@ -1,68 +1,68 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using System.Diagnostics; - -namespace XnaDevRu.BulletX -{ - internal static class BulletDebug - { - [Conditional("DEBUG")] - public static void Assert(Boolean condition) - { - //if (!condition) - //{ - // Throw("No info available"); - //} - Debug.Assert(condition); - } - - [Conditional("DEBUG")] - public static void Assert(Boolean condition, String message) - { - //if (!condition) - //{ - // Throw(message); - //} - Debug.Assert(condition, message); - } - - [Conditional("DEBUG")] - public static void Assert(Boolean condition, String message, String detailMessage) - { - //if (!condition) - //{ - // Throw(message); - //} - Debug.Assert(condition, message, detailMessage); - } - - private static void Throw(String message) - { - String msg = String.Format("Assertion Error: {0}", message); - - throw new BulletException(msg); - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using System.Diagnostics; + +namespace XnaDevRu.BulletX +{ + internal static class BulletDebug + { + [Conditional("DEBUG")] + public static void Assert(Boolean condition) + { + //if (!condition) + //{ + // Throw("No info available"); + //} + Debug.Assert(condition); + } + + [Conditional("DEBUG")] + public static void Assert(Boolean condition, String message) + { + //if (!condition) + //{ + // Throw(message); + //} + Debug.Assert(condition, message); + } + + [Conditional("DEBUG")] + public static void Assert(Boolean condition, String message, String detailMessage) + { + //if (!condition) + //{ + // Throw(message); + //} + Debug.Assert(condition, message, detailMessage); + } + + private static void Throw(String message) + { + String msg = String.Format("Assertion Error: {0}", message); + + throw new BulletException(msg); + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs index 12692ea..168d947 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs @@ -1,623 +1,623 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public class AxisSweep3: OverlappingPairCache - { - Vector3 _worldAabbMin; - Vector3 _worldAabbMax; - - Vector3 _quantize; - - int _numHandles; - int _maxHandles; - - Handle[] _handles; - Edge[][] _edges = new Edge[3][]; - - ushort _firstFreeHandle; - - int _invalidPair; - - public AxisSweep3(Vector3 worldAabbMin, Vector3 worldAabbMax, int maxHandles) - : base() - { - BulletDebug.Assert(maxHandles > 1 && maxHandles < 32767); - - // init bounds - _worldAabbMin = worldAabbMin; - _worldAabbMax = worldAabbMax; - - Vector3 aabbSize = _worldAabbMax - _worldAabbMin; - _quantize = new Vector3(65535.0f, 65535.0f, 65535.0f) / aabbSize; - - // allocate handles buffer and put all handles on free list - _handles = new Handle[maxHandles]; - for (int i = 0; i < maxHandles; i++) - _handles[i] = new Handle(); - _maxHandles = maxHandles; - _numHandles = 0; - - // handle 0 is reserved as the null index, and is also used as the sentinel - _firstFreeHandle = 1; - { - for (int i = _firstFreeHandle; i < maxHandles; i++) - { - _handles[i].NextFree = (ushort)(i + 1); - } - _handles[maxHandles - 1].NextFree = 0; - } - - { - // allocate edge buffers - for (int i = 0; i < 3; i++) - { - _edges[i] = new Edge[maxHandles * 2]; - for (int j = 0; j < maxHandles * 2; j++) - { - _edges[i][j] = new Edge(); - } - } - } - //removed overlap management - - // make boundary sentinels - - _handles[0].ClientData = 0; - - for (int axis = 0; axis < 3; axis++) - { - _handles[0].MinEdges[axis] = 0; - _handles[0].MaxEdges[axis] = 1; - - _edges[axis][0].Position = 0; - _edges[axis][0].Handle = 0; - _edges[axis][1].Position = 0xffff; - _edges[axis][1].Handle = 0; - } - } - - public ushort AddHandle(Vector3 aabbMin, Vector3 aabbMax, object owner, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask) - { - ushort[] min = new ushort[3], max = new ushort[3]; - Quantize(out min, aabbMin, 0); - Quantize(out max, aabbMax, 1); - - ushort handle = AllocateHandle(); - Handle oHandle = GetHandle(handle); - - oHandle.HandleID = handle; - oHandle.ClientData = owner; - oHandle.CollisionFilterGroup = collisionFilterGroup; - oHandle.CollisionFilterMask = collisionFilterMask; - - int limit = _numHandles * 2; - - // (Gluk ) - // ( Inside ) - for (int axis = 0; axis < 3; axis++) - { - _handles[0].MaxEdges[axis] += 2; - - _edges[axis][limit + 1].Position = _edges[axis][limit - 1].Position; - _edges[axis][limit + 1].Handle = _edges[axis][limit - 1].Handle; - - _edges[axis][limit - 1].Position = min[axis]; - _edges[axis][limit - 1].Handle = handle; - - _edges[axis][limit].Position = max[axis]; - _edges[axis][limit].Handle = handle; - - oHandle.MinEdges[axis] = (ushort)(limit - 1); - oHandle.MaxEdges[axis] = (ushort)limit; - } - - SortMinDown(0, oHandle.MinEdges[0], false); - SortMaxDown(0, oHandle.MaxEdges[0], false); - SortMinDown(1, oHandle.MinEdges[1], false); - SortMaxDown(1, oHandle.MaxEdges[1], false); - SortMinDown(2, oHandle.MinEdges[2], true); - SortMaxDown(2, oHandle.MaxEdges[2], true); - - return handle; - } - - public void RemoveHandle(ushort handle) - { - Handle pHandle = GetHandle(handle); - - //explicitly remove the pairs containing the proxy - //we could do it also in the sortMinUp (passing true) - //todo: compare performance - RemoveOverlappingPairsContainingProxy(pHandle); - - - // compute current limit of edge arrays - int limit = _numHandles * 2; - int axis; - - for (axis = 0; axis < 3; axis++) - { - _handles[0].MaxEdges[axis] -= 2; - } - - // remove the edges by sorting them up to the end of the list - for (axis = 0; axis < 3; axis++) - { - Edge[] pEdges = _edges[axis]; - ushort max = pHandle.MaxEdges[axis]; - pEdges[max].Position = 0xffff; - - SortMaxUp(axis, max, false); - - ushort i = pHandle.MinEdges[axis]; - pEdges[i].Position = 0xffff; - - SortMinUp(axis, i, false); - - pEdges[limit - 1].Handle = 0; - pEdges[limit - 1].Position = 0xffff; - } - - // free the handle - FreeHandle(handle); - } - - public override void ProcessAllOverlappingPairs(IOverlapCallback callback) - { - OverlappingPairs.Sort(new Comparison(BroadphasePair.ComparisonSort)); - - if (_invalidPair != 0) - OverlappingPairs.RemoveRange(OverlappingPairs.Count - _invalidPair, _invalidPair); - _invalidPair = 0; - - BroadphasePair previousPair = new BroadphasePair(); - previousPair.ProxyA = null; - previousPair.ProxyB = null; - previousPair.CollisionAlgorithm = null; - - List removal = new List(); - - for (int i = 0; i < OverlappingPairs.Count; i++) - { - bool isDuplicate = (OverlappingPairs[i] == previousPair); - previousPair = OverlappingPairs[i]; - bool needsRemoval; - if (!isDuplicate) - { - bool hasOverlap = TestOverlap(previousPair.ProxyA, previousPair.ProxyB); - if (hasOverlap) - { - needsRemoval = callback.ProcessOverlap(ref previousPair); - } - else - { - needsRemoval = true; - } - } - else - { - needsRemoval = true; - BulletDebug.Assert(previousPair.CollisionAlgorithm == null); - } - - if (needsRemoval) - { - removal.Add(previousPair); - } - } - - for (int i = 0; i < removal.Count; i++) - { - BroadphasePair pair = removal[i]; - CleanOverlappingPair(ref pair); - pair.ProxyA = null; - pair.ProxyB = null; - _invalidPair++; - OverlappingPairCount--; - } - } - - private bool TestOverlap(BroadphaseProxy proxyA, BroadphaseProxy proxyB) - { - if (proxyA == null || proxyB == null) - return false; - - Handle handleA = proxyA as Handle; - Handle handleB = proxyB as Handle; - - for (int axis = 0; axis < 3; axis++) - { - if (handleA.MaxEdges[axis] < handleB.MinEdges[axis] || - handleB.MaxEdges[axis] < handleA.MinEdges[axis]) - { - return false; - } - } - return true; - } - - private bool TestOverlap(int ignoreAxis, Handle pHandleA, Handle pHandleB) - { - for (int axis = 0; axis < 3; axis++) - { - if (axis != ignoreAxis) - { - if (pHandleA.MaxEdges[axis] < pHandleB.MinEdges[axis] || - pHandleB.MaxEdges[axis] < pHandleA.MinEdges[axis]) - { - return false; - } - } - } - - return true; - } - - private ushort AllocateHandle() - { - ushort handle = _firstFreeHandle; - _firstFreeHandle = GetHandle(handle).NextFree; - _numHandles++; - - return handle; - } - - private void FreeHandle(ushort handle) - { - BulletDebug.Assert(handle > 0 && handle < _maxHandles); - - GetHandle(handle).NextFree = _firstFreeHandle; - _firstFreeHandle = handle; - - _numHandles--; - } - - private Handle GetHandle(ushort handle) - { - return _handles[handle]; - } - - private void UpdateHandle(ushort handle, Vector3 aabbMin, Vector3 aabbMax) - { - Handle pHandle = GetHandle(handle); - - // quantize the new bounds - ushort[] min = new ushort[3]; - ushort[] max = new ushort[3]; - Quantize(out min, aabbMin, 0); - Quantize(out max, aabbMax, 1); - - // update changed edges - for (int axis = 0; axis < 3; axis++) - { - ushort emin = pHandle.MinEdges[axis]; - ushort emax = pHandle.MaxEdges[axis]; - - int dmin = (int)min[axis] - (int)_edges[axis][emin].Position; - int dmax = (int)max[axis] - (int)_edges[axis][emax].Position; - - _edges[axis][emin].Position = min[axis]; - _edges[axis][emax].Position = max[axis]; - - // expand (only adds overlaps) - if (dmin < 0) - SortMinDown(axis, emin, true); - - if (dmax > 0) - SortMaxUp(axis, emax, true); - - // shrink (only removes overlaps) - if (dmin > 0) - SortMinUp(axis, emin, true); - - if (dmax < 0) - SortMaxDown(axis, emax, true); - } - } - - private void Quantize(out ushort[] result, Vector3 point, int isMax) - { - Vector3 clampedPoint = new Vector3( - point.X, - point.Y, - point.Z - ); - - MathHelper.SetMax(ref clampedPoint, _worldAabbMin); - MathHelper.SetMin(ref clampedPoint, _worldAabbMax); - - Vector3 v = (clampedPoint - _worldAabbMin) * _quantize; - - result = new ushort[3]; - result[0] = (ushort)(((int)v.X & 0xfffe) | isMax); - result[1] = (ushort)(((int)v.Y & 0xfffe) | isMax); - result[2] = (ushort)(((int)v.Z & 0xfffe) | isMax); - } - - private void SortMinDown(int axis, ushort edge, bool updateOverlaps) - { - Edge pEdge = _edges[axis][edge]; - Edge pPrev = _edges[axis][edge - 1]; - Handle pHandleEdge = GetHandle(pEdge.Handle); - - while (pEdge.Position < pPrev.Position) - { - Handle pHandlePrev = GetHandle(pPrev.Handle); - - if (pPrev.IsMax()) - { - // if previous edge is a maximum check the bounds and add an overlap if necessary - if (updateOverlaps && TestOverlap(axis, pHandleEdge, pHandlePrev)) - { - AddOverlappingPair(pHandleEdge, pHandlePrev); - } - - // update edge reference in other handle - pHandlePrev.MaxEdges[axis]++; - } - else - pHandlePrev.MinEdges[axis]++; - - pHandleEdge.MinEdges[axis]--; - - // swap the edges - pEdge.Swap(ref pPrev); - - // decrement - edge--; - pEdge = _edges[axis][edge]; - pPrev = _edges[axis][edge - 1]; - } - } - - private void SortMinUp(int axis, ushort edge, bool updateOverlaps) - { - Edge pEdge = _edges[axis][edge]; - Edge pNext = _edges[axis][edge + 1]; - Handle pHandleEdge = GetHandle(pEdge.Handle); - - while ((pNext.Handle != 0) && (pEdge.Position >= pNext.Position)) - { - Handle pHandleNext = GetHandle(pNext.Handle); - - if (pNext.IsMax()) - { - // if next edge is maximum remove any overlap between the two handles - if (updateOverlaps) - { - //Handle handle0 = GetHandle(pEdge.Handle); - //Handle handle1 = GetHandle(pNext.Handle); - //BroadphasePair tmpPair = new BroadphasePair(handle0, handle1); - //RemoveOverlappingPair(tmpPair); - } - - // update edge reference in other handle - pHandleNext.MaxEdges[axis]--; - } - else - pHandleNext.MinEdges[axis]--; - - pHandleEdge.MinEdges[axis]++; - - // swap the edges - pEdge.Swap(ref pNext); - - // increment - edge++; - pEdge = _edges[axis][edge]; - pNext = _edges[axis][edge + 1]; - } - } - - private void SortMaxDown(int axis, ushort edge, bool updateOverlaps) - { - Edge pEdge = _edges[axis][edge]; - Edge pPrev = _edges[axis][edge - 1]; - Handle pHandleEdge = GetHandle(pEdge.Handle); - - while (pEdge.Position < pPrev.Position) - { - Handle pHandlePrev = GetHandle(pPrev.Handle); - - if (!pPrev.IsMax()) - { - // if previous edge was a minimum remove any overlap between the two handles - if (updateOverlaps) - { - //this is done during the overlappingpairarray iteration/narrowphase collision - //Handle handle0 = GetHandle(pEdge.Handle); - //Handle handle1 = GetHandle(pPrev.Handle); - //BroadphasePair pair = FindPair(handle0, handle1); - - //if (pair != null) - //{ - // RemoveOverlappingPair(pair); - //} - } - - // update edge reference in other handle - pHandlePrev.MinEdges[axis]++; ; - } - else - pHandlePrev.MaxEdges[axis]++; - - pHandleEdge.MaxEdges[axis]--; - - // swap the edges - pEdge.Swap(ref pPrev); - - // decrement - edge--; - pEdge = _edges[axis][edge]; - pPrev = _edges[axis][edge - 1]; - } - } - - private void SortMaxUp(int axis, ushort edge, bool updateOverlaps) - { - Edge pEdge = _edges[axis][edge]; - Edge pNext = _edges[axis][edge + 1]; - Handle pHandleEdge = GetHandle(pEdge.Handle); - - while ((pNext.Handle!=0) && (pEdge.Position >= pNext.Position)) - { - Handle pHandleNext = GetHandle(pNext.Handle); - - if (!pNext.IsMax()) - { - // if next edge is a minimum check the bounds and add an overlap if necessary - if (updateOverlaps && TestOverlap(axis, pHandleEdge, pHandleNext)) - { - Handle handle0 = GetHandle(pEdge.Handle); - Handle handle1 = GetHandle(pNext.Handle); - AddOverlappingPair(handle0, handle1); - } - - // update edge reference in other handle - pHandleNext.MinEdges[axis]--; - } - else - pHandleNext.MaxEdges[axis]--; - - pHandleEdge.MaxEdges[axis]++; - - // swap the edges - pEdge.Swap(ref pNext); - - // increment - edge++; - pEdge = _edges[axis][edge]; - pNext = _edges[axis][edge + 1]; - } - } - - #region Abstract - - public override void RefreshOverlappingPairs() - { - } - - public override BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask) - { - ushort handleId = AddHandle(min, max, userData, collisionFilterGroup, collisionFilterMask); - - Handle handle = GetHandle(handleId); - - return handle; - } - - public override void DestroyProxy(BroadphaseProxy proxy) - { - Handle handle = proxy as Handle; - RemoveHandle(handle.HandleID); - } - - public override void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax) - { - Handle handle = proxy as Handle; - UpdateHandle(handle.HandleID, aabbMin, aabbMax); - } - #endregion - } - - public class Edge - { - ushort position; - ushort handle; - - public ushort Position - { - get { return position; } - set { position = value; } - } - - public ushort Handle - { - get { return handle; } - set { handle = value; } - } - - public bool IsMax() - { - return (position & (ushort)1) == 1; - } - - public void Swap(ref Edge e) - { - ushort tmpPosition = this.position; - ushort tmpHandle = this.handle; - this.position = e.position; - this.handle = e.handle; - e.position = tmpPosition; - e.handle = tmpHandle; - } - } - - public class Handle: BroadphaseProxy - { - ushort[] minEdges, maxEdges; - ushort pad; - ushort handleID; - - public ushort[] MinEdges - { - get { return minEdges; } - set { minEdges = value; } - } - - public ushort[] MaxEdges - { - get { return maxEdges; } - set { maxEdges = value; } - } - - public ushort HandleID - { - get { return handleID; } - set { handleID = value; } - } - - public ushort Pad - { - get { return pad; } - set { pad = value; } - } - - public ushort NextFree - { - get { return minEdges[0]; } - set { minEdges[0] = value;} - } - - public Handle() - { - minEdges = new ushort[3]; - maxEdges = new ushort[3]; - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public class AxisSweep3: OverlappingPairCache + { + Vector3 _worldAabbMin; + Vector3 _worldAabbMax; + + Vector3 _quantize; + + int _numHandles; + int _maxHandles; + + Handle[] _handles; + Edge[][] _edges = new Edge[3][]; + + ushort _firstFreeHandle; + + int _invalidPair; + + public AxisSweep3(Vector3 worldAabbMin, Vector3 worldAabbMax, int maxHandles) + : base() + { + BulletDebug.Assert(maxHandles > 1 && maxHandles < 32767); + + // init bounds + _worldAabbMin = worldAabbMin; + _worldAabbMax = worldAabbMax; + + Vector3 aabbSize = _worldAabbMax - _worldAabbMin; + _quantize = new Vector3(65535.0f, 65535.0f, 65535.0f) / aabbSize; + + // allocate handles buffer and put all handles on free list + _handles = new Handle[maxHandles]; + for (int i = 0; i < maxHandles; i++) + _handles[i] = new Handle(); + _maxHandles = maxHandles; + _numHandles = 0; + + // handle 0 is reserved as the null index, and is also used as the sentinel + _firstFreeHandle = 1; + { + for (int i = _firstFreeHandle; i < maxHandles; i++) + { + _handles[i].NextFree = (ushort)(i + 1); + } + _handles[maxHandles - 1].NextFree = 0; + } + + { + // allocate edge buffers + for (int i = 0; i < 3; i++) + { + _edges[i] = new Edge[maxHandles * 2]; + for (int j = 0; j < maxHandles * 2; j++) + { + _edges[i][j] = new Edge(); + } + } + } + //removed overlap management + + // make boundary sentinels + + _handles[0].ClientData = 0; + + for (int axis = 0; axis < 3; axis++) + { + _handles[0].MinEdges[axis] = 0; + _handles[0].MaxEdges[axis] = 1; + + _edges[axis][0].Position = 0; + _edges[axis][0].Handle = 0; + _edges[axis][1].Position = 0xffff; + _edges[axis][1].Handle = 0; + } + } + + public ushort AddHandle(Vector3 aabbMin, Vector3 aabbMax, object owner, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask) + { + ushort[] min = new ushort[3], max = new ushort[3]; + Quantize(out min, aabbMin, 0); + Quantize(out max, aabbMax, 1); + + ushort handle = AllocateHandle(); + Handle oHandle = GetHandle(handle); + + oHandle.HandleID = handle; + oHandle.ClientData = owner; + oHandle.CollisionFilterGroup = collisionFilterGroup; + oHandle.CollisionFilterMask = collisionFilterMask; + + int limit = _numHandles * 2; + + // (Gluk ) + // ( Inside ) + for (int axis = 0; axis < 3; axis++) + { + _handles[0].MaxEdges[axis] += 2; + + _edges[axis][limit + 1].Position = _edges[axis][limit - 1].Position; + _edges[axis][limit + 1].Handle = _edges[axis][limit - 1].Handle; + + _edges[axis][limit - 1].Position = min[axis]; + _edges[axis][limit - 1].Handle = handle; + + _edges[axis][limit].Position = max[axis]; + _edges[axis][limit].Handle = handle; + + oHandle.MinEdges[axis] = (ushort)(limit - 1); + oHandle.MaxEdges[axis] = (ushort)limit; + } + + SortMinDown(0, oHandle.MinEdges[0], false); + SortMaxDown(0, oHandle.MaxEdges[0], false); + SortMinDown(1, oHandle.MinEdges[1], false); + SortMaxDown(1, oHandle.MaxEdges[1], false); + SortMinDown(2, oHandle.MinEdges[2], true); + SortMaxDown(2, oHandle.MaxEdges[2], true); + + return handle; + } + + public void RemoveHandle(ushort handle) + { + Handle pHandle = GetHandle(handle); + + //explicitly remove the pairs containing the proxy + //we could do it also in the sortMinUp (passing true) + //todo: compare performance + RemoveOverlappingPairsContainingProxy(pHandle); + + + // compute current limit of edge arrays + int limit = _numHandles * 2; + int axis; + + for (axis = 0; axis < 3; axis++) + { + _handles[0].MaxEdges[axis] -= 2; + } + + // remove the edges by sorting them up to the end of the list + for (axis = 0; axis < 3; axis++) + { + Edge[] pEdges = _edges[axis]; + ushort max = pHandle.MaxEdges[axis]; + pEdges[max].Position = 0xffff; + + SortMaxUp(axis, max, false); + + ushort i = pHandle.MinEdges[axis]; + pEdges[i].Position = 0xffff; + + SortMinUp(axis, i, false); + + pEdges[limit - 1].Handle = 0; + pEdges[limit - 1].Position = 0xffff; + } + + // free the handle + FreeHandle(handle); + } + + public override void ProcessAllOverlappingPairs(IOverlapCallback callback) + { + OverlappingPairs.Sort(new Comparison(BroadphasePair.ComparisonSort)); + + if (_invalidPair != 0) + OverlappingPairs.RemoveRange(OverlappingPairs.Count - _invalidPair, _invalidPair); + _invalidPair = 0; + + BroadphasePair previousPair = new BroadphasePair(); + previousPair.ProxyA = null; + previousPair.ProxyB = null; + previousPair.CollisionAlgorithm = null; + + List removal = new List(); + + for (int i = 0; i < OverlappingPairs.Count; i++) + { + bool isDuplicate = (OverlappingPairs[i] == previousPair); + previousPair = OverlappingPairs[i]; + bool needsRemoval; + if (!isDuplicate) + { + bool hasOverlap = TestOverlap(previousPair.ProxyA, previousPair.ProxyB); + if (hasOverlap) + { + needsRemoval = callback.ProcessOverlap(ref previousPair); + } + else + { + needsRemoval = true; + } + } + else + { + needsRemoval = true; + BulletDebug.Assert(previousPair.CollisionAlgorithm == null); + } + + if (needsRemoval) + { + removal.Add(previousPair); + } + } + + for (int i = 0; i < removal.Count; i++) + { + BroadphasePair pair = removal[i]; + CleanOverlappingPair(ref pair); + pair.ProxyA = null; + pair.ProxyB = null; + _invalidPair++; + OverlappingPairCount--; + } + } + + private bool TestOverlap(BroadphaseProxy proxyA, BroadphaseProxy proxyB) + { + if (proxyA == null || proxyB == null) + return false; + + Handle handleA = proxyA as Handle; + Handle handleB = proxyB as Handle; + + for (int axis = 0; axis < 3; axis++) + { + if (handleA.MaxEdges[axis] < handleB.MinEdges[axis] || + handleB.MaxEdges[axis] < handleA.MinEdges[axis]) + { + return false; + } + } + return true; + } + + private bool TestOverlap(int ignoreAxis, Handle pHandleA, Handle pHandleB) + { + for (int axis = 0; axis < 3; axis++) + { + if (axis != ignoreAxis) + { + if (pHandleA.MaxEdges[axis] < pHandleB.MinEdges[axis] || + pHandleB.MaxEdges[axis] < pHandleA.MinEdges[axis]) + { + return false; + } + } + } + + return true; + } + + private ushort AllocateHandle() + { + ushort handle = _firstFreeHandle; + _firstFreeHandle = GetHandle(handle).NextFree; + _numHandles++; + + return handle; + } + + private void FreeHandle(ushort handle) + { + BulletDebug.Assert(handle > 0 && handle < _maxHandles); + + GetHandle(handle).NextFree = _firstFreeHandle; + _firstFreeHandle = handle; + + _numHandles--; + } + + private Handle GetHandle(ushort handle) + { + return _handles[handle]; + } + + private void UpdateHandle(ushort handle, Vector3 aabbMin, Vector3 aabbMax) + { + Handle pHandle = GetHandle(handle); + + // quantize the new bounds + ushort[] min = new ushort[3]; + ushort[] max = new ushort[3]; + Quantize(out min, aabbMin, 0); + Quantize(out max, aabbMax, 1); + + // update changed edges + for (int axis = 0; axis < 3; axis++) + { + ushort emin = pHandle.MinEdges[axis]; + ushort emax = pHandle.MaxEdges[axis]; + + int dmin = (int)min[axis] - (int)_edges[axis][emin].Position; + int dmax = (int)max[axis] - (int)_edges[axis][emax].Position; + + _edges[axis][emin].Position = min[axis]; + _edges[axis][emax].Position = max[axis]; + + // expand (only adds overlaps) + if (dmin < 0) + SortMinDown(axis, emin, true); + + if (dmax > 0) + SortMaxUp(axis, emax, true); + + // shrink (only removes overlaps) + if (dmin > 0) + SortMinUp(axis, emin, true); + + if (dmax < 0) + SortMaxDown(axis, emax, true); + } + } + + private void Quantize(out ushort[] result, Vector3 point, int isMax) + { + Vector3 clampedPoint = new Vector3( + point.X, + point.Y, + point.Z + ); + + MathHelper.SetMax(ref clampedPoint, _worldAabbMin); + MathHelper.SetMin(ref clampedPoint, _worldAabbMax); + + Vector3 v = (clampedPoint - _worldAabbMin) * _quantize; + + result = new ushort[3]; + result[0] = (ushort)(((int)v.X & 0xfffe) | isMax); + result[1] = (ushort)(((int)v.Y & 0xfffe) | isMax); + result[2] = (ushort)(((int)v.Z & 0xfffe) | isMax); + } + + private void SortMinDown(int axis, ushort edge, bool updateOverlaps) + { + Edge pEdge = _edges[axis][edge]; + Edge pPrev = _edges[axis][edge - 1]; + Handle pHandleEdge = GetHandle(pEdge.Handle); + + while (pEdge.Position < pPrev.Position) + { + Handle pHandlePrev = GetHandle(pPrev.Handle); + + if (pPrev.IsMax()) + { + // if previous edge is a maximum check the bounds and add an overlap if necessary + if (updateOverlaps && TestOverlap(axis, pHandleEdge, pHandlePrev)) + { + AddOverlappingPair(pHandleEdge, pHandlePrev); + } + + // update edge reference in other handle + pHandlePrev.MaxEdges[axis]++; + } + else + pHandlePrev.MinEdges[axis]++; + + pHandleEdge.MinEdges[axis]--; + + // swap the edges + pEdge.Swap(ref pPrev); + + // decrement + edge--; + pEdge = _edges[axis][edge]; + pPrev = _edges[axis][edge - 1]; + } + } + + private void SortMinUp(int axis, ushort edge, bool updateOverlaps) + { + Edge pEdge = _edges[axis][edge]; + Edge pNext = _edges[axis][edge + 1]; + Handle pHandleEdge = GetHandle(pEdge.Handle); + + while ((pNext.Handle != 0) && (pEdge.Position >= pNext.Position)) + { + Handle pHandleNext = GetHandle(pNext.Handle); + + if (pNext.IsMax()) + { + // if next edge is maximum remove any overlap between the two handles + if (updateOverlaps) + { + //Handle handle0 = GetHandle(pEdge.Handle); + //Handle handle1 = GetHandle(pNext.Handle); + //BroadphasePair tmpPair = new BroadphasePair(handle0, handle1); + //RemoveOverlappingPair(tmpPair); + } + + // update edge reference in other handle + pHandleNext.MaxEdges[axis]--; + } + else + pHandleNext.MinEdges[axis]--; + + pHandleEdge.MinEdges[axis]++; + + // swap the edges + pEdge.Swap(ref pNext); + + // increment + edge++; + pEdge = _edges[axis][edge]; + pNext = _edges[axis][edge + 1]; + } + } + + private void SortMaxDown(int axis, ushort edge, bool updateOverlaps) + { + Edge pEdge = _edges[axis][edge]; + Edge pPrev = _edges[axis][edge - 1]; + Handle pHandleEdge = GetHandle(pEdge.Handle); + + while (pEdge.Position < pPrev.Position) + { + Handle pHandlePrev = GetHandle(pPrev.Handle); + + if (!pPrev.IsMax()) + { + // if previous edge was a minimum remove any overlap between the two handles + if (updateOverlaps) + { + //this is done during the overlappingpairarray iteration/narrowphase collision + //Handle handle0 = GetHandle(pEdge.Handle); + //Handle handle1 = GetHandle(pPrev.Handle); + //BroadphasePair pair = FindPair(handle0, handle1); + + //if (pair != null) + //{ + // RemoveOverlappingPair(pair); + //} + } + + // update edge reference in other handle + pHandlePrev.MinEdges[axis]++; ; + } + else + pHandlePrev.MaxEdges[axis]++; + + pHandleEdge.MaxEdges[axis]--; + + // swap the edges + pEdge.Swap(ref pPrev); + + // decrement + edge--; + pEdge = _edges[axis][edge]; + pPrev = _edges[axis][edge - 1]; + } + } + + private void SortMaxUp(int axis, ushort edge, bool updateOverlaps) + { + Edge pEdge = _edges[axis][edge]; + Edge pNext = _edges[axis][edge + 1]; + Handle pHandleEdge = GetHandle(pEdge.Handle); + + while ((pNext.Handle!=0) && (pEdge.Position >= pNext.Position)) + { + Handle pHandleNext = GetHandle(pNext.Handle); + + if (!pNext.IsMax()) + { + // if next edge is a minimum check the bounds and add an overlap if necessary + if (updateOverlaps && TestOverlap(axis, pHandleEdge, pHandleNext)) + { + Handle handle0 = GetHandle(pEdge.Handle); + Handle handle1 = GetHandle(pNext.Handle); + AddOverlappingPair(handle0, handle1); + } + + // update edge reference in other handle + pHandleNext.MinEdges[axis]--; + } + else + pHandleNext.MaxEdges[axis]--; + + pHandleEdge.MaxEdges[axis]++; + + // swap the edges + pEdge.Swap(ref pNext); + + // increment + edge++; + pEdge = _edges[axis][edge]; + pNext = _edges[axis][edge + 1]; + } + } + + #region Abstract + + public override void RefreshOverlappingPairs() + { + } + + public override BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask) + { + ushort handleId = AddHandle(min, max, userData, collisionFilterGroup, collisionFilterMask); + + Handle handle = GetHandle(handleId); + + return handle; + } + + public override void DestroyProxy(BroadphaseProxy proxy) + { + Handle handle = proxy as Handle; + RemoveHandle(handle.HandleID); + } + + public override void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax) + { + Handle handle = proxy as Handle; + UpdateHandle(handle.HandleID, aabbMin, aabbMax); + } + #endregion + } + + public class Edge + { + ushort position; + ushort handle; + + public ushort Position + { + get { return position; } + set { position = value; } + } + + public ushort Handle + { + get { return handle; } + set { handle = value; } + } + + public bool IsMax() + { + return (position & (ushort)1) == 1; + } + + public void Swap(ref Edge e) + { + ushort tmpPosition = this.position; + ushort tmpHandle = this.handle; + this.position = e.position; + this.handle = e.handle; + e.position = tmpPosition; + e.handle = tmpHandle; + } + } + + public class Handle: BroadphaseProxy + { + ushort[] minEdges, maxEdges; + ushort pad; + ushort handleID; + + public ushort[] MinEdges + { + get { return minEdges; } + set { minEdges = value; } + } + + public ushort[] MaxEdges + { + get { return maxEdges; } + set { maxEdges = value; } + } + + public ushort HandleID + { + get { return handleID; } + set { handleID = value; } + } + + public ushort Pad + { + get { return pad; } + set { pad = value; } + } + + public ushort NextFree + { + get { return minEdges[0]; } + set { minEdges[0] = value;} + } + + public Handle() + { + minEdges = new ushort[3]; + maxEdges = new ushort[3]; + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs index dcfd0d8..eb1fd4b 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs @@ -1,68 +1,68 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; - -namespace XnaDevRu.BulletX -{ - /// Dispatcher uses these types - /// IMPORTANT NOTE:The types are ordered polyhedral, implicit convex and concave - /// to facilitate type checking - public enum BroadphaseNativeTypes - { - // polyhedral convex shapes - Box, - Triangle, - Tetrahedral, - ConvexTriangleMesh, - ConvexHull, - //implicit convex shapes - ImplicitConvexShapes, - Sphere, - MultiSphere, - Capsule, - Cone, - Convex, - Cylinder, - MinkowskiSum, - MinkowskiDifference, - //concave shapes - ConcaveShapesStart, - //keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy! - TriangleMesh, - //used for demo integration FAST/Swift collision library and Bullet - FastConcaveMesh, - //terrain - Terrain, - //Used for GIMPACT Trimesh integration - Gimpact, - - Empty, - StaticPlane, - ConcaveShapesEnd, - - Compound, - - MaxBroadphaseCollisionTypes, - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace XnaDevRu.BulletX +{ + /// Dispatcher uses these types + /// IMPORTANT NOTE:The types are ordered polyhedral, implicit convex and concave + /// to facilitate type checking + public enum BroadphaseNativeTypes + { + // polyhedral convex shapes + Box, + Triangle, + Tetrahedral, + ConvexTriangleMesh, + ConvexHull, + //implicit convex shapes + ImplicitConvexShapes, + Sphere, + MultiSphere, + Capsule, + Cone, + Convex, + Cylinder, + MinkowskiSum, + MinkowskiDifference, + //concave shapes + ConcaveShapesStart, + //keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy! + TriangleMesh, + //used for demo integration FAST/Swift collision library and Bullet + FastConcaveMesh, + //terrain + Terrain, + //Used for GIMPACT Trimesh integration + Gimpact, + + Empty, + StaticPlane, + ConcaveShapesEnd, + + Compound, + + MaxBroadphaseCollisionTypes, + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs index 2b9e114..eec77ac 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs @@ -1,113 +1,113 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; - -namespace XnaDevRu.BulletX -{ - public class BroadphasePair - { - private BroadphaseProxy _proxyA; - private BroadphaseProxy _proxyB; - - private CollisionAlgorithm _algorithm; - private object _userInfo; - - public BroadphasePair() - { - } - - public BroadphasePair(BroadphasePair other) - { - _proxyA = other._proxyA; - _proxyB = other._proxyB; - - _algorithm = other._algorithm; - _userInfo = null; - } - - public BroadphasePair(BroadphaseProxy proxyA, BroadphaseProxy proxyB) - { - _proxyA = proxyA; - _proxyB = proxyB; - - _algorithm = null; - _userInfo = null; - } - - public BroadphaseProxy ProxyA { get { return _proxyA; } set { _proxyA = value; } } - public BroadphaseProxy ProxyB { get { return _proxyB; } set { _proxyB = value; } } - - public CollisionAlgorithm CollisionAlgorithm { get { return _algorithm; } set { _algorithm = value; } } - public object UserInfo { get { return _userInfo; } set { _userInfo = value; } } - - public override int GetHashCode() - { - return _proxyA.GetHashCode() ^ _proxyB.GetHashCode(); - } - - public override bool Equals(object obj) - { - if (obj is BroadphasePair) - return this == (BroadphasePair)obj; - return false; - } - - public static int ComparisonSort(BroadphasePair a, BroadphasePair b) - { - int aAId = a.ProxyA != null ? a.ProxyA.ComparisonID : -1; - int aBId = a.ProxyB != null ? a.ProxyB.ComparisonID : -1; - int aCId = a.CollisionAlgorithm != null ? a.CollisionAlgorithm.ComparisonID : -1; - int bAId = b.ProxyA != null ? b.ProxyA.ComparisonID : -1; - int bBId = b.ProxyB != null ? b.ProxyB.ComparisonID : -1; - int bCId = b.CollisionAlgorithm != null ? b.CollisionAlgorithm.ComparisonID : -1; - - if (aAId > bAId || - (a.ProxyA == b.ProxyA && aBId > bBId) || - (a.ProxyA == b.ProxyA && a.ProxyB == b.ProxyB && aCId > bCId)) - return -1; - else - return 1; - } - - public static bool operator ==(BroadphasePair a, BroadphasePair b) - { - if (object.Equals(a, null) && object.Equals(b, null)) - return true; - if (object.Equals(a, null) || object.Equals(b, null)) - return false; - - return (a.ProxyA == b.ProxyA) && (a.ProxyB == b.ProxyB); - } - - public static bool operator !=(BroadphasePair a, BroadphasePair b) - { - if (object.Equals(a, null) && object.Equals(b, null)) - return true; - if (object.Equals(a, null) || object.Equals(b, null)) - return false; - - return (a.ProxyA != b.ProxyA) || (a.ProxyB != b.ProxyB); - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace XnaDevRu.BulletX +{ + public class BroadphasePair + { + private BroadphaseProxy _proxyA; + private BroadphaseProxy _proxyB; + + private CollisionAlgorithm _algorithm; + private object _userInfo; + + public BroadphasePair() + { + } + + public BroadphasePair(BroadphasePair other) + { + _proxyA = other._proxyA; + _proxyB = other._proxyB; + + _algorithm = other._algorithm; + _userInfo = null; + } + + public BroadphasePair(BroadphaseProxy proxyA, BroadphaseProxy proxyB) + { + _proxyA = proxyA; + _proxyB = proxyB; + + _algorithm = null; + _userInfo = null; + } + + public BroadphaseProxy ProxyA { get { return _proxyA; } set { _proxyA = value; } } + public BroadphaseProxy ProxyB { get { return _proxyB; } set { _proxyB = value; } } + + public CollisionAlgorithm CollisionAlgorithm { get { return _algorithm; } set { _algorithm = value; } } + public object UserInfo { get { return _userInfo; } set { _userInfo = value; } } + + public override int GetHashCode() + { + return _proxyA.GetHashCode() ^ _proxyB.GetHashCode(); + } + + public override bool Equals(object obj) + { + if (obj is BroadphasePair) + return this == (BroadphasePair)obj; + return false; + } + + public static int ComparisonSort(BroadphasePair a, BroadphasePair b) + { + int aAId = a.ProxyA != null ? a.ProxyA.ComparisonID : -1; + int aBId = a.ProxyB != null ? a.ProxyB.ComparisonID : -1; + int aCId = a.CollisionAlgorithm != null ? a.CollisionAlgorithm.ComparisonID : -1; + int bAId = b.ProxyA != null ? b.ProxyA.ComparisonID : -1; + int bBId = b.ProxyB != null ? b.ProxyB.ComparisonID : -1; + int bCId = b.CollisionAlgorithm != null ? b.CollisionAlgorithm.ComparisonID : -1; + + if (aAId > bAId || + (a.ProxyA == b.ProxyA && aBId > bBId) || + (a.ProxyA == b.ProxyA && a.ProxyB == b.ProxyB && aCId > bCId)) + return -1; + else + return 1; + } + + public static bool operator ==(BroadphasePair a, BroadphasePair b) + { + if (object.Equals(a, null) && object.Equals(b, null)) + return true; + if (object.Equals(a, null) || object.Equals(b, null)) + return false; + + return (a.ProxyA == b.ProxyA) && (a.ProxyB == b.ProxyB); + } + + public static bool operator !=(BroadphasePair a, BroadphasePair b) + { + if (object.Equals(a, null) && object.Equals(b, null)) + return true; + if (object.Equals(a, null) || object.Equals(b, null)) + return false; + + return (a.ProxyA != b.ProxyA) || (a.ProxyB != b.ProxyB); + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs index aed82d5..0abc613 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs @@ -1,91 +1,91 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; - -namespace XnaDevRu.BulletX -{ - public class BroadphaseProxy - { - //Usually the client CollisionObject or Rigidbody class - private object _clientObject; - private CollisionFilterGroups _collisionFilterGroup; - private CollisionFilterGroups _collisionFilterMask; - private readonly int _comparisonID; - - private static int _globalCount = 0; - - public BroadphaseProxy() - { - _comparisonID = _globalCount++; - } - - public BroadphaseProxy(object userData, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask) - : this() - { - _clientObject = userData; - _collisionFilterGroup = collisionFilterGroup; - _collisionFilterMask = collisionFilterMask; - } - - public object ClientData { get { return _clientObject; } set { _clientObject = value; } } - public CollisionFilterGroups CollisionFilterGroup { get { return _collisionFilterGroup; } set { _collisionFilterGroup = value; } } - public CollisionFilterGroups CollisionFilterMask { get { return _collisionFilterMask; } set { _collisionFilterMask = value; } } - internal int ComparisonID { get { return _comparisonID; } } - - public static bool IsPolyhedral(BroadphaseNativeTypes proxyType) - { - return (proxyType < BroadphaseNativeTypes.ImplicitConvexShapes); - } - - public static bool IsConvex(BroadphaseNativeTypes proxyType) - { - return (proxyType < BroadphaseNativeTypes.ConcaveShapesStart); - } - - public static bool IsConcave(BroadphaseNativeTypes proxyType) - { - return ((proxyType > BroadphaseNativeTypes.ConcaveShapesStart) && - (proxyType < BroadphaseNativeTypes.ConcaveShapesEnd)); - } - public static bool IsCompound(BroadphaseNativeTypes proxyType) - { - return (proxyType == BroadphaseNativeTypes.Compound); - } - public static bool IsInfinite(BroadphaseNativeTypes proxyType) - { - return (proxyType == BroadphaseNativeTypes.StaticPlane); - } - - //optional filtering to cull potential collisions - public enum CollisionFilterGroups - { - Default = 1, - Static = 2, - Kinematic = 4, - Debris = 8, - Sensor = 16, - All = Default | Static | Kinematic | Debris | Sensor, - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace XnaDevRu.BulletX +{ + public class BroadphaseProxy + { + //Usually the client CollisionObject or Rigidbody class + private object _clientObject; + private CollisionFilterGroups _collisionFilterGroup; + private CollisionFilterGroups _collisionFilterMask; + private readonly int _comparisonID; + + private static int _globalCount = 0; + + public BroadphaseProxy() + { + _comparisonID = _globalCount++; + } + + public BroadphaseProxy(object userData, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask) + : this() + { + _clientObject = userData; + _collisionFilterGroup = collisionFilterGroup; + _collisionFilterMask = collisionFilterMask; + } + + public object ClientData { get { return _clientObject; } set { _clientObject = value; } } + public CollisionFilterGroups CollisionFilterGroup { get { return _collisionFilterGroup; } set { _collisionFilterGroup = value; } } + public CollisionFilterGroups CollisionFilterMask { get { return _collisionFilterMask; } set { _collisionFilterMask = value; } } + internal int ComparisonID { get { return _comparisonID; } } + + public static bool IsPolyhedral(BroadphaseNativeTypes proxyType) + { + return (proxyType < BroadphaseNativeTypes.ImplicitConvexShapes); + } + + public static bool IsConvex(BroadphaseNativeTypes proxyType) + { + return (proxyType < BroadphaseNativeTypes.ConcaveShapesStart); + } + + public static bool IsConcave(BroadphaseNativeTypes proxyType) + { + return ((proxyType > BroadphaseNativeTypes.ConcaveShapesStart) && + (proxyType < BroadphaseNativeTypes.ConcaveShapesEnd)); + } + public static bool IsCompound(BroadphaseNativeTypes proxyType) + { + return (proxyType == BroadphaseNativeTypes.Compound); + } + public static bool IsInfinite(BroadphaseNativeTypes proxyType) + { + return (proxyType == BroadphaseNativeTypes.StaticPlane); + } + + //optional filtering to cull potential collisions + public enum CollisionFilterGroups + { + Default = 1, + Static = 2, + Kinematic = 4, + Debris = 8, + Sensor = 16, + All = Default | Static | Kinematic | Debris | Sensor, + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs index 1e59ad8..387340d 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs @@ -1,51 +1,51 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; - -namespace XnaDevRu.BulletX -{ - /// - /// CollisionAlgorithm is an collision interface that is compatible with the Broadphase and Dispatcher. - /// It is persistent over frames - /// - public abstract class CollisionAlgorithm - { - private IDispatcher _dispatcher; - private readonly int _comparisonID = 0; - - private static int _globalCount = 0; - - public CollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) - { - _comparisonID = _globalCount++; - _dispatcher = collisionAlgorithmConstructionInfo.Dispatcher; - } - - protected IDispatcher Dispatcher { get { return _dispatcher; } set { _dispatcher = value; } } - internal int ComparisonID { get { return _comparisonID; } } - - public abstract void ProcessCollision(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut); - public abstract float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut); - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace XnaDevRu.BulletX +{ + /// + /// CollisionAlgorithm is an collision interface that is compatible with the Broadphase and Dispatcher. + /// It is persistent over frames + /// + public abstract class CollisionAlgorithm + { + private IDispatcher _dispatcher; + private readonly int _comparisonID = 0; + + private static int _globalCount = 0; + + public CollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) + { + _comparisonID = _globalCount++; + _dispatcher = collisionAlgorithmConstructionInfo.Dispatcher; + } + + protected IDispatcher Dispatcher { get { return _dispatcher; } set { _dispatcher = value; } } + internal int ComparisonID { get { return _comparisonID; } } + + public abstract void ProcessCollision(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut); + public abstract float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut); + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs index 381752a..de238b9 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs @@ -1,42 +1,42 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; - -namespace XnaDevRu.BulletX -{ - public struct CollisionAlgorithmConstructionInfo - { - private IDispatcher _dispatcher; - private PersistentManifold _manifold; - - public CollisionAlgorithmConstructionInfo(IDispatcher dispatcher) - { - _dispatcher = dispatcher; - _manifold = null; - } - - public IDispatcher Dispatcher { get { return _dispatcher; } set { _dispatcher = value; } } - public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace XnaDevRu.BulletX +{ + public struct CollisionAlgorithmConstructionInfo + { + private IDispatcher _dispatcher; + private PersistentManifold _manifold; + + public CollisionAlgorithmConstructionInfo(IDispatcher dispatcher) + { + _dispatcher = dispatcher; + _manifold = null; + } + + public IDispatcher Dispatcher { get { return _dispatcher; } set { _dispatcher = value; } } + public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs index 8c0367f..cde3221 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs @@ -1,54 +1,54 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; - -namespace XnaDevRu.BulletX -{ - public enum DispatchFunction - { - Discrete = 1, - Continuous, - } - - public class DispatcherInfo - { - private float _timeStep; - private int _stepCount; - private DispatchFunction _dispatchFunc = DispatchFunction.Discrete; - private float _timeOfImpact = 1; - private bool _useContinuous; - private bool _enableSatConvex; - private bool _enableSpu; - private IDebugDraw _debugDraw; - - public float TimeStep { get { return _timeStep; } set { _timeStep = value; } } - public int StepCount { get { return _stepCount; } set { _stepCount = value; } } - public DispatchFunction DispatchFunction { get { return _dispatchFunc; } set { _dispatchFunc = value; } } - public float TimeOfImpact { get { return _timeOfImpact; } set { _timeOfImpact = value; } } - public bool UseContinuous { get { return _useContinuous; } set { _useContinuous = value; } } - public bool EnableSatConvex { get { return _enableSatConvex; } set { _enableSatConvex = value; } } - public bool enableSpu { get { return _enableSpu; } set { _enableSpu = value; } } - public IDebugDraw DebugDraw { get { return _debugDraw; } set { _debugDraw = value; } } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace XnaDevRu.BulletX +{ + public enum DispatchFunction + { + Discrete = 1, + Continuous, + } + + public class DispatcherInfo + { + private float _timeStep; + private int _stepCount; + private DispatchFunction _dispatchFunc = DispatchFunction.Discrete; + private float _timeOfImpact = 1; + private bool _useContinuous; + private bool _enableSatConvex; + private bool _enableSpu; + private IDebugDraw _debugDraw; + + public float TimeStep { get { return _timeStep; } set { _timeStep = value; } } + public int StepCount { get { return _stepCount; } set { _stepCount = value; } } + public DispatchFunction DispatchFunction { get { return _dispatchFunc; } set { _dispatchFunc = value; } } + public float TimeOfImpact { get { return _timeOfImpact; } set { _timeOfImpact = value; } } + public bool UseContinuous { get { return _useContinuous; } set { _useContinuous = value; } } + public bool EnableSatConvex { get { return _enableSatConvex; } set { _enableSatConvex = value; } } + public bool enableSpu { get { return _enableSpu; } set { _enableSpu = value; } } + public IDebugDraw DebugDraw { get { return _debugDraw; } set { _debugDraw = value; } } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs index 89618af..fddd8af 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs @@ -1,36 +1,36 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public interface IBroadphase - { - BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask); - void DestroyProxy(BroadphaseProxy proxy); - void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax); - void CleanProxyFromPairs(BroadphaseProxy proxy); - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public interface IBroadphase + { + BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask); + void DestroyProxy(BroadphaseProxy proxy); + void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax); + void CleanProxyFromPairs(BroadphaseProxy proxy); + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs index 52831b7..7677179 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs @@ -1,42 +1,42 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; - -namespace XnaDevRu.BulletX -{ - public interface IDispatcher - { - CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB, PersistentManifold sharedManifold); - CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB); - PersistentManifold GetNewManifold(object bodyA, object bodyB); - void ReleaseManifold(PersistentManifold manifold); - void ClearManifold(PersistentManifold manifold); - bool NeedsCollision(CollisionObject bodyA, CollisionObject bodyB); - bool NeedsResponse(CollisionObject bodyA, CollisionObject bodyB); - void DispatchAllCollisionPairs(OverlappingPairCache pairCache, DispatcherInfo dispatchInfo); - PersistentManifold GetManifoldByIndex(int index); - - int ManifoldCount { get; } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace XnaDevRu.BulletX +{ + public interface IDispatcher + { + CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB, PersistentManifold sharedManifold); + CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB); + PersistentManifold GetNewManifold(object bodyA, object bodyB); + void ReleaseManifold(PersistentManifold manifold); + void ClearManifold(PersistentManifold manifold); + bool NeedsCollision(CollisionObject bodyA, CollisionObject bodyB); + bool NeedsResponse(CollisionObject bodyA, CollisionObject bodyB); + void DispatchAllCollisionPairs(OverlappingPairCache pairCache, DispatcherInfo dispatchInfo); + PersistentManifold GetManifoldByIndex(int index); + + int ManifoldCount { get; } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs index 4e128da..2456911 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs @@ -1,33 +1,33 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; - -namespace XnaDevRu.BulletX -{ - public interface IOverlapCallback - { - //return true for deletion of the pair - bool ProcessOverlap(ref BroadphasePair pair); - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace XnaDevRu.BulletX +{ + public interface IOverlapCallback + { + //return true for deletion of the pair + bool ProcessOverlap(ref BroadphasePair pair); + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs index 165912a..6548560 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs @@ -1,159 +1,159 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; - -namespace XnaDevRu.BulletX -{ - public abstract class OverlappingPairCache : IBroadphase - { - private static int _overlappingPairCount = 0; - private List _overlappingPairs = new List(); - //during the dispatch, check that user doesn't destroy/create proxy - private bool _blockedForChanges; - - public List OverlappingPairs { get { return _overlappingPairs; } set { _overlappingPairs = value; } } - public bool BlockedForChanges { get { return _blockedForChanges; } set { _blockedForChanges = value; } } - - public static int OverlappingPairCount { get { return _overlappingPairCount; } set { _overlappingPairCount = value; } } - - public void RemoveOverlappingPair(BroadphasePair pair) - { - if (!_overlappingPairs.Contains(pair)) - return; - - CleanOverlappingPair(ref pair); - _overlappingPairs.Remove(pair); - } - - public void AddOverlappingPair(BroadphaseProxy proxyA, BroadphaseProxy proxyB) - { - //don't add overlap with own - bool test = proxyA != proxyB; - BulletDebug.Assert(proxyA != proxyB); - - if (!NeedsBroadphaseCollision(proxyA, proxyB)) - return; - - BroadphasePair pair = new BroadphasePair(proxyA, proxyB); - _overlappingPairs.Add(pair); - _overlappingPairCount++; - } - - //this FindPair becomes really slow. Either sort the list to speedup the query, or - //use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed. - //we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address) - //Also we can use a 2D bitmap, which can be useful for a future GPU implementation - public BroadphasePair FindPair(BroadphaseProxy proxyA, BroadphaseProxy proxyB) - { - if (!NeedsBroadphaseCollision(proxyA, proxyB)) - return null; - - BroadphasePair pair = new BroadphasePair(proxyA, proxyB); - for (int i = 0; i < _overlappingPairs.Count; i++) - { - if (_overlappingPairs[i] == pair) - { - return _overlappingPairs[i]; - } - } - - return null; - } - - public void CleanProxyFromPairs(BroadphaseProxy proxy) - { - for (int i = 0; i < _overlappingPairs.Count; i++) - { - BroadphasePair pair = _overlappingPairs[i]; - if (pair.ProxyA == proxy || - pair.ProxyB == proxy) - { - CleanOverlappingPair(ref pair); - _overlappingPairs[i] = pair; - } - } - } - - public void RemoveOverlappingPairsContainingProxy(BroadphaseProxy proxy) - { - for (int i = _overlappingPairs.Count - 1; i >= 0; i--) - { - BroadphasePair pair = _overlappingPairs[i]; - if (pair.ProxyA == proxy || - pair.ProxyB == proxy) - { - RemoveOverlappingPair(pair); - i++; - } - } - } - - public bool NeedsBroadphaseCollision(BroadphaseProxy proxy0, BroadphaseProxy proxy1) - { - bool collides = (proxy0.CollisionFilterGroup & proxy1.CollisionFilterMask) != 0; - collides = collides && ((proxy1.CollisionFilterGroup & proxy0.CollisionFilterMask) != 0); - - return collides; - } - - public virtual void ProcessAllOverlappingPairs(IOverlapCallback callback) - { - List deleting = new List(); - for (int i = 0; i < _overlappingPairs.Count; i++) - { - BroadphasePair p = _overlappingPairs[i]; - if (callback.ProcessOverlap(ref p)) - { - CleanOverlappingPair(ref p); - deleting.Add(p); - _overlappingPairCount--; - } - } - - for (int i = 0; i < deleting.Count; i++) - _overlappingPairs.Remove(deleting[i]); - } - - public void CleanOverlappingPair(ref BroadphasePair pair) - { - if (pair.CollisionAlgorithm != null) - { - if (pair.CollisionAlgorithm is IDisposable) - (pair.CollisionAlgorithm as IDisposable).Dispose(); - pair.CollisionAlgorithm = null; - } - } - - public abstract void RefreshOverlappingPairs(); - - #region IBroadphase Members - public abstract BroadphaseProxy CreateProxy(MonoXnaCompactMaths.Vector3 min, MonoXnaCompactMaths.Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask); - - public abstract void DestroyProxy(BroadphaseProxy proxy); - - public abstract void SetAabb(BroadphaseProxy proxy, MonoXnaCompactMaths.Vector3 aabbMin, MonoXnaCompactMaths.Vector3 aabbMax); - - #endregion - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace XnaDevRu.BulletX +{ + public abstract class OverlappingPairCache : IBroadphase + { + private static int _overlappingPairCount = 0; + private List _overlappingPairs = new List(); + //during the dispatch, check that user doesn't destroy/create proxy + private bool _blockedForChanges; + + public List OverlappingPairs { get { return _overlappingPairs; } set { _overlappingPairs = value; } } + public bool BlockedForChanges { get { return _blockedForChanges; } set { _blockedForChanges = value; } } + + public static int OverlappingPairCount { get { return _overlappingPairCount; } set { _overlappingPairCount = value; } } + + public void RemoveOverlappingPair(BroadphasePair pair) + { + if (!_overlappingPairs.Contains(pair)) + return; + + CleanOverlappingPair(ref pair); + _overlappingPairs.Remove(pair); + } + + public void AddOverlappingPair(BroadphaseProxy proxyA, BroadphaseProxy proxyB) + { + //don't add overlap with own + bool test = proxyA != proxyB; + BulletDebug.Assert(proxyA != proxyB); + + if (!NeedsBroadphaseCollision(proxyA, proxyB)) + return; + + BroadphasePair pair = new BroadphasePair(proxyA, proxyB); + _overlappingPairs.Add(pair); + _overlappingPairCount++; + } + + //this FindPair becomes really slow. Either sort the list to speedup the query, or + //use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed. + //we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address) + //Also we can use a 2D bitmap, which can be useful for a future GPU implementation + public BroadphasePair FindPair(BroadphaseProxy proxyA, BroadphaseProxy proxyB) + { + if (!NeedsBroadphaseCollision(proxyA, proxyB)) + return null; + + BroadphasePair pair = new BroadphasePair(proxyA, proxyB); + for (int i = 0; i < _overlappingPairs.Count; i++) + { + if (_overlappingPairs[i] == pair) + { + return _overlappingPairs[i]; + } + } + + return null; + } + + public void CleanProxyFromPairs(BroadphaseProxy proxy) + { + for (int i = 0; i < _overlappingPairs.Count; i++) + { + BroadphasePair pair = _overlappingPairs[i]; + if (pair.ProxyA == proxy || + pair.ProxyB == proxy) + { + CleanOverlappingPair(ref pair); + _overlappingPairs[i] = pair; + } + } + } + + public void RemoveOverlappingPairsContainingProxy(BroadphaseProxy proxy) + { + for (int i = _overlappingPairs.Count - 1; i >= 0; i--) + { + BroadphasePair pair = _overlappingPairs[i]; + if (pair.ProxyA == proxy || + pair.ProxyB == proxy) + { + RemoveOverlappingPair(pair); + i++; + } + } + } + + public bool NeedsBroadphaseCollision(BroadphaseProxy proxy0, BroadphaseProxy proxy1) + { + bool collides = (proxy0.CollisionFilterGroup & proxy1.CollisionFilterMask) != 0; + collides = collides && ((proxy1.CollisionFilterGroup & proxy0.CollisionFilterMask) != 0); + + return collides; + } + + public virtual void ProcessAllOverlappingPairs(IOverlapCallback callback) + { + List deleting = new List(); + for (int i = 0; i < _overlappingPairs.Count; i++) + { + BroadphasePair p = _overlappingPairs[i]; + if (callback.ProcessOverlap(ref p)) + { + CleanOverlappingPair(ref p); + deleting.Add(p); + _overlappingPairCount--; + } + } + + for (int i = 0; i < deleting.Count; i++) + _overlappingPairs.Remove(deleting[i]); + } + + public void CleanOverlappingPair(ref BroadphasePair pair) + { + if (pair.CollisionAlgorithm != null) + { + if (pair.CollisionAlgorithm is IDisposable) + (pair.CollisionAlgorithm as IDisposable).Dispose(); + pair.CollisionAlgorithm = null; + } + } + + public abstract void RefreshOverlappingPairs(); + + #region IBroadphase Members + public abstract BroadphaseProxy CreateProxy(MonoXnaCompactMaths.Vector3 min, MonoXnaCompactMaths.Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask); + + public abstract void DestroyProxy(BroadphaseProxy proxy); + + public abstract void SetAabb(BroadphaseProxy proxy, MonoXnaCompactMaths.Vector3 aabbMin, MonoXnaCompactMaths.Vector3 aabbMax); + + #endregion + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs index 1dc3f34..f37b3f5 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs @@ -1,128 +1,128 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public class SimpleBroadphase : OverlappingPairCache - { - private int _maxProxies; - private List _proxies = new List(); - - public SimpleBroadphase() - : this(16384) { } - - public SimpleBroadphase(int maxProxies) - : base() - { - _maxProxies = maxProxies; - } - - public override BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask) - { - if (_proxies.Count >= _maxProxies) - { - BulletDebug.Assert(false); - return null; //should never happen, but don't let the game crash ;-) - } - BulletDebug.Assert(min.X <= max.X && min.Y <= max.Y && min.Z <= max.Z); - - SimpleBroadphaseProxy proxy = new SimpleBroadphaseProxy(min, max, shapeType, userData, collisionFilterGroup, collisionFilterMask); - _proxies.Add(proxy); - - return proxy; - } - - - public override void DestroyProxy(BroadphaseProxy proxy) - { - RemoveOverlappingPairsContainingProxy(proxy); - _proxies.Remove(proxy as SimpleBroadphaseProxy); - } - - public override void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax) - { - SimpleBroadphaseProxy simpleProxy = GetSimpleProxyFromProxy(proxy); - simpleProxy.Minimum = aabbMin; - simpleProxy.Maximum = aabbMax; - } - - private SimpleBroadphaseProxy GetSimpleProxyFromProxy(BroadphaseProxy proxy) - { - return proxy as SimpleBroadphaseProxy; - } - - public override void RefreshOverlappingPairs() - { - for (int i = 0; i < _proxies.Count; i++) - { - SimpleBroadphaseProxy proxyA = _proxies[i]; - - for (int j = i + 1; j < _proxies.Count; j++) - { - SimpleBroadphaseProxy proxyB = _proxies[j]; - - if (AabbOverlap(proxyA, proxyB)) - { - if (FindPair(proxyA, proxyB) == null) - { - AddOverlappingPair(proxyA, proxyB); - } - } - } - } - - CheckOverlapCallback check = new CheckOverlapCallback(); - ProcessAllOverlappingPairs(check); - } - - public static bool AabbOverlap(SimpleBroadphaseProxy proxyA, SimpleBroadphaseProxy proxyB) - { - return proxyA.Minimum.X <= proxyB.Maximum.X && proxyB.Minimum.X <= proxyA.Maximum.X && - proxyA.Minimum.Y <= proxyB.Maximum.Y && proxyB.Minimum.Y <= proxyA.Maximum.Y && - proxyA.Minimum.Z <= proxyB.Maximum.Z && proxyB.Minimum.Z <= proxyA.Maximum.Z; - } - - private void Validate() - { - for (int i = 0; i < _proxies.Count; i++) - { - for (int j = i + 1; j < _proxies.Count; j++) - { - if (_proxies[i] == _proxies[j]) - throw new BulletException(); - } - } - } - } - - public class CheckOverlapCallback : IOverlapCallback - { - public bool ProcessOverlap(ref BroadphasePair pair) - { - return (!SimpleBroadphase.AabbOverlap(pair.ProxyA as SimpleBroadphaseProxy, pair.ProxyB as SimpleBroadphaseProxy)); - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public class SimpleBroadphase : OverlappingPairCache + { + private int _maxProxies; + private List _proxies = new List(); + + public SimpleBroadphase() + : this(16384) { } + + public SimpleBroadphase(int maxProxies) + : base() + { + _maxProxies = maxProxies; + } + + public override BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask) + { + if (_proxies.Count >= _maxProxies) + { + BulletDebug.Assert(false); + return null; //should never happen, but don't let the game crash ;-) + } + BulletDebug.Assert(min.X <= max.X && min.Y <= max.Y && min.Z <= max.Z); + + SimpleBroadphaseProxy proxy = new SimpleBroadphaseProxy(min, max, shapeType, userData, collisionFilterGroup, collisionFilterMask); + _proxies.Add(proxy); + + return proxy; + } + + + public override void DestroyProxy(BroadphaseProxy proxy) + { + RemoveOverlappingPairsContainingProxy(proxy); + _proxies.Remove(proxy as SimpleBroadphaseProxy); + } + + public override void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax) + { + SimpleBroadphaseProxy simpleProxy = GetSimpleProxyFromProxy(proxy); + simpleProxy.Minimum = aabbMin; + simpleProxy.Maximum = aabbMax; + } + + private SimpleBroadphaseProxy GetSimpleProxyFromProxy(BroadphaseProxy proxy) + { + return proxy as SimpleBroadphaseProxy; + } + + public override void RefreshOverlappingPairs() + { + for (int i = 0; i < _proxies.Count; i++) + { + SimpleBroadphaseProxy proxyA = _proxies[i]; + + for (int j = i + 1; j < _proxies.Count; j++) + { + SimpleBroadphaseProxy proxyB = _proxies[j]; + + if (AabbOverlap(proxyA, proxyB)) + { + if (FindPair(proxyA, proxyB) == null) + { + AddOverlappingPair(proxyA, proxyB); + } + } + } + } + + CheckOverlapCallback check = new CheckOverlapCallback(); + ProcessAllOverlappingPairs(check); + } + + public static bool AabbOverlap(SimpleBroadphaseProxy proxyA, SimpleBroadphaseProxy proxyB) + { + return proxyA.Minimum.X <= proxyB.Maximum.X && proxyB.Minimum.X <= proxyA.Maximum.X && + proxyA.Minimum.Y <= proxyB.Maximum.Y && proxyB.Minimum.Y <= proxyA.Maximum.Y && + proxyA.Minimum.Z <= proxyB.Maximum.Z && proxyB.Minimum.Z <= proxyA.Maximum.Z; + } + + private void Validate() + { + for (int i = 0; i < _proxies.Count; i++) + { + for (int j = i + 1; j < _proxies.Count; j++) + { + if (_proxies[i] == _proxies[j]) + throw new BulletException(); + } + } + } + } + + public class CheckOverlapCallback : IOverlapCallback + { + public bool ProcessOverlap(ref BroadphasePair pair) + { + return (!SimpleBroadphase.AabbOverlap(pair.ProxyA as SimpleBroadphaseProxy, pair.ProxyB as SimpleBroadphaseProxy)); + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs index f473ef2..cfc2150 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs @@ -1,46 +1,46 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public class SimpleBroadphaseProxy : BroadphaseProxy - { - private Vector3 _min; - private Vector3 _max; - - public SimpleBroadphaseProxy() { } - - public SimpleBroadphaseProxy(Vector3 minPoint, Vector3 maxPoint, BroadphaseNativeTypes shapeType, object userData, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask) - : base(userData, collisionFilterGroup, collisionFilterMask) - { - _min = minPoint; - _max = maxPoint; - } - - public Vector3 Minimum { get { return _min; } set { _min = value; } } - public Vector3 Maximum { get { return _max; } set { _max = value; } } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public class SimpleBroadphaseProxy : BroadphaseProxy + { + private Vector3 _min; + private Vector3 _max; + + public SimpleBroadphaseProxy() { } + + public SimpleBroadphaseProxy(Vector3 minPoint, Vector3 maxPoint, BroadphaseNativeTypes shapeType, object userData, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask) + : base(userData, collisionFilterGroup, collisionFilterMask) + { + _min = minPoint; + _max = maxPoint; + } + + public Vector3 Minimum { get { return _min; } set { _min = value; } } + public Vector3 Maximum { get { return _max; } set { _max = value; } } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs index ba638d1..6bebf93 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs @@ -1,59 +1,59 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - internal class BridgeTriangleRaycastCallback : TriangleRaycastCallback - { - CollisionWorld.RayResultCallback _resultCallback; - CollisionObject _collisionObject; - TriangleMeshShape _triangleMesh; - - public BridgeTriangleRaycastCallback(Vector3 from, Vector3 to, - CollisionWorld.RayResultCallback resultCallback, CollisionObject collisionObject, TriangleMeshShape triangleMesh) - : base(from, to) - { - _resultCallback = resultCallback; - _collisionObject = collisionObject; - _triangleMesh = triangleMesh; - } - - public override float ReportHit(Vector3 hitNormalLocal, float hitFraction, int partId, int triangleIndex) - { - CollisionWorld.LocalShapeInfo shapeInfo = new CollisionWorld.LocalShapeInfo(); - shapeInfo.ShapePart = partId; - shapeInfo.TriangleIndex = triangleIndex; - - CollisionWorld.LocalRayResult rayResult = new CollisionWorld.LocalRayResult - (_collisionObject, - shapeInfo, - hitNormalLocal, - hitFraction); - - return _resultCallback.AddSingleResult(rayResult); - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + internal class BridgeTriangleRaycastCallback : TriangleRaycastCallback + { + CollisionWorld.RayResultCallback _resultCallback; + CollisionObject _collisionObject; + TriangleMeshShape _triangleMesh; + + public BridgeTriangleRaycastCallback(Vector3 from, Vector3 to, + CollisionWorld.RayResultCallback resultCallback, CollisionObject collisionObject, TriangleMeshShape triangleMesh) + : base(from, to) + { + _resultCallback = resultCallback; + _collisionObject = collisionObject; + _triangleMesh = triangleMesh; + } + + public override float ReportHit(Vector3 hitNormalLocal, float hitFraction, int partId, int triangleIndex) + { + CollisionWorld.LocalShapeInfo shapeInfo = new CollisionWorld.LocalShapeInfo(); + shapeInfo.ShapePart = partId; + shapeInfo.TriangleIndex = triangleIndex; + + CollisionWorld.LocalRayResult rayResult = new CollisionWorld.LocalRayResult + (_collisionObject, + shapeInfo, + hitNormalLocal, + hitFraction); + + return _resultCallback.AddSingleResult(rayResult); + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs index 4e3903b..3bd5601 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs @@ -1,40 +1,40 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public class CollisionAlgorithmCreateFunction - { - private bool _swapped; - - public bool IsSwapped { get { return _swapped; } set { _swapped = value; } } - - public virtual CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) - { - return null; - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public class CollisionAlgorithmCreateFunction + { + private bool _swapped; + + public bool IsSwapped { get { return _swapped; } set { _swapped = value; } } + + public virtual CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) + { + return null; + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs index 75d35b0..336defd 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs @@ -1,280 +1,280 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public delegate void NearCallback(ref BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo); - - public class CollisionDispatcher : IDispatcher - { - private List _manifolds = new List(); - - //private bool _useIslands; - private NearCallback _nearCallback; - - //private ManifoldResult _defaultManifoldResult; - - private CollisionAlgorithmCreateFunction[,] _doubleDispatch = new CollisionAlgorithmCreateFunction[(int)BroadphaseNativeTypes.MaxBroadphaseCollisionTypes, (int)BroadphaseNativeTypes.MaxBroadphaseCollisionTypes]; - - //default CreationFunctions, filling the m_doubleDispatch table - private CollisionAlgorithmCreateFunction _convexConvexCreateFunc; - private CollisionAlgorithmCreateFunction _convexConcaveCreateFunc; - private CollisionAlgorithmCreateFunction _swappedConvexConcaveCreateFunc; - private CollisionAlgorithmCreateFunction _compoundCreateFunc; - private CollisionAlgorithmCreateFunction _swappedCompoundCreateFunc; - private CollisionAlgorithmCreateFunction _emptyCreateFunc; - - private int _count; - private static int _manifoldCount = 0; - - public CollisionDispatcher() - { - NearCallback = DefaultNearCallback; - //_useIslands = true; - //default CreationFunctions, filling the m_doubleDispatch table - _convexConvexCreateFunc = new ConvexConvexAlgorithm.CreateFunc(); - _convexConcaveCreateFunc = new ConvexConcaveCollisionAlgorithm.CreateFunc(); - _swappedConvexConcaveCreateFunc = new ConvexConcaveCollisionAlgorithm.SwappedCreateFunc(); - _compoundCreateFunc = new CompoundCollisionAlgorithm.CreateFunc(); - _swappedCompoundCreateFunc = new CompoundCollisionAlgorithm.SwappedCreateFunc(); - _emptyCreateFunc = new EmptyAlgorithm.CreateFunc(); - - for (BroadphaseNativeTypes i = BroadphaseNativeTypes.Box; i < BroadphaseNativeTypes.MaxBroadphaseCollisionTypes; i++) - { - for (BroadphaseNativeTypes j = BroadphaseNativeTypes.Box; j < BroadphaseNativeTypes.MaxBroadphaseCollisionTypes; j++) - { - _doubleDispatch[(int)i, (int)j] = FindCreateFunction(i, j); - if (_doubleDispatch[(int)i, (int)j] == null) - throw new BulletException(); - } - } - } - - public int Count { get { return _count; } set { _count = value; } } - public int ManifoldCount { get { return _manifolds.Count; } } - public List Manifolds { get { return _manifolds; } } - - public static int GlobalManifoldCount { get { return _manifoldCount; } set { _manifoldCount = value; } } - - public PersistentManifold GetManifoldByIndex(int index) - { - return _manifolds[index]; - } - - //registerCollisionCreateFunc allows registration of custom/alternative collision create functions - public void RegisterCollisionCreateFunc(BroadphaseNativeTypes proxyTypeA, BroadphaseNativeTypes proxyTypeB, CollisionAlgorithmCreateFunction createFunc) - { - _doubleDispatch[(int)proxyTypeA, (int)proxyTypeB] = createFunc; - } - - public virtual PersistentManifold GetNewManifold(object bodyA, object bodyB) - { - _manifoldCount++; - - CollisionObject body0 = bodyA as CollisionObject; - CollisionObject body1 = bodyB as CollisionObject; - - PersistentManifold manifold = new PersistentManifold(body0, body1); - _manifolds.Add(manifold); - - return manifold; - } - - public virtual void ReleaseManifold(PersistentManifold manifold) - { - _manifoldCount--; - - ClearManifold(manifold); - _manifolds.Remove(manifold); - } - - - public virtual void ClearManifold(PersistentManifold manifold) - { - manifold.ClearManifold(); - } - - public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB) - { - return FindAlgorithm(bodyA, bodyB, null); - } - - public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB, PersistentManifold sharedManifold) - { - CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo = new CollisionAlgorithmConstructionInfo(); - collisionAlgorithmConstructionInfo.Dispatcher = this; - collisionAlgorithmConstructionInfo.Manifold = sharedManifold; - CollisionAlgorithm collisionAlgorithm = _doubleDispatch[(int)bodyA.CollisionShape.ShapeType, (int)bodyB.CollisionShape.ShapeType].CreateCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB); - return collisionAlgorithm; - } - - /*public CollisionAlgorithm internalFindAlgorithm(CollisionObject body0, CollisionObject body1) - { - return internalFindAlgorithm(body0, body1, null); - } - - public CollisionAlgorithm internalFindAlgorithm(CollisionObject body0, CollisionObject body1, PersistentManifold sharedManifold) - { - m_count++; - - CollisionAlgorithmConstructionInfo ci = new CollisionAlgorithmConstructionInfo(); - ci.m_dispatcher = this; - - if (body0.getCollisionShape().isConvex() && body1.getCollisionShape().isConvex()) - { - return new ConvexConvexAlgorithm(sharedManifold, ci, body0, body1); - } - - if (body0.getCollisionShape().isConvex() && body1.getCollisionShape().isConcave()) - { - return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, false); - } - - if (body1.getCollisionShape().isConvex() && body0.getCollisionShape().isConcave()) - { - return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, true); - } - - if (body0.getCollisionShape().isCompound()) - { - return new CompoundCollisionAlgorithm(ci, body0, body1, false); - } - else - { - if (body1.getCollisionShape().isCompound()) - { - return new CompoundCollisionAlgorithm(ci, body0, body1, true); - } - } - - //failed to find an algorithm - return new EmptyAlgorithm(ci); - }*/ - - public virtual bool NeedsCollision(CollisionObject bodyA, CollisionObject bodyB) - { - if (bodyA == null || bodyB == null) - throw new BulletException(); - - bool needsCollision = true; - - //broadphase filtering already deals with this - /*if ((body0.isStaticObject() || body0.isKinematicObject()) && - (body1.isStaticObject() || body1.isKinematicObject())) - { - printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n"); - }*/ - - if ((!bodyA.IsActive) && (!bodyB.IsActive)) - needsCollision = false; - - return needsCollision; - } - - public virtual bool NeedsResponse(CollisionObject bodyA, CollisionObject bodyB) - { - //here you can do filtering - bool hasResponse = bodyA.HasContactResponse && bodyB.HasContactResponse; - hasResponse = hasResponse && (!bodyA.IsStaticOrKinematicObject || !bodyB.IsStaticOrKinematicObject); - return hasResponse; - } - - public virtual void DispatchAllCollisionPairs(OverlappingPairCache pairCache, DispatcherInfo dispatchInfo) - { - CollisionPairCallback collisionCallback = new CollisionPairCallback(dispatchInfo, this); - pairCache.ProcessAllOverlappingPairs(collisionCallback); - } - - private CollisionAlgorithmCreateFunction FindCreateFunction(BroadphaseNativeTypes proxyTypeA, BroadphaseNativeTypes proxyTypeB) - { - if (BroadphaseProxy.IsConvex(proxyTypeA) && BroadphaseProxy.IsConvex(proxyTypeB)) - { - return _convexConvexCreateFunc; - } - - if (BroadphaseProxy.IsConvex(proxyTypeA) && BroadphaseProxy.IsConcave(proxyTypeB)) - { - return _convexConcaveCreateFunc; - } - - if (BroadphaseProxy.IsConvex(proxyTypeB) && BroadphaseProxy.IsConcave(proxyTypeA)) - { - return _swappedConvexConcaveCreateFunc; - } - - if (BroadphaseProxy.IsCompound(proxyTypeA)) - { - return _compoundCreateFunc; - } - else - { - if (BroadphaseProxy.IsCompound(proxyTypeB)) - { - return _swappedCompoundCreateFunc; - } - } - - //failed to find an algorithm - return _emptyCreateFunc; - } - - public NearCallback NearCallback { get { return _nearCallback; } set { _nearCallback = value; } } - - //by default, Bullet will use this near callback - public static void DefaultNearCallback(ref BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo) - { - CollisionObject collisionObjectA = collisionPair.ProxyA.ClientData as CollisionObject; - CollisionObject collisionObjectB = collisionPair.ProxyB.ClientData as CollisionObject; - - if (dispatcher.NeedsCollision(collisionObjectA, collisionObjectB)) - { - //dispatcher will keep algorithms persistent in the collision pair - if (collisionPair.CollisionAlgorithm == null) - { - collisionPair.CollisionAlgorithm = dispatcher.FindAlgorithm(collisionObjectA, collisionObjectB); - } - - if (collisionPair.CollisionAlgorithm != null) - { - ManifoldResult contactPointResult = new ManifoldResult(collisionObjectA, collisionObjectB); - - if (dispatchInfo.DispatchFunction == DispatchFunction.Discrete) - { - //discrete collision detection query - collisionPair.CollisionAlgorithm.ProcessCollision(collisionObjectA, collisionObjectB, dispatchInfo, contactPointResult); - } - else - { - //continuous collision detection query, time of impact (toi) - float timeOfImpact = collisionPair.CollisionAlgorithm.CalculateTimeOfImpact(collisionObjectA, collisionObjectB, dispatchInfo, contactPointResult); - if (dispatchInfo.TimeOfImpact > timeOfImpact) - dispatchInfo.TimeOfImpact = timeOfImpact; - } - } - } - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public delegate void NearCallback(ref BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo); + + public class CollisionDispatcher : IDispatcher + { + private List _manifolds = new List(); + + //private bool _useIslands; + private NearCallback _nearCallback; + + //private ManifoldResult _defaultManifoldResult; + + private CollisionAlgorithmCreateFunction[,] _doubleDispatch = new CollisionAlgorithmCreateFunction[(int)BroadphaseNativeTypes.MaxBroadphaseCollisionTypes, (int)BroadphaseNativeTypes.MaxBroadphaseCollisionTypes]; + + //default CreationFunctions, filling the m_doubleDispatch table + private CollisionAlgorithmCreateFunction _convexConvexCreateFunc; + private CollisionAlgorithmCreateFunction _convexConcaveCreateFunc; + private CollisionAlgorithmCreateFunction _swappedConvexConcaveCreateFunc; + private CollisionAlgorithmCreateFunction _compoundCreateFunc; + private CollisionAlgorithmCreateFunction _swappedCompoundCreateFunc; + private CollisionAlgorithmCreateFunction _emptyCreateFunc; + + private int _count; + private static int _manifoldCount = 0; + + public CollisionDispatcher() + { + NearCallback = DefaultNearCallback; + //_useIslands = true; + //default CreationFunctions, filling the m_doubleDispatch table + _convexConvexCreateFunc = new ConvexConvexAlgorithm.CreateFunc(); + _convexConcaveCreateFunc = new ConvexConcaveCollisionAlgorithm.CreateFunc(); + _swappedConvexConcaveCreateFunc = new ConvexConcaveCollisionAlgorithm.SwappedCreateFunc(); + _compoundCreateFunc = new CompoundCollisionAlgorithm.CreateFunc(); + _swappedCompoundCreateFunc = new CompoundCollisionAlgorithm.SwappedCreateFunc(); + _emptyCreateFunc = new EmptyAlgorithm.CreateFunc(); + + for (BroadphaseNativeTypes i = BroadphaseNativeTypes.Box; i < BroadphaseNativeTypes.MaxBroadphaseCollisionTypes; i++) + { + for (BroadphaseNativeTypes j = BroadphaseNativeTypes.Box; j < BroadphaseNativeTypes.MaxBroadphaseCollisionTypes; j++) + { + _doubleDispatch[(int)i, (int)j] = FindCreateFunction(i, j); + if (_doubleDispatch[(int)i, (int)j] == null) + throw new BulletException(); + } + } + } + + public int Count { get { return _count; } set { _count = value; } } + public int ManifoldCount { get { return _manifolds.Count; } } + public List Manifolds { get { return _manifolds; } } + + public static int GlobalManifoldCount { get { return _manifoldCount; } set { _manifoldCount = value; } } + + public PersistentManifold GetManifoldByIndex(int index) + { + return _manifolds[index]; + } + + //registerCollisionCreateFunc allows registration of custom/alternative collision create functions + public void RegisterCollisionCreateFunc(BroadphaseNativeTypes proxyTypeA, BroadphaseNativeTypes proxyTypeB, CollisionAlgorithmCreateFunction createFunc) + { + _doubleDispatch[(int)proxyTypeA, (int)proxyTypeB] = createFunc; + } + + public virtual PersistentManifold GetNewManifold(object bodyA, object bodyB) + { + _manifoldCount++; + + CollisionObject body0 = bodyA as CollisionObject; + CollisionObject body1 = bodyB as CollisionObject; + + PersistentManifold manifold = new PersistentManifold(body0, body1); + _manifolds.Add(manifold); + + return manifold; + } + + public virtual void ReleaseManifold(PersistentManifold manifold) + { + _manifoldCount--; + + ClearManifold(manifold); + _manifolds.Remove(manifold); + } + + + public virtual void ClearManifold(PersistentManifold manifold) + { + manifold.ClearManifold(); + } + + public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB) + { + return FindAlgorithm(bodyA, bodyB, null); + } + + public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB, PersistentManifold sharedManifold) + { + CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo = new CollisionAlgorithmConstructionInfo(); + collisionAlgorithmConstructionInfo.Dispatcher = this; + collisionAlgorithmConstructionInfo.Manifold = sharedManifold; + CollisionAlgorithm collisionAlgorithm = _doubleDispatch[(int)bodyA.CollisionShape.ShapeType, (int)bodyB.CollisionShape.ShapeType].CreateCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB); + return collisionAlgorithm; + } + + /*public CollisionAlgorithm internalFindAlgorithm(CollisionObject body0, CollisionObject body1) + { + return internalFindAlgorithm(body0, body1, null); + } + + public CollisionAlgorithm internalFindAlgorithm(CollisionObject body0, CollisionObject body1, PersistentManifold sharedManifold) + { + m_count++; + + CollisionAlgorithmConstructionInfo ci = new CollisionAlgorithmConstructionInfo(); + ci.m_dispatcher = this; + + if (body0.getCollisionShape().isConvex() && body1.getCollisionShape().isConvex()) + { + return new ConvexConvexAlgorithm(sharedManifold, ci, body0, body1); + } + + if (body0.getCollisionShape().isConvex() && body1.getCollisionShape().isConcave()) + { + return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, false); + } + + if (body1.getCollisionShape().isConvex() && body0.getCollisionShape().isConcave()) + { + return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, true); + } + + if (body0.getCollisionShape().isCompound()) + { + return new CompoundCollisionAlgorithm(ci, body0, body1, false); + } + else + { + if (body1.getCollisionShape().isCompound()) + { + return new CompoundCollisionAlgorithm(ci, body0, body1, true); + } + } + + //failed to find an algorithm + return new EmptyAlgorithm(ci); + }*/ + + public virtual bool NeedsCollision(CollisionObject bodyA, CollisionObject bodyB) + { + if (bodyA == null || bodyB == null) + throw new BulletException(); + + bool needsCollision = true; + + //broadphase filtering already deals with this + /*if ((body0.isStaticObject() || body0.isKinematicObject()) && + (body1.isStaticObject() || body1.isKinematicObject())) + { + printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n"); + }*/ + + if ((!bodyA.IsActive) && (!bodyB.IsActive)) + needsCollision = false; + + return needsCollision; + } + + public virtual bool NeedsResponse(CollisionObject bodyA, CollisionObject bodyB) + { + //here you can do filtering + bool hasResponse = bodyA.HasContactResponse && bodyB.HasContactResponse; + hasResponse = hasResponse && (!bodyA.IsStaticOrKinematicObject || !bodyB.IsStaticOrKinematicObject); + return hasResponse; + } + + public virtual void DispatchAllCollisionPairs(OverlappingPairCache pairCache, DispatcherInfo dispatchInfo) + { + CollisionPairCallback collisionCallback = new CollisionPairCallback(dispatchInfo, this); + pairCache.ProcessAllOverlappingPairs(collisionCallback); + } + + private CollisionAlgorithmCreateFunction FindCreateFunction(BroadphaseNativeTypes proxyTypeA, BroadphaseNativeTypes proxyTypeB) + { + if (BroadphaseProxy.IsConvex(proxyTypeA) && BroadphaseProxy.IsConvex(proxyTypeB)) + { + return _convexConvexCreateFunc; + } + + if (BroadphaseProxy.IsConvex(proxyTypeA) && BroadphaseProxy.IsConcave(proxyTypeB)) + { + return _convexConcaveCreateFunc; + } + + if (BroadphaseProxy.IsConvex(proxyTypeB) && BroadphaseProxy.IsConcave(proxyTypeA)) + { + return _swappedConvexConcaveCreateFunc; + } + + if (BroadphaseProxy.IsCompound(proxyTypeA)) + { + return _compoundCreateFunc; + } + else + { + if (BroadphaseProxy.IsCompound(proxyTypeB)) + { + return _swappedCompoundCreateFunc; + } + } + + //failed to find an algorithm + return _emptyCreateFunc; + } + + public NearCallback NearCallback { get { return _nearCallback; } set { _nearCallback = value; } } + + //by default, Bullet will use this near callback + public static void DefaultNearCallback(ref BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo) + { + CollisionObject collisionObjectA = collisionPair.ProxyA.ClientData as CollisionObject; + CollisionObject collisionObjectB = collisionPair.ProxyB.ClientData as CollisionObject; + + if (dispatcher.NeedsCollision(collisionObjectA, collisionObjectB)) + { + //dispatcher will keep algorithms persistent in the collision pair + if (collisionPair.CollisionAlgorithm == null) + { + collisionPair.CollisionAlgorithm = dispatcher.FindAlgorithm(collisionObjectA, collisionObjectB); + } + + if (collisionPair.CollisionAlgorithm != null) + { + ManifoldResult contactPointResult = new ManifoldResult(collisionObjectA, collisionObjectB); + + if (dispatchInfo.DispatchFunction == DispatchFunction.Discrete) + { + //discrete collision detection query + collisionPair.CollisionAlgorithm.ProcessCollision(collisionObjectA, collisionObjectB, dispatchInfo, contactPointResult); + } + else + { + //continuous collision detection query, time of impact (toi) + float timeOfImpact = collisionPair.CollisionAlgorithm.CalculateTimeOfImpact(collisionObjectA, collisionObjectB, dispatchInfo, contactPointResult); + if (dispatchInfo.TimeOfImpact > timeOfImpact) + dispatchInfo.TimeOfImpact = timeOfImpact; + } + } + } + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs index 4e9cf58..3eae564 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs @@ -1,163 +1,163 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public enum ActivationState - { - Nothing = 0, - Active, - IslandSleeping, - WantsDeactivation, - DisableDeactivation, - DisableSimulation, - } - - public enum CollisionOptions - { - StaticObject = 1, - KinematicObject = 2, - NoContactResponse = 4, - CustomMaterialCallback = 8,//this allows per-triangle material (friction/restitution) - } - - /// - /// btCollisionObject can be used to manage collision detection objects. - /// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy. - /// They can be added to the btCollisionWorld. - /// - public class CollisionObject - { - protected Matrix _worldTransform; - private BroadphaseProxy _broadphase; - private CollisionShape _collisionShape; - - //m_interpolationWorldTransform is used for CCD and interpolation - //it can be either previous or future (predicted) transform - private Matrix _interpolationWorldTransform; - - private CollisionOptions _collisionFlags; - - private int _islandTag; - private ActivationState _activationState; - private float _deactivationTime; - - private float _friction; - private float _restitution; - - //users can point to their objects, m_userPointer is not used by Bullet - private object _userData; - - //m_internalOwner one is used by optional Bullet high level interface - private object _internalOwner; - - //time of impact calculation - private float _hitFraction; - - //Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: - private float _ccdSweptSphereRadius; - - // Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionTreshold - private float _ccdSquareMotionThreshold; - //those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities) - //without destroying the continuous interpolated motion (which uses this interpolation velocities) - private Vector3 _interpolationLinearVelocity; - private Vector3 _interpolationAngularVelocity; - - private int _companionID; - - public CollisionObject() - { - _activationState = ActivationState.Active; - _hitFraction = 1; - } - - public bool IsStaticObject { get { return (_collisionFlags & CollisionOptions.StaticObject) != 0; } } - public bool IsKinematicObject { get { return (_collisionFlags & CollisionOptions.KinematicObject) != 0; } } - public bool IsStaticOrKinematicObject { get { return (_collisionFlags & (CollisionOptions.KinematicObject | CollisionOptions.StaticObject)) != 0; } } - - public bool HasContactResponse { get { return (_collisionFlags & CollisionOptions.NoContactResponse) == 0; } } - public bool MergesSimulationIslands - { - get - { - //static objects, kinematic and object without contact response don't merge islands - return (_collisionFlags & (CollisionOptions.StaticObject | CollisionOptions.KinematicObject | CollisionOptions.NoContactResponse)) == 0; - } - } - - public ActivationState ActivationState - { - get { return _activationState; } - set - { - if ((_activationState != ActivationState.DisableDeactivation) && (_activationState != ActivationState.DisableSimulation)) - _activationState = value; - } - } - - public bool IsActive { get { return ((ActivationState != ActivationState.IslandSleeping) && (ActivationState != ActivationState.DisableSimulation)); } } - public float Restitution { get { return _restitution; } set { _restitution = value; } } - public float Friction { get { return _friction; } set { _friction = value; } } - public CollisionShape CollisionShape { get { return _collisionShape; } set { _collisionShape = value; } } - public float DeactivationTime { get { return _deactivationTime; } set { _deactivationTime = value; } } - public object Owner { get { return _internalOwner; } protected set { _internalOwner = value; } } - public Matrix WorldTransform { get { return _worldTransform; } set { _worldTransform = value; } } - public BroadphaseProxy Broadphase { get { return _broadphase; } set { _broadphase = value; } } - public Matrix InterpolationWorldTransform { get { return _interpolationWorldTransform; } set { _interpolationWorldTransform = value; } } - public Vector3 InterpolationLinearVelocity { get { return _interpolationLinearVelocity; } protected set { _interpolationLinearVelocity = value; } } - public Vector3 InterpolationAngularVelocity { get { return _interpolationAngularVelocity; } protected set { _interpolationAngularVelocity = value; } } - public int IslandTag { get { return _islandTag; } set { _islandTag = value; } } - public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } - public CollisionOptions CollisionFlags { get { return _collisionFlags; } set { _collisionFlags = value; } } - //Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm - public float CcdSweptSphereRadius { get { return _ccdSweptSphereRadius; } set { _ccdSweptSphereRadius = value; } } - // Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold - public float CcdSquareMotionThreshold { get { return _ccdSquareMotionThreshold; } set { _ccdSquareMotionThreshold = value; } } - //users can point to their objects, userPointer is not used by Bullet - public object UserData { get { return _userData; } set { _userData = value; } } - public int CompanionID { get { return _companionID; } set { _companionID = value; } } - - public void ForceActivationState(ActivationState newState) - { - _activationState = newState; - } - - public void Activate() - { - Activate(false); - } - - public void Activate(bool forceActivation) - { - if (forceActivation || (_collisionFlags & (CollisionOptions.StaticObject | CollisionOptions.KinematicObject)) == 0) - { - ActivationState = ActivationState.Active; - _deactivationTime = 0; - } - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public enum ActivationState + { + Nothing = 0, + Active, + IslandSleeping, + WantsDeactivation, + DisableDeactivation, + DisableSimulation, + } + + public enum CollisionOptions + { + StaticObject = 1, + KinematicObject = 2, + NoContactResponse = 4, + CustomMaterialCallback = 8,//this allows per-triangle material (friction/restitution) + } + + /// + /// btCollisionObject can be used to manage collision detection objects. + /// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy. + /// They can be added to the btCollisionWorld. + /// + public class CollisionObject + { + protected Matrix _worldTransform; + private BroadphaseProxy _broadphase; + private CollisionShape _collisionShape; + + //m_interpolationWorldTransform is used for CCD and interpolation + //it can be either previous or future (predicted) transform + private Matrix _interpolationWorldTransform; + + private CollisionOptions _collisionFlags; + + private int _islandTag; + private ActivationState _activationState; + private float _deactivationTime; + + private float _friction; + private float _restitution; + + //users can point to their objects, m_userPointer is not used by Bullet + private object _userData; + + //m_internalOwner one is used by optional Bullet high level interface + private object _internalOwner; + + //time of impact calculation + private float _hitFraction; + + //Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: + private float _ccdSweptSphereRadius; + + // Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionTreshold + private float _ccdSquareMotionThreshold; + //those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities) + //without destroying the continuous interpolated motion (which uses this interpolation velocities) + private Vector3 _interpolationLinearVelocity; + private Vector3 _interpolationAngularVelocity; + + private int _companionID; + + public CollisionObject() + { + _activationState = ActivationState.Active; + _hitFraction = 1; + } + + public bool IsStaticObject { get { return (_collisionFlags & CollisionOptions.StaticObject) != 0; } } + public bool IsKinematicObject { get { return (_collisionFlags & CollisionOptions.KinematicObject) != 0; } } + public bool IsStaticOrKinematicObject { get { return (_collisionFlags & (CollisionOptions.KinematicObject | CollisionOptions.StaticObject)) != 0; } } + + public bool HasContactResponse { get { return (_collisionFlags & CollisionOptions.NoContactResponse) == 0; } } + public bool MergesSimulationIslands + { + get + { + //static objects, kinematic and object without contact response don't merge islands + return (_collisionFlags & (CollisionOptions.StaticObject | CollisionOptions.KinematicObject | CollisionOptions.NoContactResponse)) == 0; + } + } + + public ActivationState ActivationState + { + get { return _activationState; } + set + { + if ((_activationState != ActivationState.DisableDeactivation) && (_activationState != ActivationState.DisableSimulation)) + _activationState = value; + } + } + + public bool IsActive { get { return ((ActivationState != ActivationState.IslandSleeping) && (ActivationState != ActivationState.DisableSimulation)); } } + public float Restitution { get { return _restitution; } set { _restitution = value; } } + public float Friction { get { return _friction; } set { _friction = value; } } + public CollisionShape CollisionShape { get { return _collisionShape; } set { _collisionShape = value; } } + public float DeactivationTime { get { return _deactivationTime; } set { _deactivationTime = value; } } + public object Owner { get { return _internalOwner; } protected set { _internalOwner = value; } } + public Matrix WorldTransform { get { return _worldTransform; } set { _worldTransform = value; } } + public BroadphaseProxy Broadphase { get { return _broadphase; } set { _broadphase = value; } } + public Matrix InterpolationWorldTransform { get { return _interpolationWorldTransform; } set { _interpolationWorldTransform = value; } } + public Vector3 InterpolationLinearVelocity { get { return _interpolationLinearVelocity; } protected set { _interpolationLinearVelocity = value; } } + public Vector3 InterpolationAngularVelocity { get { return _interpolationAngularVelocity; } protected set { _interpolationAngularVelocity = value; } } + public int IslandTag { get { return _islandTag; } set { _islandTag = value; } } + public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } + public CollisionOptions CollisionFlags { get { return _collisionFlags; } set { _collisionFlags = value; } } + //Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm + public float CcdSweptSphereRadius { get { return _ccdSweptSphereRadius; } set { _ccdSweptSphereRadius = value; } } + // Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold + public float CcdSquareMotionThreshold { get { return _ccdSquareMotionThreshold; } set { _ccdSquareMotionThreshold = value; } } + //users can point to their objects, userPointer is not used by Bullet + public object UserData { get { return _userData; } set { _userData = value; } } + public int CompanionID { get { return _companionID; } set { _companionID = value; } } + + public void ForceActivationState(ActivationState newState) + { + _activationState = newState; + } + + public void Activate() + { + Activate(false); + } + + public void Activate(bool forceActivation) + { + if (forceActivation || (_collisionFlags & (CollisionOptions.StaticObject | CollisionOptions.KinematicObject)) == 0) + { + ActivationState = ActivationState.Active; + _deactivationTime = 0; + } + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs index 7a85731..6ba1c87 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs @@ -1,48 +1,48 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public class CollisionPairCallback : IOverlapCallback - { - private DispatcherInfo _dispatchInfo; - private CollisionDispatcher _dispatcher; - - public CollisionPairCallback(DispatcherInfo dispatchInfo, CollisionDispatcher dispatcher) - { - _dispatchInfo = dispatchInfo; - _dispatcher = dispatcher; - } - - #region IOverlapCallback Members - public bool ProcessOverlap(ref BroadphasePair pair) - { - _dispatcher.NearCallback(ref pair, _dispatcher, _dispatchInfo); - return false; - } - #endregion - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public class CollisionPairCallback : IOverlapCallback + { + private DispatcherInfo _dispatchInfo; + private CollisionDispatcher _dispatcher; + + public CollisionPairCallback(DispatcherInfo dispatchInfo, CollisionDispatcher dispatcher) + { + _dispatchInfo = dispatchInfo; + _dispatcher = dispatcher; + } + + #region IOverlapCallback Members + public bool ProcessOverlap(ref BroadphasePair pair) + { + _dispatcher.NearCallback(ref pair, _dispatcher, _dispatchInfo); + return false; + } + #endregion + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs index 968e219..cb520bb 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs @@ -1,358 +1,358 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public class CollisionWorld - { - private List _collisionObjects = new List(); - private IDispatcher _dispatcher; - private OverlappingPairCache _broadphasePairCache; - private bool _ownsDispatcher; - private bool _ownsBroadphasePairCache; - private DispatcherInfo _dispatchInfo = new DispatcherInfo(); - - /// - /// this constructor doesn't own the dispatcher and paircache/broadphase - /// - /// - /// - public CollisionWorld(IDispatcher dispatcher, OverlappingPairCache pairCache) - { - _dispatcher = dispatcher; - _broadphasePairCache = pairCache; - _ownsDispatcher = false; - _ownsBroadphasePairCache = false; - } - - public DispatcherInfo DispatchInfo { get { return _dispatchInfo; } protected set { _dispatchInfo = value; } } - public List CollisionObjects { get { return _collisionObjects; } protected set { _collisionObjects = value; } } - public IBroadphase Broadphase { get { return _broadphasePairCache; } } - public OverlappingPairCache BroadphasePairCache { get { return _broadphasePairCache; } protected set { _broadphasePairCache = value; } } - public IDispatcher Dispatcher { get { return _dispatcher; } protected set { _dispatcher = value; } } - public int CollisionObjectsCount { get { return _collisionObjects.Count; } } - protected bool OwnsDispatcher { get { return _ownsDispatcher; } set { _ownsDispatcher = value; } } - protected bool OwnsBroadphasePairCache { get { return _ownsBroadphasePairCache; } set { _ownsBroadphasePairCache = value; } } - - // rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback - // This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback. - public void RayTest(Vector3 rayFromWorld, Vector3 rayToWorld, RayResultCallback resultCallback) - { - Matrix rayFromTrans, rayToTrans; - - rayFromTrans = Matrix.Identity; - rayFromTrans.Translation = rayFromWorld; - - rayToTrans = Matrix.Identity; - rayToTrans.Translation = rayToWorld; - - // brute force go over all objects. Once there is a broadphase, use that, or - // add a raycast against aabb first. - - foreach (CollisionObject collisionObject in _collisionObjects) - { - //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); - Vector3 collisionObjectAabbMin, collisionObjectAabbMax; - collisionObject.CollisionShape.GetAabb(collisionObject.WorldTransform, out collisionObjectAabbMin, out collisionObjectAabbMax); - - float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing - Vector3 hitNormal = new Vector3(); - - //if (MathHelper.TestAabbAgainstAabb2(rayAabbMin, rayAabbMax, collisionObjectAabbMin, collisionObjectAabbMax)) - if (MathHelper.RayAabb(rayFromWorld, rayToWorld, collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal)) - { - RayTestSingle(rayFromTrans, rayToTrans, - collisionObject, collisionObject.CollisionShape, - collisionObject.WorldTransform, resultCallback); - - } - } - } - - // rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest. - // In a future implementation, we consider moving the ray test as a virtual method in CollisionShape. - // This allows more customization. - public static void RayTestSingle(Matrix rayFromTrans, Matrix rayToTrans, - CollisionObject collisionObject, - CollisionShape collisionShape, - Matrix colObjWorldTransform, - RayResultCallback resultCallback) - { - SphereShape pointShape=new SphereShape(0.0f); - - if (collisionShape.IsConvex) - { - CastResult castResult = new CastResult(); - castResult.Fraction = 1f;//?? - - ConvexShape convexShape = collisionShape as ConvexShape; - VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver(); - SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, convexShape, simplexSolver); - //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); - //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); - - if (convexCaster.CalcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult)) - { - //add hit - if (castResult.Normal.LengthSquared() > 0.0001f) - { - castResult.Normal.Normalize(); - if (castResult.Fraction < resultCallback.ClosestHitFraction) - { - - CollisionWorld.LocalRayResult localRayResult = new LocalRayResult - ( - collisionObject, - new LocalShapeInfo(), - castResult.Normal, - castResult.Fraction - ); - - resultCallback.AddSingleResult(localRayResult); - } - } - } - else - { - if (collisionShape.IsConcave) - { - - TriangleMeshShape triangleMesh = collisionShape as TriangleMeshShape; - - Matrix worldTocollisionObject = MathHelper.InvertMatrix(colObjWorldTransform); - - Vector3 rayFromLocal = Vector3.TransformNormal(rayFromTrans.Translation, worldTocollisionObject); - Vector3 rayToLocal = Vector3.TransformNormal(rayToTrans.Translation, worldTocollisionObject); - - BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh); - rcb.HitFraction = resultCallback.ClosestHitFraction; - - Vector3 rayAabbMinLocal = rayFromLocal; - MathHelper.SetMin(ref rayAabbMinLocal, rayToLocal); - Vector3 rayAabbMaxLocal = rayFromLocal; - MathHelper.SetMax(ref rayAabbMaxLocal, rayToLocal); - - triangleMesh.ProcessAllTriangles(rcb, rayAabbMinLocal, rayAabbMaxLocal); - } - else - { - //todo: use AABB tree or other BVH acceleration structure! - if (collisionShape.IsCompound) - { - CompoundShape compoundShape = collisionShape as CompoundShape; - for (int i = 0; i < compoundShape.ChildShapeCount; i++) - { - Matrix childTrans = compoundShape.GetChildTransform(i); - CollisionShape childCollisionShape = compoundShape.GetChildShape(i); - Matrix childWorldTrans = colObjWorldTransform * childTrans; - RayTestSingle(rayFromTrans, rayToTrans, - collisionObject, - childCollisionShape, - childWorldTrans, - resultCallback); - } - } - } - } - } - } - - public void AddCollisionObject(CollisionObject collisionObject, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask) - { - //check that the object isn't already added - if (!_collisionObjects.Contains(collisionObject)) - { - _collisionObjects.Add(collisionObject); - - //calculate new AABB - Matrix trans = collisionObject.WorldTransform; - - Vector3 minAabb; - Vector3 maxAabb; - collisionObject.CollisionShape.GetAabb(trans, out minAabb, out maxAabb); - - BroadphaseNativeTypes type = collisionObject.CollisionShape.ShapeType; - collisionObject.Broadphase = Broadphase.CreateProxy( - minAabb, - maxAabb, - type, - collisionObject, - collisionFilterGroup, - collisionFilterMask - ); - } - } - - public void AddCollisionObject(CollisionObject collisionObject) - { - AddCollisionObject(collisionObject, BroadphaseProxy.CollisionFilterGroups.Default, BroadphaseProxy.CollisionFilterGroups.Default); - } - - public void RemoveCollisionObject(CollisionObject collisionObject) - { - BroadphaseProxy bp = collisionObject.Broadphase; - if (bp != null) - { - // - // only clear the cached algorithms - // - Broadphase.CleanProxyFromPairs(bp); - Broadphase.DestroyProxy(bp); - collisionObject.Broadphase = null; - } - - _collisionObjects.Remove(collisionObject); - } - - public virtual void PerformDiscreteCollisionDetection() - { - DispatcherInfo dispatchInfo = DispatchInfo; - //update aabb (of all moved objects) - - Vector3 aabbMin, aabbMax; - for (int i = 0; i < _collisionObjects.Count; i++) - { - _collisionObjects[i].CollisionShape.GetAabb(_collisionObjects[i].WorldTransform, out aabbMin, out aabbMax); - _broadphasePairCache.SetAabb(_collisionObjects[i].Broadphase, aabbMin, aabbMax); - } - - _broadphasePairCache.RefreshOverlappingPairs(); - - IDispatcher dispatcher = Dispatcher; - if (dispatcher != null) - dispatcher.DispatchAllCollisionPairs(_broadphasePairCache, dispatchInfo); - } - - public void Dispose(bool disposing) - { - if (disposing) - { - //clean up remaining objects - foreach (CollisionObject collisionObject in _collisionObjects) - { - BroadphaseProxy bp = collisionObject.Broadphase; - if (bp != null) - { - // - // only clear the cached algorithms - // - Broadphase.CleanProxyFromPairs(bp); - Broadphase.DestroyProxy(bp); - } - } - } - } - - /// - /// LocalShapeInfo gives extra information for complex shapes - /// Currently, only TriangleMeshShape is available, so it just contains triangleIndex and subpart - /// - public struct LocalShapeInfo - { - private int _shapePart; - private int _triangleIndex; - - public int ShapePart { get { return _shapePart; } set { _shapePart = value; } } - public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } } - } - - public struct LocalRayResult - { - private CollisionObject _collisionObject; - private LocalShapeInfo _localShapeInfo; - private Vector3 _hitNormalLocal; - private float _hitFraction; - - public LocalRayResult(CollisionObject collisionObject, - LocalShapeInfo localShapeInfo, - Vector3 hitNormalLocal, - float hitFraction) - { - _collisionObject = collisionObject; - _localShapeInfo = localShapeInfo; - _hitNormalLocal = hitNormalLocal; - _hitFraction = hitFraction; - } - - public CollisionObject CollisionObject { get { return _collisionObject; } set { _collisionObject = value; } } - public LocalShapeInfo LocalShapeInfo { get { return _localShapeInfo; } set { _localShapeInfo = value; } } - public Vector3 HitNormalLocal { get { return _hitNormalLocal; } set { _hitNormalLocal = value; } } - public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } - } - - /// - /// RayResultCallback is used to report new raycast results - /// - public abstract class RayResultCallback - { - private float _closestHitFraction; - - public RayResultCallback() - { - _closestHitFraction = 1; - } - - public float ClosestHitFraction { get { return _closestHitFraction; } set { _closestHitFraction = value; } } - public bool HasHit { get { return _closestHitFraction < 1; } } - - public abstract float AddSingleResult(LocalRayResult rayResult); - } - - public class ClosestRayResultCallback : RayResultCallback - { - private Vector3 _rayFromWorld;//used to calculate hitPointWorld from hitFraction - private Vector3 _rayToWorld; - - private Vector3 _hitNormalWorld; - private Vector3 _hitPointWorld; - private CollisionObject _collisionObject; - - public ClosestRayResultCallback(Vector3 rayFromWorld, Vector3 rayToWorld) - { - _rayFromWorld = rayFromWorld; - _rayToWorld = rayToWorld; - _collisionObject = null; - } - - public Vector3 RayFromWorld { get { return _rayFromWorld; } set { _rayFromWorld = value; } } - public Vector3 RayToWorld { get { return _rayToWorld; } set { _rayToWorld = value; } } - public Vector3 HitNormalWorld { get { return _hitNormalWorld; } set { _hitNormalWorld = value; } } - public Vector3 HitPointWorld { get { return _hitPointWorld; } set { _hitPointWorld = value; } } - public CollisionObject CollisionObject { get { return _collisionObject; } set { _collisionObject = value; } } - - public override float AddSingleResult(LocalRayResult rayResult) - { - //caller already does the filter on the m_closestHitFraction - //assert(rayResult.m_hitFraction <= m_closestHitFraction); - ClosestHitFraction = rayResult.HitFraction; - _collisionObject = rayResult.CollisionObject; - _hitNormalWorld = Vector3.TransformNormal(rayResult.HitNormalLocal, _collisionObject.WorldTransform); - MathHelper.SetInterpolate3(_rayFromWorld, _rayToWorld, rayResult.HitFraction, ref _hitPointWorld); - return rayResult.HitFraction; - } - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public class CollisionWorld + { + private List _collisionObjects = new List(); + private IDispatcher _dispatcher; + private OverlappingPairCache _broadphasePairCache; + private bool _ownsDispatcher; + private bool _ownsBroadphasePairCache; + private DispatcherInfo _dispatchInfo = new DispatcherInfo(); + + /// + /// this constructor doesn't own the dispatcher and paircache/broadphase + /// + /// + /// + public CollisionWorld(IDispatcher dispatcher, OverlappingPairCache pairCache) + { + _dispatcher = dispatcher; + _broadphasePairCache = pairCache; + _ownsDispatcher = false; + _ownsBroadphasePairCache = false; + } + + public DispatcherInfo DispatchInfo { get { return _dispatchInfo; } protected set { _dispatchInfo = value; } } + public List CollisionObjects { get { return _collisionObjects; } protected set { _collisionObjects = value; } } + public IBroadphase Broadphase { get { return _broadphasePairCache; } } + public OverlappingPairCache BroadphasePairCache { get { return _broadphasePairCache; } protected set { _broadphasePairCache = value; } } + public IDispatcher Dispatcher { get { return _dispatcher; } protected set { _dispatcher = value; } } + public int CollisionObjectsCount { get { return _collisionObjects.Count; } } + protected bool OwnsDispatcher { get { return _ownsDispatcher; } set { _ownsDispatcher = value; } } + protected bool OwnsBroadphasePairCache { get { return _ownsBroadphasePairCache; } set { _ownsBroadphasePairCache = value; } } + + // rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback + // This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback. + public void RayTest(Vector3 rayFromWorld, Vector3 rayToWorld, RayResultCallback resultCallback) + { + Matrix rayFromTrans, rayToTrans; + + rayFromTrans = Matrix.Identity; + rayFromTrans.Translation = rayFromWorld; + + rayToTrans = Matrix.Identity; + rayToTrans.Translation = rayToWorld; + + // brute force go over all objects. Once there is a broadphase, use that, or + // add a raycast against aabb first. + + foreach (CollisionObject collisionObject in _collisionObjects) + { + //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); + Vector3 collisionObjectAabbMin, collisionObjectAabbMax; + collisionObject.CollisionShape.GetAabb(collisionObject.WorldTransform, out collisionObjectAabbMin, out collisionObjectAabbMax); + + float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing + Vector3 hitNormal = new Vector3(); + + //if (MathHelper.TestAabbAgainstAabb2(rayAabbMin, rayAabbMax, collisionObjectAabbMin, collisionObjectAabbMax)) + if (MathHelper.RayAabb(rayFromWorld, rayToWorld, collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal)) + { + RayTestSingle(rayFromTrans, rayToTrans, + collisionObject, collisionObject.CollisionShape, + collisionObject.WorldTransform, resultCallback); + + } + } + } + + // rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest. + // In a future implementation, we consider moving the ray test as a virtual method in CollisionShape. + // This allows more customization. + public static void RayTestSingle(Matrix rayFromTrans, Matrix rayToTrans, + CollisionObject collisionObject, + CollisionShape collisionShape, + Matrix colObjWorldTransform, + RayResultCallback resultCallback) + { + SphereShape pointShape=new SphereShape(0.0f); + + if (collisionShape.IsConvex) + { + CastResult castResult = new CastResult(); + castResult.Fraction = 1f;//?? + + ConvexShape convexShape = collisionShape as ConvexShape; + VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver(); + SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, convexShape, simplexSolver); + //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); + //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); + + if (convexCaster.CalcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult)) + { + //add hit + if (castResult.Normal.LengthSquared() > 0.0001f) + { + castResult.Normal.Normalize(); + if (castResult.Fraction < resultCallback.ClosestHitFraction) + { + + CollisionWorld.LocalRayResult localRayResult = new LocalRayResult + ( + collisionObject, + new LocalShapeInfo(), + castResult.Normal, + castResult.Fraction + ); + + resultCallback.AddSingleResult(localRayResult); + } + } + } + else + { + if (collisionShape.IsConcave) + { + + TriangleMeshShape triangleMesh = collisionShape as TriangleMeshShape; + + Matrix worldTocollisionObject = MathHelper.InvertMatrix(colObjWorldTransform); + + Vector3 rayFromLocal = Vector3.TransformNormal(rayFromTrans.Translation, worldTocollisionObject); + Vector3 rayToLocal = Vector3.TransformNormal(rayToTrans.Translation, worldTocollisionObject); + + BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh); + rcb.HitFraction = resultCallback.ClosestHitFraction; + + Vector3 rayAabbMinLocal = rayFromLocal; + MathHelper.SetMin(ref rayAabbMinLocal, rayToLocal); + Vector3 rayAabbMaxLocal = rayFromLocal; + MathHelper.SetMax(ref rayAabbMaxLocal, rayToLocal); + + triangleMesh.ProcessAllTriangles(rcb, rayAabbMinLocal, rayAabbMaxLocal); + } + else + { + //todo: use AABB tree or other BVH acceleration structure! + if (collisionShape.IsCompound) + { + CompoundShape compoundShape = collisionShape as CompoundShape; + for (int i = 0; i < compoundShape.ChildShapeCount; i++) + { + Matrix childTrans = compoundShape.GetChildTransform(i); + CollisionShape childCollisionShape = compoundShape.GetChildShape(i); + Matrix childWorldTrans = colObjWorldTransform * childTrans; + RayTestSingle(rayFromTrans, rayToTrans, + collisionObject, + childCollisionShape, + childWorldTrans, + resultCallback); + } + } + } + } + } + } + + public void AddCollisionObject(CollisionObject collisionObject, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask) + { + //check that the object isn't already added + if (!_collisionObjects.Contains(collisionObject)) + { + _collisionObjects.Add(collisionObject); + + //calculate new AABB + Matrix trans = collisionObject.WorldTransform; + + Vector3 minAabb; + Vector3 maxAabb; + collisionObject.CollisionShape.GetAabb(trans, out minAabb, out maxAabb); + + BroadphaseNativeTypes type = collisionObject.CollisionShape.ShapeType; + collisionObject.Broadphase = Broadphase.CreateProxy( + minAabb, + maxAabb, + type, + collisionObject, + collisionFilterGroup, + collisionFilterMask + ); + } + } + + public void AddCollisionObject(CollisionObject collisionObject) + { + AddCollisionObject(collisionObject, BroadphaseProxy.CollisionFilterGroups.Default, BroadphaseProxy.CollisionFilterGroups.Default); + } + + public void RemoveCollisionObject(CollisionObject collisionObject) + { + BroadphaseProxy bp = collisionObject.Broadphase; + if (bp != null) + { + // + // only clear the cached algorithms + // + Broadphase.CleanProxyFromPairs(bp); + Broadphase.DestroyProxy(bp); + collisionObject.Broadphase = null; + } + + _collisionObjects.Remove(collisionObject); + } + + public virtual void PerformDiscreteCollisionDetection() + { + DispatcherInfo dispatchInfo = DispatchInfo; + //update aabb (of all moved objects) + + Vector3 aabbMin, aabbMax; + for (int i = 0; i < _collisionObjects.Count; i++) + { + _collisionObjects[i].CollisionShape.GetAabb(_collisionObjects[i].WorldTransform, out aabbMin, out aabbMax); + _broadphasePairCache.SetAabb(_collisionObjects[i].Broadphase, aabbMin, aabbMax); + } + + _broadphasePairCache.RefreshOverlappingPairs(); + + IDispatcher dispatcher = Dispatcher; + if (dispatcher != null) + dispatcher.DispatchAllCollisionPairs(_broadphasePairCache, dispatchInfo); + } + + public void Dispose(bool disposing) + { + if (disposing) + { + //clean up remaining objects + foreach (CollisionObject collisionObject in _collisionObjects) + { + BroadphaseProxy bp = collisionObject.Broadphase; + if (bp != null) + { + // + // only clear the cached algorithms + // + Broadphase.CleanProxyFromPairs(bp); + Broadphase.DestroyProxy(bp); + } + } + } + } + + /// + /// LocalShapeInfo gives extra information for complex shapes + /// Currently, only TriangleMeshShape is available, so it just contains triangleIndex and subpart + /// + public struct LocalShapeInfo + { + private int _shapePart; + private int _triangleIndex; + + public int ShapePart { get { return _shapePart; } set { _shapePart = value; } } + public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } } + } + + public struct LocalRayResult + { + private CollisionObject _collisionObject; + private LocalShapeInfo _localShapeInfo; + private Vector3 _hitNormalLocal; + private float _hitFraction; + + public LocalRayResult(CollisionObject collisionObject, + LocalShapeInfo localShapeInfo, + Vector3 hitNormalLocal, + float hitFraction) + { + _collisionObject = collisionObject; + _localShapeInfo = localShapeInfo; + _hitNormalLocal = hitNormalLocal; + _hitFraction = hitFraction; + } + + public CollisionObject CollisionObject { get { return _collisionObject; } set { _collisionObject = value; } } + public LocalShapeInfo LocalShapeInfo { get { return _localShapeInfo; } set { _localShapeInfo = value; } } + public Vector3 HitNormalLocal { get { return _hitNormalLocal; } set { _hitNormalLocal = value; } } + public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } + } + + /// + /// RayResultCallback is used to report new raycast results + /// + public abstract class RayResultCallback + { + private float _closestHitFraction; + + public RayResultCallback() + { + _closestHitFraction = 1; + } + + public float ClosestHitFraction { get { return _closestHitFraction; } set { _closestHitFraction = value; } } + public bool HasHit { get { return _closestHitFraction < 1; } } + + public abstract float AddSingleResult(LocalRayResult rayResult); + } + + public class ClosestRayResultCallback : RayResultCallback + { + private Vector3 _rayFromWorld;//used to calculate hitPointWorld from hitFraction + private Vector3 _rayToWorld; + + private Vector3 _hitNormalWorld; + private Vector3 _hitPointWorld; + private CollisionObject _collisionObject; + + public ClosestRayResultCallback(Vector3 rayFromWorld, Vector3 rayToWorld) + { + _rayFromWorld = rayFromWorld; + _rayToWorld = rayToWorld; + _collisionObject = null; + } + + public Vector3 RayFromWorld { get { return _rayFromWorld; } set { _rayFromWorld = value; } } + public Vector3 RayToWorld { get { return _rayToWorld; } set { _rayToWorld = value; } } + public Vector3 HitNormalWorld { get { return _hitNormalWorld; } set { _hitNormalWorld = value; } } + public Vector3 HitPointWorld { get { return _hitPointWorld; } set { _hitPointWorld = value; } } + public CollisionObject CollisionObject { get { return _collisionObject; } set { _collisionObject = value; } } + + public override float AddSingleResult(LocalRayResult rayResult) + { + //caller already does the filter on the m_closestHitFraction + //assert(rayResult.m_hitFraction <= m_closestHitFraction); + ClosestHitFraction = rayResult.HitFraction; + _collisionObject = rayResult.CollisionObject; + _hitNormalWorld = Vector3.TransformNormal(rayResult.HitNormalLocal, _collisionObject.WorldTransform); + MathHelper.SetInterpolate3(_rayFromWorld, _rayToWorld, rayResult.HitFraction, ref _hitPointWorld); + return rayResult.HitFraction; + } + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs index 3fec7d0..6f2278a 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs @@ -1,157 +1,157 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using System.Diagnostics; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public class CompoundCollisionAlgorithm : CollisionAlgorithm - { - private List _childCollisionAlgorithms; - private bool _isSwapped; - - public CompoundCollisionAlgorithm( - CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, - CollisionObject bodyA, - CollisionObject bodyB, bool isSwapped) - : base(collisionAlgorithmConstructionInfo) - { - //Begin - _isSwapped = isSwapped; - - CollisionObject collisionObject = isSwapped ? bodyB : bodyA; - CollisionObject otherObject = isSwapped ? bodyA : bodyB; - - BulletDebug.Assert(collisionObject.CollisionShape.IsCompound); - - CompoundShape compoundShape = collisionObject.CollisionShape as CompoundShape; - int childrenNumber = compoundShape.ChildShapeCount; - int index = 0; - - _childCollisionAlgorithms = new List(childrenNumber); - - for (index = 0; index < childrenNumber; index++) - { - CollisionShape childShape = compoundShape.GetChildShape(index); - CollisionShape orgShape = collisionObject.CollisionShape; - - collisionObject.CollisionShape = childShape; - _childCollisionAlgorithms[index] = collisionAlgorithmConstructionInfo.Dispatcher.FindAlgorithm(collisionObject, otherObject); - collisionObject.CollisionShape = orgShape; - } - } - - public override void ProcessCollision( - CollisionObject bodyA, - CollisionObject bodyB, - DispatcherInfo dispatchInfo, ManifoldResult resultOut) - { - //Begin - - CollisionObject collisionObject = _isSwapped ? bodyB : bodyB; - CollisionObject otherObject = _isSwapped ? bodyA : bodyB; - - //Debug.Assert(collisionObject.getCollisionShape().isCompound()); - BulletDebug.Assert(collisionObject.CollisionShape.IsCompound); - - CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape; - - int childrenNumber = _childCollisionAlgorithms.Count; - - for (int i = 0; i < childrenNumber; i++) - { - CompoundShape childShape = compoundShape.GetChildShape(i) as CompoundShape; - - Matrix orgTransform = collisionObject.WorldTransform; - CollisionShape orgShape = collisionObject.CollisionShape; - - Matrix childTransform = compoundShape.GetChildTransform(i); - Matrix newChildWorld = orgTransform * childTransform; - - collisionObject.WorldTransform = newChildWorld; - collisionObject.CollisionShape = childShape; - _childCollisionAlgorithms[i].ProcessCollision(collisionObject, otherObject, dispatchInfo, resultOut); - - collisionObject.CollisionShape = orgShape; - collisionObject.WorldTransform = orgTransform; - } - } - - public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) - { - CollisionObject collisionObject = _isSwapped ? bodyB : bodyA; - CollisionObject otherObject = _isSwapped ? bodyA : bodyB; - - BulletDebug.Assert(collisionObject.CollisionShape.IsCompound); - - CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape; - - float hitFraction = 1.0f; - - for (int i = 0; i < _childCollisionAlgorithms.Count; i++) - { - CollisionShape childShape = compoundShape.GetChildShape(i); - - Matrix orgTransform = collisionObject.WorldTransform; - CollisionShape orgShape = collisionObject.CollisionShape; - - Matrix childTransform = compoundShape.GetChildTransform(i); - Matrix newChildWorld = orgTransform * childTransform; - collisionObject.WorldTransform = newChildWorld; - - collisionObject.CollisionShape = childShape; - float frac = _childCollisionAlgorithms[i].CalculateTimeOfImpact( - collisionObject, otherObject, dispatchInfo, resultOut - ); - - if (frac < hitFraction) - { - hitFraction = frac; - } - - collisionObject.CollisionShape = orgShape; - collisionObject.WorldTransform = orgTransform; - } - - return hitFraction; - } - - public class CreateFunc : CollisionAlgorithmCreateFunction - { - public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) - { - return new CompoundCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, false); - } - }; - - public class SwappedCreateFunc : CollisionAlgorithmCreateFunction - { - public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) - { - return new CompoundCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, true); - } - }; - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using System.Diagnostics; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public class CompoundCollisionAlgorithm : CollisionAlgorithm + { + private List _childCollisionAlgorithms; + private bool _isSwapped; + + public CompoundCollisionAlgorithm( + CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, + CollisionObject bodyA, + CollisionObject bodyB, bool isSwapped) + : base(collisionAlgorithmConstructionInfo) + { + //Begin + _isSwapped = isSwapped; + + CollisionObject collisionObject = isSwapped ? bodyB : bodyA; + CollisionObject otherObject = isSwapped ? bodyA : bodyB; + + BulletDebug.Assert(collisionObject.CollisionShape.IsCompound); + + CompoundShape compoundShape = collisionObject.CollisionShape as CompoundShape; + int childrenNumber = compoundShape.ChildShapeCount; + int index = 0; + + _childCollisionAlgorithms = new List(childrenNumber); + + for (index = 0; index < childrenNumber; index++) + { + CollisionShape childShape = compoundShape.GetChildShape(index); + CollisionShape orgShape = collisionObject.CollisionShape; + + collisionObject.CollisionShape = childShape; + _childCollisionAlgorithms[index] = collisionAlgorithmConstructionInfo.Dispatcher.FindAlgorithm(collisionObject, otherObject); + collisionObject.CollisionShape = orgShape; + } + } + + public override void ProcessCollision( + CollisionObject bodyA, + CollisionObject bodyB, + DispatcherInfo dispatchInfo, ManifoldResult resultOut) + { + //Begin + + CollisionObject collisionObject = _isSwapped ? bodyB : bodyB; + CollisionObject otherObject = _isSwapped ? bodyA : bodyB; + + //Debug.Assert(collisionObject.getCollisionShape().isCompound()); + BulletDebug.Assert(collisionObject.CollisionShape.IsCompound); + + CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape; + + int childrenNumber = _childCollisionAlgorithms.Count; + + for (int i = 0; i < childrenNumber; i++) + { + CompoundShape childShape = compoundShape.GetChildShape(i) as CompoundShape; + + Matrix orgTransform = collisionObject.WorldTransform; + CollisionShape orgShape = collisionObject.CollisionShape; + + Matrix childTransform = compoundShape.GetChildTransform(i); + Matrix newChildWorld = orgTransform * childTransform; + + collisionObject.WorldTransform = newChildWorld; + collisionObject.CollisionShape = childShape; + _childCollisionAlgorithms[i].ProcessCollision(collisionObject, otherObject, dispatchInfo, resultOut); + + collisionObject.CollisionShape = orgShape; + collisionObject.WorldTransform = orgTransform; + } + } + + public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) + { + CollisionObject collisionObject = _isSwapped ? bodyB : bodyA; + CollisionObject otherObject = _isSwapped ? bodyA : bodyB; + + BulletDebug.Assert(collisionObject.CollisionShape.IsCompound); + + CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape; + + float hitFraction = 1.0f; + + for (int i = 0; i < _childCollisionAlgorithms.Count; i++) + { + CollisionShape childShape = compoundShape.GetChildShape(i); + + Matrix orgTransform = collisionObject.WorldTransform; + CollisionShape orgShape = collisionObject.CollisionShape; + + Matrix childTransform = compoundShape.GetChildTransform(i); + Matrix newChildWorld = orgTransform * childTransform; + collisionObject.WorldTransform = newChildWorld; + + collisionObject.CollisionShape = childShape; + float frac = _childCollisionAlgorithms[i].CalculateTimeOfImpact( + collisionObject, otherObject, dispatchInfo, resultOut + ); + + if (frac < hitFraction) + { + hitFraction = frac; + } + + collisionObject.CollisionShape = orgShape; + collisionObject.WorldTransform = orgTransform; + } + + return hitFraction; + } + + public class CreateFunc : CollisionAlgorithmCreateFunction + { + public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) + { + return new CompoundCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, false); + } + }; + + public class SwappedCreateFunc : CollisionAlgorithmCreateFunction + { + public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) + { + return new CompoundCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, true); + } + }; + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs index c4949f0..39438e4 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs @@ -1,189 +1,189 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public class ConvexConcaveCollisionAlgorithm : CollisionAlgorithm - { - private bool _isSwapped; - private ConvexTriangleCallback _convexTriangleCallback; - - public ConvexConcaveCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped) - : base(collisionAlgorithmConstructionInfo) - { - _isSwapped = isSwapped; - _convexTriangleCallback = new ConvexTriangleCallback(collisionAlgorithmConstructionInfo.Dispatcher, bodyA, bodyB, isSwapped); - } - - public void ClearCache() - { - _convexTriangleCallback.ClearCache(); - } - - public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) - { - CollisionObject convexBody = _isSwapped ? bodyB : bodyA; - CollisionObject triBody = _isSwapped ? bodyA : bodyB; - - if (triBody.CollisionShape.IsConcave) - { - CollisionObject triOb = triBody; - ConcaveShape concaveShape = triOb.CollisionShape as ConcaveShape; - - if (convexBody.CollisionShape.IsConvex) - { - float collisionMarginTriangle = concaveShape.Margin; - - resultOut.SetPersistentManifold(_convexTriangleCallback.Manifold); - _convexTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut); - - //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here. - //m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr); - - _convexTriangleCallback.Manifold.SetBodies(convexBody, triBody); - concaveShape.ProcessAllTriangles(_convexTriangleCallback, _convexTriangleCallback.AabbMin, _convexTriangleCallback.AabbMax); - } - } - } - - public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) - { - CollisionObject convexbody = _isSwapped ? bodyB : bodyA; - CollisionObject triBody = _isSwapped ? bodyA : bodyB; - - - //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast) - - //only perform CCD above a certain threshold, this prevents blocking on the long run - //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame... - float squareMot0 = (convexbody.InterpolationWorldTransform.Translation - convexbody.WorldTransform.Translation).LengthSquared(); - if (squareMot0 < convexbody.CcdSquareMotionThreshold) - { - return 1; - } - - Matrix triInv = MathHelper.InvertMatrix(triBody.WorldTransform); - Matrix convexFromLocal = triInv * convexbody.WorldTransform; - Matrix convexToLocal = triInv * convexbody.InterpolationWorldTransform; - - if (triBody.CollisionShape.IsConcave) - { - Vector3 rayAabbMin = convexFromLocal.Translation; - MathHelper.SetMin(ref rayAabbMin, convexToLocal.Translation); - Vector3 rayAabbMax = convexFromLocal.Translation; - MathHelper.SetMax(ref rayAabbMax, convexToLocal.Translation); - float ccdRadius0 = convexbody.CcdSweptSphereRadius; - rayAabbMin -= new Vector3(ccdRadius0, ccdRadius0, ccdRadius0); - rayAabbMax += new Vector3(ccdRadius0, ccdRadius0, ccdRadius0); - - float curHitFraction = 1f; //is this available? - LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(convexFromLocal, convexToLocal, - convexbody.CcdSweptSphereRadius, curHitFraction); - - raycastCallback.HitFraction = convexbody.HitFraction; - - CollisionObject concavebody = triBody; - - ConcaveShape triangleMesh = concavebody.CollisionShape as ConcaveShape; - - if (triangleMesh != null) - { - triangleMesh.ProcessAllTriangles(raycastCallback, rayAabbMin, rayAabbMax); - } - - if (raycastCallback.HitFraction < convexbody.HitFraction) - { - convexbody.HitFraction = raycastCallback.HitFraction; - return raycastCallback.HitFraction; - } - } - - return 1; - } - - public class CreateFunc : CollisionAlgorithmCreateFunction - { - public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) - { - return new ConvexConcaveCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, false); - } - } - - public class SwappedCreateFunc : CollisionAlgorithmCreateFunction - { - public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) - { - return new ConvexConcaveCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, true); - } - } - - private class LocalTriangleSphereCastCallback : ITriangleCallback - { - private Matrix _ccdSphereFromTrans; - private Matrix _ccdSphereToTrans; - private Matrix _meshTransform; - - private float _ccdSphereRadius; - private float _hitFraction; - - public LocalTriangleSphereCastCallback(Matrix from, Matrix to, float ccdSphereRadius, float hitFraction) - { - _ccdSphereFromTrans = from; - _ccdSphereToTrans = to; - _ccdSphereRadius = ccdSphereRadius; - _hitFraction = hitFraction; - } - - public Matrix CcdSphereFromTrans { get { return _ccdSphereFromTrans; } set { _ccdSphereFromTrans = value; } } - public Matrix CcdSphereToTrans { get { return _ccdSphereToTrans; } set { _ccdSphereToTrans = value; } } - public Matrix MeshTransform { get { return _meshTransform; } set { _meshTransform = value; } } - public float CcdSphereRadius { get { return _ccdSphereRadius; } set { _ccdSphereRadius = value; } } - public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } - - public void ProcessTriangle(Vector3[] triangle, int partId, int triangleIndex) - { - //do a swept sphere for now - Matrix ident = Matrix.Identity; - CastResult castResult = new CastResult(); - castResult.Fraction = _hitFraction; - SphereShape pointShape = new SphereShape(_ccdSphereRadius); - TriangleShape triShape = new TriangleShape(triangle[0], triangle[1], triangle[2]); - VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver(); - SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, triShape, simplexSolver); - //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); - //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); - //local space? - - if (convexCaster.CalcTimeOfImpact(_ccdSphereFromTrans, _ccdSphereToTrans, - ident, ident, castResult)) - { - if (_hitFraction > castResult.Fraction) - _hitFraction = castResult.Fraction; - } - } - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public class ConvexConcaveCollisionAlgorithm : CollisionAlgorithm + { + private bool _isSwapped; + private ConvexTriangleCallback _convexTriangleCallback; + + public ConvexConcaveCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped) + : base(collisionAlgorithmConstructionInfo) + { + _isSwapped = isSwapped; + _convexTriangleCallback = new ConvexTriangleCallback(collisionAlgorithmConstructionInfo.Dispatcher, bodyA, bodyB, isSwapped); + } + + public void ClearCache() + { + _convexTriangleCallback.ClearCache(); + } + + public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) + { + CollisionObject convexBody = _isSwapped ? bodyB : bodyA; + CollisionObject triBody = _isSwapped ? bodyA : bodyB; + + if (triBody.CollisionShape.IsConcave) + { + CollisionObject triOb = triBody; + ConcaveShape concaveShape = triOb.CollisionShape as ConcaveShape; + + if (convexBody.CollisionShape.IsConvex) + { + float collisionMarginTriangle = concaveShape.Margin; + + resultOut.SetPersistentManifold(_convexTriangleCallback.Manifold); + _convexTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut); + + //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here. + //m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr); + + _convexTriangleCallback.Manifold.SetBodies(convexBody, triBody); + concaveShape.ProcessAllTriangles(_convexTriangleCallback, _convexTriangleCallback.AabbMin, _convexTriangleCallback.AabbMax); + } + } + } + + public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) + { + CollisionObject convexbody = _isSwapped ? bodyB : bodyA; + CollisionObject triBody = _isSwapped ? bodyA : bodyB; + + + //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast) + + //only perform CCD above a certain threshold, this prevents blocking on the long run + //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame... + float squareMot0 = (convexbody.InterpolationWorldTransform.Translation - convexbody.WorldTransform.Translation).LengthSquared(); + if (squareMot0 < convexbody.CcdSquareMotionThreshold) + { + return 1; + } + + Matrix triInv = MathHelper.InvertMatrix(triBody.WorldTransform); + Matrix convexFromLocal = triInv * convexbody.WorldTransform; + Matrix convexToLocal = triInv * convexbody.InterpolationWorldTransform; + + if (triBody.CollisionShape.IsConcave) + { + Vector3 rayAabbMin = convexFromLocal.Translation; + MathHelper.SetMin(ref rayAabbMin, convexToLocal.Translation); + Vector3 rayAabbMax = convexFromLocal.Translation; + MathHelper.SetMax(ref rayAabbMax, convexToLocal.Translation); + float ccdRadius0 = convexbody.CcdSweptSphereRadius; + rayAabbMin -= new Vector3(ccdRadius0, ccdRadius0, ccdRadius0); + rayAabbMax += new Vector3(ccdRadius0, ccdRadius0, ccdRadius0); + + float curHitFraction = 1f; //is this available? + LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(convexFromLocal, convexToLocal, + convexbody.CcdSweptSphereRadius, curHitFraction); + + raycastCallback.HitFraction = convexbody.HitFraction; + + CollisionObject concavebody = triBody; + + ConcaveShape triangleMesh = concavebody.CollisionShape as ConcaveShape; + + if (triangleMesh != null) + { + triangleMesh.ProcessAllTriangles(raycastCallback, rayAabbMin, rayAabbMax); + } + + if (raycastCallback.HitFraction < convexbody.HitFraction) + { + convexbody.HitFraction = raycastCallback.HitFraction; + return raycastCallback.HitFraction; + } + } + + return 1; + } + + public class CreateFunc : CollisionAlgorithmCreateFunction + { + public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) + { + return new ConvexConcaveCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, false); + } + } + + public class SwappedCreateFunc : CollisionAlgorithmCreateFunction + { + public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) + { + return new ConvexConcaveCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, true); + } + } + + private class LocalTriangleSphereCastCallback : ITriangleCallback + { + private Matrix _ccdSphereFromTrans; + private Matrix _ccdSphereToTrans; + private Matrix _meshTransform; + + private float _ccdSphereRadius; + private float _hitFraction; + + public LocalTriangleSphereCastCallback(Matrix from, Matrix to, float ccdSphereRadius, float hitFraction) + { + _ccdSphereFromTrans = from; + _ccdSphereToTrans = to; + _ccdSphereRadius = ccdSphereRadius; + _hitFraction = hitFraction; + } + + public Matrix CcdSphereFromTrans { get { return _ccdSphereFromTrans; } set { _ccdSphereFromTrans = value; } } + public Matrix CcdSphereToTrans { get { return _ccdSphereToTrans; } set { _ccdSphereToTrans = value; } } + public Matrix MeshTransform { get { return _meshTransform; } set { _meshTransform = value; } } + public float CcdSphereRadius { get { return _ccdSphereRadius; } set { _ccdSphereRadius = value; } } + public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } + + public void ProcessTriangle(Vector3[] triangle, int partId, int triangleIndex) + { + //do a swept sphere for now + Matrix ident = Matrix.Identity; + CastResult castResult = new CastResult(); + castResult.Fraction = _hitFraction; + SphereShape pointShape = new SphereShape(_ccdSphereRadius); + TriangleShape triShape = new TriangleShape(triangle[0], triangle[1], triangle[2]); + VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver(); + SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, triShape, simplexSolver); + //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); + //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); + //local space? + + if (convexCaster.CalcTimeOfImpact(_ccdSphereFromTrans, _ccdSphereToTrans, + ident, ident, castResult)) + { + if (_hitFraction > castResult.Fraction) + _hitFraction = castResult.Fraction; + } + } + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs index 8fa4837..01c8fe0 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs @@ -1,193 +1,193 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; - -namespace XnaDevRu.BulletX -{ - public class ConvexConvexAlgorithm : CollisionAlgorithm, IDisposable - { - private const bool DisableCcd = false; - private GjkPairDetector _gjkPairDetector; - private bool _ownManifold; - private PersistentManifold _manifold; - private bool _lowLevelOfDetail; - - public ConvexConvexAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver) - : base(collisionAlgorithmConstructionInfo) - { - _gjkPairDetector = new GjkPairDetector(null, null, simplexSolver, penetrationDepthSolver); - _ownManifold = false; - _manifold = manifold; - _lowLevelOfDetail = false; - } - - public bool LowLevelOfDetail { get { return _lowLevelOfDetail; } set { _lowLevelOfDetail = value; } } - public bool OwnManifold { get { return _ownManifold; } set { _ownManifold = value; } } - public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } } - - public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) - { - if (_manifold == null) - { - //swapped? - _manifold = Dispatcher.GetNewManifold(bodyA, bodyB); - _ownManifold = true; - } - resultOut.SetPersistentManifold(_manifold); - - ConvexShape min0 = bodyA.CollisionShape as ConvexShape; - ConvexShape min1 = bodyB.CollisionShape as ConvexShape; - - GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); - - //TODO: if (dispatchInfo.m_useContinuous) - _gjkPairDetector.setMinkowskiA(min0); - _gjkPairDetector.setMinkowskiB(min1); - input.MaximumDistanceSquared = min0.Margin + min1.Margin + PersistentManifold.ContactBreakingThreshold; - input.MaximumDistanceSquared *= input.MaximumDistanceSquared; - - // input.m_maximumDistanceSquared = 1e30f; - - input.TransformA = bodyA.WorldTransform; - input.TransformB = bodyB.WorldTransform; - - _gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.DebugDraw); - } - - public override float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) - { - //Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold - - //Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold - //col0->m_worldTransform, - float resultFraction = 1f; - - float squareMotA = (colA.InterpolationWorldTransform.Translation - colA.WorldTransform.Translation).LengthSquared(); - float squareMotB = (colB.InterpolationWorldTransform.Translation - colB.WorldTransform.Translation).LengthSquared(); - - if (squareMotA < colA.CcdSquareMotionThreshold && - squareMotB < colB.CcdSquareMotionThreshold) - return resultFraction; - - if (DisableCcd) - return 1f; - - //An adhoc way of testing the Continuous Collision Detection algorithms - //One object is approximated as a sphere, to simplify things - //Starting in penetration should report no time of impact - //For proper CCD, better accuracy and handling of 'allowed' penetration should be added - //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies) - - // Convex0 against sphere for Convex1 - { - ConvexShape convexA = colA.CollisionShape as ConvexShape; - - SphereShape sphereB = new SphereShape(colB.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation - CastResult result = new CastResult(); - VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver(); - //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); - //Simplification, one object is simplified as a sphere - GjkConvexCast ccdB = new GjkConvexCast(convexA, sphereB, voronoiSimplex); - //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); - if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform, - colB.WorldTransform, colB.InterpolationWorldTransform, result)) - { - //store result.m_fraction in both bodies - if (colA.HitFraction > result.Fraction) - colA.HitFraction = result.Fraction; - - if (colB.HitFraction > result.Fraction) - colB.HitFraction = result.Fraction; - - if (resultFraction > result.Fraction) - resultFraction = result.Fraction; - } - } - - // Sphere (for convex0) against Convex1 - { - ConvexShape convexB = colB.CollisionShape as ConvexShape; - - SphereShape sphereA = new SphereShape(colA.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation - CastResult result = new CastResult(); - VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver(); - //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); - ///Simplification, one object is simplified as a sphere - GjkConvexCast ccdB = new GjkConvexCast(sphereA, convexB, voronoiSimplex); - //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); - if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform, - colB.WorldTransform, colB.InterpolationWorldTransform, result)) - { - //store result.m_fraction in both bodies - if (colA.HitFraction > result.Fraction) - colA.HitFraction = result.Fraction; - - if (colB.HitFraction > result.Fraction) - colB.HitFraction = result.Fraction; - - if (resultFraction > result.Fraction) - resultFraction = result.Fraction; - } - } - return resultFraction; - } - - public class CreateFunc : CollisionAlgorithmCreateFunction - { - private IConvexPenetrationDepthSolver _penetrationDepthSolver; - private ISimplexSolver _simplexSolver; - //private bool _ownsSolvers; - - public CreateFunc() - { - //_ownsSolvers = true; - _simplexSolver = new VoronoiSimplexSolver(); - _penetrationDepthSolver = new GjkEpaPenetrationDepthSolver(); - } - - public CreateFunc(ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver) - { - //_ownsSolvers = false; - _simplexSolver = simplexSolver; - _penetrationDepthSolver = penetrationDepthSolver; - } - - public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) - { - return new ConvexConvexAlgorithm(collisionAlgorithmConstructionInfo.Manifold, collisionAlgorithmConstructionInfo, bodyA, bodyB, _simplexSolver, _penetrationDepthSolver); - } - } - - #region IDisposable Members - public void Dispose() - { - if (_ownManifold) - { - if (_manifold != null) - Dispatcher.ReleaseManifold(_manifold); - } - } - #endregion - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace XnaDevRu.BulletX +{ + public class ConvexConvexAlgorithm : CollisionAlgorithm, IDisposable + { + private const bool DisableCcd = false; + private GjkPairDetector _gjkPairDetector; + private bool _ownManifold; + private PersistentManifold _manifold; + private bool _lowLevelOfDetail; + + public ConvexConvexAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver) + : base(collisionAlgorithmConstructionInfo) + { + _gjkPairDetector = new GjkPairDetector(null, null, simplexSolver, penetrationDepthSolver); + _ownManifold = false; + _manifold = manifold; + _lowLevelOfDetail = false; + } + + public bool LowLevelOfDetail { get { return _lowLevelOfDetail; } set { _lowLevelOfDetail = value; } } + public bool OwnManifold { get { return _ownManifold; } set { _ownManifold = value; } } + public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } } + + public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) + { + if (_manifold == null) + { + //swapped? + _manifold = Dispatcher.GetNewManifold(bodyA, bodyB); + _ownManifold = true; + } + resultOut.SetPersistentManifold(_manifold); + + ConvexShape min0 = bodyA.CollisionShape as ConvexShape; + ConvexShape min1 = bodyB.CollisionShape as ConvexShape; + + GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); + + //TODO: if (dispatchInfo.m_useContinuous) + _gjkPairDetector.setMinkowskiA(min0); + _gjkPairDetector.setMinkowskiB(min1); + input.MaximumDistanceSquared = min0.Margin + min1.Margin + PersistentManifold.ContactBreakingThreshold; + input.MaximumDistanceSquared *= input.MaximumDistanceSquared; + + // input.m_maximumDistanceSquared = 1e30f; + + input.TransformA = bodyA.WorldTransform; + input.TransformB = bodyB.WorldTransform; + + _gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.DebugDraw); + } + + public override float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) + { + //Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold + + //Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold + //col0->m_worldTransform, + float resultFraction = 1f; + + float squareMotA = (colA.InterpolationWorldTransform.Translation - colA.WorldTransform.Translation).LengthSquared(); + float squareMotB = (colB.InterpolationWorldTransform.Translation - colB.WorldTransform.Translation).LengthSquared(); + + if (squareMotA < colA.CcdSquareMotionThreshold && + squareMotB < colB.CcdSquareMotionThreshold) + return resultFraction; + + if (DisableCcd) + return 1f; + + //An adhoc way of testing the Continuous Collision Detection algorithms + //One object is approximated as a sphere, to simplify things + //Starting in penetration should report no time of impact + //For proper CCD, better accuracy and handling of 'allowed' penetration should be added + //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies) + + // Convex0 against sphere for Convex1 + { + ConvexShape convexA = colA.CollisionShape as ConvexShape; + + SphereShape sphereB = new SphereShape(colB.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation + CastResult result = new CastResult(); + VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver(); + //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); + //Simplification, one object is simplified as a sphere + GjkConvexCast ccdB = new GjkConvexCast(convexA, sphereB, voronoiSimplex); + //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); + if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform, + colB.WorldTransform, colB.InterpolationWorldTransform, result)) + { + //store result.m_fraction in both bodies + if (colA.HitFraction > result.Fraction) + colA.HitFraction = result.Fraction; + + if (colB.HitFraction > result.Fraction) + colB.HitFraction = result.Fraction; + + if (resultFraction > result.Fraction) + resultFraction = result.Fraction; + } + } + + // Sphere (for convex0) against Convex1 + { + ConvexShape convexB = colB.CollisionShape as ConvexShape; + + SphereShape sphereA = new SphereShape(colA.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation + CastResult result = new CastResult(); + VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver(); + //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); + ///Simplification, one object is simplified as a sphere + GjkConvexCast ccdB = new GjkConvexCast(sphereA, convexB, voronoiSimplex); + //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); + if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform, + colB.WorldTransform, colB.InterpolationWorldTransform, result)) + { + //store result.m_fraction in both bodies + if (colA.HitFraction > result.Fraction) + colA.HitFraction = result.Fraction; + + if (colB.HitFraction > result.Fraction) + colB.HitFraction = result.Fraction; + + if (resultFraction > result.Fraction) + resultFraction = result.Fraction; + } + } + return resultFraction; + } + + public class CreateFunc : CollisionAlgorithmCreateFunction + { + private IConvexPenetrationDepthSolver _penetrationDepthSolver; + private ISimplexSolver _simplexSolver; + //private bool _ownsSolvers; + + public CreateFunc() + { + //_ownsSolvers = true; + _simplexSolver = new VoronoiSimplexSolver(); + _penetrationDepthSolver = new GjkEpaPenetrationDepthSolver(); + } + + public CreateFunc(ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver) + { + //_ownsSolvers = false; + _simplexSolver = simplexSolver; + _penetrationDepthSolver = penetrationDepthSolver; + } + + public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) + { + return new ConvexConvexAlgorithm(collisionAlgorithmConstructionInfo.Manifold, collisionAlgorithmConstructionInfo, bodyA, bodyB, _simplexSolver, _penetrationDepthSolver); + } + } + + #region IDisposable Members + public void Dispose() + { + if (_ownManifold) + { + if (_manifold != null) + Dispatcher.ReleaseManifold(_manifold); + } + } + #endregion + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs index 5355817..ba21c72 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs @@ -1,130 +1,130 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public class ConvexTriangleCallback : ITriangleCallback, IDisposable - { - private CollisionObject _convexBody; - private CollisionObject _triBody; - - private Vector3 _aabbMin; - private Vector3 _aabbMax; - - private ManifoldResult _resultOut; - - private IDispatcher _dispatcher; - private DispatcherInfo _dispatchInfo; - private float _collisionMarginTriangle; - - private int _triangleCount; - - private PersistentManifold _manifold; - - public ConvexTriangleCallback(IDispatcher dispatcher, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped) - { - _dispatcher = dispatcher; - _dispatchInfo = null; - _convexBody = isSwapped ? bodyB : bodyA; - _triBody = isSwapped ? bodyA : bodyB; - - // create the manifold from the dispatcher 'manifold pool' - _manifold = _dispatcher.GetNewManifold(_convexBody, _triBody); - ClearCache(); - } - - public Vector3 AabbMin { get { return _aabbMin; } } - public Vector3 AabbMax { get { return _aabbMax; } } - public int TriangleCount { get { return _triangleCount; } set { _triangleCount = value; } } - public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } } - - public void SetTimeStepAndCounters(float collisionMarginTriangle, DispatcherInfo dispatchInfo, ManifoldResult resultOut) - { - _dispatchInfo = dispatchInfo; - _collisionMarginTriangle = collisionMarginTriangle; - _resultOut = resultOut; - - //recalc aabbs - Matrix convexInTriangleSpace = MathHelper.InvertMatrix(_triBody.WorldTransform) * _convexBody.WorldTransform; - CollisionShape convexShape = _convexBody.CollisionShape; - //CollisionShape* triangleShape = static_cast(triBody->m_collisionShape); - convexShape.GetAabb(convexInTriangleSpace, out _aabbMin, out _aabbMax); - float extraMargin = collisionMarginTriangle; - Vector3 extra = new Vector3(extraMargin, extraMargin, extraMargin); - - _aabbMax += extra; - _aabbMin -= extra; - } - - public void ClearCache() - { - _dispatcher.ClearManifold(_manifold); - } - - #region ITriangleCallback Members - public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) - { - //aabb filter is already applied! - CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo = new CollisionAlgorithmConstructionInfo(); - collisionAlgorithmConstructionInfo.Dispatcher = _dispatcher; - - CollisionObject collisionObject = _triBody; - - //debug drawing of the overlapping triangles - /*if (m_dispatchInfoPtr && m_dispatchInfoPtr.m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0) - { - Vector3 color = new Vector3(255, 255, 0); - btTransform & tr = ob->WorldTransform; - m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]), tr(triangle[1]), color); - m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]), tr(triangle[2]), color); - m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]), tr(triangle[0]), color); - }*/ - - if (_convexBody.CollisionShape.IsConvex) - { - TriangleShape triangleShape = new TriangleShape(triangle[0], triangle[1], triangle[2]); - triangleShape.Margin=_collisionMarginTriangle; - - CollisionShape tempShape = collisionObject.CollisionShape; - collisionObject.CollisionShape = triangleShape; - - CollisionAlgorithm collisionAlgorithm = collisionAlgorithmConstructionInfo.Dispatcher.FindAlgorithm(_convexBody, _triBody, _manifold); - - _resultOut.SetShapeIdentifiers(-1, -1, partID, triangleIndex); - collisionAlgorithm.ProcessCollision(_convexBody, _triBody, _dispatchInfo, _resultOut); - collisionObject.CollisionShape = tempShape; - } - } - #endregion - #region IDisposable Members - public void Dispose() - { - ClearCache(); - _dispatcher.ReleaseManifold(_manifold); - } - #endregion - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public class ConvexTriangleCallback : ITriangleCallback, IDisposable + { + private CollisionObject _convexBody; + private CollisionObject _triBody; + + private Vector3 _aabbMin; + private Vector3 _aabbMax; + + private ManifoldResult _resultOut; + + private IDispatcher _dispatcher; + private DispatcherInfo _dispatchInfo; + private float _collisionMarginTriangle; + + private int _triangleCount; + + private PersistentManifold _manifold; + + public ConvexTriangleCallback(IDispatcher dispatcher, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped) + { + _dispatcher = dispatcher; + _dispatchInfo = null; + _convexBody = isSwapped ? bodyB : bodyA; + _triBody = isSwapped ? bodyA : bodyB; + + // create the manifold from the dispatcher 'manifold pool' + _manifold = _dispatcher.GetNewManifold(_convexBody, _triBody); + ClearCache(); + } + + public Vector3 AabbMin { get { return _aabbMin; } } + public Vector3 AabbMax { get { return _aabbMax; } } + public int TriangleCount { get { return _triangleCount; } set { _triangleCount = value; } } + public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } } + + public void SetTimeStepAndCounters(float collisionMarginTriangle, DispatcherInfo dispatchInfo, ManifoldResult resultOut) + { + _dispatchInfo = dispatchInfo; + _collisionMarginTriangle = collisionMarginTriangle; + _resultOut = resultOut; + + //recalc aabbs + Matrix convexInTriangleSpace = MathHelper.InvertMatrix(_triBody.WorldTransform) * _convexBody.WorldTransform; + CollisionShape convexShape = _convexBody.CollisionShape; + //CollisionShape* triangleShape = static_cast(triBody->m_collisionShape); + convexShape.GetAabb(convexInTriangleSpace, out _aabbMin, out _aabbMax); + float extraMargin = collisionMarginTriangle; + Vector3 extra = new Vector3(extraMargin, extraMargin, extraMargin); + + _aabbMax += extra; + _aabbMin -= extra; + } + + public void ClearCache() + { + _dispatcher.ClearManifold(_manifold); + } + + #region ITriangleCallback Members + public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) + { + //aabb filter is already applied! + CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo = new CollisionAlgorithmConstructionInfo(); + collisionAlgorithmConstructionInfo.Dispatcher = _dispatcher; + + CollisionObject collisionObject = _triBody; + + //debug drawing of the overlapping triangles + /*if (m_dispatchInfoPtr && m_dispatchInfoPtr.m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0) + { + Vector3 color = new Vector3(255, 255, 0); + btTransform & tr = ob->WorldTransform; + m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]), tr(triangle[1]), color); + m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]), tr(triangle[2]), color); + m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]), tr(triangle[0]), color); + }*/ + + if (_convexBody.CollisionShape.IsConvex) + { + TriangleShape triangleShape = new TriangleShape(triangle[0], triangle[1], triangle[2]); + triangleShape.Margin=_collisionMarginTriangle; + + CollisionShape tempShape = collisionObject.CollisionShape; + collisionObject.CollisionShape = triangleShape; + + CollisionAlgorithm collisionAlgorithm = collisionAlgorithmConstructionInfo.Dispatcher.FindAlgorithm(_convexBody, _triBody, _manifold); + + _resultOut.SetShapeIdentifiers(-1, -1, partID, triangleIndex); + collisionAlgorithm.ProcessCollision(_convexBody, _triBody, _dispatchInfo, _resultOut); + collisionObject.CollisionShape = tempShape; + } + } + #endregion + #region IDisposable Members + public void Dispose() + { + ClearCache(); + _dispatcher.ReleaseManifold(_manifold); + } + #endregion + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs index 13cb3d9..5995e2c 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs @@ -1,52 +1,52 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; - -namespace XnaDevRu.BulletX -{ - /// - /// EmptyAlgorithm is a stub for unsupported collision pairs. - /// The dispatcher can dispatch a persistent btEmptyAlgorithm to avoid a search every frame. - /// - public class EmptyAlgorithm : CollisionAlgorithm - { - public EmptyAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) - : base(collisionAlgorithmConstructionInfo) { } - - public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { } - - public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) - { - return 1f; - } - - public class CreateFunc : CollisionAlgorithmCreateFunction - { - public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) - { - return new EmptyAlgorithm(collisionAlgorithmConstructionInfo); - } - }; - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace XnaDevRu.BulletX +{ + /// + /// EmptyAlgorithm is a stub for unsupported collision pairs. + /// The dispatcher can dispatch a persistent btEmptyAlgorithm to avoid a search every frame. + /// + public class EmptyAlgorithm : CollisionAlgorithm + { + public EmptyAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) + : base(collisionAlgorithmConstructionInfo) { } + + public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { } + + public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) + { + return 1f; + } + + public class CreateFunc : CollisionAlgorithmCreateFunction + { + public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) + { + return new EmptyAlgorithm(collisionAlgorithmConstructionInfo); + } + }; + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs index 63f37ea..69143a1 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs @@ -1,147 +1,147 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public delegate bool ContactAddedCallback(ManifoldPoint contactPoint, CollisionObject collisionObjectA, int partIdA, int indexA, CollisionObject collisionObjectB, int partIdB, int indexB); - - public class ManifoldResult : DiscreteCollisionDetectorInterface.Result - { - private PersistentManifold _manifold; - private static ContactAddedCallback _contactAddedCallback = null; - - //we need this for compounds - private Matrix _rootTransA; - private Matrix _rootTransB; - - private CollisionObject _bodyA; - private CollisionObject _bodyB; - private int _partIdA; - private int _partIdB; - private int _indexA; - private int _indexB; - - public ManifoldResult() - { - } - - public ManifoldResult(CollisionObject bodyA, CollisionObject bodyB) - { - _bodyA = bodyA; - _bodyB = bodyB; - _rootTransA = bodyA.WorldTransform; - _rootTransB = bodyB.WorldTransform; - } - - public static ContactAddedCallback ContactAddedCallback { get { return _contactAddedCallback; } set { _contactAddedCallback = value; } } - - public void SetPersistentManifold(PersistentManifold manifold) - { - _manifold = manifold; - } - - public override void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB) - { - _partIdA = partIdA; - _partIdB = partIdB; - _indexA = indexA; - _indexB = indexB; - } - - public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) - { - if (_manifold == null) - throw new BulletException("Manifold Pointer is null."); - - //order in manifold needs to match - - if (depth > PersistentManifold.ContactBreakingThreshold) - return; - - bool isSwapped = _manifold.BodyA != _bodyA; - - Vector3 pointA = pointInWorld + normalOnBInWorld * depth; - Vector3 localA; - Vector3 localB; - - if (isSwapped) - { - localA = MathHelper.InvXForm(_rootTransB, pointA); - localB = MathHelper.InvXForm(_rootTransA, pointInWorld); - } - else - { - localA = MathHelper.InvXForm(_rootTransA, pointA); - localB = MathHelper.InvXForm(_rootTransB, pointInWorld); - } - - ManifoldPoint newPt = new ManifoldPoint(localA, localB, normalOnBInWorld, depth); - - int insertIndex = _manifold.GetCacheEntry(newPt); - - newPt.CombinedFriction = CalculateCombinedFriction(_bodyA, _bodyB); - newPt.CombinedRestitution = CalculateCombinedRestitution(_bodyA, _bodyB); - - //User can override friction and/or restitution - if (_contactAddedCallback != null && - //and if either of the two bodies requires custom material - ((_bodyA.CollisionFlags & CollisionOptions.CustomMaterialCallback) != 0 || - (_bodyB.CollisionFlags & CollisionOptions.CustomMaterialCallback) != 0)) - { - //experimental feature info, for per-triangle material etc. - CollisionObject obj0 = isSwapped ? _bodyB : _bodyA; - CollisionObject obj1 = isSwapped ? _bodyA : _bodyB; - _contactAddedCallback(newPt, obj0, _partIdA, _indexA, obj1, _partIdB, _indexB); - } - - if (insertIndex >= 0) - { - _manifold.ReplaceContactPoint(newPt, insertIndex); - } - else - { - _manifold.AddManifoldPoint(newPt); - } - } - - private float CalculateCombinedFriction(CollisionObject bodyA, CollisionObject bodyB) - { - float friction = bodyA.Friction * bodyB.Friction; - - float MaxFriction = 10; - if (friction < -MaxFriction) - friction = -MaxFriction; - if (friction > MaxFriction) - friction = MaxFriction; - return friction; - } - - private float CalculateCombinedRestitution(CollisionObject bodyA, CollisionObject bodyB) - { - return bodyA.Restitution * bodyB.Restitution; - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public delegate bool ContactAddedCallback(ManifoldPoint contactPoint, CollisionObject collisionObjectA, int partIdA, int indexA, CollisionObject collisionObjectB, int partIdB, int indexB); + + public class ManifoldResult : DiscreteCollisionDetectorInterface.Result + { + private PersistentManifold _manifold; + private static ContactAddedCallback _contactAddedCallback = null; + + //we need this for compounds + private Matrix _rootTransA; + private Matrix _rootTransB; + + private CollisionObject _bodyA; + private CollisionObject _bodyB; + private int _partIdA; + private int _partIdB; + private int _indexA; + private int _indexB; + + public ManifoldResult() + { + } + + public ManifoldResult(CollisionObject bodyA, CollisionObject bodyB) + { + _bodyA = bodyA; + _bodyB = bodyB; + _rootTransA = bodyA.WorldTransform; + _rootTransB = bodyB.WorldTransform; + } + + public static ContactAddedCallback ContactAddedCallback { get { return _contactAddedCallback; } set { _contactAddedCallback = value; } } + + public void SetPersistentManifold(PersistentManifold manifold) + { + _manifold = manifold; + } + + public override void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB) + { + _partIdA = partIdA; + _partIdB = partIdB; + _indexA = indexA; + _indexB = indexB; + } + + public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) + { + if (_manifold == null) + throw new BulletException("Manifold Pointer is null."); + + //order in manifold needs to match + + if (depth > PersistentManifold.ContactBreakingThreshold) + return; + + bool isSwapped = _manifold.BodyA != _bodyA; + + Vector3 pointA = pointInWorld + normalOnBInWorld * depth; + Vector3 localA; + Vector3 localB; + + if (isSwapped) + { + localA = MathHelper.InvXForm(_rootTransB, pointA); + localB = MathHelper.InvXForm(_rootTransA, pointInWorld); + } + else + { + localA = MathHelper.InvXForm(_rootTransA, pointA); + localB = MathHelper.InvXForm(_rootTransB, pointInWorld); + } + + ManifoldPoint newPt = new ManifoldPoint(localA, localB, normalOnBInWorld, depth); + + int insertIndex = _manifold.GetCacheEntry(newPt); + + newPt.CombinedFriction = CalculateCombinedFriction(_bodyA, _bodyB); + newPt.CombinedRestitution = CalculateCombinedRestitution(_bodyA, _bodyB); + + //User can override friction and/or restitution + if (_contactAddedCallback != null && + //and if either of the two bodies requires custom material + ((_bodyA.CollisionFlags & CollisionOptions.CustomMaterialCallback) != 0 || + (_bodyB.CollisionFlags & CollisionOptions.CustomMaterialCallback) != 0)) + { + //experimental feature info, for per-triangle material etc. + CollisionObject obj0 = isSwapped ? _bodyB : _bodyA; + CollisionObject obj1 = isSwapped ? _bodyA : _bodyB; + _contactAddedCallback(newPt, obj0, _partIdA, _indexA, obj1, _partIdB, _indexB); + } + + if (insertIndex >= 0) + { + _manifold.ReplaceContactPoint(newPt, insertIndex); + } + else + { + _manifold.AddManifoldPoint(newPt); + } + } + + private float CalculateCombinedFriction(CollisionObject bodyA, CollisionObject bodyB) + { + float friction = bodyA.Friction * bodyB.Friction; + + float MaxFriction = 10; + if (friction < -MaxFriction) + friction = -MaxFriction; + if (friction > MaxFriction) + friction = MaxFriction; + return friction; + } + + private float CalculateCombinedRestitution(CollisionObject bodyA, CollisionObject bodyB) + { + return bodyA.Restitution * bodyB.Restitution; + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs index 957843b..39eb53a 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs @@ -1,304 +1,304 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public class SimulationIslandManager - { - private UnionFind _unionFind = new UnionFind(); - - public void InitUnionFind(int n) - { - _unionFind.Reset(n); - } - - public UnionFind UnionFind { get { return _unionFind; } } - - public virtual void UpdateActivationState(CollisionWorld world, IDispatcher dispatcher) - { - InitUnionFind(world.CollisionObjectsCount); - - // put the index into m_controllers into m_tag - int index = 0; - for (int i = 0; i < world.CollisionObjects.Count; i++) - { - world.CollisionObjects[i].IslandTag = index; - world.CollisionObjects[i].HitFraction = 1; - world.CollisionObjects[i].CompanionID = -1; - index++; - } - // do the union find - FindUnions(dispatcher); - } - - public virtual void StoreIslandActivationState(CollisionWorld world) - { - // put the islandId ('find' value) into m_tag - int index = 0; - for (int i = 0; i < world.CollisionObjects.Count; i++) - { - if (world.CollisionObjects[i].MergesSimulationIslands) - { - world.CollisionObjects[i].IslandTag = _unionFind.Find(index); - world.CollisionObjects[i].CompanionID = -1; - } - else - { - world.CollisionObjects[i].IslandTag = -1; - world.CollisionObjects[i].CompanionID = -2; - } - index++; - } - } - - public void FindUnions(IDispatcher dispatcher) - { - for (int i = 0; i < dispatcher.ManifoldCount; i++) - { - PersistentManifold manifold = dispatcher.GetManifoldByIndex(i); - //static objects (invmass 0.f) don't merge ! - - CollisionObject colObjA = manifold.BodyA as CollisionObject; - CollisionObject colObjB = manifold.BodyB as CollisionObject; - - if (((colObjA != null) && (colObjA.MergesSimulationIslands)) && - ((colObjB != null) && (colObjB.MergesSimulationIslands))) - { - _unionFind.Unite(colObjA.IslandTag, colObjB.IslandTag); - } - } - } - - public void BuildAndProcessIslands(IDispatcher dispatcher, List collisionObjects, IIslandCallback callback) - { - //we are going to sort the unionfind array, and store the element id in the size - //afterwards, we clean unionfind, to make sure no-one uses it anymore - UnionFind.SortIslands(); - int numElem = UnionFind.ElementCount; - - int endIslandIndex = 1; - int startIslandIndex; - - //update the sleeping state for bodies, if all are sleeping - for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) - { - int islandId = UnionFind[startIslandIndex].ID; - for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++) - { - } - - //int numSleeping = 0; - - bool allSleeping = true; - - int idx; - for (idx = startIslandIndex; idx < endIslandIndex; idx++) - { - int i = UnionFind[idx].Size; - - CollisionObject colObjA = collisionObjects[i]; - if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1)) - { - Console.WriteLine("error in island management"); - } - - BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1)); - if (colObjA.IslandTag == islandId) - { - if (colObjA.ActivationState == ActivationState.Active) - { - allSleeping = false; - } - if (colObjA.ActivationState == ActivationState.DisableDeactivation) - { - allSleeping = false; - } - } - } - - - if (allSleeping) - { - for (idx = startIslandIndex; idx < endIslandIndex; idx++) - { - int i = UnionFind[idx].Size; - CollisionObject colObjA = collisionObjects[i]; - if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1)) - { - Console.WriteLine("error in island management"); - } - - BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1)); - - if (colObjA.IslandTag == islandId) - { - colObjA.ActivationState =ActivationState.IslandSleeping; - } - } - } - else - { - for (idx = startIslandIndex; idx < endIslandIndex; idx++) - { - int i = UnionFind[idx].Size; - - CollisionObject colObjA = collisionObjects[i]; - if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1)) - { - Console.WriteLine("error in island management"); - } - - BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1)); - - if (colObjA.IslandTag == islandId) - { - if (colObjA.ActivationState == ActivationState.IslandSleeping) - { - colObjA.ActivationState = ActivationState.WantsDeactivation; - } - } - } - } - } - - //int maxNumManifolds = dispatcher.ManifoldCount; - List islandmanifold = new List(dispatcher.ManifoldCount); - - for (int i = 0; i < dispatcher.ManifoldCount; i++) - { - PersistentManifold manifold = dispatcher.GetManifoldByIndex(i); - - CollisionObject colObjA = manifold.BodyA as CollisionObject; - CollisionObject colObjB = manifold.BodyB as CollisionObject; - - //todo: check sleeping conditions! - if (((colObjA != null) && colObjA.ActivationState != ActivationState.IslandSleeping) || - ((colObjB != null) && colObjB.ActivationState != ActivationState.IslandSleeping)) - { - - //kinematic objects don't merge islands, but wake up all connected objects - if (colObjA.IsStaticOrKinematicObject && colObjA.ActivationState != ActivationState.IslandSleeping) - { - colObjB.Activate(); - } - if (colObjB.IsStaticOrKinematicObject && colObjB.ActivationState != ActivationState.IslandSleeping) - { - colObjA.Activate(); - } - - //filtering for response - if (dispatcher.NeedsResponse(colObjA, colObjB)) - islandmanifold.Add(manifold); - } - } - - int numManifolds = islandmanifold.Count; - - // Sort manifolds, based on islands - // Sort the vector using predicate and std::sort - islandmanifold.Sort(new Comparison(PersistentManifoldSortPredicate)); - - //now process all active islands (sets of manifolds for now) - int startManifoldIndex = 0; - int endManifoldIndex = 1; - - List islandBodies = new List(); - - for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) - { - int islandId = UnionFind[startIslandIndex].ID; - bool islandSleeping = false; - for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++) - { - int i = UnionFind[endIslandIndex].Size; - CollisionObject colObjA = collisionObjects[i]; - islandBodies.Add(colObjA); - if (!colObjA.IsActive) - islandSleeping = true; - } - - //find the accompanying contact manifold for this islandId - int numIslandManifolds = 0; - List startManifold = new List(numIslandManifolds); - - if (startManifoldIndex < numManifolds) - { - int curIslandID = GetIslandId(islandmanifold[startManifoldIndex]); - if (curIslandID == islandId) - { - for (int k = startManifoldIndex; k < islandmanifold.Count; k++) - { - startManifold.Add(islandmanifold[k]); - } - for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == GetIslandId(islandmanifold[endManifoldIndex])); endManifoldIndex++) { } - - // Process the actual simulation, only if not sleeping/deactivated - numIslandManifolds = endManifoldIndex - startManifoldIndex; - } - } - - if (!islandSleeping) - { - callback.ProcessIsland(islandBodies, startManifold, numIslandManifolds, islandId); - } - - if (numIslandManifolds != 0) - { - startManifoldIndex = endManifoldIndex; - } - - islandBodies.Clear(); - } - } - - private static int GetIslandId(PersistentManifold lhs) - { - int islandId; - CollisionObject rcolObjA = lhs.BodyA as CollisionObject; - CollisionObject rcolObjB = lhs.BodyB as CollisionObject; - islandId = rcolObjA.IslandTag >= 0 ? rcolObjA.IslandTag : rcolObjB.IslandTag; - return islandId; - } - - private static int PersistentManifoldSortPredicate(PersistentManifold lhs, PersistentManifold rhs) - { - int rIslandIdA, lIslandIdB; - rIslandIdA = GetIslandId(rhs); - lIslandIdB = GetIslandId(lhs); - //return lIslandId0 < rIslandId0; - if (lIslandIdB < rIslandIdA) - return -1; - //else if (lIslandIdB > rIslandIdA) - // return 1; - return 1; - } - - public interface IIslandCallback - { - void ProcessIsland(List bodies, List manifolds, int numManifolds, int islandID); - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public class SimulationIslandManager + { + private UnionFind _unionFind = new UnionFind(); + + public void InitUnionFind(int n) + { + _unionFind.Reset(n); + } + + public UnionFind UnionFind { get { return _unionFind; } } + + public virtual void UpdateActivationState(CollisionWorld world, IDispatcher dispatcher) + { + InitUnionFind(world.CollisionObjectsCount); + + // put the index into m_controllers into m_tag + int index = 0; + for (int i = 0; i < world.CollisionObjects.Count; i++) + { + world.CollisionObjects[i].IslandTag = index; + world.CollisionObjects[i].HitFraction = 1; + world.CollisionObjects[i].CompanionID = -1; + index++; + } + // do the union find + FindUnions(dispatcher); + } + + public virtual void StoreIslandActivationState(CollisionWorld world) + { + // put the islandId ('find' value) into m_tag + int index = 0; + for (int i = 0; i < world.CollisionObjects.Count; i++) + { + if (world.CollisionObjects[i].MergesSimulationIslands) + { + world.CollisionObjects[i].IslandTag = _unionFind.Find(index); + world.CollisionObjects[i].CompanionID = -1; + } + else + { + world.CollisionObjects[i].IslandTag = -1; + world.CollisionObjects[i].CompanionID = -2; + } + index++; + } + } + + public void FindUnions(IDispatcher dispatcher) + { + for (int i = 0; i < dispatcher.ManifoldCount; i++) + { + PersistentManifold manifold = dispatcher.GetManifoldByIndex(i); + //static objects (invmass 0.f) don't merge ! + + CollisionObject colObjA = manifold.BodyA as CollisionObject; + CollisionObject colObjB = manifold.BodyB as CollisionObject; + + if (((colObjA != null) && (colObjA.MergesSimulationIslands)) && + ((colObjB != null) && (colObjB.MergesSimulationIslands))) + { + _unionFind.Unite(colObjA.IslandTag, colObjB.IslandTag); + } + } + } + + public void BuildAndProcessIslands(IDispatcher dispatcher, List collisionObjects, IIslandCallback callback) + { + //we are going to sort the unionfind array, and store the element id in the size + //afterwards, we clean unionfind, to make sure no-one uses it anymore + UnionFind.SortIslands(); + int numElem = UnionFind.ElementCount; + + int endIslandIndex = 1; + int startIslandIndex; + + //update the sleeping state for bodies, if all are sleeping + for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) + { + int islandId = UnionFind[startIslandIndex].ID; + for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++) + { + } + + //int numSleeping = 0; + + bool allSleeping = true; + + int idx; + for (idx = startIslandIndex; idx < endIslandIndex; idx++) + { + int i = UnionFind[idx].Size; + + CollisionObject colObjA = collisionObjects[i]; + if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1)) + { + Console.WriteLine("error in island management"); + } + + BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1)); + if (colObjA.IslandTag == islandId) + { + if (colObjA.ActivationState == ActivationState.Active) + { + allSleeping = false; + } + if (colObjA.ActivationState == ActivationState.DisableDeactivation) + { + allSleeping = false; + } + } + } + + + if (allSleeping) + { + for (idx = startIslandIndex; idx < endIslandIndex; idx++) + { + int i = UnionFind[idx].Size; + CollisionObject colObjA = collisionObjects[i]; + if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1)) + { + Console.WriteLine("error in island management"); + } + + BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1)); + + if (colObjA.IslandTag == islandId) + { + colObjA.ActivationState =ActivationState.IslandSleeping; + } + } + } + else + { + for (idx = startIslandIndex; idx < endIslandIndex; idx++) + { + int i = UnionFind[idx].Size; + + CollisionObject colObjA = collisionObjects[i]; + if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1)) + { + Console.WriteLine("error in island management"); + } + + BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1)); + + if (colObjA.IslandTag == islandId) + { + if (colObjA.ActivationState == ActivationState.IslandSleeping) + { + colObjA.ActivationState = ActivationState.WantsDeactivation; + } + } + } + } + } + + //int maxNumManifolds = dispatcher.ManifoldCount; + List islandmanifold = new List(dispatcher.ManifoldCount); + + for (int i = 0; i < dispatcher.ManifoldCount; i++) + { + PersistentManifold manifold = dispatcher.GetManifoldByIndex(i); + + CollisionObject colObjA = manifold.BodyA as CollisionObject; + CollisionObject colObjB = manifold.BodyB as CollisionObject; + + //todo: check sleeping conditions! + if (((colObjA != null) && colObjA.ActivationState != ActivationState.IslandSleeping) || + ((colObjB != null) && colObjB.ActivationState != ActivationState.IslandSleeping)) + { + + //kinematic objects don't merge islands, but wake up all connected objects + if (colObjA.IsStaticOrKinematicObject && colObjA.ActivationState != ActivationState.IslandSleeping) + { + colObjB.Activate(); + } + if (colObjB.IsStaticOrKinematicObject && colObjB.ActivationState != ActivationState.IslandSleeping) + { + colObjA.Activate(); + } + + //filtering for response + if (dispatcher.NeedsResponse(colObjA, colObjB)) + islandmanifold.Add(manifold); + } + } + + int numManifolds = islandmanifold.Count; + + // Sort manifolds, based on islands + // Sort the vector using predicate and std::sort + islandmanifold.Sort(new Comparison(PersistentManifoldSortPredicate)); + + //now process all active islands (sets of manifolds for now) + int startManifoldIndex = 0; + int endManifoldIndex = 1; + + List islandBodies = new List(); + + for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) + { + int islandId = UnionFind[startIslandIndex].ID; + bool islandSleeping = false; + for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++) + { + int i = UnionFind[endIslandIndex].Size; + CollisionObject colObjA = collisionObjects[i]; + islandBodies.Add(colObjA); + if (!colObjA.IsActive) + islandSleeping = true; + } + + //find the accompanying contact manifold for this islandId + int numIslandManifolds = 0; + List startManifold = new List(numIslandManifolds); + + if (startManifoldIndex < numManifolds) + { + int curIslandID = GetIslandId(islandmanifold[startManifoldIndex]); + if (curIslandID == islandId) + { + for (int k = startManifoldIndex; k < islandmanifold.Count; k++) + { + startManifold.Add(islandmanifold[k]); + } + for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == GetIslandId(islandmanifold[endManifoldIndex])); endManifoldIndex++) { } + + // Process the actual simulation, only if not sleeping/deactivated + numIslandManifolds = endManifoldIndex - startManifoldIndex; + } + } + + if (!islandSleeping) + { + callback.ProcessIsland(islandBodies, startManifold, numIslandManifolds, islandId); + } + + if (numIslandManifolds != 0) + { + startManifoldIndex = endManifoldIndex; + } + + islandBodies.Clear(); + } + } + + private static int GetIslandId(PersistentManifold lhs) + { + int islandId; + CollisionObject rcolObjA = lhs.BodyA as CollisionObject; + CollisionObject rcolObjB = lhs.BodyB as CollisionObject; + islandId = rcolObjA.IslandTag >= 0 ? rcolObjA.IslandTag : rcolObjB.IslandTag; + return islandId; + } + + private static int PersistentManifoldSortPredicate(PersistentManifold lhs, PersistentManifold rhs) + { + int rIslandIdA, lIslandIdB; + rIslandIdA = GetIslandId(rhs); + lIslandIdB = GetIslandId(lhs); + //return lIslandId0 < rIslandId0; + if (lIslandIdB < rIslandIdA) + return -1; + //else if (lIslandIdB > rIslandIdA) + // return 1; + return 1; + } + + public interface IIslandCallback + { + void ProcessIsland(List bodies, List manifolds, int numManifolds, int islandID); + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs index 8933638..2b4e801 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs @@ -1,270 +1,270 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// - /// SphereBoxCollisionAlgorithm provides sphere-box collision detection. - /// Other features are frame-coherency (persistent data) and collision response. - /// - public class SphereBoxCollisionAlgorithm : CollisionAlgorithm, IDisposable - { - private bool _ownManifold; - private PersistentManifold _manifold; - private bool _isSwapped; - - public SphereBoxCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject collisionObjectA, CollisionObject collisionObjectB, bool isSwapped) - : base(collisionAlgorithmConstructionInfo) - { - _ownManifold = false; - _manifold = manifold; - _isSwapped = isSwapped; - - CollisionObject sphereObject = _isSwapped ? collisionObjectB : collisionObjectA; - CollisionObject boxObject = _isSwapped ? collisionObjectA : collisionObjectB; - - if (_manifold == null && Dispatcher.NeedsCollision(sphereObject, boxObject)) - { - _manifold = Dispatcher.GetNewManifold(sphereObject, boxObject); - _ownManifold = true; - } - } - - public float GetSphereDistance(CollisionObject boxObject, out Vector3 pointOnBox, out Vector3 pointOnSphere, Vector3 sphereCenter, float radius) - { - pointOnBox = new Vector3(); - pointOnSphere = new Vector3(); - - float margins; - Vector3[] bounds = new Vector3[2]; - BoxShape boxShape = boxObject.CollisionShape as BoxShape; - - bounds[0] = -boxShape.HalfExtents; - bounds[1] = boxShape.HalfExtents; - - margins = boxShape.Margin; //also add sphereShape margin? - - Matrix m44T = boxObject.WorldTransform; - - Vector3[] boundsVec = new Vector3[2]; - float penetration; - - boundsVec[0] = bounds[0]; - boundsVec[1] = bounds[1]; - - Vector3 marginsVec = new Vector3(margins, margins, margins); - - // add margins - bounds[0] += marginsVec; - bounds[1] -= marginsVec; - - ///////////////////////////////////////////////// - - Vector3 tmp, prel, normal, v3P; - Vector3[] n = new Vector3[6]; - float sep = 10000000.0f, sepThis; - - n[0] = new Vector3(-1.0f, 0.0f, 0.0f); - n[1] = new Vector3(0.0f, -1.0f, 0.0f); - n[2] = new Vector3(0.0f, 0.0f, -1.0f); - n[3] = new Vector3(1.0f, 0.0f, 0.0f); - n[4] = new Vector3(0.0f, 1.0f, 0.0f); - n[5] = new Vector3(0.0f, 0.0f, 1.0f); - - // convert point in local space - prel = MathHelper.InvXForm(m44T, sphereCenter); - - bool found = false; - - v3P = prel; - - for (int i = 0; i < 6; i++) - { - int j = i < 3 ? 0 : 1; - if ((sepThis = (Vector3.Dot(v3P - bounds[j], n[i]))) > 0.0f) - { - v3P = v3P - n[i] * sepThis; - found = true; - } - } - - // - - if (found) - { - bounds[0] = boundsVec[0]; - bounds[1] = boundsVec[1]; - - normal = Vector3.Normalize(prel - v3P); - pointOnBox = v3P + normal * margins; - pointOnSphere = prel - normal * radius; - - if ((Vector3.Dot(pointOnSphere - pointOnBox, normal)) > 0.0f) - { - return 1.0f; - } - - // transform back in world space - tmp = MathHelper.MatrixToVector(m44T, pointOnBox); - pointOnBox = tmp; - tmp = MathHelper.MatrixToVector(m44T, pointOnSphere); - pointOnSphere = tmp; - float seps2 = (pointOnBox - pointOnSphere).LengthSquared(); - - //if this fails, fallback into deeper penetration case, below - if (seps2 > MathHelper.Epsilon) - { - sep = -(float)Math.Sqrt(seps2); - normal = (pointOnBox - pointOnSphere); - normal *= 1f / sep; - } - return sep; - } - - ////////////////////////////////////////////////// - // Deep penetration case - - penetration = GetSpherePenetration(boxObject, ref pointOnBox, ref pointOnSphere, sphereCenter, radius, bounds[0], bounds[1]); - - bounds[0] = boundsVec[0]; - bounds[1] = boundsVec[1]; - - if (penetration <= 0.0f) - return (penetration - margins); - else - return 1.0f; - } - - public float GetSpherePenetration(CollisionObject boxObject, ref Vector3 pointOnBox, ref Vector3 pointOnSphere, Vector3 sphereCenter, float radius, Vector3 aabbMin, Vector3 aabbMax) - { - Vector3[] bounds = new Vector3[2]; - - bounds[0] = aabbMin; - bounds[1] = aabbMax; - - Vector3 p0 = new Vector3(), tmp, prel, normal = new Vector3(); - Vector3[] n = new Vector3[6]; - float sep = -10000000.0f, sepThis; - - n[0] = new Vector3(-1.0f, 0.0f, 0.0f); - n[1] = new Vector3(0.0f, -1.0f, 0.0f); - n[2] = new Vector3(0.0f, 0.0f, -1.0f); - n[3] = new Vector3(1.0f, 0.0f, 0.0f); - n[4] = new Vector3(0.0f, 1.0f, 0.0f); - n[5] = new Vector3(0.0f, 0.0f, 1.0f); - - Matrix m44T = boxObject.WorldTransform; - - // convert point in local space - prel = MathHelper.InvXForm(m44T, sphereCenter); - - /////////// - - for (int i = 0; i < 6; i++) - { - int j = i < 3 ? 0 : 1; - if ((sepThis = (Vector3.Dot(prel - bounds[j], n[i])) - radius) > 0.0f) return 1.0f; - if (sepThis > sep) - { - p0 = bounds[j]; - normal = n[i]; - sep = sepThis; - } - } - - pointOnBox = prel - normal * (Vector3.Dot(normal, (prel - p0))); - pointOnSphere = pointOnBox + normal * sep; - - // transform back in world space - tmp = MathHelper.MatrixToVector(m44T, pointOnBox); - pointOnBox = tmp; - tmp = MathHelper.MatrixToVector(m44T, pointOnSphere); - pointOnSphere = tmp; - normal = Vector3.Normalize(pointOnBox - pointOnSphere); - - return sep; - } - - public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) - { - if (_manifold == null) - return; - - CollisionObject sphereObject = _isSwapped ? bodyB : bodyA; - CollisionObject boxObject = _isSwapped ? bodyA : bodyB; - - SphereShape sphereA = sphereObject.CollisionShape as SphereShape; - - Vector3 pOnBox, pOnSphere; - Vector3 sphereCenter = sphereObject.WorldTransform.Translation; - float radius = sphereA.Radius; - - float dist = GetSphereDistance(boxObject, out pOnBox, out pOnSphere, sphereCenter, radius); - - if (dist < MathHelper.Epsilon) - { - Vector3 normalOnSurfaceB = Vector3.Normalize(pOnBox - pOnSphere); - - // report a contact. internally this will be kept persistent, and contact reduction is done - resultOut.SetPersistentManifold(_manifold); - resultOut.AddContactPoint(normalOnSurfaceB, pOnBox, dist); - } - } - - public override float CalculateTimeOfImpact(CollisionObject collisionObjectA, CollisionObject collisionObjectB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) - { - //not yet - return 1; - } - - public class CreateFunc : CollisionAlgorithmCreateFunction - { - public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) - { - if (!IsSwapped) - return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, false); - else - return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, true); - } - } - - #region IDisposable Members - public void Dispose() - { - Dispose(true); - } - - public void Dispose(bool disposing) - { - if (disposing && _ownManifold) - { - if (_manifold != null) - Dispatcher.ReleaseManifold(_manifold); - } - } - #endregion - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// + /// SphereBoxCollisionAlgorithm provides sphere-box collision detection. + /// Other features are frame-coherency (persistent data) and collision response. + /// + public class SphereBoxCollisionAlgorithm : CollisionAlgorithm, IDisposable + { + private bool _ownManifold; + private PersistentManifold _manifold; + private bool _isSwapped; + + public SphereBoxCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject collisionObjectA, CollisionObject collisionObjectB, bool isSwapped) + : base(collisionAlgorithmConstructionInfo) + { + _ownManifold = false; + _manifold = manifold; + _isSwapped = isSwapped; + + CollisionObject sphereObject = _isSwapped ? collisionObjectB : collisionObjectA; + CollisionObject boxObject = _isSwapped ? collisionObjectA : collisionObjectB; + + if (_manifold == null && Dispatcher.NeedsCollision(sphereObject, boxObject)) + { + _manifold = Dispatcher.GetNewManifold(sphereObject, boxObject); + _ownManifold = true; + } + } + + public float GetSphereDistance(CollisionObject boxObject, out Vector3 pointOnBox, out Vector3 pointOnSphere, Vector3 sphereCenter, float radius) + { + pointOnBox = new Vector3(); + pointOnSphere = new Vector3(); + + float margins; + Vector3[] bounds = new Vector3[2]; + BoxShape boxShape = boxObject.CollisionShape as BoxShape; + + bounds[0] = -boxShape.HalfExtents; + bounds[1] = boxShape.HalfExtents; + + margins = boxShape.Margin; //also add sphereShape margin? + + Matrix m44T = boxObject.WorldTransform; + + Vector3[] boundsVec = new Vector3[2]; + float penetration; + + boundsVec[0] = bounds[0]; + boundsVec[1] = bounds[1]; + + Vector3 marginsVec = new Vector3(margins, margins, margins); + + // add margins + bounds[0] += marginsVec; + bounds[1] -= marginsVec; + + ///////////////////////////////////////////////// + + Vector3 tmp, prel, normal, v3P; + Vector3[] n = new Vector3[6]; + float sep = 10000000.0f, sepThis; + + n[0] = new Vector3(-1.0f, 0.0f, 0.0f); + n[1] = new Vector3(0.0f, -1.0f, 0.0f); + n[2] = new Vector3(0.0f, 0.0f, -1.0f); + n[3] = new Vector3(1.0f, 0.0f, 0.0f); + n[4] = new Vector3(0.0f, 1.0f, 0.0f); + n[5] = new Vector3(0.0f, 0.0f, 1.0f); + + // convert point in local space + prel = MathHelper.InvXForm(m44T, sphereCenter); + + bool found = false; + + v3P = prel; + + for (int i = 0; i < 6; i++) + { + int j = i < 3 ? 0 : 1; + if ((sepThis = (Vector3.Dot(v3P - bounds[j], n[i]))) > 0.0f) + { + v3P = v3P - n[i] * sepThis; + found = true; + } + } + + // + + if (found) + { + bounds[0] = boundsVec[0]; + bounds[1] = boundsVec[1]; + + normal = Vector3.Normalize(prel - v3P); + pointOnBox = v3P + normal * margins; + pointOnSphere = prel - normal * radius; + + if ((Vector3.Dot(pointOnSphere - pointOnBox, normal)) > 0.0f) + { + return 1.0f; + } + + // transform back in world space + tmp = MathHelper.MatrixToVector(m44T, pointOnBox); + pointOnBox = tmp; + tmp = MathHelper.MatrixToVector(m44T, pointOnSphere); + pointOnSphere = tmp; + float seps2 = (pointOnBox - pointOnSphere).LengthSquared(); + + //if this fails, fallback into deeper penetration case, below + if (seps2 > MathHelper.Epsilon) + { + sep = -(float)Math.Sqrt(seps2); + normal = (pointOnBox - pointOnSphere); + normal *= 1f / sep; + } + return sep; + } + + ////////////////////////////////////////////////// + // Deep penetration case + + penetration = GetSpherePenetration(boxObject, ref pointOnBox, ref pointOnSphere, sphereCenter, radius, bounds[0], bounds[1]); + + bounds[0] = boundsVec[0]; + bounds[1] = boundsVec[1]; + + if (penetration <= 0.0f) + return (penetration - margins); + else + return 1.0f; + } + + public float GetSpherePenetration(CollisionObject boxObject, ref Vector3 pointOnBox, ref Vector3 pointOnSphere, Vector3 sphereCenter, float radius, Vector3 aabbMin, Vector3 aabbMax) + { + Vector3[] bounds = new Vector3[2]; + + bounds[0] = aabbMin; + bounds[1] = aabbMax; + + Vector3 p0 = new Vector3(), tmp, prel, normal = new Vector3(); + Vector3[] n = new Vector3[6]; + float sep = -10000000.0f, sepThis; + + n[0] = new Vector3(-1.0f, 0.0f, 0.0f); + n[1] = new Vector3(0.0f, -1.0f, 0.0f); + n[2] = new Vector3(0.0f, 0.0f, -1.0f); + n[3] = new Vector3(1.0f, 0.0f, 0.0f); + n[4] = new Vector3(0.0f, 1.0f, 0.0f); + n[5] = new Vector3(0.0f, 0.0f, 1.0f); + + Matrix m44T = boxObject.WorldTransform; + + // convert point in local space + prel = MathHelper.InvXForm(m44T, sphereCenter); + + /////////// + + for (int i = 0; i < 6; i++) + { + int j = i < 3 ? 0 : 1; + if ((sepThis = (Vector3.Dot(prel - bounds[j], n[i])) - radius) > 0.0f) return 1.0f; + if (sepThis > sep) + { + p0 = bounds[j]; + normal = n[i]; + sep = sepThis; + } + } + + pointOnBox = prel - normal * (Vector3.Dot(normal, (prel - p0))); + pointOnSphere = pointOnBox + normal * sep; + + // transform back in world space + tmp = MathHelper.MatrixToVector(m44T, pointOnBox); + pointOnBox = tmp; + tmp = MathHelper.MatrixToVector(m44T, pointOnSphere); + pointOnSphere = tmp; + normal = Vector3.Normalize(pointOnBox - pointOnSphere); + + return sep; + } + + public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) + { + if (_manifold == null) + return; + + CollisionObject sphereObject = _isSwapped ? bodyB : bodyA; + CollisionObject boxObject = _isSwapped ? bodyA : bodyB; + + SphereShape sphereA = sphereObject.CollisionShape as SphereShape; + + Vector3 pOnBox, pOnSphere; + Vector3 sphereCenter = sphereObject.WorldTransform.Translation; + float radius = sphereA.Radius; + + float dist = GetSphereDistance(boxObject, out pOnBox, out pOnSphere, sphereCenter, radius); + + if (dist < MathHelper.Epsilon) + { + Vector3 normalOnSurfaceB = Vector3.Normalize(pOnBox - pOnSphere); + + // report a contact. internally this will be kept persistent, and contact reduction is done + resultOut.SetPersistentManifold(_manifold); + resultOut.AddContactPoint(normalOnSurfaceB, pOnBox, dist); + } + } + + public override float CalculateTimeOfImpact(CollisionObject collisionObjectA, CollisionObject collisionObjectB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) + { + //not yet + return 1; + } + + public class CreateFunc : CollisionAlgorithmCreateFunction + { + public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) + { + if (!IsSwapped) + return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, false); + else + return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, true); + } + } + + #region IDisposable Members + public void Dispose() + { + Dispose(true); + } + + public void Dispose(bool disposing) + { + if (disposing && _ownManifold) + { + if (_manifold != null) + Dispatcher.ReleaseManifold(_manifold); + } + } + #endregion + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs index 7a76d24..3a780c1 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs @@ -1,104 +1,104 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public class SphereSphereCollisionAlgorithm : CollisionAlgorithm - { - private bool _ownManifold; - private PersistentManifold _manifold; - - public SphereSphereCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) - : base(collisionAlgorithmConstructionInfo) - { - _ownManifold = false; - _manifold = manifold; - - if (_manifold == null) - { - _manifold = Dispatcher.GetNewManifold(bodyA, bodyB); - _ownManifold = true; - } - } - - public SphereSphereCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) - : base(collisionAlgorithmConstructionInfo) { } - - ~SphereSphereCollisionAlgorithm() - { - if (_ownManifold) - { - if (_manifold != null) - Dispatcher.ReleaseManifold(_manifold); - } - } - - public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) - { - if (_manifold == null) - return; - - SphereShape sphereA = bodyA.CollisionShape as SphereShape; - SphereShape sphereB = bodyB.CollisionShape as SphereShape; - - Vector3 diff = bodyA.WorldTransform.Translation - bodyB.WorldTransform.Translation; - float len = diff.Length(); - float radiusA = sphereA.Radius; - float radiusB = sphereB.Radius; - - //if distance positive, don't generate a new contact - if (len > (radiusA + radiusB)) - return; - - //distance (negative means penetration) - float dist = len - (radiusA + radiusB); - - Vector3 normalOnSurfaceB = diff / len; - //point on A (worldspace) - Vector3 posA = bodyA.WorldTransform.Translation - radiusA * normalOnSurfaceB; - //point on B (worldspace) - Vector3 posB = bodyB.WorldTransform.Translation + radiusB * normalOnSurfaceB; - - // report a contact. internally this will be kept persistent, and contact reduction is done - resultOut.SetPersistentManifold(_manifold); - resultOut.AddContactPoint(normalOnSurfaceB, posB, dist); - } - - public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) - { - //not yet - return 1f; - } - - public class CreateFunc : CollisionAlgorithmCreateFunction - { - public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) - { - return new SphereSphereCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB); - } - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public class SphereSphereCollisionAlgorithm : CollisionAlgorithm + { + private bool _ownManifold; + private PersistentManifold _manifold; + + public SphereSphereCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) + : base(collisionAlgorithmConstructionInfo) + { + _ownManifold = false; + _manifold = manifold; + + if (_manifold == null) + { + _manifold = Dispatcher.GetNewManifold(bodyA, bodyB); + _ownManifold = true; + } + } + + public SphereSphereCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) + : base(collisionAlgorithmConstructionInfo) { } + + ~SphereSphereCollisionAlgorithm() + { + if (_ownManifold) + { + if (_manifold != null) + Dispatcher.ReleaseManifold(_manifold); + } + } + + public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) + { + if (_manifold == null) + return; + + SphereShape sphereA = bodyA.CollisionShape as SphereShape; + SphereShape sphereB = bodyB.CollisionShape as SphereShape; + + Vector3 diff = bodyA.WorldTransform.Translation - bodyB.WorldTransform.Translation; + float len = diff.Length(); + float radiusA = sphereA.Radius; + float radiusB = sphereB.Radius; + + //if distance positive, don't generate a new contact + if (len > (radiusA + radiusB)) + return; + + //distance (negative means penetration) + float dist = len - (radiusA + radiusB); + + Vector3 normalOnSurfaceB = diff / len; + //point on A (worldspace) + Vector3 posA = bodyA.WorldTransform.Translation - radiusA * normalOnSurfaceB; + //point on B (worldspace) + Vector3 posB = bodyB.WorldTransform.Translation + radiusB * normalOnSurfaceB; + + // report a contact. internally this will be kept persistent, and contact reduction is done + resultOut.SetPersistentManifold(_manifold); + resultOut.AddContactPoint(normalOnSurfaceB, posB, dist); + } + + public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) + { + //not yet + return 1f; + } + + public class CreateFunc : CollisionAlgorithmCreateFunction + { + public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) + { + return new SphereSphereCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB); + } + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs index 1ca5cfb..00dfaf1 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs @@ -1,100 +1,100 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// - /// SphereSphereCollisionAlgorithm provides sphere-sphere collision detection. - /// Other features are frame-coherency (persistent data) and collision response. - /// Also provides the most basic sample for custom/user btCollisionAlgorithm - /// - public class SphereTriangleCollisionAlgorithm : CollisionAlgorithm, IDisposable - { - private bool _ownManifold; - private PersistentManifold _manifold; - private bool _isSwapped; - - public SphereTriangleCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped) - : base(collisionAlgorithmConstructionInfo) - { - _ownManifold = false; - _manifold = manifold; - _isSwapped = isSwapped; - - if (_manifold == null) - { - _manifold = Dispatcher.GetNewManifold(bodyA, bodyB); - _ownManifold = true; - } - } - - public SphereTriangleCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) - : base(collisionAlgorithmConstructionInfo) { } - - public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) - { - if (_manifold == null) - return; - - SphereShape sphere = bodyA.CollisionShape as SphereShape; - TriangleShape triangle = bodyB.CollisionShape as TriangleShape; - - /// report a contact. internally this will be kept persistent, and contact reduction is done - resultOut.SetPersistentManifold(_manifold); - SphereTriangleDetector detector = new SphereTriangleDetector(sphere, triangle); - - DiscreteCollisionDetectorInterface.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); - input.MaximumDistanceSquared = 1e30f;//todo: tighter bounds - input.TransformA = bodyA.WorldTransform; - input.TransformB = bodyB.WorldTransform; - - detector.GetClosestPoints(input, resultOut, null); - } - - public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) - { - //not yet - return 1f; - } - - public class CreateFunc : CollisionAlgorithmCreateFunction - { - public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) - { - return new SphereTriangleCollisionAlgorithm(collisionAlgorithmConstructionInfo.Manifold, collisionAlgorithmConstructionInfo, bodyA, bodyB, IsSwapped); - } - } - - #region IDisposable Members - public void Dispose() - { - if (_ownManifold) - if (_manifold != null) - Dispatcher.ReleaseManifold(_manifold); - } - #endregion - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// + /// SphereSphereCollisionAlgorithm provides sphere-sphere collision detection. + /// Other features are frame-coherency (persistent data) and collision response. + /// Also provides the most basic sample for custom/user btCollisionAlgorithm + /// + public class SphereTriangleCollisionAlgorithm : CollisionAlgorithm, IDisposable + { + private bool _ownManifold; + private PersistentManifold _manifold; + private bool _isSwapped; + + public SphereTriangleCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped) + : base(collisionAlgorithmConstructionInfo) + { + _ownManifold = false; + _manifold = manifold; + _isSwapped = isSwapped; + + if (_manifold == null) + { + _manifold = Dispatcher.GetNewManifold(bodyA, bodyB); + _ownManifold = true; + } + } + + public SphereTriangleCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) + : base(collisionAlgorithmConstructionInfo) { } + + public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) + { + if (_manifold == null) + return; + + SphereShape sphere = bodyA.CollisionShape as SphereShape; + TriangleShape triangle = bodyB.CollisionShape as TriangleShape; + + /// report a contact. internally this will be kept persistent, and contact reduction is done + resultOut.SetPersistentManifold(_manifold); + SphereTriangleDetector detector = new SphereTriangleDetector(sphere, triangle); + + DiscreteCollisionDetectorInterface.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); + input.MaximumDistanceSquared = 1e30f;//todo: tighter bounds + input.TransformA = bodyA.WorldTransform; + input.TransformB = bodyB.WorldTransform; + + detector.GetClosestPoints(input, resultOut, null); + } + + public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) + { + //not yet + return 1f; + } + + public class CreateFunc : CollisionAlgorithmCreateFunction + { + public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) + { + return new SphereTriangleCollisionAlgorithm(collisionAlgorithmConstructionInfo.Manifold, collisionAlgorithmConstructionInfo, bodyA, bodyB, IsSwapped); + } + } + + #region IDisposable Members + public void Dispose() + { + if (_ownManifold) + if (_manifold != null) + Dispatcher.ReleaseManifold(_manifold); + } + #endregion + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs index 865754a..4d578d8 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs @@ -1,214 +1,214 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public class SphereTriangleDetector : DiscreteCollisionDetectorInterface - { - private SphereShape _sphere; - private TriangleShape _triangle; - private const int MaxOverlap = 0; - - public SphereTriangleDetector(SphereShape sphere, TriangleShape triangle) - { - this._sphere = sphere; - this._triangle = triangle; - } - - public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw) - { - Matrix transformA = input.TransformA; - Matrix transformB = input.TransformB; - - Vector3 point = new Vector3(); - Vector3 normal = new Vector3(); - Single timeOfImpact = 1.0f; - Single depth = 0.0f; - - //move sphere into triangle space - Matrix sphereInTr = MathHelper.InverseTimes(transformB, transformA); - - if (Collide(sphereInTr.Translation, point, normal, depth, timeOfImpact)) - output.AddContactPoint(Vector3.TransformNormal(normal, transformB), Vector3.TransformNormal(point, transformB), depth); - } - - /// - /// See also geometrictools.com - /// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv - /// - /// - /// - /// - /// - /// - private float SegmentSquareDistance(Vector3 from, Vector3 to, Vector3 point, Vector3 nearest) - { - Vector3 diff = point - from; - Vector3 v = to - from; - float t = Vector3.Dot(v, diff); - - if (t > 0) - { - float dotVV = Vector3.Dot(v, v); - if (t < dotVV) - { - t /= dotVV; - diff -= t * v; - } - else - { - t = 1; - diff -= v; - } - } - else - t = 0; - - nearest = from + t * v; - return Vector3.Dot(diff, diff); - } - - private bool Collide(Vector3 sphereCenter, Vector3 point, Vector3 resultNormal, float depth, float timeOfImpact) - { - Vector3[] vertices = _triangle.Vertices; - Vector3 c = sphereCenter; - float r = _sphere.Radius; - - Vector3 delta = new Vector3(); - - Vector3 normal = Vector3.Cross(vertices[1] - vertices[0], vertices[2] - vertices[0]); - normal = Vector3.Normalize(normal); - Vector3 p1ToCentre = c - vertices[0]; - float distanceFromPlane = Vector3.Dot(p1ToCentre, normal); - - if (distanceFromPlane < 0) - { - //triangle facing the other way - distanceFromPlane *= -1; - normal *= -1; - } - - float contactMargin = PersistentManifold.ContactBreakingThreshold; - bool isInsideContactPlane = distanceFromPlane < r + contactMargin; - bool isInsideShellPlane = distanceFromPlane < r; - - float deltaDotNormal = Vector3.Dot(delta, normal); - if (!isInsideShellPlane && deltaDotNormal >= 0.0f) - return false; - - // Check for contact / intersection - bool hasContact = false; - Vector3 contactPoint = new Vector3(); - if (isInsideContactPlane) - { - if (FaceContains(c, vertices, normal)) - { - // Inside the contact wedge - touches a point on the shell plane - hasContact = true; - contactPoint = c - normal * distanceFromPlane; - } - else - { - // Could be inside one of the contact capsules - float contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin); - Vector3 nearestOnEdge = new Vector3(); - for (int i = 0; i < _triangle.EdgeCount; i++) - { - Vector3 pa, pb; - _triangle.GetEdge(i, out pa, out pb); - - float distanceSqr = SegmentSquareDistance(pa, pb, c, nearestOnEdge); - if (distanceSqr < contactCapsuleRadiusSqr) - { - // Yep, we're inside a capsule - hasContact = true; - contactPoint = nearestOnEdge; - } - } - } - } - - if (hasContact) - { - Vector3 contactToCentre = c - contactPoint; - float distanceSqr = contactToCentre.LengthSquared(); - if (distanceSqr < (r - MaxOverlap) * (r - MaxOverlap)) - { - float distance = (float)Math.Sqrt(distanceSqr); - resultNormal = contactToCentre; - resultNormal = Vector3.Normalize(resultNormal); - point = contactPoint; - depth = -(r - distance); - return true; - } - - if (Vector3.Dot(delta, contactToCentre) >= 0.0f) - return false; - - // Moving towards the contact point -> collision - point = contactPoint; - timeOfImpact = 0.0f; - return true; - } - return false; - } - - private bool PointInTriangle(Vector3[] vertices, Vector3 normal, Vector3 p) - { - Vector3 p1 = vertices[0]; - Vector3 p2 = vertices[1]; - Vector3 p3 = vertices[2]; - - Vector3 edge1 = p2 - p1; - Vector3 edge2 = p3 - p2; - Vector3 edge3 = p1 - p3; - - Vector3 p1ToP = p - p1; - Vector3 p2ToP = p - p2; - Vector3 p3ToP = p - p3; - - Vector3 edge1Normal = Vector3.Cross(edge1, normal); - Vector3 edge2Normal = Vector3.Cross(edge2, normal); - Vector3 edge3Normal = Vector3.Cross(edge3, normal); - - float r1, r2, r3; - r1 = Vector3.Dot(edge1Normal, p1ToP); - r2 = Vector3.Dot(edge2Normal, p2ToP); - r3 = Vector3.Dot(edge3Normal, p3ToP); - if ((r1 > 0 && r2 > 0 && r3 > 0) || - (r1 <= 0 && r2 <= 0 && r3 <= 0)) - return true; - return false; - } - - private bool FaceContains(Vector3 p, Vector3[] vertices, Vector3 normal) - { - Vector3 lp = p; - Vector3 lnormal = normal; - return PointInTriangle(vertices, lnormal, lp); - } - } +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public class SphereTriangleDetector : DiscreteCollisionDetectorInterface + { + private SphereShape _sphere; + private TriangleShape _triangle; + private const int MaxOverlap = 0; + + public SphereTriangleDetector(SphereShape sphere, TriangleShape triangle) + { + this._sphere = sphere; + this._triangle = triangle; + } + + public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw) + { + Matrix transformA = input.TransformA; + Matrix transformB = input.TransformB; + + Vector3 point = new Vector3(); + Vector3 normal = new Vector3(); + Single timeOfImpact = 1.0f; + Single depth = 0.0f; + + //move sphere into triangle space + Matrix sphereInTr = MathHelper.InverseTimes(transformB, transformA); + + if (Collide(sphereInTr.Translation, point, normal, depth, timeOfImpact)) + output.AddContactPoint(Vector3.TransformNormal(normal, transformB), Vector3.TransformNormal(point, transformB), depth); + } + + /// + /// See also geometrictools.com + /// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv + /// + /// + /// + /// + /// + /// + private float SegmentSquareDistance(Vector3 from, Vector3 to, Vector3 point, Vector3 nearest) + { + Vector3 diff = point - from; + Vector3 v = to - from; + float t = Vector3.Dot(v, diff); + + if (t > 0) + { + float dotVV = Vector3.Dot(v, v); + if (t < dotVV) + { + t /= dotVV; + diff -= t * v; + } + else + { + t = 1; + diff -= v; + } + } + else + t = 0; + + nearest = from + t * v; + return Vector3.Dot(diff, diff); + } + + private bool Collide(Vector3 sphereCenter, Vector3 point, Vector3 resultNormal, float depth, float timeOfImpact) + { + Vector3[] vertices = _triangle.Vertices; + Vector3 c = sphereCenter; + float r = _sphere.Radius; + + Vector3 delta = new Vector3(); + + Vector3 normal = Vector3.Cross(vertices[1] - vertices[0], vertices[2] - vertices[0]); + normal = Vector3.Normalize(normal); + Vector3 p1ToCentre = c - vertices[0]; + float distanceFromPlane = Vector3.Dot(p1ToCentre, normal); + + if (distanceFromPlane < 0) + { + //triangle facing the other way + distanceFromPlane *= -1; + normal *= -1; + } + + float contactMargin = PersistentManifold.ContactBreakingThreshold; + bool isInsideContactPlane = distanceFromPlane < r + contactMargin; + bool isInsideShellPlane = distanceFromPlane < r; + + float deltaDotNormal = Vector3.Dot(delta, normal); + if (!isInsideShellPlane && deltaDotNormal >= 0.0f) + return false; + + // Check for contact / intersection + bool hasContact = false; + Vector3 contactPoint = new Vector3(); + if (isInsideContactPlane) + { + if (FaceContains(c, vertices, normal)) + { + // Inside the contact wedge - touches a point on the shell plane + hasContact = true; + contactPoint = c - normal * distanceFromPlane; + } + else + { + // Could be inside one of the contact capsules + float contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin); + Vector3 nearestOnEdge = new Vector3(); + for (int i = 0; i < _triangle.EdgeCount; i++) + { + Vector3 pa, pb; + _triangle.GetEdge(i, out pa, out pb); + + float distanceSqr = SegmentSquareDistance(pa, pb, c, nearestOnEdge); + if (distanceSqr < contactCapsuleRadiusSqr) + { + // Yep, we're inside a capsule + hasContact = true; + contactPoint = nearestOnEdge; + } + } + } + } + + if (hasContact) + { + Vector3 contactToCentre = c - contactPoint; + float distanceSqr = contactToCentre.LengthSquared(); + if (distanceSqr < (r - MaxOverlap) * (r - MaxOverlap)) + { + float distance = (float)Math.Sqrt(distanceSqr); + resultNormal = contactToCentre; + resultNormal = Vector3.Normalize(resultNormal); + point = contactPoint; + depth = -(r - distance); + return true; + } + + if (Vector3.Dot(delta, contactToCentre) >= 0.0f) + return false; + + // Moving towards the contact point -> collision + point = contactPoint; + timeOfImpact = 0.0f; + return true; + } + return false; + } + + private bool PointInTriangle(Vector3[] vertices, Vector3 normal, Vector3 p) + { + Vector3 p1 = vertices[0]; + Vector3 p2 = vertices[1]; + Vector3 p3 = vertices[2]; + + Vector3 edge1 = p2 - p1; + Vector3 edge2 = p3 - p2; + Vector3 edge3 = p1 - p3; + + Vector3 p1ToP = p - p1; + Vector3 p2ToP = p - p2; + Vector3 p3ToP = p - p3; + + Vector3 edge1Normal = Vector3.Cross(edge1, normal); + Vector3 edge2Normal = Vector3.Cross(edge2, normal); + Vector3 edge3Normal = Vector3.Cross(edge3, normal); + + float r1, r2, r3; + r1 = Vector3.Dot(edge1Normal, p1ToP); + r2 = Vector3.Dot(edge2Normal, p2ToP); + r3 = Vector3.Dot(edge3Normal, p3ToP); + if ((r1 > 0 && r2 > 0 && r3 > 0) || + (r1 <= 0 && r2 <= 0 && r3 <= 0)) + return true; + return false; + } + + private bool FaceContains(Vector3 p, Vector3[] vertices, Vector3 normal) + { + Vector3 lp = p; + Vector3 lnormal = normal; + return PointInTriangle(vertices, lnormal, lp); + } + } } \ No newline at end of file diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/UnionFind.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/UnionFind.cs index a825fad..3c7daea 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/UnionFind.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/UnionFind.cs @@ -1,151 +1,151 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; - -namespace XnaDevRu.BulletX -{ - public class UnionFind : IDisposable - { - private List _elements = new List(); - - public int ElementCount - { - get { return _elements.Count; } - } - - public void SortIslands() - { - for (int i = 0; i < _elements.Count; i++) - { - _elements[i].ID = Find(i); - _elements[i].Size = i; - } - - _elements.Sort(Sort); - } - - private static int Sort(Element x, Element y) - { - if (x.ID < y.ID) return -1; - //else if (x.ID > y.ID) return 1; - else return 0; - } - - public void Reset(int number) - { - Allocate(number); - - for (int i = 0; i < number; i++) - { - Element element = new Element(); - element.ID = i; - element.Size = 1; - _elements.Insert(i, element); - } - } - - public bool IsRoot(int index) - { - return (_elements[index].Size == index); - } - - public Element this[int index] - { - get { return _elements[index]; } - } - - public void Allocate(int number) - { - //Does nothing - _elements = new List(number); - } - - public bool Find(int i, int j) - { - return (Find(i) == Find(j)); - } - - public int Find(int i) - { - while (i != _elements[i].ID) - { - //Element element = _elements[i]; - //element.ID = _elements[_elements[i].ID].ID; - _elements[i].ID = _elements[_elements[i].ID].ID; - i = _elements[i].ID; - } - - return i; - } - - public void Unite(int p, int q) - { - int i = Find(p), j = Find(q); - if (i == j) - return; - - //weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) ) - //if (_elements[i].Size < _elements[j].Size) - //{ - // Element element = _elements[i]; - // element.ID = j; - // _elements[i] = element; - - // element = _elements[j]; - // element.Size += _elements[i].Size; - // _elements[j] = element; - //} - //else - //{ - // Element element = _elements[j]; - // element.ID = i; - // _elements[j] = element; - - // element = _elements[i]; - // element.Size += _elements[j].Size; - // _elements[i] = element; - //} - _elements[i].ID = j; - _elements[j].Size += _elements[i].Size; - } - - #region IDisposable Members - - public void Dispose() - { - _elements.Clear(); - } - - #endregion - } - - public class Element - { - private int _id; - private int _size; - - public int ID { get { return _id; } set { _id = value; } } - public int Size { get { return _size; } set { _size = value; } } - } +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace XnaDevRu.BulletX +{ + public class UnionFind : IDisposable + { + private List _elements = new List(); + + public int ElementCount + { + get { return _elements.Count; } + } + + public void SortIslands() + { + for (int i = 0; i < _elements.Count; i++) + { + _elements[i].ID = Find(i); + _elements[i].Size = i; + } + + _elements.Sort(Sort); + } + + private static int Sort(Element x, Element y) + { + if (x.ID < y.ID) return -1; + //else if (x.ID > y.ID) return 1; + else return 0; + } + + public void Reset(int number) + { + Allocate(number); + + for (int i = 0; i < number; i++) + { + Element element = new Element(); + element.ID = i; + element.Size = 1; + _elements.Insert(i, element); + } + } + + public bool IsRoot(int index) + { + return (_elements[index].Size == index); + } + + public Element this[int index] + { + get { return _elements[index]; } + } + + public void Allocate(int number) + { + //Does nothing + _elements = new List(number); + } + + public bool Find(int i, int j) + { + return (Find(i) == Find(j)); + } + + public int Find(int i) + { + while (i != _elements[i].ID) + { + //Element element = _elements[i]; + //element.ID = _elements[_elements[i].ID].ID; + _elements[i].ID = _elements[_elements[i].ID].ID; + i = _elements[i].ID; + } + + return i; + } + + public void Unite(int p, int q) + { + int i = Find(p), j = Find(q); + if (i == j) + return; + + //weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) ) + //if (_elements[i].Size < _elements[j].Size) + //{ + // Element element = _elements[i]; + // element.ID = j; + // _elements[i] = element; + + // element = _elements[j]; + // element.Size += _elements[i].Size; + // _elements[j] = element; + //} + //else + //{ + // Element element = _elements[j]; + // element.ID = i; + // _elements[j] = element; + + // element = _elements[i]; + // element.Size += _elements[j].Size; + // _elements[i] = element; + //} + _elements[i].ID = j; + _elements[j].Size += _elements[i].Size; + } + + #region IDisposable Members + + public void Dispose() + { + _elements.Clear(); + } + + #endregion + } + + public class Element + { + private int _id; + private int _size; + + public int ID { get { return _id; } set { _id = value; } } + public int Size { get { return _size; } set { _size = value; } } + } } \ No newline at end of file diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BUSimplex1to4.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BUSimplex1to4.cs index b75c2b6..34f7442 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BUSimplex1to4.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BUSimplex1to4.cs @@ -1,215 +1,215 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// - /// BUSimplex1to4 implements feature based and implicit simplex of up to 4 vertices (tetrahedron, triangle, line, vertex). - /// - public class BUSimplex1to4 : PolyhedralConvexShape - { - private int _numVertices = 0; - private Vector3[] _vertices = new Vector3[4]; - - public BUSimplex1to4() { } - - public BUSimplex1to4(Vector3 pointA) - { - AddVertex(pointA); - } - - public BUSimplex1to4(Vector3 pointA, Vector3 pointB) - { - AddVertex(pointA); - AddVertex(pointB); - } - - public BUSimplex1to4(Vector3 pointA, Vector3 pointB, Vector3 pointC) - { - AddVertex(pointA); - AddVertex(pointB); - AddVertex(pointC); - } - - public BUSimplex1to4(Vector3 pointA, Vector3 pointB, Vector3 pointC, Vector3 pointD) - { - AddVertex(pointA); - AddVertex(pointB); - AddVertex(pointC); - AddVertex(pointD); - } - - protected Vector3[] Vertices { get { return _vertices; } set { _vertices = value; } } - - public override int VertexCount - { - get - { - return _numVertices; - } - } - - public override int EdgeCount - { - get - { - //euler formula, F-E+V = 2, so E = F+V-2 - switch (_numVertices) - { - case 0: return 0; - case 1: return 0; - case 2: return 1; - case 3: return 3; - case 4: return 6; - } - return 0; - } - } - - public override int PlaneCount - { - get - { - switch (_numVertices) - { - case 0: - return 0; - case 1: - return 0; - case 2: - return 0; - case 3: - return 2; - case 4: - return 4; - } - return 0; - } - } - - public override BroadphaseNativeTypes ShapeType - { - get - { - return BroadphaseNativeTypes.Tetrahedral; - } - } - - public override string Name - { - get - { - return "BUSimplex1to4"; - } - } - - public void AddVertex(Vector3 v) - { - _vertices[_numVertices++] = v; - } - - public void Reset() - { - _numVertices = 0; - } - - public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) - { - switch (_numVertices) - { - case 2: - pa = _vertices[0]; - pb = _vertices[1]; - return; - case 3: - switch (i) - { - case 0: - pa = _vertices[0]; - pb = _vertices[1]; - return; - case 1: - pa = _vertices[1]; - pb = _vertices[2]; - return; - case 2: - pa = _vertices[2]; - pb = _vertices[0]; - return; - } - break; - case 4: - switch (i) - { - case 0: - pa = _vertices[0]; - pb = _vertices[1]; - return; - case 1: - pa = _vertices[1]; - pb = _vertices[2]; - return; - case 2: - pa = _vertices[2]; - pb = _vertices[0]; - return; - case 3: - pa = _vertices[0]; - pb = _vertices[3]; - return; - case 4: - pa = _vertices[1]; - pb = _vertices[3]; - return; - case 5: - pa = _vertices[2]; - pb = _vertices[3]; - return; - } - break; - } - - pa = new Vector3(); - pb = new Vector3(); - } - - public override void GetVertex(int i, out Vector3 vtx) - { - vtx = _vertices[i]; - } - - public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) - { - planeNormal = new Vector3(); - planeSupport = new Vector3(); - } - - public override bool IsInside(Vector3 pt, float tolerance) - { - return false; - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// + /// BUSimplex1to4 implements feature based and implicit simplex of up to 4 vertices (tetrahedron, triangle, line, vertex). + /// + public class BUSimplex1to4 : PolyhedralConvexShape + { + private int _numVertices = 0; + private Vector3[] _vertices = new Vector3[4]; + + public BUSimplex1to4() { } + + public BUSimplex1to4(Vector3 pointA) + { + AddVertex(pointA); + } + + public BUSimplex1to4(Vector3 pointA, Vector3 pointB) + { + AddVertex(pointA); + AddVertex(pointB); + } + + public BUSimplex1to4(Vector3 pointA, Vector3 pointB, Vector3 pointC) + { + AddVertex(pointA); + AddVertex(pointB); + AddVertex(pointC); + } + + public BUSimplex1to4(Vector3 pointA, Vector3 pointB, Vector3 pointC, Vector3 pointD) + { + AddVertex(pointA); + AddVertex(pointB); + AddVertex(pointC); + AddVertex(pointD); + } + + protected Vector3[] Vertices { get { return _vertices; } set { _vertices = value; } } + + public override int VertexCount + { + get + { + return _numVertices; + } + } + + public override int EdgeCount + { + get + { + //euler formula, F-E+V = 2, so E = F+V-2 + switch (_numVertices) + { + case 0: return 0; + case 1: return 0; + case 2: return 1; + case 3: return 3; + case 4: return 6; + } + return 0; + } + } + + public override int PlaneCount + { + get + { + switch (_numVertices) + { + case 0: + return 0; + case 1: + return 0; + case 2: + return 0; + case 3: + return 2; + case 4: + return 4; + } + return 0; + } + } + + public override BroadphaseNativeTypes ShapeType + { + get + { + return BroadphaseNativeTypes.Tetrahedral; + } + } + + public override string Name + { + get + { + return "BUSimplex1to4"; + } + } + + public void AddVertex(Vector3 v) + { + _vertices[_numVertices++] = v; + } + + public void Reset() + { + _numVertices = 0; + } + + public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) + { + switch (_numVertices) + { + case 2: + pa = _vertices[0]; + pb = _vertices[1]; + return; + case 3: + switch (i) + { + case 0: + pa = _vertices[0]; + pb = _vertices[1]; + return; + case 1: + pa = _vertices[1]; + pb = _vertices[2]; + return; + case 2: + pa = _vertices[2]; + pb = _vertices[0]; + return; + } + break; + case 4: + switch (i) + { + case 0: + pa = _vertices[0]; + pb = _vertices[1]; + return; + case 1: + pa = _vertices[1]; + pb = _vertices[2]; + return; + case 2: + pa = _vertices[2]; + pb = _vertices[0]; + return; + case 3: + pa = _vertices[0]; + pb = _vertices[3]; + return; + case 4: + pa = _vertices[1]; + pb = _vertices[3]; + return; + case 5: + pa = _vertices[2]; + pb = _vertices[3]; + return; + } + break; + } + + pa = new Vector3(); + pb = new Vector3(); + } + + public override void GetVertex(int i, out Vector3 vtx) + { + vtx = _vertices[i]; + } + + public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) + { + planeNormal = new Vector3(); + planeSupport = new Vector3(); + } + + public override bool IsInside(Vector3 pt, float tolerance) + { + return false; + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs index ba6f3b7..7fab5c7 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs @@ -1,316 +1,316 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public class BoxShape : PolyhedralConvexShape - { - public BoxShape(Vector3 boxHalfExtents) - { - ImplicitShapeDimensions = boxHalfExtents; - } - - public override int VertexCount - { - get - { - return 8; - } - } - - public override int EdgeCount - { - get - { - return 12; - } - } - - public override BroadphaseNativeTypes ShapeType - { - get - { - return BroadphaseNativeTypes.Box; - } - } - - public override string Name - { - get - { - return "Box"; - } - } - - public override int PreferredPenetrationDirectionsCount - { - get - { - return 6; - } - } - - public override int PlaneCount - { - get - { - return 6; - } - } - - public Vector3 HalfExtents { get { return ImplicitShapeDimensions * LocalScaling; } } - - public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) - { - int edgeVert0 = 0; - int edgeVert1 = 0; - - switch (i) - { - case 0: - edgeVert0 = 0; - edgeVert1 = 1; - break; - case 1: - edgeVert0 = 0; - edgeVert1 = 2; - break; - case 2: - edgeVert0 = 1; - edgeVert1 = 3; - - break; - case 3: - edgeVert0 = 2; - edgeVert1 = 3; - break; - case 4: - edgeVert0 = 0; - edgeVert1 = 4; - break; - case 5: - edgeVert0 = 1; - edgeVert1 = 5; - - break; - case 6: - edgeVert0 = 2; - edgeVert1 = 6; - break; - case 7: - edgeVert0 = 3; - edgeVert1 = 7; - break; - case 8: - edgeVert0 = 4; - edgeVert1 = 5; - break; - case 9: - edgeVert0 = 4; - edgeVert1 = 6; - break; - case 10: - edgeVert0 = 5; - edgeVert1 = 7; - break; - case 11: - edgeVert0 = 6; - edgeVert1 = 7; - break; - default: - throw new BulletException(); - - } - - GetVertex(edgeVert0, out pa); - GetVertex(edgeVert1, out pb); - } - - public override void GetVertex(int i, out Vector3 vtx) - { - Vector3 halfExtents = HalfExtents; - - vtx = new Vector3( - halfExtents.X * (1 - (i & 1)) - halfExtents.X * (i & 1), - halfExtents.Y * (1 - ((i & 2) >> 1)) - halfExtents.Y * ((i & 2) >> 1), - halfExtents.Z * (1 - ((i & 4) >> 2)) - halfExtents.Z * ((i & 4) >> 2)); - } - - public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) - { - //this plane might not be aligned... - Vector4 plane; - GetPlaneEquation(out plane, i); - planeNormal = new Vector3(plane.X, plane.Y, plane.Z); - planeSupport = LocalGetSupportingVertex(-planeNormal); - } - - public override bool IsInside(Vector3 pt, float tolerance) - { - Vector3 halfExtents = HalfExtents; - - //btScalar minDist = 2*tolerance; - - bool result = (pt.X <= ( halfExtents.X + tolerance)) && - (pt.X >= (-halfExtents.X - tolerance)) && - (pt.Y <= ( halfExtents.Y + tolerance)) && - (pt.Y >= (-halfExtents.Y - tolerance)) && - (pt.Z <= ( halfExtents.Z + tolerance)) && - (pt.Z >= (-halfExtents.Z - tolerance)); - - return result; - } - - public override Vector3 LocalGetSupportingVertex(Vector3 vec) - { - Vector3 halfExtents = HalfExtents; - - return new Vector3( vec.X < 0.0f ? -halfExtents.X : halfExtents.X, - vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y, - vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z); - } - - public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) - { - Vector3 halfExtents = HalfExtents; - Vector3 margin = new Vector3(Margin, Margin, Margin); - halfExtents -= margin; - - return new Vector3( vec.X < 0.0f ? -halfExtents.X : halfExtents.X, - vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y, - vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z); - } - - public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) - { - Vector3 halfExtents = HalfExtents; - Vector3 margin = new Vector3(Margin, Margin, Margin); - halfExtents -= margin; - - for (int i = 0; i < vectors.Length; i++) - { - Vector3 vec = vectors[i]; - supportVerticesOut[i] = new Vector3(vec.X < 0.0f ? -halfExtents.X : halfExtents.X, - vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y, - vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z); - } - } - - public virtual void GetPlaneEquation(out Vector4 plane, int i) - { - Vector3 halfExtents = HalfExtents; - - switch (i) - { - case 0: - plane = new Vector4(1, 0, 0, 0); - plane.W = -halfExtents.X; - break; - case 1: - plane = new Vector4(-1, 0, 0, 0); - plane.W = -halfExtents.X; - break; - case 2: - plane = new Vector4(0, 1, 0, 0); - plane.W = -halfExtents.Y; - break; - case 3: - plane = new Vector4(0, -1, 0, 0); - plane.W = -halfExtents.Y; - break; - case 4: - plane = new Vector4(0, 0, 1, 0); - plane.W = -halfExtents.Z; - break; - case 5: - plane = new Vector4(0, 0, -1, 0); - plane.W = -halfExtents.Z; - break; - default: - throw new BulletException(); - } - } - - public override void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector) - { - switch (index) - { - case 0: - penetrationVector = new Vector3(1, 0, 0); - break; - case 1: - penetrationVector = new Vector3(-1, 0, 0); - break; - case 2: - penetrationVector = new Vector3(0, 1, 0); - break; - case 3: - penetrationVector = new Vector3(0, -1, 0); - break; - case 4: - penetrationVector = new Vector3(0, 0, 1); - break; - case 5: - penetrationVector = new Vector3(0, 0, -1); - break; - default: - throw new BulletException(); - } - } - - public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) - { - Vector3 halfExtents = HalfExtents; - - Matrix abs_b = MathHelper.Absolute(t); - Vector3 center = t.Translation; - Vector3 row1 = new Vector3(abs_b.M11, abs_b.M12, abs_b.M13); - Vector3 row2 = new Vector3(abs_b.M21, abs_b.M22, abs_b.M23); - Vector3 row3 = new Vector3(abs_b.M31, abs_b.M32, abs_b.M33); - Vector3 extent = new Vector3(Vector3.Dot(row1, halfExtents), - Vector3.Dot(row2, halfExtents), - Vector3.Dot(row3, halfExtents)); - extent += new Vector3(Margin, Margin, Margin); - - aabbMin = center - extent; - aabbMax = center + extent; - } - - public override void CalculateLocalInertia(float mass, out Vector3 inertia) - { - Vector3 halfExtents = HalfExtents; - - float lx = 2f * (halfExtents.X); - float ly = 2f * (halfExtents.Y); - float lz = 2f * (halfExtents.Z); - - inertia = new Vector3(); - inertia.X = mass / (12.0f) * (ly * ly + lz * lz); - inertia.Y = mass / (12.0f) * (lx * lx + lz * lz); - inertia.Z = mass / (12.0f) * (lx * lx + ly * ly); - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public class BoxShape : PolyhedralConvexShape + { + public BoxShape(Vector3 boxHalfExtents) + { + ImplicitShapeDimensions = boxHalfExtents; + } + + public override int VertexCount + { + get + { + return 8; + } + } + + public override int EdgeCount + { + get + { + return 12; + } + } + + public override BroadphaseNativeTypes ShapeType + { + get + { + return BroadphaseNativeTypes.Box; + } + } + + public override string Name + { + get + { + return "Box"; + } + } + + public override int PreferredPenetrationDirectionsCount + { + get + { + return 6; + } + } + + public override int PlaneCount + { + get + { + return 6; + } + } + + public Vector3 HalfExtents { get { return ImplicitShapeDimensions * LocalScaling; } } + + public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) + { + int edgeVert0 = 0; + int edgeVert1 = 0; + + switch (i) + { + case 0: + edgeVert0 = 0; + edgeVert1 = 1; + break; + case 1: + edgeVert0 = 0; + edgeVert1 = 2; + break; + case 2: + edgeVert0 = 1; + edgeVert1 = 3; + + break; + case 3: + edgeVert0 = 2; + edgeVert1 = 3; + break; + case 4: + edgeVert0 = 0; + edgeVert1 = 4; + break; + case 5: + edgeVert0 = 1; + edgeVert1 = 5; + + break; + case 6: + edgeVert0 = 2; + edgeVert1 = 6; + break; + case 7: + edgeVert0 = 3; + edgeVert1 = 7; + break; + case 8: + edgeVert0 = 4; + edgeVert1 = 5; + break; + case 9: + edgeVert0 = 4; + edgeVert1 = 6; + break; + case 10: + edgeVert0 = 5; + edgeVert1 = 7; + break; + case 11: + edgeVert0 = 6; + edgeVert1 = 7; + break; + default: + throw new BulletException(); + + } + + GetVertex(edgeVert0, out pa); + GetVertex(edgeVert1, out pb); + } + + public override void GetVertex(int i, out Vector3 vtx) + { + Vector3 halfExtents = HalfExtents; + + vtx = new Vector3( + halfExtents.X * (1 - (i & 1)) - halfExtents.X * (i & 1), + halfExtents.Y * (1 - ((i & 2) >> 1)) - halfExtents.Y * ((i & 2) >> 1), + halfExtents.Z * (1 - ((i & 4) >> 2)) - halfExtents.Z * ((i & 4) >> 2)); + } + + public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) + { + //this plane might not be aligned... + Vector4 plane; + GetPlaneEquation(out plane, i); + planeNormal = new Vector3(plane.X, plane.Y, plane.Z); + planeSupport = LocalGetSupportingVertex(-planeNormal); + } + + public override bool IsInside(Vector3 pt, float tolerance) + { + Vector3 halfExtents = HalfExtents; + + //btScalar minDist = 2*tolerance; + + bool result = (pt.X <= ( halfExtents.X + tolerance)) && + (pt.X >= (-halfExtents.X - tolerance)) && + (pt.Y <= ( halfExtents.Y + tolerance)) && + (pt.Y >= (-halfExtents.Y - tolerance)) && + (pt.Z <= ( halfExtents.Z + tolerance)) && + (pt.Z >= (-halfExtents.Z - tolerance)); + + return result; + } + + public override Vector3 LocalGetSupportingVertex(Vector3 vec) + { + Vector3 halfExtents = HalfExtents; + + return new Vector3( vec.X < 0.0f ? -halfExtents.X : halfExtents.X, + vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y, + vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z); + } + + public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) + { + Vector3 halfExtents = HalfExtents; + Vector3 margin = new Vector3(Margin, Margin, Margin); + halfExtents -= margin; + + return new Vector3( vec.X < 0.0f ? -halfExtents.X : halfExtents.X, + vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y, + vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z); + } + + public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) + { + Vector3 halfExtents = HalfExtents; + Vector3 margin = new Vector3(Margin, Margin, Margin); + halfExtents -= margin; + + for (int i = 0; i < vectors.Length; i++) + { + Vector3 vec = vectors[i]; + supportVerticesOut[i] = new Vector3(vec.X < 0.0f ? -halfExtents.X : halfExtents.X, + vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y, + vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z); + } + } + + public virtual void GetPlaneEquation(out Vector4 plane, int i) + { + Vector3 halfExtents = HalfExtents; + + switch (i) + { + case 0: + plane = new Vector4(1, 0, 0, 0); + plane.W = -halfExtents.X; + break; + case 1: + plane = new Vector4(-1, 0, 0, 0); + plane.W = -halfExtents.X; + break; + case 2: + plane = new Vector4(0, 1, 0, 0); + plane.W = -halfExtents.Y; + break; + case 3: + plane = new Vector4(0, -1, 0, 0); + plane.W = -halfExtents.Y; + break; + case 4: + plane = new Vector4(0, 0, 1, 0); + plane.W = -halfExtents.Z; + break; + case 5: + plane = new Vector4(0, 0, -1, 0); + plane.W = -halfExtents.Z; + break; + default: + throw new BulletException(); + } + } + + public override void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector) + { + switch (index) + { + case 0: + penetrationVector = new Vector3(1, 0, 0); + break; + case 1: + penetrationVector = new Vector3(-1, 0, 0); + break; + case 2: + penetrationVector = new Vector3(0, 1, 0); + break; + case 3: + penetrationVector = new Vector3(0, -1, 0); + break; + case 4: + penetrationVector = new Vector3(0, 0, 1); + break; + case 5: + penetrationVector = new Vector3(0, 0, -1); + break; + default: + throw new BulletException(); + } + } + + public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) + { + Vector3 halfExtents = HalfExtents; + + Matrix abs_b = MathHelper.Absolute(t); + Vector3 center = t.Translation; + Vector3 row1 = new Vector3(abs_b.M11, abs_b.M12, abs_b.M13); + Vector3 row2 = new Vector3(abs_b.M21, abs_b.M22, abs_b.M23); + Vector3 row3 = new Vector3(abs_b.M31, abs_b.M32, abs_b.M33); + Vector3 extent = new Vector3(Vector3.Dot(row1, halfExtents), + Vector3.Dot(row2, halfExtents), + Vector3.Dot(row3, halfExtents)); + extent += new Vector3(Margin, Margin, Margin); + + aabbMin = center - extent; + aabbMax = center + extent; + } + + public override void CalculateLocalInertia(float mass, out Vector3 inertia) + { + Vector3 halfExtents = HalfExtents; + + float lx = 2f * (halfExtents.X); + float ly = 2f * (halfExtents.Y); + float lz = 2f * (halfExtents.Z); + + inertia = new Vector3(); + inertia.X = mass / (12.0f) * (ly * ly + lz * lz); + inertia.Y = mass / (12.0f) * (lx * lx + lz * lz); + inertia.Z = mass / (12.0f) * (lx * lx + ly * ly); + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs index 00247a0..03e1e44 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs @@ -1,83 +1,83 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - class MyNodeOverlapCallback : INodeOverlapCallback - { - StridingMeshInterface _meshInterface; - ITriangleCallback _callback; - Vector3[] _triangle = new Vector3[3]; - - public MyNodeOverlapCallback(ITriangleCallback callback, StridingMeshInterface meshInterface) - { - _meshInterface = meshInterface; - _callback = callback; - } - - public void ProcessNode(OptimizedBvhNode node) - { - List verts; - List indicies; - int numtriangles; - - _meshInterface.GetLockedReadOnlyVertexIndexBase(out verts, out indicies, out numtriangles, node.SubPart); - Vector3 meshScaling = _meshInterface.Scaling; - - for (int j = 0; j < 3; j++) - { - _triangle[j] = verts[indicies[j + node.TriangleIndex * 3]] * meshScaling; - } - - _callback.ProcessTriangle(_triangle, node.SubPart, node.TriangleIndex); - _meshInterface.UnLockReadOnlyVertexBase(node.SubPart); - } - } - - public class BvhTriangleMeshShape : TriangleMeshShape - { - OptimizedBvh _bvh = new OptimizedBvh(); - - public BvhTriangleMeshShape(StridingMeshInterface meshInterface) : base(meshInterface) - { - _bvh.Build(meshInterface); - } - - public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) - { - MyNodeOverlapCallback myNodeCallback = new MyNodeOverlapCallback(callback, MeshInterface); - _bvh.ReportAabbOverlappingNodex(myNodeCallback, aabbMin, aabbMax); - } - - public override string Name - { - get - { - return "BvhTriangleMesh"; - } - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + class MyNodeOverlapCallback : INodeOverlapCallback + { + StridingMeshInterface _meshInterface; + ITriangleCallback _callback; + Vector3[] _triangle = new Vector3[3]; + + public MyNodeOverlapCallback(ITriangleCallback callback, StridingMeshInterface meshInterface) + { + _meshInterface = meshInterface; + _callback = callback; + } + + public void ProcessNode(OptimizedBvhNode node) + { + List verts; + List indicies; + int numtriangles; + + _meshInterface.GetLockedReadOnlyVertexIndexBase(out verts, out indicies, out numtriangles, node.SubPart); + Vector3 meshScaling = _meshInterface.Scaling; + + for (int j = 0; j < 3; j++) + { + _triangle[j] = verts[indicies[j + node.TriangleIndex * 3]] * meshScaling; + } + + _callback.ProcessTriangle(_triangle, node.SubPart, node.TriangleIndex); + _meshInterface.UnLockReadOnlyVertexBase(node.SubPart); + } + } + + public class BvhTriangleMeshShape : TriangleMeshShape + { + OptimizedBvh _bvh = new OptimizedBvh(); + + public BvhTriangleMeshShape(StridingMeshInterface meshInterface) : base(meshInterface) + { + _bvh.Build(meshInterface); + } + + public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) + { + MyNodeOverlapCallback myNodeCallback = new MyNodeOverlapCallback(callback, MeshInterface); + _bvh.ReportAabbOverlappingNodex(myNodeCallback, aabbMin, aabbMax); + } + + public override string Name + { + get + { + return "BvhTriangleMesh"; + } + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs index a9ce3be..1a4fac2 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs @@ -1,148 +1,148 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// - /// CollisionShape provides generic interface for collidable objects - /// - public abstract class CollisionShape - { - //debugging support - private string _tempDebug; - - public abstract string Name { get; } - public string ExtraDebugInfo { get { return _tempDebug; } set { _tempDebug = value; } } - - public bool IsPolyhedral - { - get - { - return BroadphaseProxy.IsPolyhedral(ShapeType); - } - } - - public bool IsConvex - { - get - { - return BroadphaseProxy.IsConvex(ShapeType); - } - } - public bool IsConcave - { - get - { - return BroadphaseProxy.IsConcave(ShapeType); - } - } - public bool IsCompound - { - get - { - return BroadphaseProxy.IsCompound(ShapeType); - } - } - - //isInfinite is used to catch simulation error (aabb check) - public bool IsInfinite - { - get - { - return BroadphaseProxy.IsInfinite(ShapeType); - } - } - - public abstract float Margin { get; set; } - public abstract Vector3 LocalScaling { get; set; } - public abstract BroadphaseNativeTypes ShapeType { get; } - - - public virtual void GetBoundingSphere(out Vector3 center, out float radius) - { - Matrix tr = Matrix.Identity; - Vector3 aabbMin, aabbMax; - - GetAabb(tr, out aabbMin, out aabbMax); - - radius = (aabbMax - aabbMin).Length() * 0.5f; - center = (aabbMin + aabbMax) * 0.5f; - } - - public virtual float GetAngularMotionDisc() - { - Vector3 center; - float disc; - GetBoundingSphere(out center, out disc); - disc += center.Length(); - return disc; - } - - //calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep) - //result is conservative - public void CalculateTemporalAabb(Matrix currentTransform, Vector3 linearVelocity, Vector3 angularVelocity, float timeStep, out Vector3 temporalAabbMin, out Vector3 temporalAabbMax) - { - //start with static aabb - GetAabb(currentTransform, out temporalAabbMin, out temporalAabbMax); - - float temporalAabbMaxx = temporalAabbMax.X; - float temporalAabbMaxy = temporalAabbMax.Y; - float temporalAabbMaxz = temporalAabbMax.Z; - float temporalAabbMinx = temporalAabbMin.X; - float temporalAabbMiny = temporalAabbMin.Y; - float temporalAabbMinz = temporalAabbMin.Z; - - // add linear motion - Vector3 linMotion = linearVelocity * timeStep; - //todo: simd would have a vector max/min operation, instead of per-element access - if (linMotion.X > 0) - temporalAabbMaxx += linMotion.X; - else - temporalAabbMinx += linMotion.X; - if (linMotion.Y > 0) - temporalAabbMaxy += linMotion.Y; - else - temporalAabbMiny += linMotion.Y; - if (linMotion.Z > 0) - temporalAabbMaxz += linMotion.Z; - else - temporalAabbMinz += linMotion.Z; - - //add conservative angular motion - float angularMotion = angularVelocity.Length() * GetAngularMotionDisc() * timeStep; - Vector3 angularMotion3d = new Vector3(angularMotion, angularMotion, angularMotion); - temporalAabbMin = new Vector3(temporalAabbMinx, temporalAabbMiny, temporalAabbMinz); - temporalAabbMax = new Vector3(temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz); - - temporalAabbMin -= angularMotion3d; - temporalAabbMax += angularMotion3d; - } - - public abstract void GetAabb(Matrix transform, out Vector3 aabbMin, out Vector3 aabbMax); - - public abstract void CalculateLocalInertia(float mass, out Vector3 inertia); - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// + /// CollisionShape provides generic interface for collidable objects + /// + public abstract class CollisionShape + { + //debugging support + private string _tempDebug; + + public abstract string Name { get; } + public string ExtraDebugInfo { get { return _tempDebug; } set { _tempDebug = value; } } + + public bool IsPolyhedral + { + get + { + return BroadphaseProxy.IsPolyhedral(ShapeType); + } + } + + public bool IsConvex + { + get + { + return BroadphaseProxy.IsConvex(ShapeType); + } + } + public bool IsConcave + { + get + { + return BroadphaseProxy.IsConcave(ShapeType); + } + } + public bool IsCompound + { + get + { + return BroadphaseProxy.IsCompound(ShapeType); + } + } + + //isInfinite is used to catch simulation error (aabb check) + public bool IsInfinite + { + get + { + return BroadphaseProxy.IsInfinite(ShapeType); + } + } + + public abstract float Margin { get; set; } + public abstract Vector3 LocalScaling { get; set; } + public abstract BroadphaseNativeTypes ShapeType { get; } + + + public virtual void GetBoundingSphere(out Vector3 center, out float radius) + { + Matrix tr = Matrix.Identity; + Vector3 aabbMin, aabbMax; + + GetAabb(tr, out aabbMin, out aabbMax); + + radius = (aabbMax - aabbMin).Length() * 0.5f; + center = (aabbMin + aabbMax) * 0.5f; + } + + public virtual float GetAngularMotionDisc() + { + Vector3 center; + float disc; + GetBoundingSphere(out center, out disc); + disc += center.Length(); + return disc; + } + + //calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep) + //result is conservative + public void CalculateTemporalAabb(Matrix currentTransform, Vector3 linearVelocity, Vector3 angularVelocity, float timeStep, out Vector3 temporalAabbMin, out Vector3 temporalAabbMax) + { + //start with static aabb + GetAabb(currentTransform, out temporalAabbMin, out temporalAabbMax); + + float temporalAabbMaxx = temporalAabbMax.X; + float temporalAabbMaxy = temporalAabbMax.Y; + float temporalAabbMaxz = temporalAabbMax.Z; + float temporalAabbMinx = temporalAabbMin.X; + float temporalAabbMiny = temporalAabbMin.Y; + float temporalAabbMinz = temporalAabbMin.Z; + + // add linear motion + Vector3 linMotion = linearVelocity * timeStep; + //todo: simd would have a vector max/min operation, instead of per-element access + if (linMotion.X > 0) + temporalAabbMaxx += linMotion.X; + else + temporalAabbMinx += linMotion.X; + if (linMotion.Y > 0) + temporalAabbMaxy += linMotion.Y; + else + temporalAabbMiny += linMotion.Y; + if (linMotion.Z > 0) + temporalAabbMaxz += linMotion.Z; + else + temporalAabbMinz += linMotion.Z; + + //add conservative angular motion + float angularMotion = angularVelocity.Length() * GetAngularMotionDisc() * timeStep; + Vector3 angularMotion3d = new Vector3(angularMotion, angularMotion, angularMotion); + temporalAabbMin = new Vector3(temporalAabbMinx, temporalAabbMiny, temporalAabbMinz); + temporalAabbMax = new Vector3(temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz); + + temporalAabbMin -= angularMotion3d; + temporalAabbMax += angularMotion3d; + } + + public abstract void GetAabb(Matrix transform, out Vector3 aabbMin, out Vector3 aabbMax); + + public abstract void CalculateLocalInertia(float mass, out Vector3 inertia); + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs index 08aeec8..bd13ffd 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs @@ -1,183 +1,183 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// - /// CompoundShape allows to store multiple other CollisionShapes - /// This allows for concave collision objects. This is more general then the Static Concave TriangleMeshShape. - /// - public class CompoundShape : CollisionShape - { - private List _childTransforms = new List(); - private List _childShapes = new List(); - private Vector3 _localAabbMin; - private Vector3 _localAabbMax; - - private OptimizedBvh _aabbTree; - private float _collisionMargin; - private Vector3 _localScaling; - - public CompoundShape() - { - _localAabbMin = new Vector3(1e30f, 1e30f, 1e30f); - _localAabbMax = new Vector3(-1e30f, -1e30f, -1e30f); - _aabbTree = null; - _collisionMargin = 0f; - _localScaling = new Vector3(1f, 1f, 1f); - } - - public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) - { - Vector3 localHalfExtents = 0.5f * (_localAabbMax - _localAabbMin); - Vector3 localCenter = 0.5f * (_localAabbMax + _localAabbMin); - - Matrix abs_b = MathHelper.Absolute(t); - - Vector3 row1 = new Vector3(abs_b.M11, abs_b.M12, abs_b.M13); - Vector3 row2 = new Vector3(abs_b.M21, abs_b.M22, abs_b.M23); - Vector3 row3 = new Vector3(abs_b.M31, abs_b.M32, abs_b.M33); - - Vector3 center = new Vector3(Vector3.Dot(row1, localCenter) + t.Translation.X, - Vector3.Dot(row2, localCenter) + t.Translation.Y, - Vector3.Dot(row3, localCenter) + t.Translation.Z); - - Vector3 extent = new Vector3(Vector3.Dot(row1, localHalfExtents), - Vector3.Dot(row2, localHalfExtents), - Vector3.Dot(row3, localHalfExtents)); - - aabbMin = center - extent; - aabbMax = center + extent; - } - - public override BroadphaseNativeTypes ShapeType - { - get - { - return BroadphaseNativeTypes.Compound; - } - } - - public override Vector3 LocalScaling - { - get - { - return _localScaling; - } - set - { - _localScaling = value; - } - } - - public override string Name - { - get - { - return "Compound"; - } - } - - public override float Margin - { - get - { - return _collisionMargin; - } - set - { - _collisionMargin = value; - } - } - - public int ChildShapeCount { get { return _childShapes.Count; } } - //this is optional, but should make collision queries faster, by culling non-overlapping nodes - public OptimizedBvh AabbTree { get { return _aabbTree; } } - - public CollisionShape GetChildShape(int index) - { - return _childShapes[index]; - } - - public Matrix GetChildTransform(int index) - { - return _childTransforms[index]; - } - - public override void CalculateLocalInertia(float mass, out Vector3 inertia) - { - //approximation: take the inertia from the aabb for now - Matrix ident = Matrix.Identity; - Vector3 aabbMin, aabbMax; - GetAabb(ident, out aabbMin, out aabbMax); - - Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; - - float lx = 2f * (halfExtents.X); - float ly = 2f * (halfExtents.Y); - float lz = 2f * (halfExtents.Z); - - inertia = new Vector3(); - inertia.X = mass / (12.0f) * (ly * ly + lz * lz); - inertia.Y = mass / (12.0f) * (lx * lx + lz * lz); - inertia.Z = mass / (12.0f) * (lx * lx + ly * ly); - } - - public void AddChildShape(Matrix localTransform, CollisionShape shape) - { - _childTransforms.Add(localTransform); - _childShapes.Add(shape); - - //extend the local aabbMin/aabbMax - Vector3 localAabbMin, localAabbMax; - shape.GetAabb(localTransform, out localAabbMin, out localAabbMax); - if (_localAabbMin.X > localAabbMin.X) - { - _localAabbMin.X = localAabbMin.X; - } - if (_localAabbMax.X < localAabbMax.X) - { - _localAabbMax.X = localAabbMax.X; - } - if (_localAabbMin.Y > localAabbMin.Y) - { - _localAabbMin.Y = localAabbMin.Y; - } - if (_localAabbMax.Y < localAabbMax.Y) - { - _localAabbMax.Y = localAabbMax.Y; - } - if (_localAabbMin.Z > localAabbMin.Z) - { - _localAabbMin.Z = localAabbMin.Z; - } - if (_localAabbMax.Z < localAabbMax.Z) - { - _localAabbMax.Z = localAabbMax.Z; - } - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// + /// CompoundShape allows to store multiple other CollisionShapes + /// This allows for concave collision objects. This is more general then the Static Concave TriangleMeshShape. + /// + public class CompoundShape : CollisionShape + { + private List _childTransforms = new List(); + private List _childShapes = new List(); + private Vector3 _localAabbMin; + private Vector3 _localAabbMax; + + private OptimizedBvh _aabbTree; + private float _collisionMargin; + private Vector3 _localScaling; + + public CompoundShape() + { + _localAabbMin = new Vector3(1e30f, 1e30f, 1e30f); + _localAabbMax = new Vector3(-1e30f, -1e30f, -1e30f); + _aabbTree = null; + _collisionMargin = 0f; + _localScaling = new Vector3(1f, 1f, 1f); + } + + public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) + { + Vector3 localHalfExtents = 0.5f * (_localAabbMax - _localAabbMin); + Vector3 localCenter = 0.5f * (_localAabbMax + _localAabbMin); + + Matrix abs_b = MathHelper.Absolute(t); + + Vector3 row1 = new Vector3(abs_b.M11, abs_b.M12, abs_b.M13); + Vector3 row2 = new Vector3(abs_b.M21, abs_b.M22, abs_b.M23); + Vector3 row3 = new Vector3(abs_b.M31, abs_b.M32, abs_b.M33); + + Vector3 center = new Vector3(Vector3.Dot(row1, localCenter) + t.Translation.X, + Vector3.Dot(row2, localCenter) + t.Translation.Y, + Vector3.Dot(row3, localCenter) + t.Translation.Z); + + Vector3 extent = new Vector3(Vector3.Dot(row1, localHalfExtents), + Vector3.Dot(row2, localHalfExtents), + Vector3.Dot(row3, localHalfExtents)); + + aabbMin = center - extent; + aabbMax = center + extent; + } + + public override BroadphaseNativeTypes ShapeType + { + get + { + return BroadphaseNativeTypes.Compound; + } + } + + public override Vector3 LocalScaling + { + get + { + return _localScaling; + } + set + { + _localScaling = value; + } + } + + public override string Name + { + get + { + return "Compound"; + } + } + + public override float Margin + { + get + { + return _collisionMargin; + } + set + { + _collisionMargin = value; + } + } + + public int ChildShapeCount { get { return _childShapes.Count; } } + //this is optional, but should make collision queries faster, by culling non-overlapping nodes + public OptimizedBvh AabbTree { get { return _aabbTree; } } + + public CollisionShape GetChildShape(int index) + { + return _childShapes[index]; + } + + public Matrix GetChildTransform(int index) + { + return _childTransforms[index]; + } + + public override void CalculateLocalInertia(float mass, out Vector3 inertia) + { + //approximation: take the inertia from the aabb for now + Matrix ident = Matrix.Identity; + Vector3 aabbMin, aabbMax; + GetAabb(ident, out aabbMin, out aabbMax); + + Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; + + float lx = 2f * (halfExtents.X); + float ly = 2f * (halfExtents.Y); + float lz = 2f * (halfExtents.Z); + + inertia = new Vector3(); + inertia.X = mass / (12.0f) * (ly * ly + lz * lz); + inertia.Y = mass / (12.0f) * (lx * lx + lz * lz); + inertia.Z = mass / (12.0f) * (lx * lx + ly * ly); + } + + public void AddChildShape(Matrix localTransform, CollisionShape shape) + { + _childTransforms.Add(localTransform); + _childShapes.Add(shape); + + //extend the local aabbMin/aabbMax + Vector3 localAabbMin, localAabbMax; + shape.GetAabb(localTransform, out localAabbMin, out localAabbMax); + if (_localAabbMin.X > localAabbMin.X) + { + _localAabbMin.X = localAabbMin.X; + } + if (_localAabbMax.X < localAabbMax.X) + { + _localAabbMax.X = localAabbMax.X; + } + if (_localAabbMin.Y > localAabbMin.Y) + { + _localAabbMin.Y = localAabbMin.Y; + } + if (_localAabbMax.Y < localAabbMax.Y) + { + _localAabbMax.Y = localAabbMax.Y; + } + if (_localAabbMin.Z > localAabbMin.Z) + { + _localAabbMin.Z = localAabbMin.Z; + } + if (_localAabbMax.Z < localAabbMax.Z) + { + _localAabbMax.Z = localAabbMax.Z; + } + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs index 1e773f0..c0b177d 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs @@ -1,55 +1,55 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public abstract class ConcaveShape : CollisionShape - { - private float _collisionMargin; - - public ConcaveShape() { } - - public float CollisionMargin - { - get { return _collisionMargin; } - set { _collisionMargin = value; } - } - - public override float Margin - { - get - { - return _collisionMargin; - } - set - { - _collisionMargin = value; - } - } - - public abstract void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax); - } +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public abstract class ConcaveShape : CollisionShape + { + private float _collisionMargin; + + public ConcaveShape() { } + + public float CollisionMargin + { + get { return _collisionMargin; } + set { _collisionMargin = value; } + } + + public override float Margin + { + get + { + return _collisionMargin; + } + set + { + _collisionMargin = value; + } + } + + public abstract void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax); + } } \ No newline at end of file diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs index 361c277..52084f4 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs @@ -1,208 +1,208 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// - /// ConeShape implements a Cone shape, around the X axis - /// - public class ConeShapeX : ConeShape - { - public ConeShapeX(float radius, float height) - : base(radius, height) - { - ConeUpIndex = 0; - } - } - - /// - /// ConeShape implements a Cone shape, around the Z axis - /// - public class ConeShapeZ : ConeShape - { - public ConeShapeZ(float radius, float height) - : base(radius, height) - { - ConeUpIndex = 2; - } - } - - /// - /// ConeShape implements a Cone shape, around the Y axis - /// - public class ConeShape : ConvexShape - { - private float _sinAngle; - private float _radius; - private float _height; - private int[] _coneIndices = new int[3]; - - public ConeShape(float radius, float height) - { - _radius = radius; - _height = height; - ConeUpIndex = 1; - _sinAngle = (_radius / (float)Math.Sqrt(_radius * _radius + _height * _height)); - } - - public float Radius { get { return _radius; } } - public float Height { get { return _height; } } - - public override BroadphaseNativeTypes ShapeType - { - get - { - return BroadphaseNativeTypes.Cone; - } - } - - public override string Name - { - get - { - return "Cone"; - } - } - - //choose upAxis index - public int ConeUpIndex - { - get { return _coneIndices[1]; } - set - { - switch (value) - { - case 0: - _coneIndices[0] = 1; - _coneIndices[1] = 0; - _coneIndices[2] = 2; - break; - case 1: - _coneIndices[0] = 0; - _coneIndices[1] = 1; - _coneIndices[2] = 2; - break; - case 2: - _coneIndices[0] = 0; - _coneIndices[1] = 2; - _coneIndices[2] = 1; - break; - default: - BulletDebug.Assert(false); - break; - } - } - } - - private Vector3 ConeLocalSupport(Vector3 v) - { - float halfHeight = _height * 0.5f; - bool condition; - - if (_coneIndices[1] == 0) - condition = v.X > v.Length() * _sinAngle; - else if (_coneIndices[1] == 1) - condition = v.Y > v.Length() * _sinAngle; - else - condition = v.Z > v.Length() * _sinAngle; - - if (condition) - { - Vector3 tmp = new Vector3(); - MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], halfHeight); - return tmp; - } - else - { - float s = (float)Math.Sqrt(MathHelper.GetValueByIndex(v, _coneIndices[0]) * MathHelper.GetValueByIndex(v, _coneIndices[0]) - + MathHelper.GetValueByIndex(v, _coneIndices[2]) * MathHelper.GetValueByIndex(v, _coneIndices[2])); - if (s > MathHelper.Epsilon) - { - float d = _radius / s; - Vector3 tmp = new Vector3(); - MathHelper.SetValueByIndex(ref tmp, _coneIndices[0], MathHelper.GetValueByIndex(v, _coneIndices[0]) * d); - MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], -halfHeight); - MathHelper.SetValueByIndex(ref tmp, _coneIndices[2], MathHelper.GetValueByIndex(v, _coneIndices[2]) * d); - return tmp; - } - else - { - Vector3 tmp = new Vector3(); - MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], -halfHeight); - return tmp; - } - } - } - - public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) - { - return ConeLocalSupport(vec); - } - - public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) - { - for (int i = 0; i < vectors.Length; i++) - supportVerticesOut[i] = ConeLocalSupport(vectors[i]); - } - - public override void CalculateLocalInertia(float mass, out Vector3 inertia) - { - Matrix identity = Matrix.Identity; - Vector3 aabbMin, aabbMax; - GetAabb(identity, out aabbMin, out aabbMax); - - Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; - - float margin = Margin; - - float lx = 2f * (halfExtents.X + margin); - float ly = 2f * (halfExtents.Y + margin); - float lz = 2f * (halfExtents.Z + margin); - float x2 = lx * lx; - float y2 = ly * ly; - float z2 = lz * lz; - float scaledmass = mass * 0.08333333f; - - inertia = scaledmass * (new Vector3(y2 + z2, x2 + z2, x2 + y2)); - } - - public override Vector3 LocalGetSupportingVertex(Vector3 vec) - { - Vector3 supVertex = ConeLocalSupport(vec); - if (Margin != 0) - { - Vector3 vecnorm = vec; - if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) - { - vecnorm = new Vector3(-1f, -1f, -1f); - } - vecnorm = Vector3.Normalize(vecnorm); - supVertex += Margin * vecnorm; - } - return supVertex; - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// + /// ConeShape implements a Cone shape, around the X axis + /// + public class ConeShapeX : ConeShape + { + public ConeShapeX(float radius, float height) + : base(radius, height) + { + ConeUpIndex = 0; + } + } + + /// + /// ConeShape implements a Cone shape, around the Z axis + /// + public class ConeShapeZ : ConeShape + { + public ConeShapeZ(float radius, float height) + : base(radius, height) + { + ConeUpIndex = 2; + } + } + + /// + /// ConeShape implements a Cone shape, around the Y axis + /// + public class ConeShape : ConvexShape + { + private float _sinAngle; + private float _radius; + private float _height; + private int[] _coneIndices = new int[3]; + + public ConeShape(float radius, float height) + { + _radius = radius; + _height = height; + ConeUpIndex = 1; + _sinAngle = (_radius / (float)Math.Sqrt(_radius * _radius + _height * _height)); + } + + public float Radius { get { return _radius; } } + public float Height { get { return _height; } } + + public override BroadphaseNativeTypes ShapeType + { + get + { + return BroadphaseNativeTypes.Cone; + } + } + + public override string Name + { + get + { + return "Cone"; + } + } + + //choose upAxis index + public int ConeUpIndex + { + get { return _coneIndices[1]; } + set + { + switch (value) + { + case 0: + _coneIndices[0] = 1; + _coneIndices[1] = 0; + _coneIndices[2] = 2; + break; + case 1: + _coneIndices[0] = 0; + _coneIndices[1] = 1; + _coneIndices[2] = 2; + break; + case 2: + _coneIndices[0] = 0; + _coneIndices[1] = 2; + _coneIndices[2] = 1; + break; + default: + BulletDebug.Assert(false); + break; + } + } + } + + private Vector3 ConeLocalSupport(Vector3 v) + { + float halfHeight = _height * 0.5f; + bool condition; + + if (_coneIndices[1] == 0) + condition = v.X > v.Length() * _sinAngle; + else if (_coneIndices[1] == 1) + condition = v.Y > v.Length() * _sinAngle; + else + condition = v.Z > v.Length() * _sinAngle; + + if (condition) + { + Vector3 tmp = new Vector3(); + MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], halfHeight); + return tmp; + } + else + { + float s = (float)Math.Sqrt(MathHelper.GetValueByIndex(v, _coneIndices[0]) * MathHelper.GetValueByIndex(v, _coneIndices[0]) + + MathHelper.GetValueByIndex(v, _coneIndices[2]) * MathHelper.GetValueByIndex(v, _coneIndices[2])); + if (s > MathHelper.Epsilon) + { + float d = _radius / s; + Vector3 tmp = new Vector3(); + MathHelper.SetValueByIndex(ref tmp, _coneIndices[0], MathHelper.GetValueByIndex(v, _coneIndices[0]) * d); + MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], -halfHeight); + MathHelper.SetValueByIndex(ref tmp, _coneIndices[2], MathHelper.GetValueByIndex(v, _coneIndices[2]) * d); + return tmp; + } + else + { + Vector3 tmp = new Vector3(); + MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], -halfHeight); + return tmp; + } + } + } + + public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) + { + return ConeLocalSupport(vec); + } + + public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) + { + for (int i = 0; i < vectors.Length; i++) + supportVerticesOut[i] = ConeLocalSupport(vectors[i]); + } + + public override void CalculateLocalInertia(float mass, out Vector3 inertia) + { + Matrix identity = Matrix.Identity; + Vector3 aabbMin, aabbMax; + GetAabb(identity, out aabbMin, out aabbMax); + + Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; + + float margin = Margin; + + float lx = 2f * (halfExtents.X + margin); + float ly = 2f * (halfExtents.Y + margin); + float lz = 2f * (halfExtents.Z + margin); + float x2 = lx * lx; + float y2 = ly * ly; + float z2 = lz * lz; + float scaledmass = mass * 0.08333333f; + + inertia = scaledmass * (new Vector3(y2 + z2, x2 + z2, x2 + y2)); + } + + public override Vector3 LocalGetSupportingVertex(Vector3 vec) + { + Vector3 supVertex = ConeLocalSupport(vec); + if (Margin != 0) + { + Vector3 vecnorm = vec; + if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) + { + vecnorm = new Vector3(-1f, -1f, -1f); + } + vecnorm = Vector3.Normalize(vecnorm); + supVertex += Margin * vecnorm; + } + return supVertex; + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs index eb78533..8d06c90 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs @@ -1,184 +1,184 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// - /// ConvexHullShape implements an implicit (getSupportingVertex) Convex Hull of a Point Cloud (vertices) - /// No connectivity is needed. localGetSupportingVertex iterates linearly though all vertices. - /// on modern hardware, due to cache coherency this isn't that bad. Complex algorithms tend to trash the cash. - /// (memory is much slower then the cpu) - /// - public class ConvexHullShape : PolyhedralConvexShape - { - private List _points = new List(); - - public ConvexHullShape() { } - - public override int VertexCount - { - get - { - return _points.Count; - } - } - - public override int EdgeCount - { - get - { - return _points.Count; - } - } - - public override int PlaneCount - { - get - { - return 0; - } - } - - public override BroadphaseNativeTypes ShapeType - { - get - { - return BroadphaseNativeTypes.ConvexHull; - } - } - - public override string Name - { - get - { - return "Convex"; - } - } - - public override Vector3 LocalGetSupportingVertex(Vector3 vec) - { - Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); - - if (Margin != 0) - { - Vector3 vecnorm = vec; - if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) - { - vecnorm=new Vector3(-1, -1, -1); - } - vecnorm = Vector3.Normalize(vecnorm); - supVertex += Margin * vecnorm; - } - return supVertex; - } - - public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec0) - { - Vector3 supVec = new Vector3(); - float newDot, maxDot = -1e30f; - - Vector3 vec = vec0; - float lenSqr = vec.LengthSquared(); - if (lenSqr < 0.0001f) - { - vec = new Vector3(1, 0, 0); - } - else - { - float rlen = 1f / (float)Math.Sqrt(lenSqr); - vec *= rlen; - } - - for (int i = 0; i < _points.Count; i++) - { - Vector3 vtx = _points[i] * LocalScaling; - - newDot = Vector3.Dot(vec, vtx); - if (newDot > maxDot) - { - maxDot = newDot; - supVec = vtx; - } - } - return supVec; - } - - public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) - { - float newDot; - //use 'w' component of supportVerticesOut? - /*{ - for (int i = 0; i < numVectors; i++) - { - supportVerticesOut[i][3] = -1e30f; - } - }*/ - #warning Warning! - for (int i = 0; i < _points.Count; i++) - { - Vector3 vtx = _points[i] * LocalScaling; - - for (int j = 0; j < vectors.Length; j++) - { - newDot = Vector3.Dot(vectors[j], vtx); - if (newDot > -1e30f) - { - //WARNING: don't swap next lines, the w component would get overwritten! - supportVerticesOut[j] = vtx; - //supportVerticesOut[j][3] = newDot; - #warning Warning! - } - } - } - } - - public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) - { - int index0 = i % _points.Count; - int index1 = (i + 1) % _points.Count; - pa = _points[index0] * LocalScaling; - pb = _points[index1] * LocalScaling; - } - - public override void GetVertex(int i, out Vector3 vtx) - { - vtx = _points[i] * LocalScaling; - } - - public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) - { - planeNormal = new Vector3(); - planeSupport = new Vector3(); - BulletDebug.Assert(false); - } - - public override bool IsInside(Vector3 pt, float tolerance) - { - BulletDebug.Assert(false); - return false; - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// + /// ConvexHullShape implements an implicit (getSupportingVertex) Convex Hull of a Point Cloud (vertices) + /// No connectivity is needed. localGetSupportingVertex iterates linearly though all vertices. + /// on modern hardware, due to cache coherency this isn't that bad. Complex algorithms tend to trash the cash. + /// (memory is much slower then the cpu) + /// + public class ConvexHullShape : PolyhedralConvexShape + { + private List _points = new List(); + + public ConvexHullShape() { } + + public override int VertexCount + { + get + { + return _points.Count; + } + } + + public override int EdgeCount + { + get + { + return _points.Count; + } + } + + public override int PlaneCount + { + get + { + return 0; + } + } + + public override BroadphaseNativeTypes ShapeType + { + get + { + return BroadphaseNativeTypes.ConvexHull; + } + } + + public override string Name + { + get + { + return "Convex"; + } + } + + public override Vector3 LocalGetSupportingVertex(Vector3 vec) + { + Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); + + if (Margin != 0) + { + Vector3 vecnorm = vec; + if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) + { + vecnorm=new Vector3(-1, -1, -1); + } + vecnorm = Vector3.Normalize(vecnorm); + supVertex += Margin * vecnorm; + } + return supVertex; + } + + public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec0) + { + Vector3 supVec = new Vector3(); + float newDot, maxDot = -1e30f; + + Vector3 vec = vec0; + float lenSqr = vec.LengthSquared(); + if (lenSqr < 0.0001f) + { + vec = new Vector3(1, 0, 0); + } + else + { + float rlen = 1f / (float)Math.Sqrt(lenSqr); + vec *= rlen; + } + + for (int i = 0; i < _points.Count; i++) + { + Vector3 vtx = _points[i] * LocalScaling; + + newDot = Vector3.Dot(vec, vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + return supVec; + } + + public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) + { + float newDot; + //use 'w' component of supportVerticesOut? + /*{ + for (int i = 0; i < numVectors; i++) + { + supportVerticesOut[i][3] = -1e30f; + } + }*/ + #warning Warning! + for (int i = 0; i < _points.Count; i++) + { + Vector3 vtx = _points[i] * LocalScaling; + + for (int j = 0; j < vectors.Length; j++) + { + newDot = Vector3.Dot(vectors[j], vtx); + if (newDot > -1e30f) + { + //WARNING: don't swap next lines, the w component would get overwritten! + supportVerticesOut[j] = vtx; + //supportVerticesOut[j][3] = newDot; + #warning Warning! + } + } + } + } + + public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) + { + int index0 = i % _points.Count; + int index1 = (i + 1) % _points.Count; + pa = _points[index0] * LocalScaling; + pb = _points[index1] * LocalScaling; + } + + public override void GetVertex(int i, out Vector3 vtx) + { + vtx = _points[i] * LocalScaling; + } + + public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) + { + planeNormal = new Vector3(); + planeSupport = new Vector3(); + BulletDebug.Assert(false); + } + + public override bool IsInside(Vector3 pt, float tolerance) + { + BulletDebug.Assert(false); + return false; + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs index 02d678e..d5c2fbd 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs @@ -1,141 +1,141 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// - /// ConvexShape is an abstract shape interface. - /// The explicit part provides plane-equations, the implicit part provides GetClosestPoint interface. - /// used in combination with GJK or btConvexCast - /// - public abstract class ConvexShape : CollisionShape - { - private const int _maxPreferredPenetrationDirections = 10; - private const float _convexDistanceMargin = 0.04f; - - private Vector3 _localScaling; - private Vector3 _implicitShapeDimensions; - private float _collisionMargin; - - public ConvexShape() - : base() - { - _localScaling = Vector3.One; - _collisionMargin = ConvexDistanceMargin; - } - - public static int MaxPreferredPenetrationDirections { get { return _maxPreferredPenetrationDirections; } } - public static float ConvexDistanceMargin { get { return _convexDistanceMargin; } } - - public Vector3 ImplicitShapeDimensions { get { return _implicitShapeDimensions; } protected set { _implicitShapeDimensions = value; } } - public virtual int PreferredPenetrationDirectionsCount { get { return 0; } } - - protected float CollisionMargin { get { return _collisionMargin; } set { _collisionMargin = value; } } - - public virtual void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector) - { - penetrationVector = new Vector3(); - BulletDebug.Assert(false); - } - - public abstract Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec); - //notice that the vectors should be unit length - public abstract void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut); - - /// - /// getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version - /// - /// - /// - /// - public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) - { - GetAabbSlow(t, out aabbMin, out aabbMax); - } - - public override Vector3 LocalScaling - { - get - { - return _localScaling; - } - set - { - _localScaling = value; - } - } - - public override float Margin - { - get - { - return _collisionMargin; - } - set - { - _collisionMargin = value; - } - } - - public virtual Vector3 LocalGetSupportingVertex(Vector3 vec) - { - Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); - - if (Margin != 0f) - { - Vector3 vecnorm = vec; - if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) - { - vecnorm = new Vector3(-1f, -1f, -1f); - } - vecnorm.Normalize(); - supVertex += Margin * vecnorm; - } - return supVertex; - } - - public virtual void GetAabbSlow(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) - { - float margin = Margin; - aabbMax = new Vector3(); - aabbMin = new Vector3(); - - for (int i = 0; i < 3; i++) - { - Vector3 vec = new Vector3(0f, 0f, 0f); - MathHelper.SetElement(ref vec, i, 1); - - Vector3 sv = LocalGetSupportingVertex(Vector3.TransformNormal(vec, t)); - - Vector3 tmp = MathHelper.MatrixToVector(t, sv); - MathHelper.SetElement(ref aabbMax, i, MathHelper.GetElement(tmp, i) + margin); - MathHelper.SetElement(ref vec, i, -1f); - tmp = MathHelper.MatrixToVector(t, LocalGetSupportingVertex(Vector3.TransformNormal(vec, t))); - MathHelper.SetElement(ref aabbMin, i, MathHelper.GetElement(tmp, i) - margin); - } - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// + /// ConvexShape is an abstract shape interface. + /// The explicit part provides plane-equations, the implicit part provides GetClosestPoint interface. + /// used in combination with GJK or btConvexCast + /// + public abstract class ConvexShape : CollisionShape + { + private const int _maxPreferredPenetrationDirections = 10; + private const float _convexDistanceMargin = 0.04f; + + private Vector3 _localScaling; + private Vector3 _implicitShapeDimensions; + private float _collisionMargin; + + public ConvexShape() + : base() + { + _localScaling = Vector3.One; + _collisionMargin = ConvexDistanceMargin; + } + + public static int MaxPreferredPenetrationDirections { get { return _maxPreferredPenetrationDirections; } } + public static float ConvexDistanceMargin { get { return _convexDistanceMargin; } } + + public Vector3 ImplicitShapeDimensions { get { return _implicitShapeDimensions; } protected set { _implicitShapeDimensions = value; } } + public virtual int PreferredPenetrationDirectionsCount { get { return 0; } } + + protected float CollisionMargin { get { return _collisionMargin; } set { _collisionMargin = value; } } + + public virtual void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector) + { + penetrationVector = new Vector3(); + BulletDebug.Assert(false); + } + + public abstract Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec); + //notice that the vectors should be unit length + public abstract void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut); + + /// + /// getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version + /// + /// + /// + /// + public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) + { + GetAabbSlow(t, out aabbMin, out aabbMax); + } + + public override Vector3 LocalScaling + { + get + { + return _localScaling; + } + set + { + _localScaling = value; + } + } + + public override float Margin + { + get + { + return _collisionMargin; + } + set + { + _collisionMargin = value; + } + } + + public virtual Vector3 LocalGetSupportingVertex(Vector3 vec) + { + Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); + + if (Margin != 0f) + { + Vector3 vecnorm = vec; + if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) + { + vecnorm = new Vector3(-1f, -1f, -1f); + } + vecnorm.Normalize(); + supVertex += Margin * vecnorm; + } + return supVertex; + } + + public virtual void GetAabbSlow(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) + { + float margin = Margin; + aabbMax = new Vector3(); + aabbMin = new Vector3(); + + for (int i = 0; i < 3; i++) + { + Vector3 vec = new Vector3(0f, 0f, 0f); + MathHelper.SetElement(ref vec, i, 1); + + Vector3 sv = LocalGetSupportingVertex(Vector3.TransformNormal(vec, t)); + + Vector3 tmp = MathHelper.MatrixToVector(t, sv); + MathHelper.SetElement(ref aabbMax, i, MathHelper.GetElement(tmp, i) + margin); + MathHelper.SetElement(ref vec, i, -1f); + tmp = MathHelper.MatrixToVector(t, LocalGetSupportingVertex(Vector3.TransformNormal(vec, t))); + MathHelper.SetElement(ref aabbMin, i, MathHelper.GetElement(tmp, i) - margin); + } + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs index 0fd3fa7..3e46c66 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs @@ -1,185 +1,185 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// - /// ConvexTriangleMeshShape is a convex hull of a triangle mesh. If you just have a point cloud, you can use ConvexHullShape instead. - /// It uses the StridingMeshInterface instead of a point cloud. This can avoid the duplication of the triangle mesh data. - /// - public class ConvexTriangleMeshShape : PolyhedralConvexShape - { - private StridingMeshInterface _stridingMesh; - - public ConvexTriangleMeshShape(StridingMeshInterface meshInterface) - { - _stridingMesh = meshInterface; - } - - public StridingMeshInterface getStridingMesh() - { - return _stridingMesh; - } - - public override int VertexCount - { - get - { - return 0; - } - } - - public override int EdgeCount - { - get - { - return 0; - } - } - - public override int PlaneCount - { - get - { - return 0; - } - } - - public override Vector3 LocalScaling - { - get - { - return base.LocalScaling; - } - set - { - _stridingMesh.Scaling = value; - } - } - - public override BroadphaseNativeTypes ShapeType - { - get - { - return BroadphaseNativeTypes.ConvexTriangleMesh; - } - } - - public override string Name - { - get - { - return "ConvexTrimesh"; - } - } - - public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) - { - pa = new Vector3(); - pb = new Vector3(); - BulletDebug.Assert(false); - } - - public override void GetVertex(int i, out Vector3 vtx) - { - vtx = new Vector3(); - BulletDebug.Assert(false); - } - - public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) - { - planeNormal = new Vector3(); - planeSupport = new Vector3(); - BulletDebug.Assert(false); - } - - public override bool IsInside(Vector3 pt, float tolerance) - { - BulletDebug.Assert(false); - return false; - } - - public override Vector3 LocalGetSupportingVertex(Vector3 vec) - { - Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); - - if (Margin != 0) - { - Vector3 vecnorm = vec; - if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) - { - vecnorm = new Vector3(-1, -1, -1); - } - vecnorm = Vector3.Normalize(vecnorm); - supVertex += Margin * vecnorm; - } - return supVertex; - } - - public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec0) - { - Vector3 supVec = new Vector3(); - - Vector3 vec = vec0; - float lenSqr = vec.LengthSquared(); - if (lenSqr < 0.0001f) - { - vec = new Vector3(1, 0, 0); - } - else - { - float rlen = 1f / (float)Math.Sqrt(lenSqr); - vec *= rlen; - } - - LocalSupportVertexCallback supportCallback = new LocalSupportVertexCallback(vec); - Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); - _stridingMesh.InternalProcessAllTriangles(supportCallback, -aabbMax, aabbMax); - supVec = supportCallback.SupportVertexLocal; - - return supVec; - } - - public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) - { - //use 'w' component of supportVerticesOut? - /*{ - for (int i = 0; i < numVectors; i++) - { - supportVerticesOut[i][3] = -1e30f; - } - }*/ - for (int j = 0; j < vectors.Length; j++) - { - Vector3 vec = vectors[j]; - LocalSupportVertexCallback supportCallback = new LocalSupportVertexCallback(vec); - Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); - _stridingMesh.InternalProcessAllTriangles(supportCallback, -aabbMax, aabbMax); - supportVerticesOut[j] = supportCallback.SupportVertexLocal; - } - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// + /// ConvexTriangleMeshShape is a convex hull of a triangle mesh. If you just have a point cloud, you can use ConvexHullShape instead. + /// It uses the StridingMeshInterface instead of a point cloud. This can avoid the duplication of the triangle mesh data. + /// + public class ConvexTriangleMeshShape : PolyhedralConvexShape + { + private StridingMeshInterface _stridingMesh; + + public ConvexTriangleMeshShape(StridingMeshInterface meshInterface) + { + _stridingMesh = meshInterface; + } + + public StridingMeshInterface getStridingMesh() + { + return _stridingMesh; + } + + public override int VertexCount + { + get + { + return 0; + } + } + + public override int EdgeCount + { + get + { + return 0; + } + } + + public override int PlaneCount + { + get + { + return 0; + } + } + + public override Vector3 LocalScaling + { + get + { + return base.LocalScaling; + } + set + { + _stridingMesh.Scaling = value; + } + } + + public override BroadphaseNativeTypes ShapeType + { + get + { + return BroadphaseNativeTypes.ConvexTriangleMesh; + } + } + + public override string Name + { + get + { + return "ConvexTrimesh"; + } + } + + public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) + { + pa = new Vector3(); + pb = new Vector3(); + BulletDebug.Assert(false); + } + + public override void GetVertex(int i, out Vector3 vtx) + { + vtx = new Vector3(); + BulletDebug.Assert(false); + } + + public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) + { + planeNormal = new Vector3(); + planeSupport = new Vector3(); + BulletDebug.Assert(false); + } + + public override bool IsInside(Vector3 pt, float tolerance) + { + BulletDebug.Assert(false); + return false; + } + + public override Vector3 LocalGetSupportingVertex(Vector3 vec) + { + Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); + + if (Margin != 0) + { + Vector3 vecnorm = vec; + if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) + { + vecnorm = new Vector3(-1, -1, -1); + } + vecnorm = Vector3.Normalize(vecnorm); + supVertex += Margin * vecnorm; + } + return supVertex; + } + + public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec0) + { + Vector3 supVec = new Vector3(); + + Vector3 vec = vec0; + float lenSqr = vec.LengthSquared(); + if (lenSqr < 0.0001f) + { + vec = new Vector3(1, 0, 0); + } + else + { + float rlen = 1f / (float)Math.Sqrt(lenSqr); + vec *= rlen; + } + + LocalSupportVertexCallback supportCallback = new LocalSupportVertexCallback(vec); + Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); + _stridingMesh.InternalProcessAllTriangles(supportCallback, -aabbMax, aabbMax); + supVec = supportCallback.SupportVertexLocal; + + return supVec; + } + + public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) + { + //use 'w' component of supportVerticesOut? + /*{ + for (int i = 0; i < numVectors; i++) + { + supportVerticesOut[i][3] = -1e30f; + } + }*/ + for (int j = 0; j < vectors.Length; j++) + { + Vector3 vec = vectors[j]; + LocalSupportVertexCallback supportCallback = new LocalSupportVertexCallback(vec); + Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); + _stridingMesh.InternalProcessAllTriangles(supportCallback, -aabbMax, aabbMax); + supportVerticesOut[j] = supportCallback.SupportVertexLocal; + } + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs index 9a86eaf..85bf769 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs @@ -1,136 +1,136 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// - /// implements cylinder shape interface - /// - public class CylinderShape : BoxShape - { - public CylinderShape(Vector3 halfExtents) - : base(halfExtents) - { - } - - public override BroadphaseNativeTypes ShapeType - { - get - { - return BroadphaseNativeTypes.Cylinder; - } - } - - public virtual int UpAxis - { - get - { - return 1; - } - } - - public virtual float Radius - { - get - { - return HalfExtents.Z; - } - } - - //debugging - public override string Name - { - get - { - return "CylinderY"; - } - } - - //getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version - public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) - { - GetAabbSlow(t, out aabbMin, out aabbMax); - } - - public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) - { - return CylinderLocalSupportY(HalfExtents, vec); - } - - public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) - { - for (int i = 0; i < vectors.Length; i++) - { - supportVerticesOut[i] = CylinderLocalSupportY(HalfExtents, vectors[i]); - } - } - - public override Vector3 LocalGetSupportingVertex(Vector3 vec) - { - - Vector3 supVertex; - supVertex = LocalGetSupportingVertexWithoutMargin(vec); - - if (Margin != 0) - { - Vector3 vecnorm = vec; - if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) - { - vecnorm=new Vector3(-1, -1, -1); - } - vecnorm = Vector3.Normalize(vecnorm); - supVertex += Margin * vecnorm; - } - return supVertex; - } - - private Vector3 CylinderLocalSupportY(Vector3 halfExtents, Vector3 v) - { - float radius = halfExtents.X; - float halfHeight = halfExtents.Y; - - Vector3 tmp = new Vector3(); - float d; - - float s = (float)Math.Sqrt(v.X * v.X + v.Z * v.Z); - if (s != 0) - { - d = radius / s; - tmp.X = v.X * d; - tmp.Y = v.Y < 0 ? -halfHeight : halfHeight; - tmp.Z = v.Z * d; - return tmp; - } - else - { - tmp.X = radius; - tmp.Y = v.Y < 0 ? -halfHeight : halfHeight; - tmp.Z = 0; - return tmp; - } - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// + /// implements cylinder shape interface + /// + public class CylinderShape : BoxShape + { + public CylinderShape(Vector3 halfExtents) + : base(halfExtents) + { + } + + public override BroadphaseNativeTypes ShapeType + { + get + { + return BroadphaseNativeTypes.Cylinder; + } + } + + public virtual int UpAxis + { + get + { + return 1; + } + } + + public virtual float Radius + { + get + { + return HalfExtents.Z; + } + } + + //debugging + public override string Name + { + get + { + return "CylinderY"; + } + } + + //getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version + public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) + { + GetAabbSlow(t, out aabbMin, out aabbMax); + } + + public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) + { + return CylinderLocalSupportY(HalfExtents, vec); + } + + public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) + { + for (int i = 0; i < vectors.Length; i++) + { + supportVerticesOut[i] = CylinderLocalSupportY(HalfExtents, vectors[i]); + } + } + + public override Vector3 LocalGetSupportingVertex(Vector3 vec) + { + + Vector3 supVertex; + supVertex = LocalGetSupportingVertexWithoutMargin(vec); + + if (Margin != 0) + { + Vector3 vecnorm = vec; + if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) + { + vecnorm=new Vector3(-1, -1, -1); + } + vecnorm = Vector3.Normalize(vecnorm); + supVertex += Margin * vecnorm; + } + return supVertex; + } + + private Vector3 CylinderLocalSupportY(Vector3 halfExtents, Vector3 v) + { + float radius = halfExtents.X; + float halfHeight = halfExtents.Y; + + Vector3 tmp = new Vector3(); + float d; + + float s = (float)Math.Sqrt(v.X * v.X + v.Z * v.Z); + if (s != 0) + { + d = radius / s; + tmp.X = v.X * d; + tmp.Y = v.Y < 0 ? -halfHeight : halfHeight; + tmp.Z = v.Z * d; + return tmp; + } + else + { + tmp.X = radius; + tmp.Y = v.Y < 0 ? -halfHeight : halfHeight; + tmp.Z = 0; + return tmp; + } + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs index 9dc8e2d..da84462 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs @@ -1,100 +1,100 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public class CylinderShapeX : CylinderShape - { - public CylinderShapeX(Vector3 halfExtents) - : base(halfExtents) { } - - public override int UpAxis - { - get - { - return 0; - } - } - - public override float Radius - { - get - { - return HalfExtents.Y; - } - } - - //debugging - public override string Name - { - get - { - return "CylinderX"; - } - } - - public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) - { - return CylinderLocalSupportX(HalfExtents, vec); - } - - public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) - { - for (int i = 0; i < vectors.Length; i++) - { - supportVerticesOut[i] = CylinderLocalSupportX(HalfExtents, vectors[i]); - } - } - - private Vector3 CylinderLocalSupportX(Vector3 halfExtents, Vector3 v) - { - //mapping depends on how cylinder local orientation is - // extents of the cylinder is: X,Y is for radius, and Z for height - float radius = halfExtents.Y; - float halfHeight = halfExtents.X; - - Vector3 tmp = new Vector3(); - float d; - - float s = (float)Math.Sqrt(v.Y * v.Y + v.Z * v.Z); - if (s != 0) - { - d = radius / s; - tmp.Y = v.Y * d; - tmp.X = v.X < 0 ? -halfHeight : halfHeight; - tmp.Z = v.Z * d; - return tmp; - } - else - { - tmp.Y = radius; - tmp.X = v.X < 0 ? -halfHeight : halfHeight; - tmp.Z = 0; - return tmp; - } - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public class CylinderShapeX : CylinderShape + { + public CylinderShapeX(Vector3 halfExtents) + : base(halfExtents) { } + + public override int UpAxis + { + get + { + return 0; + } + } + + public override float Radius + { + get + { + return HalfExtents.Y; + } + } + + //debugging + public override string Name + { + get + { + return "CylinderX"; + } + } + + public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) + { + return CylinderLocalSupportX(HalfExtents, vec); + } + + public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) + { + for (int i = 0; i < vectors.Length; i++) + { + supportVerticesOut[i] = CylinderLocalSupportX(HalfExtents, vectors[i]); + } + } + + private Vector3 CylinderLocalSupportX(Vector3 halfExtents, Vector3 v) + { + //mapping depends on how cylinder local orientation is + // extents of the cylinder is: X,Y is for radius, and Z for height + float radius = halfExtents.Y; + float halfHeight = halfExtents.X; + + Vector3 tmp = new Vector3(); + float d; + + float s = (float)Math.Sqrt(v.Y * v.Y + v.Z * v.Z); + if (s != 0) + { + d = radius / s; + tmp.Y = v.Y * d; + tmp.X = v.X < 0 ? -halfHeight : halfHeight; + tmp.Z = v.Z * d; + return tmp; + } + else + { + tmp.Y = radius; + tmp.X = v.X < 0 ? -halfHeight : halfHeight; + tmp.Z = 0; + return tmp; + } + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs index 1913e9c..10a1995 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs @@ -1,100 +1,100 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public class CylinderShapeZ : CylinderShape - { - public CylinderShapeZ(Vector3 halfExtents) - : base(halfExtents) { } - - public override int UpAxis - { - get - { - return 2; - } - } - - public override float Radius - { - get - { - return HalfExtents.X; - } - } - - //debugging - public override string Name - { - get - { - return "CylinderZ"; - } - } - - public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) - { - return CylinderLocalSupportZ(HalfExtents, vec); - } - - public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) - { - for (int i = 0; i < vectors.Length; i++) - { - supportVerticesOut[i] = CylinderLocalSupportZ(HalfExtents, vectors[i]); - } - } - - Vector3 CylinderLocalSupportZ(Vector3 halfExtents, Vector3 v) - { - //mapping depends on how cylinder local orientation is - // extents of the cylinder is: X,Y is for radius, and Z for height - float radius = halfExtents.X; - float halfHeight = halfExtents.Z; - - Vector3 tmp = new Vector3(); - float d; - - float s = (float)Math.Sqrt(v.X * v.X + v.Y * v.Y); - if (s != 0) - { - d = radius / s; - tmp.X = v.X * d; - tmp.Z = v.Z < 0 ? -halfHeight : halfHeight; - tmp.Y = v.Y * d; - return tmp; - } - else - { - tmp.X = radius; - tmp.Z = v.Z < 0 ? -halfHeight : halfHeight; - tmp.Y = 0; - return tmp; - } - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public class CylinderShapeZ : CylinderShape + { + public CylinderShapeZ(Vector3 halfExtents) + : base(halfExtents) { } + + public override int UpAxis + { + get + { + return 2; + } + } + + public override float Radius + { + get + { + return HalfExtents.X; + } + } + + //debugging + public override string Name + { + get + { + return "CylinderZ"; + } + } + + public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) + { + return CylinderLocalSupportZ(HalfExtents, vec); + } + + public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) + { + for (int i = 0; i < vectors.Length; i++) + { + supportVerticesOut[i] = CylinderLocalSupportZ(HalfExtents, vectors[i]); + } + } + + Vector3 CylinderLocalSupportZ(Vector3 halfExtents, Vector3 v) + { + //mapping depends on how cylinder local orientation is + // extents of the cylinder is: X,Y is for radius, and Z for height + float radius = halfExtents.X; + float halfHeight = halfExtents.Z; + + Vector3 tmp = new Vector3(); + float d; + + float s = (float)Math.Sqrt(v.X * v.X + v.Y * v.Y); + if (s != 0) + { + d = radius / s; + tmp.X = v.X * d; + tmp.Z = v.Z < 0 ? -halfHeight : halfHeight; + tmp.Y = v.Y * d; + return tmp; + } + else + { + tmp.X = radius; + tmp.Z = v.Z < 0 ? -halfHeight : halfHeight; + tmp.Y = 0; + return tmp; + } + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs index b094c39..ef6f134 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs @@ -1,80 +1,80 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; -using System.Diagnostics; - -namespace XnaDevRu.BulletX -{ - public class EmptyShape : ConcaveShape - { - private Vector3 _localScaling; - - public override string Name - { - get - { - return "Empty"; - } - } - - public override BroadphaseNativeTypes ShapeType - { - get - { - return BroadphaseNativeTypes.Empty; - } - } - - public override Vector3 LocalScaling - { - get - { - return _localScaling; - } - set - { - _localScaling = value; - } - } - - public override void ProcessAllTriangles(ITriangleCallback callback, MonoXnaCompactMaths.Vector3 aabbMin, MonoXnaCompactMaths.Vector3 aabbMax) - { - throw new Exception("The method or operation is not implemented."); - } - - public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) - { - Vector3 margin = new Vector3(Margin, Margin, Margin); - aabbMin = t.Translation - margin; - aabbMax = t.Translation + margin; - } - - public override void CalculateLocalInertia(float mass, out Vector3 inertia) - { - inertia = new Vector3(); - BulletDebug.Assert(false); - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; +using System.Diagnostics; + +namespace XnaDevRu.BulletX +{ + public class EmptyShape : ConcaveShape + { + private Vector3 _localScaling; + + public override string Name + { + get + { + return "Empty"; + } + } + + public override BroadphaseNativeTypes ShapeType + { + get + { + return BroadphaseNativeTypes.Empty; + } + } + + public override Vector3 LocalScaling + { + get + { + return _localScaling; + } + set + { + _localScaling = value; + } + } + + public override void ProcessAllTriangles(ITriangleCallback callback, MonoXnaCompactMaths.Vector3 aabbMin, MonoXnaCompactMaths.Vector3 aabbMax) + { + throw new Exception("The method or operation is not implemented."); + } + + public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) + { + Vector3 margin = new Vector3(Margin, Margin, Margin); + aabbMin = t.Translation - margin; + aabbMax = t.Translation + margin; + } + + public override void CalculateLocalInertia(float mass, out Vector3 inertia) + { + inertia = new Vector3(); + BulletDebug.Assert(false); + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs index 5073096..dffd865 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs @@ -1,55 +1,55 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public class FilteredCallback : ITriangleIndexCallback - { - private ITriangleCallback _callback; - private Vector3 _aabbMin; - private Vector3 _aabbMax; - - public FilteredCallback(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) - { - _callback = callback; - _aabbMin = aabbMin; - _aabbMax = aabbMax; - } - - public ITriangleCallback TriangleCallback { get { return _callback; } set { _callback = value; } } - public Vector3 AabbMin { get { return _aabbMin; } set { _aabbMin = value; } } - public Vector3 AabbMax { get { return _aabbMax; } set { _aabbMax = value; } } - - public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) - { - if (MathHelper.TestTriangleAgainstAabb2(triangle, _aabbMin, _aabbMax)) - { - //check aabb in triangle-space, before doing this - _callback.ProcessTriangle(triangle, partId, triangleIndex); - } - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public class FilteredCallback : ITriangleIndexCallback + { + private ITriangleCallback _callback; + private Vector3 _aabbMin; + private Vector3 _aabbMax; + + public FilteredCallback(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) + { + _callback = callback; + _aabbMin = aabbMin; + _aabbMax = aabbMax; + } + + public ITriangleCallback TriangleCallback { get { return _callback; } set { _callback = value; } } + public Vector3 AabbMin { get { return _aabbMin; } set { _aabbMin = value; } } + public Vector3 AabbMax { get { return _aabbMax; } set { _aabbMax = value; } } + + public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) + { + if (MathHelper.TestTriangleAgainstAabb2(triangle, _aabbMin, _aabbMax)) + { + //check aabb in triangle-space, before doing this + _callback.ProcessTriangle(triangle, partId, triangleIndex); + } + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs index b88d804..51c65b7 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs @@ -1,33 +1,33 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public interface ITriangleIndexCallback - { - void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex); - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public interface ITriangleIndexCallback + { + void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex); + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs index 951707d..1e5e141 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs @@ -1,58 +1,58 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public class LocalSupportVertexCallback : ITriangleIndexCallback - { - private Vector3 _supportVertexLocal; - private float _maxDot; - private Vector3 _supportVecLocal; - - public LocalSupportVertexCallback(Vector3 supportVecLocal) - { - _supportVertexLocal = new Vector3(); - _maxDot = -1e30f; - _supportVecLocal = supportVecLocal; - } - - public float MaxDot { get { return _maxDot; } set { _maxDot = value; } } - public Vector3 SupportVertexLocal { get { return _supportVecLocal; } set { _supportVecLocal = value; } } - - public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) - { - for (int i = 0; i < 3; i++) - { - float dot = Vector3.Dot(_supportVecLocal, triangle[i]); - if (dot > _maxDot) - { - _maxDot = dot; - _supportVertexLocal = triangle[i]; - } - } - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public class LocalSupportVertexCallback : ITriangleIndexCallback + { + private Vector3 _supportVertexLocal; + private float _maxDot; + private Vector3 _supportVecLocal; + + public LocalSupportVertexCallback(Vector3 supportVecLocal) + { + _supportVertexLocal = new Vector3(); + _maxDot = -1e30f; + _supportVecLocal = supportVecLocal; + } + + public float MaxDot { get { return _maxDot; } set { _maxDot = value; } } + public Vector3 SupportVertexLocal { get { return _supportVecLocal; } set { _supportVecLocal = value; } } + + public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) + { + for (int i = 0; i < 3; i++) + { + float dot = Vector3.Dot(_supportVecLocal, triangle[i]); + if (dot > _maxDot) + { + _maxDot = dot; + _supportVertexLocal = triangle[i]; + } + } + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs index 6f70cae..8cdcfbe 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs @@ -1,99 +1,99 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// - /// MinkowskiSumShape represents implicit (getSupportingVertex) based minkowski sum of two convex implicit shapes. - /// - public class MinkowskiSumShape : ConvexShape - { - private Matrix _transformA; - private Matrix _transformB; - private ConvexShape _shapeA; - private ConvexShape _shapeB; - - public MinkowskiSumShape(ConvexShape shapeA, ConvexShape shapeB) - { - _shapeA = shapeA; - _shapeB = shapeB; - _transformA = Matrix.Identity; - _transformB = Matrix.Identity; - } - - public Matrix TransformA { get { return _transformA; } set { _transformA = value; } } - public Matrix TransformB { get { return _transformB; } set { _transformB = value; } } - public ConvexShape ShapeA { get { return _shapeA; } } - public ConvexShape ShapeB { get { return _shapeB; } } - - public override float Margin - { - get - { - return _shapeA.Margin + _shapeB.Margin; - } - set - { - base.Margin = value; - } - } - - public override BroadphaseNativeTypes ShapeType - { - get - { - return BroadphaseNativeTypes.MinkowskiDifference; - } - } - - public override string Name - { - get - { - return "MinkowskiSum"; - } - } - - public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) - { - Vector3 supVertexA = MathHelper.MatrixToVector(_transformA, _shapeA.LocalGetSupportingVertexWithoutMargin(Vector3.TransformNormal(vec, _transformA))); - Vector3 supVertexB = MathHelper.MatrixToVector(_transformB, _shapeB.LocalGetSupportingVertexWithoutMargin(Vector3.TransformNormal(vec, _transformB))); - return supVertexA + supVertexB; - } - - public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) - { - for (int i = 0; i < vectors.Length; i++) - supportVerticesOut[i] = LocalGetSupportingVertexWithoutMargin(vectors[i]); - } - - public override void CalculateLocalInertia(float mass, out Vector3 inertia) - { - inertia = new Vector3(); - BulletDebug.Assert(false); - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// + /// MinkowskiSumShape represents implicit (getSupportingVertex) based minkowski sum of two convex implicit shapes. + /// + public class MinkowskiSumShape : ConvexShape + { + private Matrix _transformA; + private Matrix _transformB; + private ConvexShape _shapeA; + private ConvexShape _shapeB; + + public MinkowskiSumShape(ConvexShape shapeA, ConvexShape shapeB) + { + _shapeA = shapeA; + _shapeB = shapeB; + _transformA = Matrix.Identity; + _transformB = Matrix.Identity; + } + + public Matrix TransformA { get { return _transformA; } set { _transformA = value; } } + public Matrix TransformB { get { return _transformB; } set { _transformB = value; } } + public ConvexShape ShapeA { get { return _shapeA; } } + public ConvexShape ShapeB { get { return _shapeB; } } + + public override float Margin + { + get + { + return _shapeA.Margin + _shapeB.Margin; + } + set + { + base.Margin = value; + } + } + + public override BroadphaseNativeTypes ShapeType + { + get + { + return BroadphaseNativeTypes.MinkowskiDifference; + } + } + + public override string Name + { + get + { + return "MinkowskiSum"; + } + } + + public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) + { + Vector3 supVertexA = MathHelper.MatrixToVector(_transformA, _shapeA.LocalGetSupportingVertexWithoutMargin(Vector3.TransformNormal(vec, _transformA))); + Vector3 supVertexB = MathHelper.MatrixToVector(_transformB, _shapeB.LocalGetSupportingVertexWithoutMargin(Vector3.TransformNormal(vec, _transformB))); + return supVertexA + supVertexB; + } + + public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) + { + for (int i = 0; i < vectors.Length; i++) + supportVerticesOut[i] = LocalGetSupportingVertexWithoutMargin(vectors[i]); + } + + public override void CalculateLocalInertia(float mass, out Vector3 inertia) + { + inertia = new Vector3(); + BulletDebug.Assert(false); + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs index 9959c01..73fd094 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs @@ -1,154 +1,154 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// - /// MultiSphereShape represents implicit convex hull of a collection of spheres (using getSupportingVertex) - /// - public class MultiSphereShape : ConvexShape - { - private const int _maxNumSpheres = 5; - private Vector3[] _localPositions = new Vector3[MaxNumSpheres]; - private float[] _radi = new float[MaxNumSpheres]; - private Vector3 _inertiaHalfExtents; - - private int m_numSpheres; - - public MultiSphereShape(Vector3 inertiaHalfExtents, Vector3[] positions, float[] radi, int numSpheres) - { - _inertiaHalfExtents = inertiaHalfExtents; - float startMargin = 1e30f; - - m_numSpheres = numSpheres; - for (int i = 0; i < m_numSpheres; i++) - { - _localPositions[i] = positions[i]; - _radi[i] = radi[i]; - if (radi[i] < startMargin) - startMargin = radi[i]; - } - Margin = startMargin; - } - - public static int MaxNumSpheres { get { return _maxNumSpheres; } } - - public override BroadphaseNativeTypes ShapeType - { - get - { - return BroadphaseNativeTypes.MultiSphere; - } - } - - public override string Name - { - get - { - return "MultiSphere"; - } - } - - public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vecA) - { - Vector3 supVec = new Vector3(); - - float maxDot = -1e30f; - - - Vector3 vec = vecA; - float lenSqr = vec.LengthSquared(); - if (lenSqr < 0.0001f) - { - vec = new Vector3(1, 0, 0); - } - else - { - float rlen = 1f / (float)Math.Sqrt(lenSqr); - vec *= rlen; - } - - Vector3 vtx; - float newDot; - - for (int i = 0; i < m_numSpheres; i++) - { - vtx = _localPositions[i] + vec * LocalScaling * _radi[i] - vec * Margin; - newDot = Vector3.Dot(vec, vtx); - if (newDot > maxDot) - { - maxDot = newDot; - supVec = vtx; - } - } - - return supVec; - } - - public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) - { - for (int j = 0; j < vectors.Length; j++) - { - float maxDot = -1e30f; - Vector3 vtx; - float newDot; - - for (int i = 0; i < m_numSpheres; i++) - { - vtx = _localPositions[i] + vectors[j] * LocalScaling * _radi[i] - vectors[j] * Margin; - newDot = Vector3.Dot(vectors[j], vtx); - if (newDot > maxDot) - { - maxDot = newDot; - supportVerticesOut[j] = vtx; - } - } - } - } - - public override void CalculateLocalInertia(float mass, out Vector3 inertia) - { - //as an approximation, take the inertia of the box that bounds the spheres - Matrix ident = Matrix.Identity; - Vector3 halfExtents = _inertiaHalfExtents; - - float margin = ConvexDistanceMargin; - - float lx = 2f * (halfExtents.X + margin); - float ly = 2f * (halfExtents.Y + margin); - float lz = 2f * (halfExtents.Z + margin); - float x2 = lx * lx; - float y2 = ly * ly; - float z2 = lz * lz; - float scaledmass = mass * 0.08333333f; - - inertia = new Vector3(); - inertia.X = scaledmass * (y2 + z2); - inertia.Y = scaledmass * (x2 + z2); - inertia.Z = scaledmass * (x2 + y2); - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// + /// MultiSphereShape represents implicit convex hull of a collection of spheres (using getSupportingVertex) + /// + public class MultiSphereShape : ConvexShape + { + private const int _maxNumSpheres = 5; + private Vector3[] _localPositions = new Vector3[MaxNumSpheres]; + private float[] _radi = new float[MaxNumSpheres]; + private Vector3 _inertiaHalfExtents; + + private int m_numSpheres; + + public MultiSphereShape(Vector3 inertiaHalfExtents, Vector3[] positions, float[] radi, int numSpheres) + { + _inertiaHalfExtents = inertiaHalfExtents; + float startMargin = 1e30f; + + m_numSpheres = numSpheres; + for (int i = 0; i < m_numSpheres; i++) + { + _localPositions[i] = positions[i]; + _radi[i] = radi[i]; + if (radi[i] < startMargin) + startMargin = radi[i]; + } + Margin = startMargin; + } + + public static int MaxNumSpheres { get { return _maxNumSpheres; } } + + public override BroadphaseNativeTypes ShapeType + { + get + { + return BroadphaseNativeTypes.MultiSphere; + } + } + + public override string Name + { + get + { + return "MultiSphere"; + } + } + + public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vecA) + { + Vector3 supVec = new Vector3(); + + float maxDot = -1e30f; + + + Vector3 vec = vecA; + float lenSqr = vec.LengthSquared(); + if (lenSqr < 0.0001f) + { + vec = new Vector3(1, 0, 0); + } + else + { + float rlen = 1f / (float)Math.Sqrt(lenSqr); + vec *= rlen; + } + + Vector3 vtx; + float newDot; + + for (int i = 0; i < m_numSpheres; i++) + { + vtx = _localPositions[i] + vec * LocalScaling * _radi[i] - vec * Margin; + newDot = Vector3.Dot(vec, vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + + return supVec; + } + + public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) + { + for (int j = 0; j < vectors.Length; j++) + { + float maxDot = -1e30f; + Vector3 vtx; + float newDot; + + for (int i = 0; i < m_numSpheres; i++) + { + vtx = _localPositions[i] + vectors[j] * LocalScaling * _radi[i] - vectors[j] * Margin; + newDot = Vector3.Dot(vectors[j], vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supportVerticesOut[j] = vtx; + } + } + } + } + + public override void CalculateLocalInertia(float mass, out Vector3 inertia) + { + //as an approximation, take the inertia of the box that bounds the spheres + Matrix ident = Matrix.Identity; + Vector3 halfExtents = _inertiaHalfExtents; + + float margin = ConvexDistanceMargin; + + float lx = 2f * (halfExtents.X + margin); + float ly = 2f * (halfExtents.Y + margin); + float lz = 2f * (halfExtents.Z + margin); + float x2 = lx * lx; + float y2 = ly * ly; + float z2 = lz * lz; + float scaledmass = mass * 0.08333333f; + + inertia = new Vector3(); + inertia.X = scaledmass * (y2 + z2); + inertia.Y = scaledmass * (x2 + z2); + inertia.Z = scaledmass * (x2 + y2); + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs index fac344e..324cdbc 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs @@ -1,32 +1,32 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; - -namespace XnaDevRu.BulletX -{ - public interface INodeOverlapCallback - { - void ProcessNode(OptimizedBvhNode node); - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace XnaDevRu.BulletX +{ + public interface INodeOverlapCallback + { + void ProcessNode(OptimizedBvhNode node); + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs index c22c990..5b7a5e4 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs @@ -1,293 +1,293 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// - /// OptimizedBvh store an AABB tree that can be quickly traversed on CPU (and SPU,GPU in future) - /// - public class OptimizedBvh - { - private static int _maxIterations = 0; - private OptimizedBvhNode _rootNode; - - private OptimizedBvhNode[] _contiguousNodes; - private int _curNodeIndex; - - private List _leafNodes = new List(); - - public OptimizedBvh() { } - - public void Build(StridingMeshInterface triangles) - { - NodeTriangleCallback callback = new NodeTriangleCallback(_leafNodes); - - Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); - Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); - - triangles.InternalProcessAllTriangles(callback, aabbMin, aabbMax); - - //now we have an array of leafnodes in m_leafNodes - _contiguousNodes = new OptimizedBvhNode[2 * _leafNodes.Count]; - for (int i = 0; i < _contiguousNodes.Length; i++) - _contiguousNodes[i] = new OptimizedBvhNode(); - _curNodeIndex = 0; - - _rootNode = BuildTree(_leafNodes, 0, _leafNodes.Count); - } - - public OptimizedBvhNode BuildTree(List leafNodes, int startIndex, int endIndex) - { - OptimizedBvhNode internalNode; - - int splitAxis, splitIndex, i; - int numIndices = endIndex - startIndex; - int curIndex = _curNodeIndex; - - if (numIndices <= 0) - throw new BulletException(); - - if (numIndices == 1) - { - _contiguousNodes[_curNodeIndex++] = leafNodes[startIndex]; - //return new (&m_contiguousNodes[m_curNodeIndex++]) btOptimizedBvhNode(leafNodes[startIndex]); - return leafNodes[startIndex]; - } - - //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. - splitAxis = CalculateSplittingAxis(leafNodes, startIndex, endIndex); - - splitIndex = SortAndCalculateSplittingIndex(leafNodes, startIndex, endIndex, splitAxis); - - internalNode = _contiguousNodes[_curNodeIndex++]; - - internalNode.AabbMax = new Vector3(-1e30f, -1e30f, -1e30f); - internalNode.AabbMin = new Vector3(1e30f, 1e30f, 1e30f); - - for (i = startIndex; i < endIndex; i++) - { - internalNode.AabbMax = MathHelper.SetMax(internalNode.AabbMax, leafNodes[i].AabbMax); - internalNode.AabbMin = MathHelper.SetMin(internalNode.AabbMin, leafNodes[i].AabbMin); - } - - //internalNode->m_escapeIndex; - internalNode.LeftChild = BuildTree(leafNodes, startIndex, splitIndex); - internalNode.RightChild = BuildTree(leafNodes, splitIndex, endIndex); - - internalNode.EscapeIndex = _curNodeIndex - curIndex; - return internalNode; - } - - public int CalculateSplittingAxis(List leafNodes, int startIndex, int endIndex) - { - Vector3 means = new Vector3(); - Vector3 variance = new Vector3(); - int numIndices = endIndex - startIndex; - - for (int i = startIndex; i < endIndex; i++) - { - Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); - means += center; - } - means *= (1f / (float)numIndices); - - for (int i = startIndex; i < endIndex; i++) - { - Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); - Vector3 diff2 = center - means; - diff2 = diff2 * diff2; - variance += diff2; - } - variance *= (1f / ((float)numIndices - 1)); - - return MathHelper.MaxAxis(variance); - } - - public int SortAndCalculateSplittingIndex(List leafNodes, int startIndex, int endIndex, int splitAxis) - { - int splitIndex = startIndex; - int numIndices = endIndex - startIndex; - float splitValue; - - Vector3 means = new Vector3(); - for (int i = startIndex; i < endIndex; i++) - { - Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); - means += center; - } - means *= (1f / (float)numIndices); - - if (splitAxis == 0) - splitValue = means.X; - else if (splitAxis == 1) - splitValue = means.Y; - else if (splitAxis == 2) - splitValue = means.Z; - else - throw new ArgumentException(); - - //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'. - for (int i = startIndex; i < endIndex; i++) - { - Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); - float centerSplit; - - if (splitAxis == 0) - centerSplit = means.X; - else if (splitAxis == 1) - centerSplit = means.Y; - else if (splitAxis == 2) - centerSplit = means.Z; - else - throw new ArgumentException(); - - if (centerSplit > splitValue) - { - //swap - OptimizedBvhNode tmp = leafNodes[i]; - leafNodes[i] = leafNodes[splitIndex]; - leafNodes[splitIndex] = tmp; - splitIndex++; - } - } - if ((splitIndex == startIndex) || (splitIndex == (endIndex - 1))) - { - splitIndex = startIndex + (numIndices >> 1); - } - return splitIndex; - } - - public void WalkTree(OptimizedBvhNode rootNode, INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) - { - bool isLeafNode, aabbOverlap = MathHelper.TestAabbAgainstAabb2(aabbMin, aabbMax, rootNode.AabbMin, rootNode.AabbMax); - if (aabbOverlap) - { - isLeafNode = (rootNode.LeftChild == null && rootNode.RightChild == null); - if (isLeafNode) - { - nodeCallback.ProcessNode(rootNode); - } - else - { - WalkTree(rootNode.LeftChild, nodeCallback, aabbMin, aabbMax); - WalkTree(rootNode.RightChild, nodeCallback, aabbMin, aabbMax); - } - } - } - - public void WalkStacklessTree(OptimizedBvhNode[] rootNodeArray, INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) - { - int escapeIndex, curIndex = 0; - int walkIterations = 0; - bool aabbOverlap, isLeafNode; - int rootNodeIndex = 0; - OptimizedBvhNode rootNode = rootNodeArray[rootNodeIndex]; - - while (curIndex < _curNodeIndex) - { - //catch bugs in tree data - if (walkIterations >= _curNodeIndex) - throw new BulletException(); - - walkIterations++; - aabbOverlap = MathHelper.TestAabbAgainstAabb2(aabbMin, aabbMax, rootNode.AabbMin, rootNode.AabbMax); - isLeafNode = (rootNode.LeftChild == null && rootNode.RightChild == null); - - if (isLeafNode && aabbOverlap) - { - nodeCallback.ProcessNode(rootNode); - } - - if (aabbOverlap || isLeafNode) - { - rootNodeIndex++; // this - curIndex++; - if (rootNodeIndex < rootNodeArray.Length) - rootNode = rootNodeArray[rootNodeIndex]; - } - else - { - escapeIndex = rootNode.EscapeIndex; - rootNodeIndex += escapeIndex; // and this - curIndex += escapeIndex; - if (rootNodeIndex < rootNodeArray.Length) - rootNode = rootNodeArray[rootNodeIndex]; - } - - } - - if (_maxIterations < walkIterations) - _maxIterations = walkIterations; - } - - public void ReportAabbOverlappingNodex(INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) - { - //either choose recursive traversal (walkTree) or stackless (walkStacklessTree) - //walkTree(m_rootNode1,nodeCallback,aabbMin,aabbMax); - //WalkStacklessTree(_rootNode, nodeCallback, aabbMin, aabbMax); - WalkStacklessTree(_contiguousNodes, nodeCallback, aabbMin, aabbMax); - } - - public void ReportSphereOverlappingNodex(INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) { } - } - - public class NodeTriangleCallback : ITriangleIndexCallback - { - private List _triangleNodes; - - public NodeTriangleCallback(List triangleNodes) - { - _triangleNodes = triangleNodes; - } - - public List TriangleNodes { get { return _triangleNodes; } set { _triangleNodes = value; } } - - public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) - { - - OptimizedBvhNode node = new OptimizedBvhNode(); - node.AabbMin = new Vector3(1e30f, 1e30f, 1e30f); - node.AabbMax = new Vector3(-1e30f, -1e30f, -1e30f); - - node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[0]); - node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[0]); - node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[1]); - node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[1]); - node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[2]); - node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[2]); - - node.EscapeIndex = -1; - node.LeftChild = null; - node.RightChild = null; - - //for child nodes - node.SubPart = partId; - node.TriangleIndex = triangleIndex; - - _triangleNodes.Add(node); - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// + /// OptimizedBvh store an AABB tree that can be quickly traversed on CPU (and SPU,GPU in future) + /// + public class OptimizedBvh + { + private static int _maxIterations = 0; + private OptimizedBvhNode _rootNode; + + private OptimizedBvhNode[] _contiguousNodes; + private int _curNodeIndex; + + private List _leafNodes = new List(); + + public OptimizedBvh() { } + + public void Build(StridingMeshInterface triangles) + { + NodeTriangleCallback callback = new NodeTriangleCallback(_leafNodes); + + Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); + Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); + + triangles.InternalProcessAllTriangles(callback, aabbMin, aabbMax); + + //now we have an array of leafnodes in m_leafNodes + _contiguousNodes = new OptimizedBvhNode[2 * _leafNodes.Count]; + for (int i = 0; i < _contiguousNodes.Length; i++) + _contiguousNodes[i] = new OptimizedBvhNode(); + _curNodeIndex = 0; + + _rootNode = BuildTree(_leafNodes, 0, _leafNodes.Count); + } + + public OptimizedBvhNode BuildTree(List leafNodes, int startIndex, int endIndex) + { + OptimizedBvhNode internalNode; + + int splitAxis, splitIndex, i; + int numIndices = endIndex - startIndex; + int curIndex = _curNodeIndex; + + if (numIndices <= 0) + throw new BulletException(); + + if (numIndices == 1) + { + _contiguousNodes[_curNodeIndex++] = leafNodes[startIndex]; + //return new (&m_contiguousNodes[m_curNodeIndex++]) btOptimizedBvhNode(leafNodes[startIndex]); + return leafNodes[startIndex]; + } + + //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. + splitAxis = CalculateSplittingAxis(leafNodes, startIndex, endIndex); + + splitIndex = SortAndCalculateSplittingIndex(leafNodes, startIndex, endIndex, splitAxis); + + internalNode = _contiguousNodes[_curNodeIndex++]; + + internalNode.AabbMax = new Vector3(-1e30f, -1e30f, -1e30f); + internalNode.AabbMin = new Vector3(1e30f, 1e30f, 1e30f); + + for (i = startIndex; i < endIndex; i++) + { + internalNode.AabbMax = MathHelper.SetMax(internalNode.AabbMax, leafNodes[i].AabbMax); + internalNode.AabbMin = MathHelper.SetMin(internalNode.AabbMin, leafNodes[i].AabbMin); + } + + //internalNode->m_escapeIndex; + internalNode.LeftChild = BuildTree(leafNodes, startIndex, splitIndex); + internalNode.RightChild = BuildTree(leafNodes, splitIndex, endIndex); + + internalNode.EscapeIndex = _curNodeIndex - curIndex; + return internalNode; + } + + public int CalculateSplittingAxis(List leafNodes, int startIndex, int endIndex) + { + Vector3 means = new Vector3(); + Vector3 variance = new Vector3(); + int numIndices = endIndex - startIndex; + + for (int i = startIndex; i < endIndex; i++) + { + Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); + means += center; + } + means *= (1f / (float)numIndices); + + for (int i = startIndex; i < endIndex; i++) + { + Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); + Vector3 diff2 = center - means; + diff2 = diff2 * diff2; + variance += diff2; + } + variance *= (1f / ((float)numIndices - 1)); + + return MathHelper.MaxAxis(variance); + } + + public int SortAndCalculateSplittingIndex(List leafNodes, int startIndex, int endIndex, int splitAxis) + { + int splitIndex = startIndex; + int numIndices = endIndex - startIndex; + float splitValue; + + Vector3 means = new Vector3(); + for (int i = startIndex; i < endIndex; i++) + { + Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); + means += center; + } + means *= (1f / (float)numIndices); + + if (splitAxis == 0) + splitValue = means.X; + else if (splitAxis == 1) + splitValue = means.Y; + else if (splitAxis == 2) + splitValue = means.Z; + else + throw new ArgumentException(); + + //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'. + for (int i = startIndex; i < endIndex; i++) + { + Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); + float centerSplit; + + if (splitAxis == 0) + centerSplit = means.X; + else if (splitAxis == 1) + centerSplit = means.Y; + else if (splitAxis == 2) + centerSplit = means.Z; + else + throw new ArgumentException(); + + if (centerSplit > splitValue) + { + //swap + OptimizedBvhNode tmp = leafNodes[i]; + leafNodes[i] = leafNodes[splitIndex]; + leafNodes[splitIndex] = tmp; + splitIndex++; + } + } + if ((splitIndex == startIndex) || (splitIndex == (endIndex - 1))) + { + splitIndex = startIndex + (numIndices >> 1); + } + return splitIndex; + } + + public void WalkTree(OptimizedBvhNode rootNode, INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) + { + bool isLeafNode, aabbOverlap = MathHelper.TestAabbAgainstAabb2(aabbMin, aabbMax, rootNode.AabbMin, rootNode.AabbMax); + if (aabbOverlap) + { + isLeafNode = (rootNode.LeftChild == null && rootNode.RightChild == null); + if (isLeafNode) + { + nodeCallback.ProcessNode(rootNode); + } + else + { + WalkTree(rootNode.LeftChild, nodeCallback, aabbMin, aabbMax); + WalkTree(rootNode.RightChild, nodeCallback, aabbMin, aabbMax); + } + } + } + + public void WalkStacklessTree(OptimizedBvhNode[] rootNodeArray, INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) + { + int escapeIndex, curIndex = 0; + int walkIterations = 0; + bool aabbOverlap, isLeafNode; + int rootNodeIndex = 0; + OptimizedBvhNode rootNode = rootNodeArray[rootNodeIndex]; + + while (curIndex < _curNodeIndex) + { + //catch bugs in tree data + if (walkIterations >= _curNodeIndex) + throw new BulletException(); + + walkIterations++; + aabbOverlap = MathHelper.TestAabbAgainstAabb2(aabbMin, aabbMax, rootNode.AabbMin, rootNode.AabbMax); + isLeafNode = (rootNode.LeftChild == null && rootNode.RightChild == null); + + if (isLeafNode && aabbOverlap) + { + nodeCallback.ProcessNode(rootNode); + } + + if (aabbOverlap || isLeafNode) + { + rootNodeIndex++; // this + curIndex++; + if (rootNodeIndex < rootNodeArray.Length) + rootNode = rootNodeArray[rootNodeIndex]; + } + else + { + escapeIndex = rootNode.EscapeIndex; + rootNodeIndex += escapeIndex; // and this + curIndex += escapeIndex; + if (rootNodeIndex < rootNodeArray.Length) + rootNode = rootNodeArray[rootNodeIndex]; + } + + } + + if (_maxIterations < walkIterations) + _maxIterations = walkIterations; + } + + public void ReportAabbOverlappingNodex(INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) + { + //either choose recursive traversal (walkTree) or stackless (walkStacklessTree) + //walkTree(m_rootNode1,nodeCallback,aabbMin,aabbMax); + //WalkStacklessTree(_rootNode, nodeCallback, aabbMin, aabbMax); + WalkStacklessTree(_contiguousNodes, nodeCallback, aabbMin, aabbMax); + } + + public void ReportSphereOverlappingNodex(INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) { } + } + + public class NodeTriangleCallback : ITriangleIndexCallback + { + private List _triangleNodes; + + public NodeTriangleCallback(List triangleNodes) + { + _triangleNodes = triangleNodes; + } + + public List TriangleNodes { get { return _triangleNodes; } set { _triangleNodes = value; } } + + public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) + { + + OptimizedBvhNode node = new OptimizedBvhNode(); + node.AabbMin = new Vector3(1e30f, 1e30f, 1e30f); + node.AabbMax = new Vector3(-1e30f, -1e30f, -1e30f); + + node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[0]); + node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[0]); + node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[1]); + node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[1]); + node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[2]); + node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[2]); + + node.EscapeIndex = -1; + node.LeftChild = null; + node.RightChild = null; + + //for child nodes + node.SubPart = partId; + node.TriangleIndex = triangleIndex; + + _triangleNodes.Add(node); + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs index d43f2fb..83cc6e5 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs @@ -1,63 +1,63 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// - /// OptimizedBvhNode contains both internal and leaf node information. - /// It hasn't been optimized yet for storage. Some obvious optimizations are: - /// Removal of the pointers (can already be done, they are not used for traversal) - /// and storing aabbmin/max as quantized integers. - /// 'subpart' doesn't need an integer either. It allows to re-use graphics triangle - /// meshes stored in a non-uniform way (like batches/subparts of triangle-fans - /// - public class OptimizedBvhNode - { - private Vector3 _aabbMin; - private Vector3 _aabbMax; - - //these 2 pointers are obsolete, the stackless traversal just uses the escape index - private OptimizedBvhNode _leftChild; - private OptimizedBvhNode _rightChild; - - private int _escapeIndex; - - //for child nodes - private int _subPart; - private int _triangleIndex; - - public Vector3 AabbMin { get { return _aabbMin; } set { _aabbMin = value; } } - public Vector3 AabbMax { get { return _aabbMax; } set { _aabbMax = value; } } - - public OptimizedBvhNode LeftChild { get { return _leftChild; } set { _leftChild = value; } } - public OptimizedBvhNode RightChild { get { return _rightChild; } set { _rightChild = value; } } - - public int EscapeIndex { get { return _escapeIndex; } set { _escapeIndex = value; } } - - public int SubPart { get { return _subPart; } set { _subPart = value; } } - public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// + /// OptimizedBvhNode contains both internal and leaf node information. + /// It hasn't been optimized yet for storage. Some obvious optimizations are: + /// Removal of the pointers (can already be done, they are not used for traversal) + /// and storing aabbmin/max as quantized integers. + /// 'subpart' doesn't need an integer either. It allows to re-use graphics triangle + /// meshes stored in a non-uniform way (like batches/subparts of triangle-fans + /// + public class OptimizedBvhNode + { + private Vector3 _aabbMin; + private Vector3 _aabbMax; + + //these 2 pointers are obsolete, the stackless traversal just uses the escape index + private OptimizedBvhNode _leftChild; + private OptimizedBvhNode _rightChild; + + private int _escapeIndex; + + //for child nodes + private int _subPart; + private int _triangleIndex; + + public Vector3 AabbMin { get { return _aabbMin; } set { _aabbMin = value; } } + public Vector3 AabbMax { get { return _aabbMax; } set { _aabbMax = value; } } + + public OptimizedBvhNode LeftChild { get { return _leftChild; } set { _leftChild = value; } } + public OptimizedBvhNode RightChild { get { return _rightChild; } set { _rightChild = value; } } + + public int EscapeIndex { get { return _escapeIndex; } set { _escapeIndex = value; } } + + public int SubPart { get { return _subPart; } set { _subPart = value; } } + public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs index f61371a..1a911b3 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs @@ -1,133 +1,133 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public abstract class PolyhedralConvexShape : ConvexShape - { - public PolyhedralConvexShape() - { - //m_optionalHull = null; - } - - public abstract int VertexCount { get; } - public abstract int EdgeCount { get; } - public abstract int PlaneCount { get; } - - public abstract void GetEdge(int i, out Vector3 pointA, out Vector3 pointB); - public abstract void GetVertex(int i, out Vector3 vertex); - public abstract void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i); - // abstract int getIndex(int i); - - public abstract bool IsInside(Vector3 point, float tolerance); - - // optional Hull is for optional Separating Axis Test Hull collision detection, see Hull.cpp - //public class Hull m_optionalHull; - - public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) - { - Vector3 supVec = new Vector3(); - - float maxDot = -1e30f; - - float lenSqr = vec.LengthSquared(); - if (lenSqr < 0.0001f) - { - vec = new Vector3(1, 0, 0); - } - else - { - float rlen = 1f / (float)Math.Sqrt(lenSqr); - vec *= rlen; - } - - Vector3 vtx; - float newDot; - - for (int i = 0; i < VertexCount; i++) - { - GetVertex(i, out vtx); - newDot = Vector3.Dot(vec, vtx); - if (newDot > maxDot) - { - maxDot = newDot; - supVec = vtx; - } - } - return supVec; - } - - public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) - { - #warning Think about this - /*Vector3 vtx; - float newDot; - - for (int i = 0; i < vectors.Length; i++) - { - supportVerticesOut[i][3] = -1e30f; - } - - for (int j = 0; j < vectors.Length; j++) - { - Vector3 vec = vectors[j]; - - for (int i = 0; i < getNumVertices(); i++) - { - getVertex(i, out vtx); - newDot = Vector3.Dot(vec,vtx); - if (newDot > supportVerticesOut[j][3]) - { - //WARNING: don't swap next lines, the w component would get overwritten! - supportVerticesOut[j] = vtx; - supportVerticesOut[j][3] = newDot; - } - } - }*/ - } - - public override void CalculateLocalInertia(float mass, out Vector3 inertia) - { - //not yet, return box inertia - float margin = Margin; - - Matrix ident = Matrix.Identity; - Vector3 aabbMin, aabbMax; - GetAabb(ident, out aabbMin, out aabbMax); - Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; - - float lx = 2f * (halfExtents.X + margin); - float ly = 2f * (halfExtents.Y + margin); - float lz = 2f * (halfExtents.Z + margin); - float x2 = lx * lx; - float y2 = ly * ly; - float z2 = lz * lz; - float scaledmass = mass * 0.08333333f; - - inertia = scaledmass * (new Vector3(y2 + z2, x2 + z2, x2 + y2)); - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public abstract class PolyhedralConvexShape : ConvexShape + { + public PolyhedralConvexShape() + { + //m_optionalHull = null; + } + + public abstract int VertexCount { get; } + public abstract int EdgeCount { get; } + public abstract int PlaneCount { get; } + + public abstract void GetEdge(int i, out Vector3 pointA, out Vector3 pointB); + public abstract void GetVertex(int i, out Vector3 vertex); + public abstract void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i); + // abstract int getIndex(int i); + + public abstract bool IsInside(Vector3 point, float tolerance); + + // optional Hull is for optional Separating Axis Test Hull collision detection, see Hull.cpp + //public class Hull m_optionalHull; + + public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) + { + Vector3 supVec = new Vector3(); + + float maxDot = -1e30f; + + float lenSqr = vec.LengthSquared(); + if (lenSqr < 0.0001f) + { + vec = new Vector3(1, 0, 0); + } + else + { + float rlen = 1f / (float)Math.Sqrt(lenSqr); + vec *= rlen; + } + + Vector3 vtx; + float newDot; + + for (int i = 0; i < VertexCount; i++) + { + GetVertex(i, out vtx); + newDot = Vector3.Dot(vec, vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + return supVec; + } + + public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) + { + #warning Think about this + /*Vector3 vtx; + float newDot; + + for (int i = 0; i < vectors.Length; i++) + { + supportVerticesOut[i][3] = -1e30f; + } + + for (int j = 0; j < vectors.Length; j++) + { + Vector3 vec = vectors[j]; + + for (int i = 0; i < getNumVertices(); i++) + { + getVertex(i, out vtx); + newDot = Vector3.Dot(vec,vtx); + if (newDot > supportVerticesOut[j][3]) + { + //WARNING: don't swap next lines, the w component would get overwritten! + supportVerticesOut[j] = vtx; + supportVerticesOut[j][3] = newDot; + } + } + }*/ + } + + public override void CalculateLocalInertia(float mass, out Vector3 inertia) + { + //not yet, return box inertia + float margin = Margin; + + Matrix ident = Matrix.Identity; + Vector3 aabbMin, aabbMax; + GetAabb(ident, out aabbMin, out aabbMax); + Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; + + float lx = 2f * (halfExtents.X + margin); + float ly = 2f * (halfExtents.Y + margin); + float lz = 2f * (halfExtents.Z + margin); + float x2 = lx * lx; + float y2 = ly * ly; + float z2 = lz * lz; + float scaledmass = mass * 0.08333333f; + + inertia = scaledmass * (new Vector3(y2 + z2, x2 + z2, x2 + y2)); + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs index c660a6f..cc3cea9 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs @@ -1,116 +1,116 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// - /// btSphereShape implements an implicit (getSupportingVertex) Sphere - /// - public class SphereShape : ConvexShape - { - public SphereShape(float radius) - : base() - { - Vector3 temp = ImplicitShapeDimensions; - temp.X = radius; - ImplicitShapeDimensions = temp; - } - - public float Radius { get { return ImplicitShapeDimensions.X; } } - - public override BroadphaseNativeTypes ShapeType - { - get - { - return BroadphaseNativeTypes.Sphere; - } - } - - public override string Name - { - get - { - return "Sphere"; - } - } - - public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) - { - return new Vector3(); - } - - /// - /// to improve gjk behaviour, use radius+margin as the full margin, so never get into the penetration case - /// this means, non-uniform scaling is not supported anymore - /// - public override float Margin - { - get - { - return LocalScaling.X * Radius + base.Margin; - } - set - { - base.Margin = value; - } - } - - public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) - { - if (supportVerticesOut != null) - for (int i = 0; i < supportVerticesOut.Length; i++) - supportVerticesOut[i] = new Vector3(); - } - - public override void CalculateLocalInertia(float mass, out Vector3 inertia) - { - float elem = 0.4f * mass * Margin * Margin; - inertia = new Vector3(elem, elem, elem); - } - - public override Vector3 LocalGetSupportingVertex(Vector3 vec) - { - Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); - - Vector3 vecnorm = vec; - if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) - { - vecnorm = new Vector3(-1f, -1f, -1f); - } - vecnorm.Normalize(); - supVertex += Margin * vecnorm; - return supVertex; - } - - public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) - { - Vector3 center = t.Translation; - Vector3 extent = new Vector3(Margin, Margin, Margin); - aabbMin = center - extent; - aabbMax = center + extent; - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// + /// btSphereShape implements an implicit (getSupportingVertex) Sphere + /// + public class SphereShape : ConvexShape + { + public SphereShape(float radius) + : base() + { + Vector3 temp = ImplicitShapeDimensions; + temp.X = radius; + ImplicitShapeDimensions = temp; + } + + public float Radius { get { return ImplicitShapeDimensions.X; } } + + public override BroadphaseNativeTypes ShapeType + { + get + { + return BroadphaseNativeTypes.Sphere; + } + } + + public override string Name + { + get + { + return "Sphere"; + } + } + + public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) + { + return new Vector3(); + } + + /// + /// to improve gjk behaviour, use radius+margin as the full margin, so never get into the penetration case + /// this means, non-uniform scaling is not supported anymore + /// + public override float Margin + { + get + { + return LocalScaling.X * Radius + base.Margin; + } + set + { + base.Margin = value; + } + } + + public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) + { + if (supportVerticesOut != null) + for (int i = 0; i < supportVerticesOut.Length; i++) + supportVerticesOut[i] = new Vector3(); + } + + public override void CalculateLocalInertia(float mass, out Vector3 inertia) + { + float elem = 0.4f * mass * Margin * Margin; + inertia = new Vector3(elem, elem, elem); + } + + public override Vector3 LocalGetSupportingVertex(Vector3 vec) + { + Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); + + Vector3 vecnorm = vec; + if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) + { + vecnorm = new Vector3(-1f, -1f, -1f); + } + vecnorm.Normalize(); + supVertex += Margin * vecnorm; + return supVertex; + } + + public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) + { + Vector3 center = t.Translation; + Vector3 extent = new Vector3(Margin, Margin, Margin); + aabbMin = center - extent; + aabbMax = center + extent; + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs index 49604a0..a79a145 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs @@ -1,124 +1,124 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public class StaticPlaneShape : ConcaveShape - { - private Vector3 _localAabbMin; - private Vector3 _localAabbMax; - - private Vector3 _planeNormal; - private float _planeConstant; - private Vector3 _localScaling; - - public StaticPlaneShape(Vector3 planeNormal, float planeConstant) - { - _planeNormal = planeNormal; - _planeConstant = planeConstant; - _localScaling = new Vector3(); - } - - protected Vector3 LocalAabbMin { get { return _localAabbMin; } set { _localAabbMin = value; } } - protected Vector3 LocalAabbMax { get { return _localAabbMax; } set { _localAabbMax = value; } } - - protected Vector3 PlaneNormal { get { return _planeNormal; } set { _planeNormal = value; } } - protected float PlaneConstant { get { return _planeConstant; } set { _planeConstant = value; } } - - public override BroadphaseNativeTypes ShapeType - { - get - { - return BroadphaseNativeTypes.StaticPlane; - } - } - - public override Vector3 LocalScaling - { - get - { - return _localScaling; - } - set - { - _localScaling = value; - } - } - - public override string Name - { - get - { - return "StaticPlane"; - } - } - - public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) - { - Vector3 infvec = new Vector3(1e30f, 1e30f, 1e30f); - - Vector3 center = _planeNormal * _planeConstant; - aabbMin = center + infvec * _planeNormal; - aabbMax = aabbMin; - MathHelper.SetMin(ref aabbMin, center - infvec * _planeNormal); - MathHelper.SetMax(ref aabbMax, center - infvec * _planeNormal); - - aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); - aabbMax = new Vector3(1e30f, 1e30f, 1e30f); - } - - public override void CalculateLocalInertia(float mass, out Vector3 inertia) - { - //moving concave objects not supported - inertia = new Vector3(); - } - - public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) { - Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; - float radius = halfExtents.Length(); - Vector3 center = (aabbMax + aabbMin) * 0.5f; - - //this is where the triangles are generated, given AABB and plane equation (normal/constant) - Vector3 tangentDir0 = new Vector3(), tangentDir1 = new Vector3(); - - //tangentDir0/tangentDir1 can be precalculated - MathHelper.PlaneSpace1(_planeNormal, ref tangentDir0, ref tangentDir1); - - Vector3 projectedCenter = center - (Vector3.Dot(_planeNormal, center) - _planeConstant) * _planeNormal; - - Vector3[] triangle = new Vector3[3]; - triangle[0] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius; - triangle[1] = projectedCenter + tangentDir0 * radius - tangentDir1 * radius; - triangle[2] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius; - callback.ProcessTriangle(triangle, 0, 0); - - triangle[0] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius; - triangle[1] = projectedCenter - tangentDir0 * radius + tangentDir1 * radius; - triangle[2] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius; - callback.ProcessTriangle(triangle, 0, 1); - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public class StaticPlaneShape : ConcaveShape + { + private Vector3 _localAabbMin; + private Vector3 _localAabbMax; + + private Vector3 _planeNormal; + private float _planeConstant; + private Vector3 _localScaling; + + public StaticPlaneShape(Vector3 planeNormal, float planeConstant) + { + _planeNormal = planeNormal; + _planeConstant = planeConstant; + _localScaling = new Vector3(); + } + + protected Vector3 LocalAabbMin { get { return _localAabbMin; } set { _localAabbMin = value; } } + protected Vector3 LocalAabbMax { get { return _localAabbMax; } set { _localAabbMax = value; } } + + protected Vector3 PlaneNormal { get { return _planeNormal; } set { _planeNormal = value; } } + protected float PlaneConstant { get { return _planeConstant; } set { _planeConstant = value; } } + + public override BroadphaseNativeTypes ShapeType + { + get + { + return BroadphaseNativeTypes.StaticPlane; + } + } + + public override Vector3 LocalScaling + { + get + { + return _localScaling; + } + set + { + _localScaling = value; + } + } + + public override string Name + { + get + { + return "StaticPlane"; + } + } + + public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) + { + Vector3 infvec = new Vector3(1e30f, 1e30f, 1e30f); + + Vector3 center = _planeNormal * _planeConstant; + aabbMin = center + infvec * _planeNormal; + aabbMax = aabbMin; + MathHelper.SetMin(ref aabbMin, center - infvec * _planeNormal); + MathHelper.SetMax(ref aabbMax, center - infvec * _planeNormal); + + aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); + aabbMax = new Vector3(1e30f, 1e30f, 1e30f); + } + + public override void CalculateLocalInertia(float mass, out Vector3 inertia) + { + //moving concave objects not supported + inertia = new Vector3(); + } + + public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) { + Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; + float radius = halfExtents.Length(); + Vector3 center = (aabbMax + aabbMin) * 0.5f; + + //this is where the triangles are generated, given AABB and plane equation (normal/constant) + Vector3 tangentDir0 = new Vector3(), tangentDir1 = new Vector3(); + + //tangentDir0/tangentDir1 can be precalculated + MathHelper.PlaneSpace1(_planeNormal, ref tangentDir0, ref tangentDir1); + + Vector3 projectedCenter = center - (Vector3.Dot(_planeNormal, center) - _planeConstant) * _planeNormal; + + Vector3[] triangle = new Vector3[3]; + triangle[0] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius; + triangle[1] = projectedCenter + tangentDir0 * radius - tangentDir1 * radius; + triangle[2] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius; + callback.ProcessTriangle(triangle, 0, 0); + + triangle[0] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius; + triangle[1] = projectedCenter - tangentDir0 * radius + tangentDir1 * radius; + triangle[2] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius; + callback.ProcessTriangle(triangle, 0, 1); + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs index 2a2ce27..20baccb 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs @@ -1,115 +1,115 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// - /// PHY_ScalarType enumerates possible scalar types. - /// See the StridingMeshInterface for its use - /// - public enum PHY_ScalarType - { - PHY_FLOAT, - PHY_DOUBLE, - PHY_INTEGER, - PHY_SHORT, - PHY_FIXEDPOINT88 - } - - /// - /// StridingMeshInterface is the interface class for high performance access to triangle meshes - /// It allows for sharing graphics and collision meshes. Also it provides locking/unlocking of graphics meshes that are in gpu memory. - /// - public abstract class StridingMeshInterface - { - protected Vector3 _scaling; - - public StridingMeshInterface() - { - _scaling = new Vector3(1f,1f,1f); - } - - public void InternalProcessAllTriangles(ITriangleIndexCallback callback, Vector3 aabbMin, Vector3 aabbMax) - { - int numtotalphysicsverts = 0; - int numtriangles, gfxindex; - int part, graphicssubparts = SubPartsCount(); - Vector3[] triangle = new Vector3[3]; - List verts; - List indicies; - - Vector3 meshScaling = Scaling; - - //if the number of parts is big, the performance might drop due to the innerloop switch on indextype - for (part = 0; part < graphicssubparts; part++) - { - GetLockedReadOnlyVertexIndexBase(out verts, out indicies, out numtriangles, part); - numtotalphysicsverts += numtriangles * 3; //upper bound - - for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) - { - triangle[0] = verts[indicies[gfxindex * 3 + 0]]; - triangle[1] = verts[indicies[gfxindex * 3 + 1]]; - triangle[2] = verts[indicies[gfxindex * 3 + 2]]; - - callback.ProcessTriangleIndex(triangle, part, gfxindex); - } - - UnLockReadOnlyVertexBase(part); - } - } - - - // get read and write access to a subpart of a triangle mesh - // this subpart has a continuous array of vertices and indices - // in this way the mesh can be handled as chunks of memory with striding - // very similar to OpenGL vertexarray support - // make a call to unLockVertexBase when the read and write access is finished - public abstract void GetLockedVertexIndexBase(out List verts, out List indicies, out int numfaces, int subpart); - - public abstract void GetLockedReadOnlyVertexIndexBase(out List verts, out List indicies, out int numfaces, int subpart); - - // unLockVertexBase finishes the access to a subpart of the triangle mesh - // make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished - public abstract void UnLockVertexBase(int subpart); - - public abstract void UnLockReadOnlyVertexBase(int subpart); - - - // getNumSubParts returns the number of seperate subparts - // each subpart has a continuous array of vertices and indices - public abstract int SubPartsCount(); - - public abstract void PreallocateVertices(int numverts); - public abstract void PreallocateIndices(int numindices); - - public Vector3 Scaling - { - get { return _scaling; } - set { _scaling = value; } - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// + /// PHY_ScalarType enumerates possible scalar types. + /// See the StridingMeshInterface for its use + /// + public enum PHY_ScalarType + { + PHY_FLOAT, + PHY_DOUBLE, + PHY_INTEGER, + PHY_SHORT, + PHY_FIXEDPOINT88 + } + + /// + /// StridingMeshInterface is the interface class for high performance access to triangle meshes + /// It allows for sharing graphics and collision meshes. Also it provides locking/unlocking of graphics meshes that are in gpu memory. + /// + public abstract class StridingMeshInterface + { + protected Vector3 _scaling; + + public StridingMeshInterface() + { + _scaling = new Vector3(1f,1f,1f); + } + + public void InternalProcessAllTriangles(ITriangleIndexCallback callback, Vector3 aabbMin, Vector3 aabbMax) + { + int numtotalphysicsverts = 0; + int numtriangles, gfxindex; + int part, graphicssubparts = SubPartsCount(); + Vector3[] triangle = new Vector3[3]; + List verts; + List indicies; + + Vector3 meshScaling = Scaling; + + //if the number of parts is big, the performance might drop due to the innerloop switch on indextype + for (part = 0; part < graphicssubparts; part++) + { + GetLockedReadOnlyVertexIndexBase(out verts, out indicies, out numtriangles, part); + numtotalphysicsverts += numtriangles * 3; //upper bound + + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) + { + triangle[0] = verts[indicies[gfxindex * 3 + 0]]; + triangle[1] = verts[indicies[gfxindex * 3 + 1]]; + triangle[2] = verts[indicies[gfxindex * 3 + 2]]; + + callback.ProcessTriangleIndex(triangle, part, gfxindex); + } + + UnLockReadOnlyVertexBase(part); + } + } + + + // get read and write access to a subpart of a triangle mesh + // this subpart has a continuous array of vertices and indices + // in this way the mesh can be handled as chunks of memory with striding + // very similar to OpenGL vertexarray support + // make a call to unLockVertexBase when the read and write access is finished + public abstract void GetLockedVertexIndexBase(out List verts, out List indicies, out int numfaces, int subpart); + + public abstract void GetLockedReadOnlyVertexIndexBase(out List verts, out List indicies, out int numfaces, int subpart); + + // unLockVertexBase finishes the access to a subpart of the triangle mesh + // make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished + public abstract void UnLockVertexBase(int subpart); + + public abstract void UnLockReadOnlyVertexBase(int subpart); + + + // getNumSubParts returns the number of seperate subparts + // each subpart has a continuous array of vertices and indices + public abstract int SubPartsCount(); + + public abstract void PreallocateVertices(int numverts); + public abstract void PreallocateIndices(int numindices); + + public Vector3 Scaling + { + get { return _scaling; } + set { _scaling = value; } + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs index ffb5813..eab822e 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs @@ -1,67 +1,67 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - internal class SupportVertexCallback : ITriangleCallback - { - private Vector3 _supportVertexLocal; - - private Matrix _worldTransform; - private float _maxDot; - private Vector3 _supportVecLocal; - - public SupportVertexCallback(Vector3 supportVecWorld, Matrix trans) - { - _supportVertexLocal = new Vector3(); - _worldTransform = trans; - _maxDot = -1e30f; - _supportVecLocal = Vector3.TransformNormal(supportVecWorld, _worldTransform); - } - - public Matrix WorldTransform { get { return _worldTransform; } set { _worldTransform = value; } } - public float MaxDot { get { return _maxDot; } set { _maxDot = value; } } - public Vector3 SupportVectorLocal { get { return _supportVecLocal; } set { _supportVecLocal = value; } } - - public Vector3 SupportVertexLocal { get { return _supportVertexLocal; } } - public Vector3 SupportVertexWorldSpace { get { return MathHelper.MatrixToVector(_worldTransform, _supportVertexLocal); } } - - #region ITriangleCallback Members - public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) - { - for (int i = 0; i < 3; i++) - { - float dot = Vector3.Dot(_supportVecLocal, triangle[i]); - if (dot > _maxDot) - { - _maxDot = dot; - _supportVertexLocal = triangle[i]; - } - } - } - #endregion - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + internal class SupportVertexCallback : ITriangleCallback + { + private Vector3 _supportVertexLocal; + + private Matrix _worldTransform; + private float _maxDot; + private Vector3 _supportVecLocal; + + public SupportVertexCallback(Vector3 supportVecWorld, Matrix trans) + { + _supportVertexLocal = new Vector3(); + _worldTransform = trans; + _maxDot = -1e30f; + _supportVecLocal = Vector3.TransformNormal(supportVecWorld, _worldTransform); + } + + public Matrix WorldTransform { get { return _worldTransform; } set { _worldTransform = value; } } + public float MaxDot { get { return _maxDot; } set { _maxDot = value; } } + public Vector3 SupportVectorLocal { get { return _supportVecLocal; } set { _supportVecLocal = value; } } + + public Vector3 SupportVertexLocal { get { return _supportVertexLocal; } } + public Vector3 SupportVertexWorldSpace { get { return MathHelper.MatrixToVector(_worldTransform, _supportVertexLocal); } } + + #region ITriangleCallback Members + public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) + { + for (int i = 0; i < 3; i++) + { + float dot = Vector3.Dot(_supportVecLocal, triangle[i]); + if (dot > _maxDot) + { + _maxDot = dot; + _supportVertexLocal = triangle[i]; + } + } + } + #endregion + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs index 158e42a..a472583 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs @@ -1,80 +1,80 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public class Triangle - { - private Vector3 _vertexA; - private Vector3 _vertexB; - private Vector3 _vertexC; - private int _partId; - private int _triangleIndex; - - public Vector3 VertexA { get { return _vertexA; } set { _vertexA = value; } } - public Vector3 VertexB { get { return _vertexB; } set { _vertexB = value; } } - public Vector3 VertexC { get { return _vertexC; } set { _vertexC = value; } } - public int PartId { get { return _partId; } set { _partId = value; } } - public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } } - } - - /// - /// example usage of this class: - /// TriangleBuffer triBuf; - /// concaveShape.processAllTriangles(triBuf, out aabbMin, out aabbMax); - /// for (int i = 0; i < triBuf.getNumTriangles(); i++) - /// { - /// Triangle tri = triBuf.getTriangle(i); - /// //do something useful here with the triangle - /// } - /// - public class TriangleBuffer : ITriangleCallback - { - private List _triangleBuffer = new List(); - - public int TriangleCount { get { return _triangleBuffer.Count; } } - public Triangle this[int index] { get { return _triangleBuffer[index]; } } - - public void ClearBuffer() - { - _triangleBuffer.Clear(); - } - - #region ITriangleCallback Members - public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) - { - Triangle tri = new Triangle(); - tri.VertexA = triangle[0]; - tri.VertexB = triangle[1]; - tri.VertexC = triangle[2]; - tri.PartId = partID; - tri.TriangleIndex = triangleIndex; - - _triangleBuffer.Add(tri); - } - #endregion - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public class Triangle + { + private Vector3 _vertexA; + private Vector3 _vertexB; + private Vector3 _vertexC; + private int _partId; + private int _triangleIndex; + + public Vector3 VertexA { get { return _vertexA; } set { _vertexA = value; } } + public Vector3 VertexB { get { return _vertexB; } set { _vertexB = value; } } + public Vector3 VertexC { get { return _vertexC; } set { _vertexC = value; } } + public int PartId { get { return _partId; } set { _partId = value; } } + public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } } + } + + /// + /// example usage of this class: + /// TriangleBuffer triBuf; + /// concaveShape.processAllTriangles(triBuf, out aabbMin, out aabbMax); + /// for (int i = 0; i < triBuf.getNumTriangles(); i++) + /// { + /// Triangle tri = triBuf.getTriangle(i); + /// //do something useful here with the triangle + /// } + /// + public class TriangleBuffer : ITriangleCallback + { + private List _triangleBuffer = new List(); + + public int TriangleCount { get { return _triangleBuffer.Count; } } + public Triangle this[int index] { get { return _triangleBuffer[index]; } } + + public void ClearBuffer() + { + _triangleBuffer.Clear(); + } + + #region ITriangleCallback Members + public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) + { + Triangle tri = new Triangle(); + tri.VertexA = triangle[0]; + tri.VertexB = triangle[1]; + tri.VertexC = triangle[2]; + tri.PartId = partID; + tri.TriangleIndex = triangleIndex; + + _triangleBuffer.Add(tri); + } + #endregion + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs index ac892c2..938765b 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs @@ -1,33 +1,33 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public interface ITriangleCallback - { - void ProcessTriangle(Vector3[] triangle, int partId, int triangleIndex); - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public interface ITriangleCallback + { + void ProcessTriangle(Vector3[] triangle, int partId, int triangleIndex); + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs index 6d0d7b5..52c08ed 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs @@ -1,136 +1,136 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// - /// IndexedMesh indexes into existing vertex and index arrays, in a similar way OpenGL glDrawElements - /// instead of the number of indices, we pass the number of triangles - /// - public struct IndexedMesh - { - private int _numTriangles; - private int[] _triangleIndexBase; - private int _triangleIndexStride; - private int _numVertices; - private Vector3[] _vertexBase; - private int _vertexStride; - - public IndexedMesh(int numTriangleIndices, int[] triangleIndexBase, int triangleIndexStride, int numVertices, Vector3[] vertexBase, int vertexStride) - { - _numTriangles = numTriangleIndices; - _triangleIndexBase = triangleIndexBase; - _triangleIndexStride = triangleIndexStride; - _vertexBase = vertexBase; - _numVertices = numVertices; - _vertexStride = vertexStride; - } - - public IndexedMesh(int[] triangleIndexBase, Vector3[] vertexBase) - { - _numTriangles = triangleIndexBase.Length; - _triangleIndexBase = triangleIndexBase; - _triangleIndexStride = 32; - _vertexBase = vertexBase; - _numVertices = vertexBase.Length; - _vertexStride = 24; - } - - public int TriangleCount { get { return _numTriangles; } set { _numTriangles = value; } } - public int[] TriangleIndexBase { get { return _triangleIndexBase; } set { _triangleIndexBase = value; } } - public int TriangleIndexStride { get { return _triangleIndexStride; } set { _triangleIndexStride = value; } } - public int VertexCount { get { return _numVertices; } set { _numVertices = value; } } - public Vector3[] VertexBase { get { return _vertexBase; } set { _vertexBase = value; } } - public int VertexStride { get { return _vertexStride; } set { _vertexStride = value; } } - } - - /// - /// TriangleIndexVertexArray allows to use multiple meshes, by indexing into existing triangle/index arrays. - /// Additional meshes can be added using addIndexedMesh - /// - public class TriangleIndexVertexArray : StridingMeshInterface - { - List _indexedMeshes = new List(); - - public TriangleIndexVertexArray() { } - - public TriangleIndexVertexArray(int numTriangleIndices, int[] triangleIndexBase, int triangleIndexStride, int numVertices, Vector3[] vertexBase, int vertexStride) - { - IndexedMesh mesh = new IndexedMesh(); - mesh.TriangleCount = numTriangleIndices; - mesh.TriangleIndexBase = triangleIndexBase; - mesh.TriangleIndexStride = triangleIndexStride; - mesh.VertexBase = vertexBase; - mesh.VertexCount = numVertices; - mesh.VertexStride = vertexStride; - - AddIndexedMesh(mesh); - } - - public TriangleIndexVertexArray(int[] triangleIndexBase, Vector3[] vertexBase) - : this(triangleIndexBase.Length, triangleIndexBase, 32, vertexBase.Length, vertexBase, 24) { } - - public void AddIndexedMesh(IndexedMesh indexedMesh) - { - _indexedMeshes.Add(indexedMesh); - } - - public override void GetLockedVertexIndexBase(out List verts, out List indicies, out int numfaces, int subpart) - { - throw new Exception("The method or operation is not implemented."); - } - - public override void GetLockedReadOnlyVertexIndexBase(out List verts, out List indicies, out int numfaces, int subpart) - { - throw new Exception("The method or operation is not implemented."); - } - - public override void UnLockVertexBase(int subpart) - { - throw new Exception("The method or operation is not implemented."); - } - - public override void UnLockReadOnlyVertexBase(int subpart) - { - throw new Exception("The method or operation is not implemented."); - } - - public override int SubPartsCount() - { - return _indexedMeshes.Count; - } - - public override void PreallocateVertices(int numverts) - { - throw new Exception("The method or operation is not implemented."); - } - - public override void PreallocateIndices(int numindices) - { - throw new Exception("The method or operation is not implemented."); - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// + /// IndexedMesh indexes into existing vertex and index arrays, in a similar way OpenGL glDrawElements + /// instead of the number of indices, we pass the number of triangles + /// + public struct IndexedMesh + { + private int _numTriangles; + private int[] _triangleIndexBase; + private int _triangleIndexStride; + private int _numVertices; + private Vector3[] _vertexBase; + private int _vertexStride; + + public IndexedMesh(int numTriangleIndices, int[] triangleIndexBase, int triangleIndexStride, int numVertices, Vector3[] vertexBase, int vertexStride) + { + _numTriangles = numTriangleIndices; + _triangleIndexBase = triangleIndexBase; + _triangleIndexStride = triangleIndexStride; + _vertexBase = vertexBase; + _numVertices = numVertices; + _vertexStride = vertexStride; + } + + public IndexedMesh(int[] triangleIndexBase, Vector3[] vertexBase) + { + _numTriangles = triangleIndexBase.Length; + _triangleIndexBase = triangleIndexBase; + _triangleIndexStride = 32; + _vertexBase = vertexBase; + _numVertices = vertexBase.Length; + _vertexStride = 24; + } + + public int TriangleCount { get { return _numTriangles; } set { _numTriangles = value; } } + public int[] TriangleIndexBase { get { return _triangleIndexBase; } set { _triangleIndexBase = value; } } + public int TriangleIndexStride { get { return _triangleIndexStride; } set { _triangleIndexStride = value; } } + public int VertexCount { get { return _numVertices; } set { _numVertices = value; } } + public Vector3[] VertexBase { get { return _vertexBase; } set { _vertexBase = value; } } + public int VertexStride { get { return _vertexStride; } set { _vertexStride = value; } } + } + + /// + /// TriangleIndexVertexArray allows to use multiple meshes, by indexing into existing triangle/index arrays. + /// Additional meshes can be added using addIndexedMesh + /// + public class TriangleIndexVertexArray : StridingMeshInterface + { + List _indexedMeshes = new List(); + + public TriangleIndexVertexArray() { } + + public TriangleIndexVertexArray(int numTriangleIndices, int[] triangleIndexBase, int triangleIndexStride, int numVertices, Vector3[] vertexBase, int vertexStride) + { + IndexedMesh mesh = new IndexedMesh(); + mesh.TriangleCount = numTriangleIndices; + mesh.TriangleIndexBase = triangleIndexBase; + mesh.TriangleIndexStride = triangleIndexStride; + mesh.VertexBase = vertexBase; + mesh.VertexCount = numVertices; + mesh.VertexStride = vertexStride; + + AddIndexedMesh(mesh); + } + + public TriangleIndexVertexArray(int[] triangleIndexBase, Vector3[] vertexBase) + : this(triangleIndexBase.Length, triangleIndexBase, 32, vertexBase.Length, vertexBase, 24) { } + + public void AddIndexedMesh(IndexedMesh indexedMesh) + { + _indexedMeshes.Add(indexedMesh); + } + + public override void GetLockedVertexIndexBase(out List verts, out List indicies, out int numfaces, int subpart) + { + throw new Exception("The method or operation is not implemented."); + } + + public override void GetLockedReadOnlyVertexIndexBase(out List verts, out List indicies, out int numfaces, int subpart) + { + throw new Exception("The method or operation is not implemented."); + } + + public override void UnLockVertexBase(int subpart) + { + throw new Exception("The method or operation is not implemented."); + } + + public override void UnLockReadOnlyVertexBase(int subpart) + { + throw new Exception("The method or operation is not implemented."); + } + + public override int SubPartsCount() + { + return _indexedMeshes.Count; + } + + public override void PreallocateVertices(int numverts) + { + throw new Exception("The method or operation is not implemented."); + } + + public override void PreallocateIndices(int numindices) + { + throw new Exception("The method or operation is not implemented."); + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs index 1426355..d5b93ba 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs @@ -1,102 +1,102 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; - -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - class TriangleMesh : StridingMeshInterface - { - int _numTriangles; - List _verts; - - public TriangleMesh() - { - _numTriangles = 0; - _verts = new List(); - } - - void AddTriangle(Vector3 vertex0, Vector3 vertex1, Vector3 vertex2) - { - _verts.Add(vertex0); - _verts.Add(vertex1); - _verts.Add(vertex2); - _numTriangles++; - } - - public override void GetLockedVertexIndexBase(out List verts, out List indicies, out int numfaces, int subpart) - { - verts = new List(); - for (int i = 0; i < 3; i++) - { - verts.Add(_verts[subpart * 3 + i]); - } - indicies = new List(); - indicies.Add(0); - indicies.Add(1); - indicies.Add(2); - numfaces = 1; - } - - public override void GetLockedReadOnlyVertexIndexBase(out List verts, out List indicies, out int numfaces, int subpart) - { - verts = new List(); - for (int i = 0; i < 3; i++) - { - verts.Add(_verts[subpart * 3 + i]); - } - indicies = new List(); - indicies.Add(0); - indicies.Add(1); - indicies.Add(2); - numfaces = 1; - } - - public override void UnLockVertexBase(int subpart) - { - - } - - public override void UnLockReadOnlyVertexBase(int subpart) - { - - } - - public override int SubPartsCount() - { - return _numTriangles; - } - - public override void PreallocateVertices(int numverts) - { - - } - - public override void PreallocateIndices(int numindices) - { - - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + class TriangleMesh : StridingMeshInterface + { + int _numTriangles; + List _verts; + + public TriangleMesh() + { + _numTriangles = 0; + _verts = new List(); + } + + void AddTriangle(Vector3 vertex0, Vector3 vertex1, Vector3 vertex2) + { + _verts.Add(vertex0); + _verts.Add(vertex1); + _verts.Add(vertex2); + _numTriangles++; + } + + public override void GetLockedVertexIndexBase(out List verts, out List indicies, out int numfaces, int subpart) + { + verts = new List(); + for (int i = 0; i < 3; i++) + { + verts.Add(_verts[subpart * 3 + i]); + } + indicies = new List(); + indicies.Add(0); + indicies.Add(1); + indicies.Add(2); + numfaces = 1; + } + + public override void GetLockedReadOnlyVertexIndexBase(out List verts, out List indicies, out int numfaces, int subpart) + { + verts = new List(); + for (int i = 0; i < 3; i++) + { + verts.Add(_verts[subpart * 3 + i]); + } + indicies = new List(); + indicies.Add(0); + indicies.Add(1); + indicies.Add(2); + numfaces = 1; + } + + public override void UnLockVertexBase(int subpart) + { + + } + + public override void UnLockReadOnlyVertexBase(int subpart) + { + + } + + public override int SubPartsCount() + { + return _numTriangles; + } + + public override void PreallocateVertices(int numverts) + { + + } + + public override void PreallocateIndices(int numindices) + { + + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs index bc8f09e..5f5a0cf 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs @@ -1,160 +1,160 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// - /// Concave triangle mesh. Uses an interface to access the triangles to allow for sharing graphics/physics triangles. - /// - public class TriangleMeshShape : ConcaveShape - { - private StridingMeshInterface _meshInterface; - private Vector3 _localAabbMin; - private Vector3 _localAabbMax; - - public TriangleMeshShape(StridingMeshInterface meshInterface) - { - this._meshInterface = meshInterface; - RecalcLocalAabb(); - } - - protected StridingMeshInterface MeshInterface { get { return _meshInterface; } set { _meshInterface = value; } } - protected Vector3 LocalAabbMin { get { return _localAabbMin; } set { _localAabbMin = value; } } - protected Vector3 LocalAabbMax { get { return _localAabbMax; } set { _localAabbMax = value; } } - - public override BroadphaseNativeTypes ShapeType - { - get - { - return BroadphaseNativeTypes.TriangleMesh; - } - } - - public override Vector3 LocalScaling - { - get - { - return _meshInterface.Scaling; - } - set - { - _meshInterface.Scaling = value; - } - } - - public override string Name - { - get - { - return "TriangleMesh"; - } - } - - public void RecalcLocalAabb() - { - { - Vector3 vec = new Vector3(); - vec.X = 1f; - Vector3 tmp = LocalGetSupportingVertex(vec); - _localAabbMax.X = tmp.X + CollisionMargin; - vec.X = -1f; - tmp = LocalGetSupportingVertex(vec); - _localAabbMin.X = tmp.X - CollisionMargin; - } - { - Vector3 vec = new Vector3(); - vec.Y = 1f; - Vector3 tmp = LocalGetSupportingVertex(vec); - _localAabbMax.Y = tmp.Y + CollisionMargin; - vec.Y = -1f; - tmp = LocalGetSupportingVertex(vec); - _localAabbMin.Y = tmp.Y - CollisionMargin; - } - { - Vector3 vec = new Vector3(); - vec.Z = 1f; - Vector3 tmp = LocalGetSupportingVertex(vec); - _localAabbMax.Z = tmp.Z + CollisionMargin; - vec.Z = -1f; - tmp = LocalGetSupportingVertex(vec); - _localAabbMin.Z = tmp.Z - CollisionMargin; - } - } - - public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) - { - LocalProcessAllTriangles(callback, aabbMax, aabbMax); - } - - protected void LocalProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) - { - FilteredCallback filterCallback = new FilteredCallback(callback, aabbMin, aabbMax); - _meshInterface.InternalProcessAllTriangles(filterCallback, aabbMin, aabbMax); - } - - public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) - { - Vector3 localHalfExtents = 0.5f * (_localAabbMax - _localAabbMin); - Vector3 localCenter = 0.5f * (_localAabbMax + _localAabbMin); - - Matrix abs_b = MathHelper.Absolute(t); - - Vector3 center = MathHelper.MatrixToVector(t, localCenter); - - Vector3 extent = new Vector3(Vector3.Dot(new Vector3(abs_b.M11, abs_b.M12, abs_b.M13), localHalfExtents), - Vector3.Dot(new Vector3(abs_b.M21, abs_b.M22, abs_b.M23), localHalfExtents), - Vector3.Dot(new Vector3(abs_b.M31, abs_b.M32, abs_b.M33), localHalfExtents)); - extent += new Vector3(Margin, Margin, Margin); - - aabbMin = center - extent; - aabbMax = center + extent; - } - - public override void CalculateLocalInertia(float mass, out Vector3 inertia) - { - inertia = new Vector3(); - //moving concave objects not supported - BulletDebug.Assert(false); - } - - public virtual Vector3 LocalGetSupportingVertex(Vector3 vec) - { - Vector3 supportVertex; - Matrix ident = Matrix.Identity; - SupportVertexCallback supportCallback = new SupportVertexCallback(vec, ident); - Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); - LocalProcessAllTriangles(supportCallback, -aabbMax, aabbMax); - supportVertex = supportCallback.SupportVertexLocal; - return supportVertex; - } - - public virtual Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) - { - BulletDebug.Assert(false); - return LocalGetSupportingVertex(vec); - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// + /// Concave triangle mesh. Uses an interface to access the triangles to allow for sharing graphics/physics triangles. + /// + public class TriangleMeshShape : ConcaveShape + { + private StridingMeshInterface _meshInterface; + private Vector3 _localAabbMin; + private Vector3 _localAabbMax; + + public TriangleMeshShape(StridingMeshInterface meshInterface) + { + this._meshInterface = meshInterface; + RecalcLocalAabb(); + } + + protected StridingMeshInterface MeshInterface { get { return _meshInterface; } set { _meshInterface = value; } } + protected Vector3 LocalAabbMin { get { return _localAabbMin; } set { _localAabbMin = value; } } + protected Vector3 LocalAabbMax { get { return _localAabbMax; } set { _localAabbMax = value; } } + + public override BroadphaseNativeTypes ShapeType + { + get + { + return BroadphaseNativeTypes.TriangleMesh; + } + } + + public override Vector3 LocalScaling + { + get + { + return _meshInterface.Scaling; + } + set + { + _meshInterface.Scaling = value; + } + } + + public override string Name + { + get + { + return "TriangleMesh"; + } + } + + public void RecalcLocalAabb() + { + { + Vector3 vec = new Vector3(); + vec.X = 1f; + Vector3 tmp = LocalGetSupportingVertex(vec); + _localAabbMax.X = tmp.X + CollisionMargin; + vec.X = -1f; + tmp = LocalGetSupportingVertex(vec); + _localAabbMin.X = tmp.X - CollisionMargin; + } + { + Vector3 vec = new Vector3(); + vec.Y = 1f; + Vector3 tmp = LocalGetSupportingVertex(vec); + _localAabbMax.Y = tmp.Y + CollisionMargin; + vec.Y = -1f; + tmp = LocalGetSupportingVertex(vec); + _localAabbMin.Y = tmp.Y - CollisionMargin; + } + { + Vector3 vec = new Vector3(); + vec.Z = 1f; + Vector3 tmp = LocalGetSupportingVertex(vec); + _localAabbMax.Z = tmp.Z + CollisionMargin; + vec.Z = -1f; + tmp = LocalGetSupportingVertex(vec); + _localAabbMin.Z = tmp.Z - CollisionMargin; + } + } + + public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) + { + LocalProcessAllTriangles(callback, aabbMax, aabbMax); + } + + protected void LocalProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) + { + FilteredCallback filterCallback = new FilteredCallback(callback, aabbMin, aabbMax); + _meshInterface.InternalProcessAllTriangles(filterCallback, aabbMin, aabbMax); + } + + public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) + { + Vector3 localHalfExtents = 0.5f * (_localAabbMax - _localAabbMin); + Vector3 localCenter = 0.5f * (_localAabbMax + _localAabbMin); + + Matrix abs_b = MathHelper.Absolute(t); + + Vector3 center = MathHelper.MatrixToVector(t, localCenter); + + Vector3 extent = new Vector3(Vector3.Dot(new Vector3(abs_b.M11, abs_b.M12, abs_b.M13), localHalfExtents), + Vector3.Dot(new Vector3(abs_b.M21, abs_b.M22, abs_b.M23), localHalfExtents), + Vector3.Dot(new Vector3(abs_b.M31, abs_b.M32, abs_b.M33), localHalfExtents)); + extent += new Vector3(Margin, Margin, Margin); + + aabbMin = center - extent; + aabbMax = center + extent; + } + + public override void CalculateLocalInertia(float mass, out Vector3 inertia) + { + inertia = new Vector3(); + //moving concave objects not supported + BulletDebug.Assert(false); + } + + public virtual Vector3 LocalGetSupportingVertex(Vector3 vec) + { + Vector3 supportVertex; + Matrix ident = Matrix.Identity; + SupportVertexCallback supportCallback = new SupportVertexCallback(vec, ident); + Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); + LocalProcessAllTriangles(supportCallback, -aabbMax, aabbMax); + supportVertex = supportCallback.SupportVertexLocal; + return supportVertex; + } + + public virtual Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) + { + BulletDebug.Assert(false); + return LocalGetSupportingVertex(vec); + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs index 59ffad0..d5a17f3 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs @@ -1,187 +1,187 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public class TriangleShape : PolyhedralConvexShape - { - private Vector3[] _vertices = new Vector3[3]; - - public TriangleShape(Vector3 pointA, Vector3 pointB, Vector3 pointC) - { - _vertices[0] = pointA; - _vertices[1] = pointB; - _vertices[2] = pointC; - } - - public override int PreferredPenetrationDirectionsCount - { - get - { - return 2; - } - } - - public Vector3[] Vertices - { - get - { - return _vertices; - } - } - - public override int VertexCount - { - get - { - return 3; - } - } - - public override int EdgeCount - { - get - { - return 3; - } - } - - public override int PlaneCount - { - get - { - return 1; - } - } - - public override BroadphaseNativeTypes ShapeType - { - get - { - return BroadphaseNativeTypes.Triangle; - } - } - - public override string Name - { - get - { - return "Triangle"; - } - } - - public override void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector) - { - CalculateNormal(out penetrationVector); - if (index != 0) - penetrationVector *= -1f; - } - - public virtual void GetPlaneEquation(int i, out Vector3 planeNormal, out Vector3 planeSupport) - { - CalculateNormal(out planeNormal); - planeSupport = _vertices[0]; - } - - public void CalculateNormal(out Vector3 normal) - { - normal = Vector3.Normalize(Vector3.Cross(_vertices[1] - _vertices[0], _vertices[2] - _vertices[0])); - } - - public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) - { - Vector3 dots = new Vector3(Vector3.Dot(vec, _vertices[0]), Vector3.Dot(vec, _vertices[1]), Vector3.Dot(vec, _vertices[2])); - return _vertices[MathHelper.MaxAxis(dots)]; - } - - public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) - { - for (int i = 0; i < vectors.Length; i++) - { - Vector3 dir = vectors[i]; - Vector3 dots = new Vector3(Vector3.Dot(dir, _vertices[0]), Vector3.Dot(dir, _vertices[1]), Vector3.Dot(dir, _vertices[2])); - supportVerticesOut[i] = _vertices[MathHelper.MaxAxis(dots)]; - } - } - - public override void CalculateLocalInertia(float mass, out Vector3 inertia) - { - inertia = new Vector3(); - BulletDebug.Assert(false); - } - - public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) - { - GetVertex(i, out pa); - GetVertex((i + 1) % 3, out pb); - } - - public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) - { - GetAabbSlow(t, out aabbMin, out aabbMax); - } - - public override void GetVertex(int i, out Vector3 vtx) - { - vtx = _vertices[i]; - } - - public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) - { - GetPlaneEquation(i, out planeNormal, out planeSupport); - } - - public override bool IsInside(Vector3 pt, float tolerance) - { - Vector3 normal; - CalculateNormal(out normal); - //distance to plane - float dist = Vector3.Dot(pt, normal); - float planeconst = Vector3.Dot(_vertices[0], normal); - dist -= planeconst; - if (dist >= -tolerance && dist <= tolerance) - { - //inside check on edge-planes - int i; - for (i = 0; i < 3; i++) - { - Vector3 pa, pb; - GetEdge(i, out pa, out pb); - Vector3 edge = pb - pa; - Vector3 edgeNormal = Vector3.Cross(edge, normal); - edgeNormal = Vector3.Normalize(edgeNormal); - float distance = Vector3.Dot(pt, edgeNormal); - float edgeConst = Vector3.Dot(pa, edgeNormal); - distance -= edgeConst; - if (distance < -tolerance) - return false; - } - return true; - } - return false; - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public class TriangleShape : PolyhedralConvexShape + { + private Vector3[] _vertices = new Vector3[3]; + + public TriangleShape(Vector3 pointA, Vector3 pointB, Vector3 pointC) + { + _vertices[0] = pointA; + _vertices[1] = pointB; + _vertices[2] = pointC; + } + + public override int PreferredPenetrationDirectionsCount + { + get + { + return 2; + } + } + + public Vector3[] Vertices + { + get + { + return _vertices; + } + } + + public override int VertexCount + { + get + { + return 3; + } + } + + public override int EdgeCount + { + get + { + return 3; + } + } + + public override int PlaneCount + { + get + { + return 1; + } + } + + public override BroadphaseNativeTypes ShapeType + { + get + { + return BroadphaseNativeTypes.Triangle; + } + } + + public override string Name + { + get + { + return "Triangle"; + } + } + + public override void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector) + { + CalculateNormal(out penetrationVector); + if (index != 0) + penetrationVector *= -1f; + } + + public virtual void GetPlaneEquation(int i, out Vector3 planeNormal, out Vector3 planeSupport) + { + CalculateNormal(out planeNormal); + planeSupport = _vertices[0]; + } + + public void CalculateNormal(out Vector3 normal) + { + normal = Vector3.Normalize(Vector3.Cross(_vertices[1] - _vertices[0], _vertices[2] - _vertices[0])); + } + + public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) + { + Vector3 dots = new Vector3(Vector3.Dot(vec, _vertices[0]), Vector3.Dot(vec, _vertices[1]), Vector3.Dot(vec, _vertices[2])); + return _vertices[MathHelper.MaxAxis(dots)]; + } + + public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) + { + for (int i = 0; i < vectors.Length; i++) + { + Vector3 dir = vectors[i]; + Vector3 dots = new Vector3(Vector3.Dot(dir, _vertices[0]), Vector3.Dot(dir, _vertices[1]), Vector3.Dot(dir, _vertices[2])); + supportVerticesOut[i] = _vertices[MathHelper.MaxAxis(dots)]; + } + } + + public override void CalculateLocalInertia(float mass, out Vector3 inertia) + { + inertia = new Vector3(); + BulletDebug.Assert(false); + } + + public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) + { + GetVertex(i, out pa); + GetVertex((i + 1) % 3, out pb); + } + + public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) + { + GetAabbSlow(t, out aabbMin, out aabbMax); + } + + public override void GetVertex(int i, out Vector3 vtx) + { + vtx = _vertices[i]; + } + + public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) + { + GetPlaneEquation(i, out planeNormal, out planeSupport); + } + + public override bool IsInside(Vector3 pt, float tolerance) + { + Vector3 normal; + CalculateNormal(out normal); + //distance to plane + float dist = Vector3.Dot(pt, normal); + float planeconst = Vector3.Dot(_vertices[0], normal); + dist -= planeconst; + if (dist >= -tolerance && dist <= tolerance) + { + //inside check on edge-planes + int i; + for (i = 0; i < 3; i++) + { + Vector3 pa, pb; + GetEdge(i, out pa, out pb); + Vector3 edge = pb - pa; + Vector3 edgeNormal = Vector3.Cross(edge, normal); + edgeNormal = Vector3.Normalize(edgeNormal); + float distance = Vector3.Dot(pt, edgeNormal); + float edgeConst = Vector3.Dot(pa, edgeNormal); + distance -= edgeConst; + if (distance < -tolerance) + return false; + } + return true; + } + return false; + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs index 497f220..d011ef2 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs @@ -1,199 +1,199 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// - /// ContinuousConvexCollision implements angular and linear time of impact for convex objects. - /// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis). - /// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent. - /// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops - /// - public class ContinuousConvexCollision : IConvexCast - { - /// - /// This maximum should not be necessary. It allows for untested/degenerate cases in production code. - /// You don't want your game ever to lock-up. - /// - private const int MaxIterations = 1000; - - private ISimplexSolver _simplexSolver; - private IConvexPenetrationDepthSolver _penetrationDepthSolver; - private ConvexShape _convexA; - private ConvexShape _convexB; - - public ContinuousConvexCollision(ConvexShape convexA, ConvexShape convexB, - ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver) - { - _simplexSolver = simplexSolver; - _penetrationDepthSolver = penetrationDepthSolver; - _convexA = convexA; - _convexB = convexB; - } - - public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result) - { - _simplexSolver.Reset(); - - // compute linear and angular velocity for this interval, to interpolate - Vector3 linVelA = new Vector3(), angVelA = new Vector3(), linVelB = new Vector3(), angVelB = new Vector3(); - TransformUtil.CalculateVelocity(fromA, toA, 1f, ref linVelA, ref angVelA); - TransformUtil.CalculateVelocity(fromB, toB, 1f, ref linVelB, ref angVelB); - - float boundingRadiusA = _convexA.GetAngularMotionDisc(); - float boundingRadiusB = _convexB.GetAngularMotionDisc(); - - float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA + - angVelB.Length() * boundingRadiusB; - - float radius = 0.001f; - - float lambda = 0f; - Vector3 v = new Vector3(1f, 0f, 0f); - - int maxIter = MaxIterations; - - Vector3 n = new Vector3(); - bool hasResult = false; - Vector3 c; - - float lastLambda = lambda; - //float epsilon = 0.001f; - - int numIter = 0; - //first solution, using GJK - - - Matrix identityTrans = Matrix.Identity; - - SphereShape raySphere = new SphereShape(0f); - raySphere.Margin=0f; - - - //result.drawCoordSystem(sphereTr); - - PointCollector pointCollector1 = new PointCollector(); - - GjkPairDetector gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver); - GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); - - //we don't use margins during CCD - gjk.setIgnoreMargin(true); - - input.TransformA = fromA; - input.TransformB = fromB; - - DiscreteCollisionDetectorInterface.Result r = (DiscreteCollisionDetectorInterface.Result)pointCollector1; - gjk.GetClosestPoints(input, r, null); - - hasResult = pointCollector1.HasResult; - c = pointCollector1.PointInWorld; - - if (hasResult) - { - float dist; - dist = pointCollector1.Distance; - n = pointCollector1.NormalOnBInWorld; - - //not close enough - while (dist > radius) - { - numIter++; - if (numIter > maxIter) - return false; //todo: report a failure - - float dLambda = 0f; - - //calculate safe moving fraction from distance / (linear+rotational velocity) - - //float clippedDist = GEN_min(angularConservativeRadius,dist); - //float clippedDist = dist; - - float projectedLinearVelocity = Vector3.Dot(linVelB - linVelA, n); - - dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity); - - lambda = lambda + dLambda; - - if (lambda > 1f) return false; - if (lambda < 0f) return false; - - //todo: next check with relative epsilon - if (lambda <= lastLambda) - break; - lastLambda = lambda; - - - //interpolate to next lambda - Matrix interpolatedTransA = new Matrix(), interpolatedTransB = new Matrix(), relativeTrans; - - TransformUtil.IntegrateTransform(fromA, linVelA, angVelA, lambda, ref interpolatedTransA); - TransformUtil.IntegrateTransform(fromB, linVelB, angVelB, lambda, ref interpolatedTransB); - - relativeTrans = MathHelper.InverseTimes(interpolatedTransB, interpolatedTransA); - - result.DebugDraw(lambda); - - PointCollector pointCollector = new PointCollector(); - gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver); - input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); - input.TransformA = interpolatedTransA; - input.TransformB = interpolatedTransB; - - // !!!!!!!!!! - r = (DiscreteCollisionDetectorInterface.Result)pointCollector1; - gjk.GetClosestPoints(input, r, null); - - if (pointCollector.HasResult) - { - if (pointCollector.Distance < 0f) - { - //degenerate ?! - result.Fraction = lastLambda; - result.Normal = n; - return true; - } - c = pointCollector.PointInWorld; - - dist = pointCollector.Distance; - } - else - { - //?? - return false; - } - - } - - result.Fraction = lambda; - result.Normal = n; - return true; - } - - return false; - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// + /// ContinuousConvexCollision implements angular and linear time of impact for convex objects. + /// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis). + /// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent. + /// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops + /// + public class ContinuousConvexCollision : IConvexCast + { + /// + /// This maximum should not be necessary. It allows for untested/degenerate cases in production code. + /// You don't want your game ever to lock-up. + /// + private const int MaxIterations = 1000; + + private ISimplexSolver _simplexSolver; + private IConvexPenetrationDepthSolver _penetrationDepthSolver; + private ConvexShape _convexA; + private ConvexShape _convexB; + + public ContinuousConvexCollision(ConvexShape convexA, ConvexShape convexB, + ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver) + { + _simplexSolver = simplexSolver; + _penetrationDepthSolver = penetrationDepthSolver; + _convexA = convexA; + _convexB = convexB; + } + + public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result) + { + _simplexSolver.Reset(); + + // compute linear and angular velocity for this interval, to interpolate + Vector3 linVelA = new Vector3(), angVelA = new Vector3(), linVelB = new Vector3(), angVelB = new Vector3(); + TransformUtil.CalculateVelocity(fromA, toA, 1f, ref linVelA, ref angVelA); + TransformUtil.CalculateVelocity(fromB, toB, 1f, ref linVelB, ref angVelB); + + float boundingRadiusA = _convexA.GetAngularMotionDisc(); + float boundingRadiusB = _convexB.GetAngularMotionDisc(); + + float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA + + angVelB.Length() * boundingRadiusB; + + float radius = 0.001f; + + float lambda = 0f; + Vector3 v = new Vector3(1f, 0f, 0f); + + int maxIter = MaxIterations; + + Vector3 n = new Vector3(); + bool hasResult = false; + Vector3 c; + + float lastLambda = lambda; + //float epsilon = 0.001f; + + int numIter = 0; + //first solution, using GJK + + + Matrix identityTrans = Matrix.Identity; + + SphereShape raySphere = new SphereShape(0f); + raySphere.Margin=0f; + + + //result.drawCoordSystem(sphereTr); + + PointCollector pointCollector1 = new PointCollector(); + + GjkPairDetector gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver); + GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); + + //we don't use margins during CCD + gjk.setIgnoreMargin(true); + + input.TransformA = fromA; + input.TransformB = fromB; + + DiscreteCollisionDetectorInterface.Result r = (DiscreteCollisionDetectorInterface.Result)pointCollector1; + gjk.GetClosestPoints(input, r, null); + + hasResult = pointCollector1.HasResult; + c = pointCollector1.PointInWorld; + + if (hasResult) + { + float dist; + dist = pointCollector1.Distance; + n = pointCollector1.NormalOnBInWorld; + + //not close enough + while (dist > radius) + { + numIter++; + if (numIter > maxIter) + return false; //todo: report a failure + + float dLambda = 0f; + + //calculate safe moving fraction from distance / (linear+rotational velocity) + + //float clippedDist = GEN_min(angularConservativeRadius,dist); + //float clippedDist = dist; + + float projectedLinearVelocity = Vector3.Dot(linVelB - linVelA, n); + + dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity); + + lambda = lambda + dLambda; + + if (lambda > 1f) return false; + if (lambda < 0f) return false; + + //todo: next check with relative epsilon + if (lambda <= lastLambda) + break; + lastLambda = lambda; + + + //interpolate to next lambda + Matrix interpolatedTransA = new Matrix(), interpolatedTransB = new Matrix(), relativeTrans; + + TransformUtil.IntegrateTransform(fromA, linVelA, angVelA, lambda, ref interpolatedTransA); + TransformUtil.IntegrateTransform(fromB, linVelB, angVelB, lambda, ref interpolatedTransB); + + relativeTrans = MathHelper.InverseTimes(interpolatedTransB, interpolatedTransA); + + result.DebugDraw(lambda); + + PointCollector pointCollector = new PointCollector(); + gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver); + input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); + input.TransformA = interpolatedTransA; + input.TransformB = interpolatedTransB; + + // !!!!!!!!!! + r = (DiscreteCollisionDetectorInterface.Result)pointCollector1; + gjk.GetClosestPoints(input, r, null); + + if (pointCollector.HasResult) + { + if (pointCollector.Distance < 0f) + { + //degenerate ?! + result.Fraction = lastLambda; + result.Normal = n; + return true; + } + c = pointCollector.PointInWorld; + + dist = pointCollector.Distance; + } + else + { + //?? + return false; + } + + } + + result.Fraction = lambda; + result.Normal = n; + return true; + } + + return false; + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs index 9e88a7e..93501ed 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs @@ -1,73 +1,73 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - - /// - /// CastResult stores the closest result - /// alternatively, add a callback method to decide about closest/all results - /// - public class CastResult - { - private Vector3 _normal; - private float _fraction; - private Matrix _hitTransformA; - private Matrix _hitTransformB; - private IDebugDraw _debugDrawer; - - public CastResult() - { - _fraction = 1e30f; - } - - public Vector3 Normal { get { return _normal; } set { _normal = value; } } - public float Fraction { get { return _fraction; } set { _fraction = value; } } - public Matrix HitTransformA { get { return _hitTransformA; } set { _hitTransformA = value; } } - public Matrix HitTransformB { get { return _hitTransformB; } set { _hitTransformB = value; } } - public IDebugDraw DebugDrawer { get { return _debugDrawer; } set { _debugDrawer = value; } } - - public virtual void DebugDraw(float fraction) { } - public virtual void DrawCoordSystem(Matrix trans) { } - } - - /// - /// ConvexCast is an interface for Casting - /// - public interface IConvexCast - { - /// - /// cast a convex against another convex object - /// - /// - /// - /// - /// - /// - /// - bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result); - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + + /// + /// CastResult stores the closest result + /// alternatively, add a callback method to decide about closest/all results + /// + public class CastResult + { + private Vector3 _normal; + private float _fraction; + private Matrix _hitTransformA; + private Matrix _hitTransformB; + private IDebugDraw _debugDrawer; + + public CastResult() + { + _fraction = 1e30f; + } + + public Vector3 Normal { get { return _normal; } set { _normal = value; } } + public float Fraction { get { return _fraction; } set { _fraction = value; } } + public Matrix HitTransformA { get { return _hitTransformA; } set { _hitTransformA = value; } } + public Matrix HitTransformB { get { return _hitTransformB; } set { _hitTransformB = value; } } + public IDebugDraw DebugDrawer { get { return _debugDrawer; } set { _debugDrawer = value; } } + + public virtual void DebugDraw(float fraction) { } + public virtual void DrawCoordSystem(Matrix trans) { } + } + + /// + /// ConvexCast is an interface for Casting + /// + public interface IConvexCast + { + /// + /// cast a convex against another convex object + /// + /// + /// + /// + /// + /// + /// + bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result); + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs index fae2557..5b07d9b 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs @@ -1,117 +1,117 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public abstract class DiscreteCollisionDetectorInterface - { - public abstract class Result - { - public abstract void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB); - public abstract void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth); - } - - public class ClosestPointInput - { - private float _maximumDistanceSquared; - private Matrix _transformA, _transformB; - - #region Properties - public Matrix TransformB - { - get { return _transformB; } - set { _transformB = value; } - } - - public Matrix TransformA - { - get { return _transformA; } - set { _transformA = value; } - } - - public float MaximumDistanceSquared - { - get { return _maximumDistanceSquared; } - set { _maximumDistanceSquared = value; } - } - #endregion - - public ClosestPointInput() - { - _maximumDistanceSquared = 1e30f; - } - } - - public abstract void GetClosestPoints(ClosestPointInput input, Result output, IDebugDraw debugDraw); - } - - public class StorageResult : DiscreteCollisionDetectorInterface.Result - { - private Vector3 _closestPointInB; - private Vector3 _normalOnSurfaceB; - private float _distance; //negative means penetration ! - - #region Properties - - public float Distance - { - get { return _distance; } - set { _distance = value; } - } - public Vector3 NormalOnSurfaceB - { - get { return _normalOnSurfaceB; } - set { _normalOnSurfaceB = value; } - } - public Vector3 ClosestPointInB - { - get { return _closestPointInB; } - set { _closestPointInB = value; } - } - - #endregion - - public StorageResult() - { - _distance = 1e30f; - } - - public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) - { - if (depth < _distance) - { - _normalOnSurfaceB = normalOnBInWorld; - _closestPointInB = pointInWorld; - _distance = depth; - } - } - - public override void SetShapeIdentifiers(int partId0, int index0, int partId1, int index1) - { - throw new Exception("The method or operation is not implemented."); - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public abstract class DiscreteCollisionDetectorInterface + { + public abstract class Result + { + public abstract void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB); + public abstract void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth); + } + + public class ClosestPointInput + { + private float _maximumDistanceSquared; + private Matrix _transformA, _transformB; + + #region Properties + public Matrix TransformB + { + get { return _transformB; } + set { _transformB = value; } + } + + public Matrix TransformA + { + get { return _transformA; } + set { _transformA = value; } + } + + public float MaximumDistanceSquared + { + get { return _maximumDistanceSquared; } + set { _maximumDistanceSquared = value; } + } + #endregion + + public ClosestPointInput() + { + _maximumDistanceSquared = 1e30f; + } + } + + public abstract void GetClosestPoints(ClosestPointInput input, Result output, IDebugDraw debugDraw); + } + + public class StorageResult : DiscreteCollisionDetectorInterface.Result + { + private Vector3 _closestPointInB; + private Vector3 _normalOnSurfaceB; + private float _distance; //negative means penetration ! + + #region Properties + + public float Distance + { + get { return _distance; } + set { _distance = value; } + } + public Vector3 NormalOnSurfaceB + { + get { return _normalOnSurfaceB; } + set { _normalOnSurfaceB = value; } + } + public Vector3 ClosestPointInB + { + get { return _closestPointInB; } + set { _closestPointInB = value; } + } + + #endregion + + public StorageResult() + { + _distance = 1e30f; + } + + public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) + { + if (depth < _distance) + { + _normalOnSurfaceB = normalOnBInWorld; + _closestPointInB = pointInWorld; + _distance = depth; + } + } + + public override void SetShapeIdentifiers(int partId0, int index0, int partId1, int index1) + { + throw new Exception("The method or operation is not implemented."); + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs index 4ee9599..ea7d335 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs @@ -1,176 +1,176 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// - /// GjkConvexCast performs a raycast on a convex object using support mapping. - /// - public class GjkConvexCast : IConvexCast - { - private VoronoiSimplexSolver _simplexSolver; - private ConvexShape _convexA, _convexB; - - public GjkConvexCast(ConvexShape convexShapeA, ConvexShape convexShapeB, VoronoiSimplexSolver solver) - { - _simplexSolver = solver; - - _convexA = convexShapeA; - _convexB = convexShapeB; - } - - #region IConvexCast Members - - /// - /// cast a convex against another convex object - /// - /// - /// - /// - /// - /// - /// - public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result) - { - MinkowskiSumShape combined = new MinkowskiSumShape(_convexA, _convexB); - - Matrix rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB; - Matrix rayToLocalA = MathHelper.InvertMatrix(toA) * toB; - - Matrix transformA = fromA; - Matrix transformB = fromB; - - transformA.Translation = new Vector3(0, 0, 0); - transformB.Translation = new Vector3(0, 0, 0); - - combined.TransformA = transformA; - combined.TransformB = transformB; - - float radius = 0.01f; - float lambda = 0; - - Vector3 s = rayFromLocalA.Translation; - Vector3 r = rayToLocalA.Translation - rayFromLocalA.Translation; - Vector3 x = s; - Vector3 n = new Vector3(); - Vector3 c = new Vector3(); - - bool hasResult = false; - float lastLambda = lambda; - - IConvexPenetrationDepthSolver penSolver = null; - Matrix identityTransform = Matrix.Identity; - - SphereShape raySphere = new SphereShape(0.0f); - raySphere.Margin=0.0f; - - Matrix sphereTransform = Matrix.Identity; - sphereTransform.Translation = rayFromLocalA.Translation; - - result.DrawCoordSystem(sphereTransform); - - { - PointCollector pointCollector = new PointCollector(); - GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver); - - GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); - input.TransformA = sphereTransform; - input.TransformB = identityTransform; - - gjk.GetClosestPoints(input, pointCollector, null); - - hasResult = pointCollector.HasResult; - - c = pointCollector.PointInWorld; - n = pointCollector.NormalOnBInWorld; - } - - if (hasResult) - { - float dist = (c - x).Length(); - - if (dist < radius) - { - lastLambda = 1.0f; - } - - while (dist > radius) - { - n = x - c; - float dot = Vector3.Dot(n, r); - - if (dot >= -(MathHelper.Epsilon * MathHelper.Epsilon)) return false; - - lambda = lambda - Vector3.Distance(n, n) / dot; - if (lambda <= lastLambda) break; - - lastLambda = lambda; - - x = s + lambda * r; - - sphereTransform.Translation = x; - result.DrawCoordSystem(sphereTransform); - PointCollector pointCollector = new PointCollector(); - - GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver); - GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); - input.TransformA = sphereTransform; - input.TransformB = identityTransform; - - gjk.GetClosestPoints(input, pointCollector, null); - - if (pointCollector.HasResult) - { - if (pointCollector.Distance < 0.0f) - { - result.Fraction = lastLambda; - result.Normal = n; - return true; - } - - c = pointCollector.PointInWorld; - dist = (c - x).Length(); - } - else - { - return false; - } - } - - if (lastLambda < 1.0f) - { - result.Fraction = lastLambda; - result.Normal = n; - return true; - } - } - - return false; - } - - #endregion - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// + /// GjkConvexCast performs a raycast on a convex object using support mapping. + /// + public class GjkConvexCast : IConvexCast + { + private VoronoiSimplexSolver _simplexSolver; + private ConvexShape _convexA, _convexB; + + public GjkConvexCast(ConvexShape convexShapeA, ConvexShape convexShapeB, VoronoiSimplexSolver solver) + { + _simplexSolver = solver; + + _convexA = convexShapeA; + _convexB = convexShapeB; + } + + #region IConvexCast Members + + /// + /// cast a convex against another convex object + /// + /// + /// + /// + /// + /// + /// + public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result) + { + MinkowskiSumShape combined = new MinkowskiSumShape(_convexA, _convexB); + + Matrix rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB; + Matrix rayToLocalA = MathHelper.InvertMatrix(toA) * toB; + + Matrix transformA = fromA; + Matrix transformB = fromB; + + transformA.Translation = new Vector3(0, 0, 0); + transformB.Translation = new Vector3(0, 0, 0); + + combined.TransformA = transformA; + combined.TransformB = transformB; + + float radius = 0.01f; + float lambda = 0; + + Vector3 s = rayFromLocalA.Translation; + Vector3 r = rayToLocalA.Translation - rayFromLocalA.Translation; + Vector3 x = s; + Vector3 n = new Vector3(); + Vector3 c = new Vector3(); + + bool hasResult = false; + float lastLambda = lambda; + + IConvexPenetrationDepthSolver penSolver = null; + Matrix identityTransform = Matrix.Identity; + + SphereShape raySphere = new SphereShape(0.0f); + raySphere.Margin=0.0f; + + Matrix sphereTransform = Matrix.Identity; + sphereTransform.Translation = rayFromLocalA.Translation; + + result.DrawCoordSystem(sphereTransform); + + { + PointCollector pointCollector = new PointCollector(); + GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver); + + GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); + input.TransformA = sphereTransform; + input.TransformB = identityTransform; + + gjk.GetClosestPoints(input, pointCollector, null); + + hasResult = pointCollector.HasResult; + + c = pointCollector.PointInWorld; + n = pointCollector.NormalOnBInWorld; + } + + if (hasResult) + { + float dist = (c - x).Length(); + + if (dist < radius) + { + lastLambda = 1.0f; + } + + while (dist > radius) + { + n = x - c; + float dot = Vector3.Dot(n, r); + + if (dot >= -(MathHelper.Epsilon * MathHelper.Epsilon)) return false; + + lambda = lambda - Vector3.Distance(n, n) / dot; + if (lambda <= lastLambda) break; + + lastLambda = lambda; + + x = s + lambda * r; + + sphereTransform.Translation = x; + result.DrawCoordSystem(sphereTransform); + PointCollector pointCollector = new PointCollector(); + + GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver); + GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); + input.TransformA = sphereTransform; + input.TransformB = identityTransform; + + gjk.GetClosestPoints(input, pointCollector, null); + + if (pointCollector.HasResult) + { + if (pointCollector.Distance < 0.0f) + { + result.Fraction = lastLambda; + result.Normal = n; + return true; + } + + c = pointCollector.PointInWorld; + dist = (c - x).Length(); + } + else + { + return false; + } + } + + if (lastLambda < 1.0f) + { + result.Fraction = lastLambda; + result.Normal = n; + return true; + } + } + + return false; + } + + #endregion + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs index 50ae7ab..a3064db 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs @@ -1,633 +1,633 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// - /// GJK-EPA collision solver by Nathanael Presson - /// Nov.2006 - /// - public class GjkEpa - { - //private static readonly int _precision = 1 /* U(sizeof(F) == 4)*/; - - private static readonly float _infinity = MathHelper.Infinity; - //private static readonly float _pi = (float)Math.PI; - private static readonly float _twoPi = (float)(Math.PI * 2); - - private static readonly int _gjkMaxIterations = 128; - private static readonly int _gjkHashSize = 1 << 6; - private static readonly int _gjkHashMask = _gjkHashSize - 1; - private static readonly float _gjkInSimplexEpsilon = 0.0001f; - private static readonly float _gjkSquareInSimplexEpsilon = _gjkInSimplexEpsilon * _gjkInSimplexEpsilon; - - private static readonly int _epaMaxIterations = 256; - private static readonly float _epaInFaceEpsilon = 0.01f; - private static readonly float _epaAccuracy = 0.001f; - - public static float EpaAccuracy { get { return _epaAccuracy; } } - - private static float Abs(float v) { return (v < 0 ? -v : v); } - private static float Sign(float v) { return (v < 0 ? -1 : 1); } - - static void Swap(ref T a, ref T b) - { - T t = a; - a = b; - b = t; - } - - public class Gjk - { - public class MinkowskiVertice - { - private Vector3 _vertice; /* Minkowski vertice */ - private Vector3 _ray; /* Ray */ - - public Vector3 Vertice { get { return _vertice; } set { _vertice = value; } } - public Vector3 Ray { get { return _ray; } set { _ray = value; } } - } - public class He - { - private Vector3 _ray; - private He _next; - - public He Next { get { return _next; } set { _next = value; } } - public Vector3 Ray { get { return _ray; } set { _ray = value; } } - } - - private He[] _table = new He[_gjkHashSize]; - private Matrix[] _wrotations = new Matrix[2]; - private Vector3[] _positions = new Vector3[2]; - private ConvexShape[] _shapes = new ConvexShape[2]; - private MinkowskiVertice[] _simplex = new MinkowskiVertice[5]; - private Vector3 _ray; - private int _order; - private int _iterations; - private float _margin; - private bool _failed; - - public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA, - Matrix wrotationB, Vector3 positionB, ConvexShape shapeB) - : this(wrotationA, positionA, shapeA, wrotationB, positionB, shapeB, 0) { } - - public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA, - Matrix wrotationB, Vector3 positionB, ConvexShape shapeB, - float pmargin) - { - for (int i = 0; i < _simplex.Length; i++) - _simplex[i] = new MinkowskiVertice(); - - for (int i = 0; i < _wrotations.Length; i++) - _wrotations[i] = new Matrix(); - - for (int i = 0; i < _positions.Length; i++) - _positions[i] = new Vector3(); - - _wrotations[0] = wrotationA; _positions[0] = positionA; - _shapes[0] = shapeA; - _wrotations[0].Translation = Vector3.Zero; - _wrotations[1] = wrotationB; _positions[1] = positionB; - _shapes[1] = shapeB; - _wrotations[1].Translation = Vector3.Zero; - //sablock = sa->BeginBlock(); - _margin = pmargin; - _failed = false; - } - - public bool Failed { get { return _failed; } } - public int Iterations { get { return _iterations; } } - public int Order { get { return _order; } } - public MinkowskiVertice[] Simplex { get { return _simplex; } } - - public int Hash(Vector3 v) - { - int h = ((int)(v.X * 15461) ^ (int)(v.Y * 83003) ^ (int)(v.Z * 15473)); - return (h * 169639) & _gjkHashMask; - } - - public bool FetchSupport() - { - int h = Hash(_ray); - He e = _table[h]; - while (e != null) - { - if (e.Ray == _ray) - { - --_order; - return (false); - } - else e = e.Next; - } - e = new He(); - e.Ray = _ray; - e.Next = _table[h]; - _table[h] = e; - Support(_ray, ref _simplex[++_order]); - return (Vector3.Dot(_ray, _simplex[_order].Vertice) > 0); - } - - public Vector3 LocalSupport(Vector3 d, int i) - { - Matrix m = _wrotations[i]; - m.Translation = Vector3.Zero; - Vector3 vtx = Vector3.TransformNormal(d, m); - Vector3 result = MathHelper.MatrixToVector(_wrotations[i], _shapes[i].LocalGetSupportingVertex(vtx)); - return (result + _positions[i]); - } - - public void Support(Vector3 d, ref MinkowskiVertice v) - { - v.Ray = d; - v.Vertice = LocalSupport(d, 0) - LocalSupport(-d, 1) + d * _margin; - } - - public bool SolveSimplex2(Vector3 ao, Vector3 ab) - { - if (Vector3.Dot(ab, ao) >= 0) - { - Vector3 cabo = Vector3.Cross(ab, ao); - if (cabo.LengthSquared() > _gjkSquareInSimplexEpsilon) - { _ray = Vector3.Cross(cabo, ab); } - else - { return true; } - } - else - { - _order = 0; - _simplex[0].Ray = _simplex[1].Ray; - _simplex[0].Vertice = _simplex[1].Vertice; - - _ray = ao; - } - return false; - } - - public bool SolveSimplex3(Vector3 ao, Vector3 ab, Vector3 ac) - { - return (SolveSimplex3a(ao, ab, ac, Vector3.Cross(ab, ac))); - } - - public bool SolveSimplex3a(Vector3 ao, Vector3 ab, Vector3 ac, Vector3 cabc) - { - if ((Vector3.Dot(Vector3.Cross(cabc, ab), ao)) < -_gjkInSimplexEpsilon) - { - _order = 1; - _simplex[0].Vertice = _simplex[1].Vertice; - _simplex[0].Ray = _simplex[1].Ray; - - _simplex[1].Vertice = _simplex[2].Vertice; - _simplex[1].Ray = _simplex[2].Ray; - - return (SolveSimplex2(ao, ab)); - } - else if (Vector3.Dot(Vector3.Cross(cabc, ac), ao) > +_gjkInSimplexEpsilon) - { - _order = 1; - _simplex[1].Vertice = _simplex[2].Vertice; - _simplex[1].Ray = _simplex[2].Ray; - - return (SolveSimplex2(ao, ac)); - } - else - { - float d = Vector3.Dot(cabc, ao); - if (Abs(d) > _gjkInSimplexEpsilon) - { - if (d > 0) - { _ray = cabc; } - else - { _ray = -cabc; Swap(ref _simplex[0], ref _simplex[1]); } - return (false); - } - else return (true); - } - } - - public bool SolveSimplex4(Vector3 ao, Vector3 ab, Vector3 ac, Vector3 ad) - { - Vector3 crs; - if (Vector3.Dot((crs = Vector3.Cross(ab, ac)), ao) > _gjkInSimplexEpsilon) - { - _order = 2; - _simplex[0].Vertice = _simplex[1].Vertice; - _simplex[0].Ray = _simplex[1].Ray; - - _simplex[1].Vertice = _simplex[2].Vertice; - _simplex[1].Ray = _simplex[2].Ray; - - _simplex[2].Vertice = _simplex[3].Vertice; - _simplex[2].Ray = _simplex[3].Ray; - - return (SolveSimplex3a(ao, ab, ac, crs)); - } - else if (Vector3.Dot((crs = Vector3.Cross(ac, ad)), ao) > _gjkInSimplexEpsilon) - { - _order = 2; - _simplex[2].Vertice = _simplex[3].Vertice; - _simplex[2].Ray = _simplex[3].Ray; - - return (SolveSimplex3a(ao, ac, ad, crs)); - } - else if (Vector3.Dot((crs = Vector3.Cross(ad, ab)), ao) > _gjkInSimplexEpsilon) - { - _order = 2; - - _simplex[1].Vertice = _simplex[0].Vertice; - _simplex[1].Ray = _simplex[0].Ray; - - _simplex[0].Vertice = _simplex[2].Vertice; - _simplex[0].Ray = _simplex[2].Ray; - - _simplex[2].Vertice = _simplex[3].Vertice; - _simplex[2].Ray = _simplex[3].Ray; - - return (SolveSimplex3a(ao, ad, ab, crs)); - } - else return (true); - } - - public bool SearchOrigin() - { - return SearchOrigin(new Vector3(1, 0, 0)); - } - - public bool SearchOrigin(Vector3 initray) - { - _iterations = 0; - unchecked - { - _order = (int)(-1); - } - _failed = false; - _ray = Vector3.Normalize(initray); - - //ClearMemory(table, sizeof(void*) * GJK_hashsize); - for (int i = 0; i < _table.Length; i++) - _table[i] = null; - FetchSupport(); - _ray = -_simplex[0].Vertice; - for (; _iterations < _gjkMaxIterations; ++_iterations) - { - float rl = _ray.Length(); - _ray /= rl > 0 ? rl : 1; - if (FetchSupport()) - { - bool found = (false); - switch (_order) - { - case 1: found = SolveSimplex2(-_simplex[1].Vertice, _simplex[0].Vertice - _simplex[1].Vertice); break; - case 2: found = SolveSimplex3(-_simplex[2].Vertice, _simplex[1].Vertice - _simplex[2].Vertice, _simplex[0].Vertice - _simplex[2].Vertice); break; - case 3: found = SolveSimplex4(-_simplex[3].Vertice, _simplex[2].Vertice - _simplex[3].Vertice, _simplex[1].Vertice - _simplex[3].Vertice, _simplex[0].Vertice - _simplex[3].Vertice); break; - } - if (found) return (true); - } - else return (false); - } - _failed = true; - return (false); - } - - public bool EncloseOrigin() - { - switch (_order) - { - /* Point */ - case 0: break; - /* Line */ - case 1: - Vector3 ab = _simplex[1].Vertice - _simplex[0].Vertice; - Vector3[] b ={ Vector3.Cross(ab, new Vector3(1, 0, 0)), - Vector3.Cross(ab, new Vector3(0, 1, 0)), - Vector3.Cross(ab, new Vector3(0, 0, 1)) }; - float[] m ={ b[0].LengthSquared(), b[1].LengthSquared(), b[2].LengthSquared() }; - Matrix r = Matrix.CreateFromQuaternion(new Quaternion(Vector3.Normalize(ab), _twoPi / 3)); - Vector3 w = b[m[0] > m[1] ? m[0] > m[2] ? 0 : 2 : m[1] > m[2] ? 1 : 2]; - Support(Vector3.Normalize(w), ref _simplex[4]); w = Vector3.TransformNormal(w, r); - Support(Vector3.Normalize(w), ref _simplex[2]); w = Vector3.TransformNormal(w, r); - Support(Vector3.Normalize(w), ref _simplex[3]); w = Vector3.TransformNormal(w, r); - _order = 4; - return true; - /* Triangle */ - case 2: - Vector3 n = Vector3.Normalize(Vector3.Cross(_simplex[1].Vertice - _simplex[0].Vertice, _simplex[2].Vertice - _simplex[0].Vertice)); - Support(n, ref _simplex[3]); - Support(-n, ref _simplex[4]); - _order = 4; - return true; - /* Tetrahedron */ - case 3: return (true); - /* Hexahedron */ - case 4: return (true); - } - return (false); - } - } - - public class Epa - { - public class Face - { - public Gjk.MinkowskiVertice[] _vertices = new Gjk.MinkowskiVertice[3]; - public Face[] _faces = new Face[3]; - public int[] _e = new int[3]; - public Vector3 _n; - public float _d; - public int _mark; - public Face _prev; - public Face _next; - } - - private Gjk _gjk; - private Face _root; - private int _nfaces; - private int _iterations; - private Vector3[,] _features = new Vector3[2, 3]; - private Vector3[] _nearest = new Vector3[2]; - private Vector3 _normal; - private float _depth; - private bool _failed; - - public Epa(Gjk gjk) - { - this._gjk = gjk; - } - - public bool Failed { get { return _failed; } } - public int Iterations { get { return _iterations; } } - public Vector3 Normal { get { return _normal; } } - public Vector3[] Nearest { get { return _nearest; } } - - public Vector3 GetCoordinates(Face face) - { - Vector3 o = face._n * -face._d; - float[] a ={ Vector3.Cross(face._vertices[0].Vertice - o, face._vertices[1].Vertice - o).Length(), - Vector3.Cross(face._vertices[1].Vertice - o, face._vertices[2].Vertice - o).Length(), - Vector3.Cross(face._vertices[2].Vertice - o, face._vertices[0].Vertice - o).Length()}; - float sm = a[0] + a[1] + a[2]; - return (new Vector3(a[1], a[2], a[0]) / (sm > 0 ? sm : 1)); - } - - public Face FindBest() - { - Face bf = null; - if (_root != null) - { - Face cf = _root; - float bd = _infinity; - do - { - if (cf._d < bd) { bd = cf._d; bf = cf; } - } while (null != (cf = cf._next)); - } - return bf; - } - - public bool Set(ref Face f, Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c) - { - Vector3 nrm = Vector3.Cross(b.Vertice - a.Vertice, c.Vertice - a.Vertice); - float len = nrm.Length(); - bool valid = (Vector3.Dot(Vector3.Cross(a.Vertice, b.Vertice), nrm) >= -_epaInFaceEpsilon && - Vector3.Dot(Vector3.Cross(b.Vertice, c.Vertice), nrm) >= -_epaInFaceEpsilon && - Vector3.Dot(Vector3.Cross(c.Vertice, a.Vertice), nrm) >= -_epaInFaceEpsilon); - f._vertices[0] = a; - f._vertices[1] = b; - f._vertices[2] = c; - f._mark = 0; - f._n = nrm / (len > 0 ? len : _infinity); - f._d = Max(0, -Vector3.Dot(f._n, a.Vertice)); - return valid; - } - - public Face NewFace(Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c) - { - Face pf = new Face(); - if (Set(ref pf, a, b, c)) - { - if (_root != null) _root._prev = pf; - pf._prev = null; - pf._next = _root; - _root = pf; - ++_nfaces; - } - else - { - pf._prev = pf._next = null; - } - return (pf); - } - - public void Detach(ref Face face) - { - if (face._prev != null || face._next != null) - { - --_nfaces; - if (face == _root) - { - _root = face._next; - _root._prev = null; - } - else - { - if (face._next == null) - { - face._prev._next = null; - } - else - { - face._prev._next = face._next; - face._next._prev = face._prev; - } - } - face._prev = face._next = null; - } - } - - public void Link(ref Face f0, int e0, ref Face f1, int e1) - { - f0._faces[e0] = f1; f1._e[e1] = e0; - f1._faces[e1] = f0; f0._e[e0] = e1; - } - - public Gjk.MinkowskiVertice Support(Vector3 w) - { - Gjk.MinkowskiVertice v = new Gjk.MinkowskiVertice(); - _gjk.Support(w, ref v); - return v; - } - - private static int[] mod3 ={ 0, 1, 2, 0, 1 }; - - public int BuildHorizon(int markid, Gjk.MinkowskiVertice w, ref Face f, int e, ref Face cf, ref Face ff) - { - int ne = (0); - if (f._mark != markid) - { - int e1 = (mod3[e + 1]); - if ((Vector3.Dot(f._n, w.Vertice) + f._d) > 0) - { - Face nf = NewFace(f._vertices[e1], f._vertices[e], w); - Link(ref nf, 0, ref f, e); - if (cf != null) Link(ref cf, 1, ref nf, 2); else ff = nf; - cf = nf; ne = 1; - } - else - { - int e2 = (mod3[e + 2]); - Detach(ref f); - f._mark = markid; - ne += BuildHorizon(markid, w, ref f._faces[e1], f._e[e1], ref cf, ref ff); - ne += BuildHorizon(markid, w, ref f._faces[e2], f._e[e2], ref cf, ref ff); - } - } - return (ne); - } - - public float EvaluatePD() - { - return EvaluatePD(_epaAccuracy); - } - - private int[,] fidx; - private int[,] eidx; - - public float EvaluatePD(float accuracy) - { - //Block* sablock = sa->BeginBlock(); - Face bestface = null; - int markid = 1; - _depth = -_infinity; - _normal = new Vector3(); - _root = null; - _nfaces = 0; - _iterations = 0; - _failed = false; - /* Prepare hull */ - if (_gjk.EncloseOrigin()) - { - int nfidx = 0; - int neidx = 0; - Gjk.MinkowskiVertice[] basemkv = new Gjk.MinkowskiVertice[5]; - Face[] basefaces = new Face[6]; - switch (_gjk.Order) - { - /* Tetrahedron */ - case 3: - { - fidx = new int[,] { { 2, 1, 0 }, { 3, 0, 1 }, { 3, 1, 2 }, { 3, 2, 0 } }; - eidx = new int[,] { { 0, 0, 2, 1 }, { 0, 1, 1, 1 }, { 0, 2, 3, 1 }, { 1, 0, 3, 2 }, { 2, 0, 1, 2 }, { 3, 0, 2, 2 } }; - nfidx = 4; neidx = 6; - } break; - /* Hexahedron */ - case 4: - { - fidx = new int[,] { { 2, 0, 4 }, { 4, 1, 2 }, { 1, 4, 0 }, { 0, 3, 1 }, { 0, 2, 3 }, { 1, 3, 2 } }; - eidx = new int[,] { { 0, 0, 4, 0 }, { 0, 1, 2, 1 }, { 0, 2, 1, 2 }, { 1, 1, 5, 2 }, { 1, 0, 2, 0 }, { 2, 2, 3, 2 }, { 3, 1, 5, 0 }, { 3, 0, 4, 2 }, { 5, 1, 4, 1 } }; - nfidx = 6; neidx = 9; - } break; - } - int i; - - for (i = 0; i <= _gjk.Order; ++i) - { - //basemkv[i] = (GJK::Mkv*)sa->Allocate(sizeof(GJK::Mkv)); - basemkv[i] = new Gjk.MinkowskiVertice(); - basemkv[i].Vertice = _gjk.Simplex[i].Vertice; - basemkv[i].Ray = _gjk.Simplex[i].Ray; - } - for (i = 0; i < nfidx; ++i) - { - basefaces[i] = NewFace(basemkv[fidx[i, 0]], basemkv[fidx[i, 1]], basemkv[fidx[i, 2]]); - } - for (i = 0; i < neidx; ++i) - { - Link(ref basefaces[eidx[i, 0]], eidx[i, 1], ref basefaces[eidx[i, 2]], eidx[i, 3]); - } - } - if (0 == _nfaces) - { - return _depth; - } - /* Expand hull */ - for (; _iterations < _epaMaxIterations; ++_iterations) - { - Face bf = FindBest(); - if (bf != null) - { - Gjk.MinkowskiVertice w = Support(-bf._n); - float d = Vector3.Dot(bf._n, w.Vertice) + bf._d; - bestface = bf; - if (d < -accuracy) - { - Face cf = null; - Face ff = null; - int nf = 0; - Detach(ref bf); - bf._mark = ++markid; - for (int i = 0; i < 3; ++i) - { - nf += BuildHorizon(markid, w, ref bf._faces[i], bf._e[i], ref cf, ref ff); - } - if (nf <= 2) { break; } - Link(ref cf, 1, ref ff, 2); - } - else break; - } - else break; - } - /* Extract contact */ - if (bestface != null) - { - Vector3 b = GetCoordinates(bestface); - _normal = bestface._n; - _depth = Max(0, bestface._d); - for (int i = 0; i < 2; ++i) - { - float s = i != 0 ? -1 : 1; - for (int j = 0; j < 3; ++j) - { - _features[i, j] = _gjk.LocalSupport(s * bestface._vertices[j].Ray, i); - } - } - _nearest[0] = _features[0, 0] * b.X + _features[0, 1] * b.Y + _features[0, 2] * b.Z; - _nearest[1] = _features[1, 0] * b.X + _features[1, 1] * b.Y + _features[1, 2] * b.Z; - } - else _failed = true; - return _depth; - } - - private float Max(float a, float b) - { - return (a > b ? a : b); - } - - private float Min(float a, float b) - { - return (a < b ? a : b); - } - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// + /// GJK-EPA collision solver by Nathanael Presson + /// Nov.2006 + /// + public class GjkEpa + { + //private static readonly int _precision = 1 /* U(sizeof(F) == 4)*/; + + private static readonly float _infinity = MathHelper.Infinity; + //private static readonly float _pi = (float)Math.PI; + private static readonly float _twoPi = (float)(Math.PI * 2); + + private static readonly int _gjkMaxIterations = 128; + private static readonly int _gjkHashSize = 1 << 6; + private static readonly int _gjkHashMask = _gjkHashSize - 1; + private static readonly float _gjkInSimplexEpsilon = 0.0001f; + private static readonly float _gjkSquareInSimplexEpsilon = _gjkInSimplexEpsilon * _gjkInSimplexEpsilon; + + private static readonly int _epaMaxIterations = 256; + private static readonly float _epaInFaceEpsilon = 0.01f; + private static readonly float _epaAccuracy = 0.001f; + + public static float EpaAccuracy { get { return _epaAccuracy; } } + + private static float Abs(float v) { return (v < 0 ? -v : v); } + private static float Sign(float v) { return (v < 0 ? -1 : 1); } + + static void Swap(ref T a, ref T b) + { + T t = a; + a = b; + b = t; + } + + public class Gjk + { + public class MinkowskiVertice + { + private Vector3 _vertice; /* Minkowski vertice */ + private Vector3 _ray; /* Ray */ + + public Vector3 Vertice { get { return _vertice; } set { _vertice = value; } } + public Vector3 Ray { get { return _ray; } set { _ray = value; } } + } + public class He + { + private Vector3 _ray; + private He _next; + + public He Next { get { return _next; } set { _next = value; } } + public Vector3 Ray { get { return _ray; } set { _ray = value; } } + } + + private He[] _table = new He[_gjkHashSize]; + private Matrix[] _wrotations = new Matrix[2]; + private Vector3[] _positions = new Vector3[2]; + private ConvexShape[] _shapes = new ConvexShape[2]; + private MinkowskiVertice[] _simplex = new MinkowskiVertice[5]; + private Vector3 _ray; + private int _order; + private int _iterations; + private float _margin; + private bool _failed; + + public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA, + Matrix wrotationB, Vector3 positionB, ConvexShape shapeB) + : this(wrotationA, positionA, shapeA, wrotationB, positionB, shapeB, 0) { } + + public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA, + Matrix wrotationB, Vector3 positionB, ConvexShape shapeB, + float pmargin) + { + for (int i = 0; i < _simplex.Length; i++) + _simplex[i] = new MinkowskiVertice(); + + for (int i = 0; i < _wrotations.Length; i++) + _wrotations[i] = new Matrix(); + + for (int i = 0; i < _positions.Length; i++) + _positions[i] = new Vector3(); + + _wrotations[0] = wrotationA; _positions[0] = positionA; + _shapes[0] = shapeA; + _wrotations[0].Translation = Vector3.Zero; + _wrotations[1] = wrotationB; _positions[1] = positionB; + _shapes[1] = shapeB; + _wrotations[1].Translation = Vector3.Zero; + //sablock = sa->BeginBlock(); + _margin = pmargin; + _failed = false; + } + + public bool Failed { get { return _failed; } } + public int Iterations { get { return _iterations; } } + public int Order { get { return _order; } } + public MinkowskiVertice[] Simplex { get { return _simplex; } } + + public int Hash(Vector3 v) + { + int h = ((int)(v.X * 15461) ^ (int)(v.Y * 83003) ^ (int)(v.Z * 15473)); + return (h * 169639) & _gjkHashMask; + } + + public bool FetchSupport() + { + int h = Hash(_ray); + He e = _table[h]; + while (e != null) + { + if (e.Ray == _ray) + { + --_order; + return (false); + } + else e = e.Next; + } + e = new He(); + e.Ray = _ray; + e.Next = _table[h]; + _table[h] = e; + Support(_ray, ref _simplex[++_order]); + return (Vector3.Dot(_ray, _simplex[_order].Vertice) > 0); + } + + public Vector3 LocalSupport(Vector3 d, int i) + { + Matrix m = _wrotations[i]; + m.Translation = Vector3.Zero; + Vector3 vtx = Vector3.TransformNormal(d, m); + Vector3 result = MathHelper.MatrixToVector(_wrotations[i], _shapes[i].LocalGetSupportingVertex(vtx)); + return (result + _positions[i]); + } + + public void Support(Vector3 d, ref MinkowskiVertice v) + { + v.Ray = d; + v.Vertice = LocalSupport(d, 0) - LocalSupport(-d, 1) + d * _margin; + } + + public bool SolveSimplex2(Vector3 ao, Vector3 ab) + { + if (Vector3.Dot(ab, ao) >= 0) + { + Vector3 cabo = Vector3.Cross(ab, ao); + if (cabo.LengthSquared() > _gjkSquareInSimplexEpsilon) + { _ray = Vector3.Cross(cabo, ab); } + else + { return true; } + } + else + { + _order = 0; + _simplex[0].Ray = _simplex[1].Ray; + _simplex[0].Vertice = _simplex[1].Vertice; + + _ray = ao; + } + return false; + } + + public bool SolveSimplex3(Vector3 ao, Vector3 ab, Vector3 ac) + { + return (SolveSimplex3a(ao, ab, ac, Vector3.Cross(ab, ac))); + } + + public bool SolveSimplex3a(Vector3 ao, Vector3 ab, Vector3 ac, Vector3 cabc) + { + if ((Vector3.Dot(Vector3.Cross(cabc, ab), ao)) < -_gjkInSimplexEpsilon) + { + _order = 1; + _simplex[0].Vertice = _simplex[1].Vertice; + _simplex[0].Ray = _simplex[1].Ray; + + _simplex[1].Vertice = _simplex[2].Vertice; + _simplex[1].Ray = _simplex[2].Ray; + + return (SolveSimplex2(ao, ab)); + } + else if (Vector3.Dot(Vector3.Cross(cabc, ac), ao) > +_gjkInSimplexEpsilon) + { + _order = 1; + _simplex[1].Vertice = _simplex[2].Vertice; + _simplex[1].Ray = _simplex[2].Ray; + + return (SolveSimplex2(ao, ac)); + } + else + { + float d = Vector3.Dot(cabc, ao); + if (Abs(d) > _gjkInSimplexEpsilon) + { + if (d > 0) + { _ray = cabc; } + else + { _ray = -cabc; Swap(ref _simplex[0], ref _simplex[1]); } + return (false); + } + else return (true); + } + } + + public bool SolveSimplex4(Vector3 ao, Vector3 ab, Vector3 ac, Vector3 ad) + { + Vector3 crs; + if (Vector3.Dot((crs = Vector3.Cross(ab, ac)), ao) > _gjkInSimplexEpsilon) + { + _order = 2; + _simplex[0].Vertice = _simplex[1].Vertice; + _simplex[0].Ray = _simplex[1].Ray; + + _simplex[1].Vertice = _simplex[2].Vertice; + _simplex[1].Ray = _simplex[2].Ray; + + _simplex[2].Vertice = _simplex[3].Vertice; + _simplex[2].Ray = _simplex[3].Ray; + + return (SolveSimplex3a(ao, ab, ac, crs)); + } + else if (Vector3.Dot((crs = Vector3.Cross(ac, ad)), ao) > _gjkInSimplexEpsilon) + { + _order = 2; + _simplex[2].Vertice = _simplex[3].Vertice; + _simplex[2].Ray = _simplex[3].Ray; + + return (SolveSimplex3a(ao, ac, ad, crs)); + } + else if (Vector3.Dot((crs = Vector3.Cross(ad, ab)), ao) > _gjkInSimplexEpsilon) + { + _order = 2; + + _simplex[1].Vertice = _simplex[0].Vertice; + _simplex[1].Ray = _simplex[0].Ray; + + _simplex[0].Vertice = _simplex[2].Vertice; + _simplex[0].Ray = _simplex[2].Ray; + + _simplex[2].Vertice = _simplex[3].Vertice; + _simplex[2].Ray = _simplex[3].Ray; + + return (SolveSimplex3a(ao, ad, ab, crs)); + } + else return (true); + } + + public bool SearchOrigin() + { + return SearchOrigin(new Vector3(1, 0, 0)); + } + + public bool SearchOrigin(Vector3 initray) + { + _iterations = 0; + unchecked + { + _order = (int)(-1); + } + _failed = false; + _ray = Vector3.Normalize(initray); + + //ClearMemory(table, sizeof(void*) * GJK_hashsize); + for (int i = 0; i < _table.Length; i++) + _table[i] = null; + FetchSupport(); + _ray = -_simplex[0].Vertice; + for (; _iterations < _gjkMaxIterations; ++_iterations) + { + float rl = _ray.Length(); + _ray /= rl > 0 ? rl : 1; + if (FetchSupport()) + { + bool found = (false); + switch (_order) + { + case 1: found = SolveSimplex2(-_simplex[1].Vertice, _simplex[0].Vertice - _simplex[1].Vertice); break; + case 2: found = SolveSimplex3(-_simplex[2].Vertice, _simplex[1].Vertice - _simplex[2].Vertice, _simplex[0].Vertice - _simplex[2].Vertice); break; + case 3: found = SolveSimplex4(-_simplex[3].Vertice, _simplex[2].Vertice - _simplex[3].Vertice, _simplex[1].Vertice - _simplex[3].Vertice, _simplex[0].Vertice - _simplex[3].Vertice); break; + } + if (found) return (true); + } + else return (false); + } + _failed = true; + return (false); + } + + public bool EncloseOrigin() + { + switch (_order) + { + /* Point */ + case 0: break; + /* Line */ + case 1: + Vector3 ab = _simplex[1].Vertice - _simplex[0].Vertice; + Vector3[] b ={ Vector3.Cross(ab, new Vector3(1, 0, 0)), + Vector3.Cross(ab, new Vector3(0, 1, 0)), + Vector3.Cross(ab, new Vector3(0, 0, 1)) }; + float[] m ={ b[0].LengthSquared(), b[1].LengthSquared(), b[2].LengthSquared() }; + Matrix r = Matrix.CreateFromQuaternion(new Quaternion(Vector3.Normalize(ab), _twoPi / 3)); + Vector3 w = b[m[0] > m[1] ? m[0] > m[2] ? 0 : 2 : m[1] > m[2] ? 1 : 2]; + Support(Vector3.Normalize(w), ref _simplex[4]); w = Vector3.TransformNormal(w, r); + Support(Vector3.Normalize(w), ref _simplex[2]); w = Vector3.TransformNormal(w, r); + Support(Vector3.Normalize(w), ref _simplex[3]); w = Vector3.TransformNormal(w, r); + _order = 4; + return true; + /* Triangle */ + case 2: + Vector3 n = Vector3.Normalize(Vector3.Cross(_simplex[1].Vertice - _simplex[0].Vertice, _simplex[2].Vertice - _simplex[0].Vertice)); + Support(n, ref _simplex[3]); + Support(-n, ref _simplex[4]); + _order = 4; + return true; + /* Tetrahedron */ + case 3: return (true); + /* Hexahedron */ + case 4: return (true); + } + return (false); + } + } + + public class Epa + { + public class Face + { + public Gjk.MinkowskiVertice[] _vertices = new Gjk.MinkowskiVertice[3]; + public Face[] _faces = new Face[3]; + public int[] _e = new int[3]; + public Vector3 _n; + public float _d; + public int _mark; + public Face _prev; + public Face _next; + } + + private Gjk _gjk; + private Face _root; + private int _nfaces; + private int _iterations; + private Vector3[,] _features = new Vector3[2, 3]; + private Vector3[] _nearest = new Vector3[2]; + private Vector3 _normal; + private float _depth; + private bool _failed; + + public Epa(Gjk gjk) + { + this._gjk = gjk; + } + + public bool Failed { get { return _failed; } } + public int Iterations { get { return _iterations; } } + public Vector3 Normal { get { return _normal; } } + public Vector3[] Nearest { get { return _nearest; } } + + public Vector3 GetCoordinates(Face face) + { + Vector3 o = face._n * -face._d; + float[] a ={ Vector3.Cross(face._vertices[0].Vertice - o, face._vertices[1].Vertice - o).Length(), + Vector3.Cross(face._vertices[1].Vertice - o, face._vertices[2].Vertice - o).Length(), + Vector3.Cross(face._vertices[2].Vertice - o, face._vertices[0].Vertice - o).Length()}; + float sm = a[0] + a[1] + a[2]; + return (new Vector3(a[1], a[2], a[0]) / (sm > 0 ? sm : 1)); + } + + public Face FindBest() + { + Face bf = null; + if (_root != null) + { + Face cf = _root; + float bd = _infinity; + do + { + if (cf._d < bd) { bd = cf._d; bf = cf; } + } while (null != (cf = cf._next)); + } + return bf; + } + + public bool Set(ref Face f, Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c) + { + Vector3 nrm = Vector3.Cross(b.Vertice - a.Vertice, c.Vertice - a.Vertice); + float len = nrm.Length(); + bool valid = (Vector3.Dot(Vector3.Cross(a.Vertice, b.Vertice), nrm) >= -_epaInFaceEpsilon && + Vector3.Dot(Vector3.Cross(b.Vertice, c.Vertice), nrm) >= -_epaInFaceEpsilon && + Vector3.Dot(Vector3.Cross(c.Vertice, a.Vertice), nrm) >= -_epaInFaceEpsilon); + f._vertices[0] = a; + f._vertices[1] = b; + f._vertices[2] = c; + f._mark = 0; + f._n = nrm / (len > 0 ? len : _infinity); + f._d = Max(0, -Vector3.Dot(f._n, a.Vertice)); + return valid; + } + + public Face NewFace(Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c) + { + Face pf = new Face(); + if (Set(ref pf, a, b, c)) + { + if (_root != null) _root._prev = pf; + pf._prev = null; + pf._next = _root; + _root = pf; + ++_nfaces; + } + else + { + pf._prev = pf._next = null; + } + return (pf); + } + + public void Detach(ref Face face) + { + if (face._prev != null || face._next != null) + { + --_nfaces; + if (face == _root) + { + _root = face._next; + _root._prev = null; + } + else + { + if (face._next == null) + { + face._prev._next = null; + } + else + { + face._prev._next = face._next; + face._next._prev = face._prev; + } + } + face._prev = face._next = null; + } + } + + public void Link(ref Face f0, int e0, ref Face f1, int e1) + { + f0._faces[e0] = f1; f1._e[e1] = e0; + f1._faces[e1] = f0; f0._e[e0] = e1; + } + + public Gjk.MinkowskiVertice Support(Vector3 w) + { + Gjk.MinkowskiVertice v = new Gjk.MinkowskiVertice(); + _gjk.Support(w, ref v); + return v; + } + + private static int[] mod3 ={ 0, 1, 2, 0, 1 }; + + public int BuildHorizon(int markid, Gjk.MinkowskiVertice w, ref Face f, int e, ref Face cf, ref Face ff) + { + int ne = (0); + if (f._mark != markid) + { + int e1 = (mod3[e + 1]); + if ((Vector3.Dot(f._n, w.Vertice) + f._d) > 0) + { + Face nf = NewFace(f._vertices[e1], f._vertices[e], w); + Link(ref nf, 0, ref f, e); + if (cf != null) Link(ref cf, 1, ref nf, 2); else ff = nf; + cf = nf; ne = 1; + } + else + { + int e2 = (mod3[e + 2]); + Detach(ref f); + f._mark = markid; + ne += BuildHorizon(markid, w, ref f._faces[e1], f._e[e1], ref cf, ref ff); + ne += BuildHorizon(markid, w, ref f._faces[e2], f._e[e2], ref cf, ref ff); + } + } + return (ne); + } + + public float EvaluatePD() + { + return EvaluatePD(_epaAccuracy); + } + + private int[,] fidx; + private int[,] eidx; + + public float EvaluatePD(float accuracy) + { + //Block* sablock = sa->BeginBlock(); + Face bestface = null; + int markid = 1; + _depth = -_infinity; + _normal = new Vector3(); + _root = null; + _nfaces = 0; + _iterations = 0; + _failed = false; + /* Prepare hull */ + if (_gjk.EncloseOrigin()) + { + int nfidx = 0; + int neidx = 0; + Gjk.MinkowskiVertice[] basemkv = new Gjk.MinkowskiVertice[5]; + Face[] basefaces = new Face[6]; + switch (_gjk.Order) + { + /* Tetrahedron */ + case 3: + { + fidx = new int[,] { { 2, 1, 0 }, { 3, 0, 1 }, { 3, 1, 2 }, { 3, 2, 0 } }; + eidx = new int[,] { { 0, 0, 2, 1 }, { 0, 1, 1, 1 }, { 0, 2, 3, 1 }, { 1, 0, 3, 2 }, { 2, 0, 1, 2 }, { 3, 0, 2, 2 } }; + nfidx = 4; neidx = 6; + } break; + /* Hexahedron */ + case 4: + { + fidx = new int[,] { { 2, 0, 4 }, { 4, 1, 2 }, { 1, 4, 0 }, { 0, 3, 1 }, { 0, 2, 3 }, { 1, 3, 2 } }; + eidx = new int[,] { { 0, 0, 4, 0 }, { 0, 1, 2, 1 }, { 0, 2, 1, 2 }, { 1, 1, 5, 2 }, { 1, 0, 2, 0 }, { 2, 2, 3, 2 }, { 3, 1, 5, 0 }, { 3, 0, 4, 2 }, { 5, 1, 4, 1 } }; + nfidx = 6; neidx = 9; + } break; + } + int i; + + for (i = 0; i <= _gjk.Order; ++i) + { + //basemkv[i] = (GJK::Mkv*)sa->Allocate(sizeof(GJK::Mkv)); + basemkv[i] = new Gjk.MinkowskiVertice(); + basemkv[i].Vertice = _gjk.Simplex[i].Vertice; + basemkv[i].Ray = _gjk.Simplex[i].Ray; + } + for (i = 0; i < nfidx; ++i) + { + basefaces[i] = NewFace(basemkv[fidx[i, 0]], basemkv[fidx[i, 1]], basemkv[fidx[i, 2]]); + } + for (i = 0; i < neidx; ++i) + { + Link(ref basefaces[eidx[i, 0]], eidx[i, 1], ref basefaces[eidx[i, 2]], eidx[i, 3]); + } + } + if (0 == _nfaces) + { + return _depth; + } + /* Expand hull */ + for (; _iterations < _epaMaxIterations; ++_iterations) + { + Face bf = FindBest(); + if (bf != null) + { + Gjk.MinkowskiVertice w = Support(-bf._n); + float d = Vector3.Dot(bf._n, w.Vertice) + bf._d; + bestface = bf; + if (d < -accuracy) + { + Face cf = null; + Face ff = null; + int nf = 0; + Detach(ref bf); + bf._mark = ++markid; + for (int i = 0; i < 3; ++i) + { + nf += BuildHorizon(markid, w, ref bf._faces[i], bf._e[i], ref cf, ref ff); + } + if (nf <= 2) { break; } + Link(ref cf, 1, ref ff, 2); + } + else break; + } + else break; + } + /* Extract contact */ + if (bestface != null) + { + Vector3 b = GetCoordinates(bestface); + _normal = bestface._n; + _depth = Max(0, bestface._d); + for (int i = 0; i < 2; ++i) + { + float s = i != 0 ? -1 : 1; + for (int j = 0; j < 3; ++j) + { + _features[i, j] = _gjk.LocalSupport(s * bestface._vertices[j].Ray, i); + } + } + _nearest[0] = _features[0, 0] * b.X + _features[0, 1] * b.Y + _features[0, 2] * b.Z; + _nearest[1] = _features[1, 0] * b.X + _features[1, 1] * b.Y + _features[1, 2] * b.Z; + } + else _failed = true; + return _depth; + } + + private float Max(float a, float b) + { + return (a > b ? a : b); + } + + private float Min(float a, float b) + { + return (a < b ? a : b); + } + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs index 739b4fc..551449f 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs @@ -1,56 +1,56 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// - /// EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to - /// calculate the penetration depth between two convex shapes. - /// - public class GjkEpaPenetrationDepthSolver : IConvexPenetrationDepthSolver - { - public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver, ConvexShape convexA, ConvexShape convexB, Matrix transformA, Matrix transformB, Vector3 vector, out Vector3 ptrA, out Vector3 ptrB, IDebugDraw debugDraw) - { - float radialmargin = 0; - - GjkEpaSolver.Results results; - if (GjkEpaSolver.Collide(convexA, transformA, - convexB, transformB, - radialmargin, out results)) - { - // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); - //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); - ptrA = results.Witnesses[0]; - ptrB = results.Witnesses[1]; - return true; - } - ptrA = new Vector3(); - ptrB = new Vector3(); - - return false; - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// + /// EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to + /// calculate the penetration depth between two convex shapes. + /// + public class GjkEpaPenetrationDepthSolver : IConvexPenetrationDepthSolver + { + public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver, ConvexShape convexA, ConvexShape convexB, Matrix transformA, Matrix transformB, Vector3 vector, out Vector3 ptrA, out Vector3 ptrB, IDebugDraw debugDraw) + { + float radialmargin = 0; + + GjkEpaSolver.Results results; + if (GjkEpaSolver.Collide(convexA, transformA, + convexB, transformB, + radialmargin, out results)) + { + // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); + //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); + ptrA = results.Witnesses[0]; + ptrB = results.Witnesses[1]; + return true; + } + ptrA = new Vector3(); + ptrB = new Vector3(); + + return false; + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs index aa9d61e..20af192 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs @@ -1,101 +1,101 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// - /// GjkEpaSolver contributed under zlib by Nathanael Presson - /// - public class GjkEpaSolver - { - public struct Results - { - public enum Status - { - Separated, /* Shapes doesnt penetrate */ - Penetrating, /* Shapes are penetrating */ - GjkFailed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ - EpaFailed, /* EPA phase fail, bigger problem, need to save parameters, and debug */ - } - - private Vector3[] _witnesses; - private Vector3 _normal; - private float _depth; - private int _epaIterations; - private int _gjkIterations; - private Status _status; - - public Vector3[] Witnesses { get { return _witnesses; } set { _witnesses = value; } } - public Vector3 Normal { get { return _normal; } set { _normal = value; } } - public float Depth { get { return _depth; } set { _depth = value; } } - public int EpaIterations { get { return _epaIterations; } set { _epaIterations = value; } } - public int GjkIterations { get { return _gjkIterations; } set { _gjkIterations = value; } } - public Status ResultStatus { get { return _status; } set { _status = value; } } - } - - public static bool Collide(ConvexShape shapeA, Matrix wtrsA, - ConvexShape shapeB, Matrix wtrsB, - float radialmargin, - out Results results) - { - /* Initialize */ - results = new Results(); - results.Witnesses = new Vector3[2]; - results.Witnesses[0] = - results.Witnesses[1] = - results.Normal = new Vector3(); - results.Depth = 0; - results.ResultStatus = Results.Status.Separated; - results.EpaIterations = 0; - results.GjkIterations = 0; - /* Use GJK to locate origin */ - GjkEpa.Gjk gjk = new GjkEpa.Gjk(wtrsA, wtrsA.Translation, shapeA, - wtrsB, wtrsB.Translation, shapeB, - radialmargin + GjkEpa.EpaAccuracy); - bool collide = gjk.SearchOrigin(); - results.GjkIterations = gjk.Iterations + 1; - if (collide) - { - /* Then EPA for penetration depth */ - GjkEpa.Epa epa = new GjkEpa.Epa(gjk); - float pd = epa.EvaluatePD(); - results.EpaIterations = epa.Iterations + 1; - if (pd > 0) - { - results.ResultStatus = Results.Status.Penetrating; - results.Normal = epa.Normal; - results.Depth = pd; - results.Witnesses[0] = epa.Nearest[0]; - results.Witnesses[1] = epa.Nearest[1]; - return true; - } - else { if (epa.Failed) results.ResultStatus = Results.Status.EpaFailed; } - } - else { if (gjk.Failed) results.ResultStatus = Results.Status.GjkFailed; } - return false; - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// + /// GjkEpaSolver contributed under zlib by Nathanael Presson + /// + public class GjkEpaSolver + { + public struct Results + { + public enum Status + { + Separated, /* Shapes doesnt penetrate */ + Penetrating, /* Shapes are penetrating */ + GjkFailed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ + EpaFailed, /* EPA phase fail, bigger problem, need to save parameters, and debug */ + } + + private Vector3[] _witnesses; + private Vector3 _normal; + private float _depth; + private int _epaIterations; + private int _gjkIterations; + private Status _status; + + public Vector3[] Witnesses { get { return _witnesses; } set { _witnesses = value; } } + public Vector3 Normal { get { return _normal; } set { _normal = value; } } + public float Depth { get { return _depth; } set { _depth = value; } } + public int EpaIterations { get { return _epaIterations; } set { _epaIterations = value; } } + public int GjkIterations { get { return _gjkIterations; } set { _gjkIterations = value; } } + public Status ResultStatus { get { return _status; } set { _status = value; } } + } + + public static bool Collide(ConvexShape shapeA, Matrix wtrsA, + ConvexShape shapeB, Matrix wtrsB, + float radialmargin, + out Results results) + { + /* Initialize */ + results = new Results(); + results.Witnesses = new Vector3[2]; + results.Witnesses[0] = + results.Witnesses[1] = + results.Normal = new Vector3(); + results.Depth = 0; + results.ResultStatus = Results.Status.Separated; + results.EpaIterations = 0; + results.GjkIterations = 0; + /* Use GJK to locate origin */ + GjkEpa.Gjk gjk = new GjkEpa.Gjk(wtrsA, wtrsA.Translation, shapeA, + wtrsB, wtrsB.Translation, shapeB, + radialmargin + GjkEpa.EpaAccuracy); + bool collide = gjk.SearchOrigin(); + results.GjkIterations = gjk.Iterations + 1; + if (collide) + { + /* Then EPA for penetration depth */ + GjkEpa.Epa epa = new GjkEpa.Epa(gjk); + float pd = epa.EvaluatePD(); + results.EpaIterations = epa.Iterations + 1; + if (pd > 0) + { + results.ResultStatus = Results.Status.Penetrating; + results.Normal = epa.Normal; + results.Depth = pd; + results.Witnesses[0] = epa.Nearest[0]; + results.Witnesses[1] = epa.Nearest[1]; + return true; + } + else { if (epa.Failed) results.ResultStatus = Results.Status.EpaFailed; } + } + else { if (gjk.Failed) results.ResultStatus = Results.Status.GjkFailed; } + return false; + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs index 0831ff5..7557ed9 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs @@ -1,343 +1,343 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public class GjkPairDetector : DiscreteCollisionDetectorInterface - { - private Vector3 _cachedSeparatingAxis; - private IConvexPenetrationDepthSolver _penetrationDepthSolver; - private ISimplexSolver _simplexSolver; - private ConvexShape _minkowskiA, _minkowskiB; - private bool _ignoreMargin; - - private int _lastUsedMethod; - private int _currentIteration; - private int _degenerateSimplex; - private int _catchDegeneracies; - - private static int _numDeepPenetrationChecks = 0; - private static int _numGjkChecks = 0; - - private const float RelativeError2 = 1.0e-6f; - - #region Properties - public int LastUsedMethod - { - get { return _lastUsedMethod; } - set { _lastUsedMethod = value; } - } - - public int CurrentIteration - { - get { return _currentIteration; } - set { _currentIteration = value; } - } - - public int DegenerateSimplex - { - get { return _degenerateSimplex; } - set { _degenerateSimplex = value; } - } - - public int CatchDegeneracies - { - get { return _catchDegeneracies; } - set { _catchDegeneracies = value; } - } - - public static int DeepPenetrationChecksCount { get { return _numDeepPenetrationChecks; } } - public static int GjkChecksCount { get { return _numGjkChecks; } } - #endregion - - public GjkPairDetector(ConvexShape objectA, ConvexShape objectB, - ISimplexSolver simplexSolver, - IConvexPenetrationDepthSolver penetrationDepthSolver) - { - _cachedSeparatingAxis = new Vector3(0, 0, 1); - - _penetrationDepthSolver = penetrationDepthSolver; - _simplexSolver = simplexSolver; - _minkowskiA = objectA; - _minkowskiB = objectB; - _ignoreMargin = false; - _lastUsedMethod = -1; - _catchDegeneracies = 1; - } - - public void setMinkowskiA(ConvexShape minkA) - { - _minkowskiA = minkA; - } - - public void setMinkowskiB(ConvexShape minkB) - { - _minkowskiB = minkB; - } - public void setCachedSeperatingAxis(Vector3 seperatingAxis) - { - _cachedSeparatingAxis = seperatingAxis; - } - - public void setPenetrationDepthSolver(IConvexPenetrationDepthSolver penetrationDepthSolver) - { - this._penetrationDepthSolver = penetrationDepthSolver; - } - - public void setIgnoreMargin(bool ignoreMargin) - { - this._ignoreMargin = ignoreMargin; - } - - public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw) - { - float distance = 0; - - Vector3 normalInB = new Vector3(); - Vector3 pointOnA = new Vector3(), pointOnB = new Vector3(); - - Matrix localTransA = input.TransformA; - Matrix localTransB = input.TransformB; - - Vector3 positionOffset = (localTransA.Translation + localTransB.Translation) * 0.5f; - localTransA.Translation -= positionOffset; - localTransB.Translation -= positionOffset; - - float marginA = _minkowskiA.Margin; - float marginB = _minkowskiB.Margin; - - _numGjkChecks++; - - if (_ignoreMargin) - { - marginA = 0; - marginB = 0; - } - - _currentIteration = 0; - - int gjkMaxIter = 1000; - _cachedSeparatingAxis = new Vector3(0, 1, 0); - - bool isValid = false; - bool checkSimplex = false; - bool checkPenetration = true; - _degenerateSimplex = 0; - - _lastUsedMethod = -1; - - { - float squaredDistance = MathHelper.Infinity; - float delta = 0; - - float margin = marginA + marginB; - - _simplexSolver.Reset(); - - while (true) - { - Matrix transABasis = input.TransformA; - transABasis.Translation = Vector3.Zero; - - Matrix transBBasis = input.TransformB; - transBBasis.Translation = Vector3.Zero; - - Vector3 seperatingAxisInA = Vector3.TransformNormal(-_cachedSeparatingAxis, transABasis); - Vector3 seperatingAxisInB = Vector3.TransformNormal(_cachedSeparatingAxis, transBBasis); - - Vector3 pInA = _minkowskiA.LocalGetSupportingVertexWithoutMargin(seperatingAxisInA); - Vector3 qInB = _minkowskiB.LocalGetSupportingVertexWithoutMargin(seperatingAxisInB); - Vector3 pWorld = MathHelper.MatrixToVector(localTransA, pInA); - Vector3 qWorld = MathHelper.MatrixToVector(localTransB, qInB); - - Vector3 w = pWorld - qWorld; - delta = Vector3.Dot(_cachedSeparatingAxis, w); - - if ((delta > 0.0) && (delta * delta > squaredDistance * input.MaximumDistanceSquared)) - { - checkPenetration = false; - break; - } - - if (_simplexSolver.InSimplex(w)) - { - _degenerateSimplex = 1; - checkSimplex = true; - break; - } - - float f0 = squaredDistance - delta; - float f1 = squaredDistance * RelativeError2; - - if (f0 <= f1) - { - if (f0 <= 0.0f) - { - _degenerateSimplex = 2; - } - - checkSimplex = true; - break; - } - - _simplexSolver.AddVertex(w, pWorld, qWorld); - - if (!_simplexSolver.Closest(out _cachedSeparatingAxis)) - { - _degenerateSimplex = 3; - checkSimplex = true; - break; - } - - float previouseSquaredDistance = squaredDistance; - squaredDistance = _cachedSeparatingAxis.LengthSquared(); - - if (previouseSquaredDistance - squaredDistance <= MathHelper.Epsilon * previouseSquaredDistance) - { - _simplexSolver.BackupClosest(out _cachedSeparatingAxis); - checkSimplex = true; - break; - } - - if (_currentIteration++ > gjkMaxIter) - { -#if DEBUG - Console.WriteLine("GjkPairDetector maxIter exceeded: {0}", _currentIteration); - Console.WriteLine("sepAxis=({0},{1},{2}), squaredDistance = {3}, shapeTypeA={4}, shapeTypeB={5}", - _cachedSeparatingAxis.X, - _cachedSeparatingAxis.Y, - _cachedSeparatingAxis.Z, - squaredDistance, - _minkowskiA.ShapeType, - _minkowskiB.ShapeType - ); -#endif - break; - } - - bool check = (!_simplexSolver.FullSimplex); - - if (!check) - { - _simplexSolver.BackupClosest(out _cachedSeparatingAxis); - break; - } - } - - if (checkSimplex) - { - _simplexSolver.ComputePoints(out pointOnA, out pointOnB); - normalInB = pointOnA - pointOnB; - float lenSqr = _cachedSeparatingAxis.LengthSquared(); - - if (lenSqr < 0.0001f) - { - _degenerateSimplex = 5; - } - - if (lenSqr > MathHelper.Epsilon * MathHelper.Epsilon) - { - float rlen = 1.0f / (float)Math.Sqrt((float)lenSqr); - normalInB *= rlen; - float s = (float)Math.Sqrt((float)squaredDistance); - - BulletDebug.Assert(s > 0); - pointOnA -= _cachedSeparatingAxis * (marginA / s); - pointOnB += _cachedSeparatingAxis * (marginB / s); - distance = ((1 / rlen) - margin); - - isValid = true; - - _lastUsedMethod = 1; - } - else - { - _lastUsedMethod = 2; - } - } - - bool catchDegeneratePenetrationCase = - (_catchDegeneracies != 0 && _penetrationDepthSolver != null && _degenerateSimplex != 0 && ((distance + margin) < 0.01f)); - - if (checkPenetration && (!isValid || catchDegeneratePenetrationCase)) - { -#warning Check this - if (_penetrationDepthSolver != null) - { - Vector3 tmpPointOnA, tmpPointOnB; - - _numDeepPenetrationChecks++; - - bool isValid2 = _penetrationDepthSolver.CalculatePenetrationDepth( - _simplexSolver, _minkowskiA, _minkowskiB, localTransA, localTransB, - _cachedSeparatingAxis, out tmpPointOnA, out tmpPointOnB, - debugDraw - ); - - if (isValid2) - { - Vector3 tmpNormalInB = tmpPointOnB - tmpPointOnA; - float lengSqr = tmpNormalInB.LengthSquared(); - - if (lengSqr > (MathHelper.Epsilon * MathHelper.Epsilon)) - { - tmpNormalInB /= (float)Math.Sqrt((float)lengSqr); - float distance2 = -(tmpPointOnA - tmpPointOnB).Length(); - - if (!isValid || (distance2 < distance)) - { - distance = distance2; - pointOnA = tmpPointOnA; - pointOnB = tmpPointOnB; - normalInB = tmpNormalInB; - isValid = true; - _lastUsedMethod = 3; - } - else - { - - } - } - else - { - _lastUsedMethod = 4; - } - } - else - { - _lastUsedMethod = 5; - } - } - } - - if (isValid) - { - output.AddContactPoint(normalInB, pointOnB + positionOffset, distance); - } - } - } - } +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public class GjkPairDetector : DiscreteCollisionDetectorInterface + { + private Vector3 _cachedSeparatingAxis; + private IConvexPenetrationDepthSolver _penetrationDepthSolver; + private ISimplexSolver _simplexSolver; + private ConvexShape _minkowskiA, _minkowskiB; + private bool _ignoreMargin; + + private int _lastUsedMethod; + private int _currentIteration; + private int _degenerateSimplex; + private int _catchDegeneracies; + + private static int _numDeepPenetrationChecks = 0; + private static int _numGjkChecks = 0; + + private const float RelativeError2 = 1.0e-6f; + + #region Properties + public int LastUsedMethod + { + get { return _lastUsedMethod; } + set { _lastUsedMethod = value; } + } + + public int CurrentIteration + { + get { return _currentIteration; } + set { _currentIteration = value; } + } + + public int DegenerateSimplex + { + get { return _degenerateSimplex; } + set { _degenerateSimplex = value; } + } + + public int CatchDegeneracies + { + get { return _catchDegeneracies; } + set { _catchDegeneracies = value; } + } + + public static int DeepPenetrationChecksCount { get { return _numDeepPenetrationChecks; } } + public static int GjkChecksCount { get { return _numGjkChecks; } } + #endregion + + public GjkPairDetector(ConvexShape objectA, ConvexShape objectB, + ISimplexSolver simplexSolver, + IConvexPenetrationDepthSolver penetrationDepthSolver) + { + _cachedSeparatingAxis = new Vector3(0, 0, 1); + + _penetrationDepthSolver = penetrationDepthSolver; + _simplexSolver = simplexSolver; + _minkowskiA = objectA; + _minkowskiB = objectB; + _ignoreMargin = false; + _lastUsedMethod = -1; + _catchDegeneracies = 1; + } + + public void setMinkowskiA(ConvexShape minkA) + { + _minkowskiA = minkA; + } + + public void setMinkowskiB(ConvexShape minkB) + { + _minkowskiB = minkB; + } + public void setCachedSeperatingAxis(Vector3 seperatingAxis) + { + _cachedSeparatingAxis = seperatingAxis; + } + + public void setPenetrationDepthSolver(IConvexPenetrationDepthSolver penetrationDepthSolver) + { + this._penetrationDepthSolver = penetrationDepthSolver; + } + + public void setIgnoreMargin(bool ignoreMargin) + { + this._ignoreMargin = ignoreMargin; + } + + public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw) + { + float distance = 0; + + Vector3 normalInB = new Vector3(); + Vector3 pointOnA = new Vector3(), pointOnB = new Vector3(); + + Matrix localTransA = input.TransformA; + Matrix localTransB = input.TransformB; + + Vector3 positionOffset = (localTransA.Translation + localTransB.Translation) * 0.5f; + localTransA.Translation -= positionOffset; + localTransB.Translation -= positionOffset; + + float marginA = _minkowskiA.Margin; + float marginB = _minkowskiB.Margin; + + _numGjkChecks++; + + if (_ignoreMargin) + { + marginA = 0; + marginB = 0; + } + + _currentIteration = 0; + + int gjkMaxIter = 1000; + _cachedSeparatingAxis = new Vector3(0, 1, 0); + + bool isValid = false; + bool checkSimplex = false; + bool checkPenetration = true; + _degenerateSimplex = 0; + + _lastUsedMethod = -1; + + { + float squaredDistance = MathHelper.Infinity; + float delta = 0; + + float margin = marginA + marginB; + + _simplexSolver.Reset(); + + while (true) + { + Matrix transABasis = input.TransformA; + transABasis.Translation = Vector3.Zero; + + Matrix transBBasis = input.TransformB; + transBBasis.Translation = Vector3.Zero; + + Vector3 seperatingAxisInA = Vector3.TransformNormal(-_cachedSeparatingAxis, transABasis); + Vector3 seperatingAxisInB = Vector3.TransformNormal(_cachedSeparatingAxis, transBBasis); + + Vector3 pInA = _minkowskiA.LocalGetSupportingVertexWithoutMargin(seperatingAxisInA); + Vector3 qInB = _minkowskiB.LocalGetSupportingVertexWithoutMargin(seperatingAxisInB); + Vector3 pWorld = MathHelper.MatrixToVector(localTransA, pInA); + Vector3 qWorld = MathHelper.MatrixToVector(localTransB, qInB); + + Vector3 w = pWorld - qWorld; + delta = Vector3.Dot(_cachedSeparatingAxis, w); + + if ((delta > 0.0) && (delta * delta > squaredDistance * input.MaximumDistanceSquared)) + { + checkPenetration = false; + break; + } + + if (_simplexSolver.InSimplex(w)) + { + _degenerateSimplex = 1; + checkSimplex = true; + break; + } + + float f0 = squaredDistance - delta; + float f1 = squaredDistance * RelativeError2; + + if (f0 <= f1) + { + if (f0 <= 0.0f) + { + _degenerateSimplex = 2; + } + + checkSimplex = true; + break; + } + + _simplexSolver.AddVertex(w, pWorld, qWorld); + + if (!_simplexSolver.Closest(out _cachedSeparatingAxis)) + { + _degenerateSimplex = 3; + checkSimplex = true; + break; + } + + float previouseSquaredDistance = squaredDistance; + squaredDistance = _cachedSeparatingAxis.LengthSquared(); + + if (previouseSquaredDistance - squaredDistance <= MathHelper.Epsilon * previouseSquaredDistance) + { + _simplexSolver.BackupClosest(out _cachedSeparatingAxis); + checkSimplex = true; + break; + } + + if (_currentIteration++ > gjkMaxIter) + { +#if DEBUG + Console.WriteLine("GjkPairDetector maxIter exceeded: {0}", _currentIteration); + Console.WriteLine("sepAxis=({0},{1},{2}), squaredDistance = {3}, shapeTypeA={4}, shapeTypeB={5}", + _cachedSeparatingAxis.X, + _cachedSeparatingAxis.Y, + _cachedSeparatingAxis.Z, + squaredDistance, + _minkowskiA.ShapeType, + _minkowskiB.ShapeType + ); +#endif + break; + } + + bool check = (!_simplexSolver.FullSimplex); + + if (!check) + { + _simplexSolver.BackupClosest(out _cachedSeparatingAxis); + break; + } + } + + if (checkSimplex) + { + _simplexSolver.ComputePoints(out pointOnA, out pointOnB); + normalInB = pointOnA - pointOnB; + float lenSqr = _cachedSeparatingAxis.LengthSquared(); + + if (lenSqr < 0.0001f) + { + _degenerateSimplex = 5; + } + + if (lenSqr > MathHelper.Epsilon * MathHelper.Epsilon) + { + float rlen = 1.0f / (float)Math.Sqrt((float)lenSqr); + normalInB *= rlen; + float s = (float)Math.Sqrt((float)squaredDistance); + + BulletDebug.Assert(s > 0); + pointOnA -= _cachedSeparatingAxis * (marginA / s); + pointOnB += _cachedSeparatingAxis * (marginB / s); + distance = ((1 / rlen) - margin); + + isValid = true; + + _lastUsedMethod = 1; + } + else + { + _lastUsedMethod = 2; + } + } + + bool catchDegeneratePenetrationCase = + (_catchDegeneracies != 0 && _penetrationDepthSolver != null && _degenerateSimplex != 0 && ((distance + margin) < 0.01f)); + + if (checkPenetration && (!isValid || catchDegeneratePenetrationCase)) + { +#warning Check this + if (_penetrationDepthSolver != null) + { + Vector3 tmpPointOnA, tmpPointOnB; + + _numDeepPenetrationChecks++; + + bool isValid2 = _penetrationDepthSolver.CalculatePenetrationDepth( + _simplexSolver, _minkowskiA, _minkowskiB, localTransA, localTransB, + _cachedSeparatingAxis, out tmpPointOnA, out tmpPointOnB, + debugDraw + ); + + if (isValid2) + { + Vector3 tmpNormalInB = tmpPointOnB - tmpPointOnA; + float lengSqr = tmpNormalInB.LengthSquared(); + + if (lengSqr > (MathHelper.Epsilon * MathHelper.Epsilon)) + { + tmpNormalInB /= (float)Math.Sqrt((float)lengSqr); + float distance2 = -(tmpPointOnA - tmpPointOnB).Length(); + + if (!isValid || (distance2 < distance)) + { + distance = distance2; + pointOnA = tmpPointOnA; + pointOnB = tmpPointOnB; + normalInB = tmpNormalInB; + isValid = true; + _lastUsedMethod = 3; + } + else + { + + } + } + else + { + _lastUsedMethod = 4; + } + } + else + { + _lastUsedMethod = 5; + } + } + } + + if (isValid) + { + output.AddContactPoint(normalInB, pointOnB + positionOffset, distance); + } + } + } + } } \ No newline at end of file diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/IConvexPenetrationDepthSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/IConvexPenetrationDepthSolver.cs index 59fd486..08206c8 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/IConvexPenetrationDepthSolver.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/IConvexPenetrationDepthSolver.cs @@ -1,42 +1,42 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// - /// IConvexPenetrationDepthSolver provides an interface for penetration depth calculation. - /// - public interface IConvexPenetrationDepthSolver - { - bool CalculatePenetrationDepth( - ISimplexSolver simplexSolver, - ConvexShape convexA, ConvexShape convexB, - Matrix transformA, Matrix transformB, - Vector3 vector, out Vector3 ptrA, out Vector3 ptrB, - IDebugDraw debugDraw//, StackAlloc stackAlloc - ); - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// + /// IConvexPenetrationDepthSolver provides an interface for penetration depth calculation. + /// + public interface IConvexPenetrationDepthSolver + { + bool CalculatePenetrationDepth( + ISimplexSolver simplexSolver, + ConvexShape convexA, ConvexShape convexB, + Matrix transformA, Matrix transformB, + Vector3 vector, out Vector3 ptrA, out Vector3 ptrB, + IDebugDraw debugDraw//, StackAlloc stackAlloc + ); + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs index 71b1eaa..f1b1807 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs @@ -1,48 +1,48 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// ISimplexSolver can incrementally calculate distance between origin and up to 4 vertices - /// Used by GJK or Linear Casting. Can be implemented by the Johnson-algorithm or alternative approaches based on - /// voronoi regions or barycentric coordinates - public interface ISimplexSolver - { - void Reset(); - void AddVertex(Vector3 w, Vector3 p, Vector3 q); - bool Closest(out Vector3 v); - - int GetSimplex(out Vector3[] pBuf, out Vector3[] qBuf, out Vector3[] yBuf); - bool InSimplex(Vector3 w); - void BackupClosest(out Vector3 v); - void ComputePoints(out Vector3 pA, out Vector3 pB); - - int NumVertices { get;} - bool EmptySimplex { get;} - float MaxVertex { get;} - bool FullSimplex { get;} - } +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// ISimplexSolver can incrementally calculate distance between origin and up to 4 vertices + /// Used by GJK or Linear Casting. Can be implemented by the Johnson-algorithm or alternative approaches based on + /// voronoi regions or barycentric coordinates + public interface ISimplexSolver + { + void Reset(); + void AddVertex(Vector3 w, Vector3 p, Vector3 q); + bool Closest(out Vector3 v); + + int GetSimplex(out Vector3[] pBuf, out Vector3[] qBuf, out Vector3[] yBuf); + bool InSimplex(Vector3 w); + void BackupClosest(out Vector3 v); + void ComputePoints(out Vector3 pA, out Vector3 pB); + + int NumVertices { get;} + bool EmptySimplex { get;} + float MaxVertex { get;} + bool FullSimplex { get;} + } } \ No newline at end of file diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ManifoldPoint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ManifoldPoint.cs index 751a9df..f5bcedc 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ManifoldPoint.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ManifoldPoint.cs @@ -1,78 +1,78 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public class ManifoldPoint - { - private Vector3 _localPointA; - private Vector3 _localPointB; - private Vector3 _positionWorldOnB; - private Vector3 _positionWorldOnA; - private Vector3 _normalWorldOnB; - - private float _distance; - private float _combinedFriction; - private float _combinedRestitution; - - private object _userPersistentData; - - private int _lifeTime;//lifetime of the contactpoint in frames - - public ManifoldPoint() - : this(new Vector3(), new Vector3(), new Vector3(), 0f) - { - } - - public ManifoldPoint(Vector3 pointA, Vector3 pointB, - Vector3 normal, - float distance) - { - _localPointA = pointA; - _localPointB = pointB; - _normalWorldOnB = normal; - _distance = distance; - _positionWorldOnA = new Vector3(); - _positionWorldOnB = new Vector3(); - } - - public float Distance { get { return _distance; } set { _distance = value; } } - public int LifeTime { get { return _lifeTime; } set { _lifeTime = value; } } - - public Vector3 PositionWorldOnA { get { return _positionWorldOnA; } set { _positionWorldOnA = value; } } - public Vector3 PositionWorldOnB { get { return _positionWorldOnB; } set { _positionWorldOnB = value; } } - - public Vector3 LocalPointA { get { return _localPointA; } set { _localPointA = value; } } - public Vector3 LocalPointB { get { return _localPointB; } set { _localPointB = value; } } - - public Vector3 NormalWorldOnB { get { return _normalWorldOnB; } } - - public float CombinedFriction { get { return _combinedFriction; } set { _combinedFriction = value; } } - public float CombinedRestitution { get { return _combinedRestitution; } set { _combinedRestitution = value; } } - - public object UserPersistentData { get { return _userPersistentData; } set { _userPersistentData = value; } } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public class ManifoldPoint + { + private Vector3 _localPointA; + private Vector3 _localPointB; + private Vector3 _positionWorldOnB; + private Vector3 _positionWorldOnA; + private Vector3 _normalWorldOnB; + + private float _distance; + private float _combinedFriction; + private float _combinedRestitution; + + private object _userPersistentData; + + private int _lifeTime;//lifetime of the contactpoint in frames + + public ManifoldPoint() + : this(new Vector3(), new Vector3(), new Vector3(), 0f) + { + } + + public ManifoldPoint(Vector3 pointA, Vector3 pointB, + Vector3 normal, + float distance) + { + _localPointA = pointA; + _localPointB = pointB; + _normalWorldOnB = normal; + _distance = distance; + _positionWorldOnA = new Vector3(); + _positionWorldOnB = new Vector3(); + } + + public float Distance { get { return _distance; } set { _distance = value; } } + public int LifeTime { get { return _lifeTime; } set { _lifeTime = value; } } + + public Vector3 PositionWorldOnA { get { return _positionWorldOnA; } set { _positionWorldOnA = value; } } + public Vector3 PositionWorldOnB { get { return _positionWorldOnB; } set { _positionWorldOnB = value; } } + + public Vector3 LocalPointA { get { return _localPointA; } set { _localPointA = value; } } + public Vector3 LocalPointB { get { return _localPointB; } set { _localPointB = value; } } + + public Vector3 NormalWorldOnB { get { return _normalWorldOnB; } } + + public float CombinedFriction { get { return _combinedFriction; } set { _combinedFriction = value; } } + public float CombinedRestitution { get { return _combinedRestitution; } set { _combinedRestitution = value; } } + + public object UserPersistentData { get { return _userPersistentData; } set { _userPersistentData = value; } } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs index 81b05b8..7363e28 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs @@ -1,246 +1,246 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// - /// MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation. - /// Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points. - /// - public class MinkowskiPenetrationDepthSolver : IConvexPenetrationDepthSolver - { - private const int UnitSpherePointsCount = 42; - - private static Vector3[] penetrationDirections = - { - new Vector3(0.000000f , -0.000000f,-1.000000f), - new Vector3(0.723608f , -0.525725f,-0.447219f), - new Vector3(-0.276388f , -0.850649f,-0.447219f), - new Vector3(-0.894426f , -0.000000f,-0.447216f), - new Vector3(-0.276388f , 0.850649f,-0.447220f), - new Vector3(0.723608f , 0.525725f,-0.447219f), - new Vector3(0.276388f , -0.850649f,0.447220f), - new Vector3(-0.723608f , -0.525725f,0.447219f), - new Vector3(-0.723608f , 0.525725f,0.447219f), - new Vector3(0.276388f , 0.850649f,0.447219f), - new Vector3(0.894426f , 0.000000f,0.447216f), - new Vector3(-0.000000f , 0.000000f,1.000000f), - new Vector3(0.425323f , -0.309011f,-0.850654f), - new Vector3(-0.162456f , -0.499995f,-0.850654f), - new Vector3(0.262869f , -0.809012f,-0.525738f), - new Vector3(0.425323f , 0.309011f,-0.850654f), - new Vector3(0.850648f , -0.000000f,-0.525736f), - new Vector3(-0.525730f , -0.000000f,-0.850652f), - new Vector3(-0.688190f , -0.499997f,-0.525736f), - new Vector3(-0.162456f , 0.499995f,-0.850654f), - new Vector3(-0.688190f , 0.499997f,-0.525736f), - new Vector3(0.262869f , 0.809012f,-0.525738f), - new Vector3(0.951058f , 0.309013f,0.000000f), - new Vector3(0.951058f , -0.309013f,0.000000f), - new Vector3(0.587786f , -0.809017f,0.000000f), - new Vector3(0.000000f , -1.000000f,0.000000f), - new Vector3(-0.587786f , -0.809017f,0.000000f), - new Vector3(-0.951058f , -0.309013f,-0.000000f), - new Vector3(-0.951058f , 0.309013f,-0.000000f), - new Vector3(-0.587786f , 0.809017f,-0.000000f), - new Vector3(-0.000000f , 1.000000f,-0.000000f), - new Vector3(0.587786f , 0.809017f,-0.000000f), - new Vector3(0.688190f , -0.499997f,0.525736f), - new Vector3(-0.262869f , -0.809012f,0.525738f), - new Vector3(-0.850648f , 0.000000f,0.525736f), - new Vector3(-0.262869f , 0.809012f,0.525738f), - new Vector3(0.688190f , 0.499997f,0.525736f), - new Vector3(0.525730f , 0.000000f,0.850652f), - new Vector3(0.162456f , -0.499995f,0.850654f), - new Vector3(-0.425323f , -0.309011f,0.850654f), - new Vector3(-0.425323f , 0.309011f,0.850654f), - new Vector3(0.162456f , 0.499995f,0.850654f) - }; - - private class IntermediateResult : DiscreteCollisionDetectorInterface.Result - { - private Vector3 _normalOnBInWorld; - private Vector3 _pointInWorld; - private float _depth; - private bool _hasResult; - - public IntermediateResult() - { - _hasResult = false; - } - - public bool HasResult { get { return _hasResult; } } - public float Depth { get { return _depth; } } - public Vector3 PointInWorld { get { return _pointInWorld; } } - - public override void SetShapeIdentifiers(int partId0, int index0, int partId1, int index1) - { - } - - public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) - { - _normalOnBInWorld = normalOnBInWorld; - _pointInWorld = pointInWorld; - _depth = depth; - _hasResult = true; - } - } - - #region IConvexPenetrationDepthSolver Members - public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver, - ConvexShape convexA, ConvexShape convexB, - Matrix transformA, Matrix transformB, - Vector3 v, out Vector3 pa, out Vector3 pb, IDebugDraw debugDraw) - { - pa = new Vector3(); - pb = new Vector3(); - //just take fixed number of orientation, and sample the penetration depth in that direction - float minProj = 1e30f; - Vector3 minNorm = new Vector3(); - Vector3 minA = new Vector3(), minB = new Vector3(); - Vector3 seperatingAxisInA, seperatingAxisInB; - Vector3 pInA, qInB, pWorld, qWorld, w; - - Vector3[] supportVerticesABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; - Vector3[] supportVerticesBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; - Vector3[] seperatingAxisInABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; - Vector3[] seperatingAxisInBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; - - int numSampleDirections = UnitSpherePointsCount; - - for (int i = 0; i < numSampleDirections; i++) - { - Vector3 norm = penetrationDirections[i]; - seperatingAxisInABatch[i] = Vector3.TransformNormal((-norm), transformA); - seperatingAxisInBBatch[i] = Vector3.TransformNormal(norm, transformB); - } - - { - int numPDA = convexA.PreferredPenetrationDirectionsCount; - if (numPDA != 0) - { - for (int i = 0; i < numPDA; i++) - { - Vector3 norm; - convexA.GetPreferredPenetrationDirection(i, out norm); - norm = Vector3.TransformNormal(norm, transformA); - penetrationDirections[numSampleDirections] = norm; - seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA); - seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB); - numSampleDirections++; - } - } - } - - { - int numPDB = convexB.PreferredPenetrationDirectionsCount; - if (numPDB != 0) - { - for (int i = 0; i < numPDB; i++) - { - Vector3 norm; - convexB.GetPreferredPenetrationDirection(i, out norm); - norm = Vector3.TransformNormal(norm, transformB); - penetrationDirections[numSampleDirections] = norm; - seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA); - seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB); - numSampleDirections++; - } - } - } - - convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch); //, numSampleDirections); - convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch); //, numSampleDirections); - - for (int i = 0; i < numSampleDirections; i++) - { - Vector3 norm = penetrationDirections[i]; - seperatingAxisInA = seperatingAxisInABatch[i]; - seperatingAxisInB = seperatingAxisInBBatch[i]; - - pInA = supportVerticesABatch[i]; - qInB = supportVerticesBBatch[i]; - - pWorld = MathHelper.MatrixToVector(transformA, pInA); - qWorld = MathHelper.MatrixToVector(transformB, qInB); - w = qWorld - pWorld; - float delta = Vector3.Dot(norm, w); - //find smallest delta - if (delta < minProj) - { - minProj = delta; - minNorm = norm; - minA = pWorld; - minB = qWorld; - } - } - - //add the margins - minA += minNorm * convexA.Margin; - minB -= minNorm * convexB.Margin; - //no penetration - if (minProj < 0) - return false; - - minProj += (convexA.Margin + convexB.Margin); - - GjkPairDetector gjkdet = new GjkPairDetector(convexA, convexB, simplexSolver, null); - - float offsetDist = minProj; - Vector3 offset = minNorm * offsetDist; - - GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); - - Vector3 newOrg = transformA.Translation + offset; - - Matrix displacedTrans = transformA; - displacedTrans.Translation = newOrg; - - input.TransformA = displacedTrans; - input.TransformB = transformB; - input.MaximumDistanceSquared = 1e30f;//minProj; - - IntermediateResult res = new IntermediateResult(); - gjkdet.GetClosestPoints(input, res, debugDraw); - - float correctedMinNorm = minProj - res.Depth; - - //the penetration depth is over-estimated, relax it - float penetration_relaxation = 1; - minNorm *= penetration_relaxation; - - if (res.HasResult) - { - - pa = res.PointInWorld - minNorm * correctedMinNorm; - pb = res.PointInWorld; - } - - return res.HasResult; - } - #endregion - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// + /// MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation. + /// Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points. + /// + public class MinkowskiPenetrationDepthSolver : IConvexPenetrationDepthSolver + { + private const int UnitSpherePointsCount = 42; + + private static Vector3[] penetrationDirections = + { + new Vector3(0.000000f , -0.000000f,-1.000000f), + new Vector3(0.723608f , -0.525725f,-0.447219f), + new Vector3(-0.276388f , -0.850649f,-0.447219f), + new Vector3(-0.894426f , -0.000000f,-0.447216f), + new Vector3(-0.276388f , 0.850649f,-0.447220f), + new Vector3(0.723608f , 0.525725f,-0.447219f), + new Vector3(0.276388f , -0.850649f,0.447220f), + new Vector3(-0.723608f , -0.525725f,0.447219f), + new Vector3(-0.723608f , 0.525725f,0.447219f), + new Vector3(0.276388f , 0.850649f,0.447219f), + new Vector3(0.894426f , 0.000000f,0.447216f), + new Vector3(-0.000000f , 0.000000f,1.000000f), + new Vector3(0.425323f , -0.309011f,-0.850654f), + new Vector3(-0.162456f , -0.499995f,-0.850654f), + new Vector3(0.262869f , -0.809012f,-0.525738f), + new Vector3(0.425323f , 0.309011f,-0.850654f), + new Vector3(0.850648f , -0.000000f,-0.525736f), + new Vector3(-0.525730f , -0.000000f,-0.850652f), + new Vector3(-0.688190f , -0.499997f,-0.525736f), + new Vector3(-0.162456f , 0.499995f,-0.850654f), + new Vector3(-0.688190f , 0.499997f,-0.525736f), + new Vector3(0.262869f , 0.809012f,-0.525738f), + new Vector3(0.951058f , 0.309013f,0.000000f), + new Vector3(0.951058f , -0.309013f,0.000000f), + new Vector3(0.587786f , -0.809017f,0.000000f), + new Vector3(0.000000f , -1.000000f,0.000000f), + new Vector3(-0.587786f , -0.809017f,0.000000f), + new Vector3(-0.951058f , -0.309013f,-0.000000f), + new Vector3(-0.951058f , 0.309013f,-0.000000f), + new Vector3(-0.587786f , 0.809017f,-0.000000f), + new Vector3(-0.000000f , 1.000000f,-0.000000f), + new Vector3(0.587786f , 0.809017f,-0.000000f), + new Vector3(0.688190f , -0.499997f,0.525736f), + new Vector3(-0.262869f , -0.809012f,0.525738f), + new Vector3(-0.850648f , 0.000000f,0.525736f), + new Vector3(-0.262869f , 0.809012f,0.525738f), + new Vector3(0.688190f , 0.499997f,0.525736f), + new Vector3(0.525730f , 0.000000f,0.850652f), + new Vector3(0.162456f , -0.499995f,0.850654f), + new Vector3(-0.425323f , -0.309011f,0.850654f), + new Vector3(-0.425323f , 0.309011f,0.850654f), + new Vector3(0.162456f , 0.499995f,0.850654f) + }; + + private class IntermediateResult : DiscreteCollisionDetectorInterface.Result + { + private Vector3 _normalOnBInWorld; + private Vector3 _pointInWorld; + private float _depth; + private bool _hasResult; + + public IntermediateResult() + { + _hasResult = false; + } + + public bool HasResult { get { return _hasResult; } } + public float Depth { get { return _depth; } } + public Vector3 PointInWorld { get { return _pointInWorld; } } + + public override void SetShapeIdentifiers(int partId0, int index0, int partId1, int index1) + { + } + + public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) + { + _normalOnBInWorld = normalOnBInWorld; + _pointInWorld = pointInWorld; + _depth = depth; + _hasResult = true; + } + } + + #region IConvexPenetrationDepthSolver Members + public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver, + ConvexShape convexA, ConvexShape convexB, + Matrix transformA, Matrix transformB, + Vector3 v, out Vector3 pa, out Vector3 pb, IDebugDraw debugDraw) + { + pa = new Vector3(); + pb = new Vector3(); + //just take fixed number of orientation, and sample the penetration depth in that direction + float minProj = 1e30f; + Vector3 minNorm = new Vector3(); + Vector3 minA = new Vector3(), minB = new Vector3(); + Vector3 seperatingAxisInA, seperatingAxisInB; + Vector3 pInA, qInB, pWorld, qWorld, w; + + Vector3[] supportVerticesABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; + Vector3[] supportVerticesBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; + Vector3[] seperatingAxisInABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; + Vector3[] seperatingAxisInBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; + + int numSampleDirections = UnitSpherePointsCount; + + for (int i = 0; i < numSampleDirections; i++) + { + Vector3 norm = penetrationDirections[i]; + seperatingAxisInABatch[i] = Vector3.TransformNormal((-norm), transformA); + seperatingAxisInBBatch[i] = Vector3.TransformNormal(norm, transformB); + } + + { + int numPDA = convexA.PreferredPenetrationDirectionsCount; + if (numPDA != 0) + { + for (int i = 0; i < numPDA; i++) + { + Vector3 norm; + convexA.GetPreferredPenetrationDirection(i, out norm); + norm = Vector3.TransformNormal(norm, transformA); + penetrationDirections[numSampleDirections] = norm; + seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA); + seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB); + numSampleDirections++; + } + } + } + + { + int numPDB = convexB.PreferredPenetrationDirectionsCount; + if (numPDB != 0) + { + for (int i = 0; i < numPDB; i++) + { + Vector3 norm; + convexB.GetPreferredPenetrationDirection(i, out norm); + norm = Vector3.TransformNormal(norm, transformB); + penetrationDirections[numSampleDirections] = norm; + seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA); + seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB); + numSampleDirections++; + } + } + } + + convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch); //, numSampleDirections); + convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch); //, numSampleDirections); + + for (int i = 0; i < numSampleDirections; i++) + { + Vector3 norm = penetrationDirections[i]; + seperatingAxisInA = seperatingAxisInABatch[i]; + seperatingAxisInB = seperatingAxisInBBatch[i]; + + pInA = supportVerticesABatch[i]; + qInB = supportVerticesBBatch[i]; + + pWorld = MathHelper.MatrixToVector(transformA, pInA); + qWorld = MathHelper.MatrixToVector(transformB, qInB); + w = qWorld - pWorld; + float delta = Vector3.Dot(norm, w); + //find smallest delta + if (delta < minProj) + { + minProj = delta; + minNorm = norm; + minA = pWorld; + minB = qWorld; + } + } + + //add the margins + minA += minNorm * convexA.Margin; + minB -= minNorm * convexB.Margin; + //no penetration + if (minProj < 0) + return false; + + minProj += (convexA.Margin + convexB.Margin); + + GjkPairDetector gjkdet = new GjkPairDetector(convexA, convexB, simplexSolver, null); + + float offsetDist = minProj; + Vector3 offset = minNorm * offsetDist; + + GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); + + Vector3 newOrg = transformA.Translation + offset; + + Matrix displacedTrans = transformA; + displacedTrans.Translation = newOrg; + + input.TransformA = displacedTrans; + input.TransformB = transformB; + input.MaximumDistanceSquared = 1e30f;//minProj; + + IntermediateResult res = new IntermediateResult(); + gjkdet.GetClosestPoints(input, res, debugDraw); + + float correctedMinNorm = minProj - res.Depth; + + //the penetration depth is over-estimated, relax it + float penetration_relaxation = 1; + minNorm *= penetration_relaxation; + + if (res.HasResult) + { + + pa = res.PointInWorld - minNorm * correctedMinNorm; + pb = res.PointInWorld; + } + + return res.HasResult; + } + #endregion + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs index 47cfe9a..04a533d 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs @@ -1,272 +1,272 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public delegate bool ContactDestroyedCallback(object userPersistentData); - - public class PersistentManifold - { - private static ContactDestroyedCallback _contactDestroyedCallback = null; - private static float _contactBreakingThreshold = 0.02f; - - private ManifoldPoint[] _pointCache = new ManifoldPoint[4]; - - // this two body pointers can point to the physics rigidbody class. - // object will allow any rigidbody class - private object _bodyA; - private object _bodyB; - private int _cachedPoints; - - public PersistentManifold(object bodyA, object bodyB) - { - _bodyA = bodyA; - _bodyB = bodyB; - _cachedPoints = 0; - } - - public object BodyA { get { return _bodyA; } } - public object BodyB { get { return _bodyB; } } - - public int ContactsCount { get { return _cachedPoints; } } - - public static ContactDestroyedCallback ContactDestroyedCallback { get { return _contactDestroyedCallback; } set { _contactDestroyedCallback = value; } } - public static float ContactBreakingThreshold { get { return _contactBreakingThreshold; } } - - public void SetBodies(object bodyA, object bodyB) - { - _bodyA = bodyA; - _bodyB = bodyB; - } - - public ManifoldPoint GetContactPoint(int index) - { - if (index >= _cachedPoints) - throw new ArgumentOutOfRangeException("index", "index must be smaller than cachedPoints"); - - return _pointCache[index]; - } - - public int GetCacheEntry(ManifoldPoint newPoint) - { - float shortestDist = ContactBreakingThreshold * ContactBreakingThreshold; - int size = ContactsCount; - int nearestPoint = -1; - for (int i = 0; i < size; i++) - { - ManifoldPoint mp = _pointCache[i]; - - Vector3 diffA = mp.LocalPointA - newPoint.LocalPointA; - float distToManiPoint = Vector3.Dot(diffA, diffA); - if (distToManiPoint < shortestDist) - { - shortestDist = distToManiPoint; - nearestPoint = i; - } - } - return nearestPoint; - } - - public void AddManifoldPoint(ManifoldPoint newPoint) - { - if (!ValidContactDistance(newPoint)) - throw new BulletException(); - - int insertIndex = ContactsCount; - if (insertIndex == 4) - { - //sort cache so best points come first, based on area - insertIndex = SortCachedPoints(newPoint); - } - else - { - _cachedPoints++; - } - ReplaceContactPoint(newPoint, insertIndex); - } - - public void RemoveContactPoint(int index) - { - ClearUserCache(_pointCache[index]); - - int lastUsedIndex = ContactsCount - 1; - _pointCache[index] = _pointCache[lastUsedIndex]; - //get rid of duplicated userPersistentData pointer - _pointCache[lastUsedIndex].UserPersistentData = null; - _cachedPoints--; - } - - public void ReplaceContactPoint(ManifoldPoint newPoint, int insertIndex) - { - BulletDebug.Assert(ValidContactDistance(newPoint)); - - if (_pointCache[insertIndex] != null) - { - int lifeTime = _pointCache[insertIndex].LifeTime; - BulletDebug.Assert(lifeTime >= 0); - object cache = _pointCache[insertIndex].UserPersistentData; - - _pointCache[insertIndex] = newPoint; - - _pointCache[insertIndex].UserPersistentData = cache; - _pointCache[insertIndex].LifeTime = lifeTime; - } - else - { - _pointCache[insertIndex] = newPoint; - } - - //ClearUserCache(_pointCache[insertIndex]); - //_pointCache[insertIndex] = newPoint; - } - - public bool ValidContactDistance(ManifoldPoint pt) - { - return pt.Distance <= ContactBreakingThreshold; - } - - // calculated new worldspace coordinates and depth, and reject points that exceed the collision margin - public void RefreshContactPoints(Matrix trA, Matrix trB) - { - // first refresh worldspace positions and distance - for (int i = ContactsCount - 1; i >= 0; i--) - { - ManifoldPoint manifoldPoint = _pointCache[i]; - manifoldPoint.PositionWorldOnA = MathHelper.MatrixToVector(trA,manifoldPoint.LocalPointA); - manifoldPoint.PositionWorldOnB = MathHelper.MatrixToVector(trB, manifoldPoint.LocalPointB); - manifoldPoint.Distance = Vector3.Dot(manifoldPoint.PositionWorldOnA - manifoldPoint.PositionWorldOnB, manifoldPoint.NormalWorldOnB); - manifoldPoint.LifeTime++; - } - - // then - float distance2d; - Vector3 projectedDifference, projectedPoint; - for (int i = ContactsCount - 1; i >= 0; i--) - { - - ManifoldPoint manifoldPoint = _pointCache[i]; - //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction) - if (!ValidContactDistance(manifoldPoint)) - { - RemoveContactPoint(i); - } - else - { - //contact also becomes invalid when relative movement orthogonal to normal exceeds margin - projectedPoint = manifoldPoint.PositionWorldOnA - manifoldPoint.NormalWorldOnB * manifoldPoint.Distance; - projectedDifference = manifoldPoint.PositionWorldOnB - projectedPoint; - distance2d = Vector3.Dot(projectedDifference, projectedDifference); - if (distance2d > ContactBreakingThreshold * ContactBreakingThreshold) - { - RemoveContactPoint(i); - } - } - } - } - - public void ClearManifold() - { - for (int i = 0; i < _cachedPoints; i++) - { - ClearUserCache(_pointCache[i]); - } - _cachedPoints = 0; - } - - private void ClearUserCache(ManifoldPoint pt) - { - if (pt != null) - { - object oldPtr = pt.UserPersistentData; - - if (oldPtr != null) - { - if (pt.UserPersistentData != null && _contactDestroyedCallback != null) - { - _contactDestroyedCallback(pt.UserPersistentData); - pt.UserPersistentData = null; - } - } - } - } - - // sort cached points so most isolated points come first - private int SortCachedPoints(ManifoldPoint pt) - { - //calculate 4 possible cases areas, and take biggest area - //also need to keep 'deepest' - - int maxPenetrationIndex = -1; - float maxPenetration = pt.Distance; - for (int i = 0; i < 4; i++) - { - if (_pointCache[i].Distance < maxPenetration) - { - maxPenetrationIndex = i; - maxPenetration = _pointCache[i].Distance; - } - } - - float res0 = 0, res1 = 0, res2 = 0, res3 = 0; - if (maxPenetrationIndex != 0) - { - Vector3 a0 = pt.LocalPointA - _pointCache[1].LocalPointA; - Vector3 b0 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA; - Vector3 cross = Vector3.Cross(a0, b0); - res0 = cross.LengthSquared(); - } - if (maxPenetrationIndex != 1) - { - Vector3 a1 = pt.LocalPointA - _pointCache[0].LocalPointA; - Vector3 b1 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA; - Vector3 cross = Vector3.Cross(a1, b1); - res1 = cross.LengthSquared(); - } - - if (maxPenetrationIndex != 2) - { - Vector3 a2 = pt.LocalPointA - _pointCache[0].LocalPointA; - Vector3 b2 = _pointCache[3].LocalPointA - _pointCache[1].LocalPointA; - Vector3 cross = Vector3.Cross(a2, b2); - res2 = cross.LengthSquared(); - } - - if (maxPenetrationIndex != 3) - { - Vector3 a3 = pt.LocalPointA - _pointCache[0].LocalPointA; - Vector3 b3 = _pointCache[2].LocalPointA - _pointCache[1].LocalPointA; - Vector3 cross = Vector3.Cross(a3, b3); - res3 = cross.LengthSquared(); - } - - Vector4 maxvec = new Vector4(res0, res1, res2, res3); - int biggestarea = MathHelper.ClosestAxis(maxvec); - return biggestarea; - } - - private int FindContactPoint(ManifoldPoint unUsed, int numUnused, ManifoldPoint pt) { return 0; } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public delegate bool ContactDestroyedCallback(object userPersistentData); + + public class PersistentManifold + { + private static ContactDestroyedCallback _contactDestroyedCallback = null; + private static float _contactBreakingThreshold = 0.02f; + + private ManifoldPoint[] _pointCache = new ManifoldPoint[4]; + + // this two body pointers can point to the physics rigidbody class. + // object will allow any rigidbody class + private object _bodyA; + private object _bodyB; + private int _cachedPoints; + + public PersistentManifold(object bodyA, object bodyB) + { + _bodyA = bodyA; + _bodyB = bodyB; + _cachedPoints = 0; + } + + public object BodyA { get { return _bodyA; } } + public object BodyB { get { return _bodyB; } } + + public int ContactsCount { get { return _cachedPoints; } } + + public static ContactDestroyedCallback ContactDestroyedCallback { get { return _contactDestroyedCallback; } set { _contactDestroyedCallback = value; } } + public static float ContactBreakingThreshold { get { return _contactBreakingThreshold; } } + + public void SetBodies(object bodyA, object bodyB) + { + _bodyA = bodyA; + _bodyB = bodyB; + } + + public ManifoldPoint GetContactPoint(int index) + { + if (index >= _cachedPoints) + throw new ArgumentOutOfRangeException("index", "index must be smaller than cachedPoints"); + + return _pointCache[index]; + } + + public int GetCacheEntry(ManifoldPoint newPoint) + { + float shortestDist = ContactBreakingThreshold * ContactBreakingThreshold; + int size = ContactsCount; + int nearestPoint = -1; + for (int i = 0; i < size; i++) + { + ManifoldPoint mp = _pointCache[i]; + + Vector3 diffA = mp.LocalPointA - newPoint.LocalPointA; + float distToManiPoint = Vector3.Dot(diffA, diffA); + if (distToManiPoint < shortestDist) + { + shortestDist = distToManiPoint; + nearestPoint = i; + } + } + return nearestPoint; + } + + public void AddManifoldPoint(ManifoldPoint newPoint) + { + if (!ValidContactDistance(newPoint)) + throw new BulletException(); + + int insertIndex = ContactsCount; + if (insertIndex == 4) + { + //sort cache so best points come first, based on area + insertIndex = SortCachedPoints(newPoint); + } + else + { + _cachedPoints++; + } + ReplaceContactPoint(newPoint, insertIndex); + } + + public void RemoveContactPoint(int index) + { + ClearUserCache(_pointCache[index]); + + int lastUsedIndex = ContactsCount - 1; + _pointCache[index] = _pointCache[lastUsedIndex]; + //get rid of duplicated userPersistentData pointer + _pointCache[lastUsedIndex].UserPersistentData = null; + _cachedPoints--; + } + + public void ReplaceContactPoint(ManifoldPoint newPoint, int insertIndex) + { + BulletDebug.Assert(ValidContactDistance(newPoint)); + + if (_pointCache[insertIndex] != null) + { + int lifeTime = _pointCache[insertIndex].LifeTime; + BulletDebug.Assert(lifeTime >= 0); + object cache = _pointCache[insertIndex].UserPersistentData; + + _pointCache[insertIndex] = newPoint; + + _pointCache[insertIndex].UserPersistentData = cache; + _pointCache[insertIndex].LifeTime = lifeTime; + } + else + { + _pointCache[insertIndex] = newPoint; + } + + //ClearUserCache(_pointCache[insertIndex]); + //_pointCache[insertIndex] = newPoint; + } + + public bool ValidContactDistance(ManifoldPoint pt) + { + return pt.Distance <= ContactBreakingThreshold; + } + + // calculated new worldspace coordinates and depth, and reject points that exceed the collision margin + public void RefreshContactPoints(Matrix trA, Matrix trB) + { + // first refresh worldspace positions and distance + for (int i = ContactsCount - 1; i >= 0; i--) + { + ManifoldPoint manifoldPoint = _pointCache[i]; + manifoldPoint.PositionWorldOnA = MathHelper.MatrixToVector(trA,manifoldPoint.LocalPointA); + manifoldPoint.PositionWorldOnB = MathHelper.MatrixToVector(trB, manifoldPoint.LocalPointB); + manifoldPoint.Distance = Vector3.Dot(manifoldPoint.PositionWorldOnA - manifoldPoint.PositionWorldOnB, manifoldPoint.NormalWorldOnB); + manifoldPoint.LifeTime++; + } + + // then + float distance2d; + Vector3 projectedDifference, projectedPoint; + for (int i = ContactsCount - 1; i >= 0; i--) + { + + ManifoldPoint manifoldPoint = _pointCache[i]; + //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction) + if (!ValidContactDistance(manifoldPoint)) + { + RemoveContactPoint(i); + } + else + { + //contact also becomes invalid when relative movement orthogonal to normal exceeds margin + projectedPoint = manifoldPoint.PositionWorldOnA - manifoldPoint.NormalWorldOnB * manifoldPoint.Distance; + projectedDifference = manifoldPoint.PositionWorldOnB - projectedPoint; + distance2d = Vector3.Dot(projectedDifference, projectedDifference); + if (distance2d > ContactBreakingThreshold * ContactBreakingThreshold) + { + RemoveContactPoint(i); + } + } + } + } + + public void ClearManifold() + { + for (int i = 0; i < _cachedPoints; i++) + { + ClearUserCache(_pointCache[i]); + } + _cachedPoints = 0; + } + + private void ClearUserCache(ManifoldPoint pt) + { + if (pt != null) + { + object oldPtr = pt.UserPersistentData; + + if (oldPtr != null) + { + if (pt.UserPersistentData != null && _contactDestroyedCallback != null) + { + _contactDestroyedCallback(pt.UserPersistentData); + pt.UserPersistentData = null; + } + } + } + } + + // sort cached points so most isolated points come first + private int SortCachedPoints(ManifoldPoint pt) + { + //calculate 4 possible cases areas, and take biggest area + //also need to keep 'deepest' + + int maxPenetrationIndex = -1; + float maxPenetration = pt.Distance; + for (int i = 0; i < 4; i++) + { + if (_pointCache[i].Distance < maxPenetration) + { + maxPenetrationIndex = i; + maxPenetration = _pointCache[i].Distance; + } + } + + float res0 = 0, res1 = 0, res2 = 0, res3 = 0; + if (maxPenetrationIndex != 0) + { + Vector3 a0 = pt.LocalPointA - _pointCache[1].LocalPointA; + Vector3 b0 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA; + Vector3 cross = Vector3.Cross(a0, b0); + res0 = cross.LengthSquared(); + } + if (maxPenetrationIndex != 1) + { + Vector3 a1 = pt.LocalPointA - _pointCache[0].LocalPointA; + Vector3 b1 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA; + Vector3 cross = Vector3.Cross(a1, b1); + res1 = cross.LengthSquared(); + } + + if (maxPenetrationIndex != 2) + { + Vector3 a2 = pt.LocalPointA - _pointCache[0].LocalPointA; + Vector3 b2 = _pointCache[3].LocalPointA - _pointCache[1].LocalPointA; + Vector3 cross = Vector3.Cross(a2, b2); + res2 = cross.LengthSquared(); + } + + if (maxPenetrationIndex != 3) + { + Vector3 a3 = pt.LocalPointA - _pointCache[0].LocalPointA; + Vector3 b3 = _pointCache[2].LocalPointA - _pointCache[1].LocalPointA; + Vector3 cross = Vector3.Cross(a3, b3); + res3 = cross.LengthSquared(); + } + + Vector4 maxvec = new Vector4(res0, res1, res2, res3); + int biggestarea = MathHelper.ClosestAxis(maxvec); + return biggestarea; + } + + private int FindContactPoint(ManifoldPoint unUsed, int numUnused, ManifoldPoint pt) { return 0; } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs index 4e0b0a0..c70741e 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs @@ -1,64 +1,64 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public class PointCollector : DiscreteCollisionDetectorInterface.Result - { - private Vector3 _normalOnBInWorld; - private Vector3 _pointInWorld; - private float _distance; //negative means penetration - private bool _hasResult; - - public PointCollector() - { - _distance = 1e30f; - _hasResult = false; - } - - public Vector3 NormalOnBInWorld { get { return _normalOnBInWorld; } } - public Vector3 PointInWorld { get { return _pointInWorld; } } - public float Distance { get { return _distance; } } - public bool HasResult { get { return _hasResult; } } - - public override void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB) - { - //?? - } - - public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) - { - if (depth < _distance) - { - _hasResult = true; - _normalOnBInWorld = normalOnBInWorld; - _pointInWorld = pointInWorld; - //negative means penetration - _distance = depth; - } - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public class PointCollector : DiscreteCollisionDetectorInterface.Result + { + private Vector3 _normalOnBInWorld; + private Vector3 _pointInWorld; + private float _distance; //negative means penetration + private bool _hasResult; + + public PointCollector() + { + _distance = 1e30f; + _hasResult = false; + } + + public Vector3 NormalOnBInWorld { get { return _normalOnBInWorld; } } + public Vector3 PointInWorld { get { return _pointInWorld; } } + public float Distance { get { return _distance; } } + public bool HasResult { get { return _hasResult; } } + + public override void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB) + { + //?? + } + + public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) + { + if (depth < _distance) + { + _hasResult = true; + _normalOnBInWorld = normalOnBInWorld; + _pointInWorld = pointInWorld; + //negative means penetration + _distance = depth; + } + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs index 6d320c4..5c92a0c 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs @@ -1,142 +1,142 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - /// - /// SubsimplexConvexCast implements Gino van den Bergens' paper - /// "Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection" - /// GJK based Ray Cast, optimized version - /// Objects should not start in overlap, otherwise results are not defined. - /// - public class SubsimplexConvexCast : IConvexCast - { - private ISimplexSolver _simplexSolver; - private ConvexShape _convexA; - private ConvexShape _convexB; - - /// - /// Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases. - /// See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565 - /// - private const int MaxIterations = 32; - - public SubsimplexConvexCast(ConvexShape shapeA, ConvexShape shapeB, ISimplexSolver simplexSolver) - { - _simplexSolver = simplexSolver; - _convexA = shapeA; - _convexB = shapeB; - } - - #region IConvexCast Members - /// - /// SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects. - /// Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using GjkPairDetector. - /// - /// - /// - /// - /// - /// - /// - public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result) - { - MinkowskiSumShape convex = new MinkowskiSumShape(_convexA, _convexB); - - Matrix rayFromLocalA; - Matrix rayToLocalA; - - rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB; - rayToLocalA = MathHelper.InvertMatrix(toA) * toB; - - _simplexSolver.Reset(); - - convex.TransformB = rayFromLocalA; - - float lambda = 0; - //todo: need to verify this: - //because of minkowski difference, we need the inverse direction - - Vector3 s = -rayFromLocalA.Translation; - Vector3 r = -(rayToLocalA.Translation - rayFromLocalA.Translation); - Vector3 x = s; - Vector3 v; - Vector3 arbitraryPoint = convex.LocalGetSupportingVertex(r); - - v = x - arbitraryPoint; - - int maxIter = MaxIterations; - - Vector3 n = new Vector3(); - float lastLambda = lambda; - - float dist2 = v.LengthSquared(); - float epsilon = 0.0001f; - - Vector3 w, p; - float VdotR; - - while ((dist2 > epsilon) && (maxIter-- != 0)) - { - p = convex.LocalGetSupportingVertex(v); - w = x - p; - - float VdotW = Vector3.Dot(v, w); - - if (VdotW > 0) - { - VdotR = Vector3.Dot(v, r); - - if (VdotR >= -(MathHelper.Epsilon * MathHelper.Epsilon)) - return false; - else - { - lambda = lambda - VdotW / VdotR; - x = s + lambda * r; - _simplexSolver.Reset(); - //check next line - w = x - p; - lastLambda = lambda; - n = v; - } - } - _simplexSolver.AddVertex(w, x, p); - if (_simplexSolver.Closest(out v)) - { - dist2 = v.LengthSquared(); - } - else - { - dist2 = 0f; - } - } - result.Fraction = lambda; - result.Normal = n; - return true; - } - #endregion - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + /// + /// SubsimplexConvexCast implements Gino van den Bergens' paper + /// "Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection" + /// GJK based Ray Cast, optimized version + /// Objects should not start in overlap, otherwise results are not defined. + /// + public class SubsimplexConvexCast : IConvexCast + { + private ISimplexSolver _simplexSolver; + private ConvexShape _convexA; + private ConvexShape _convexB; + + /// + /// Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases. + /// See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565 + /// + private const int MaxIterations = 32; + + public SubsimplexConvexCast(ConvexShape shapeA, ConvexShape shapeB, ISimplexSolver simplexSolver) + { + _simplexSolver = simplexSolver; + _convexA = shapeA; + _convexB = shapeB; + } + + #region IConvexCast Members + /// + /// SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects. + /// Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using GjkPairDetector. + /// + /// + /// + /// + /// + /// + /// + public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result) + { + MinkowskiSumShape convex = new MinkowskiSumShape(_convexA, _convexB); + + Matrix rayFromLocalA; + Matrix rayToLocalA; + + rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB; + rayToLocalA = MathHelper.InvertMatrix(toA) * toB; + + _simplexSolver.Reset(); + + convex.TransformB = rayFromLocalA; + + float lambda = 0; + //todo: need to verify this: + //because of minkowski difference, we need the inverse direction + + Vector3 s = -rayFromLocalA.Translation; + Vector3 r = -(rayToLocalA.Translation - rayFromLocalA.Translation); + Vector3 x = s; + Vector3 v; + Vector3 arbitraryPoint = convex.LocalGetSupportingVertex(r); + + v = x - arbitraryPoint; + + int maxIter = MaxIterations; + + Vector3 n = new Vector3(); + float lastLambda = lambda; + + float dist2 = v.LengthSquared(); + float epsilon = 0.0001f; + + Vector3 w, p; + float VdotR; + + while ((dist2 > epsilon) && (maxIter-- != 0)) + { + p = convex.LocalGetSupportingVertex(v); + w = x - p; + + float VdotW = Vector3.Dot(v, w); + + if (VdotW > 0) + { + VdotR = Vector3.Dot(v, r); + + if (VdotR >= -(MathHelper.Epsilon * MathHelper.Epsilon)) + return false; + else + { + lambda = lambda - VdotW / VdotR; + x = s + lambda * r; + _simplexSolver.Reset(); + //check next line + w = x - p; + lastLambda = lambda; + n = v; + } + } + _simplexSolver.AddVertex(w, x, p); + if (_simplexSolver.Closest(out v)) + { + dist2 = v.LengthSquared(); + } + else + { + dist2 = 0f; + } + } + result.Fraction = lambda; + result.Normal = n; + return true; + } + #endregion + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs index c127460..1e0fb3b 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs @@ -1,115 +1,115 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public abstract class TriangleRaycastCallback : ITriangleCallback - { - private Vector3 _from; - private Vector3 _to; - private float _hitFraction; - - public TriangleRaycastCallback(Vector3 from, Vector3 to) - { - _from = from; - _to = to; - _hitFraction = 1; - } - - public Vector3 From { get { return _from; } set { _from = value; } } - public Vector3 To { get { return _to; } set { _to = value; } } - public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } - - public abstract float ReportHit(Vector3 hitNormalLocal, float hitFraction, int partId, int triangleIndex); - - #region ITriangleCallback Members - - public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) - { - Vector3 vertA = triangle[0]; - Vector3 vertB = triangle[1]; - Vector3 vertC = triangle[2]; - - Vector3 vBA = vertB - vertA; - Vector3 vCA = vertC - vertA; - - Vector3 triangleNormal = Vector3.Cross(vBA, vCA); - - float dist = Vector3.Dot(vertA, triangleNormal); - float distA = Vector3.Dot(triangleNormal, _from); - distA -= dist; - float distB = Vector3.Dot(triangleNormal, _to); - distB -= dist; - - if (distA * distB >= 0.0f) - { - return; // same sign - } - - float projLength = distA - distB; - float distance = (distA) / (projLength); - // Now we have the intersection point on the plane, we'll see if it's inside the triangle - // Add an epsilon as a tolerance for the raycast, - // in case the ray hits exacly on the edge of the triangle. - // It must be scaled for the triangle size. - - if (distance < _hitFraction) - { - float edgeTolerance = triangleNormal.LengthSquared(); - edgeTolerance *= -0.0001f; - Vector3 point = new Vector3(); - MathHelper.SetInterpolate3(_from, _to, distance, ref point); - - Vector3 vertexAPoint = vertA - point; - Vector3 vertexBPoint = vertB - point; - Vector3 contactPointA = Vector3.Cross(vertexAPoint, vertexBPoint); - - if (Vector3.Dot(contactPointA, triangleNormal) >= edgeTolerance) - { - Vector3 vertexCPoint = vertC - point; - Vector3 contactPointB = Vector3.Cross(vertexBPoint, vertexCPoint); - if (Vector3.Dot(contactPointB, triangleNormal) >= edgeTolerance) - { - Vector3 contactPointC = Vector3.Cross(vertexCPoint, vertexAPoint); - - if (Vector3.Dot(contactPointC, triangleNormal) >= edgeTolerance) - { - if (distA > 0) - { - _hitFraction = ReportHit(triangleNormal, distance, partID, triangleIndex); - } - else - { - _hitFraction = ReportHit(-triangleNormal, distance, partID, triangleIndex); - } - } - } - } - } - } - #endregion - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public abstract class TriangleRaycastCallback : ITriangleCallback + { + private Vector3 _from; + private Vector3 _to; + private float _hitFraction; + + public TriangleRaycastCallback(Vector3 from, Vector3 to) + { + _from = from; + _to = to; + _hitFraction = 1; + } + + public Vector3 From { get { return _from; } set { _from = value; } } + public Vector3 To { get { return _to; } set { _to = value; } } + public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } + + public abstract float ReportHit(Vector3 hitNormalLocal, float hitFraction, int partId, int triangleIndex); + + #region ITriangleCallback Members + + public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) + { + Vector3 vertA = triangle[0]; + Vector3 vertB = triangle[1]; + Vector3 vertC = triangle[2]; + + Vector3 vBA = vertB - vertA; + Vector3 vCA = vertC - vertA; + + Vector3 triangleNormal = Vector3.Cross(vBA, vCA); + + float dist = Vector3.Dot(vertA, triangleNormal); + float distA = Vector3.Dot(triangleNormal, _from); + distA -= dist; + float distB = Vector3.Dot(triangleNormal, _to); + distB -= dist; + + if (distA * distB >= 0.0f) + { + return; // same sign + } + + float projLength = distA - distB; + float distance = (distA) / (projLength); + // Now we have the intersection point on the plane, we'll see if it's inside the triangle + // Add an epsilon as a tolerance for the raycast, + // in case the ray hits exacly on the edge of the triangle. + // It must be scaled for the triangle size. + + if (distance < _hitFraction) + { + float edgeTolerance = triangleNormal.LengthSquared(); + edgeTolerance *= -0.0001f; + Vector3 point = new Vector3(); + MathHelper.SetInterpolate3(_from, _to, distance, ref point); + + Vector3 vertexAPoint = vertA - point; + Vector3 vertexBPoint = vertB - point; + Vector3 contactPointA = Vector3.Cross(vertexAPoint, vertexBPoint); + + if (Vector3.Dot(contactPointA, triangleNormal) >= edgeTolerance) + { + Vector3 vertexCPoint = vertC - point; + Vector3 contactPointB = Vector3.Cross(vertexBPoint, vertexCPoint); + if (Vector3.Dot(contactPointB, triangleNormal) >= edgeTolerance) + { + Vector3 contactPointC = Vector3.Cross(vertexCPoint, vertexAPoint); + + if (Vector3.Dot(contactPointC, triangleNormal) >= edgeTolerance) + { + if (distA > 0) + { + _hitFraction = ReportHit(triangleNormal, distance, partID, triangleIndex); + } + else + { + _hitFraction = ReportHit(-triangleNormal, distance, partID, triangleIndex); + } + } + } + } + } + } + #endregion + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs index 16f3dab..1bc94d3 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs @@ -1,643 +1,643 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public class UsageBitfield - { - private bool _usedVertexA, _usedVertexB, _usedVertexC, _usedVertexD; - - public bool UsedVertexA { get { return _usedVertexA; } set { _usedVertexA = value; } } - public bool UsedVertexB { get { return _usedVertexB; } set { _usedVertexB = value; } } - public bool UsedVertexC { get { return _usedVertexC; } set { _usedVertexC = value; } } - public bool UsedVertexD { get { return _usedVertexD; } set { _usedVertexD = value; } } - - public void Reset() - { - _usedVertexA = _usedVertexB = _usedVertexC = _usedVertexD = false; - } - } - - public class SubSimplexClosestResult - { - private Vector3 _closestPointOnSimplex; - - //MASK for m_usedVertices - //stores the simplex vertex-usage, using the MASK, - // if m_usedVertices & MASK then the related vertex is used - private UsageBitfield _usedVertices = new UsageBitfield(); - private float[] _barycentricCoords = new float[4]; - private bool _degenerate; - - public Vector3 ClosestPointOnSimplex { get { return _closestPointOnSimplex; } set { _closestPointOnSimplex = value; } } - public UsageBitfield UsedVertices { get { return _usedVertices; } set { _usedVertices = value; } } - public float[] BarycentricCoords { get { return _barycentricCoords; } set { _barycentricCoords = value; } } - public bool Degenerate { get { return _degenerate; } set { _degenerate = value; } } - - public void Reset() - { - _degenerate = false; - SetBarycentricCoordinates(); - _usedVertices.Reset(); - } - - public bool IsValid - { - get - { - return (_barycentricCoords[0] >= 0f) && - (_barycentricCoords[1] >= 0f) && - (_barycentricCoords[2] >= 0f) && - (_barycentricCoords[3] >= 0f); - } - } - - public void SetBarycentricCoordinates() - { - SetBarycentricCoordinates(0f, 0f, 0f, 0f); - } - - public void SetBarycentricCoordinates(float a, float b, float c, float d) - { - _barycentricCoords[0] = a; - _barycentricCoords[1] = b; - _barycentricCoords[2] = c; - _barycentricCoords[3] = d; - } - } - - /// VoronoiSimplexSolver is an implementation of the closest point distance - /// algorithm from a 1-4 points simplex to the origin. - /// Can be used with GJK, as an alternative to Johnson distance algorithm. - public class VoronoiSimplexSolver : ISimplexSolver - { - private const int VertexA = 0, VertexB = 1, VertexC = 2, VertexD = 3; - - private const int VoronoiSimplexMaxVerts = 5; - private const bool CatchDegenerateTetrahedron = true; - - private int _numVertices; - - private Vector3[] _simplexVectorW = new Vector3[VoronoiSimplexMaxVerts]; - private Vector3[] _simplexPointsP = new Vector3[VoronoiSimplexMaxVerts]; - private Vector3[] _simplexPointsQ = new Vector3[VoronoiSimplexMaxVerts]; - - private Vector3 _cachedPA; - private Vector3 _cachedPB; - private Vector3 _cachedV; - private Vector3 _lastW; - private bool _cachedValidClosest; - - private SubSimplexClosestResult _cachedBC = new SubSimplexClosestResult(); - - private bool _needsUpdate; - - #region ISimplexSolver Members - - public bool FullSimplex - { - get - { - return _numVertices == 4; - } - } - - public int NumVertices - { - get - { - return _numVertices; - } - } - - public void Reset() - { - _cachedValidClosest = false; - _numVertices = 0; - _needsUpdate = true; - _lastW = new Vector3(1e30f, 1e30f, 1e30f); - _cachedBC.Reset(); - } - - public void AddVertex(Vector3 w, Vector3 p, Vector3 q) - { - _lastW = w; - _needsUpdate = true; - - _simplexVectorW[_numVertices] = w; - _simplexPointsP[_numVertices] = p; - _simplexPointsQ[_numVertices] = q; - - _numVertices++; - } - - //return/calculate the closest vertex - public bool Closest(out Vector3 v) - { - bool succes = UpdateClosestVectorAndPoints(); - v = _cachedV; - return succes; - } - - public float MaxVertex - { - get - { - int numverts = NumVertices; - float maxV = 0f, curLen2; - for (int i = 0; i < numverts; i++) - { - curLen2 = _simplexVectorW[i].LengthSquared(); - if (maxV < curLen2) maxV = curLen2; - } - return maxV; - } - } - - //return the current simplex - public int GetSimplex(out Vector3[] pBuf, out Vector3[] qBuf, out Vector3[] yBuf) - { - int numverts = NumVertices; - pBuf = new Vector3[numverts]; - qBuf = new Vector3[numverts]; - yBuf = new Vector3[numverts]; - for (int i = 0; i < numverts; i++) - { - yBuf[i] = _simplexVectorW[i]; - pBuf[i] = _simplexPointsP[i]; - qBuf[i] = _simplexPointsQ[i]; - } - return numverts; - } - - public bool InSimplex(Vector3 w) - { - //check in case lastW is already removed - if (w == _lastW) return true; - - //w is in the current (reduced) simplex - int numverts = NumVertices; - for (int i = 0; i < numverts; i++) - if (_simplexVectorW[i] == w) return true; - - return false; - } - - public void BackupClosest(out Vector3 v) - { - v = _cachedV; - } - - public bool EmptySimplex - { - get - { - return NumVertices == 0; - } - } - - public void ComputePoints(out Vector3 p1, out Vector3 p2) - { - UpdateClosestVectorAndPoints(); - p1 = _cachedPA; - p2 = _cachedPB; - } - - #endregion - - public void RemoveVertex(int index) - { - BulletDebug.Assert(_numVertices > 0); - _numVertices--; - _simplexVectorW[index] = _simplexVectorW[_numVertices]; - _simplexPointsP[index] = _simplexPointsP[_numVertices]; - _simplexPointsQ[index] = _simplexPointsQ[_numVertices]; - } - - public void ReduceVertices(UsageBitfield usedVerts) - { - if ((NumVertices >= 4) && (!usedVerts.UsedVertexD)) RemoveVertex(3); - if ((NumVertices >= 3) && (!usedVerts.UsedVertexC)) RemoveVertex(2); - if ((NumVertices >= 2) && (!usedVerts.UsedVertexB)) RemoveVertex(1); - if ((NumVertices >= 1) && (!usedVerts.UsedVertexA)) RemoveVertex(0); - } - - public bool UpdateClosestVectorAndPoints() - { - if (_needsUpdate) - { - _cachedBC.Reset(); - _needsUpdate = false; - - Vector3 p, a, b, c, d; - switch (NumVertices) - { - case 0: - _cachedValidClosest = false; - break; - case 1: - _cachedPA = _simplexPointsP[0]; - _cachedPB = _simplexPointsQ[0]; - _cachedV = _cachedPA - _cachedPB; - _cachedBC.Reset(); - _cachedBC.SetBarycentricCoordinates(1f, 0f, 0f, 0f); - _cachedValidClosest = _cachedBC.IsValid; - break; - case 2: - //closest point origin from line segment - Vector3 from = _simplexVectorW[0]; - Vector3 to = _simplexVectorW[1]; - Vector3 nearest; - - Vector3 diff = -from; - Vector3 v = to - from; - float t = Vector3.Dot(v, diff); - - if (t > 0) - { - float dotVV = v.LengthSquared(); - if (t < dotVV) - { - t /= dotVV; - diff -= t * v; - _cachedBC.UsedVertices.UsedVertexA = true; - _cachedBC.UsedVertices.UsedVertexB = true; - } - else - { - t = 1; - diff -= v; - //reduce to 1 point - _cachedBC.UsedVertices.UsedVertexB = true; - } - } - else - { - t = 0; - //reduce to 1 point - _cachedBC.UsedVertices.UsedVertexA = true; - } - - _cachedBC.SetBarycentricCoordinates(1 - t, t, 0, 0); - nearest = from + t * v; - - _cachedPA = _simplexPointsP[0] + t * (_simplexPointsP[1] - _simplexPointsP[0]); - _cachedPB = _simplexPointsQ[0] + t * (_simplexPointsQ[1] - _simplexPointsQ[0]); - _cachedV = _cachedPA - _cachedPB; - - ReduceVertices(_cachedBC.UsedVertices); - - _cachedValidClosest = _cachedBC.IsValid; - break; - case 3: - //closest point origin from triangle - p = new Vector3(); - a = _simplexVectorW[0]; - b = _simplexVectorW[1]; - c = _simplexVectorW[2]; - - ClosestPtPointTriangle(p, a, b, c, ref _cachedBC); - _cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] + - _simplexPointsP[1] * _cachedBC.BarycentricCoords[1] + - _simplexPointsP[2] * _cachedBC.BarycentricCoords[2] + - _simplexPointsP[3] * _cachedBC.BarycentricCoords[3]; - - _cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] + - _simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] + - _simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] + - _simplexPointsQ[3] * _cachedBC.BarycentricCoords[3]; - - _cachedV = _cachedPA - _cachedPB; - - ReduceVertices(_cachedBC.UsedVertices); - _cachedValidClosest = _cachedBC.IsValid; - break; - case 4: - p = new Vector3(); - a = _simplexVectorW[0]; - b = _simplexVectorW[1]; - c = _simplexVectorW[2]; - d = _simplexVectorW[3]; - - bool hasSeperation = ClosestPtPointTetrahedron(p, a, b, c, d, ref _cachedBC); - - if (hasSeperation) - { - _cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] + - _simplexPointsP[1] * _cachedBC.BarycentricCoords[1] + - _simplexPointsP[2] * _cachedBC.BarycentricCoords[2] + - _simplexPointsP[3] * _cachedBC.BarycentricCoords[3]; - - _cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] + - _simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] + - _simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] + - _simplexPointsQ[3] * _cachedBC.BarycentricCoords[3]; - - _cachedV = _cachedPA - _cachedPB; - ReduceVertices(_cachedBC.UsedVertices); - } - else - { - if (_cachedBC.Degenerate) - { - _cachedValidClosest = false; - } - else - { - _cachedValidClosest = true; - //degenerate case == false, penetration = true + zero - _cachedV.X = _cachedV.Y = _cachedV.Z = 0f; - } - break; // !!!!!!!!!!!! proverit na vsakiy sluchai - } - - _cachedValidClosest = _cachedBC.IsValid; - - //closest point origin from tetrahedron - break; - default: - _cachedValidClosest = false; - break; - } - } - - return _cachedValidClosest; - } - - public bool ClosestPtPointTriangle(Vector3 p, Vector3 a, Vector3 b, Vector3 c, - ref SubSimplexClosestResult result) - { - result.UsedVertices.Reset(); - - float v, w; - - // Check if P in vertex region outside A - Vector3 ab = b - a; - Vector3 ac = c - a; - Vector3 ap = p - a; - float d1 = Vector3.Dot(ab, ap); - float d2 = Vector3.Dot(ac, ap); - if (d1 <= 0f && d2 <= 0f) - { - result.ClosestPointOnSimplex = a; - result.UsedVertices.UsedVertexA = true; - result.SetBarycentricCoordinates(1, 0, 0, 0); - return true; // a; // barycentric coordinates (1,0,0) - } - - // Check if P in vertex region outside B - Vector3 bp = p - b; - float d3 = Vector3.Dot(ab, bp); - float d4 = Vector3.Dot(ac, bp); - if (d3 >= 0f && d4 <= d3) - { - result.ClosestPointOnSimplex = b; - result.UsedVertices.UsedVertexB = true; - result.SetBarycentricCoordinates(0, 1, 0, 0); - - return true; // b; // barycentric coordinates (0,1,0) - } - // Check if P in edge region of AB, if so return projection of P onto AB - float vc = d1 * d4 - d3 * d2; - if (vc <= 0f && d1 >= 0f && d3 <= 0f) - { - v = d1 / (d1 - d3); - result.ClosestPointOnSimplex = a + v * ab; - result.UsedVertices.UsedVertexA = true; - result.UsedVertices.UsedVertexB = true; - result.SetBarycentricCoordinates(1 - v, v, 0, 0); - return true; - //return a + v * ab; // barycentric coordinates (1-v,v,0) - } - - // Check if P in vertex region outside C - Vector3 cp = p - c; - float d5 = Vector3.Dot(ab, cp); - float d6 = Vector3.Dot(ac, cp); - if (d6 >= 0f && d5 <= d6) - { - result.ClosestPointOnSimplex = c; - result.UsedVertices.UsedVertexC = true; - result.SetBarycentricCoordinates(0, 0, 1, 0); - return true;//c; // barycentric coordinates (0,0,1) - } - - // Check if P in edge region of AC, if so return projection of P onto AC - float vb = d5 * d2 - d1 * d6; - if (vb <= 0f && d2 >= 0f && d6 <= 0f) - { - w = d2 / (d2 - d6); - result.ClosestPointOnSimplex = a + w * ac; - result.UsedVertices.UsedVertexA = true; - result.UsedVertices.UsedVertexC = true; - result.SetBarycentricCoordinates(1 - w, 0, w, 0); - return true; - //return a + w * ac; // barycentric coordinates (1-w,0,w) - } - - // Check if P in edge region of BC, if so return projection of P onto BC - float va = d3 * d6 - d5 * d4; - if (va <= 0f && (d4 - d3) >= 0f && (d5 - d6) >= 0f) - { - w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); - - result.ClosestPointOnSimplex = b + w * (c - b); - result.UsedVertices.UsedVertexB = true; - result.UsedVertices.UsedVertexC = true; - result.SetBarycentricCoordinates(0, 1 - w, w, 0); - return true; - // return b + w * (c - b); // barycentric coordinates (0,1-w,w) - } - - // P inside face region. Compute Q through its barycentric coordinates (u,v,w) - float denom = 1.0f / (va + vb + vc); - v = vb * denom; - w = vc * denom; - - result.ClosestPointOnSimplex = a + ab * v + ac * w; - result.UsedVertices.UsedVertexA = true; - result.UsedVertices.UsedVertexB = true; - result.UsedVertices.UsedVertexC = true; - result.SetBarycentricCoordinates(1 - v - w, v, w, 0); - - return true; - } - - /// Test if point p and d lie on opposite sides of plane through abc - public int PointOutsideOfPlane(Vector3 p, Vector3 a, Vector3 b, Vector3 c, Vector3 d) - { - Vector3 normal = Vector3.Cross(b - a, c - a); - - float signp = Vector3.Dot(p - a, normal); // [AP AB AC] - float signd = Vector3.Dot(d - a, normal); // [AD AB AC] - - if (CatchDegenerateTetrahedron) - if (signd * signd < (1e-4f * 1e-4f)) return -1; - - // Points on opposite sides if expression signs are opposite - return signp * signd < 0f ? 1 : 0; - } - - public bool ClosestPtPointTetrahedron(Vector3 p, Vector3 a, Vector3 b, Vector3 c, Vector3 d, - ref SubSimplexClosestResult finalResult) - { - SubSimplexClosestResult tempResult = new SubSimplexClosestResult(); - - // Start out assuming point inside all halfspaces, so closest to itself - finalResult.ClosestPointOnSimplex = p; - finalResult.UsedVertices.Reset(); - finalResult.UsedVertices.UsedVertexA = true; - finalResult.UsedVertices.UsedVertexB = true; - finalResult.UsedVertices.UsedVertexC = true; - finalResult.UsedVertices.UsedVertexD = true; - - int pointOutsideABC = PointOutsideOfPlane(p, a, b, c, d); - int pointOutsideACD = PointOutsideOfPlane(p, a, c, d, b); - int pointOutsideADB = PointOutsideOfPlane(p, a, d, b, c); - int pointOutsideBDC = PointOutsideOfPlane(p, b, d, c, a); - - if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0) - { - finalResult.Degenerate = true; - return false; - } - - if (pointOutsideABC == 0 && pointOutsideACD == 0 && pointOutsideADB == 0 && pointOutsideBDC == 0) - return false; - - float bestSqDist = float.MaxValue; - // If point outside face abc then compute closest point on abc - if (pointOutsideABC != 0) - { - ClosestPtPointTriangle(p, a, b, c, ref tempResult); - Vector3 q = tempResult.ClosestPointOnSimplex; - - float sqDist = ((Vector3)(q - p)).LengthSquared(); - // Update best closest point if (squared) distance is less than current best - if (sqDist < bestSqDist) - { - bestSqDist = sqDist; - finalResult.ClosestPointOnSimplex = q; - //convert result bitmask! - finalResult.UsedVertices.Reset(); - finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA; - finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexB; - finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexC; - finalResult.SetBarycentricCoordinates( - tempResult.BarycentricCoords[VertexA], - tempResult.BarycentricCoords[VertexB], - tempResult.BarycentricCoords[VertexC], - 0); - } - } - - // Repeat test for face acd - if (pointOutsideACD != 0) - { - ClosestPtPointTriangle(p, a, c, d, ref tempResult); - Vector3 q = tempResult.ClosestPointOnSimplex; - //convert result bitmask! - - float sqDist = ((Vector3)(q - p)).LengthSquared(); - if (sqDist < bestSqDist) - { - bestSqDist = sqDist; - finalResult.ClosestPointOnSimplex = q; - finalResult.UsedVertices.Reset(); - finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA; - finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexB; - finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexC; - finalResult.SetBarycentricCoordinates( - tempResult.BarycentricCoords[VertexA], - 0, - tempResult.BarycentricCoords[VertexB], - tempResult.BarycentricCoords[VertexC]); - } - } - // Repeat test for face adb - - if (pointOutsideADB != 0) - { - ClosestPtPointTriangle(p, a, d, b, ref tempResult); - Vector3 q = tempResult.ClosestPointOnSimplex; - //convert result bitmask! - - float sqDist = ((Vector3)(q - p)).LengthSquared(); - if (sqDist < bestSqDist) - { - bestSqDist = sqDist; - finalResult.ClosestPointOnSimplex = q; - finalResult.UsedVertices.Reset(); - finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA; - finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexB; - finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexC; - finalResult.SetBarycentricCoordinates( - tempResult.BarycentricCoords[VertexA], - tempResult.BarycentricCoords[VertexC], - 0, - tempResult.BarycentricCoords[VertexB]); - - } - } - // Repeat test for face bdc - - if (pointOutsideBDC != 0) - { - ClosestPtPointTriangle(p, b, d, c, ref tempResult); - Vector3 q = tempResult.ClosestPointOnSimplex; - //convert result bitmask! - float sqDist = ((Vector3)(q - p)).LengthSquared(); - if (sqDist < bestSqDist) - { - bestSqDist = sqDist; - finalResult.ClosestPointOnSimplex = q; - finalResult.UsedVertices.Reset(); - finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexA; - finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexB; - finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexC; - - finalResult.SetBarycentricCoordinates( - 0, - tempResult.BarycentricCoords[VertexA], - tempResult.BarycentricCoords[VertexC], - tempResult.BarycentricCoords[VertexB]); - } - } - - //help! we ended up full ! - - if (finalResult.UsedVertices.UsedVertexA && - finalResult.UsedVertices.UsedVertexB && - finalResult.UsedVertices.UsedVertexC && - finalResult.UsedVertices.UsedVertexD) - { - return true; - } - - return true; - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public class UsageBitfield + { + private bool _usedVertexA, _usedVertexB, _usedVertexC, _usedVertexD; + + public bool UsedVertexA { get { return _usedVertexA; } set { _usedVertexA = value; } } + public bool UsedVertexB { get { return _usedVertexB; } set { _usedVertexB = value; } } + public bool UsedVertexC { get { return _usedVertexC; } set { _usedVertexC = value; } } + public bool UsedVertexD { get { return _usedVertexD; } set { _usedVertexD = value; } } + + public void Reset() + { + _usedVertexA = _usedVertexB = _usedVertexC = _usedVertexD = false; + } + } + + public class SubSimplexClosestResult + { + private Vector3 _closestPointOnSimplex; + + //MASK for m_usedVertices + //stores the simplex vertex-usage, using the MASK, + // if m_usedVertices & MASK then the related vertex is used + private UsageBitfield _usedVertices = new UsageBitfield(); + private float[] _barycentricCoords = new float[4]; + private bool _degenerate; + + public Vector3 ClosestPointOnSimplex { get { return _closestPointOnSimplex; } set { _closestPointOnSimplex = value; } } + public UsageBitfield UsedVertices { get { return _usedVertices; } set { _usedVertices = value; } } + public float[] BarycentricCoords { get { return _barycentricCoords; } set { _barycentricCoords = value; } } + public bool Degenerate { get { return _degenerate; } set { _degenerate = value; } } + + public void Reset() + { + _degenerate = false; + SetBarycentricCoordinates(); + _usedVertices.Reset(); + } + + public bool IsValid + { + get + { + return (_barycentricCoords[0] >= 0f) && + (_barycentricCoords[1] >= 0f) && + (_barycentricCoords[2] >= 0f) && + (_barycentricCoords[3] >= 0f); + } + } + + public void SetBarycentricCoordinates() + { + SetBarycentricCoordinates(0f, 0f, 0f, 0f); + } + + public void SetBarycentricCoordinates(float a, float b, float c, float d) + { + _barycentricCoords[0] = a; + _barycentricCoords[1] = b; + _barycentricCoords[2] = c; + _barycentricCoords[3] = d; + } + } + + /// VoronoiSimplexSolver is an implementation of the closest point distance + /// algorithm from a 1-4 points simplex to the origin. + /// Can be used with GJK, as an alternative to Johnson distance algorithm. + public class VoronoiSimplexSolver : ISimplexSolver + { + private const int VertexA = 0, VertexB = 1, VertexC = 2, VertexD = 3; + + private const int VoronoiSimplexMaxVerts = 5; + private const bool CatchDegenerateTetrahedron = true; + + private int _numVertices; + + private Vector3[] _simplexVectorW = new Vector3[VoronoiSimplexMaxVerts]; + private Vector3[] _simplexPointsP = new Vector3[VoronoiSimplexMaxVerts]; + private Vector3[] _simplexPointsQ = new Vector3[VoronoiSimplexMaxVerts]; + + private Vector3 _cachedPA; + private Vector3 _cachedPB; + private Vector3 _cachedV; + private Vector3 _lastW; + private bool _cachedValidClosest; + + private SubSimplexClosestResult _cachedBC = new SubSimplexClosestResult(); + + private bool _needsUpdate; + + #region ISimplexSolver Members + + public bool FullSimplex + { + get + { + return _numVertices == 4; + } + } + + public int NumVertices + { + get + { + return _numVertices; + } + } + + public void Reset() + { + _cachedValidClosest = false; + _numVertices = 0; + _needsUpdate = true; + _lastW = new Vector3(1e30f, 1e30f, 1e30f); + _cachedBC.Reset(); + } + + public void AddVertex(Vector3 w, Vector3 p, Vector3 q) + { + _lastW = w; + _needsUpdate = true; + + _simplexVectorW[_numVertices] = w; + _simplexPointsP[_numVertices] = p; + _simplexPointsQ[_numVertices] = q; + + _numVertices++; + } + + //return/calculate the closest vertex + public bool Closest(out Vector3 v) + { + bool succes = UpdateClosestVectorAndPoints(); + v = _cachedV; + return succes; + } + + public float MaxVertex + { + get + { + int numverts = NumVertices; + float maxV = 0f, curLen2; + for (int i = 0; i < numverts; i++) + { + curLen2 = _simplexVectorW[i].LengthSquared(); + if (maxV < curLen2) maxV = curLen2; + } + return maxV; + } + } + + //return the current simplex + public int GetSimplex(out Vector3[] pBuf, out Vector3[] qBuf, out Vector3[] yBuf) + { + int numverts = NumVertices; + pBuf = new Vector3[numverts]; + qBuf = new Vector3[numverts]; + yBuf = new Vector3[numverts]; + for (int i = 0; i < numverts; i++) + { + yBuf[i] = _simplexVectorW[i]; + pBuf[i] = _simplexPointsP[i]; + qBuf[i] = _simplexPointsQ[i]; + } + return numverts; + } + + public bool InSimplex(Vector3 w) + { + //check in case lastW is already removed + if (w == _lastW) return true; + + //w is in the current (reduced) simplex + int numverts = NumVertices; + for (int i = 0; i < numverts; i++) + if (_simplexVectorW[i] == w) return true; + + return false; + } + + public void BackupClosest(out Vector3 v) + { + v = _cachedV; + } + + public bool EmptySimplex + { + get + { + return NumVertices == 0; + } + } + + public void ComputePoints(out Vector3 p1, out Vector3 p2) + { + UpdateClosestVectorAndPoints(); + p1 = _cachedPA; + p2 = _cachedPB; + } + + #endregion + + public void RemoveVertex(int index) + { + BulletDebug.Assert(_numVertices > 0); + _numVertices--; + _simplexVectorW[index] = _simplexVectorW[_numVertices]; + _simplexPointsP[index] = _simplexPointsP[_numVertices]; + _simplexPointsQ[index] = _simplexPointsQ[_numVertices]; + } + + public void ReduceVertices(UsageBitfield usedVerts) + { + if ((NumVertices >= 4) && (!usedVerts.UsedVertexD)) RemoveVertex(3); + if ((NumVertices >= 3) && (!usedVerts.UsedVertexC)) RemoveVertex(2); + if ((NumVertices >= 2) && (!usedVerts.UsedVertexB)) RemoveVertex(1); + if ((NumVertices >= 1) && (!usedVerts.UsedVertexA)) RemoveVertex(0); + } + + public bool UpdateClosestVectorAndPoints() + { + if (_needsUpdate) + { + _cachedBC.Reset(); + _needsUpdate = false; + + Vector3 p, a, b, c, d; + switch (NumVertices) + { + case 0: + _cachedValidClosest = false; + break; + case 1: + _cachedPA = _simplexPointsP[0]; + _cachedPB = _simplexPointsQ[0]; + _cachedV = _cachedPA - _cachedPB; + _cachedBC.Reset(); + _cachedBC.SetBarycentricCoordinates(1f, 0f, 0f, 0f); + _cachedValidClosest = _cachedBC.IsValid; + break; + case 2: + //closest point origin from line segment + Vector3 from = _simplexVectorW[0]; + Vector3 to = _simplexVectorW[1]; + Vector3 nearest; + + Vector3 diff = -from; + Vector3 v = to - from; + float t = Vector3.Dot(v, diff); + + if (t > 0) + { + float dotVV = v.LengthSquared(); + if (t < dotVV) + { + t /= dotVV; + diff -= t * v; + _cachedBC.UsedVertices.UsedVertexA = true; + _cachedBC.UsedVertices.UsedVertexB = true; + } + else + { + t = 1; + diff -= v; + //reduce to 1 point + _cachedBC.UsedVertices.UsedVertexB = true; + } + } + else + { + t = 0; + //reduce to 1 point + _cachedBC.UsedVertices.UsedVertexA = true; + } + + _cachedBC.SetBarycentricCoordinates(1 - t, t, 0, 0); + nearest = from + t * v; + + _cachedPA = _simplexPointsP[0] + t * (_simplexPointsP[1] - _simplexPointsP[0]); + _cachedPB = _simplexPointsQ[0] + t * (_simplexPointsQ[1] - _simplexPointsQ[0]); + _cachedV = _cachedPA - _cachedPB; + + ReduceVertices(_cachedBC.UsedVertices); + + _cachedValidClosest = _cachedBC.IsValid; + break; + case 3: + //closest point origin from triangle + p = new Vector3(); + a = _simplexVectorW[0]; + b = _simplexVectorW[1]; + c = _simplexVectorW[2]; + + ClosestPtPointTriangle(p, a, b, c, ref _cachedBC); + _cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] + + _simplexPointsP[1] * _cachedBC.BarycentricCoords[1] + + _simplexPointsP[2] * _cachedBC.BarycentricCoords[2] + + _simplexPointsP[3] * _cachedBC.BarycentricCoords[3]; + + _cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] + + _simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] + + _simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] + + _simplexPointsQ[3] * _cachedBC.BarycentricCoords[3]; + + _cachedV = _cachedPA - _cachedPB; + + ReduceVertices(_cachedBC.UsedVertices); + _cachedValidClosest = _cachedBC.IsValid; + break; + case 4: + p = new Vector3(); + a = _simplexVectorW[0]; + b = _simplexVectorW[1]; + c = _simplexVectorW[2]; + d = _simplexVectorW[3]; + + bool hasSeperation = ClosestPtPointTetrahedron(p, a, b, c, d, ref _cachedBC); + + if (hasSeperation) + { + _cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] + + _simplexPointsP[1] * _cachedBC.BarycentricCoords[1] + + _simplexPointsP[2] * _cachedBC.BarycentricCoords[2] + + _simplexPointsP[3] * _cachedBC.BarycentricCoords[3]; + + _cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] + + _simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] + + _simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] + + _simplexPointsQ[3] * _cachedBC.BarycentricCoords[3]; + + _cachedV = _cachedPA - _cachedPB; + ReduceVertices(_cachedBC.UsedVertices); + } + else + { + if (_cachedBC.Degenerate) + { + _cachedValidClosest = false; + } + else + { + _cachedValidClosest = true; + //degenerate case == false, penetration = true + zero + _cachedV.X = _cachedV.Y = _cachedV.Z = 0f; + } + break; // !!!!!!!!!!!! proverit na vsakiy sluchai + } + + _cachedValidClosest = _cachedBC.IsValid; + + //closest point origin from tetrahedron + break; + default: + _cachedValidClosest = false; + break; + } + } + + return _cachedValidClosest; + } + + public bool ClosestPtPointTriangle(Vector3 p, Vector3 a, Vector3 b, Vector3 c, + ref SubSimplexClosestResult result) + { + result.UsedVertices.Reset(); + + float v, w; + + // Check if P in vertex region outside A + Vector3 ab = b - a; + Vector3 ac = c - a; + Vector3 ap = p - a; + float d1 = Vector3.Dot(ab, ap); + float d2 = Vector3.Dot(ac, ap); + if (d1 <= 0f && d2 <= 0f) + { + result.ClosestPointOnSimplex = a; + result.UsedVertices.UsedVertexA = true; + result.SetBarycentricCoordinates(1, 0, 0, 0); + return true; // a; // barycentric coordinates (1,0,0) + } + + // Check if P in vertex region outside B + Vector3 bp = p - b; + float d3 = Vector3.Dot(ab, bp); + float d4 = Vector3.Dot(ac, bp); + if (d3 >= 0f && d4 <= d3) + { + result.ClosestPointOnSimplex = b; + result.UsedVertices.UsedVertexB = true; + result.SetBarycentricCoordinates(0, 1, 0, 0); + + return true; // b; // barycentric coordinates (0,1,0) + } + // Check if P in edge region of AB, if so return projection of P onto AB + float vc = d1 * d4 - d3 * d2; + if (vc <= 0f && d1 >= 0f && d3 <= 0f) + { + v = d1 / (d1 - d3); + result.ClosestPointOnSimplex = a + v * ab; + result.UsedVertices.UsedVertexA = true; + result.UsedVertices.UsedVertexB = true; + result.SetBarycentricCoordinates(1 - v, v, 0, 0); + return true; + //return a + v * ab; // barycentric coordinates (1-v,v,0) + } + + // Check if P in vertex region outside C + Vector3 cp = p - c; + float d5 = Vector3.Dot(ab, cp); + float d6 = Vector3.Dot(ac, cp); + if (d6 >= 0f && d5 <= d6) + { + result.ClosestPointOnSimplex = c; + result.UsedVertices.UsedVertexC = true; + result.SetBarycentricCoordinates(0, 0, 1, 0); + return true;//c; // barycentric coordinates (0,0,1) + } + + // Check if P in edge region of AC, if so return projection of P onto AC + float vb = d5 * d2 - d1 * d6; + if (vb <= 0f && d2 >= 0f && d6 <= 0f) + { + w = d2 / (d2 - d6); + result.ClosestPointOnSimplex = a + w * ac; + result.UsedVertices.UsedVertexA = true; + result.UsedVertices.UsedVertexC = true; + result.SetBarycentricCoordinates(1 - w, 0, w, 0); + return true; + //return a + w * ac; // barycentric coordinates (1-w,0,w) + } + + // Check if P in edge region of BC, if so return projection of P onto BC + float va = d3 * d6 - d5 * d4; + if (va <= 0f && (d4 - d3) >= 0f && (d5 - d6) >= 0f) + { + w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); + + result.ClosestPointOnSimplex = b + w * (c - b); + result.UsedVertices.UsedVertexB = true; + result.UsedVertices.UsedVertexC = true; + result.SetBarycentricCoordinates(0, 1 - w, w, 0); + return true; + // return b + w * (c - b); // barycentric coordinates (0,1-w,w) + } + + // P inside face region. Compute Q through its barycentric coordinates (u,v,w) + float denom = 1.0f / (va + vb + vc); + v = vb * denom; + w = vc * denom; + + result.ClosestPointOnSimplex = a + ab * v + ac * w; + result.UsedVertices.UsedVertexA = true; + result.UsedVertices.UsedVertexB = true; + result.UsedVertices.UsedVertexC = true; + result.SetBarycentricCoordinates(1 - v - w, v, w, 0); + + return true; + } + + /// Test if point p and d lie on opposite sides of plane through abc + public int PointOutsideOfPlane(Vector3 p, Vector3 a, Vector3 b, Vector3 c, Vector3 d) + { + Vector3 normal = Vector3.Cross(b - a, c - a); + + float signp = Vector3.Dot(p - a, normal); // [AP AB AC] + float signd = Vector3.Dot(d - a, normal); // [AD AB AC] + + if (CatchDegenerateTetrahedron) + if (signd * signd < (1e-4f * 1e-4f)) return -1; + + // Points on opposite sides if expression signs are opposite + return signp * signd < 0f ? 1 : 0; + } + + public bool ClosestPtPointTetrahedron(Vector3 p, Vector3 a, Vector3 b, Vector3 c, Vector3 d, + ref SubSimplexClosestResult finalResult) + { + SubSimplexClosestResult tempResult = new SubSimplexClosestResult(); + + // Start out assuming point inside all halfspaces, so closest to itself + finalResult.ClosestPointOnSimplex = p; + finalResult.UsedVertices.Reset(); + finalResult.UsedVertices.UsedVertexA = true; + finalResult.UsedVertices.UsedVertexB = true; + finalResult.UsedVertices.UsedVertexC = true; + finalResult.UsedVertices.UsedVertexD = true; + + int pointOutsideABC = PointOutsideOfPlane(p, a, b, c, d); + int pointOutsideACD = PointOutsideOfPlane(p, a, c, d, b); + int pointOutsideADB = PointOutsideOfPlane(p, a, d, b, c); + int pointOutsideBDC = PointOutsideOfPlane(p, b, d, c, a); + + if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0) + { + finalResult.Degenerate = true; + return false; + } + + if (pointOutsideABC == 0 && pointOutsideACD == 0 && pointOutsideADB == 0 && pointOutsideBDC == 0) + return false; + + float bestSqDist = float.MaxValue; + // If point outside face abc then compute closest point on abc + if (pointOutsideABC != 0) + { + ClosestPtPointTriangle(p, a, b, c, ref tempResult); + Vector3 q = tempResult.ClosestPointOnSimplex; + + float sqDist = ((Vector3)(q - p)).LengthSquared(); + // Update best closest point if (squared) distance is less than current best + if (sqDist < bestSqDist) + { + bestSqDist = sqDist; + finalResult.ClosestPointOnSimplex = q; + //convert result bitmask! + finalResult.UsedVertices.Reset(); + finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA; + finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexB; + finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexC; + finalResult.SetBarycentricCoordinates( + tempResult.BarycentricCoords[VertexA], + tempResult.BarycentricCoords[VertexB], + tempResult.BarycentricCoords[VertexC], + 0); + } + } + + // Repeat test for face acd + if (pointOutsideACD != 0) + { + ClosestPtPointTriangle(p, a, c, d, ref tempResult); + Vector3 q = tempResult.ClosestPointOnSimplex; + //convert result bitmask! + + float sqDist = ((Vector3)(q - p)).LengthSquared(); + if (sqDist < bestSqDist) + { + bestSqDist = sqDist; + finalResult.ClosestPointOnSimplex = q; + finalResult.UsedVertices.Reset(); + finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA; + finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexB; + finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexC; + finalResult.SetBarycentricCoordinates( + tempResult.BarycentricCoords[VertexA], + 0, + tempResult.BarycentricCoords[VertexB], + tempResult.BarycentricCoords[VertexC]); + } + } + // Repeat test for face adb + + if (pointOutsideADB != 0) + { + ClosestPtPointTriangle(p, a, d, b, ref tempResult); + Vector3 q = tempResult.ClosestPointOnSimplex; + //convert result bitmask! + + float sqDist = ((Vector3)(q - p)).LengthSquared(); + if (sqDist < bestSqDist) + { + bestSqDist = sqDist; + finalResult.ClosestPointOnSimplex = q; + finalResult.UsedVertices.Reset(); + finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA; + finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexB; + finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexC; + finalResult.SetBarycentricCoordinates( + tempResult.BarycentricCoords[VertexA], + tempResult.BarycentricCoords[VertexC], + 0, + tempResult.BarycentricCoords[VertexB]); + + } + } + // Repeat test for face bdc + + if (pointOutsideBDC != 0) + { + ClosestPtPointTriangle(p, b, d, c, ref tempResult); + Vector3 q = tempResult.ClosestPointOnSimplex; + //convert result bitmask! + float sqDist = ((Vector3)(q - p)).LengthSquared(); + if (sqDist < bestSqDist) + { + bestSqDist = sqDist; + finalResult.ClosestPointOnSimplex = q; + finalResult.UsedVertices.Reset(); + finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexA; + finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexB; + finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexC; + + finalResult.SetBarycentricCoordinates( + 0, + tempResult.BarycentricCoords[VertexA], + tempResult.BarycentricCoords[VertexC], + tempResult.BarycentricCoords[VertexB]); + } + } + + //help! we ended up full ! + + if (finalResult.UsedVertices.UsedVertexA && + finalResult.UsedVertices.UsedVertexB && + finalResult.UsedVertices.UsedVertexC && + finalResult.UsedVertices.UsedVertexD) + { + return true; + } + + return true; + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs index e4a9ae2..e731643 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs @@ -1,488 +1,488 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX.Dynamics -{ - public delegate float ContactSolverFunc (RigidBody bodyA, RigidBody bodyB, ManifoldPoint contactPoint, ContactSolverInfo info); - - public enum ContactSolverType - { - Default = 0, - TypeA, - TypeB, - User, - MaxContactSolverType, - } - - public class ConstraintPersistentData - { - // total applied impulse during most recent frame - private float _appliedImpulse; - private float _previousAppliedImpulse; - private float _accumulatedTangentImpulse0; - private float _accumulatedTangentImpulse1; - - private float _jacDiagABInv; - private float _jacDiagABInvTangentA; - private float _jacDiagABInvTangentB; - private int _persistentLifeTime; - private float _restitution; - private float _friction; - private float _penetration; - private Vector3 _frictionWorldTangentialA; - private Vector3 _frictionWorldTangentialB; - - private Vector3 _frictionAngularComponent0A; - private Vector3 _frictionAngularComponent0B; - private Vector3 _frictionAngularComponent1A; - private Vector3 _frictionAngularComponent1B; - - //some data doesn't need to be persistent over frames: todo: clean/reuse this - private Vector3 _angularComponentA; - private Vector3 _angularComponentB; - - private ContactSolverFunc _contactSolverFunc; - private ContactSolverFunc _frictionSolverFunc; - - public float AppliedImpulse { get { return _appliedImpulse; } set { _appliedImpulse = value; } } - public float PreviousAppliedImpulse { get { return _previousAppliedImpulse; } set { _previousAppliedImpulse = value; } } - public float AccumulatedTangentImpulseA { get { return _accumulatedTangentImpulse0; } set { _accumulatedTangentImpulse0 = value; } } - public float AccumulatedTangentImpulseB { get { return _accumulatedTangentImpulse1; } set { _accumulatedTangentImpulse1 = value; } } - - public float JacDiagABInv { get { return _jacDiagABInv; } set { _jacDiagABInv = value; } } - public float JacDiagABInvTangentA { get { return _jacDiagABInvTangentA; } set { _jacDiagABInvTangentA = value; } } - public float JacDiagABInvTangentB { get { return _jacDiagABInvTangentB; } set { _jacDiagABInvTangentB = value; } } - public int PersistentLifeTime { get { return _persistentLifeTime; } set { _persistentLifeTime = value; } } - public float Restitution { get { return _restitution; } set { _restitution = value; } } - public float Friction { get { return _friction; } set { _friction = value; } } - public float Penetration { get { return _penetration; } set { _penetration = value; } } - public Vector3 FrictionWorldTangentialA { get { return _frictionWorldTangentialA; } set { _frictionWorldTangentialA = value; } } - public Vector3 FrictionWorldTangentialB { get { return _frictionWorldTangentialB; } set { _frictionWorldTangentialB = value; } } - - public Vector3 FrictionAngularComponent0A { get { return _frictionAngularComponent0A; } set { _frictionAngularComponent0A = value; } } - public Vector3 FrictionAngularComponent0B { get { return _frictionAngularComponent0B; } set { _frictionAngularComponent0B = value; } } - public Vector3 FrictionAngularComponent1A { get { return _frictionAngularComponent1A; } set { _frictionAngularComponent1A = value; } } - public Vector3 FrictionAngularComponent1B { get { return _frictionAngularComponent1B; } set { _frictionAngularComponent1B = value; } } - - public Vector3 AngularComponentA { get { return _angularComponentA; } set { _angularComponentA = value; } } - public Vector3 AngularComponentB { get { return _angularComponentB; } set { _angularComponentB = value; } } - - public ContactSolverFunc ContactSolverFunc { get { return _contactSolverFunc; } set { _contactSolverFunc = value; } } - public ContactSolverFunc FrictionSolverFunc { get { return _frictionSolverFunc; } set { _frictionSolverFunc = value; } } - } - - public static class ContactConstraint - { - private const int UseInternalApplyImpulse = 1; - - /// - /// bilateral constraint between two dynamic objects - /// positive distance = separation, negative distance = penetration - /// - /// - /// - /// - /// - /// - /// - /// - /// - public static void ResolveSingleBilateral(RigidBody bodyA, Vector3 posA, - RigidBody bodyB, Vector3 posB, - float distance, Vector3 normal, out float impulse, float timeStep) - { - float normalLenSqr = normal.LengthSquared(); - - if (Math.Abs(normalLenSqr) >= 1.1f) - throw new BulletException(); - - /*if (normalLenSqr > 1.1f) - { - impulse = 0f; - return; - }*/ - Vector3 rel_pos1 = posA - bodyA.CenterOfMassPosition; - Vector3 rel_pos2 = posB - bodyB.CenterOfMassPosition; - //this jacobian entry could be re-used for all iterations - - Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1); - Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2); - Vector3 vel = vel1 - vel2; - - - JacobianEntry jac = new JacobianEntry(Matrix.Transpose(bodyA.CenterOfMassTransform), - Matrix.Transpose(bodyB.CenterOfMassTransform), - rel_pos1, rel_pos2, normal, bodyA.InvInertiaDiagLocal, bodyA.InverseMass, - bodyB.InvInertiaDiagLocal, bodyB.InverseMass); - - float jacDiagAB = jac.Diagonal; - float jacDiagABInv = 1f / jacDiagAB; - - float rel_vel = jac.GetRelativeVelocity( - bodyA.LinearVelocity, - Vector3.TransformNormal(bodyA.AngularVelocity, Matrix.Transpose(bodyA.CenterOfMassTransform)), - bodyB.LinearVelocity, - Vector3.TransformNormal(bodyB.AngularVelocity, Matrix.Transpose(bodyB.CenterOfMassTransform))); - float a; - a = jacDiagABInv; - - - rel_vel = Vector3.Dot(normal, vel); - - float contactDamping = 0.2f; - - float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; - impulse = velocityImpulse; - } - - - /// - /// contact constraint resolution: - /// calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint - /// positive distance = separation, negative distance = penetration - /// - /// - /// - /// - /// - /// - public static float ResolveSingleCollision(RigidBody bodyA, RigidBody bodyB, - ManifoldPoint contactPoint, ContactSolverInfo solverInfo) - { - Vector3 pos1 = contactPoint.PositionWorldOnA; - Vector3 pos2 = contactPoint.PositionWorldOnB; - - - // printf("distance=%f\n",distance); - - Vector3 normal = contactPoint.NormalWorldOnB; - - Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition; - Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition; - - Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1); - Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2); - Vector3 vel = vel1 - vel2; - float rel_vel; - rel_vel = Vector3.Dot(normal, vel); - - - float Kfps = 1f / solverInfo.TimeStep; - - //float damping = solverInfo.m_damping; - float Kerp = solverInfo.Erp; - - float Kcor = Kerp * Kfps; - - //printf("dist=%f\n",distance); - - ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; - if (cpd == null) - throw new BulletException(); - - float distance = cpd.Penetration;//contactPoint.getDistance(); - - - //distance = 0.f; - float positionalError = Kcor * -distance; - //jacDiagABInv; - float velocityError = cpd.Restitution - rel_vel;// * damping; - - - float penetrationImpulse = positionalError * cpd.JacDiagABInv; - float velocityImpulse = velocityError * cpd.JacDiagABInv; - float normalImpulse = penetrationImpulse + velocityImpulse; - - // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse - float oldNormalImpulse = cpd.AppliedImpulse; - float sum = oldNormalImpulse + normalImpulse; - cpd.AppliedImpulse = 0f > sum ? 0f : sum; - - normalImpulse = cpd.AppliedImpulse - oldNormalImpulse; - - if (bodyA.InverseMass != 0) - { - bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse); - } - if (bodyB.InverseMass != 0) - { - bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse); - } - - /*body1.applyImpulse(normal * (normalImpulse), rel_pos1); - body2.applyImpulse(-normal * (normalImpulse), rel_pos2);*/ - - return normalImpulse; - } - - public static float ResolveSingleFriction(RigidBody bodyA, RigidBody bodyB, - ManifoldPoint contactPoint, ContactSolverInfo solverInfo) - { - - Vector3 pos1 = contactPoint.PositionWorldOnA; - Vector3 pos2 = contactPoint.PositionWorldOnB; - - Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition; - Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition; - - ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; - if (cpd == null) - throw new BulletException(); - - float combinedFriction = cpd.Friction; - - float limit = cpd.AppliedImpulse * combinedFriction; - - //friction - if (cpd.AppliedImpulse > 0) - { - //apply friction in the 2 tangential directions - - // 1st tangent - Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1); - Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2); - Vector3 vel = vel1 - vel2; - - float j1, j2; - - { - - float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel); - - // calculate j that moves us to zero relative velocity - j1 = -vrel * cpd.JacDiagABInvTangentA; - float oldTangentImpulse = cpd.AccumulatedTangentImpulseA; - cpd.AccumulatedTangentImpulseA = oldTangentImpulse + j1; - float atia = cpd.AccumulatedTangentImpulseA; - MathHelper.SetMin(ref atia, limit); - MathHelper.SetMax(ref atia, -limit); - cpd.AccumulatedTangentImpulseA = atia; - j1 = cpd.AccumulatedTangentImpulseA - oldTangentImpulse; - - } - { - // 2nd tangent - - float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel); - - // calculate j that moves us to zero relative velocity - j2 = -vrel * cpd.JacDiagABInvTangentB; - float oldTangentImpulse = cpd.AccumulatedTangentImpulseB; - cpd.AccumulatedTangentImpulseB = oldTangentImpulse + j2; - float atib = cpd.AccumulatedTangentImpulseB; - MathHelper.SetMin(ref atib, limit); - MathHelper.SetMax(ref atib, -limit); - cpd.AccumulatedTangentImpulseB = atib; - j2 = cpd.AccumulatedTangentImpulseB - oldTangentImpulse; - } - - if (bodyA.InverseMass != 0) - { - bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyA.InverseMass, cpd.FrictionAngularComponent0A, j1); - bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyA.InverseMass, cpd.FrictionAngularComponent1A, j2); - } - if (bodyB.InverseMass != 0) - { - bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyB.InverseMass, cpd.FrictionAngularComponent0B, -j1); - bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyB.InverseMass, cpd.FrictionAngularComponent1B, -j2); - } - - } - return cpd.AppliedImpulse; - } - - public static float ResolveSingleFrictionOriginal( - RigidBody bodyA, - RigidBody bodyB, - ManifoldPoint contactPoint, - ContactSolverInfo solverInfo) - { - Vector3 posA = contactPoint.PositionWorldOnA; - Vector3 posB = contactPoint.PositionWorldOnB; - - Vector3 relPosA = posA - bodyA.CenterOfMassPosition; - Vector3 relPosB = posB - bodyB.CenterOfMassPosition; - - ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; - if (cpd == null) - throw new BulletException(); - - float combinedFriction = cpd.Friction; - - float limit = cpd.AppliedImpulse * combinedFriction; - //if (contactPoint.m_appliedImpulse>0.f) - //friction - { - //apply friction in the 2 tangential directions - - { - // 1st tangent - Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); - Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); - Vector3 vel = velA - velB; - - float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel); - - // calculate j that moves us to zero relative velocity - float j = -vrel * cpd.JacDiagABInvTangentA; - float total = cpd.AccumulatedTangentImpulseA + j; - if (limit < total) - total = limit; - if (total < -limit) - total = -limit; - j = total - cpd.AccumulatedTangentImpulseA; - cpd.AccumulatedTangentImpulseA = total; - bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialA, relPosA); - bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialA, relPosB); - } - - - { - // 2nd tangent - Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); - Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); - Vector3 vel = velA - velB; - - float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel); - - // calculate j that moves us to zero relative velocity - float j = -vrel * cpd.JacDiagABInvTangentB; - float total = cpd.AccumulatedTangentImpulseB + j; - if (limit < total) - total = limit; - if (total < -limit) - total = -limit; - j = total - cpd.AccumulatedTangentImpulseB; - cpd.AccumulatedTangentImpulseB = total; - bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialB, relPosA); - bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialB, relPosB); - } - } - - return cpd.AppliedImpulse; - } - - //velocity + friction - //response between two dynamic objects with friction - public static float ResolveSingleCollisionCombined( - RigidBody bodyA, - RigidBody bodyB, - ManifoldPoint contactPoint, - ContactSolverInfo solverInfo) - { - - Vector3 posA = contactPoint.PositionWorldOnA; - Vector3 posB = contactPoint.PositionWorldOnB; - Vector3 normal = contactPoint.NormalWorldOnB; - - Vector3 relPosA = posA - bodyA.CenterOfMassPosition; - Vector3 relPosB = posB - bodyB.CenterOfMassPosition; - - Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); - Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); - Vector3 vel = velA - velB; - float relVel; - relVel = Vector3.Dot(normal, vel); - - float Kfps = 1f / solverInfo.TimeStep; - - //float damping = solverInfo.m_damping; - float Kerp = solverInfo.Erp; - float Kcor = Kerp * Kfps; - - ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; - if (cpd == null) - throw new BulletException(); - - float distance = cpd.Penetration; - float positionalError = Kcor * -distance; - float velocityError = cpd.Restitution - relVel;// * damping; - - float penetrationImpulse = positionalError * cpd.JacDiagABInv; - - float velocityImpulse = velocityError * cpd.JacDiagABInv; - - float normalImpulse = penetrationImpulse + velocityImpulse; - - // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse - float oldNormalImpulse = cpd.AppliedImpulse; - float sum = oldNormalImpulse + normalImpulse; - cpd.AppliedImpulse = 0 > sum ? 0 : sum; - - normalImpulse = cpd.AppliedImpulse - oldNormalImpulse; - - if (bodyA.InverseMass != 0) - { - bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse); - } - if (bodyB.InverseMass != 0) - { - bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse); - } - - { - //friction - Vector3 vel12 = bodyA.GetVelocityInLocalPoint(relPosA); - Vector3 vel22 = bodyB.GetVelocityInLocalPoint(relPosB); - Vector3 vel3 = vel12 - vel22; - - relVel = Vector3.Dot(normal, vel3); - - - Vector3 latVel = vel3 - normal * relVel; - float lat_rel_vel = latVel.Length(); - - float combinedFriction = cpd.Friction; - - if (cpd.AppliedImpulse > 0) - if (lat_rel_vel > float.Epsilon) - { - latVel /= lat_rel_vel; - Vector3 temp1 = Vector3.TransformNormal(Vector3.Cross(relPosA, latVel), bodyA.InvInertiaTensorWorld); - Vector3 temp2 = Vector3.TransformNormal(Vector3.Cross(relPosB, latVel), bodyB.InvInertiaTensorWorld); - float friction_impulse = lat_rel_vel / - (bodyA.InverseMass + bodyB.InverseMass + Vector3.Dot(latVel, Vector3.Cross(temp1, relPosA) + Vector3.Cross(temp2, relPosB))); - float normal_impulse = cpd.AppliedImpulse * combinedFriction; - - MathHelper.SetMin(ref friction_impulse, normal_impulse); - MathHelper.SetMin(ref friction_impulse, -normal_impulse); - bodyA.ApplyImpulse(latVel * -friction_impulse, relPosA); - bodyB.ApplyImpulse(latVel * friction_impulse, relPosB); - } - } - return normalImpulse; - } - - public static float ResolveSingleFrictionEmpty( - RigidBody bodyA, - RigidBody bodyB, - ManifoldPoint contactPoint, - ContactSolverInfo solverInfo) - { - return 0; - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX.Dynamics +{ + public delegate float ContactSolverFunc (RigidBody bodyA, RigidBody bodyB, ManifoldPoint contactPoint, ContactSolverInfo info); + + public enum ContactSolverType + { + Default = 0, + TypeA, + TypeB, + User, + MaxContactSolverType, + } + + public class ConstraintPersistentData + { + // total applied impulse during most recent frame + private float _appliedImpulse; + private float _previousAppliedImpulse; + private float _accumulatedTangentImpulse0; + private float _accumulatedTangentImpulse1; + + private float _jacDiagABInv; + private float _jacDiagABInvTangentA; + private float _jacDiagABInvTangentB; + private int _persistentLifeTime; + private float _restitution; + private float _friction; + private float _penetration; + private Vector3 _frictionWorldTangentialA; + private Vector3 _frictionWorldTangentialB; + + private Vector3 _frictionAngularComponent0A; + private Vector3 _frictionAngularComponent0B; + private Vector3 _frictionAngularComponent1A; + private Vector3 _frictionAngularComponent1B; + + //some data doesn't need to be persistent over frames: todo: clean/reuse this + private Vector3 _angularComponentA; + private Vector3 _angularComponentB; + + private ContactSolverFunc _contactSolverFunc; + private ContactSolverFunc _frictionSolverFunc; + + public float AppliedImpulse { get { return _appliedImpulse; } set { _appliedImpulse = value; } } + public float PreviousAppliedImpulse { get { return _previousAppliedImpulse; } set { _previousAppliedImpulse = value; } } + public float AccumulatedTangentImpulseA { get { return _accumulatedTangentImpulse0; } set { _accumulatedTangentImpulse0 = value; } } + public float AccumulatedTangentImpulseB { get { return _accumulatedTangentImpulse1; } set { _accumulatedTangentImpulse1 = value; } } + + public float JacDiagABInv { get { return _jacDiagABInv; } set { _jacDiagABInv = value; } } + public float JacDiagABInvTangentA { get { return _jacDiagABInvTangentA; } set { _jacDiagABInvTangentA = value; } } + public float JacDiagABInvTangentB { get { return _jacDiagABInvTangentB; } set { _jacDiagABInvTangentB = value; } } + public int PersistentLifeTime { get { return _persistentLifeTime; } set { _persistentLifeTime = value; } } + public float Restitution { get { return _restitution; } set { _restitution = value; } } + public float Friction { get { return _friction; } set { _friction = value; } } + public float Penetration { get { return _penetration; } set { _penetration = value; } } + public Vector3 FrictionWorldTangentialA { get { return _frictionWorldTangentialA; } set { _frictionWorldTangentialA = value; } } + public Vector3 FrictionWorldTangentialB { get { return _frictionWorldTangentialB; } set { _frictionWorldTangentialB = value; } } + + public Vector3 FrictionAngularComponent0A { get { return _frictionAngularComponent0A; } set { _frictionAngularComponent0A = value; } } + public Vector3 FrictionAngularComponent0B { get { return _frictionAngularComponent0B; } set { _frictionAngularComponent0B = value; } } + public Vector3 FrictionAngularComponent1A { get { return _frictionAngularComponent1A; } set { _frictionAngularComponent1A = value; } } + public Vector3 FrictionAngularComponent1B { get { return _frictionAngularComponent1B; } set { _frictionAngularComponent1B = value; } } + + public Vector3 AngularComponentA { get { return _angularComponentA; } set { _angularComponentA = value; } } + public Vector3 AngularComponentB { get { return _angularComponentB; } set { _angularComponentB = value; } } + + public ContactSolverFunc ContactSolverFunc { get { return _contactSolverFunc; } set { _contactSolverFunc = value; } } + public ContactSolverFunc FrictionSolverFunc { get { return _frictionSolverFunc; } set { _frictionSolverFunc = value; } } + } + + public static class ContactConstraint + { + private const int UseInternalApplyImpulse = 1; + + /// + /// bilateral constraint between two dynamic objects + /// positive distance = separation, negative distance = penetration + /// + /// + /// + /// + /// + /// + /// + /// + /// + public static void ResolveSingleBilateral(RigidBody bodyA, Vector3 posA, + RigidBody bodyB, Vector3 posB, + float distance, Vector3 normal, out float impulse, float timeStep) + { + float normalLenSqr = normal.LengthSquared(); + + if (Math.Abs(normalLenSqr) >= 1.1f) + throw new BulletException(); + + /*if (normalLenSqr > 1.1f) + { + impulse = 0f; + return; + }*/ + Vector3 rel_pos1 = posA - bodyA.CenterOfMassPosition; + Vector3 rel_pos2 = posB - bodyB.CenterOfMassPosition; + //this jacobian entry could be re-used for all iterations + + Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1); + Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2); + Vector3 vel = vel1 - vel2; + + + JacobianEntry jac = new JacobianEntry(Matrix.Transpose(bodyA.CenterOfMassTransform), + Matrix.Transpose(bodyB.CenterOfMassTransform), + rel_pos1, rel_pos2, normal, bodyA.InvInertiaDiagLocal, bodyA.InverseMass, + bodyB.InvInertiaDiagLocal, bodyB.InverseMass); + + float jacDiagAB = jac.Diagonal; + float jacDiagABInv = 1f / jacDiagAB; + + float rel_vel = jac.GetRelativeVelocity( + bodyA.LinearVelocity, + Vector3.TransformNormal(bodyA.AngularVelocity, Matrix.Transpose(bodyA.CenterOfMassTransform)), + bodyB.LinearVelocity, + Vector3.TransformNormal(bodyB.AngularVelocity, Matrix.Transpose(bodyB.CenterOfMassTransform))); + float a; + a = jacDiagABInv; + + + rel_vel = Vector3.Dot(normal, vel); + + float contactDamping = 0.2f; + + float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; + impulse = velocityImpulse; + } + + + /// + /// contact constraint resolution: + /// calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint + /// positive distance = separation, negative distance = penetration + /// + /// + /// + /// + /// + /// + public static float ResolveSingleCollision(RigidBody bodyA, RigidBody bodyB, + ManifoldPoint contactPoint, ContactSolverInfo solverInfo) + { + Vector3 pos1 = contactPoint.PositionWorldOnA; + Vector3 pos2 = contactPoint.PositionWorldOnB; + + + // printf("distance=%f\n",distance); + + Vector3 normal = contactPoint.NormalWorldOnB; + + Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition; + Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition; + + Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1); + Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2); + Vector3 vel = vel1 - vel2; + float rel_vel; + rel_vel = Vector3.Dot(normal, vel); + + + float Kfps = 1f / solverInfo.TimeStep; + + //float damping = solverInfo.m_damping; + float Kerp = solverInfo.Erp; + + float Kcor = Kerp * Kfps; + + //printf("dist=%f\n",distance); + + ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; + if (cpd == null) + throw new BulletException(); + + float distance = cpd.Penetration;//contactPoint.getDistance(); + + + //distance = 0.f; + float positionalError = Kcor * -distance; + //jacDiagABInv; + float velocityError = cpd.Restitution - rel_vel;// * damping; + + + float penetrationImpulse = positionalError * cpd.JacDiagABInv; + float velocityImpulse = velocityError * cpd.JacDiagABInv; + float normalImpulse = penetrationImpulse + velocityImpulse; + + // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse + float oldNormalImpulse = cpd.AppliedImpulse; + float sum = oldNormalImpulse + normalImpulse; + cpd.AppliedImpulse = 0f > sum ? 0f : sum; + + normalImpulse = cpd.AppliedImpulse - oldNormalImpulse; + + if (bodyA.InverseMass != 0) + { + bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse); + } + if (bodyB.InverseMass != 0) + { + bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse); + } + + /*body1.applyImpulse(normal * (normalImpulse), rel_pos1); + body2.applyImpulse(-normal * (normalImpulse), rel_pos2);*/ + + return normalImpulse; + } + + public static float ResolveSingleFriction(RigidBody bodyA, RigidBody bodyB, + ManifoldPoint contactPoint, ContactSolverInfo solverInfo) + { + + Vector3 pos1 = contactPoint.PositionWorldOnA; + Vector3 pos2 = contactPoint.PositionWorldOnB; + + Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition; + Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition; + + ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; + if (cpd == null) + throw new BulletException(); + + float combinedFriction = cpd.Friction; + + float limit = cpd.AppliedImpulse * combinedFriction; + + //friction + if (cpd.AppliedImpulse > 0) + { + //apply friction in the 2 tangential directions + + // 1st tangent + Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1); + Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2); + Vector3 vel = vel1 - vel2; + + float j1, j2; + + { + + float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel); + + // calculate j that moves us to zero relative velocity + j1 = -vrel * cpd.JacDiagABInvTangentA; + float oldTangentImpulse = cpd.AccumulatedTangentImpulseA; + cpd.AccumulatedTangentImpulseA = oldTangentImpulse + j1; + float atia = cpd.AccumulatedTangentImpulseA; + MathHelper.SetMin(ref atia, limit); + MathHelper.SetMax(ref atia, -limit); + cpd.AccumulatedTangentImpulseA = atia; + j1 = cpd.AccumulatedTangentImpulseA - oldTangentImpulse; + + } + { + // 2nd tangent + + float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel); + + // calculate j that moves us to zero relative velocity + j2 = -vrel * cpd.JacDiagABInvTangentB; + float oldTangentImpulse = cpd.AccumulatedTangentImpulseB; + cpd.AccumulatedTangentImpulseB = oldTangentImpulse + j2; + float atib = cpd.AccumulatedTangentImpulseB; + MathHelper.SetMin(ref atib, limit); + MathHelper.SetMax(ref atib, -limit); + cpd.AccumulatedTangentImpulseB = atib; + j2 = cpd.AccumulatedTangentImpulseB - oldTangentImpulse; + } + + if (bodyA.InverseMass != 0) + { + bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyA.InverseMass, cpd.FrictionAngularComponent0A, j1); + bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyA.InverseMass, cpd.FrictionAngularComponent1A, j2); + } + if (bodyB.InverseMass != 0) + { + bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyB.InverseMass, cpd.FrictionAngularComponent0B, -j1); + bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyB.InverseMass, cpd.FrictionAngularComponent1B, -j2); + } + + } + return cpd.AppliedImpulse; + } + + public static float ResolveSingleFrictionOriginal( + RigidBody bodyA, + RigidBody bodyB, + ManifoldPoint contactPoint, + ContactSolverInfo solverInfo) + { + Vector3 posA = contactPoint.PositionWorldOnA; + Vector3 posB = contactPoint.PositionWorldOnB; + + Vector3 relPosA = posA - bodyA.CenterOfMassPosition; + Vector3 relPosB = posB - bodyB.CenterOfMassPosition; + + ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; + if (cpd == null) + throw new BulletException(); + + float combinedFriction = cpd.Friction; + + float limit = cpd.AppliedImpulse * combinedFriction; + //if (contactPoint.m_appliedImpulse>0.f) + //friction + { + //apply friction in the 2 tangential directions + + { + // 1st tangent + Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); + Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); + Vector3 vel = velA - velB; + + float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel); + + // calculate j that moves us to zero relative velocity + float j = -vrel * cpd.JacDiagABInvTangentA; + float total = cpd.AccumulatedTangentImpulseA + j; + if (limit < total) + total = limit; + if (total < -limit) + total = -limit; + j = total - cpd.AccumulatedTangentImpulseA; + cpd.AccumulatedTangentImpulseA = total; + bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialA, relPosA); + bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialA, relPosB); + } + + + { + // 2nd tangent + Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); + Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); + Vector3 vel = velA - velB; + + float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel); + + // calculate j that moves us to zero relative velocity + float j = -vrel * cpd.JacDiagABInvTangentB; + float total = cpd.AccumulatedTangentImpulseB + j; + if (limit < total) + total = limit; + if (total < -limit) + total = -limit; + j = total - cpd.AccumulatedTangentImpulseB; + cpd.AccumulatedTangentImpulseB = total; + bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialB, relPosA); + bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialB, relPosB); + } + } + + return cpd.AppliedImpulse; + } + + //velocity + friction + //response between two dynamic objects with friction + public static float ResolveSingleCollisionCombined( + RigidBody bodyA, + RigidBody bodyB, + ManifoldPoint contactPoint, + ContactSolverInfo solverInfo) + { + + Vector3 posA = contactPoint.PositionWorldOnA; + Vector3 posB = contactPoint.PositionWorldOnB; + Vector3 normal = contactPoint.NormalWorldOnB; + + Vector3 relPosA = posA - bodyA.CenterOfMassPosition; + Vector3 relPosB = posB - bodyB.CenterOfMassPosition; + + Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA); + Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB); + Vector3 vel = velA - velB; + float relVel; + relVel = Vector3.Dot(normal, vel); + + float Kfps = 1f / solverInfo.TimeStep; + + //float damping = solverInfo.m_damping; + float Kerp = solverInfo.Erp; + float Kcor = Kerp * Kfps; + + ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData; + if (cpd == null) + throw new BulletException(); + + float distance = cpd.Penetration; + float positionalError = Kcor * -distance; + float velocityError = cpd.Restitution - relVel;// * damping; + + float penetrationImpulse = positionalError * cpd.JacDiagABInv; + + float velocityImpulse = velocityError * cpd.JacDiagABInv; + + float normalImpulse = penetrationImpulse + velocityImpulse; + + // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse + float oldNormalImpulse = cpd.AppliedImpulse; + float sum = oldNormalImpulse + normalImpulse; + cpd.AppliedImpulse = 0 > sum ? 0 : sum; + + normalImpulse = cpd.AppliedImpulse - oldNormalImpulse; + + if (bodyA.InverseMass != 0) + { + bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse); + } + if (bodyB.InverseMass != 0) + { + bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse); + } + + { + //friction + Vector3 vel12 = bodyA.GetVelocityInLocalPoint(relPosA); + Vector3 vel22 = bodyB.GetVelocityInLocalPoint(relPosB); + Vector3 vel3 = vel12 - vel22; + + relVel = Vector3.Dot(normal, vel3); + + + Vector3 latVel = vel3 - normal * relVel; + float lat_rel_vel = latVel.Length(); + + float combinedFriction = cpd.Friction; + + if (cpd.AppliedImpulse > 0) + if (lat_rel_vel > float.Epsilon) + { + latVel /= lat_rel_vel; + Vector3 temp1 = Vector3.TransformNormal(Vector3.Cross(relPosA, latVel), bodyA.InvInertiaTensorWorld); + Vector3 temp2 = Vector3.TransformNormal(Vector3.Cross(relPosB, latVel), bodyB.InvInertiaTensorWorld); + float friction_impulse = lat_rel_vel / + (bodyA.InverseMass + bodyB.InverseMass + Vector3.Dot(latVel, Vector3.Cross(temp1, relPosA) + Vector3.Cross(temp2, relPosB))); + float normal_impulse = cpd.AppliedImpulse * combinedFriction; + + MathHelper.SetMin(ref friction_impulse, normal_impulse); + MathHelper.SetMin(ref friction_impulse, -normal_impulse); + bodyA.ApplyImpulse(latVel * -friction_impulse, relPosA); + bodyB.ApplyImpulse(latVel * friction_impulse, relPosB); + } + } + return normalImpulse; + } + + public static float ResolveSingleFrictionEmpty( + RigidBody bodyA, + RigidBody bodyB, + ManifoldPoint contactPoint, + ContactSolverInfo solverInfo) + { + return 0; + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactSolverInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactSolverInfo.cs index c1763df..d3424f6 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactSolverInfo.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactSolverInfo.cs @@ -1,62 +1,62 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; - -namespace XnaDevRu.BulletX.Dynamics -{ - public class ContactSolverInfo - { - private float _tau; - private float _damping; - private float _friction; - private float _timeStep; - private float _restitution; - private int _numIterations; - private float _maxErrorReduction; - private float _sor; - private float _erp; - - public ContactSolverInfo() - { - _tau = 0.6f; - _damping = 1.0f; - _friction = 0.3f; - _restitution = 0f; - _maxErrorReduction = 20f; - _numIterations = 10; - _erp = 0.4f; - _sor = 1.3f; - } - - public float Tau { get { return _tau; } set { _tau = value; } } - public float Damping { get { return _damping; } set { _damping = value; } } - public float Friction { get { return _friction; } set { _friction = value; } } - public float TimeStep { get { return _timeStep; } set { _timeStep = value; } } - public float Restitution { get { return _restitution; } set { _restitution = value; } } - public int IterationsCount { get { return _numIterations; } set { _numIterations = value; } } - public float MaxErrorReduction { get { return _maxErrorReduction; } set { _maxErrorReduction = value; } } - public float Sor { get { return _sor; } set { _sor = value; } } - public float Erp { get { return _erp; } set { _erp = value; } } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace XnaDevRu.BulletX.Dynamics +{ + public class ContactSolverInfo + { + private float _tau; + private float _damping; + private float _friction; + private float _timeStep; + private float _restitution; + private int _numIterations; + private float _maxErrorReduction; + private float _sor; + private float _erp; + + public ContactSolverInfo() + { + _tau = 0.6f; + _damping = 1.0f; + _friction = 0.3f; + _restitution = 0f; + _maxErrorReduction = 20f; + _numIterations = 10; + _erp = 0.4f; + _sor = 1.3f; + } + + public float Tau { get { return _tau; } set { _tau = value; } } + public float Damping { get { return _damping; } set { _damping = value; } } + public float Friction { get { return _friction; } set { _friction = value; } } + public float TimeStep { get { return _timeStep; } set { _timeStep = value; } } + public float Restitution { get { return _restitution; } set { _restitution = value; } } + public int IterationsCount { get { return _numIterations; } set { _numIterations = value; } } + public float MaxErrorReduction { get { return _maxErrorReduction; } set { _maxErrorReduction = value; } } + public float Sor { get { return _sor; } set { _sor = value; } } + public float Erp { get { return _erp; } set { _erp = value; } } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs index 479c863..f1e0b1e 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs @@ -1,440 +1,440 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX.Dynamics -{ - /// - /// Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space - /// Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked' - /// Work in progress (is still a Hinge actually) - /// - public class Generic6DofConstraint : TypedConstraint - { - private static readonly float[] _sign = { 1.0f, -1.0f, 1.0f }; - private static readonly int[] _axisA = { 1, 0, 0 }; - private static readonly int[] _axisB = { 2, 2, 1 }; - - private JacobianEntry[] _jacLinear = new JacobianEntry[3]; // 3 orthogonal linear constraints - private JacobianEntry[] _jacAng = new JacobianEntry[3]; // 3 orthogonal angular constraints - - private Matrix _frameInA; // the constraint space w.r.t body A - private Matrix _frameInB; // the constraint space w.r.t body B - - private float[] _lowerLimit = new float[6]; // the constraint lower limits - private float[] _upperLimit = new float[6]; // the constraint upper limits - - private float[] _accumulatedImpulse = new float[6]; - - public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, Matrix frameInA, Matrix frameInB) - : base(rbA, rbB) - { - _frameInA = frameInA; - _frameInB = frameInB; - //free means upper < lower, - //locked means upper == lower - //limited means upper > lower - //so start all locked - for (int i = 0; i < 6; ++i) - { - _lowerLimit[i] = 0.0f; - _upperLimit[i] = 0.0f; - _accumulatedImpulse[i] = 0.0f; - } - } - - public Generic6DofConstraint() { } - - public void UpdateRHS(float timeStep) { } - - public float ComputeAngle(int axis) - { - float angle = 0; - - switch (axis) - { - case 0: - { - Vector3 v1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 1), RigidBodyA.CenterOfMassTransform); - Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform); - Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform); - - float s = Vector3.Dot(v1, w2); - float c = Vector3.Dot(v1, v2); - - angle = (float)Math.Atan2(s, c); - break; - } - case 1: - { - Vector3 w1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 2), RigidBodyA.CenterOfMassTransform); - Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform); - Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform); - - float s = Vector3.Dot(w1, u2); - float c = Vector3.Dot(w1, w2); - - angle = (float)Math.Atan2(s, c); - break; - } - case 2: - { - Vector3 u1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 0), RigidBodyA.CenterOfMassTransform); - Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform); - Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform); - - float s = Vector3.Dot(u1, v2); - float c = Vector3.Dot(u1, u2); - - angle = (float)Math.Atan2(s, c); - break; - } - default: BulletDebug.Assert(false); break; - } - - return angle; - } - - public void SetLinearLowerLimit(Vector3 linearLower) - { - _lowerLimit[0] = linearLower.X; - _lowerLimit[1] = linearLower.Y; - _lowerLimit[2] = linearLower.Z; - } - - public void SetLinearUpperLimit(Vector3 linearUpper) - { - _upperLimit[0] = linearUpper.X; - _upperLimit[1] = linearUpper.Y; - _upperLimit[2] = linearUpper.Z; - } - - public void SetAngularLowerLimit(Vector3 angularLower) - { - _lowerLimit[3] = angularLower.X; - _lowerLimit[4] = angularLower.Y; - _lowerLimit[5] = angularLower.Z; - } - - public void SetAngularUpperLimit(Vector3 angularUpper) - { - _upperLimit[3] = angularUpper.X; - _upperLimit[4] = angularUpper.Y; - _upperLimit[5] = angularUpper.Z; - } - - //first 3 are linear, next 3 are angular - public void SetLimit(int axis, float lo, float hi) - { - _lowerLimit[axis] = lo; - _upperLimit[axis] = hi; - } - - //free means upper < lower, - //locked means upper == lower - //limited means upper > lower - //limitIndex: first 3 are linear, next 3 are angular - public bool IsLimited(int limitIndex) - { - return (_upperLimit[limitIndex] >= _lowerLimit[limitIndex]); - } - - public override void BuildJacobian() - { - Vector3 localNormalInA = new Vector3(0, 0, 0); - - Vector3 pivotInA = _frameInA.Translation; - Vector3 pivotInB = _frameInB.Translation; - - Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform); - Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform); - - Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; - Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; - - //linear part - for (int i = 0; i < 3; i++) - { - if (IsLimited(i)) - { - if (i == 0) - localNormalInA = new Vector3(1, 0, 0); - else if (i == 1) - localNormalInA = new Vector3(0, 1, 0); - else - localNormalInA = new Vector3(0, 0, 1); - - Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform); - - // Create linear atom - _jacLinear[i] = new JacobianEntry( - MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), - MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), - MathHelper.Transform(pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, - MathHelper.Transform(pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, - normalWorld, - RigidBodyA.InvInertiaDiagLocal, - RigidBodyA.InverseMass, - RigidBodyB.InvInertiaDiagLocal, - RigidBodyB.InverseMass); - - //optionally disable warmstarting - _accumulatedImpulse[i] = 0f; - - // Apply accumulated impulse - Vector3 impulse_vector = _accumulatedImpulse[i] * normalWorld; - - RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1); - RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2); - } - } - - // angular part - for (int i = 0; i < 3; i++) - { - if (IsLimited(i + 3)) - { - Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform); - Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform); - - Vector3 axis = _sign[i] * Vector3.Cross(axisA, axisB); - - // Create angular atom - _jacAng[i] = new JacobianEntry(axis, - MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), - MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), - RigidBodyA.InvInertiaDiagLocal, - RigidBodyB.InvInertiaDiagLocal); - - _accumulatedImpulse[i + 3] = 0f; - - // Apply accumulated impulse - Vector3 impulse_vector = _accumulatedImpulse[i + 3] * axis; - - RigidBodyA.ApplyTorqueImpulse(impulse_vector); - RigidBodyB.ApplyTorqueImpulse(-impulse_vector); - } - } - } - - public override void SolveConstraint(float timeStep) - { - float tau = 0.1f; - float damping = 1.0f; - - Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform); - Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform); - - Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; - Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; - - Vector3 localNormalInA = new Vector3(); - - // linear - for (int i = 0; i < 3; i++) - { - if (IsLimited(i)) - { - Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform)); - Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform)); - - if (i == 0) - localNormalInA = new Vector3(1, 0, 0); - else if (i == 1) - localNormalInA = new Vector3(0, 1, 0); - else - localNormalInA = new Vector3(0, 0, 1); - - Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform); - - float jacDiagABInv = 1f / _jacLinear[i].Diagonal; - - //velocity error (first order error) - float rel_vel = _jacLinear[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA, - RigidBodyB.LinearVelocity, angvelB); - - //positional error (zeroth order error) - float depth = -Vector3.Dot(pivotAInW - pivotBInW, normalWorld); - float lo = -1e30f; - float hi = 1e30f; - - //handle the limits - if (_lowerLimit[i] < _upperLimit[i]) - { - if (depth > _upperLimit[i]) - { - depth -= _upperLimit[i]; - lo = 0f; - } - else - { - if (depth < _lowerLimit[i]) - { - depth -= _lowerLimit[i]; - hi = 0f; - } - else - { - continue; - } - } - } - - float normalImpulse = (tau * depth / timeStep - damping * rel_vel) * jacDiagABInv; - float oldNormalImpulse = _accumulatedImpulse[i]; - float sum = oldNormalImpulse + normalImpulse; - _accumulatedImpulse[i] = sum > hi ? 0f : sum < lo ? 0f : sum; - normalImpulse = _accumulatedImpulse[i] - oldNormalImpulse; - - Vector3 impulse_vector = normalWorld * normalImpulse; - RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1); - RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2); - } - } - - Vector3 axis; - float angle; - Matrix frameAWorld = RigidBodyA.CenterOfMassTransform * _frameInA; - Matrix frameBWorld = RigidBodyB.CenterOfMassTransform * _frameInB; - - TransformUtil.CalculateDiffAxisAngle(frameAWorld, frameBWorld, out axis, out angle); - Quaternion diff = new Quaternion(axis, angle); - Matrix diffMat = Matrix.CreateFromQuaternion(diff); - Vector3 xyz; - // this is not perfect, we can first check which axis are limited, and choose a more appropriate order - MatrixToEulerXYZ(diffMat, out xyz); - - // angular - for (int i = 0; i < 3; i++) - { - if (IsLimited(i + 3)) - { - Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform)); - Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform)); - - float jacDiagABInv = 1f / _jacAng[i].Diagonal; - - //velocity error (first order error) - float rel_vel = _jacAng[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA, - RigidBodyB.LinearVelocity, angvelB); - - //positional error (zeroth order error) - Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform); - Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform); - - float rel_pos = _sign[i] * Vector3.Dot(axisA, axisB); - - float lo = -1e30f; - float hi = 1e30f; - - //handle the twist limit - if (_lowerLimit[i + 3] < _upperLimit[i + 3]) - { - //clamp the values - float loLimit = _upperLimit[i + 3] > -3.1415 ? _lowerLimit[i + 3] : -1e30f; - float hiLimit = _upperLimit[i + 3] < 3.1415 ? _upperLimit[i + 3] : 1e30f; - - float projAngle; - - if (i == 0) - projAngle = -2f * xyz.Z; - else if (i == 1) - projAngle = -2f * xyz.Y; - else - projAngle = -2f * xyz.Z; - - if (projAngle < loLimit) - { - hi = 0f; - rel_pos = loLimit - projAngle; - } - else - { - if (projAngle > hiLimit) - { - lo = 0f; - rel_pos = (hiLimit - projAngle); - } - else - { - continue; - } - } - } - - //impulse - - float normalImpulse = -(tau * rel_pos / timeStep + damping * rel_vel) * jacDiagABInv; - float oldNormalImpulse = _accumulatedImpulse[i + 3]; - float sum = oldNormalImpulse + normalImpulse; - _accumulatedImpulse[i + 3] = sum > hi ? 0f : sum < lo ? 0f : sum; - normalImpulse = _accumulatedImpulse[i + 3] - oldNormalImpulse; - - Vector3 axis2 = _sign[i] * Vector3.Cross(axisA, axisB); - Vector3 impulse_vector = axis2 * normalImpulse; - - RigidBodyA.ApplyTorqueImpulse(impulse_vector); - RigidBodyB.ApplyTorqueImpulse(-impulse_vector); - } - } - } - - //MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html - private bool MatrixToEulerXYZ(Matrix mat, out Vector3 xyz) - { - // rot = cy*cz -cy*sz sy - // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx - // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy - xyz = new Vector3(); - - if (MathHelper.GetElement(mat, 2) < 1.0f) - { - if (MathHelper.GetElement(mat, 2) > -1.0f) - { - xyz.X = (float)Math.Atan2(-MathHelper.GetElement(mat, 5), MathHelper.GetElement(mat, 8)); - xyz.Y = (float)Math.Asin(MathHelper.GetElement(mat, 2)); - xyz.Z = (float)Math.Atan2(-MathHelper.GetElement(mat, 1), MathHelper.GetElement(mat, 0)); - return true; - } - else - { - // WARNING. Not unique. XA - ZA = -atan2(r10,r11) - xyz.X = -(float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4)); - xyz.Y = -(float)Math.PI / 2; - xyz.Z = 0.0f; - return false; - } - } - else - { - // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) - xyz.X = (float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4)); - xyz.Y = (float)Math.PI / 2; - xyz.Z = 0.0f; - return false; - } - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX.Dynamics +{ + /// + /// Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space + /// Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked' + /// Work in progress (is still a Hinge actually) + /// + public class Generic6DofConstraint : TypedConstraint + { + private static readonly float[] _sign = { 1.0f, -1.0f, 1.0f }; + private static readonly int[] _axisA = { 1, 0, 0 }; + private static readonly int[] _axisB = { 2, 2, 1 }; + + private JacobianEntry[] _jacLinear = new JacobianEntry[3]; // 3 orthogonal linear constraints + private JacobianEntry[] _jacAng = new JacobianEntry[3]; // 3 orthogonal angular constraints + + private Matrix _frameInA; // the constraint space w.r.t body A + private Matrix _frameInB; // the constraint space w.r.t body B + + private float[] _lowerLimit = new float[6]; // the constraint lower limits + private float[] _upperLimit = new float[6]; // the constraint upper limits + + private float[] _accumulatedImpulse = new float[6]; + + public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, Matrix frameInA, Matrix frameInB) + : base(rbA, rbB) + { + _frameInA = frameInA; + _frameInB = frameInB; + //free means upper < lower, + //locked means upper == lower + //limited means upper > lower + //so start all locked + for (int i = 0; i < 6; ++i) + { + _lowerLimit[i] = 0.0f; + _upperLimit[i] = 0.0f; + _accumulatedImpulse[i] = 0.0f; + } + } + + public Generic6DofConstraint() { } + + public void UpdateRHS(float timeStep) { } + + public float ComputeAngle(int axis) + { + float angle = 0; + + switch (axis) + { + case 0: + { + Vector3 v1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 1), RigidBodyA.CenterOfMassTransform); + Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform); + Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform); + + float s = Vector3.Dot(v1, w2); + float c = Vector3.Dot(v1, v2); + + angle = (float)Math.Atan2(s, c); + break; + } + case 1: + { + Vector3 w1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 2), RigidBodyA.CenterOfMassTransform); + Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform); + Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform); + + float s = Vector3.Dot(w1, u2); + float c = Vector3.Dot(w1, w2); + + angle = (float)Math.Atan2(s, c); + break; + } + case 2: + { + Vector3 u1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 0), RigidBodyA.CenterOfMassTransform); + Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform); + Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform); + + float s = Vector3.Dot(u1, v2); + float c = Vector3.Dot(u1, u2); + + angle = (float)Math.Atan2(s, c); + break; + } + default: BulletDebug.Assert(false); break; + } + + return angle; + } + + public void SetLinearLowerLimit(Vector3 linearLower) + { + _lowerLimit[0] = linearLower.X; + _lowerLimit[1] = linearLower.Y; + _lowerLimit[2] = linearLower.Z; + } + + public void SetLinearUpperLimit(Vector3 linearUpper) + { + _upperLimit[0] = linearUpper.X; + _upperLimit[1] = linearUpper.Y; + _upperLimit[2] = linearUpper.Z; + } + + public void SetAngularLowerLimit(Vector3 angularLower) + { + _lowerLimit[3] = angularLower.X; + _lowerLimit[4] = angularLower.Y; + _lowerLimit[5] = angularLower.Z; + } + + public void SetAngularUpperLimit(Vector3 angularUpper) + { + _upperLimit[3] = angularUpper.X; + _upperLimit[4] = angularUpper.Y; + _upperLimit[5] = angularUpper.Z; + } + + //first 3 are linear, next 3 are angular + public void SetLimit(int axis, float lo, float hi) + { + _lowerLimit[axis] = lo; + _upperLimit[axis] = hi; + } + + //free means upper < lower, + //locked means upper == lower + //limited means upper > lower + //limitIndex: first 3 are linear, next 3 are angular + public bool IsLimited(int limitIndex) + { + return (_upperLimit[limitIndex] >= _lowerLimit[limitIndex]); + } + + public override void BuildJacobian() + { + Vector3 localNormalInA = new Vector3(0, 0, 0); + + Vector3 pivotInA = _frameInA.Translation; + Vector3 pivotInB = _frameInB.Translation; + + Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform); + Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform); + + Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; + Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; + + //linear part + for (int i = 0; i < 3; i++) + { + if (IsLimited(i)) + { + if (i == 0) + localNormalInA = new Vector3(1, 0, 0); + else if (i == 1) + localNormalInA = new Vector3(0, 1, 0); + else + localNormalInA = new Vector3(0, 0, 1); + + Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform); + + // Create linear atom + _jacLinear[i] = new JacobianEntry( + MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), + MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), + MathHelper.Transform(pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, + MathHelper.Transform(pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, + normalWorld, + RigidBodyA.InvInertiaDiagLocal, + RigidBodyA.InverseMass, + RigidBodyB.InvInertiaDiagLocal, + RigidBodyB.InverseMass); + + //optionally disable warmstarting + _accumulatedImpulse[i] = 0f; + + // Apply accumulated impulse + Vector3 impulse_vector = _accumulatedImpulse[i] * normalWorld; + + RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1); + RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2); + } + } + + // angular part + for (int i = 0; i < 3; i++) + { + if (IsLimited(i + 3)) + { + Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform); + Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform); + + Vector3 axis = _sign[i] * Vector3.Cross(axisA, axisB); + + // Create angular atom + _jacAng[i] = new JacobianEntry(axis, + MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), + MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), + RigidBodyA.InvInertiaDiagLocal, + RigidBodyB.InvInertiaDiagLocal); + + _accumulatedImpulse[i + 3] = 0f; + + // Apply accumulated impulse + Vector3 impulse_vector = _accumulatedImpulse[i + 3] * axis; + + RigidBodyA.ApplyTorqueImpulse(impulse_vector); + RigidBodyB.ApplyTorqueImpulse(-impulse_vector); + } + } + } + + public override void SolveConstraint(float timeStep) + { + float tau = 0.1f; + float damping = 1.0f; + + Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform); + Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform); + + Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; + Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; + + Vector3 localNormalInA = new Vector3(); + + // linear + for (int i = 0; i < 3; i++) + { + if (IsLimited(i)) + { + Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform)); + Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform)); + + if (i == 0) + localNormalInA = new Vector3(1, 0, 0); + else if (i == 1) + localNormalInA = new Vector3(0, 1, 0); + else + localNormalInA = new Vector3(0, 0, 1); + + Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform); + + float jacDiagABInv = 1f / _jacLinear[i].Diagonal; + + //velocity error (first order error) + float rel_vel = _jacLinear[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA, + RigidBodyB.LinearVelocity, angvelB); + + //positional error (zeroth order error) + float depth = -Vector3.Dot(pivotAInW - pivotBInW, normalWorld); + float lo = -1e30f; + float hi = 1e30f; + + //handle the limits + if (_lowerLimit[i] < _upperLimit[i]) + { + if (depth > _upperLimit[i]) + { + depth -= _upperLimit[i]; + lo = 0f; + } + else + { + if (depth < _lowerLimit[i]) + { + depth -= _lowerLimit[i]; + hi = 0f; + } + else + { + continue; + } + } + } + + float normalImpulse = (tau * depth / timeStep - damping * rel_vel) * jacDiagABInv; + float oldNormalImpulse = _accumulatedImpulse[i]; + float sum = oldNormalImpulse + normalImpulse; + _accumulatedImpulse[i] = sum > hi ? 0f : sum < lo ? 0f : sum; + normalImpulse = _accumulatedImpulse[i] - oldNormalImpulse; + + Vector3 impulse_vector = normalWorld * normalImpulse; + RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1); + RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2); + } + } + + Vector3 axis; + float angle; + Matrix frameAWorld = RigidBodyA.CenterOfMassTransform * _frameInA; + Matrix frameBWorld = RigidBodyB.CenterOfMassTransform * _frameInB; + + TransformUtil.CalculateDiffAxisAngle(frameAWorld, frameBWorld, out axis, out angle); + Quaternion diff = new Quaternion(axis, angle); + Matrix diffMat = Matrix.CreateFromQuaternion(diff); + Vector3 xyz; + // this is not perfect, we can first check which axis are limited, and choose a more appropriate order + MatrixToEulerXYZ(diffMat, out xyz); + + // angular + for (int i = 0; i < 3; i++) + { + if (IsLimited(i + 3)) + { + Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform)); + Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform)); + + float jacDiagABInv = 1f / _jacAng[i].Diagonal; + + //velocity error (first order error) + float rel_vel = _jacAng[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA, + RigidBodyB.LinearVelocity, angvelB); + + //positional error (zeroth order error) + Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform); + Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform); + + float rel_pos = _sign[i] * Vector3.Dot(axisA, axisB); + + float lo = -1e30f; + float hi = 1e30f; + + //handle the twist limit + if (_lowerLimit[i + 3] < _upperLimit[i + 3]) + { + //clamp the values + float loLimit = _upperLimit[i + 3] > -3.1415 ? _lowerLimit[i + 3] : -1e30f; + float hiLimit = _upperLimit[i + 3] < 3.1415 ? _upperLimit[i + 3] : 1e30f; + + float projAngle; + + if (i == 0) + projAngle = -2f * xyz.Z; + else if (i == 1) + projAngle = -2f * xyz.Y; + else + projAngle = -2f * xyz.Z; + + if (projAngle < loLimit) + { + hi = 0f; + rel_pos = loLimit - projAngle; + } + else + { + if (projAngle > hiLimit) + { + lo = 0f; + rel_pos = (hiLimit - projAngle); + } + else + { + continue; + } + } + } + + //impulse + + float normalImpulse = -(tau * rel_pos / timeStep + damping * rel_vel) * jacDiagABInv; + float oldNormalImpulse = _accumulatedImpulse[i + 3]; + float sum = oldNormalImpulse + normalImpulse; + _accumulatedImpulse[i + 3] = sum > hi ? 0f : sum < lo ? 0f : sum; + normalImpulse = _accumulatedImpulse[i + 3] - oldNormalImpulse; + + Vector3 axis2 = _sign[i] * Vector3.Cross(axisA, axisB); + Vector3 impulse_vector = axis2 * normalImpulse; + + RigidBodyA.ApplyTorqueImpulse(impulse_vector); + RigidBodyB.ApplyTorqueImpulse(-impulse_vector); + } + } + } + + //MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html + private bool MatrixToEulerXYZ(Matrix mat, out Vector3 xyz) + { + // rot = cy*cz -cy*sz sy + // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx + // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy + xyz = new Vector3(); + + if (MathHelper.GetElement(mat, 2) < 1.0f) + { + if (MathHelper.GetElement(mat, 2) > -1.0f) + { + xyz.X = (float)Math.Atan2(-MathHelper.GetElement(mat, 5), MathHelper.GetElement(mat, 8)); + xyz.Y = (float)Math.Asin(MathHelper.GetElement(mat, 2)); + xyz.Z = (float)Math.Atan2(-MathHelper.GetElement(mat, 1), MathHelper.GetElement(mat, 0)); + return true; + } + else + { + // WARNING. Not unique. XA - ZA = -atan2(r10,r11) + xyz.X = -(float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4)); + xyz.Y = -(float)Math.PI / 2; + xyz.Z = 0.0f; + return false; + } + } + else + { + // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) + xyz.X = (float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4)); + xyz.Y = (float)Math.PI / 2; + xyz.Z = 0.0f; + return false; + } + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs index 3e35550..02bcf8a 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs @@ -1,246 +1,246 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX.Dynamics -{ - /// - /// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space - /// axis defines the orientation of the hinge axis - /// - public class HingeConstraint : TypedConstraint - { - private JacobianEntry[] _jac = new JacobianEntry[3]; //3 orthogonal linear constraints - private JacobianEntry[] _jacAng = new JacobianEntry[3]; //2 orthogonal angular constraints + 1 for limit/motor - - private Vector3 _pivotInA; - private Vector3 _pivotInB; - private Vector3 _axisInA; - private Vector3 _axisInB; - - private bool _angularOnly; - - private float _motorTargetVelocity; - private float _maxMotorImpulse; - private bool _enableAngularMotor; - - public HingeConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB, Vector3 axisInA, Vector3 axisInB) - : base(rbA, rbB) - { - _pivotInA = pivotInA; - _pivotInB = pivotInB; - _axisInA = axisInA; - _axisInB = -axisInB; - _angularOnly = false; - } - - public HingeConstraint(RigidBody rbA, Vector3 pivotInA, Vector3 axisInA) - : base(rbA) - { - _pivotInA = pivotInA; - _pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, pivotInA); - _axisInA = axisInA; - //fixed axis in worldspace - _axisInB = MathHelper.TransformNormal(-axisInA, rbA.CenterOfMassTransform); - _angularOnly = false; - } - - public HingeConstraint() { } - - public bool AngularOnly { set { _angularOnly = value; } } - - public override void BuildJacobian() - { - AppliedImpulse = 0f; - - Vector3 normal = new Vector3(); - - if (!_angularOnly) - { - for (int i = 0; i < 3; i++) - { - MathHelper.SetElement(ref normal, i, 1); - _jac[i] = new JacobianEntry( - MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), - MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), - MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, - MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, - normal, - RigidBodyA.InvInertiaDiagLocal, - RigidBodyA.InverseMass, - RigidBodyB.InvInertiaDiagLocal, - RigidBodyB.InverseMass); - MathHelper.SetElement(ref normal, i, 0); - } - } - - //calculate two perpendicular jointAxis, orthogonal to hingeAxis - //these two jointAxis require equal angular velocities for both bodies - //this is unused for now, it's a todo - Vector3 jointAxisALocal = new Vector3(); - Vector3 jointAxisBLocal = new Vector3(); - MathHelper.PlaneSpace1(_axisInA, ref jointAxisALocal, ref jointAxisBLocal); - - Vector3 jointAxisA = MathHelper.TransformNormal(jointAxisALocal, RigidBodyA.CenterOfMassTransform); - Vector3 jointAxisB = MathHelper.TransformNormal(jointAxisBLocal, RigidBodyA.CenterOfMassTransform); - Vector3 hingeAxisWorld = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform); - - _jacAng[0] = new JacobianEntry(jointAxisA, - MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), - MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), - RigidBodyA.InvInertiaDiagLocal, - RigidBodyB.InvInertiaDiagLocal); - - _jacAng[1] = new JacobianEntry(jointAxisB, - MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), - MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), - RigidBodyA.InvInertiaDiagLocal, - RigidBodyB.InvInertiaDiagLocal); - - _jacAng[2] = new JacobianEntry(hingeAxisWorld, - MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), - MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), - RigidBodyA.InvInertiaDiagLocal, - RigidBodyB.InvInertiaDiagLocal); - } - - public override void SolveConstraint(float timeStep) - { - Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform); - Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform); - - Vector3 normal = new Vector3(0, 0, 0); - float tau = 0.3f; - float damping = 1f; - - //linear part - if (!_angularOnly) - { - for (int i = 0; i < 3; i++) - { - if (i == 0) - normal = new Vector3(1, 0, 0); - else if (i == 1) - normal = new Vector3(0, 1, 0); - else - normal = new Vector3(0, 0, 1); - - float jacDiagABInv = 1f / _jac[i].Diagonal; - - Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; - Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; - - Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1); - Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2); - Vector3 vel = vel1 - vel2; - float rel_vel; - rel_vel = Vector3.Dot(normal, vel); - //positional error (zeroth order error) - float depth = -Vector3.Dot(pivotAInW - pivotBInW, normal); //this is the error projected on the normal - float impulse = depth * tau / timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv * damping; - AppliedImpulse += impulse; - Vector3 impulse_vector = normal * impulse; - RigidBodyA.ApplyImpulse(impulse_vector, pivotAInW - RigidBodyA.CenterOfMassPosition); - RigidBodyB.ApplyImpulse(-impulse_vector, pivotBInW - RigidBodyB.CenterOfMassPosition); - } - } - //solve angular part - // get axes in world space - Vector3 axisA = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform); - Vector3 axisB = MathHelper.TransformNormal(_axisInB, RigidBodyB.CenterOfMassTransform); - - Vector3 angVelA = RigidBodyA.AngularVelocity; - Vector3 angVelB = RigidBodyB.AngularVelocity; - Vector3 angVelAroundHingeAxisA = axisA * Vector3.Dot(axisA, angVelA); - Vector3 angVelAroundHingeAxisB = axisB * Vector3.Dot(axisB, angVelB); - - Vector3 angAOrthog = angVelA - angVelAroundHingeAxisA; - Vector3 angBOrthog = angVelB - angVelAroundHingeAxisB; - Vector3 velrelOrthog = angAOrthog - angBOrthog; - - //solve angular velocity correction - float relaxation = 1f; - float len = velrelOrthog.Length(); - if (len > 0.00001f) - { - Vector3 normal2 = Vector3.Normalize(velrelOrthog); - float denom = RigidBodyA.ComputeAngularImpulseDenominator(normal2) + - RigidBodyB.ComputeAngularImpulseDenominator(normal2); - // scale for mass and relaxation - velrelOrthog *= (1f / denom) * 0.9f; - } - - //solve angular positional correction - Vector3 angularError = -Vector3.Cross(axisA, axisB) * (1f / timeStep); - float len2 = angularError.Length(); - if (len2 > 0.00001f) - { - Vector3 normal2 = Vector3.Normalize(angularError); - float denom2 = RigidBodyA.ComputeAngularImpulseDenominator(normal2) + - RigidBodyB.ComputeAngularImpulseDenominator(normal2); - angularError *= (1f / denom2) * relaxation; - } - - RigidBodyA.ApplyTorqueImpulse(-velrelOrthog + angularError); - RigidBodyB.ApplyTorqueImpulse(velrelOrthog - angularError); - - //apply motor - if (_enableAngularMotor) - { - //todo: add limits too - Vector3 angularLimit = Vector3.Zero; - - Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; - float projRelVel = Vector3.Dot(velrel, axisA); - - float desiredMotorVel = _motorTargetVelocity; - float motorRelvel = desiredMotorVel - projRelVel; - - float denom3 = RigidBodyA.ComputeAngularImpulseDenominator(axisA) + - RigidBodyB.ComputeAngularImpulseDenominator(axisA); - - float unclippedMotorImpulse = (1f / denom3) * motorRelvel; - //todo: should clip against accumulated impulse - float clippedMotorImpulse = unclippedMotorImpulse > _maxMotorImpulse ? _maxMotorImpulse : unclippedMotorImpulse; - clippedMotorImpulse = clippedMotorImpulse < -_maxMotorImpulse ? -_maxMotorImpulse : clippedMotorImpulse; - Vector3 motorImp = clippedMotorImpulse * axisA; - - RigidBodyA.ApplyTorqueImpulse(motorImp + angularLimit); - RigidBodyB.ApplyTorqueImpulse(-motorImp - angularLimit); - } - } - - public void EnableAngularMotor(bool enableMotor, float targetVelocity, float maxMotorImpulse) - { - _enableAngularMotor = enableMotor; - _motorTargetVelocity = targetVelocity; - _maxMotorImpulse = maxMotorImpulse; - } - - public void UpdateRHS(float timeStep) - { - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX.Dynamics +{ + /// + /// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space + /// axis defines the orientation of the hinge axis + /// + public class HingeConstraint : TypedConstraint + { + private JacobianEntry[] _jac = new JacobianEntry[3]; //3 orthogonal linear constraints + private JacobianEntry[] _jacAng = new JacobianEntry[3]; //2 orthogonal angular constraints + 1 for limit/motor + + private Vector3 _pivotInA; + private Vector3 _pivotInB; + private Vector3 _axisInA; + private Vector3 _axisInB; + + private bool _angularOnly; + + private float _motorTargetVelocity; + private float _maxMotorImpulse; + private bool _enableAngularMotor; + + public HingeConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB, Vector3 axisInA, Vector3 axisInB) + : base(rbA, rbB) + { + _pivotInA = pivotInA; + _pivotInB = pivotInB; + _axisInA = axisInA; + _axisInB = -axisInB; + _angularOnly = false; + } + + public HingeConstraint(RigidBody rbA, Vector3 pivotInA, Vector3 axisInA) + : base(rbA) + { + _pivotInA = pivotInA; + _pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, pivotInA); + _axisInA = axisInA; + //fixed axis in worldspace + _axisInB = MathHelper.TransformNormal(-axisInA, rbA.CenterOfMassTransform); + _angularOnly = false; + } + + public HingeConstraint() { } + + public bool AngularOnly { set { _angularOnly = value; } } + + public override void BuildJacobian() + { + AppliedImpulse = 0f; + + Vector3 normal = new Vector3(); + + if (!_angularOnly) + { + for (int i = 0; i < 3; i++) + { + MathHelper.SetElement(ref normal, i, 1); + _jac[i] = new JacobianEntry( + MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), + MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), + MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, + MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, + normal, + RigidBodyA.InvInertiaDiagLocal, + RigidBodyA.InverseMass, + RigidBodyB.InvInertiaDiagLocal, + RigidBodyB.InverseMass); + MathHelper.SetElement(ref normal, i, 0); + } + } + + //calculate two perpendicular jointAxis, orthogonal to hingeAxis + //these two jointAxis require equal angular velocities for both bodies + //this is unused for now, it's a todo + Vector3 jointAxisALocal = new Vector3(); + Vector3 jointAxisBLocal = new Vector3(); + MathHelper.PlaneSpace1(_axisInA, ref jointAxisALocal, ref jointAxisBLocal); + + Vector3 jointAxisA = MathHelper.TransformNormal(jointAxisALocal, RigidBodyA.CenterOfMassTransform); + Vector3 jointAxisB = MathHelper.TransformNormal(jointAxisBLocal, RigidBodyA.CenterOfMassTransform); + Vector3 hingeAxisWorld = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform); + + _jacAng[0] = new JacobianEntry(jointAxisA, + MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), + MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), + RigidBodyA.InvInertiaDiagLocal, + RigidBodyB.InvInertiaDiagLocal); + + _jacAng[1] = new JacobianEntry(jointAxisB, + MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), + MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), + RigidBodyA.InvInertiaDiagLocal, + RigidBodyB.InvInertiaDiagLocal); + + _jacAng[2] = new JacobianEntry(hingeAxisWorld, + MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), + MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), + RigidBodyA.InvInertiaDiagLocal, + RigidBodyB.InvInertiaDiagLocal); + } + + public override void SolveConstraint(float timeStep) + { + Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform); + Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform); + + Vector3 normal = new Vector3(0, 0, 0); + float tau = 0.3f; + float damping = 1f; + + //linear part + if (!_angularOnly) + { + for (int i = 0; i < 3; i++) + { + if (i == 0) + normal = new Vector3(1, 0, 0); + else if (i == 1) + normal = new Vector3(0, 1, 0); + else + normal = new Vector3(0, 0, 1); + + float jacDiagABInv = 1f / _jac[i].Diagonal; + + Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; + Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; + + Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1); + Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2); + Vector3 vel = vel1 - vel2; + float rel_vel; + rel_vel = Vector3.Dot(normal, vel); + //positional error (zeroth order error) + float depth = -Vector3.Dot(pivotAInW - pivotBInW, normal); //this is the error projected on the normal + float impulse = depth * tau / timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv * damping; + AppliedImpulse += impulse; + Vector3 impulse_vector = normal * impulse; + RigidBodyA.ApplyImpulse(impulse_vector, pivotAInW - RigidBodyA.CenterOfMassPosition); + RigidBodyB.ApplyImpulse(-impulse_vector, pivotBInW - RigidBodyB.CenterOfMassPosition); + } + } + //solve angular part + // get axes in world space + Vector3 axisA = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform); + Vector3 axisB = MathHelper.TransformNormal(_axisInB, RigidBodyB.CenterOfMassTransform); + + Vector3 angVelA = RigidBodyA.AngularVelocity; + Vector3 angVelB = RigidBodyB.AngularVelocity; + Vector3 angVelAroundHingeAxisA = axisA * Vector3.Dot(axisA, angVelA); + Vector3 angVelAroundHingeAxisB = axisB * Vector3.Dot(axisB, angVelB); + + Vector3 angAOrthog = angVelA - angVelAroundHingeAxisA; + Vector3 angBOrthog = angVelB - angVelAroundHingeAxisB; + Vector3 velrelOrthog = angAOrthog - angBOrthog; + + //solve angular velocity correction + float relaxation = 1f; + float len = velrelOrthog.Length(); + if (len > 0.00001f) + { + Vector3 normal2 = Vector3.Normalize(velrelOrthog); + float denom = RigidBodyA.ComputeAngularImpulseDenominator(normal2) + + RigidBodyB.ComputeAngularImpulseDenominator(normal2); + // scale for mass and relaxation + velrelOrthog *= (1f / denom) * 0.9f; + } + + //solve angular positional correction + Vector3 angularError = -Vector3.Cross(axisA, axisB) * (1f / timeStep); + float len2 = angularError.Length(); + if (len2 > 0.00001f) + { + Vector3 normal2 = Vector3.Normalize(angularError); + float denom2 = RigidBodyA.ComputeAngularImpulseDenominator(normal2) + + RigidBodyB.ComputeAngularImpulseDenominator(normal2); + angularError *= (1f / denom2) * relaxation; + } + + RigidBodyA.ApplyTorqueImpulse(-velrelOrthog + angularError); + RigidBodyB.ApplyTorqueImpulse(velrelOrthog - angularError); + + //apply motor + if (_enableAngularMotor) + { + //todo: add limits too + Vector3 angularLimit = Vector3.Zero; + + Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; + float projRelVel = Vector3.Dot(velrel, axisA); + + float desiredMotorVel = _motorTargetVelocity; + float motorRelvel = desiredMotorVel - projRelVel; + + float denom3 = RigidBodyA.ComputeAngularImpulseDenominator(axisA) + + RigidBodyB.ComputeAngularImpulseDenominator(axisA); + + float unclippedMotorImpulse = (1f / denom3) * motorRelvel; + //todo: should clip against accumulated impulse + float clippedMotorImpulse = unclippedMotorImpulse > _maxMotorImpulse ? _maxMotorImpulse : unclippedMotorImpulse; + clippedMotorImpulse = clippedMotorImpulse < -_maxMotorImpulse ? -_maxMotorImpulse : clippedMotorImpulse; + Vector3 motorImp = clippedMotorImpulse * axisA; + + RigidBodyA.ApplyTorqueImpulse(motorImp + angularLimit); + RigidBodyB.ApplyTorqueImpulse(-motorImp - angularLimit); + } + } + + public void EnableAngularMotor(bool enableMotor, float targetVelocity, float maxMotorImpulse) + { + _enableAngularMotor = enableMotor; + _motorTargetVelocity = targetVelocity; + _maxMotorImpulse = maxMotorImpulse; + } + + public void UpdateRHS(float timeStep) + { + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/IConstraintSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/IConstraintSolver.cs index de25225..a83eaa5 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/IConstraintSolver.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/IConstraintSolver.cs @@ -1,32 +1,32 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; - -namespace XnaDevRu.BulletX.Dynamics -{ - public interface IConstraintSolver - { - float SolveGroup(List bodies, List manifolds, int numManifolds, List constraints, ContactSolverInfo info, IDebugDraw debugDrawer); - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace XnaDevRu.BulletX.Dynamics +{ + public interface IConstraintSolver + { + float SolveGroup(List bodies, List manifolds, int numManifolds, List constraints, ContactSolverInfo info, IDebugDraw debugDrawer); + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs index 3b8d8d8..6249998 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs @@ -1,155 +1,155 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX.Dynamics -{ - /// - /// Jacobian entry is an abstraction that allows to describe constraints - /// it can be used in combination with a constraint solver - /// Can be used to relate the effect of an impulse to the constraint error - /// - public class JacobianEntry - { - private Vector3 _linearJointAxis; - private Vector3 _aJ; - private Vector3 _bJ; - private Vector3 _0MinvJt; - private Vector3 _1MinvJt; - private float _adiag; - - public JacobianEntry() { } - - //constraint between two different rigidbodies - public JacobianEntry( - Matrix world2A, - Matrix world2B, - Vector3 relPosA, Vector3 relPosB, - Vector3 jointAxis, - Vector3 inertiaInvA, - float massInvA, - Vector3 inertiaInvB, - float massInvB) - { - _linearJointAxis = jointAxis; - _aJ = Vector3.TransformNormal(Vector3.Cross(relPosA, _linearJointAxis), world2A); - _bJ = Vector3.TransformNormal(Vector3.Cross(relPosB, -_linearJointAxis), world2B); - _0MinvJt = inertiaInvA * _aJ; - _1MinvJt = inertiaInvB * _bJ; - _adiag = massInvA + Vector3.Dot(_0MinvJt, _aJ) + massInvB + Vector3.Dot(_1MinvJt, _bJ); - - if (_adiag <= 0.0f) - throw new BulletException(); - } - - //angular constraint between two different rigidbodies - public JacobianEntry(Vector3 jointAxis, - Matrix world2A, - Matrix world2B, - Vector3 inertiaInvA, - Vector3 inertiaInvB) - { - _linearJointAxis = new Vector3(); - _aJ = Vector3.TransformNormal(jointAxis, world2A); - _bJ = Vector3.TransformNormal(-jointAxis, world2B); - _0MinvJt = inertiaInvA * _aJ; - _1MinvJt = inertiaInvB * _bJ; - _adiag = Vector3.Dot(_0MinvJt, _aJ) + Vector3.Dot(_1MinvJt, _bJ); - - if (_adiag <= 0.0f) - throw new BulletException(); - } - - //angular constraint between two different rigidbodies - public JacobianEntry(Vector3 axisInA, - Vector3 axisInB, - Vector3 inertiaInvA, - Vector3 inertiaInvB) - { - _linearJointAxis = new Vector3(); - _aJ = axisInA; - _bJ = -axisInB; - _0MinvJt = inertiaInvA * _aJ; - _1MinvJt = inertiaInvB * _bJ; - _adiag = Vector3.Dot(_0MinvJt, _aJ) + Vector3.Dot(_1MinvJt, _bJ); - - if (_adiag <= 0.0f) - throw new BulletException(); - } - - //constraint on one rigidbody - public JacobianEntry( - Matrix world2A, - Vector3 rel_pos1, Vector3 rel_pos2, - Vector3 jointAxis, - Vector3 inertiaInvA, - float massInvA) - { - _linearJointAxis = jointAxis; - _aJ = Vector3.TransformNormal(Vector3.Cross(rel_pos1, jointAxis), world2A); - _bJ = Vector3.TransformNormal(Vector3.Cross(rel_pos2, -jointAxis), world2A); - _0MinvJt = inertiaInvA * _aJ; - _1MinvJt = new Vector3(); - _adiag = massInvA + Vector3.Dot(_0MinvJt, _aJ); - - if (_adiag <= 0.0f) - throw new BulletException(); - } - - public float Diagonal { get { return _adiag; } } - - // for two constraints on the same rigidbody (for example vehicle friction) - public float GetNonDiagonal(JacobianEntry jacB, float massInvA) - { - float lin = massInvA * Vector3.Dot(_linearJointAxis, jacB._linearJointAxis); - float ang = Vector3.Dot(_0MinvJt, jacB._aJ); - return lin + ang; - } - - // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies) - public float GetNonDiagonal(JacobianEntry jacB, float massInvA, float massInvB) - { - Vector3 lin = _linearJointAxis * jacB._linearJointAxis; - Vector3 ang0 = _0MinvJt * jacB._aJ; - Vector3 ang1 = _1MinvJt * jacB._bJ; - Vector3 lin0 = massInvA * lin; - Vector3 lin1 = massInvB * lin; - Vector3 sum = ang0 + ang1 + lin0 + lin1; - return sum.X + sum.Y + sum.Z; - } - - public float GetRelativeVelocity(Vector3 linvelA, Vector3 angvelA, Vector3 linvelB, Vector3 angvelB) - { - Vector3 linrel = linvelA - linvelB; - Vector3 angvela = angvelA * _aJ; - Vector3 angvelb = angvelB * _bJ; - linrel *= _linearJointAxis; - angvela += angvelb; - angvela += linrel; - float rel_vel2 = angvela.X + angvela.Y + angvela.Z; - return rel_vel2 + float.Epsilon; - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX.Dynamics +{ + /// + /// Jacobian entry is an abstraction that allows to describe constraints + /// it can be used in combination with a constraint solver + /// Can be used to relate the effect of an impulse to the constraint error + /// + public class JacobianEntry + { + private Vector3 _linearJointAxis; + private Vector3 _aJ; + private Vector3 _bJ; + private Vector3 _0MinvJt; + private Vector3 _1MinvJt; + private float _adiag; + + public JacobianEntry() { } + + //constraint between two different rigidbodies + public JacobianEntry( + Matrix world2A, + Matrix world2B, + Vector3 relPosA, Vector3 relPosB, + Vector3 jointAxis, + Vector3 inertiaInvA, + float massInvA, + Vector3 inertiaInvB, + float massInvB) + { + _linearJointAxis = jointAxis; + _aJ = Vector3.TransformNormal(Vector3.Cross(relPosA, _linearJointAxis), world2A); + _bJ = Vector3.TransformNormal(Vector3.Cross(relPosB, -_linearJointAxis), world2B); + _0MinvJt = inertiaInvA * _aJ; + _1MinvJt = inertiaInvB * _bJ; + _adiag = massInvA + Vector3.Dot(_0MinvJt, _aJ) + massInvB + Vector3.Dot(_1MinvJt, _bJ); + + if (_adiag <= 0.0f) + throw new BulletException(); + } + + //angular constraint between two different rigidbodies + public JacobianEntry(Vector3 jointAxis, + Matrix world2A, + Matrix world2B, + Vector3 inertiaInvA, + Vector3 inertiaInvB) + { + _linearJointAxis = new Vector3(); + _aJ = Vector3.TransformNormal(jointAxis, world2A); + _bJ = Vector3.TransformNormal(-jointAxis, world2B); + _0MinvJt = inertiaInvA * _aJ; + _1MinvJt = inertiaInvB * _bJ; + _adiag = Vector3.Dot(_0MinvJt, _aJ) + Vector3.Dot(_1MinvJt, _bJ); + + if (_adiag <= 0.0f) + throw new BulletException(); + } + + //angular constraint between two different rigidbodies + public JacobianEntry(Vector3 axisInA, + Vector3 axisInB, + Vector3 inertiaInvA, + Vector3 inertiaInvB) + { + _linearJointAxis = new Vector3(); + _aJ = axisInA; + _bJ = -axisInB; + _0MinvJt = inertiaInvA * _aJ; + _1MinvJt = inertiaInvB * _bJ; + _adiag = Vector3.Dot(_0MinvJt, _aJ) + Vector3.Dot(_1MinvJt, _bJ); + + if (_adiag <= 0.0f) + throw new BulletException(); + } + + //constraint on one rigidbody + public JacobianEntry( + Matrix world2A, + Vector3 rel_pos1, Vector3 rel_pos2, + Vector3 jointAxis, + Vector3 inertiaInvA, + float massInvA) + { + _linearJointAxis = jointAxis; + _aJ = Vector3.TransformNormal(Vector3.Cross(rel_pos1, jointAxis), world2A); + _bJ = Vector3.TransformNormal(Vector3.Cross(rel_pos2, -jointAxis), world2A); + _0MinvJt = inertiaInvA * _aJ; + _1MinvJt = new Vector3(); + _adiag = massInvA + Vector3.Dot(_0MinvJt, _aJ); + + if (_adiag <= 0.0f) + throw new BulletException(); + } + + public float Diagonal { get { return _adiag; } } + + // for two constraints on the same rigidbody (for example vehicle friction) + public float GetNonDiagonal(JacobianEntry jacB, float massInvA) + { + float lin = massInvA * Vector3.Dot(_linearJointAxis, jacB._linearJointAxis); + float ang = Vector3.Dot(_0MinvJt, jacB._aJ); + return lin + ang; + } + + // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies) + public float GetNonDiagonal(JacobianEntry jacB, float massInvA, float massInvB) + { + Vector3 lin = _linearJointAxis * jacB._linearJointAxis; + Vector3 ang0 = _0MinvJt * jacB._aJ; + Vector3 ang1 = _1MinvJt * jacB._bJ; + Vector3 lin0 = massInvA * lin; + Vector3 lin1 = massInvB * lin; + Vector3 sum = ang0 + ang1 + lin0 + lin1; + return sum.X + sum.Y + sum.Z; + } + + public float GetRelativeVelocity(Vector3 linvelA, Vector3 angvelA, Vector3 linvelB, Vector3 angvelB) + { + Vector3 linrel = linvelA - linvelB; + Vector3 angvela = angvelA * _aJ; + Vector3 angvelb = angvelB * _bJ; + linrel *= _linearJointAxis; + angvela += angvelb; + angvela += linrel; + float rel_vel2 = angvela.X + angvela.Y + angvela.Z; + return rel_vel2 + float.Epsilon; + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs index d4206ea..8ffd3d5 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs @@ -1,157 +1,157 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX.Dynamics -{ - public class ConstraintSetting - { - private float _tau, _damping; - - public ConstraintSetting() - { - _tau = 0.3f; - _damping = 1.0f; - } - - public float Damping - { - get { return _damping; } - set { _damping = value; } - } - - public float Tau - { - get { return _tau; } - set { _tau = value; } - } - } - - public class Point2PointConstraint : TypedConstraint - { - private JacobianEntry[] _jacobian; - private Vector3 _pivotInA, _pivotInB; - - private ConstraintSetting _setting = new ConstraintSetting(); - - public Point2PointConstraint() - { - _jacobian = new JacobianEntry[3]; - } - - public Point2PointConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB) - : base(rbA, rbB) - { - _jacobian = new JacobianEntry[3]; - - _pivotInA = pivotInA; - _pivotInB = pivotInB; - } - - public Point2PointConstraint(RigidBody rbA, Vector3 pivotInA) - : base(rbA) - { - _jacobian = new JacobianEntry[3]; - - _pivotInA = pivotInA; - _pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, _pivotInA); - } - - public ConstraintSetting Settings { get { return _setting; } set { _setting = value; } } - - public Vector3 PivotInA - { - set - { - _pivotInA = value; - } - } - - public Vector3 PivotInB - { - set - { - _pivotInB = value; - } - } - - public override void BuildJacobian() - { - Vector3 normal = new Vector3(); - - for (int i = 0; i < 3; i++) - { - MathHelper.SetElement(ref normal, i, 1); - _jacobian[i] = new JacobianEntry( - MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), - MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), - MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, - MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, - normal, - RigidBodyA.InvInertiaDiagLocal, - RigidBodyA.InverseMass, - RigidBodyB.InvInertiaDiagLocal, - RigidBodyB.InverseMass - ); - MathHelper.SetElement(ref normal, i, 0); - } - } - - public override void SolveConstraint(float timeStep) - { - Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform); - Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform); - - Vector3 normal = new Vector3(); - - for (int i = 0; i < 3; i++) - { - MathHelper.SetElement(ref normal, i, 1); - - float jacDiagABInv = 1.0f / _jacobian[i].Diagonal; - - Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; - Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; - - Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1); - Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2); - - Vector3 vel = vel1 - vel2; - - float rel_vel = Vector3.Dot(normal, vel); - float depth = -Vector3.Dot((pivotAInW - pivotBInW), normal); - - float impulse = depth * _setting.Tau / timeStep * jacDiagABInv - _setting.Damping * rel_vel * jacDiagABInv; - AppliedImpulse += impulse; - Vector3 impulseVector = normal * impulse; - - RigidBodyA.ApplyImpulse(impulseVector, pivotAInW - RigidBodyA.CenterOfMassPosition); - RigidBodyB.ApplyImpulse(-impulseVector, pivotBInW - RigidBodyB.CenterOfMassPosition); - - MathHelper.SetElement(ref normal, i, 0); - } - } - } +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX.Dynamics +{ + public class ConstraintSetting + { + private float _tau, _damping; + + public ConstraintSetting() + { + _tau = 0.3f; + _damping = 1.0f; + } + + public float Damping + { + get { return _damping; } + set { _damping = value; } + } + + public float Tau + { + get { return _tau; } + set { _tau = value; } + } + } + + public class Point2PointConstraint : TypedConstraint + { + private JacobianEntry[] _jacobian; + private Vector3 _pivotInA, _pivotInB; + + private ConstraintSetting _setting = new ConstraintSetting(); + + public Point2PointConstraint() + { + _jacobian = new JacobianEntry[3]; + } + + public Point2PointConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB) + : base(rbA, rbB) + { + _jacobian = new JacobianEntry[3]; + + _pivotInA = pivotInA; + _pivotInB = pivotInB; + } + + public Point2PointConstraint(RigidBody rbA, Vector3 pivotInA) + : base(rbA) + { + _jacobian = new JacobianEntry[3]; + + _pivotInA = pivotInA; + _pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, _pivotInA); + } + + public ConstraintSetting Settings { get { return _setting; } set { _setting = value; } } + + public Vector3 PivotInA + { + set + { + _pivotInA = value; + } + } + + public Vector3 PivotInB + { + set + { + _pivotInB = value; + } + } + + public override void BuildJacobian() + { + Vector3 normal = new Vector3(); + + for (int i = 0; i < 3; i++) + { + MathHelper.SetElement(ref normal, i, 1); + _jacobian[i] = new JacobianEntry( + MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), + MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), + MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, + MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, + normal, + RigidBodyA.InvInertiaDiagLocal, + RigidBodyA.InverseMass, + RigidBodyB.InvInertiaDiagLocal, + RigidBodyB.InverseMass + ); + MathHelper.SetElement(ref normal, i, 0); + } + } + + public override void SolveConstraint(float timeStep) + { + Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform); + Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform); + + Vector3 normal = new Vector3(); + + for (int i = 0; i < 3; i++) + { + MathHelper.SetElement(ref normal, i, 1); + + float jacDiagABInv = 1.0f / _jacobian[i].Diagonal; + + Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; + Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; + + Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1); + Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2); + + Vector3 vel = vel1 - vel2; + + float rel_vel = Vector3.Dot(normal, vel); + float depth = -Vector3.Dot((pivotAInW - pivotBInW), normal); + + float impulse = depth * _setting.Tau / timeStep * jacDiagABInv - _setting.Damping * rel_vel * jacDiagABInv; + AppliedImpulse += impulse; + Vector3 impulseVector = normal * impulse; + + RigidBodyA.ApplyImpulse(impulseVector, pivotAInW - RigidBodyA.CenterOfMassPosition); + RigidBodyB.ApplyImpulse(-impulseVector, pivotBInW - RigidBodyB.CenterOfMassPosition); + + MathHelper.SetElement(ref normal, i, 0); + } + } + } } \ No newline at end of file diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs index 4d5e93d..f275550 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs @@ -1,915 +1,915 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX.Dynamics -{ - [Flags] - public enum SolverMode - { - None = 0, - RandomizeOrder = 1, - FrictionSeperate = 2, - UseWarmstarting = 4, - CacheFriendly = 8, - } - - public class SequentialImpulseConstraintSolver : IConstraintSolver - { - private static int _totalContactPoints = 0; - - private SolverMode _solverMode; - private int _totalCpd = 0; - private ContactSolverFunc[,] _contactDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType]; - private ContactSolverFunc[,] _frictionDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType]; - - private float _penetrationResolveFactor = 0.9f; - private List _tmpSolverBodyPool = new List(); - private List _tmpSolverConstraintPool = new List(); - private List _tmpSolverFrictionConstraintPool = new List(); - - private const int _sequentialImpulseMaxSolverPoints = 16384; - private static OrderIndex[] _order = new OrderIndex[SequentialImpulseMaxSolverPoints]; - private static long _seed2 = 0; - - public SequentialImpulseConstraintSolver() - { - _solverMode = SolverMode.RandomizeOrder | SolverMode.CacheFriendly; - PersistentManifold.ContactDestroyedCallback = MyContactDestroyedCallback; - - //initialize default friction/contact funcs - int i, j; - for (i = 0; i < (int)ContactSolverType.MaxContactSolverType; i++) - for (j = 0; j < (int)ContactSolverType.MaxContactSolverType; j++) - { - - _contactDispatch[i, j] = ContactConstraint.ResolveSingleCollision; - _frictionDispatch[i, j] = ContactConstraint.ResolveSingleFriction; - } - } - - public SolverMode SolverMode { get { return _solverMode; } set { _solverMode = value; } } - public static int SequentialImpulseMaxSolverPoints { get { return _sequentialImpulseMaxSolverPoints; } } - protected static OrderIndex[] Order { get { return _order; } set { _order = value; } } - public static long RandSeed { get { return _seed2; } set { _seed2 = value; } } - - /// - /// Advanced: Override the default contact solving function for contacts, for certain types of rigidbody - /// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType - /// - public void SetContactSolverFunc(ContactSolverFunc func, int typeA, int typeB) - { - _contactDispatch[typeA, typeB] = func; - } - - /// - /// Advanced: Override the default friction solving function for contacts, for certain types of rigidbody - /// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType - /// - public void SetFrictionSolverFunc(ContactSolverFunc func, int typeA, int typeB) - { - _frictionDispatch[typeA, typeB] = func; - } - - protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) - { - float maxImpulse = 0; - - Vector3 color = new Vector3(0, 1, 0); - if (cp.Distance <= 0) - { - if (iter == 0) - if(debugDraw != null) - debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color); - - ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; - float impulse = cpd.ContactSolverFunc( - bodyA, bodyB, - cp, - info); - - if (maxImpulse < impulse) - maxImpulse = impulse; - } - return maxImpulse; - } - - protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter) - { - return Solve(bodyA, bodyB, cp, info, iter, null); - } - - protected float SolveCombinedContactFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) - { - float maxImpulse = 0; - - Vector3 color = new Vector3(0, 1, 0); - if (cp.Distance <= 0) - { - if (iter == 0) - if (debugDraw != null) - debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color); - - float impulse = ContactConstraint.ResolveSingleCollisionCombined( - bodyA, bodyB, - cp, - info); - - if (maxImpulse < impulse) - maxImpulse = impulse; - } - return maxImpulse; - } - - protected float SolveFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) - { - Vector3 color = new Vector3(0, 1, 0); - - if (cp.Distance <= 0) - { - - ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; - cpd.FrictionSolverFunc( - bodyA, bodyB, - cp, - info); - } - return 0; - } - - protected void PrepareConstraints(PersistentManifold manifold, ContactSolverInfo info) - { - RigidBody body0 = manifold.BodyA as RigidBody; - RigidBody body1 = manifold.BodyB as RigidBody; - - - //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop - { - manifold.RefreshContactPoints(body0.CenterOfMassTransform, body1.CenterOfMassTransform); - - int numpoints = manifold.ContactsCount; - - _totalContactPoints += numpoints; - - Vector3 color = new Vector3(0, 1, 0); - for (int i = 0; i < numpoints; i++) - { - ManifoldPoint cp = manifold.GetContactPoint(i); - if (cp.Distance <= 0) - { - Vector3 pos1 = cp.PositionWorldOnA; - Vector3 pos2 = cp.PositionWorldOnB; - - Vector3 rel_pos1 = pos1 - body0.CenterOfMassPosition; - Vector3 rel_pos2 = pos2 - body1.CenterOfMassPosition; - - - //this jacobian entry is re-used for all iterations - JacobianEntry jac = new JacobianEntry(MatrixOperations.Transpose(body0.CenterOfMassTransform), - MatrixOperations.Transpose(body1.CenterOfMassTransform), - rel_pos1, rel_pos2, cp.NormalWorldOnB, body0.InvInertiaDiagLocal, body0.InverseMass, - body1.InvInertiaDiagLocal, body1.InverseMass); - - float jacDiagAB = jac.Diagonal; - - ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; - if (cpd != null) - { - //might be invalid - cpd.PersistentLifeTime++; - if (cpd.PersistentLifeTime != cp.LifeTime) - { - //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime()); - cpd = new ConstraintPersistentData(); - cpd.PersistentLifeTime = cp.LifeTime; - - } - } - else - { - - cpd = new ConstraintPersistentData(); - _totalCpd++; - //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd); - cp.UserPersistentData = cpd; - cpd.PersistentLifeTime = cp.LifeTime; - //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime()); - - } - if (cpd == null) - throw new BulletException(); - - cpd.JacDiagABInv = 1f / jacDiagAB; - - //Dependent on Rigidbody A and B types, fetch the contact/friction response func - //perhaps do a similar thing for friction/restutution combiner funcs... - - cpd.FrictionSolverFunc = _frictionDispatch[(int)body0.FrictionSolverType, (int)body1.FrictionSolverType]; - cpd.ContactSolverFunc = _contactDispatch[(int)body0.ContactSolverType, (int)body1.ContactSolverType]; - - Vector3 vel1 = body0.GetVelocityInLocalPoint(rel_pos1); - Vector3 vel2 = body1.GetVelocityInLocalPoint(rel_pos2); - Vector3 vel = vel1 - vel2; - float rel_vel; - rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel); - - float combinedRestitution = cp.CombinedRestitution; - - cpd.Penetration = cp.Distance; - cpd.Friction = cp.CombinedFriction; - cpd.Restitution = RestitutionCurve(rel_vel, combinedRestitution); - if (cpd.Restitution < 0f) - { - cpd.Restitution = 0.0f; - - }; - - //restitution and penetration work in same direction so - //rel_vel - - float penVel = -cpd.Penetration / info.TimeStep; - - if (cpd.Restitution > penVel) - { - cpd.Penetration = 0; - } - - - float relaxation = info.Damping; - if ((_solverMode & SolverMode.UseWarmstarting) != 0) - { - cpd.AppliedImpulse *= relaxation; - } - else - { - cpd.AppliedImpulse = 0f; - } - - //for friction - cpd.PreviousAppliedImpulse = cpd.AppliedImpulse; - - //re-calculate friction direction every frame, todo: check if this is really needed - Vector3 fwta = cpd.FrictionWorldTangentialA; - Vector3 fwtb = cpd.FrictionWorldTangentialB; - MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref fwta, ref fwtb); - cpd.FrictionWorldTangentialA = fwta; - cpd.FrictionWorldTangentialB = fwtb; - - cpd.AccumulatedTangentImpulseA = 0; - cpd.AccumulatedTangentImpulseB = 0; - float denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialA); - float denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialA); - float denom = relaxation / (denom0 + denom1); - cpd.JacDiagABInvTangentA = denom; - - - denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialB); - denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialB); - denom = relaxation / (denom0 + denom1); - cpd.JacDiagABInvTangentB = denom; - - Vector3 totalImpulse = cp.NormalWorldOnB * cpd.AppliedImpulse; - - { - Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); - cpd.AngularComponentA = Vector3.TransformNormal(torqueAxis0, body0.InvInertiaTensorWorld); - Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); - cpd.AngularComponentB = Vector3.TransformNormal(torqueAxis1, body1.InvInertiaTensorWorld); - } - { - Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialA); - cpd.FrictionAngularComponent0A = Vector3.TransformNormal(ftorqueAxis0, body0.InvInertiaTensorWorld); - } - { - Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialB); - cpd.FrictionAngularComponent1A = Vector3.TransformNormal(ftorqueAxis1, body0.InvInertiaTensorWorld); - } - { - Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialA); - cpd.FrictionAngularComponent0B = Vector3.TransformNormal(ftorqueAxis0, body1.InvInertiaTensorWorld); - } - { - Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialB); - cpd.FrictionAngularComponent1B = Vector3.TransformNormal(ftorqueAxis1, body1.InvInertiaTensorWorld); - } - - - //apply previous frames impulse on both bodies - body0.ApplyImpulse(totalImpulse, rel_pos1); - body1.ApplyImpulse(-totalImpulse, rel_pos2); - } - } - } - } - - private bool MyContactDestroyedCallback(object userPersistentData) - { - if (userPersistentData == null) - throw new BulletException(); - ConstraintPersistentData cpd = userPersistentData as ConstraintPersistentData; - _totalCpd--; - return true; - } - - private float RestitutionCurve(float relVel, float restitution) - { - float rest = restitution * -relVel; - return rest; - } - - //velocity + friction - //response between two dynamic objects with friction - public virtual float ResolveSingleCollisionCombinedCacheFriendly( - SolverBody bodyA, - SolverBody bodyB, - SolverConstraint contactConstraint, - ContactSolverInfo solverInfo) - { - float normalImpulse = 0; - - if (contactConstraint.Penetration < 0) - return 0; - - float relVel; - float velADotn = Vector3.Dot(contactConstraint.ContactNormal,bodyA.LinearVelocity) - + Vector3.Dot(contactConstraint.RelPosACrossNormal,bodyA.AngularVelocity); - float velBDotn = Vector3.Dot(contactConstraint.ContactNormal,bodyB.LinearVelocity) - + Vector3.Dot(contactConstraint.RelPosBCrossNormal,bodyB.AngularVelocity); - - relVel = velADotn - velBDotn; - - float positionalError = contactConstraint.Penetration; - float velocityError = contactConstraint.Restitution - relVel;// * damping; - - float penetrationImpulse = positionalError * contactConstraint.JacDiagABInv; - float velocityImpulse = velocityError * contactConstraint.JacDiagABInv; - normalImpulse = penetrationImpulse + velocityImpulse; - - // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse - float oldNormalImpulse = contactConstraint.AppliedImpulse; - float sum = oldNormalImpulse + normalImpulse; - contactConstraint.AppliedImpulse = 0 > sum ? 0 : sum; - - float oldVelocityImpulse = contactConstraint.AppliedVelocityImpulse; - float velocitySum = oldVelocityImpulse + velocityImpulse; - contactConstraint.AppliedVelocityImpulse = 0 > velocitySum ? 0 : velocitySum; - - normalImpulse = contactConstraint.AppliedImpulse - oldNormalImpulse; - - if (bodyA.InvMass != 0) - { - bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, - contactConstraint.AngularComponentA, normalImpulse); - } - if (bodyB.InvMass != 0) - { - bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, - contactConstraint.AngularComponentB, -normalImpulse); - } - - return normalImpulse; - } - - public virtual float ResolveSingleFrictionCacheFriendly( - SolverBody bodyA, - SolverBody bodyB, - SolverConstraint contactConstraint, - ContactSolverInfo solverInfo, - float appliedNormalImpulse) - { - float combinedFriction = contactConstraint.Friction; - float limit = appliedNormalImpulse * combinedFriction; - - if (appliedNormalImpulse > 0) - //friction - { - float j1; - { - float relVel; - float velADotn = Vector3.Dot(contactConstraint.ContactNormal, bodyA.LinearVelocity) - + Vector3.Dot(contactConstraint.RelPosACrossNormal, bodyA.AngularVelocity); - float velBDotn = Vector3.Dot(contactConstraint.ContactNormal, bodyB.LinearVelocity) - + Vector3.Dot(contactConstraint.RelPosBCrossNormal, bodyB.AngularVelocity); - relVel = velADotn - velBDotn; - - // calculate j that moves us to zero relative velocity - j1 = -relVel * contactConstraint.JacDiagABInv; - float oldTangentImpulse = contactConstraint.AppliedImpulse; - contactConstraint.AppliedImpulse = oldTangentImpulse + j1; - - float test = contactConstraint.AppliedImpulse; - MathHelper.SetMin(ref test, limit); - MathHelper.SetMax(ref test, -limit); - contactConstraint.AppliedImpulse = test; - - j1 = contactConstraint.AppliedImpulse - oldTangentImpulse; - } - - if (bodyA.InvMass != 0) - { - bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, contactConstraint.AngularComponentA, j1); - } - if (bodyB.InvMass != 0) - { - bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, contactConstraint.AngularComponentB, -j1); - } - } - return 0; - } - - public virtual float SolveGroupCacheFriendly(List bodies, List manifolds, int numManifolds, List constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer) - { - if (constraints.Count + numManifolds == 0) - { - return 0; - } - - for (int i = 0; i < numManifolds; i++) - { - PersistentManifold manifold = manifolds[i]; - RigidBody rbA = (RigidBody)manifold.BodyA; - RigidBody rbB = (RigidBody)manifold.BodyB; - - manifold.RefreshContactPoints(rbA.CenterOfMassTransform, rbB.CenterOfMassTransform); - } - - int minReservation = manifolds.Count * 2; - - _tmpSolverBodyPool = new List(minReservation); - - for (int i = 0; i < bodies.Count; i++) - { - RigidBody rb = RigidBody.Upcast(bodies[i]); - if (rb != null && rb.IslandTag >= 0) - { - BulletDebug.Assert(rb.CompanionID < 0); - int solverBodyId = _tmpSolverBodyPool.Count; - SolverBody solverBody; - InitSolverBody(out solverBody, rb); - _tmpSolverBodyPool.Add(solverBody); - rb.CompanionID = solverBodyId; - } - } - - _tmpSolverConstraintPool = new List(minReservation); - _tmpSolverFrictionConstraintPool = new List(minReservation); - - for (int i = 0; i < numManifolds; i++) - { - PersistentManifold manifold = manifolds[i]; - RigidBody rb0 = (RigidBody)manifold.BodyA; - RigidBody rb1 = (RigidBody)manifold.BodyB; - - int solverBodyIdA = -1; - int solverBodyIdB = -1; - - //if (i == 89) - // System.Diagnostics.Debugger.Break(); - - if (manifold.ContactsCount != 0) - { - if (rb0.IslandTag >= 0) - { - solverBodyIdA = rb0.CompanionID; - } - else - { - //create a static body - solverBodyIdA = _tmpSolverBodyPool.Count; - SolverBody solverBody; - InitSolverBody(out solverBody, rb0); - _tmpSolverBodyPool.Add(solverBody); - } - - if (rb1.IslandTag >= 0) - { - solverBodyIdB = rb1.CompanionID; - } - else - { - //create a static body - solverBodyIdB = _tmpSolverBodyPool.Count; - SolverBody solverBody; - InitSolverBody(out solverBody, rb1); - _tmpSolverBodyPool.Add(solverBody); - } - } - - if (solverBodyIdB == -1 || solverBodyIdA == -1) - System.Diagnostics.Debug.WriteLine(string.Format("We're in ass ! {0}", i)); - - for (int j = 0; j < manifold.ContactsCount; j++) - { - ManifoldPoint cp = manifold.GetContactPoint(j); - - int frictionIndex = _tmpSolverConstraintPool.Count; - - if (cp.Distance <= 0) - { - - Vector3 pos1 = cp.PositionWorldOnA; - Vector3 pos2 = cp.PositionWorldOnB; - - Vector3 rel_pos1 = pos1 - rb0.CenterOfMassPosition; - Vector3 rel_pos2 = pos2 - rb1.CenterOfMassPosition; - - float relaxation = 1; - { - SolverConstraint solverConstraint = new SolverConstraint(); - _tmpSolverConstraintPool.Add(solverConstraint); - - solverConstraint.SolverBodyIdA = solverBodyIdA; - solverConstraint.SolverBodyIdB = solverBodyIdB; - solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Contact; - - //can be optimized, the cross products are already calculated - float denom0 = rb0.ComputeImpulseDenominator(pos1, cp.NormalWorldOnB); - float denom1 = rb1.ComputeImpulseDenominator(pos2, cp.NormalWorldOnB); - float denom = relaxation / (denom0 + denom1); - solverConstraint.JacDiagABInv = denom; - - solverConstraint.ContactNormal = cp.NormalWorldOnB; - solverConstraint.RelPosACrossNormal = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); - solverConstraint.RelPosBCrossNormal = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); - - Vector3 vel1 = rb0.GetVelocityInLocalPoint(rel_pos1); - Vector3 vel2 = rb1.GetVelocityInLocalPoint(rel_pos2); - - Vector3 vel = vel1 - vel2; - float rel_vel; - rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel); - - - solverConstraint.Penetration = cp.Distance;//btScalar(infoGlobal.m_numIterations); - solverConstraint.Friction = cp.CombinedFriction; - float rest = RestitutionCurve(rel_vel, cp.CombinedRestitution); - if (rest <= 0) - { - rest = 0; - } - - float penVel = -solverConstraint.Penetration / infoGlobal.TimeStep; - if (rest > penVel) - { - rest = 0; - } - solverConstraint.Restitution = rest; - - solverConstraint.Penetration *= -(infoGlobal.Erp / infoGlobal.TimeStep); - - solverConstraint.AppliedImpulse = 0f; - solverConstraint.AppliedVelocityImpulse = 0f; - -#warning Check to see if we need Vector3.Transform - Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); - solverConstraint.AngularComponentA = Vector3.TransformNormal(torqueAxis0, rb0.InvInertiaTensorWorld); - Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); - solverConstraint.AngularComponentB = Vector3.TransformNormal(torqueAxis1, rb1.InvInertiaTensorWorld); - } - //create 2 '1d axis' constraints for 2 tangential friction directions - - //re-calculate friction direction every frame, todo: check if this is really needed - Vector3 frictionTangential0a = new Vector3(), - frictionTangential1b = new Vector3(); - - MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref frictionTangential0a, ref frictionTangential1b); - { - SolverConstraint solverConstraint = new SolverConstraint(); - _tmpSolverFrictionConstraintPool.Add(solverConstraint); - solverConstraint.ContactNormal = frictionTangential0a; - - solverConstraint.SolverBodyIdA = solverBodyIdA; - solverConstraint.SolverBodyIdB = solverBodyIdB; - solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction; - solverConstraint.FrictionIndex = frictionIndex; - - solverConstraint.Friction = cp.CombinedFriction; - - solverConstraint.AppliedImpulse = 0; - solverConstraint.AppliedVelocityImpulse = 0; - - float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal); - float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal); - float denom = relaxation / (denom0 + denom1); - solverConstraint.JacDiagABInv = denom; - - { - Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal); - solverConstraint.RelPosACrossNormal = ftorqueAxis0; - solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis0, rb0.InvInertiaTensorWorld); - } - { - Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal); - solverConstraint.RelPosBCrossNormal = ftorqueAxis0; - solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis0, rb1.InvInertiaTensorWorld); - } - } - - - { - - SolverConstraint solverConstraint = new SolverConstraint(); - _tmpSolverFrictionConstraintPool.Add(solverConstraint); - solverConstraint.ContactNormal = frictionTangential1b; - - solverConstraint.SolverBodyIdA = solverBodyIdA; - solverConstraint.SolverBodyIdB = solverBodyIdB; - solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction; - solverConstraint.FrictionIndex = frictionIndex; - - solverConstraint.Friction = cp.CombinedFriction; - - solverConstraint.AppliedImpulse = 0; - solverConstraint.AppliedVelocityImpulse = 0; - - float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal); - float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal); - float denom = relaxation / (denom0 + denom1); - solverConstraint.JacDiagABInv = denom; - { - Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal); - solverConstraint.RelPosACrossNormal = ftorqueAxis1; - solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis1, rb0.InvInertiaTensorWorld); - } - { - Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal); - solverConstraint.RelPosBCrossNormal = ftorqueAxis1; - solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis1, rb1.InvInertiaTensorWorld); - } - } - } - } - } - - ContactSolverInfo info = infoGlobal; - { - for (int j = 0; j < constraints.Count; j++) - { - TypedConstraint constraint = constraints[j]; - constraint.BuildJacobian(); - } - } - - int numConstraintPool = _tmpSolverConstraintPool.Count; - int numFrictionPool = _tmpSolverFrictionConstraintPool.Count; - - //todo: use stack allocator for such temporarily memory, same for solver bodies/constraints - List gOrderTmpConstraintPool = new List(numConstraintPool); - List gOrderFrictionConstraintPool = new List(numFrictionPool); - { - for (int i = 0; i < numConstraintPool; i++) - { - gOrderTmpConstraintPool.Add(i); - } - for (int i = 0; i < numFrictionPool; i++) - { - gOrderFrictionConstraintPool.Add(i); - } - } - - //should traverse the contacts random order... - int iteration; - { - for (iteration = 0; iteration < info.IterationsCount; iteration++) - { - - int j; - if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None) - { - if ((iteration & 7) == 0) - { - for (j = 0; j < numConstraintPool; ++j) - { - int tmp = gOrderTmpConstraintPool[j]; - int swapi = RandInt2(j + 1); - gOrderTmpConstraintPool[j] = gOrderTmpConstraintPool[swapi]; - gOrderTmpConstraintPool[swapi] = tmp; - } - - for (j = 0; j < numFrictionPool; ++j) - { - int tmp = gOrderFrictionConstraintPool[j]; - int swapi = RandInt2(j + 1); - gOrderFrictionConstraintPool[j] = gOrderFrictionConstraintPool[swapi]; - gOrderFrictionConstraintPool[swapi] = tmp; - } - } - } - - for (j = 0; j < constraints.Count; j++) - { - TypedConstraint constraint = constraints[j]; - //todo: use solver bodies, so we don't need to copy from/to btRigidBody - - if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0)) - { - _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].WriteBackVelocity(); - } - if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0)) - { - _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].WriteBackVelocity(); - } - - constraint.SolveConstraint(info.TimeStep); - - if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0)) - { - _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].ReadVelocity(); - } - if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0)) - { - _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].ReadVelocity(); - } - - } - - { - int numPoolConstraints = _tmpSolverConstraintPool.Count; - for (j = 0; j < numPoolConstraints; j++) - { - SolverConstraint solveManifold = _tmpSolverConstraintPool[gOrderTmpConstraintPool[j]]; - ResolveSingleCollisionCombinedCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA], - _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info); - } - } - - { - int numFrictionPoolConstraints = _tmpSolverFrictionConstraintPool.Count; - for (j = 0; j < numFrictionPoolConstraints; j++) - { - SolverConstraint solveManifold = _tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]]; - float appliedNormalImpulse = _tmpSolverConstraintPool[solveManifold.FrictionIndex].AppliedImpulse; - - ResolveSingleFrictionCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA], - _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info, appliedNormalImpulse); - } - } - } - } - - for (int i = 0; i < _tmpSolverBodyPool.Count; i++) - { - _tmpSolverBodyPool[i].WriteBackVelocity(); - } - - _tmpSolverBodyPool.Clear(); - _tmpSolverConstraintPool.Clear(); - _tmpSolverFrictionConstraintPool.Clear(); - - return 0; - } - - public virtual float SolveGroup(List bodies, List manifolds, int numManifolds, List constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer) - { - if ((_solverMode & SolverMode.CacheFriendly) != SolverMode.None) - { - return SolveGroupCacheFriendly(bodies, manifolds, numManifolds, constraints, infoGlobal, debugDrawer); - } - - ContactSolverInfo info = infoGlobal; - int totalPoints = 0; - - int numiter = infoGlobal.IterationsCount; - - for (int j = 0; j < manifolds.Count; j++) - { - PersistentManifold manifold = manifolds[j]; - PrepareConstraints(manifold, info); - - for (int p = 0; p < manifolds[j].ContactsCount; p++) - { - _order[totalPoints].ManifoldIndex = j; - _order[totalPoints].PointIndex = p; - totalPoints++; - } - } - - for (int j = 0; j < constraints.Count; j++) - { - constraints[j].BuildJacobian(); - } - - //should traverse the contacts random order... - int iteration; - - for (iteration = 0; iteration < numiter; iteration++) - { - int j; - if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None) - { - if ((iteration & 7) == 0) - { - for (j = 0; j < totalPoints; ++j) - { - OrderIndex tmp = _order[j]; - int swapi = RandInt2(j + 1); - _order[j] = _order[swapi]; - _order[swapi] = tmp; - } - } - } - - for (j = 0; j < constraints.Count; j++) - { - constraints[j].SolveConstraint(info.TimeStep); - } - - for (j = 0; j < totalPoints; j++) - { - PersistentManifold manifold = manifolds[_order[j].ManifoldIndex]; - Solve((RigidBody)manifold.BodyA, (RigidBody)manifold.BodyB, - manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer); - } - - for (j = 0; j < totalPoints; j++) - { - PersistentManifold manifold = manifolds[_order[j].ManifoldIndex]; - SolveFriction((RigidBody)manifold.BodyA, - (RigidBody)manifold.BodyB, manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer); - } - } - - return 0; - } - - private void InitSolverBody(out SolverBody solverBody, RigidBody rigidBody) - { - solverBody = new SolverBody(); - solverBody.AngularVelocity = rigidBody.AngularVelocity; - solverBody.CenterOfMassPosition = rigidBody.CenterOfMassPosition; - solverBody.Friction = rigidBody.Friction; - solverBody.InvMass = rigidBody.InverseMass; - solverBody.LinearVelocity = rigidBody.LinearVelocity; - solverBody.OriginalBody = rigidBody; - solverBody.AngularFactor = rigidBody.AngularFactor; - } - - private long Rand2() - { - _seed2 = (1664525L * _seed2 + 1013904223L) & 0xffffffff; - return _seed2; - } - - private int RandInt2(int n) - { - // seems good; xor-fold and modulus - long un = n; - long r = Rand2(); - - // note: probably more aggressive than it needs to be -- might be - // able to get away without one or two of the innermost branches. - if (un <= 0x00010000L) - { - r ^= (r >> 16); - if (un <= 0x00000100L) - { - r ^= (r >> 8); - if (un <= 0x00000010L) - { - r ^= (r >> 4); - if (un <= 0x00000004L) - { - r ^= (r >> 2); - if (un <= 0x00000002L) - { - r ^= (r >> 1); - } - } - } - } - } - return (int)(r % un); - } - - protected struct OrderIndex - { - private int _manifoldIndex; - private int _pointIndex; - - public int ManifoldIndex { get { return _manifoldIndex; } set { _manifoldIndex = value; } } - public int PointIndex { get { return _pointIndex; } set { _pointIndex = value; } } - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX.Dynamics +{ + [Flags] + public enum SolverMode + { + None = 0, + RandomizeOrder = 1, + FrictionSeperate = 2, + UseWarmstarting = 4, + CacheFriendly = 8, + } + + public class SequentialImpulseConstraintSolver : IConstraintSolver + { + private static int _totalContactPoints = 0; + + private SolverMode _solverMode; + private int _totalCpd = 0; + private ContactSolverFunc[,] _contactDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType]; + private ContactSolverFunc[,] _frictionDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType]; + + private float _penetrationResolveFactor = 0.9f; + private List _tmpSolverBodyPool = new List(); + private List _tmpSolverConstraintPool = new List(); + private List _tmpSolverFrictionConstraintPool = new List(); + + private const int _sequentialImpulseMaxSolverPoints = 16384; + private static OrderIndex[] _order = new OrderIndex[SequentialImpulseMaxSolverPoints]; + private static long _seed2 = 0; + + public SequentialImpulseConstraintSolver() + { + _solverMode = SolverMode.RandomizeOrder | SolverMode.CacheFriendly; + PersistentManifold.ContactDestroyedCallback = MyContactDestroyedCallback; + + //initialize default friction/contact funcs + int i, j; + for (i = 0; i < (int)ContactSolverType.MaxContactSolverType; i++) + for (j = 0; j < (int)ContactSolverType.MaxContactSolverType; j++) + { + + _contactDispatch[i, j] = ContactConstraint.ResolveSingleCollision; + _frictionDispatch[i, j] = ContactConstraint.ResolveSingleFriction; + } + } + + public SolverMode SolverMode { get { return _solverMode; } set { _solverMode = value; } } + public static int SequentialImpulseMaxSolverPoints { get { return _sequentialImpulseMaxSolverPoints; } } + protected static OrderIndex[] Order { get { return _order; } set { _order = value; } } + public static long RandSeed { get { return _seed2; } set { _seed2 = value; } } + + /// + /// Advanced: Override the default contact solving function for contacts, for certain types of rigidbody + /// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType + /// + public void SetContactSolverFunc(ContactSolverFunc func, int typeA, int typeB) + { + _contactDispatch[typeA, typeB] = func; + } + + /// + /// Advanced: Override the default friction solving function for contacts, for certain types of rigidbody + /// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType + /// + public void SetFrictionSolverFunc(ContactSolverFunc func, int typeA, int typeB) + { + _frictionDispatch[typeA, typeB] = func; + } + + protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) + { + float maxImpulse = 0; + + Vector3 color = new Vector3(0, 1, 0); + if (cp.Distance <= 0) + { + if (iter == 0) + if(debugDraw != null) + debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color); + + ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; + float impulse = cpd.ContactSolverFunc( + bodyA, bodyB, + cp, + info); + + if (maxImpulse < impulse) + maxImpulse = impulse; + } + return maxImpulse; + } + + protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter) + { + return Solve(bodyA, bodyB, cp, info, iter, null); + } + + protected float SolveCombinedContactFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) + { + float maxImpulse = 0; + + Vector3 color = new Vector3(0, 1, 0); + if (cp.Distance <= 0) + { + if (iter == 0) + if (debugDraw != null) + debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color); + + float impulse = ContactConstraint.ResolveSingleCollisionCombined( + bodyA, bodyB, + cp, + info); + + if (maxImpulse < impulse) + maxImpulse = impulse; + } + return maxImpulse; + } + + protected float SolveFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) + { + Vector3 color = new Vector3(0, 1, 0); + + if (cp.Distance <= 0) + { + + ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; + cpd.FrictionSolverFunc( + bodyA, bodyB, + cp, + info); + } + return 0; + } + + protected void PrepareConstraints(PersistentManifold manifold, ContactSolverInfo info) + { + RigidBody body0 = manifold.BodyA as RigidBody; + RigidBody body1 = manifold.BodyB as RigidBody; + + + //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop + { + manifold.RefreshContactPoints(body0.CenterOfMassTransform, body1.CenterOfMassTransform); + + int numpoints = manifold.ContactsCount; + + _totalContactPoints += numpoints; + + Vector3 color = new Vector3(0, 1, 0); + for (int i = 0; i < numpoints; i++) + { + ManifoldPoint cp = manifold.GetContactPoint(i); + if (cp.Distance <= 0) + { + Vector3 pos1 = cp.PositionWorldOnA; + Vector3 pos2 = cp.PositionWorldOnB; + + Vector3 rel_pos1 = pos1 - body0.CenterOfMassPosition; + Vector3 rel_pos2 = pos2 - body1.CenterOfMassPosition; + + + //this jacobian entry is re-used for all iterations + JacobianEntry jac = new JacobianEntry(MatrixOperations.Transpose(body0.CenterOfMassTransform), + MatrixOperations.Transpose(body1.CenterOfMassTransform), + rel_pos1, rel_pos2, cp.NormalWorldOnB, body0.InvInertiaDiagLocal, body0.InverseMass, + body1.InvInertiaDiagLocal, body1.InverseMass); + + float jacDiagAB = jac.Diagonal; + + ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; + if (cpd != null) + { + //might be invalid + cpd.PersistentLifeTime++; + if (cpd.PersistentLifeTime != cp.LifeTime) + { + //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime()); + cpd = new ConstraintPersistentData(); + cpd.PersistentLifeTime = cp.LifeTime; + + } + } + else + { + + cpd = new ConstraintPersistentData(); + _totalCpd++; + //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd); + cp.UserPersistentData = cpd; + cpd.PersistentLifeTime = cp.LifeTime; + //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime()); + + } + if (cpd == null) + throw new BulletException(); + + cpd.JacDiagABInv = 1f / jacDiagAB; + + //Dependent on Rigidbody A and B types, fetch the contact/friction response func + //perhaps do a similar thing for friction/restutution combiner funcs... + + cpd.FrictionSolverFunc = _frictionDispatch[(int)body0.FrictionSolverType, (int)body1.FrictionSolverType]; + cpd.ContactSolverFunc = _contactDispatch[(int)body0.ContactSolverType, (int)body1.ContactSolverType]; + + Vector3 vel1 = body0.GetVelocityInLocalPoint(rel_pos1); + Vector3 vel2 = body1.GetVelocityInLocalPoint(rel_pos2); + Vector3 vel = vel1 - vel2; + float rel_vel; + rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel); + + float combinedRestitution = cp.CombinedRestitution; + + cpd.Penetration = cp.Distance; + cpd.Friction = cp.CombinedFriction; + cpd.Restitution = RestitutionCurve(rel_vel, combinedRestitution); + if (cpd.Restitution < 0f) + { + cpd.Restitution = 0.0f; + + }; + + //restitution and penetration work in same direction so + //rel_vel + + float penVel = -cpd.Penetration / info.TimeStep; + + if (cpd.Restitution > penVel) + { + cpd.Penetration = 0; + } + + + float relaxation = info.Damping; + if ((_solverMode & SolverMode.UseWarmstarting) != 0) + { + cpd.AppliedImpulse *= relaxation; + } + else + { + cpd.AppliedImpulse = 0f; + } + + //for friction + cpd.PreviousAppliedImpulse = cpd.AppliedImpulse; + + //re-calculate friction direction every frame, todo: check if this is really needed + Vector3 fwta = cpd.FrictionWorldTangentialA; + Vector3 fwtb = cpd.FrictionWorldTangentialB; + MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref fwta, ref fwtb); + cpd.FrictionWorldTangentialA = fwta; + cpd.FrictionWorldTangentialB = fwtb; + + cpd.AccumulatedTangentImpulseA = 0; + cpd.AccumulatedTangentImpulseB = 0; + float denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialA); + float denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialA); + float denom = relaxation / (denom0 + denom1); + cpd.JacDiagABInvTangentA = denom; + + + denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialB); + denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialB); + denom = relaxation / (denom0 + denom1); + cpd.JacDiagABInvTangentB = denom; + + Vector3 totalImpulse = cp.NormalWorldOnB * cpd.AppliedImpulse; + + { + Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); + cpd.AngularComponentA = Vector3.TransformNormal(torqueAxis0, body0.InvInertiaTensorWorld); + Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); + cpd.AngularComponentB = Vector3.TransformNormal(torqueAxis1, body1.InvInertiaTensorWorld); + } + { + Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialA); + cpd.FrictionAngularComponent0A = Vector3.TransformNormal(ftorqueAxis0, body0.InvInertiaTensorWorld); + } + { + Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialB); + cpd.FrictionAngularComponent1A = Vector3.TransformNormal(ftorqueAxis1, body0.InvInertiaTensorWorld); + } + { + Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialA); + cpd.FrictionAngularComponent0B = Vector3.TransformNormal(ftorqueAxis0, body1.InvInertiaTensorWorld); + } + { + Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialB); + cpd.FrictionAngularComponent1B = Vector3.TransformNormal(ftorqueAxis1, body1.InvInertiaTensorWorld); + } + + + //apply previous frames impulse on both bodies + body0.ApplyImpulse(totalImpulse, rel_pos1); + body1.ApplyImpulse(-totalImpulse, rel_pos2); + } + } + } + } + + private bool MyContactDestroyedCallback(object userPersistentData) + { + if (userPersistentData == null) + throw new BulletException(); + ConstraintPersistentData cpd = userPersistentData as ConstraintPersistentData; + _totalCpd--; + return true; + } + + private float RestitutionCurve(float relVel, float restitution) + { + float rest = restitution * -relVel; + return rest; + } + + //velocity + friction + //response between two dynamic objects with friction + public virtual float ResolveSingleCollisionCombinedCacheFriendly( + SolverBody bodyA, + SolverBody bodyB, + SolverConstraint contactConstraint, + ContactSolverInfo solverInfo) + { + float normalImpulse = 0; + + if (contactConstraint.Penetration < 0) + return 0; + + float relVel; + float velADotn = Vector3.Dot(contactConstraint.ContactNormal,bodyA.LinearVelocity) + + Vector3.Dot(contactConstraint.RelPosACrossNormal,bodyA.AngularVelocity); + float velBDotn = Vector3.Dot(contactConstraint.ContactNormal,bodyB.LinearVelocity) + + Vector3.Dot(contactConstraint.RelPosBCrossNormal,bodyB.AngularVelocity); + + relVel = velADotn - velBDotn; + + float positionalError = contactConstraint.Penetration; + float velocityError = contactConstraint.Restitution - relVel;// * damping; + + float penetrationImpulse = positionalError * contactConstraint.JacDiagABInv; + float velocityImpulse = velocityError * contactConstraint.JacDiagABInv; + normalImpulse = penetrationImpulse + velocityImpulse; + + // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse + float oldNormalImpulse = contactConstraint.AppliedImpulse; + float sum = oldNormalImpulse + normalImpulse; + contactConstraint.AppliedImpulse = 0 > sum ? 0 : sum; + + float oldVelocityImpulse = contactConstraint.AppliedVelocityImpulse; + float velocitySum = oldVelocityImpulse + velocityImpulse; + contactConstraint.AppliedVelocityImpulse = 0 > velocitySum ? 0 : velocitySum; + + normalImpulse = contactConstraint.AppliedImpulse - oldNormalImpulse; + + if (bodyA.InvMass != 0) + { + bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, + contactConstraint.AngularComponentA, normalImpulse); + } + if (bodyB.InvMass != 0) + { + bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, + contactConstraint.AngularComponentB, -normalImpulse); + } + + return normalImpulse; + } + + public virtual float ResolveSingleFrictionCacheFriendly( + SolverBody bodyA, + SolverBody bodyB, + SolverConstraint contactConstraint, + ContactSolverInfo solverInfo, + float appliedNormalImpulse) + { + float combinedFriction = contactConstraint.Friction; + float limit = appliedNormalImpulse * combinedFriction; + + if (appliedNormalImpulse > 0) + //friction + { + float j1; + { + float relVel; + float velADotn = Vector3.Dot(contactConstraint.ContactNormal, bodyA.LinearVelocity) + + Vector3.Dot(contactConstraint.RelPosACrossNormal, bodyA.AngularVelocity); + float velBDotn = Vector3.Dot(contactConstraint.ContactNormal, bodyB.LinearVelocity) + + Vector3.Dot(contactConstraint.RelPosBCrossNormal, bodyB.AngularVelocity); + relVel = velADotn - velBDotn; + + // calculate j that moves us to zero relative velocity + j1 = -relVel * contactConstraint.JacDiagABInv; + float oldTangentImpulse = contactConstraint.AppliedImpulse; + contactConstraint.AppliedImpulse = oldTangentImpulse + j1; + + float test = contactConstraint.AppliedImpulse; + MathHelper.SetMin(ref test, limit); + MathHelper.SetMax(ref test, -limit); + contactConstraint.AppliedImpulse = test; + + j1 = contactConstraint.AppliedImpulse - oldTangentImpulse; + } + + if (bodyA.InvMass != 0) + { + bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, contactConstraint.AngularComponentA, j1); + } + if (bodyB.InvMass != 0) + { + bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, contactConstraint.AngularComponentB, -j1); + } + } + return 0; + } + + public virtual float SolveGroupCacheFriendly(List bodies, List manifolds, int numManifolds, List constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer) + { + if (constraints.Count + numManifolds == 0) + { + return 0; + } + + for (int i = 0; i < numManifolds; i++) + { + PersistentManifold manifold = manifolds[i]; + RigidBody rbA = (RigidBody)manifold.BodyA; + RigidBody rbB = (RigidBody)manifold.BodyB; + + manifold.RefreshContactPoints(rbA.CenterOfMassTransform, rbB.CenterOfMassTransform); + } + + int minReservation = manifolds.Count * 2; + + _tmpSolverBodyPool = new List(minReservation); + + for (int i = 0; i < bodies.Count; i++) + { + RigidBody rb = RigidBody.Upcast(bodies[i]); + if (rb != null && rb.IslandTag >= 0) + { + BulletDebug.Assert(rb.CompanionID < 0); + int solverBodyId = _tmpSolverBodyPool.Count; + SolverBody solverBody; + InitSolverBody(out solverBody, rb); + _tmpSolverBodyPool.Add(solverBody); + rb.CompanionID = solverBodyId; + } + } + + _tmpSolverConstraintPool = new List(minReservation); + _tmpSolverFrictionConstraintPool = new List(minReservation); + + for (int i = 0; i < numManifolds; i++) + { + PersistentManifold manifold = manifolds[i]; + RigidBody rb0 = (RigidBody)manifold.BodyA; + RigidBody rb1 = (RigidBody)manifold.BodyB; + + int solverBodyIdA = -1; + int solverBodyIdB = -1; + + //if (i == 89) + // System.Diagnostics.Debugger.Break(); + + if (manifold.ContactsCount != 0) + { + if (rb0.IslandTag >= 0) + { + solverBodyIdA = rb0.CompanionID; + } + else + { + //create a static body + solverBodyIdA = _tmpSolverBodyPool.Count; + SolverBody solverBody; + InitSolverBody(out solverBody, rb0); + _tmpSolverBodyPool.Add(solverBody); + } + + if (rb1.IslandTag >= 0) + { + solverBodyIdB = rb1.CompanionID; + } + else + { + //create a static body + solverBodyIdB = _tmpSolverBodyPool.Count; + SolverBody solverBody; + InitSolverBody(out solverBody, rb1); + _tmpSolverBodyPool.Add(solverBody); + } + } + + if (solverBodyIdB == -1 || solverBodyIdA == -1) + System.Diagnostics.Debug.WriteLine(string.Format("We're in ass ! {0}", i)); + + for (int j = 0; j < manifold.ContactsCount; j++) + { + ManifoldPoint cp = manifold.GetContactPoint(j); + + int frictionIndex = _tmpSolverConstraintPool.Count; + + if (cp.Distance <= 0) + { + + Vector3 pos1 = cp.PositionWorldOnA; + Vector3 pos2 = cp.PositionWorldOnB; + + Vector3 rel_pos1 = pos1 - rb0.CenterOfMassPosition; + Vector3 rel_pos2 = pos2 - rb1.CenterOfMassPosition; + + float relaxation = 1; + { + SolverConstraint solverConstraint = new SolverConstraint(); + _tmpSolverConstraintPool.Add(solverConstraint); + + solverConstraint.SolverBodyIdA = solverBodyIdA; + solverConstraint.SolverBodyIdB = solverBodyIdB; + solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Contact; + + //can be optimized, the cross products are already calculated + float denom0 = rb0.ComputeImpulseDenominator(pos1, cp.NormalWorldOnB); + float denom1 = rb1.ComputeImpulseDenominator(pos2, cp.NormalWorldOnB); + float denom = relaxation / (denom0 + denom1); + solverConstraint.JacDiagABInv = denom; + + solverConstraint.ContactNormal = cp.NormalWorldOnB; + solverConstraint.RelPosACrossNormal = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); + solverConstraint.RelPosBCrossNormal = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); + + Vector3 vel1 = rb0.GetVelocityInLocalPoint(rel_pos1); + Vector3 vel2 = rb1.GetVelocityInLocalPoint(rel_pos2); + + Vector3 vel = vel1 - vel2; + float rel_vel; + rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel); + + + solverConstraint.Penetration = cp.Distance;//btScalar(infoGlobal.m_numIterations); + solverConstraint.Friction = cp.CombinedFriction; + float rest = RestitutionCurve(rel_vel, cp.CombinedRestitution); + if (rest <= 0) + { + rest = 0; + } + + float penVel = -solverConstraint.Penetration / infoGlobal.TimeStep; + if (rest > penVel) + { + rest = 0; + } + solverConstraint.Restitution = rest; + + solverConstraint.Penetration *= -(infoGlobal.Erp / infoGlobal.TimeStep); + + solverConstraint.AppliedImpulse = 0f; + solverConstraint.AppliedVelocityImpulse = 0f; + +#warning Check to see if we need Vector3.Transform + Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); + solverConstraint.AngularComponentA = Vector3.TransformNormal(torqueAxis0, rb0.InvInertiaTensorWorld); + Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); + solverConstraint.AngularComponentB = Vector3.TransformNormal(torqueAxis1, rb1.InvInertiaTensorWorld); + } + //create 2 '1d axis' constraints for 2 tangential friction directions + + //re-calculate friction direction every frame, todo: check if this is really needed + Vector3 frictionTangential0a = new Vector3(), + frictionTangential1b = new Vector3(); + + MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref frictionTangential0a, ref frictionTangential1b); + { + SolverConstraint solverConstraint = new SolverConstraint(); + _tmpSolverFrictionConstraintPool.Add(solverConstraint); + solverConstraint.ContactNormal = frictionTangential0a; + + solverConstraint.SolverBodyIdA = solverBodyIdA; + solverConstraint.SolverBodyIdB = solverBodyIdB; + solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction; + solverConstraint.FrictionIndex = frictionIndex; + + solverConstraint.Friction = cp.CombinedFriction; + + solverConstraint.AppliedImpulse = 0; + solverConstraint.AppliedVelocityImpulse = 0; + + float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal); + float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal); + float denom = relaxation / (denom0 + denom1); + solverConstraint.JacDiagABInv = denom; + + { + Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal); + solverConstraint.RelPosACrossNormal = ftorqueAxis0; + solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis0, rb0.InvInertiaTensorWorld); + } + { + Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal); + solverConstraint.RelPosBCrossNormal = ftorqueAxis0; + solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis0, rb1.InvInertiaTensorWorld); + } + } + + + { + + SolverConstraint solverConstraint = new SolverConstraint(); + _tmpSolverFrictionConstraintPool.Add(solverConstraint); + solverConstraint.ContactNormal = frictionTangential1b; + + solverConstraint.SolverBodyIdA = solverBodyIdA; + solverConstraint.SolverBodyIdB = solverBodyIdB; + solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction; + solverConstraint.FrictionIndex = frictionIndex; + + solverConstraint.Friction = cp.CombinedFriction; + + solverConstraint.AppliedImpulse = 0; + solverConstraint.AppliedVelocityImpulse = 0; + + float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal); + float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal); + float denom = relaxation / (denom0 + denom1); + solverConstraint.JacDiagABInv = denom; + { + Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal); + solverConstraint.RelPosACrossNormal = ftorqueAxis1; + solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis1, rb0.InvInertiaTensorWorld); + } + { + Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal); + solverConstraint.RelPosBCrossNormal = ftorqueAxis1; + solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis1, rb1.InvInertiaTensorWorld); + } + } + } + } + } + + ContactSolverInfo info = infoGlobal; + { + for (int j = 0; j < constraints.Count; j++) + { + TypedConstraint constraint = constraints[j]; + constraint.BuildJacobian(); + } + } + + int numConstraintPool = _tmpSolverConstraintPool.Count; + int numFrictionPool = _tmpSolverFrictionConstraintPool.Count; + + //todo: use stack allocator for such temporarily memory, same for solver bodies/constraints + List gOrderTmpConstraintPool = new List(numConstraintPool); + List gOrderFrictionConstraintPool = new List(numFrictionPool); + { + for (int i = 0; i < numConstraintPool; i++) + { + gOrderTmpConstraintPool.Add(i); + } + for (int i = 0; i < numFrictionPool; i++) + { + gOrderFrictionConstraintPool.Add(i); + } + } + + //should traverse the contacts random order... + int iteration; + { + for (iteration = 0; iteration < info.IterationsCount; iteration++) + { + + int j; + if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None) + { + if ((iteration & 7) == 0) + { + for (j = 0; j < numConstraintPool; ++j) + { + int tmp = gOrderTmpConstraintPool[j]; + int swapi = RandInt2(j + 1); + gOrderTmpConstraintPool[j] = gOrderTmpConstraintPool[swapi]; + gOrderTmpConstraintPool[swapi] = tmp; + } + + for (j = 0; j < numFrictionPool; ++j) + { + int tmp = gOrderFrictionConstraintPool[j]; + int swapi = RandInt2(j + 1); + gOrderFrictionConstraintPool[j] = gOrderFrictionConstraintPool[swapi]; + gOrderFrictionConstraintPool[swapi] = tmp; + } + } + } + + for (j = 0; j < constraints.Count; j++) + { + TypedConstraint constraint = constraints[j]; + //todo: use solver bodies, so we don't need to copy from/to btRigidBody + + if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0)) + { + _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].WriteBackVelocity(); + } + if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0)) + { + _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].WriteBackVelocity(); + } + + constraint.SolveConstraint(info.TimeStep); + + if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0)) + { + _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].ReadVelocity(); + } + if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0)) + { + _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].ReadVelocity(); + } + + } + + { + int numPoolConstraints = _tmpSolverConstraintPool.Count; + for (j = 0; j < numPoolConstraints; j++) + { + SolverConstraint solveManifold = _tmpSolverConstraintPool[gOrderTmpConstraintPool[j]]; + ResolveSingleCollisionCombinedCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA], + _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info); + } + } + + { + int numFrictionPoolConstraints = _tmpSolverFrictionConstraintPool.Count; + for (j = 0; j < numFrictionPoolConstraints; j++) + { + SolverConstraint solveManifold = _tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]]; + float appliedNormalImpulse = _tmpSolverConstraintPool[solveManifold.FrictionIndex].AppliedImpulse; + + ResolveSingleFrictionCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA], + _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info, appliedNormalImpulse); + } + } + } + } + + for (int i = 0; i < _tmpSolverBodyPool.Count; i++) + { + _tmpSolverBodyPool[i].WriteBackVelocity(); + } + + _tmpSolverBodyPool.Clear(); + _tmpSolverConstraintPool.Clear(); + _tmpSolverFrictionConstraintPool.Clear(); + + return 0; + } + + public virtual float SolveGroup(List bodies, List manifolds, int numManifolds, List constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer) + { + if ((_solverMode & SolverMode.CacheFriendly) != SolverMode.None) + { + return SolveGroupCacheFriendly(bodies, manifolds, numManifolds, constraints, infoGlobal, debugDrawer); + } + + ContactSolverInfo info = infoGlobal; + int totalPoints = 0; + + int numiter = infoGlobal.IterationsCount; + + for (int j = 0; j < manifolds.Count; j++) + { + PersistentManifold manifold = manifolds[j]; + PrepareConstraints(manifold, info); + + for (int p = 0; p < manifolds[j].ContactsCount; p++) + { + _order[totalPoints].ManifoldIndex = j; + _order[totalPoints].PointIndex = p; + totalPoints++; + } + } + + for (int j = 0; j < constraints.Count; j++) + { + constraints[j].BuildJacobian(); + } + + //should traverse the contacts random order... + int iteration; + + for (iteration = 0; iteration < numiter; iteration++) + { + int j; + if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None) + { + if ((iteration & 7) == 0) + { + for (j = 0; j < totalPoints; ++j) + { + OrderIndex tmp = _order[j]; + int swapi = RandInt2(j + 1); + _order[j] = _order[swapi]; + _order[swapi] = tmp; + } + } + } + + for (j = 0; j < constraints.Count; j++) + { + constraints[j].SolveConstraint(info.TimeStep); + } + + for (j = 0; j < totalPoints; j++) + { + PersistentManifold manifold = manifolds[_order[j].ManifoldIndex]; + Solve((RigidBody)manifold.BodyA, (RigidBody)manifold.BodyB, + manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer); + } + + for (j = 0; j < totalPoints; j++) + { + PersistentManifold manifold = manifolds[_order[j].ManifoldIndex]; + SolveFriction((RigidBody)manifold.BodyA, + (RigidBody)manifold.BodyB, manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer); + } + } + + return 0; + } + + private void InitSolverBody(out SolverBody solverBody, RigidBody rigidBody) + { + solverBody = new SolverBody(); + solverBody.AngularVelocity = rigidBody.AngularVelocity; + solverBody.CenterOfMassPosition = rigidBody.CenterOfMassPosition; + solverBody.Friction = rigidBody.Friction; + solverBody.InvMass = rigidBody.InverseMass; + solverBody.LinearVelocity = rigidBody.LinearVelocity; + solverBody.OriginalBody = rigidBody; + solverBody.AngularFactor = rigidBody.AngularFactor; + } + + private long Rand2() + { + _seed2 = (1664525L * _seed2 + 1013904223L) & 0xffffffff; + return _seed2; + } + + private int RandInt2(int n) + { + // seems good; xor-fold and modulus + long un = n; + long r = Rand2(); + + // note: probably more aggressive than it needs to be -- might be + // able to get away without one or two of the innermost branches. + if (un <= 0x00010000L) + { + r ^= (r >> 16); + if (un <= 0x00000100L) + { + r ^= (r >> 8); + if (un <= 0x00000010L) + { + r ^= (r >> 4); + if (un <= 0x00000004L) + { + r ^= (r >> 2); + if (un <= 0x00000002L) + { + r ^= (r >> 1); + } + } + } + } + } + return (int)(r % un); + } + + protected struct OrderIndex + { + private int _manifoldIndex; + private int _pointIndex; + + public int ManifoldIndex { get { return _manifoldIndex; } set { _manifoldIndex = value; } } + public int PointIndex { get { return _pointIndex; } set { _pointIndex = value; } } + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs index 6ced783..9d4060d 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs @@ -1,188 +1,188 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX.Dynamics -{ - /// - /// constraint class used for lateral tyre friction - /// - public class Solve2LinearConstraint - { - private float _tau; - private float _damping; - - public Solve2LinearConstraint(float tau, float damping) - { - _tau = tau; - _damping = damping; - } - - // solve unilateral constraint (equality, direct method) - public void ResolveUnilateralPairConstraint( - RigidBody body1, RigidBody body2, - Matrix world2A, - Matrix world2B, - Vector3 invInertiaADiag, - float invMassA, - Vector3 linvelA, Vector3 angvelA, - Vector3 rel_posA1, - Vector3 invInertiaBDiag, - float invMassB, - Vector3 linvelB, Vector3 angvelB, - Vector3 rel_posA2, - float depthA, Vector3 normalA, - Vector3 rel_posB1, Vector3 rel_posB2, - float depthB, Vector3 normalB, - out float imp0, out float imp1) - { - imp0 = 0; - imp1 = 0; - - float len = Math.Abs(normalA.Length()) - 1f; - if (Math.Abs(len) >= float.Epsilon) - return; - - BulletDebug.Assert(len < float.Epsilon); - - //this jacobian entry could be re-used for all iterations - JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA, - invInertiaBDiag, invMassB); - JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA, - invInertiaBDiag, invMassB); - - float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1)); - float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1)); - - // btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv - float massTerm = 1f / (invMassA + invMassB); - - // calculate rhs (or error) terms - float dv0 = depthA * _tau * massTerm - vel0 * _damping; - float dv1 = depthB * _tau * massTerm - vel1 * _damping; - - float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB); - float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag); - - imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet; - imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet; - } - - // solving 2x2 lcp problem (inequality, direct solution ) - public void ResolveBilateralPairConstraint( - RigidBody body1, RigidBody body2, - Matrix world2A, Matrix world2B, - Vector3 invInertiaADiag, - float invMassA, - Vector3 linvelA, Vector3 angvelA, - Vector3 rel_posA1, - Vector3 invInertiaBDiag, - float invMassB, - Vector3 linvelB, Vector3 angvelB, - Vector3 rel_posA2, - float depthA, Vector3 normalA, - Vector3 rel_posB1, Vector3 rel_posB2, - float depthB, Vector3 normalB, - out float imp0, out float imp1) - { - imp0 = 0f; - imp1 = 0f; - - float len = Math.Abs(normalA.Length()) - 1f; - if (Math.Abs(len) >= float.Epsilon) - return; - - BulletDebug.Assert(len < float.Epsilon); - - JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA, - invInertiaBDiag, invMassB); - JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA, - invInertiaBDiag, invMassB); - - float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1)); - float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1)); - - // calculate rhs (or error) terms - float dv0 = depthA * _tau - vel0 * _damping; - float dv1 = depthB * _tau - vel1 * _damping; - - float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB); - float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag); - - imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet; - imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet; - - if (imp0 > 0.0f) - { - if (imp1 <= 0.0f) - { - imp1 = 0f; - - // now imp0>0 imp1<0 - imp0 = dv0 / jacA.Diagonal; - if (imp0 < 0.0f) - imp0 = 0f; - } - } - else - { - imp0 = 0f; - - imp1 = dv1 / jacB.Diagonal; - if (imp1 <= 0.0f) - { - imp1 = 0f; - // now imp0>0 imp1<0 - imp0 = dv0 / jacA.Diagonal; - if (imp0 > 0.0f) - { - } - else - { - imp0 = 0f; - } - } - } - } - - //public void ResolveAngularConstraint( - // Matrix invInertiaAWS, - // float invMassA, - // Vector3 linvelA, Vector3 angvelA, - // Vector3 rel_posA1, - // Matrix invInertiaBWS, - // float invMassB, - // Vector3 linvelB, Vector3 angvelB, - // Vector3 rel_posA2, - // float depthA, Vector3 normalA, - // Vector3 rel_posB1, Vector3 rel_posB2, - // float depthB, Vector3 normalB, - // out float imp0, out float imp1) - //{ - // imp0 = 0; - // imp1 = 0; - //} - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX.Dynamics +{ + /// + /// constraint class used for lateral tyre friction + /// + public class Solve2LinearConstraint + { + private float _tau; + private float _damping; + + public Solve2LinearConstraint(float tau, float damping) + { + _tau = tau; + _damping = damping; + } + + // solve unilateral constraint (equality, direct method) + public void ResolveUnilateralPairConstraint( + RigidBody body1, RigidBody body2, + Matrix world2A, + Matrix world2B, + Vector3 invInertiaADiag, + float invMassA, + Vector3 linvelA, Vector3 angvelA, + Vector3 rel_posA1, + Vector3 invInertiaBDiag, + float invMassB, + Vector3 linvelB, Vector3 angvelB, + Vector3 rel_posA2, + float depthA, Vector3 normalA, + Vector3 rel_posB1, Vector3 rel_posB2, + float depthB, Vector3 normalB, + out float imp0, out float imp1) + { + imp0 = 0; + imp1 = 0; + + float len = Math.Abs(normalA.Length()) - 1f; + if (Math.Abs(len) >= float.Epsilon) + return; + + BulletDebug.Assert(len < float.Epsilon); + + //this jacobian entry could be re-used for all iterations + JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA, + invInertiaBDiag, invMassB); + JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA, + invInertiaBDiag, invMassB); + + float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1)); + float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1)); + + // btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv + float massTerm = 1f / (invMassA + invMassB); + + // calculate rhs (or error) terms + float dv0 = depthA * _tau * massTerm - vel0 * _damping; + float dv1 = depthB * _tau * massTerm - vel1 * _damping; + + float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB); + float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag); + + imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet; + imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet; + } + + // solving 2x2 lcp problem (inequality, direct solution ) + public void ResolveBilateralPairConstraint( + RigidBody body1, RigidBody body2, + Matrix world2A, Matrix world2B, + Vector3 invInertiaADiag, + float invMassA, + Vector3 linvelA, Vector3 angvelA, + Vector3 rel_posA1, + Vector3 invInertiaBDiag, + float invMassB, + Vector3 linvelB, Vector3 angvelB, + Vector3 rel_posA2, + float depthA, Vector3 normalA, + Vector3 rel_posB1, Vector3 rel_posB2, + float depthB, Vector3 normalB, + out float imp0, out float imp1) + { + imp0 = 0f; + imp1 = 0f; + + float len = Math.Abs(normalA.Length()) - 1f; + if (Math.Abs(len) >= float.Epsilon) + return; + + BulletDebug.Assert(len < float.Epsilon); + + JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA, + invInertiaBDiag, invMassB); + JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA, + invInertiaBDiag, invMassB); + + float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1)); + float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1)); + + // calculate rhs (or error) terms + float dv0 = depthA * _tau - vel0 * _damping; + float dv1 = depthB * _tau - vel1 * _damping; + + float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB); + float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag); + + imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet; + imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet; + + if (imp0 > 0.0f) + { + if (imp1 <= 0.0f) + { + imp1 = 0f; + + // now imp0>0 imp1<0 + imp0 = dv0 / jacA.Diagonal; + if (imp0 < 0.0f) + imp0 = 0f; + } + } + else + { + imp0 = 0f; + + imp1 = dv1 / jacB.Diagonal; + if (imp1 <= 0.0f) + { + imp1 = 0f; + // now imp0>0 imp1<0 + imp0 = dv0 / jacA.Diagonal; + if (imp0 > 0.0f) + { + } + else + { + imp0 = 0f; + } + } + } + } + + //public void ResolveAngularConstraint( + // Matrix invInertiaAWS, + // float invMassA, + // Vector3 linvelA, Vector3 angvelA, + // Vector3 rel_posA1, + // Matrix invInertiaBWS, + // float invMassB, + // Vector3 linvelB, Vector3 angvelB, + // Vector3 rel_posA2, + // float depthA, Vector3 normalA, + // Vector3 rel_posB1, Vector3 rel_posB2, + // float depthB, Vector3 normalB, + // out float imp0, out float imp1) + //{ + // imp0 = 0; + // imp1 = 0; + //} + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverBody.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverBody.cs index 09b73cc..6996272 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverBody.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverBody.cs @@ -1,78 +1,78 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; -using System.Runtime.InteropServices; - -namespace XnaDevRu.BulletX.Dynamics -{ - public class SolverBody - { - private Vector3 _centerOfMassPosition = new Vector3(); - private Vector3 _linearVelocity = new Vector3(); - private Vector3 _angularVelocity = new Vector3(); - private RigidBody _originalBody = null; - private float _invMass; - private float _friction; - private float _angularFactor; - - public Vector3 CenterOfMassPosition { get { return _centerOfMassPosition; } set { _centerOfMassPosition = value; } } - public Vector3 LinearVelocity { get { return _linearVelocity; } set { _linearVelocity = value; } } - public Vector3 AngularVelocity { get { return _angularVelocity; } set { _angularVelocity = value; } } - public RigidBody OriginalBody { get { return _originalBody; } set { _originalBody = value; } } - public float InvMass { get { return _invMass; } set { _invMass = value; } } - public float Friction { get { return _friction; } set { _friction = value; } } - public float AngularFactor { get { return _angularFactor; } set { _angularFactor = value; } } - - public void GetVelocityInLocalPoint(Vector3 relPos, out Vector3 velocity) - { - velocity = _linearVelocity + Vector3.Cross(_angularVelocity, relPos); - } - - public void WriteBackVelocity() - { - if (_invMass != 0) - { - _originalBody.LinearVelocity = _linearVelocity; - _originalBody.AngularVelocity = _angularVelocity; - } - } - - public void ReadVelocity() - { - if (_invMass != 0) - { - _linearVelocity = _originalBody.LinearVelocity; - _angularVelocity = _originalBody.AngularVelocity; - } - } - - //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position - internal void ApplyImpulse(Vector3 linearComponent, Vector3 angularComponent, float impulseMagnitude) - { - _linearVelocity += linearComponent * impulseMagnitude; - _angularVelocity += angularComponent * impulseMagnitude * _angularFactor; - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; +using System.Runtime.InteropServices; + +namespace XnaDevRu.BulletX.Dynamics +{ + public class SolverBody + { + private Vector3 _centerOfMassPosition = new Vector3(); + private Vector3 _linearVelocity = new Vector3(); + private Vector3 _angularVelocity = new Vector3(); + private RigidBody _originalBody = null; + private float _invMass; + private float _friction; + private float _angularFactor; + + public Vector3 CenterOfMassPosition { get { return _centerOfMassPosition; } set { _centerOfMassPosition = value; } } + public Vector3 LinearVelocity { get { return _linearVelocity; } set { _linearVelocity = value; } } + public Vector3 AngularVelocity { get { return _angularVelocity; } set { _angularVelocity = value; } } + public RigidBody OriginalBody { get { return _originalBody; } set { _originalBody = value; } } + public float InvMass { get { return _invMass; } set { _invMass = value; } } + public float Friction { get { return _friction; } set { _friction = value; } } + public float AngularFactor { get { return _angularFactor; } set { _angularFactor = value; } } + + public void GetVelocityInLocalPoint(Vector3 relPos, out Vector3 velocity) + { + velocity = _linearVelocity + Vector3.Cross(_angularVelocity, relPos); + } + + public void WriteBackVelocity() + { + if (_invMass != 0) + { + _originalBody.LinearVelocity = _linearVelocity; + _originalBody.AngularVelocity = _angularVelocity; + } + } + + public void ReadVelocity() + { + if (_invMass != 0) + { + _linearVelocity = _originalBody.LinearVelocity; + _angularVelocity = _originalBody.AngularVelocity; + } + } + + //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position + internal void ApplyImpulse(Vector3 linearComponent, Vector3 angularComponent, float impulseMagnitude) + { + _linearVelocity += linearComponent * impulseMagnitude; + _angularVelocity += angularComponent * impulseMagnitude * _angularFactor; + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverConstraint.cs index 3e3a64a..4fd0361 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverConstraint.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverConstraint.cs @@ -1,77 +1,77 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; -using System.Runtime.InteropServices; - -namespace XnaDevRu.BulletX.Dynamics -{ - //1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. - public class SolverConstraint - { - private Vector3 _relpos1CrossNormal = new Vector3(); - private Vector3 _relpos2CrossNormal = new Vector3(); - private Vector3 _contactNormal = new Vector3(); - private Vector3 _angularComponentA = new Vector3(); - private Vector3 _angularComponentB = new Vector3(); - - private float _appliedVelocityImpulse; - private int _solverBodyIdA; - int _solverBodyIdB; - private float _friction; - private float _restitution; - private float _jacDiagABInv; - private float _penetration; - private float _appliedImpulse; - - private SolverConstraintType _constraintType = SolverConstraintType.Contact; - private int _frictionIndex; - private int[] _unusedPadding = new int[2]; - - public Vector3 RelPosACrossNormal { get { return _relpos1CrossNormal; } set { _relpos1CrossNormal = value; } } - public Vector3 RelPosBCrossNormal { get { return _relpos2CrossNormal; } set { _relpos2CrossNormal = value; } } - public Vector3 ContactNormal { get { return _contactNormal; } set { _contactNormal = value; } } - public Vector3 AngularComponentA { get { return _angularComponentA; } set { _angularComponentA = value; } } - public Vector3 AngularComponentB { get { return _angularComponentB; } set { _angularComponentB = value; } } - - public float AppliedVelocityImpulse { get { return _appliedVelocityImpulse; } set { _appliedVelocityImpulse = value; } } - public int SolverBodyIdA { get { return _solverBodyIdA; } set { _solverBodyIdA = value; } } - public int SolverBodyIdB { get { return _solverBodyIdB; } set { _solverBodyIdB = value; } } - public float Friction { get { return _friction; } set { _friction = value; } } - public float Restitution { get { return _restitution; } set { _restitution = value; } } - public float JacDiagABInv { get { return _jacDiagABInv; } set { _jacDiagABInv = value; } } - public float Penetration { get { return _penetration; } set { _penetration = value; } } - public float AppliedImpulse { get { return _appliedImpulse; } set { _appliedImpulse = value; } } - - public SolverConstraintType ConstraintType { get { return _constraintType; } set { _constraintType = value; } } - public int FrictionIndex { get { return _frictionIndex; } set { _frictionIndex = value; } } - public int[] UnusedPadding { get { return _unusedPadding; } set { _unusedPadding = value; } } - - public enum SolverConstraintType - { - Contact = 0, - Friction, - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; +using System.Runtime.InteropServices; + +namespace XnaDevRu.BulletX.Dynamics +{ + //1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. + public class SolverConstraint + { + private Vector3 _relpos1CrossNormal = new Vector3(); + private Vector3 _relpos2CrossNormal = new Vector3(); + private Vector3 _contactNormal = new Vector3(); + private Vector3 _angularComponentA = new Vector3(); + private Vector3 _angularComponentB = new Vector3(); + + private float _appliedVelocityImpulse; + private int _solverBodyIdA; + int _solverBodyIdB; + private float _friction; + private float _restitution; + private float _jacDiagABInv; + private float _penetration; + private float _appliedImpulse; + + private SolverConstraintType _constraintType = SolverConstraintType.Contact; + private int _frictionIndex; + private int[] _unusedPadding = new int[2]; + + public Vector3 RelPosACrossNormal { get { return _relpos1CrossNormal; } set { _relpos1CrossNormal = value; } } + public Vector3 RelPosBCrossNormal { get { return _relpos2CrossNormal; } set { _relpos2CrossNormal = value; } } + public Vector3 ContactNormal { get { return _contactNormal; } set { _contactNormal = value; } } + public Vector3 AngularComponentA { get { return _angularComponentA; } set { _angularComponentA = value; } } + public Vector3 AngularComponentB { get { return _angularComponentB; } set { _angularComponentB = value; } } + + public float AppliedVelocityImpulse { get { return _appliedVelocityImpulse; } set { _appliedVelocityImpulse = value; } } + public int SolverBodyIdA { get { return _solverBodyIdA; } set { _solverBodyIdA = value; } } + public int SolverBodyIdB { get { return _solverBodyIdB; } set { _solverBodyIdB = value; } } + public float Friction { get { return _friction; } set { _friction = value; } } + public float Restitution { get { return _restitution; } set { _restitution = value; } } + public float JacDiagABInv { get { return _jacDiagABInv; } set { _jacDiagABInv = value; } } + public float Penetration { get { return _penetration; } set { _penetration = value; } } + public float AppliedImpulse { get { return _appliedImpulse; } set { _appliedImpulse = value; } } + + public SolverConstraintType ConstraintType { get { return _constraintType; } set { _constraintType = value; } } + public int FrictionIndex { get { return _frictionIndex; } set { _frictionIndex = value; } } + public int[] UnusedPadding { get { return _unusedPadding; } set { _unusedPadding = value; } } + + public enum SolverConstraintType + { + Contact = 0, + Friction, + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/TypedConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/TypedConstraint.cs index abd10f1..8131463 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/TypedConstraint.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/TypedConstraint.cs @@ -1,88 +1,88 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX.Dynamics -{ - public abstract class TypedConstraint - { - private static RigidBody _fixed = new RigidBody(0, null, null, new Vector3(), 0, 0, 0.5f, 0); - private int _userConstraintType; - private int _userConstraintId; - - private RigidBody _rbA; - private RigidBody _rbB; - private float _appliedImpulse; - - public TypedConstraint() - : this(_fixed, _fixed) { } - - public TypedConstraint(RigidBody rbA) - : this(rbA, _fixed) { } - - public TypedConstraint(RigidBody rbA, RigidBody rbB) - { - _userConstraintType = -1; - _userConstraintId = -1; - _rbA = rbA; - _rbB = rbB; - _appliedImpulse = 0; - - _fixed.SetMassProps(0, new Vector3()); - } - - public virtual RigidBody RigidBodyA { get { return _rbA; } protected set { _rbA = value; } } - public virtual RigidBody RigidBodyB { get { return _rbB; } protected set { _rbB = value; } } - - public float AppliedImpulse { get { return _appliedImpulse; } protected set { _appliedImpulse = value; } } - public int UserConstraintId { get { return _userConstraintId; } set { _userConstraintId = value; } } - public int UserConstraintType { get { return _userConstraintType; } set { _userConstraintType = value; } } - - public abstract void BuildJacobian(); - public abstract void SolveConstraint(float timeStep); - - public static int SortConstraintOnIslandPredicate(TypedConstraint left, TypedConstraint right) - { - int rightIslandID, leftIslandID; - rightIslandID = GetConstraintIslandId(right); - leftIslandID = GetConstraintIslandId(left); - if (leftIslandID < rightIslandID) - return -1; - else - return 1; - return 0; - } - - internal static int GetConstraintIslandId(TypedConstraint lhs) - { - int islandId; - - CollisionObject colObjA = lhs.RigidBodyA; - CollisionObject colObjB = lhs.RigidBodyB; - islandId = colObjA.IslandTag >= 0 ? colObjA.IslandTag : colObjB.IslandTag; - return islandId; - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX.Dynamics +{ + public abstract class TypedConstraint + { + private static RigidBody _fixed = new RigidBody(0, null, null, new Vector3(), 0, 0, 0.5f, 0); + private int _userConstraintType; + private int _userConstraintId; + + private RigidBody _rbA; + private RigidBody _rbB; + private float _appliedImpulse; + + public TypedConstraint() + : this(_fixed, _fixed) { } + + public TypedConstraint(RigidBody rbA) + : this(rbA, _fixed) { } + + public TypedConstraint(RigidBody rbA, RigidBody rbB) + { + _userConstraintType = -1; + _userConstraintId = -1; + _rbA = rbA; + _rbB = rbB; + _appliedImpulse = 0; + + _fixed.SetMassProps(0, new Vector3()); + } + + public virtual RigidBody RigidBodyA { get { return _rbA; } protected set { _rbA = value; } } + public virtual RigidBody RigidBodyB { get { return _rbB; } protected set { _rbB = value; } } + + public float AppliedImpulse { get { return _appliedImpulse; } protected set { _appliedImpulse = value; } } + public int UserConstraintId { get { return _userConstraintId; } set { _userConstraintId = value; } } + public int UserConstraintType { get { return _userConstraintType; } set { _userConstraintType = value; } } + + public abstract void BuildJacobian(); + public abstract void SolveConstraint(float timeStep); + + public static int SortConstraintOnIslandPredicate(TypedConstraint left, TypedConstraint right) + { + int rightIslandID, leftIslandID; + rightIslandID = GetConstraintIslandId(right); + leftIslandID = GetConstraintIslandId(left); + if (leftIslandID < rightIslandID) + return -1; + else + return 1; + return 0; + } + + internal static int GetConstraintIslandId(TypedConstraint lhs) + { + int islandId; + + CollisionObject colObjA = lhs.RigidBodyA; + CollisionObject colObjB = lhs.RigidBodyB; + islandId = colObjA.IslandTag >= 0 ? colObjA.IslandTag : colObjB.IslandTag; + return islandId; + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs index acceb98..3702c2a 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs @@ -1,790 +1,790 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX.Dynamics -{ - /// - /// DiscreteDynamicsWorld provides discrete rigid body simulation - /// those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController - /// - public class DiscreteDynamicsWorld : DynamicsWorld - { - private static bool _reportMe = true; - - private IConstraintSolver _constraintSolver; - private SimulationIslandManager _islandManager; - private List _constraints = new List(); - private IDebugDraw _debugDrawer; - private ContactSolverInfo _solverInfo = new ContactSolverInfo(); - private Vector3 _gravity; - //for variable timesteps - private float _localTime; - //for variable timesteps - private bool _ownsIslandManager; - private bool _ownsConstraintSolver; - private List _vehicles = new List(); - private int _profileTimings; - - public DiscreteDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache) - : this(dispatcher, pairCache, null) { } - - //this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those - public DiscreteDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache, IConstraintSolver constraintSolver) - : base(dispatcher, pairCache) - { - _constraintSolver = constraintSolver != null ? constraintSolver : new SequentialImpulseConstraintSolver(); - _debugDrawer = null; - _gravity = new Vector3(0, -10, 0); - _localTime = 1f / 60f; - _profileTimings = 0; - _islandManager = new SimulationIslandManager(); - _ownsIslandManager = true; - _ownsConstraintSolver = constraintSolver == null; - } - - public ContactSolverInfo SolverInfo { get { return _solverInfo; } } - public SimulationIslandManager SimulationIslandManager { get { return _islandManager; } } - public CollisionWorld CollisionWorld { get { return this; } } - - public override IDebugDraw DebugDrawer - { - get - { - return _debugDrawer; - } - set - { - _debugDrawer = value; - } - } - - public override Vector3 Gravity - { - set - { - _gravity = value; - for (int i = 0; i < CollisionObjects.Count; i++) - { - CollisionObject colObj = CollisionObjects[i]; - RigidBody body = RigidBody.Upcast(colObj); - if (body != null) - { - body.Gravity = value; - } - } - } - } - - public override IConstraintSolver ConstraintSolver - { - set - { - _ownsConstraintSolver = false; - _constraintSolver = value; - } - } - - public override int ConstraintsCount - { - get - { - return _constraints.Count; - } - } - - //if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's - public override void StepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep) - { - int numSimulationSubSteps = 0; - - if (maxSubSteps != 0) - { - //fixed timestep with interpolation - _localTime += timeStep; - if (_localTime >= fixedTimeStep) - { - numSimulationSubSteps = (int)(_localTime / fixedTimeStep); - _localTime -= numSimulationSubSteps * fixedTimeStep; - } - } - else - { - //variable timestep - fixedTimeStep = timeStep; - _localTime = timeStep; - if (Math.Abs(timeStep) < float.Epsilon) - { - numSimulationSubSteps = 0; - maxSubSteps = 0; - } - else - { - numSimulationSubSteps = 1; - maxSubSteps = 1; - } - } - - //process some debugging flags - if (DebugDrawer != null) - { - RigidBody.DisableDeactivation = (DebugDrawer.DebugMode & DebugDrawModes.NoDeactivation) != 0; - } - if (numSimulationSubSteps != 0) - { - - SaveKinematicState(fixedTimeStep); - - //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt - int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps; - - for (int i = 0; i < clampedSimulationSteps; i++) - { - InternalSingleStepSimulation(fixedTimeStep); - SynchronizeMotionStates(); - } - - } - - SynchronizeMotionStates(); - - //return numSimulationSubSteps; - } - - public void StepSimulation(float timeStep, int maxSubSteps) - { - StepSimulation(timeStep, maxSubSteps, 1f / 60f); - } - - public override void UpdateAabbs() - { - Vector3 colorvec = new Vector3(1, 0, 0); - for (int i = 0; i < CollisionObjects.Count; i++) - { - CollisionObject colObj = CollisionObjects[i]; - RigidBody body = RigidBody.Upcast(colObj); - - if (body != null) - { - // if (body->IsActive() && (!body->IsStatic())) - { - Vector3 minAabb, maxAabb; - colObj.CollisionShape.GetAabb(colObj.WorldTransform, out minAabb, out maxAabb); - OverlappingPairCache bp = BroadphasePairCache; - - //moving objects should be moderately sized, probably something wrong if not - if (colObj.IsStaticObject || ((maxAabb - minAabb).LengthSquared() < 1e12f)) - { - bp.SetAabb(body.Broadphase, minAabb, maxAabb); - } - else - { - //something went wrong, investigate - //this assert is unwanted in 3D modelers (danger of loosing work) - BulletDebug.Assert(false); - body.ActivationState = ActivationState.DisableSimulation; - - if (_reportMe) - { - _reportMe = false; - Console.WriteLine("Overflow in AABB, object removed from simulation \n"); - Console.WriteLine("If you can reproduce this, please email bugs@continuousphysics.com\n"); - Console.WriteLine("Please include above information, your Platform, version of OS.\n"); - Console.WriteLine("Thanks.\n"); - } - } - if (_debugDrawer != null && (_debugDrawer.DebugMode & DebugDrawModes.DrawAabb) != 0) - DrawAabb(_debugDrawer, minAabb, maxAabb, colorvec); - } - } - } - } - - public override void AddConstraint(TypedConstraint constraint) - { - _constraints.Add(constraint); - } - - public override void RemoveConstraint(TypedConstraint constraint) - { - _constraints.Remove(constraint); - } - - public void AddVehicle(RaycastVehicle vehicle) - { - _vehicles.Add(vehicle); - } - - public void RemoveVehicle(RaycastVehicle vehicle) - { - _vehicles.Remove(vehicle); - } - - public override void AddRigidBody(RigidBody body) - { - if (!body.IsStaticOrKinematicObject) - { - body.Gravity = _gravity; - } - - if (body.CollisionShape != null) - { - bool isDynamic = !(body.IsStaticObject || body.IsKinematicObject); - BroadphaseProxy.CollisionFilterGroups collisionFilterGroup = isDynamic ? BroadphaseProxy.CollisionFilterGroups.Default : BroadphaseProxy.CollisionFilterGroups.Static; - BroadphaseProxy.CollisionFilterGroups collisionFilterMask = isDynamic ? BroadphaseProxy.CollisionFilterGroups.All : (BroadphaseProxy.CollisionFilterGroups.All ^ BroadphaseProxy.CollisionFilterGroups.Static); - - AddCollisionObject(body, collisionFilterGroup, collisionFilterMask); - } - } - - public override void RemoveRigidBody(RigidBody body) - { - RemoveCollisionObject(body); - } - - public void DebugDrawObject(Matrix worldTransform, CollisionShape shape, Vector3 color) - { - if (shape.ShapeType == BroadphaseNativeTypes.Compound) - { - CompoundShape compoundShape = shape as CompoundShape; - for (int i = compoundShape.ChildShapeCount - 1; i >= 0; i--) - { - Matrix childTrans = compoundShape.GetChildTransform(i); - CollisionShape colShape = compoundShape.GetChildShape(i); - DebugDrawObject(worldTransform * childTrans, colShape, color); - } - - } - else - { - switch (shape.ShapeType) - { - - case BroadphaseNativeTypes.Sphere: - { - SphereShape sphereShape = shape as SphereShape; - float radius = sphereShape.Margin;//radius doesn't include the margin, so draw with margin - Vector3 start = worldTransform.Translation; - DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(radius, 0, 0), worldTransform), color); - DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(0, radius, 0), worldTransform), color); - DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(0, 0, radius), worldTransform), color); - //drawSphere - break; - } - case BroadphaseNativeTypes.MultiSphere: - case BroadphaseNativeTypes.Cone: - { - ConeShape coneShape = shape as ConeShape; - float radius = coneShape.Radius;//+coneShape->getMargin(); - float height = coneShape.Height;//+coneShape->getMargin(); - Vector3 start = worldTransform.Translation; - DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(radius, 0f, -0.5f * height), worldTransform), color); - DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(-radius, 0f, -0.5f * height), worldTransform), color); - DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(0f, radius, -0.5f * height), worldTransform), color); - DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(0f, -radius, -0.5f * height), worldTransform), color); - break; - } - case BroadphaseNativeTypes.Cylinder: - { - CylinderShape cylinder = shape as CylinderShape; - int upAxis = cylinder.UpAxis; - float radius = cylinder.Radius; - float halfHeight = MathHelper.GetElement(cylinder.HalfExtents, upAxis); - Vector3 start = worldTransform.Translation; - Vector3 offsetHeight = new Vector3(); - MathHelper.SetElement(ref offsetHeight, upAxis, halfHeight); - Vector3 offsetRadius = new Vector3(); - MathHelper.SetElement(ref offsetRadius, (upAxis + 1) % 3, radius); - DebugDrawer.DrawLine(start + Vector3.TransformNormal(offsetHeight + offsetRadius, worldTransform), start + Vector3.TransformNormal(-offsetHeight + offsetRadius, worldTransform), color); - DebugDrawer.DrawLine(start + Vector3.TransformNormal(offsetHeight - offsetRadius, worldTransform), start + Vector3.TransformNormal(-offsetHeight - offsetRadius, worldTransform), color); - break; - } - default: - { - if (shape.ShapeType == BroadphaseNativeTypes.TriangleMesh) - { - TriangleMeshShape concaveMesh = shape as TriangleMeshShape; - //btVector3 aabbMax(1e30f,1e30f,1e30f); - //btVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f); - - //todo pass camera, for some culling - Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); - Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); - - DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color); - concaveMesh.ProcessAllTriangles(drawCallback, aabbMin, aabbMax); - } - - if (shape.ShapeType == BroadphaseNativeTypes.ConvexTriangleMesh) - { - ConvexTriangleMeshShape convexMesh = shape as ConvexTriangleMeshShape; - //todo: pass camera for some culling - Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); - Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); - //DebugDrawcallback drawCallback; - DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color); - convexMesh.getStridingMesh().InternalProcessAllTriangles(drawCallback, aabbMin, aabbMax); - } - - // for polyhedral shapes - if (shape.IsPolyhedral) - { - PolyhedralConvexShape polyshape = shape as PolyhedralConvexShape; - - for (int i = 0; i < polyshape.EdgeCount; i++) - { - Vector3 a, b; - polyshape.GetEdge(i, out a, out b); - a = Vector3.TransformNormal(a, worldTransform); - b = Vector3.TransformNormal(b, worldTransform); - DebugDrawer.DrawLine(a, b, color); - } - } - break; - } - } - } - } - - public override TypedConstraint GetConstraint(int index) - { - return _constraints[index]; - } - - public static void DrawAabb(IDebugDraw debugDrawer, Vector3 from, Vector3 to, Vector3 color) - { - Vector3 halfExtents = (to - from) * 0.5f; - Vector3 center = (to + from) * 0.5f; - - Vector3 edgecoord = new Vector3(1f, 1f, 1f), pa, pb; - for (int i = 0; i < 4; i++) - { - for (int j = 0; j < 3; j++) - { - pa = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y, - edgecoord.Z * halfExtents.Z); - pa += center; - - int othercoord = j % 3; - MathHelper.SetElement(ref edgecoord, othercoord, MathHelper.GetElement(edgecoord, othercoord) * -1f); - pb = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y, - edgecoord.Z * halfExtents.Z); - pb += center; - - debugDrawer.DrawLine(pa, pb, color); - } - edgecoord = new Vector3(-1f, -1f, -1f); - if (i < 3) - MathHelper.SetElement(ref edgecoord, i, MathHelper.GetElement(edgecoord, i) * -1f); - } - } - - protected void PredictUnconstraintMotion(float timeStep) - { - for (int i = 0; i < CollisionObjects.Count; i++) - { - CollisionObject colObj = CollisionObjects[i]; - RigidBody body = RigidBody.Upcast(colObj); - if (body != null) - { - if (!body.IsStaticOrKinematicObject) - { - if (body.IsActive) - { - body.ApplyForces(timeStep); - body.IntegrateVelocities(timeStep); - Matrix temp = body.InterpolationWorldTransform; - body.PredictIntegratedTransform(timeStep, ref temp); - body.InterpolationWorldTransform = temp; - } - } - } - } - } - - protected void IntegrateTransforms(float timeStep) - { - Matrix predictedTrans = new Matrix(); - for (int i = 0; i < CollisionObjects.Count; i++) - { - CollisionObject colObj = CollisionObjects[i]; - RigidBody body = RigidBody.Upcast(colObj); - if (body != null) - { - if (body.IsActive && (!body.IsStaticOrKinematicObject)) - { - body.PredictIntegratedTransform(timeStep, ref predictedTrans); - body.ProceedToTransform(predictedTrans); - } - } - } - } - - protected void CalculateSimulationIslands() - { - SimulationIslandManager.UpdateActivationState(this, Dispatcher); - - for (int i = 0; i < _constraints.Count; i++) - { - TypedConstraint constraint = _constraints[i]; - - RigidBody colObj0 = constraint.RigidBodyA; - RigidBody colObj1 = constraint.RigidBodyB; - - if (((colObj0 != null) && (colObj0.MergesSimulationIslands)) && - ((colObj1 != null) && (colObj1.MergesSimulationIslands))) - { - if (colObj0.IsActive || colObj1.IsActive) - { - - SimulationIslandManager.UnionFind.Unite((colObj0).IslandTag, - (colObj1).IslandTag); - } - } - } - - //Store the island id in each body - SimulationIslandManager.StoreIslandActivationState(this); - } - - //protected void SolveNonContactConstraints(ContactSolverInfo solverInfo) - //{ - // //constraint preparation: building jacobians - // for (int i = 0; i < _constraints.Count; i++) - // { - // TypedConstraint constraint = _constraints[i]; - // constraint.BuildJacobian(); - // } - - // //solve the regular non-contact constraints (point 2 point, hinge, generic d6) - // for (int g = 0; g < solverInfo.IterationsCount; g++) - // { - // for (int i = 0; i < _constraints.Count; i++) - // { - // TypedConstraint constraint = _constraints[i]; - // constraint.SolveConstraint(solverInfo.TimeStep); - // } - // } - //} - - //protected void SolveContactConstraints(ContactSolverInfo solverInfo) - //{ - // InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, _debugDrawer); - - // // solve all the contact points and contact friction - // _islandManager.BuildAndProcessIslands(Dispatcher, CollisionObjects, solverCallback); - //} - - protected void SolveConstraints(ContactSolverInfo solverInfo) - { - //sorted version of all btTypedConstraint, based on islandId - List sortedConstraints = new List(ConstraintsCount); - - for (int i = 0; i < ConstraintsCount; i++) - { - sortedConstraints.Add(_constraints[i]); - } - - sortedConstraints.Sort(new Comparison(TypedConstraint.SortConstraintOnIslandPredicate)); - List constraintsPtr = ConstraintsCount != 0 ? sortedConstraints : new List(); - - InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, constraintsPtr, _debugDrawer); - - // solve all the constraints for this island - _islandManager.BuildAndProcessIslands(CollisionWorld.Dispatcher, CollisionWorld.CollisionObjects, solverCallback); - } - - protected void UpdateActivationState(float timeStep) - { - for (int i = 0; i < CollisionObjects.Count; i++) - { - CollisionObject colObj = CollisionObjects[i]; - RigidBody body = RigidBody.Upcast(colObj); - if (body != null) - { - body.UpdateDeactivation(timeStep); - - if (body.WantsSleeping()) - { - if (body.IsStaticOrKinematicObject) - { - body.ActivationState = ActivationState.IslandSleeping; - } - else - { - if (body.ActivationState == ActivationState.Active) - body.ActivationState = ActivationState.WantsDeactivation; - } - } - else - { - if (body.ActivationState != ActivationState.DisableDeactivation) - body.ActivationState = ActivationState.Active; - } - } - } - } - - protected void UpdateVehicles(float timeStep) - { - for (int i = 0; i < _vehicles.Count; i++) - { - RaycastVehicle vehicle = _vehicles[i]; - vehicle.updateVehicle(timeStep); - } - } - - protected void StartProfiling(float timeStep) { } - - protected virtual void InternalSingleStepSimulation(float timeStep) - { - StartProfiling(timeStep); - - //update aabbs information - UpdateAabbs(); - - //apply gravity, predict motion - PredictUnconstraintMotion(timeStep); - - DispatcherInfo dispatchInfo = DispatchInfo; - dispatchInfo.TimeStep = timeStep; - dispatchInfo.StepCount = 0; - dispatchInfo.DebugDraw = DebugDrawer; - - //perform collision detection - PerformDiscreteCollisionDetection(); - - CalculateSimulationIslands(); - - SolverInfo.TimeStep = timeStep; - - //solve contact and other joint constraints - SolveConstraints(SolverInfo); - - //CallbackTriggers(); - - //integrate transforms - IntegrateTransforms(timeStep); - - //update vehicle simulation - UpdateVehicles(timeStep); - - UpdateActivationState(timeStep); - } - - protected void SynchronizeMotionStates() - { - //debug vehicle wheels - { - //todo: iterate over awake simulation islands! - for (int i = 0; i < CollisionObjects.Count; i++) - { - CollisionObject colObj = CollisionObjects[i]; - if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0) - { - Vector3 color = new Vector3(255f, 255f, 255f); - switch (colObj.ActivationState) - { - case ActivationState.Active: - color = new Vector3(255f, 255f, 255f); break; - case ActivationState.IslandSleeping: - color = new Vector3(0f, 255f, 0f); break; - case ActivationState.WantsDeactivation: - color = new Vector3(0f, 255f, 255f); break; - case ActivationState.DisableDeactivation: - color = new Vector3(255f, 0f, 0f); break; - case ActivationState.DisableSimulation: - color = new Vector3(255f, 255f, 0f); break; - default: - color = new Vector3(255f, 0f, 0f); break; - } - - DebugDrawObject(colObj.WorldTransform, colObj.CollisionShape, color); - } - RigidBody body = RigidBody.Upcast(colObj); - if (body != null && body.MotionState != null && !body.IsStaticOrKinematicObject) - { - //if (body.ActivationState != ActivationState.IslandSleeping) - { - Matrix interpolatedTransform = new Matrix(); - TransformUtil.IntegrateTransform(body.InterpolationWorldTransform, - body.InterpolationLinearVelocity, body.InterpolationAngularVelocity, _localTime, ref interpolatedTransform); - body.MotionState.SetWorldTransform(interpolatedTransform); - } - } - } - } - - if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0) - { - for (int i = 0; i < _vehicles.Count; i++) - { - for (int v = 0; v < _vehicles[i].getNumWheels(); v++) - { - Vector3 wheelColor = new Vector3(0, 255, 255); - if (_vehicles[i].getWheelInfo(v).RaycastInfo.IsInContact) - { - wheelColor = new Vector3(0, 0, 255); - } - else - { - wheelColor = new Vector3(255, 0, 255); - } - - //synchronize the wheels with the (interpolated) chassis worldtransform - _vehicles[i].updateWheelTransform(v, true); - - Vector3 wheelPosWS = _vehicles[i].getWheelInfo(v).WorldTransform.Translation; - - Vector3 axle = new Vector3( - MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 0, _vehicles[i].getRightAxis()), - MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 1, _vehicles[i].getRightAxis()), - MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 2, _vehicles[i].getRightAxis())); - - - //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS - //debug wheels (cylinders) - _debugDrawer.DrawLine(wheelPosWS, wheelPosWS + axle, wheelColor); - _debugDrawer.DrawLine(wheelPosWS, _vehicles[i].getWheelInfo(v).RaycastInfo.ContactPointWS, wheelColor); - } - } - } - } - - protected void SaveKinematicState(float timeStep) - { - for (int i = 0; i < CollisionObjects.Count; i++) - { - CollisionObject colObj = CollisionObjects[i]; - RigidBody body = RigidBody.Upcast(colObj); - if (body != null) - { - if (body.ActivationState != ActivationState.IslandSleeping) - { - if (body.IsKinematicObject) - { - //to calculate velocities next frame - body.SaveKinematicState(timeStep); - } - } - } - } - } - - internal class InplaceSolverIslandCallback : SimulationIslandManager.IIslandCallback - { - private ContactSolverInfo _solverInfo; - private IConstraintSolver _solver; - private IDebugDraw _debugDrawer; - private List _sortedConstraints; - - public InplaceSolverIslandCallback( - ContactSolverInfo solverInfo, - IConstraintSolver solver, - List sortedConstraints, - IDebugDraw debugDrawer) - { - _solverInfo = solverInfo; - _solver = solver; - _sortedConstraints = sortedConstraints; - _debugDrawer = debugDrawer; - } - - public ContactSolverInfo SolverInfo { get { return _solverInfo; } set { _solverInfo = value; } } - public IConstraintSolver Solver { get { return _solver; } set { _solver = value; } } - public List Constraints { get { return _sortedConstraints; } set { _sortedConstraints = value; } } - public IDebugDraw DebugDrawer { get { return _debugDrawer; } set { _debugDrawer = value; } } - - public void ProcessIsland(List bodies, List manifolds, int numManifolds, int islandID) - { - //also add all non-contact constraints/joints for this island - List startConstraint = new List(); - int numCurConstraints = 0; - int startIndex = 0; - int i; - - //find the first constraint for this island - for (i = 0; i < _sortedConstraints.Count; i++) - { - if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID) - { - //startConstraint = &m_sortedConstraints[i]; - startIndex = i; - break; - } - } - //count the number of constraints in this island - for (; i < _sortedConstraints.Count; i++) - { - if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID) - { - numCurConstraints++; - } - } - - for (i = startIndex; i < startIndex + numCurConstraints; i++) - { - startConstraint.Add(_sortedConstraints[i]); - } - - _solver.SolveGroup(bodies, manifolds, numManifolds, startConstraint, _solverInfo, _debugDrawer); - } - } - } - - internal class DebugDrawCallback : ITriangleIndexCallback, ITriangleCallback - { - private IDebugDraw _debugDrawer; - private Vector3 _color; - private Matrix _worldTrans; - - public DebugDrawCallback(IDebugDraw debugDrawer, Matrix worldTrans, Vector3 color) - { - _debugDrawer = debugDrawer; - _worldTrans = worldTrans; - _color = color; - } - - public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) - { - ProcessTriangle(triangle, partId, triangleIndex); - } - - #region ITriangleCallback Members - - public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) - { - Vector3 wv0, wv1, wv2; - wv0 = Vector3.TransformNormal(triangle[0], _worldTrans); - wv1 = Vector3.TransformNormal(triangle[1], _worldTrans); - wv2 = Vector3.TransformNormal(triangle[2], _worldTrans); - _debugDrawer.DrawLine(wv0, wv1, _color); - _debugDrawer.DrawLine(wv1, wv2, _color); - _debugDrawer.DrawLine(wv2, wv0, _color); - } - - #endregion - } +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX.Dynamics +{ + /// + /// DiscreteDynamicsWorld provides discrete rigid body simulation + /// those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController + /// + public class DiscreteDynamicsWorld : DynamicsWorld + { + private static bool _reportMe = true; + + private IConstraintSolver _constraintSolver; + private SimulationIslandManager _islandManager; + private List _constraints = new List(); + private IDebugDraw _debugDrawer; + private ContactSolverInfo _solverInfo = new ContactSolverInfo(); + private Vector3 _gravity; + //for variable timesteps + private float _localTime; + //for variable timesteps + private bool _ownsIslandManager; + private bool _ownsConstraintSolver; + private List _vehicles = new List(); + private int _profileTimings; + + public DiscreteDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache) + : this(dispatcher, pairCache, null) { } + + //this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those + public DiscreteDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache, IConstraintSolver constraintSolver) + : base(dispatcher, pairCache) + { + _constraintSolver = constraintSolver != null ? constraintSolver : new SequentialImpulseConstraintSolver(); + _debugDrawer = null; + _gravity = new Vector3(0, -10, 0); + _localTime = 1f / 60f; + _profileTimings = 0; + _islandManager = new SimulationIslandManager(); + _ownsIslandManager = true; + _ownsConstraintSolver = constraintSolver == null; + } + + public ContactSolverInfo SolverInfo { get { return _solverInfo; } } + public SimulationIslandManager SimulationIslandManager { get { return _islandManager; } } + public CollisionWorld CollisionWorld { get { return this; } } + + public override IDebugDraw DebugDrawer + { + get + { + return _debugDrawer; + } + set + { + _debugDrawer = value; + } + } + + public override Vector3 Gravity + { + set + { + _gravity = value; + for (int i = 0; i < CollisionObjects.Count; i++) + { + CollisionObject colObj = CollisionObjects[i]; + RigidBody body = RigidBody.Upcast(colObj); + if (body != null) + { + body.Gravity = value; + } + } + } + } + + public override IConstraintSolver ConstraintSolver + { + set + { + _ownsConstraintSolver = false; + _constraintSolver = value; + } + } + + public override int ConstraintsCount + { + get + { + return _constraints.Count; + } + } + + //if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's + public override void StepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep) + { + int numSimulationSubSteps = 0; + + if (maxSubSteps != 0) + { + //fixed timestep with interpolation + _localTime += timeStep; + if (_localTime >= fixedTimeStep) + { + numSimulationSubSteps = (int)(_localTime / fixedTimeStep); + _localTime -= numSimulationSubSteps * fixedTimeStep; + } + } + else + { + //variable timestep + fixedTimeStep = timeStep; + _localTime = timeStep; + if (Math.Abs(timeStep) < float.Epsilon) + { + numSimulationSubSteps = 0; + maxSubSteps = 0; + } + else + { + numSimulationSubSteps = 1; + maxSubSteps = 1; + } + } + + //process some debugging flags + if (DebugDrawer != null) + { + RigidBody.DisableDeactivation = (DebugDrawer.DebugMode & DebugDrawModes.NoDeactivation) != 0; + } + if (numSimulationSubSteps != 0) + { + + SaveKinematicState(fixedTimeStep); + + //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt + int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps; + + for (int i = 0; i < clampedSimulationSteps; i++) + { + InternalSingleStepSimulation(fixedTimeStep); + SynchronizeMotionStates(); + } + + } + + SynchronizeMotionStates(); + + //return numSimulationSubSteps; + } + + public void StepSimulation(float timeStep, int maxSubSteps) + { + StepSimulation(timeStep, maxSubSteps, 1f / 60f); + } + + public override void UpdateAabbs() + { + Vector3 colorvec = new Vector3(1, 0, 0); + for (int i = 0; i < CollisionObjects.Count; i++) + { + CollisionObject colObj = CollisionObjects[i]; + RigidBody body = RigidBody.Upcast(colObj); + + if (body != null) + { + // if (body->IsActive() && (!body->IsStatic())) + { + Vector3 minAabb, maxAabb; + colObj.CollisionShape.GetAabb(colObj.WorldTransform, out minAabb, out maxAabb); + OverlappingPairCache bp = BroadphasePairCache; + + //moving objects should be moderately sized, probably something wrong if not + if (colObj.IsStaticObject || ((maxAabb - minAabb).LengthSquared() < 1e12f)) + { + bp.SetAabb(body.Broadphase, minAabb, maxAabb); + } + else + { + //something went wrong, investigate + //this assert is unwanted in 3D modelers (danger of loosing work) + BulletDebug.Assert(false); + body.ActivationState = ActivationState.DisableSimulation; + + if (_reportMe) + { + _reportMe = false; + Console.WriteLine("Overflow in AABB, object removed from simulation \n"); + Console.WriteLine("If you can reproduce this, please email bugs@continuousphysics.com\n"); + Console.WriteLine("Please include above information, your Platform, version of OS.\n"); + Console.WriteLine("Thanks.\n"); + } + } + if (_debugDrawer != null && (_debugDrawer.DebugMode & DebugDrawModes.DrawAabb) != 0) + DrawAabb(_debugDrawer, minAabb, maxAabb, colorvec); + } + } + } + } + + public override void AddConstraint(TypedConstraint constraint) + { + _constraints.Add(constraint); + } + + public override void RemoveConstraint(TypedConstraint constraint) + { + _constraints.Remove(constraint); + } + + public void AddVehicle(RaycastVehicle vehicle) + { + _vehicles.Add(vehicle); + } + + public void RemoveVehicle(RaycastVehicle vehicle) + { + _vehicles.Remove(vehicle); + } + + public override void AddRigidBody(RigidBody body) + { + if (!body.IsStaticOrKinematicObject) + { + body.Gravity = _gravity; + } + + if (body.CollisionShape != null) + { + bool isDynamic = !(body.IsStaticObject || body.IsKinematicObject); + BroadphaseProxy.CollisionFilterGroups collisionFilterGroup = isDynamic ? BroadphaseProxy.CollisionFilterGroups.Default : BroadphaseProxy.CollisionFilterGroups.Static; + BroadphaseProxy.CollisionFilterGroups collisionFilterMask = isDynamic ? BroadphaseProxy.CollisionFilterGroups.All : (BroadphaseProxy.CollisionFilterGroups.All ^ BroadphaseProxy.CollisionFilterGroups.Static); + + AddCollisionObject(body, collisionFilterGroup, collisionFilterMask); + } + } + + public override void RemoveRigidBody(RigidBody body) + { + RemoveCollisionObject(body); + } + + public void DebugDrawObject(Matrix worldTransform, CollisionShape shape, Vector3 color) + { + if (shape.ShapeType == BroadphaseNativeTypes.Compound) + { + CompoundShape compoundShape = shape as CompoundShape; + for (int i = compoundShape.ChildShapeCount - 1; i >= 0; i--) + { + Matrix childTrans = compoundShape.GetChildTransform(i); + CollisionShape colShape = compoundShape.GetChildShape(i); + DebugDrawObject(worldTransform * childTrans, colShape, color); + } + + } + else + { + switch (shape.ShapeType) + { + + case BroadphaseNativeTypes.Sphere: + { + SphereShape sphereShape = shape as SphereShape; + float radius = sphereShape.Margin;//radius doesn't include the margin, so draw with margin + Vector3 start = worldTransform.Translation; + DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(radius, 0, 0), worldTransform), color); + DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(0, radius, 0), worldTransform), color); + DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(0, 0, radius), worldTransform), color); + //drawSphere + break; + } + case BroadphaseNativeTypes.MultiSphere: + case BroadphaseNativeTypes.Cone: + { + ConeShape coneShape = shape as ConeShape; + float radius = coneShape.Radius;//+coneShape->getMargin(); + float height = coneShape.Height;//+coneShape->getMargin(); + Vector3 start = worldTransform.Translation; + DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(radius, 0f, -0.5f * height), worldTransform), color); + DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(-radius, 0f, -0.5f * height), worldTransform), color); + DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(0f, radius, -0.5f * height), worldTransform), color); + DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(0f, -radius, -0.5f * height), worldTransform), color); + break; + } + case BroadphaseNativeTypes.Cylinder: + { + CylinderShape cylinder = shape as CylinderShape; + int upAxis = cylinder.UpAxis; + float radius = cylinder.Radius; + float halfHeight = MathHelper.GetElement(cylinder.HalfExtents, upAxis); + Vector3 start = worldTransform.Translation; + Vector3 offsetHeight = new Vector3(); + MathHelper.SetElement(ref offsetHeight, upAxis, halfHeight); + Vector3 offsetRadius = new Vector3(); + MathHelper.SetElement(ref offsetRadius, (upAxis + 1) % 3, radius); + DebugDrawer.DrawLine(start + Vector3.TransformNormal(offsetHeight + offsetRadius, worldTransform), start + Vector3.TransformNormal(-offsetHeight + offsetRadius, worldTransform), color); + DebugDrawer.DrawLine(start + Vector3.TransformNormal(offsetHeight - offsetRadius, worldTransform), start + Vector3.TransformNormal(-offsetHeight - offsetRadius, worldTransform), color); + break; + } + default: + { + if (shape.ShapeType == BroadphaseNativeTypes.TriangleMesh) + { + TriangleMeshShape concaveMesh = shape as TriangleMeshShape; + //btVector3 aabbMax(1e30f,1e30f,1e30f); + //btVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f); + + //todo pass camera, for some culling + Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); + Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); + + DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color); + concaveMesh.ProcessAllTriangles(drawCallback, aabbMin, aabbMax); + } + + if (shape.ShapeType == BroadphaseNativeTypes.ConvexTriangleMesh) + { + ConvexTriangleMeshShape convexMesh = shape as ConvexTriangleMeshShape; + //todo: pass camera for some culling + Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); + Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); + //DebugDrawcallback drawCallback; + DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color); + convexMesh.getStridingMesh().InternalProcessAllTriangles(drawCallback, aabbMin, aabbMax); + } + + // for polyhedral shapes + if (shape.IsPolyhedral) + { + PolyhedralConvexShape polyshape = shape as PolyhedralConvexShape; + + for (int i = 0; i < polyshape.EdgeCount; i++) + { + Vector3 a, b; + polyshape.GetEdge(i, out a, out b); + a = Vector3.TransformNormal(a, worldTransform); + b = Vector3.TransformNormal(b, worldTransform); + DebugDrawer.DrawLine(a, b, color); + } + } + break; + } + } + } + } + + public override TypedConstraint GetConstraint(int index) + { + return _constraints[index]; + } + + public static void DrawAabb(IDebugDraw debugDrawer, Vector3 from, Vector3 to, Vector3 color) + { + Vector3 halfExtents = (to - from) * 0.5f; + Vector3 center = (to + from) * 0.5f; + + Vector3 edgecoord = new Vector3(1f, 1f, 1f), pa, pb; + for (int i = 0; i < 4; i++) + { + for (int j = 0; j < 3; j++) + { + pa = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y, + edgecoord.Z * halfExtents.Z); + pa += center; + + int othercoord = j % 3; + MathHelper.SetElement(ref edgecoord, othercoord, MathHelper.GetElement(edgecoord, othercoord) * -1f); + pb = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y, + edgecoord.Z * halfExtents.Z); + pb += center; + + debugDrawer.DrawLine(pa, pb, color); + } + edgecoord = new Vector3(-1f, -1f, -1f); + if (i < 3) + MathHelper.SetElement(ref edgecoord, i, MathHelper.GetElement(edgecoord, i) * -1f); + } + } + + protected void PredictUnconstraintMotion(float timeStep) + { + for (int i = 0; i < CollisionObjects.Count; i++) + { + CollisionObject colObj = CollisionObjects[i]; + RigidBody body = RigidBody.Upcast(colObj); + if (body != null) + { + if (!body.IsStaticOrKinematicObject) + { + if (body.IsActive) + { + body.ApplyForces(timeStep); + body.IntegrateVelocities(timeStep); + Matrix temp = body.InterpolationWorldTransform; + body.PredictIntegratedTransform(timeStep, ref temp); + body.InterpolationWorldTransform = temp; + } + } + } + } + } + + protected void IntegrateTransforms(float timeStep) + { + Matrix predictedTrans = new Matrix(); + for (int i = 0; i < CollisionObjects.Count; i++) + { + CollisionObject colObj = CollisionObjects[i]; + RigidBody body = RigidBody.Upcast(colObj); + if (body != null) + { + if (body.IsActive && (!body.IsStaticOrKinematicObject)) + { + body.PredictIntegratedTransform(timeStep, ref predictedTrans); + body.ProceedToTransform(predictedTrans); + } + } + } + } + + protected void CalculateSimulationIslands() + { + SimulationIslandManager.UpdateActivationState(this, Dispatcher); + + for (int i = 0; i < _constraints.Count; i++) + { + TypedConstraint constraint = _constraints[i]; + + RigidBody colObj0 = constraint.RigidBodyA; + RigidBody colObj1 = constraint.RigidBodyB; + + if (((colObj0 != null) && (colObj0.MergesSimulationIslands)) && + ((colObj1 != null) && (colObj1.MergesSimulationIslands))) + { + if (colObj0.IsActive || colObj1.IsActive) + { + + SimulationIslandManager.UnionFind.Unite((colObj0).IslandTag, + (colObj1).IslandTag); + } + } + } + + //Store the island id in each body + SimulationIslandManager.StoreIslandActivationState(this); + } + + //protected void SolveNonContactConstraints(ContactSolverInfo solverInfo) + //{ + // //constraint preparation: building jacobians + // for (int i = 0; i < _constraints.Count; i++) + // { + // TypedConstraint constraint = _constraints[i]; + // constraint.BuildJacobian(); + // } + + // //solve the regular non-contact constraints (point 2 point, hinge, generic d6) + // for (int g = 0; g < solverInfo.IterationsCount; g++) + // { + // for (int i = 0; i < _constraints.Count; i++) + // { + // TypedConstraint constraint = _constraints[i]; + // constraint.SolveConstraint(solverInfo.TimeStep); + // } + // } + //} + + //protected void SolveContactConstraints(ContactSolverInfo solverInfo) + //{ + // InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, _debugDrawer); + + // // solve all the contact points and contact friction + // _islandManager.BuildAndProcessIslands(Dispatcher, CollisionObjects, solverCallback); + //} + + protected void SolveConstraints(ContactSolverInfo solverInfo) + { + //sorted version of all btTypedConstraint, based on islandId + List sortedConstraints = new List(ConstraintsCount); + + for (int i = 0; i < ConstraintsCount; i++) + { + sortedConstraints.Add(_constraints[i]); + } + + sortedConstraints.Sort(new Comparison(TypedConstraint.SortConstraintOnIslandPredicate)); + List constraintsPtr = ConstraintsCount != 0 ? sortedConstraints : new List(); + + InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, constraintsPtr, _debugDrawer); + + // solve all the constraints for this island + _islandManager.BuildAndProcessIslands(CollisionWorld.Dispatcher, CollisionWorld.CollisionObjects, solverCallback); + } + + protected void UpdateActivationState(float timeStep) + { + for (int i = 0; i < CollisionObjects.Count; i++) + { + CollisionObject colObj = CollisionObjects[i]; + RigidBody body = RigidBody.Upcast(colObj); + if (body != null) + { + body.UpdateDeactivation(timeStep); + + if (body.WantsSleeping()) + { + if (body.IsStaticOrKinematicObject) + { + body.ActivationState = ActivationState.IslandSleeping; + } + else + { + if (body.ActivationState == ActivationState.Active) + body.ActivationState = ActivationState.WantsDeactivation; + } + } + else + { + if (body.ActivationState != ActivationState.DisableDeactivation) + body.ActivationState = ActivationState.Active; + } + } + } + } + + protected void UpdateVehicles(float timeStep) + { + for (int i = 0; i < _vehicles.Count; i++) + { + RaycastVehicle vehicle = _vehicles[i]; + vehicle.updateVehicle(timeStep); + } + } + + protected void StartProfiling(float timeStep) { } + + protected virtual void InternalSingleStepSimulation(float timeStep) + { + StartProfiling(timeStep); + + //update aabbs information + UpdateAabbs(); + + //apply gravity, predict motion + PredictUnconstraintMotion(timeStep); + + DispatcherInfo dispatchInfo = DispatchInfo; + dispatchInfo.TimeStep = timeStep; + dispatchInfo.StepCount = 0; + dispatchInfo.DebugDraw = DebugDrawer; + + //perform collision detection + PerformDiscreteCollisionDetection(); + + CalculateSimulationIslands(); + + SolverInfo.TimeStep = timeStep; + + //solve contact and other joint constraints + SolveConstraints(SolverInfo); + + //CallbackTriggers(); + + //integrate transforms + IntegrateTransforms(timeStep); + + //update vehicle simulation + UpdateVehicles(timeStep); + + UpdateActivationState(timeStep); + } + + protected void SynchronizeMotionStates() + { + //debug vehicle wheels + { + //todo: iterate over awake simulation islands! + for (int i = 0; i < CollisionObjects.Count; i++) + { + CollisionObject colObj = CollisionObjects[i]; + if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0) + { + Vector3 color = new Vector3(255f, 255f, 255f); + switch (colObj.ActivationState) + { + case ActivationState.Active: + color = new Vector3(255f, 255f, 255f); break; + case ActivationState.IslandSleeping: + color = new Vector3(0f, 255f, 0f); break; + case ActivationState.WantsDeactivation: + color = new Vector3(0f, 255f, 255f); break; + case ActivationState.DisableDeactivation: + color = new Vector3(255f, 0f, 0f); break; + case ActivationState.DisableSimulation: + color = new Vector3(255f, 255f, 0f); break; + default: + color = new Vector3(255f, 0f, 0f); break; + } + + DebugDrawObject(colObj.WorldTransform, colObj.CollisionShape, color); + } + RigidBody body = RigidBody.Upcast(colObj); + if (body != null && body.MotionState != null && !body.IsStaticOrKinematicObject) + { + //if (body.ActivationState != ActivationState.IslandSleeping) + { + Matrix interpolatedTransform = new Matrix(); + TransformUtil.IntegrateTransform(body.InterpolationWorldTransform, + body.InterpolationLinearVelocity, body.InterpolationAngularVelocity, _localTime, ref interpolatedTransform); + body.MotionState.SetWorldTransform(interpolatedTransform); + } + } + } + } + + if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0) + { + for (int i = 0; i < _vehicles.Count; i++) + { + for (int v = 0; v < _vehicles[i].getNumWheels(); v++) + { + Vector3 wheelColor = new Vector3(0, 255, 255); + if (_vehicles[i].getWheelInfo(v).RaycastInfo.IsInContact) + { + wheelColor = new Vector3(0, 0, 255); + } + else + { + wheelColor = new Vector3(255, 0, 255); + } + + //synchronize the wheels with the (interpolated) chassis worldtransform + _vehicles[i].updateWheelTransform(v, true); + + Vector3 wheelPosWS = _vehicles[i].getWheelInfo(v).WorldTransform.Translation; + + Vector3 axle = new Vector3( + MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 0, _vehicles[i].getRightAxis()), + MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 1, _vehicles[i].getRightAxis()), + MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 2, _vehicles[i].getRightAxis())); + + + //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS + //debug wheels (cylinders) + _debugDrawer.DrawLine(wheelPosWS, wheelPosWS + axle, wheelColor); + _debugDrawer.DrawLine(wheelPosWS, _vehicles[i].getWheelInfo(v).RaycastInfo.ContactPointWS, wheelColor); + } + } + } + } + + protected void SaveKinematicState(float timeStep) + { + for (int i = 0; i < CollisionObjects.Count; i++) + { + CollisionObject colObj = CollisionObjects[i]; + RigidBody body = RigidBody.Upcast(colObj); + if (body != null) + { + if (body.ActivationState != ActivationState.IslandSleeping) + { + if (body.IsKinematicObject) + { + //to calculate velocities next frame + body.SaveKinematicState(timeStep); + } + } + } + } + } + + internal class InplaceSolverIslandCallback : SimulationIslandManager.IIslandCallback + { + private ContactSolverInfo _solverInfo; + private IConstraintSolver _solver; + private IDebugDraw _debugDrawer; + private List _sortedConstraints; + + public InplaceSolverIslandCallback( + ContactSolverInfo solverInfo, + IConstraintSolver solver, + List sortedConstraints, + IDebugDraw debugDrawer) + { + _solverInfo = solverInfo; + _solver = solver; + _sortedConstraints = sortedConstraints; + _debugDrawer = debugDrawer; + } + + public ContactSolverInfo SolverInfo { get { return _solverInfo; } set { _solverInfo = value; } } + public IConstraintSolver Solver { get { return _solver; } set { _solver = value; } } + public List Constraints { get { return _sortedConstraints; } set { _sortedConstraints = value; } } + public IDebugDraw DebugDrawer { get { return _debugDrawer; } set { _debugDrawer = value; } } + + public void ProcessIsland(List bodies, List manifolds, int numManifolds, int islandID) + { + //also add all non-contact constraints/joints for this island + List startConstraint = new List(); + int numCurConstraints = 0; + int startIndex = 0; + int i; + + //find the first constraint for this island + for (i = 0; i < _sortedConstraints.Count; i++) + { + if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID) + { + //startConstraint = &m_sortedConstraints[i]; + startIndex = i; + break; + } + } + //count the number of constraints in this island + for (; i < _sortedConstraints.Count; i++) + { + if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID) + { + numCurConstraints++; + } + } + + for (i = startIndex; i < startIndex + numCurConstraints; i++) + { + startConstraint.Add(_sortedConstraints[i]); + } + + _solver.SolveGroup(bodies, manifolds, numManifolds, startConstraint, _solverInfo, _debugDrawer); + } + } + } + + internal class DebugDrawCallback : ITriangleIndexCallback, ITriangleCallback + { + private IDebugDraw _debugDrawer; + private Vector3 _color; + private Matrix _worldTrans; + + public DebugDrawCallback(IDebugDraw debugDrawer, Matrix worldTrans, Vector3 color) + { + _debugDrawer = debugDrawer; + _worldTrans = worldTrans; + _color = color; + } + + public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) + { + ProcessTriangle(triangle, partId, triangleIndex); + } + + #region ITriangleCallback Members + + public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) + { + Vector3 wv0, wv1, wv2; + wv0 = Vector3.TransformNormal(triangle[0], _worldTrans); + wv1 = Vector3.TransformNormal(triangle[1], _worldTrans); + wv2 = Vector3.TransformNormal(triangle[2], _worldTrans); + _debugDrawer.DrawLine(wv0, wv1, _color); + _debugDrawer.DrawLine(wv1, wv2, _color); + _debugDrawer.DrawLine(wv2, wv0, _color); + } + + #endregion + } } \ No newline at end of file diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DynamicsWorld.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DynamicsWorld.cs index 647c8ec..34ae608 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DynamicsWorld.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DynamicsWorld.cs @@ -1,59 +1,59 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX.Dynamics -{ - public abstract class DynamicsWorld : CollisionWorld - { - public DynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache) - : base(dispatcher, pairCache) { } - - //once a rigidbody is added to the dynamics world, it will get this gravity assigned - //existing rigidbodies in the world get gravity assigned too, during this method - public abstract Vector3 Gravity { set; } - public abstract IConstraintSolver ConstraintSolver { set; } - public virtual int ConstraintsCount { get { return 0; } } - public abstract IDebugDraw DebugDrawer { get; set; } - - //stepSimulation proceeds the simulation over timeStep units - public abstract void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep); - - public void StepSimulation(float timeStep) - { - StepSimulation(timeStep, 1, 1f / 60f); - } - - public abstract void UpdateAabbs(); - - public virtual void AddConstraint(TypedConstraint constraint) { } - public virtual void RemoveConstraint(TypedConstraint constraint) { } - - public abstract void AddRigidBody(RigidBody body); - public abstract void RemoveRigidBody(RigidBody body); - - public virtual TypedConstraint GetConstraint(int index) { return null; } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX.Dynamics +{ + public abstract class DynamicsWorld : CollisionWorld + { + public DynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache) + : base(dispatcher, pairCache) { } + + //once a rigidbody is added to the dynamics world, it will get this gravity assigned + //existing rigidbodies in the world get gravity assigned too, during this method + public abstract Vector3 Gravity { set; } + public abstract IConstraintSolver ConstraintSolver { set; } + public virtual int ConstraintsCount { get { return 0; } } + public abstract IDebugDraw DebugDrawer { get; set; } + + //stepSimulation proceeds the simulation over timeStep units + public abstract void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep); + + public void StepSimulation(float timeStep) + { + StepSimulation(timeStep, 1, 1f / 60f); + } + + public abstract void UpdateAabbs(); + + public virtual void AddConstraint(TypedConstraint constraint) { } + public virtual void RemoveConstraint(TypedConstraint constraint) { } + + public abstract void AddRigidBody(RigidBody body); + public abstract void RemoveRigidBody(RigidBody body); + + public virtual TypedConstraint GetConstraint(int index) { return null; } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/RigidBody.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/RigidBody.cs index ec14daf..12a7485 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/RigidBody.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/RigidBody.cs @@ -1,447 +1,447 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX.Dynamics -{ - public class RigidBody : CollisionObject - { - private static float _linearAirDamping = 1; - //'temporarily' global variables - private static float _rigidBodyDeactivationTime = 2; - private static bool _disableDeactivation = false; - - private static float _linearSleepingThreshold = 0.8f; - private static float _angularSleepingThreshold = 1.0f; - private static int _uniqueId = 0; - - private Matrix _invInertiaTensorWorld; - private Vector3 _linearVelocity; - private Vector3 _angularVelocity; - private float _inverseMass; - private float _angularFactor; - - private Vector3 _gravity; - private Vector3 _invInertiaLocal; - private Vector3 _totalForce; - private Vector3 _totalTorque; - - private float _linearDamping; - private float _angularDamping; - - //m_optionalMotionState allows to automatic synchronize the world transform for active objects - private MotionState _optionalMotionState; - - //for experimental overriding of friction/contact solver func - private ContactSolverType _contactSolverType; - private ContactSolverType _frictionSolverType; - - private int _debugBodyId; - - //Bullet 2.20b has experimental damping code to reduce jitter just before objects fall asleep/deactivate - //doesn't work very well yet (value 0 disabled this damping) - //note there this influences deactivation thresholds! - private float _clippedAngvelThresholdSqr = 0.01f; - private float _clippedLinearThresholdSqr = 0.01f; - - private float _jitterVelocityDampingFactor = 0.7f; - - public RigidBody(float mass, MotionState motionState, CollisionShape collisionShape, Vector3 localInertia, float linearDamping, float angularDamping, float friction, float restitution) - { - _optionalMotionState = motionState; - _angularFactor = 1; - _angularDamping = 0.5f; - - if (motionState != null) - { - motionState.GetWorldTransform(out _worldTransform); - } - else - { - WorldTransform = Matrix.Identity; - } - - InterpolationWorldTransform = WorldTransform; - InterpolationLinearVelocity = new Vector3(); - InterpolationAngularVelocity = new Vector3(); - - //moved to btCollisionObject - Friction = friction; - Restitution = restitution; - - CollisionShape = collisionShape; - _debugBodyId = UniqueID++; - - //m_internalOwner is to allow upcasting from collision object to rigid body - Owner = this; - - SetMassProps(mass, localInertia); - SetDamping(linearDamping, angularDamping); - UpdateInertiaTensor(); - } - - public int DebugBodyID { get { return _debugBodyId; } set { _debugBodyId = value; } } - - public ContactSolverType ContactSolverType { get { return _contactSolverType; } set { _contactSolverType = value; } } - public ContactSolverType FrictionSolverType { get { return _frictionSolverType; } set { _frictionSolverType = value; } } - - public float AngularFactor { get { return _angularFactor; } set { _angularFactor = value; } } - - //is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase? - public bool IsInWorld { get { return Broadphase != null; } } - - public Vector3 Gravity - { - get { return _gravity; } - set - { - if (_inverseMass != 0.0f) - { - _gravity = value * (1.0f / _inverseMass); - } - } - } - public Matrix InvInertiaTensorWorld { get { return _invInertiaTensorWorld; } } - public float InverseMass { get { return _inverseMass; } } - public Vector3 InvInertiaDiagLocal { get { return _invInertiaLocal; } set { _invInertiaLocal = value; } } - public Vector3 CenterOfMassPosition { get { return WorldTransform.Translation; } } - public Quaternion Orientation { get { return Quaternion.CreateFromRotationMatrix(WorldTransform); } } - public Matrix CenterOfMassTransform - { - get { return WorldTransform; } - set - { - InterpolationWorldTransform = value; - InterpolationLinearVelocity = LinearVelocity; - InterpolationAngularVelocity = AngularVelocity; - WorldTransform = value; - UpdateInertiaTensor(); - } - } - - public Vector3 LinearVelocity - { - get { return _linearVelocity; } - set - { - if (CollisionFlags == CollisionOptions.StaticObject) - throw new BulletException("Static objects can't have linear velocity!"); - _linearVelocity = value; - } - } - - public Vector3 AngularVelocity - { - get { return _angularVelocity; } - set - { - if (CollisionFlags == CollisionOptions.StaticObject) - throw new BulletException("Static objects can't have angular velocity!"); - _angularVelocity = value; - } - } - - //MotionState allows to automatic synchronize the world transform for active objects - public MotionState MotionState - { - get { return _optionalMotionState; } - set - { - _optionalMotionState = value; - if (_optionalMotionState != null) - value.GetWorldTransform(out _worldTransform); - } - } - - public static float LinearAirDamping { get { return _linearAirDamping; } set { _linearAirDamping = value; } } - public static float RigidBodyDeactivationTime { get { return _rigidBodyDeactivationTime; } set { _rigidBodyDeactivationTime = value; } } - public static bool DisableDeactivation { get { return _disableDeactivation; } set { _disableDeactivation = value; } } - public static float LinearSleepingThreshold { get { return _linearSleepingThreshold; } set { _linearSleepingThreshold = value; } } - public static float AngularSleepingThreshold { get { return _angularSleepingThreshold; } set { _angularSleepingThreshold = value; } } - public static int UniqueID { get { return _uniqueId; } set { _uniqueId = value; } } - - public void ProceedToTransform(Matrix newTrans) - { - CenterOfMassTransform = newTrans; - } - - //to keep collision detection and dynamics separate we don't store a rigidbody pointer - //but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast - public static RigidBody Upcast(CollisionObject colObj) - { - return colObj.Owner as RigidBody; - } - - // continuous collision detection needs prediction - public void PredictIntegratedTransform(float step, ref Matrix predictedTransform) - { - if ((_angularVelocity.LengthSquared() < _clippedAngvelThresholdSqr) && - (_linearVelocity.LengthSquared() < _clippedLinearThresholdSqr)) - { - _angularVelocity *= _jitterVelocityDampingFactor; - _linearVelocity *= _jitterVelocityDampingFactor; - } - - TransformUtil.IntegrateTransform(WorldTransform, _linearVelocity, _angularVelocity, step, ref predictedTransform); - } - - public void SaveKinematicState(float step) - { - //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities - if (step != 0) - { - //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform - if (MotionState != null) - MotionState.GetWorldTransform(out _worldTransform); - - TransformUtil.CalculateVelocity(InterpolationWorldTransform, WorldTransform, step, ref _linearVelocity, ref _angularVelocity); - InterpolationLinearVelocity = _linearVelocity; - InterpolationAngularVelocity = _angularVelocity; - InterpolationWorldTransform = WorldTransform; - } - } - - public void ApplyForces(float step) - { - if (IsStaticOrKinematicObject) - return; - - ApplyCentralForce(_gravity); - - _linearVelocity *= (1 - step * LinearAirDamping * _linearDamping) < 0.0f ? 0.0f : (1.0f < (1 - step * LinearAirDamping * _linearDamping) ? 1.0f : (1 - step * LinearAirDamping * _linearDamping)); - _angularVelocity *= (1 - step * _angularDamping) < 0.0f ? 0.0f : (1.0f < (1 - step * _angularDamping) ? 1.0f : (1 - step * _angularDamping)); - - float speed = _linearVelocity.Length(); - if (speed < _linearDamping) - { - float dampVel = 0.005f; - if (speed > dampVel) - { - Vector3 dir = _linearVelocity; - dir.Normalize(); - _linearVelocity -= dir * dampVel; - } - else - { - _linearVelocity = new Vector3(); - } - } - - float angSpeed = _angularVelocity.Length(); - if (angSpeed < _angularDamping) - { - float angDampVel = 0.005f; - if (angSpeed > angDampVel) - { - Vector3 dir = _angularVelocity; - dir.Normalize(); - _angularVelocity -= dir * angDampVel; - } - else - { - _angularVelocity = new Vector3(); - } - } - } - - public void SetDamping(float linDamping, float angDamping) - { - _linearDamping = linDamping < 0.0f ? 0.0f : (1.0f < linDamping ? 1.0f : linDamping); - _angularDamping = angDamping < 0.0f ? 0.0f : (1.0f < angDamping ? 1.0f : angDamping); - } - - public void SetMassProps(float mass, Vector3 inertia) - { - if (mass == 0) - { - CollisionFlags |= CollisionOptions.StaticObject; - _inverseMass = 0; - } - else - { - CollisionFlags &= (~CollisionOptions.StaticObject); - _inverseMass = 1.0f / mass; - } - - _invInertiaLocal = new Vector3(inertia.X != 0.0f ? 1.0f / inertia.X : 0.0f, - inertia.Y != 0.0f ? 1.0f / inertia.Y : 0.0f, - inertia.Z != 0.0f ? 1.0f / inertia.Z : 0.0f); - } - - public void IntegrateVelocities(float step) - { - if (IsStaticOrKinematicObject) - return; - - _linearVelocity += _totalForce * (_inverseMass * step); - _angularVelocity += Vector3.TransformNormal(_totalTorque, _invInertiaTensorWorld) * step; - - //float MAX_ANGVEL = MonoXnaCompactMaths.MathHelper.PiOver2; - float MAX_ANGVEL = (float)Math.PI / 2.0f; - /// clamp angular velocity. collision calculations will fail on higher angular velocities - float angvel = _angularVelocity.Length(); - if (angvel * step > MAX_ANGVEL) - { - _angularVelocity *= (MAX_ANGVEL / step) / angvel; - } - - ClearForces(); - } - - public void ApplyCentralForce(Vector3 force) - { - _totalForce += force; - } - - public void ApplyTorque(Vector3 torque) - { - _totalTorque += torque; - } - - public void ApplyForce(Vector3 force, Vector3 rel_pos) - { - ApplyCentralForce(force); - ApplyTorque(Vector3.Cross(rel_pos, force)); - } - - public void ApplyCentralImpulse(Vector3 impulse) - { - _linearVelocity += impulse * _inverseMass; - } - - public void ApplyTorqueImpulse(Vector3 torque) - { - _angularVelocity += Vector3.TransformNormal(torque, _invInertiaTensorWorld); - } - - public void ApplyImpulse(Vector3 impulse, Vector3 rel_pos) - { - if (_inverseMass != 0) - { - ApplyCentralImpulse(impulse); - if (_angularFactor != 0) - ApplyTorqueImpulse(Vector3.Cross(rel_pos, impulse) * _angularFactor); - } - } - - public void InternalApplyImpulse(Vector3 linearComponent, Vector3 angularComponent, float impulseMagnitude) - { - if (_inverseMass != 0) - { - _linearVelocity += linearComponent * impulseMagnitude; - if (_angularFactor != 0) - _angularVelocity += angularComponent * impulseMagnitude * _angularFactor; - } - } - - public void ClearForces() - { - _totalForce = new Vector3(); - _totalTorque = new Vector3(); - } - - public void UpdateInertiaTensor() - { - Matrix temp = WorldTransform; - temp.Translation = Vector3.Zero; - _invInertiaTensorWorld = MatrixOperations.Multiply(MatrixOperations.Scaled(WorldTransform, _invInertiaLocal), Matrix.Transpose(temp)); - } - - public Vector3 GetVelocityInLocalPoint(Vector3 relPos) - { - //we also calculate lin/ang velocity for kinematic objects - return _linearVelocity + Vector3.Cross(_angularVelocity, relPos); - - //for kinematic objects, we could also use use: - // return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep; - } - - public void Translate(Vector3 v) - { - Matrix m = WorldTransform; - m.Translation += v; - WorldTransform = m; - } - - public void GetAabb(out Vector3 aabbMin, out Vector3 aabbMax) - { - CollisionShape.GetAabb(WorldTransform, out aabbMin, out aabbMax); - } - - public float ComputeImpulseDenominator(Vector3 pos, Vector3 normal) - { - Vector3 r0 = pos - CenterOfMassPosition; - Vector3 c0 = Vector3.Cross(r0, normal); - Vector3 vec = Vector3.Cross(Vector3.TransformNormal(c0, InvInertiaTensorWorld), r0); - - return _inverseMass + Vector3.Dot(normal, vec); - - } - - public float ComputeAngularImpulseDenominator(Vector3 axis) - { - Vector3 vec = Vector3.TransformNormal(axis, InvInertiaTensorWorld); - return Vector3.Dot(axis, vec); - } - - public void UpdateDeactivation(float timeStep) - { - if ((ActivationState == ActivationState.IslandSleeping) || (ActivationState == ActivationState.DisableDeactivation)) - return; - - if ((LinearVelocity.LengthSquared() < LinearSleepingThreshold * LinearSleepingThreshold) && - (AngularVelocity.LengthSquared() < AngularSleepingThreshold * AngularSleepingThreshold)) - { - DeactivationTime += timeStep; - } - else - { - DeactivationTime = 0; - ActivationState = ActivationState.Nothing; - } - - } - - public bool WantsSleeping() - { - - if (ActivationState == ActivationState.DisableDeactivation) - return false; - - //disable deactivation - if (DisableDeactivation || (RigidBodyDeactivationTime == 0)) - return false; - - if ((ActivationState == ActivationState.IslandSleeping) || (ActivationState == ActivationState.WantsDeactivation)) - return true; - - if (DeactivationTime > RigidBodyDeactivationTime) - { - return true; - } - return false; - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX.Dynamics +{ + public class RigidBody : CollisionObject + { + private static float _linearAirDamping = 1; + //'temporarily' global variables + private static float _rigidBodyDeactivationTime = 2; + private static bool _disableDeactivation = false; + + private static float _linearSleepingThreshold = 0.8f; + private static float _angularSleepingThreshold = 1.0f; + private static int _uniqueId = 0; + + private Matrix _invInertiaTensorWorld; + private Vector3 _linearVelocity; + private Vector3 _angularVelocity; + private float _inverseMass; + private float _angularFactor; + + private Vector3 _gravity; + private Vector3 _invInertiaLocal; + private Vector3 _totalForce; + private Vector3 _totalTorque; + + private float _linearDamping; + private float _angularDamping; + + //m_optionalMotionState allows to automatic synchronize the world transform for active objects + private MotionState _optionalMotionState; + + //for experimental overriding of friction/contact solver func + private ContactSolverType _contactSolverType; + private ContactSolverType _frictionSolverType; + + private int _debugBodyId; + + //Bullet 2.20b has experimental damping code to reduce jitter just before objects fall asleep/deactivate + //doesn't work very well yet (value 0 disabled this damping) + //note there this influences deactivation thresholds! + private float _clippedAngvelThresholdSqr = 0.01f; + private float _clippedLinearThresholdSqr = 0.01f; + + private float _jitterVelocityDampingFactor = 0.7f; + + public RigidBody(float mass, MotionState motionState, CollisionShape collisionShape, Vector3 localInertia, float linearDamping, float angularDamping, float friction, float restitution) + { + _optionalMotionState = motionState; + _angularFactor = 1; + _angularDamping = 0.5f; + + if (motionState != null) + { + motionState.GetWorldTransform(out _worldTransform); + } + else + { + WorldTransform = Matrix.Identity; + } + + InterpolationWorldTransform = WorldTransform; + InterpolationLinearVelocity = new Vector3(); + InterpolationAngularVelocity = new Vector3(); + + //moved to btCollisionObject + Friction = friction; + Restitution = restitution; + + CollisionShape = collisionShape; + _debugBodyId = UniqueID++; + + //m_internalOwner is to allow upcasting from collision object to rigid body + Owner = this; + + SetMassProps(mass, localInertia); + SetDamping(linearDamping, angularDamping); + UpdateInertiaTensor(); + } + + public int DebugBodyID { get { return _debugBodyId; } set { _debugBodyId = value; } } + + public ContactSolverType ContactSolverType { get { return _contactSolverType; } set { _contactSolverType = value; } } + public ContactSolverType FrictionSolverType { get { return _frictionSolverType; } set { _frictionSolverType = value; } } + + public float AngularFactor { get { return _angularFactor; } set { _angularFactor = value; } } + + //is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase? + public bool IsInWorld { get { return Broadphase != null; } } + + public Vector3 Gravity + { + get { return _gravity; } + set + { + if (_inverseMass != 0.0f) + { + _gravity = value * (1.0f / _inverseMass); + } + } + } + public Matrix InvInertiaTensorWorld { get { return _invInertiaTensorWorld; } } + public float InverseMass { get { return _inverseMass; } } + public Vector3 InvInertiaDiagLocal { get { return _invInertiaLocal; } set { _invInertiaLocal = value; } } + public Vector3 CenterOfMassPosition { get { return WorldTransform.Translation; } } + public Quaternion Orientation { get { return Quaternion.CreateFromRotationMatrix(WorldTransform); } } + public Matrix CenterOfMassTransform + { + get { return WorldTransform; } + set + { + InterpolationWorldTransform = value; + InterpolationLinearVelocity = LinearVelocity; + InterpolationAngularVelocity = AngularVelocity; + WorldTransform = value; + UpdateInertiaTensor(); + } + } + + public Vector3 LinearVelocity + { + get { return _linearVelocity; } + set + { + if (CollisionFlags == CollisionOptions.StaticObject) + throw new BulletException("Static objects can't have linear velocity!"); + _linearVelocity = value; + } + } + + public Vector3 AngularVelocity + { + get { return _angularVelocity; } + set + { + if (CollisionFlags == CollisionOptions.StaticObject) + throw new BulletException("Static objects can't have angular velocity!"); + _angularVelocity = value; + } + } + + //MotionState allows to automatic synchronize the world transform for active objects + public MotionState MotionState + { + get { return _optionalMotionState; } + set + { + _optionalMotionState = value; + if (_optionalMotionState != null) + value.GetWorldTransform(out _worldTransform); + } + } + + public static float LinearAirDamping { get { return _linearAirDamping; } set { _linearAirDamping = value; } } + public static float RigidBodyDeactivationTime { get { return _rigidBodyDeactivationTime; } set { _rigidBodyDeactivationTime = value; } } + public static bool DisableDeactivation { get { return _disableDeactivation; } set { _disableDeactivation = value; } } + public static float LinearSleepingThreshold { get { return _linearSleepingThreshold; } set { _linearSleepingThreshold = value; } } + public static float AngularSleepingThreshold { get { return _angularSleepingThreshold; } set { _angularSleepingThreshold = value; } } + public static int UniqueID { get { return _uniqueId; } set { _uniqueId = value; } } + + public void ProceedToTransform(Matrix newTrans) + { + CenterOfMassTransform = newTrans; + } + + //to keep collision detection and dynamics separate we don't store a rigidbody pointer + //but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast + public static RigidBody Upcast(CollisionObject colObj) + { + return colObj.Owner as RigidBody; + } + + // continuous collision detection needs prediction + public void PredictIntegratedTransform(float step, ref Matrix predictedTransform) + { + if ((_angularVelocity.LengthSquared() < _clippedAngvelThresholdSqr) && + (_linearVelocity.LengthSquared() < _clippedLinearThresholdSqr)) + { + _angularVelocity *= _jitterVelocityDampingFactor; + _linearVelocity *= _jitterVelocityDampingFactor; + } + + TransformUtil.IntegrateTransform(WorldTransform, _linearVelocity, _angularVelocity, step, ref predictedTransform); + } + + public void SaveKinematicState(float step) + { + //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities + if (step != 0) + { + //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform + if (MotionState != null) + MotionState.GetWorldTransform(out _worldTransform); + + TransformUtil.CalculateVelocity(InterpolationWorldTransform, WorldTransform, step, ref _linearVelocity, ref _angularVelocity); + InterpolationLinearVelocity = _linearVelocity; + InterpolationAngularVelocity = _angularVelocity; + InterpolationWorldTransform = WorldTransform; + } + } + + public void ApplyForces(float step) + { + if (IsStaticOrKinematicObject) + return; + + ApplyCentralForce(_gravity); + + _linearVelocity *= (1 - step * LinearAirDamping * _linearDamping) < 0.0f ? 0.0f : (1.0f < (1 - step * LinearAirDamping * _linearDamping) ? 1.0f : (1 - step * LinearAirDamping * _linearDamping)); + _angularVelocity *= (1 - step * _angularDamping) < 0.0f ? 0.0f : (1.0f < (1 - step * _angularDamping) ? 1.0f : (1 - step * _angularDamping)); + + float speed = _linearVelocity.Length(); + if (speed < _linearDamping) + { + float dampVel = 0.005f; + if (speed > dampVel) + { + Vector3 dir = _linearVelocity; + dir.Normalize(); + _linearVelocity -= dir * dampVel; + } + else + { + _linearVelocity = new Vector3(); + } + } + + float angSpeed = _angularVelocity.Length(); + if (angSpeed < _angularDamping) + { + float angDampVel = 0.005f; + if (angSpeed > angDampVel) + { + Vector3 dir = _angularVelocity; + dir.Normalize(); + _angularVelocity -= dir * angDampVel; + } + else + { + _angularVelocity = new Vector3(); + } + } + } + + public void SetDamping(float linDamping, float angDamping) + { + _linearDamping = linDamping < 0.0f ? 0.0f : (1.0f < linDamping ? 1.0f : linDamping); + _angularDamping = angDamping < 0.0f ? 0.0f : (1.0f < angDamping ? 1.0f : angDamping); + } + + public void SetMassProps(float mass, Vector3 inertia) + { + if (mass == 0) + { + CollisionFlags |= CollisionOptions.StaticObject; + _inverseMass = 0; + } + else + { + CollisionFlags &= (~CollisionOptions.StaticObject); + _inverseMass = 1.0f / mass; + } + + _invInertiaLocal = new Vector3(inertia.X != 0.0f ? 1.0f / inertia.X : 0.0f, + inertia.Y != 0.0f ? 1.0f / inertia.Y : 0.0f, + inertia.Z != 0.0f ? 1.0f / inertia.Z : 0.0f); + } + + public void IntegrateVelocities(float step) + { + if (IsStaticOrKinematicObject) + return; + + _linearVelocity += _totalForce * (_inverseMass * step); + _angularVelocity += Vector3.TransformNormal(_totalTorque, _invInertiaTensorWorld) * step; + + //float MAX_ANGVEL = MonoXnaCompactMaths.MathHelper.PiOver2; + float MAX_ANGVEL = (float)Math.PI / 2.0f; + /// clamp angular velocity. collision calculations will fail on higher angular velocities + float angvel = _angularVelocity.Length(); + if (angvel * step > MAX_ANGVEL) + { + _angularVelocity *= (MAX_ANGVEL / step) / angvel; + } + + ClearForces(); + } + + public void ApplyCentralForce(Vector3 force) + { + _totalForce += force; + } + + public void ApplyTorque(Vector3 torque) + { + _totalTorque += torque; + } + + public void ApplyForce(Vector3 force, Vector3 rel_pos) + { + ApplyCentralForce(force); + ApplyTorque(Vector3.Cross(rel_pos, force)); + } + + public void ApplyCentralImpulse(Vector3 impulse) + { + _linearVelocity += impulse * _inverseMass; + } + + public void ApplyTorqueImpulse(Vector3 torque) + { + _angularVelocity += Vector3.TransformNormal(torque, _invInertiaTensorWorld); + } + + public void ApplyImpulse(Vector3 impulse, Vector3 rel_pos) + { + if (_inverseMass != 0) + { + ApplyCentralImpulse(impulse); + if (_angularFactor != 0) + ApplyTorqueImpulse(Vector3.Cross(rel_pos, impulse) * _angularFactor); + } + } + + public void InternalApplyImpulse(Vector3 linearComponent, Vector3 angularComponent, float impulseMagnitude) + { + if (_inverseMass != 0) + { + _linearVelocity += linearComponent * impulseMagnitude; + if (_angularFactor != 0) + _angularVelocity += angularComponent * impulseMagnitude * _angularFactor; + } + } + + public void ClearForces() + { + _totalForce = new Vector3(); + _totalTorque = new Vector3(); + } + + public void UpdateInertiaTensor() + { + Matrix temp = WorldTransform; + temp.Translation = Vector3.Zero; + _invInertiaTensorWorld = MatrixOperations.Multiply(MatrixOperations.Scaled(WorldTransform, _invInertiaLocal), Matrix.Transpose(temp)); + } + + public Vector3 GetVelocityInLocalPoint(Vector3 relPos) + { + //we also calculate lin/ang velocity for kinematic objects + return _linearVelocity + Vector3.Cross(_angularVelocity, relPos); + + //for kinematic objects, we could also use use: + // return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep; + } + + public void Translate(Vector3 v) + { + Matrix m = WorldTransform; + m.Translation += v; + WorldTransform = m; + } + + public void GetAabb(out Vector3 aabbMin, out Vector3 aabbMax) + { + CollisionShape.GetAabb(WorldTransform, out aabbMin, out aabbMax); + } + + public float ComputeImpulseDenominator(Vector3 pos, Vector3 normal) + { + Vector3 r0 = pos - CenterOfMassPosition; + Vector3 c0 = Vector3.Cross(r0, normal); + Vector3 vec = Vector3.Cross(Vector3.TransformNormal(c0, InvInertiaTensorWorld), r0); + + return _inverseMass + Vector3.Dot(normal, vec); + + } + + public float ComputeAngularImpulseDenominator(Vector3 axis) + { + Vector3 vec = Vector3.TransformNormal(axis, InvInertiaTensorWorld); + return Vector3.Dot(axis, vec); + } + + public void UpdateDeactivation(float timeStep) + { + if ((ActivationState == ActivationState.IslandSleeping) || (ActivationState == ActivationState.DisableDeactivation)) + return; + + if ((LinearVelocity.LengthSquared() < LinearSleepingThreshold * LinearSleepingThreshold) && + (AngularVelocity.LengthSquared() < AngularSleepingThreshold * AngularSleepingThreshold)) + { + DeactivationTime += timeStep; + } + else + { + DeactivationTime = 0; + ActivationState = ActivationState.Nothing; + } + + } + + public bool WantsSleeping() + { + + if (ActivationState == ActivationState.DisableDeactivation) + return false; + + //disable deactivation + if (DisableDeactivation || (RigidBodyDeactivationTime == 0)) + return false; + + if ((ActivationState == ActivationState.IslandSleeping) || (ActivationState == ActivationState.WantsDeactivation)) + return true; + + if (DeactivationTime > RigidBodyDeactivationTime) + { + return true; + } + return false; + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/SimpleDynamicsWorld.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/SimpleDynamicsWorld.cs index aa91a03..3430a73 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/SimpleDynamicsWorld.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/SimpleDynamicsWorld.cs @@ -1,211 +1,211 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX.Dynamics -{ - public class SimpleDynamicsWorld : DynamicsWorld - { - private IConstraintSolver _constraintSolver; - private bool _ownsConstraintSolver; - private Vector3 _gravity; - private IDebugDraw _debugDrawer; - - /// - /// this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver - /// - /// - /// - /// - public SimpleDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache, IConstraintSolver constraintSolver) - : base(dispatcher, pairCache) - { - _constraintSolver = constraintSolver; - _ownsConstraintSolver = false; - _gravity = new Vector3(0, 0, -10); - } - - public override Vector3 Gravity - { - set - { - _gravity = value; - for (int i = 0; i < CollisionObjects.Count; i++) - { - CollisionObject colObj = CollisionObjects[i]; - RigidBody body = RigidBody.Upcast(colObj); - if (body != null) - { - body.Gravity = value; - } - } - } - } - - public override IConstraintSolver ConstraintSolver - { - set - { - _ownsConstraintSolver = false; - _constraintSolver = value; - } - } - - public override IDebugDraw DebugDrawer - { - get - { - return _debugDrawer; - } - set - { - _debugDrawer = value; - } - } - - public override void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep) - { - //apply gravity, predict motion - PredictUnconstraintMotion(timeStep); - - DispatcherInfo dispatchInfo = new DispatcherInfo(); - dispatchInfo.TimeStep = timeStep; - dispatchInfo.StepCount = 0; - dispatchInfo.DebugDraw = DebugDrawer; - //perform collision detection - PerformDiscreteCollisionDetection(); - - //solve contact constraints - int numManifolds = Dispatcher.ManifoldCount; - if (numManifolds != 0) - { - - List manifolds = (Dispatcher as CollisionDispatcher).Manifolds; - //int numManifolds = m_dispatcher1.GetNumManifolds(); - ContactSolverInfo infoGlobal = new ContactSolverInfo(); - infoGlobal.TimeStep = timeStep; - - _constraintSolver.SolveGroup(new List(), manifolds, manifolds.Count, new List(), infoGlobal, _debugDrawer); - } - //integrate transforms - IntegrateTransforms(timeStep); - - UpdateAabbs(); - - SynchronizeMotionStates(); - } - - public override void UpdateAabbs() - { - for (int i = 0; i < CollisionObjects.Count; i++) - { - CollisionObject colObj = CollisionObjects[i]; - RigidBody body = RigidBody.Upcast(colObj); - if (body != null) - { - if (body.IsActive && (!body.IsStaticObject)) - { - Vector3 minAabb, maxAabb; - colObj.CollisionShape.GetAabb(colObj.WorldTransform, out minAabb, out maxAabb); - IBroadphase bp = Broadphase; - bp.SetAabb(body.Broadphase, minAabb, maxAabb); - } - } - } - } - - public override void AddRigidBody(RigidBody body) - { - body.Gravity = _gravity; - - if (body.CollisionShape != null) - { - AddCollisionObject(body); - } - } - - public override void RemoveRigidBody(RigidBody body) - { - RemoveCollisionObject(body); - } - - public void SynchronizeMotionStates() - { - for (int i = 0; i < CollisionObjects.Count; i++) - { - CollisionObject colObj = CollisionObjects[i]; - RigidBody body = RigidBody.Upcast(colObj); - if (body != null && body.MotionState != null) - { - if (body.ActivationState != ActivationState.IslandSleeping) - { - body.MotionState.SetWorldTransform(body.WorldTransform); - } - } - } - } - - protected void PredictUnconstraintMotion(float timeStep) - { - for (int i = 0; i < CollisionObjects.Count; i++) - { - CollisionObject colObj = CollisionObjects[i]; - RigidBody body = RigidBody.Upcast(colObj); - if (body != null) - { - if (!body.IsStaticObject) - { - if (body.IsActive) - { - body.ApplyForces(timeStep); - body.IntegrateVelocities(timeStep); - Matrix temp = body.InterpolationWorldTransform; - body.PredictIntegratedTransform(timeStep, ref temp); - body.InterpolationWorldTransform = temp; - } - } - } - } - } - - protected void IntegrateTransforms(float timeStep) - { - Matrix predictedTrans = Matrix.Identity; - for (int i = 0; i < CollisionObjects.Count; i++) - { - CollisionObject colObj = CollisionObjects[i]; - RigidBody body = RigidBody.Upcast(colObj); - if (body != null) - { - if (body.IsActive && (!body.IsStaticObject)) - { - body.PredictIntegratedTransform(timeStep, ref predictedTrans); - body.ProceedToTransform(predictedTrans); - } - } - } - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX.Dynamics +{ + public class SimpleDynamicsWorld : DynamicsWorld + { + private IConstraintSolver _constraintSolver; + private bool _ownsConstraintSolver; + private Vector3 _gravity; + private IDebugDraw _debugDrawer; + + /// + /// this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver + /// + /// + /// + /// + public SimpleDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache, IConstraintSolver constraintSolver) + : base(dispatcher, pairCache) + { + _constraintSolver = constraintSolver; + _ownsConstraintSolver = false; + _gravity = new Vector3(0, 0, -10); + } + + public override Vector3 Gravity + { + set + { + _gravity = value; + for (int i = 0; i < CollisionObjects.Count; i++) + { + CollisionObject colObj = CollisionObjects[i]; + RigidBody body = RigidBody.Upcast(colObj); + if (body != null) + { + body.Gravity = value; + } + } + } + } + + public override IConstraintSolver ConstraintSolver + { + set + { + _ownsConstraintSolver = false; + _constraintSolver = value; + } + } + + public override IDebugDraw DebugDrawer + { + get + { + return _debugDrawer; + } + set + { + _debugDrawer = value; + } + } + + public override void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep) + { + //apply gravity, predict motion + PredictUnconstraintMotion(timeStep); + + DispatcherInfo dispatchInfo = new DispatcherInfo(); + dispatchInfo.TimeStep = timeStep; + dispatchInfo.StepCount = 0; + dispatchInfo.DebugDraw = DebugDrawer; + //perform collision detection + PerformDiscreteCollisionDetection(); + + //solve contact constraints + int numManifolds = Dispatcher.ManifoldCount; + if (numManifolds != 0) + { + + List manifolds = (Dispatcher as CollisionDispatcher).Manifolds; + //int numManifolds = m_dispatcher1.GetNumManifolds(); + ContactSolverInfo infoGlobal = new ContactSolverInfo(); + infoGlobal.TimeStep = timeStep; + + _constraintSolver.SolveGroup(new List(), manifolds, manifolds.Count, new List(), infoGlobal, _debugDrawer); + } + //integrate transforms + IntegrateTransforms(timeStep); + + UpdateAabbs(); + + SynchronizeMotionStates(); + } + + public override void UpdateAabbs() + { + for (int i = 0; i < CollisionObjects.Count; i++) + { + CollisionObject colObj = CollisionObjects[i]; + RigidBody body = RigidBody.Upcast(colObj); + if (body != null) + { + if (body.IsActive && (!body.IsStaticObject)) + { + Vector3 minAabb, maxAabb; + colObj.CollisionShape.GetAabb(colObj.WorldTransform, out minAabb, out maxAabb); + IBroadphase bp = Broadphase; + bp.SetAabb(body.Broadphase, minAabb, maxAabb); + } + } + } + } + + public override void AddRigidBody(RigidBody body) + { + body.Gravity = _gravity; + + if (body.CollisionShape != null) + { + AddCollisionObject(body); + } + } + + public override void RemoveRigidBody(RigidBody body) + { + RemoveCollisionObject(body); + } + + public void SynchronizeMotionStates() + { + for (int i = 0; i < CollisionObjects.Count; i++) + { + CollisionObject colObj = CollisionObjects[i]; + RigidBody body = RigidBody.Upcast(colObj); + if (body != null && body.MotionState != null) + { + if (body.ActivationState != ActivationState.IslandSleeping) + { + body.MotionState.SetWorldTransform(body.WorldTransform); + } + } + } + } + + protected void PredictUnconstraintMotion(float timeStep) + { + for (int i = 0; i < CollisionObjects.Count; i++) + { + CollisionObject colObj = CollisionObjects[i]; + RigidBody body = RigidBody.Upcast(colObj); + if (body != null) + { + if (!body.IsStaticObject) + { + if (body.IsActive) + { + body.ApplyForces(timeStep); + body.IntegrateVelocities(timeStep); + Matrix temp = body.InterpolationWorldTransform; + body.PredictIntegratedTransform(timeStep, ref temp); + body.InterpolationWorldTransform = temp; + } + } + } + } + } + + protected void IntegrateTransforms(float timeStep) + { + Matrix predictedTrans = Matrix.Identity; + for (int i = 0; i < CollisionObjects.Count; i++) + { + CollisionObject colObj = CollisionObjects[i]; + RigidBody body = RigidBody.Upcast(colObj); + if (body != null) + { + if (body.IsActive && (!body.IsStaticObject)) + { + body.PredictIntegratedTransform(timeStep, ref predictedTrans); + body.ProceedToTransform(predictedTrans); + } + } + } + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/RaycastVehicle.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/RaycastVehicle.cs index 90db9ee..4e0e64b 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/RaycastVehicle.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/RaycastVehicle.cs @@ -1,94 +1,94 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX.Dynamics -{ - public class RaycastVehicle : TypedConstraint - { - public override void BuildJacobian() - { - throw new Exception("The method or operation is not implemented."); - } - - public override void SolveConstraint(float timeStep) - { - throw new Exception("The method or operation is not implemented."); - } - - public int getNumWheels() - { - throw new Exception("The method or operation is not implemented."); - } - - public WheelInfo getWheelInfo(int v) - { - throw new Exception("The method or operation is not implemented."); - } - - public void updateWheelTransform(int v, bool p) - { - throw new Exception("The method or operation is not implemented."); - } - - public int getRightAxis() - { - throw new Exception("The method or operation is not implemented."); - } - - public void updateVehicle(float timeStep) - { - throw new Exception("The method or operation is not implemented."); - } - } - - public class DefaultVehicleRaycaster : IVehicleRaycaster - { - DynamicsWorld _dynamicsWorld; - - public DefaultVehicleRaycaster(DynamicsWorld world) - { - _dynamicsWorld = world; - } - - public object CastRay(Vector3 from, Vector3 to, out VehicleRaycasterResult result) - { - CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(from, to); - _dynamicsWorld.RayTest(from, to, rayCallback); - - result = new VehicleRaycasterResult(); - - if (!rayCallback.HasHit) return 0; - RigidBody body = RigidBody.Upcast(rayCallback.CollisionObject); - if (body == null) return 0; - - result.HitPointInWorld = rayCallback.HitPointWorld; - result.HitNormalInWorld = rayCallback.HitNormalWorld; - result.HitNormalInWorld.Normalize(); - result.DistFraction = rayCallback.ClosestHitFraction; - return body; - } - } +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX.Dynamics +{ + public class RaycastVehicle : TypedConstraint + { + public override void BuildJacobian() + { + throw new Exception("The method or operation is not implemented."); + } + + public override void SolveConstraint(float timeStep) + { + throw new Exception("The method or operation is not implemented."); + } + + public int getNumWheels() + { + throw new Exception("The method or operation is not implemented."); + } + + public WheelInfo getWheelInfo(int v) + { + throw new Exception("The method or operation is not implemented."); + } + + public void updateWheelTransform(int v, bool p) + { + throw new Exception("The method or operation is not implemented."); + } + + public int getRightAxis() + { + throw new Exception("The method or operation is not implemented."); + } + + public void updateVehicle(float timeStep) + { + throw new Exception("The method or operation is not implemented."); + } + } + + public class DefaultVehicleRaycaster : IVehicleRaycaster + { + DynamicsWorld _dynamicsWorld; + + public DefaultVehicleRaycaster(DynamicsWorld world) + { + _dynamicsWorld = world; + } + + public object CastRay(Vector3 from, Vector3 to, out VehicleRaycasterResult result) + { + CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(from, to); + _dynamicsWorld.RayTest(from, to, rayCallback); + + result = new VehicleRaycasterResult(); + + if (!rayCallback.HasHit) return 0; + RigidBody body = RigidBody.Upcast(rayCallback.CollisionObject); + if (body == null) return 0; + + result.HitPointInWorld = rayCallback.HitPointWorld; + result.HitNormalInWorld = rayCallback.HitNormalWorld; + result.HitNormalInWorld.Normalize(); + result.DistFraction = rayCallback.ClosestHitFraction; + return body; + } + } } \ No newline at end of file diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/VehicleRaycaster.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/VehicleRaycaster.cs index 86829e3..14c13c6 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/VehicleRaycaster.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/VehicleRaycaster.cs @@ -1,49 +1,49 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX.Dynamics -{ - public interface IVehicleRaycaster - { - object CastRay(Vector3 from, Vector3 to, out VehicleRaycasterResult result); - } - - public class VehicleRaycasterResult - { - private Single _distFraction; - private Vector3 _hitNormalInWorld; - private Vector3 _hitPointInWorld; - - public VehicleRaycasterResult() - { - _distFraction = -1; - } - - public float DistFraction { get { return _distFraction; } set { _distFraction = value; } } - public Vector3 HitNormalInWorld { get { return _hitNormalInWorld; } set { _hitNormalInWorld = value; } } - public Vector3 HitPointInWorld { get { return _hitPointInWorld; } set { _hitPointInWorld = value; } } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX.Dynamics +{ + public interface IVehicleRaycaster + { + object CastRay(Vector3 from, Vector3 to, out VehicleRaycasterResult result); + } + + public class VehicleRaycasterResult + { + private Single _distFraction; + private Vector3 _hitNormalInWorld; + private Vector3 _hitPointInWorld; + + public VehicleRaycasterResult() + { + _distFraction = -1; + } + + public float DistFraction { get { return _distFraction; } set { _distFraction = value; } } + public Vector3 HitNormalInWorld { get { return _hitNormalInWorld; } set { _hitNormalInWorld = value; } } + public Vector3 HitPointInWorld { get { return _hitPointInWorld; } set { _hitPointInWorld = value; } } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/WheelInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/WheelInfo.cs index d4a94f3..516e455 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/WheelInfo.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/WheelInfo.cs @@ -1,529 +1,529 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX.Dynamics -{ - public struct WheelInfoConstructionInfo - { - private Vector3 _chassicConnectionCS; - private Vector3 _wheelDirectionCS; - private Vector3 _wheelAxisCS; - - private Single _suspensionRestLength; - private Single _maxSuspensionTravelCm; - private Single _wheelRadius; - private Single _suspensionStiffness; - private Single _wheelsDampingCompression; - private Single _wheelsDampingRelaxation; - private Single _frictionSlip; - - private Boolean _isFrontWheel; - - #region Basic Properties - public Vector3 ChassicConnectionCS - { - get { return _chassicConnectionCS; } - set { _chassicConnectionCS = value; } - } - - public Vector3 WheelDirectionCS - { - get { return _wheelDirectionCS; } - set { _wheelDirectionCS = value; } - } - - public Vector3 WheelAxleCS - { - get { return _wheelAxisCS; } - set { _wheelAxisCS = value; } - } - - - public Single SuspensionRestLength - { - get { return _suspensionRestLength; } - set { _suspensionRestLength = value; } - } - - public Single MaxSuspensionTravelCm - { - get { return _maxSuspensionTravelCm; } - set { _maxSuspensionTravelCm = value; } - } - - public Single WheelRadius - { - get { return _wheelRadius; } - set { _wheelRadius = value; } - } - - - public Single SuspensionStiffness - { - get { return _suspensionStiffness; } - set { _suspensionStiffness = value; } - } - - public Single WheelsDampingCompression - { - get { return _wheelsDampingCompression; } - set { _wheelsDampingCompression = value; } - } - - public Single WheelsDampingRelaxation - { - get { return _wheelsDampingRelaxation; } - set { _wheelsDampingRelaxation = value; } - } - - public Single FrictionSlip - { - get { return _frictionSlip; } - set { _frictionSlip = value; } - } - - - public Boolean IsFrontWheel - { - get { return _isFrontWheel; } - set { _isFrontWheel = value; } - } - #endregion - } - - public struct RaycastInfo - { - private Vector3 _contractNormalWS; - private Vector3 _contractPointWS; - - private Vector3 _hardPointWS; - private Vector3 _wheelDirectionWS; - private Vector3 _wheelAxleWS; - - private Single _suspensionLength; - private Boolean _isInContract; - - #region Basic Properties - public Single SuspensionLength - { - get { return _suspensionLength; } - set { _suspensionLength = value; } - } - - public Boolean IsInContact - { - get { return _isInContract; } - set { _isInContract = value; } - } - - public Vector3 ContactNormalWS - { - get { return _contractNormalWS; } - set { _contractNormalWS = value; } - } - - public Vector3 ContactPointWS - { - get { return _contractPointWS; } - set { _contractPointWS = value; } - } - - public Vector3 HardPointWS - { - get { return _hardPointWS; } - set { _hardPointWS = value; } - } - - public Vector3 WheelDirectionWS - { - get { return _wheelDirectionWS; } - set { _wheelDirectionWS = value; } - } - - public Vector3 WheelAxleWS - { - get { return _wheelAxleWS; } - set { _wheelAxleWS = value; } - } - #endregion - } - - public struct WheelInfo - { - private RaycastInfo _raycastInfo; - - private Matrix _worldTransform; - - private Vector3 _chassicConnectionPointCS; - private Vector3 _wheelDirectionCS; - private Vector3 _wheelAxleCS; - - private Single _suspensionRestLength; - private Single _maxSuspensionTravelCm; - - private Single _wheelsRadius; - private Single _rollInfluence; - private Single _suspensionStiffness; - private Single _wheelsDampingCompression; - private Single _wheelsDampingRelaxation; - private Single _frictionSlip; - private Single _steering; - private Single _rotation; - private Single _deltaRotation; - - private Single _engineForce; - private Single _brake; - private Boolean _isFrontWheel; - - - private Single _clippedInvContactDotSuspension; - private Single _skidInfo; - private Single _wheelsSuspensionForce; - private Single _suspensionRelativeVelocity; - //can be used to store pointer to sync transforms... - private object _clientInfo; - - #region Constructor - public WheelInfo(WheelInfoConstructionInfo constructionInfo) - { - _suspensionRestLength = constructionInfo.SuspensionRestLength; - _maxSuspensionTravelCm = constructionInfo.MaxSuspensionTravelCm; - - _wheelsRadius = constructionInfo.WheelRadius; - _wheelsDampingCompression = constructionInfo.WheelsDampingCompression; - _wheelsDampingRelaxation = constructionInfo.WheelsDampingRelaxation; - _wheelDirectionCS = constructionInfo.WheelDirectionCS; - - _suspensionStiffness = constructionInfo.SuspensionStiffness; - _chassicConnectionPointCS = constructionInfo.ChassicConnectionCS; - - _wheelAxleCS = constructionInfo.WheelAxleCS; - _frictionSlip = constructionInfo.FrictionSlip; - - _clippedInvContactDotSuspension = 0; - _suspensionRelativeVelocity = 0; - _wheelsSuspensionForce = 0; - _skidInfo = 0; - - _steering = 0; - _engineForce = 0; - _rotation = 0; - _rotation = 0; - _deltaRotation = 0; - _brake = 0; - _rollInfluence = 0.1f; - _brake = 0; - _rollInfluence = 0.1f; - - _isFrontWheel = constructionInfo.IsFrontWheel; - - _raycastInfo = default(RaycastInfo); - _worldTransform = default(Matrix); - _clientInfo = null; - } - #endregion - - #region BasicProperties - public object ClientInfo { get { return _clientInfo; } set { _clientInfo = value; } } - - public RaycastInfo RaycastInfo - { - get { return _raycastInfo; } - set { _raycastInfo = value; } - } - - public Matrix WorldTransform - { - get { return _worldTransform; } - set { _worldTransform = value; } - } - - public Vector3 ChassicConnectionPointCS - { - get { return _chassicConnectionPointCS; } - set { _chassicConnectionPointCS = value; } - } - public Vector3 WheelDirectionCS - { - get { return _wheelDirectionCS; } - set { _wheelDirectionCS = value; } - } - public Vector3 WheelAxleCS - { - get { return _wheelAxleCS; } - set { _wheelAxleCS = value; } - } - - public Single SuspensionRestLength - { - get { return _suspensionRestLength; } - set { _suspensionRestLength = value; } - } - - - public Single MaxSuspensionTravelCm - { - get { return _maxSuspensionTravelCm; } - set { _maxSuspensionTravelCm = value; } - } - - public Single WheelsRadius - { - get { return _wheelsRadius; } - set { _wheelsRadius = value; } - } - - public Single SuspensionStiffness - { - get { return _suspensionStiffness; } - set { _suspensionStiffness = value; } - } - - public Single WheelsDampingCompression - { - get { return _wheelsDampingCompression; } - set { _wheelsDampingCompression = value; } - } - - public Single WheelsDampingRelaxation - { - get { return _wheelsDampingRelaxation; } - set { _wheelsDampingRelaxation = value; } - } - - public Single FrictionSlip - { - get { return _frictionSlip; } - set { _frictionSlip = value; } - } - - public Single Steering - { - get { return _steering; } - set { _steering = value; } - } - - public Single Rotation - { - get { return _rotation; } - set { _rotation = value; } - } - - public Single DeltaRotation - { - get { return _deltaRotation; } - set { _deltaRotation = value; } - } - - public Single RollInfluence - { - get { return _rollInfluence; } - set { _rollInfluence = value; } - } - - public Single EngineForce - { - get { return _engineForce; } - set { _engineForce = value; } - } - - public Single Brake - { - get { return _brake; } - set { _brake = value; } - } - - public Boolean IsFrontWheel - { - get { return _isFrontWheel; } - set { _isFrontWheel = value; } - } - - public Single ClippedInvContactDotSuspension - { - get { return _clippedInvContactDotSuspension; } - set { _clippedInvContactDotSuspension = value; } - } - - public Single SuspensionRelativeVelocity - { - get { return _suspensionRelativeVelocity; } - set { _suspensionRelativeVelocity = value; } - } - - public Single WheelsSuspensionForce - { - get { return _wheelsSuspensionForce; } - set { _wheelsSuspensionForce = value; } - } - - public Single SkidInfo - { - get { return _skidInfo; } - set { _skidInfo = value; } - } - #endregion - - /// - /// - /// - /// - /// Not used! - public void UpdateWheel(RigidBody chassis, RaycastInfo paramRaycastInfo) - { - if (_raycastInfo.IsInContact) - { - float project = Vector3.Dot(_raycastInfo.ContactNormalWS, _raycastInfo.WheelDirectionWS); - - Vector3 chassisVelocityAtContactPoint = new Vector3(); - Vector3 relpos = _raycastInfo.ContactPointWS - chassis.CenterOfMassPosition; - chassisVelocityAtContactPoint = chassis.GetVelocityInLocalPoint(relpos); - float projVel = Vector3.Dot(_raycastInfo.ContactNormalWS, chassisVelocityAtContactPoint); - - if (project >= -0.1f) - { - _suspensionRelativeVelocity = 0; - _clippedInvContactDotSuspension = 1.0f / 0.1f; - } - else - { - float inv = -1 / project; - _suspensionRelativeVelocity = projVel * inv; - _clippedInvContactDotSuspension = inv; - } - } - else - { - _raycastInfo.SuspensionLength = _suspensionRestLength; - _suspensionRelativeVelocity = 0.0f; - _raycastInfo.ContactNormalWS = -_raycastInfo.WheelDirectionWS; - _clippedInvContactDotSuspension = 1.0f; - } - } - - // if (m_raycastInfo.m_isInContact) - - //{ - // btScalar project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS ); - // btVector3 chassis_velocity_at_contactPoint; - // btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition(); - // chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos ); - // btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); - // if ( project >= -0.1f) - // { - // m_suspensionRelativeVelocity = 0.0f; - // m_clippedInvContactDotSuspension = 1.0f / 0.1f; - // } - // else - // { - // btScalar inv = -1.f / project; - // m_suspensionRelativeVelocity = projVel * inv; - // m_clippedInvContactDotSuspension = inv; - // } - - //} - - //else // Not in contact : position wheel in a nice (rest length) position - //{ - // m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength(); - // m_suspensionRelativeVelocity = 0.0f; - // m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS; - // m_clippedInvContactDotSuspension = 1.0f; - //} - }; - - //btScalar m_clippedInvContactDotSuspension; - //btScalar m_suspensionRelativeVelocity; - //btScalar m_wheelsSuspensionForce; - //btScalar m_skidInfo; - - //void* m_clientInfo;//can be used to store pointer to sync transforms... - - //btWheelInfo(btWheelInfoConstructionInfo& ci) - - //{ - - // m_suspensionRestLength1 = ci.m_suspensionRestLength; - // m_maxSuspensionTravelCm = ci.m_maxSuspensionTravelCm; - - // m_wheelsRadius = ci.m_wheelRadius; - // m_suspensionStiffness = ci.m_suspensionStiffness; - // m_wheelsDampingCompression = ci.m_wheelsDampingCompression; - // m_wheelsDampingRelaxation = ci.m_wheelsDampingRelaxation; - // m_chassisConnectionPointCS = ci.m_chassisConnectionCS; - // m_wheelDirectionCS = ci.m_wheelDirectionCS; - // m_wheelAxleCS = ci.m_wheelAxleCS; - // m_frictionSlip = ci.m_frictionSlip; - // m_steering = 0.f; - // m_engineForce = 0.f; - // m_rotation = 0.f; - // m_deltaRotation = 0.f; - // m_brake = 0.f; - // m_rollInfluence = 0.1f; - // m_bIsFrontWheel = ci.m_bIsFrontWheel; - - //} - - //void updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo); - - //btScalar m_clippedInvContactDotSuspension; - //btScalar m_suspensionRelativeVelocity; - ////calculated by suspension - //btScalar m_wheelsSuspensionForce; - //btScalar m_skidInfo; - - //}; - - //struct RaycastInfo - //{ - // //set by raycaster - // btVector3 m_contactNormalWS;//contactnormal - // btVector3 m_contactPointWS;//raycast hitpoint - // btScalar m_suspensionLength; - // btVector3 m_hardPointWS;//raycast starting point - // btVector3 m_wheelDirectionWS; //direction in worldspace - // btVector3 m_wheelAxleWS; // axle in worldspace - // bool m_isInContact; - // void* m_groundObject; //could be general void* ptr - //}; - - //struct btWheelInfoConstructionInfo - //{ - // btVector3 m_chassisConnectionCS; - // btVector3 m_wheelDirectionCS; - // btVector3 m_wheelAxleCS; - // btScalar m_suspensionRestLength; - // btScalar m_maxSuspensionTravelCm; - // btScalar m_wheelRadius; - - // float m_suspensionStiffness; - // float m_wheelsDampingCompression; - // float m_wheelsDampingRelaxation; - // float m_frictionSlip; - // bool m_bIsFrontWheel; - - //}; -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX.Dynamics +{ + public struct WheelInfoConstructionInfo + { + private Vector3 _chassicConnectionCS; + private Vector3 _wheelDirectionCS; + private Vector3 _wheelAxisCS; + + private Single _suspensionRestLength; + private Single _maxSuspensionTravelCm; + private Single _wheelRadius; + private Single _suspensionStiffness; + private Single _wheelsDampingCompression; + private Single _wheelsDampingRelaxation; + private Single _frictionSlip; + + private Boolean _isFrontWheel; + + #region Basic Properties + public Vector3 ChassicConnectionCS + { + get { return _chassicConnectionCS; } + set { _chassicConnectionCS = value; } + } + + public Vector3 WheelDirectionCS + { + get { return _wheelDirectionCS; } + set { _wheelDirectionCS = value; } + } + + public Vector3 WheelAxleCS + { + get { return _wheelAxisCS; } + set { _wheelAxisCS = value; } + } + + + public Single SuspensionRestLength + { + get { return _suspensionRestLength; } + set { _suspensionRestLength = value; } + } + + public Single MaxSuspensionTravelCm + { + get { return _maxSuspensionTravelCm; } + set { _maxSuspensionTravelCm = value; } + } + + public Single WheelRadius + { + get { return _wheelRadius; } + set { _wheelRadius = value; } + } + + + public Single SuspensionStiffness + { + get { return _suspensionStiffness; } + set { _suspensionStiffness = value; } + } + + public Single WheelsDampingCompression + { + get { return _wheelsDampingCompression; } + set { _wheelsDampingCompression = value; } + } + + public Single WheelsDampingRelaxation + { + get { return _wheelsDampingRelaxation; } + set { _wheelsDampingRelaxation = value; } + } + + public Single FrictionSlip + { + get { return _frictionSlip; } + set { _frictionSlip = value; } + } + + + public Boolean IsFrontWheel + { + get { return _isFrontWheel; } + set { _isFrontWheel = value; } + } + #endregion + } + + public struct RaycastInfo + { + private Vector3 _contractNormalWS; + private Vector3 _contractPointWS; + + private Vector3 _hardPointWS; + private Vector3 _wheelDirectionWS; + private Vector3 _wheelAxleWS; + + private Single _suspensionLength; + private Boolean _isInContract; + + #region Basic Properties + public Single SuspensionLength + { + get { return _suspensionLength; } + set { _suspensionLength = value; } + } + + public Boolean IsInContact + { + get { return _isInContract; } + set { _isInContract = value; } + } + + public Vector3 ContactNormalWS + { + get { return _contractNormalWS; } + set { _contractNormalWS = value; } + } + + public Vector3 ContactPointWS + { + get { return _contractPointWS; } + set { _contractPointWS = value; } + } + + public Vector3 HardPointWS + { + get { return _hardPointWS; } + set { _hardPointWS = value; } + } + + public Vector3 WheelDirectionWS + { + get { return _wheelDirectionWS; } + set { _wheelDirectionWS = value; } + } + + public Vector3 WheelAxleWS + { + get { return _wheelAxleWS; } + set { _wheelAxleWS = value; } + } + #endregion + } + + public struct WheelInfo + { + private RaycastInfo _raycastInfo; + + private Matrix _worldTransform; + + private Vector3 _chassicConnectionPointCS; + private Vector3 _wheelDirectionCS; + private Vector3 _wheelAxleCS; + + private Single _suspensionRestLength; + private Single _maxSuspensionTravelCm; + + private Single _wheelsRadius; + private Single _rollInfluence; + private Single _suspensionStiffness; + private Single _wheelsDampingCompression; + private Single _wheelsDampingRelaxation; + private Single _frictionSlip; + private Single _steering; + private Single _rotation; + private Single _deltaRotation; + + private Single _engineForce; + private Single _brake; + private Boolean _isFrontWheel; + + + private Single _clippedInvContactDotSuspension; + private Single _skidInfo; + private Single _wheelsSuspensionForce; + private Single _suspensionRelativeVelocity; + //can be used to store pointer to sync transforms... + private object _clientInfo; + + #region Constructor + public WheelInfo(WheelInfoConstructionInfo constructionInfo) + { + _suspensionRestLength = constructionInfo.SuspensionRestLength; + _maxSuspensionTravelCm = constructionInfo.MaxSuspensionTravelCm; + + _wheelsRadius = constructionInfo.WheelRadius; + _wheelsDampingCompression = constructionInfo.WheelsDampingCompression; + _wheelsDampingRelaxation = constructionInfo.WheelsDampingRelaxation; + _wheelDirectionCS = constructionInfo.WheelDirectionCS; + + _suspensionStiffness = constructionInfo.SuspensionStiffness; + _chassicConnectionPointCS = constructionInfo.ChassicConnectionCS; + + _wheelAxleCS = constructionInfo.WheelAxleCS; + _frictionSlip = constructionInfo.FrictionSlip; + + _clippedInvContactDotSuspension = 0; + _suspensionRelativeVelocity = 0; + _wheelsSuspensionForce = 0; + _skidInfo = 0; + + _steering = 0; + _engineForce = 0; + _rotation = 0; + _rotation = 0; + _deltaRotation = 0; + _brake = 0; + _rollInfluence = 0.1f; + _brake = 0; + _rollInfluence = 0.1f; + + _isFrontWheel = constructionInfo.IsFrontWheel; + + _raycastInfo = default(RaycastInfo); + _worldTransform = default(Matrix); + _clientInfo = null; + } + #endregion + + #region BasicProperties + public object ClientInfo { get { return _clientInfo; } set { _clientInfo = value; } } + + public RaycastInfo RaycastInfo + { + get { return _raycastInfo; } + set { _raycastInfo = value; } + } + + public Matrix WorldTransform + { + get { return _worldTransform; } + set { _worldTransform = value; } + } + + public Vector3 ChassicConnectionPointCS + { + get { return _chassicConnectionPointCS; } + set { _chassicConnectionPointCS = value; } + } + public Vector3 WheelDirectionCS + { + get { return _wheelDirectionCS; } + set { _wheelDirectionCS = value; } + } + public Vector3 WheelAxleCS + { + get { return _wheelAxleCS; } + set { _wheelAxleCS = value; } + } + + public Single SuspensionRestLength + { + get { return _suspensionRestLength; } + set { _suspensionRestLength = value; } + } + + + public Single MaxSuspensionTravelCm + { + get { return _maxSuspensionTravelCm; } + set { _maxSuspensionTravelCm = value; } + } + + public Single WheelsRadius + { + get { return _wheelsRadius; } + set { _wheelsRadius = value; } + } + + public Single SuspensionStiffness + { + get { return _suspensionStiffness; } + set { _suspensionStiffness = value; } + } + + public Single WheelsDampingCompression + { + get { return _wheelsDampingCompression; } + set { _wheelsDampingCompression = value; } + } + + public Single WheelsDampingRelaxation + { + get { return _wheelsDampingRelaxation; } + set { _wheelsDampingRelaxation = value; } + } + + public Single FrictionSlip + { + get { return _frictionSlip; } + set { _frictionSlip = value; } + } + + public Single Steering + { + get { return _steering; } + set { _steering = value; } + } + + public Single Rotation + { + get { return _rotation; } + set { _rotation = value; } + } + + public Single DeltaRotation + { + get { return _deltaRotation; } + set { _deltaRotation = value; } + } + + public Single RollInfluence + { + get { return _rollInfluence; } + set { _rollInfluence = value; } + } + + public Single EngineForce + { + get { return _engineForce; } + set { _engineForce = value; } + } + + public Single Brake + { + get { return _brake; } + set { _brake = value; } + } + + public Boolean IsFrontWheel + { + get { return _isFrontWheel; } + set { _isFrontWheel = value; } + } + + public Single ClippedInvContactDotSuspension + { + get { return _clippedInvContactDotSuspension; } + set { _clippedInvContactDotSuspension = value; } + } + + public Single SuspensionRelativeVelocity + { + get { return _suspensionRelativeVelocity; } + set { _suspensionRelativeVelocity = value; } + } + + public Single WheelsSuspensionForce + { + get { return _wheelsSuspensionForce; } + set { _wheelsSuspensionForce = value; } + } + + public Single SkidInfo + { + get { return _skidInfo; } + set { _skidInfo = value; } + } + #endregion + + /// + /// + /// + /// + /// Not used! + public void UpdateWheel(RigidBody chassis, RaycastInfo paramRaycastInfo) + { + if (_raycastInfo.IsInContact) + { + float project = Vector3.Dot(_raycastInfo.ContactNormalWS, _raycastInfo.WheelDirectionWS); + + Vector3 chassisVelocityAtContactPoint = new Vector3(); + Vector3 relpos = _raycastInfo.ContactPointWS - chassis.CenterOfMassPosition; + chassisVelocityAtContactPoint = chassis.GetVelocityInLocalPoint(relpos); + float projVel = Vector3.Dot(_raycastInfo.ContactNormalWS, chassisVelocityAtContactPoint); + + if (project >= -0.1f) + { + _suspensionRelativeVelocity = 0; + _clippedInvContactDotSuspension = 1.0f / 0.1f; + } + else + { + float inv = -1 / project; + _suspensionRelativeVelocity = projVel * inv; + _clippedInvContactDotSuspension = inv; + } + } + else + { + _raycastInfo.SuspensionLength = _suspensionRestLength; + _suspensionRelativeVelocity = 0.0f; + _raycastInfo.ContactNormalWS = -_raycastInfo.WheelDirectionWS; + _clippedInvContactDotSuspension = 1.0f; + } + } + + // if (m_raycastInfo.m_isInContact) + + //{ + // btScalar project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS ); + // btVector3 chassis_velocity_at_contactPoint; + // btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition(); + // chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos ); + // btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); + // if ( project >= -0.1f) + // { + // m_suspensionRelativeVelocity = 0.0f; + // m_clippedInvContactDotSuspension = 1.0f / 0.1f; + // } + // else + // { + // btScalar inv = -1.f / project; + // m_suspensionRelativeVelocity = projVel * inv; + // m_clippedInvContactDotSuspension = inv; + // } + + //} + + //else // Not in contact : position wheel in a nice (rest length) position + //{ + // m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength(); + // m_suspensionRelativeVelocity = 0.0f; + // m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS; + // m_clippedInvContactDotSuspension = 1.0f; + //} + }; + + //btScalar m_clippedInvContactDotSuspension; + //btScalar m_suspensionRelativeVelocity; + //btScalar m_wheelsSuspensionForce; + //btScalar m_skidInfo; + + //void* m_clientInfo;//can be used to store pointer to sync transforms... + + //btWheelInfo(btWheelInfoConstructionInfo& ci) + + //{ + + // m_suspensionRestLength1 = ci.m_suspensionRestLength; + // m_maxSuspensionTravelCm = ci.m_maxSuspensionTravelCm; + + // m_wheelsRadius = ci.m_wheelRadius; + // m_suspensionStiffness = ci.m_suspensionStiffness; + // m_wheelsDampingCompression = ci.m_wheelsDampingCompression; + // m_wheelsDampingRelaxation = ci.m_wheelsDampingRelaxation; + // m_chassisConnectionPointCS = ci.m_chassisConnectionCS; + // m_wheelDirectionCS = ci.m_wheelDirectionCS; + // m_wheelAxleCS = ci.m_wheelAxleCS; + // m_frictionSlip = ci.m_frictionSlip; + // m_steering = 0.f; + // m_engineForce = 0.f; + // m_rotation = 0.f; + // m_deltaRotation = 0.f; + // m_brake = 0.f; + // m_rollInfluence = 0.1f; + // m_bIsFrontWheel = ci.m_bIsFrontWheel; + + //} + + //void updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo); + + //btScalar m_clippedInvContactDotSuspension; + //btScalar m_suspensionRelativeVelocity; + ////calculated by suspension + //btScalar m_wheelsSuspensionForce; + //btScalar m_skidInfo; + + //}; + + //struct RaycastInfo + //{ + // //set by raycaster + // btVector3 m_contactNormalWS;//contactnormal + // btVector3 m_contactPointWS;//raycast hitpoint + // btScalar m_suspensionLength; + // btVector3 m_hardPointWS;//raycast starting point + // btVector3 m_wheelDirectionWS; //direction in worldspace + // btVector3 m_wheelAxleWS; // axle in worldspace + // bool m_isInContact; + // void* m_groundObject; //could be general void* ptr + //}; + + //struct btWheelInfoConstructionInfo + //{ + // btVector3 m_chassisConnectionCS; + // btVector3 m_wheelDirectionCS; + // btVector3 m_wheelAxleCS; + // btScalar m_suspensionRestLength; + // btScalar m_maxSuspensionTravelCm; + // btScalar m_wheelRadius; + + // float m_suspensionStiffness; + // float m_wheelsDampingCompression; + // float m_wheelsDampingRelaxation; + // float m_frictionSlip; + // bool m_bIsFrontWheel; + + //}; +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Exceptions/BulletException.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Exceptions/BulletException.cs index abd4885..3ae3ad6 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Exceptions/BulletException.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Exceptions/BulletException.cs @@ -1,53 +1,53 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using System.Runtime.Serialization; - -namespace XnaDevRu.BulletX -{ - public class BulletException : Exception - { - internal BulletException() - : base("Bullet Physics Library has thrown an exception.") - { - } - - internal BulletException(string message) - : base(message) - { - } - - internal BulletException(string message, Exception innerException) - : base(message, innerException) - { - } - -#if !XBOX - internal BulletException(SerializationInfo info, StreamingContext context) - : base(info, context) - { - } -#endif - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using System.Runtime.Serialization; + +namespace XnaDevRu.BulletX +{ + public class BulletException : Exception + { + internal BulletException() + : base("Bullet Physics Library has thrown an exception.") + { + } + + internal BulletException(string message) + : base(message) + { + } + + internal BulletException(string message, Exception innerException) + : base(message, innerException) + { + } + +#if !XBOX + internal BulletException(SerializationInfo info, StreamingContext context) + : base(info, context) + { + } +#endif + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/IDebugDraw.cs b/libraries/ModifiedBulletX/ModifiedBulletX/IDebugDraw.cs index ef5b5dc..5af979c 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/IDebugDraw.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/IDebugDraw.cs @@ -1,60 +1,60 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public enum DebugDrawModes - { - NoDebug = 0, - DrawWireframe = 1, - DrawAabb = 2, - DrawFeaturesText = 4, - DrawContactPoints = 8, - NoDeactivation = 16, - NoHelpText = 32, - DrawText = 64, - ProfileTimings = 128, - EnableSatComparison = 256, - DisableBulletLcp = 512, - EnableCcd = 1024, - MaxDebugDrawMode - } - - public interface IDebugDraw - { - void DrawLine(Vector3 from, Vector3 to, Vector3 color); - - void DrawContactPoint( - Vector3 pointOnB, - Vector3 normalOnB, - float distance, - int lifeTime, - Vector3 color - ); - - DebugDrawModes DebugMode { get; set; } - } +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public enum DebugDrawModes + { + NoDebug = 0, + DrawWireframe = 1, + DrawAabb = 2, + DrawFeaturesText = 4, + DrawContactPoints = 8, + NoDeactivation = 16, + NoHelpText = 32, + DrawText = 64, + ProfileTimings = 128, + EnableSatComparison = 256, + DisableBulletLcp = 512, + EnableCcd = 1024, + MaxDebugDrawMode + } + + public interface IDebugDraw + { + void DrawLine(Vector3 from, Vector3 to, Vector3 color); + + void DrawContactPoint( + Vector3 pointOnB, + Vector3 normalOnB, + float distance, + int lifeTime, + Vector3 color + ); + + DebugDrawModes DebugMode { get; set; } + } } \ No newline at end of file diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/DefaultMotionState.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/DefaultMotionState.cs index 2a45a20..73dc675 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/DefaultMotionState.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/DefaultMotionState.cs @@ -1,66 +1,66 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - // DefaultMotionState provides a common implementation to synchronize world transforms with offsets - public class DefaultMotionState : MotionState - { - private Matrix _graphicsWorldTransform; - private Matrix _centerOfMassOffset; - private Matrix _startWorldTransform; - private object _userData; - - public DefaultMotionState() - : this(Matrix.Identity, Matrix.Identity) { } - - public DefaultMotionState(Matrix startTransform, Matrix centerOfMassOffset) - { - _graphicsWorldTransform = startTransform; - _centerOfMassOffset = centerOfMassOffset; - _startWorldTransform = startTransform; - } - - public Matrix GraphicsWorldTransform { get { return _graphicsWorldTransform; } set { _graphicsWorldTransform = value; } } - public Matrix CenterOfMassOffset { get { return _centerOfMassOffset; } set { _centerOfMassOffset = value; } } - public Matrix StartWorldTransform { get { return _startWorldTransform; } set { _startWorldTransform = value; } } - public object UserData { get { return _userData; } set { _userData = value; } } - - // synchronizes world transform from user to physics - public override void GetWorldTransform(out Matrix centerOfMassWorldTrans) - { - centerOfMassWorldTrans = MathHelper.InvertMatrix(_centerOfMassOffset) * _graphicsWorldTransform; - } - - // synchronizes world transform from physics to user - // Bullet only calls the update of worldtransform for active objects - public override void SetWorldTransform(Matrix centerOfMassWorldTrans) - { - _graphicsWorldTransform = MatrixOperations.Multiply(centerOfMassWorldTrans, _centerOfMassOffset); - _graphicsWorldTransform.Translation = centerOfMassWorldTrans.Translation; - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + // DefaultMotionState provides a common implementation to synchronize world transforms with offsets + public class DefaultMotionState : MotionState + { + private Matrix _graphicsWorldTransform; + private Matrix _centerOfMassOffset; + private Matrix _startWorldTransform; + private object _userData; + + public DefaultMotionState() + : this(Matrix.Identity, Matrix.Identity) { } + + public DefaultMotionState(Matrix startTransform, Matrix centerOfMassOffset) + { + _graphicsWorldTransform = startTransform; + _centerOfMassOffset = centerOfMassOffset; + _startWorldTransform = startTransform; + } + + public Matrix GraphicsWorldTransform { get { return _graphicsWorldTransform; } set { _graphicsWorldTransform = value; } } + public Matrix CenterOfMassOffset { get { return _centerOfMassOffset; } set { _centerOfMassOffset = value; } } + public Matrix StartWorldTransform { get { return _startWorldTransform; } set { _startWorldTransform = value; } } + public object UserData { get { return _userData; } set { _userData = value; } } + + // synchronizes world transform from user to physics + public override void GetWorldTransform(out Matrix centerOfMassWorldTrans) + { + centerOfMassWorldTrans = MathHelper.InvertMatrix(_centerOfMassOffset) * _graphicsWorldTransform; + } + + // synchronizes world transform from physics to user + // Bullet only calls the update of worldtransform for active objects + public override void SetWorldTransform(Matrix centerOfMassWorldTrans) + { + _graphicsWorldTransform = MatrixOperations.Multiply(centerOfMassWorldTrans, _centerOfMassOffset); + _graphicsWorldTransform.Translation = centerOfMassWorldTrans.Translation; + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs index 2e3f258..15c7bb8 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs @@ -1,581 +1,581 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; - -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public static class MathHelper - { - internal const float Sqrt12 = 0.7071067811865475244008443621048490f; - internal const float Infinity = 3.402823466e+38f; - internal const float Epsilon = 1.192092896e-07f; - - public static Vector3 MatrixToVector(Matrix m, Vector3 v) - { - return new Vector3( - Vector3.Dot(new Vector3(m.M11, m.M12, m.M13), v) + m.Translation.X, - Vector3.Dot(new Vector3(m.M21, m.M22, m.M23), v) + m.Translation.Y, - Vector3.Dot(new Vector3(m.M31, m.M32, m.M33), v) + m.Translation.Z - ); - } - - internal static int ClosestAxis(Vector4 v) - { - return MaxAxis(Absolute(v)); - } - - internal static Vector4 Absolute(Vector4 v) - { - return new Vector4(Math.Abs(v.X), Math.Abs(v.Y), Math.Abs(v.Z), Math.Abs(v.W)); - } - - internal static int MaxAxis(Vector4 v) - { - int maxIndex = -1; - float maxVal = float.MinValue; - if (v.X > maxVal) - { - maxIndex = 0; - maxVal = v.X; - } - if (v.Y > maxVal) - { - maxIndex = 1; - maxVal = v.Y; - } - if (v.Z > maxVal) - { - maxIndex = 2; - maxVal = v.Z; - } - if (v.W > maxVal) - { - maxIndex = 3; - maxVal = v.W; - } - - return maxIndex; - } - - internal static int MaxAxis(Vector3 v) - { - return v.X < v.Y ? (v.Y < v.Z ? 2 : 1) : (v.X < v.Z ? 2 : 0); - } - - // conservative test for overlap between two aabbs - internal static bool TestAabbAgainstAabb2(Vector3 aabbMinA, Vector3 aabbMaxA, Vector3 aabbMinB, Vector3 aabbMaxB) - { - bool overlap = true; - overlap = (aabbMinA.X > aabbMaxB.X || aabbMaxA.X < aabbMinB.X) ? false : overlap; - overlap = (aabbMinA.Z > aabbMaxB.Z || aabbMaxA.Z < aabbMinB.Z) ? false : overlap; - overlap = (aabbMinA.Y > aabbMaxB.Y || aabbMaxA.Y < aabbMinB.Y) ? false : overlap; - return overlap; - } - - internal static bool TestTriangleAgainstAabb2(Vector3[] vertices, Vector3 aabbMin, Vector3 aabbMax) - { - Vector3 p1 = vertices[0]; - Vector3 p2 = vertices[1]; - Vector3 p3 = vertices[2]; - - if (Math.Min(Math.Min(p1.X, p2.X), p3.X) > aabbMax.X) return false; - if (Math.Max(Math.Max(p1.X, p2.X), p3.X) < aabbMin.X) return false; - - if (Math.Min(Math.Min(p1.Z, p2.Z), p3.Z) > aabbMax.Z) return false; - if (Math.Max(Math.Max(p1.Z, p2.Z), p3.Z) < aabbMin.Z) return false; - - if (Math.Min(Math.Min(p1.Y, p2.Y), p3.Y) > aabbMax.Y) return false; - if (Math.Max(Math.Max(p1.Y, p2.Y), p3.Y) < aabbMin.Y) return false; - return true; - } - - internal static void SetInterpolate3(Vector3 vA, Vector3 vB, float rt, ref Vector3 interpolated) - { - float s = 1.0f - rt; - interpolated.X = s * vA.X + rt * vB.X; - interpolated.Y = s * vA.Y + rt * vB.Y; - interpolated.Z = s * vA.Z + rt * vB.Z; - } - - internal static void PlaneSpace1(Vector3 n, ref Vector3 p, ref Vector3 q) - { - if (Math.Abs(n.Z) > Sqrt12) - { - // choose p in y-z plane - float a = n.Y * n.Y + n.Z * n.Z; - float k = 1f / (float)Math.Sqrt(a); - p.X = 0; - p.Y = -n.Z * k; - p.Z = n.Y * k; - // set q = n x p - q.X = a * k; - q.Y = -n.X * p.Z; - q.Z = n.X * p.Y; - } - else - { - // choose p in x-y plane - float a = n.X * n.X + n.Y * n.Y; - float k = 1f / (float)Math.Sqrt(a); - p.X = -n.Y * k; - p.Y = n.X * k; - p.Z = 0; - // set q = n x p - q.X = -n.Z * p.Y; - q.Y = n.Z * p.X; - q.Z = a * k; - } - } - - internal static bool RayAabb(Vector3 rayFrom, - Vector3 rayTo, - Vector3 aabbMin, - Vector3 aabbMax, - float param, Vector3 normal) - { - Vector3 aabbHalfExtent = (aabbMax - aabbMin) * 0.5f; - Vector3 aabbCenter = (aabbMax + aabbMin) * 0.5f; - Vector3 source = rayFrom - aabbCenter; - Vector3 target = rayTo - aabbCenter; - int sourceOutcode = Outcode(source, aabbHalfExtent); - int targetOutcode = Outcode(target, aabbHalfExtent); - if ((sourceOutcode & targetOutcode) == 0x0) - { - float lambda_enter = 0; - float lambda_exit = param; - Vector3 r = target - source; - float normSign = 1; - Vector3 hitNormal = new Vector3(); - int bit = 1; - - for (int j = 0; j < 2; j++) - { - { - if ((sourceOutcode & bit) != 0) - { - float lambda = (-source.X - aabbHalfExtent.X * normSign) / r.X; - if (lambda_enter <= lambda) - { - lambda_enter = lambda; - hitNormal = new Vector3(); - hitNormal.X = normSign; - } - } - else if ((targetOutcode & bit) != 0) - { - float lambda = (-source.X - aabbHalfExtent.X * normSign) / r.X; - SetMin(ref lambda_exit, lambda); - } - bit <<= 1; - } - { - if ((sourceOutcode & bit) != 0) - { - float lambda = (-source.Y - aabbHalfExtent.Y * normSign) / r.Y; - if (lambda_enter <= lambda) - { - lambda_enter = lambda; - hitNormal = new Vector3(); - hitNormal.Y = normSign; - } - } - else if ((targetOutcode & bit) != 0) - { - float lambda = (-source.Y - aabbHalfExtent.Y * normSign) / r.Y; - SetMin(ref lambda_exit, lambda); - } - bit <<= 1; - } - { - if ((sourceOutcode & bit) != 0) - { - float lambda = (-source.Z - aabbHalfExtent.Z * normSign) / r.Z; - if (lambda_enter <= lambda) - { - lambda_enter = lambda; - hitNormal = new Vector3(); - hitNormal.Z = normSign; - } - } - else if ((targetOutcode & bit) != 0) - { - float lambda = (-source.Z - aabbHalfExtent.Z * normSign) / r.Z; - SetMin(ref lambda_exit, lambda); - } - bit <<= 1; - } - normSign = -1; - } - if (lambda_enter <= lambda_exit) - { - param = lambda_enter; - normal = hitNormal; - return true; - } - } - return false; - } - - internal static void SetMin(ref float a, float b) - { - if (a > b) - a = b; - } - - internal static void SetMax(ref float a, float b) - { - if (a < b) - a = b; - } - - internal static void SetMax(ref Vector3 self, Vector3 other) - { - if (other.X > self.X) - self.X = other.X; - - if (other.Y > self.Y) - self.Y = other.Y; - - if (other.Z > self.Z) - self.Z = other.Z; - } - - internal static Vector3 SetMax(Vector3 self, Vector3 other) - { - if (other.X > self.X) - self.X = other.X; - - if (other.Y > self.Y) - self.Y = other.Y; - - if (other.Z > self.Z) - self.Z = other.Z; - - return self; - } - - internal static void SetMin(ref Vector3 self, Vector3 other) - { - if (other.X < self.X) - self.X = other.X; - - if (other.Y < self.Y) - self.Y = other.Y; - - if (other.Z < self.Z) - self.Z = other.Z; - } - - internal static Vector3 SetMin(Vector3 self, Vector3 other) - { - if (other.X < self.X) - self.X = other.X; - - if (other.Y < self.Y) - self.Y = other.Y; - - if (other.Z < self.Z) - self.Z = other.Z; - - return self; - } - - internal static int Outcode(Vector3 p, Vector3 halfExtent) - { - return (p.X < -halfExtent.X ? 0x01 : 0x0) | - (p.X > halfExtent.X ? 0x08 : 0x0) | - (p.Y < -halfExtent.Y ? 0x02 : 0x0) | - (p.Y > halfExtent.Y ? 0x10 : 0x0) | - (p.Z < -halfExtent.Z ? 0x4 : 0x0) | - (p.Z > halfExtent.Z ? 0x20 : 0x0); - } - - internal static Matrix Absolute(Matrix m) - { - return new Matrix(Math.Abs(m.M11), Math.Abs(m.M12), Math.Abs(m.M13), Math.Abs(m.M14), - Math.Abs(m.M21), Math.Abs(m.M22), Math.Abs(m.M23), Math.Abs(m.M24), - Math.Abs(m.M31), Math.Abs(m.M32), Math.Abs(m.M33), Math.Abs(m.M34), - Math.Abs(m.M41), Math.Abs(m.M42), Math.Abs(m.M43), Math.Abs(m.M44)); - } - - internal static void SetValueByIndex(ref Vector3 v, int i, float value) - { - if (i == 0) - v.X = value; - else if (i == 1) - v.Y = value; - else - v.Z = value; - } - - internal static float GetValueByIndex(Vector3 v, int i) - { - if (i == 0) - return v.X; - else if (i == 1) - return v.Y; - else - return v.Z; - } - - internal static Vector3 InvXForm(Matrix m, Vector3 v) - { - v -= m.Translation; - m.Translation = new Vector3(); - return MathHelper.Transform(v, Matrix.Transpose(m)); - } - - internal static Matrix InverseTimes(Matrix m, Matrix t) - { - Vector3 v = t.Translation - m.Translation; - - Matrix mat = TransposeTimes(m, t); - mat.Translation = Vector3.Transform(v, m); - return mat; - } - - internal static Matrix TransposeTimes(Matrix mA, Matrix mB) - { - return new Matrix( - mA.M11 * mB.M11 + mA.M21 * mB.M21 + mA.M31 * mB.M31, - mA.M11 * mB.M12 + mA.M21 * mB.M22 + mA.M31 * mB.M32, - mA.M11 * mB.M13 + mA.M21 * mB.M23 + mA.M31 * mB.M33, - 0, - mA.M12 * mB.M11 + mA.M22 * mB.M21 + mA.M32 * mB.M31, - mA.M12 * mB.M12 + mA.M22 * mB.M22 + mA.M32 * mB.M32, - mA.M12 * mB.M13 + mA.M22 * mB.M23 + mA.M32 * mB.M33, - 0, - mA.M13 * mB.M11 + mA.M23 * mB.M21 + mA.M33 * mB.M31, - mA.M13 * mB.M12 + mA.M23 * mB.M22 + mA.M33 * mB.M32, - mA.M13 * mB.M13 + mA.M23 * mB.M23 + mA.M33 * mB.M33, - 0, 0, 0, 0, 1); - } - - internal static Vector3 GetColumn(Matrix m, int column) - { - switch (column) - { - case 1: - return new Vector3(m.M11, m.M21, m.M31); - case 2: - return new Vector3(m.M12, m.M22, m.M32); - case 3: - return new Vector3(m.M13, m.M23, m.M33); - default: - throw new ArgumentOutOfRangeException("column"); - } - } - - internal static Vector3 GetRow(Matrix m, int row) - { - switch (row) - { - case 1: - return new Vector3(m.M11, m.M12, m.M13); - case 2: - return new Vector3(m.M21, m.M22, m.M23); - case 3: - return new Vector3(m.M31, m.M32, m.M33); - default: - throw new ArgumentOutOfRangeException("row"); - } - } - - internal static Quaternion GetRotation(Matrix m) - { - float trace = m.M11 + m.M22 + m.M33; - Quaternion q = new Quaternion(); - - if (trace > 0) - { - float s = (float)Math.Sqrt(trace + 1.0f); - q.W = s * 0.5f; - s = 0.5f / s; - - q.X = (m.M32 - m.M23) * s; - q.Y = (m.M13 - m.M31) * s; - q.Z = (m.M21 - m.M12) * s; - } - else - { - int i = m.M11 < m.M22 ? - (m.M22 < m.M33 ? 2 : 1) : - (m.M11 < m.M33 ? 2 : 0); - int j = (i + 1) % 3; - int k = (i + 2) % 3; - - float s = (float)Math.Sqrt(GetElement(m, i, i) - GetElement(m, j, j) - GetElement(m, k, k) + 1.0f); - SetElement(ref q, i, s * 0.5f); - s = 0.5f / s; - - q.W = (GetElement(m, k, j) - GetElement(m, j, k)) * s; - SetElement(ref q, j, (GetElement(m, j, i) + GetElement(m, i, j)) * s); - SetElement(ref q, k, (GetElement(m, k, i) + GetElement(m, i, k)) * s); - } - return q; - } - - internal static float SetElement(ref Quaternion q, int index, float value) - { - switch (index) - { - case 0: - q.X = value; break; - case 1: - q.Y = value; break; - case 2: - q.Z = value; break; - case 3: - q.W = value; break; - } - - return 0; - } - - internal static float GetElement(Quaternion q, int index) - { - switch (index) - { - case 0: - return q.X; - case 1: - return q.Y; - case 2: - return q.Z; - default: - return q.W; - } - } - - internal static float GetElement(Matrix mat, int index) - { - int row = index % 3; - int col = index / 3; - - return GetElement(mat, row, col); - } - - internal static float GetElement(Matrix mat, int row, int col) - { - switch (row) - { - case 0: - switch (col) - { - case 0: - return mat.M11; - case 1: - return mat.M12; - case 2: - return mat.M13; - } break; - case 1: - switch (col) - { - case 0: - return mat.M21; - case 1: - return mat.M22; - case 2: - return mat.M23; - } break; - case 2: - switch (col) - { - case 0: - return mat.M31; - case 1: - return mat.M32; - case 2: - return mat.M33; - } break; - } - - return 0; - } - - internal static float GetElement(Vector3 v, int index) - { - if (index == 0) - return v.X; - if (index == 1) - return v.Y; - if (index == 2) - return v.Z; - - throw new ArgumentOutOfRangeException("index"); - } - - internal static void SetElement(ref Vector3 v, int index, float value) - { - if (index == 0) - v.X = value; - else if (index == 1) - v.Y = value; - else if (index == 2) - v.Z = value; - else - throw new ArgumentOutOfRangeException("index"); - } - - public static Matrix InvertMatrix(Matrix m) - { - Vector3 pos = m.Translation; - m.Translation = Vector3.Zero; - Matrix inv = Matrix.Transpose(m); - pos = Vector3.Transform(-pos, m); - inv.Translation = pos; - return inv; - } - - public static Matrix GetDisplayMatrix(Matrix m) - { - Matrix displayMatrix = m; - displayMatrix.Translation = Vector3.Zero; - displayMatrix = Matrix.Transpose(displayMatrix); - displayMatrix.Translation = m.Translation; - return displayMatrix; - } - - internal static Vector3 Transform(Vector3 position, Matrix matrix) - { - Vector3 vector = new Vector3(); - vector.X = (((position.X * matrix.M11) + (position.Y * matrix.M12)) + (position.Z * matrix.M13)) + matrix.M41; - vector.Y = (((position.X * matrix.M21) + (position.Y * matrix.M22)) + (position.Z * matrix.M23)) + matrix.M42; - vector.Z = (((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33)) + matrix.M43; - return vector; - } - - internal static Vector3 TransformNormal(Vector3 position, Matrix matrix) - { - Vector3 vector = new Vector3(); - vector.X = (((position.X * matrix.M11) + (position.Y * matrix.M12)) + (position.Z * matrix.M13)); - vector.Y = (((position.X * matrix.M21) + (position.Y * matrix.M22)) + (position.Z * matrix.M23)); - vector.Z = (((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33)); - return vector; - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public static class MathHelper + { + internal const float Sqrt12 = 0.7071067811865475244008443621048490f; + internal const float Infinity = 3.402823466e+38f; + internal const float Epsilon = 1.192092896e-07f; + + public static Vector3 MatrixToVector(Matrix m, Vector3 v) + { + return new Vector3( + Vector3.Dot(new Vector3(m.M11, m.M12, m.M13), v) + m.Translation.X, + Vector3.Dot(new Vector3(m.M21, m.M22, m.M23), v) + m.Translation.Y, + Vector3.Dot(new Vector3(m.M31, m.M32, m.M33), v) + m.Translation.Z + ); + } + + internal static int ClosestAxis(Vector4 v) + { + return MaxAxis(Absolute(v)); + } + + internal static Vector4 Absolute(Vector4 v) + { + return new Vector4(Math.Abs(v.X), Math.Abs(v.Y), Math.Abs(v.Z), Math.Abs(v.W)); + } + + internal static int MaxAxis(Vector4 v) + { + int maxIndex = -1; + float maxVal = float.MinValue; + if (v.X > maxVal) + { + maxIndex = 0; + maxVal = v.X; + } + if (v.Y > maxVal) + { + maxIndex = 1; + maxVal = v.Y; + } + if (v.Z > maxVal) + { + maxIndex = 2; + maxVal = v.Z; + } + if (v.W > maxVal) + { + maxIndex = 3; + maxVal = v.W; + } + + return maxIndex; + } + + internal static int MaxAxis(Vector3 v) + { + return v.X < v.Y ? (v.Y < v.Z ? 2 : 1) : (v.X < v.Z ? 2 : 0); + } + + // conservative test for overlap between two aabbs + internal static bool TestAabbAgainstAabb2(Vector3 aabbMinA, Vector3 aabbMaxA, Vector3 aabbMinB, Vector3 aabbMaxB) + { + bool overlap = true; + overlap = (aabbMinA.X > aabbMaxB.X || aabbMaxA.X < aabbMinB.X) ? false : overlap; + overlap = (aabbMinA.Z > aabbMaxB.Z || aabbMaxA.Z < aabbMinB.Z) ? false : overlap; + overlap = (aabbMinA.Y > aabbMaxB.Y || aabbMaxA.Y < aabbMinB.Y) ? false : overlap; + return overlap; + } + + internal static bool TestTriangleAgainstAabb2(Vector3[] vertices, Vector3 aabbMin, Vector3 aabbMax) + { + Vector3 p1 = vertices[0]; + Vector3 p2 = vertices[1]; + Vector3 p3 = vertices[2]; + + if (Math.Min(Math.Min(p1.X, p2.X), p3.X) > aabbMax.X) return false; + if (Math.Max(Math.Max(p1.X, p2.X), p3.X) < aabbMin.X) return false; + + if (Math.Min(Math.Min(p1.Z, p2.Z), p3.Z) > aabbMax.Z) return false; + if (Math.Max(Math.Max(p1.Z, p2.Z), p3.Z) < aabbMin.Z) return false; + + if (Math.Min(Math.Min(p1.Y, p2.Y), p3.Y) > aabbMax.Y) return false; + if (Math.Max(Math.Max(p1.Y, p2.Y), p3.Y) < aabbMin.Y) return false; + return true; + } + + internal static void SetInterpolate3(Vector3 vA, Vector3 vB, float rt, ref Vector3 interpolated) + { + float s = 1.0f - rt; + interpolated.X = s * vA.X + rt * vB.X; + interpolated.Y = s * vA.Y + rt * vB.Y; + interpolated.Z = s * vA.Z + rt * vB.Z; + } + + internal static void PlaneSpace1(Vector3 n, ref Vector3 p, ref Vector3 q) + { + if (Math.Abs(n.Z) > Sqrt12) + { + // choose p in y-z plane + float a = n.Y * n.Y + n.Z * n.Z; + float k = 1f / (float)Math.Sqrt(a); + p.X = 0; + p.Y = -n.Z * k; + p.Z = n.Y * k; + // set q = n x p + q.X = a * k; + q.Y = -n.X * p.Z; + q.Z = n.X * p.Y; + } + else + { + // choose p in x-y plane + float a = n.X * n.X + n.Y * n.Y; + float k = 1f / (float)Math.Sqrt(a); + p.X = -n.Y * k; + p.Y = n.X * k; + p.Z = 0; + // set q = n x p + q.X = -n.Z * p.Y; + q.Y = n.Z * p.X; + q.Z = a * k; + } + } + + internal static bool RayAabb(Vector3 rayFrom, + Vector3 rayTo, + Vector3 aabbMin, + Vector3 aabbMax, + float param, Vector3 normal) + { + Vector3 aabbHalfExtent = (aabbMax - aabbMin) * 0.5f; + Vector3 aabbCenter = (aabbMax + aabbMin) * 0.5f; + Vector3 source = rayFrom - aabbCenter; + Vector3 target = rayTo - aabbCenter; + int sourceOutcode = Outcode(source, aabbHalfExtent); + int targetOutcode = Outcode(target, aabbHalfExtent); + if ((sourceOutcode & targetOutcode) == 0x0) + { + float lambda_enter = 0; + float lambda_exit = param; + Vector3 r = target - source; + float normSign = 1; + Vector3 hitNormal = new Vector3(); + int bit = 1; + + for (int j = 0; j < 2; j++) + { + { + if ((sourceOutcode & bit) != 0) + { + float lambda = (-source.X - aabbHalfExtent.X * normSign) / r.X; + if (lambda_enter <= lambda) + { + lambda_enter = lambda; + hitNormal = new Vector3(); + hitNormal.X = normSign; + } + } + else if ((targetOutcode & bit) != 0) + { + float lambda = (-source.X - aabbHalfExtent.X * normSign) / r.X; + SetMin(ref lambda_exit, lambda); + } + bit <<= 1; + } + { + if ((sourceOutcode & bit) != 0) + { + float lambda = (-source.Y - aabbHalfExtent.Y * normSign) / r.Y; + if (lambda_enter <= lambda) + { + lambda_enter = lambda; + hitNormal = new Vector3(); + hitNormal.Y = normSign; + } + } + else if ((targetOutcode & bit) != 0) + { + float lambda = (-source.Y - aabbHalfExtent.Y * normSign) / r.Y; + SetMin(ref lambda_exit, lambda); + } + bit <<= 1; + } + { + if ((sourceOutcode & bit) != 0) + { + float lambda = (-source.Z - aabbHalfExtent.Z * normSign) / r.Z; + if (lambda_enter <= lambda) + { + lambda_enter = lambda; + hitNormal = new Vector3(); + hitNormal.Z = normSign; + } + } + else if ((targetOutcode & bit) != 0) + { + float lambda = (-source.Z - aabbHalfExtent.Z * normSign) / r.Z; + SetMin(ref lambda_exit, lambda); + } + bit <<= 1; + } + normSign = -1; + } + if (lambda_enter <= lambda_exit) + { + param = lambda_enter; + normal = hitNormal; + return true; + } + } + return false; + } + + internal static void SetMin(ref float a, float b) + { + if (a > b) + a = b; + } + + internal static void SetMax(ref float a, float b) + { + if (a < b) + a = b; + } + + internal static void SetMax(ref Vector3 self, Vector3 other) + { + if (other.X > self.X) + self.X = other.X; + + if (other.Y > self.Y) + self.Y = other.Y; + + if (other.Z > self.Z) + self.Z = other.Z; + } + + internal static Vector3 SetMax(Vector3 self, Vector3 other) + { + if (other.X > self.X) + self.X = other.X; + + if (other.Y > self.Y) + self.Y = other.Y; + + if (other.Z > self.Z) + self.Z = other.Z; + + return self; + } + + internal static void SetMin(ref Vector3 self, Vector3 other) + { + if (other.X < self.X) + self.X = other.X; + + if (other.Y < self.Y) + self.Y = other.Y; + + if (other.Z < self.Z) + self.Z = other.Z; + } + + internal static Vector3 SetMin(Vector3 self, Vector3 other) + { + if (other.X < self.X) + self.X = other.X; + + if (other.Y < self.Y) + self.Y = other.Y; + + if (other.Z < self.Z) + self.Z = other.Z; + + return self; + } + + internal static int Outcode(Vector3 p, Vector3 halfExtent) + { + return (p.X < -halfExtent.X ? 0x01 : 0x0) | + (p.X > halfExtent.X ? 0x08 : 0x0) | + (p.Y < -halfExtent.Y ? 0x02 : 0x0) | + (p.Y > halfExtent.Y ? 0x10 : 0x0) | + (p.Z < -halfExtent.Z ? 0x4 : 0x0) | + (p.Z > halfExtent.Z ? 0x20 : 0x0); + } + + internal static Matrix Absolute(Matrix m) + { + return new Matrix(Math.Abs(m.M11), Math.Abs(m.M12), Math.Abs(m.M13), Math.Abs(m.M14), + Math.Abs(m.M21), Math.Abs(m.M22), Math.Abs(m.M23), Math.Abs(m.M24), + Math.Abs(m.M31), Math.Abs(m.M32), Math.Abs(m.M33), Math.Abs(m.M34), + Math.Abs(m.M41), Math.Abs(m.M42), Math.Abs(m.M43), Math.Abs(m.M44)); + } + + internal static void SetValueByIndex(ref Vector3 v, int i, float value) + { + if (i == 0) + v.X = value; + else if (i == 1) + v.Y = value; + else + v.Z = value; + } + + internal static float GetValueByIndex(Vector3 v, int i) + { + if (i == 0) + return v.X; + else if (i == 1) + return v.Y; + else + return v.Z; + } + + internal static Vector3 InvXForm(Matrix m, Vector3 v) + { + v -= m.Translation; + m.Translation = new Vector3(); + return MathHelper.Transform(v, Matrix.Transpose(m)); + } + + internal static Matrix InverseTimes(Matrix m, Matrix t) + { + Vector3 v = t.Translation - m.Translation; + + Matrix mat = TransposeTimes(m, t); + mat.Translation = Vector3.Transform(v, m); + return mat; + } + + internal static Matrix TransposeTimes(Matrix mA, Matrix mB) + { + return new Matrix( + mA.M11 * mB.M11 + mA.M21 * mB.M21 + mA.M31 * mB.M31, + mA.M11 * mB.M12 + mA.M21 * mB.M22 + mA.M31 * mB.M32, + mA.M11 * mB.M13 + mA.M21 * mB.M23 + mA.M31 * mB.M33, + 0, + mA.M12 * mB.M11 + mA.M22 * mB.M21 + mA.M32 * mB.M31, + mA.M12 * mB.M12 + mA.M22 * mB.M22 + mA.M32 * mB.M32, + mA.M12 * mB.M13 + mA.M22 * mB.M23 + mA.M32 * mB.M33, + 0, + mA.M13 * mB.M11 + mA.M23 * mB.M21 + mA.M33 * mB.M31, + mA.M13 * mB.M12 + mA.M23 * mB.M22 + mA.M33 * mB.M32, + mA.M13 * mB.M13 + mA.M23 * mB.M23 + mA.M33 * mB.M33, + 0, 0, 0, 0, 1); + } + + internal static Vector3 GetColumn(Matrix m, int column) + { + switch (column) + { + case 1: + return new Vector3(m.M11, m.M21, m.M31); + case 2: + return new Vector3(m.M12, m.M22, m.M32); + case 3: + return new Vector3(m.M13, m.M23, m.M33); + default: + throw new ArgumentOutOfRangeException("column"); + } + } + + internal static Vector3 GetRow(Matrix m, int row) + { + switch (row) + { + case 1: + return new Vector3(m.M11, m.M12, m.M13); + case 2: + return new Vector3(m.M21, m.M22, m.M23); + case 3: + return new Vector3(m.M31, m.M32, m.M33); + default: + throw new ArgumentOutOfRangeException("row"); + } + } + + internal static Quaternion GetRotation(Matrix m) + { + float trace = m.M11 + m.M22 + m.M33; + Quaternion q = new Quaternion(); + + if (trace > 0) + { + float s = (float)Math.Sqrt(trace + 1.0f); + q.W = s * 0.5f; + s = 0.5f / s; + + q.X = (m.M32 - m.M23) * s; + q.Y = (m.M13 - m.M31) * s; + q.Z = (m.M21 - m.M12) * s; + } + else + { + int i = m.M11 < m.M22 ? + (m.M22 < m.M33 ? 2 : 1) : + (m.M11 < m.M33 ? 2 : 0); + int j = (i + 1) % 3; + int k = (i + 2) % 3; + + float s = (float)Math.Sqrt(GetElement(m, i, i) - GetElement(m, j, j) - GetElement(m, k, k) + 1.0f); + SetElement(ref q, i, s * 0.5f); + s = 0.5f / s; + + q.W = (GetElement(m, k, j) - GetElement(m, j, k)) * s; + SetElement(ref q, j, (GetElement(m, j, i) + GetElement(m, i, j)) * s); + SetElement(ref q, k, (GetElement(m, k, i) + GetElement(m, i, k)) * s); + } + return q; + } + + internal static float SetElement(ref Quaternion q, int index, float value) + { + switch (index) + { + case 0: + q.X = value; break; + case 1: + q.Y = value; break; + case 2: + q.Z = value; break; + case 3: + q.W = value; break; + } + + return 0; + } + + internal static float GetElement(Quaternion q, int index) + { + switch (index) + { + case 0: + return q.X; + case 1: + return q.Y; + case 2: + return q.Z; + default: + return q.W; + } + } + + internal static float GetElement(Matrix mat, int index) + { + int row = index % 3; + int col = index / 3; + + return GetElement(mat, row, col); + } + + internal static float GetElement(Matrix mat, int row, int col) + { + switch (row) + { + case 0: + switch (col) + { + case 0: + return mat.M11; + case 1: + return mat.M12; + case 2: + return mat.M13; + } break; + case 1: + switch (col) + { + case 0: + return mat.M21; + case 1: + return mat.M22; + case 2: + return mat.M23; + } break; + case 2: + switch (col) + { + case 0: + return mat.M31; + case 1: + return mat.M32; + case 2: + return mat.M33; + } break; + } + + return 0; + } + + internal static float GetElement(Vector3 v, int index) + { + if (index == 0) + return v.X; + if (index == 1) + return v.Y; + if (index == 2) + return v.Z; + + throw new ArgumentOutOfRangeException("index"); + } + + internal static void SetElement(ref Vector3 v, int index, float value) + { + if (index == 0) + v.X = value; + else if (index == 1) + v.Y = value; + else if (index == 2) + v.Z = value; + else + throw new ArgumentOutOfRangeException("index"); + } + + public static Matrix InvertMatrix(Matrix m) + { + Vector3 pos = m.Translation; + m.Translation = Vector3.Zero; + Matrix inv = Matrix.Transpose(m); + pos = Vector3.Transform(-pos, m); + inv.Translation = pos; + return inv; + } + + public static Matrix GetDisplayMatrix(Matrix m) + { + Matrix displayMatrix = m; + displayMatrix.Translation = Vector3.Zero; + displayMatrix = Matrix.Transpose(displayMatrix); + displayMatrix.Translation = m.Translation; + return displayMatrix; + } + + internal static Vector3 Transform(Vector3 position, Matrix matrix) + { + Vector3 vector = new Vector3(); + vector.X = (((position.X * matrix.M11) + (position.Y * matrix.M12)) + (position.Z * matrix.M13)) + matrix.M41; + vector.Y = (((position.X * matrix.M21) + (position.Y * matrix.M22)) + (position.Z * matrix.M23)) + matrix.M42; + vector.Z = (((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33)) + matrix.M43; + return vector; + } + + internal static Vector3 TransformNormal(Vector3 position, Matrix matrix) + { + Vector3 vector = new Vector3(); + vector.X = (((position.X * matrix.M11) + (position.Y * matrix.M12)) + (position.Z * matrix.M13)); + vector.Y = (((position.X * matrix.M21) + (position.Y * matrix.M22)) + (position.Z * matrix.M23)); + vector.Z = (((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33)); + return vector; + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs index 04e42d9..93a99f2 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs @@ -1,116 +1,116 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - internal static class MatrixOperations - { - public static void SetRotation(ref Matrix m, Quaternion q) - { - float d = q.LengthSquared(); - BulletDebug.Assert(d != 0); - float s = 2f / d; - float xs = q.X * s, ys = q.Y * s, zs = q.Z * s; - float wx = q.W * xs, wy = q.W * ys, wz = q.W * zs; - float xx = q.X * xs, xy = q.X * ys, xz = q.X * zs; - float yy = q.Y * ys, yz = q.Y * zs, zz = q.Z * zs; - m = new Matrix(1 - (yy + zz), xy - wz, xz + wy, 0, - xy + wz, 1 - (xx + zz), yz - wx, 0, - xz - wy, yz + wx, 1 - (xx + yy), 0, - m.M41, m.M42, m.M43, 1); - } - - public static Quaternion GetRotation(Matrix m) - { - Quaternion q = new Quaternion(); - - float trace = m.M11 + m.M22 + m.M33; - - if (trace > 0) - { - float s = (float)Math.Sqrt(trace + 1); - q.W = s * 0.5f; - s = 0.5f / s; - - q.X = (m.M32 - m.M23) * s; - q.Y = (m.M13 - m.M31) * s; - q.Z = (m.M21 - m.M12) * s; - } - else - { - int i = m.M11 < m.M22 ? - (m.M22 < m.M33 ? 2 : 1) : - (m.M11 < m.M33 ? 2 : 0); - int j = (i + 1) % 3; - int k = (i + 2) % 3; - - float s = (float)Math.Sqrt(MathHelper.GetElement(m, i, i) - MathHelper.GetElement(m, j, j) - MathHelper.GetElement(m, k, k) + 1); - MathHelper.SetElement(ref q, i, s * 0.5f); - s = 0.5f / s; - - q.W = (MathHelper.GetElement(m, k, j) - MathHelper.GetElement(m, j, k)) * s; - MathHelper.SetElement(ref q, j, (MathHelper.GetElement(m, j, i) + MathHelper.GetElement(m, i, j)) * s); - MathHelper.SetElement(ref q, k, (MathHelper.GetElement(m, k, i) + MathHelper.GetElement(m, i, k)) * s); - } - - return q; - } - - public static Matrix Scaled(Matrix m, Vector3 v) - { - return new Matrix( m.M11 * v.X, m.M12 * v.Y, m.M13 * v.Z, 0, - m.M21 * v.X, m.M22 * v.Y, m.M23 * v.Z, 0, - m.M31 * v.X, m.M32 * v.Y, m.M33 * v.Z, 0, - 0, 0, 0, 1); - } - - public static Matrix Multiply(Matrix a, Matrix b) - { - /*return btMatrix3x3( - m2.tdot(0, m1[0]), m2.tdot(1, m1[0]), m2.tdot(2, m1[0]), - m2.tdot(0, m1[1]), m2.tdot(1, m1[1]), m2.tdot(2, m1[1]), - m2.tdot(0, m1[2]), m2.tdot(1, m1[2]), m2.tdot(2, m1[2]));*/ - return new Matrix( - Dot(b, 0, MathHelper.GetRow(a, 1)), Dot(b, 1, MathHelper.GetRow(a, 1)), Dot(b, 2, MathHelper.GetRow(a, 1)), 0, - Dot(b, 0, MathHelper.GetRow(a, 2)), Dot(b, 1, MathHelper.GetRow(a, 2)), Dot(b, 2, MathHelper.GetRow(a, 2)), 0, - Dot(b, 0, MathHelper.GetRow(a, 3)), Dot(b, 1, MathHelper.GetRow(a, 3)), Dot(b, 2, MathHelper.GetRow(a, 3)), 0, - 0, 0, 0, 1); - } - - public static float Dot(Matrix m, int c, Vector3 v) - { - return MathHelper.GetElement(m, 0, c) * v.X + MathHelper.GetElement(m, 1, c) * v.Y + MathHelper.GetElement(m, 2, c) * v.Z; - } - - public static Matrix Transpose(Matrix m) - { - return new Matrix( m.M11, m.M21, m.M31, 0, - m.M12, m.M22, m.M32, 0, - m.M13, m.M23, m.M33, 0, - 0, 0, 0, 1); - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + internal static class MatrixOperations + { + public static void SetRotation(ref Matrix m, Quaternion q) + { + float d = q.LengthSquared(); + BulletDebug.Assert(d != 0); + float s = 2f / d; + float xs = q.X * s, ys = q.Y * s, zs = q.Z * s; + float wx = q.W * xs, wy = q.W * ys, wz = q.W * zs; + float xx = q.X * xs, xy = q.X * ys, xz = q.X * zs; + float yy = q.Y * ys, yz = q.Y * zs, zz = q.Z * zs; + m = new Matrix(1 - (yy + zz), xy - wz, xz + wy, 0, + xy + wz, 1 - (xx + zz), yz - wx, 0, + xz - wy, yz + wx, 1 - (xx + yy), 0, + m.M41, m.M42, m.M43, 1); + } + + public static Quaternion GetRotation(Matrix m) + { + Quaternion q = new Quaternion(); + + float trace = m.M11 + m.M22 + m.M33; + + if (trace > 0) + { + float s = (float)Math.Sqrt(trace + 1); + q.W = s * 0.5f; + s = 0.5f / s; + + q.X = (m.M32 - m.M23) * s; + q.Y = (m.M13 - m.M31) * s; + q.Z = (m.M21 - m.M12) * s; + } + else + { + int i = m.M11 < m.M22 ? + (m.M22 < m.M33 ? 2 : 1) : + (m.M11 < m.M33 ? 2 : 0); + int j = (i + 1) % 3; + int k = (i + 2) % 3; + + float s = (float)Math.Sqrt(MathHelper.GetElement(m, i, i) - MathHelper.GetElement(m, j, j) - MathHelper.GetElement(m, k, k) + 1); + MathHelper.SetElement(ref q, i, s * 0.5f); + s = 0.5f / s; + + q.W = (MathHelper.GetElement(m, k, j) - MathHelper.GetElement(m, j, k)) * s; + MathHelper.SetElement(ref q, j, (MathHelper.GetElement(m, j, i) + MathHelper.GetElement(m, i, j)) * s); + MathHelper.SetElement(ref q, k, (MathHelper.GetElement(m, k, i) + MathHelper.GetElement(m, i, k)) * s); + } + + return q; + } + + public static Matrix Scaled(Matrix m, Vector3 v) + { + return new Matrix( m.M11 * v.X, m.M12 * v.Y, m.M13 * v.Z, 0, + m.M21 * v.X, m.M22 * v.Y, m.M23 * v.Z, 0, + m.M31 * v.X, m.M32 * v.Y, m.M33 * v.Z, 0, + 0, 0, 0, 1); + } + + public static Matrix Multiply(Matrix a, Matrix b) + { + /*return btMatrix3x3( + m2.tdot(0, m1[0]), m2.tdot(1, m1[0]), m2.tdot(2, m1[0]), + m2.tdot(0, m1[1]), m2.tdot(1, m1[1]), m2.tdot(2, m1[1]), + m2.tdot(0, m1[2]), m2.tdot(1, m1[2]), m2.tdot(2, m1[2]));*/ + return new Matrix( + Dot(b, 0, MathHelper.GetRow(a, 1)), Dot(b, 1, MathHelper.GetRow(a, 1)), Dot(b, 2, MathHelper.GetRow(a, 1)), 0, + Dot(b, 0, MathHelper.GetRow(a, 2)), Dot(b, 1, MathHelper.GetRow(a, 2)), Dot(b, 2, MathHelper.GetRow(a, 2)), 0, + Dot(b, 0, MathHelper.GetRow(a, 3)), Dot(b, 1, MathHelper.GetRow(a, 3)), Dot(b, 2, MathHelper.GetRow(a, 3)), 0, + 0, 0, 0, 1); + } + + public static float Dot(Matrix m, int c, Vector3 v) + { + return MathHelper.GetElement(m, 0, c) * v.X + MathHelper.GetElement(m, 1, c) * v.Y + MathHelper.GetElement(m, 2, c) * v.Z; + } + + public static Matrix Transpose(Matrix m) + { + return new Matrix( m.M11, m.M21, m.M31, 0, + m.M12, m.M22, m.M32, 0, + m.M13, m.M23, m.M33, 0, + 0, 0, 0, 1); + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MotionState.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MotionState.cs index 4039177..407a285 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MotionState.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MotionState.cs @@ -1,44 +1,44 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public abstract class MotionState - { - /*public abstract Vector3 getWorldPosition(); - public abstract Vector3 getWorldScaling(); - public abstract Quaternion getWorldOrientation(); - - public abstract void setWorldPosition(Vector3 worldPos); - public abstract void setWorldOrientation(Quaternion orn); - - public abstract void calculateWorldTransformations();*/ - - public abstract void GetWorldTransform(out Matrix worldTransform); - //Bullet only calls the update of worldtransform for active objects - public abstract void SetWorldTransform(Matrix worldTransform); - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public abstract class MotionState + { + /*public abstract Vector3 getWorldPosition(); + public abstract Vector3 getWorldScaling(); + public abstract Quaternion getWorldOrientation(); + + public abstract void setWorldPosition(Vector3 worldPos); + public abstract void setWorldOrientation(Quaternion orn); + + public abstract void calculateWorldTransformations();*/ + + public abstract void GetWorldTransform(out Matrix worldTransform); + //Bullet only calls the update of worldtransform for active objects + public abstract void SetWorldTransform(Matrix worldTransform); + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/QuadWord.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/QuadWord.cs index 35c9fed..0f59e87 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/QuadWord.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/QuadWord.cs @@ -1,88 +1,88 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; - -namespace XnaDevRu.BulletX.LinearMath -{ - internal abstract class QuadWord - { - private float x; - private float y; - private float z; - private float w; - - public float X { get { return x; } set { x = value; } } - public float Y { get { return y; } set { y = value; } } - public float Z { get { return z; } set { z = value; } } - public float W { get { return w; } set { w = value; } } - - public QuadWord() { } - - public QuadWord(float x, float y, float z, float w) - { - X = x; - Y = y; - Z = z; - W = w; - } - - public QuadWord(float x, float y, float z) - { - X = x; - Y = y; - Z = z; - W = 0; - } - - public void SetMax(QuadWord other) - { - if (other.X > X) - X = other.X; - - if (other.Y > Y) - Y = other.Y; - - if (other.Z > Z) - Z = other.Z; - - if (other.W > W) - W = other.W; - } - - public void SetMin(QuadWord other) - { - if (other.X < X) - X = other.X; - - if (other.Y < Y) - Y = other.Y; - - if (other.Z < Z) - Z = other.Z; - - if (other.W < W) - W = other.W; - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace XnaDevRu.BulletX.LinearMath +{ + internal abstract class QuadWord + { + private float x; + private float y; + private float z; + private float w; + + public float X { get { return x; } set { x = value; } } + public float Y { get { return y; } set { y = value; } } + public float Z { get { return z; } set { z = value; } } + public float W { get { return w; } set { w = value; } } + + public QuadWord() { } + + public QuadWord(float x, float y, float z, float w) + { + X = x; + Y = y; + Z = z; + W = w; + } + + public QuadWord(float x, float y, float z) + { + X = x; + Y = y; + Z = z; + W = 0; + } + + public void SetMax(QuadWord other) + { + if (other.X > X) + X = other.X; + + if (other.Y > Y) + Y = other.Y; + + if (other.Z > Z) + Z = other.Z; + + if (other.W > W) + W = other.W; + } + + public void SetMin(QuadWord other) + { + if (other.X < X) + X = other.X; + + if (other.Y < Y) + Y = other.Y; + + if (other.Z < Z) + Z = other.Z; + + if (other.W < W) + W = other.W; + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs index 5425a05..0685d4a 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs @@ -1,198 +1,198 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; - -namespace XnaDevRu.BulletX.LinearMath -{ - internal class Quaternion : QuadWord - { - public Quaternion() { } - - public Quaternion(float x, float y, float z, float w) - : base(x, y, z, w) { } - - public Quaternion(Vector3 axis, float angle) - { - SetRotation(axis, angle); - } - - public Quaternion(float yaw, float pitch, float roll) - { - SetEuler(yaw, pitch, roll); - } - - public void SetRotation(Vector3 axis, float angle) - { - float d = axis.Length(); - if (d == 0) throw new DivideByZeroException(); - float s = (float)Math.Sin(angle * 0.5f) / d; - X = axis.X * s; - Y = axis.Y * s; - Z = axis.Z * s; - W = (float)Math.Cos(angle * 0.5f); - } - - public void SetEuler(float yaw, float pitch, float roll) - { - float halfYaw = yaw * 0.5f; - float halfPitch = pitch * 0.5f; - float halfRoll = roll * 0.5f; - float cosYaw = (float)Math.Cos(halfYaw); - float sinYaw = (float)Math.Sin(halfYaw); - float cosPitch = (float)Math.Cos(halfPitch); - float sinPitch = (float)Math.Sin(halfPitch); - float cosRoll = (float)Math.Cos(halfRoll); - float sinRoll = (float)Math.Sin(halfRoll); - X = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw; - Y = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw; - Z = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw; - W = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw; - } - - public float LengthSquared() - { - return Dot(this, this); - } - - public float Length() - { - return (float)Math.Sqrt(LengthSquared()); - } - - public float Angle() - { - return 2f * (float)Math.Acos(W); - } - - public static float Angle(Quaternion a, Quaternion b) - { - float s = (float)Math.Sqrt(a.LengthSquared() * b.LengthSquared()); - if (s == 0) throw new DivideByZeroException(); - return (float)Math.Acos(Dot(a, b) / s); - } - - public static Quaternion Farthest(Quaternion a, Quaternion b) - { - Quaternion diff, sum; - diff = a - b; - sum = a + b; - if (Dot(diff, diff) > Dot(sum, sum)) - return b; - return -b; - } - - public static Quaternion Slerp(Quaternion a, Quaternion b, float c) - { - float theta = Angle(a, b); - if (theta != 0) - { - float d = 1f / (float)Math.Sin(theta); - float s0 = (float)Math.Sin((1f - c) * theta); - float s1 = (float)Math.Sin(c * theta); - return new Quaternion( - (a.X * s0 + b.X * s1) * d, - (a.Y * s0 + b.Y * s1) * d, - (a.Z * s0 + b.Z * s1) * d, - (a.W * s0 + b.W * s1) * d); - } - else - { - return a; - } - } - - public static Quaternion Inverse(Quaternion a) - { - return new Quaternion(a.X, a.Y, a.Z, -a.W); - } - - public static Quaternion Normalize(Quaternion a) - { - return a / a.Length(); - } - - public static float Dot(Quaternion a, Quaternion b) - { - return a.X * b.X + a.Y * b.Y + a.Z * b.Z + a.W * b.W; - } - - public static Quaternion operator +(Quaternion a, Quaternion b) - { - return new Quaternion(a.X + b.X, a.Y + b.Y, a.Z + b.Z, a.W + b.W); - } - - public static Quaternion operator -(Quaternion a, Quaternion b) - { - return new Quaternion(a.X - b.X, a.Y - b.Y, a.Z - b.Z, a.W - b.W); - } - - public static Quaternion operator -(Quaternion a) - { - return new Quaternion(-a.X, -a.Y, -a.Z, -a.W); - } - - public static Quaternion operator *(Quaternion a, float b) - { - return new Quaternion(a.X * b, a.Y * b, a.Z * b, a.W * b); - } - - public static Quaternion operator *(Quaternion a, Quaternion b) - { - return new Quaternion( - a.W * b.X + a.X * b.W + a.Y * b.Z - a.Z * b.Y, - a.W * b.Y + a.Y * b.W + a.Z * b.X - a.X * b.Z, - a.W * b.Z + a.Z * b.W + a.X * b.Y - a.Y * b.X, - a.W * b.W - a.X * b.X - a.Y * b.Y - a.Z * b.Z); - } - - public static Quaternion operator *(Quaternion a, Vector3 b) - { - return new Quaternion( - a.W * b.X + a.Y * b.Z - a.Z * b.Y, - a.W * b.Y + a.Z * b.X - a.X * b.Z, - a.W * b.Z + a.X * b.Y - a.Y * b.X, - -a.X * b.X - a.Y * b.Y - a.Z * b.Z); - } - - public static Quaternion operator *(Vector3 w, Quaternion q) - { - return new Quaternion( - w.X * q.W + w.Y * q.Z - w.Z * q.Y, - w.Y * q.W + w.Z * q.X - w.X * q.Z, - w.Z * q.W + w.X * q.Y - w.Y * q.X, - -w.X * q.X - w.Y * q.Y - w.Z * q.Z); - } - - public static Quaternion operator /(Quaternion a, float b) - { - if (b == 0) throw new DivideByZeroException(); - return new Quaternion(a.X / b, a.Y / b, a.Z / b, a.W / b); - } - - public static explicit operator MonoXnaCompactMaths.Quaternion(Quaternion a) - { - return new MonoXnaCompactMaths.Quaternion(a.X, a.Y, a.Z, a.W); - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace XnaDevRu.BulletX.LinearMath +{ + internal class Quaternion : QuadWord + { + public Quaternion() { } + + public Quaternion(float x, float y, float z, float w) + : base(x, y, z, w) { } + + public Quaternion(Vector3 axis, float angle) + { + SetRotation(axis, angle); + } + + public Quaternion(float yaw, float pitch, float roll) + { + SetEuler(yaw, pitch, roll); + } + + public void SetRotation(Vector3 axis, float angle) + { + float d = axis.Length(); + if (d == 0) throw new DivideByZeroException(); + float s = (float)Math.Sin(angle * 0.5f) / d; + X = axis.X * s; + Y = axis.Y * s; + Z = axis.Z * s; + W = (float)Math.Cos(angle * 0.5f); + } + + public void SetEuler(float yaw, float pitch, float roll) + { + float halfYaw = yaw * 0.5f; + float halfPitch = pitch * 0.5f; + float halfRoll = roll * 0.5f; + float cosYaw = (float)Math.Cos(halfYaw); + float sinYaw = (float)Math.Sin(halfYaw); + float cosPitch = (float)Math.Cos(halfPitch); + float sinPitch = (float)Math.Sin(halfPitch); + float cosRoll = (float)Math.Cos(halfRoll); + float sinRoll = (float)Math.Sin(halfRoll); + X = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw; + Y = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw; + Z = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw; + W = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw; + } + + public float LengthSquared() + { + return Dot(this, this); + } + + public float Length() + { + return (float)Math.Sqrt(LengthSquared()); + } + + public float Angle() + { + return 2f * (float)Math.Acos(W); + } + + public static float Angle(Quaternion a, Quaternion b) + { + float s = (float)Math.Sqrt(a.LengthSquared() * b.LengthSquared()); + if (s == 0) throw new DivideByZeroException(); + return (float)Math.Acos(Dot(a, b) / s); + } + + public static Quaternion Farthest(Quaternion a, Quaternion b) + { + Quaternion diff, sum; + diff = a - b; + sum = a + b; + if (Dot(diff, diff) > Dot(sum, sum)) + return b; + return -b; + } + + public static Quaternion Slerp(Quaternion a, Quaternion b, float c) + { + float theta = Angle(a, b); + if (theta != 0) + { + float d = 1f / (float)Math.Sin(theta); + float s0 = (float)Math.Sin((1f - c) * theta); + float s1 = (float)Math.Sin(c * theta); + return new Quaternion( + (a.X * s0 + b.X * s1) * d, + (a.Y * s0 + b.Y * s1) * d, + (a.Z * s0 + b.Z * s1) * d, + (a.W * s0 + b.W * s1) * d); + } + else + { + return a; + } + } + + public static Quaternion Inverse(Quaternion a) + { + return new Quaternion(a.X, a.Y, a.Z, -a.W); + } + + public static Quaternion Normalize(Quaternion a) + { + return a / a.Length(); + } + + public static float Dot(Quaternion a, Quaternion b) + { + return a.X * b.X + a.Y * b.Y + a.Z * b.Z + a.W * b.W; + } + + public static Quaternion operator +(Quaternion a, Quaternion b) + { + return new Quaternion(a.X + b.X, a.Y + b.Y, a.Z + b.Z, a.W + b.W); + } + + public static Quaternion operator -(Quaternion a, Quaternion b) + { + return new Quaternion(a.X - b.X, a.Y - b.Y, a.Z - b.Z, a.W - b.W); + } + + public static Quaternion operator -(Quaternion a) + { + return new Quaternion(-a.X, -a.Y, -a.Z, -a.W); + } + + public static Quaternion operator *(Quaternion a, float b) + { + return new Quaternion(a.X * b, a.Y * b, a.Z * b, a.W * b); + } + + public static Quaternion operator *(Quaternion a, Quaternion b) + { + return new Quaternion( + a.W * b.X + a.X * b.W + a.Y * b.Z - a.Z * b.Y, + a.W * b.Y + a.Y * b.W + a.Z * b.X - a.X * b.Z, + a.W * b.Z + a.Z * b.W + a.X * b.Y - a.Y * b.X, + a.W * b.W - a.X * b.X - a.Y * b.Y - a.Z * b.Z); + } + + public static Quaternion operator *(Quaternion a, Vector3 b) + { + return new Quaternion( + a.W * b.X + a.Y * b.Z - a.Z * b.Y, + a.W * b.Y + a.Z * b.X - a.X * b.Z, + a.W * b.Z + a.X * b.Y - a.Y * b.X, + -a.X * b.X - a.Y * b.Y - a.Z * b.Z); + } + + public static Quaternion operator *(Vector3 w, Quaternion q) + { + return new Quaternion( + w.X * q.W + w.Y * q.Z - w.Z * q.Y, + w.Y * q.W + w.Z * q.X - w.X * q.Z, + w.Z * q.W + w.X * q.Y - w.Y * q.X, + -w.X * q.X - w.Y * q.Y - w.Z * q.Z); + } + + public static Quaternion operator /(Quaternion a, float b) + { + if (b == 0) throw new DivideByZeroException(); + return new Quaternion(a.X / b, a.Y / b, a.Z / b, a.W / b); + } + + public static explicit operator MonoXnaCompactMaths.Quaternion(Quaternion a) + { + return new MonoXnaCompactMaths.Quaternion(a.X, a.Y, a.Z, a.W); + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs index 031faab..ccf92a0 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs @@ -1,102 +1,102 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; -using MonoXnaCompactMaths; - -namespace XnaDevRu.BulletX -{ - public static class TransformUtil - { - //const float AngularMotionTreshold = 0.5f * MonoXnaCompactMaths.MathHelper.PiOver2; - const float AngularMotionTreshold = 0.5f * (float)Math.PI / 2.0f; - - public static void IntegrateTransform(Matrix currentTransform, Vector3 linearVelocity, Vector3 angularVelocity, float timeStep, ref Matrix predictedTransform) - { - predictedTransform.Translation = currentTransform.Translation + linearVelocity * timeStep; - //exponential map - Vector3 axis; - float angle = angularVelocity.Length(); - //limit the angular motion - if (angle * timeStep > AngularMotionTreshold) - { - angle = AngularMotionTreshold / timeStep; - } - - if (angle < 0.001f) - { - // use Taylor's expansions of sync function - axis = angularVelocity * (0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f) * angle * angle); - } - else - { - // sync(fAngle) = sin(c*fAngle)/t - axis = angularVelocity * ((float)Math.Sin(0.5f * angle * timeStep) / angle); - } - Quaternion dorn = new Quaternion(axis.X, axis.Y, axis.Z, (float)Math.Cos(angle * timeStep * 0.5f)); - Quaternion ornA = MatrixOperations.GetRotation(currentTransform); - - Quaternion predictedOrn = dorn * ornA; - predictedOrn.Normalize(); - - MatrixOperations.SetRotation(ref predictedTransform, predictedOrn); - - Matrix test = Matrix.CreateFromQuaternion(predictedOrn); - } - - public static void CalculateVelocity(Matrix transformA, Matrix transformB, float timeStep, ref Vector3 linearVelocity, ref Vector3 angularVelocity) - { - linearVelocity = (transformB.Translation - transformA.Translation) / timeStep; - Matrix dmat = transformB * MathHelper.InvertMatrix(transformA); - Quaternion dorn = Quaternion.CreateFromRotationMatrix(dmat); - - Vector3 axis; - float angle = 2 * (float)Math.Acos(dorn.W); - axis = new Vector3(dorn.X, dorn.Y, dorn.Z); - //axis[3] = 0.f; - //check for axis length - float len = axis.LengthSquared(); - if (len < MathHelper.Epsilon * MathHelper.Epsilon) - axis = new Vector3(1f, 0f, 0f); - else - axis /= (float)Math.Sqrt(len); - - angularVelocity = axis * angle / timeStep; - } - - public static void CalculateDiffAxisAngle(Matrix transformA, Matrix transformB, out Vector3 axis, out float angle) - { - Matrix dmat = transformB * MathHelper.InvertMatrix(transformA); - Quaternion dorn = MathHelper.GetRotation(dmat); - - angle = 2f * (float)Math.Acos(dorn.W); - axis = new Vector3(dorn.X, dorn.Y, dorn.Z); - //check for axis length - float len = axis.LengthSquared(); - if (len < MathHelper.Epsilon * MathHelper.Epsilon) - axis = new Vector3(1f, 0f, 0f); - else - axis /= (float)Math.Sqrt(len); - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public static class TransformUtil + { + //const float AngularMotionTreshold = 0.5f * MonoXnaCompactMaths.MathHelper.PiOver2; + const float AngularMotionTreshold = 0.5f * (float)Math.PI / 2.0f; + + public static void IntegrateTransform(Matrix currentTransform, Vector3 linearVelocity, Vector3 angularVelocity, float timeStep, ref Matrix predictedTransform) + { + predictedTransform.Translation = currentTransform.Translation + linearVelocity * timeStep; + //exponential map + Vector3 axis; + float angle = angularVelocity.Length(); + //limit the angular motion + if (angle * timeStep > AngularMotionTreshold) + { + angle = AngularMotionTreshold / timeStep; + } + + if (angle < 0.001f) + { + // use Taylor's expansions of sync function + axis = angularVelocity * (0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f) * angle * angle); + } + else + { + // sync(fAngle) = sin(c*fAngle)/t + axis = angularVelocity * ((float)Math.Sin(0.5f * angle * timeStep) / angle); + } + Quaternion dorn = new Quaternion(axis.X, axis.Y, axis.Z, (float)Math.Cos(angle * timeStep * 0.5f)); + Quaternion ornA = MatrixOperations.GetRotation(currentTransform); + + Quaternion predictedOrn = dorn * ornA; + predictedOrn.Normalize(); + + MatrixOperations.SetRotation(ref predictedTransform, predictedOrn); + + Matrix test = Matrix.CreateFromQuaternion(predictedOrn); + } + + public static void CalculateVelocity(Matrix transformA, Matrix transformB, float timeStep, ref Vector3 linearVelocity, ref Vector3 angularVelocity) + { + linearVelocity = (transformB.Translation - transformA.Translation) / timeStep; + Matrix dmat = transformB * MathHelper.InvertMatrix(transformA); + Quaternion dorn = Quaternion.CreateFromRotationMatrix(dmat); + + Vector3 axis; + float angle = 2 * (float)Math.Acos(dorn.W); + axis = new Vector3(dorn.X, dorn.Y, dorn.Z); + //axis[3] = 0.f; + //check for axis length + float len = axis.LengthSquared(); + if (len < MathHelper.Epsilon * MathHelper.Epsilon) + axis = new Vector3(1f, 0f, 0f); + else + axis /= (float)Math.Sqrt(len); + + angularVelocity = axis * angle / timeStep; + } + + public static void CalculateDiffAxisAngle(Matrix transformA, Matrix transformB, out Vector3 axis, out float angle) + { + Matrix dmat = transformB * MathHelper.InvertMatrix(transformA); + Quaternion dorn = MathHelper.GetRotation(dmat); + + angle = 2f * (float)Math.Acos(dorn.W); + axis = new Vector3(dorn.X, dorn.Y, dorn.Z); + //check for axis length + float len = axis.LengthSquared(); + if (len < MathHelper.Epsilon * MathHelper.Epsilon) + axis = new Vector3(1f, 0f, 0f); + else + axis /= (float)Math.Sqrt(len); + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector3.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector3.cs index 79b1262..adeec25 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector3.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector3.cs @@ -1,221 +1,221 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; - -namespace XnaDevRu.BulletX.LinearMath -{ - internal class Vector3 : QuadWord - { - public Vector3() { } - - public Vector3(float x, float y, float z) - : base(x, y, z) { } - - public void SetInterpolate3(Vector3 a, Vector3 b, float c) - { - float s = 1.0f - c; - X = s * a.X + c * b.X; - Y = s * a.Y + c * b.Y; - Z = s * a.Z + c * b.Z; - } - - public float LengthSquared() - { - return Dot(this, this); - } - - public float Length() - { - return (float)Math.Sqrt(LengthSquared()); - } - - public int MinAxis() - { - return X < Y ? (X < Z ? 0 : 2) : (Y < Z ? 1 : 2); - } - - public int MaxAxis() - { - return X < Y ? (Y < Z ? 2 : 1) : (X < Z ? 2 : 0); - } - - public int FurthestAxis() - { - return Absolute(this).MinAxis(); - } - - public int ClosestAxis() - { - return Absolute(this).MaxAxis(); - } - - public Vector3 Rotate(Vector3 axis, float angle) - { - Vector3 o = axis * Dot(axis, this); - Vector3 x = this - o; - Vector3 y = Cross(axis, this); - - return (o + x * (float)Math.Cos(angle) + y * (float)Math.Sin(angle)); - } - - public static Vector3 Lerp(Vector3 a, Vector3 b, float c) - { - return new Vector3( - a.X + (b.X - a.X) * c, - a.Y + (b.Y - a.Y) * c, - a.Z + (b.Z - a.Z) * c); - } - - public static float Angle(Vector3 a, Vector3 b) - { - float s = (float)Math.Sqrt(a.LengthSquared() * b.LengthSquared()); - if (s == 0) throw new DivideByZeroException(); - return (float)Math.Acos(Dot(a, b) / s); - } - - public static Vector3 Absolute(Vector3 a) - { - return new Vector3( - Math.Abs(a.X), - Math.Abs(a.Y), - Math.Abs(a.Z)); - } - - public static Vector3 Normalize(Vector3 a) - { - return a / a.Length(); - } - - public static Vector3 Cross(Vector3 a, Vector3 b) - { - return new Vector3( - a.Y * b.Z - a.Z * b.Y, - a.Z * b.X - a.X * b.Z, - a.X * b.Y - a.Y * b.X); - } - - public static float Dot(Vector3 a, Vector3 b) - { - return a.X * b.X + a.Y * b.Y + a.Z * b.Z; - } - - public static float Triple(Vector3 a, Vector3 b, Vector3 c) - { - return a.X * (b.Y * c.Z - b.Z * c.Y) + - a.Y * (b.Z * c.X - b.X * c.Z) + - a.Z * (b.X * c.Y - b.Y * c.X); - } - - public static float Distance(Vector3 a, Vector3 b) - { - return (b - a).Length(); - } - - public static float DistanceSquared(Vector3 a, Vector3 b) - { - return (b - a).LengthSquared(); - } - - public static Vector3 Rotate(Vector3 a, Vector3 axis, float angle) - { - Vector3 o = axis * Dot(axis, a); - Vector3 x = a - o; - Vector3 y = Cross(axis, a); - - return (o + x * (float)Math.Cos(angle) + y * (float)Math.Sin(angle)); - } - - public static Vector3 operator +(Vector3 a, Vector3 b) - { - return new Vector3(a.X + b.X, a.Y + b.Y, a.Z + b.Z); - } - - public static Vector3 operator -(Vector3 a, Vector3 b) - { - return new Vector3(a.X - b.X, a.Y - b.Y, a.Z - b.Z); - } - - public static Vector3 operator -(Vector3 a) - { - return new Vector3(-a.X, -a.Y, -a.Z); - } - - public static Vector3 operator *(float b, Vector3 a) - { - return new Vector3(a.X * b, a.Y * b, a.Z * b); - } - - public static Vector3 operator *(Vector3 a, float b) - { - return new Vector3(a.X * b, a.Y * b, a.Z * b); - } - - public static Vector3 operator *(Vector3 a, Vector3 b) - { - return new Vector3(a.X * b.X, a.Y * b.Y, a.Z * b.Z); - } - - public static Vector3 operator /(Vector3 a, float b) - { - if (b == 0) throw new DivideByZeroException(); - return new Vector3(a.X / b, a.Y / b, a.Z / b); - } - - public static Vector3 operator /(Vector3 a, Vector3 b) - { - if (b.X == 0 || b.Y == 0 || b.Z == 0) throw new DivideByZeroException(); - return new Vector3(a.X / b.X, a.Y / b.Y, a.Z / b.Z); - } - - public static bool operator ==(Vector3 a, Vector3 b) - { - return a.X == b.X && a.Y == b.Y && a.Z == b.Z; - } - - public static bool operator !=(Vector3 a, Vector3 b) - { - return a.X != b.X || a.Y != b.Y || a.Z != b.Z; - } - - public static explicit operator MonoXnaCompactMaths.Vector3(Vector3 a) - { - return new MonoXnaCompactMaths.Vector3(a.X, a.Y, a.Z); - } - - public override bool Equals(object obj) - { - return object.Equals(this, obj); - } - - public override int GetHashCode() - { - return X.GetHashCode() & Y.GetHashCode() & Z.GetHashCode(); - } - - public override string ToString() - { - return string.Format("{0}, {1}, {2}", X, Y, Z); - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace XnaDevRu.BulletX.LinearMath +{ + internal class Vector3 : QuadWord + { + public Vector3() { } + + public Vector3(float x, float y, float z) + : base(x, y, z) { } + + public void SetInterpolate3(Vector3 a, Vector3 b, float c) + { + float s = 1.0f - c; + X = s * a.X + c * b.X; + Y = s * a.Y + c * b.Y; + Z = s * a.Z + c * b.Z; + } + + public float LengthSquared() + { + return Dot(this, this); + } + + public float Length() + { + return (float)Math.Sqrt(LengthSquared()); + } + + public int MinAxis() + { + return X < Y ? (X < Z ? 0 : 2) : (Y < Z ? 1 : 2); + } + + public int MaxAxis() + { + return X < Y ? (Y < Z ? 2 : 1) : (X < Z ? 2 : 0); + } + + public int FurthestAxis() + { + return Absolute(this).MinAxis(); + } + + public int ClosestAxis() + { + return Absolute(this).MaxAxis(); + } + + public Vector3 Rotate(Vector3 axis, float angle) + { + Vector3 o = axis * Dot(axis, this); + Vector3 x = this - o; + Vector3 y = Cross(axis, this); + + return (o + x * (float)Math.Cos(angle) + y * (float)Math.Sin(angle)); + } + + public static Vector3 Lerp(Vector3 a, Vector3 b, float c) + { + return new Vector3( + a.X + (b.X - a.X) * c, + a.Y + (b.Y - a.Y) * c, + a.Z + (b.Z - a.Z) * c); + } + + public static float Angle(Vector3 a, Vector3 b) + { + float s = (float)Math.Sqrt(a.LengthSquared() * b.LengthSquared()); + if (s == 0) throw new DivideByZeroException(); + return (float)Math.Acos(Dot(a, b) / s); + } + + public static Vector3 Absolute(Vector3 a) + { + return new Vector3( + Math.Abs(a.X), + Math.Abs(a.Y), + Math.Abs(a.Z)); + } + + public static Vector3 Normalize(Vector3 a) + { + return a / a.Length(); + } + + public static Vector3 Cross(Vector3 a, Vector3 b) + { + return new Vector3( + a.Y * b.Z - a.Z * b.Y, + a.Z * b.X - a.X * b.Z, + a.X * b.Y - a.Y * b.X); + } + + public static float Dot(Vector3 a, Vector3 b) + { + return a.X * b.X + a.Y * b.Y + a.Z * b.Z; + } + + public static float Triple(Vector3 a, Vector3 b, Vector3 c) + { + return a.X * (b.Y * c.Z - b.Z * c.Y) + + a.Y * (b.Z * c.X - b.X * c.Z) + + a.Z * (b.X * c.Y - b.Y * c.X); + } + + public static float Distance(Vector3 a, Vector3 b) + { + return (b - a).Length(); + } + + public static float DistanceSquared(Vector3 a, Vector3 b) + { + return (b - a).LengthSquared(); + } + + public static Vector3 Rotate(Vector3 a, Vector3 axis, float angle) + { + Vector3 o = axis * Dot(axis, a); + Vector3 x = a - o; + Vector3 y = Cross(axis, a); + + return (o + x * (float)Math.Cos(angle) + y * (float)Math.Sin(angle)); + } + + public static Vector3 operator +(Vector3 a, Vector3 b) + { + return new Vector3(a.X + b.X, a.Y + b.Y, a.Z + b.Z); + } + + public static Vector3 operator -(Vector3 a, Vector3 b) + { + return new Vector3(a.X - b.X, a.Y - b.Y, a.Z - b.Z); + } + + public static Vector3 operator -(Vector3 a) + { + return new Vector3(-a.X, -a.Y, -a.Z); + } + + public static Vector3 operator *(float b, Vector3 a) + { + return new Vector3(a.X * b, a.Y * b, a.Z * b); + } + + public static Vector3 operator *(Vector3 a, float b) + { + return new Vector3(a.X * b, a.Y * b, a.Z * b); + } + + public static Vector3 operator *(Vector3 a, Vector3 b) + { + return new Vector3(a.X * b.X, a.Y * b.Y, a.Z * b.Z); + } + + public static Vector3 operator /(Vector3 a, float b) + { + if (b == 0) throw new DivideByZeroException(); + return new Vector3(a.X / b, a.Y / b, a.Z / b); + } + + public static Vector3 operator /(Vector3 a, Vector3 b) + { + if (b.X == 0 || b.Y == 0 || b.Z == 0) throw new DivideByZeroException(); + return new Vector3(a.X / b.X, a.Y / b.Y, a.Z / b.Z); + } + + public static bool operator ==(Vector3 a, Vector3 b) + { + return a.X == b.X && a.Y == b.Y && a.Z == b.Z; + } + + public static bool operator !=(Vector3 a, Vector3 b) + { + return a.X != b.X || a.Y != b.Y || a.Z != b.Z; + } + + public static explicit operator MonoXnaCompactMaths.Vector3(Vector3 a) + { + return new MonoXnaCompactMaths.Vector3(a.X, a.Y, a.Z); + } + + public override bool Equals(object obj) + { + return object.Equals(this, obj); + } + + public override int GetHashCode() + { + return X.GetHashCode() & Y.GetHashCode() & Z.GetHashCode(); + } + + public override string ToString() + { + return string.Format("{0}, {1}, {2}", X, Y, Z); + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector4.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector4.cs index a64ce97..de98ccd 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector4.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector4.cs @@ -1,110 +1,110 @@ -/* - Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru - Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. -*/ - -using System; -using System.Collections.Generic; -using System.Text; - -namespace XnaDevRu.BulletX.LinearMath -{ - internal class Vector4 : Vector3 - { - public Vector4() { } - - public Vector4(float x, float y, float z, float w) - : base(x, y, z) { W = w; } - - public static Vector4 Absolute4(Vector4 a) - { - return new Vector4( - Math.Abs(a.X), - Math.Abs(a.Y), - Math.Abs(a.Z), - Math.Abs(a.W)); - } - - public int MaxAxis4() - { - int maxIndex = -1; - float maxVal = -1e30f; - if (X > maxVal) - { - maxIndex = 0; - maxVal = X; - } - if (Y > maxVal) - { - maxIndex = 1; - maxVal = Y; - } - if (Z > maxVal) - { - maxIndex = 2; - maxVal = Z; - } - if (W > maxVal) - { - maxIndex = 3; - maxVal = W; - } - - return maxIndex; - } - - public int MinAxis4() - { - int minIndex = -1; - float minVal = 1e30f; - if (X < minVal) - { - minIndex = 0; - minVal = X; - } - if (Y < minVal) - { - minIndex = 1; - minVal = Y; - } - if (Z < minVal) - { - minIndex = 2; - minVal = Z; - } - if (W < minVal) - { - minIndex = 3; - minVal = W; - } - - return minIndex; - } - - public int ClosestAxis4() - { - return Absolute4(this).MaxAxis4(); - } - - public static explicit operator MonoXnaCompactMaths.Vector4(Vector4 a) - { - return new MonoXnaCompactMaths.Vector4(a.X, a.Y, a.Z, a.W); - } - } -} +/* + Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru + Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + + This software is provided 'as-is', without any express or implied + warranty. In no event will the authors be held liable for any damages + arising from the use of this software. + + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace XnaDevRu.BulletX.LinearMath +{ + internal class Vector4 : Vector3 + { + public Vector4() { } + + public Vector4(float x, float y, float z, float w) + : base(x, y, z) { W = w; } + + public static Vector4 Absolute4(Vector4 a) + { + return new Vector4( + Math.Abs(a.X), + Math.Abs(a.Y), + Math.Abs(a.Z), + Math.Abs(a.W)); + } + + public int MaxAxis4() + { + int maxIndex = -1; + float maxVal = -1e30f; + if (X > maxVal) + { + maxIndex = 0; + maxVal = X; + } + if (Y > maxVal) + { + maxIndex = 1; + maxVal = Y; + } + if (Z > maxVal) + { + maxIndex = 2; + maxVal = Z; + } + if (W > maxVal) + { + maxIndex = 3; + maxVal = W; + } + + return maxIndex; + } + + public int MinAxis4() + { + int minIndex = -1; + float minVal = 1e30f; + if (X < minVal) + { + minIndex = 0; + minVal = X; + } + if (Y < minVal) + { + minIndex = 1; + minVal = Y; + } + if (Z < minVal) + { + minIndex = 2; + minVal = Z; + } + if (W < minVal) + { + minIndex = 3; + minVal = W; + } + + return minIndex; + } + + public int ClosestAxis4() + { + return Absolute4(this).MaxAxis4(); + } + + public static explicit operator MonoXnaCompactMaths.Vector4(Vector4 a) + { + return new MonoXnaCompactMaths.Vector4(a.X, a.Y, a.Z, a.W); + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj b/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj index bed8d19..06b98bc 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj @@ -1,168 +1,168 @@ - - - Debug - AnyCPU - 8.0.50727 - 2.0 - {44270344-ACA7-4875-B585-81D5C06D0489} - Library - Properties - XnaDevRu.BulletX - Modified.XnaDevRu.BulletX - - - true - full - false - bin\Debug\ - DEBUG;TRACE - prompt - 4 - - - pdbonly - true - bin\Release\ - TRACE - prompt - 4 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - {121147BC-B06B-406C-84E9-907F268CF0EB} - MonoXnaCompactMaths - - - - - - - - - - + + + Debug + AnyCPU + 8.0.50727 + 2.0 + {44270344-ACA7-4875-B585-81D5C06D0489} + Library + Properties + XnaDevRu.BulletX + Modified.XnaDevRu.BulletX + + + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + {121147BC-B06B-406C-84E9-907F268CF0EB} + MonoXnaCompactMaths + + + + + + + + + + \ No newline at end of file diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Properties/AssemblyInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Properties/AssemblyInfo.cs index efc8fd6..03d1e3e 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Properties/AssemblyInfo.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Properties/AssemblyInfo.cs @@ -1,35 +1,35 @@ -using System.Reflection; -using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; - -// La información general sobre un ensamblado se controla mediante el siguiente -// conjunto de atributos. Cambie estos atributos para modificar la información -// asociada con un ensamblado. -[assembly: AssemblyTitle("Modified.XnaDevRu.BulletX")] -[assembly: AssemblyDescription("Modified Bullet for XNA")] -[assembly: AssemblyConfiguration("")] -[assembly: AssemblyCompany("XNADev.ru")] -[assembly: AssemblyProduct("Bullet for XNA")] -[assembly: AssemblyCopyright("Copyright © 2007 XNADev.ru")] -[assembly: AssemblyTrademark("")] -[assembly: AssemblyCulture("")] - -// Si establece ComVisible como false hace que los tipos de este ensamblado no sean visibles -// a los componentes COM. Si necesita obtener acceso a un tipo en este ensamblado desde -// COM, establezca el atributo ComVisible como true en este tipo. -[assembly: ComVisible(false)] - -// El siguiente GUID sirve como identificador de la biblioteca de tipos si este proyecto se expone a COM -[assembly: Guid("a6148224-0271-4cd8-9d0a-5d4955cd2b2c")] - -// La información de versión de un ensamblado consta de los cuatro valores siguientes: -// -// Versión principal -// Versión secundaria -// Número de versión de compilación -// Revisión -// -// Puede especificar todos los valores o puede establecer como valores predeterminados los números de revisión y generación -// mediante el asterisco ('*'), como se muestra a continuación: -[assembly: AssemblyVersion("1.0.0.0")] -[assembly: AssemblyFileVersion("1.0.0.0")] +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// La información general sobre un ensamblado se controla mediante el siguiente +// conjunto de atributos. Cambie estos atributos para modificar la información +// asociada con un ensamblado. +[assembly: AssemblyTitle("Modified.XnaDevRu.BulletX")] +[assembly: AssemblyDescription("Modified Bullet for XNA")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("XNADev.ru")] +[assembly: AssemblyProduct("Bullet for XNA")] +[assembly: AssemblyCopyright("Copyright © 2007 XNADev.ru")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// Si establece ComVisible como false hace que los tipos de este ensamblado no sean visibles +// a los componentes COM. Si necesita obtener acceso a un tipo en este ensamblado desde +// COM, establezca el atributo ComVisible como true en este tipo. +[assembly: ComVisible(false)] + +// El siguiente GUID sirve como identificador de la biblioteca de tipos si este proyecto se expone a COM +[assembly: Guid("a6148224-0271-4cd8-9d0a-5d4955cd2b2c")] + +// La información de versión de un ensamblado consta de los cuatro valores siguientes: +// +// Versión principal +// Versión secundaria +// Número de versión de compilación +// Revisión +// +// Puede especificar todos los valores o puede establecer como valores predeterminados los números de revisión y generación +// mediante el asterisco ('*'), como se muestra a continuación: +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs index a104d89..22842ca 100644 --- a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs +++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs @@ -1,683 +1,683 @@ -#region License -/* -MIT License -Copyright © 2006 The Mono.Xna Team - -All rights reserved. - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. -*/ -#endregion License - -using System; -using System.ComponentModel; -using System.Runtime.InteropServices; - -namespace MonoXnaCompactMaths -{ - [Serializable] - [StructLayout(LayoutKind.Sequential)] - //[TypeConverter(typeof(MatrixConverter))] - public struct Matrix : IEquatable - { - #region Public Constructors - - public Matrix(float m11, float m12, float m13, float m14, float m21, float m22, float m23, float m24, float m31, - float m32, float m33, float m34, float m41, float m42, float m43, float m44) - { - this.M11 = m11; - this.M12 = m12; - this.M13 = m13; - this.M14 = m14; - this.M21 = m21; - this.M22 = m22; - this.M23 = m23; - this.M24 = m24; - this.M31 = m31; - this.M32 = m32; - this.M33 = m33; - this.M34 = m34; - this.M41 = m41; - this.M42 = m42; - this.M43 = m43; - this.M44 = m44; - } - - #endregion Public Constructors - - - #region Public Fields - - public float M11; - public float M12; - public float M13; - public float M14; - public float M21; - public float M22; - public float M23; - public float M24; - public float M31; - public float M32; - public float M33; - public float M34; - public float M41; - public float M42; - public float M43; - public float M44; - - #endregion Public Fields - - - #region Private Members - private static Matrix identity = new Matrix(1f, 0f, 0f, 0f, 0f, 1f, 0f, 0f, 0f, 0f, 1f, 0f, 0f, 0f, 0f, 1f); - #endregion Private Members - - - #region Public Properties - - public Vector3 Backward - { - get - { - return new Vector3(this.M31, this.M32, this.M33); - } - set - { - this.M31 = value.X; - this.M32 = value.Y; - this.M33 = value.Z; - } - } - - - public Vector3 Down - { - get - { - return new Vector3(-this.M21, -this.M22, -this.M23); - } - set - { - this.M21 = -value.X; - this.M22 = -value.Y; - this.M23 = -value.Z; - } - } - - - public Vector3 Forward - { - get - { - return new Vector3(-this.M31, -this.M32, -this.M33); - } - set - { - this.M31 = -value.X; - this.M32 = -value.Y; - this.M33 = -value.Z; - } - } - - - public static Matrix Identity - { - get { return identity; } - } - - - public Vector3 Left - { - get - { - return new Vector3(-this.M11, -this.M12, -this.M13); - } - set - { - this.M11 = -value.X; - this.M12 = -value.Y; - this.M13 = -value.Z; - } - } - - - public Vector3 Right - { - get - { - return new Vector3(this.M11, this.M12, this.M13); - } - set - { - this.M11 = value.X; - this.M12 = value.Y; - this.M13 = value.Z; - } - } - - - public Vector3 Translation - { - get - { - return new Vector3(this.M41, this.M42, this.M43); - } - set - { - this.M41 = value.X; - this.M42 = value.Y; - this.M43 = value.Z; - } - } - - - public Vector3 Up - { - get - { - return new Vector3(this.M21, this.M22, this.M23); - } - set - { - this.M21 = value.X; - this.M22 = value.Y; - this.M23 = value.Z; - } - } - #endregion Public Properties - - - #region Public Methods - - public static Matrix Add(Matrix matrix1, Matrix matrix2) - { - throw new NotImplementedException(); - } - - - public static void Add(ref Matrix matrix1, ref Matrix matrix2, out Matrix result) - { - throw new NotImplementedException(); - } - - - public static Matrix CreateBillboard(Vector3 objectPosition, Vector3 cameraPosition, - Vector3 cameraUpVector, Nullable cameraForwardVector) - { - throw new NotImplementedException(); - } - - - public static void CreateBillboard(ref Vector3 objectPosition, ref Vector3 cameraPosition, - ref Vector3 cameraUpVector, Vector3? cameraForwardVector, out Matrix result) - { - throw new NotImplementedException(); - } - - - public static Matrix CreateConstrainedBillboard(Vector3 objectPosition, Vector3 cameraPosition, - Vector3 rotateAxis, Nullable cameraForwardVector, Nullable objectForwardVector) - { - throw new NotImplementedException(); - } - - - public static void CreateConstrainedBillboard(ref Vector3 objectPosition, ref Vector3 cameraPosition, - ref Vector3 rotateAxis, Vector3? cameraForwardVector, Vector3? objectForwardVector, out Matrix result) - { - throw new NotImplementedException(); - } - - - public static Matrix CreateFromAxisAngle(Vector3 axis, float angle) - { - throw new NotImplementedException(); - } - - - public static void CreateFromAxisAngle(ref Vector3 axis, float angle, out Matrix result) - { - throw new NotImplementedException(); - } - - - public static Matrix CreateFromQuaternion(Quaternion quaternion) - { - //--- - //http://lists.ximian.com/pipermail/mono-patches/2006-December/084667.html - float xx = quaternion.X * quaternion.X; - float xy = quaternion.X * quaternion.Y; - float xw = quaternion.X * quaternion.W; - float yy = quaternion.Y * quaternion.Y; - float yw = quaternion.Y * quaternion.W; - float yz = quaternion.Y * quaternion.Z; - float zx = quaternion.Z * quaternion.X; - float zw = quaternion.Z * quaternion.W; - float zz = quaternion.Z * quaternion.Z; - return new Matrix(1f - (2f * (yy + zz)), 2f * (xy + zw), 2f * (zx - yw), 0f, 2f * (xy - zw), - 1f - (2f * (zz + xx)), 2f * (yz + xw), 0f, 2f * (zx + yw), 2f * (yz - xw), - 1f - (2f * (yy + xx)), 0f, 0f, 0f, 0f, 1f); - //throw new NotImplementedException(); - } - - - public static void CreateFromQuaternion(ref Quaternion quaternion, out Matrix result) - { - throw new NotImplementedException(); - } - - - public static Matrix CreateLookAt(Vector3 cameraPosition, Vector3 cameraTarget, Vector3 cameraUpVector) - { - throw new NotImplementedException(); - } - - - public static void CreateLookAt(ref Vector3 cameraPosition, ref Vector3 cameraTarget, ref Vector3 cameraUpVector, out Matrix result) - { - throw new NotImplementedException(); - } - - - public static Matrix CreateOrthographic(float width, float height, float zNearPlane, float zFarPlane) - { - throw new NotImplementedException(); - } - - - public static void CreateOrthographic(float width, float height, float zNearPlane, float zFarPlane, out Matrix result) - { - throw new NotImplementedException(); - } - - - public static Matrix CreateOrthographicOffCenter(float left, float right, float bottom, float top, float zNearPlane, float zFarPlane) - { - throw new NotImplementedException(); - } - - - public static void CreateOrthographicOffCenter(float left, float right, float bottom, float top, - float zNearPlane, float zFarPlane, out Matrix result) - { - throw new NotImplementedException(); - } - - - public static Matrix CreatePerspective(float width, float height, float zNearPlane, float zFarPlane) - { - throw new NotImplementedException(); - } - - - public static void CreatePerspective(float width, float height, float zNearPlane, float zFarPlane, out Matrix result) - { - throw new NotImplementedException(); - } - - - public static Matrix CreatePerspectiveFieldOfView(float fieldOfView, float aspectRatio, float zNearPlane, float zFarPlane) - { - throw new NotImplementedException(); - } - - - public static void CreatePerspectiveFieldOfView(float fieldOfView, float aspectRatio, float nearPlaneDistance, float farPlaneDistance, out Matrix result) - { - throw new NotImplementedException(); - } - - - public static Matrix CreatePerspectiveOffCenter(float left, float right, float bottom, float top, float zNearPlane, float zFarPlane) - { - throw new NotImplementedException(); - } - - - public static void CreatePerspectiveOffCenter(float left, float right, float bottom, float top, float nearPlaneDistance, float farPlaneDistance, out Matrix result) - { - throw new NotImplementedException(); - } - - - public static Matrix CreateRotationX(float radians) - { - throw new NotImplementedException(); - } - - - public static void CreateRotationX(float radians, out Matrix result) - { - throw new NotImplementedException(); - } - - - public static Matrix CreateRotationY(float radians) - { - throw new NotImplementedException(); - } - - - public static void CreateRotationY(float radians, out Matrix result) - { - throw new NotImplementedException(); - } - - - public static Matrix CreateRotationZ(float radians) - { - throw new NotImplementedException(); - } - - - public static void CreateRotationZ(float radians, out Matrix result) - { - throw new NotImplementedException(); - } - - - public static Matrix CreateScale(float scale) - { - throw new NotImplementedException(); - } - - - public static void CreateScale(float scale, out Matrix result) - { - throw new NotImplementedException(); - } - - - public static Matrix CreateScale(float xScale, float yScale, float zScale) - { - throw new NotImplementedException(); - } - - - public static void CreateScale(float xScale, float yScale, float zScale, out Matrix result) - { - throw new NotImplementedException(); - } - - - public static Matrix CreateScale(Vector3 scales) - { - throw new NotImplementedException(); - } - - - public static void CreateScale(ref Vector3 scales, out Matrix result) - { - throw new NotImplementedException(); - } - - - public static Matrix CreateTranslation(float xPosition, float yPosition, float zPosition) - { - throw new NotImplementedException(); - } - - - public static void CreateTranslation(ref Vector3 position, out Matrix result) - { - throw new NotImplementedException(); - } - - - public static Matrix CreateTranslation(Vector3 position) - { - throw new NotImplementedException(); - } - - - public static void CreateTranslation(float xPosition, float yPosition, float zPosition, out Matrix result) - { - throw new NotImplementedException(); - } - - - public float Determinant() - { - throw new NotImplementedException(); - } - - - public static Matrix Divide(Matrix matrix1, Matrix matrix2) - { - throw new NotImplementedException(); - } - - - public static void Divide(ref Matrix matrix1, ref Matrix matrix2, out Matrix result) - { - throw new NotImplementedException(); - } - - - public static Matrix Divide(Matrix matrix1, float divider) - { - throw new NotImplementedException(); - } - - - public static void Divide(ref Matrix matrix1, float divider, out Matrix result) - { - throw new NotImplementedException(); - } - - - public bool Equals(Matrix other) - { - throw new NotImplementedException(); - } - - - public override bool Equals(object obj) - { - throw new NotImplementedException(); - } - - - public override int GetHashCode() - { - throw new NotImplementedException(); - } - - - public static Matrix Invert(Matrix matrix) - { - throw new NotImplementedException(); - } - - - public static void Invert(ref Matrix matrix, out Matrix result) - { - throw new NotImplementedException(); - } - - - public static Matrix Lerp(Matrix matrix1, Matrix matrix2, float amount) - { - throw new NotImplementedException(); - } - - - public static void Lerp(ref Matrix matrix1, ref Matrix matrix2, float amount, out Matrix result) - { - throw new NotImplementedException(); - } - - public static Matrix Multiply(Matrix matrix1, Matrix matrix2) - { - throw new NotImplementedException(); - } - - - public static void Multiply(ref Matrix matrix1, ref Matrix matrix2, out Matrix result) - { - throw new NotImplementedException(); - } - - - public static Matrix Multiply(Matrix matrix1, float factor) - { - throw new NotImplementedException(); - } - - - public static void Multiply(ref Matrix matrix1, float factor, out Matrix result) - { - throw new NotImplementedException(); - } - - - public static Matrix Negate(Matrix matrix) - { - throw new NotImplementedException(); - } - - - public static void Negate(ref Matrix matrix, out Matrix result) - { - throw new NotImplementedException(); - } - - - public static Matrix operator +(Matrix matrix1, Matrix matrix2) - { - throw new NotImplementedException(); - } - - - public static Matrix operator /(Matrix matrix1, Matrix matrix2) - { - throw new NotImplementedException(); - } - - - public static Matrix operator /(Matrix matrix1, float divider) - { - throw new NotImplementedException(); - } - - - public static bool operator ==(Matrix matrix1, Matrix matrix2) - { - throw new NotImplementedException(); - } - - - public static bool operator !=(Matrix matrix1, Matrix matrix2) - { - throw new NotImplementedException(); - } - - - public static Matrix operator *(Matrix matrix1, Matrix matrix2) - { - //--- - float[, ] arrayMatrix1 = new float[4, 4]; - float[, ] arrayMatrix2 = new float[4, 4]; - float[, ] arrayMatrixProduct = new float[4, 4]; - arrayMatrix1[0, 0] = matrix1.M11; arrayMatrix1[0, 1] = matrix1.M12; arrayMatrix1[0, 2] = matrix1.M13; arrayMatrix1[0, 3] = matrix1.M14; - arrayMatrix1[1, 0] = matrix1.M21; arrayMatrix1[1, 1] = matrix1.M22; arrayMatrix1[1, 2] = matrix1.M23; arrayMatrix1[1, 3] = matrix1.M24; - arrayMatrix1[2, 0] = matrix1.M31; arrayMatrix1[2, 1] = matrix1.M32; arrayMatrix1[2, 2] = matrix1.M33; arrayMatrix1[2, 3] = matrix1.M34; - arrayMatrix1[3, 0] = matrix1.M41; arrayMatrix1[3, 1] = matrix1.M42; arrayMatrix1[3, 2] = matrix1.M43; arrayMatrix1[3, 3] = matrix1.M44; - - arrayMatrix2[0, 0] = matrix2.M11; arrayMatrix2[0, 1] = matrix2.M12; arrayMatrix2[0, 2] = matrix2.M13; arrayMatrix2[0, 3] = matrix2.M14; - arrayMatrix2[1, 0] = matrix2.M21; arrayMatrix2[1, 1] = matrix2.M22; arrayMatrix2[1, 2] = matrix2.M23; arrayMatrix2[1, 3] = matrix2.M24; - arrayMatrix2[2, 0] = matrix2.M31; arrayMatrix2[2, 1] = matrix2.M32; arrayMatrix2[2, 2] = matrix2.M33; arrayMatrix2[2, 3] = matrix2.M34; - arrayMatrix2[3, 0] = matrix2.M41; arrayMatrix2[3, 1] = matrix2.M42; arrayMatrix2[3, 2] = matrix2.M43; arrayMatrix2[3, 3] = matrix2.M44; - - int n = 4; - for (int i = 0; i < n; i++) - { - for (int j = 0; j < n; j++) - { - // (AB)[i,j] = Sum(k=0; k < 4; k++) { A[i,k] * B[k, j] } - for (int k = 0; k < n; k++) - { - arrayMatrixProduct[i, j] += arrayMatrix1[i, k] * arrayMatrix2[k, j]; - } - } - } - return new Matrix( arrayMatrixProduct[0, 0], arrayMatrixProduct[0, 1], arrayMatrixProduct[0, 2], arrayMatrixProduct[0, 3], - arrayMatrixProduct[1, 0], arrayMatrixProduct[1, 1], arrayMatrixProduct[1, 2], arrayMatrixProduct[1, 3], - arrayMatrixProduct[2, 0], arrayMatrixProduct[2, 1], arrayMatrixProduct[2, 2], arrayMatrixProduct[2, 3], - arrayMatrixProduct[3, 1], arrayMatrixProduct[3, 1], arrayMatrixProduct[3, 2], arrayMatrixProduct[3, 3]); - //--- - //throw new NotImplementedException(); - } - - - public static Matrix operator *(Matrix matrix, float scaleFactor) - { - throw new NotImplementedException(); - } - - - public static Matrix operator -(Matrix matrix1, Matrix matrix2) - { - throw new NotImplementedException(); - } - - - public static Matrix operator -(Matrix matrix1) - { - throw new NotImplementedException(); - } - - - public static Matrix Subtract(Matrix matrix1, Matrix matrix2) - { - throw new NotImplementedException(); - } - - - public static void Subtract(ref Matrix matrix1, ref Matrix matrix2, out Matrix result) - { - throw new NotImplementedException(); - } - - - public override string ToString() - { - throw new NotImplementedException(); - } - - - public static Matrix Transpose(Matrix matrix) - { - //--- - return new Matrix( matrix.M11, matrix.M21, matrix.M31, matrix.M41, - matrix.M12, matrix.M22, matrix.M32, matrix.M42, - matrix.M13, matrix.M23, matrix.M33, matrix.M43, - matrix.M14, matrix.M24, matrix.M34, matrix.M44); - //--- - //throw new NotImplementedException(); - } - - - public static void Transpose(ref Matrix matrix, out Matrix result) - { - throw new NotImplementedException(); - } - #endregion Public Methods - } -} +#region License +/* +MIT License +Copyright © 2006 The Mono.Xna Team + +All rights reserved. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ +#endregion License + +using System; +using System.ComponentModel; +using System.Runtime.InteropServices; + +namespace MonoXnaCompactMaths +{ + [Serializable] + [StructLayout(LayoutKind.Sequential)] + //[TypeConverter(typeof(MatrixConverter))] + public struct Matrix : IEquatable + { + #region Public Constructors + + public Matrix(float m11, float m12, float m13, float m14, float m21, float m22, float m23, float m24, float m31, + float m32, float m33, float m34, float m41, float m42, float m43, float m44) + { + this.M11 = m11; + this.M12 = m12; + this.M13 = m13; + this.M14 = m14; + this.M21 = m21; + this.M22 = m22; + this.M23 = m23; + this.M24 = m24; + this.M31 = m31; + this.M32 = m32; + this.M33 = m33; + this.M34 = m34; + this.M41 = m41; + this.M42 = m42; + this.M43 = m43; + this.M44 = m44; + } + + #endregion Public Constructors + + + #region Public Fields + + public float M11; + public float M12; + public float M13; + public float M14; + public float M21; + public float M22; + public float M23; + public float M24; + public float M31; + public float M32; + public float M33; + public float M34; + public float M41; + public float M42; + public float M43; + public float M44; + + #endregion Public Fields + + + #region Private Members + private static Matrix identity = new Matrix(1f, 0f, 0f, 0f, 0f, 1f, 0f, 0f, 0f, 0f, 1f, 0f, 0f, 0f, 0f, 1f); + #endregion Private Members + + + #region Public Properties + + public Vector3 Backward + { + get + { + return new Vector3(this.M31, this.M32, this.M33); + } + set + { + this.M31 = value.X; + this.M32 = value.Y; + this.M33 = value.Z; + } + } + + + public Vector3 Down + { + get + { + return new Vector3(-this.M21, -this.M22, -this.M23); + } + set + { + this.M21 = -value.X; + this.M22 = -value.Y; + this.M23 = -value.Z; + } + } + + + public Vector3 Forward + { + get + { + return new Vector3(-this.M31, -this.M32, -this.M33); + } + set + { + this.M31 = -value.X; + this.M32 = -value.Y; + this.M33 = -value.Z; + } + } + + + public static Matrix Identity + { + get { return identity; } + } + + + public Vector3 Left + { + get + { + return new Vector3(-this.M11, -this.M12, -this.M13); + } + set + { + this.M11 = -value.X; + this.M12 = -value.Y; + this.M13 = -value.Z; + } + } + + + public Vector3 Right + { + get + { + return new Vector3(this.M11, this.M12, this.M13); + } + set + { + this.M11 = value.X; + this.M12 = value.Y; + this.M13 = value.Z; + } + } + + + public Vector3 Translation + { + get + { + return new Vector3(this.M41, this.M42, this.M43); + } + set + { + this.M41 = value.X; + this.M42 = value.Y; + this.M43 = value.Z; + } + } + + + public Vector3 Up + { + get + { + return new Vector3(this.M21, this.M22, this.M23); + } + set + { + this.M21 = value.X; + this.M22 = value.Y; + this.M23 = value.Z; + } + } + #endregion Public Properties + + + #region Public Methods + + public static Matrix Add(Matrix matrix1, Matrix matrix2) + { + throw new NotImplementedException(); + } + + + public static void Add(ref Matrix matrix1, ref Matrix matrix2, out Matrix result) + { + throw new NotImplementedException(); + } + + + public static Matrix CreateBillboard(Vector3 objectPosition, Vector3 cameraPosition, + Vector3 cameraUpVector, Nullable cameraForwardVector) + { + throw new NotImplementedException(); + } + + + public static void CreateBillboard(ref Vector3 objectPosition, ref Vector3 cameraPosition, + ref Vector3 cameraUpVector, Vector3? cameraForwardVector, out Matrix result) + { + throw new NotImplementedException(); + } + + + public static Matrix CreateConstrainedBillboard(Vector3 objectPosition, Vector3 cameraPosition, + Vector3 rotateAxis, Nullable cameraForwardVector, Nullable objectForwardVector) + { + throw new NotImplementedException(); + } + + + public static void CreateConstrainedBillboard(ref Vector3 objectPosition, ref Vector3 cameraPosition, + ref Vector3 rotateAxis, Vector3? cameraForwardVector, Vector3? objectForwardVector, out Matrix result) + { + throw new NotImplementedException(); + } + + + public static Matrix CreateFromAxisAngle(Vector3 axis, float angle) + { + throw new NotImplementedException(); + } + + + public static void CreateFromAxisAngle(ref Vector3 axis, float angle, out Matrix result) + { + throw new NotImplementedException(); + } + + + public static Matrix CreateFromQuaternion(Quaternion quaternion) + { + //--- + //http://lists.ximian.com/pipermail/mono-patches/2006-December/084667.html + float xx = quaternion.X * quaternion.X; + float xy = quaternion.X * quaternion.Y; + float xw = quaternion.X * quaternion.W; + float yy = quaternion.Y * quaternion.Y; + float yw = quaternion.Y * quaternion.W; + float yz = quaternion.Y * quaternion.Z; + float zx = quaternion.Z * quaternion.X; + float zw = quaternion.Z * quaternion.W; + float zz = quaternion.Z * quaternion.Z; + return new Matrix(1f - (2f * (yy + zz)), 2f * (xy + zw), 2f * (zx - yw), 0f, 2f * (xy - zw), + 1f - (2f * (zz + xx)), 2f * (yz + xw), 0f, 2f * (zx + yw), 2f * (yz - xw), + 1f - (2f * (yy + xx)), 0f, 0f, 0f, 0f, 1f); + //throw new NotImplementedException(); + } + + + public static void CreateFromQuaternion(ref Quaternion quaternion, out Matrix result) + { + throw new NotImplementedException(); + } + + + public static Matrix CreateLookAt(Vector3 cameraPosition, Vector3 cameraTarget, Vector3 cameraUpVector) + { + throw new NotImplementedException(); + } + + + public static void CreateLookAt(ref Vector3 cameraPosition, ref Vector3 cameraTarget, ref Vector3 cameraUpVector, out Matrix result) + { + throw new NotImplementedException(); + } + + + public static Matrix CreateOrthographic(float width, float height, float zNearPlane, float zFarPlane) + { + throw new NotImplementedException(); + } + + + public static void CreateOrthographic(float width, float height, float zNearPlane, float zFarPlane, out Matrix result) + { + throw new NotImplementedException(); + } + + + public static Matrix CreateOrthographicOffCenter(float left, float right, float bottom, float top, float zNearPlane, float zFarPlane) + { + throw new NotImplementedException(); + } + + + public static void CreateOrthographicOffCenter(float left, float right, float bottom, float top, + float zNearPlane, float zFarPlane, out Matrix result) + { + throw new NotImplementedException(); + } + + + public static Matrix CreatePerspective(float width, float height, float zNearPlane, float zFarPlane) + { + throw new NotImplementedException(); + } + + + public static void CreatePerspective(float width, float height, float zNearPlane, float zFarPlane, out Matrix result) + { + throw new NotImplementedException(); + } + + + public static Matrix CreatePerspectiveFieldOfView(float fieldOfView, float aspectRatio, float zNearPlane, float zFarPlane) + { + throw new NotImplementedException(); + } + + + public static void CreatePerspectiveFieldOfView(float fieldOfView, float aspectRatio, float nearPlaneDistance, float farPlaneDistance, out Matrix result) + { + throw new NotImplementedException(); + } + + + public static Matrix CreatePerspectiveOffCenter(float left, float right, float bottom, float top, float zNearPlane, float zFarPlane) + { + throw new NotImplementedException(); + } + + + public static void CreatePerspectiveOffCenter(float left, float right, float bottom, float top, float nearPlaneDistance, float farPlaneDistance, out Matrix result) + { + throw new NotImplementedException(); + } + + + public static Matrix CreateRotationX(float radians) + { + throw new NotImplementedException(); + } + + + public static void CreateRotationX(float radians, out Matrix result) + { + throw new NotImplementedException(); + } + + + public static Matrix CreateRotationY(float radians) + { + throw new NotImplementedException(); + } + + + public static void CreateRotationY(float radians, out Matrix result) + { + throw new NotImplementedException(); + } + + + public static Matrix CreateRotationZ(float radians) + { + throw new NotImplementedException(); + } + + + public static void CreateRotationZ(float radians, out Matrix result) + { + throw new NotImplementedException(); + } + + + public static Matrix CreateScale(float scale) + { + throw new NotImplementedException(); + } + + + public static void CreateScale(float scale, out Matrix result) + { + throw new NotImplementedException(); + } + + + public static Matrix CreateScale(float xScale, float yScale, float zScale) + { + throw new NotImplementedException(); + } + + + public static void CreateScale(float xScale, float yScale, float zScale, out Matrix result) + { + throw new NotImplementedException(); + } + + + public static Matrix CreateScale(Vector3 scales) + { + throw new NotImplementedException(); + } + + + public static void CreateScale(ref Vector3 scales, out Matrix result) + { + throw new NotImplementedException(); + } + + + public static Matrix CreateTranslation(float xPosition, float yPosition, float zPosition) + { + throw new NotImplementedException(); + } + + + public static void CreateTranslation(ref Vector3 position, out Matrix result) + { + throw new NotImplementedException(); + } + + + public static Matrix CreateTranslation(Vector3 position) + { + throw new NotImplementedException(); + } + + + public static void CreateTranslation(float xPosition, float yPosition, float zPosition, out Matrix result) + { + throw new NotImplementedException(); + } + + + public float Determinant() + { + throw new NotImplementedException(); + } + + + public static Matrix Divide(Matrix matrix1, Matrix matrix2) + { + throw new NotImplementedException(); + } + + + public static void Divide(ref Matrix matrix1, ref Matrix matrix2, out Matrix result) + { + throw new NotImplementedException(); + } + + + public static Matrix Divide(Matrix matrix1, float divider) + { + throw new NotImplementedException(); + } + + + public static void Divide(ref Matrix matrix1, float divider, out Matrix result) + { + throw new NotImplementedException(); + } + + + public bool Equals(Matrix other) + { + throw new NotImplementedException(); + } + + + public override bool Equals(object obj) + { + throw new NotImplementedException(); + } + + + public override int GetHashCode() + { + throw new NotImplementedException(); + } + + + public static Matrix Invert(Matrix matrix) + { + throw new NotImplementedException(); + } + + + public static void Invert(ref Matrix matrix, out Matrix result) + { + throw new NotImplementedException(); + } + + + public static Matrix Lerp(Matrix matrix1, Matrix matrix2, float amount) + { + throw new NotImplementedException(); + } + + + public static void Lerp(ref Matrix matrix1, ref Matrix matrix2, float amount, out Matrix result) + { + throw new NotImplementedException(); + } + + public static Matrix Multiply(Matrix matrix1, Matrix matrix2) + { + throw new NotImplementedException(); + } + + + public static void Multiply(ref Matrix matrix1, ref Matrix matrix2, out Matrix result) + { + throw new NotImplementedException(); + } + + + public static Matrix Multiply(Matrix matrix1, float factor) + { + throw new NotImplementedException(); + } + + + public static void Multiply(ref Matrix matrix1, float factor, out Matrix result) + { + throw new NotImplementedException(); + } + + + public static Matrix Negate(Matrix matrix) + { + throw new NotImplementedException(); + } + + + public static void Negate(ref Matrix matrix, out Matrix result) + { + throw new NotImplementedException(); + } + + + public static Matrix operator +(Matrix matrix1, Matrix matrix2) + { + throw new NotImplementedException(); + } + + + public static Matrix operator /(Matrix matrix1, Matrix matrix2) + { + throw new NotImplementedException(); + } + + + public static Matrix operator /(Matrix matrix1, float divider) + { + throw new NotImplementedException(); + } + + + public static bool operator ==(Matrix matrix1, Matrix matrix2) + { + throw new NotImplementedException(); + } + + + public static bool operator !=(Matrix matrix1, Matrix matrix2) + { + throw new NotImplementedException(); + } + + + public static Matrix operator *(Matrix matrix1, Matrix matrix2) + { + //--- + float[, ] arrayMatrix1 = new float[4, 4]; + float[, ] arrayMatrix2 = new float[4, 4]; + float[, ] arrayMatrixProduct = new float[4, 4]; + arrayMatrix1[0, 0] = matrix1.M11; arrayMatrix1[0, 1] = matrix1.M12; arrayMatrix1[0, 2] = matrix1.M13; arrayMatrix1[0, 3] = matrix1.M14; + arrayMatrix1[1, 0] = matrix1.M21; arrayMatrix1[1, 1] = matrix1.M22; arrayMatrix1[1, 2] = matrix1.M23; arrayMatrix1[1, 3] = matrix1.M24; + arrayMatrix1[2, 0] = matrix1.M31; arrayMatrix1[2, 1] = matrix1.M32; arrayMatrix1[2, 2] = matrix1.M33; arrayMatrix1[2, 3] = matrix1.M34; + arrayMatrix1[3, 0] = matrix1.M41; arrayMatrix1[3, 1] = matrix1.M42; arrayMatrix1[3, 2] = matrix1.M43; arrayMatrix1[3, 3] = matrix1.M44; + + arrayMatrix2[0, 0] = matrix2.M11; arrayMatrix2[0, 1] = matrix2.M12; arrayMatrix2[0, 2] = matrix2.M13; arrayMatrix2[0, 3] = matrix2.M14; + arrayMatrix2[1, 0] = matrix2.M21; arrayMatrix2[1, 1] = matrix2.M22; arrayMatrix2[1, 2] = matrix2.M23; arrayMatrix2[1, 3] = matrix2.M24; + arrayMatrix2[2, 0] = matrix2.M31; arrayMatrix2[2, 1] = matrix2.M32; arrayMatrix2[2, 2] = matrix2.M33; arrayMatrix2[2, 3] = matrix2.M34; + arrayMatrix2[3, 0] = matrix2.M41; arrayMatrix2[3, 1] = matrix2.M42; arrayMatrix2[3, 2] = matrix2.M43; arrayMatrix2[3, 3] = matrix2.M44; + + int n = 4; + for (int i = 0; i < n; i++) + { + for (int j = 0; j < n; j++) + { + // (AB)[i,j] = Sum(k=0; k < 4; k++) { A[i,k] * B[k, j] } + for (int k = 0; k < n; k++) + { + arrayMatrixProduct[i, j] += arrayMatrix1[i, k] * arrayMatrix2[k, j]; + } + } + } + return new Matrix( arrayMatrixProduct[0, 0], arrayMatrixProduct[0, 1], arrayMatrixProduct[0, 2], arrayMatrixProduct[0, 3], + arrayMatrixProduct[1, 0], arrayMatrixProduct[1, 1], arrayMatrixProduct[1, 2], arrayMatrixProduct[1, 3], + arrayMatrixProduct[2, 0], arrayMatrixProduct[2, 1], arrayMatrixProduct[2, 2], arrayMatrixProduct[2, 3], + arrayMatrixProduct[3, 1], arrayMatrixProduct[3, 1], arrayMatrixProduct[3, 2], arrayMatrixProduct[3, 3]); + //--- + //throw new NotImplementedException(); + } + + + public static Matrix operator *(Matrix matrix, float scaleFactor) + { + throw new NotImplementedException(); + } + + + public static Matrix operator -(Matrix matrix1, Matrix matrix2) + { + throw new NotImplementedException(); + } + + + public static Matrix operator -(Matrix matrix1) + { + throw new NotImplementedException(); + } + + + public static Matrix Subtract(Matrix matrix1, Matrix matrix2) + { + throw new NotImplementedException(); + } + + + public static void Subtract(ref Matrix matrix1, ref Matrix matrix2, out Matrix result) + { + throw new NotImplementedException(); + } + + + public override string ToString() + { + throw new NotImplementedException(); + } + + + public static Matrix Transpose(Matrix matrix) + { + //--- + return new Matrix( matrix.M11, matrix.M21, matrix.M31, matrix.M41, + matrix.M12, matrix.M22, matrix.M32, matrix.M42, + matrix.M13, matrix.M23, matrix.M33, matrix.M43, + matrix.M14, matrix.M24, matrix.M34, matrix.M44); + //--- + //throw new NotImplementedException(); + } + + + public static void Transpose(ref Matrix matrix, out Matrix result) + { + throw new NotImplementedException(); + } + #endregion Public Methods + } +} diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/MonoXnaCompactMaths.csproj b/libraries/ModifiedBulletX/MonoXnaCompactMaths/MonoXnaCompactMaths.csproj index c3ec4dd..79c5263 100644 --- a/libraries/ModifiedBulletX/MonoXnaCompactMaths/MonoXnaCompactMaths.csproj +++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/MonoXnaCompactMaths.csproj @@ -1,51 +1,51 @@ - - - - Debug - AnyCPU - 8.0.50727 - 2.0 - {121147BC-B06B-406C-84E9-907F268CF0EB} - Library - Properties - MonoXnaCompactMaths - MonoXnaCompactMaths - - - - - true - full - false - bin\Debug\ - DEBUG;TRACE - prompt - 4 - - - pdbonly - true - bin\Release\ - TRACE - prompt - 4 - - - - - - - - - - - - - + + + + Debug + AnyCPU + 8.0.50727 + 2.0 + {121147BC-B06B-406C-84E9-907F268CF0EB} + Library + Properties + MonoXnaCompactMaths + MonoXnaCompactMaths + + + + + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + + + + + + + + + + + \ No newline at end of file diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Properties/AssemblyInfo.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Properties/AssemblyInfo.cs index 748b94a..366f664 100644 --- a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Properties/AssemblyInfo.cs +++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Properties/AssemblyInfo.cs @@ -1,33 +1,33 @@ -using System.Reflection; -using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; - -// La información general sobre un ensamblado se controla mediante el siguiente -// conjunto de atributos. Cambie estos atributos para modificar la información -// asociada con un ensamblado. -[assembly: AssemblyTitle("MonoXnaCompactMaths")] -[assembly: AssemblyDescription("MonoXnaCompactMaths (Vector3, Vector4, Quaternion and Matrix)")] -[assembly: AssemblyConfiguration("")] -[assembly: AssemblyCompany("")] -[assembly: AssemblyProduct("")] -[assembly: AssemblyCopyright("Copyright 2006 - 2007 The Mono.Xna Team")] -[assembly: AssemblyTrademark("")] -[assembly: AssemblyCulture("")] - -// Si establece ComVisible como false hace que los tipos de este ensamblado no sean visibles -// a los componentes COM. Si necesita obtener acceso a un tipo en este ensamblado desde -// COM, establezca el atributo ComVisible como true en este tipo. -[assembly: ComVisible(false)] - -// El siguiente GUID sirve como identificador de la biblioteca de tipos si este proyecto se expone a COM -[assembly: Guid("79f9dfd1-60f8-44b8-ab08-6f6e341976b7")] - -// La información de versión de un ensamblado consta de los cuatro valores siguientes: -// -// Versión principal -// Versión secundaria -// Número de versión de compilación -// Revisión -// -[assembly: AssemblyVersion("1.0.0.0")] -[assembly: AssemblyFileVersion("1.0.0.0")] +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// La información general sobre un ensamblado se controla mediante el siguiente +// conjunto de atributos. Cambie estos atributos para modificar la información +// asociada con un ensamblado. +[assembly: AssemblyTitle("MonoXnaCompactMaths")] +[assembly: AssemblyDescription("MonoXnaCompactMaths (Vector3, Vector4, Quaternion and Matrix)")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("")] +[assembly: AssemblyCopyright("Copyright 2006 - 2007 The Mono.Xna Team")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// Si establece ComVisible como false hace que los tipos de este ensamblado no sean visibles +// a los componentes COM. Si necesita obtener acceso a un tipo en este ensamblado desde +// COM, establezca el atributo ComVisible como true en este tipo. +[assembly: ComVisible(false)] + +// El siguiente GUID sirve como identificador de la biblioteca de tipos si este proyecto se expone a COM +[assembly: Guid("79f9dfd1-60f8-44b8-ab08-6f6e341976b7")] + +// La información de versión de un ensamblado consta de los cuatro valores siguientes: +// +// Versión principal +// Versión secundaria +// Número de versión de compilación +// Revisión +// +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs index 6ed1443..b4f1873 100644 --- a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs +++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs @@ -1,346 +1,346 @@ -#region License -/* -MIT License -Copyright © 2006 The Mono.Xna Team - -All rights reserved. - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. -*/ -#endregion License - -using System; -using System.ComponentModel; - -namespace MonoXnaCompactMaths -{ - [Serializable] - //[TypeConverter(typeof(QuaternionConverter))] - public struct Quaternion : IEquatable - { - public float X; - public float Y; - public float Z; - public float W; - static Quaternion identity = new Quaternion(0, 0, 0, 1); - - - public Quaternion(float x, float y, float z, float w) - { - this.X = x; - this.Y = y; - this.Z = z; - this.W = w; - } - - - public Quaternion(Vector3 vectorPart, float scalarPart) - { - this.X = vectorPart.X; - this.Y = vectorPart.Y; - this.Z = vectorPart.Z; - this.W = scalarPart; - } - - public static Quaternion Identity - { - get{ return identity; } - } - - - public static Quaternion Add(Quaternion quaternion1, Quaternion quaternion2) - { - throw new NotImplementedException(); - } - - - public static void Add(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result) - { - throw new NotImplementedException(); - } - - - public static Quaternion CreateFromAxisAngle(Vector3 axis, float angle) - { - throw new NotImplementedException(); - } - - - public static void CreateFromAxisAngle(ref Vector3 axis, float angle, out Quaternion result) - { - throw new NotImplementedException(); - } - - - public static Quaternion CreateFromRotationMatrix(Matrix matrix) - { - throw new NotImplementedException(); - } - - - public static void CreateFromRotationMatrix(ref Matrix matrix, out Quaternion result) - { - throw new NotImplementedException(); - } - - - public static Quaternion Divide(Quaternion quaternion1, Quaternion quaternion2) - { - throw new NotImplementedException(); - } - - - public static void Divide(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result) - { - throw new NotImplementedException(); - } - - - public static float Dot(Quaternion quaternion1, Quaternion quaternion2) - { - throw new NotImplementedException(); - } - - - public static void Dot(ref Quaternion quaternion1, ref Quaternion quaternion2, out float result) - { - throw new NotImplementedException(); - } - - - public override bool Equals(object obj) - { - throw new NotImplementedException(); - } - - - public bool Equals(Quaternion other) - { - throw new NotImplementedException(); - } - - - public override int GetHashCode() - { - throw new NotImplementedException(); - } - - - public static Quaternion Inverse(Quaternion quaternion) - { - throw new NotImplementedException(); - } - - - public static void Inverse(ref Quaternion quaternion, out Quaternion result) - { - throw new NotImplementedException(); - } - - - public float Length() - { - //--- - return (float)Math.Sqrt(Math.Pow(this.W, 2.0) + Math.Pow(this.X, 2.0) + Math.Pow(this.Y, 2.0) + Math.Pow(this.Z, 2.0)); - //--- - //throw new NotImplementedException(); - } - - - public float LengthSquared() - { - //--- - return (float)(Math.Pow(this.W, 2.0) + Math.Pow(this.X, 2.0) + Math.Pow(this.Y, 2.0) + Math.Pow(this.Z, 2.0)); - //--- - //throw new NotImplementedException(); - } - - - public static Quaternion Lerp(Quaternion quaternion1, Quaternion quaternion2, float amount) - { - throw new NotImplementedException(); - } - - - public static void Lerp(ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result) - { - throw new NotImplementedException(); - } - - - public static Quaternion Slerp(Quaternion quaternion1, Quaternion quaternion2, float amount) - { - throw new NotImplementedException(); - } - - - public static void Slerp(ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result) - { - throw new NotImplementedException(); - } - - - public static Quaternion Subtract(Quaternion quaternion1, Quaternion quaternion2) - { - throw new NotImplementedException(); - } - - - public static void Subtract(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result) - { - throw new NotImplementedException(); - } - - - public static Quaternion Multiply(Quaternion quaternion1, Quaternion quaternion2) - { - throw new NotImplementedException(); - } - - - public static Quaternion Multiply(Quaternion quaternion1, float scaleFactor) - { - throw new NotImplementedException(); - } - - - public static void Multiply(ref Quaternion quaternion1, float scaleFactor, out Quaternion result) - { - throw new NotImplementedException(); - } - - - public static void Multiply(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result) - { - throw new NotImplementedException(); - } - - - public static Quaternion Negate(Quaternion quaternion) - { - throw new NotImplementedException(); - } - - - public static void Negate(ref Quaternion quaternion, out Quaternion result) - { - throw new NotImplementedException(); - } - - - public void Normalize() - { - //--- - this = Normalize(this); - //--- - //throw new NotImplementedException(); - } - - - public static Quaternion Normalize(Quaternion quaternion) - { - //--- - return quaternion / quaternion.Length(); - //--- - //throw new NotImplementedException(); - } - - - public static void Normalize(ref Quaternion quaternion, out Quaternion result) - { - throw new NotImplementedException(); - } - - - public static Quaternion operator +(Quaternion quaternion1, Quaternion quaternion2) - { - throw new NotImplementedException(); - } - - - public static Quaternion operator /(Quaternion quaternion1, Quaternion quaternion2) - { - throw new NotImplementedException(); - } - public static Quaternion operator /(Quaternion quaternion, float factor) - { - quaternion.W /= factor; - quaternion.X /= factor; - quaternion.Y /= factor; - quaternion.Z /= factor; - return quaternion; - } - - public static bool operator ==(Quaternion quaternion1, Quaternion quaternion2) - { - throw new NotImplementedException(); - } - - - public static bool operator !=(Quaternion quaternion1, Quaternion quaternion2) - { - throw new NotImplementedException(); - } - - - public static Quaternion operator *(Quaternion quaternion1, Quaternion quaternion2) - { - //--- - //Grassmann product - Quaternion quaternionProduct = new Quaternion(); - - quaternionProduct.W = quaternion1.W * quaternion2.W - quaternion1.X * quaternion2.X - quaternion1.Y * quaternion2.Y - quaternion1.Z * quaternion2.Z; - quaternionProduct.X = quaternion1.W * quaternion2.X + quaternion1.X * quaternion2.W + quaternion1.Y * quaternion2.Z - quaternion1.Z * quaternion2.Y; - quaternionProduct.Y = quaternion1.W * quaternion2.Y - quaternion1.X * quaternion2.Z + quaternion1.Y * quaternion2.W + quaternion1.Z * quaternion2.X; - quaternionProduct.Z = quaternion1.W * quaternion2.Z + quaternion1.X * quaternion2.Y - quaternion1.Y * quaternion2.X + quaternion1.Z * quaternion2.W; - return quaternionProduct; - //--- - //throw new NotImplementedException(); - } - - - public static Quaternion operator *(Quaternion quaternion1, float scaleFactor) - { - throw new NotImplementedException(); - } - - - public static Quaternion operator -(Quaternion quaternion1, Quaternion quaternion2) - { - throw new NotImplementedException(); - } - - - public static Quaternion operator -(Quaternion quaternion) - { - throw new NotImplementedException(); - } - - - public override string ToString() - { - throw new NotImplementedException(); - } - - private static void Conjugate(ref Quaternion quaternion, out Quaternion result) - { - throw new NotImplementedException(); - } - } -} +#region License +/* +MIT License +Copyright © 2006 The Mono.Xna Team + +All rights reserved. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ +#endregion License + +using System; +using System.ComponentModel; + +namespace MonoXnaCompactMaths +{ + [Serializable] + //[TypeConverter(typeof(QuaternionConverter))] + public struct Quaternion : IEquatable + { + public float X; + public float Y; + public float Z; + public float W; + static Quaternion identity = new Quaternion(0, 0, 0, 1); + + + public Quaternion(float x, float y, float z, float w) + { + this.X = x; + this.Y = y; + this.Z = z; + this.W = w; + } + + + public Quaternion(Vector3 vectorPart, float scalarPart) + { + this.X = vectorPart.X; + this.Y = vectorPart.Y; + this.Z = vectorPart.Z; + this.W = scalarPart; + } + + public static Quaternion Identity + { + get{ return identity; } + } + + + public static Quaternion Add(Quaternion quaternion1, Quaternion quaternion2) + { + throw new NotImplementedException(); + } + + + public static void Add(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result) + { + throw new NotImplementedException(); + } + + + public static Quaternion CreateFromAxisAngle(Vector3 axis, float angle) + { + throw new NotImplementedException(); + } + + + public static void CreateFromAxisAngle(ref Vector3 axis, float angle, out Quaternion result) + { + throw new NotImplementedException(); + } + + + public static Quaternion CreateFromRotationMatrix(Matrix matrix) + { + throw new NotImplementedException(); + } + + + public static void CreateFromRotationMatrix(ref Matrix matrix, out Quaternion result) + { + throw new NotImplementedException(); + } + + + public static Quaternion Divide(Quaternion quaternion1, Quaternion quaternion2) + { + throw new NotImplementedException(); + } + + + public static void Divide(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result) + { + throw new NotImplementedException(); + } + + + public static float Dot(Quaternion quaternion1, Quaternion quaternion2) + { + throw new NotImplementedException(); + } + + + public static void Dot(ref Quaternion quaternion1, ref Quaternion quaternion2, out float result) + { + throw new NotImplementedException(); + } + + + public override bool Equals(object obj) + { + throw new NotImplementedException(); + } + + + public bool Equals(Quaternion other) + { + throw new NotImplementedException(); + } + + + public override int GetHashCode() + { + throw new NotImplementedException(); + } + + + public static Quaternion Inverse(Quaternion quaternion) + { + throw new NotImplementedException(); + } + + + public static void Inverse(ref Quaternion quaternion, out Quaternion result) + { + throw new NotImplementedException(); + } + + + public float Length() + { + //--- + return (float)Math.Sqrt(Math.Pow(this.W, 2.0) + Math.Pow(this.X, 2.0) + Math.Pow(this.Y, 2.0) + Math.Pow(this.Z, 2.0)); + //--- + //throw new NotImplementedException(); + } + + + public float LengthSquared() + { + //--- + return (float)(Math.Pow(this.W, 2.0) + Math.Pow(this.X, 2.0) + Math.Pow(this.Y, 2.0) + Math.Pow(this.Z, 2.0)); + //--- + //throw new NotImplementedException(); + } + + + public static Quaternion Lerp(Quaternion quaternion1, Quaternion quaternion2, float amount) + { + throw new NotImplementedException(); + } + + + public static void Lerp(ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result) + { + throw new NotImplementedException(); + } + + + public static Quaternion Slerp(Quaternion quaternion1, Quaternion quaternion2, float amount) + { + throw new NotImplementedException(); + } + + + public static void Slerp(ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result) + { + throw new NotImplementedException(); + } + + + public static Quaternion Subtract(Quaternion quaternion1, Quaternion quaternion2) + { + throw new NotImplementedException(); + } + + + public static void Subtract(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result) + { + throw new NotImplementedException(); + } + + + public static Quaternion Multiply(Quaternion quaternion1, Quaternion quaternion2) + { + throw new NotImplementedException(); + } + + + public static Quaternion Multiply(Quaternion quaternion1, float scaleFactor) + { + throw new NotImplementedException(); + } + + + public static void Multiply(ref Quaternion quaternion1, float scaleFactor, out Quaternion result) + { + throw new NotImplementedException(); + } + + + public static void Multiply(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result) + { + throw new NotImplementedException(); + } + + + public static Quaternion Negate(Quaternion quaternion) + { + throw new NotImplementedException(); + } + + + public static void Negate(ref Quaternion quaternion, out Quaternion result) + { + throw new NotImplementedException(); + } + + + public void Normalize() + { + //--- + this = Normalize(this); + //--- + //throw new NotImplementedException(); + } + + + public static Quaternion Normalize(Quaternion quaternion) + { + //--- + return quaternion / quaternion.Length(); + //--- + //throw new NotImplementedException(); + } + + + public static void Normalize(ref Quaternion quaternion, out Quaternion result) + { + throw new NotImplementedException(); + } + + + public static Quaternion operator +(Quaternion quaternion1, Quaternion quaternion2) + { + throw new NotImplementedException(); + } + + + public static Quaternion operator /(Quaternion quaternion1, Quaternion quaternion2) + { + throw new NotImplementedException(); + } + public static Quaternion operator /(Quaternion quaternion, float factor) + { + quaternion.W /= factor; + quaternion.X /= factor; + quaternion.Y /= factor; + quaternion.Z /= factor; + return quaternion; + } + + public static bool operator ==(Quaternion quaternion1, Quaternion quaternion2) + { + throw new NotImplementedException(); + } + + + public static bool operator !=(Quaternion quaternion1, Quaternion quaternion2) + { + throw new NotImplementedException(); + } + + + public static Quaternion operator *(Quaternion quaternion1, Quaternion quaternion2) + { + //--- + //Grassmann product + Quaternion quaternionProduct = new Quaternion(); + + quaternionProduct.W = quaternion1.W * quaternion2.W - quaternion1.X * quaternion2.X - quaternion1.Y * quaternion2.Y - quaternion1.Z * quaternion2.Z; + quaternionProduct.X = quaternion1.W * quaternion2.X + quaternion1.X * quaternion2.W + quaternion1.Y * quaternion2.Z - quaternion1.Z * quaternion2.Y; + quaternionProduct.Y = quaternion1.W * quaternion2.Y - quaternion1.X * quaternion2.Z + quaternion1.Y * quaternion2.W + quaternion1.Z * quaternion2.X; + quaternionProduct.Z = quaternion1.W * quaternion2.Z + quaternion1.X * quaternion2.Y - quaternion1.Y * quaternion2.X + quaternion1.Z * quaternion2.W; + return quaternionProduct; + //--- + //throw new NotImplementedException(); + } + + + public static Quaternion operator *(Quaternion quaternion1, float scaleFactor) + { + throw new NotImplementedException(); + } + + + public static Quaternion operator -(Quaternion quaternion1, Quaternion quaternion2) + { + throw new NotImplementedException(); + } + + + public static Quaternion operator -(Quaternion quaternion) + { + throw new NotImplementedException(); + } + + + public override string ToString() + { + throw new NotImplementedException(); + } + + private static void Conjugate(ref Quaternion quaternion, out Quaternion result) + { + throw new NotImplementedException(); + } + } +} diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector3.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector3.cs index fd4bcd2..f246415 100644 --- a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector3.cs +++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector3.cs @@ -1,620 +1,620 @@ -#region License -/* -MIT License -Copyright © 2006 The Mono.Xna Team - -All rights reserved. - -Authors: - * Alan McGovern - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. -*/ -#endregion License - -using System; -using System.ComponentModel; -using System.Text; -using System.Runtime.InteropServices; - -namespace MonoXnaCompactMaths -{ - [Serializable] - [StructLayout(LayoutKind.Sequential)] - //[TypeConverter(typeof(Vector3Converter))] - public struct Vector3 : IEquatable - { - #region Private Fields - - private static Vector3 zero = new Vector3(0f, 0f, 0f); - private static Vector3 one = new Vector3(1f, 1f, 1f); - private static Vector3 unitX = new Vector3(1f, 0f, 0f); - private static Vector3 unitY = new Vector3(0f, 1f, 0f); - private static Vector3 unitZ = new Vector3(0f, 0f, 1f); - private static Vector3 up = new Vector3(0f, 1f, 0f); - private static Vector3 down = new Vector3(0f, -1f, 0f); - private static Vector3 right = new Vector3(1f, 0f, 0f); - private static Vector3 left = new Vector3(-1f, 0f, 0f); - private static Vector3 forward = new Vector3(0f, 0f, -1f); - private static Vector3 backward = new Vector3(0f, 0f, 1f); - - #endregion Private Fields - - - #region Public Fields - - public float X; - public float Y; - public float Z; - - #endregion Public Fields - - - #region Properties - - public static Vector3 Zero - { - get { return zero; } - } - - public static Vector3 One - { - get { return one; } - } - - public static Vector3 UnitX - { - get { return unitX; } - } - - public static Vector3 UnitY - { - get { return unitY; } - } - - public static Vector3 UnitZ - { - get { return unitZ; } - } - - public static Vector3 Up - { - get { return up; } - } - - public static Vector3 Down - { - get { return down; } - } - - public static Vector3 Right - { - get { return right; } - } - - public static Vector3 Left - { - get { return left; } - } - - public static Vector3 Forward - { - get { return forward; } - } - - public static Vector3 Backward - { - get { return backward; } - } - - #endregion Properties - - - #region Constructors - - public Vector3(float x, float y, float z) - { - this.X = x; - this.Y = y; - this.Z = z; - } - - - public Vector3(float value) - { - this.X = value; - this.Y = value; - this.Z = value; - } - - - /*public Vector3(Vector2 value, float z) - { - this.X = value.X; - this.Y = value.Y; - this.Z = z; - }*/ - - - #endregion Constructors - - - #region Public Methods - - public static Vector3 Add(Vector3 value1, Vector3 value2) - { - value1.X += value2.X; - value1.Y += value2.Y; - value1.Z += value2.Z; - return value1; - } - - public static void Add(ref Vector3 value1, ref Vector3 value2, out Vector3 result) - { - result.X = value1.X + value2.X; - result.Y = value1.Y + value2.Y; - result.Z = value1.Z + value2.Z; - } - - /*public static Vector3 Barycentric(Vector3 value1, Vector3 value2, Vector3 value3, float amount1, float amount2) - { - return new Vector3( - MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2), - MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2), - MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2)); - }*/ - - /*public static void Barycentric(ref Vector3 value1, ref Vector3 value2, ref Vector3 value3, float amount1, float amount2, out Vector3 result) - { - result = new Vector3( - MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2), - MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2), - MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2)); - }*/ - - /*public static Vector3 CatmullRom(Vector3 value1, Vector3 value2, Vector3 value3, Vector3 value4, float amount) - { - return new Vector3( - MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount), - MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount), - MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount)); - }*/ - - /*public static void CatmullRom(ref Vector3 value1, ref Vector3 value2, ref Vector3 value3, ref Vector3 value4, float amount, out Vector3 result) - { - result = new Vector3( - MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount), - MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount), - MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount)); - }*/ - - /*public static Vector3 Clamp(Vector3 value1, Vector3 min, Vector3 max) - { - return new Vector3( - MathHelper.Clamp(value1.X, min.X, max.X), - MathHelper.Clamp(value1.Y, min.Y, max.Y), - MathHelper.Clamp(value1.Z, min.Z, max.Z)); - }*/ - - /*public static void Clamp(ref Vector3 value1, ref Vector3 min, ref Vector3 max, out Vector3 result) - { - result = new Vector3( - MathHelper.Clamp(value1.X, min.X, max.X), - MathHelper.Clamp(value1.Y, min.Y, max.Y), - MathHelper.Clamp(value1.Z, min.Z, max.Z)); - }*/ - - public static Vector3 Cross(Vector3 vector1, Vector3 vector2) - { - Cross(ref vector1, ref vector2, out vector1); - return vector1; - } - - public static void Cross(ref Vector3 vector1, ref Vector3 vector2, out Vector3 result) - { - result = new Vector3(vector1.Y * vector2.Z - vector2.Y * vector1.Z, - -(vector1.X * vector2.Z - vector2.X * vector1.Z), - vector1.X * vector2.Y - vector2.X * vector1.Y); - } - - public static float Distance(Vector3 vector1, Vector3 vector2) - { - float result; - DistanceSquared(ref vector1, ref vector2, out result); - return (float)Math.Sqrt(result); - } - - public static void Distance(ref Vector3 value1, ref Vector3 value2, out float result) - { - DistanceSquared(ref value1, ref value2, out result); - result = (float)Math.Sqrt(result); - } - - public static float DistanceSquared(Vector3 value1, Vector3 value2) - { - float result; - DistanceSquared(ref value1, ref value2, out result); - return result; - } - - public static void DistanceSquared(ref Vector3 value1, ref Vector3 value2, out float result) - { - result = (value1.X - value2.X) * (value1.X - value2.X) + - (value1.Y - value2.Y) * (value1.Y - value2.Y) + - (value1.Z - value2.Z) * (value1.Z - value2.Z); - } - - public static Vector3 Divide(Vector3 value1, Vector3 value2) - { - value1.X /= value2.X; - value1.Y /= value2.Y; - value1.Z /= value2.Z; - return value1; - } - - public static Vector3 Divide(Vector3 value1, float value2) - { - float factor = 1 / value2; - value1.X *= factor; - value1.Y *= factor; - value1.Z *= factor; - return value1; - } - - public static void Divide(ref Vector3 value1, float divisor, out Vector3 result) - { - float factor = 1 / divisor; - result.X = value1.X * factor; - result.Y = value1.Y * factor; - result.Z = value1.Z * factor; - } - - public static void Divide(ref Vector3 value1, ref Vector3 value2, out Vector3 result) - { - result.X = value1.X / value2.X; - result.Y = value1.Y / value2.Y; - result.Z = value1.Z / value2.Z; - } - - public static float Dot(Vector3 vector1, Vector3 vector2) - { - return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z; - } - - public static void Dot(ref Vector3 vector1, ref Vector3 vector2, out float result) - { - result = vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z; - } - - public override bool Equals(object obj) - { - return (obj is Vector3) ? this == (Vector3)obj : false; - } - - public bool Equals(Vector3 other) - { - return this == other; - } - - public override int GetHashCode() - { - return (int)(this.X + this.Y + this.Z); - } - - /*public static Vector3 Hermite(Vector3 value1, Vector3 tangent1, Vector3 value2, Vector3 tangent2, float amount) - { - Vector3 result = new Vector3(); - Hermite(ref value1, ref tangent1, ref value2, ref tangent2, amount, out result); - return result; - }*/ - - /*public static void Hermite(ref Vector3 value1, ref Vector3 tangent1, ref Vector3 value2, ref Vector3 tangent2, float amount, out Vector3 result) - { - result.X = MathHelper.Hermite(value1.X, tangent1.X, value2.X, tangent2.X, amount); - result.Y = MathHelper.Hermite(value1.Y, tangent1.Y, value2.Y, tangent2.Y, amount); - result.Z = MathHelper.Hermite(value1.Z, tangent1.Z, value2.Z, tangent2.Z, amount); - }*/ - - public float Length() - { - float result; - DistanceSquared(ref this, ref zero, out result); - return (float)Math.Sqrt(result); - } - - public float LengthSquared() - { - float result; - DistanceSquared(ref this, ref zero, out result); - return result; - } - - /*public static Vector3 Lerp(Vector3 value1, Vector3 value2, float amount) - { - return new Vector3( - MathHelper.Lerp(value1.X, value2.X, amount), - MathHelper.Lerp(value1.Y, value2.Y, amount), - MathHelper.Lerp(value1.Z, value2.Z, amount)); - }*/ - - /*public static void Lerp(ref Vector3 value1, ref Vector3 value2, float amount, out Vector3 result) - { - result = new Vector3( - MathHelper.Lerp(value1.X, value2.X, amount), - MathHelper.Lerp(value1.Y, value2.Y, amount), - MathHelper.Lerp(value1.Z, value2.Z, amount)); - }*/ - - /*public static Vector3 Max(Vector3 value1, Vector3 value2) - { - return new Vector3( - MathHelper.Max(value1.X, value2.X), - MathHelper.Max(value1.Y, value2.Y), - MathHelper.Max(value1.Z, value2.Z)); - }*/ - - /*public static void Max(ref Vector3 value1, ref Vector3 value2, out Vector3 result) - { - result = new Vector3( - MathHelper.Max(value1.X, value2.X), - MathHelper.Max(value1.Y, value2.Y), - MathHelper.Max(value1.Z, value2.Z)); - }*/ - - /*public static Vector3 Min(Vector3 value1, Vector3 value2) - { - return new Vector3( - MathHelper.Min(value1.X, value2.X), - MathHelper.Min(value1.Y, value2.Y), - MathHelper.Min(value1.Z, value2.Z)); - }*/ - - /*public static void Min(ref Vector3 value1, ref Vector3 value2, out Vector3 result) - { - result = new Vector3( - MathHelper.Min(value1.X, value2.X), - MathHelper.Min(value1.Y, value2.Y), - MathHelper.Min(value1.Z, value2.Z)); - }*/ - - public static Vector3 Multiply(Vector3 value1, Vector3 value2) - { - value1.X *= value2.X; - value1.Y *= value2.Y; - value1.Z *= value2.Z; - return value1; - } - - public static Vector3 Multiply(Vector3 value1, float scaleFactor) - { - value1.X *= scaleFactor; - value1.Y *= scaleFactor; - value1.Z *= scaleFactor; - return value1; - } - - public static void Multiply(ref Vector3 value1, float scaleFactor, out Vector3 result) - { - result.X = value1.X * scaleFactor; - result.Y = value1.Y * scaleFactor; - result.Z = value1.Z * scaleFactor; - } - - public static void Multiply(ref Vector3 value1, ref Vector3 value2, out Vector3 result) - { - result.X = value1.X * value2.X; - result.Y = value1.Y * value2.Y; - result.Z = value1.Z * value2.Z; - } - - public static Vector3 Negate(Vector3 value) - { - value = new Vector3(-value.X, -value.Y, -value.Z); - return value; - } - - public static void Negate(ref Vector3 value, out Vector3 result) - { - result = new Vector3(-value.X, -value.Y, -value.Z); - } - - public void Normalize() - { - Normalize(ref this, out this); - } - - public static Vector3 Normalize(Vector3 vector) - { - Normalize(ref vector, out vector); - return vector; - } - - public static void Normalize(ref Vector3 value, out Vector3 result) - { - float factor; - Distance(ref value, ref zero, out factor); - factor = 1f / factor; - result.X = value.X * factor; - result.Y = value.Y * factor; - result.Z = value.Z * factor; - } - - public static Vector3 Reflect(Vector3 vector, Vector3 normal) - { - throw new NotImplementedException(); - } - - public static void Reflect(ref Vector3 vector, ref Vector3 normal, out Vector3 result) - { - throw new NotImplementedException(); - } - - /*public static Vector3 SmoothStep(Vector3 value1, Vector3 value2, float amount) - { - return new Vector3( - MathHelper.SmoothStep(value1.X, value2.X, amount), - MathHelper.SmoothStep(value1.Y, value2.Y, amount), - MathHelper.SmoothStep(value1.Z, value2.Z, amount)); - }*/ - - /*public static void SmoothStep(ref Vector3 value1, ref Vector3 value2, float amount, out Vector3 result) - { - result = new Vector3( - MathHelper.SmoothStep(value1.X, value2.X, amount), - MathHelper.SmoothStep(value1.Y, value2.Y, amount), - MathHelper.SmoothStep(value1.Z, value2.Z, amount)); - }*/ - - public static Vector3 Subtract(Vector3 value1, Vector3 value2) - { - value1.X -= value2.X; - value1.Y -= value2.Y; - value1.Z -= value2.Z; - return value1; - } - - public static void Subtract(ref Vector3 value1, ref Vector3 value2, out Vector3 result) - { - result.X = value1.X - value2.X; - result.Y = value1.Y - value2.Y; - result.Z = value1.Z - value2.Z; - } - - public override string ToString() - { - StringBuilder sb = new StringBuilder(32); - sb.Append("{X:"); - sb.Append(this.X); - sb.Append(" Y:"); - sb.Append(this.Y); - sb.Append(" Z:"); - sb.Append(this.Z); - sb.Append("}"); - return sb.ToString(); - } - - public static Vector3 Transform(Vector3 position, Matrix matrix) - { - Transform(ref position, ref matrix, out position); - return position; - } - - public static void Transform(ref Vector3 position, ref Matrix matrix, out Vector3 result) - { - result = new Vector3((position.X * matrix.M11) + (position.Y * matrix.M21) + (position.Z * matrix.M31) + matrix.M41, - (position.X * matrix.M12) + (position.Y * matrix.M22) + (position.Z * matrix.M32) + matrix.M42, - (position.X * matrix.M13) + (position.Y * matrix.M23) + (position.Z * matrix.M33) + matrix.M43); - } - - public static Vector3 TransformNormal(Vector3 normal, Matrix matrix) - { - TransformNormal(ref normal, ref matrix, out normal); - return normal; - } - - public static void TransformNormal(ref Vector3 normal, ref Matrix matrix, out Vector3 result) - { - result = new Vector3((normal.X * matrix.M11) + (normal.Y * matrix.M21) + (normal.Z * matrix.M31), - (normal.X * matrix.M12) + (normal.Y * matrix.M22) + (normal.Z * matrix.M32), - (normal.X * matrix.M13) + (normal.Y * matrix.M23) + (normal.Z * matrix.M33)); - } - - #endregion Public methods - - - #region Operators - - public static bool operator ==(Vector3 value1, Vector3 value2) - { - return value1.X == value2.X - && value1.Y == value2.Y - && value1.Z == value2.Z; - } - - public static bool operator !=(Vector3 value1, Vector3 value2) - { - return !(value1 == value2); - } - - public static Vector3 operator +(Vector3 value1, Vector3 value2) - { - value1.X += value2.X; - value1.Y += value2.Y; - value1.Z += value2.Z; - return value1; - } - - public static Vector3 operator -(Vector3 value) - { - value = new Vector3(-value.X, -value.Y, -value.Z); - return value; - } - - public static Vector3 operator -(Vector3 value1, Vector3 value2) - { - value1.X -= value2.X; - value1.Y -= value2.Y; - value1.Z -= value2.Z; - return value1; - } - - public static Vector3 operator *(Vector3 value1, Vector3 value2) - { - value1.X *= value2.X; - value1.Y *= value2.Y; - value1.Z *= value2.Z; - return value1; - } - - public static Vector3 operator *(Vector3 value, float scaleFactor) - { - value.X *= scaleFactor; - value.Y *= scaleFactor; - value.Z *= scaleFactor; - return value; - } - - public static Vector3 operator *(float scaleFactor, Vector3 value) - { - value.X *= scaleFactor; - value.Y *= scaleFactor; - value.Z *= scaleFactor; - return value; - } - - public static Vector3 operator /(Vector3 value1, Vector3 value2) - { - value1.X /= value2.X; - value1.Y /= value2.Y; - value1.Z /= value2.Z; - return value1; - } - - public static Vector3 operator /(Vector3 value, float divider) - { - float factor = 1 / divider; - value.X *= factor; - value.Y *= factor; - value.Z *= factor; - return value; - } - - #endregion - } -} +#region License +/* +MIT License +Copyright © 2006 The Mono.Xna Team + +All rights reserved. + +Authors: + * Alan McGovern + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ +#endregion License + +using System; +using System.ComponentModel; +using System.Text; +using System.Runtime.InteropServices; + +namespace MonoXnaCompactMaths +{ + [Serializable] + [StructLayout(LayoutKind.Sequential)] + //[TypeConverter(typeof(Vector3Converter))] + public struct Vector3 : IEquatable + { + #region Private Fields + + private static Vector3 zero = new Vector3(0f, 0f, 0f); + private static Vector3 one = new Vector3(1f, 1f, 1f); + private static Vector3 unitX = new Vector3(1f, 0f, 0f); + private static Vector3 unitY = new Vector3(0f, 1f, 0f); + private static Vector3 unitZ = new Vector3(0f, 0f, 1f); + private static Vector3 up = new Vector3(0f, 1f, 0f); + private static Vector3 down = new Vector3(0f, -1f, 0f); + private static Vector3 right = new Vector3(1f, 0f, 0f); + private static Vector3 left = new Vector3(-1f, 0f, 0f); + private static Vector3 forward = new Vector3(0f, 0f, -1f); + private static Vector3 backward = new Vector3(0f, 0f, 1f); + + #endregion Private Fields + + + #region Public Fields + + public float X; + public float Y; + public float Z; + + #endregion Public Fields + + + #region Properties + + public static Vector3 Zero + { + get { return zero; } + } + + public static Vector3 One + { + get { return one; } + } + + public static Vector3 UnitX + { + get { return unitX; } + } + + public static Vector3 UnitY + { + get { return unitY; } + } + + public static Vector3 UnitZ + { + get { return unitZ; } + } + + public static Vector3 Up + { + get { return up; } + } + + public static Vector3 Down + { + get { return down; } + } + + public static Vector3 Right + { + get { return right; } + } + + public static Vector3 Left + { + get { return left; } + } + + public static Vector3 Forward + { + get { return forward; } + } + + public static Vector3 Backward + { + get { return backward; } + } + + #endregion Properties + + + #region Constructors + + public Vector3(float x, float y, float z) + { + this.X = x; + this.Y = y; + this.Z = z; + } + + + public Vector3(float value) + { + this.X = value; + this.Y = value; + this.Z = value; + } + + + /*public Vector3(Vector2 value, float z) + { + this.X = value.X; + this.Y = value.Y; + this.Z = z; + }*/ + + + #endregion Constructors + + + #region Public Methods + + public static Vector3 Add(Vector3 value1, Vector3 value2) + { + value1.X += value2.X; + value1.Y += value2.Y; + value1.Z += value2.Z; + return value1; + } + + public static void Add(ref Vector3 value1, ref Vector3 value2, out Vector3 result) + { + result.X = value1.X + value2.X; + result.Y = value1.Y + value2.Y; + result.Z = value1.Z + value2.Z; + } + + /*public static Vector3 Barycentric(Vector3 value1, Vector3 value2, Vector3 value3, float amount1, float amount2) + { + return new Vector3( + MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2), + MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2), + MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2)); + }*/ + + /*public static void Barycentric(ref Vector3 value1, ref Vector3 value2, ref Vector3 value3, float amount1, float amount2, out Vector3 result) + { + result = new Vector3( + MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2), + MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2), + MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2)); + }*/ + + /*public static Vector3 CatmullRom(Vector3 value1, Vector3 value2, Vector3 value3, Vector3 value4, float amount) + { + return new Vector3( + MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount), + MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount), + MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount)); + }*/ + + /*public static void CatmullRom(ref Vector3 value1, ref Vector3 value2, ref Vector3 value3, ref Vector3 value4, float amount, out Vector3 result) + { + result = new Vector3( + MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount), + MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount), + MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount)); + }*/ + + /*public static Vector3 Clamp(Vector3 value1, Vector3 min, Vector3 max) + { + return new Vector3( + MathHelper.Clamp(value1.X, min.X, max.X), + MathHelper.Clamp(value1.Y, min.Y, max.Y), + MathHelper.Clamp(value1.Z, min.Z, max.Z)); + }*/ + + /*public static void Clamp(ref Vector3 value1, ref Vector3 min, ref Vector3 max, out Vector3 result) + { + result = new Vector3( + MathHelper.Clamp(value1.X, min.X, max.X), + MathHelper.Clamp(value1.Y, min.Y, max.Y), + MathHelper.Clamp(value1.Z, min.Z, max.Z)); + }*/ + + public static Vector3 Cross(Vector3 vector1, Vector3 vector2) + { + Cross(ref vector1, ref vector2, out vector1); + return vector1; + } + + public static void Cross(ref Vector3 vector1, ref Vector3 vector2, out Vector3 result) + { + result = new Vector3(vector1.Y * vector2.Z - vector2.Y * vector1.Z, + -(vector1.X * vector2.Z - vector2.X * vector1.Z), + vector1.X * vector2.Y - vector2.X * vector1.Y); + } + + public static float Distance(Vector3 vector1, Vector3 vector2) + { + float result; + DistanceSquared(ref vector1, ref vector2, out result); + return (float)Math.Sqrt(result); + } + + public static void Distance(ref Vector3 value1, ref Vector3 value2, out float result) + { + DistanceSquared(ref value1, ref value2, out result); + result = (float)Math.Sqrt(result); + } + + public static float DistanceSquared(Vector3 value1, Vector3 value2) + { + float result; + DistanceSquared(ref value1, ref value2, out result); + return result; + } + + public static void DistanceSquared(ref Vector3 value1, ref Vector3 value2, out float result) + { + result = (value1.X - value2.X) * (value1.X - value2.X) + + (value1.Y - value2.Y) * (value1.Y - value2.Y) + + (value1.Z - value2.Z) * (value1.Z - value2.Z); + } + + public static Vector3 Divide(Vector3 value1, Vector3 value2) + { + value1.X /= value2.X; + value1.Y /= value2.Y; + value1.Z /= value2.Z; + return value1; + } + + public static Vector3 Divide(Vector3 value1, float value2) + { + float factor = 1 / value2; + value1.X *= factor; + value1.Y *= factor; + value1.Z *= factor; + return value1; + } + + public static void Divide(ref Vector3 value1, float divisor, out Vector3 result) + { + float factor = 1 / divisor; + result.X = value1.X * factor; + result.Y = value1.Y * factor; + result.Z = value1.Z * factor; + } + + public static void Divide(ref Vector3 value1, ref Vector3 value2, out Vector3 result) + { + result.X = value1.X / value2.X; + result.Y = value1.Y / value2.Y; + result.Z = value1.Z / value2.Z; + } + + public static float Dot(Vector3 vector1, Vector3 vector2) + { + return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z; + } + + public static void Dot(ref Vector3 vector1, ref Vector3 vector2, out float result) + { + result = vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z; + } + + public override bool Equals(object obj) + { + return (obj is Vector3) ? this == (Vector3)obj : false; + } + + public bool Equals(Vector3 other) + { + return this == other; + } + + public override int GetHashCode() + { + return (int)(this.X + this.Y + this.Z); + } + + /*public static Vector3 Hermite(Vector3 value1, Vector3 tangent1, Vector3 value2, Vector3 tangent2, float amount) + { + Vector3 result = new Vector3(); + Hermite(ref value1, ref tangent1, ref value2, ref tangent2, amount, out result); + return result; + }*/ + + /*public static void Hermite(ref Vector3 value1, ref Vector3 tangent1, ref Vector3 value2, ref Vector3 tangent2, float amount, out Vector3 result) + { + result.X = MathHelper.Hermite(value1.X, tangent1.X, value2.X, tangent2.X, amount); + result.Y = MathHelper.Hermite(value1.Y, tangent1.Y, value2.Y, tangent2.Y, amount); + result.Z = MathHelper.Hermite(value1.Z, tangent1.Z, value2.Z, tangent2.Z, amount); + }*/ + + public float Length() + { + float result; + DistanceSquared(ref this, ref zero, out result); + return (float)Math.Sqrt(result); + } + + public float LengthSquared() + { + float result; + DistanceSquared(ref this, ref zero, out result); + return result; + } + + /*public static Vector3 Lerp(Vector3 value1, Vector3 value2, float amount) + { + return new Vector3( + MathHelper.Lerp(value1.X, value2.X, amount), + MathHelper.Lerp(value1.Y, value2.Y, amount), + MathHelper.Lerp(value1.Z, value2.Z, amount)); + }*/ + + /*public static void Lerp(ref Vector3 value1, ref Vector3 value2, float amount, out Vector3 result) + { + result = new Vector3( + MathHelper.Lerp(value1.X, value2.X, amount), + MathHelper.Lerp(value1.Y, value2.Y, amount), + MathHelper.Lerp(value1.Z, value2.Z, amount)); + }*/ + + /*public static Vector3 Max(Vector3 value1, Vector3 value2) + { + return new Vector3( + MathHelper.Max(value1.X, value2.X), + MathHelper.Max(value1.Y, value2.Y), + MathHelper.Max(value1.Z, value2.Z)); + }*/ + + /*public static void Max(ref Vector3 value1, ref Vector3 value2, out Vector3 result) + { + result = new Vector3( + MathHelper.Max(value1.X, value2.X), + MathHelper.Max(value1.Y, value2.Y), + MathHelper.Max(value1.Z, value2.Z)); + }*/ + + /*public static Vector3 Min(Vector3 value1, Vector3 value2) + { + return new Vector3( + MathHelper.Min(value1.X, value2.X), + MathHelper.Min(value1.Y, value2.Y), + MathHelper.Min(value1.Z, value2.Z)); + }*/ + + /*public static void Min(ref Vector3 value1, ref Vector3 value2, out Vector3 result) + { + result = new Vector3( + MathHelper.Min(value1.X, value2.X), + MathHelper.Min(value1.Y, value2.Y), + MathHelper.Min(value1.Z, value2.Z)); + }*/ + + public static Vector3 Multiply(Vector3 value1, Vector3 value2) + { + value1.X *= value2.X; + value1.Y *= value2.Y; + value1.Z *= value2.Z; + return value1; + } + + public static Vector3 Multiply(Vector3 value1, float scaleFactor) + { + value1.X *= scaleFactor; + value1.Y *= scaleFactor; + value1.Z *= scaleFactor; + return value1; + } + + public static void Multiply(ref Vector3 value1, float scaleFactor, out Vector3 result) + { + result.X = value1.X * scaleFactor; + result.Y = value1.Y * scaleFactor; + result.Z = value1.Z * scaleFactor; + } + + public static void Multiply(ref Vector3 value1, ref Vector3 value2, out Vector3 result) + { + result.X = value1.X * value2.X; + result.Y = value1.Y * value2.Y; + result.Z = value1.Z * value2.Z; + } + + public static Vector3 Negate(Vector3 value) + { + value = new Vector3(-value.X, -value.Y, -value.Z); + return value; + } + + public static void Negate(ref Vector3 value, out Vector3 result) + { + result = new Vector3(-value.X, -value.Y, -value.Z); + } + + public void Normalize() + { + Normalize(ref this, out this); + } + + public static Vector3 Normalize(Vector3 vector) + { + Normalize(ref vector, out vector); + return vector; + } + + public static void Normalize(ref Vector3 value, out Vector3 result) + { + float factor; + Distance(ref value, ref zero, out factor); + factor = 1f / factor; + result.X = value.X * factor; + result.Y = value.Y * factor; + result.Z = value.Z * factor; + } + + public static Vector3 Reflect(Vector3 vector, Vector3 normal) + { + throw new NotImplementedException(); + } + + public static void Reflect(ref Vector3 vector, ref Vector3 normal, out Vector3 result) + { + throw new NotImplementedException(); + } + + /*public static Vector3 SmoothStep(Vector3 value1, Vector3 value2, float amount) + { + return new Vector3( + MathHelper.SmoothStep(value1.X, value2.X, amount), + MathHelper.SmoothStep(value1.Y, value2.Y, amount), + MathHelper.SmoothStep(value1.Z, value2.Z, amount)); + }*/ + + /*public static void SmoothStep(ref Vector3 value1, ref Vector3 value2, float amount, out Vector3 result) + { + result = new Vector3( + MathHelper.SmoothStep(value1.X, value2.X, amount), + MathHelper.SmoothStep(value1.Y, value2.Y, amount), + MathHelper.SmoothStep(value1.Z, value2.Z, amount)); + }*/ + + public static Vector3 Subtract(Vector3 value1, Vector3 value2) + { + value1.X -= value2.X; + value1.Y -= value2.Y; + value1.Z -= value2.Z; + return value1; + } + + public static void Subtract(ref Vector3 value1, ref Vector3 value2, out Vector3 result) + { + result.X = value1.X - value2.X; + result.Y = value1.Y - value2.Y; + result.Z = value1.Z - value2.Z; + } + + public override string ToString() + { + StringBuilder sb = new StringBuilder(32); + sb.Append("{X:"); + sb.Append(this.X); + sb.Append(" Y:"); + sb.Append(this.Y); + sb.Append(" Z:"); + sb.Append(this.Z); + sb.Append("}"); + return sb.ToString(); + } + + public static Vector3 Transform(Vector3 position, Matrix matrix) + { + Transform(ref position, ref matrix, out position); + return position; + } + + public static void Transform(ref Vector3 position, ref Matrix matrix, out Vector3 result) + { + result = new Vector3((position.X * matrix.M11) + (position.Y * matrix.M21) + (position.Z * matrix.M31) + matrix.M41, + (position.X * matrix.M12) + (position.Y * matrix.M22) + (position.Z * matrix.M32) + matrix.M42, + (position.X * matrix.M13) + (position.Y * matrix.M23) + (position.Z * matrix.M33) + matrix.M43); + } + + public static Vector3 TransformNormal(Vector3 normal, Matrix matrix) + { + TransformNormal(ref normal, ref matrix, out normal); + return normal; + } + + public static void TransformNormal(ref Vector3 normal, ref Matrix matrix, out Vector3 result) + { + result = new Vector3((normal.X * matrix.M11) + (normal.Y * matrix.M21) + (normal.Z * matrix.M31), + (normal.X * matrix.M12) + (normal.Y * matrix.M22) + (normal.Z * matrix.M32), + (normal.X * matrix.M13) + (normal.Y * matrix.M23) + (normal.Z * matrix.M33)); + } + + #endregion Public methods + + + #region Operators + + public static bool operator ==(Vector3 value1, Vector3 value2) + { + return value1.X == value2.X + && value1.Y == value2.Y + && value1.Z == value2.Z; + } + + public static bool operator !=(Vector3 value1, Vector3 value2) + { + return !(value1 == value2); + } + + public static Vector3 operator +(Vector3 value1, Vector3 value2) + { + value1.X += value2.X; + value1.Y += value2.Y; + value1.Z += value2.Z; + return value1; + } + + public static Vector3 operator -(Vector3 value) + { + value = new Vector3(-value.X, -value.Y, -value.Z); + return value; + } + + public static Vector3 operator -(Vector3 value1, Vector3 value2) + { + value1.X -= value2.X; + value1.Y -= value2.Y; + value1.Z -= value2.Z; + return value1; + } + + public static Vector3 operator *(Vector3 value1, Vector3 value2) + { + value1.X *= value2.X; + value1.Y *= value2.Y; + value1.Z *= value2.Z; + return value1; + } + + public static Vector3 operator *(Vector3 value, float scaleFactor) + { + value.X *= scaleFactor; + value.Y *= scaleFactor; + value.Z *= scaleFactor; + return value; + } + + public static Vector3 operator *(float scaleFactor, Vector3 value) + { + value.X *= scaleFactor; + value.Y *= scaleFactor; + value.Z *= scaleFactor; + return value; + } + + public static Vector3 operator /(Vector3 value1, Vector3 value2) + { + value1.X /= value2.X; + value1.Y /= value2.Y; + value1.Z /= value2.Z; + return value1; + } + + public static Vector3 operator /(Vector3 value, float divider) + { + float factor = 1 / divider; + value.X *= factor; + value.Y *= factor; + value.Z *= factor; + return value; + } + + #endregion + } +} diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector4.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector4.cs index c75b7c8..abb30a8 100644 --- a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector4.cs +++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector4.cs @@ -1,630 +1,630 @@ -#region License -/* -MIT License -Copyright © 2006 The Mono.Xna Team - -All rights reserved. - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. -*/ -#endregion License - -using System; -using System.ComponentModel; -using System.Text; -using System.Runtime.InteropServices; - -namespace MonoXnaCompactMaths -{ - [Serializable] - [StructLayout(LayoutKind.Sequential)] - //[TypeConverter(typeof(Vector4Converter))] - public struct Vector4 : IEquatable - { - #region Private Fields - - private static Vector4 zeroVector = new Vector4(); - private static Vector4 unitVector = new Vector4(1f, 1f, 1f, 1f); - private static Vector4 unitXVector = new Vector4(1f, 0f, 0f, 0f); - private static Vector4 unitYVector = new Vector4(0f, 1f, 0f, 0f); - private static Vector4 unitZVector = new Vector4(0f, 0f, 1f, 0f); - private static Vector4 unitWVector = new Vector4(0f, 0f, 0f, 1f); - - #endregion Private Fields - - - #region Public Fields - - public float X; - public float Y; - public float Z; - public float W; - - #endregion Public Fields - - - #region Properties - - public static Vector4 Zero - { - get { return zeroVector; } - } - - public static Vector4 One - { - get { return unitVector; } - } - - public static Vector4 UnitX - { - get { return unitXVector; } - } - - public static Vector4 UnitY - { - get { return unitYVector; } - } - - public static Vector4 UnitZ - { - get { return unitZVector; } - } - - public static Vector4 UnitW - { - get { return unitWVector; } - } - - #endregion Properties - - - #region Constructors - - public Vector4(float x, float y, float z, float w) - { - this.X = x; - this.Y = y; - this.Z = z; - this.W = w; - } - - /*public Vector4(Vector2 value, float z, float w) - { - this.X = value.X; - this.Y = value.Y; - this.Z = z; - this.W = w; - }*/ - - public Vector4(Vector3 value, float w) - { - this.X = value.X; - this.Y = value.Y; - this.Z = value.Z; - this.W = w; - } - - public Vector4(float value) - { - this.X = value; - this.Y = value; - this.Z = value; - this.W = value; - } - - #endregion - - - #region Public Methods - - public static Vector4 Add(Vector4 value1, Vector4 value2) - { - value1.W += value2.W; - value1.X += value2.X; - value1.Y += value2.Y; - value1.Z += value2.Z; - return value1; - } - - public static void Add(ref Vector4 value1, ref Vector4 value2, out Vector4 result) - { - result.W = value1.W + value2.W; - result.X = value1.X + value2.X; - result.Y = value1.Y + value2.Y; - result.Z = value1.Z + value2.Z; - } - - /*public static Vector4 Barycentric(Vector4 value1, Vector4 value2, Vector4 value3, float amount1, float amount2) - { - return new Vector4( - MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2), - MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2), - MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2), - MathHelper.Barycentric(value1.W, value2.W, value3.W, amount1, amount2)); - }*/ - - /*public static void Barycentric(ref Vector4 value1, ref Vector4 value2, ref Vector4 value3, float amount1, float amount2, out Vector4 result) - { - result = new Vector4( - MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2), - MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2), - MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2), - MathHelper.Barycentric(value1.W, value2.W, value3.W, amount1, amount2)); - }*/ - - /*public static Vector4 CatmullRom(Vector4 value1, Vector4 value2, Vector4 value3, Vector4 value4, float amount) - { - return new Vector4( - MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount), - MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount), - MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount), - MathHelper.CatmullRom(value1.W, value2.W, value3.W, value4.W, amount)); - }*/ - - /*public static void CatmullRom(ref Vector4 value1, ref Vector4 value2, ref Vector4 value3, ref Vector4 value4, float amount, out Vector4 result) - { - result = new Vector4( - MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount), - MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount), - MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount), - MathHelper.CatmullRom(value1.W, value2.W, value3.W, value4.W, amount)); - }*/ - - /*public static Vector4 Clamp(Vector4 value1, Vector4 min, Vector4 max) - { - return new Vector4( - MathHelper.Clamp(value1.X, min.X, max.X), - MathHelper.Clamp(value1.Y, min.Y, max.Y), - MathHelper.Clamp(value1.Z, min.Z, max.Z), - MathHelper.Clamp(value1.W, min.W, max.W)); - }*/ - - /*public static void Clamp(ref Vector4 value1, ref Vector4 min, ref Vector4 max, out Vector4 result) - { - result = new Vector4( - MathHelper.Clamp(value1.X, min.X, max.X), - MathHelper.Clamp(value1.Y, min.Y, max.Y), - MathHelper.Clamp(value1.Z, min.Z, max.Z), - MathHelper.Clamp(value1.W, min.W, max.W)); - }*/ - - public static float Distance(Vector4 value1, Vector4 value2) - { - return (float)Math.Sqrt(DistanceSquared(value1, value2)); - } - - public static void Distance(ref Vector4 value1, ref Vector4 value2, out float result) - { - result = (float)Math.Sqrt(DistanceSquared(value1, value2)); - } - - public static float DistanceSquared(Vector4 value1, Vector4 value2) - { - float result; - DistanceSquared(ref value1, ref value2, out result); - return result; - } - - public static void DistanceSquared(ref Vector4 value1, ref Vector4 value2, out float result) - { - result = (value1.W - value2.W) * (value1.W - value2.W) + - (value1.X - value2.X) * (value1.X - value2.X) + - (value1.Y - value2.Y) * (value1.Y - value2.Y) + - (value1.Z - value2.Z) * (value1.Z - value2.Z); - } - - public static Vector4 Divide(Vector4 value1, Vector4 value2) - { - value1.W /= value2.W; - value1.X /= value2.X; - value1.Y /= value2.Y; - value1.Z /= value2.Z; - return value1; - } - - public static Vector4 Divide(Vector4 value1, float divider) - { - float factor = 1f / divider; - value1.W *= factor; - value1.X *= factor; - value1.Y *= factor; - value1.Z *= factor; - return value1; - } - - public static void Divide(ref Vector4 value1, float divider, out Vector4 result) - { - float factor = 1f / divider; - result.W = value1.W * factor; - result.X = value1.X * factor; - result.Y = value1.Y * factor; - result.Z = value1.Z * factor; - } - - public static void Divide(ref Vector4 value1, ref Vector4 value2, out Vector4 result) - { - result.W = value1.W / value2.W; - result.X = value1.X / value2.X; - result.Y = value1.Y / value2.Y; - result.Z = value1.Z / value2.Z; - } - - public static float Dot(Vector4 vector1, Vector4 vector2) - { - return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z + vector1.W * vector2.W; - } - - public static void Dot(ref Vector4 vector1, ref Vector4 vector2, out float result) - { - result = vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z + vector1.W * vector2.W; - } - - public override bool Equals(object obj) - { - return (obj is Vector4) ? this == (Vector4)obj : false; - } - - public bool Equals(Vector4 other) - { - return this.W == other.W - && this.X == other.X - && this.Y == other.Y - && this.Z == other.Z; - } - - public override int GetHashCode() - { - return (int)(this.W + this.X + this.Y + this.Y); - } - - /*public static Vector4 Hermite(Vector4 value1, Vector4 tangent1, Vector4 value2, Vector4 tangent2, float amount) - { - Vector4 result = new Vector4(); - Hermite(ref value1, ref tangent1, ref value2, ref tangent2, amount, out result); - return result; - }*/ - - /*public static void Hermite(ref Vector4 value1, ref Vector4 tangent1, ref Vector4 value2, ref Vector4 tangent2, float amount, out Vector4 result) - { - result.W = MathHelper.Hermite(value1.W, tangent1.W, value2.W, tangent2.W, amount); - result.X = MathHelper.Hermite(value1.X, tangent1.X, value2.X, tangent2.X, amount); - result.Y = MathHelper.Hermite(value1.Y, tangent1.Y, value2.Y, tangent2.Y, amount); - result.Z = MathHelper.Hermite(value1.Z, tangent1.Z, value2.Z, tangent2.Z, amount); - }*/ - - public float Length() - { - float result; - DistanceSquared(ref this, ref zeroVector, out result); - return (float)Math.Sqrt(result); - } - - public float LengthSquared() - { - float result; - DistanceSquared(ref this, ref zeroVector, out result); - return result; - } - - /*public static Vector4 Lerp(Vector4 value1, Vector4 value2, float amount) - { - return new Vector4( - MathHelper.Lerp(value1.X, value2.X, amount), - MathHelper.Lerp(value1.Y, value2.Y, amount), - MathHelper.Lerp(value1.Z, value2.Z, amount), - MathHelper.Lerp(value1.W, value2.W, amount)); - }*/ - - /*public static void Lerp(ref Vector4 value1, ref Vector4 value2, float amount, out Vector4 result) - { - result = new Vector4( - MathHelper.Lerp(value1.X, value2.X, amount), - MathHelper.Lerp(value1.Y, value2.Y, amount), - MathHelper.Lerp(value1.Z, value2.Z, amount), - MathHelper.Lerp(value1.W, value2.W, amount)); - }*/ - - /*public static Vector4 Max(Vector4 value1, Vector4 value2) - { - return new Vector4( - MathHelper.Max(value1.X, value2.X), - MathHelper.Max(value1.Y, value2.Y), - MathHelper.Max(value1.Z, value2.Z), - MathHelper.Max(value1.W, value2.W)); - }*/ - - /*public static void Max(ref Vector4 value1, ref Vector4 value2, out Vector4 result) - { - result = new Vector4( - MathHelper.Max(value1.X, value2.X), - MathHelper.Max(value1.Y, value2.Y), - MathHelper.Max(value1.Z, value2.Z), - MathHelper.Max(value1.W, value2.W)); - }*/ - - /*public static Vector4 Min(Vector4 value1, Vector4 value2) - { - return new Vector4( - MathHelper.Min(value1.X, value2.X), - MathHelper.Min(value1.Y, value2.Y), - MathHelper.Min(value1.Z, value2.Z), - MathHelper.Min(value1.W, value2.W)); - }*/ - - /*public static void Min(ref Vector4 value1, ref Vector4 value2, out Vector4 result) - { - result = new Vector4( - MathHelper.Min(value1.X, value2.X), - MathHelper.Min(value1.Y, value2.Y), - MathHelper.Min(value1.Z, value2.Z), - MathHelper.Min(value1.W, value2.W)); - }*/ - - public static Vector4 Multiply(Vector4 value1, Vector4 value2) - { - value1.W *= value2.W; - value1.X *= value2.X; - value1.Y *= value2.Y; - value1.Z *= value2.Z; - return value1; - } - - public static Vector4 Multiply(Vector4 value1, float scaleFactor) - { - value1.W *= scaleFactor; - value1.X *= scaleFactor; - value1.Y *= scaleFactor; - value1.Z *= scaleFactor; - return value1; - } - - public static void Multiply(ref Vector4 value1, float scaleFactor, out Vector4 result) - { - result.W = value1.W * scaleFactor; - result.X = value1.X * scaleFactor; - result.Y = value1.Y * scaleFactor; - result.Z = value1.Z * scaleFactor; - } - - public static void Multiply(ref Vector4 value1, ref Vector4 value2, out Vector4 result) - { - result.W = value1.W * value2.W; - result.X = value1.X * value2.X; - result.Y = value1.Y * value2.Y; - result.Z = value1.Z * value2.Z; - } - - public static Vector4 Negate(Vector4 value) - { - value = new Vector4(-value.X, -value.Y, -value.Z, -value.W); - return value; - } - - public static void Negate(ref Vector4 value, out Vector4 result) - { - result = new Vector4(-value.X, -value.Y, -value.Z,-value.W); - } - - public void Normalize() - { - Normalize(ref this, out this); - } - - public static Vector4 Normalize(Vector4 vector) - { - Normalize(ref vector, out vector); - return vector; - } - - public static void Normalize(ref Vector4 vector, out Vector4 result) - { - float factor; - DistanceSquared(ref vector, ref zeroVector, out factor); - factor = 1f / (float)Math.Sqrt(factor); - - result.W = vector.W * factor; - result.X = vector.X * factor; - result.Y = vector.Y * factor; - result.Z = vector.Z * factor; - } - - /*public static Vector4 SmoothStep(Vector4 value1, Vector4 value2, float amount) - { - return new Vector4( - MathHelper.SmoothStep(value1.X, value2.X, amount), - MathHelper.SmoothStep(value1.Y, value2.Y, amount), - MathHelper.SmoothStep(value1.Z, value2.Z, amount), - MathHelper.SmoothStep(value1.W, value2.W, amount)); - }*/ - - /*public static void SmoothStep(ref Vector4 value1, ref Vector4 value2, float amount, out Vector4 result) - { - result = new Vector4( - MathHelper.SmoothStep(value1.X, value2.X, amount), - MathHelper.SmoothStep(value1.Y, value2.Y, amount), - MathHelper.SmoothStep(value1.Z, value2.Z, amount), - MathHelper.SmoothStep(value1.W, value2.W, amount)); - }*/ - - public static Vector4 Subtract(Vector4 value1, Vector4 value2) - { - value1.W -= value2.W; - value1.X -= value2.X; - value1.Y -= value2.Y; - value1.Z -= value2.Z; - return value1; - } - - public static void Subtract(ref Vector4 value1, ref Vector4 value2, out Vector4 result) - { - result.W = value1.W - value2.W; - result.X = value1.X - value2.X; - result.Y = value1.Y - value2.Y; - result.Z = value1.Z - value2.Z; - } - - /*public static Vector4 Transform(Vector2 position, Matrix matrix) - { - Vector4 result; - Transform(ref position, ref matrix, out result); - return result; - }*/ - - public static Vector4 Transform(Vector3 position, Matrix matrix) - { - Vector4 result; - Transform(ref position, ref matrix, out result); - return result; - } - - public static Vector4 Transform(Vector4 vector, Matrix matrix) - { - Transform(ref vector, ref matrix, out vector); - return vector; - } - - /*public static void Transform(ref Vector2 position, ref Matrix matrix, out Vector4 result) - { - result = new Vector4((position.X * matrix.M11) + (position.Y * matrix.M21) + matrix.M41, - (position.X * matrix.M12) + (position.Y * matrix.M22) + matrix.M42, - (position.X * matrix.M13) + (position.Y * matrix.M23) + matrix.M43, - (position.X * matrix.M14) + (position.Y * matrix.M24) + matrix.M44); - }*/ - - public static void Transform(ref Vector3 position, ref Matrix matrix, out Vector4 result) - { - result = new Vector4((position.X * matrix.M11) + (position.Y * matrix.M21) + (position.Z * matrix.M31) + matrix.M41, - (position.X * matrix.M12) + (position.Y * matrix.M22) + (position.Z * matrix.M32) + matrix.M42, - (position.X * matrix.M13) + (position.Y * matrix.M23) + (position.Z * matrix.M33) + matrix.M43, - (position.X * matrix.M14) + (position.Y * matrix.M24) + (position.Z * matrix.M34) + matrix.M44); - } - - public static void Transform(ref Vector4 vector, ref Matrix matrix, out Vector4 result) - { - result = new Vector4((vector.X * matrix.M11) + (vector.Y * matrix.M21) + (vector.Z * matrix.M31) + (vector.W * matrix.M41), - (vector.X * matrix.M12) + (vector.Y * matrix.M22) + (vector.Z * matrix.M32) + (vector.W * matrix.M42), - (vector.X * matrix.M13) + (vector.Y * matrix.M23) + (vector.Z * matrix.M33) + (vector.W * matrix.M43), - (vector.X * matrix.M14) + (vector.Y * matrix.M24) + (vector.Z * matrix.M34) + (vector.W * matrix.M44)); - } - - public override string ToString() - { - StringBuilder sb = new StringBuilder(32); - sb.Append("{X:"); - sb.Append(this.X); - sb.Append(" Y:"); - sb.Append(this.Y); - sb.Append(" Z:"); - sb.Append(this.Z); - sb.Append(" W:"); - sb.Append(this.W); - sb.Append("}"); - return sb.ToString(); - } - - #endregion Public Methods - - - #region Operators - - public static Vector4 operator -(Vector4 value) - { - return new Vector4(-value.X, -value.Y, -value.Z, -value.W); - } - - public static bool operator ==(Vector4 value1, Vector4 value2) - { - return value1.W == value2.W - && value1.X == value2.X - && value1.Y == value2.Y - && value1.Z == value2.Z; - } - - public static bool operator !=(Vector4 value1, Vector4 value2) - { - return !(value1 == value2); - } - - public static Vector4 operator +(Vector4 value1, Vector4 value2) - { - value1.W += value2.W; - value1.X += value2.X; - value1.Y += value2.Y; - value1.Z += value2.Z; - return value1; - } - - public static Vector4 operator -(Vector4 value1, Vector4 value2) - { - value1.W -= value2.W; - value1.X -= value2.X; - value1.Y -= value2.Y; - value1.Z -= value2.Z; - return value1; - } - - public static Vector4 operator *(Vector4 value1, Vector4 value2) - { - value1.W *= value2.W; - value1.X *= value2.X; - value1.Y *= value2.Y; - value1.Z *= value2.Z; - return value1; - } - - public static Vector4 operator *(Vector4 value1, float scaleFactor) - { - value1.W *= scaleFactor; - value1.X *= scaleFactor; - value1.Y *= scaleFactor; - value1.Z *= scaleFactor; - return value1; - } - - public static Vector4 operator *(float scaleFactor, Vector4 value1) - { - value1.W *= scaleFactor; - value1.X *= scaleFactor; - value1.Y *= scaleFactor; - value1.Z *= scaleFactor; - return value1; - } - - public static Vector4 operator /(Vector4 value1, Vector4 value2) - { - value1.W /= value2.W; - value1.X /= value2.X; - value1.Y /= value2.Y; - value1.Z /= value2.Z; - return value1; - } - - public static Vector4 operator /(Vector4 value1, float divider) - { - float factor = 1f / divider; - value1.W *= factor; - value1.X *= factor; - value1.Y *= factor; - value1.Z *= factor; - return value1; - } - - #endregion Operators - } -} +#region License +/* +MIT License +Copyright © 2006 The Mono.Xna Team + +All rights reserved. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ +#endregion License + +using System; +using System.ComponentModel; +using System.Text; +using System.Runtime.InteropServices; + +namespace MonoXnaCompactMaths +{ + [Serializable] + [StructLayout(LayoutKind.Sequential)] + //[TypeConverter(typeof(Vector4Converter))] + public struct Vector4 : IEquatable + { + #region Private Fields + + private static Vector4 zeroVector = new Vector4(); + private static Vector4 unitVector = new Vector4(1f, 1f, 1f, 1f); + private static Vector4 unitXVector = new Vector4(1f, 0f, 0f, 0f); + private static Vector4 unitYVector = new Vector4(0f, 1f, 0f, 0f); + private static Vector4 unitZVector = new Vector4(0f, 0f, 1f, 0f); + private static Vector4 unitWVector = new Vector4(0f, 0f, 0f, 1f); + + #endregion Private Fields + + + #region Public Fields + + public float X; + public float Y; + public float Z; + public float W; + + #endregion Public Fields + + + #region Properties + + public static Vector4 Zero + { + get { return zeroVector; } + } + + public static Vector4 One + { + get { return unitVector; } + } + + public static Vector4 UnitX + { + get { return unitXVector; } + } + + public static Vector4 UnitY + { + get { return unitYVector; } + } + + public static Vector4 UnitZ + { + get { return unitZVector; } + } + + public static Vector4 UnitW + { + get { return unitWVector; } + } + + #endregion Properties + + + #region Constructors + + public Vector4(float x, float y, float z, float w) + { + this.X = x; + this.Y = y; + this.Z = z; + this.W = w; + } + + /*public Vector4(Vector2 value, float z, float w) + { + this.X = value.X; + this.Y = value.Y; + this.Z = z; + this.W = w; + }*/ + + public Vector4(Vector3 value, float w) + { + this.X = value.X; + this.Y = value.Y; + this.Z = value.Z; + this.W = w; + } + + public Vector4(float value) + { + this.X = value; + this.Y = value; + this.Z = value; + this.W = value; + } + + #endregion + + + #region Public Methods + + public static Vector4 Add(Vector4 value1, Vector4 value2) + { + value1.W += value2.W; + value1.X += value2.X; + value1.Y += value2.Y; + value1.Z += value2.Z; + return value1; + } + + public static void Add(ref Vector4 value1, ref Vector4 value2, out Vector4 result) + { + result.W = value1.W + value2.W; + result.X = value1.X + value2.X; + result.Y = value1.Y + value2.Y; + result.Z = value1.Z + value2.Z; + } + + /*public static Vector4 Barycentric(Vector4 value1, Vector4 value2, Vector4 value3, float amount1, float amount2) + { + return new Vector4( + MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2), + MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2), + MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2), + MathHelper.Barycentric(value1.W, value2.W, value3.W, amount1, amount2)); + }*/ + + /*public static void Barycentric(ref Vector4 value1, ref Vector4 value2, ref Vector4 value3, float amount1, float amount2, out Vector4 result) + { + result = new Vector4( + MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2), + MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2), + MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2), + MathHelper.Barycentric(value1.W, value2.W, value3.W, amount1, amount2)); + }*/ + + /*public static Vector4 CatmullRom(Vector4 value1, Vector4 value2, Vector4 value3, Vector4 value4, float amount) + { + return new Vector4( + MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount), + MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount), + MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount), + MathHelper.CatmullRom(value1.W, value2.W, value3.W, value4.W, amount)); + }*/ + + /*public static void CatmullRom(ref Vector4 value1, ref Vector4 value2, ref Vector4 value3, ref Vector4 value4, float amount, out Vector4 result) + { + result = new Vector4( + MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount), + MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount), + MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount), + MathHelper.CatmullRom(value1.W, value2.W, value3.W, value4.W, amount)); + }*/ + + /*public static Vector4 Clamp(Vector4 value1, Vector4 min, Vector4 max) + { + return new Vector4( + MathHelper.Clamp(value1.X, min.X, max.X), + MathHelper.Clamp(value1.Y, min.Y, max.Y), + MathHelper.Clamp(value1.Z, min.Z, max.Z), + MathHelper.Clamp(value1.W, min.W, max.W)); + }*/ + + /*public static void Clamp(ref Vector4 value1, ref Vector4 min, ref Vector4 max, out Vector4 result) + { + result = new Vector4( + MathHelper.Clamp(value1.X, min.X, max.X), + MathHelper.Clamp(value1.Y, min.Y, max.Y), + MathHelper.Clamp(value1.Z, min.Z, max.Z), + MathHelper.Clamp(value1.W, min.W, max.W)); + }*/ + + public static float Distance(Vector4 value1, Vector4 value2) + { + return (float)Math.Sqrt(DistanceSquared(value1, value2)); + } + + public static void Distance(ref Vector4 value1, ref Vector4 value2, out float result) + { + result = (float)Math.Sqrt(DistanceSquared(value1, value2)); + } + + public static float DistanceSquared(Vector4 value1, Vector4 value2) + { + float result; + DistanceSquared(ref value1, ref value2, out result); + return result; + } + + public static void DistanceSquared(ref Vector4 value1, ref Vector4 value2, out float result) + { + result = (value1.W - value2.W) * (value1.W - value2.W) + + (value1.X - value2.X) * (value1.X - value2.X) + + (value1.Y - value2.Y) * (value1.Y - value2.Y) + + (value1.Z - value2.Z) * (value1.Z - value2.Z); + } + + public static Vector4 Divide(Vector4 value1, Vector4 value2) + { + value1.W /= value2.W; + value1.X /= value2.X; + value1.Y /= value2.Y; + value1.Z /= value2.Z; + return value1; + } + + public static Vector4 Divide(Vector4 value1, float divider) + { + float factor = 1f / divider; + value1.W *= factor; + value1.X *= factor; + value1.Y *= factor; + value1.Z *= factor; + return value1; + } + + public static void Divide(ref Vector4 value1, float divider, out Vector4 result) + { + float factor = 1f / divider; + result.W = value1.W * factor; + result.X = value1.X * factor; + result.Y = value1.Y * factor; + result.Z = value1.Z * factor; + } + + public static void Divide(ref Vector4 value1, ref Vector4 value2, out Vector4 result) + { + result.W = value1.W / value2.W; + result.X = value1.X / value2.X; + result.Y = value1.Y / value2.Y; + result.Z = value1.Z / value2.Z; + } + + public static float Dot(Vector4 vector1, Vector4 vector2) + { + return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z + vector1.W * vector2.W; + } + + public static void Dot(ref Vector4 vector1, ref Vector4 vector2, out float result) + { + result = vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z + vector1.W * vector2.W; + } + + public override bool Equals(object obj) + { + return (obj is Vector4) ? this == (Vector4)obj : false; + } + + public bool Equals(Vector4 other) + { + return this.W == other.W + && this.X == other.X + && this.Y == other.Y + && this.Z == other.Z; + } + + public override int GetHashCode() + { + return (int)(this.W + this.X + this.Y + this.Y); + } + + /*public static Vector4 Hermite(Vector4 value1, Vector4 tangent1, Vector4 value2, Vector4 tangent2, float amount) + { + Vector4 result = new Vector4(); + Hermite(ref value1, ref tangent1, ref value2, ref tangent2, amount, out result); + return result; + }*/ + + /*public static void Hermite(ref Vector4 value1, ref Vector4 tangent1, ref Vector4 value2, ref Vector4 tangent2, float amount, out Vector4 result) + { + result.W = MathHelper.Hermite(value1.W, tangent1.W, value2.W, tangent2.W, amount); + result.X = MathHelper.Hermite(value1.X, tangent1.X, value2.X, tangent2.X, amount); + result.Y = MathHelper.Hermite(value1.Y, tangent1.Y, value2.Y, tangent2.Y, amount); + result.Z = MathHelper.Hermite(value1.Z, tangent1.Z, value2.Z, tangent2.Z, amount); + }*/ + + public float Length() + { + float result; + DistanceSquared(ref this, ref zeroVector, out result); + return (float)Math.Sqrt(result); + } + + public float LengthSquared() + { + float result; + DistanceSquared(ref this, ref zeroVector, out result); + return result; + } + + /*public static Vector4 Lerp(Vector4 value1, Vector4 value2, float amount) + { + return new Vector4( + MathHelper.Lerp(value1.X, value2.X, amount), + MathHelper.Lerp(value1.Y, value2.Y, amount), + MathHelper.Lerp(value1.Z, value2.Z, amount), + MathHelper.Lerp(value1.W, value2.W, amount)); + }*/ + + /*public static void Lerp(ref Vector4 value1, ref Vector4 value2, float amount, out Vector4 result) + { + result = new Vector4( + MathHelper.Lerp(value1.X, value2.X, amount), + MathHelper.Lerp(value1.Y, value2.Y, amount), + MathHelper.Lerp(value1.Z, value2.Z, amount), + MathHelper.Lerp(value1.W, value2.W, amount)); + }*/ + + /*public static Vector4 Max(Vector4 value1, Vector4 value2) + { + return new Vector4( + MathHelper.Max(value1.X, value2.X), + MathHelper.Max(value1.Y, value2.Y), + MathHelper.Max(value1.Z, value2.Z), + MathHelper.Max(value1.W, value2.W)); + }*/ + + /*public static void Max(ref Vector4 value1, ref Vector4 value2, out Vector4 result) + { + result = new Vector4( + MathHelper.Max(value1.X, value2.X), + MathHelper.Max(value1.Y, value2.Y), + MathHelper.Max(value1.Z, value2.Z), + MathHelper.Max(value1.W, value2.W)); + }*/ + + /*public static Vector4 Min(Vector4 value1, Vector4 value2) + { + return new Vector4( + MathHelper.Min(value1.X, value2.X), + MathHelper.Min(value1.Y, value2.Y), + MathHelper.Min(value1.Z, value2.Z), + MathHelper.Min(value1.W, value2.W)); + }*/ + + /*public static void Min(ref Vector4 value1, ref Vector4 value2, out Vector4 result) + { + result = new Vector4( + MathHelper.Min(value1.X, value2.X), + MathHelper.Min(value1.Y, value2.Y), + MathHelper.Min(value1.Z, value2.Z), + MathHelper.Min(value1.W, value2.W)); + }*/ + + public static Vector4 Multiply(Vector4 value1, Vector4 value2) + { + value1.W *= value2.W; + value1.X *= value2.X; + value1.Y *= value2.Y; + value1.Z *= value2.Z; + return value1; + } + + public static Vector4 Multiply(Vector4 value1, float scaleFactor) + { + value1.W *= scaleFactor; + value1.X *= scaleFactor; + value1.Y *= scaleFactor; + value1.Z *= scaleFactor; + return value1; + } + + public static void Multiply(ref Vector4 value1, float scaleFactor, out Vector4 result) + { + result.W = value1.W * scaleFactor; + result.X = value1.X * scaleFactor; + result.Y = value1.Y * scaleFactor; + result.Z = value1.Z * scaleFactor; + } + + public static void Multiply(ref Vector4 value1, ref Vector4 value2, out Vector4 result) + { + result.W = value1.W * value2.W; + result.X = value1.X * value2.X; + result.Y = value1.Y * value2.Y; + result.Z = value1.Z * value2.Z; + } + + public static Vector4 Negate(Vector4 value) + { + value = new Vector4(-value.X, -value.Y, -value.Z, -value.W); + return value; + } + + public static void Negate(ref Vector4 value, out Vector4 result) + { + result = new Vector4(-value.X, -value.Y, -value.Z,-value.W); + } + + public void Normalize() + { + Normalize(ref this, out this); + } + + public static Vector4 Normalize(Vector4 vector) + { + Normalize(ref vector, out vector); + return vector; + } + + public static void Normalize(ref Vector4 vector, out Vector4 result) + { + float factor; + DistanceSquared(ref vector, ref zeroVector, out factor); + factor = 1f / (float)Math.Sqrt(factor); + + result.W = vector.W * factor; + result.X = vector.X * factor; + result.Y = vector.Y * factor; + result.Z = vector.Z * factor; + } + + /*public static Vector4 SmoothStep(Vector4 value1, Vector4 value2, float amount) + { + return new Vector4( + MathHelper.SmoothStep(value1.X, value2.X, amount), + MathHelper.SmoothStep(value1.Y, value2.Y, amount), + MathHelper.SmoothStep(value1.Z, value2.Z, amount), + MathHelper.SmoothStep(value1.W, value2.W, amount)); + }*/ + + /*public static void SmoothStep(ref Vector4 value1, ref Vector4 value2, float amount, out Vector4 result) + { + result = new Vector4( + MathHelper.SmoothStep(value1.X, value2.X, amount), + MathHelper.SmoothStep(value1.Y, value2.Y, amount), + MathHelper.SmoothStep(value1.Z, value2.Z, amount), + MathHelper.SmoothStep(value1.W, value2.W, amount)); + }*/ + + public static Vector4 Subtract(Vector4 value1, Vector4 value2) + { + value1.W -= value2.W; + value1.X -= value2.X; + value1.Y -= value2.Y; + value1.Z -= value2.Z; + return value1; + } + + public static void Subtract(ref Vector4 value1, ref Vector4 value2, out Vector4 result) + { + result.W = value1.W - value2.W; + result.X = value1.X - value2.X; + result.Y = value1.Y - value2.Y; + result.Z = value1.Z - value2.Z; + } + + /*public static Vector4 Transform(Vector2 position, Matrix matrix) + { + Vector4 result; + Transform(ref position, ref matrix, out result); + return result; + }*/ + + public static Vector4 Transform(Vector3 position, Matrix matrix) + { + Vector4 result; + Transform(ref position, ref matrix, out result); + return result; + } + + public static Vector4 Transform(Vector4 vector, Matrix matrix) + { + Transform(ref vector, ref matrix, out vector); + return vector; + } + + /*public static void Transform(ref Vector2 position, ref Matrix matrix, out Vector4 result) + { + result = new Vector4((position.X * matrix.M11) + (position.Y * matrix.M21) + matrix.M41, + (position.X * matrix.M12) + (position.Y * matrix.M22) + matrix.M42, + (position.X * matrix.M13) + (position.Y * matrix.M23) + matrix.M43, + (position.X * matrix.M14) + (position.Y * matrix.M24) + matrix.M44); + }*/ + + public static void Transform(ref Vector3 position, ref Matrix matrix, out Vector4 result) + { + result = new Vector4((position.X * matrix.M11) + (position.Y * matrix.M21) + (position.Z * matrix.M31) + matrix.M41, + (position.X * matrix.M12) + (position.Y * matrix.M22) + (position.Z * matrix.M32) + matrix.M42, + (position.X * matrix.M13) + (position.Y * matrix.M23) + (position.Z * matrix.M33) + matrix.M43, + (position.X * matrix.M14) + (position.Y * matrix.M24) + (position.Z * matrix.M34) + matrix.M44); + } + + public static void Transform(ref Vector4 vector, ref Matrix matrix, out Vector4 result) + { + result = new Vector4((vector.X * matrix.M11) + (vector.Y * matrix.M21) + (vector.Z * matrix.M31) + (vector.W * matrix.M41), + (vector.X * matrix.M12) + (vector.Y * matrix.M22) + (vector.Z * matrix.M32) + (vector.W * matrix.M42), + (vector.X * matrix.M13) + (vector.Y * matrix.M23) + (vector.Z * matrix.M33) + (vector.W * matrix.M43), + (vector.X * matrix.M14) + (vector.Y * matrix.M24) + (vector.Z * matrix.M34) + (vector.W * matrix.M44)); + } + + public override string ToString() + { + StringBuilder sb = new StringBuilder(32); + sb.Append("{X:"); + sb.Append(this.X); + sb.Append(" Y:"); + sb.Append(this.Y); + sb.Append(" Z:"); + sb.Append(this.Z); + sb.Append(" W:"); + sb.Append(this.W); + sb.Append("}"); + return sb.ToString(); + } + + #endregion Public Methods + + + #region Operators + + public static Vector4 operator -(Vector4 value) + { + return new Vector4(-value.X, -value.Y, -value.Z, -value.W); + } + + public static bool operator ==(Vector4 value1, Vector4 value2) + { + return value1.W == value2.W + && value1.X == value2.X + && value1.Y == value2.Y + && value1.Z == value2.Z; + } + + public static bool operator !=(Vector4 value1, Vector4 value2) + { + return !(value1 == value2); + } + + public static Vector4 operator +(Vector4 value1, Vector4 value2) + { + value1.W += value2.W; + value1.X += value2.X; + value1.Y += value2.Y; + value1.Z += value2.Z; + return value1; + } + + public static Vector4 operator -(Vector4 value1, Vector4 value2) + { + value1.W -= value2.W; + value1.X -= value2.X; + value1.Y -= value2.Y; + value1.Z -= value2.Z; + return value1; + } + + public static Vector4 operator *(Vector4 value1, Vector4 value2) + { + value1.W *= value2.W; + value1.X *= value2.X; + value1.Y *= value2.Y; + value1.Z *= value2.Z; + return value1; + } + + public static Vector4 operator *(Vector4 value1, float scaleFactor) + { + value1.W *= scaleFactor; + value1.X *= scaleFactor; + value1.Y *= scaleFactor; + value1.Z *= scaleFactor; + return value1; + } + + public static Vector4 operator *(float scaleFactor, Vector4 value1) + { + value1.W *= scaleFactor; + value1.X *= scaleFactor; + value1.Y *= scaleFactor; + value1.Z *= scaleFactor; + return value1; + } + + public static Vector4 operator /(Vector4 value1, Vector4 value2) + { + value1.W /= value2.W; + value1.X /= value2.X; + value1.Y /= value2.Y; + value1.Z /= value2.Z; + return value1; + } + + public static Vector4 operator /(Vector4 value1, float divider) + { + float factor = 1f / divider; + value1.W *= factor; + value1.X *= factor; + value1.Y *= factor; + value1.Z *= factor; + return value1; + } + + #endregion Operators + } +} -- cgit v1.1