From d56f7f3f77cbae1a3507fdda386b01276a9832ac Mon Sep 17 00:00:00 2001 From: Regalis Date: Wed, 8 Jul 2015 11:37:47 +0300 Subject: [PATCH] Server job assigning logic, submarine movement syncing, submarine collision improvements, spawnpoints in levels --- .../Collision/Collision.cs | 1863 +++++++++++++++++ .../Collision/Distance.cs | 794 +++++++ .../Collision/DynamicTree.cs | 1034 +++++++++ .../Collision/DynamicTreeBroadPhase.cs | 347 +++ .../Collision/IBroadPhase.cs | 32 + .../Collision/Shapes/ChainShape.cs | 273 +++ .../Collision/Shapes/CircleShape.cs | 198 ++ .../Collision/Shapes/EdgeShape.cs | 252 +++ .../Collision/Shapes/PolygonShape.cs | 471 +++++ .../Collision/Shapes/Shape.cs | 255 +++ .../Collision/TimeOfImpact.cs | 498 +++++ .../Common/ConvexHull/ChainHull.cs | 143 ++ .../Common/ConvexHull/GiftWrap.cs | 90 + .../Common/ConvexHull/Melkman.cs | 132 ++ Farseer Physics Engine 3.5/Common/Curve.cs | 273 +++ Farseer Physics Engine 3.5/Common/CurveKey.cs | 167 ++ .../Common/CurveKeyCollection.cs | 176 ++ .../Common/Decomposition/BayazitDecomposer.cs | 243 +++ .../CDT/Delaunay/DelaunayTriangle.cs | 420 ++++ .../CDT/Delaunay/Sweep/AdvancingFront.cs | 180 ++ .../CDT/Delaunay/Sweep/AdvancingFrontNode.cs | 64 + .../CDT/Delaunay/Sweep/DTSweep.cs | 1151 ++++++++++ .../CDT/Delaunay/Sweep/DTSweepConstraint.cs | 66 + .../CDT/Delaunay/Sweep/DTSweepContext.cs | 236 +++ .../Delaunay/Sweep/DTSweepPointComparator.cs | 69 + .../Delaunay/Sweep/PointOnEdgeException.cs | 43 + .../Decomposition/CDT/ITriangulatable.cs | 48 + .../Common/Decomposition/CDT/Orientation.cs | 40 + .../Decomposition/CDT/Polygon/Polygon.cs | 272 +++ .../Decomposition/CDT/Polygon/PolygonPoint.cs | 48 + .../Decomposition/CDT/Polygon/PolygonSet.cs | 65 + .../CDT/Sets/ConstrainedPointSet.cs | 114 + .../Common/Decomposition/CDT/Sets/PointSet.cs | 84 + .../CDT/TriangulationConstraint.cs | 46 + .../Decomposition/CDT/TriangulationContext.cs | 84 + .../Decomposition/CDT/TriangulationMode.cs | 40 + .../Decomposition/CDT/TriangulationPoint.cs | 82 + .../Decomposition/CDT/TriangulationUtil.cs | 175 ++ .../Decomposition/CDT/Util/FixedArray3.cs | 118 ++ .../Decomposition/CDT/Util/FixedBitArray3.cs | 118 ++ .../Decomposition/CDT/Util/PointGenerator.cs | 38 + .../CDT/Util/PolygonGenerator.cs | 98 + .../Common/Decomposition/CDTDecomposer.cs | 75 + .../Common/Decomposition/EarclipDecomposer.cs | 403 ++++ .../Decomposition/FlipcodeDecomposer.cs | 152 ++ .../Common/Decomposition/Seidel/Edge.cs | 60 + .../Decomposition/Seidel/MonotoneMountain.cs | 166 ++ .../Common/Decomposition/Seidel/Node.cs | 41 + .../Common/Decomposition/Seidel/Point.cs | 61 + .../Common/Decomposition/Seidel/QueryGraph.cs | 78 + .../Common/Decomposition/Seidel/Sink.cs | 27 + .../Common/Decomposition/Seidel/Trapezoid.cs | 123 ++ .../Decomposition/Seidel/TrapezoidalMap.cs | 195 ++ .../Decomposition/Seidel/Triangulator.cs | 203 ++ .../Common/Decomposition/Seidel/XNode.cs | 21 + .../Common/Decomposition/Seidel/YNode.cs | 29 + .../Common/Decomposition/SeidelDecomposer.cs | 109 + .../Common/Decomposition/Triangulate.cs | 172 ++ .../Common/FixedArray.cs | 224 ++ Farseer Physics Engine 3.5/Common/HashSet.cs | 78 + .../Common/LineTools.cs | 287 +++ Farseer Physics Engine 3.5/Common/Math.cs | 806 +++++++ .../Common/MathHelper.cs | 158 ++ Farseer Physics Engine 3.5/Common/Matrix.cs | 1130 ++++++++++ Farseer Physics Engine 3.5/Common/Path.cs | 337 +++ .../Common/PathManager.cs | 186 ++ .../Common/PhysicsLogic/FilterData.cs | 133 ++ .../Common/PhysicsLogic/PhysicsLogic.cs | 66 + .../Common/PhysicsLogic/RealExplosion.cs | 418 ++++ .../Common/PhysicsLogic/SimpleExplosion.cs | 92 + .../PolygonManipulation/CuttingTools.cs | 217 ++ .../PolygonManipulation/SimpleCombiner.cs | 226 ++ .../PolygonManipulation/SimplifyTools.cs | 302 +++ .../PolygonManipulation/YuPengClipper.cs | 514 +++++ .../Common/PolygonTools.cs | 360 ++++ .../Common/Serialization.cs | 1480 +++++++++++++ .../Common/Stopwatch.cs | 112 + .../Common/TextureTools/MarchingSquares.cs | 800 +++++++ .../Common/TextureTools/Terrain.cs | 264 +++ .../Common/TextureTools/TextureConverter.cs | 1258 +++++++++++ Farseer Physics Engine 3.5/Common/Vector2.cs | 588 ++++++ Farseer Physics Engine 3.5/Common/Vector3.cs | 648 ++++++ Farseer Physics Engine 3.5/Common/Vertices.cs | 582 +++++ .../Controllers/AbstractForceController.cs | 323 +++ .../Controllers/BuoyancyController.cs | 134 ++ .../Controllers/Controller.cs | 72 + .../Controllers/GravityController.cs | 110 + .../Controllers/SimpleWindForce.cs | 75 + .../Controllers/VelocityLimitController.cs | 129 ++ Farseer Physics Engine 3.5/ConvertUnits.cs | 108 + Farseer Physics Engine 3.5/DebugViewBase.cs | 165 ++ Farseer Physics Engine 3.5/Dynamics/Body.cs | 1362 ++++++++++++ .../Dynamics/BreakableBody.cs | 148 ++ .../Dynamics/ContactManager.cs | 445 ++++ .../Dynamics/Contacts/Contact.cs | 480 +++++ .../Dynamics/Contacts/ContactSolver.cs | 979 +++++++++ .../Dynamics/Fixture.cs | 619 ++++++ Farseer Physics Engine 3.5/Dynamics/Island.cs | 449 ++++ .../Dynamics/Joints/AngleJoint.cs | 128 ++ .../Dynamics/Joints/DistanceJoint.cs | 331 +++ .../Dynamics/Joints/FixedMouseJoint.cs | 261 +++ .../Dynamics/Joints/FrictionJoint.cs | 272 +++ .../Dynamics/Joints/GearJoint.cs | 478 +++++ .../Dynamics/Joints/Joint.cs | 253 +++ .../Dynamics/Joints/MotorJoint.cs | 320 +++ .../Dynamics/Joints/PrismaticJoint.cs | 768 +++++++ .../Dynamics/Joints/PulleyJoint.cs | 396 ++++ .../Dynamics/Joints/RevoluteJoint.cs | 619 ++++++ .../Dynamics/Joints/RopeJoint.cs | 291 +++ .../Dynamics/Joints/WeldJoint.cs | 388 ++++ .../Dynamics/Joints/WheelJoint.cs | 513 +++++ .../Dynamics/TimeStep.cs | 66 + Farseer Physics Engine 3.5/Dynamics/World.cs | 1510 +++++++++++++ .../Dynamics/WorldCallbacks.cs | 63 + .../Factories/BodyFactory.cs | 281 +++ .../Factories/FixtureFactory.cs | 129 ++ .../Factories/JointFactory.cs | 168 ++ .../Factories/LinkFactory.cs | 64 + .../Farseer Physics Engine.sln | 124 ++ .../Farseer Physics MonoGame.csproj | 180 ++ .../Farseer Physics Silverlight.csproj | 533 +++++ .../Farseer Physics WP7.csproj | 265 +++ .../Farseer Physics XNA.csproj | 266 +++ .../Farseer Physics Xbox360.csproj | 256 +++ .../Farseer Physics.csproj | 218 ++ .../Properties/AssemblyInfo.cs | 34 + Farseer Physics Engine 3.5/Settings.cs | 285 +++ Farseer Physics Engine 3.5/StyleCop.Cache | 6 + Subsurface/Characters/CharacterInfo.cs | 26 +- .../Characters/HumanoidAnimController.cs | 4 +- Subsurface/Characters/Job.cs | 6 - Subsurface/Characters/Jobs/Job.cs | 30 +- Subsurface/Characters/Jobs/JobPrefab.cs | 59 +- Subsurface/DebugConsole.cs | 4 +- Subsurface/Events/MonsterEvent.cs | 2 +- Subsurface/Events/ScriptedEvent.cs | 8 +- Subsurface/Events/TaskManager.cs | 14 +- Subsurface/GUI/GUI.cs | 19 +- Subsurface/GUI/GUIComponent.cs | 2 + Subsurface/GUI/GUITextBox.cs | 4 +- Subsurface/GameSession/CrewManager.cs | 2 +- Subsurface/GameSession/GameSession.cs | 8 +- Subsurface/Items/Item.cs | 4 +- Subsurface/Map/Level.cs | 82 +- Subsurface/Map/MapEntity.cs | 6 + Subsurface/Map/Submarine.cs | 43 +- Subsurface/Map/WayPoint.cs | 11 +- Subsurface/Networking/GameClient.cs | 45 +- Subsurface/Networking/GameServer.cs | 164 +- Subsurface/Screens/NetLobbyScreen.cs | 132 +- Subsurface/StyleCop.Cache | 150 +- Subsurface/Subsurface.csproj | 9 +- Subsurface/ToolBox.cs | 12 +- Subsurface_Solution.sln | 2 +- Subsurface_Solution.v12.suo | Bin 378368 -> 385536 bytes 155 files changed, 39772 insertions(+), 261 deletions(-) create mode 100644 Farseer Physics Engine 3.5/Collision/Collision.cs create mode 100644 Farseer Physics Engine 3.5/Collision/Distance.cs create mode 100644 Farseer Physics Engine 3.5/Collision/DynamicTree.cs create mode 100644 Farseer Physics Engine 3.5/Collision/DynamicTreeBroadPhase.cs create mode 100644 Farseer Physics Engine 3.5/Collision/IBroadPhase.cs create mode 100644 Farseer Physics Engine 3.5/Collision/Shapes/ChainShape.cs create mode 100644 Farseer Physics Engine 3.5/Collision/Shapes/CircleShape.cs create mode 100644 Farseer Physics Engine 3.5/Collision/Shapes/EdgeShape.cs create mode 100644 Farseer Physics Engine 3.5/Collision/Shapes/PolygonShape.cs create mode 100644 Farseer Physics Engine 3.5/Collision/Shapes/Shape.cs create mode 100644 Farseer Physics Engine 3.5/Collision/TimeOfImpact.cs create mode 100644 Farseer Physics Engine 3.5/Common/ConvexHull/ChainHull.cs create mode 100644 Farseer Physics Engine 3.5/Common/ConvexHull/GiftWrap.cs create mode 100644 Farseer Physics Engine 3.5/Common/ConvexHull/Melkman.cs create mode 100644 Farseer Physics Engine 3.5/Common/Curve.cs create mode 100644 Farseer Physics Engine 3.5/Common/CurveKey.cs create mode 100644 Farseer Physics Engine 3.5/Common/CurveKeyCollection.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/BayazitDecomposer.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/DelaunayTriangle.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFront.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFrontNode.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/DTSweep.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepConstraint.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepContext.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepPointComparator.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/PointOnEdgeException.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/CDT/ITriangulatable.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/CDT/Orientation.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/CDT/Polygon/Polygon.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/CDT/Polygon/PolygonPoint.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/CDT/Polygon/PolygonSet.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/CDT/Sets/ConstrainedPointSet.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/CDT/Sets/PointSet.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationConstraint.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationContext.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationMode.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationPoint.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationUtil.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/FixedArray3.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/FixedBitArray3.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/PointGenerator.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/PolygonGenerator.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/CDTDecomposer.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/EarclipDecomposer.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/FlipcodeDecomposer.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Edge.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/Seidel/MonotoneMountain.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Node.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Point.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/Seidel/QueryGraph.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Sink.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Trapezoid.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/Seidel/TrapezoidalMap.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Triangulator.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/Seidel/XNode.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/Seidel/YNode.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/SeidelDecomposer.cs create mode 100644 Farseer Physics Engine 3.5/Common/Decomposition/Triangulate.cs create mode 100644 Farseer Physics Engine 3.5/Common/FixedArray.cs create mode 100644 Farseer Physics Engine 3.5/Common/HashSet.cs create mode 100644 Farseer Physics Engine 3.5/Common/LineTools.cs create mode 100644 Farseer Physics Engine 3.5/Common/Math.cs create mode 100644 Farseer Physics Engine 3.5/Common/MathHelper.cs create mode 100644 Farseer Physics Engine 3.5/Common/Matrix.cs create mode 100644 Farseer Physics Engine 3.5/Common/Path.cs create mode 100644 Farseer Physics Engine 3.5/Common/PathManager.cs create mode 100644 Farseer Physics Engine 3.5/Common/PhysicsLogic/FilterData.cs create mode 100644 Farseer Physics Engine 3.5/Common/PhysicsLogic/PhysicsLogic.cs create mode 100644 Farseer Physics Engine 3.5/Common/PhysicsLogic/RealExplosion.cs create mode 100644 Farseer Physics Engine 3.5/Common/PhysicsLogic/SimpleExplosion.cs create mode 100644 Farseer Physics Engine 3.5/Common/PolygonManipulation/CuttingTools.cs create mode 100644 Farseer Physics Engine 3.5/Common/PolygonManipulation/SimpleCombiner.cs create mode 100644 Farseer Physics Engine 3.5/Common/PolygonManipulation/SimplifyTools.cs create mode 100644 Farseer Physics Engine 3.5/Common/PolygonManipulation/YuPengClipper.cs create mode 100644 Farseer Physics Engine 3.5/Common/PolygonTools.cs create mode 100644 Farseer Physics Engine 3.5/Common/Serialization.cs create mode 100644 Farseer Physics Engine 3.5/Common/Stopwatch.cs create mode 100644 Farseer Physics Engine 3.5/Common/TextureTools/MarchingSquares.cs create mode 100644 Farseer Physics Engine 3.5/Common/TextureTools/Terrain.cs create mode 100644 Farseer Physics Engine 3.5/Common/TextureTools/TextureConverter.cs create mode 100644 Farseer Physics Engine 3.5/Common/Vector2.cs create mode 100644 Farseer Physics Engine 3.5/Common/Vector3.cs create mode 100644 Farseer Physics Engine 3.5/Common/Vertices.cs create mode 100644 Farseer Physics Engine 3.5/Controllers/AbstractForceController.cs create mode 100644 Farseer Physics Engine 3.5/Controllers/BuoyancyController.cs create mode 100644 Farseer Physics Engine 3.5/Controllers/Controller.cs create mode 100644 Farseer Physics Engine 3.5/Controllers/GravityController.cs create mode 100644 Farseer Physics Engine 3.5/Controllers/SimpleWindForce.cs create mode 100644 Farseer Physics Engine 3.5/Controllers/VelocityLimitController.cs create mode 100644 Farseer Physics Engine 3.5/ConvertUnits.cs create mode 100644 Farseer Physics Engine 3.5/DebugViewBase.cs create mode 100644 Farseer Physics Engine 3.5/Dynamics/Body.cs create mode 100644 Farseer Physics Engine 3.5/Dynamics/BreakableBody.cs create mode 100644 Farseer Physics Engine 3.5/Dynamics/ContactManager.cs create mode 100644 Farseer Physics Engine 3.5/Dynamics/Contacts/Contact.cs create mode 100644 Farseer Physics Engine 3.5/Dynamics/Contacts/ContactSolver.cs create mode 100644 Farseer Physics Engine 3.5/Dynamics/Fixture.cs create mode 100644 Farseer Physics Engine 3.5/Dynamics/Island.cs create mode 100644 Farseer Physics Engine 3.5/Dynamics/Joints/AngleJoint.cs create mode 100644 Farseer Physics Engine 3.5/Dynamics/Joints/DistanceJoint.cs create mode 100644 Farseer Physics Engine 3.5/Dynamics/Joints/FixedMouseJoint.cs create mode 100644 Farseer Physics Engine 3.5/Dynamics/Joints/FrictionJoint.cs create mode 100644 Farseer Physics Engine 3.5/Dynamics/Joints/GearJoint.cs create mode 100644 Farseer Physics Engine 3.5/Dynamics/Joints/Joint.cs create mode 100644 Farseer Physics Engine 3.5/Dynamics/Joints/MotorJoint.cs create mode 100644 Farseer Physics Engine 3.5/Dynamics/Joints/PrismaticJoint.cs create mode 100644 Farseer Physics Engine 3.5/Dynamics/Joints/PulleyJoint.cs create mode 100644 Farseer Physics Engine 3.5/Dynamics/Joints/RevoluteJoint.cs create mode 100644 Farseer Physics Engine 3.5/Dynamics/Joints/RopeJoint.cs create mode 100644 Farseer Physics Engine 3.5/Dynamics/Joints/WeldJoint.cs create mode 100644 Farseer Physics Engine 3.5/Dynamics/Joints/WheelJoint.cs create mode 100644 Farseer Physics Engine 3.5/Dynamics/TimeStep.cs create mode 100644 Farseer Physics Engine 3.5/Dynamics/World.cs create mode 100644 Farseer Physics Engine 3.5/Dynamics/WorldCallbacks.cs create mode 100644 Farseer Physics Engine 3.5/Factories/BodyFactory.cs create mode 100644 Farseer Physics Engine 3.5/Factories/FixtureFactory.cs create mode 100644 Farseer Physics Engine 3.5/Factories/JointFactory.cs create mode 100644 Farseer Physics Engine 3.5/Factories/LinkFactory.cs create mode 100644 Farseer Physics Engine 3.5/Farseer Physics Engine.sln create mode 100644 Farseer Physics Engine 3.5/Farseer Physics MonoGame.csproj create mode 100644 Farseer Physics Engine 3.5/Farseer Physics Silverlight.csproj create mode 100644 Farseer Physics Engine 3.5/Farseer Physics WP7.csproj create mode 100644 Farseer Physics Engine 3.5/Farseer Physics XNA.csproj create mode 100644 Farseer Physics Engine 3.5/Farseer Physics Xbox360.csproj create mode 100644 Farseer Physics Engine 3.5/Farseer Physics.csproj create mode 100644 Farseer Physics Engine 3.5/Properties/AssemblyInfo.cs create mode 100644 Farseer Physics Engine 3.5/Settings.cs create mode 100644 Farseer Physics Engine 3.5/StyleCop.Cache delete mode 100644 Subsurface/Characters/Job.cs diff --git a/Farseer Physics Engine 3.5/Collision/Collision.cs b/Farseer Physics Engine 3.5/Collision/Collision.cs new file mode 100644 index 000000000..287d58687 --- /dev/null +++ b/Farseer Physics Engine 3.5/Collision/Collision.cs @@ -0,0 +1,1863 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Runtime.InteropServices; +using FarseerPhysics.Collision.Shapes; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Collision +{ + internal enum ContactFeatureType : byte + { + Vertex = 0, + Face = 1, + } + + /// + /// The features that intersect to form the contact point + /// This must be 4 bytes or less. + /// + public struct ContactFeature + { + /// + /// Feature index on ShapeA + /// + public byte IndexA; + + /// + /// Feature index on ShapeB + /// + public byte IndexB; + + /// + /// The feature type on ShapeA + /// + public byte TypeA; + + /// + /// The feature type on ShapeB + /// + public byte TypeB; + } + + /// + /// Contact ids to facilitate warm starting. + /// + [StructLayout(LayoutKind.Explicit)] + public struct ContactID + { + /// + /// The features that intersect to form the contact point + /// + [FieldOffset(0)] + public ContactFeature Features; + + /// + /// Used to quickly compare contact ids. + /// + [FieldOffset(0)] + public uint Key; + } + + /// + /// A manifold point is a contact point belonging to a contact + /// manifold. It holds details related to the geometry and dynamics + /// of the contact points. + /// The local point usage depends on the manifold type: + /// -ShapeType.Circles: the local center of circleB + /// -SeparationFunction.FaceA: the local center of cirlceB or the clip point of polygonB + /// -SeparationFunction.FaceB: the clip point of polygonA + /// This structure is stored across time steps, so we keep it small. + /// Note: the impulses are used for internal caching and may not + /// provide reliable contact forces, especially for high speed collisions. + /// + public struct ManifoldPoint + { + /// + /// Uniquely identifies a contact point between two Shapes + /// + public ContactID Id; + + /// + /// Usage depends on manifold type + /// + public Vector2 LocalPoint; + + /// + /// The non-penetration impulse + /// + public float NormalImpulse; + + /// + /// The friction impulse + /// + public float TangentImpulse; + } + + public enum ManifoldType + { + Circles, + FaceA, + FaceB + } + + /// + /// A manifold for two touching convex Shapes. + /// Box2D supports multiple types of contact: + /// - Clip point versus plane with radius + /// - Point versus point with radius (circles) + /// The local point usage depends on the manifold type: + /// - ShapeType.Circles: the local center of circleA + /// - SeparationFunction.FaceA: the center of faceA + /// - SeparationFunction.FaceB: the center of faceB + /// Similarly the local normal usage: + /// - ShapeType.Circles: not used + /// - SeparationFunction.FaceA: the normal on polygonA + /// - SeparationFunction.FaceB: the normal on polygonB + /// We store contacts in this way so that position correction can + /// account for movement, which is critical for continuous physics. + /// All contact scenarios must be expressed in one of these types. + /// This structure is stored across time steps, so we keep it small. + /// + public struct Manifold + { + /// + /// Not use for Type.SeparationFunction.Points + /// + public Vector2 LocalNormal; + + /// + /// Usage depends on manifold type + /// + public Vector2 LocalPoint; + + /// + /// The number of manifold points + /// + public int PointCount; + + /// + /// The points of contact + /// + public FixedArray2 Points; + + public ManifoldType Type; + } + + /// + /// This is used for determining the state of contact points. + /// + public enum PointState + { + /// + /// Point does not exist + /// + Null, + + /// + /// Point was added in the update + /// + Add, + + /// + /// Point persisted across the update + /// + Persist, + + /// + /// Point was removed in the update + /// + Remove, + } + + /// + /// Used for computing contact manifolds. + /// + public struct ClipVertex + { + public ContactID ID; + public Vector2 V; + } + + /// + /// Ray-cast input data. + /// + public struct RayCastInput + { + /// + /// The ray extends from p1 to p1 + maxFraction * (p2 - p1). + /// If you supply a max fraction of 1, the ray extends from p1 to p2. + /// A max fraction of 0.5 makes the ray go from p1 and half way to p2. + /// + public float MaxFraction; + + /// + /// The starting point of the ray. + /// + public Vector2 Point1; + + /// + /// The ending point of the ray. + /// + public Vector2 Point2; + } + + /// + /// Ray-cast output data. + /// + public struct RayCastOutput + { + /// + /// The ray hits at p1 + fraction * (p2 - p1), where p1 and p2 come from RayCastInput. + /// Contains the actual fraction of the ray where it has the intersection point. + /// + public float Fraction; + + /// + /// The normal of the face of the shape the ray has hit. + /// + public Vector2 Normal; + } + + /// + /// An axis aligned bounding box. + /// + public struct AABB + { + /// + /// The lower vertex + /// + public Vector2 LowerBound; + + /// + /// The upper vertex + /// + public Vector2 UpperBound; + + public AABB(Vector2 min, Vector2 max) + : this(ref min, ref max) + { + } + + public AABB(ref Vector2 min, ref Vector2 max) + { + LowerBound = min; + UpperBound = max; + } + + public AABB(Vector2 center, float width, float height) + { + LowerBound = center - new Vector2(width / 2, height / 2); + UpperBound = center + new Vector2(width / 2, height / 2); + } + + public float Width + { + get { return UpperBound.X - LowerBound.X; } + } + + public float Height + { + get { return UpperBound.Y - LowerBound.Y; } + } + + /// + /// Get the center of the AABB. + /// + public Vector2 Center + { + get { return 0.5f * (LowerBound + UpperBound); } + } + + /// + /// Get the extents of the AABB (half-widths). + /// + public Vector2 Extents + { + get { return 0.5f * (UpperBound - LowerBound); } + } + + /// + /// Get the perimeter length + /// + public float Perimeter + { + get + { + float wx = UpperBound.X - LowerBound.X; + float wy = UpperBound.Y - LowerBound.Y; + return 2.0f * (wx + wy); + } + } + + /// + /// Gets the vertices of the AABB. + /// + /// The corners of the AABB + public Vertices Vertices + { + get + { + Vertices vertices = new Vertices(4); + vertices.Add(UpperBound); + vertices.Add(new Vector2(UpperBound.X, LowerBound.Y)); + vertices.Add(LowerBound); + vertices.Add(new Vector2(LowerBound.X, UpperBound.Y)); + return vertices; + } + } + + /// + /// First quadrant + /// + public AABB Q1 + { + get { return new AABB(Center, UpperBound); } + } + + /// + /// Second quadrant + /// + public AABB Q2 + { + get { return new AABB(new Vector2(LowerBound.X, Center.Y), new Vector2(Center.X, UpperBound.Y)); } + } + + /// + /// Third quadrant + /// + public AABB Q3 + { + get { return new AABB(LowerBound, Center); } + } + + /// + /// Forth quadrant + /// + public AABB Q4 + { + get { return new AABB(new Vector2(Center.X, LowerBound.Y), new Vector2(UpperBound.X, Center.Y)); } + } + + /// + /// Verify that the bounds are sorted. And the bounds are valid numbers (not NaN). + /// + /// + /// true if this instance is valid; otherwise, false. + /// + public bool IsValid() + { + Vector2 d = UpperBound - LowerBound; + bool valid = d.X >= 0.0f && d.Y >= 0.0f; + valid = valid && LowerBound.IsValid() && UpperBound.IsValid(); + return valid; + } + + /// + /// Combine an AABB into this one. + /// + /// The aabb. + public void Combine(ref AABB aabb) + { + LowerBound = Vector2.Min(LowerBound, aabb.LowerBound); + UpperBound = Vector2.Max(UpperBound, aabb.UpperBound); + } + + /// + /// Combine two AABBs into this one. + /// + /// The aabb1. + /// The aabb2. + public void Combine(ref AABB aabb1, ref AABB aabb2) + { + LowerBound = Vector2.Min(aabb1.LowerBound, aabb2.LowerBound); + UpperBound = Vector2.Max(aabb1.UpperBound, aabb2.UpperBound); + } + + /// + /// Does this aabb contain the provided AABB. + /// + /// The aabb. + /// + /// true if it contains the specified aabb; otherwise, false. + /// + public bool Contains(ref AABB aabb) + { + bool result = true; + result = result && LowerBound.X <= aabb.LowerBound.X; + result = result && LowerBound.Y <= aabb.LowerBound.Y; + result = result && aabb.UpperBound.X <= UpperBound.X; + result = result && aabb.UpperBound.Y <= UpperBound.Y; + return result; + } + + /// + /// Determines whether the AAABB contains the specified point. + /// + /// The point. + /// + /// true if it contains the specified point; otherwise, false. + /// + public bool Contains(ref Vector2 point) + { + //using epsilon to try and gaurd against float rounding errors. + return (point.X > (LowerBound.X + Settings.Epsilon) && point.X < (UpperBound.X - Settings.Epsilon) && + (point.Y > (LowerBound.Y + Settings.Epsilon) && point.Y < (UpperBound.Y - Settings.Epsilon))); + } + + /// + /// Test if the two AABBs overlap. + /// + /// The first AABB. + /// The second AABB. + /// True if they are overlapping. + public static bool TestOverlap(ref AABB a, ref AABB b) + { + Vector2 d1 = b.LowerBound - a.UpperBound; + Vector2 d2 = a.LowerBound - b.UpperBound; + + if (d1.X > 0.0f || d1.Y > 0.0f) + return false; + + if (d2.X > 0.0f || d2.Y > 0.0f) + return false; + + return true; + } + + /// + /// Raycast against this AABB using the specificed points and maxfraction (found in input) + /// + /// The results of the raycast. + /// The parameters for the raycast. + /// True if the ray intersects the AABB + public bool RayCast(out RayCastOutput output, ref RayCastInput input, bool doInteriorCheck = true) + { + // From Real-time Collision Detection, p179. + + output = new RayCastOutput(); + + float tmin = -Settings.MaxFloat; + float tmax = Settings.MaxFloat; + + Vector2 p = input.Point1; + Vector2 d = input.Point2 - input.Point1; + Vector2 absD = MathUtils.Abs(d); + + Vector2 normal = Vector2.Zero; + + for (int i = 0; i < 2; ++i) + { + float absD_i = i == 0 ? absD.X : absD.Y; + float lowerBound_i = i == 0 ? LowerBound.X : LowerBound.Y; + float upperBound_i = i == 0 ? UpperBound.X : UpperBound.Y; + float p_i = i == 0 ? p.X : p.Y; + + if (absD_i < Settings.Epsilon) + { + // Parallel. + if (p_i < lowerBound_i || upperBound_i < p_i) + { + return false; + } + } + else + { + float d_i = i == 0 ? d.X : d.Y; + + float inv_d = 1.0f / d_i; + float t1 = (lowerBound_i - p_i) * inv_d; + float t2 = (upperBound_i - p_i) * inv_d; + + // Sign of the normal vector. + float s = -1.0f; + + if (t1 > t2) + { + MathUtils.Swap(ref t1, ref t2); + s = 1.0f; + } + + // Push the min up + if (t1 > tmin) + { + if (i == 0) + { + normal.X = s; + } + else + { + normal.Y = s; + } + + tmin = t1; + } + + // Pull the max down + tmax = Math.Min(tmax, t2); + + if (tmin > tmax) + { + return false; + } + } + } + + // Does the ray start inside the box? + // Does the ray intersect beyond the max fraction? + if (doInteriorCheck && (tmin < 0.0f || input.MaxFraction < tmin)) + { + return false; + } + + // Intersection. + output.Fraction = tmin; + output.Normal = normal; + return true; + } + } + + /// + /// This holds polygon B expressed in frame A. + /// + public class TempPolygon + { + public Vector2[] Vertices = new Vector2[Settings.MaxPolygonVertices]; + public Vector2[] Normals = new Vector2[Settings.MaxPolygonVertices]; + public int Count; + } + + /// + /// This structure is used to keep track of the best separating axis. + /// + public struct EPAxis + { + public int Index; + public float Separation; + public EPAxisType Type; + } + + /// + /// Reference face used for clipping + /// + public struct ReferenceFace + { + public int i1, i2; + + public Vector2 v1, v2; + + public Vector2 normal; + + public Vector2 sideNormal1; + public float sideOffset1; + + public Vector2 sideNormal2; + public float sideOffset2; + } + + public enum EPAxisType + { + Unknown, + EdgeA, + EdgeB, + } + + /// + /// Collision methods + /// + public static class Collision + { + [ThreadStatic] + private static DistanceInput _input; + + /// + /// Test overlap between the two shapes. + /// + /// The first shape. + /// The index for the first shape. + /// The second shape. + /// The index for the second shape. + /// The transform for the first shape. + /// The transform for the seconds shape. + /// + public static bool TestOverlap(Shape shapeA, int indexA, Shape shapeB, int indexB, ref Transform xfA, ref Transform xfB) + { + _input = _input ?? new DistanceInput(); + _input.ProxyA.Set(shapeA, indexA); + _input.ProxyB.Set(shapeB, indexB); + _input.TransformA = xfA; + _input.TransformB = xfB; + _input.UseRadii = true; + + SimplexCache cache; + DistanceOutput output; + Distance.ComputeDistance(out output, out cache, _input); + + return output.Distance < 10.0f * Settings.Epsilon; + } + + public static void GetPointStates(out FixedArray2 state1, out FixedArray2 state2, ref Manifold manifold1, ref Manifold manifold2) + { + state1 = new FixedArray2(); + state2 = new FixedArray2(); + + // Detect persists and removes. + for (int i = 0; i < manifold1.PointCount; ++i) + { + ContactID id = manifold1.Points[i].Id; + + state1[i] = PointState.Remove; + + for (int j = 0; j < manifold2.PointCount; ++j) + { + if (manifold2.Points[j].Id.Key == id.Key) + { + state1[i] = PointState.Persist; + break; + } + } + } + + // Detect persists and adds. + for (int i = 0; i < manifold2.PointCount; ++i) + { + ContactID id = manifold2.Points[i].Id; + + state2[i] = PointState.Add; + + for (int j = 0; j < manifold1.PointCount; ++j) + { + if (manifold1.Points[j].Id.Key == id.Key) + { + state2[i] = PointState.Persist; + break; + } + } + } + } + + /// + /// Compute the collision manifold between two circles. + /// + public static void CollideCircles(ref Manifold manifold, CircleShape circleA, ref Transform xfA, CircleShape circleB, ref Transform xfB) + { + manifold.PointCount = 0; + + Vector2 pA = MathUtils.Mul(ref xfA, circleA.Position); + Vector2 pB = MathUtils.Mul(ref xfB, circleB.Position); + + Vector2 d = pB - pA; + float distSqr = Vector2.Dot(d, d); + float radius = circleA.Radius + circleB.Radius; + if (distSqr > radius * radius) + { + return; + } + + manifold.Type = ManifoldType.Circles; + manifold.LocalPoint = circleA.Position; + manifold.LocalNormal = Vector2.Zero; + manifold.PointCount = 1; + + ManifoldPoint p0 = manifold.Points[0]; + + p0.LocalPoint = circleB.Position; + p0.Id.Key = 0; + + manifold.Points[0] = p0; + } + + /// + /// Compute the collision manifold between a polygon and a circle. + /// + /// The manifold. + /// The polygon A. + /// The transform of A. + /// The circle B. + /// The transform of B. + public static void CollidePolygonAndCircle(ref Manifold manifold, PolygonShape polygonA, ref Transform xfA, CircleShape circleB, ref Transform xfB) + { + manifold.PointCount = 0; + + // Compute circle position in the frame of the polygon. + Vector2 c = MathUtils.Mul(ref xfB, circleB.Position); + Vector2 cLocal = MathUtils.MulT(ref xfA, c); + + // Find the min separating edge. + int normalIndex = 0; + float separation = -Settings.MaxFloat; + float radius = polygonA.Radius + circleB.Radius; + int vertexCount = polygonA.Vertices.Count; + + for (int i = 0; i < vertexCount; ++i) + { + Vector2 value1 = polygonA.Normals[i]; + Vector2 value2 = cLocal - polygonA.Vertices[i]; + float s = value1.X * value2.X + value1.Y * value2.Y; + + if (s > radius) + { + // Early out. + return; + } + + if (s > separation) + { + separation = s; + normalIndex = i; + } + } + + // Vertices that subtend the incident face. + int vertIndex1 = normalIndex; + int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0; + Vector2 v1 = polygonA.Vertices[vertIndex1]; + Vector2 v2 = polygonA.Vertices[vertIndex2]; + + // If the center is inside the polygon ... + if (separation < Settings.Epsilon) + { + manifold.PointCount = 1; + manifold.Type = ManifoldType.FaceA; + manifold.LocalNormal = polygonA.Normals[normalIndex]; + manifold.LocalPoint = 0.5f * (v1 + v2); + + ManifoldPoint p0 = manifold.Points[0]; + + p0.LocalPoint = circleB.Position; + p0.Id.Key = 0; + + manifold.Points[0] = p0; + + return; + } + + // Compute barycentric coordinates + float u1 = (cLocal.X - v1.X) * (v2.X - v1.X) + (cLocal.Y - v1.Y) * (v2.Y - v1.Y); + float u2 = (cLocal.X - v2.X) * (v1.X - v2.X) + (cLocal.Y - v2.Y) * (v1.Y - v2.Y); + + if (u1 <= 0.0f) + { + float r = (cLocal.X - v1.X) * (cLocal.X - v1.X) + (cLocal.Y - v1.Y) * (cLocal.Y - v1.Y); + if (r > radius * radius) + { + return; + } + + manifold.PointCount = 1; + manifold.Type = ManifoldType.FaceA; + manifold.LocalNormal = cLocal - v1; + float factor = 1f / + (float) + Math.Sqrt(manifold.LocalNormal.X * manifold.LocalNormal.X + + manifold.LocalNormal.Y * manifold.LocalNormal.Y); + manifold.LocalNormal.X = manifold.LocalNormal.X * factor; + manifold.LocalNormal.Y = manifold.LocalNormal.Y * factor; + manifold.LocalPoint = v1; + + ManifoldPoint p0b = manifold.Points[0]; + + p0b.LocalPoint = circleB.Position; + p0b.Id.Key = 0; + + manifold.Points[0] = p0b; + } + else if (u2 <= 0.0f) + { + float r = (cLocal.X - v2.X) * (cLocal.X - v2.X) + (cLocal.Y - v2.Y) * (cLocal.Y - v2.Y); + if (r > radius * radius) + { + return; + } + + manifold.PointCount = 1; + manifold.Type = ManifoldType.FaceA; + manifold.LocalNormal = cLocal - v2; + float factor = 1f / + (float) + Math.Sqrt(manifold.LocalNormal.X * manifold.LocalNormal.X + + manifold.LocalNormal.Y * manifold.LocalNormal.Y); + manifold.LocalNormal.X = manifold.LocalNormal.X * factor; + manifold.LocalNormal.Y = manifold.LocalNormal.Y * factor; + manifold.LocalPoint = v2; + + ManifoldPoint p0c = manifold.Points[0]; + + p0c.LocalPoint = circleB.Position; + p0c.Id.Key = 0; + + manifold.Points[0] = p0c; + } + else + { + Vector2 faceCenter = 0.5f * (v1 + v2); + Vector2 value1 = cLocal - faceCenter; + Vector2 value2 = polygonA.Normals[vertIndex1]; + float separation2 = value1.X * value2.X + value1.Y * value2.Y; + if (separation2 > radius) + { + return; + } + + manifold.PointCount = 1; + manifold.Type = ManifoldType.FaceA; + manifold.LocalNormal = polygonA.Normals[vertIndex1]; + manifold.LocalPoint = faceCenter; + + ManifoldPoint p0d = manifold.Points[0]; + + p0d.LocalPoint = circleB.Position; + p0d.Id.Key = 0; + + manifold.Points[0] = p0d; + } + } + + /// + /// Compute the collision manifold between two polygons. + /// + /// The manifold. + /// The poly A. + /// The transform A. + /// The poly B. + /// The transform B. + public static void CollidePolygons(ref Manifold manifold, PolygonShape polyA, ref Transform transformA, PolygonShape polyB, ref Transform transformB) + { + manifold.PointCount = 0; + float totalRadius = polyA.Radius + polyB.Radius; + + int edgeA = 0; + float separationA = FindMaxSeparation(out edgeA, polyA, ref transformA, polyB, ref transformB); + if (separationA > totalRadius) + return; + + int edgeB = 0; + float separationB = FindMaxSeparation(out edgeB, polyB, ref transformB, polyA, ref transformA); + if (separationB > totalRadius) + return; + + PolygonShape poly1; // reference polygon + PolygonShape poly2; // incident polygon + Transform xf1, xf2; + int edge1; // reference edge + bool flip; + const float k_relativeTol = 0.98f; + const float k_absoluteTol = 0.001f; + + if (separationB > k_relativeTol * separationA + k_absoluteTol) + { + poly1 = polyB; + poly2 = polyA; + xf1 = transformB; + xf2 = transformA; + edge1 = edgeB; + manifold.Type = ManifoldType.FaceB; + flip = true; + } + else + { + poly1 = polyA; + poly2 = polyB; + xf1 = transformA; + xf2 = transformB; + edge1 = edgeA; + manifold.Type = ManifoldType.FaceA; + flip = false; + } + + FixedArray2 incidentEdge; + FindIncidentEdge(out incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2); + + int count1 = poly1.Vertices.Count; + + int iv1 = edge1; + int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; + + Vector2 v11 = poly1.Vertices[iv1]; + Vector2 v12 = poly1.Vertices[iv2]; + + Vector2 localTangent = v12 - v11; + localTangent.Normalize(); + + Vector2 localNormal = new Vector2(localTangent.Y, -localTangent.X); + Vector2 planePoint = 0.5f * (v11 + v12); + + Vector2 tangent = MathUtils.Mul(xf1.q, localTangent); + + float normalx = tangent.Y; + float normaly = -tangent.X; + + v11 = MathUtils.Mul(ref xf1, v11); + v12 = MathUtils.Mul(ref xf1, v12); + + // Face offset. + float frontOffset = normalx * v11.X + normaly * v11.Y; + + // Side offsets, extended by polytope skin thickness. + float sideOffset1 = -(tangent.X * v11.X + tangent.Y * v11.Y) + totalRadius; + float sideOffset2 = tangent.X * v12.X + tangent.Y * v12.Y + totalRadius; + + // Clip incident edge against extruded edge1 side edges. + FixedArray2 clipPoints1; + FixedArray2 clipPoints2; + + // Clip to box side 1 + int np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1, iv1); + + if (np < 2) + return; + + // Clip to negative box side 1 + np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2, iv2); + + if (np < 2) + { + return; + } + + // Now clipPoints2 contains the clipped points. + manifold.LocalNormal = localNormal; + manifold.LocalPoint = planePoint; + + int pointCount = 0; + for (int i = 0; i < Settings.MaxManifoldPoints; ++i) + { + Vector2 value = clipPoints2[i].V; + float separation = normalx * value.X + normaly * value.Y - frontOffset; + + if (separation <= totalRadius) + { + ManifoldPoint cp = manifold.Points[pointCount]; + cp.LocalPoint = MathUtils.MulT(ref xf2, clipPoints2[i].V); + cp.Id = clipPoints2[i].ID; + + if (flip) + { + // Swap features + ContactFeature cf = cp.Id.Features; + cp.Id.Features.IndexA = cf.IndexB; + cp.Id.Features.IndexB = cf.IndexA; + cp.Id.Features.TypeA = cf.TypeB; + cp.Id.Features.TypeB = cf.TypeA; + } + + manifold.Points[pointCount] = cp; + + ++pointCount; + } + } + + manifold.PointCount = pointCount; + } + + /// + /// Compute contact points for edge versus circle. + /// This accounts for edge connectivity. + /// + /// The manifold. + /// The edge A. + /// The transform A. + /// The circle B. + /// The transform B. + public static void CollideEdgeAndCircle(ref Manifold manifold, EdgeShape edgeA, ref Transform transformA, CircleShape circleB, ref Transform transformB) + { + manifold.PointCount = 0; + + // Compute circle in frame of edge + Vector2 Q = MathUtils.MulT(ref transformA, MathUtils.Mul(ref transformB, ref circleB._position)); + + Vector2 A = edgeA.Vertex1, B = edgeA.Vertex2; + Vector2 e = B - A; + + // Barycentric coordinates + float u = Vector2.Dot(e, B - Q); + float v = Vector2.Dot(e, Q - A); + + float radius = edgeA.Radius + circleB.Radius; + + ContactFeature cf; + cf.IndexB = 0; + cf.TypeB = (byte)ContactFeatureType.Vertex; + + Vector2 P, d; + + // Region A + if (v <= 0.0f) + { + P = A; + d = Q - P; + float dd; + Vector2.Dot(ref d, ref d, out dd); + if (dd > radius * radius) + { + return; + } + + // Is there an edge connected to A? + if (edgeA.HasVertex0) + { + Vector2 A1 = edgeA.Vertex0; + Vector2 B1 = A; + Vector2 e1 = B1 - A1; + float u1 = Vector2.Dot(e1, B1 - Q); + + // Is the circle in Region AB of the previous edge? + if (u1 > 0.0f) + { + return; + } + } + + cf.IndexA = 0; + cf.TypeA = (byte)ContactFeatureType.Vertex; + manifold.PointCount = 1; + manifold.Type = ManifoldType.Circles; + manifold.LocalNormal = Vector2.Zero; + manifold.LocalPoint = P; + ManifoldPoint mp = new ManifoldPoint(); + mp.Id.Key = 0; + mp.Id.Features = cf; + mp.LocalPoint = circleB.Position; + manifold.Points[0] = mp; + return; + } + + // Region B + if (u <= 0.0f) + { + P = B; + d = Q - P; + float dd; + Vector2.Dot(ref d, ref d, out dd); + if (dd > radius * radius) + { + return; + } + + // Is there an edge connected to B? + if (edgeA.HasVertex3) + { + Vector2 B2 = edgeA.Vertex3; + Vector2 A2 = B; + Vector2 e2 = B2 - A2; + float v2 = Vector2.Dot(e2, Q - A2); + + // Is the circle in Region AB of the next edge? + if (v2 > 0.0f) + { + return; + } + } + + cf.IndexA = 1; + cf.TypeA = (byte)ContactFeatureType.Vertex; + manifold.PointCount = 1; + manifold.Type = ManifoldType.Circles; + manifold.LocalNormal = Vector2.Zero; + manifold.LocalPoint = P; + ManifoldPoint mp = new ManifoldPoint(); + mp.Id.Key = 0; + mp.Id.Features = cf; + mp.LocalPoint = circleB.Position; + manifold.Points[0] = mp; + return; + } + + // Region AB + float den; + Vector2.Dot(ref e, ref e, out den); + Debug.Assert(den > 0.0f); + P = (1.0f / den) * (u * A + v * B); + d = Q - P; + float dd2; + Vector2.Dot(ref d, ref d, out dd2); + if (dd2 > radius * radius) + { + return; + } + + Vector2 n = new Vector2(-e.Y, e.X); + if (Vector2.Dot(n, Q - A) < 0.0f) + { + n = new Vector2(-n.X, -n.Y); + } + n.Normalize(); + + cf.IndexA = 0; + cf.TypeA = (byte)ContactFeatureType.Face; + manifold.PointCount = 1; + manifold.Type = ManifoldType.FaceA; + manifold.LocalNormal = n; + manifold.LocalPoint = A; + ManifoldPoint mp2 = new ManifoldPoint(); + mp2.Id.Key = 0; + mp2.Id.Features = cf; + mp2.LocalPoint = circleB.Position; + manifold.Points[0] = mp2; + } + + /// + /// Collides and edge and a polygon, taking into account edge adjacency. + /// + /// The manifold. + /// The edge A. + /// The xf A. + /// The polygon B. + /// The xf B. + public static void CollideEdgeAndPolygon(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB) + { + EPCollider collider = new EPCollider(); + collider.Collide(ref manifold, edgeA, ref xfA, polygonB, ref xfB); + } + + private class EPCollider + { + private TempPolygon _polygonB = new TempPolygon(); + + Transform _xf; + Vector2 _centroidB; + Vector2 _v0, _v1, _v2, _v3; + Vector2 _normal0, _normal1, _normal2; + Vector2 _normal; + Vector2 _lowerLimit, _upperLimit; + float _radius; + bool _front; + + public void Collide(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB) + { + // Algorithm: + // 1. Classify v1 and v2 + // 2. Classify polygon centroid as front or back + // 3. Flip normal if necessary + // 4. Initialize normal range to [-pi, pi] about face normal + // 5. Adjust normal range according to adjacent edges + // 6. Visit each separating axes, only accept axes within the range + // 7. Return if _any_ axis indicates separation + // 8. Clip + + _xf = MathUtils.MulT(xfA, xfB); + + _centroidB = MathUtils.Mul(ref _xf, polygonB.MassData.Centroid); + + _v0 = edgeA.Vertex0; + _v1 = edgeA._vertex1; + _v2 = edgeA._vertex2; + _v3 = edgeA.Vertex3; + + bool hasVertex0 = edgeA.HasVertex0; + bool hasVertex3 = edgeA.HasVertex3; + + Vector2 edge1 = _v2 - _v1; + edge1.Normalize(); + _normal1 = new Vector2(edge1.Y, -edge1.X); + float offset1 = Vector2.Dot(_normal1, _centroidB - _v1); + float offset0 = 0.0f, offset2 = 0.0f; + bool convex1 = false, convex2 = false; + + // Is there a preceding edge? + if (hasVertex0) + { + Vector2 edge0 = _v1 - _v0; + edge0.Normalize(); + _normal0 = new Vector2(edge0.Y, -edge0.X); + convex1 = MathUtils.Cross(edge0, edge1) >= 0.0f; + offset0 = Vector2.Dot(_normal0, _centroidB - _v0); + } + + // Is there a following edge? + if (hasVertex3) + { + Vector2 edge2 = _v3 - _v2; + edge2.Normalize(); + _normal2 = new Vector2(edge2.Y, -edge2.X); + convex2 = MathUtils.Cross(edge1, edge2) > 0.0f; + offset2 = Vector2.Dot(_normal2, _centroidB - _v2); + } + + // Determine front or back collision. Determine collision normal limits. + if (hasVertex0 && hasVertex3) + { + if (convex1 && convex2) + { + _front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f; + if (_front) + { + _normal = _normal1; + _lowerLimit = _normal0; + _upperLimit = _normal2; + } + else + { + _normal = -_normal1; + _lowerLimit = -_normal1; + _upperLimit = -_normal1; + } + } + else if (convex1) + { + _front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f); + if (_front) + { + _normal = _normal1; + _lowerLimit = _normal0; + _upperLimit = _normal1; + } + else + { + _normal = -_normal1; + _lowerLimit = -_normal2; + _upperLimit = -_normal1; + } + } + else if (convex2) + { + _front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f); + if (_front) + { + _normal = _normal1; + _lowerLimit = _normal1; + _upperLimit = _normal2; + } + else + { + _normal = -_normal1; + _lowerLimit = -_normal1; + _upperLimit = -_normal0; + } + } + else + { + _front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f; + if (_front) + { + _normal = _normal1; + _lowerLimit = _normal1; + _upperLimit = _normal1; + } + else + { + _normal = -_normal1; + _lowerLimit = -_normal2; + _upperLimit = -_normal0; + } + } + } + else if (hasVertex0) + { + if (convex1) + { + _front = offset0 >= 0.0f || offset1 >= 0.0f; + if (_front) + { + _normal = _normal1; + _lowerLimit = _normal0; + _upperLimit = -_normal1; + } + else + { + _normal = -_normal1; + _lowerLimit = _normal1; + _upperLimit = -_normal1; + } + } + else + { + _front = offset0 >= 0.0f && offset1 >= 0.0f; + if (_front) + { + _normal = _normal1; + _lowerLimit = _normal1; + _upperLimit = -_normal1; + } + else + { + _normal = -_normal1; + _lowerLimit = _normal1; + _upperLimit = -_normal0; + } + } + } + else if (hasVertex3) + { + if (convex2) + { + _front = offset1 >= 0.0f || offset2 >= 0.0f; + if (_front) + { + _normal = _normal1; + _lowerLimit = -_normal1; + _upperLimit = _normal2; + } + else + { + _normal = -_normal1; + _lowerLimit = -_normal1; + _upperLimit = _normal1; + } + } + else + { + _front = offset1 >= 0.0f && offset2 >= 0.0f; + if (_front) + { + _normal = _normal1; + _lowerLimit = -_normal1; + _upperLimit = _normal1; + } + else + { + _normal = -_normal1; + _lowerLimit = -_normal2; + _upperLimit = _normal1; + } + } + } + else + { + _front = offset1 >= 0.0f; + if (_front) + { + _normal = _normal1; + _lowerLimit = -_normal1; + _upperLimit = -_normal1; + } + else + { + _normal = -_normal1; + _lowerLimit = _normal1; + _upperLimit = _normal1; + } + } + + // Get polygonB in frameA + _polygonB.Count = polygonB.Vertices.Count; + for (int i = 0; i < polygonB.Vertices.Count; ++i) + { + _polygonB.Vertices[i] = MathUtils.Mul(ref _xf, polygonB.Vertices[i]); + _polygonB.Normals[i] = MathUtils.Mul(_xf.q, polygonB.Normals[i]); + } + + _radius = 2.0f * Settings.PolygonRadius; + + manifold.PointCount = 0; + + EPAxis edgeAxis = ComputeEdgeSeparation(); + + // If no valid normal can be found than this edge should not collide. + if (edgeAxis.Type == EPAxisType.Unknown) + { + return; + } + + if (edgeAxis.Separation > _radius) + { + return; + } + + EPAxis polygonAxis = ComputePolygonSeparation(); + if (polygonAxis.Type != EPAxisType.Unknown && polygonAxis.Separation > _radius) + { + return; + } + + // Use hysteresis for jitter reduction. + const float k_relativeTol = 0.98f; + const float k_absoluteTol = 0.001f; + + EPAxis primaryAxis; + if (polygonAxis.Type == EPAxisType.Unknown) + { + primaryAxis = edgeAxis; + } + else if (polygonAxis.Separation > k_relativeTol * edgeAxis.Separation + k_absoluteTol) + { + primaryAxis = polygonAxis; + } + else + { + primaryAxis = edgeAxis; + } + + FixedArray2 ie = new FixedArray2(); + ReferenceFace rf; + if (primaryAxis.Type == EPAxisType.EdgeA) + { + manifold.Type = ManifoldType.FaceA; + + // Search for the polygon normal that is most anti-parallel to the edge normal. + int bestIndex = 0; + float bestValue = Vector2.Dot(_normal, _polygonB.Normals[0]); + for (int i = 1; i < _polygonB.Count; ++i) + { + float value = Vector2.Dot(_normal, _polygonB.Normals[i]); + if (value < bestValue) + { + bestValue = value; + bestIndex = i; + } + } + + int i1 = bestIndex; + int i2 = i1 + 1 < _polygonB.Count ? i1 + 1 : 0; + + ClipVertex c0 = ie[0]; + c0.V = _polygonB.Vertices[i1]; + c0.ID.Features.IndexA = 0; + c0.ID.Features.IndexB = (byte)i1; + c0.ID.Features.TypeA = (byte)ContactFeatureType.Face; + c0.ID.Features.TypeB = (byte)ContactFeatureType.Vertex; + ie[0] = c0; + + ClipVertex c1 = ie[1]; + c1.V = _polygonB.Vertices[i2]; + c1.ID.Features.IndexA = 0; + c1.ID.Features.IndexB = (byte)i2; + c1.ID.Features.TypeA = (byte)ContactFeatureType.Face; + c1.ID.Features.TypeB = (byte)ContactFeatureType.Vertex; + ie[1] = c1; + + if (_front) + { + rf.i1 = 0; + rf.i2 = 1; + rf.v1 = _v1; + rf.v2 = _v2; + rf.normal = _normal1; + } + else + { + rf.i1 = 1; + rf.i2 = 0; + rf.v1 = _v2; + rf.v2 = _v1; + rf.normal = -_normal1; + } + } + else + { + manifold.Type = ManifoldType.FaceB; + ClipVertex c0 = ie[0]; + c0.V = _v1; + c0.ID.Features.IndexA = 0; + c0.ID.Features.IndexB = (byte)primaryAxis.Index; + c0.ID.Features.TypeA = (byte)ContactFeatureType.Vertex; + c0.ID.Features.TypeB = (byte)ContactFeatureType.Face; + ie[0] = c0; + + ClipVertex c1 = ie[1]; + c1.V = _v2; + c1.ID.Features.IndexA = 0; + c1.ID.Features.IndexB = (byte)primaryAxis.Index; + c1.ID.Features.TypeA = (byte)ContactFeatureType.Vertex; + c1.ID.Features.TypeB = (byte)ContactFeatureType.Face; + ie[1] = c1; + + rf.i1 = primaryAxis.Index; + rf.i2 = rf.i1 + 1 < _polygonB.Count ? rf.i1 + 1 : 0; + rf.v1 = _polygonB.Vertices[rf.i1]; + rf.v2 = _polygonB.Vertices[rf.i2]; + rf.normal = _polygonB.Normals[rf.i1]; + } + + rf.sideNormal1 = new Vector2(rf.normal.Y, -rf.normal.X); + rf.sideNormal2 = -rf.sideNormal1; + rf.sideOffset1 = Vector2.Dot(rf.sideNormal1, rf.v1); + rf.sideOffset2 = Vector2.Dot(rf.sideNormal2, rf.v2); + + // Clip incident edge against extruded edge1 side edges. + FixedArray2 clipPoints1; + FixedArray2 clipPoints2; + int np; + + // Clip to box side 1 + np = ClipSegmentToLine(out clipPoints1, ref ie, rf.sideNormal1, rf.sideOffset1, rf.i1); + + if (np < Settings.MaxManifoldPoints) + { + return; + } + + // Clip to negative box side 1 + np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2); + + if (np < Settings.MaxManifoldPoints) + { + return; + } + + // Now clipPoints2 contains the clipped points. + if (primaryAxis.Type == EPAxisType.EdgeA) + { + manifold.LocalNormal = rf.normal; + manifold.LocalPoint = rf.v1; + } + else + { + manifold.LocalNormal = polygonB.Normals[rf.i1]; + manifold.LocalPoint = polygonB.Vertices[rf.i1]; + } + + int pointCount = 0; + for (int i = 0; i < Settings.MaxManifoldPoints; ++i) + { + float separation = Vector2.Dot(rf.normal, clipPoints2[i].V - rf.v1); + + if (separation <= _radius) + { + ManifoldPoint cp = manifold.Points[pointCount]; + + if (primaryAxis.Type == EPAxisType.EdgeA) + { + cp.LocalPoint = MathUtils.MulT(ref _xf, clipPoints2[i].V); + cp.Id = clipPoints2[i].ID; + } + else + { + cp.LocalPoint = clipPoints2[i].V; + cp.Id.Features.TypeA = clipPoints2[i].ID.Features.TypeB; + cp.Id.Features.TypeB = clipPoints2[i].ID.Features.TypeA; + cp.Id.Features.IndexA = clipPoints2[i].ID.Features.IndexB; + cp.Id.Features.IndexB = clipPoints2[i].ID.Features.IndexA; + } + + manifold.Points[pointCount] = cp; + ++pointCount; + } + } + + manifold.PointCount = pointCount; + } + + private EPAxis ComputeEdgeSeparation() + { + EPAxis axis; + axis.Type = EPAxisType.EdgeA; + axis.Index = _front ? 0 : 1; + axis.Separation = Settings.MaxFloat; + + for (int i = 0; i < _polygonB.Count; ++i) + { + float s = Vector2.Dot(_normal, _polygonB.Vertices[i] - _v1); + if (s < axis.Separation) + { + axis.Separation = s; + } + } + + return axis; + } + + private EPAxis ComputePolygonSeparation() + { + EPAxis axis; + axis.Type = EPAxisType.Unknown; + axis.Index = -1; + axis.Separation = -Settings.MaxFloat; + + Vector2 perp = new Vector2(-_normal.Y, _normal.X); + + for (int i = 0; i < _polygonB.Count; ++i) + { + Vector2 n = -_polygonB.Normals[i]; + + float s1 = Vector2.Dot(n, _polygonB.Vertices[i] - _v1); + float s2 = Vector2.Dot(n, _polygonB.Vertices[i] - _v2); + float s = Math.Min(s1, s2); + + if (s > _radius) + { + // No collision + axis.Type = EPAxisType.EdgeB; + axis.Index = i; + axis.Separation = s; + return axis; + } + + // Adjacency + if (Vector2.Dot(n, perp) >= 0.0f) + { + if (Vector2.Dot(n - _upperLimit, _normal) < -Settings.AngularSlop) + { + continue; + } + } + else + { + if (Vector2.Dot(n - _lowerLimit, _normal) < -Settings.AngularSlop) + { + continue; + } + } + + if (s > axis.Separation) + { + axis.Type = EPAxisType.EdgeB; + axis.Index = i; + axis.Separation = s; + } + } + + return axis; + } + } + + /// + /// Clipping for contact manifolds. + /// + /// The v out. + /// The v in. + /// The normal. + /// The offset. + /// The vertex index A. + /// + private static int ClipSegmentToLine(out FixedArray2 vOut, ref FixedArray2 vIn, Vector2 normal, float offset, int vertexIndexA) + { + vOut = new FixedArray2(); + + ClipVertex v0 = vIn[0]; + ClipVertex v1 = vIn[1]; + + // Start with no output points + int numOut = 0; + + // Calculate the distance of end points to the line + float distance0 = normal.X * v0.V.X + normal.Y * v0.V.Y - offset; + float distance1 = normal.X * v1.V.X + normal.Y * v1.V.Y - offset; + + // If the points are behind the plane + if (distance0 <= 0.0f) vOut[numOut++] = v0; + if (distance1 <= 0.0f) vOut[numOut++] = v1; + + // If the points are on different sides of the plane + if (distance0 * distance1 < 0.0f) + { + // Find intersection point of edge and plane + float interp = distance0 / (distance0 - distance1); + + ClipVertex cv = vOut[numOut]; + + cv.V.X = v0.V.X + interp * (v1.V.X - v0.V.X); + cv.V.Y = v0.V.Y + interp * (v1.V.Y - v0.V.Y); + + // VertexA is hitting edgeB. + cv.ID.Features.IndexA = (byte)vertexIndexA; + cv.ID.Features.IndexB = v0.ID.Features.IndexB; + cv.ID.Features.TypeA = (byte)ContactFeatureType.Vertex; + cv.ID.Features.TypeB = (byte)ContactFeatureType.Face; + + vOut[numOut] = cv; + + ++numOut; + } + + return numOut; + } + + /// + /// Find the separation between poly1 and poly2 for a give edge normal on poly1. + /// + /// The poly1. + /// The XF1. + /// The edge1. + /// The poly2. + /// The XF2. + /// + private static float EdgeSeparation(PolygonShape poly1, ref Transform xf1, int edge1, PolygonShape poly2, ref Transform xf2) + { + List vertices1 = poly1.Vertices; + List normals1 = poly1.Normals; + + int count2 = poly2.Vertices.Count; + List vertices2 = poly2.Vertices; + + Debug.Assert(0 <= edge1 && edge1 < poly1.Vertices.Count); + + // Convert normal from poly1's frame into poly2's frame. + Vector2 normal1World = MathUtils.Mul(xf1.q, normals1[edge1]); + Vector2 normal1 = MathUtils.MulT(xf2.q, normal1World); + + // Find support vertex on poly2 for -normal. + int index = 0; + float minDot = Settings.MaxFloat; + + for (int i = 0; i < count2; ++i) + { + float dot = Vector2.Dot(vertices2[i], normal1); + if (dot < minDot) + { + minDot = dot; + index = i; + } + } + + Vector2 v1 = MathUtils.Mul(ref xf1, vertices1[edge1]); + Vector2 v2 = MathUtils.Mul(ref xf2, vertices2[index]); + float separation = Vector2.Dot(v2 - v1, normal1World); + return separation; + } + + /// + /// Find the max separation between poly1 and poly2 using edge normals from poly1. + /// + /// Index of the edge. + /// The poly1. + /// The XF1. + /// The poly2. + /// The XF2. + /// + private static float FindMaxSeparation(out int edgeIndex, PolygonShape poly1, ref Transform xf1, PolygonShape poly2, ref Transform xf2) + { + int count1 = poly1.Vertices.Count; + List normals1 = poly1.Normals; + + // Vector pointing from the centroid of poly1 to the centroid of poly2. + Vector2 d = MathUtils.Mul(ref xf2, poly2.MassData.Centroid) - MathUtils.Mul(ref xf1, poly1.MassData.Centroid); + Vector2 dLocal1 = MathUtils.MulT(xf1.q, d); + + // Find edge normal on poly1 that has the largest projection onto d. + int edge = 0; + float maxDot = -Settings.MaxFloat; + for (int i = 0; i < count1; ++i) + { + float dot = Vector2.Dot(normals1[i], dLocal1); + if (dot > maxDot) + { + maxDot = dot; + edge = i; + } + } + + // Get the separation for the edge normal. + float s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2); + + // Check the separation for the previous edge normal. + int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1; + float sPrev = EdgeSeparation(poly1, ref xf1, prevEdge, poly2, ref xf2); + + // Check the separation for the next edge normal. + int nextEdge = edge + 1 < count1 ? edge + 1 : 0; + float sNext = EdgeSeparation(poly1, ref xf1, nextEdge, poly2, ref xf2); + + // Find the best edge and the search direction. + int bestEdge; + float bestSeparation; + int increment; + if (sPrev > s && sPrev > sNext) + { + increment = -1; + bestEdge = prevEdge; + bestSeparation = sPrev; + } + else if (sNext > s) + { + increment = 1; + bestEdge = nextEdge; + bestSeparation = sNext; + } + else + { + edgeIndex = edge; + return s; + } + + // Perform a local search for the best edge normal. + for (; ; ) + { + if (increment == -1) + edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1; + else + edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0; + + s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2); + + if (s > bestSeparation) + { + bestEdge = edge; + bestSeparation = s; + } + else + { + break; + } + } + + edgeIndex = bestEdge; + return bestSeparation; + } + + private static void FindIncidentEdge(out FixedArray2 c, PolygonShape poly1, ref Transform xf1, int edge1, PolygonShape poly2, ref Transform xf2) + { + c = new FixedArray2(); + Vertices normals1 = poly1.Normals; + + int count2 = poly2.Vertices.Count; + Vertices vertices2 = poly2.Vertices; + Vertices normals2 = poly2.Normals; + + Debug.Assert(0 <= edge1 && edge1 < poly1.Vertices.Count); + + // Get the normal of the reference edge in poly2's frame. + Vector2 normal1 = MathUtils.MulT(xf2.q, MathUtils.Mul(xf1.q, normals1[edge1])); + + + // Find the incident edge on poly2. + int index = 0; + float minDot = Settings.MaxFloat; + for (int i = 0; i < count2; ++i) + { + float dot = Vector2.Dot(normal1, normals2[i]); + if (dot < minDot) + { + minDot = dot; + index = i; + } + } + + // Build the clip vertices for the incident edge. + int i1 = index; + int i2 = i1 + 1 < count2 ? i1 + 1 : 0; + + ClipVertex cv0 = c[0]; + + cv0.V = MathUtils.Mul(ref xf2, vertices2[i1]); + cv0.ID.Features.IndexA = (byte)edge1; + cv0.ID.Features.IndexB = (byte)i1; + cv0.ID.Features.TypeA = (byte)ContactFeatureType.Face; + cv0.ID.Features.TypeB = (byte)ContactFeatureType.Vertex; + + c[0] = cv0; + + ClipVertex cv1 = c[1]; + cv1.V = MathUtils.Mul(ref xf2, vertices2[i2]); + cv1.ID.Features.IndexA = (byte)edge1; + cv1.ID.Features.IndexB = (byte)i2; + cv1.ID.Features.TypeA = (byte)ContactFeatureType.Face; + cv1.ID.Features.TypeB = (byte)ContactFeatureType.Vertex; + + c[1] = cv1; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Collision/Distance.cs b/Farseer Physics Engine 3.5/Collision/Distance.cs new file mode 100644 index 000000000..058fc802f --- /dev/null +++ b/Farseer Physics Engine 3.5/Collision/Distance.cs @@ -0,0 +1,794 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using FarseerPhysics.Collision.Shapes; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Collision +{ + /// + /// A distance proxy is used by the GJK algorithm. + /// It encapsulates any shape. + /// + public class DistanceProxy + { + internal float Radius; + internal Vertices Vertices = new Vertices(); + + // GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates. + + /// + /// Initialize the proxy using the given shape. The shape + /// must remain in scope while the proxy is in use. + /// + /// The shape. + /// The index. + public void Set(Shape shape, int index) + { + switch (shape.ShapeType) + { + case ShapeType.Circle: + { + CircleShape circle = (CircleShape)shape; + Vertices.Clear(); + Vertices.Add(circle.Position); + Radius = circle.Radius; + } + break; + + case ShapeType.Polygon: + { + PolygonShape polygon = (PolygonShape)shape; + Vertices.Clear(); + for (int i = 0; i < polygon.Vertices.Count; i++) + { + Vertices.Add(polygon.Vertices[i]); + } + Radius = polygon.Radius; + } + break; + + case ShapeType.Chain: + { + ChainShape chain = (ChainShape)shape; + Debug.Assert(0 <= index && index < chain.Vertices.Count); + Vertices.Clear(); + Vertices.Add(chain.Vertices[index]); + Vertices.Add(index + 1 < chain.Vertices.Count ? chain.Vertices[index + 1] : chain.Vertices[0]); + + Radius = chain.Radius; + } + break; + + case ShapeType.Edge: + { + EdgeShape edge = (EdgeShape)shape; + Vertices.Clear(); + Vertices.Add(edge.Vertex1); + Vertices.Add(edge.Vertex2); + Radius = edge.Radius; + } + break; + + default: + Debug.Assert(false); + break; + } + } + + /// + /// Get the supporting vertex index in the given direction. + /// + /// The direction. + /// + public int GetSupport(Vector2 direction) + { + int bestIndex = 0; + float bestValue = Vector2.Dot(Vertices[0], direction); + for (int i = 1; i < Vertices.Count; ++i) + { + float value = Vector2.Dot(Vertices[i], direction); + if (value > bestValue) + { + bestIndex = i; + bestValue = value; + } + } + + return bestIndex; + } + + /// + /// Get the supporting vertex in the given direction. + /// + /// The direction. + /// + public Vector2 GetSupportVertex(Vector2 direction) + { + int bestIndex = 0; + float bestValue = Vector2.Dot(Vertices[0], direction); + for (int i = 1; i < Vertices.Count; ++i) + { + float value = Vector2.Dot(Vertices[i], direction); + if (value > bestValue) + { + bestIndex = i; + bestValue = value; + } + } + + return Vertices[bestIndex]; + } + } + + /// + /// Used to warm start ComputeDistance. + /// Set count to zero on first call. + /// + public struct SimplexCache + { + /// + /// Length or area + /// + public ushort Count; + + /// + /// Vertices on shape A + /// + public FixedArray3 IndexA; + + /// + /// Vertices on shape B + /// + public FixedArray3 IndexB; + + public float Metric; + } + + /// + /// Input for Distance.ComputeDistance(). + /// You have to option to use the shape radii in the computation. + /// + public class DistanceInput + { + public DistanceProxy ProxyA = new DistanceProxy(); + public DistanceProxy ProxyB = new DistanceProxy(); + public Transform TransformA; + public Transform TransformB; + public bool UseRadii; + } + + /// + /// Output for Distance.ComputeDistance(). + /// + public struct DistanceOutput + { + public float Distance; + + /// + /// Number of GJK iterations used + /// + public int Iterations; + + /// + /// Closest point on shapeA + /// + public Vector2 PointA; + + /// + /// Closest point on shapeB + /// + public Vector2 PointB; + } + + internal struct SimplexVertex + { + /// + /// Barycentric coordinate for closest point + /// + public float A; + + /// + /// wA index + /// + public int IndexA; + + /// + /// wB index + /// + public int IndexB; + + /// + /// wB - wA + /// + public Vector2 W; + + /// + /// Support point in proxyA + /// + public Vector2 WA; + + /// + /// Support point in proxyB + /// + public Vector2 WB; + } + + internal struct Simplex + { + internal int Count; + internal FixedArray3 V; + + internal void ReadCache(ref SimplexCache cache, DistanceProxy proxyA, ref Transform transformA, DistanceProxy proxyB, ref Transform transformB) + { + Debug.Assert(cache.Count <= 3); + + // Copy data from cache. + Count = cache.Count; + for (int i = 0; i < Count; ++i) + { + SimplexVertex v = V[i]; + v.IndexA = cache.IndexA[i]; + v.IndexB = cache.IndexB[i]; + Vector2 wALocal = proxyA.Vertices[v.IndexA]; + Vector2 wBLocal = proxyB.Vertices[v.IndexB]; + v.WA = MathUtils.Mul(ref transformA, wALocal); + v.WB = MathUtils.Mul(ref transformB, wBLocal); + v.W = v.WB - v.WA; + v.A = 0.0f; + V[i] = v; + } + + // Compute the new simplex metric, if it is substantially different than + // old metric then flush the simplex. + if (Count > 1) + { + float metric1 = cache.Metric; + float metric2 = GetMetric(); + if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.Epsilon) + { + // Reset the simplex. + Count = 0; + } + } + + // If the cache is empty or invalid ... + if (Count == 0) + { + SimplexVertex v = V[0]; + v.IndexA = 0; + v.IndexB = 0; + Vector2 wALocal = proxyA.Vertices[0]; + Vector2 wBLocal = proxyB.Vertices[0]; + v.WA = MathUtils.Mul(ref transformA, wALocal); + v.WB = MathUtils.Mul(ref transformB, wBLocal); + v.W = v.WB - v.WA; + v.A = 1.0f; + V[0] = v; + Count = 1; + } + } + + internal void WriteCache(ref SimplexCache cache) + { + cache.Metric = GetMetric(); + cache.Count = (UInt16)Count; + for (int i = 0; i < Count; ++i) + { + cache.IndexA[i] = (byte)(V[i].IndexA); + cache.IndexB[i] = (byte)(V[i].IndexB); + } + } + + internal Vector2 GetSearchDirection() + { + switch (Count) + { + case 1: + return -V[0].W; + + case 2: + { + Vector2 e12 = V[1].W - V[0].W; + float sgn = MathUtils.Cross(e12, -V[0].W); + if (sgn > 0.0f) + { + // Origin is left of e12. + return new Vector2(-e12.Y, e12.X); + } + else + { + // Origin is right of e12. + return new Vector2(e12.Y, -e12.X); + } + } + + default: + Debug.Assert(false); + return Vector2.Zero; + } + } + + internal Vector2 GetClosestPoint() + { + switch (Count) + { + case 0: + Debug.Assert(false); + return Vector2.Zero; + + case 1: + return V[0].W; + + case 2: + return V[0].A * V[0].W + V[1].A * V[1].W; + + case 3: + return Vector2.Zero; + + default: + Debug.Assert(false); + return Vector2.Zero; + } + } + + internal void GetWitnessPoints(out Vector2 pA, out Vector2 pB) + { + switch (Count) + { + case 0: + pA = Vector2.Zero; + pB = Vector2.Zero; + Debug.Assert(false); + break; + + case 1: + pA = V[0].WA; + pB = V[0].WB; + break; + + case 2: + pA = V[0].A * V[0].WA + V[1].A * V[1].WA; + pB = V[0].A * V[0].WB + V[1].A * V[1].WB; + break; + + case 3: + pA = V[0].A * V[0].WA + V[1].A * V[1].WA + V[2].A * V[2].WA; + pB = pA; + break; + + default: + throw new Exception(); + } + } + + internal float GetMetric() + { + switch (Count) + { + case 0: + Debug.Assert(false); + return 0.0f; + case 1: + return 0.0f; + + case 2: + return (V[0].W - V[1].W).Length(); + + case 3: + return MathUtils.Cross(V[1].W - V[0].W, V[2].W - V[0].W); + + default: + Debug.Assert(false); + return 0.0f; + } + } + + // Solve a line segment using barycentric coordinates. + // + // p = a1 * w1 + a2 * w2 + // a1 + a2 = 1 + // + // The vector from the origin to the closest point on the line is + // perpendicular to the line. + // e12 = w2 - w1 + // dot(p, e) = 0 + // a1 * dot(w1, e) + a2 * dot(w2, e) = 0 + // + // 2-by-2 linear system + // [1 1 ][a1] = [1] + // [w1.e12 w2.e12][a2] = [0] + // + // Define + // d12_1 = dot(w2, e12) + // d12_2 = -dot(w1, e12) + // d12 = d12_1 + d12_2 + // + // Solution + // a1 = d12_1 / d12 + // a2 = d12_2 / d12 + + internal void Solve2() + { + Vector2 w1 = V[0].W; + Vector2 w2 = V[1].W; + Vector2 e12 = w2 - w1; + + // w1 region + float d12_2 = -Vector2.Dot(w1, e12); + if (d12_2 <= 0.0f) + { + // a2 <= 0, so we clamp it to 0 + SimplexVertex v0 = V[0]; + v0.A = 1.0f; + V[0] = v0; + Count = 1; + return; + } + + // w2 region + float d12_1 = Vector2.Dot(w2, e12); + if (d12_1 <= 0.0f) + { + // a1 <= 0, so we clamp it to 0 + SimplexVertex v1 = V[1]; + v1.A = 1.0f; + V[1] = v1; + Count = 1; + V[0] = V[1]; + return; + } + + // Must be in e12 region. + float inv_d12 = 1.0f / (d12_1 + d12_2); + SimplexVertex v0_2 = V[0]; + SimplexVertex v1_2 = V[1]; + v0_2.A = d12_1 * inv_d12; + v1_2.A = d12_2 * inv_d12; + V[0] = v0_2; + V[1] = v1_2; + Count = 2; + } + + // Possible regions: + // - points[2] + // - edge points[0]-points[2] + // - edge points[1]-points[2] + // - inside the triangle + internal void Solve3() + { + Vector2 w1 = V[0].W; + Vector2 w2 = V[1].W; + Vector2 w3 = V[2].W; + + // Edge12 + // [1 1 ][a1] = [1] + // [w1.e12 w2.e12][a2] = [0] + // a3 = 0 + Vector2 e12 = w2 - w1; + float w1e12 = Vector2.Dot(w1, e12); + float w2e12 = Vector2.Dot(w2, e12); + float d12_1 = w2e12; + float d12_2 = -w1e12; + + // Edge13 + // [1 1 ][a1] = [1] + // [w1.e13 w3.e13][a3] = [0] + // a2 = 0 + Vector2 e13 = w3 - w1; + float w1e13 = Vector2.Dot(w1, e13); + float w3e13 = Vector2.Dot(w3, e13); + float d13_1 = w3e13; + float d13_2 = -w1e13; + + // Edge23 + // [1 1 ][a2] = [1] + // [w2.e23 w3.e23][a3] = [0] + // a1 = 0 + Vector2 e23 = w3 - w2; + float w2e23 = Vector2.Dot(w2, e23); + float w3e23 = Vector2.Dot(w3, e23); + float d23_1 = w3e23; + float d23_2 = -w2e23; + + // Triangle123 + float n123 = MathUtils.Cross(e12, e13); + + float d123_1 = n123 * MathUtils.Cross(w2, w3); + float d123_2 = n123 * MathUtils.Cross(w3, w1); + float d123_3 = n123 * MathUtils.Cross(w1, w2); + + // w1 region + if (d12_2 <= 0.0f && d13_2 <= 0.0f) + { + SimplexVertex v0_1 = V[0]; + v0_1.A = 1.0f; + V[0] = v0_1; + Count = 1; + return; + } + + // e12 + if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f) + { + float inv_d12 = 1.0f / (d12_1 + d12_2); + SimplexVertex v0_2 = V[0]; + SimplexVertex v1_2 = V[1]; + v0_2.A = d12_1 * inv_d12; + v1_2.A = d12_2 * inv_d12; + V[0] = v0_2; + V[1] = v1_2; + Count = 2; + return; + } + + // e13 + if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f) + { + float inv_d13 = 1.0f / (d13_1 + d13_2); + SimplexVertex v0_3 = V[0]; + SimplexVertex v2_3 = V[2]; + v0_3.A = d13_1 * inv_d13; + v2_3.A = d13_2 * inv_d13; + V[0] = v0_3; + V[2] = v2_3; + Count = 2; + V[1] = V[2]; + return; + } + + // w2 region + if (d12_1 <= 0.0f && d23_2 <= 0.0f) + { + SimplexVertex v1_4 = V[1]; + v1_4.A = 1.0f; + V[1] = v1_4; + Count = 1; + V[0] = V[1]; + return; + } + + // w3 region + if (d13_1 <= 0.0f && d23_1 <= 0.0f) + { + SimplexVertex v2_5 = V[2]; + v2_5.A = 1.0f; + V[2] = v2_5; + Count = 1; + V[0] = V[2]; + return; + } + + // e23 + if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f) + { + float inv_d23 = 1.0f / (d23_1 + d23_2); + SimplexVertex v1_6 = V[1]; + SimplexVertex v2_6 = V[2]; + v1_6.A = d23_1 * inv_d23; + v2_6.A = d23_2 * inv_d23; + V[1] = v1_6; + V[2] = v2_6; + Count = 2; + V[0] = V[2]; + return; + } + + // Must be in triangle123 + float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3); + SimplexVertex v0_7 = V[0]; + SimplexVertex v1_7 = V[1]; + SimplexVertex v2_7 = V[2]; + v0_7.A = d123_1 * inv_d123; + v1_7.A = d123_2 * inv_d123; + v2_7.A = d123_3 * inv_d123; + V[0] = v0_7; + V[1] = v1_7; + V[2] = v2_7; + Count = 3; + } + } + + /// + /// The Gilbert–Johnson–Keerthi distance algorithm that provides the distance between shapes. + /// + public static class Distance + { + /// + /// The number of calls made to the ComputeDistance() function. + /// Note: This is only activated when Settings.EnableDiagnostics = true + /// + [ThreadStatic] + public static int GJKCalls; + + /// + /// The number of iterations that was made on the last call to ComputeDistance(). + /// Note: This is only activated when Settings.EnableDiagnostics = true + /// + [ThreadStatic] + public static int GJKIters; + + /// + /// The maximum numer of iterations ever mae with calls to the CompteDistance() funtion. + /// Note: This is only activated when Settings.EnableDiagnostics = true + /// + [ThreadStatic] + public static int GJKMaxIters; + + public static void ComputeDistance(out DistanceOutput output, out SimplexCache cache, DistanceInput input) + { + cache = new SimplexCache(); + + if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled + ++GJKCalls; + + // Initialize the simplex. + Simplex simplex = new Simplex(); + simplex.ReadCache(ref cache, input.ProxyA, ref input.TransformA, input.ProxyB, ref input.TransformB); + + // These store the vertices of the last simplex so that we + // can check for duplicates and prevent cycling. + FixedArray3 saveA = new FixedArray3(); + FixedArray3 saveB = new FixedArray3(); + + //float distanceSqr1 = Settings.MaxFloat; + + // Main iteration loop. + int iter = 0; + while (iter < Settings.MaxGJKIterations) + { + // Copy simplex so we can identify duplicates. + int saveCount = simplex.Count; + for (int i = 0; i < saveCount; ++i) + { + saveA[i] = simplex.V[i].IndexA; + saveB[i] = simplex.V[i].IndexB; + } + + switch (simplex.Count) + { + case 1: + break; + case 2: + simplex.Solve2(); + break; + case 3: + simplex.Solve3(); + break; + default: + Debug.Assert(false); + break; + } + + // If we have 3 points, then the origin is in the corresponding triangle. + if (simplex.Count == 3) + { + break; + } + + //FPE: This code was not used anyway. + // Compute closest point. + //Vector2 p = simplex.GetClosestPoint(); + //float distanceSqr2 = p.LengthSquared(); + + // Ensure progress + //if (distanceSqr2 >= distanceSqr1) + //{ + //break; + //} + //distanceSqr1 = distanceSqr2; + + // Get search direction. + Vector2 d = simplex.GetSearchDirection(); + + // Ensure the search direction is numerically fit. + if (d.LengthSquared() < Settings.Epsilon * Settings.Epsilon) + { + // The origin is probably contained by a line segment + // or triangle. Thus the shapes are overlapped. + + // We can't return zero here even though there may be overlap. + // In case the simplex is a point, segment, or triangle it is difficult + // to determine if the origin is contained in the CSO or very close to it. + break; + } + + // Compute a tentative new simplex vertex using support points. + SimplexVertex vertex = simplex.V[simplex.Count]; + vertex.IndexA = input.ProxyA.GetSupport(MathUtils.MulT(input.TransformA.q, -d)); + vertex.WA = MathUtils.Mul(ref input.TransformA, input.ProxyA.Vertices[vertex.IndexA]); + + vertex.IndexB = input.ProxyB.GetSupport(MathUtils.MulT(input.TransformB.q, d)); + vertex.WB = MathUtils.Mul(ref input.TransformB, input.ProxyB.Vertices[vertex.IndexB]); + vertex.W = vertex.WB - vertex.WA; + simplex.V[simplex.Count] = vertex; + + // Iteration count is equated to the number of support point calls. + ++iter; + + if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled + ++GJKIters; + + // Check for duplicate support points. This is the main termination criteria. + bool duplicate = false; + for (int i = 0; i < saveCount; ++i) + { + if (vertex.IndexA == saveA[i] && vertex.IndexB == saveB[i]) + { + duplicate = true; + break; + } + } + + // If we found a duplicate support point we must exit to avoid cycling. + if (duplicate) + { + break; + } + + // New vertex is ok and needed. + ++simplex.Count; + } + + if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled + GJKMaxIters = Math.Max(GJKMaxIters, iter); + + // Prepare output. + simplex.GetWitnessPoints(out output.PointA, out output.PointB); + output.Distance = (output.PointA - output.PointB).Length(); + output.Iterations = iter; + + // Cache the simplex. + simplex.WriteCache(ref cache); + + // Apply radii if requested. + if (input.UseRadii) + { + float rA = input.ProxyA.Radius; + float rB = input.ProxyB.Radius; + + if (output.Distance > rA + rB && output.Distance > Settings.Epsilon) + { + // Shapes are still no overlapped. + // Move the witness points to the outer surface. + output.Distance -= rA + rB; + Vector2 normal = output.PointB - output.PointA; + normal.Normalize(); + output.PointA += rA * normal; + output.PointB -= rB * normal; + } + else + { + // Shapes are overlapped when radii are considered. + // Move the witness points to the middle. + Vector2 p = 0.5f * (output.PointA + output.PointB); + output.PointA = p; + output.PointB = p; + output.Distance = 0.0f; + } + } + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Collision/DynamicTree.cs b/Farseer Physics Engine 3.5/Collision/DynamicTree.cs new file mode 100644 index 000000000..78813c18b --- /dev/null +++ b/Farseer Physics Engine 3.5/Collision/DynamicTree.cs @@ -0,0 +1,1034 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Collision +{ + /// + /// A node in the dynamic tree. The client does not interact with this directly. + /// + internal class TreeNode + { + /// + /// Enlarged AABB + /// + internal AABB AABB; + + internal int Child1; + internal int Child2; + + internal int Height; + internal int ParentOrNext; + internal T UserData; + + internal bool IsLeaf() + { + return Child1 == DynamicTree.NullNode; + } + } + + /// + /// A dynamic tree arranges data in a binary tree to accelerate + /// queries such as volume queries and ray casts. Leafs are proxies + /// with an AABB. In the tree we expand the proxy AABB by Settings.b2_fatAABBFactor + /// so that the proxy AABB is bigger than the client object. This allows the client + /// object to move by small amounts without triggering a tree update. + /// + /// Nodes are pooled and relocatable, so we use node indices rather than pointers. + /// + public class DynamicTree + { + private Stack _raycastStack = new Stack(256); + private Stack _queryStack = new Stack(256); + private int _freeList; + private int _nodeCapacity; + private int _nodeCount; + private TreeNode[] _nodes; + private int _root; + internal const int NullNode = -1; + + /// + /// Constructing the tree initializes the node pool. + /// + public DynamicTree() + { + _root = NullNode; + + _nodeCapacity = 16; + _nodeCount = 0; + _nodes = new TreeNode[_nodeCapacity]; + + // Build a linked list for the free list. + for (int i = 0; i < _nodeCapacity - 1; ++i) + { + _nodes[i] = new TreeNode(); + _nodes[i].ParentOrNext = i + 1; + _nodes[i].Height = 1; + } + _nodes[_nodeCapacity - 1] = new TreeNode(); + _nodes[_nodeCapacity - 1].ParentOrNext = NullNode; + _nodes[_nodeCapacity - 1].Height = 1; + _freeList = 0; + } + + /// + /// Compute the height of the binary tree in O(N) time. Should not be called often. + /// + public int Height + { + get + { + if (_root == NullNode) + { + return 0; + } + + return _nodes[_root].Height; + } + } + + /// + /// Get the ratio of the sum of the node areas to the root area. + /// + public float AreaRatio + { + get + { + if (_root == NullNode) + { + return 0.0f; + } + + TreeNode root = _nodes[_root]; + float rootArea = root.AABB.Perimeter; + + float totalArea = 0.0f; + for (int i = 0; i < _nodeCapacity; ++i) + { + TreeNode node = _nodes[i]; + if (node.Height < 0) + { + // Free node in pool + continue; + } + + totalArea += node.AABB.Perimeter; + } + + return totalArea / rootArea; + } + } + + /// + /// Get the maximum balance of an node in the tree. The balance is the difference + /// in height of the two children of a node. + /// + public int MaxBalance + { + get + { + int maxBalance = 0; + for (int i = 0; i < _nodeCapacity; ++i) + { + TreeNode node = _nodes[i]; + if (node.Height <= 1) + { + continue; + } + + Debug.Assert(node.IsLeaf() == false); + + int child1 = node.Child1; + int child2 = node.Child2; + int balance = Math.Abs(_nodes[child2].Height - _nodes[child1].Height); + maxBalance = Math.Max(maxBalance, balance); + } + + return maxBalance; + } + } + + /// + /// Create a proxy in the tree as a leaf node. We return the index + /// of the node instead of a pointer so that we can grow + /// the node pool. + /// /// + /// The aabb. + /// The user data. + /// Index of the created proxy + public int AddProxy(ref AABB aabb, T userData) + { + int proxyId = AllocateNode(); + + // Fatten the aabb. + Vector2 r = new Vector2(Settings.AABBExtension, Settings.AABBExtension); + _nodes[proxyId].AABB.LowerBound = aabb.LowerBound - r; + _nodes[proxyId].AABB.UpperBound = aabb.UpperBound + r; + _nodes[proxyId].UserData = userData; + _nodes[proxyId].Height = 0; + + InsertLeaf(proxyId); + + return proxyId; + } + + /// + /// Destroy a proxy. This asserts if the id is invalid. + /// + /// The proxy id. + public void RemoveProxy(int proxyId) + { + Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity); + Debug.Assert(_nodes[proxyId].IsLeaf()); + + RemoveLeaf(proxyId); + FreeNode(proxyId); + } + + /// + /// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB, + /// then the proxy is removed from the tree and re-inserted. Otherwise + /// the function returns immediately. + /// + /// The proxy id. + /// The aabb. + /// The displacement. + /// true if the proxy was re-inserted. + public bool MoveProxy(int proxyId, ref AABB aabb, Vector2 displacement) + { + Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity); + + Debug.Assert(_nodes[proxyId].IsLeaf()); + + if (_nodes[proxyId].AABB.Contains(ref aabb)) + { + return false; + } + + RemoveLeaf(proxyId); + + // Extend AABB. + AABB b = aabb; + Vector2 r = new Vector2(Settings.AABBExtension, Settings.AABBExtension); + b.LowerBound = b.LowerBound - r; + b.UpperBound = b.UpperBound + r; + + // Predict AABB displacement. + Vector2 d = Settings.AABBMultiplier * displacement; + + if (d.X < 0.0f) + { + b.LowerBound.X += d.X; + } + else + { + b.UpperBound.X += d.X; + } + + if (d.Y < 0.0f) + { + b.LowerBound.Y += d.Y; + } + else + { + b.UpperBound.Y += d.Y; + } + + _nodes[proxyId].AABB = b; + + InsertLeaf(proxyId); + return true; + } + + /// + /// Get proxy user data. + /// + /// + /// The proxy id. + /// the proxy user data or 0 if the id is invalid. + public T GetUserData(int proxyId) + { + Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity); + return _nodes[proxyId].UserData; + } + + /// + /// Get the fat AABB for a proxy. + /// + /// The proxy id. + /// The fat AABB. + public void GetFatAABB(int proxyId, out AABB fatAABB) + { + Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity); + fatAABB = _nodes[proxyId].AABB; + } + + /// + /// Query an AABB for overlapping proxies. The callback class + /// is called for each proxy that overlaps the supplied AABB. + /// + /// The callback. + /// The aabb. + public void Query(Func callback, ref AABB aabb) + { + _queryStack.Clear(); + _queryStack.Push(_root); + + while (_queryStack.Count > 0) + { + int nodeId = _queryStack.Pop(); + if (nodeId == NullNode) + { + continue; + } + + TreeNode node = _nodes[nodeId]; + + if (AABB.TestOverlap(ref node.AABB, ref aabb)) + { + if (node.IsLeaf()) + { + bool proceed = callback(nodeId); + if (proceed == false) + { + return; + } + } + else + { + _queryStack.Push(node.Child1); + _queryStack.Push(node.Child2); + } + } + } + } + + /// + /// Ray-cast against the proxies in the tree. This relies on the callback + /// to perform a exact ray-cast in the case were the proxy contains a Shape. + /// The callback also performs the any collision filtering. This has performance + /// roughly equal to k * log(n), where k is the number of collisions and n is the + /// number of proxies in the tree. + /// + /// A callback class that is called for each proxy that is hit by the ray. + /// The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). + public void RayCast(Func callback, ref RayCastInput input) + { + Vector2 p1 = input.Point1; + Vector2 p2 = input.Point2; + Vector2 r = p2 - p1; + Debug.Assert(r.LengthSquared() > 0.0f); + r.Normalize(); + + // v is perpendicular to the segment. + Vector2 absV = MathUtils.Abs(new Vector2(-r.Y, r.X)); //FPE: Inlined the 'v' variable + + // Separating axis for segment (Gino, p80). + // |dot(v, p1 - c)| > dot(|v|, h) + + float maxFraction = input.MaxFraction; + + // Build a bounding box for the segment. + AABB segmentAABB = new AABB(); + { + Vector2 t = p1 + maxFraction * (p2 - p1); + Vector2.Min(ref p1, ref t, out segmentAABB.LowerBound); + Vector2.Max(ref p1, ref t, out segmentAABB.UpperBound); + } + + _raycastStack.Clear(); + _raycastStack.Push(_root); + + while (_raycastStack.Count > 0) + { + int nodeId = _raycastStack.Pop(); + if (nodeId == NullNode) + { + continue; + } + + TreeNode node = _nodes[nodeId]; + + if (AABB.TestOverlap(ref node.AABB, ref segmentAABB) == false) + { + continue; + } + + // Separating axis for segment (Gino, p80). + // |dot(v, p1 - c)| > dot(|v|, h) + Vector2 c = node.AABB.Center; + Vector2 h = node.AABB.Extents; + float separation = Math.Abs(Vector2.Dot(new Vector2(-r.Y, r.X), p1 - c)) - Vector2.Dot(absV, h); + if (separation > 0.0f) + { + continue; + } + + if (node.IsLeaf()) + { + RayCastInput subInput; + subInput.Point1 = input.Point1; + subInput.Point2 = input.Point2; + subInput.MaxFraction = maxFraction; + + float value = callback(subInput, nodeId); + + if (value == 0.0f) + { + // the client has terminated the raycast. + return; + } + + if (value > 0.0f) + { + // Update segment bounding box. + maxFraction = value; + Vector2 t = p1 + maxFraction * (p2 - p1); + segmentAABB.LowerBound = Vector2.Min(p1, t); + segmentAABB.UpperBound = Vector2.Max(p1, t); + } + } + else + { + _raycastStack.Push(node.Child1); + _raycastStack.Push(node.Child2); + } + } + } + + private int AllocateNode() + { + // Expand the node pool as needed. + if (_freeList == NullNode) + { + Debug.Assert(_nodeCount == _nodeCapacity); + + // The free list is empty. Rebuild a bigger pool. + TreeNode[] oldNodes = _nodes; + _nodeCapacity *= 2; + _nodes = new TreeNode[_nodeCapacity]; + Array.Copy(oldNodes, _nodes, _nodeCount); + + // Build a linked list for the free list. The parent + // pointer becomes the "next" pointer. + for (int i = _nodeCount; i < _nodeCapacity - 1; ++i) + { + _nodes[i] = new TreeNode(); + _nodes[i].ParentOrNext = i + 1; + _nodes[i].Height = -1; + } + _nodes[_nodeCapacity - 1] = new TreeNode(); + _nodes[_nodeCapacity - 1].ParentOrNext = NullNode; + _nodes[_nodeCapacity - 1].Height = -1; + _freeList = _nodeCount; + } + + // Peel a node off the free list. + int nodeId = _freeList; + _freeList = _nodes[nodeId].ParentOrNext; + _nodes[nodeId].ParentOrNext = NullNode; + _nodes[nodeId].Child1 = NullNode; + _nodes[nodeId].Child2 = NullNode; + _nodes[nodeId].Height = 0; + _nodes[nodeId].UserData = default(T); + ++_nodeCount; + return nodeId; + } + + private void FreeNode(int nodeId) + { + Debug.Assert(0 <= nodeId && nodeId < _nodeCapacity); + Debug.Assert(0 < _nodeCount); + _nodes[nodeId].ParentOrNext = _freeList; + _nodes[nodeId].Height = -1; + _freeList = nodeId; + --_nodeCount; + } + + private void InsertLeaf(int leaf) + { + if (_root == NullNode) + { + _root = leaf; + _nodes[_root].ParentOrNext = NullNode; + return; + } + + // Find the best sibling for this node + AABB leafAABB = _nodes[leaf].AABB; + int index = _root; + while (_nodes[index].IsLeaf() == false) + { + int child1 = _nodes[index].Child1; + int child2 = _nodes[index].Child2; + + float area = _nodes[index].AABB.Perimeter; + + AABB combinedAABB = new AABB(); + combinedAABB.Combine(ref _nodes[index].AABB, ref leafAABB); + float combinedArea = combinedAABB.Perimeter; + + // Cost of creating a new parent for this node and the new leaf + float cost = 2.0f * combinedArea; + + // Minimum cost of pushing the leaf further down the tree + float inheritanceCost = 2.0f * (combinedArea - area); + + // Cost of descending into child1 + float cost1; + if (_nodes[child1].IsLeaf()) + { + AABB aabb = new AABB(); + aabb.Combine(ref leafAABB, ref _nodes[child1].AABB); + cost1 = aabb.Perimeter + inheritanceCost; + } + else + { + AABB aabb = new AABB(); + aabb.Combine(ref leafAABB, ref _nodes[child1].AABB); + float oldArea = _nodes[child1].AABB.Perimeter; + float newArea = aabb.Perimeter; + cost1 = (newArea - oldArea) + inheritanceCost; + } + + // Cost of descending into child2 + float cost2; + if (_nodes[child2].IsLeaf()) + { + AABB aabb = new AABB(); + aabb.Combine(ref leafAABB, ref _nodes[child2].AABB); + cost2 = aabb.Perimeter + inheritanceCost; + } + else + { + AABB aabb = new AABB(); + aabb.Combine(ref leafAABB, ref _nodes[child2].AABB); + float oldArea = _nodes[child2].AABB.Perimeter; + float newArea = aabb.Perimeter; + cost2 = newArea - oldArea + inheritanceCost; + } + + // Descend according to the minimum cost. + if (cost < cost1 && cost1 < cost2) + { + break; + } + + // Descend + if (cost1 < cost2) + { + index = child1; + } + else + { + index = child2; + } + } + + int sibling = index; + + // Create a new parent. + int oldParent = _nodes[sibling].ParentOrNext; + int newParent = AllocateNode(); + _nodes[newParent].ParentOrNext = oldParent; + _nodes[newParent].UserData = default(T); + _nodes[newParent].AABB.Combine(ref leafAABB, ref _nodes[sibling].AABB); + _nodes[newParent].Height = _nodes[sibling].Height + 1; + + if (oldParent != NullNode) + { + // The sibling was not the root. + if (_nodes[oldParent].Child1 == sibling) + { + _nodes[oldParent].Child1 = newParent; + } + else + { + _nodes[oldParent].Child2 = newParent; + } + + _nodes[newParent].Child1 = sibling; + _nodes[newParent].Child2 = leaf; + _nodes[sibling].ParentOrNext = newParent; + _nodes[leaf].ParentOrNext = newParent; + } + else + { + // The sibling was the root. + _nodes[newParent].Child1 = sibling; + _nodes[newParent].Child2 = leaf; + _nodes[sibling].ParentOrNext = newParent; + _nodes[leaf].ParentOrNext = newParent; + _root = newParent; + } + + // Walk back up the tree fixing heights and AABBs + index = _nodes[leaf].ParentOrNext; + while (index != NullNode) + { + index = Balance(index); + + int child1 = _nodes[index].Child1; + int child2 = _nodes[index].Child2; + + Debug.Assert(child1 != NullNode); + Debug.Assert(child2 != NullNode); + + _nodes[index].Height = 1 + Math.Max(_nodes[child1].Height, _nodes[child2].Height); + _nodes[index].AABB.Combine(ref _nodes[child1].AABB, ref _nodes[child2].AABB); + + index = _nodes[index].ParentOrNext; + } + + //Validate(); + } + + private void RemoveLeaf(int leaf) + { + if (leaf == _root) + { + _root = NullNode; + return; + } + + int parent = _nodes[leaf].ParentOrNext; + int grandParent = _nodes[parent].ParentOrNext; + int sibling; + if (_nodes[parent].Child1 == leaf) + { + sibling = _nodes[parent].Child2; + } + else + { + sibling = _nodes[parent].Child1; + } + + if (grandParent != NullNode) + { + // Destroy parent and connect sibling to grandParent. + if (_nodes[grandParent].Child1 == parent) + { + _nodes[grandParent].Child1 = sibling; + } + else + { + _nodes[grandParent].Child2 = sibling; + } + _nodes[sibling].ParentOrNext = grandParent; + FreeNode(parent); + + // Adjust ancestor bounds. + int index = grandParent; + while (index != NullNode) + { + index = Balance(index); + + int child1 = _nodes[index].Child1; + int child2 = _nodes[index].Child2; + + _nodes[index].AABB.Combine(ref _nodes[child1].AABB, ref _nodes[child2].AABB); + _nodes[index].Height = 1 + Math.Max(_nodes[child1].Height, _nodes[child2].Height); + + index = _nodes[index].ParentOrNext; + } + } + else + { + _root = sibling; + _nodes[sibling].ParentOrNext = NullNode; + FreeNode(parent); + } + + //Validate(); + } + + /// + /// Perform a left or right rotation if node A is imbalanced. + /// + /// + /// the new root index. + private int Balance(int iA) + { + Debug.Assert(iA != NullNode); + + TreeNode A = _nodes[iA]; + if (A.IsLeaf() || A.Height < 2) + { + return iA; + } + + int iB = A.Child1; + int iC = A.Child2; + Debug.Assert(0 <= iB && iB < _nodeCapacity); + Debug.Assert(0 <= iC && iC < _nodeCapacity); + + TreeNode B = _nodes[iB]; + TreeNode C = _nodes[iC]; + + int balance = C.Height - B.Height; + + // Rotate C up + if (balance > 1) + { + int iF = C.Child1; + int iG = C.Child2; + TreeNode F = _nodes[iF]; + TreeNode G = _nodes[iG]; + Debug.Assert(0 <= iF && iF < _nodeCapacity); + Debug.Assert(0 <= iG && iG < _nodeCapacity); + + // Swap A and C + C.Child1 = iA; + C.ParentOrNext = A.ParentOrNext; + A.ParentOrNext = iC; + + // A's old parent should point to C + if (C.ParentOrNext != NullNode) + { + if (_nodes[C.ParentOrNext].Child1 == iA) + { + _nodes[C.ParentOrNext].Child1 = iC; + } + else + { + Debug.Assert(_nodes[C.ParentOrNext].Child2 == iA); + _nodes[C.ParentOrNext].Child2 = iC; + } + } + else + { + _root = iC; + } + + // Rotate + if (F.Height > G.Height) + { + C.Child2 = iF; + A.Child2 = iG; + G.ParentOrNext = iA; + A.AABB.Combine(ref B.AABB, ref G.AABB); + C.AABB.Combine(ref A.AABB, ref F.AABB); + + A.Height = 1 + Math.Max(B.Height, G.Height); + C.Height = 1 + Math.Max(A.Height, F.Height); + } + else + { + C.Child2 = iG; + A.Child2 = iF; + F.ParentOrNext = iA; + A.AABB.Combine(ref B.AABB, ref F.AABB); + C.AABB.Combine(ref A.AABB, ref G.AABB); + + A.Height = 1 + Math.Max(B.Height, F.Height); + C.Height = 1 + Math.Max(A.Height, G.Height); + } + + return iC; + } + + // Rotate B up + if (balance < -1) + { + int iD = B.Child1; + int iE = B.Child2; + TreeNode D = _nodes[iD]; + TreeNode E = _nodes[iE]; + Debug.Assert(0 <= iD && iD < _nodeCapacity); + Debug.Assert(0 <= iE && iE < _nodeCapacity); + + // Swap A and B + B.Child1 = iA; + B.ParentOrNext = A.ParentOrNext; + A.ParentOrNext = iB; + + // A's old parent should point to B + if (B.ParentOrNext != NullNode) + { + if (_nodes[B.ParentOrNext].Child1 == iA) + { + _nodes[B.ParentOrNext].Child1 = iB; + } + else + { + Debug.Assert(_nodes[B.ParentOrNext].Child2 == iA); + _nodes[B.ParentOrNext].Child2 = iB; + } + } + else + { + _root = iB; + } + + // Rotate + if (D.Height > E.Height) + { + B.Child2 = iD; + A.Child1 = iE; + E.ParentOrNext = iA; + A.AABB.Combine(ref C.AABB, ref E.AABB); + B.AABB.Combine(ref A.AABB, ref D.AABB); + + A.Height = 1 + Math.Max(C.Height, E.Height); + B.Height = 1 + Math.Max(A.Height, D.Height); + } + else + { + B.Child2 = iE; + A.Child1 = iD; + D.ParentOrNext = iA; + A.AABB.Combine(ref C.AABB, ref D.AABB); + B.AABB.Combine(ref A.AABB, ref E.AABB); + + A.Height = 1 + Math.Max(C.Height, D.Height); + B.Height = 1 + Math.Max(A.Height, E.Height); + } + + return iB; + } + + return iA; + } + + /// + /// Compute the height of a sub-tree. + /// + /// The node id to use as parent. + /// The height of the tree. + public int ComputeHeight(int nodeId) + { + Debug.Assert(0 <= nodeId && nodeId < _nodeCapacity); + TreeNode node = _nodes[nodeId]; + + if (node.IsLeaf()) + { + return 0; + } + + int height1 = ComputeHeight(node.Child1); + int height2 = ComputeHeight(node.Child2); + return 1 + Math.Max(height1, height2); + } + + /// + /// Compute the height of the entire tree. + /// + /// The height of the tree. + public int ComputeHeight() + { + int height = ComputeHeight(_root); + return height; + } + + public void ValidateStructure(int index) + { + if (index == NullNode) + { + return; + } + + if (index == _root) + { + Debug.Assert(_nodes[index].ParentOrNext == NullNode); + } + + TreeNode node = _nodes[index]; + + int child1 = node.Child1; + int child2 = node.Child2; + + if (node.IsLeaf()) + { + Debug.Assert(child1 == NullNode); + Debug.Assert(child2 == NullNode); + Debug.Assert(node.Height == 0); + return; + } + + Debug.Assert(0 <= child1 && child1 < _nodeCapacity); + Debug.Assert(0 <= child2 && child2 < _nodeCapacity); + + Debug.Assert(_nodes[child1].ParentOrNext == index); + Debug.Assert(_nodes[child2].ParentOrNext == index); + + ValidateStructure(child1); + ValidateStructure(child2); + } + + public void ValidateMetrics(int index) + { + if (index == NullNode) + { + return; + } + + TreeNode node = _nodes[index]; + + int child1 = node.Child1; + int child2 = node.Child2; + + if (node.IsLeaf()) + { + Debug.Assert(child1 == NullNode); + Debug.Assert(child2 == NullNode); + Debug.Assert(node.Height == 0); + return; + } + + Debug.Assert(0 <= child1 && child1 < _nodeCapacity); + Debug.Assert(0 <= child2 && child2 < _nodeCapacity); + + int height1 = _nodes[child1].Height; + int height2 = _nodes[child2].Height; + int height = 1 + Math.Max(height1, height2); + Debug.Assert(node.Height == height); + + AABB AABB = new AABB(); + AABB.Combine(ref _nodes[child1].AABB, ref _nodes[child2].AABB); + + Debug.Assert(AABB.LowerBound == node.AABB.LowerBound); + Debug.Assert(AABB.UpperBound == node.AABB.UpperBound); + + ValidateMetrics(child1); + ValidateMetrics(child2); + } + + /// + /// Validate this tree. For testing. + /// + public void Validate() + { + ValidateStructure(_root); + ValidateMetrics(_root); + + int freeCount = 0; + int freeIndex = _freeList; + while (freeIndex != NullNode) + { + Debug.Assert(0 <= freeIndex && freeIndex < _nodeCapacity); + freeIndex = _nodes[freeIndex].ParentOrNext; + ++freeCount; + } + + Debug.Assert(Height == ComputeHeight()); + + Debug.Assert(_nodeCount + freeCount == _nodeCapacity); + } + + /// + /// Build an optimal tree. Very expensive. For testing. + /// + public void RebuildBottomUp() + { + int[] nodes = new int[_nodeCount]; + int count = 0; + + // Build array of leaves. Free the rest. + for (int i = 0; i < _nodeCapacity; ++i) + { + if (_nodes[i].Height < 0) + { + // free node in pool + continue; + } + + if (_nodes[i].IsLeaf()) + { + _nodes[i].ParentOrNext = NullNode; + nodes[count] = i; + ++count; + } + else + { + FreeNode(i); + } + } + + while (count > 1) + { + float minCost = Settings.MaxFloat; + int iMin = -1, jMin = -1; + for (int i = 0; i < count; ++i) + { + AABB AABBi = _nodes[nodes[i]].AABB; + + for (int j = i + 1; j < count; ++j) + { + AABB AABBj = _nodes[nodes[j]].AABB; + AABB b = new AABB(); + b.Combine(ref AABBi, ref AABBj); + float cost = b.Perimeter; + if (cost < minCost) + { + iMin = i; + jMin = j; + minCost = cost; + } + } + } + + int index1 = nodes[iMin]; + int index2 = nodes[jMin]; + TreeNode child1 = _nodes[index1]; + TreeNode child2 = _nodes[index2]; + + int parentIndex = AllocateNode(); + TreeNode parent = _nodes[parentIndex]; + parent.Child1 = index1; + parent.Child2 = index2; + parent.Height = 1 + Math.Max(child1.Height, child2.Height); + parent.AABB.Combine(ref child1.AABB, ref child2.AABB); + parent.ParentOrNext = NullNode; + + child1.ParentOrNext = parentIndex; + child2.ParentOrNext = parentIndex; + + nodes[jMin] = nodes[count - 1]; + nodes[iMin] = parentIndex; + --count; + } + + _root = nodes[0]; + + Validate(); + } + + /// + /// Shift the origin of the nodes + /// + /// The displacement to use. + public void ShiftOrigin(Vector2 newOrigin) + { + // Build array of leaves. Free the rest. + for (int i = 0; i < _nodeCapacity; ++i) + { + _nodes[i].AABB.LowerBound -= newOrigin; + _nodes[i].AABB.UpperBound -= newOrigin; + } + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Collision/DynamicTreeBroadPhase.cs b/Farseer Physics Engine 3.5/Collision/DynamicTreeBroadPhase.cs new file mode 100644 index 000000000..40f0dc028 --- /dev/null +++ b/Farseer Physics Engine 3.5/Collision/DynamicTreeBroadPhase.cs @@ -0,0 +1,347 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using FarseerPhysics.Dynamics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Collision +{ + internal struct Pair : IComparable + { + public int ProxyIdA; + public int ProxyIdB; + + #region IComparable Members + + public int CompareTo(Pair other) + { + if (ProxyIdA < other.ProxyIdA) + { + return -1; + } + if (ProxyIdA == other.ProxyIdA) + { + if (ProxyIdB < other.ProxyIdB) + { + return -1; + } + if (ProxyIdB == other.ProxyIdB) + { + return 0; + } + } + + return 1; + } + + #endregion + } + + /// + /// The broad-phase is used for computing pairs and performing volume queries and ray casts. + /// This broad-phase does not persist pairs. Instead, this reports potentially new pairs. + /// It is up to the client to consume the new pairs and to track subsequent overlap. + /// + public class DynamicTreeBroadPhase : IBroadPhase + { + private const int NullProxy = -1; + private int[] _moveBuffer; + private int _moveCapacity; + private int _moveCount; + + private Pair[] _pairBuffer; + private int _pairCapacity; + private int _pairCount; + private int _proxyCount; + private Func _queryCallback; + private int _queryProxyId; + private DynamicTree _tree = new DynamicTree(); + + /// + /// Constructs a new broad phase based on the dynamic tree implementation + /// + public DynamicTreeBroadPhase() + { + _queryCallback = QueryCallback; + _proxyCount = 0; + + _pairCapacity = 16; + _pairCount = 0; + _pairBuffer = new Pair[_pairCapacity]; + + _moveCapacity = 16; + _moveCount = 0; + _moveBuffer = new int[_moveCapacity]; + } + + /// + /// Get the number of proxies. + /// + /// The proxy count. + public int ProxyCount + { + get { return _proxyCount; } + } + + /// + /// Create a proxy with an initial AABB. Pairs are not reported until + /// UpdatePairs is called. + /// + /// The user data. + /// + public int AddProxy(ref FixtureProxy proxy) + { + int proxyId = _tree.AddProxy(ref proxy.AABB, proxy); + ++_proxyCount; + BufferMove(proxyId); + return proxyId; + } + + /// + /// Destroy a proxy. It is up to the client to remove any pairs. + /// + /// The proxy id. + public void RemoveProxy(int proxyId) + { + UnBufferMove(proxyId); + --_proxyCount; + _tree.RemoveProxy(proxyId); + } + + public void MoveProxy(int proxyId, ref AABB aabb, Vector2 displacement) + { + bool buffer = _tree.MoveProxy(proxyId, ref aabb, displacement); + if (buffer) + { + BufferMove(proxyId); + } + } + + public void TouchProxy(int proxyId) + { + BufferMove(proxyId); + } + + private void BufferMove(int proxyId) + { + if (_moveCount == _moveCapacity) + { + int[] oldBuffer = _moveBuffer; + _moveCapacity *= 2; + _moveBuffer = new int[_moveCapacity]; + Array.Copy(oldBuffer, _moveBuffer, _moveCount); + } + + _moveBuffer[_moveCount] = proxyId; + ++_moveCount; + } + + private void UnBufferMove(int proxyId) + { + for (int i = 0; i < _moveCount; ++i) + { + if (_moveBuffer[i] == proxyId) + { + _moveBuffer[i] = NullProxy; + } + } + } + + /// + /// This is called from DynamicTree.Query when we are gathering pairs. + /// + /// + /// + private bool QueryCallback(int proxyId) + { + // A proxy cannot form a pair with itself. + if (proxyId == _queryProxyId) + { + return true; + } + + // Grow the pair buffer as needed. + if (_pairCount == _pairCapacity) + { + Pair[] oldBuffer = _pairBuffer; + _pairCapacity *= 2; + _pairBuffer = new Pair[_pairCapacity]; + Array.Copy(oldBuffer, _pairBuffer, _pairCount); + } + + _pairBuffer[_pairCount].ProxyIdA = Math.Min(proxyId, _queryProxyId); + _pairBuffer[_pairCount].ProxyIdB = Math.Max(proxyId, _queryProxyId); + ++_pairCount; + + return true; + } + + /// + /// Get the AABB for a proxy. + /// + /// The proxy id. + /// The aabb. + public void GetFatAABB(int proxyId, out AABB aabb) + { + _tree.GetFatAABB(proxyId, out aabb); + } + + /// + /// Get user data from a proxy. Returns null if the id is invalid. + /// + /// The proxy id. + /// + public FixtureProxy GetProxy(int proxyId) + { + return _tree.GetUserData(proxyId); + } + + /// + /// Test overlap of fat AABBs. + /// + /// The proxy id A. + /// The proxy id B. + /// + public bool TestOverlap(int proxyIdA, int proxyIdB) + { + AABB aabbA, aabbB; + _tree.GetFatAABB(proxyIdA, out aabbA); + _tree.GetFatAABB(proxyIdB, out aabbB); + return AABB.TestOverlap(ref aabbA, ref aabbB); + } + + /// + /// Update the pairs. This results in pair callbacks. This can only add pairs. + /// + /// The callback. + public void UpdatePairs(BroadphaseDelegate callback) + { + // Reset pair buffer + _pairCount = 0; + + // Perform tree queries for all moving proxies. + for (int j = 0; j < _moveCount; ++j) + { + _queryProxyId = _moveBuffer[j]; + if (_queryProxyId == NullProxy) + { + continue; + } + + // We have to query the tree with the fat AABB so that + // we don't fail to create a pair that may touch later. + AABB fatAABB; + _tree.GetFatAABB(_queryProxyId, out fatAABB); + + // Query tree, create pairs and add them pair buffer. + _tree.Query(_queryCallback, ref fatAABB); + } + + // Reset move buffer + _moveCount = 0; + + // Sort the pair buffer to expose duplicates. + Array.Sort(_pairBuffer, 0, _pairCount); + + // Send the pairs back to the client. + int i = 0; + while (i < _pairCount) + { + Pair primaryPair = _pairBuffer[i]; + FixtureProxy userDataA = _tree.GetUserData(primaryPair.ProxyIdA); + FixtureProxy userDataB = _tree.GetUserData(primaryPair.ProxyIdB); + + callback(ref userDataA, ref userDataB); + ++i; + + // Skip any duplicate pairs. + while (i < _pairCount) + { + Pair pair = _pairBuffer[i]; + if (pair.ProxyIdA != primaryPair.ProxyIdA || pair.ProxyIdB != primaryPair.ProxyIdB) + { + break; + } + ++i; + } + } + + // Try to keep the tree balanced. + //_tree.Rebalance(4); + } + + /// + /// Query an AABB for overlapping proxies. The callback class + /// is called for each proxy that overlaps the supplied AABB. + /// + /// The callback. + /// The aabb. + public void Query(Func callback, ref AABB aabb) + { + _tree.Query(callback, ref aabb); + } + + /// + /// Ray-cast against the proxies in the tree. This relies on the callback + /// to perform a exact ray-cast in the case were the proxy contains a shape. + /// The callback also performs the any collision filtering. This has performance + /// roughly equal to k * log(n), where k is the number of collisions and n is the + /// number of proxies in the tree. + /// + /// A callback class that is called for each proxy that is hit by the ray. + /// The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). + public void RayCast(Func callback, ref RayCastInput input) + { + _tree.RayCast(callback, ref input); + } + + public void ShiftOrigin(Vector2 newOrigin) + { + _tree.ShiftOrigin(newOrigin); + } + + /// + /// Get the tree quality based on the area of the tree. + /// + public float TreeQuality + { + get { return _tree.AreaRatio; } + } + + /// + /// Gets the balance of the tree. + /// + public int TreeBalance + { + get { return _tree.MaxBalance; } + } + + /// + /// Gets the height of the tree. + /// + public int TreeHeight + { + get { return _tree.Height; } + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Collision/IBroadPhase.cs b/Farseer Physics Engine 3.5/Collision/IBroadPhase.cs new file mode 100644 index 000000000..fdf7001ba --- /dev/null +++ b/Farseer Physics Engine 3.5/Collision/IBroadPhase.cs @@ -0,0 +1,32 @@ +using System; +using FarseerPhysics.Dynamics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Collision +{ + public interface IBroadPhase + { + int ProxyCount { get; } + void UpdatePairs(BroadphaseDelegate callback); + + bool TestOverlap(int proxyIdA, int proxyIdB); + + int AddProxy(ref FixtureProxy proxy); + + void RemoveProxy(int proxyId); + + void MoveProxy(int proxyId, ref AABB aabb, Vector2 displacement); + + FixtureProxy GetProxy(int proxyId); + + void TouchProxy(int proxyId); + + void GetFatAABB(int proxyId, out AABB aabb); + + void Query(Func callback, ref AABB aabb); + + void RayCast(Func callback, ref RayCastInput input); + + void ShiftOrigin(Vector2 newOrigin); + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Collision/Shapes/ChainShape.cs b/Farseer Physics Engine 3.5/Collision/Shapes/ChainShape.cs new file mode 100644 index 000000000..cd41b43ff --- /dev/null +++ b/Farseer Physics Engine 3.5/Collision/Shapes/ChainShape.cs @@ -0,0 +1,273 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Collision.Shapes +{ + /// + /// A chain shape is a free form sequence of line segments. + /// The chain has two-sided collision, so you can use inside and outside collision. + /// Therefore, you may use any winding order. + /// Connectivity information is used to create smooth collisions. + /// WARNING: The chain will not collide properly if there are self-intersections. + /// + public class ChainShape : Shape + { + /// + /// The vertices. These are not owned/freed by the chain Shape. + /// + public Vertices Vertices; + private Vector2 _prevVertex, _nextVertex; + private bool _hasPrevVertex, _hasNextVertex; + private static EdgeShape _edgeShape = new EdgeShape(); + + /// + /// Constructor for ChainShape. By default have 0 in density. + /// + public ChainShape() + : base(0) + { + ShapeType = ShapeType.Chain; + _radius = Settings.PolygonRadius; + } + + /// + /// Create a new chainshape from the vertices. + /// + /// The vertices to use. Must contain 2 or more vertices. + /// Set to true to create a closed loop. It connects the first vertice to the last, and automatically adjusts connectivity to create smooth collisions along the chain. + public ChainShape(Vertices vertices, bool createLoop = false) + : base(0) + { + ShapeType = ShapeType.Chain; + _radius = Settings.PolygonRadius; + + if (!(vertices != null && vertices.Count >= 2)) + { + int lkmsdgkldf = 1; + } + + Debug.Assert(vertices != null && vertices.Count >= 2); + Debug.Assert(vertices[0] != vertices[vertices.Count - 1]); // FPE. See http://www.box2d.org/forum/viewtopic.php?f=4&t=7973&p=35363 + + for (int i = 1; i < vertices.Count; ++i) + { + Vector2 v1 = vertices[i - 1]; + Vector2 v2 = vertices[i]; + + // If the code crashes here, it means your vertices are too close together. + + if (Vector2.DistanceSquared(v1, v2) < Settings.LinearSlop * Settings.LinearSlop) + { + int asldmfk = 1; + } + + Debug.Assert(Vector2.DistanceSquared(v1, v2) > Settings.LinearSlop * Settings.LinearSlop); + } + + Vertices = new Vertices(vertices); + + if (createLoop) + { + Vertices.Add(vertices[0]); + PrevVertex = Vertices[Vertices.Count - 2]; //FPE: We use the properties instead of the private fields here. + NextVertex = Vertices[1]; //FPE: We use the properties instead of the private fields here. + } + } + + public override int ChildCount + { + // edge count = vertex count - 1 + get { return Vertices.Count - 1; } + } + + /// + /// Establish connectivity to a vertex that precedes the first vertex. + /// Don't call this for loops. + /// + public Vector2 PrevVertex + { + get { return _prevVertex; } + set + { + Debug.Assert(value != null); + + _prevVertex = value; + _hasPrevVertex = true; + } + } + + /// + /// Establish connectivity to a vertex that follows the last vertex. + /// Don't call this for loops. + /// + public Vector2 NextVertex + { + get { return _nextVertex; } + set + { + Debug.Assert(value != null); + + _nextVertex = value; + _hasNextVertex = true; + } + } + + /// + /// This method has been optimized to reduce garbage. + /// + /// The cached edge to set properties on. + /// The index. + internal void GetChildEdge(EdgeShape edge, int index) + { + Debug.Assert(0 <= index && index < Vertices.Count - 1); + Debug.Assert(edge != null); + + edge.ShapeType = ShapeType.Edge; + edge._radius = _radius; + + edge.Vertex1 = Vertices[index + 0]; + edge.Vertex2 = Vertices[index + 1]; + + if (index > 0) + { + edge.Vertex0 = Vertices[index - 1]; + edge.HasVertex0 = true; + } + else + { + edge.Vertex0 = _prevVertex; + edge.HasVertex0 = _hasPrevVertex; + } + + if (index < Vertices.Count - 2) + { + edge.Vertex3 = Vertices[index + 2]; + edge.HasVertex3 = true; + } + else + { + edge.Vertex3 = _nextVertex; + edge.HasVertex3 = _hasNextVertex; + } + } + + /// + /// Get a child edge. + /// + /// The index. + public EdgeShape GetChildEdge(int index) + { + EdgeShape edgeShape = new EdgeShape(); + GetChildEdge(edgeShape, index); + return edgeShape; + } + + public override bool TestPoint(ref Transform transform, ref Vector2 point) + { + return false; + } + + public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex) + { + Debug.Assert(childIndex < Vertices.Count); + + int i1 = childIndex; + int i2 = childIndex + 1; + if (i2 == Vertices.Count) + { + i2 = 0; + } + + _edgeShape.Vertex1 = Vertices[i1]; + _edgeShape.Vertex2 = Vertices[i2]; + + return _edgeShape.RayCast(out output, ref input, ref transform, 0); + } + + public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex) + { + Debug.Assert(childIndex < Vertices.Count); + + int i1 = childIndex; + int i2 = childIndex + 1; + if (i2 == Vertices.Count) + { + i2 = 0; + } + + Vector2 v1 = MathUtils.Mul(ref transform, Vertices[i1]); + Vector2 v2 = MathUtils.Mul(ref transform, Vertices[i2]); + + aabb.LowerBound = Vector2.Min(v1, v2); + aabb.UpperBound = Vector2.Max(v1, v2); + } + + protected override void ComputeProperties() + { + //Does nothing. Chain shapes don't have properties. + } + + public override float ComputeSubmergedArea(ref Vector2 normal, float offset, ref Transform xf, out Vector2 sc) + { + sc = Vector2.Zero; + return 0; + } + + /// + /// Compare the chain to another chain + /// + /// The other chain + /// True if the two chain shapes are the same + public bool CompareTo(ChainShape shape) + { + if (Vertices.Count != shape.Vertices.Count) + return false; + + for (int i = 0; i < Vertices.Count; i++) + { + if (Vertices[i] != shape.Vertices[i]) + return false; + } + + return PrevVertex == shape.PrevVertex && NextVertex == shape.NextVertex; + } + + public override Shape Clone() + { + ChainShape clone = new ChainShape(); + clone.ShapeType = ShapeType; + clone._density = _density; + clone._radius = _radius; + clone.PrevVertex = _prevVertex; + clone.NextVertex = _nextVertex; + clone._hasNextVertex = _hasNextVertex; + clone._hasPrevVertex = _hasPrevVertex; + clone.Vertices = new Vertices(Vertices); + clone.MassData = MassData; + return clone; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Collision/Shapes/CircleShape.cs b/Farseer Physics Engine 3.5/Collision/Shapes/CircleShape.cs new file mode 100644 index 000000000..2a2c7f20f --- /dev/null +++ b/Farseer Physics Engine 3.5/Collision/Shapes/CircleShape.cs @@ -0,0 +1,198 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Collision.Shapes +{ + /// + /// A circle shape. + /// + public class CircleShape : Shape + { + internal Vector2 _position; + + /// + /// Create a new circle with the desired radius and density. + /// + /// The radius of the circle. + /// The density of the circle. + public CircleShape(float radius, float density) + : base(density) + { + Debug.Assert(radius >= 0); + Debug.Assert(density >= 0); + + ShapeType = ShapeType.Circle; + _position = Vector2.Zero; + Radius = radius; // The Radius property cache 2radius and calls ComputeProperties(). So no need to call ComputeProperties() here. + } + + internal CircleShape() + : base(0) + { + ShapeType = ShapeType.Circle; + _radius = 0.0f; + _position = Vector2.Zero; + } + + public override int ChildCount + { + get { return 1; } + } + + /// + /// Get or set the position of the circle + /// + public Vector2 Position + { + get { return _position; } + set + { + _position = value; + ComputeProperties(); //TODO: Optimize here + } + } + + public override bool TestPoint(ref Transform transform, ref Vector2 point) + { + Vector2 center = transform.p + MathUtils.Mul(transform.q, Position); + Vector2 d = point - center; + return Vector2.Dot(d, d) <= _2radius; + } + + public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex) + { + // Collision Detection in Interactive 3D Environments by Gino van den Bergen + // From Section 3.1.2 + // x = s + a * r + // norm(x) = radius + + output = new RayCastOutput(); + + Vector2 position = transform.p + MathUtils.Mul(transform.q, Position); + Vector2 s = input.Point1 - position; + float b = Vector2.Dot(s, s) - _2radius; + + // Solve quadratic equation. + Vector2 r = input.Point2 - input.Point1; + float c = Vector2.Dot(s, r); + float rr = Vector2.Dot(r, r); + float sigma = c * c - rr * b; + + // Check for negative discriminant and short segment. + if (sigma < 0.0f || rr < Settings.Epsilon) + { + return false; + } + + // Find the point of intersection of the line with the circle. + float a = -(c + (float)Math.Sqrt(sigma)); + + // Is the intersection point on the segment? + if (0.0f <= a && a <= input.MaxFraction * rr) + { + a /= rr; + output.Fraction = a; + + //TODO: Check results here + output.Normal = s + a * r; + output.Normal.Normalize(); + return true; + } + + return false; + } + + public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex) + { + Vector2 p = transform.p + MathUtils.Mul(transform.q, Position); + aabb.LowerBound = new Vector2(p.X - Radius, p.Y - Radius); + aabb.UpperBound = new Vector2(p.X + Radius, p.Y + Radius); + } + + protected override sealed void ComputeProperties() + { + float area = Settings.Pi * _2radius; + MassData.Area = area; + MassData.Mass = Density * area; + MassData.Centroid = Position; + + // inertia about the local origin + MassData.Inertia = MassData.Mass * (0.5f * _2radius + Vector2.Dot(Position, Position)); + } + + public override float ComputeSubmergedArea(ref Vector2 normal, float offset, ref Transform xf, out Vector2 sc) + { + sc = Vector2.Zero; + + Vector2 p = MathUtils.Mul(ref xf, Position); + float l = -(Vector2.Dot(normal, p) - offset); + if (l < -Radius + Settings.Epsilon) + { + //Completely dry + return 0; + } + if (l > Radius) + { + //Completely wet + sc = p; + return Settings.Pi * _2radius; + } + + //Magic + float l2 = l * l; + float area = _2radius * (float)((Math.Asin(l / Radius) + Settings.Pi / 2) + l * Math.Sqrt(_2radius - l2)); + float com = -2.0f / 3.0f * (float)Math.Pow(_2radius - l2, 1.5f) / area; + + sc.X = p.X + normal.X * com; + sc.Y = p.Y + normal.Y * com; + + return area; + } + + /// + /// Compare the circle to another circle + /// + /// The other circle + /// True if the two circles are the same size and have the same position + public bool CompareTo(CircleShape shape) + { + return (Radius == shape.Radius && Position == shape.Position); + } + + public override Shape Clone() + { + CircleShape clone = new CircleShape(); + clone.ShapeType = ShapeType; + clone._radius = Radius; + clone._2radius = _2radius; //FPE note: We also copy the cache + clone._density = _density; + clone._position = _position; + clone.MassData = MassData; + return clone; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Collision/Shapes/EdgeShape.cs b/Farseer Physics Engine 3.5/Collision/Shapes/EdgeShape.cs new file mode 100644 index 000000000..3857e723f --- /dev/null +++ b/Farseer Physics Engine 3.5/Collision/Shapes/EdgeShape.cs @@ -0,0 +1,252 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Collision.Shapes +{ + /// + /// A line segment (edge) shape. These can be connected in chains or loops + /// to other edge shapes. + /// The connectivity information is used to ensure correct contact normals. + /// + public class EdgeShape : Shape + { + /// + /// Edge start vertex + /// + internal Vector2 _vertex1; + + /// + /// Edge end vertex + /// + internal Vector2 _vertex2; + + internal EdgeShape() + : base(0) + { + ShapeType = ShapeType.Edge; + _radius = Settings.PolygonRadius; + } + + /// + /// Create a new EdgeShape with the specified start and end. + /// + /// The start of the edge. + /// The end of the edge. + public EdgeShape(Vector2 start, Vector2 end) + : base(0) + { + ShapeType = ShapeType.Edge; + _radius = Settings.PolygonRadius; + Set(start, end); + } + + public override int ChildCount + { + get { return 1; } + } + + /// + /// Is true if the edge is connected to an adjacent vertex before vertex 1. + /// + public bool HasVertex0 { get; set; } + + /// + /// Is true if the edge is connected to an adjacent vertex after vertex2. + /// + public bool HasVertex3 { get; set; } + + /// + /// Optional adjacent vertices. These are used for smooth collision. + /// + public Vector2 Vertex0 { get; set; } + + /// + /// Optional adjacent vertices. These are used for smooth collision. + /// + public Vector2 Vertex3 { get; set; } + + /// + /// These are the edge vertices + /// + public Vector2 Vertex1 + { + get { return _vertex1; } + set + { + _vertex1 = value; + ComputeProperties(); + } + } + + /// + /// These are the edge vertices + /// + public Vector2 Vertex2 + { + get { return _vertex2; } + set + { + _vertex2 = value; + ComputeProperties(); + } + } + + /// + /// Set this as an isolated edge. + /// + /// The start. + /// The end. + public void Set(Vector2 start, Vector2 end) + { + _vertex1 = start; + _vertex2 = end; + HasVertex0 = false; + HasVertex3 = false; + + ComputeProperties(); + } + + public override bool TestPoint(ref Transform transform, ref Vector2 point) + { + return false; + } + + public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex) + { + // p = p1 + t * d + // v = v1 + s * e + // p1 + t * d = v1 + s * e + // s * e - t * d = p1 - v1 + + output = new RayCastOutput(); + + // Put the ray into the edge's frame of reference. + Vector2 p1 = MathUtils.MulT(transform.q, input.Point1 - transform.p); + Vector2 p2 = MathUtils.MulT(transform.q, input.Point2 - transform.p); + Vector2 d = p2 - p1; + + Vector2 v1 = _vertex1; + Vector2 v2 = _vertex2; + Vector2 e = v2 - v1; + Vector2 normal = new Vector2(e.Y, -e.X); //TODO: Could possibly cache the normal. + normal.Normalize(); + + // q = p1 + t * d + // dot(normal, q - v1) = 0 + // dot(normal, p1 - v1) + t * dot(normal, d) = 0 + float numerator = Vector2.Dot(normal, v1 - p1); + float denominator = Vector2.Dot(normal, d); + + if (denominator == 0.0f) + { + return false; + } + + float t = numerator / denominator; + if (t < 0.0f || input.MaxFraction < t) + { + return false; + } + + Vector2 q = p1 + t * d; + + // q = v1 + s * r + // s = dot(q - v1, r) / dot(r, r) + Vector2 r = v2 - v1; + float rr = Vector2.Dot(r, r); + if (rr == 0.0f) + { + return false; + } + + float s = Vector2.Dot(q - v1, r) / rr; + if (s < 0.0f || 1.0f < s) + { + return false; + } + + output.Fraction = t; + if (numerator > 0.0f) + { + output.Normal = -normal; + } + else + { + output.Normal = normal; + } + return true; + } + + public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex) + { + Vector2 v1 = MathUtils.Mul(ref transform, _vertex1); + Vector2 v2 = MathUtils.Mul(ref transform, _vertex2); + + Vector2 lower = Vector2.Min(v1, v2); + Vector2 upper = Vector2.Max(v1, v2); + + Vector2 r = new Vector2(Radius, Radius); + aabb.LowerBound = lower - r; + aabb.UpperBound = upper + r; + } + + protected override void ComputeProperties() + { + MassData.Centroid = 0.5f * (_vertex1 + _vertex2); + } + + public override float ComputeSubmergedArea(ref Vector2 normal, float offset, ref Transform xf, out Vector2 sc) + { + sc = Vector2.Zero; + return 0; + } + + public bool CompareTo(EdgeShape shape) + { + return (HasVertex0 == shape.HasVertex0 && + HasVertex3 == shape.HasVertex3 && + Vertex0 == shape.Vertex0 && + Vertex1 == shape.Vertex1 && + Vertex2 == shape.Vertex2 && + Vertex3 == shape.Vertex3); + } + + public override Shape Clone() + { + EdgeShape clone = new EdgeShape(); + clone.ShapeType = ShapeType; + clone._radius = _radius; + clone._density = _density; + clone.HasVertex0 = HasVertex0; + clone.HasVertex3 = HasVertex3; + clone.Vertex0 = Vertex0; + clone._vertex1 = _vertex1; + clone._vertex2 = _vertex2; + clone.Vertex3 = Vertex3; + clone.MassData = MassData; + return clone; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Collision/Shapes/PolygonShape.cs b/Farseer Physics Engine 3.5/Collision/Shapes/PolygonShape.cs new file mode 100644 index 000000000..2480555a2 --- /dev/null +++ b/Farseer Physics Engine 3.5/Collision/Shapes/PolygonShape.cs @@ -0,0 +1,471 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System.Diagnostics; +using FarseerPhysics.Common; +using FarseerPhysics.Common.ConvexHull; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Collision.Shapes +{ + /// + /// Represents a simple non-selfintersecting convex polygon. + /// Create a convex hull from the given array of points. + /// + public class PolygonShape : Shape + { + private Vertices _vertices; + private Vertices _normals; + + /// + /// Initializes a new instance of the class. + /// + /// The vertices. + /// The density. + public PolygonShape(Vertices vertices, float density) + : base(density) + { + ShapeType = ShapeType.Polygon; + _radius = Settings.PolygonRadius; + + Vertices = vertices; + } + + /// + /// Create a new PolygonShape with the specified density. + /// + /// The density. + public PolygonShape(float density) + : base(density) + { + Debug.Assert(density >= 0f); + + ShapeType = ShapeType.Polygon; + _radius = Settings.PolygonRadius; + _vertices = new Vertices(Settings.MaxPolygonVertices); + _normals = new Vertices(Settings.MaxPolygonVertices); + } + + internal PolygonShape() + : base(0) + { + ShapeType = ShapeType.Polygon; + _radius = Settings.PolygonRadius; + _vertices = new Vertices(Settings.MaxPolygonVertices); + _normals = new Vertices(Settings.MaxPolygonVertices); + } + + /// + /// Create a convex hull from the given array of local points. + /// The number of vertices must be in the range [3, Settings.MaxPolygonVertices]. + /// Warning: the points may be re-ordered, even if they form a convex polygon + /// Warning: collinear points are handled but not removed. Collinear points may lead to poor stacking behavior. + /// + public Vertices Vertices + { + get { return _vertices; } + set + { + _vertices = new Vertices(value); + + //Debug.Assert(_vertices.Count >= 3 && _vertices.Count <= Settings.MaxPolygonVertices); + + if (Settings.UseConvexHullPolygons) + { + //FPE note: This check is required as the GiftWrap algorithm early exits on triangles + //So instead of giftwrapping a triangle, we just force it to be clock wise. + if (_vertices.Count <= 3) + _vertices.ForceCounterClockWise(); + else + _vertices = GiftWrap.GetConvexHull(_vertices); + } + + _normals = new Vertices(_vertices.Count); + + // Compute normals. Ensure the edges have non-zero length. + for (int i = 0; i < _vertices.Count; ++i) + { + int next = i + 1 < _vertices.Count ? i + 1 : 0; + Vector2 edge = _vertices[next] - _vertices[i]; + Debug.Assert(edge.LengthSquared() > Settings.Epsilon * Settings.Epsilon); + + //FPE optimization: Normals.Add(MathHelper.Cross(edge, 1.0f)); + Vector2 temp = new Vector2(edge.Y, -edge.X); + temp.Normalize(); + _normals.Add(temp); + } + + // Compute the polygon mass data + ComputeProperties(); + } + } + + public Vertices Normals { get { return _normals; } } + + public override int ChildCount { get { return 1; } } + + protected override void ComputeProperties() + { + // Polygon mass, centroid, and inertia. + // Let rho be the polygon density in mass per unit area. + // Then: + // mass = rho * int(dA) + // centroid.X = (1/mass) * rho * int(x * dA) + // centroid.Y = (1/mass) * rho * int(y * dA) + // I = rho * int((x*x + y*y) * dA) + // + // We can compute these integrals by summing all the integrals + // for each triangle of the polygon. To evaluate the integral + // for a single triangle, we make a change of variables to + // the (u,v) coordinates of the triangle: + // x = x0 + e1x * u + e2x * v + // y = y0 + e1y * u + e2y * v + // where 0 <= u && 0 <= v && u + v <= 1. + // + // We integrate u from [0,1-v] and then v from [0,1]. + // We also need to use the Jacobian of the transformation: + // D = cross(e1, e2) + // + // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3) + // + // The rest of the derivation is handled by computer algebra. + + Debug.Assert(Vertices.Count >= 3); + + //FPE optimization: Early exit as polygons with 0 density does not have any properties. + if (_density <= 0) + return; + + //FPE optimization: Consolidated the calculate centroid and mass code to a single method. + Vector2 center = Vector2.Zero; + float area = 0.0f; + float I = 0.0f; + + // pRef is the reference point for forming triangles. + // It's location doesn't change the result (except for rounding error). + Vector2 s = Vector2.Zero; + + // This code would put the reference point inside the polygon. + for (int i = 0; i < Vertices.Count; ++i) + { + s += Vertices[i]; + } + s *= 1.0f / Vertices.Count; + + const float k_inv3 = 1.0f / 3.0f; + + for (int i = 0; i < Vertices.Count; ++i) + { + // Triangle vertices. + Vector2 e1 = Vertices[i] - s; + Vector2 e2 = i + 1 < Vertices.Count ? Vertices[i + 1] - s : Vertices[0] - s; + + float D = MathUtils.Cross(e1, e2); + + float triangleArea = 0.5f * D; + area += triangleArea; + + // Area weighted centroid + center += triangleArea * k_inv3 * (e1 + e2); + + float ex1 = e1.X, ey1 = e1.Y; + float ex2 = e2.X, ey2 = e2.Y; + + float intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2; + float inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2; + + I += (0.25f * k_inv3 * D) * (intx2 + inty2); + } + + //The area is too small for the engine to handle. + Debug.Assert(area > Settings.Epsilon); + + // We save the area + MassData.Area = area; + + // Total mass + MassData.Mass = _density * area; + + // Center of mass + center *= 1.0f / area; + MassData.Centroid = center + s; + + // Inertia tensor relative to the local origin (point s). + MassData.Inertia = _density * I; + + // Shift to center of mass then to original body origin. + MassData.Inertia += MassData.Mass * (Vector2.Dot(MassData.Centroid, MassData.Centroid) - Vector2.Dot(center, center)); + } + + public override bool TestPoint(ref Transform transform, ref Vector2 point) + { + Vector2 pLocal = MathUtils.MulT(transform.q, point - transform.p); + + for (int i = 0; i < Vertices.Count; ++i) + { + float dot = Vector2.Dot(Normals[i], pLocal - Vertices[i]); + if (dot > 0.0f) + { + return false; + } + } + + return true; + } + + public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex) + { + output = new RayCastOutput(); + + // Put the ray into the polygon's frame of reference. + Vector2 p1 = MathUtils.MulT(transform.q, input.Point1 - transform.p); + Vector2 p2 = MathUtils.MulT(transform.q, input.Point2 - transform.p); + Vector2 d = p2 - p1; + + float lower = 0.0f, upper = input.MaxFraction; + + int index = -1; + + for (int i = 0; i < Vertices.Count; ++i) + { + // p = p1 + a * d + // dot(normal, p - v) = 0 + // dot(normal, p1 - v) + a * dot(normal, d) = 0 + float numerator = Vector2.Dot(Normals[i], Vertices[i] - p1); + float denominator = Vector2.Dot(Normals[i], d); + + if (denominator == 0.0f) + { + if (numerator < 0.0f) + { + return false; + } + } + else + { + // Note: we want this predicate without division: + // lower < numerator / denominator, where denominator < 0 + // Since denominator < 0, we have to flip the inequality: + // lower < numerator / denominator <==> denominator * lower > numerator. + if (denominator < 0.0f && numerator < lower * denominator) + { + // Increase lower. + // The segment enters this half-space. + lower = numerator / denominator; + index = i; + } + else if (denominator > 0.0f && numerator < upper * denominator) + { + // Decrease upper. + // The segment exits this half-space. + upper = numerator / denominator; + } + } + + // The use of epsilon here causes the assert on lower to trip + // in some cases. Apparently the use of epsilon was to make edge + // shapes work, but now those are handled separately. + //if (upper < lower - b2_epsilon) + if (upper < lower) + { + return false; + } + } + + Debug.Assert(0.0f <= lower && lower <= input.MaxFraction); + + if (index >= 0) + { + output.Fraction = lower; + output.Normal = MathUtils.Mul(transform.q, Normals[index]); + return true; + } + + return false; + } + + /// + /// Given a transform, compute the associated axis aligned bounding box for a child shape. + /// + /// The aabb results. + /// The world transform of the shape. + /// The child shape index. + public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex) + { + Vector2 lower = MathUtils.Mul(ref transform, Vertices[0]); + Vector2 upper = lower; + + for (int i = 1; i < Vertices.Count; ++i) + { + Vector2 v = MathUtils.Mul(ref transform, Vertices[i]); + lower = Vector2.Min(lower, v); + upper = Vector2.Max(upper, v); + } + + Vector2 r = new Vector2(Radius, Radius); + aabb.LowerBound = lower - r; + aabb.UpperBound = upper + r; + } + + public override float ComputeSubmergedArea(ref Vector2 normal, float offset, ref Transform xf, out Vector2 sc) + { + sc = Vector2.Zero; + + //Transform plane into shape co-ordinates + Vector2 normalL = MathUtils.MulT(xf.q, normal); + float offsetL = offset - Vector2.Dot(normal, xf.p); + + float[] depths = new float[Settings.MaxPolygonVertices]; + int diveCount = 0; + int intoIndex = -1; + int outoIndex = -1; + + bool lastSubmerged = false; + int i; + for (i = 0; i < Vertices.Count; i++) + { + depths[i] = Vector2.Dot(normalL, Vertices[i]) - offsetL; + bool isSubmerged = depths[i] < -Settings.Epsilon; + if (i > 0) + { + if (isSubmerged) + { + if (!lastSubmerged) + { + intoIndex = i - 1; + diveCount++; + } + } + else + { + if (lastSubmerged) + { + outoIndex = i - 1; + diveCount++; + } + } + } + lastSubmerged = isSubmerged; + } + switch (diveCount) + { + case 0: + if (lastSubmerged) + { + //Completely submerged + sc = MathUtils.Mul(ref xf, MassData.Centroid); + return MassData.Mass / Density; + } + + //Completely dry + return 0; + case 1: + if (intoIndex == -1) + { + intoIndex = Vertices.Count - 1; + } + else + { + outoIndex = Vertices.Count - 1; + } + break; + } + + int intoIndex2 = (intoIndex + 1) % Vertices.Count; + int outoIndex2 = (outoIndex + 1) % Vertices.Count; + + float intoLambda = (0 - depths[intoIndex]) / (depths[intoIndex2] - depths[intoIndex]); + float outoLambda = (0 - depths[outoIndex]) / (depths[outoIndex2] - depths[outoIndex]); + + Vector2 intoVec = new Vector2(Vertices[intoIndex].X * (1 - intoLambda) + Vertices[intoIndex2].X * intoLambda, Vertices[intoIndex].Y * (1 - intoLambda) + Vertices[intoIndex2].Y * intoLambda); + Vector2 outoVec = new Vector2(Vertices[outoIndex].X * (1 - outoLambda) + Vertices[outoIndex2].X * outoLambda, Vertices[outoIndex].Y * (1 - outoLambda) + Vertices[outoIndex2].Y * outoLambda); + + //Initialize accumulator + float area = 0; + Vector2 center = new Vector2(0, 0); + Vector2 p2 = Vertices[intoIndex2]; + + const float k_inv3 = 1.0f / 3.0f; + + //An awkward loop from intoIndex2+1 to outIndex2 + i = intoIndex2; + while (i != outoIndex2) + { + i = (i + 1) % Vertices.Count; + Vector2 p3; + if (i == outoIndex2) + p3 = outoVec; + else + p3 = Vertices[i]; + //Add the triangle formed by intoVec,p2,p3 + { + Vector2 e1 = p2 - intoVec; + Vector2 e2 = p3 - intoVec; + + float D = MathUtils.Cross(e1, e2); + + float triangleArea = 0.5f * D; + + area += triangleArea; + + // Area weighted centroid + center += triangleArea * k_inv3 * (intoVec + p2 + p3); + } + + p2 = p3; + } + + //Normalize and transform centroid + center *= 1.0f / area; + + sc = MathUtils.Mul(ref xf, center); + + return area; + } + + public bool CompareTo(PolygonShape shape) + { + if (Vertices.Count != shape.Vertices.Count) + return false; + + for (int i = 0; i < Vertices.Count; i++) + { + if (Vertices[i] != shape.Vertices[i]) + return false; + } + + return (Radius == shape.Radius && MassData == shape.MassData); + } + + public override Shape Clone() + { + PolygonShape clone = new PolygonShape(); + clone.ShapeType = ShapeType; + clone._radius = _radius; + clone._density = _density; + clone._vertices = new Vertices(_vertices); + clone._normals = new Vertices(_normals); + clone.MassData = MassData; + return clone; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Collision/Shapes/Shape.cs b/Farseer Physics Engine 3.5/Collision/Shapes/Shape.cs new file mode 100644 index 000000000..8452afcfd --- /dev/null +++ b/Farseer Physics Engine 3.5/Collision/Shapes/Shape.cs @@ -0,0 +1,255 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Collision.Shapes +{ + /// + /// This holds the mass data computed for a shape. + /// + public struct MassData : IEquatable + { + /// + /// The area of the shape + /// + public float Area { get; internal set; } + + /// + /// The position of the shape's centroid relative to the shape's origin. + /// + public Vector2 Centroid { get; internal set; } + + /// + /// The rotational inertia of the shape about the local origin. + /// + public float Inertia { get; internal set; } + + /// + /// The mass of the shape, usually in kilograms. + /// + public float Mass { get; internal set; } + + /// + /// The equal operator + /// + /// + /// + /// + public static bool operator ==(MassData left, MassData right) + { + return (left.Area == right.Area && left.Mass == right.Mass && left.Centroid == right.Centroid && left.Inertia == right.Inertia); + } + + /// + /// The not equal operator + /// + /// + /// + /// + public static bool operator !=(MassData left, MassData right) + { + return !(left == right); + } + + public bool Equals(MassData other) + { + return this == other; + } + + public override bool Equals(object obj) + { + if (ReferenceEquals(null, obj)) + return false; + + if (obj.GetType() != typeof(MassData)) + return false; + + return Equals((MassData)obj); + } + + public override int GetHashCode() + { + unchecked + { + int result = Area.GetHashCode(); + result = (result * 397) ^ Centroid.GetHashCode(); + result = (result * 397) ^ Inertia.GetHashCode(); + result = (result * 397) ^ Mass.GetHashCode(); + return result; + } + } + } + + public enum ShapeType + { + Unknown = -1, + Circle = 0, + Edge = 1, + Polygon = 2, + Chain = 3, + TypeCount = 4, + } + + /// + /// A shape is used for collision detection. You can create a shape however you like. + /// Shapes used for simulation in World are created automatically when a Fixture + /// is created. Shapes may encapsulate a one or more child shapes. + /// + public abstract class Shape + { + internal float _density; + internal float _radius; + internal float _2radius; + + protected Shape(float density) + { + _density = density; + ShapeType = ShapeType.Unknown; + } + + /// + /// Contains the properties of the shape such as: + /// - Area of the shape + /// - Centroid + /// - Inertia + /// - Mass + /// + public MassData MassData; + + /// + /// Get the type of this shape. + /// + /// The type of the shape. + public ShapeType ShapeType { get; internal set; } + + /// + /// Get the number of child primitives. + /// + /// + public abstract int ChildCount { get; } + + /// + /// Gets or sets the density. + /// Changing the density causes a recalculation of shape properties. + /// + /// The density. + public float Density + { + get { return _density; } + set + { + Debug.Assert(value >= 0); + + _density = value; + ComputeProperties(); + } + } + + /// + /// Radius of the Shape + /// Changing the radius causes a recalculation of shape properties. + /// + public float Radius + { + get { return _radius; } + set + { + Debug.Assert(value >= 0); + + _radius = value; + _2radius = _radius * _radius; + + ComputeProperties(); + } + } + + /// + /// Clone the concrete shape + /// + /// A clone of the shape + public abstract Shape Clone(); + + /// + /// Test a point for containment in this shape. + /// Note: This only works for convex shapes. + /// + /// The shape world transform. + /// A point in world coordinates. + /// True if the point is inside the shape + public abstract bool TestPoint(ref Transform transform, ref Vector2 point); + + /// + /// Cast a ray against a child shape. + /// + /// The ray-cast results. + /// The ray-cast input parameters. + /// The transform to be applied to the shape. + /// The child shape index. + /// True if the ray-cast hits the shape + public abstract bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex); + + /// + /// Given a transform, compute the associated axis aligned bounding box for a child shape. + /// + /// The aabb results. + /// The world transform of the shape. + /// The child shape index. + public abstract void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex); + + /// + /// Compute the mass properties of this shape using its dimensions and density. + /// The inertia tensor is computed about the local origin, not the centroid. + /// + protected abstract void ComputeProperties(); + + /// + /// Compare this shape to another shape based on type and properties. + /// + /// The other shape + /// True if the two shapes are the same. + public bool CompareTo(Shape shape) + { + if (shape is PolygonShape && this is PolygonShape) + return ((PolygonShape)this).CompareTo((PolygonShape)shape); + + if (shape is CircleShape && this is CircleShape) + return ((CircleShape)this).CompareTo((CircleShape)shape); + + if (shape is EdgeShape && this is EdgeShape) + return ((EdgeShape)this).CompareTo((EdgeShape)shape); + + if (shape is ChainShape && this is ChainShape) + return ((ChainShape)this).CompareTo((ChainShape)shape); + + return false; + } + + /// + /// Used for the buoyancy controller + /// + public abstract float ComputeSubmergedArea(ref Vector2 normal, float offset, ref Transform xf, out Vector2 sc); + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Collision/TimeOfImpact.cs b/Farseer Physics Engine 3.5/Collision/TimeOfImpact.cs new file mode 100644 index 000000000..ecc2ff192 --- /dev/null +++ b/Farseer Physics Engine 3.5/Collision/TimeOfImpact.cs @@ -0,0 +1,498 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Collision +{ + /// + /// Input parameters for CalculateTimeOfImpact + /// + public class TOIInput + { + public DistanceProxy ProxyA = new DistanceProxy(); + public DistanceProxy ProxyB = new DistanceProxy(); + public Sweep SweepA; + public Sweep SweepB; + public float TMax; // defines sweep interval [0, tMax] + } + + public enum TOIOutputState + { + Unknown, + Failed, + Overlapped, + Touching, + Seperated, + } + + public struct TOIOutput + { + public TOIOutputState State; + public float T; + } + + public enum SeparationFunctionType + { + Points, + FaceA, + FaceB + } + + public static class SeparationFunction + { + [ThreadStatic] + private static Vector2 _axis; + [ThreadStatic] + private static Vector2 _localPoint; + [ThreadStatic] + private static DistanceProxy _proxyA; + [ThreadStatic] + private static DistanceProxy _proxyB; + [ThreadStatic] + private static Sweep _sweepA, _sweepB; + [ThreadStatic] + private static SeparationFunctionType _type; + + public static void Set(ref SimplexCache cache, DistanceProxy proxyA, ref Sweep sweepA, DistanceProxy proxyB, ref Sweep sweepB, float t1) + { + _localPoint = Vector2.Zero; + _proxyA = proxyA; + _proxyB = proxyB; + int count = cache.Count; + Debug.Assert(0 < count && count < 3); + + _sweepA = sweepA; + _sweepB = sweepB; + + Transform xfA, xfB; + _sweepA.GetTransform(out xfA, t1); + _sweepB.GetTransform(out xfB, t1); + + if (count == 1) + { + _type = SeparationFunctionType.Points; + Vector2 localPointA = _proxyA.Vertices[cache.IndexA[0]]; + Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]]; + Vector2 pointA = MathUtils.Mul(ref xfA, localPointA); + Vector2 pointB = MathUtils.Mul(ref xfB, localPointB); + _axis = pointB - pointA; + _axis.Normalize(); + } + else if (cache.IndexA[0] == cache.IndexA[1]) + { + // Two points on B and one on A. + _type = SeparationFunctionType.FaceB; + Vector2 localPointB1 = proxyB.Vertices[cache.IndexB[0]]; + Vector2 localPointB2 = proxyB.Vertices[cache.IndexB[1]]; + + Vector2 a = localPointB2 - localPointB1; + _axis = new Vector2(a.Y, -a.X); + _axis.Normalize(); + Vector2 normal = MathUtils.Mul(ref xfB.q, _axis); + + _localPoint = 0.5f * (localPointB1 + localPointB2); + Vector2 pointB = MathUtils.Mul(ref xfB, _localPoint); + + Vector2 localPointA = proxyA.Vertices[cache.IndexA[0]]; + Vector2 pointA = MathUtils.Mul(ref xfA, localPointA); + + float s = Vector2.Dot(pointA - pointB, normal); + if (s < 0.0f) + { + _axis = -_axis; + } + } + else + { + // Two points on A and one or two points on B. + _type = SeparationFunctionType.FaceA; + Vector2 localPointA1 = _proxyA.Vertices[cache.IndexA[0]]; + Vector2 localPointA2 = _proxyA.Vertices[cache.IndexA[1]]; + + Vector2 a = localPointA2 - localPointA1; + _axis = new Vector2(a.Y, -a.X); + _axis.Normalize(); + Vector2 normal = MathUtils.Mul(ref xfA.q, _axis); + + _localPoint = 0.5f * (localPointA1 + localPointA2); + Vector2 pointA = MathUtils.Mul(ref xfA, _localPoint); + + Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]]; + Vector2 pointB = MathUtils.Mul(ref xfB, localPointB); + + float s = Vector2.Dot(pointB - pointA, normal); + if (s < 0.0f) + { + _axis = -_axis; + } + } + + //FPE note: the returned value that used to be here has been removed, as it was not used. + } + + public static float FindMinSeparation(out int indexA, out int indexB, float t) + { + Transform xfA, xfB; + _sweepA.GetTransform(out xfA, t); + _sweepB.GetTransform(out xfB, t); + + switch (_type) + { + case SeparationFunctionType.Points: + { + Vector2 axisA = MathUtils.MulT(ref xfA.q, _axis); + Vector2 axisB = MathUtils.MulT(ref xfB.q, -_axis); + + indexA = _proxyA.GetSupport(axisA); + indexB = _proxyB.GetSupport(axisB); + + Vector2 localPointA = _proxyA.Vertices[indexA]; + Vector2 localPointB = _proxyB.Vertices[indexB]; + + Vector2 pointA = MathUtils.Mul(ref xfA, localPointA); + Vector2 pointB = MathUtils.Mul(ref xfB, localPointB); + + float separation = Vector2.Dot(pointB - pointA, _axis); + return separation; + } + + case SeparationFunctionType.FaceA: + { + Vector2 normal = MathUtils.Mul(ref xfA.q, _axis); + Vector2 pointA = MathUtils.Mul(ref xfA, _localPoint); + + Vector2 axisB = MathUtils.MulT(ref xfB.q, -normal); + + indexA = -1; + indexB = _proxyB.GetSupport(axisB); + + Vector2 localPointB = _proxyB.Vertices[indexB]; + Vector2 pointB = MathUtils.Mul(ref xfB, localPointB); + + float separation = Vector2.Dot(pointB - pointA, normal); + return separation; + } + + case SeparationFunctionType.FaceB: + { + Vector2 normal = MathUtils.Mul(ref xfB.q, _axis); + Vector2 pointB = MathUtils.Mul(ref xfB, _localPoint); + + Vector2 axisA = MathUtils.MulT(ref xfA.q, -normal); + + indexB = -1; + indexA = _proxyA.GetSupport(axisA); + + Vector2 localPointA = _proxyA.Vertices[indexA]; + Vector2 pointA = MathUtils.Mul(ref xfA, localPointA); + + float separation = Vector2.Dot(pointA - pointB, normal); + return separation; + } + + default: + Debug.Assert(false); + indexA = -1; + indexB = -1; + return 0.0f; + } + } + + public static float Evaluate(int indexA, int indexB, float t) + { + Transform xfA, xfB; + _sweepA.GetTransform(out xfA, t); + _sweepB.GetTransform(out xfB, t); + + switch (_type) + { + case SeparationFunctionType.Points: + { + Vector2 localPointA = _proxyA.Vertices[indexA]; + Vector2 localPointB = _proxyB.Vertices[indexB]; + + Vector2 pointA = MathUtils.Mul(ref xfA, localPointA); + Vector2 pointB = MathUtils.Mul(ref xfB, localPointB); + float separation = Vector2.Dot(pointB - pointA, _axis); + + return separation; + } + case SeparationFunctionType.FaceA: + { + Vector2 normal = MathUtils.Mul(ref xfA.q, _axis); + Vector2 pointA = MathUtils.Mul(ref xfA, _localPoint); + + Vector2 localPointB = _proxyB.Vertices[indexB]; + Vector2 pointB = MathUtils.Mul(ref xfB, localPointB); + + float separation = Vector2.Dot(pointB - pointA, normal); + return separation; + } + case SeparationFunctionType.FaceB: + { + Vector2 normal = MathUtils.Mul(ref xfB.q, _axis); + Vector2 pointB = MathUtils.Mul(ref xfB, _localPoint); + + Vector2 localPointA = _proxyA.Vertices[indexA]; + Vector2 pointA = MathUtils.Mul(ref xfA, localPointA); + + float separation = Vector2.Dot(pointA - pointB, normal); + return separation; + } + default: + Debug.Assert(false); + return 0.0f; + } + } + } + + public static class TimeOfImpact + { + // CCD via the local separating axis method. This seeks progression + // by computing the largest time at which separation is maintained. + + [ThreadStatic] + public static int TOICalls, TOIIters, TOIMaxIters; + [ThreadStatic] + public static int TOIRootIters, TOIMaxRootIters; + [ThreadStatic] + private static DistanceInput _distanceInput; + + /// + /// Compute the upper bound on time before two shapes penetrate. Time is represented as + /// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate, + /// non-tunneling collision. If you change the time interval, you should call this function + /// again. + /// Note: use Distance() to compute the contact point and normal at the time of impact. + /// + /// The output. + /// The input. + public static void CalculateTimeOfImpact(out TOIOutput output, TOIInput input) + { + if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled + ++TOICalls; + + output = new TOIOutput(); + output.State = TOIOutputState.Unknown; + output.T = input.TMax; + + Sweep sweepA = input.SweepA; + Sweep sweepB = input.SweepB; + + // Large rotations can make the root finder fail, so we normalize the + // sweep angles. + sweepA.Normalize(); + sweepB.Normalize(); + + float tMax = input.TMax; + + float totalRadius = input.ProxyA.Radius + input.ProxyB.Radius; + float target = Math.Max(Settings.LinearSlop, totalRadius - 3.0f * Settings.LinearSlop); + const float tolerance = 0.25f * Settings.LinearSlop; + Debug.Assert(target > tolerance); + + float t1 = 0.0f; + const int k_maxIterations = 20; + int iter = 0; + + // Prepare input for distance query. + _distanceInput = _distanceInput ?? new DistanceInput(); + _distanceInput.ProxyA = input.ProxyA; + _distanceInput.ProxyB = input.ProxyB; + _distanceInput.UseRadii = false; + + // The outer loop progressively attempts to compute new separating axes. + // This loop terminates when an axis is repeated (no progress is made). + for (; ; ) + { + Transform xfA, xfB; + sweepA.GetTransform(out xfA, t1); + sweepB.GetTransform(out xfB, t1); + + // Get the distance between shapes. We can also use the results + // to get a separating axis. + _distanceInput.TransformA = xfA; + _distanceInput.TransformB = xfB; + DistanceOutput distanceOutput; + SimplexCache cache; + Distance.ComputeDistance(out distanceOutput, out cache, _distanceInput); + + // If the shapes are overlapped, we give up on continuous collision. + if (distanceOutput.Distance <= 0.0f) + { + // Failure! + output.State = TOIOutputState.Overlapped; + output.T = 0.0f; + break; + } + + if (distanceOutput.Distance < target + tolerance) + { + // Victory! + output.State = TOIOutputState.Touching; + output.T = t1; + break; + } + + SeparationFunction.Set(ref cache, input.ProxyA, ref sweepA, input.ProxyB, ref sweepB, t1); + + // Compute the TOI on the separating axis. We do this by successively + // resolving the deepest point. This loop is bounded by the number of vertices. + bool done = false; + float t2 = tMax; + int pushBackIter = 0; + for (; ; ) + { + // Find the deepest point at t2. Store the witness point indices. + int indexA, indexB; + float s2 = SeparationFunction.FindMinSeparation(out indexA, out indexB, t2); + + // Is the final configuration separated? + if (s2 > target + tolerance) + { + // Victory! + output.State = TOIOutputState.Seperated; + output.T = tMax; + done = true; + break; + } + + // Has the separation reached tolerance? + if (s2 > target - tolerance) + { + // Advance the sweeps + t1 = t2; + break; + } + + // Compute the initial separation of the witness points. + float s1 = SeparationFunction.Evaluate(indexA, indexB, t1); + + // Check for initial overlap. This might happen if the root finder + // runs out of iterations. + if (s1 < target - tolerance) + { + output.State = TOIOutputState.Failed; + output.T = t1; + done = true; + break; + } + + // Check for touching + if (s1 <= target + tolerance) + { + // Victory! t1 should hold the TOI (could be 0.0). + output.State = TOIOutputState.Touching; + output.T = t1; + done = true; + break; + } + + // Compute 1D root of: f(x) - target = 0 + int rootIterCount = 0; + float a1 = t1, a2 = t2; + for (; ; ) + { + // Use a mix of the secant rule and bisection. + float t; + if ((rootIterCount & 1) != 0) + { + // Secant rule to improve convergence. + t = a1 + (target - s1) * (a2 - a1) / (s2 - s1); + } + else + { + // Bisection to guarantee progress. + t = 0.5f * (a1 + a2); + } + + ++rootIterCount; + + if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled + ++TOIRootIters; + + float s = SeparationFunction.Evaluate(indexA, indexB, t); + + if (Math.Abs(s - target) < tolerance) + { + // t2 holds a tentative value for t1 + t2 = t; + break; + } + + // Ensure we continue to bracket the root. + if (s > target) + { + a1 = t; + s1 = s; + } + else + { + a2 = t; + s2 = s; + } + + if (rootIterCount == 50) + { + break; + } + } + + if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled + TOIMaxRootIters = Math.Max(TOIMaxRootIters, rootIterCount); + + ++pushBackIter; + + if (pushBackIter == Settings.MaxPolygonVertices) + { + break; + } + } + + ++iter; + + if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled + ++TOIIters; + + if (done) + { + break; + } + + if (iter == k_maxIterations) + { + // Root finder got stuck. Semi-victory. + output.State = TOIOutputState.Failed; + output.T = t1; + break; + } + } + + if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled + TOIMaxIters = Math.Max(TOIMaxIters, iter); + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/ConvexHull/ChainHull.cs b/Farseer Physics Engine 3.5/Common/ConvexHull/ChainHull.cs new file mode 100644 index 000000000..3aaa1fe8b --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/ConvexHull/ChainHull.cs @@ -0,0 +1,143 @@ +using System.Collections.Generic; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common.ConvexHull +{ + /// + /// Andrew's Monotone Chain Convex Hull algorithm. + /// Used to get the convex hull of a point cloud. + /// + /// Source: http://www.softsurfer.com/Archive/algorithm_0109/algorithm_0109.htm + /// + public static class ChainHull + { + //Copyright 2001, softSurfer (www.softsurfer.com) + + private static PointComparer _pointComparer = new PointComparer(); + + /// + /// Returns the convex hull from the given vertices.. + /// + public static Vertices GetConvexHull(Vertices vertices) + { + if (vertices.Count <= 3) + return vertices; + + Vertices pointSet = new Vertices(vertices); + + //Sort by X-axis + pointSet.Sort(_pointComparer); + + Vector2[] h = new Vector2[pointSet.Count]; + Vertices res; + + int top = -1; // indices for bottom and top of the stack + int i; // array scan index + + // Get the indices of points with min x-coord and min|max y-coord + const int minmin = 0; + float xmin = pointSet[0].X; + for (i = 1; i < pointSet.Count; i++) + { + if (pointSet[i].X != xmin) + break; + } + + // degenerate case: all x-coords == xmin + int minmax = i - 1; + if (minmax == pointSet.Count - 1) + { + h[++top] = pointSet[minmin]; + + if (pointSet[minmax].Y != pointSet[minmin].Y) // a nontrivial segment + h[++top] = pointSet[minmax]; + + h[++top] = pointSet[minmin]; // add polygon endpoint + + res = new Vertices(top + 1); + for (int j = 0; j < top + 1; j++) + { + res.Add(h[j]); + } + + return res; + } + + top = -1; + + // Get the indices of points with max x-coord and min|max y-coord + int maxmax = pointSet.Count - 1; + float xmax = pointSet[pointSet.Count - 1].X; + for (i = pointSet.Count - 2; i >= 0; i--) + { + if (pointSet[i].X != xmax) + break; + } + int maxmin = i + 1; + + // Compute the lower hull on the stack H + h[++top] = pointSet[minmin]; // push minmin point onto stack + i = minmax; + while (++i <= maxmin) + { + // the lower line joins P[minmin] with P[maxmin] + if (MathUtils.Area(pointSet[minmin], pointSet[maxmin], pointSet[i]) >= 0 && i < maxmin) + continue; // ignore P[i] above or on the lower line + + while (top > 0) // there are at least 2 points on the stack + { + // test if P[i] is left of the line at the stack top + if (MathUtils.Area(h[top - 1], h[top], pointSet[i]) > 0) + break; // P[i] is a new hull vertex + + top--; // pop top point off stack + } + h[++top] = pointSet[i]; // push P[i] onto stack + } + + // Next, compute the upper hull on the stack H above the bottom hull + if (maxmax != maxmin) // if distinct xmax points + h[++top] = pointSet[maxmax]; // push maxmax point onto stack + int bot = top; + i = maxmin; + while (--i >= minmax) + { + // the upper line joins P[maxmax] with P[minmax] + if (MathUtils.Area(pointSet[maxmax], pointSet[minmax], pointSet[i]) >= 0 && i > minmax) + continue; // ignore P[i] below or on the upper line + + while (top > bot) // at least 2 points on the upper stack + { + // test if P[i] is left of the line at the stack top + if (MathUtils.Area(h[top - 1], h[top], pointSet[i]) > 0) + break; // P[i] is a new hull vertex + + top--; // pop top point off stack + } + + h[++top] = pointSet[i]; // push P[i] onto stack + } + + if (minmax != minmin) + h[++top] = pointSet[minmin]; // push joining endpoint onto stack + + res = new Vertices(top + 1); + + for (int j = 0; j < top + 1; j++) + { + res.Add(h[j]); + } + + return res; + } + + private class PointComparer : Comparer + { + public override int Compare(Vector2 a, Vector2 b) + { + int f = a.X.CompareTo(b.X); + return f != 0 ? f : a.Y.CompareTo(b.Y); + } + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/ConvexHull/GiftWrap.cs b/Farseer Physics Engine 3.5/Common/ConvexHull/GiftWrap.cs new file mode 100644 index 000000000..5165095f2 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/ConvexHull/GiftWrap.cs @@ -0,0 +1,90 @@ +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common.ConvexHull +{ + /// + /// Giftwrap convex hull algorithm. + /// O(nh) time complexity, where n is the number of points and h is the number of points on the convex hull. + /// + /// See http://en.wikipedia.org/wiki/Gift_wrapping_algorithm for more details. + /// + public static class GiftWrap + { + //Extracted from Box2D + + /// + /// Returns the convex hull from the given vertices. + /// + /// The vertices. + public static Vertices GetConvexHull(Vertices vertices) + { + if (vertices.Count <= 3) + return vertices; + + // Find the right most point on the hull + int i0 = 0; + float x0 = vertices[0].X; + for (int i = 1; i < vertices.Count; ++i) + { + float x = vertices[i].X; + if (x > x0 || (x == x0 && vertices[i].Y < vertices[i0].Y)) + { + i0 = i; + x0 = x; + } + } + + int[] hull = new int[vertices.Count]; + int m = 0; + int ih = i0; + + for (; ; ) + { + hull[m] = ih; + + int ie = 0; + for (int j = 1; j < vertices.Count; ++j) + { + if (ie == ih) + { + ie = j; + continue; + } + + Vector2 r = vertices[ie] - vertices[hull[m]]; + Vector2 v = vertices[j] - vertices[hull[m]]; + + float c = MathUtils.Cross(ref r, ref v); + if (c < 0.0f) + { + ie = j; + } + + // Collinearity check + if (c == 0.0f && v.LengthSquared() > r.LengthSquared()) + { + ie = j; + } + } + + ++m; + ih = ie; + + if (ie == i0) + { + break; + } + } + + Vertices result = new Vertices(m); + + // Copy vertices. + for (int i = 0; i < m; ++i) + { + result.Add(vertices[hull[i]]); + } + + return result; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/ConvexHull/Melkman.cs b/Farseer Physics Engine 3.5/Common/ConvexHull/Melkman.cs new file mode 100644 index 000000000..514cf85e8 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/ConvexHull/Melkman.cs @@ -0,0 +1,132 @@ +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common.ConvexHull +{ + /// + /// Creates a convex hull. + /// Note: + /// 1. Vertices must be of a simple polygon, i.e. edges do not overlap. + /// 2. Melkman does not work on point clouds + /// + /// + /// Implemented using Melkman's Convex Hull Algorithm - O(n) time complexity. + /// Reference: http://www.ams.sunysb.edu/~jsbm/courses/345/melkman.pdf + /// + public static class Melkman + { + //Melkman based convex hull algorithm contributed by Cowdozer + + /// + /// Returns a convex hull from the given vertices. + /// + /// A convex hull in counter clockwise winding order. + public static Vertices GetConvexHull(Vertices vertices) + { + if (vertices.Count <= 3) + return vertices; + + //We'll never need a queue larger than the current number of Vertices +1 + //Create double-ended queue + Vector2[] deque = new Vector2[vertices.Count + 1]; + int qf = 3, qb = 0; //Queue front index, queue back index + + //Start by placing first 3 vertices in convex CCW order + int startIndex = 3; + float k = MathUtils.Area(vertices[0], vertices[1], vertices[2]); + if (k == 0) + { + //Vertices are collinear. + deque[0] = vertices[0]; + deque[1] = vertices[2]; //We can skip vertex 1 because it should be between 0 and 2 + deque[2] = vertices[0]; + qf = 2; + + //Go until the end of the collinear sequence of vertices + for (startIndex = 3; startIndex < vertices.Count; startIndex++) + { + Vector2 tmp = vertices[startIndex]; + if (MathUtils.Area(ref deque[0], ref deque[1], ref tmp) == 0) //This point is also collinear + deque[1] = vertices[startIndex]; + else break; + } + } + else + { + deque[0] = deque[3] = vertices[2]; + if (k > 0) + { + //Is Left. Set deque = {2, 0, 1, 2} + deque[1] = vertices[0]; + deque[2] = vertices[1]; + } + else + { + //Is Right. Set deque = {2, 1, 0, 2} + deque[1] = vertices[1]; + deque[2] = vertices[0]; + } + } + + int qfm1 = qf == 0 ? deque.Length - 1 : qf - 1; + int qbm1 = qb == deque.Length - 1 ? 0 : qb + 1; + + //Add vertices one at a time and adjust convex hull as needed + for (int i = startIndex; i < vertices.Count; i++) + { + Vector2 nextPt = vertices[i]; + + //Ignore if it is already within the convex hull we have constructed + if (MathUtils.Area(ref deque[qfm1], ref deque[qf], ref nextPt) > 0 && MathUtils.Area(ref deque[qb], ref deque[qbm1], ref nextPt) > 0) + continue; + + //Pop front until convex + while (!(MathUtils.Area(ref deque[qfm1], ref deque[qf], ref nextPt) > 0)) + { + //Pop the front element from the queue + qf = qfm1; //qf--; + qfm1 = qf == 0 ? deque.Length - 1 : qf - 1; //qfm1 = qf - 1; + } + + //Add vertex to the front of the queue + qf = qf == deque.Length - 1 ? 0 : qf + 1; //qf++; + qfm1 = qf == 0 ? deque.Length - 1 : qf - 1; //qfm1 = qf - 1; + deque[qf] = nextPt; + + //Pop back until convex + while (!(MathUtils.Area(ref deque[qb], ref deque[qbm1], ref nextPt) > 0)) + { + //Pop the back element from the queue + qb = qbm1; //qb++; + qbm1 = qb == deque.Length - 1 ? 0 : qb + 1; //qbm1 = qb + 1; + } + //Add vertex to the back of the queue + qb = qb == 0 ? deque.Length - 1 : qb - 1; //qb--; + qbm1 = qb == deque.Length - 1 ? 0 : qb + 1; //qbm1 = qb + 1; + deque[qb] = nextPt; + } + + //Create the convex hull from what is left in the deque + if (qb < qf) + { + Vertices convexHull = new Vertices(qf); + + for (int i = qb; i < qf; i++) + convexHull.Add(deque[i]); + + return convexHull; + } + else + { + Vertices convexHull = new Vertices(qf + deque.Length); + + for (int i = 0; i < qf; i++) + convexHull.Add(deque[i]); + + for (int i = qb; i < deque.Length; i++) + convexHull.Add(deque[i]); + + return convexHull; + } + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Curve.cs b/Farseer Physics Engine 3.5/Common/Curve.cs new file mode 100644 index 000000000..ac918a3cf --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Curve.cs @@ -0,0 +1,273 @@ +#if !XNA && !WINDOWS_PHONE && !XBOX && !ANDROID + +#region License + +/* +MIT License +Copyright © 2006 The Mono.Xna Team + +All rights reserved. + +Authors: +Olivier Dufour (Duff) + +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; + +namespace Microsoft.Xna.Framework +{ + public enum CurveLoopType + { + Constant, + Cycle, + CycleOffset, + Oscillate, + Linear + } + + public enum CurveContinuity + { + Smooth, + Step + } + + public enum CurveTangent + { + Flat, + Linear, + Smooth + } + + public class Curve + { +#region Private Fields + + private CurveKeyCollection keys; + private CurveLoopType postLoop; + private CurveLoopType preLoop; + +#endregion Private Fields + +#region Public Properties + + public bool IsConstant + { + get { return keys.Count <= 1; } + } + + public CurveKeyCollection Keys + { + get { return keys; } + } + + public CurveLoopType PostLoop + { + get { return postLoop; } + set { postLoop = value; } + } + + public CurveLoopType PreLoop + { + get { return preLoop; } + set { preLoop = value; } + } + +#endregion Public Properties + +#region Public Constructors + + public Curve() + { + keys = new CurveKeyCollection(); + } + +#endregion Public Constructors + +#region Public Methods + + public void ComputeTangent(int keyIndex, CurveTangent tangentInType, CurveTangent tangentOutType) + { + throw new NotImplementedException(); + } + + public void ComputeTangent(int keyIndex, CurveTangent tangentType) + { + ComputeTangent(keyIndex, tangentType, tangentType); + } + + public void ComputeTangents(CurveTangent tangentInType, CurveTangent tangentOutType) + { + throw new NotImplementedException(); + } + + public void ComputeTangents(CurveTangent tangentType) + { + ComputeTangents(tangentType, tangentType); + } + + public Curve Clone() + { + Curve curve = new Curve(); + + curve.keys = keys.Clone(); + curve.preLoop = preLoop; + curve.postLoop = postLoop; + + return curve; + } + + public float Evaluate(float position) + { + CurveKey first = keys[0]; + CurveKey last = keys[keys.Count - 1]; + + if (position < first.Position) + { + switch (PreLoop) + { + case CurveLoopType.Constant: + //constant + return first.Value; + + case CurveLoopType.Linear: + // linear y = a*x +b with a tangeant of last point + return first.Value - first.TangentIn*(first.Position - position); + + case CurveLoopType.Cycle: + //start -> end / start -> end + int cycle = GetNumberOfCycle(position); + float virtualPos = position - (cycle*(last.Position - first.Position)); + return GetCurvePosition(virtualPos); + + case CurveLoopType.CycleOffset: + //make the curve continue (with no step) so must up the curve each cycle of delta(value) + cycle = GetNumberOfCycle(position); + virtualPos = position - (cycle*(last.Position - first.Position)); + return (GetCurvePosition(virtualPos) + cycle*(last.Value - first.Value)); + + case CurveLoopType.Oscillate: + //go back on curve from end and target start + // start-> end / end -> start + cycle = GetNumberOfCycle(position); + if (0 == cycle%2f) //if pair + virtualPos = position - (cycle*(last.Position - first.Position)); + else + virtualPos = last.Position - position + first.Position + + (cycle*(last.Position - first.Position)); + return GetCurvePosition(virtualPos); + } + } + else if (position > last.Position) + { + int cycle; + switch (PostLoop) + { + case CurveLoopType.Constant: + //constant + return last.Value; + + case CurveLoopType.Linear: + // linear y = a*x +b with a tangeant of last point + return last.Value + first.TangentOut*(position - last.Position); + + case CurveLoopType.Cycle: + //start -> end / start -> end + cycle = GetNumberOfCycle(position); + float virtualPos = position - (cycle*(last.Position - first.Position)); + return GetCurvePosition(virtualPos); + + case CurveLoopType.CycleOffset: + //make the curve continue (with no step) so must up the curve each cycle of delta(value) + cycle = GetNumberOfCycle(position); + virtualPos = position - (cycle*(last.Position - first.Position)); + return (GetCurvePosition(virtualPos) + cycle*(last.Value - first.Value)); + + case CurveLoopType.Oscillate: + //go back on curve from end and target start + // start-> end / end -> start + cycle = GetNumberOfCycle(position); + virtualPos = position - (cycle*(last.Position - first.Position)); + if (0 == cycle%2f) //if pair + virtualPos = position - (cycle*(last.Position - first.Position)); + else + virtualPos = last.Position - position + first.Position + + (cycle*(last.Position - first.Position)); + return GetCurvePosition(virtualPos); + } + } + + //in curve + return GetCurvePosition(position); + } + +#endregion Public Methods + +#region Private Methods + + private int GetNumberOfCycle(float position) + { + float cycle = (position - keys[0].Position)/(keys[keys.Count - 1].Position - keys[0].Position); + if (cycle < 0f) + cycle--; + return (int) cycle; + } + + private float GetCurvePosition(float position) + { + //only for position in curve + CurveKey prev = keys[0]; + CurveKey next; + for (int i = 1; i < keys.Count; i++) + { + next = Keys[i]; + if (next.Position >= position) + { + if (prev.Continuity == CurveContinuity.Step) + { + if (position >= 1f) + { + return next.Value; + } + return prev.Value; + } + float t = (position - prev.Position)/(next.Position - prev.Position); //to have t in [0,1] + float ts = t*t; + float tss = ts*t; + //After a lot of search on internet I have found all about spline function + // and bezier (phi'sss ancien) but finaly use hermite curve + //http://en.wikipedia.org/wiki/Cubic_Hermite_spline + //P(t) = (2*t^3 - 3t^2 + 1)*P0 + (t^3 - 2t^2 + t)m0 + (-2t^3 + 3t^2)P1 + (t^3-t^2)m1 + //with P0.value = prev.value , m0 = prev.tangentOut, P1= next.value, m1 = next.TangentIn + return (2*tss - 3*ts + 1f)*prev.Value + (tss - 2*ts + t)*prev.TangentOut + (3*ts - 2*tss)*next.Value + + (tss - ts)*next.TangentIn; + } + prev = next; + } + return 0f; + } + +#endregion + } +} + +#endif \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/CurveKey.cs b/Farseer Physics Engine 3.5/Common/CurveKey.cs new file mode 100644 index 000000000..b2a0d9b6e --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/CurveKey.cs @@ -0,0 +1,167 @@ +#if !XNA && !WINDOWS_PHONE && !XBOX && !ANDROID + +#region License + +/* +MIT License +Copyright © 2006 The Mono.Xna Team + +All rights reserved. + +Authors: +Olivier Dufour (Duff) + +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; + +namespace Microsoft.Xna.Framework +{ + public class CurveKey : IEquatable, IComparable + { +#region Private Fields + + private CurveContinuity continuity; + private float position; + private float tangentIn; + private float tangentOut; + private float value; + +#endregion Private Fields + +#region Properties + + public CurveContinuity Continuity + { + get { return continuity; } + set { continuity = value; } + } + + public float Position + { + get { return position; } + } + + public float TangentIn + { + get { return tangentIn; } + set { tangentIn = value; } + } + + public float TangentOut + { + get { return tangentOut; } + set { tangentOut = value; } + } + + public float Value + { + get { return value; } + set { this.value = value; } + } + +#endregion + +#region Constructors + + public CurveKey(float position, float value) + : this(position, value, 0, 0, CurveContinuity.Smooth) + { + } + + public CurveKey(float position, float value, float tangentIn, float tangentOut) + : this(position, value, tangentIn, tangentOut, CurveContinuity.Smooth) + { + } + + public CurveKey(float position, float value, float tangentIn, float tangentOut, CurveContinuity continuity) + { + this.position = position; + this.value = value; + this.tangentIn = tangentIn; + this.tangentOut = tangentOut; + this.continuity = continuity; + } + +#endregion Constructors + +#region Public Methods + +#region IComparable Members + + public int CompareTo(CurveKey other) + { + return position.CompareTo(other.position); + } + +#endregion + +#region IEquatable Members + + public bool Equals(CurveKey other) + { + return (this == other); + } + +#endregion + + public static bool operator !=(CurveKey a, CurveKey b) + { + return !(a == b); + } + + public static bool operator ==(CurveKey a, CurveKey b) + { + if (Equals(a, null)) + return Equals(b, null); + + if (Equals(b, null)) + return Equals(a, null); + + return (a.position == b.position) + && (a.value == b.value) + && (a.tangentIn == b.tangentIn) + && (a.tangentOut == b.tangentOut) + && (a.continuity == b.continuity); + } + + public CurveKey Clone() + { + return new CurveKey(position, value, tangentIn, tangentOut, continuity); + } + + public override bool Equals(object obj) + { + return (obj is CurveKey) ? ((CurveKey) obj) == this : false; + } + + public override int GetHashCode() + { + return position.GetHashCode() ^ value.GetHashCode() ^ tangentIn.GetHashCode() ^ + tangentOut.GetHashCode() ^ continuity.GetHashCode(); + } + +#endregion + } +} + +#endif \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/CurveKeyCollection.cs b/Farseer Physics Engine 3.5/Common/CurveKeyCollection.cs new file mode 100644 index 000000000..e4494ba41 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/CurveKeyCollection.cs @@ -0,0 +1,176 @@ +#if !XNA && !WINDOWS_PHONE && !XBOX && !ANDROID + +#region License + +/* +MIT License +Copyright © 2006 The Mono.Xna Team + +All rights reserved. + +Authors: +Olivier Dufour (Duff) + +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.Collections; +using System.Collections.Generic; + +namespace Microsoft.Xna.Framework +{ + public class CurveKeyCollection : ICollection, IEnumerable, IEnumerable + { +#region Private Fields + + private List innerlist; + private bool isReadOnly = false; + +#endregion Private Fields + +#region Properties + + public CurveKey this[int index] + { + get { return innerlist[index]; } + set + { + if (value == null) + throw new ArgumentNullException(); + + if (index >= innerlist.Count) + throw new IndexOutOfRangeException(); + + if (innerlist[index].Position == value.Position) + innerlist[index] = value; + else + { + innerlist.RemoveAt(index); + innerlist.Add(value); + } + } + } + + public int Count + { + get { return innerlist.Count; } + } + + public bool IsReadOnly + { + get { return isReadOnly; } + } + +#endregion Properties + +#region Constructors + + public CurveKeyCollection() + { + innerlist = new List(); + } + +#endregion Constructors + +#region Public Methods + + public void Add(CurveKey item) + { + if (item == null) + throw new ArgumentNullException("Value cannot be null.", (Exception) null); + + if (innerlist.Count == 0) + { + innerlist.Add(item); + return; + } + + for (int i = 0; i < innerlist.Count; i++) + { + if (item.Position < innerlist[i].Position) + { + innerlist.Insert(i, item); + return; + } + } + + innerlist.Add(item); + } + + public void Clear() + { + innerlist.Clear(); + } + + public bool Contains(CurveKey item) + { + return innerlist.Contains(item); + } + + public void CopyTo(CurveKey[] array, int arrayIndex) + { + innerlist.CopyTo(array, arrayIndex); + } + + public IEnumerator GetEnumerator() + { + return innerlist.GetEnumerator(); + } + + public bool Remove(CurveKey item) + { + return innerlist.Remove(item); + } + + IEnumerator IEnumerable.GetEnumerator() + { + return innerlist.GetEnumerator(); + } + + public CurveKeyCollection Clone() + { + CurveKeyCollection ckc = new CurveKeyCollection(); + foreach (CurveKey key in innerlist) + ckc.Add(key); + return ckc; + } + + public int IndexOf(CurveKey item) + { + return innerlist.IndexOf(item); + } + + public void RemoveAt(int index) + { + if (index != Count && index > -1) + innerlist.RemoveAt(index); + else + throw new ArgumentOutOfRangeException( + "Index was out of range. Must be non-negative and less than the size of the collection.\r\nParameter name: index", + (Exception) null); + } + +#endregion Public Methods + } +} + +#endif \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/BayazitDecomposer.cs b/Farseer Physics Engine 3.5/Common/Decomposition/BayazitDecomposer.cs new file mode 100644 index 000000000..48df75f83 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/BayazitDecomposer.cs @@ -0,0 +1,243 @@ +using System.Collections.Generic; +using System.Diagnostics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common.Decomposition +{ + //From phed rev 36: http://code.google.com/p/phed/source/browse/trunk/Polygon.cpp + + /// + /// Convex decomposition algorithm created by Mark Bayazit (http://mnbayazit.com/) + /// + /// Properties: + /// - Tries to decompose using polygons instead of triangles. + /// - Tends to produce optimal results with low processing time. + /// - Running time is O(nr), n = number of vertices, r = reflex vertices. + /// - Does not support holes. + /// + /// For more information about this algorithm, see http://mnbayazit.com/406/bayazit + /// + internal static class BayazitDecomposer + { + /// + /// Decompose the polygon into several smaller non-concave polygon. + /// If the polygon is already convex, it will return the original polygon, unless it is over Settings.MaxPolygonVertices. + /// + public static List ConvexPartition(Vertices vertices) + { + Debug.Assert(vertices.Count > 3); + Debug.Assert(vertices.IsCounterClockWise()); + + return TriangulatePolygon(vertices); + } + + private static List TriangulatePolygon(Vertices vertices) + { + List list = new List(); + Vector2 lowerInt = new Vector2(); + Vector2 upperInt = new Vector2(); // intersection points + int lowerIndex = 0, upperIndex = 0; + Vertices lowerPoly, upperPoly; + + for (int i = 0; i < vertices.Count; ++i) + { + if (Reflex(i, vertices)) + { + float upperDist; + float lowerDist = upperDist = float.MaxValue; + for (int j = 0; j < vertices.Count; ++j) + { + // if line intersects with an edge + float d; + Vector2 p; + if (Left(At(i - 1, vertices), At(i, vertices), At(j, vertices)) && RightOn(At(i - 1, vertices), At(i, vertices), At(j - 1, vertices))) + { + // find the point of intersection + p = LineTools.LineIntersect(At(i - 1, vertices), At(i, vertices), At(j, vertices), At(j - 1, vertices)); + + if (Right(At(i + 1, vertices), At(i, vertices), p)) + { + // make sure it's inside the poly + d = SquareDist(At(i, vertices), p); + if (d < lowerDist) + { + // keep only the closest intersection + lowerDist = d; + lowerInt = p; + lowerIndex = j; + } + } + } + + if (Left(At(i + 1, vertices), At(i, vertices), At(j + 1, vertices)) && RightOn(At(i + 1, vertices), At(i, vertices), At(j, vertices))) + { + p = LineTools.LineIntersect(At(i + 1, vertices), At(i, vertices), At(j, vertices), At(j + 1, vertices)); + + if (Left(At(i - 1, vertices), At(i, vertices), p)) + { + d = SquareDist(At(i, vertices), p); + if (d < upperDist) + { + upperDist = d; + upperIndex = j; + upperInt = p; + } + } + } + } + + // if there are no vertices to connect to, choose a point in the middle + if (lowerIndex == (upperIndex + 1) % vertices.Count) + { + Vector2 p = ((lowerInt + upperInt) / 2); + + lowerPoly = Copy(i, upperIndex, vertices); + lowerPoly.Add(p); + upperPoly = Copy(lowerIndex, i, vertices); + upperPoly.Add(p); + } + else + { + double highestScore = 0, bestIndex = lowerIndex; + while (upperIndex < lowerIndex) + upperIndex += vertices.Count; + + for (int j = lowerIndex; j <= upperIndex; ++j) + { + if (CanSee(i, j, vertices)) + { + double score = 1 / (SquareDist(At(i, vertices), At(j, vertices)) + 1); + if (Reflex(j, vertices)) + { + if (RightOn(At(j - 1, vertices), At(j, vertices), At(i, vertices)) && LeftOn(At(j + 1, vertices), At(j, vertices), At(i, vertices))) + score += 3; + else + score += 2; + } + else + { + score += 1; + } + if (score > highestScore) + { + bestIndex = j; + highestScore = score; + } + } + } + lowerPoly = Copy(i, (int)bestIndex, vertices); + upperPoly = Copy((int)bestIndex, i, vertices); + } + list.AddRange(TriangulatePolygon(lowerPoly)); + list.AddRange(TriangulatePolygon(upperPoly)); + return list; + } + } + + // polygon is already convex + if (vertices.Count > Settings.MaxPolygonVertices) + { + lowerPoly = Copy(0, vertices.Count / 2, vertices); + upperPoly = Copy(vertices.Count / 2, 0, vertices); + list.AddRange(TriangulatePolygon(lowerPoly)); + list.AddRange(TriangulatePolygon(upperPoly)); + } + else + list.Add(vertices); + + return list; + } + + private static Vector2 At(int i, Vertices vertices) + { + int s = vertices.Count; + return vertices[i < 0 ? s - 1 - ((-i - 1) % s) : i % s]; + } + + private static Vertices Copy(int i, int j, Vertices vertices) + { + while (j < i) + j += vertices.Count; + + Vertices p = new Vertices(j); + + for (; i <= j; ++i) + { + p.Add(At(i, vertices)); + } + return p; + } + + private static bool CanSee(int i, int j, Vertices vertices) + { + if (Reflex(i, vertices)) + { + if (LeftOn(At(i, vertices), At(i - 1, vertices), At(j, vertices)) && RightOn(At(i, vertices), At(i + 1, vertices), At(j, vertices))) + return false; + } + else + { + if (RightOn(At(i, vertices), At(i + 1, vertices), At(j, vertices)) || LeftOn(At(i, vertices), At(i - 1, vertices), At(j, vertices))) + return false; + } + if (Reflex(j, vertices)) + { + if (LeftOn(At(j, vertices), At(j - 1, vertices), At(i, vertices)) && RightOn(At(j, vertices), At(j + 1, vertices), At(i, vertices))) + return false; + } + else + { + if (RightOn(At(j, vertices), At(j + 1, vertices), At(i, vertices)) || LeftOn(At(j, vertices), At(j - 1, vertices), At(i, vertices))) + return false; + } + for (int k = 0; k < vertices.Count; ++k) + { + if ((k + 1) % vertices.Count == i || k == i || (k + 1) % vertices.Count == j || k == j) + continue; // ignore incident edges + + Vector2 intersectionPoint; + + if (LineTools.LineIntersect(At(i, vertices), At(j, vertices), At(k, vertices), At(k + 1, vertices), out intersectionPoint)) + return false; + } + return true; + } + + private static bool Reflex(int i, Vertices vertices) + { + return Right(i, vertices); + } + + private static bool Right(int i, Vertices vertices) + { + return Right(At(i - 1, vertices), At(i, vertices), At(i + 1, vertices)); + } + + private static bool Left(Vector2 a, Vector2 b, Vector2 c) + { + return MathUtils.Area(ref a, ref b, ref c) > 0; + } + + private static bool LeftOn(Vector2 a, Vector2 b, Vector2 c) + { + return MathUtils.Area(ref a, ref b, ref c) >= 0; + } + + private static bool Right(Vector2 a, Vector2 b, Vector2 c) + { + return MathUtils.Area(ref a, ref b, ref c) < 0; + } + + private static bool RightOn(Vector2 a, Vector2 b, Vector2 c) + { + return MathUtils.Area(ref a, ref b, ref c) <= 0; + } + + private static float SquareDist(Vector2 a, Vector2 b) + { + float dx = b.X - a.X; + float dy = b.Y - a.Y; + return dx * dx + dy * dy; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/DelaunayTriangle.cs b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/DelaunayTriangle.cs new file mode 100644 index 000000000..a0c6d467c --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/DelaunayTriangle.cs @@ -0,0 +1,420 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +// Changes from the Java version +// attributification +// Future possibilities +// Flattening out the number of indirections +// Replacing arrays of 3 with fixed-length arrays? +// Replacing bool[3] with a bit array of some sort? +// Bundling everything into an AoS mess? +// Hardcode them all as ABC ? + +using System; +using System.Collections.Generic; +using System.Diagnostics; +using FarseerPhysics.Common.Decomposition.CDT.Delaunay.Sweep; +using FarseerPhysics.Common.Decomposition.CDT.Util; + +namespace FarseerPhysics.Common.Decomposition.CDT.Delaunay +{ + internal class DelaunayTriangle + { + /** Neighbor pointers */ + + /** Flags to determine if an edge is a Delauney edge */ + public FixedBitArray3 EdgeIsConstrained; + + /** Flags to determine if an edge is a Constrained edge */ + public FixedBitArray3 EdgeIsDelaunay; + public Util.FixedArray3 Neighbors; + + /** Has this triangle been marked as an interior triangle? */ + + public Util.FixedArray3 Points; + + public DelaunayTriangle(TriangulationPoint p1, TriangulationPoint p2, TriangulationPoint p3) + { + Points[0] = p1; + Points[1] = p2; + Points[2] = p3; + } + + public bool IsInterior { get; set; } + + public int IndexOf(TriangulationPoint p) + { + int i = Points.IndexOf(p); + if (i == -1) throw new Exception("Calling index with a point that doesn't exist in triangle"); + return i; + } + + //TODO: Port note - different implementation + public int IndexCW(TriangulationPoint p) + { + int index = IndexOf(p); + switch (index) + { + case 0: + return 2; + case 1: + return 0; + default: + return 1; + } + } + + //TODO: Port note - different implementation + public int IndexCCW(TriangulationPoint p) + { + int index = IndexOf(p); + switch (index) + { + case 0: + return 1; + case 1: + return 2; + default: + return 0; + } + } + + public bool Contains(TriangulationPoint p) + { + return (p == Points[0] || p == Points[1] || p == Points[2]); + } + + public bool Contains(DTSweepConstraint e) + { + return (Contains(e.P) && Contains(e.Q)); + } + + public bool Contains(TriangulationPoint p, TriangulationPoint q) + { + return (Contains(p) && Contains(q)); + } + + /// + /// Update neighbor pointers + /// + /// Point 1 of the shared edge + /// Point 2 of the shared edge + /// This triangle's new neighbor + private void MarkNeighbor(TriangulationPoint p1, TriangulationPoint p2, DelaunayTriangle t) + { + if ((p1 == Points[2] && p2 == Points[1]) || (p1 == Points[1] && p2 == Points[2])) + { + Neighbors[0] = t; + } + else if ((p1 == Points[0] && p2 == Points[2]) || (p1 == Points[2] && p2 == Points[0])) + { + Neighbors[1] = t; + } + else if ((p1 == Points[0] && p2 == Points[1]) || (p1 == Points[1] && p2 == Points[0])) + { + Neighbors[2] = t; + } + else + { + Debug.WriteLine("Neighbor error, please report!"); + // throw new Exception("Neighbor error, please report!"); + } + } + + /// + /// Exhaustive search to update neighbor pointers + /// + public void MarkNeighbor(DelaunayTriangle t) + { + if (t.Contains(Points[1], Points[2])) + { + Neighbors[0] = t; + t.MarkNeighbor(Points[1], Points[2], this); + } + else if (t.Contains(Points[0], Points[2])) + { + Neighbors[1] = t; + t.MarkNeighbor(Points[0], Points[2], this); + } + else if (t.Contains(Points[0], Points[1])) + { + Neighbors[2] = t; + t.MarkNeighbor(Points[0], Points[1], this); + } + else + { + Debug.WriteLine("markNeighbor failed"); + } + } + + public void ClearNeighbors() + { + Neighbors[0] = Neighbors[1] = Neighbors[2] = null; + } + + public void ClearNeighbor(DelaunayTriangle triangle) + { + if (Neighbors[0] == triangle) + { + Neighbors[0] = null; + } + else if (Neighbors[1] == triangle) + { + Neighbors[1] = null; + } + else + { + Neighbors[2] = null; + } + } + + /** + * Clears all references to all other triangles and points + */ + + public void Clear() + { + DelaunayTriangle t; + for (int i = 0; i < 3; i++) + { + t = Neighbors[i]; + if (t != null) + { + t.ClearNeighbor(this); + } + } + ClearNeighbors(); + Points[0] = Points[1] = Points[2] = null; + } + + /// Opposite triangle + /// The point in t that isn't shared between the triangles + public TriangulationPoint OppositePoint(DelaunayTriangle t, TriangulationPoint p) + { + Debug.Assert(t != this, "self-pointer error"); + return PointCW(t.PointCW(p)); + } + + public DelaunayTriangle NeighborCW(TriangulationPoint point) + { + return Neighbors[(Points.IndexOf(point) + 1)%3]; + } + + public DelaunayTriangle NeighborCCW(TriangulationPoint point) + { + return Neighbors[(Points.IndexOf(point) + 2)%3]; + } + + public DelaunayTriangle NeighborAcross(TriangulationPoint point) + { + return Neighbors[Points.IndexOf(point)]; + } + + public TriangulationPoint PointCCW(TriangulationPoint point) + { + return Points[(IndexOf(point) + 1)%3]; + } + + public TriangulationPoint PointCW(TriangulationPoint point) + { + return Points[(IndexOf(point) + 2)%3]; + } + + private void RotateCW() + { + var t = Points[2]; + Points[2] = Points[1]; + Points[1] = Points[0]; + Points[0] = t; + } + + /// + /// Legalize triangle by rotating clockwise around oPoint + /// + /// The origin point to rotate around + /// ??? + public void Legalize(TriangulationPoint oPoint, TriangulationPoint nPoint) + { + RotateCW(); + Points[IndexCCW(oPoint)] = nPoint; + } + + public override string ToString() + { + return Points[0] + "," + Points[1] + "," + Points[2]; + } + + /// + /// Finalize edge marking + /// + public void MarkNeighborEdges() + { + for (int i = 0; i < 3; i++) + if (EdgeIsConstrained[i] && Neighbors[i] != null) + { + Neighbors[i].MarkConstrainedEdge(Points[(i + 1)%3], Points[(i + 2)%3]); + } + } + + public void MarkEdge(DelaunayTriangle triangle) + { + for (int i = 0; i < 3; i++) + if (EdgeIsConstrained[i]) + { + triangle.MarkConstrainedEdge(Points[(i + 1)%3], Points[(i + 2)%3]); + } + } + + public void MarkEdge(List tList) + { + foreach (DelaunayTriangle t in tList) + for (int i = 0; i < 3; i++) + if (t.EdgeIsConstrained[i]) + { + MarkConstrainedEdge(t.Points[(i + 1)%3], t.Points[(i + 2)%3]); + } + } + + public void MarkConstrainedEdge(int index) + { + EdgeIsConstrained[index] = true; + } + + public void MarkConstrainedEdge(DTSweepConstraint edge) + { + MarkConstrainedEdge(edge.P, edge.Q); + } + + /// + /// Mark edge as constrained + /// + public void MarkConstrainedEdge(TriangulationPoint p, TriangulationPoint q) + { + int i = EdgeIndex(p, q); + if (i != -1) EdgeIsConstrained[i] = true; + } + + public double Area() + { + double b = Points[0].X - Points[1].X; + double h = Points[2].Y - Points[1].Y; + + return Math.Abs((b*h*0.5f)); + } + + public TriangulationPoint Centroid() + { + double cx = (Points[0].X + Points[1].X + Points[2].X)/3f; + double cy = (Points[0].Y + Points[1].Y + Points[2].Y)/3f; + return new TriangulationPoint(cx, cy); + } + + /// + /// Get the index of the neighbor that shares this edge (or -1 if it isn't shared) + /// + /// index of the shared edge or -1 if edge isn't shared + public int EdgeIndex(TriangulationPoint p1, TriangulationPoint p2) + { + int i1 = Points.IndexOf(p1); + int i2 = Points.IndexOf(p2); + + // Points of this triangle in the edge p1-p2 + bool a = (i1 == 0 || i2 == 0); + bool b = (i1 == 1 || i2 == 1); + bool c = (i1 == 2 || i2 == 2); + + if (b && c) return 0; + if (a && c) return 1; + if (a && b) return 2; + return -1; + } + + public bool GetConstrainedEdgeCCW(TriangulationPoint p) + { + return EdgeIsConstrained[(IndexOf(p) + 2)%3]; + } + + public bool GetConstrainedEdgeCW(TriangulationPoint p) + { + return EdgeIsConstrained[(IndexOf(p) + 1)%3]; + } + + public bool GetConstrainedEdgeAcross(TriangulationPoint p) + { + return EdgeIsConstrained[IndexOf(p)]; + } + + public void SetConstrainedEdgeCCW(TriangulationPoint p, bool ce) + { + EdgeIsConstrained[(IndexOf(p) + 2)%3] = ce; + } + + public void SetConstrainedEdgeCW(TriangulationPoint p, bool ce) + { + EdgeIsConstrained[(IndexOf(p) + 1)%3] = ce; + } + + public void SetConstrainedEdgeAcross(TriangulationPoint p, bool ce) + { + EdgeIsConstrained[IndexOf(p)] = ce; + } + + public bool GetDelaunayEdgeCCW(TriangulationPoint p) + { + return EdgeIsDelaunay[(IndexOf(p) + 2)%3]; + } + + public bool GetDelaunayEdgeCW(TriangulationPoint p) + { + return EdgeIsDelaunay[(IndexOf(p) + 1)%3]; + } + + public bool GetDelaunayEdgeAcross(TriangulationPoint p) + { + return EdgeIsDelaunay[IndexOf(p)]; + } + + public void SetDelaunayEdgeCCW(TriangulationPoint p, bool ce) + { + EdgeIsDelaunay[(IndexOf(p) + 2)%3] = ce; + } + + public void SetDelaunayEdgeCW(TriangulationPoint p, bool ce) + { + EdgeIsDelaunay[(IndexOf(p) + 1)%3] = ce; + } + + public void SetDelaunayEdgeAcross(TriangulationPoint p, bool ce) + { + EdgeIsDelaunay[IndexOf(p)] = ce; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFront.cs b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFront.cs new file mode 100644 index 000000000..95baebd3e --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFront.cs @@ -0,0 +1,180 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +// Changes from the Java version +// Removed BST code, but not all artifacts of it +// Future possibilities +// Eliminate Add/RemoveNode ? +// Comments comments and more comments! + +using System; +using System.Text; + +namespace FarseerPhysics.Common.Decomposition.CDT.Delaunay.Sweep +{ + /** + * @author Thomas Åhlen (thahlen@gmail.com) + */ + + internal class AdvancingFront + { + public AdvancingFrontNode Head; + protected AdvancingFrontNode Search; + public AdvancingFrontNode Tail; + + public AdvancingFront(AdvancingFrontNode head, AdvancingFrontNode tail) + { + Head = head; + Tail = tail; + Search = head; + AddNode(head); + AddNode(tail); + } + + public void AddNode(AdvancingFrontNode node) + { + //_searchTree.put(node.key, node); + } + + public void RemoveNode(AdvancingFrontNode node) + { + //_searchTree.delete( node.key ); + } + + public override string ToString() + { + StringBuilder sb = new StringBuilder(); + AdvancingFrontNode node = Head; + while (node != Tail) + { + sb.Append(node.Point.X).Append("->"); + node = node.Next; + } + sb.Append(Tail.Point.X); + return sb.ToString(); + } + + /// + /// MM: This seems to be used by LocateNode to guess a position in the implicit linked list of AdvancingFrontNodes near x + /// Removed an overload that depended on this being exact + /// + private AdvancingFrontNode FindSearchNode(double x) + { + // TODO: implement BST index + return Search; + } + + /// + /// We use a balancing tree to locate a node smaller or equal to given key value + /// + public AdvancingFrontNode LocateNode(TriangulationPoint point) + { + return LocateNode(point.X); + } + + private AdvancingFrontNode LocateNode(double x) + { + AdvancingFrontNode node = FindSearchNode(x); + if (x < node.Value) + { + while ((node = node.Prev) != null) + if (x >= node.Value) + { + Search = node; + return node; + } + } + else + { + while ((node = node.Next) != null) + if (x < node.Value) + { + Search = node.Prev; + return node.Prev; + } + } + return null; + } + + /// + /// This implementation will use simple node traversal algorithm to find a point on the front + /// + public AdvancingFrontNode LocatePoint(TriangulationPoint point) + { + double px = point.X; + AdvancingFrontNode node = FindSearchNode(px); + double nx = node.Point.X; + + if (px == nx) + { + if (point != node.Point) + { + // We might have two nodes with same x value for a short time + if (point == node.Prev.Point) + { + node = node.Prev; + } + else if (point == node.Next.Point) + { + node = node.Next; + } + else + { + throw new Exception("Failed to find Node for given afront point"); + //node = null; + } + } + } + else if (px < nx) + { + while ((node = node.Prev) != null) + { + if (point == node.Point) + { + break; + } + } + } + else + { + while ((node = node.Next) != null) + { + if (point == node.Point) + { + break; + } + } + } + Search = node; + return node; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFrontNode.cs b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFrontNode.cs new file mode 100644 index 000000000..a1a818b79 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFrontNode.cs @@ -0,0 +1,64 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +// Changes from the Java version +// Removed getters +// Has* turned into attributes +// Future possibilities +// Comments! + +namespace FarseerPhysics.Common.Decomposition.CDT.Delaunay.Sweep +{ + internal class AdvancingFrontNode + { + public AdvancingFrontNode Next; + public TriangulationPoint Point; + public AdvancingFrontNode Prev; + public DelaunayTriangle Triangle; + public double Value; + + public AdvancingFrontNode(TriangulationPoint point) + { + Point = point; + Value = point.X; + } + + public bool HasNext + { + get { return Next != null; } + } + + public bool HasPrev + { + get { return Prev != null; } + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/DTSweep.cs b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/DTSweep.cs new file mode 100644 index 000000000..b1179bb75 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/DTSweep.cs @@ -0,0 +1,1151 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Sweep-line, Constrained Delauney Triangulation (CDT) See: Domiter, V. and + * Zalik, B.(2008)'Sweep-line algorithm for constrained Delaunay triangulation', + * International Journal of Geographical Information Science + * + * "FlipScan" Constrained Edge Algorithm invented by author of this code. + * + * Author: Thomas Åhlén, thahlen@gmail.com + */ + +// Changes from the Java version +// Turned DTSweep into a static class +// Lots of deindentation via early bailout +// Future possibilities +// Comments! + +using System; +using System.Collections.Generic; +using System.Diagnostics; + +namespace FarseerPhysics.Common.Decomposition.CDT.Delaunay.Sweep +{ + internal static class DTSweep + { + private const double PI_div2 = Math.PI / 2; + private const double PI_3div4 = 3 * Math.PI / 4; + + /// + /// Triangulate simple polygon with holes + /// + public static void Triangulate(DTSweepContext tcx) + { + tcx.CreateAdvancingFront(); + + Sweep(tcx); + + // Finalize triangulation + if (tcx.TriangulationMode == TriangulationMode.Polygon) + { + FinalizationPolygon(tcx); + } + else + { + FinalizationConvexHull(tcx); + } + + tcx.Done(); + } + + /// + /// Start sweeping the Y-sorted point set from bottom to top + /// + private static void Sweep(DTSweepContext tcx) + { + List points = tcx.Points; + + for (int i = 1; i < points.Count; i++) + { + TriangulationPoint point = points[i]; + + AdvancingFrontNode node = PointEvent(tcx, point); + + if (point.HasEdges) + { + foreach (DTSweepConstraint e in point.Edges) + { + EdgeEvent(tcx, e, node); + } + } + tcx.Update(null); + } + } + + /// + /// If this is a Delaunay Triangulation of a pointset we need to fill so the triangle mesh gets a ConvexHull + /// + private static void FinalizationConvexHull(DTSweepContext tcx) + { + DelaunayTriangle t1, t2; + + AdvancingFrontNode n1 = tcx.aFront.Head.Next; + AdvancingFrontNode n2 = n1.Next; + + TurnAdvancingFrontConvex(tcx, n1, n2); + + // TODO: implement ConvexHull for lower right and left boundary + + // Lets remove triangles connected to the two "algorithm" points + + // XXX: When the first the nodes are points in a triangle we need to do a flip before + // removing triangles or we will lose a valid triangle. + // Same for last three nodes! + // !!! If I implement ConvexHull for lower right and left boundary this fix should not be + // needed and the removed triangles will be added again by default + n1 = tcx.aFront.Tail.Prev; + if (n1.Triangle.Contains(n1.Next.Point) && n1.Triangle.Contains(n1.Prev.Point)) + { + t1 = n1.Triangle.NeighborAcross(n1.Point); + RotateTrianglePair(n1.Triangle, n1.Point, t1, t1.OppositePoint(n1.Triangle, n1.Point)); + tcx.MapTriangleToNodes(n1.Triangle); + tcx.MapTriangleToNodes(t1); + } + n1 = tcx.aFront.Head.Next; + if (n1.Triangle.Contains(n1.Prev.Point) && n1.Triangle.Contains(n1.Next.Point)) + { + t1 = n1.Triangle.NeighborAcross(n1.Point); + RotateTrianglePair(n1.Triangle, n1.Point, t1, t1.OppositePoint(n1.Triangle, n1.Point)); + tcx.MapTriangleToNodes(n1.Triangle); + tcx.MapTriangleToNodes(t1); + } + + // Lower right boundary + TriangulationPoint first = tcx.aFront.Head.Point; + n2 = tcx.aFront.Tail.Prev; + t1 = n2.Triangle; + TriangulationPoint p1 = n2.Point; + n2.Triangle = null; + do + { + tcx.RemoveFromList(t1); + p1 = t1.PointCCW(p1); + if (p1 == first) break; + t2 = t1.NeighborCCW(p1); + t1.Clear(); + t1 = t2; + } while (true); + + // Lower left boundary + first = tcx.aFront.Head.Next.Point; + p1 = t1.PointCW(tcx.aFront.Head.Point); + t2 = t1.NeighborCW(tcx.aFront.Head.Point); + t1.Clear(); + t1 = t2; + while (p1 != first) //TODO: Port note. This was do while before. + { + tcx.RemoveFromList(t1); + p1 = t1.PointCCW(p1); + t2 = t1.NeighborCCW(p1); + t1.Clear(); + t1 = t2; + } + + // Remove current head and tail node now that we have removed all triangles attached + // to them. Then set new head and tail node points + tcx.aFront.Head = tcx.aFront.Head.Next; + tcx.aFront.Head.Prev = null; + tcx.aFront.Tail = tcx.aFront.Tail.Prev; + tcx.aFront.Tail.Next = null; + + tcx.FinalizeTriangulation(); + } + + /// + /// We will traverse the entire advancing front and fill it to form a convex hull. + /// + private static void TurnAdvancingFrontConvex(DTSweepContext tcx, AdvancingFrontNode b, AdvancingFrontNode c) + { + AdvancingFrontNode first = b; + while (c != tcx.aFront.Tail) + { + if (TriangulationUtil.Orient2d(b.Point, c.Point, c.Next.Point) == Orientation.CCW) + { + // [b,c,d] Concave - fill around c + Fill(tcx, c); + c = c.Next; + } + else + { + // [b,c,d] Convex + if (b != first && TriangulationUtil.Orient2d(b.Prev.Point, b.Point, c.Point) == Orientation.CCW) + { + // [a,b,c] Concave - fill around b + Fill(tcx, b); + b = b.Prev; + } + else + { + // [a,b,c] Convex - nothing to fill + b = c; + c = c.Next; + } + } + } + } + + private static void FinalizationPolygon(DTSweepContext tcx) + { + // Get an Internal triangle to start with + DelaunayTriangle t = tcx.aFront.Head.Next.Triangle; + TriangulationPoint p = tcx.aFront.Head.Next.Point; + while (!t.GetConstrainedEdgeCW(p)) + { + t = t.NeighborCCW(p); + } + + // Collect interior triangles constrained by edges + tcx.MeshClean(t); + } + + /// + /// Find closes node to the left of the new point and + /// create a new triangle. If needed new holes and basins + /// will be filled to. + /// + private static AdvancingFrontNode PointEvent(DTSweepContext tcx, TriangulationPoint point) + { + AdvancingFrontNode node = tcx.LocateNode(point); + AdvancingFrontNode newNode = NewFrontTriangle(tcx, point, node); + + // Only need to check +epsilon since point never have smaller + // x value than node due to how we fetch nodes from the front + if (point.X <= node.Point.X + TriangulationUtil.EPSILON) + { + Fill(tcx, node); + } + + tcx.AddNode(newNode); + + FillAdvancingFront(tcx, newNode); + return newNode; + } + + /// + /// Creates a new front triangle and legalize it + /// + private static AdvancingFrontNode NewFrontTriangle(DTSweepContext tcx, TriangulationPoint point, AdvancingFrontNode node) + { + DelaunayTriangle triangle = new DelaunayTriangle(point, node.Point, node.Next.Point); + triangle.MarkNeighbor(node.Triangle); + tcx.Triangles.Add(triangle); + + AdvancingFrontNode newNode = new AdvancingFrontNode(point); + newNode.Next = node.Next; + newNode.Prev = node; + node.Next.Prev = newNode; + node.Next = newNode; + + tcx.AddNode(newNode); // XXX: BST + + if (!Legalize(tcx, triangle)) + { + tcx.MapTriangleToNodes(triangle); + } + + return newNode; + } + + private static void EdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node) + { + try + { + tcx.EdgeEvent.ConstrainedEdge = edge; + tcx.EdgeEvent.Right = edge.P.X > edge.Q.X; + + if (IsEdgeSideOfTriangle(node.Triangle, edge.P, edge.Q)) + { + return; + } + + // For now we will do all needed filling + // TODO: integrate with flip process might give some better performance + // but for now this avoid the issue with cases that needs both flips and fills + FillEdgeEvent(tcx, edge, node); + + EdgeEvent(tcx, edge.P, edge.Q, node.Triangle, edge.Q); + } + catch (PointOnEdgeException e) + { + Debug.WriteLine("Skipping Edge: {0}", e.Message); + } + } + + private static void FillEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node) + { + if (tcx.EdgeEvent.Right) + { + FillRightAboveEdgeEvent(tcx, edge, node); + } + else + { + FillLeftAboveEdgeEvent(tcx, edge, node); + } + } + + private static void FillRightConcaveEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, + AdvancingFrontNode node) + { + Fill(tcx, node.Next); + if (node.Next.Point != edge.P) + { + // Next above or below edge? + if (TriangulationUtil.Orient2d(edge.Q, node.Next.Point, edge.P) == Orientation.CCW) + { + // Below + if (TriangulationUtil.Orient2d(node.Point, node.Next.Point, node.Next.Next.Point) == Orientation.CCW) + { + // Next is concave + FillRightConcaveEdgeEvent(tcx, edge, node); + } + else + { + // Next is convex + } + } + } + } + + private static void FillRightConvexEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node) + { + // Next concave or convex? + if (TriangulationUtil.Orient2d(node.Next.Point, node.Next.Next.Point, node.Next.Next.Next.Point) == + Orientation.CCW) + { + // Concave + FillRightConcaveEdgeEvent(tcx, edge, node.Next); + } + else + { + // Convex + // Next above or below edge? + if (TriangulationUtil.Orient2d(edge.Q, node.Next.Next.Point, edge.P) == Orientation.CCW) + { + // Below + FillRightConvexEdgeEvent(tcx, edge, node.Next); + } + else + { + // Above + } + } + } + + private static void FillRightBelowEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node) + { + if (node.Point.X < edge.P.X) // needed? + { + if (TriangulationUtil.Orient2d(node.Point, node.Next.Point, node.Next.Next.Point) == Orientation.CCW) + { + // Concave + FillRightConcaveEdgeEvent(tcx, edge, node); + } + else + { + // Convex + FillRightConvexEdgeEvent(tcx, edge, node); + // Retry this one + FillRightBelowEdgeEvent(tcx, edge, node); + } + } + } + + private static void FillRightAboveEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node) + { + while (node.Next.Point.X < edge.P.X) + { + // Check if next node is below the edge + Orientation o1 = TriangulationUtil.Orient2d(edge.Q, node.Next.Point, edge.P); + if (o1 == Orientation.CCW) + { + FillRightBelowEdgeEvent(tcx, edge, node); + } + else + { + node = node.Next; + } + } + } + + private static void FillLeftConvexEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node) + { + // Next concave or convex? + if (TriangulationUtil.Orient2d(node.Prev.Point, node.Prev.Prev.Point, node.Prev.Prev.Prev.Point) == + Orientation.CW) + { + // Concave + FillLeftConcaveEdgeEvent(tcx, edge, node.Prev); + } + else + { + // Convex + // Next above or below edge? + if (TriangulationUtil.Orient2d(edge.Q, node.Prev.Prev.Point, edge.P) == Orientation.CW) + { + // Below + FillLeftConvexEdgeEvent(tcx, edge, node.Prev); + } + else + { + // Above + } + } + } + + private static void FillLeftConcaveEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node) + { + Fill(tcx, node.Prev); + if (node.Prev.Point != edge.P) + { + // Next above or below edge? + if (TriangulationUtil.Orient2d(edge.Q, node.Prev.Point, edge.P) == Orientation.CW) + { + // Below + if (TriangulationUtil.Orient2d(node.Point, node.Prev.Point, node.Prev.Prev.Point) == Orientation.CW) + { + // Next is concave + FillLeftConcaveEdgeEvent(tcx, edge, node); + } + else + { + // Next is convex + } + } + } + } + + private static void FillLeftBelowEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node) + { + if (node.Point.X > edge.P.X) + { + if (TriangulationUtil.Orient2d(node.Point, node.Prev.Point, node.Prev.Prev.Point) == Orientation.CW) + { + // Concave + FillLeftConcaveEdgeEvent(tcx, edge, node); + } + else + { + // Convex + FillLeftConvexEdgeEvent(tcx, edge, node); + // Retry this one + FillLeftBelowEdgeEvent(tcx, edge, node); + } + } + } + + private static void FillLeftAboveEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node) + { + while (node.Prev.Point.X > edge.P.X) + { + // Check if next node is below the edge + Orientation o1 = TriangulationUtil.Orient2d(edge.Q, node.Prev.Point, edge.P); + if (o1 == Orientation.CW) + { + FillLeftBelowEdgeEvent(tcx, edge, node); + } + else + { + node = node.Prev; + } + } + } + + private static bool IsEdgeSideOfTriangle(DelaunayTriangle triangle, TriangulationPoint ep, TriangulationPoint eq) + { + int index = triangle.EdgeIndex(ep, eq); + if (index != -1) + { + triangle.MarkConstrainedEdge(index); + triangle = triangle.Neighbors[index]; + if (triangle != null) + { + triangle.MarkConstrainedEdge(ep, eq); + } + return true; + } + return false; + } + + private static void EdgeEvent(DTSweepContext tcx, TriangulationPoint ep, TriangulationPoint eq, DelaunayTriangle triangle, TriangulationPoint point) + { + if (IsEdgeSideOfTriangle(triangle, ep, eq)) + return; + + TriangulationPoint p1 = triangle.PointCCW(point); + Orientation o1 = TriangulationUtil.Orient2d(eq, p1, ep); + if (o1 == Orientation.Collinear) + { + if (triangle.Contains(eq, p1)) + { + triangle.MarkConstrainedEdge(eq, p1); + // We are modifying the constraint maybe it would be better to + // not change the given constraint and just keep a variable for the new constraint + tcx.EdgeEvent.ConstrainedEdge.Q = p1; + triangle = triangle.NeighborAcross(point); + EdgeEvent(tcx, ep, p1, triangle, p1); + } + else + { + throw new PointOnEdgeException("EdgeEvent - Point on constrained edge not supported yet"); + } + if (tcx.IsDebugEnabled) + { + Debug.WriteLine("EdgeEvent - Point on constrained edge"); + } + return; + } + + TriangulationPoint p2 = triangle.PointCW(point); + Orientation o2 = TriangulationUtil.Orient2d(eq, p2, ep); + if (o2 == Orientation.Collinear) + { + if (triangle.Contains(eq, p2)) + { + triangle.MarkConstrainedEdge(eq, p2); + // We are modifying the constraint maybe it would be better to + // not change the given constraint and just keep a variable for the new constraint + tcx.EdgeEvent.ConstrainedEdge.Q = p2; + triangle = triangle.NeighborAcross(point); + EdgeEvent(tcx, ep, p2, triangle, p2); + } + else + { + throw new PointOnEdgeException("EdgeEvent - Point on constrained edge not supported yet"); + } + if (tcx.IsDebugEnabled) + { + Debug.WriteLine("EdgeEvent - Point on constrained edge"); + } + return; + } + + if (o1 == o2) + { + // Need to decide if we are rotating CW or CCW to get to a triangle + // that will cross edge + if (o1 == Orientation.CW) + { + triangle = triangle.NeighborCCW(point); + } + else + { + triangle = triangle.NeighborCW(point); + } + EdgeEvent(tcx, ep, eq, triangle, point); + } + else + { + // This triangle crosses constraint so lets flippin start! + FlipEdgeEvent(tcx, ep, eq, triangle, point); + } + } + + private static void FlipEdgeEvent(DTSweepContext tcx, TriangulationPoint ep, TriangulationPoint eq, DelaunayTriangle t, TriangulationPoint p) + { + DelaunayTriangle ot = t.NeighborAcross(p); + TriangulationPoint op = ot.OppositePoint(t, p); + + if (ot == null) + { + // If we want to integrate the fillEdgeEvent do it here + // With current implementation we should never get here + throw new InvalidOperationException("[BUG:FIXME] FLIP failed due to missing triangle"); + } + + if (t.GetConstrainedEdgeAcross(p)) + { + throw new Exception("Intersecting Constraints"); + } + + bool inScanArea = TriangulationUtil.InScanArea(p, t.PointCCW(p), t.PointCW(p), op); + if (inScanArea) + { + // Lets rotate shared edge one vertex CW + RotateTrianglePair(t, p, ot, op); + tcx.MapTriangleToNodes(t); + tcx.MapTriangleToNodes(ot); + + if (p == eq && op == ep) + { + if (eq == tcx.EdgeEvent.ConstrainedEdge.Q + && ep == tcx.EdgeEvent.ConstrainedEdge.P) + { + if (tcx.IsDebugEnabled) Console.WriteLine("[FLIP] - constrained edge done"); // TODO: remove + t.MarkConstrainedEdge(ep, eq); + ot.MarkConstrainedEdge(ep, eq); + Legalize(tcx, t); + Legalize(tcx, ot); + } + else + { + if (tcx.IsDebugEnabled) Console.WriteLine("[FLIP] - subedge done"); // TODO: remove + // XXX: I think one of the triangles should be legalized here? + } + } + else + { + if (tcx.IsDebugEnabled) + Console.WriteLine("[FLIP] - flipping and continuing with triangle still crossing edge"); + // TODO: remove + Orientation o = TriangulationUtil.Orient2d(eq, op, ep); + t = NextFlipTriangle(tcx, o, t, ot, p, op); + FlipEdgeEvent(tcx, ep, eq, t, p); + } + } + else + { + TriangulationPoint newP = NextFlipPoint(ep, eq, ot, op); + FlipScanEdgeEvent(tcx, ep, eq, t, ot, newP); + EdgeEvent(tcx, ep, eq, t, p); + } + } + + /// + /// When we need to traverse from one triangle to the next we need + /// the point in current triangle that is the opposite point to the next + /// triangle. + /// + private static TriangulationPoint NextFlipPoint(TriangulationPoint ep, TriangulationPoint eq, DelaunayTriangle ot, TriangulationPoint op) + { + Orientation o2d = TriangulationUtil.Orient2d(eq, op, ep); + if (o2d == Orientation.CW) + { + // Right + return ot.PointCCW(op); + } + else if (o2d == Orientation.CCW) + { + // Left + return ot.PointCW(op); + } + else + { + // TODO: implement support for point on constraint edge + throw new PointOnEdgeException("Point on constrained edge not supported yet"); + } + } + + /// + /// After a flip we have two triangles and know that only one will still be + /// intersecting the edge. So decide which to contiune with and legalize the other + /// + /// + /// should be the result of an TriangulationUtil.orient2d( eq, op, ep ) + /// triangle 1 + /// triangle 2 + /// a point shared by both triangles + /// another point shared by both triangles + /// returns the triangle still intersecting the edge + private static DelaunayTriangle NextFlipTriangle(DTSweepContext tcx, Orientation o, DelaunayTriangle t, DelaunayTriangle ot, TriangulationPoint p, TriangulationPoint op) + { + int edgeIndex; + if (o == Orientation.CCW) + { + // ot is not crossing edge after flip + edgeIndex = ot.EdgeIndex(p, op); + ot.EdgeIsDelaunay[edgeIndex] = true; + Legalize(tcx, ot); + ot.EdgeIsDelaunay.Clear(); + return t; + } + // t is not crossing edge after flip + edgeIndex = t.EdgeIndex(p, op); + t.EdgeIsDelaunay[edgeIndex] = true; + Legalize(tcx, t); + t.EdgeIsDelaunay.Clear(); + return ot; + } + + /// + /// Scan part of the FlipScan algorithm
+ /// When a triangle pair isn't flippable we will scan for the next + /// point that is inside the flip triangle scan area. When found + /// we generate a new flipEdgeEvent + /// + /// + /// last point on the edge we are traversing + /// first point on the edge we are traversing + /// the current triangle sharing the point eq with edge + /// + /// + private static void FlipScanEdgeEvent(DTSweepContext tcx, TriangulationPoint ep, TriangulationPoint eq, DelaunayTriangle flipTriangle, DelaunayTriangle t, TriangulationPoint p) + { + DelaunayTriangle ot = t.NeighborAcross(p); + TriangulationPoint op = ot.OppositePoint(t, p); + + if (ot == null) + { + // If we want to integrate the fillEdgeEvent do it here + // With current implementation we should never get here + throw new Exception("[BUG:FIXME] FLIP failed due to missing triangle"); + } + + bool inScanArea = TriangulationUtil.InScanArea(eq, flipTriangle.PointCCW(eq), flipTriangle.PointCW(eq), op); + if (inScanArea) + { + // flip with new edge op->eq + FlipEdgeEvent(tcx, eq, op, ot, op); + // TODO: Actually I just figured out that it should be possible to + // improve this by getting the next ot and op before the the above + // flip and continue the flipScanEdgeEvent here + // set new ot and op here and loop back to inScanArea test + // also need to set a new flipTriangle first + // Turns out at first glance that this is somewhat complicated + // so it will have to wait. + } + else + { + TriangulationPoint newP = NextFlipPoint(ep, eq, ot, op); + FlipScanEdgeEvent(tcx, ep, eq, flipTriangle, ot, newP); + } + } + + /// + /// Fills holes in the Advancing Front + /// + private static void FillAdvancingFront(DTSweepContext tcx, AdvancingFrontNode n) + { + double angle; + + // Fill right holes + AdvancingFrontNode node = n.Next; + while (node.HasNext) + { + // if HoleAngle exceeds 90 degrees then break. + if (LargeHole_DontFill(node)) + break; + + Fill(tcx, node); + node = node.Next; + } + + // Fill left holes + node = n.Prev; + while (node.HasPrev) + { + // if HoleAngle exceeds 90 degrees then break. + if (LargeHole_DontFill(node)) + break; + + angle = HoleAngle(node); + if (angle > PI_div2 || angle < -PI_div2) + { + break; + } + Fill(tcx, node); + node = node.Prev; + } + + // Fill right basins + if (n.HasNext && n.Next.HasNext) + { + angle = BasinAngle(n); + if (angle < PI_3div4) + { + FillBasin(tcx, n); + } + } + } + + // True if HoleAngle exceeds 90 degrees. + private static bool LargeHole_DontFill(AdvancingFrontNode node) + { + AdvancingFrontNode nextNode = node.Next; + AdvancingFrontNode prevNode = node.Prev; + if (!AngleExceeds90Degrees(node.Point, nextNode.Point, prevNode.Point)) + return false; + + // Check additional points on front. + AdvancingFrontNode next2Node = nextNode.Next; + // "..Plus.." because only want angles on same side as point being added. + if ((next2Node != null) && !AngleExceedsPlus90DegreesOrIsNegative(node.Point, next2Node.Point, prevNode.Point)) + return false; + + AdvancingFrontNode prev2Node = prevNode.Prev; + // "..Plus.." because only want angles on same side as point being added. + if ((prev2Node != null) && !AngleExceedsPlus90DegreesOrIsNegative(node.Point, nextNode.Point, prev2Node.Point)) + return false; + + return true; + } + + private static bool AngleExceeds90Degrees(TriangulationPoint origin, TriangulationPoint pa, TriangulationPoint pb) + { + double angle = Angle(origin, pa, pb); + bool exceeds90Degrees = ((angle > PI_div2) || (angle < -PI_div2)); + return exceeds90Degrees; + } + + private static bool AngleExceedsPlus90DegreesOrIsNegative(TriangulationPoint origin, TriangulationPoint pa, TriangulationPoint pb) + { + double angle = Angle(origin, pa, pb); + bool exceedsPlus90DegreesOrIsNegative = (angle > PI_div2) || (angle < 0); + return exceedsPlus90DegreesOrIsNegative; + } + + private static double Angle(TriangulationPoint origin, TriangulationPoint pa, TriangulationPoint pb) + { + /* Complex plane + * ab = cosA +i*sinA + * ab = (ax + ay*i)(bx + by*i) = (ax*bx + ay*by) + i(ax*by-ay*bx) + * atan2(y,x) computes the principal value of the argument function + * applied to the complex number x+iy + * Where x = ax*bx + ay*by + * y = ax*by - ay*bx + */ + double px = origin.X; + double py = origin.Y; + double ax = pa.X - px; + double ay = pa.Y - py; + double bx = pb.X - px; + double by = pb.Y - py; + double x = ax * by - ay * bx; + double y = ax * bx + ay * by; + double angle = Math.Atan2(x, y); + return angle; + } + + /// + /// Fills a basin that has formed on the Advancing Front to the right + /// of given node. + /// First we decide a left,bottom and right node that forms the + /// boundaries of the basin. Then we do a reqursive fill. + /// + /// + /// starting node, this or next node will be left node + private static void FillBasin(DTSweepContext tcx, AdvancingFrontNode node) + { + if (TriangulationUtil.Orient2d(node.Point, node.Next.Point, node.Next.Next.Point) == Orientation.CCW) + { + // tcx.basin.leftNode = node.next.next; + tcx.Basin.leftNode = node; + } + else + { + tcx.Basin.leftNode = node.Next; + } + + // Find the bottom and right node + tcx.Basin.bottomNode = tcx.Basin.leftNode; + while (tcx.Basin.bottomNode.HasNext && tcx.Basin.bottomNode.Point.Y >= tcx.Basin.bottomNode.Next.Point.Y) + { + tcx.Basin.bottomNode = tcx.Basin.bottomNode.Next; + } + + if (tcx.Basin.bottomNode == tcx.Basin.leftNode) + { + // No valid basins + return; + } + + tcx.Basin.rightNode = tcx.Basin.bottomNode; + while (tcx.Basin.rightNode.HasNext && tcx.Basin.rightNode.Point.Y < tcx.Basin.rightNode.Next.Point.Y) + { + tcx.Basin.rightNode = tcx.Basin.rightNode.Next; + } + + if (tcx.Basin.rightNode == tcx.Basin.bottomNode) + { + // No valid basins + return; + } + + tcx.Basin.width = tcx.Basin.rightNode.Point.X - tcx.Basin.leftNode.Point.X; + tcx.Basin.leftHighest = tcx.Basin.leftNode.Point.Y > tcx.Basin.rightNode.Point.Y; + + FillBasinReq(tcx, tcx.Basin.bottomNode); + } + + /// + /// Recursive algorithm to fill a Basin with triangles + /// + private static void FillBasinReq(DTSweepContext tcx, AdvancingFrontNode node) + { + // if shallow stop filling + if (IsShallow(tcx, node)) + { + return; + } + + Fill(tcx, node); + if (node.Prev == tcx.Basin.leftNode && node.Next == tcx.Basin.rightNode) + { + return; + } + else if (node.Prev == tcx.Basin.leftNode) + { + Orientation o = TriangulationUtil.Orient2d(node.Point, node.Next.Point, node.Next.Next.Point); + if (o == Orientation.CW) + { + return; + } + node = node.Next; + } + else if (node.Next == tcx.Basin.rightNode) + { + Orientation o = TriangulationUtil.Orient2d(node.Point, node.Prev.Point, node.Prev.Prev.Point); + if (o == Orientation.CCW) + { + return; + } + node = node.Prev; + } + else + { + // Continue with the neighbor node with lowest Y value + if (node.Prev.Point.Y < node.Next.Point.Y) + { + node = node.Prev; + } + else + { + node = node.Next; + } + } + FillBasinReq(tcx, node); + } + + private static bool IsShallow(DTSweepContext tcx, AdvancingFrontNode node) + { + double height; + + if (tcx.Basin.leftHighest) + { + height = tcx.Basin.leftNode.Point.Y - node.Point.Y; + } + else + { + height = tcx.Basin.rightNode.Point.Y - node.Point.Y; + } + if (tcx.Basin.width > height) + { + return true; + } + return false; + } + + /// + /// ??? + /// + /// middle node + /// the angle between 3 front nodes + private static double HoleAngle(AdvancingFrontNode node) + { + // XXX: do we really need a signed angle for holeAngle? + // could possible save some cycles here + /* Complex plane + * ab = cosA +i*sinA + * ab = (ax + ay*i)(bx + by*i) = (ax*bx + ay*by) + i(ax*by-ay*bx) + * atan2(y,x) computes the principal value of the argument function + * applied to the complex number x+iy + * Where x = ax*bx + ay*by + * y = ax*by - ay*bx + */ + double px = node.Point.X; + double py = node.Point.Y; + double ax = node.Next.Point.X - px; + double ay = node.Next.Point.Y - py; + double bx = node.Prev.Point.X - px; + double by = node.Prev.Point.Y - py; + return Math.Atan2(ax * by - ay * bx, ax * bx + ay * by); + } + + /// + /// The basin angle is decided against the horizontal line [1,0] + /// + private static double BasinAngle(AdvancingFrontNode node) + { + double ax = node.Point.X - node.Next.Next.Point.X; + double ay = node.Point.Y - node.Next.Next.Point.Y; + return Math.Atan2(ay, ax); + } + + /// + /// Adds a triangle to the advancing front to fill a hole. + /// + /// + /// middle node, that is the bottom of the hole + private static void Fill(DTSweepContext tcx, AdvancingFrontNode node) + { + DelaunayTriangle triangle = new DelaunayTriangle(node.Prev.Point, node.Point, node.Next.Point); + // TODO: should copy the cEdge value from neighbor triangles + // for now cEdge values are copied during the legalize + triangle.MarkNeighbor(node.Prev.Triangle); + triangle.MarkNeighbor(node.Triangle); + tcx.Triangles.Add(triangle); + + // Update the advancing front + node.Prev.Next = node.Next; + node.Next.Prev = node.Prev; + tcx.RemoveNode(node); + + // If it was legalized the triangle has already been mapped + if (!Legalize(tcx, triangle)) + { + tcx.MapTriangleToNodes(triangle); + } + } + + /// + /// Returns true if triangle was legalized + /// + private static bool Legalize(DTSweepContext tcx, DelaunayTriangle t) + { + // To legalize a triangle we start by finding if any of the three edges + // violate the Delaunay condition + for (int i = 0; i < 3; i++) + { + // TODO: fix so that cEdge is always valid when creating new triangles then we can check it here + // instead of below with ot + if (t.EdgeIsDelaunay[i]) + { + continue; + } + + DelaunayTriangle ot = t.Neighbors[i]; + if (ot != null) + { + TriangulationPoint p = t.Points[i]; + TriangulationPoint op = ot.OppositePoint(t, p); + int oi = ot.IndexOf(op); + // If this is a Constrained Edge or a Delaunay Edge(only during recursive legalization) + // then we should not try to legalize + if (ot.EdgeIsConstrained[oi] || ot.EdgeIsDelaunay[oi]) + { + t.EdgeIsConstrained[i] = ot.EdgeIsConstrained[oi]; + // XXX: have no good way of setting this property when creating new triangles so lets set it here + continue; + } + + bool inside = TriangulationUtil.SmartIncircle(p, t.PointCCW(p), t.PointCW(p), op); + + if (inside) + { + // Lets mark this shared edge as Delaunay + t.EdgeIsDelaunay[i] = true; + ot.EdgeIsDelaunay[oi] = true; + + // Lets rotate shared edge one vertex CW to legalize it + RotateTrianglePair(t, p, ot, op); + + // We now got one valid Delaunay Edge shared by two triangles + // This gives us 4 new edges to check for Delaunay + + // Make sure that triangle to node mapping is done only one time for a specific triangle + bool notLegalized = !Legalize(tcx, t); + + if (notLegalized) + { + tcx.MapTriangleToNodes(t); + } + notLegalized = !Legalize(tcx, ot); + if (notLegalized) + { + tcx.MapTriangleToNodes(ot); + } + + // Reset the Delaunay edges, since they only are valid Delaunay edges + // until we add a new triangle or point. + // XXX: need to think about this. Can these edges be tried after we + // return to previous recursive level? + t.EdgeIsDelaunay[i] = false; + ot.EdgeIsDelaunay[oi] = false; + + // If triangle have been legalized no need to check the other edges since + // the recursive legalization will handles those so we can end here. + return true; + } + } + } + return false; + } + + /// + /// Rotates a triangle pair one vertex CW + /// n2 n2 + /// P +-----+ P +-----+ + /// | t /| |\ t | + /// | / | | \ | + /// n1| / |n3 n1| \ |n3 + /// | / | after CW | \ | + /// |/ oT | | oT \| + /// +-----+ oP +-----+ + /// n4 n4 + /// + private static void RotateTrianglePair(DelaunayTriangle t, TriangulationPoint p, DelaunayTriangle ot, TriangulationPoint op) + { + DelaunayTriangle n1 = t.NeighborCCW(p); + DelaunayTriangle n2 = t.NeighborCW(p); + DelaunayTriangle n3 = ot.NeighborCCW(op); + DelaunayTriangle n4 = ot.NeighborCW(op); + + bool ce1 = t.GetConstrainedEdgeCCW(p); + bool ce2 = t.GetConstrainedEdgeCW(p); + bool ce3 = ot.GetConstrainedEdgeCCW(op); + bool ce4 = ot.GetConstrainedEdgeCW(op); + + bool de1 = t.GetDelaunayEdgeCCW(p); + bool de2 = t.GetDelaunayEdgeCW(p); + bool de3 = ot.GetDelaunayEdgeCCW(op); + bool de4 = ot.GetDelaunayEdgeCW(op); + + t.Legalize(p, op); + ot.Legalize(op, p); + + // Remap dEdge + ot.SetDelaunayEdgeCCW(p, de1); + t.SetDelaunayEdgeCW(p, de2); + t.SetDelaunayEdgeCCW(op, de3); + ot.SetDelaunayEdgeCW(op, de4); + + // Remap cEdge + ot.SetConstrainedEdgeCCW(p, ce1); + t.SetConstrainedEdgeCW(p, ce2); + t.SetConstrainedEdgeCCW(op, ce3); + ot.SetConstrainedEdgeCW(op, ce4); + + // Remap neighbors + // XXX: might optimize the markNeighbor by keeping track of + // what side should be assigned to what neighbor after the + // rotation. Now mark neighbor does lots of testing to find + // the right side. + t.Neighbors.Clear(); + ot.Neighbors.Clear(); + if (n1 != null) ot.MarkNeighbor(n1); + if (n2 != null) t.MarkNeighbor(n2); + if (n3 != null) t.MarkNeighbor(n3); + if (n4 != null) ot.MarkNeighbor(n4); + t.MarkNeighbor(ot); + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepConstraint.cs b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepConstraint.cs new file mode 100644 index 000000000..8c4a6a5a5 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepConstraint.cs @@ -0,0 +1,66 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +namespace FarseerPhysics.Common.Decomposition.CDT.Delaunay.Sweep +{ + internal class DTSweepConstraint : TriangulationConstraint + { + /// + /// Give two points in any order. Will always be ordered so + /// that q.y > p.y and q.x > p.x if same y value + /// + public DTSweepConstraint(TriangulationPoint p1, TriangulationPoint p2) + { + P = p1; + Q = p2; + if (p1.Y > p2.Y) + { + Q = p1; + P = p2; + } + else if (p1.Y == p2.Y) + { + if (p1.X > p2.X) + { + Q = p1; + P = p2; + } + else if (p1.X == p2.X) + { + // logger.info( "Failed to create constraint {}={}", p1, p2 ); + // throw new DuplicatePointException( p1 + "=" + p2 ); + // return; + } + } + Q.AddEdge(this); + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepContext.cs b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepContext.cs new file mode 100644 index 000000000..05347830b --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepContext.cs @@ -0,0 +1,236 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +namespace FarseerPhysics.Common.Decomposition.CDT.Delaunay.Sweep +{ + /** + * + * @author Thomas Åhlén, thahlen@gmail.com + * + */ + + internal class DTSweepContext : TriangulationContext + { + // Inital triangle factor, seed triangle will extend 30% of + // PointSet width to both left and right. + private const float ALPHA = 0.3f; + + public DTSweepBasin Basin = new DTSweepBasin(); + public DTSweepEdgeEvent EdgeEvent = new DTSweepEdgeEvent(); + + private DTSweepPointComparator _comparator = new DTSweepPointComparator(); + public AdvancingFront aFront; + + public DTSweepContext() + { + Clear(); + } + + public TriangulationPoint Head { get; set; } + public TriangulationPoint Tail { get; set; } + + public void RemoveFromList(DelaunayTriangle triangle) + { + Triangles.Remove(triangle); + // TODO: remove all neighbor pointers to this triangle + // for( int i=0; i<3; i++ ) + // { + // if( triangle.neighbors[i] != null ) + // { + // triangle.neighbors[i].clearNeighbor( triangle ); + // } + // } + // triangle.clearNeighbors(); + } + + public void MeshClean(DelaunayTriangle triangle) + { + MeshCleanReq(triangle); + } + + private void MeshCleanReq(DelaunayTriangle triangle) + { + if (triangle != null && !triangle.IsInterior) + { + triangle.IsInterior = true; + Triangulatable.AddTriangle(triangle); + for (int i = 0; i < 3; i++) + { + if (!triangle.EdgeIsConstrained[i]) + { + MeshCleanReq(triangle.Neighbors[i]); + } + } + } + } + + public override void Clear() + { + base.Clear(); + Triangles.Clear(); + } + + public void AddNode(AdvancingFrontNode node) + { + // Console.WriteLine( "add:" + node.key + ":" + System.identityHashCode(node.key)); + // m_nodeTree.put( node.getKey(), node ); + aFront.AddNode(node); + } + + public void RemoveNode(AdvancingFrontNode node) + { + // Console.WriteLine( "remove:" + node.key + ":" + System.identityHashCode(node.key)); + // m_nodeTree.delete( node.getKey() ); + aFront.RemoveNode(node); + } + + public AdvancingFrontNode LocateNode(TriangulationPoint point) + { + return aFront.LocateNode(point); + } + + public void CreateAdvancingFront() + { + AdvancingFrontNode head, tail, middle; + // Initial triangle + DelaunayTriangle iTriangle = new DelaunayTriangle(Points[0], Tail, Head); + Triangles.Add(iTriangle); + + head = new AdvancingFrontNode(iTriangle.Points[1]); + head.Triangle = iTriangle; + middle = new AdvancingFrontNode(iTriangle.Points[0]); + middle.Triangle = iTriangle; + tail = new AdvancingFrontNode(iTriangle.Points[2]); + + aFront = new AdvancingFront(head, tail); + aFront.AddNode(middle); + + // TODO: I think it would be more intuitive if head is middles next and not previous + // so swap head and tail + aFront.Head.Next = middle; + middle.Next = aFront.Tail; + middle.Prev = aFront.Head; + aFront.Tail.Prev = middle; + } + + /// + /// Try to map a node to all sides of this triangle that don't have + /// a neighbor. + /// + public void MapTriangleToNodes(DelaunayTriangle t) + { + AdvancingFrontNode n; + for (int i = 0; i < 3; i++) + { + if (t.Neighbors[i] == null) + { + n = aFront.LocatePoint(t.PointCW(t.Points[i])); + if (n != null) + { + n.Triangle = t; + } + } + } + } + + public override void PrepareTriangulation(Triangulatable t) + { + base.PrepareTriangulation(t); + + double xmax, xmin; + double ymax, ymin; + + xmax = xmin = Points[0].X; + ymax = ymin = Points[0].Y; + + // Calculate bounds. Should be combined with the sorting + foreach (TriangulationPoint p in Points) + { + if (p.X > xmax) + xmax = p.X; + if (p.X < xmin) + xmin = p.X; + if (p.Y > ymax) + ymax = p.Y; + if (p.Y < ymin) + ymin = p.Y; + } + + double deltaX = ALPHA*(xmax - xmin); + double deltaY = ALPHA*(ymax - ymin); + TriangulationPoint p1 = new TriangulationPoint(xmax + deltaX, ymin - deltaY); + TriangulationPoint p2 = new TriangulationPoint(xmin - deltaX, ymin - deltaY); + + Head = p1; + Tail = p2; + + // long time = System.nanoTime(); + // Sort the points along y-axis + Points.Sort(_comparator); + // logger.info( "Triangulation setup [{}ms]", ( System.nanoTime() - time ) / 1e6 ); + } + + + public void FinalizeTriangulation() + { + Triangulatable.AddTriangles(Triangles); + Triangles.Clear(); + } + + public override TriangulationConstraint NewConstraint(TriangulationPoint a, TriangulationPoint b) + { + return new DTSweepConstraint(a, b); + } + + #region Nested type: DTSweepBasin + + public class DTSweepBasin + { + public AdvancingFrontNode bottomNode; + public bool leftHighest; + public AdvancingFrontNode leftNode; + public AdvancingFrontNode rightNode; + public double width; + } + + #endregion + + #region Nested type: DTSweepEdgeEvent + + public class DTSweepEdgeEvent + { + public DTSweepConstraint ConstrainedEdge; + public bool Right; + } + + #endregion + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepPointComparator.cs b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepPointComparator.cs new file mode 100644 index 000000000..dcb18d722 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepPointComparator.cs @@ -0,0 +1,69 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System.Collections.Generic; + +namespace FarseerPhysics.Common.Decomposition.CDT.Delaunay.Sweep +{ + internal class DTSweepPointComparator : IComparer + { + #region IComparer Members + + public int Compare(TriangulationPoint p1, TriangulationPoint p2) + { + if (p1.Y < p2.Y) + { + return -1; + } + else if (p1.Y > p2.Y) + { + return 1; + } + else + { + if (p1.X < p2.X) + { + return -1; + } + else if (p1.X > p2.X) + { + return 1; + } + else + { + return 0; + } + } + } + + #endregion + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/PointOnEdgeException.cs b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/PointOnEdgeException.cs new file mode 100644 index 000000000..81a8d491a --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Delaunay/Sweep/PointOnEdgeException.cs @@ -0,0 +1,43 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System; + +namespace FarseerPhysics.Common.Decomposition.CDT.Delaunay.Sweep +{ + internal class PointOnEdgeException : NotImplementedException + { + public PointOnEdgeException(string message) + : base(message) + { + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/CDT/ITriangulatable.cs b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/ITriangulatable.cs new file mode 100644 index 000000000..3e37fb70c --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/ITriangulatable.cs @@ -0,0 +1,48 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System.Collections.Generic; +using FarseerPhysics.Common.Decomposition.CDT.Delaunay; + +namespace FarseerPhysics.Common.Decomposition.CDT +{ + internal interface Triangulatable + { + IList Points { get; } // MM: Neither of these are used via interface (yet?) + IList Triangles { get; } + TriangulationMode TriangulationMode { get; } + void PrepareTriangulation(TriangulationContext tcx); + + void AddTriangle(DelaunayTriangle t); + void AddTriangles(IEnumerable list); + void ClearTriangles(); + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Orientation.cs b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Orientation.cs new file mode 100644 index 000000000..2597ac2cd --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Orientation.cs @@ -0,0 +1,40 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +namespace FarseerPhysics.Common.Decomposition.CDT +{ + internal enum Orientation + { + CW, + CCW, + Collinear + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Polygon/Polygon.cs b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Polygon/Polygon.cs new file mode 100644 index 000000000..752d6da08 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Polygon/Polygon.cs @@ -0,0 +1,272 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +// Changes from the Java version +// Polygon constructors sprused up, checks for 3+ polys +// Naming of everything +// getTriangulationMode() -> TriangulationMode { get; } +// Exceptions replaced +// Future possibilities +// We have a lot of Add/Clear methods -- we may prefer to just expose the container +// Some self-explanitory methods may deserve commenting anyways + +using System; +using System.Collections.Generic; +using System.Linq; +using FarseerPhysics.Common.Decomposition.CDT.Delaunay; + +namespace FarseerPhysics.Common.Decomposition.CDT.Polygon +{ + internal class Polygon : Triangulatable + { + protected List _holes; + protected PolygonPoint _last; + protected List _points = new List(); + protected List _steinerPoints; + protected List _triangles; + + /// + /// Create a polygon from a list of at least 3 points with no duplicates. + /// + /// A list of unique points + public Polygon(IList points) + { + if (points.Count < 3) throw new ArgumentException("List has fewer than 3 points", "points"); + + // Lets do one sanity check that first and last point hasn't got same position + // Its something that often happen when importing polygon data from other formats + if (points[0].Equals(points[points.Count - 1])) points.RemoveAt(points.Count - 1); + + _points.AddRange(points.Cast()); + } + + /// + /// Create a polygon from a list of at least 3 points with no duplicates. + /// + /// A list of unique points. + public Polygon(IEnumerable points) : this((points as IList) ?? points.ToArray()) + { + } + + public Polygon() + { + } + + public IList Holes + { + get { return _holes; } + } + + #region Triangulatable Members + + public TriangulationMode TriangulationMode + { + get { return TriangulationMode.Polygon; } + } + + public IList Points + { + get { return _points; } + } + + public IList Triangles + { + get { return _triangles; } + } + + public void AddTriangle(DelaunayTriangle t) + { + _triangles.Add(t); + } + + public void AddTriangles(IEnumerable list) + { + _triangles.AddRange(list); + } + + public void ClearTriangles() + { + if (_triangles != null) _triangles.Clear(); + } + + /// + /// Creates constraints and populates the context with points + /// + /// The context + public void PrepareTriangulation(TriangulationContext tcx) + { + if (_triangles == null) + { + _triangles = new List(_points.Count); + } + else + { + _triangles.Clear(); + } + + // Outer constraints + for (int i = 0; i < _points.Count - 1; i++) + { + tcx.NewConstraint(_points[i], _points[i + 1]); + } + tcx.NewConstraint(_points[0], _points[_points.Count - 1]); + tcx.Points.AddRange(_points); + + // Hole constraints + if (_holes != null) + { + foreach (Polygon p in _holes) + { + for (int i = 0; i < p._points.Count - 1; i++) + { + tcx.NewConstraint(p._points[i], p._points[i + 1]); + } + tcx.NewConstraint(p._points[0], p._points[p._points.Count - 1]); + tcx.Points.AddRange(p._points); + } + } + + if (_steinerPoints != null) + { + tcx.Points.AddRange(_steinerPoints); + } + } + + #endregion + + public void AddSteinerPoint(TriangulationPoint point) + { + if (_steinerPoints == null) + { + _steinerPoints = new List(); + } + _steinerPoints.Add(point); + } + + public void AddSteinerPoints(List points) + { + if (_steinerPoints == null) + { + _steinerPoints = new List(); + } + _steinerPoints.AddRange(points); + } + + public void ClearSteinerPoints() + { + if (_steinerPoints != null) + { + _steinerPoints.Clear(); + } + } + + /// + /// Add a hole to the polygon. + /// + /// A subtraction polygon fully contained inside this polygon. + public void AddHole(Polygon poly) + { + if (_holes == null) _holes = new List(); + _holes.Add(poly); + // XXX: tests could be made here to be sure it is fully inside + // addSubtraction( poly.getPoints() ); + } + + /// + /// Inserts newPoint after point. + /// + /// The point to insert after in the polygon + /// The point to insert into the polygon + public void InsertPointAfter(PolygonPoint point, PolygonPoint newPoint) + { + // Validate that + int index = _points.IndexOf(point); + if (index == -1) + throw new ArgumentException( + "Tried to insert a point into a Polygon after a point not belonging to the Polygon", "point"); + newPoint.Next = point.Next; + newPoint.Previous = point; + point.Next.Previous = newPoint; + point.Next = newPoint; + _points.Insert(index + 1, newPoint); + } + + /// + /// Inserts list (after last point in polygon?) + /// + /// + public void AddPoints(IEnumerable list) + { + PolygonPoint first; + foreach (PolygonPoint p in list) + { + p.Previous = _last; + if (_last != null) + { + p.Next = _last.Next; + _last.Next = p; + } + _last = p; + _points.Add(p); + } + first = (PolygonPoint) _points[0]; + _last.Next = first; + first.Previous = _last; + } + + /// + /// Adds a point after the last in the polygon. + /// + /// The point to add + public void AddPoint(PolygonPoint p) + { + p.Previous = _last; + p.Next = _last.Next; + _last.Next = p; + _points.Add(p); + } + + /// + /// Removes a point from the polygon. + /// + /// + public void RemovePoint(PolygonPoint p) + { + PolygonPoint next, prev; + + next = p.Next; + prev = p.Previous; + prev.Next = next; + next.Previous = prev; + _points.Remove(p); + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Polygon/PolygonPoint.cs b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Polygon/PolygonPoint.cs new file mode 100644 index 000000000..77b3d7649 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Polygon/PolygonPoint.cs @@ -0,0 +1,48 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +// Changes from the Java version +// Replaced get/set Next/Previous with attributes +// Future possibilities +// Documentation! + +namespace FarseerPhysics.Common.Decomposition.CDT.Polygon +{ + internal class PolygonPoint : TriangulationPoint + { + public PolygonPoint(double x, double y) : base(x, y) + { + } + + public PolygonPoint Next { get; set; } + public PolygonPoint Previous { get; set; } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Polygon/PolygonSet.cs b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Polygon/PolygonSet.cs new file mode 100644 index 000000000..f3142d6b3 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Polygon/PolygonSet.cs @@ -0,0 +1,65 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +// Changes from the Java version +// Replaced getPolygons with attribute +// Future possibilities +// Replace Add(Polygon) with exposed container? +// Replace entire class with HashSet ? + +using System.Collections.Generic; + +namespace FarseerPhysics.Common.Decomposition.CDT.Polygon +{ + internal class PolygonSet + { + protected List _polygons = new List(); + + public PolygonSet() + { + } + + public PolygonSet(Polygon poly) + { + _polygons.Add(poly); + } + + public IEnumerable Polygons + { + get { return _polygons; } + } + + public void Add(Polygon p) + { + _polygons.Add(p); + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Sets/ConstrainedPointSet.cs b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Sets/ConstrainedPointSet.cs new file mode 100644 index 000000000..f6294f923 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Sets/ConstrainedPointSet.cs @@ -0,0 +1,114 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System.Collections.Generic; + +namespace FarseerPhysics.Common.Decomposition.CDT.Sets +{ + /* + * Extends the PointSet by adding some Constraints on how it will be triangulated
+ * A constraint defines an edge between two points in the set, these edges can not + * be crossed. They will be enforced triangle edges after a triangulation. + *

+ * + * + * @author Thomas Åhlén, thahlen@gmail.com + */ + + internal class ConstrainedPointSet : PointSet + { + private List _constrainedPointList; + + public ConstrainedPointSet(List points, int[] index) + : base(points) + { + EdgeIndex = index; + } + + /** + * + * @param points - A list of all points in PointSet + * @param constraints - Pairs of two points defining a constraint, all points must be part of given PointSet! + */ + + public ConstrainedPointSet(List points, IEnumerable constraints) + : base(points) + { + _constrainedPointList = new List(); + _constrainedPointList.AddRange(constraints); + } + + public int[] EdgeIndex { get; private set; } + + public override TriangulationMode TriangulationMode + { + get { return TriangulationMode.Constrained; } + } + + public override void PrepareTriangulation(TriangulationContext tcx) + { + base.PrepareTriangulation(tcx); + if (_constrainedPointList != null) + { + TriangulationPoint p1, p2; + List.Enumerator iterator = _constrainedPointList.GetEnumerator(); + while (iterator.MoveNext()) + { + p1 = iterator.Current; + iterator.MoveNext(); + p2 = iterator.Current; + tcx.NewConstraint(p1, p2); + } + } + else + { + for (int i = 0; i < EdgeIndex.Length; i += 2) + { + // XXX: must change!! + tcx.NewConstraint(Points[EdgeIndex[i]], Points[EdgeIndex[i + 1]]); + } + } + } + + /** + * TODO: TO BE IMPLEMENTED! + * Peforms a validation on given input
+ * 1. Check's if there any constraint edges are crossing or collinear
+ * 2. + * @return + */ + + public bool isValid() + { + return true; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Sets/PointSet.cs b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Sets/PointSet.cs new file mode 100644 index 000000000..ea840efe4 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Sets/PointSet.cs @@ -0,0 +1,84 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System.Collections.Generic; +using FarseerPhysics.Common.Decomposition.CDT.Delaunay; + +namespace FarseerPhysics.Common.Decomposition.CDT.Sets +{ + internal class PointSet : Triangulatable + { + public PointSet(List points) + { + Points = new List(points); + } + + #region Triangulatable Members + + public IList Points { get; private set; } + public IList Triangles { get; private set; } + + public virtual TriangulationMode TriangulationMode + { + get { return TriangulationMode.Unconstrained; } + } + + public void AddTriangle(DelaunayTriangle t) + { + Triangles.Add(t); + } + + public void AddTriangles(IEnumerable list) + { + foreach (DelaunayTriangle tri in list) Triangles.Add(tri); + } + + public void ClearTriangles() + { + Triangles.Clear(); + } + + public virtual void PrepareTriangulation(TriangulationContext tcx) + { + if (Triangles == null) + { + Triangles = new List(Points.Count); + } + else + { + Triangles.Clear(); + } + tcx.Points.AddRange(Points); + } + + #endregion + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationConstraint.cs b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationConstraint.cs new file mode 100644 index 000000000..91af9274b --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationConstraint.cs @@ -0,0 +1,46 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +/** + * Forces a triangle edge between two points p and q + * when triangulating. For example used to enforce + * Polygon Edges during a polygon triangulation. + * + * @author Thomas Åhlén, thahlen@gmail.com + */ +namespace FarseerPhysics.Common.Decomposition.CDT +{ + internal class TriangulationConstraint + { + public TriangulationPoint P; + public TriangulationPoint Q; + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationContext.cs b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationContext.cs new file mode 100644 index 000000000..c1b9f7c83 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationContext.cs @@ -0,0 +1,84 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System.Collections.Generic; +using System.Runtime.CompilerServices; +using FarseerPhysics.Common.Decomposition.CDT.Delaunay; + +namespace FarseerPhysics.Common.Decomposition.CDT +{ + internal abstract class TriangulationContext + { + public readonly List Points = new List(200); + public readonly List Triangles = new List(); + private int _stepTime = -1; + + public TriangulationContext() + { + Terminated = false; + } + + public TriangulationMode TriangulationMode { get; protected set; } + public Triangulatable Triangulatable { get; private set; } + + public bool WaitUntilNotified { get; private set; } + public bool Terminated { get; set; } + + public int StepCount { get; private set; } + public virtual bool IsDebugEnabled { get; protected set; } + + public void Done() + { + StepCount++; + } + + public virtual void PrepareTriangulation(Triangulatable t) + { + Triangulatable = t; + TriangulationMode = t.TriangulationMode; + t.PrepareTriangulation(this); + } + + public abstract TriangulationConstraint NewConstraint(TriangulationPoint a, TriangulationPoint b); + + [MethodImpl(MethodImplOptions.Synchronized)] + public void Update(string message) + { + } + + public virtual void Clear() + { + Points.Clear(); + Terminated = false; + StepCount = 0; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationMode.cs b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationMode.cs new file mode 100644 index 000000000..af3b83f60 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationMode.cs @@ -0,0 +1,40 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +namespace FarseerPhysics.Common.Decomposition.CDT +{ + internal enum TriangulationMode + { + Unconstrained, + Constrained, + Polygon + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationPoint.cs b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationPoint.cs new file mode 100644 index 000000000..cc4d03352 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationPoint.cs @@ -0,0 +1,82 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System.Collections.Generic; +using FarseerPhysics.Common.Decomposition.CDT.Delaunay.Sweep; + +namespace FarseerPhysics.Common.Decomposition.CDT +{ + internal class TriangulationPoint + { + // List of edges this point constitutes an upper ending point (CDT) + + public double X, Y; + + public TriangulationPoint(double x, double y) + { + X = x; + Y = y; + } + + public List Edges { get; private set; } + + public float Xf + { + get { return (float) X; } + set { X = value; } + } + + public float Yf + { + get { return (float) Y; } + set { Y = value; } + } + + public bool HasEdges + { + get { return Edges != null; } + } + + public override string ToString() + { + return "[" + X + "," + Y + "]"; + } + + public void AddEdge(DTSweepConstraint e) + { + if (Edges == null) + { + Edges = new List(); + } + Edges.Add(e); + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationUtil.cs b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationUtil.cs new file mode 100644 index 000000000..c346e8d09 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/TriangulationUtil.cs @@ -0,0 +1,175 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +namespace FarseerPhysics.Common.Decomposition.CDT +{ + /** + * @author Thomas Åhlén, thahlen@gmail.com + */ + + internal class TriangulationUtil + { + public static double EPSILON = 1e-12; + + ///

+ /// Requirements: + /// 1. a,b and c form a triangle. + /// 2. a and d is know to be on opposite side of bc + /// + /// a + /// + + /// / \ + /// / \ + /// b/ \c + /// +-------+ + /// / B \ + /// / \ + /// + /// Facts: + /// d has to be in area B to have a chance to be inside the circle formed by a,b and c + /// d is outside B if orient2d(a,b,d) or orient2d(c,a,d) is CW + /// This preknowledge gives us a way to optimize the incircle test + /// + /// triangle point, opposite d + /// triangle point + /// triangle point + /// point opposite a + /// true if d is inside circle, false if on circle edge + public static bool SmartIncircle(TriangulationPoint pa, TriangulationPoint pb, TriangulationPoint pc, + TriangulationPoint pd) + { + double pdx = pd.X; + double pdy = pd.Y; + double adx = pa.X - pdx; + double ady = pa.Y - pdy; + double bdx = pb.X - pdx; + double bdy = pb.Y - pdy; + + double adxbdy = adx * bdy; + double bdxady = bdx * ady; + double oabd = adxbdy - bdxady; + // oabd = orient2d(pa,pb,pd); + if (oabd <= 0) return false; + + double cdx = pc.X - pdx; + double cdy = pc.Y - pdy; + + double cdxady = cdx * ady; + double adxcdy = adx * cdy; + double ocad = cdxady - adxcdy; + // ocad = orient2d(pc,pa,pd); + if (ocad <= 0) return false; + + double bdxcdy = bdx * cdy; + double cdxbdy = cdx * bdy; + + double alift = adx * adx + ady * ady; + double blift = bdx * bdx + bdy * bdy; + double clift = cdx * cdx + cdy * cdy; + + double det = alift * (bdxcdy - cdxbdy) + blift * ocad + clift * oabd; + + return det > 0; + } + /* + public static bool InScanArea(TriangulationPoint pa, TriangulationPoint pb, TriangulationPoint pc, + TriangulationPoint pd) + { + double pdx = pd.X; + double pdy = pd.Y; + double adx = pa.X - pdx; + double ady = pa.Y - pdy; + double bdx = pb.X - pdx; + double bdy = pb.Y - pdy; + + double adxbdy = adx*bdy; + double bdxady = bdx*ady; + double oabd = adxbdy - bdxady; + // oabd = orient2d(pa,pb,pd); + if (oabd <= 0) + { + return false; + } + + double cdx = pc.X - pdx; + double cdy = pc.Y - pdy; + + double cdxady = cdx*ady; + double adxcdy = adx*cdy; + double ocad = cdxady - adxcdy; + // ocad = orient2d(pc,pa,pd); + if (ocad <= 0) + { + return false; + } + return true; + } + */ + + public static bool InScanArea(TriangulationPoint pa, TriangulationPoint pb, TriangulationPoint pc, TriangulationPoint pd) + { + double oadb = (pa.X - pb.X) * (pd.Y - pb.Y) - (pd.X - pb.X) * (pa.Y - pb.Y); + if (oadb >= -EPSILON) + { + return false; + } + + double oadc = (pa.X - pc.X) * (pd.Y - pc.Y) - (pd.X - pc.X) * (pa.Y - pc.Y); + if (oadc <= EPSILON) + { + return false; + } + return true; + } + + /// Forumla to calculate signed area + /// Positive if CCW + /// Negative if CW + /// 0 if collinear + /// A[P1,P2,P3] = (x1*y2 - y1*x2) + (x2*y3 - y2*x3) + (x3*y1 - y3*x1) + /// = (x1-x3)*(y2-y3) - (y1-y3)*(x2-x3) + public static Orientation Orient2d(TriangulationPoint pa, TriangulationPoint pb, TriangulationPoint pc) + { + double detleft = (pa.X - pc.X) * (pb.Y - pc.Y); + double detright = (pa.Y - pc.Y) * (pb.X - pc.X); + double val = detleft - detright; + if (val > -EPSILON && val < EPSILON) + { + return Orientation.Collinear; + } + else if (val > 0) + { + return Orientation.CCW; + } + return Orientation.CW; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/FixedArray3.cs b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/FixedArray3.cs new file mode 100644 index 000000000..e4d5c759b --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/FixedArray3.cs @@ -0,0 +1,118 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using System.Collections; +using System.Collections.Generic; + +namespace FarseerPhysics.Common.Decomposition.CDT.Util +{ + internal struct FixedArray3 : IEnumerable where T : class + { + public T _0, _1, _2; + + public T this[int index] + { + get + { + switch (index) + { + case 0: + return _0; + case 1: + return _1; + case 2: + return _2; + default: + throw new IndexOutOfRangeException(); + } + } + set + { + switch (index) + { + case 0: + _0 = value; + break; + case 1: + _1 = value; + break; + case 2: + _2 = value; + break; + default: + throw new IndexOutOfRangeException(); + } + } + } + + #region IEnumerable Members + + public IEnumerator GetEnumerator() + { + return Enumerate().GetEnumerator(); + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + + #endregion + + public bool Contains(T value) + { + for (int i = 0; i < 3; ++i) if (this[i] == value) return true; + return false; + } + + public int IndexOf(T value) + { + for (int i = 0; i < 3; ++i) if (this[i] == value) return i; + return -1; + } + + public void Clear() + { + _0 = _1 = _2 = null; + } + + public void Clear(T value) + { + for (int i = 0; i < 3; ++i) if (this[i] == value) this[i] = null; + } + + private IEnumerable Enumerate() + { + for (int i = 0; i < 3; ++i) yield return this[i]; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/FixedBitArray3.cs b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/FixedBitArray3.cs new file mode 100644 index 000000000..bb488bad1 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/FixedBitArray3.cs @@ -0,0 +1,118 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using System.Collections; +using System.Collections.Generic; + +namespace FarseerPhysics.Common.Decomposition.CDT.Util +{ + internal struct FixedBitArray3 : IEnumerable + { + public bool _0, _1, _2; + + public bool this[int index] + { + get + { + switch (index) + { + case 0: + return _0; + case 1: + return _1; + case 2: + return _2; + default: + throw new IndexOutOfRangeException(); + } + } + set + { + switch (index) + { + case 0: + _0 = value; + break; + case 1: + _1 = value; + break; + case 2: + _2 = value; + break; + default: + throw new IndexOutOfRangeException(); + } + } + } + + #region IEnumerable Members + + public IEnumerator GetEnumerator() + { + return Enumerate().GetEnumerator(); + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + + #endregion + + public bool Contains(bool value) + { + for (int i = 0; i < 3; ++i) if (this[i] == value) return true; + return false; + } + + public int IndexOf(bool value) + { + for (int i = 0; i < 3; ++i) if (this[i] == value) return i; + return -1; + } + + public void Clear() + { + _0 = _1 = _2 = false; + } + + public void Clear(bool value) + { + for (int i = 0; i < 3; ++i) if (this[i] == value) this[i] = false; + } + + private IEnumerable Enumerate() + { + for (int i = 0; i < 3; ++i) yield return this[i]; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/PointGenerator.cs b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/PointGenerator.cs new file mode 100644 index 000000000..96f44add0 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/PointGenerator.cs @@ -0,0 +1,38 @@ +using System; +using System.Collections.Generic; + +namespace FarseerPhysics.Common.Decomposition.CDT.Util +{ + internal class PointGenerator + { + private static readonly Random RNG = new Random(); + + public static List UniformDistribution(int n, double scale) + { + List points = new List(); + for (int i = 0; i < n; i++) + { + points.Add(new TriangulationPoint(scale*(0.5 - RNG.NextDouble()), scale*(0.5 - RNG.NextDouble()))); + } + return points; + } + + public static List UniformGrid(int n, double scale) + { + double x = 0; + double size = scale/n; + double halfScale = 0.5*scale; + + List points = new List(); + for (int i = 0; i < n + 1; i++) + { + x = halfScale - i*size; + for (int j = 0; j < n + 1; j++) + { + points.Add(new TriangulationPoint(x, halfScale - j*size)); + } + } + return points; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/PolygonGenerator.cs b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/PolygonGenerator.cs new file mode 100644 index 000000000..e91bb03dc --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/CDT/Util/PolygonGenerator.cs @@ -0,0 +1,98 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using FarseerPhysics.Common.Decomposition.CDT.Polygon; + +namespace FarseerPhysics.Common.Decomposition.CDT.Util +{ + internal class PolygonGenerator + { + private static readonly Random RNG = new Random(); + + private static double PI_2 = 2.0*Math.PI; + + public static Polygon.Polygon RandomCircleSweep(double scale, int vertexCount) + { + PolygonPoint point; + PolygonPoint[] points; + double radius = scale/4; + + points = new PolygonPoint[vertexCount]; + for (int i = 0; i < vertexCount; i++) + { + do + { + if (i%250 == 0) + { + radius += scale/2*(0.5 - RNG.NextDouble()); + } + else if (i%50 == 0) + { + radius += scale/5*(0.5 - RNG.NextDouble()); + } + else + { + radius += 25*scale/vertexCount*(0.5 - RNG.NextDouble()); + } + radius = radius > scale/2 ? scale/2 : radius; + radius = radius < scale/10 ? scale/10 : radius; + } while (radius < scale/10 || radius > scale/2); + point = new PolygonPoint(radius*Math.Cos((PI_2*i)/vertexCount), + radius*Math.Sin((PI_2*i)/vertexCount)); + points[i] = point; + } + return new Polygon.Polygon(points); + } + + public static Polygon.Polygon RandomCircleSweep2(double scale, int vertexCount) + { + PolygonPoint point; + PolygonPoint[] points; + double radius = scale/4; + + points = new PolygonPoint[vertexCount]; + for (int i = 0; i < vertexCount; i++) + { + do + { + radius += scale/5*(0.5 - RNG.NextDouble()); + radius = radius > scale/2 ? scale/2 : radius; + radius = radius < scale/10 ? scale/10 : radius; + } while (radius < scale/10 || radius > scale/2); + point = new PolygonPoint(radius*Math.Cos((PI_2*i)/vertexCount), + radius*Math.Sin((PI_2*i)/vertexCount)); + points[i] = point; + } + return new Polygon.Polygon(points); + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/CDTDecomposer.cs b/Farseer Physics Engine 3.5/Common/Decomposition/CDTDecomposer.cs new file mode 100644 index 000000000..48618e32f --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/CDTDecomposer.cs @@ -0,0 +1,75 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +*/ + +using System.Collections.Generic; +using System.Diagnostics; +using FarseerPhysics.Common.Decomposition.CDT; +using FarseerPhysics.Common.Decomposition.CDT.Delaunay; +using FarseerPhysics.Common.Decomposition.CDT.Delaunay.Sweep; +using FarseerPhysics.Common.Decomposition.CDT.Polygon; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common.Decomposition +{ + /// + /// 2D constrained Delaunay triangulation algorithm. + /// Based on the paper "Sweep-line algorithm for constrained Delaunay triangulation" by V. Domiter and and B. Zalik + /// + /// Properties: + /// - Creates triangles with a large interior angle. + /// - Supports holes + /// - Generate a lot of garbage due to incapsulation of the Poly2Tri library. + /// - Running time is O(n^2), n = number of vertices. + /// - Does not care about winding order. + /// + /// Source: http://code.google.com/p/poly2tri/ + /// + internal static class CDTDecomposer + { + /// + /// Decompose the polygon into several smaller non-concave polygon. + /// + public static List ConvexPartition(Vertices vertices) + { + Debug.Assert(vertices.Count > 3); + + Polygon poly = new Polygon(); + + foreach (Vector2 vertex in vertices) + poly.Points.Add(new TriangulationPoint(vertex.X, vertex.Y)); + + if (vertices.Holes != null) + { + foreach (Vertices holeVertices in vertices.Holes) + { + Polygon hole = new Polygon(); + + foreach (Vector2 vertex in holeVertices) + hole.Points.Add(new TriangulationPoint(vertex.X, vertex.Y)); + + poly.AddHole(hole); + } + } + + DTSweepContext tcx = new DTSweepContext(); + tcx.PrepareTriangulation(poly); + DTSweep.Triangulate(tcx); + + List results = new List(); + + foreach (DelaunayTriangle triangle in poly.Triangles) + { + Vertices v = new Vertices(); + foreach (TriangulationPoint p in triangle.Points) + { + v.Add(new Vector2((float)p.X, (float)p.Y)); + } + results.Add(v); + } + + return results; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/EarclipDecomposer.cs b/Farseer Physics Engine 3.5/Common/Decomposition/EarclipDecomposer.cs new file mode 100644 index 000000000..489e56f85 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/EarclipDecomposer.cs @@ -0,0 +1,403 @@ +/* +* C# Version Ported by Matt Bettcher and Ian Qvist 2009-2010 +* +* Original C++ Version Copyright (c) 2007 Eric Jordan +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Diagnostics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common.Decomposition +{ + /// + /// Convex decomposition algorithm using ear clipping + /// + /// Properties: + /// - Only works on simple polygons. + /// - Does not support holes. + /// - Running time is O(n^2), n = number of vertices. + /// + /// Source: http://www.ewjordan.com/earClip/ + /// + internal static class EarclipDecomposer + { + //box2D rev 32 - for details, see http://www.box2d.org/forum/viewtopic.php?f=4&t=83&start=50 + + /// + /// Decompose the polygon into several smaller non-concave polygon. + /// Each resulting polygon will have no more than Settings.MaxPolygonVertices vertices. + /// + /// The vertices. + /// The tolerance. + public static List ConvexPartition(Vertices vertices, float tolerance = 0.001f) + { + Debug.Assert(vertices.Count > 3); + Debug.Assert(!vertices.IsCounterClockWise()); + + return TriangulatePolygon(vertices, tolerance); + } + + /// + /// Triangulates a polygon using simple ear-clipping algorithm. Returns + /// size of Triangle array unless the polygon can't be triangulated. + /// This should only happen if the polygon self-intersects, + /// though it will not _always_ return null for a bad polygon - it is the + /// caller's responsibility to check for self-intersection, and if it + /// doesn't, it should at least check that the return value is non-null + /// before using. You're warned! + /// + /// Triangles may be degenerate, especially if you have identical points + /// in the input to the algorithm. Check this before you use them. + /// + /// This is totally unoptimized, so for large polygons it should not be part + /// of the simulation loop. + /// + /// + /// Only works on simple polygons. + /// + private static List TriangulatePolygon(Vertices vertices, float tolerance) + { + //FPE note: Check is needed as invalid triangles can be returned in recursive calls. + if (vertices.Count < 3) + return new List(); + + List results = new List(); + + //Recurse and split on pinch points + Vertices pA, pB; + Vertices pin = new Vertices(vertices); + if (ResolvePinchPoint(pin, out pA, out pB, tolerance)) + { + List mergeA = TriangulatePolygon(pA, tolerance); + List mergeB = TriangulatePolygon(pB, tolerance); + + if (mergeA.Count == -1 || mergeB.Count == -1) + throw new Exception("Can't triangulate your polygon."); + + for (int i = 0; i < mergeA.Count; ++i) + { + results.Add(new Vertices(mergeA[i])); + } + for (int i = 0; i < mergeB.Count; ++i) + { + results.Add(new Vertices(mergeB[i])); + } + + return results; + } + + Vertices[] buffer = new Vertices[vertices.Count - 2]; + int bufferSize = 0; + float[] xrem = new float[vertices.Count]; + float[] yrem = new float[vertices.Count]; + for (int i = 0; i < vertices.Count; ++i) + { + xrem[i] = vertices[i].X; + yrem[i] = vertices[i].Y; + } + + int vNum = vertices.Count; + + while (vNum > 3) + { + // Find an ear + int earIndex = -1; + float earMaxMinCross = -10.0f; + for (int i = 0; i < vNum; ++i) + { + if (IsEar(i, xrem, yrem, vNum)) + { + int lower = Remainder(i - 1, vNum); + int upper = Remainder(i + 1, vNum); + Vector2 d1 = new Vector2(xrem[upper] - xrem[i], yrem[upper] - yrem[i]); + Vector2 d2 = new Vector2(xrem[i] - xrem[lower], yrem[i] - yrem[lower]); + Vector2 d3 = new Vector2(xrem[lower] - xrem[upper], yrem[lower] - yrem[upper]); + + d1.Normalize(); + d2.Normalize(); + d3.Normalize(); + float cross12; + MathUtils.Cross(ref d1, ref d2, out cross12); + cross12 = Math.Abs(cross12); + + float cross23; + MathUtils.Cross(ref d2, ref d3, out cross23); + cross23 = Math.Abs(cross23); + + float cross31; + MathUtils.Cross(ref d3, ref d1, out cross31); + cross31 = Math.Abs(cross31); + + //Find the maximum minimum angle + float minCross = Math.Min(cross12, Math.Min(cross23, cross31)); + if (minCross > earMaxMinCross) + { + earIndex = i; + earMaxMinCross = minCross; + } + } + } + + // If we still haven't found an ear, we're screwed. + // Note: sometimes this is happening because the + // remaining points are collinear. Really these + // should just be thrown out without halting triangulation. + if (earIndex == -1) + { + for (int i = 0; i < bufferSize; i++) + { + results.Add(buffer[i]); + } + + return results; + } + + // Clip off the ear: + // - remove the ear tip from the list + + --vNum; + float[] newx = new float[vNum]; + float[] newy = new float[vNum]; + int currDest = 0; + for (int i = 0; i < vNum; ++i) + { + if (currDest == earIndex) ++currDest; + newx[i] = xrem[currDest]; + newy[i] = yrem[currDest]; + ++currDest; + } + + // - add the clipped triangle to the triangle list + int under = (earIndex == 0) ? (vNum) : (earIndex - 1); + int over = (earIndex == vNum) ? 0 : (earIndex + 1); + Triangle toAdd = new Triangle(xrem[earIndex], yrem[earIndex], xrem[over], yrem[over], xrem[under], + yrem[under]); + buffer[bufferSize] = toAdd; + ++bufferSize; + + // - replace the old list with the new one + xrem = newx; + yrem = newy; + } + + Triangle tooAdd = new Triangle(xrem[1], yrem[1], xrem[2], yrem[2], xrem[0], yrem[0]); + buffer[bufferSize] = tooAdd; + ++bufferSize; + + for (int i = 0; i < bufferSize; i++) + { + results.Add(new Vertices(buffer[i])); + } + + return results; + } + + /// + /// Finds and fixes "pinch points," points where two polygon + /// vertices are at the same point. + /// + /// If a pinch point is found, pin is broken up into poutA and poutB + /// and true is returned; otherwise, returns false. + /// + /// Mostly for internal use. + /// + /// O(N^2) time, which sucks... + /// + /// The pin. + /// The pout A. + /// The pout B. + /// + private static bool ResolvePinchPoint(Vertices pin, out Vertices poutA, out Vertices poutB, float tolerance) + { + poutA = new Vertices(); + poutB = new Vertices(); + + if (pin.Count < 3) + return false; + + bool hasPinchPoint = false; + int pinchIndexA = -1; + int pinchIndexB = -1; + for (int i = 0; i < pin.Count; ++i) + { + for (int j = i + 1; j < pin.Count; ++j) + { + //Don't worry about pinch points where the points + //are actually just dupe neighbors + if (Math.Abs(pin[i].X - pin[j].X) < tolerance && Math.Abs(pin[i].Y - pin[j].Y) < tolerance && j != i + 1) + { + pinchIndexA = i; + pinchIndexB = j; + hasPinchPoint = true; + break; + } + } + if (hasPinchPoint) break; + } + if (hasPinchPoint) + { + int sizeA = pinchIndexB - pinchIndexA; + if (sizeA == pin.Count) return false; //has dupe points at wraparound, not a problem here + for (int i = 0; i < sizeA; ++i) + { + int ind = Remainder(pinchIndexA + i, pin.Count); // is this right + poutA.Add(pin[ind]); + } + + int sizeB = pin.Count - sizeA; + for (int i = 0; i < sizeB; ++i) + { + int ind = Remainder(pinchIndexB + i, pin.Count); // is this right + poutB.Add(pin[ind]); + } + } + return hasPinchPoint; + } + + /// + /// Fix for obnoxious behavior for the % operator for negative numbers... + /// + /// The x. + /// The modulus. + /// + private static int Remainder(int x, int modulus) + { + int rem = x % modulus; + while (rem < 0) + { + rem += modulus; + } + return rem; + } + + /// + /// Checks if vertex i is the tip of an ear in polygon defined by xv[] and yv[]. + /// + /// The i. + /// The xv. + /// The yv. + /// Length of the xv. + /// + /// Assumes clockwise orientation of polygon. + /// + /// + /// true if the specified i is ear; otherwise, false. + /// + private static bool IsEar(int i, float[] xv, float[] yv, int xvLength) + { + float dx0, dy0, dx1, dy1; + if (i >= xvLength || i < 0 || xvLength < 3) + { + return false; + } + int upper = i + 1; + int lower = i - 1; + if (i == 0) + { + dx0 = xv[0] - xv[xvLength - 1]; + dy0 = yv[0] - yv[xvLength - 1]; + dx1 = xv[1] - xv[0]; + dy1 = yv[1] - yv[0]; + lower = xvLength - 1; + } + else if (i == xvLength - 1) + { + dx0 = xv[i] - xv[i - 1]; + dy0 = yv[i] - yv[i - 1]; + dx1 = xv[0] - xv[i]; + dy1 = yv[0] - yv[i]; + upper = 0; + } + else + { + dx0 = xv[i] - xv[i - 1]; + dy0 = yv[i] - yv[i - 1]; + dx1 = xv[i + 1] - xv[i]; + dy1 = yv[i + 1] - yv[i]; + } + + float cross = dx0 * dy1 - dx1 * dy0; + + if (cross > 0) + return false; + + Triangle myTri = new Triangle(xv[i], yv[i], xv[upper], yv[upper], xv[lower], yv[lower]); + + for (int j = 0; j < xvLength; ++j) + { + if (j == i || j == lower || j == upper) + continue; + if (myTri.IsInside(xv[j], yv[j])) + return false; + } + return true; + } + + private class Triangle : Vertices + { + //Constructor automatically fixes orientation to ccw + public Triangle(float x1, float y1, float x2, float y2, float x3, float y3) + { + float cross = (x2 - x1) * (y3 - y1) - (x3 - x1) * (y2 - y1); + if (cross > 0) + { + Add(new Vector2(x1, y1)); + Add(new Vector2(x2, y2)); + Add(new Vector2(x3, y3)); + } + else + { + Add(new Vector2(x1, y1)); + Add(new Vector2(x3, y3)); + Add(new Vector2(x2, y2)); + } + } + + public bool IsInside(float x, float y) + { + Vector2 a = this[0]; + Vector2 b = this[1]; + Vector2 c = this[2]; + + if (x < a.X && x < b.X && x < c.X) return false; + if (x > a.X && x > b.X && x > c.X) return false; + if (y < a.Y && y < b.Y && y < c.Y) return false; + if (y > a.Y && y > b.Y && y > c.Y) return false; + + float vx2 = x - a.X; + float vy2 = y - a.Y; + float vx1 = b.X - a.X; + float vy1 = b.Y - a.Y; + float vx0 = c.X - a.X; + float vy0 = c.Y - a.Y; + + float dot00 = vx0 * vx0 + vy0 * vy0; + float dot01 = vx0 * vx1 + vy0 * vy1; + float dot02 = vx0 * vx2 + vy0 * vy2; + float dot11 = vx1 * vx1 + vy1 * vy1; + float dot12 = vx1 * vx2 + vy1 * vy2; + float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01); + float u = (dot11 * dot02 - dot01 * dot12) * invDenom; + float v = (dot00 * dot12 - dot01 * dot02) * invDenom; + + return ((u > 0) && (v > 0) && (u + v < 1)); + } + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/FlipcodeDecomposer.cs b/Farseer Physics Engine 3.5/Common/Decomposition/FlipcodeDecomposer.cs new file mode 100644 index 000000000..895cd3fa1 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/FlipcodeDecomposer.cs @@ -0,0 +1,152 @@ +using System.Collections.Generic; +using System.Diagnostics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common.Decomposition +{ + /// + /// Convex decomposition algorithm created by unknown + /// + /// Properties: + /// - No support for holes + /// - Very fast + /// - Only works on simple polygons + /// - Only works on counter clockwise polygons + /// + /// More information: http://www.flipcode.com/archives/Efficient_Polygon_Triangulation.shtml + /// + internal static class FlipcodeDecomposer + { + private static Vector2 _tmpA; + private static Vector2 _tmpB; + private static Vector2 _tmpC; + + /// + /// Decompose the polygon into triangles. + /// + /// Properties: + /// - Only works on counter clockwise polygons + /// + /// + /// The list of points describing the polygon + public static List ConvexPartition(Vertices vertices) + { + Debug.Assert(vertices.Count > 3); + Debug.Assert(vertices.IsCounterClockWise()); + + int[] polygon = new int[vertices.Count]; + + for (int v = 0; v < vertices.Count; v++) + polygon[v] = v; + + int nv = vertices.Count; + + // Remove nv-2 Vertices, creating 1 triangle every time + int count = 2 * nv; /* error detection */ + + List result = new List(); + + for (int v = nv - 1; nv > 2; ) + { + // If we loop, it is probably a non-simple polygon + if (0 >= (count--)) + { + // Triangulate: ERROR - probable bad polygon! + return new List(); + } + + // Three consecutive vertices in current polygon, + int u = v; + if (nv <= u) + u = 0; // Previous + v = u + 1; + if (nv <= v) + v = 0; // New v + int w = v + 1; + if (nv <= w) + w = 0; // Next + + _tmpA = vertices[polygon[u]]; + _tmpB = vertices[polygon[v]]; + _tmpC = vertices[polygon[w]]; + + if (Snip(vertices, u, v, w, nv, polygon)) + { + int s, t; + + // Output Triangle + Vertices triangle = new Vertices(3); + triangle.Add(_tmpA); + triangle.Add(_tmpB); + triangle.Add(_tmpC); + result.Add(triangle); + + // Remove v from remaining polygon + for (s = v, t = v + 1; t < nv; s++, t++) + { + polygon[s] = polygon[t]; + } + nv--; + + // Reset error detection counter + count = 2 * nv; + } + } + + return result; + } + + /// + /// Check if the point P is inside the triangle defined by + /// the points A, B, C + /// + /// The A point. + /// The B point. + /// The C point. + /// The point to be tested. + /// True if the point is inside the triangle + private static bool InsideTriangle(ref Vector2 a, ref Vector2 b, ref Vector2 c, ref Vector2 p) + { + //A cross bp + float abp = (c.X - b.X) * (p.Y - b.Y) - (c.Y - b.Y) * (p.X - b.X); + + //A cross ap + float aap = (b.X - a.X) * (p.Y - a.Y) - (b.Y - a.Y) * (p.X - a.X); + + //b cross cp + float bcp = (a.X - c.X) * (p.Y - c.Y) - (a.Y - c.Y) * (p.X - c.X); + + return ((abp >= 0.0f) && (bcp >= 0.0f) && (aap >= 0.0f)); + } + + /// + /// Cut a the contour and add a triangle into V to describe the + /// location of the cut + /// + /// The list of points defining the polygon + /// The index of the first point + /// The index of the second point + /// The index of the third point + /// The number of elements in the array. + /// The array to populate with indicies of triangles. + /// True if a triangle was found + private static bool Snip(Vertices contour, int u, int v, int w, int n, int[] V) + { + if (Settings.Epsilon > MathUtils.Area(ref _tmpA, ref _tmpB, ref _tmpC)) + return false; + + for (int p = 0; p < n; p++) + { + if ((p == u) || (p == v) || (p == w)) + continue; + + Vector2 point = contour[V[p]]; + + if (InsideTriangle(ref _tmpA, ref _tmpB, ref _tmpC, ref point)) + return false; + } + + return true; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Edge.cs b/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Edge.cs new file mode 100644 index 000000000..116158db7 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Edge.cs @@ -0,0 +1,60 @@ +using System.Collections.Generic; + +namespace FarseerPhysics.Common.Decomposition.Seidel +{ + internal class Edge + { + // Pointers used for building trapezoidal map + public Trapezoid Above; + public float B; + public Trapezoid Below; + + // Montone mountain points + public HashSet MPoints; + public Point P; + public Point Q; + + // Slope of the line (m) + public float Slope; + + + public Edge(Point p, Point q) + { + P = p; + Q = q; + + if (q.X - p.X != 0) + Slope = (q.Y - p.Y) / (q.X - p.X); + else + Slope = 0; + + B = p.Y - (p.X * Slope); + Above = null; + Below = null; + MPoints = new HashSet(); + MPoints.Add(p); + MPoints.Add(q); + } + + public bool IsAbove(Point point) + { + return P.Orient2D(Q, point) < 0; + } + + public bool IsBelow(Point point) + { + return P.Orient2D(Q, point) > 0; + } + + public void AddMpoint(Point point) + { + foreach (Point mp in MPoints) + { + if (!mp.Neq(point)) + return; + } + + MPoints.Add(point); + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/MonotoneMountain.cs b/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/MonotoneMountain.cs new file mode 100644 index 000000000..862f528a9 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/MonotoneMountain.cs @@ -0,0 +1,166 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; + +namespace FarseerPhysics.Common.Decomposition.Seidel +{ + internal class MonotoneMountain + { + // Almost Pi! + private const float PiSlop = 3.1f; + + // Triangles that constitute the mountain + public List> Triangles; + private HashSet _convexPoints; + private Point _head; + + // Monotone mountain points + private List _monoPoly; + + // Used to track which side of the line we are on + private bool _positive; + private int _size; + private Point _tail; + + public MonotoneMountain() + { + _size = 0; + _tail = null; + _head = null; + _positive = false; + _convexPoints = new HashSet(); + _monoPoly = new List(); + Triangles = new List>(); + } + + // Append a point to the list + public void Add(Point point) + { + if (_size == 0) + { + _head = point; + _size = 1; + } + else if (_size == 1) + { + // Keep repeat points out of the list + _tail = point; + _tail.Prev = _head; + _head.Next = _tail; + _size = 2; + } + else + { + // Keep repeat points out of the list + _tail.Next = point; + point.Prev = _tail; + _tail = point; + _size += 1; + } + } + + // Remove a point from the list + public void Remove(Point point) + { + Point next = point.Next; + Point prev = point.Prev; + point.Prev.Next = next; + point.Next.Prev = prev; + _size -= 1; + } + + // Partition a x-monotone mountain into triangles O(n) + // See "Computational Geometry in C", 2nd edition, by Joseph O'Rourke, page 52 + public void Process() + { + // Establish the proper sign + _positive = AngleSign(); + // create monotone polygon - for dubug purposes + GenMonoPoly(); + + // Initialize internal angles at each nonbase vertex + // Link strictly convex vertices into a list, ignore reflex vertices + Point p = _head.Next; + while (p.Neq(_tail)) + { + float a = Angle(p); + // If the point is almost colinear with it's neighbor, remove it! + if (a >= PiSlop || a <= -PiSlop || a == 0.0f) + Remove(p); + else if (IsConvex(p)) + _convexPoints.Add(p); + p = p.Next; + } + + Triangulate(); + } + + private void Triangulate() + { + while (_convexPoints.Count != 0) + { + IEnumerator e = _convexPoints.GetEnumerator(); + e.MoveNext(); + Point ear = e.Current; + + _convexPoints.Remove(ear); + Point a = ear.Prev; + Point b = ear; + Point c = ear.Next; + List triangle = new List(3); + triangle.Add(a); + triangle.Add(b); + triangle.Add(c); + + Triangles.Add(triangle); + + // Remove ear, update angles and convex list + Remove(ear); + if (Valid(a)) + _convexPoints.Add(a); + if (Valid(c)) + _convexPoints.Add(c); + } + + Debug.Assert(_size <= 3, "Triangulation bug, please report"); + } + + private bool Valid(Point p) + { + return p.Neq(_head) && p.Neq(_tail) && IsConvex(p); + } + + // Create the monotone polygon + private void GenMonoPoly() + { + Point p = _head; + while (p != null) + { + _monoPoly.Add(p); + p = p.Next; + } + } + + private float Angle(Point p) + { + Point a = (p.Next - p); + Point b = (p.Prev - p); + return (float)Math.Atan2(a.Cross(b), a.Dot(b)); + } + + private bool AngleSign() + { + Point a = (_head.Next - _head); + Point b = (_tail - _head); + return Math.Atan2(a.Cross(b), a.Dot(b)) >= 0; + } + + // Determines if the inslide angle is convex or reflex + private bool IsConvex(Point p) + { + if (_positive != (Angle(p) >= 0)) + return false; + return true; + } + } +} diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Node.cs b/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Node.cs new file mode 100644 index 000000000..0ef99d007 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Node.cs @@ -0,0 +1,41 @@ +using System.Collections.Generic; + +namespace FarseerPhysics.Common.Decomposition.Seidel +{ + // Node for a Directed Acyclic graph (DAG) + internal abstract class Node + { + protected Node LeftChild; + public List ParentList; + protected Node RightChild; + + protected Node(Node left, Node right) + { + ParentList = new List(); + LeftChild = left; + RightChild = right; + + if (left != null) + left.ParentList.Add(this); + if (right != null) + right.ParentList.Add(this); + } + + public abstract Sink Locate(Edge s); + + // Replace a node in the graph with this node + // Make sure parent pointers are updated + public void Replace(Node node) + { + foreach (Node parent in node.ParentList) + { + // Select the correct node to replace (left or right child) + if (parent.LeftChild == node) + parent.LeftChild = this; + else + parent.RightChild = this; + } + ParentList.AddRange(node.ParentList); + } + } +} diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Point.cs b/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Point.cs new file mode 100644 index 000000000..21167d4f9 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Point.cs @@ -0,0 +1,61 @@ +namespace FarseerPhysics.Common.Decomposition.Seidel +{ + internal class Point + { + // Pointers to next and previous points in Monontone Mountain + public Point Next, Prev; + public float X, Y; + + public Point(float x, float y) + { + X = x; + Y = y; + Next = null; + Prev = null; + } + + public static Point operator -(Point p1, Point p2) + { + return new Point(p1.X - p2.X, p1.Y - p2.Y); + } + + public static Point operator +(Point p1, Point p2) + { + return new Point(p1.X + p2.X, p1.Y + p2.Y); + } + + public static Point operator -(Point p1, float f) + { + return new Point(p1.X - f, p1.Y - f); + } + + public static Point operator +(Point p1, float f) + { + return new Point(p1.X + f, p1.Y + f); + } + + public float Cross(Point p) + { + return X * p.Y - Y * p.X; + } + + public float Dot(Point p) + { + return X * p.X + Y * p.Y; + } + + public bool Neq(Point p) + { + return p.X != X || p.Y != Y; + } + + public float Orient2D(Point pb, Point pc) + { + float acx = X - pc.X; + float bcx = pb.X - pc.X; + float acy = Y - pc.Y; + float bcy = pb.Y - pc.Y; + return acx * bcy - acy * bcx; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/QueryGraph.cs b/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/QueryGraph.cs new file mode 100644 index 000000000..7fb50017c --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/QueryGraph.cs @@ -0,0 +1,78 @@ +using System.Collections.Generic; + +namespace FarseerPhysics.Common.Decomposition.Seidel +{ + // Directed Acyclic graph (DAG) + // See "Computational Geometry", 3rd edition, by Mark de Berg et al, Chapter 6.2 + internal class QueryGraph + { + private Node _head; + + public QueryGraph(Node head) + { + _head = head; + } + + private Trapezoid Locate(Edge edge) + { + return _head.Locate(edge).Trapezoid; + } + + public List FollowEdge(Edge edge) + { + List trapezoids = new List(); + trapezoids.Add(Locate(edge)); + int j = 0; + + while (edge.Q.X > trapezoids[j].RightPoint.X) + { + if (edge.IsAbove(trapezoids[j].RightPoint)) + { + trapezoids.Add(trapezoids[j].UpperRight); + } + else + { + trapezoids.Add(trapezoids[j].LowerRight); + } + j += 1; + } + return trapezoids; + } + + private void Replace(Sink sink, Node node) + { + if (sink.ParentList.Count == 0) + _head = node; + else + node.Replace(sink); + } + + public void Case1(Sink sink, Edge edge, Trapezoid[] tList) + { + YNode yNode = new YNode(edge, Sink.Isink(tList[1]), Sink.Isink(tList[2])); + XNode qNode = new XNode(edge.Q, yNode, Sink.Isink(tList[3])); + XNode pNode = new XNode(edge.P, Sink.Isink(tList[0]), qNode); + Replace(sink, pNode); + } + + public void Case2(Sink sink, Edge edge, Trapezoid[] tList) + { + YNode yNode = new YNode(edge, Sink.Isink(tList[1]), Sink.Isink(tList[2])); + XNode pNode = new XNode(edge.P, Sink.Isink(tList[0]), yNode); + Replace(sink, pNode); + } + + public void Case3(Sink sink, Edge edge, Trapezoid[] tList) + { + YNode yNode = new YNode(edge, Sink.Isink(tList[0]), Sink.Isink(tList[1])); + Replace(sink, yNode); + } + + public void Case4(Sink sink, Edge edge, Trapezoid[] tList) + { + YNode yNode = new YNode(edge, Sink.Isink(tList[0]), Sink.Isink(tList[1])); + XNode qNode = new XNode(edge.Q, yNode, Sink.Isink(tList[2])); + Replace(sink, qNode); + } + } +} diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Sink.cs b/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Sink.cs new file mode 100644 index 000000000..2672f4b89 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Sink.cs @@ -0,0 +1,27 @@ +namespace FarseerPhysics.Common.Decomposition.Seidel +{ + internal class Sink : Node + { + public Trapezoid Trapezoid; + + private Sink(Trapezoid trapezoid) + : base(null, null) + { + Trapezoid = trapezoid; + trapezoid.Sink = this; + } + + public static Sink Isink(Trapezoid trapezoid) + { + if (trapezoid.Sink == null) + return new Sink(trapezoid); + + return trapezoid.Sink; + } + + public override Sink Locate(Edge edge) + { + return this; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Trapezoid.cs b/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Trapezoid.cs new file mode 100644 index 000000000..92fa994c2 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Trapezoid.cs @@ -0,0 +1,123 @@ +using System.Collections.Generic; + +namespace FarseerPhysics.Common.Decomposition.Seidel +{ + internal class Trapezoid + { + public Edge Bottom; + public bool Inside; + public Point LeftPoint; + + // Neighbor pointers + public Trapezoid LowerLeft; + public Trapezoid LowerRight; + + public Point RightPoint; + public Sink Sink; + + public Edge Top; + public Trapezoid UpperLeft; + public Trapezoid UpperRight; + + public Trapezoid(Point leftPoint, Point rightPoint, Edge top, Edge bottom) + { + LeftPoint = leftPoint; + RightPoint = rightPoint; + Top = top; + Bottom = bottom; + UpperLeft = null; + UpperRight = null; + LowerLeft = null; + LowerRight = null; + Inside = true; + Sink = null; + } + + // Update neighbors to the left + public void UpdateLeft(Trapezoid ul, Trapezoid ll) + { + UpperLeft = ul; + if (ul != null) ul.UpperRight = this; + LowerLeft = ll; + if (ll != null) ll.LowerRight = this; + } + + // Update neighbors to the right + public void UpdateRight(Trapezoid ur, Trapezoid lr) + { + UpperRight = ur; + if (ur != null) ur.UpperLeft = this; + LowerRight = lr; + if (lr != null) lr.LowerLeft = this; + } + + // Update neighbors on both sides + public void UpdateLeftRight(Trapezoid ul, Trapezoid ll, Trapezoid ur, Trapezoid lr) + { + UpperLeft = ul; + if (ul != null) ul.UpperRight = this; + LowerLeft = ll; + if (ll != null) ll.LowerRight = this; + UpperRight = ur; + if (ur != null) ur.UpperLeft = this; + LowerRight = lr; + if (lr != null) lr.LowerLeft = this; + } + + // Recursively trim outside neighbors + public void TrimNeighbors() + { + if (Inside) + { + Inside = false; + if (UpperLeft != null) UpperLeft.TrimNeighbors(); + if (LowerLeft != null) LowerLeft.TrimNeighbors(); + if (UpperRight != null) UpperRight.TrimNeighbors(); + if (LowerRight != null) LowerRight.TrimNeighbors(); + } + } + + // Determines if this point lies inside the trapezoid + public bool Contains(Point point) + { + return (point.X > LeftPoint.X && point.X < RightPoint.X && Top.IsAbove(point) && Bottom.IsBelow(point)); + } + + public List GetVertices() + { + List verts = new List(4); + verts.Add(LineIntersect(Top, LeftPoint.X)); + verts.Add(LineIntersect(Bottom, LeftPoint.X)); + verts.Add(LineIntersect(Bottom, RightPoint.X)); + verts.Add(LineIntersect(Top, RightPoint.X)); + return verts; + } + + private Point LineIntersect(Edge edge, float x) + { + float y = edge.Slope * x + edge.B; + return new Point(x, y); + } + + // Add points to monotone mountain + public void AddPoints() + { + if (LeftPoint != Bottom.P) + { + Bottom.AddMpoint(LeftPoint); + } + if (RightPoint != Bottom.Q) + { + Bottom.AddMpoint(RightPoint); + } + if (LeftPoint != Top.P) + { + Top.AddMpoint(LeftPoint); + } + if (RightPoint != Top.Q) + { + Top.AddMpoint(RightPoint); + } + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/TrapezoidalMap.cs b/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/TrapezoidalMap.cs new file mode 100644 index 000000000..7a899769d --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/TrapezoidalMap.cs @@ -0,0 +1,195 @@ +using System.Collections.Generic; + +namespace FarseerPhysics.Common.Decomposition.Seidel +{ + internal class TrapezoidalMap + { + // Trapezoid container + public HashSet Map; + + // Bottom segment that spans multiple trapezoids + private Edge _bCross; + + // Top segment that spans multiple trapezoids + private Edge _cross; + + // AABB margin + private float _margin; + + public TrapezoidalMap() + { + Map = new HashSet(); + _margin = 50.0f; + _bCross = null; + _cross = null; + } + + public void Clear() + { + _bCross = null; + _cross = null; + } + + // Case 1: segment completely enclosed by trapezoid + // break trapezoid into 4 smaller trapezoids + public Trapezoid[] Case1(Trapezoid t, Edge e) + { + Trapezoid[] trapezoids = new Trapezoid[4]; + trapezoids[0] = new Trapezoid(t.LeftPoint, e.P, t.Top, t.Bottom); + trapezoids[1] = new Trapezoid(e.P, e.Q, t.Top, e); + trapezoids[2] = new Trapezoid(e.P, e.Q, e, t.Bottom); + trapezoids[3] = new Trapezoid(e.Q, t.RightPoint, t.Top, t.Bottom); + + trapezoids[0].UpdateLeft(t.UpperLeft, t.LowerLeft); + trapezoids[1].UpdateLeftRight(trapezoids[0], null, trapezoids[3], null); + trapezoids[2].UpdateLeftRight(null, trapezoids[0], null, trapezoids[3]); + trapezoids[3].UpdateRight(t.UpperRight, t.LowerRight); + + return trapezoids; + } + + // Case 2: Trapezoid contains point p, q lies outside + // break trapezoid into 3 smaller trapezoids + public Trapezoid[] Case2(Trapezoid t, Edge e) + { + Point rp; + if (e.Q.X == t.RightPoint.X) + rp = e.Q; + else + rp = t.RightPoint; + + Trapezoid[] trapezoids = new Trapezoid[3]; + trapezoids[0] = new Trapezoid(t.LeftPoint, e.P, t.Top, t.Bottom); + trapezoids[1] = new Trapezoid(e.P, rp, t.Top, e); + trapezoids[2] = new Trapezoid(e.P, rp, e, t.Bottom); + + trapezoids[0].UpdateLeft(t.UpperLeft, t.LowerLeft); + trapezoids[1].UpdateLeftRight(trapezoids[0], null, t.UpperRight, null); + trapezoids[2].UpdateLeftRight(null, trapezoids[0], null, t.LowerRight); + + _bCross = t.Bottom; + _cross = t.Top; + + e.Above = trapezoids[1]; + e.Below = trapezoids[2]; + + return trapezoids; + } + + // Case 3: Trapezoid is bisected + public Trapezoid[] Case3(Trapezoid t, Edge e) + { + Point lp; + if (e.P.X == t.LeftPoint.X) + lp = e.P; + else + lp = t.LeftPoint; + + Point rp; + if (e.Q.X == t.RightPoint.X) + rp = e.Q; + else + rp = t.RightPoint; + + Trapezoid[] trapezoids = new Trapezoid[2]; + + if (_cross == t.Top) + { + trapezoids[0] = t.UpperLeft; + trapezoids[0].UpdateRight(t.UpperRight, null); + trapezoids[0].RightPoint = rp; + } + else + { + trapezoids[0] = new Trapezoid(lp, rp, t.Top, e); + trapezoids[0].UpdateLeftRight(t.UpperLeft, e.Above, t.UpperRight, null); + } + + if (_bCross == t.Bottom) + { + trapezoids[1] = t.LowerLeft; + trapezoids[1].UpdateRight(null, t.LowerRight); + trapezoids[1].RightPoint = rp; + } + else + { + trapezoids[1] = new Trapezoid(lp, rp, e, t.Bottom); + trapezoids[1].UpdateLeftRight(e.Below, t.LowerLeft, null, t.LowerRight); + } + + _bCross = t.Bottom; + _cross = t.Top; + + e.Above = trapezoids[0]; + e.Below = trapezoids[1]; + + return trapezoids; + } + + // Case 4: Trapezoid contains point q, p lies outside + // break trapezoid into 3 smaller trapezoids + public Trapezoid[] Case4(Trapezoid t, Edge e) + { + Point lp; + if (e.P.X == t.LeftPoint.X) + lp = e.P; + else + lp = t.LeftPoint; + + Trapezoid[] trapezoids = new Trapezoid[3]; + + if (_cross == t.Top) + { + trapezoids[0] = t.UpperLeft; + trapezoids[0].RightPoint = e.Q; + } + else + { + trapezoids[0] = new Trapezoid(lp, e.Q, t.Top, e); + trapezoids[0].UpdateLeft(t.UpperLeft, e.Above); + } + + if (_bCross == t.Bottom) + { + trapezoids[1] = t.LowerLeft; + trapezoids[1].RightPoint = e.Q; + } + else + { + trapezoids[1] = new Trapezoid(lp, e.Q, e, t.Bottom); + trapezoids[1].UpdateLeft(e.Below, t.LowerLeft); + } + + trapezoids[2] = new Trapezoid(e.Q, t.RightPoint, t.Top, t.Bottom); + trapezoids[2].UpdateLeftRight(trapezoids[0], trapezoids[1], t.UpperRight, t.LowerRight); + + return trapezoids; + } + + // Create an AABB around segments + public Trapezoid BoundingBox(List edges) + { + Point max = edges[0].P + _margin; + Point min = edges[0].Q - _margin; + + foreach (Edge e in edges) + { + if (e.P.X > max.X) max = new Point(e.P.X + _margin, max.Y); + if (e.P.Y > max.Y) max = new Point(max.X, e.P.Y + _margin); + if (e.Q.X > max.X) max = new Point(e.Q.X + _margin, max.Y); + if (e.Q.Y > max.Y) max = new Point(max.X, e.Q.Y + _margin); + if (e.P.X < min.X) min = new Point(e.P.X - _margin, min.Y); + if (e.P.Y < min.Y) min = new Point(min.X, e.P.Y - _margin); + if (e.Q.X < min.X) min = new Point(e.Q.X - _margin, min.Y); + if (e.Q.Y < min.Y) min = new Point(min.X, e.Q.Y - _margin); + } + + Edge top = new Edge(new Point(min.X, max.Y), new Point(max.X, max.Y)); + Edge bottom = new Edge(new Point(min.X, min.Y), new Point(max.X, min.Y)); + Point left = bottom.P; + Point right = top.Q; + + return new Trapezoid(left, right, top, bottom); + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Triangulator.cs b/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Triangulator.cs new file mode 100644 index 000000000..cf4481251 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/Triangulator.cs @@ -0,0 +1,203 @@ +using System; +using System.Collections.Generic; + +namespace FarseerPhysics.Common.Decomposition.Seidel +{ + internal class Triangulator + { + // Trapezoid decomposition list + public List Trapezoids; + public List> Triangles; + + // Initialize trapezoidal map and query structure + private Trapezoid _boundingBox; + private List _edgeList; + private QueryGraph _queryGraph; + private float _sheer = 0.001f; + private TrapezoidalMap _trapezoidalMap; + private List _xMonoPoly; + + public Triangulator(List polyLine, float sheer) + { + _sheer = sheer; + Triangles = new List>(); + Trapezoids = new List(); + _xMonoPoly = new List(); + _edgeList = InitEdges(polyLine); + _trapezoidalMap = new TrapezoidalMap(); + _boundingBox = _trapezoidalMap.BoundingBox(_edgeList); + _queryGraph = new QueryGraph(Sink.Isink(_boundingBox)); + + Process(); + } + + // Build the trapezoidal map and query graph + private void Process() + { + foreach (Edge edge in _edgeList) + { + List traps = _queryGraph.FollowEdge(edge); + + // Remove trapezoids from trapezoidal Map + foreach (Trapezoid t in traps) + { + _trapezoidalMap.Map.Remove(t); + + bool cp = t.Contains(edge.P); + bool cq = t.Contains(edge.Q); + Trapezoid[] tList; + + if (cp && cq) + { + tList = _trapezoidalMap.Case1(t, edge); + _queryGraph.Case1(t.Sink, edge, tList); + } + else if (cp && !cq) + { + tList = _trapezoidalMap.Case2(t, edge); + _queryGraph.Case2(t.Sink, edge, tList); + } + else if (!cp && !cq) + { + tList = _trapezoidalMap.Case3(t, edge); + _queryGraph.Case3(t.Sink, edge, tList); + } + else + { + tList = _trapezoidalMap.Case4(t, edge); + _queryGraph.Case4(t.Sink, edge, tList); + } + // Add new trapezoids to map + foreach (Trapezoid y in tList) + { + _trapezoidalMap.Map.Add(y); + } + } + _trapezoidalMap.Clear(); + } + + // Mark outside trapezoids + foreach (Trapezoid t in _trapezoidalMap.Map) + { + MarkOutside(t); + } + + // Collect interior trapezoids + foreach (Trapezoid t in _trapezoidalMap.Map) + { + if (t.Inside) + { + Trapezoids.Add(t); + t.AddPoints(); + } + } + + // Generate the triangles + CreateMountains(); + } + + // Build a list of x-monotone mountains + private void CreateMountains() + { + foreach (Edge edge in _edgeList) + { + if (edge.MPoints.Count > 2) + { + MonotoneMountain mountain = new MonotoneMountain(); + + // Sorting is a perfromance hit. Literature says this can be accomplised in + // linear time, although I don't see a way around using traditional methods + // when using a randomized incremental algorithm + + // Insertion sort is one of the fastest algorithms for sorting arrays containing + // fewer than ten elements, or for lists that are already mostly sorted. + + List points = new List(edge.MPoints); + points.Sort((p1, p2) => p1.X.CompareTo(p2.X)); + + foreach (Point p in points) + mountain.Add(p); + + // Triangulate monotone mountain + mountain.Process(); + + // Extract the triangles into a single list + foreach (List t in mountain.Triangles) + { + Triangles.Add(t); + } + + _xMonoPoly.Add(mountain); + } + } + } + + // Mark the outside trapezoids surrounding the polygon + private void MarkOutside(Trapezoid t) + { + if (t.Top == _boundingBox.Top || t.Bottom == _boundingBox.Bottom) + t.TrimNeighbors(); + } + + // Create segments and connect end points; update edge event pointer + private List InitEdges(List points) + { + List edges = new List(); + + for (int i = 0; i < points.Count - 1; i++) + { + edges.Add(new Edge(points[i], points[i + 1])); + } + edges.Add(new Edge(points[0], points[points.Count - 1])); + return OrderSegments(edges); + } + + private List OrderSegments(List edgeInput) + { + // Ignore vertical segments! + List edges = new List(); + + foreach (Edge e in edgeInput) + { + Point p = ShearTransform(e.P); + Point q = ShearTransform(e.Q); + + // Point p must be to the left of point q + if (p.X > q.X) + { + edges.Add(new Edge(q, p)); + } + else if (p.X < q.X) + { + edges.Add(new Edge(p, q)); + } + } + + // Randomized triangulation improves performance + // See Seidel's paper, or O'Rourke's book, p. 57 + Shuffle(edges); + return edges; + } + + private static void Shuffle(IList list) + { + Random rng = new Random(); + int n = list.Count; + while (n > 1) + { + n--; + int k = rng.Next(n + 1); + T value = list[k]; + list[k] = list[n]; + list[n] = value; + } + } + + // Prevents any two distinct endpoints from lying on a common vertical line, and avoiding + // the degenerate case. See Mark de Berg et al, Chapter 6.3 + private Point ShearTransform(Point point) + { + return new Point(point.X + _sheer * point.Y, point.Y); + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/XNode.cs b/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/XNode.cs new file mode 100644 index 000000000..b623415d0 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/XNode.cs @@ -0,0 +1,21 @@ +namespace FarseerPhysics.Common.Decomposition.Seidel +{ + internal class XNode : Node + { + private Point _point; + + public XNode(Point point, Node lChild, Node rChild) + : base(lChild, rChild) + { + _point = point; + } + + public override Sink Locate(Edge edge) + { + if (edge.P.X >= _point.X) + return RightChild.Locate(edge); // Move to the right in the graph + + return LeftChild.Locate(edge); // Move to the left in the graph + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/YNode.cs b/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/YNode.cs new file mode 100644 index 000000000..b534ab2d3 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/Seidel/YNode.cs @@ -0,0 +1,29 @@ +namespace FarseerPhysics.Common.Decomposition.Seidel +{ + internal class YNode : Node + { + private Edge _edge; + + public YNode(Edge edge, Node lChild, Node rChild) + : base(lChild, rChild) + { + _edge = edge; + } + + public override Sink Locate(Edge edge) + { + if (_edge.IsAbove(edge.P)) + return RightChild.Locate(edge); // Move down the graph + + if (_edge.IsBelow(edge.P)) + return LeftChild.Locate(edge); // Move up the graph + + // s and segment share the same endpoint, p + if (edge.Slope < _edge.Slope) + return RightChild.Locate(edge); // Move down the graph + + // Move up the graph + return LeftChild.Locate(edge); + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/SeidelDecomposer.cs b/Farseer Physics Engine 3.5/Common/Decomposition/SeidelDecomposer.cs new file mode 100644 index 000000000..d4c0b9d37 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/SeidelDecomposer.cs @@ -0,0 +1,109 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +*/ + +using System.Collections.Generic; +using System.Diagnostics; +using FarseerPhysics.Common.Decomposition.Seidel; +using Microsoft.Xna.Framework; +using Point = FarseerPhysics.Common.Decomposition.Seidel.Point; + +namespace FarseerPhysics.Common.Decomposition +{ + /// + /// Convex decomposition algorithm created by Raimund Seidel + /// + /// Properties: + /// - Decompose the polygon into trapezoids, then triangulate. + /// - To use the trapezoid data, use ConvexPartitionTrapezoid() + /// - Generate a lot of garbage due to incapsulation of the Poly2Tri library. + /// - Running time is O(n log n), n = number of vertices. + /// - Running time is almost linear for most simple polygons. + /// - Does not care about winding order. + /// + /// For more information, see Raimund Seidel's paper "A simple and fast incremental randomized + /// algorithm for computing trapezoidal decompositions and for triangulating polygons" + /// + /// See also: "Computational Geometry", 3rd edition, by Mark de Berg et al, Chapter 6.2 + /// "Computational Geometry in C", 2nd edition, by Joseph O'Rourke + /// + /// Original code from the Poly2Tri project by Mason Green. + /// http://code.google.com/p/poly2tri/source/browse?repo=archive#hg/scala/src/org/poly2tri/seidel + /// + /// This implementation is from Dec 14, 2010 + /// + internal static class SeidelDecomposer + { + /// + /// Decompose the polygon into several smaller non-concave polygons. + /// + /// The polygon to decompose. + /// The sheer to use if you get bad results, try using a higher value. + /// A list of triangles + public static List ConvexPartition(Vertices vertices, float sheer = 0.001f) + { + Debug.Assert(vertices.Count > 3); + + List compatList = new List(vertices.Count); + + foreach (Vector2 vertex in vertices) + { + compatList.Add(new Point(vertex.X, vertex.Y)); + } + + Triangulator t = new Triangulator(compatList, sheer); + + List list = new List(); + + foreach (List triangle in t.Triangles) + { + Vertices outTriangles = new Vertices(triangle.Count); + + foreach (Point outTriangle in triangle) + { + outTriangles.Add(new Vector2(outTriangle.X, outTriangle.Y)); + } + + list.Add(outTriangles); + } + + return list; + } + + /// + /// Decompose the polygon into several smaller non-concave polygons. + /// + /// The polygon to decompose. + /// The sheer to use if you get bad results, try using a higher value. + /// A list of trapezoids + public static List ConvexPartitionTrapezoid(Vertices vertices, float sheer = 0.001f) + { + List compatList = new List(vertices.Count); + + foreach (Vector2 vertex in vertices) + { + compatList.Add(new Point(vertex.X, vertex.Y)); + } + + Triangulator t = new Triangulator(compatList, sheer); + + List list = new List(); + + foreach (Trapezoid trapezoid in t.Trapezoids) + { + Vertices verts = new Vertices(); + + List points = trapezoid.GetVertices(); + foreach (Point point in points) + { + verts.Add(new Vector2(point.X, point.Y)); + } + + list.Add(verts); + } + + return list; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Decomposition/Triangulate.cs b/Farseer Physics Engine 3.5/Common/Decomposition/Triangulate.cs new file mode 100644 index 000000000..884180fb8 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Decomposition/Triangulate.cs @@ -0,0 +1,172 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using FarseerPhysics.Common.ConvexHull; + +namespace FarseerPhysics.Common.Decomposition +{ + public enum TriangulationAlgorithm + { + /// + /// Convex decomposition algorithm using ear clipping + /// + /// Properties: + /// - Only works on simple polygons. + /// - Does not support holes. + /// - Running time is O(n^2), n = number of vertices. + /// + Earclip, + + /// + /// Convex decomposition algorithm created by Mark Bayazit (http://mnbayazit.com/) + /// + /// Properties: + /// - Tries to decompose using polygons instead of triangles. + /// - Tends to produce optimal results with low processing time. + /// - Running time is O(nr), n = number of vertices, r = reflex vertices. + /// - Does not support holes. + /// + Bayazit, + + /// + /// Convex decomposition algorithm created by unknown + /// + /// Properties: + /// - No support for holes + /// - Very fast + /// - Only works on simple polygons + /// - Only works on counter clockwise polygons + /// + Flipcode, + + /// + /// Convex decomposition algorithm created by Raimund Seidel + /// + /// Properties: + /// - Decompose the polygon into trapezoids, then triangulate. + /// - To use the trapezoid data, use ConvexPartitionTrapezoid() + /// - Generate a lot of garbage due to incapsulation of the Poly2Tri library. + /// - Running time is O(n log n), n = number of vertices. + /// - Running time is almost linear for most simple polygons. + /// - Does not care about winding order. + /// + Seidel, + SeidelTrapezoids, + + /// + /// 2D constrained Delaunay triangulation algorithm. + /// Based on the paper "Sweep-line algorithm for constrained Delaunay triangulation" by V. Domiter and and B. Zalik + /// + /// Properties: + /// - Creates triangles with a large interior angle. + /// - Supports holes + /// - Generate a lot of garbage due to incapsulation of the Poly2Tri library. + /// - Running time is O(n^2), n = number of vertices. + /// - Does not care about winding order. + /// + Delauny + } + + public static class Triangulate + { + public static List ConvexPartition(Vertices vertices, TriangulationAlgorithm algorithm, bool discardAndFixInvalid = true, float tolerance = 0.001f) + { + if (vertices.Count <= 3) + return new List { vertices }; + + List results; + + switch (algorithm) + { + case TriangulationAlgorithm.Earclip: + if (Settings.SkipSanityChecks) + Debug.Assert(!vertices.IsCounterClockWise(), "The Earclip algorithm expects the polygon to be clockwise."); + else + { + if (vertices.IsCounterClockWise()) + { + Vertices temp = new Vertices(vertices); + temp.Reverse(); + results = EarclipDecomposer.ConvexPartition(temp, tolerance); + } + else + results = EarclipDecomposer.ConvexPartition(vertices, tolerance); + } + break; + case TriangulationAlgorithm.Bayazit: + if (Settings.SkipSanityChecks) + Debug.Assert(vertices.IsCounterClockWise(), "The polygon is not counter clockwise. This is needed for Bayazit to work correctly."); + else + { + if (!vertices.IsCounterClockWise()) + { + Vertices temp = new Vertices(vertices); + temp.Reverse(); + results = BayazitDecomposer.ConvexPartition(temp); + } + else + results = BayazitDecomposer.ConvexPartition(vertices); + } + break; + case TriangulationAlgorithm.Flipcode: + if (Settings.SkipSanityChecks) + Debug.Assert(vertices.IsCounterClockWise(), "The polygon is not counter clockwise. This is needed for Bayazit to work correctly."); + else + { + if (!vertices.IsCounterClockWise()) + { + Vertices temp = new Vertices(vertices); + temp.Reverse(); + results = FlipcodeDecomposer.ConvexPartition(temp); + } + else + results = FlipcodeDecomposer.ConvexPartition(vertices); + } + break; + case TriangulationAlgorithm.Seidel: + results = SeidelDecomposer.ConvexPartition(vertices, tolerance); + break; + case TriangulationAlgorithm.SeidelTrapezoids: + results = SeidelDecomposer.ConvexPartitionTrapezoid(vertices, tolerance); + break; + case TriangulationAlgorithm.Delauny: + results = CDTDecomposer.ConvexPartition(vertices); + break; + default: + throw new ArgumentOutOfRangeException("algorithm"); + } + + if (discardAndFixInvalid) + { + for (int i = results.Count - 1; i >= 0; i--) + { + Vertices polygon = results[i]; + + if (!ValidatePolygon(polygon)) + results.RemoveAt(i); + } + } + + return results; + } + + private static bool ValidatePolygon(Vertices polygon) + { + PolygonError errorCode = polygon.CheckPolygon(); + + if (errorCode == PolygonError.InvalidAmountOfVertices || errorCode == PolygonError.AreaTooSmall || errorCode == PolygonError.SideTooSmall || errorCode == PolygonError.NotSimple) + return false; + + if (errorCode == PolygonError.NotCounterClockWise) //NotCounterCloseWise is the last check in CheckPolygon(), thus we don't need to call ValidatePolygon again. + polygon.Reverse(); + + if (errorCode == PolygonError.NotConvex) + { + polygon = GiftWrap.GetConvexHull(polygon); + return ValidatePolygon(polygon); + } + + return true; + } + } +} diff --git a/Farseer Physics Engine 3.5/Common/FixedArray.cs b/Farseer Physics Engine 3.5/Common/FixedArray.cs new file mode 100644 index 000000000..c62847025 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/FixedArray.cs @@ -0,0 +1,224 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; + +namespace FarseerPhysics.Common +{ + public struct FixedArray2 + { + private T _value0; + private T _value1; + + public T this[int index] + { + get + { + switch (index) + { + case 0: + return _value0; + case 1: + return _value1; + default: + throw new IndexOutOfRangeException(); + } + } + set + { + switch (index) + { + case 0: + _value0 = value; + break; + case 1: + _value1 = value; + break; + default: + throw new IndexOutOfRangeException(); + } + } + } + } + + public struct FixedArray3 + { + private T _value0; + private T _value1; + private T _value2; + + public T this[int index] + { + get + { + switch (index) + { + case 0: + return _value0; + case 1: + return _value1; + case 2: + return _value2; + default: + throw new IndexOutOfRangeException(); + } + } + set + { + switch (index) + { + case 0: + _value0 = value; + break; + case 1: + _value1 = value; + break; + case 2: + _value2 = value; + break; + default: + throw new IndexOutOfRangeException(); + } + } + } + } + + public struct FixedArray4 + { + private T _value0; + private T _value1; + private T _value2; + private T _value3; + + public T this[int index] + { + get + { + switch (index) + { + case 0: + return _value0; + case 1: + return _value1; + case 2: + return _value2; + case 3: + return _value3; + default: + throw new IndexOutOfRangeException(); + } + } + set + { + switch (index) + { + case 0: + _value0 = value; + break; + case 1: + _value1 = value; + break; + case 2: + _value2 = value; + break; + case 3: + _value3 = value; + break; + default: + throw new IndexOutOfRangeException(); + } + } + } + } + + public struct FixedArray8 + { + private T _value0; + private T _value1; + private T _value2; + private T _value3; + private T _value4; + private T _value5; + private T _value6; + private T _value7; + + public T this[int index] + { + get + { + switch (index) + { + case 0: + return _value0; + case 1: + return _value1; + case 2: + return _value2; + case 3: + return _value3; + case 4: + return _value4; + case 5: + return _value5; + case 6: + return _value6; + case 7: + return _value7; + default: + throw new IndexOutOfRangeException(); + } + } + set + { + switch (index) + { + case 0: + _value0 = value; + break; + case 1: + _value1 = value; + break; + case 2: + _value2 = value; + break; + case 3: + _value3 = value; + break; + case 4: + _value4 = value; + break; + case 5: + _value5 = value; + break; + case 6: + _value6 = value; + break; + case 7: + _value7 = value; + break; + default: + throw new IndexOutOfRangeException(); + } + } + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/HashSet.cs b/Farseer Physics Engine 3.5/Common/HashSet.cs new file mode 100644 index 000000000..ce3e26f53 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/HashSet.cs @@ -0,0 +1,78 @@ +#if WINDOWS_PHONE || XBOX + +using System.Collections; +using System.Collections.Generic; + +namespace FarseerPhysics.Common +{ + public class HashSet : ICollection + { + private Dictionary _dict; + + public HashSet(int capacity) + { + _dict = new Dictionary(capacity); + } + + public HashSet() + { + _dict = new Dictionary(); + } + + #region ICollection Members + + public void Add(T item) + { + // We don't care for the value in dictionary, only keys matter. + if (!_dict.ContainsKey(item)) + _dict.Add(item, 0); + } + + public void Clear() + { + _dict.Clear(); + } + + public bool Contains(T item) + { + return _dict.ContainsKey(item); + } + + public void CopyTo(T[] array, int arrayIndex) + { + foreach (var item in _dict.Keys) + { + array[arrayIndex++] = item; + } + } + + public bool Remove(T item) + { + return _dict.Remove(item); + } + + public IEnumerator GetEnumerator() + { + return _dict.Keys.GetEnumerator(); + } + + IEnumerator IEnumerable.GetEnumerator() + { + return _dict.Keys.GetEnumerator(); + } + + // Properties + public int Count + { + get { return _dict.Keys.Count; } + } + + public bool IsReadOnly + { + get { return false; } + } + + #endregion + } +} +#endif \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/LineTools.cs b/Farseer Physics Engine 3.5/Common/LineTools.cs new file mode 100644 index 000000000..8b6b8e4da --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/LineTools.cs @@ -0,0 +1,287 @@ +using System; +using FarseerPhysics.Collision; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common +{ + /// + /// Collection of helper methods for misc collisions. + /// Does float tolerance and line collisions with lines and AABBs. + /// + public static class LineTools + { + public static float DistanceBetweenPointAndLineSegment(ref Vector2 point, ref Vector2 start, ref Vector2 end) + { + if (start == end) + return Vector2.Distance(point, start); + + Vector2 v = Vector2.Subtract(end, start); + Vector2 w = Vector2.Subtract(point, start); + + float c1 = Vector2.Dot(w, v); + if (c1 <= 0) return Vector2.Distance(point, start); + + float c2 = Vector2.Dot(v, v); + if (c2 <= c1) return Vector2.Distance(point, end); + + float b = c1 / c2; + Vector2 pointOnLine = Vector2.Add(start, Vector2.Multiply(v, b)); + return Vector2.Distance(point, pointOnLine); + } + + // From Eric Jordan's convex decomposition library + /// + ///Check if the lines a0->a1 and b0->b1 cross. + ///If they do, intersectionPoint will be filled + ///with the point of crossing. + /// + ///Grazing lines should not return true. + /// + /// + public static bool LineIntersect2(ref Vector2 a0, ref Vector2 a1, ref Vector2 b0, ref Vector2 b1, out Vector2 intersectionPoint) + { + intersectionPoint = Vector2.Zero; + + if (a0 == b0 || a0 == b1 || a1 == b0 || a1 == b1) + return false; + + float x1 = a0.X; + float y1 = a0.Y; + float x2 = a1.X; + float y2 = a1.Y; + float x3 = b0.X; + float y3 = b0.Y; + float x4 = b1.X; + float y4 = b1.Y; + + //AABB early exit + if (Math.Max(x1, x2) < Math.Min(x3, x4) || Math.Max(x3, x4) < Math.Min(x1, x2)) + return false; + + if (Math.Max(y1, y2) < Math.Min(y3, y4) || Math.Max(y3, y4) < Math.Min(y1, y2)) + return false; + + float ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3)); + float ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3)); + float denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1); + if (Math.Abs(denom) < Settings.Epsilon) + { + //Lines are too close to parallel to call + return false; + } + ua /= denom; + ub /= denom; + + if ((0 < ua) && (ua < 1) && (0 < ub) && (ub < 1)) + { + intersectionPoint.X = (x1 + ua * (x2 - x1)); + intersectionPoint.Y = (y1 + ua * (y2 - y1)); + return true; + } + + return false; + } + + //From Mark Bayazit's convex decomposition algorithm + public static Vector2 LineIntersect(Vector2 p1, Vector2 p2, Vector2 q1, Vector2 q2) + { + Vector2 i = Vector2.Zero; + float a1 = p2.Y - p1.Y; + float b1 = p1.X - p2.X; + float c1 = a1 * p1.X + b1 * p1.Y; + float a2 = q2.Y - q1.Y; + float b2 = q1.X - q2.X; + float c2 = a2 * q1.X + b2 * q1.Y; + float det = a1 * b2 - a2 * b1; + + if (!MathUtils.FloatEquals(det, 0)) + { + // lines are not parallel + i.X = (b2 * c1 - b1 * c2) / det; + i.Y = (a1 * c2 - a2 * c1) / det; + } + return i; + } + + /// + /// This method detects if two line segments (or lines) intersect, + /// and, if so, the point of intersection. Use the and + /// parameters to set whether the intersection point + /// must be on the first and second line segments. Setting these + /// both to true means you are doing a line-segment to line-segment + /// intersection. Setting one of them to true means you are doing a + /// line to line-segment intersection test, and so on. + /// Note: If two line segments are coincident, then + /// no intersection is detected (there are actually + /// infinite intersection points). + /// Author: Jeremy Bell + /// + /// The first point of the first line segment. + /// The second point of the first line segment. + /// The first point of the second line segment. + /// The second point of the second line segment. + /// This is set to the intersection + /// point if an intersection is detected. + /// Set this to true to require that the + /// intersection point be on the first line segment. + /// Set this to true to require that the + /// intersection point be on the second line segment. + /// True if an intersection is detected, false otherwise. + public static bool LineIntersect(ref Vector2 point1, ref Vector2 point2, ref Vector2 point3, ref Vector2 point4, bool firstIsSegment, bool secondIsSegment, out Vector2 point) + { + point = new Vector2(); + + // these are reused later. + // each lettered sub-calculation is used twice, except + // for b and d, which are used 3 times + float a = point4.Y - point3.Y; + float b = point2.X - point1.X; + float c = point4.X - point3.X; + float d = point2.Y - point1.Y; + + // denominator to solution of linear system + float denom = (a * b) - (c * d); + + // if denominator is 0, then lines are parallel + if (!(denom >= -Settings.Epsilon && denom <= Settings.Epsilon)) + { + float e = point1.Y - point3.Y; + float f = point1.X - point3.X; + float oneOverDenom = 1.0f / denom; + + // numerator of first equation + float ua = (c * e) - (a * f); + ua *= oneOverDenom; + + // check if intersection point of the two lines is on line segment 1 + if (!firstIsSegment || ua >= 0.0f && ua <= 1.0f) + { + // numerator of second equation + float ub = (b * e) - (d * f); + ub *= oneOverDenom; + + // check if intersection point of the two lines is on line segment 2 + // means the line segments intersect, since we know it is on + // segment 1 as well. + if (!secondIsSegment || ub >= 0.0f && ub <= 1.0f) + { + // check if they are coincident (no collision in this case) + if (ua != 0f || ub != 0f) + { + //There is an intersection + point.X = point1.X + ua * b; + point.Y = point1.Y + ua * d; + return true; + } + } + } + } + + return false; + } + + /// + /// This method detects if two line segments (or lines) intersect, + /// and, if so, the point of intersection. Use the and + /// parameters to set whether the intersection point + /// must be on the first and second line segments. Setting these + /// both to true means you are doing a line-segment to line-segment + /// intersection. Setting one of them to true means you are doing a + /// line to line-segment intersection test, and so on. + /// Note: If two line segments are coincident, then + /// no intersection is detected (there are actually + /// infinite intersection points). + /// Author: Jeremy Bell + /// + /// The first point of the first line segment. + /// The second point of the first line segment. + /// The first point of the second line segment. + /// The second point of the second line segment. + /// This is set to the intersection + /// point if an intersection is detected. + /// Set this to true to require that the + /// intersection point be on the first line segment. + /// Set this to true to require that the + /// intersection point be on the second line segment. + /// True if an intersection is detected, false otherwise. + public static bool LineIntersect(Vector2 point1, Vector2 point2, Vector2 point3, Vector2 point4, bool firstIsSegment, bool secondIsSegment, out Vector2 intersectionPoint) + { + return LineIntersect(ref point1, ref point2, ref point3, ref point4, firstIsSegment, secondIsSegment, out intersectionPoint); + } + + /// + /// This method detects if two line segments intersect, + /// and, if so, the point of intersection. + /// Note: If two line segments are coincident, then + /// no intersection is detected (there are actually + /// infinite intersection points). + /// + /// The first point of the first line segment. + /// The second point of the first line segment. + /// The first point of the second line segment. + /// The second point of the second line segment. + /// This is set to the intersection + /// point if an intersection is detected. + /// True if an intersection is detected, false otherwise. + public static bool LineIntersect(ref Vector2 point1, ref Vector2 point2, ref Vector2 point3, ref Vector2 point4, out Vector2 intersectionPoint) + { + return LineIntersect(ref point1, ref point2, ref point3, ref point4, true, true, out intersectionPoint); + } + + /// + /// This method detects if two line segments intersect, + /// and, if so, the point of intersection. + /// Note: If two line segments are coincident, then + /// no intersection is detected (there are actually + /// infinite intersection points). + /// + /// The first point of the first line segment. + /// The second point of the first line segment. + /// The first point of the second line segment. + /// The second point of the second line segment. + /// This is set to the intersection + /// point if an intersection is detected. + /// True if an intersection is detected, false otherwise. + public static bool LineIntersect(Vector2 point1, Vector2 point2, Vector2 point3, Vector2 point4, out Vector2 intersectionPoint) + { + return LineIntersect(ref point1, ref point2, ref point3, ref point4, true, true, out intersectionPoint); + } + + /// + /// Get all intersections between a line segment and a list of vertices + /// representing a polygon. The vertices reuse adjacent points, so for example + /// edges one and two are between the first and second vertices and between the + /// second and third vertices. The last edge is between vertex vertices.Count - 1 + /// and verts0. (ie, vertices from a Geometry or AABB) + /// + /// The first point of the line segment to test + /// The second point of the line segment to test. + /// The vertices, as described above + public static Vertices LineSegmentVerticesIntersect(ref Vector2 point1, ref Vector2 point2, Vertices vertices) + { + Vertices intersectionPoints = new Vertices(); + + for (int i = 0; i < vertices.Count; i++) + { + Vector2 point; + if (LineIntersect(vertices[i], vertices[vertices.NextIndex(i)], point1, point2, true, true, out point)) + { + intersectionPoints.Add(point); + } + } + + return intersectionPoints; + } + + /// + /// Get all intersections between a line segment and an AABB. + /// + /// The first point of the line segment to test + /// The second point of the line segment to test. + /// The AABB that is used for testing intersection. + public static Vertices LineSegmentAABBIntersect(ref Vector2 point1, ref Vector2 point2, AABB aabb) + { + return LineSegmentVerticesIntersect(ref point1, ref point2, aabb.Vertices); + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Math.cs b/Farseer Physics Engine 3.5/Common/Math.cs new file mode 100644 index 000000000..766212f19 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Math.cs @@ -0,0 +1,806 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using System.Runtime.InteropServices; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common +{ + public static class MathUtils + { + public static float Cross(ref Vector2 a, ref Vector2 b) + { + return a.X * b.Y - a.Y * b.X; + } + + public static float Cross(Vector2 a, Vector2 b) + { + return Cross(ref a, ref b); + } + + /// Perform the cross product on two vectors. + 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 Vector2 Cross(Vector2 a, float s) + { + return new Vector2(s * a.Y, -s * a.X); + } + + public static Vector2 Cross(float s, Vector2 a) + { + return new Vector2(-s * a.Y, s * a.X); + } + + public static Vector2 Abs(Vector2 v) + { + return new Vector2(Math.Abs(v.X), Math.Abs(v.Y)); + } + + public static Vector2 Mul(ref Mat22 A, Vector2 v) + { + return Mul(ref A, ref v); + } + + public static Vector2 Mul(ref Mat22 A, ref Vector2 v) + { + return new Vector2(A.ex.X * v.X + A.ey.X * v.Y, A.ex.Y * v.X + A.ey.Y * v.Y); + } + + public static Vector2 Mul(ref Transform T, Vector2 v) + { + return Mul(ref T, ref v); + } + + public static Vector2 Mul(ref Transform T, ref Vector2 v) + { + float x = (T.q.c * v.X - T.q.s * v.Y) + T.p.X; + float y = (T.q.s * v.X + T.q.c * v.Y) + T.p.Y; + + return new Vector2(x, y); + } + + public static Vector2 MulT(ref Mat22 A, Vector2 v) + { + return MulT(ref A, ref v); + } + + public static Vector2 MulT(ref Mat22 A, ref Vector2 v) + { + return new Vector2(v.X * A.ex.X + v.Y * A.ex.Y, v.X * A.ey.X + v.Y * A.ey.Y); + } + + public static Vector2 MulT(ref Transform T, Vector2 v) + { + return MulT(ref T, ref v); + } + + public static Vector2 MulT(ref Transform T, ref Vector2 v) + { + float px = v.X - T.p.X; + float py = v.Y - T.p.Y; + float x = (T.q.c * px + T.q.s * py); + float y = (-T.q.s * px + T.q.c * py); + + return new Vector2(x, y); + } + + // A^T * B + public static void MulT(ref Mat22 A, ref Mat22 B, out Mat22 C) + { + C = new Mat22(); + C.ex.X = A.ex.X * B.ex.X + A.ex.Y * B.ex.Y; + C.ex.Y = A.ey.X * B.ex.X + A.ey.Y * B.ex.Y; + C.ey.X = A.ex.X * B.ey.X + A.ex.Y * B.ey.Y; + C.ey.Y = A.ey.X * B.ey.X + A.ey.Y * B.ey.Y; + } + + /// Multiply a matrix times a vector. + public static Vector3 Mul(Mat33 A, Vector3 v) + { + return v.X * A.ex + v.Y * A.ey + v.Z * A.ez; + } + + // v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p + // = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p + public static Transform Mul(Transform A, Transform B) + { + Transform C = new Transform(); + C.q = Mul(A.q, B.q); + C.p = Mul(A.q, B.p) + A.p; + return C; + } + + // v2 = A.q' * (B.q * v1 + B.p - A.p) + // = A.q' * B.q * v1 + A.q' * (B.p - A.p) + public static void MulT(ref Transform A, ref Transform B, out Transform C) + { + C = new Transform(); + C.q = MulT(A.q, B.q); + C.p = MulT(A.q, B.p - A.p); + } + + public static void Swap(ref T a, ref T b) + { + T tmp = a; + a = b; + b = tmp; + } + + /// Multiply a matrix times a vector. + public static Vector2 Mul22(Mat33 A, Vector2 v) + { + return new Vector2(A.ex.X * v.X + A.ey.X * v.Y, A.ex.Y * v.X + A.ey.Y * v.Y); + } + + /// Multiply two rotations: q * r + public static Rot Mul(Rot q, Rot r) + { + // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc] + // [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc] + // s = qs * rc + qc * rs + // c = qc * rc - qs * rs + Rot qr; + qr.s = q.s * r.c + q.c * r.s; + qr.c = q.c * r.c - q.s * r.s; + return qr; + } + + public static Vector2 MulT(Transform T, Vector2 v) + { + float px = v.X - T.p.X; + float py = v.Y - T.p.Y; + float x = (T.q.c * px + T.q.s * py); + float y = (-T.q.s * px + T.q.c * py); + + return new Vector2(x, y); + } + + /// Transpose multiply two rotations: qT * r + public static Rot MulT(Rot q, Rot r) + { + // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc] + // [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc] + // s = qc * rs - qs * rc + // c = qc * rc + qs * rs + Rot qr; + qr.s = q.c * r.s - q.s * r.c; + qr.c = q.c * r.c + q.s * r.s; + return qr; + } + + // v2 = A.q' * (B.q * v1 + B.p - A.p) + // = A.q' * B.q * v1 + A.q' * (B.p - A.p) + public static Transform MulT(Transform A, Transform B) + { + Transform C = new Transform(); + C.q = MulT(A.q, B.q); + C.p = MulT(A.q, B.p - A.p); + return C; + } + + /// Rotate a vector + public static Vector2 Mul(Rot q, Vector2 v) + { + return new Vector2(q.c * v.X - q.s * v.Y, q.s * v.X + q.c * v.Y); + } + + /// Inverse rotate a vector + public static Vector2 MulT(Rot q, Vector2 v) + { + return new Vector2(q.c * v.X + q.s * v.Y, -q.s * v.X + q.c * v.Y); + } + + /// Get the skew vector such that dot(skew_vec, other) == cross(vec, other) + public static Vector2 Skew(Vector2 input) + { + return new Vector2(-input.Y, input.X); + } + + /// + /// This function is used to ensure that a floating point number is + /// not a NaN or infinity. + /// + /// The x. + /// + /// true if the specified x is valid; otherwise, false. + /// + public static bool IsValid(float x) + { + if (float.IsNaN(x)) + { + // NaN. + return false; + } + + return !float.IsInfinity(x); + } + + public static bool IsValid(this Vector2 x) + { + return IsValid(x.X) && IsValid(x.Y); + } + + /// + /// This is a approximate yet fast inverse square-root. + /// + /// The x. + /// + public static float InvSqrt(float x) + { + FloatConverter convert = new FloatConverter(); + convert.x = x; + float xhalf = 0.5f * x; + convert.i = 0x5f3759df - (convert.i >> 1); + x = convert.x; + x = x * (1.5f - xhalf * x * x); + return x; + } + + public static int Clamp(int a, int low, int high) + { + return Math.Max(low, Math.Min(a, high)); + } + + public static float Clamp(float a, float low, float high) + { + return Math.Max(low, Math.Min(a, high)); + } + + public static Vector2 Clamp(Vector2 a, Vector2 low, Vector2 high) + { + return Vector2.Max(low, Vector2.Min(a, high)); + } + + public static void Cross(ref Vector2 a, ref Vector2 b, out float c) + { + c = a.X * b.Y - a.Y * b.X; + } + + /// + /// Return the angle between two vectors on a plane + /// The angle is from vector 1 to vector 2, positive anticlockwise + /// The result is between -pi -> pi + /// + public static double VectorAngle(ref Vector2 p1, ref Vector2 p2) + { + double theta1 = Math.Atan2(p1.Y, p1.X); + double theta2 = Math.Atan2(p2.Y, p2.X); + double dtheta = theta2 - theta1; + while (dtheta > Math.PI) + dtheta -= (2 * Math.PI); + while (dtheta < -Math.PI) + dtheta += (2 * Math.PI); + + return (dtheta); + } + + /// Perform the dot product on two vectors. + public static float Dot(Vector3 a, Vector3 b) + { + return a.X * b.X + a.Y * b.Y + a.Z * b.Z; + } + + public static double VectorAngle(Vector2 p1, Vector2 p2) + { + return VectorAngle(ref p1, ref p2); + } + + /// + /// Returns a positive number if c is to the left of the line going from a to b. + /// + /// Positive number if point is left, negative if point is right, + /// and 0 if points are collinear. + public static float Area(Vector2 a, Vector2 b, Vector2 c) + { + return Area(ref a, ref b, ref c); + } + + /// + /// Returns a positive number if c is to the left of the line going from a to b. + /// + /// Positive number if point is left, negative if point is right, + /// and 0 if points are collinear. + public static float Area(ref Vector2 a, ref Vector2 b, ref Vector2 c) + { + return a.X * (b.Y - c.Y) + b.X * (c.Y - a.Y) + c.X * (a.Y - b.Y); + } + + /// + /// Determines if three vertices are collinear (ie. on a straight line) + /// + /// First vertex + /// Second vertex + /// Third vertex + /// The tolerance + /// + public static bool IsCollinear(ref Vector2 a, ref Vector2 b, ref Vector2 c, float tolerance = 0) + { + return FloatInRange(Area(ref a, ref b, ref c), -tolerance, tolerance); + } + + public static void Cross(float s, ref Vector2 a, out Vector2 b) + { + b = new Vector2(-s * a.Y, s * a.X); + } + + public static bool FloatEquals(float value1, float value2) + { + return Math.Abs(value1 - value2) <= Settings.Epsilon; + } + + /// + /// Checks if a floating point Value is equal to another, + /// within a certain tolerance. + /// + /// The first floating point Value. + /// The second floating point Value. + /// The floating point tolerance. + /// True if the values are "equal", false otherwise. + public static bool FloatEquals(float value1, float value2, float delta) + { + return FloatInRange(value1, value2 - delta, value2 + delta); + } + + /// + /// Checks if a floating point Value is within a specified + /// range of values (inclusive). + /// + /// The Value to check. + /// The minimum Value. + /// The maximum Value. + /// True if the Value is within the range specified, + /// false otherwise. + public static bool FloatInRange(float value, float min, float max) + { + return (value >= min && value <= max); + } + + #region Nested type: FloatConverter + + [StructLayout(LayoutKind.Explicit)] + private struct FloatConverter + { + [FieldOffset(0)] + public float x; + [FieldOffset(0)] + public int i; + } + + #endregion + + public static Vector2 Mul(ref Rot rot, Vector2 axis) + { + return Mul(rot, axis); + } + + public static Vector2 MulT(ref Rot rot, Vector2 axis) + { + return MulT(rot, axis); + } + } + + /// + /// A 2-by-2 matrix. Stored in column-major order. + /// + public struct Mat22 + { + public Vector2 ex, ey; + + /// + /// Construct this matrix using columns. + /// + /// The c1. + /// The c2. + public Mat22(Vector2 c1, Vector2 c2) + { + ex = c1; + ey = c2; + } + + /// + /// Construct this matrix using scalars. + /// + /// The a11. + /// The a12. + /// The a21. + /// The a22. + public Mat22(float a11, float a12, float a21, float a22) + { + ex = new Vector2(a11, a21); + ey = new Vector2(a12, a22); + } + + public Mat22 Inverse + { + get + { + float a = ex.X, b = ey.X, c = ex.Y, d = ey.Y; + float det = a * d - b * c; + if (det != 0.0f) + { + det = 1.0f / det; + } + + Mat22 result = new Mat22(); + result.ex.X = det * d; + result.ex.Y = -det * c; + + result.ey.X = -det * b; + result.ey.Y = det * a; + + return result; + } + } + + /// + /// Initialize this matrix using columns. + /// + /// The c1. + /// The c2. + public void Set(Vector2 c1, Vector2 c2) + { + ex = c1; + ey = c2; + } + + /// + /// Set this to the identity matrix. + /// + public void SetIdentity() + { + ex.X = 1.0f; + ey.X = 0.0f; + ex.Y = 0.0f; + ey.Y = 1.0f; + } + + /// + /// Set this matrix to all zeros. + /// + public void SetZero() + { + ex.X = 0.0f; + ey.X = 0.0f; + ex.Y = 0.0f; + ey.Y = 0.0f; + } + + /// + /// Solve A * x = b, where b is a column vector. This is more efficient + /// than computing the inverse in one-shot cases. + /// + /// The b. + /// + public Vector2 Solve(Vector2 b) + { + float a11 = ex.X, a12 = ey.X, a21 = ex.Y, a22 = ey.Y; + float det = a11 * a22 - a12 * a21; + if (det != 0.0f) + { + det = 1.0f / det; + } + + return new Vector2(det * (a22 * b.X - a12 * b.Y), det * (a11 * b.Y - a21 * b.X)); + } + + public static void Add(ref Mat22 A, ref Mat22 B, out Mat22 R) + { + R.ex = A.ex + B.ex; + R.ey = A.ey + B.ey; + } + } + + /// + /// A 3-by-3 matrix. Stored in column-major order. + /// + public struct Mat33 + { + public Vector3 ex, ey, ez; + + /// + /// Construct this matrix using columns. + /// + /// The c1. + /// The c2. + /// The c3. + public Mat33(Vector3 c1, Vector3 c2, Vector3 c3) + { + ex = c1; + ey = c2; + ez = c3; + } + + /// + /// Set this matrix to all zeros. + /// + public void SetZero() + { + ex = Vector3.Zero; + ey = Vector3.Zero; + ez = Vector3.Zero; + } + + /// + /// Solve A * x = b, where b is a column vector. This is more efficient + /// than computing the inverse in one-shot cases. + /// + /// The b. + /// + public Vector3 Solve33(Vector3 b) + { + float det = Vector3.Dot(ex, Vector3.Cross(ey, ez)); + if (det != 0.0f) + { + det = 1.0f / det; + } + + return new Vector3(det * Vector3.Dot(b, Vector3.Cross(ey, ez)), det * Vector3.Dot(ex, Vector3.Cross(b, ez)), det * Vector3.Dot(ex, Vector3.Cross(ey, b))); + } + + /// + /// Solve A * x = b, where b is a column vector. This is more efficient + /// than computing the inverse in one-shot cases. Solve only the upper + /// 2-by-2 matrix equation. + /// + /// The b. + /// + public Vector2 Solve22(Vector2 b) + { + float a11 = ex.X, a12 = ey.X, a21 = ex.Y, a22 = ey.Y; + float det = a11 * a22 - a12 * a21; + + if (det != 0.0f) + { + det = 1.0f / det; + } + + return new Vector2(det * (a22 * b.X - a12 * b.Y), det * (a11 * b.Y - a21 * b.X)); + } + + /// Get the inverse of this matrix as a 2-by-2. + /// Returns the zero matrix if singular. + public void GetInverse22(ref Mat33 M) + { + float a = ex.X, b = ey.X, c = ex.Y, d = ey.Y; + float det = a * d - b * c; + if (det != 0.0f) + { + det = 1.0f / det; + } + + M.ex.X = det * d; M.ey.X = -det * b; M.ex.Z = 0.0f; + M.ex.Y = -det * c; M.ey.Y = det * a; M.ey.Z = 0.0f; + M.ez.X = 0.0f; M.ez.Y = 0.0f; M.ez.Z = 0.0f; + } + + /// Get the symmetric inverse of this matrix as a 3-by-3. + /// Returns the zero matrix if singular. + public void GetSymInverse33(ref Mat33 M) + { + float det = MathUtils.Dot(ex, MathUtils.Cross(ey, ez)); + if (det != 0.0f) + { + det = 1.0f / det; + } + + float a11 = ex.X, a12 = ey.X, a13 = ez.X; + float a22 = ey.Y, a23 = ez.Y; + float a33 = ez.Z; + + M.ex.X = det * (a22 * a33 - a23 * a23); + M.ex.Y = det * (a13 * a23 - a12 * a33); + M.ex.Z = det * (a12 * a23 - a13 * a22); + + M.ey.X = M.ex.Y; + M.ey.Y = det * (a11 * a33 - a13 * a13); + M.ey.Z = det * (a13 * a12 - a11 * a23); + + M.ez.X = M.ex.Z; + M.ez.Y = M.ey.Z; + M.ez.Z = det * (a11 * a22 - a12 * a12); + } + } + + /// + /// Rotation + /// + public struct Rot + { + /// Sine and cosine + public float s, c; + + /// + /// Initialize from an angle in radians + /// + /// Angle in radians + public Rot(float angle) + { + // TODO_ERIN optimize + s = (float)Math.Sin(angle); + c = (float)Math.Cos(angle); + } + + /// + /// Set using an angle in radians. + /// + /// + public void Set(float angle) + { + // TODO_ERIN optimize + s = (float)Math.Sin(angle); + c = (float)Math.Cos(angle); + } + + /// + /// Set to the identity rotation + /// + public void SetIdentity() + { + s = 0.0f; + c = 1.0f; + } + + /// + /// Get the angle in radians + /// + public float GetAngle() + { + return (float)Math.Atan2(s, c); + } + + /// + /// Get the x-axis + /// + public Vector2 GetXAxis() + { + return new Vector2(c, s); + } + + /// + /// Get the y-axis + /// + public Vector2 GetYAxis() + { + return new Vector2(-s, c); + } + } + + /// + /// A transform contains translation and rotation. It is used to represent + /// the position and orientation of rigid frames. + /// + public struct Transform + { + public Vector2 p; + public Rot q; + + /// + /// Initialize using a position vector and a rotation matrix. + /// + /// The position. + /// The r. + public Transform(ref Vector2 position, ref Rot rotation) + { + p = position; + q = rotation; + } + + /// + /// Set this to the identity transform. + /// + public void SetIdentity() + { + p = Vector2.Zero; + q.SetIdentity(); + } + + /// + /// Set this based on the position and angle. + /// + /// The position. + /// The angle. + public void Set(Vector2 position, float angle) + { + p = position; + q.Set(angle); + } + } + + /// + /// This describes the motion of a body/shape for TOI computation. + /// Shapes are defined with respect to the body origin, which may + /// no coincide with the center of mass. However, to support dynamics + /// we must interpolate the center of mass position. + /// + public struct Sweep + { + /// + /// World angles + /// + public float A; + + public float A0; + + /// + /// Fraction of the current time step in the range [0,1] + /// c0 and a0 are the positions at alpha0. + /// + public float Alpha0; + + /// + /// Center world positions + /// + public Vector2 C; + + public Vector2 C0; + + /// + /// Local center of mass position + /// + public Vector2 LocalCenter; + + /// + /// Get the interpolated transform at a specific time. + /// + /// The transform. + /// beta is a factor in [0,1], where 0 indicates alpha0. + public void GetTransform(out Transform xfb, float beta) + { + xfb = new Transform(); + xfb.p.X = (1.0f - beta) * C0.X + beta * C.X; + xfb.p.Y = (1.0f - beta) * C0.Y + beta * C.Y; + float angle = (1.0f - beta) * A0 + beta * A; + xfb.q.Set(angle); + + // Shift to origin + xfb.p -= MathUtils.Mul(xfb.q, LocalCenter); + } + + /// + /// Advance the sweep forward, yielding a new initial state. + /// + /// new initial time.. + public void Advance(float alpha) + { + Debug.Assert(Alpha0 < 1.0f); + float beta = (alpha - Alpha0) / (1.0f - Alpha0); + C0 += beta * (C - C0); + A0 += beta * (A - A0); + Alpha0 = alpha; + } + + /// + /// Normalize the angles. + /// + public void Normalize() + { + float d = MathHelper.TwoPi * (float)Math.Floor(A0 / MathHelper.TwoPi); + A0 -= d; + A -= d; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/MathHelper.cs b/Farseer Physics Engine 3.5/Common/MathHelper.cs new file mode 100644 index 000000000..3f48a8b76 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/MathHelper.cs @@ -0,0 +1,158 @@ +#if !XNA && !WINDOWS_PHONE && !XBOX && !ANDROID + +#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; + +namespace Microsoft.Xna.Framework +{ + public static class MathHelper + { + public const float E = (float) Math.E; + public const float Log10E = 0.4342945f; + public const float Log2E = 1.442695f; + public const float Pi = (float) Math.PI; + public const float PiOver2 = (float) (Math.PI/2.0); + public const float PiOver4 = (float) (Math.PI/4.0); + public const float TwoPi = (float) (Math.PI*2.0); + + public static float Barycentric(float value1, float value2, float value3, float amount1, float amount2) + { + return value1 + (value2 - value1)*amount1 + (value3 - value1)*amount2; + } + + public static float CatmullRom(float value1, float value2, float value3, float value4, float amount) + { + // Using formula from http://www.mvps.org/directx/articles/catmull/ + // Internally using doubles not to lose precission + double amountSquared = amount*amount; + double amountCubed = amountSquared*amount; + return (float) (0.5*(2.0*value2 + + (value3 - value1)*amount + + (2.0*value1 - 5.0*value2 + 4.0*value3 - value4)*amountSquared + + (3.0*value2 - value1 - 3.0*value3 + value4)*amountCubed)); + } + + public static float Clamp(float value, float min, float max) + { + // First we check to see if we're greater than the max + value = (value > max) ? max : value; + + // Then we check to see if we're less than the min. + value = (value < min) ? min : value; + + // There's no check to see if min > max. + return value; + } + + public static float Distance(float value1, float value2) + { + return Math.Abs(value1 - value2); + } + + public static float Hermite(float value1, float tangent1, float value2, float tangent2, float amount) + { + // All transformed to double not to lose precission + // Otherwise, for high numbers of param:amount the result is NaN instead of Infinity + double v1 = value1, v2 = value2, t1 = tangent1, t2 = tangent2, s = amount, result; + double sCubed = s*s*s; + double sSquared = s*s; + + if (amount == 0f) + result = value1; + else if (amount == 1f) + result = value2; + else + result = (2*v1 - 2*v2 + t2 + t1)*sCubed + + (3*v2 - 3*v1 - 2*t1 - t2)*sSquared + + t1*s + + v1; + return (float) result; + } + + public static float Lerp(float value1, float value2, float amount) + { + return value1 + (value2 - value1)*amount; + } + + public static float Max(float value1, float value2) + { + return Math.Max(value1, value2); + } + + public static float Min(float value1, float value2) + { + return Math.Min(value1, value2); + } + + public static float SmoothStep(float value1, float value2, float amount) + { + // It is expected that 0 < amount < 1 + // If amount < 0, return value1 + // If amount > 1, return value2 + float result = Clamp(amount, 0f, 1f); + result = Hermite(value1, 0f, value2, 0f, result); + return result; + } + + public static float ToDegrees(float radians) + { + // This method uses double precission internally, + // though it returns single float + // Factor = 180 / pi + return (float) (radians*57.295779513082320876798154814105); + } + + public static float ToRadians(float degrees) + { + // This method uses double precission internally, + // though it returns single float + // Factor = pi / 180 + return (float) (degrees*0.017453292519943295769236907684886); + } + + public static float WrapAngle(float angle) + { + angle = (float) Math.IEEERemainder((double) angle, 6.2831854820251465); //2xPi precission is double + if (angle <= -3.141593f) + { + angle += 6.283185f; + return angle; + } + if (angle > 3.141593f) + { + angle -= 6.283185f; + } + return angle; + } + } +} + +#endif \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Matrix.cs b/Farseer Physics Engine 3.5/Common/Matrix.cs new file mode 100644 index 000000000..1f9850752 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Matrix.cs @@ -0,0 +1,1130 @@ +#if !XNA && !WINDOWS_PHONE && !XBOX && !ANDROID + +#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.Runtime.InteropServices; + +namespace Microsoft.Xna.Framework +{ + [StructLayout(LayoutKind.Sequential)] + public struct Matrix : IEquatable + { +#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 Static Properties + + private static Matrix identity = new Matrix(1f, 0f, 0f, 0f, 0f, 1f, 0f, 0f, 0f, 0f, 1f, 0f, 0f, 0f, 0f, 1f); + + public static Matrix Identity + { + get { return identity; } + } + +#endregion Static Properties + +#region Public Properties + + public Vector3 Backward + { + get { return new Vector3(M31, M32, M33); } + set + { + M31 = value.X; + M32 = value.Y; + M33 = value.Z; + } + } + + public Vector3 Down + { + get { return new Vector3(-M21, -M22, -M23); } + set + { + M21 = -value.X; + M22 = -value.Y; + M23 = -value.Z; + } + } + + public Vector3 Forward + { + get { return new Vector3(-M31, -M32, -M33); } + set + { + M31 = -value.X; + M32 = -value.Y; + M33 = -value.Z; + } + } + + public Vector3 Left + { + get { return new Vector3(-M11, -M12, -M13); } + set + { + M11 = -value.X; + M12 = -value.Y; + M13 = -value.Z; + } + } + + public Vector3 Right + { + get { return new Vector3(M11, M12, M13); } + set + { + M11 = value.X; + M12 = value.Y; + M13 = value.Z; + } + } + + public Vector3 Translation + { + get { return new Vector3(M41, M42, M43); } + set + { + M41 = value.X; + M42 = value.Y; + M43 = value.Z; + } + } + + public Vector3 Up + { + get { return new Vector3(M21, M22, M23); } + set + { + M21 = value.X; + M22 = value.Y; + M23 = value.Z; + } + } + +#endregion Public Properties + +#region Constructors + + /// + /// Constructor for 4x4 Matrix + /// + /// + /// A + /// + /// + /// A + /// + /// + /// A + /// + /// + /// A + /// + /// + /// A + /// + /// + /// A + /// + /// + /// A + /// + /// + /// A + /// + /// + /// A + /// + /// + /// A + /// + /// + /// A + /// + /// + /// A + /// + /// + /// A + /// + /// + /// A + /// + /// + /// A + /// + /// + /// A + /// + 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) + { + M11 = m11; + M12 = m12; + M13 = m13; + M14 = m14; + M21 = m21; + M22 = m22; + M23 = m23; + M24 = m24; + M31 = m31; + M32 = m32; + M33 = m33; + M34 = m34; + M41 = m41; + M42 = m42; + M43 = m43; + M44 = m44; + } + +#endregion Constructors + +#region Public Static Methods + + public static Matrix CreateWorld(Vector3 position, Vector3 forward, Vector3 up) + { + Matrix ret; + CreateWorld(ref position, ref forward, ref up, out ret); + return ret; + } + + public static void CreateWorld(ref Vector3 position, ref Vector3 forward, ref Vector3 up, out Matrix result) + { + Vector3 x, y, z; + Vector3.Normalize(ref forward, out z); + Vector3.Cross(ref forward, ref up, out x); + Vector3.Cross(ref x, ref forward, out y); + x.Normalize(); + y.Normalize(); + + result = new Matrix(); + result.Right = x; + result.Up = y; + result.Forward = z; + result.Translation = position; + result.M44 = 1f; + } + + /// + /// Adds second matrix to the first. + /// + /// + /// A + /// + /// + /// A + /// + /// + /// A + /// + public static Matrix Add(Matrix matrix1, Matrix matrix2) + { + matrix1.M11 += matrix2.M11; + matrix1.M12 += matrix2.M12; + matrix1.M13 += matrix2.M13; + matrix1.M14 += matrix2.M14; + matrix1.M21 += matrix2.M21; + matrix1.M22 += matrix2.M22; + matrix1.M23 += matrix2.M23; + matrix1.M24 += matrix2.M24; + matrix1.M31 += matrix2.M31; + matrix1.M32 += matrix2.M32; + matrix1.M33 += matrix2.M33; + matrix1.M34 += matrix2.M34; + matrix1.M41 += matrix2.M41; + matrix1.M42 += matrix2.M42; + matrix1.M43 += matrix2.M43; + matrix1.M44 += matrix2.M44; + return matrix1; + } + + + /// + /// Adds two Matrix and save to the result Matrix + /// + /// + /// A + /// + /// + /// A + /// + /// + /// A + /// + public static void Add(ref Matrix matrix1, ref Matrix matrix2, out Matrix result) + { + result.M11 = matrix1.M11 + matrix2.M11; + result.M12 = matrix1.M12 + matrix2.M12; + result.M13 = matrix1.M13 + matrix2.M13; + result.M14 = matrix1.M14 + matrix2.M14; + result.M21 = matrix1.M21 + matrix2.M21; + result.M22 = matrix1.M22 + matrix2.M22; + result.M23 = matrix1.M23 + matrix2.M23; + result.M24 = matrix1.M24 + matrix2.M24; + result.M31 = matrix1.M31 + matrix2.M31; + result.M32 = matrix1.M32 + matrix2.M32; + result.M33 = matrix1.M33 + matrix2.M33; + result.M34 = matrix1.M34 + matrix2.M34; + result.M41 = matrix1.M41 + matrix2.M41; + result.M42 = matrix1.M42 + matrix2.M42; + result.M43 = matrix1.M43 + matrix2.M43; + result.M44 = matrix1.M44 + matrix2.M44; + } + + + public static Matrix CreateBillboard(Vector3 objectPosition, Vector3 cameraPosition, + Vector3 cameraUpVector, Nullable cameraForwardVector) + { + Matrix ret; + CreateBillboard(ref objectPosition, ref cameraPosition, ref cameraUpVector, cameraForwardVector, out ret); + return ret; + } + + public static void CreateBillboard(ref Vector3 objectPosition, ref Vector3 cameraPosition, + ref Vector3 cameraUpVector, Vector3? cameraForwardVector, out Matrix result) + { + Vector3 translation = objectPosition - cameraPosition; + Vector3 backwards, right, up; + Vector3.Normalize(ref translation, out backwards); + Vector3.Normalize(ref cameraUpVector, out up); + Vector3.Cross(ref backwards, ref up, out right); + Vector3.Cross(ref backwards, ref right, out up); + result = Identity; + result.Backward = backwards; + result.Right = right; + result.Up = up; + result.Translation = translation; + } + + 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 CreateLookAt(Vector3 cameraPosition, Vector3 cameraTarget, Vector3 cameraUpVector) + { + Matrix ret; + CreateLookAt(ref cameraPosition, ref cameraTarget, ref cameraUpVector, out ret); + return ret; + } + + + public static void CreateLookAt(ref Vector3 cameraPosition, ref Vector3 cameraTarget, ref Vector3 cameraUpVector, + out Matrix result) + { + // http://msdn.microsoft.com/en-us/library/bb205343(v=VS.85).aspx + + Vector3 vz = Vector3.Normalize(cameraPosition - cameraTarget); + Vector3 vx = Vector3.Normalize(Vector3.Cross(cameraUpVector, vz)); + Vector3 vy = Vector3.Cross(vz, vx); + result = Identity; + result.M11 = vx.X; + result.M12 = vy.X; + result.M13 = vz.X; + result.M21 = vx.Y; + result.M22 = vy.Y; + result.M23 = vz.Y; + result.M31 = vx.Z; + result.M32 = vy.Z; + result.M33 = vz.Z; + result.M41 = -Vector3.Dot(vx, cameraPosition); + result.M42 = -Vector3.Dot(vy, cameraPosition); + result.M43 = -Vector3.Dot(vz, cameraPosition); + } + + public static Matrix CreateOrthographic(float width, float height, float zNearPlane, float zFarPlane) + { + Matrix ret; + CreateOrthographic(width, height, zNearPlane, zFarPlane, out ret); + return ret; + } + + + public static void CreateOrthographic(float width, float height, float zNearPlane, float zFarPlane, + out Matrix result) + { + result.M11 = 2/width; + result.M12 = 0; + result.M13 = 0; + result.M14 = 0; + result.M21 = 0; + result.M22 = 2/height; + result.M23 = 0; + result.M24 = 0; + result.M31 = 0; + result.M32 = 0; + result.M33 = 1/(zNearPlane - zFarPlane); + result.M34 = 0; + result.M41 = 0; + result.M42 = 0; + result.M43 = zNearPlane/(zNearPlane - zFarPlane); + result.M44 = 1; + } + + + public static Matrix CreateOrthographicOffCenter(float left, float right, float bottom, float top, + float zNearPlane, float zFarPlane) + { + Matrix ret; + CreateOrthographicOffCenter(left, right, bottom, top, zNearPlane, zFarPlane, out ret); + return ret; + } + + + public static void CreateOrthographicOffCenter(float left, float right, float bottom, float top, + float zNearPlane, float zFarPlane, out Matrix result) + { + result.M11 = 2/(right - left); + result.M12 = 0; + result.M13 = 0; + result.M14 = 0; + result.M21 = 0; + result.M22 = 2/(top - bottom); + result.M23 = 0; + result.M24 = 0; + result.M31 = 0; + result.M32 = 0; + result.M33 = 1/(zNearPlane - zFarPlane); + result.M34 = 0; + result.M41 = (left + right)/(left - right); + result.M42 = (bottom + top)/(bottom - top); + result.M43 = zNearPlane/(zNearPlane - zFarPlane); + result.M44 = 1; + } + + + 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 nearPlaneDistance, + float farPlaneDistance) + { + Matrix ret; + CreatePerspectiveFieldOfView(fieldOfView, aspectRatio, nearPlaneDistance, farPlaneDistance, out ret); + return ret; + } + + + public static void CreatePerspectiveFieldOfView(float fieldOfView, float aspectRatio, float nearPlaneDistance, + float farPlaneDistance, out Matrix result) + { + // http://msdn.microsoft.com/en-us/library/bb205351(v=VS.85).aspx + // http://msdn.microsoft.com/en-us/library/bb195665.aspx + + result = new Matrix(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); + + if (fieldOfView < 0 || fieldOfView > 3.14159262f) + throw new ArgumentOutOfRangeException("fieldOfView", + "fieldOfView takes a value between 0 and Pi (180 degrees) in radians."); + + if (nearPlaneDistance <= 0.0f) + throw new ArgumentOutOfRangeException("nearPlaneDistance", + "You should specify positive value for nearPlaneDistance."); + + if (farPlaneDistance <= 0.0f) + throw new ArgumentOutOfRangeException("farPlaneDistance", + "You should specify positive value for farPlaneDistance."); + + if (farPlaneDistance <= nearPlaneDistance) + throw new ArgumentOutOfRangeException("nearPlaneDistance", + "Near plane distance is larger than Far plane distance. Near plane distance must be smaller than Far plane distance."); + + float yscale = (float) 1/(float) Math.Tan(fieldOfView/2); + float xscale = yscale/aspectRatio; + + result.M11 = xscale; + result.M22 = yscale; + result.M33 = farPlaneDistance/(nearPlaneDistance - farPlaneDistance); + result.M34 = -1; + result.M43 = nearPlaneDistance*farPlaneDistance/(nearPlaneDistance - farPlaneDistance); + } + + + 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) + { + Matrix returnMatrix = Identity; + + returnMatrix.M22 = (float) Math.Cos(radians); + returnMatrix.M23 = (float) Math.Sin(radians); + returnMatrix.M32 = -returnMatrix.M23; + returnMatrix.M33 = returnMatrix.M22; + + return returnMatrix; + } + + + public static void CreateRotationX(float radians, out Matrix result) + { + result = Identity; + + result.M22 = (float) Math.Cos(radians); + result.M23 = (float) Math.Sin(radians); + result.M32 = -result.M23; + result.M33 = result.M22; + } + + + public static Matrix CreateRotationY(float radians) + { + Matrix returnMatrix = Identity; + + returnMatrix.M11 = (float) Math.Cos(radians); + returnMatrix.M13 = (float) Math.Sin(radians); + returnMatrix.M31 = -returnMatrix.M13; + returnMatrix.M33 = returnMatrix.M11; + + return returnMatrix; + } + + + public static void CreateRotationY(float radians, out Matrix result) + { + result = Identity; + + result.M11 = (float) Math.Cos(radians); + result.M13 = (float) Math.Sin(radians); + result.M31 = -result.M13; + result.M33 = result.M11; + } + + + public static Matrix CreateRotationZ(float radians) + { + Matrix returnMatrix = Identity; + + returnMatrix.M11 = (float) Math.Cos(radians); + returnMatrix.M12 = (float) Math.Sin(radians); + returnMatrix.M21 = -returnMatrix.M12; + returnMatrix.M22 = returnMatrix.M11; + + return returnMatrix; + } + + + public static void CreateRotationZ(float radians, out Matrix result) + { + result = Identity; + + result.M11 = (float) Math.Cos(radians); + result.M12 = (float) Math.Sin(radians); + result.M21 = -result.M12; + result.M22 = result.M11; + } + + + public static Matrix CreateScale(float scale) + { + Matrix returnMatrix = Identity; + + returnMatrix.M11 = scale; + returnMatrix.M22 = scale; + returnMatrix.M33 = scale; + + return returnMatrix; + } + + + public static void CreateScale(float scale, out Matrix result) + { + result = Identity; + + result.M11 = scale; + result.M22 = scale; + result.M33 = scale; + } + + + public static Matrix CreateScale(float xScale, float yScale, float zScale) + { + Matrix returnMatrix = Identity; + + returnMatrix.M11 = xScale; + returnMatrix.M22 = yScale; + returnMatrix.M33 = zScale; + + return returnMatrix; + } + + + public static void CreateScale(float xScale, float yScale, float zScale, out Matrix result) + { + result = Identity; + + result.M11 = xScale; + result.M22 = yScale; + result.M33 = zScale; + } + + + public static Matrix CreateScale(Vector3 scales) + { + Matrix returnMatrix = Identity; + + returnMatrix.M11 = scales.X; + returnMatrix.M22 = scales.Y; + returnMatrix.M33 = scales.Z; + + return returnMatrix; + } + + + public static void CreateScale(ref Vector3 scales, out Matrix result) + { + result = Identity; + + result.M11 = scales.X; + result.M22 = scales.Y; + result.M33 = scales.Z; + } + + + public static Matrix CreateTranslation(float xPosition, float yPosition, float zPosition) + { + Matrix returnMatrix = Identity; + + returnMatrix.M41 = xPosition; + returnMatrix.M42 = yPosition; + returnMatrix.M43 = zPosition; + + return returnMatrix; + } + + + public static void CreateTranslation(float xPosition, float yPosition, float zPosition, out Matrix result) + { + result = Identity; + + result.M41 = xPosition; + result.M42 = yPosition; + result.M43 = zPosition; + } + + + public static Matrix CreateTranslation(Vector3 position) + { + Matrix returnMatrix = Identity; + + returnMatrix.M41 = position.X; + returnMatrix.M42 = position.Y; + returnMatrix.M43 = position.Z; + + return returnMatrix; + } + + + public static void CreateTranslation(ref Vector3 position, out Matrix result) + { + result = Identity; + + result.M41 = position.X; + result.M42 = position.Y; + result.M43 = position.Z; + } + + public static Matrix Divide(Matrix matrix1, Matrix matrix2) + { + Matrix ret; + Divide(ref matrix1, ref matrix2, out ret); + return ret; + } + + + public static void Divide(ref Matrix matrix1, ref Matrix matrix2, out Matrix result) + { + Matrix inverse = Invert(matrix2); + Multiply(ref matrix1, ref inverse, out result); + } + + + public static Matrix Divide(Matrix matrix1, float divider) + { + Matrix ret; + Divide(ref matrix1, divider, out ret); + return ret; + } + + + public static void Divide(ref Matrix matrix1, float divider, out Matrix result) + { + float inverseDivider = 1f/divider; + Multiply(ref matrix1, inverseDivider, out result); + } + + public static Matrix Invert(Matrix matrix) + { + Invert(ref matrix, out matrix); + return matrix; + } + + + public static void Invert(ref Matrix matrix, out Matrix result) + { + // + // Use Laplace expansion theorem to calculate the inverse of a 4x4 matrix + // + // 1. Calculate the 2x2 determinants needed and the 4x4 determinant based on the 2x2 determinants + // 2. Create the adjugate matrix, which satisfies: A * adj(A) = det(A) * I + // 3. Divide adjugate matrix with the determinant to find the inverse + + float det1 = matrix.M11*matrix.M22 - matrix.M12*matrix.M21; + float det2 = matrix.M11*matrix.M23 - matrix.M13*matrix.M21; + float det3 = matrix.M11*matrix.M24 - matrix.M14*matrix.M21; + float det4 = matrix.M12*matrix.M23 - matrix.M13*matrix.M22; + float det5 = matrix.M12*matrix.M24 - matrix.M14*matrix.M22; + float det6 = matrix.M13*matrix.M24 - matrix.M14*matrix.M23; + float det7 = matrix.M31*matrix.M42 - matrix.M32*matrix.M41; + float det8 = matrix.M31*matrix.M43 - matrix.M33*matrix.M41; + float det9 = matrix.M31*matrix.M44 - matrix.M34*matrix.M41; + float det10 = matrix.M32*matrix.M43 - matrix.M33*matrix.M42; + float det11 = matrix.M32*matrix.M44 - matrix.M34*matrix.M42; + float det12 = matrix.M33*matrix.M44 - matrix.M34*matrix.M43; + + float detMatrix = (float) (det1*det12 - det2*det11 + det3*det10 + det4*det9 - det5*det8 + det6*det7); + + float invDetMatrix = 1f/detMatrix; + + Matrix ret; // Allow for matrix and result to point to the same structure + + ret.M11 = (matrix.M22*det12 - matrix.M23*det11 + matrix.M24*det10)*invDetMatrix; + ret.M12 = (-matrix.M12*det12 + matrix.M13*det11 - matrix.M14*det10)*invDetMatrix; + ret.M13 = (matrix.M42*det6 - matrix.M43*det5 + matrix.M44*det4)*invDetMatrix; + ret.M14 = (-matrix.M32*det6 + matrix.M33*det5 - matrix.M34*det4)*invDetMatrix; + ret.M21 = (-matrix.M21*det12 + matrix.M23*det9 - matrix.M24*det8)*invDetMatrix; + ret.M22 = (matrix.M11*det12 - matrix.M13*det9 + matrix.M14*det8)*invDetMatrix; + ret.M23 = (-matrix.M41*det6 + matrix.M43*det3 - matrix.M44*det2)*invDetMatrix; + ret.M24 = (matrix.M31*det6 - matrix.M33*det3 + matrix.M34*det2)*invDetMatrix; + ret.M31 = (matrix.M21*det11 - matrix.M22*det9 + matrix.M24*det7)*invDetMatrix; + ret.M32 = (-matrix.M11*det11 + matrix.M12*det9 - matrix.M14*det7)*invDetMatrix; + ret.M33 = (matrix.M41*det5 - matrix.M42*det3 + matrix.M44*det1)*invDetMatrix; + ret.M34 = (-matrix.M31*det5 + matrix.M32*det3 - matrix.M34*det1)*invDetMatrix; + ret.M41 = (-matrix.M21*det10 + matrix.M22*det8 - matrix.M23*det7)*invDetMatrix; + ret.M42 = (matrix.M11*det10 - matrix.M12*det8 + matrix.M13*det7)*invDetMatrix; + ret.M43 = (-matrix.M41*det4 + matrix.M42*det2 - matrix.M43*det1)*invDetMatrix; + ret.M44 = (matrix.M31*det4 - matrix.M32*det2 + matrix.M33*det1)*invDetMatrix; + + result = ret; + } + + + 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) + { + Matrix ret; + Multiply(ref matrix1, ref matrix2, out ret); + return ret; + } + + + public static void Multiply(ref Matrix matrix1, ref Matrix matrix2, out Matrix result) + { + result.M11 = matrix1.M11*matrix2.M11 + matrix1.M12*matrix2.M21 + matrix1.M13*matrix2.M31 + + matrix1.M14*matrix2.M41; + result.M12 = matrix1.M11*matrix2.M12 + matrix1.M12*matrix2.M22 + matrix1.M13*matrix2.M32 + + matrix1.M14*matrix2.M42; + result.M13 = matrix1.M11*matrix2.M13 + matrix1.M12*matrix2.M23 + matrix1.M13*matrix2.M33 + + matrix1.M14*matrix2.M43; + result.M14 = matrix1.M11*matrix2.M14 + matrix1.M12*matrix2.M24 + matrix1.M13*matrix2.M34 + + matrix1.M14*matrix2.M44; + + result.M21 = matrix1.M21*matrix2.M11 + matrix1.M22*matrix2.M21 + matrix1.M23*matrix2.M31 + + matrix1.M24*matrix2.M41; + result.M22 = matrix1.M21*matrix2.M12 + matrix1.M22*matrix2.M22 + matrix1.M23*matrix2.M32 + + matrix1.M24*matrix2.M42; + result.M23 = matrix1.M21*matrix2.M13 + matrix1.M22*matrix2.M23 + matrix1.M23*matrix2.M33 + + matrix1.M24*matrix2.M43; + result.M24 = matrix1.M21*matrix2.M14 + matrix1.M22*matrix2.M24 + matrix1.M23*matrix2.M34 + + matrix1.M24*matrix2.M44; + + result.M31 = matrix1.M31*matrix2.M11 + matrix1.M32*matrix2.M21 + matrix1.M33*matrix2.M31 + + matrix1.M34*matrix2.M41; + result.M32 = matrix1.M31*matrix2.M12 + matrix1.M32*matrix2.M22 + matrix1.M33*matrix2.M32 + + matrix1.M34*matrix2.M42; + result.M33 = matrix1.M31*matrix2.M13 + matrix1.M32*matrix2.M23 + matrix1.M33*matrix2.M33 + + matrix1.M34*matrix2.M43; + result.M34 = matrix1.M31*matrix2.M14 + matrix1.M32*matrix2.M24 + matrix1.M33*matrix2.M34 + + matrix1.M34*matrix2.M44; + + result.M41 = matrix1.M41*matrix2.M11 + matrix1.M42*matrix2.M21 + matrix1.M43*matrix2.M31 + + matrix1.M44*matrix2.M41; + result.M42 = matrix1.M41*matrix2.M12 + matrix1.M42*matrix2.M22 + matrix1.M43*matrix2.M32 + + matrix1.M44*matrix2.M42; + result.M43 = matrix1.M41*matrix2.M13 + matrix1.M42*matrix2.M23 + matrix1.M43*matrix2.M33 + + matrix1.M44*matrix2.M43; + result.M44 = matrix1.M41*matrix2.M14 + matrix1.M42*matrix2.M24 + matrix1.M43*matrix2.M34 + + matrix1.M44*matrix2.M44; + } + + + public static Matrix Multiply(Matrix matrix1, float factor) + { + matrix1.M11 *= factor; + matrix1.M12 *= factor; + matrix1.M13 *= factor; + matrix1.M14 *= factor; + matrix1.M21 *= factor; + matrix1.M22 *= factor; + matrix1.M23 *= factor; + matrix1.M24 *= factor; + matrix1.M31 *= factor; + matrix1.M32 *= factor; + matrix1.M33 *= factor; + matrix1.M34 *= factor; + matrix1.M41 *= factor; + matrix1.M42 *= factor; + matrix1.M43 *= factor; + matrix1.M44 *= factor; + return matrix1; + } + + + public static void Multiply(ref Matrix matrix1, float factor, out Matrix result) + { + result.M11 = matrix1.M11*factor; + result.M12 = matrix1.M12*factor; + result.M13 = matrix1.M13*factor; + result.M14 = matrix1.M14*factor; + result.M21 = matrix1.M21*factor; + result.M22 = matrix1.M22*factor; + result.M23 = matrix1.M23*factor; + result.M24 = matrix1.M24*factor; + result.M31 = matrix1.M31*factor; + result.M32 = matrix1.M32*factor; + result.M33 = matrix1.M33*factor; + result.M34 = matrix1.M34*factor; + result.M41 = matrix1.M41*factor; + result.M42 = matrix1.M42*factor; + result.M43 = matrix1.M43*factor; + result.M44 = matrix1.M44*factor; + } + + + public static Matrix Negate(Matrix matrix) + { + Multiply(ref matrix, -1.0f, out matrix); + return matrix; + } + + + public static void Negate(ref Matrix matrix, out Matrix result) + { + Multiply(ref matrix, -1.0f, out result); + } + + public static Matrix Subtract(Matrix matrix1, Matrix matrix2) + { + matrix1.M11 -= matrix2.M11; + matrix1.M12 -= matrix2.M12; + matrix1.M13 -= matrix2.M13; + matrix1.M14 -= matrix2.M14; + matrix1.M21 -= matrix2.M21; + matrix1.M22 -= matrix2.M22; + matrix1.M23 -= matrix2.M23; + matrix1.M24 -= matrix2.M24; + matrix1.M31 -= matrix2.M31; + matrix1.M32 -= matrix2.M32; + matrix1.M33 -= matrix2.M33; + matrix1.M34 -= matrix2.M34; + matrix1.M41 -= matrix2.M41; + matrix1.M42 -= matrix2.M42; + matrix1.M43 -= matrix2.M43; + matrix1.M44 -= matrix2.M44; + return matrix1; + } + + public static void Subtract(ref Matrix matrix1, ref Matrix matrix2, out Matrix result) + { + result.M11 = matrix1.M11 - matrix2.M11; + result.M12 = matrix1.M12 - matrix2.M12; + result.M13 = matrix1.M13 - matrix2.M13; + result.M14 = matrix1.M14 - matrix2.M14; + result.M21 = matrix1.M21 - matrix2.M21; + result.M22 = matrix1.M22 - matrix2.M22; + result.M23 = matrix1.M23 - matrix2.M23; + result.M24 = matrix1.M24 - matrix2.M24; + result.M31 = matrix1.M31 - matrix2.M31; + result.M32 = matrix1.M32 - matrix2.M32; + result.M33 = matrix1.M33 - matrix2.M33; + result.M34 = matrix1.M34 - matrix2.M34; + result.M41 = matrix1.M41 - matrix2.M41; + result.M42 = matrix1.M42 - matrix2.M42; + result.M43 = matrix1.M43 - matrix2.M43; + result.M44 = matrix1.M44 - matrix2.M44; + } + + public static Matrix Transpose(Matrix matrix) + { + Matrix ret; + Transpose(ref matrix, out ret); + return ret; + } + + + public static void Transpose(ref Matrix matrix, out Matrix result) + { + result.M11 = matrix.M11; + result.M12 = matrix.M21; + result.M13 = matrix.M31; + result.M14 = matrix.M41; + + result.M21 = matrix.M12; + result.M22 = matrix.M22; + result.M23 = matrix.M32; + result.M24 = matrix.M42; + + result.M31 = matrix.M13; + result.M32 = matrix.M23; + result.M33 = matrix.M33; + result.M34 = matrix.M43; + + result.M41 = matrix.M14; + result.M42 = matrix.M24; + result.M43 = matrix.M34; + result.M44 = matrix.M44; + } + +#endregion Public Static Methods + +#region Public Methods + + public float Determinant() + { + float minor1, minor2, minor3, minor4, minor5, minor6; + + minor1 = M31*M42 - M32*M41; + minor2 = M31*M43 - M33*M41; + minor3 = M31*M44 - M34*M41; + minor4 = M32*M43 - M33*M42; + minor5 = M32*M44 - M34*M42; + minor6 = M33*M44 - M34*M43; + + return M11*(M22*minor6 - M23*minor5 + M24*minor4) - + M12*(M21*minor6 - M23*minor3 + M24*minor2) + + M13*(M21*minor5 - M22*minor3 + M24*minor1) - + M14*(M21*minor4 - M22*minor2 + M23*minor1); + } + + public bool Equals(Matrix other) + { + return this == other; + } + +#endregion Public Methods + +#region Operators + + public static Matrix operator +(Matrix matrix1, Matrix matrix2) + { + Add(ref matrix1, ref matrix2, out matrix1); + return matrix1; + } + + public static Matrix operator /(Matrix matrix1, Matrix matrix2) + { + Matrix ret; + Divide(ref matrix1, ref matrix2, out ret); + return ret; + } + + public static Matrix operator /(Matrix matrix1, float divider) + { + Matrix ret; + Divide(ref matrix1, divider, out ret); + return ret; + } + + public static bool operator ==(Matrix matrix1, Matrix matrix2) + { + return (matrix1.M11 == matrix2.M11) && (matrix1.M12 == matrix2.M12) && + (matrix1.M13 == matrix2.M13) && (matrix1.M14 == matrix2.M14) && + (matrix1.M21 == matrix2.M21) && (matrix1.M22 == matrix2.M22) && + (matrix1.M23 == matrix2.M23) && (matrix1.M24 == matrix2.M24) && + (matrix1.M31 == matrix2.M31) && (matrix1.M32 == matrix2.M32) && + (matrix1.M33 == matrix2.M33) && (matrix1.M34 == matrix2.M34) && + (matrix1.M41 == matrix2.M41) && (matrix1.M42 == matrix2.M42) && + (matrix1.M43 == matrix2.M43) && (matrix1.M44 == matrix2.M44); + } + + public static bool operator !=(Matrix matrix1, Matrix matrix2) + { + return !(matrix1 == matrix2); + } + + public static Matrix operator *(Matrix matrix1, Matrix matrix2) + { + Matrix returnMatrix = new Matrix(); + Multiply(ref matrix1, ref matrix2, out returnMatrix); + return returnMatrix; + } + + public static Matrix operator *(Matrix matrix, float scaleFactor) + { + Multiply(ref matrix, scaleFactor, out matrix); + return matrix; + } + + public static Matrix operator *(float scaleFactor, Matrix matrix) + { + Matrix target; + target.M11 = matrix.M11*scaleFactor; + target.M12 = matrix.M12*scaleFactor; + target.M13 = matrix.M13*scaleFactor; + target.M14 = matrix.M14*scaleFactor; + target.M21 = matrix.M21*scaleFactor; + target.M22 = matrix.M22*scaleFactor; + target.M23 = matrix.M23*scaleFactor; + target.M24 = matrix.M24*scaleFactor; + target.M31 = matrix.M31*scaleFactor; + target.M32 = matrix.M32*scaleFactor; + target.M33 = matrix.M33*scaleFactor; + target.M34 = matrix.M34*scaleFactor; + target.M41 = matrix.M41*scaleFactor; + target.M42 = matrix.M42*scaleFactor; + target.M43 = matrix.M43*scaleFactor; + target.M44 = matrix.M44*scaleFactor; + return target; + } + + public static Matrix operator -(Matrix matrix1, Matrix matrix2) + { + Matrix returnMatrix = new Matrix(); + Subtract(ref matrix1, ref matrix2, out returnMatrix); + return returnMatrix; + } + + + public static Matrix operator -(Matrix matrix1) + { + Negate(ref matrix1, out matrix1); + return matrix1; + } + +#endregion + +#region Object Overrides + + public override bool Equals(object obj) + { + return this == (Matrix) obj; + } + + public override int GetHashCode() + { + throw new NotImplementedException(); + } + + public override string ToString() + { + return "{ {M11:" + M11 + " M12:" + M12 + " M13:" + M13 + " M14:" + M14 + "}" + + " {M21:" + M21 + " M22:" + M22 + " M23:" + M23 + " M24:" + M24 + "}" + + " {M31:" + M31 + " M32:" + M32 + " M33:" + M33 + " M34:" + M34 + "}" + + " {M41:" + M41 + " M42:" + M42 + " M43:" + M43 + " M44:" + M44 + "} }"; + } + +#endregion + } +} + +#endif \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Path.cs b/Farseer Physics Engine 3.5/Common/Path.cs new file mode 100644 index 000000000..f8a8bf50d --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Path.cs @@ -0,0 +1,337 @@ +using System; +using System.Collections.Generic; +using System.Text; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common +{ + //Contributed by Matthew Bettcher + + /// + /// Path: + /// Very similar to Vertices, but this + /// class contains vectors describing + /// control points on a Catmull-Rom + /// curve. + /// + public class Path + { + /// + /// All the points that makes up the curve + /// + public List ControlPoints; + + private float _deltaT; + + /// + /// Initializes a new instance of the class. + /// + public Path() + { + ControlPoints = new List(); + } + + /// + /// Initializes a new instance of the class. + /// + /// The vertices to created the path from. + public Path(Vector2[] vertices) + { + ControlPoints = new List(vertices.Length); + + for (int i = 0; i < vertices.Length; i++) + { + Add(vertices[i]); + } + } + + /// + /// Initializes a new instance of the class. + /// + /// The vertices to created the path from. + public Path(IList vertices) + { + ControlPoints = new List(vertices.Count); + for (int i = 0; i < vertices.Count; i++) + { + Add(vertices[i]); + } + } + + /// + /// True if the curve is closed. + /// + /// true if closed; otherwise, false. + public bool Closed { get; set; } + + /// + /// Gets the next index of a controlpoint + /// + /// The index. + /// + public int NextIndex(int index) + { + if (index == ControlPoints.Count - 1) + { + return 0; + } + return index + 1; + } + + /// + /// Gets the previous index of a controlpoint + /// + /// The index. + /// + public int PreviousIndex(int index) + { + if (index == 0) + { + return ControlPoints.Count - 1; + } + return index - 1; + } + + /// + /// Translates the control points by the specified vector. + /// + /// The vector. + public void Translate(ref Vector2 vector) + { + for (int i = 0; i < ControlPoints.Count; i++) + ControlPoints[i] = Vector2.Add(ControlPoints[i], vector); + } + + /// + /// Scales the control points by the specified vector. + /// + /// The Value. + public void Scale(ref Vector2 value) + { + for (int i = 0; i < ControlPoints.Count; i++) + ControlPoints[i] = Vector2.Multiply(ControlPoints[i], value); + } + + /// + /// Rotate the control points by the defined value in radians. + /// + /// The amount to rotate by in radians. + public void Rotate(float value) + { + Matrix rotationMatrix; + Matrix.CreateRotationZ(value, out rotationMatrix); + + for (int i = 0; i < ControlPoints.Count; i++) + ControlPoints[i] = Vector2.Transform(ControlPoints[i], rotationMatrix); + } + + public override string ToString() + { + StringBuilder builder = new StringBuilder(); + for (int i = 0; i < ControlPoints.Count; i++) + { + builder.Append(ControlPoints[i].ToString()); + if (i < ControlPoints.Count - 1) + { + builder.Append(" "); + } + } + return builder.ToString(); + } + + /// + /// Returns a set of points defining the + /// curve with the specifed number of divisions + /// between each control point. + /// + /// Number of divisions between each control point. + /// + public Vertices GetVertices(int divisions) + { + Vertices verts = new Vertices(); + + float timeStep = 1f / divisions; + + for (float i = 0; i < 1f; i += timeStep) + { + verts.Add(GetPosition(i)); + } + + return verts; + } + + public Vector2 GetPosition(float time) + { + Vector2 temp; + + if (ControlPoints.Count < 2) + throw new Exception("You need at least 2 control points to calculate a position."); + + if (Closed) + { + Add(ControlPoints[0]); + + _deltaT = 1f / (ControlPoints.Count - 1); + + int p = (int)(time / _deltaT); + + // use a circular indexing system + int p0 = p - 1; + if (p0 < 0) p0 = p0 + (ControlPoints.Count - 1); + else if (p0 >= ControlPoints.Count - 1) p0 = p0 - (ControlPoints.Count - 1); + int p1 = p; + if (p1 < 0) p1 = p1 + (ControlPoints.Count - 1); + else if (p1 >= ControlPoints.Count - 1) p1 = p1 - (ControlPoints.Count - 1); + int p2 = p + 1; + if (p2 < 0) p2 = p2 + (ControlPoints.Count - 1); + else if (p2 >= ControlPoints.Count - 1) p2 = p2 - (ControlPoints.Count - 1); + int p3 = p + 2; + if (p3 < 0) p3 = p3 + (ControlPoints.Count - 1); + else if (p3 >= ControlPoints.Count - 1) p3 = p3 - (ControlPoints.Count - 1); + + // relative time + float lt = (time - _deltaT * p) / _deltaT; + + temp = Vector2.CatmullRom(ControlPoints[p0], ControlPoints[p1], ControlPoints[p2], ControlPoints[p3], lt); + + RemoveAt(ControlPoints.Count - 1); + } + else + { + int p = (int)(time / _deltaT); + + // + int p0 = p - 1; + if (p0 < 0) p0 = 0; + else if (p0 >= ControlPoints.Count - 1) p0 = ControlPoints.Count - 1; + int p1 = p; + if (p1 < 0) p1 = 0; + else if (p1 >= ControlPoints.Count - 1) p1 = ControlPoints.Count - 1; + int p2 = p + 1; + if (p2 < 0) p2 = 0; + else if (p2 >= ControlPoints.Count - 1) p2 = ControlPoints.Count - 1; + int p3 = p + 2; + if (p3 < 0) p3 = 0; + else if (p3 >= ControlPoints.Count - 1) p3 = ControlPoints.Count - 1; + + // relative time + float lt = (time - _deltaT * p) / _deltaT; + + temp = Vector2.CatmullRom(ControlPoints[p0], ControlPoints[p1], ControlPoints[p2], ControlPoints[p3], lt); + } + + return temp; + } + + /// + /// Gets the normal for the given time. + /// + /// The time + /// The normal. + public Vector2 GetPositionNormal(float time) + { + float offsetTime = time + 0.0001f; + + Vector2 a = GetPosition(time); + Vector2 b = GetPosition(offsetTime); + + Vector2 output, temp; + + Vector2.Subtract(ref a, ref b, out temp); + +#if (XBOX360 || WINDOWS_PHONE) +output = new Vector2(); +#endif + output.X = -temp.Y; + output.Y = temp.X; + + Vector2.Normalize(ref output, out output); + + return output; + } + + public void Add(Vector2 point) + { + ControlPoints.Add(point); + _deltaT = 1f / (ControlPoints.Count - 1); + } + + public void Remove(Vector2 point) + { + ControlPoints.Remove(point); + _deltaT = 1f / (ControlPoints.Count - 1); + } + + public void RemoveAt(int index) + { + ControlPoints.RemoveAt(index); + _deltaT = 1f / (ControlPoints.Count - 1); + } + + public float GetLength() + { + List verts = GetVertices(ControlPoints.Count * 25); + float length = 0; + + for (int i = 1; i < verts.Count; i++) + { + length += Vector2.Distance(verts[i - 1], verts[i]); + } + + if (Closed) + length += Vector2.Distance(verts[ControlPoints.Count - 1], verts[0]); + + return length; + } + + public List SubdivideEvenly(int divisions) + { + List verts = new List(); + + float length = GetLength(); + + float deltaLength = length / divisions + 0.001f; + float t = 0.000f; + + // we always start at the first control point + Vector2 start = ControlPoints[0]; + Vector2 end = GetPosition(t); + + // increment t until we are at half the distance + while (deltaLength * 0.5f >= Vector2.Distance(start, end)) + { + end = GetPosition(t); + t += 0.0001f; + + if (t >= 1f) + break; + } + + start = end; + + // for each box + for (int i = 1; i < divisions; i++) + { + Vector2 normal = GetPositionNormal(t); + float angle = (float)Math.Atan2(normal.Y, normal.X); + + verts.Add(new Vector3(end, angle)); + + // until we reach the correct distance down the curve + while (deltaLength >= Vector2.Distance(start, end)) + { + end = GetPosition(t); + t += 0.00001f; + + if (t >= 1f) + break; + } + if (t >= 1f) + break; + + start = end; + } + return verts; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/PathManager.cs b/Farseer Physics Engine 3.5/Common/PathManager.cs new file mode 100644 index 000000000..a6debff51 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/PathManager.cs @@ -0,0 +1,186 @@ +using System; +using System.Collections.Generic; +using FarseerPhysics.Collision.Shapes; +using FarseerPhysics.Common.Decomposition; +using FarseerPhysics.Dynamics; +using FarseerPhysics.Dynamics.Joints; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common +{ + /// + /// An easy to use manager for creating paths. + /// + public static class PathManager + { + #region LinkType enum + + public enum LinkType + { + Revolute, + Slider + } + + #endregion + + //Contributed by Matthew Bettcher + + /// + /// Convert a path into a set of edges and attaches them to the specified body. + /// Note: use only for static edges. + /// + /// The path. + /// The body. + /// The subdivisions. + public static void ConvertPathToEdges(Path path, Body body, int subdivisions) + { + Vertices verts = path.GetVertices(subdivisions); + + if (path.Closed) + { + ChainShape chain = new ChainShape(verts, true); + body.CreateFixture(chain); + } + else + { + for (int i = 1; i < verts.Count; i++) + { + body.CreateFixture(new EdgeShape(verts[i], verts[i - 1])); + } + } + } + + /// + /// Convert a closed path into a polygon. + /// Convex decomposition is automatically performed. + /// + /// The path. + /// The body. + /// The density. + /// The subdivisions. + public static void ConvertPathToPolygon(Path path, Body body, float density, int subdivisions) + { + if (!path.Closed) + throw new Exception("The path must be closed to convert to a polygon."); + + List verts = path.GetVertices(subdivisions); + + List decomposedVerts = Triangulate.ConvexPartition(new Vertices(verts), TriangulationAlgorithm.Bayazit); + + foreach (Vertices item in decomposedVerts) + { + body.CreateFixture(new PolygonShape(item, density)); + } + } + + /// + /// Duplicates the given Body along the given path for approximatly the given copies. + /// + /// The world. + /// The path. + /// The shapes. + /// The type. + /// The copies. + /// + /// + public static List EvenlyDistributeShapesAlongPath(World world, Path path, IEnumerable shapes, BodyType type, int copies, object userData = null) + { + List centers = path.SubdivideEvenly(copies); + List bodyList = new List(); + + for (int i = 0; i < centers.Count; i++) + { + Body b = new Body(world); + + // copy the type from original body + b.BodyType = type; + b.Position = new Vector2(centers[i].X, centers[i].Y); + b.Rotation = centers[i].Z; + b.UserData = userData; + + foreach (Shape shape in shapes) + { + b.CreateFixture(shape); + } + + bodyList.Add(b); + } + + return bodyList; + } + + + /// + /// Duplicates the given Body along the given path for approximatly the given copies. + /// + /// The world. + /// The path. + /// The shape. + /// The type. + /// The copies. + /// The user data. + /// + public static List EvenlyDistributeShapesAlongPath(World world, Path path, Shape shape, BodyType type, + int copies, object userData) + { + List shapes = new List(1); + shapes.Add(shape); + + return EvenlyDistributeShapesAlongPath(world, path, shapes, type, copies, userData); + } + + public static List EvenlyDistributeShapesAlongPath(World world, Path path, Shape shape, BodyType type, int copies) + { + return EvenlyDistributeShapesAlongPath(world, path, shape, type, copies, null); + } + + /// + /// Moves the given body along the defined path. + /// + /// The path. + /// The body. + /// The time. + /// The strength. + /// The time step. + public static void MoveBodyOnPath(Path path, Body body, float time, float strength, float timeStep) + { + Vector2 destination = path.GetPosition(time); + Vector2 positionDelta = body.Position - destination; + Vector2 velocity = (positionDelta / timeStep) * strength; + + body.LinearVelocity = -velocity; + } + + /// + /// Attaches the bodies with revolute joints. + /// + /// The world. + /// The bodies. + /// The local anchor A. + /// The local anchor B. + /// if set to true [connect first and last]. + /// if set to true [collide connected]. + public static List AttachBodiesWithRevoluteJoint(World world, List bodies, Vector2 localAnchorA, Vector2 localAnchorB, bool connectFirstAndLast, bool collideConnected) + { + List joints = new List(bodies.Count + 1); + + for (int i = 1; i < bodies.Count; i++) + { + RevoluteJoint joint = new RevoluteJoint(bodies[i], bodies[i - 1], localAnchorA, localAnchorB); + joint.CollideConnected = collideConnected; + world.AddJoint(joint); + joints.Add(joint); + } + + if (connectFirstAndLast) + { + RevoluteJoint lastjoint = new RevoluteJoint(bodies[0], bodies[bodies.Count - 1], localAnchorA, localAnchorB); + lastjoint.CollideConnected = collideConnected; + world.AddJoint(lastjoint); + joints.Add(lastjoint); + } + + return joints; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/PhysicsLogic/FilterData.cs b/Farseer Physics Engine 3.5/Common/PhysicsLogic/FilterData.cs new file mode 100644 index 000000000..4bc9fe82d --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/PhysicsLogic/FilterData.cs @@ -0,0 +1,133 @@ +using FarseerPhysics.Dynamics; + +namespace FarseerPhysics.Common.PhysicsLogic +{ + /// + /// Contains filter data that can determine whether an object should be processed or not. + /// + public abstract class FilterData + { + /// + /// Disable the logic on specific categories. + /// Category.None by default. + /// + public Category DisabledOnCategories = Category.None; + + /// + /// Disable the logic on specific groups + /// + public int DisabledOnGroup; + + /// + /// Enable the logic on specific categories + /// Category.All by default. + /// + public Category EnabledOnCategories = Category.All; + + /// + /// Enable the logic on specific groups. + /// + public int EnabledOnGroup; + + /// + /// + /// + /// + /// + public virtual bool IsActiveOn(Body body) + { + if (body == null || !body.Enabled || body.IsStatic) + return false; + + if (body.FixtureList == null) + return false; + + foreach (Fixture fixture in body.FixtureList) + { + //Disable + if ((fixture.CollisionGroup == DisabledOnGroup) && fixture.CollisionGroup != 0 && DisabledOnGroup != 0) + return false; + + if ((fixture.CollisionCategories & DisabledOnCategories) != Category.None) + return false; + + if (EnabledOnGroup != 0 || EnabledOnCategories != Category.All) + { + //Enable + if ((fixture.CollisionGroup == EnabledOnGroup) && fixture.CollisionGroup != 0 && EnabledOnGroup != 0) + return true; + + if ((fixture.CollisionCategories & EnabledOnCategories) != Category.None && + EnabledOnCategories != Category.All) + return true; + } + else + { + return true; + } + } + + return false; + } + + /// + /// Adds the category. + /// + /// The category. + public void AddDisabledCategory(Category category) + { + DisabledOnCategories |= category; + } + + /// + /// Removes the category. + /// + /// The category. + public void RemoveDisabledCategory(Category category) + { + DisabledOnCategories &= ~category; + } + + /// + /// Determines whether this body ignores the the specified controller. + /// + /// The category. + /// + /// true if the object has the specified category; otherwise, false. + /// + public bool IsInDisabledCategory(Category category) + { + return (DisabledOnCategories & category) == category; + } + + /// + /// Adds the category. + /// + /// The category. + public void AddEnabledCategory(Category category) + { + EnabledOnCategories |= category; + } + + /// + /// Removes the category. + /// + /// The category. + public void RemoveEnabledCategory(Category category) + { + EnabledOnCategories &= ~category; + } + + /// + /// Determines whether this body ignores the the specified controller. + /// + /// The category. + /// + /// true if the object has the specified category; otherwise, false. + /// + public bool IsInEnabledInCategory(Category category) + { + return (EnabledOnCategories & category) == category; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/PhysicsLogic/PhysicsLogic.cs b/Farseer Physics Engine 3.5/Common/PhysicsLogic/PhysicsLogic.cs new file mode 100644 index 000000000..a821e8878 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/PhysicsLogic/PhysicsLogic.cs @@ -0,0 +1,66 @@ +using System; +using FarseerPhysics.Dynamics; + +namespace FarseerPhysics.Common.PhysicsLogic +{ + [Flags] + public enum PhysicsLogicType + { + Explosion = (1 << 0) + } + + public struct PhysicsLogicFilter + { + public PhysicsLogicType ControllerIgnores; + + /// + /// Ignores the controller. The controller has no effect on this body. + /// + /// The logic type. + public void IgnorePhysicsLogic(PhysicsLogicType type) + { + ControllerIgnores |= type; + } + + /// + /// Restore the controller. The controller affects this body. + /// + /// The logic type. + public void RestorePhysicsLogic(PhysicsLogicType type) + { + ControllerIgnores &= ~type; + } + + /// + /// Determines whether this body ignores the the specified controller. + /// + /// The logic type. + /// + /// true if the body has the specified flag; otherwise, false. + /// + public bool IsPhysicsLogicIgnored(PhysicsLogicType type) + { + return (ControllerIgnores & type) == type; + } + } + + public abstract class PhysicsLogic : FilterData + { + private PhysicsLogicType _type; + public World World; + + public override bool IsActiveOn(Body body) + { + if (body.PhysicsLogicFilter.IsPhysicsLogicIgnored(_type)) + return false; + + return base.IsActiveOn(body); + } + + public PhysicsLogic(World world, PhysicsLogicType type) + { + _type = type; + World = world; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/PhysicsLogic/RealExplosion.cs b/Farseer Physics Engine 3.5/Common/PhysicsLogic/RealExplosion.cs new file mode 100644 index 000000000..8b95023c0 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/PhysicsLogic/RealExplosion.cs @@ -0,0 +1,418 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using FarseerPhysics.Collision; +using FarseerPhysics.Collision.Shapes; +using FarseerPhysics.Dynamics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common.PhysicsLogic +{ + // Original Code by Steven Lu - see http://www.box2d.org/forum/viewtopic.php?f=3&t=1688 + // Ported to Farseer 3.0 by Nicolás Hormazábal + + internal struct ShapeData + { + public Body Body; + public float Max; + public float Min; // absolute angles + } + + /// + /// This is a comprarer used for + /// detecting angle difference between rays + /// + internal class RayDataComparer : IComparer + { + #region IComparer Members + + int IComparer.Compare(float a, float b) + { + float diff = (a - b); + if (diff > 0) + return 1; + if (diff < 0) + return -1; + return 0; + } + + #endregion + } + + /* Methodology: + * Force applied at a ray is inversely proportional to the square of distance from source + * AABB is used to query for shapes that may be affected + * For each RIGID BODY (not shape -- this is an optimization) that is matched, loop through its vertices to determine + * the extreme points -- if there is structure that contains outlining polygon, use that as an additional optimization + * Evenly cast a number of rays against the shape - number roughly proportional to the arc coverage + * - Something like every 3 degrees should do the trick although this can be altered depending on the distance (if really close don't need such a high density of rays) + * - There should be a minimum number of rays (3-5?) applied to each body so that small bodies far away are still accurately modeled + * - Be sure to have the forces of each ray be proportional to the average arc length covered by each. + * For each ray that actually intersects with the shape (non intersections indicate something blocking the path of explosion): + * - Apply the appropriate force dotted with the negative of the collision normal at the collision point + * - Optionally apply linear interpolation between aforementioned Normal force and the original explosion force in the direction of ray to simulate "surface friction" of sorts + */ + + /// + /// Creates a realistic explosion based on raycasting. Objects in the open will be affected, but objects behind + /// static bodies will not. A body that is half in cover, half in the open will get half the force applied to the end in + /// the open. + /// + public sealed class RealExplosion : PhysicsLogic + { + /// + /// Two degrees: maximum angle from edges to first ray tested + /// + private const float MaxEdgeOffset = MathHelper.Pi / 90; + + /// + /// Ratio of arc length to angle from edges to first ray tested. + /// Defaults to 1/40. + /// + public float EdgeRatio = 1.0f / 40.0f; + + /// + /// Ignore Explosion if it happens inside a shape. + /// Default value is false. + /// + public bool IgnoreWhenInsideShape = false; + + /// + /// Max angle between rays (used when segment is large). + /// Defaults to 15 degrees + /// + public float MaxAngle = MathHelper.Pi / 15; + + /// + /// Maximum number of shapes involved in the explosion. + /// Defaults to 100 + /// + public int MaxShapes = 100; + + /// + /// How many rays per shape/body/segment. + /// Defaults to 5 + /// + public int MinRays = 5; + + private List _data = new List(); + private RayDataComparer _rdc; + + public RealExplosion(World world) + : base(world, PhysicsLogicType.Explosion) + { + _rdc = new RayDataComparer(); + _data = new List(); + } + + /// + /// Activate the explosion at the specified position. + /// + /// The position where the explosion happens + /// The explosion radius + /// The explosion force at the explosion point (then is inversely proportional to the square of the distance) + /// A list of bodies and the amount of force that was applied to them. + public Dictionary Activate(Vector2 pos, float radius, float maxForce) + { + AABB aabb; + aabb.LowerBound = pos + new Vector2(-radius, -radius); + aabb.UpperBound = pos + new Vector2(radius, radius); + Fixture[] shapes = new Fixture[MaxShapes]; + + // More than 5 shapes in an explosion could be possible, but still strange. + Fixture[] containedShapes = new Fixture[5]; + bool exit = false; + + int shapeCount = 0; + int containedShapeCount = 0; + + // Query the world for overlapping shapes. + World.QueryAABB( + fixture => + { + if (fixture.TestPoint(ref pos)) + { + if (IgnoreWhenInsideShape) + { + exit = true; + return false; + } + + containedShapes[containedShapeCount++] = fixture; + } + else + { + shapes[shapeCount++] = fixture; + } + + // Continue the query. + return true; + }, ref aabb); + + if (exit) + return new Dictionary(); + + Dictionary exploded = new Dictionary(shapeCount + containedShapeCount); + + // Per shape max/min angles for now. + float[] vals = new float[shapeCount * 2]; + int valIndex = 0; + for (int i = 0; i < shapeCount; ++i) + { + PolygonShape ps; + CircleShape cs = shapes[i].Shape as CircleShape; + if (cs != null) + { + // We create a "diamond" approximation of the circle + Vertices v = new Vertices(); + Vector2 vec = Vector2.Zero + new Vector2(cs.Radius, 0); + v.Add(vec); + vec = Vector2.Zero + new Vector2(0, cs.Radius); + v.Add(vec); + vec = Vector2.Zero + new Vector2(-cs.Radius, cs.Radius); + v.Add(vec); + vec = Vector2.Zero + new Vector2(0, -cs.Radius); + v.Add(vec); + ps = new PolygonShape(v, 0); + } + else + ps = shapes[i].Shape as PolygonShape; + + if ((shapes[i].Body.BodyType == BodyType.Dynamic) && ps != null) + { + Vector2 toCentroid = shapes[i].Body.GetWorldPoint(ps.MassData.Centroid) - pos; + float angleToCentroid = (float)Math.Atan2(toCentroid.Y, toCentroid.X); + float min = float.MaxValue; + float max = float.MinValue; + float minAbsolute = 0.0f; + float maxAbsolute = 0.0f; + + for (int j = 0; j < ps.Vertices.Count; ++j) + { + Vector2 toVertex = (shapes[i].Body.GetWorldPoint(ps.Vertices[j]) - pos); + float newAngle = (float)Math.Atan2(toVertex.Y, toVertex.X); + float diff = (newAngle - angleToCentroid); + + diff = (diff - MathHelper.Pi) % (2 * MathHelper.Pi); + // the minus pi is important. It means cutoff for going other direction is at 180 deg where it needs to be + + if (diff < 0.0f) + diff += 2 * MathHelper.Pi; // correction for not handling negs + + diff -= MathHelper.Pi; + + if (Math.Abs(diff) > MathHelper.Pi) + continue; // Something's wrong, point not in shape but exists angle diff > 180 + + if (diff > max) + { + max = diff; + maxAbsolute = newAngle; + } + if (diff < min) + { + min = diff; + minAbsolute = newAngle; + } + } + + vals[valIndex] = minAbsolute; + ++valIndex; + vals[valIndex] = maxAbsolute; + ++valIndex; + } + } + + Array.Sort(vals, 0, valIndex, _rdc); + _data.Clear(); + bool rayMissed = true; + + for (int i = 0; i < valIndex; ++i) + { + Fixture fixture = null; + float midpt; + + int iplus = (i == valIndex - 1 ? 0 : i + 1); + if (vals[i] == vals[iplus]) + continue; + + if (i == valIndex - 1) + { + // the single edgecase + midpt = (vals[0] + MathHelper.Pi * 2 + vals[i]); + } + else + { + midpt = (vals[i + 1] + vals[i]); + } + + midpt = midpt / 2; + + Vector2 p1 = pos; + Vector2 p2 = radius * new Vector2((float)Math.Cos(midpt), (float)Math.Sin(midpt)) + pos; + + // RaycastOne + bool hitClosest = false; + World.RayCast((f, p, n, fr) => + { + Body body = f.Body; + + if (!IsActiveOn(body)) + return 0; + + hitClosest = true; + fixture = f; + return fr; + }, p1, p2); + + //draws radius points + if ((hitClosest) && (fixture.Body.BodyType == BodyType.Dynamic)) + { + if ((_data.Any()) && (_data.Last().Body == fixture.Body) && (!rayMissed)) + { + int laPos = _data.Count - 1; + ShapeData la = _data[laPos]; + la.Max = vals[iplus]; + _data[laPos] = la; + } + else + { + // make new + ShapeData d; + d.Body = fixture.Body; + d.Min = vals[i]; + d.Max = vals[iplus]; + _data.Add(d); + } + + if ((_data.Count > 1) + && (i == valIndex - 1) + && (_data.Last().Body == _data.First().Body) + && (_data.Last().Max == _data.First().Min)) + { + ShapeData fi = _data[0]; + fi.Min = _data.Last().Min; + _data.RemoveAt(_data.Count - 1); + _data[0] = fi; + while (_data.First().Min >= _data.First().Max) + { + fi.Min -= MathHelper.Pi * 2; + _data[0] = fi; + } + } + + int lastPos = _data.Count - 1; + ShapeData last = _data[lastPos]; + while ((_data.Count > 0) + && (_data.Last().Min >= _data.Last().Max)) // just making sure min fl = _data[i].Body.FixtureList; + for (int x = 0; x < fl.Count; x++) + { + Fixture f = fl[x]; + RayCastInput ri; + ri.Point1 = p1; + ri.Point2 = p2; + ri.MaxFraction = 50f; + + RayCastOutput ro; + if (f.RayCast(out ro, ref ri, 0)) + { + if (minlambda > ro.Fraction) + { + minlambda = ro.Fraction; + hitpoint = ro.Fraction * p2 + (1 - ro.Fraction) * p1; + } + } + + // the force that is to be applied for this particular ray. + // offset is angular coverage. lambda*length of segment is distance. + float impulse = (arclen / (MinRays + insertedRays)) * maxForce * 180.0f / MathHelper.Pi * (1.0f - Math.Min(1.0f, minlambda)); + + // We Apply the impulse!!! + Vector2 vectImp = Vector2.Dot(impulse * new Vector2((float)Math.Cos(j), (float)Math.Sin(j)), -ro.Normal) * new Vector2((float)Math.Cos(j), (float)Math.Sin(j)); + _data[i].Body.ApplyLinearImpulse(ref vectImp, ref hitpoint); + + // We gather the fixtures for returning them + if (exploded.ContainsKey(f)) + exploded[f] += vectImp; + else + exploded.Add(f, vectImp); + + if (minlambda > 1.0f) + hitpoint = p2; + } + } + } + + // We check contained shapes + for (int i = 0; i < containedShapeCount; ++i) + { + Fixture fix = containedShapes[i]; + + if (!IsActiveOn(fix.Body)) + continue; + + float impulse = MinRays * maxForce * 180.0f / MathHelper.Pi; + Vector2 hitPoint; + + CircleShape circShape = fix.Shape as CircleShape; + if (circShape != null) + { + hitPoint = fix.Body.GetWorldPoint(circShape.Position); + } + else + { + PolygonShape shape = fix.Shape as PolygonShape; + hitPoint = fix.Body.GetWorldPoint(shape.MassData.Centroid); + } + + Vector2 vectImp = impulse * (hitPoint - pos); + + fix.Body.ApplyLinearImpulse(ref vectImp, ref hitPoint); + + if (!exploded.ContainsKey(fix)) + exploded.Add(fix, vectImp); + } + + return exploded; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/PhysicsLogic/SimpleExplosion.cs b/Farseer Physics Engine 3.5/Common/PhysicsLogic/SimpleExplosion.cs new file mode 100644 index 000000000..448318d00 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/PhysicsLogic/SimpleExplosion.cs @@ -0,0 +1,92 @@ +using System; +using System.Collections.Generic; +using FarseerPhysics.Collision; +using FarseerPhysics.Dynamics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common.PhysicsLogic +{ + /// + /// Creates a simple explosion that ignores other bodies hiding behind static bodies. + /// + public sealed class SimpleExplosion : PhysicsLogic + { + public SimpleExplosion(World world) + : base(world, PhysicsLogicType.Explosion) + { + Power = 1; //linear + } + + /// + /// This is the power used in the power function. A value of 1 means the force + /// applied to bodies in the explosion is linear. A value of 2 means it is exponential. + /// + public float Power { get; set; } + + /// + /// Activate the explosion at the specified position. + /// + /// The position (center) of the explosion. + /// The radius of the explosion. + /// The force applied + /// A maximum amount of force. When force gets over this value, it will be equal to maxForce + /// A list of bodies and the amount of force that was applied to them. + public Dictionary Activate(Vector2 pos, float radius, float force, float maxForce = float.MaxValue) + { + HashSet affectedBodies = new HashSet(); + + AABB aabb; + aabb.LowerBound = pos - new Vector2(radius); + aabb.UpperBound = pos + new Vector2(radius); + + // Query the world for bodies within the radius. + World.QueryAABB(fixture => + { + if (Vector2.Distance(fixture.Body.Position, pos) <= radius) + { + if (!affectedBodies.Contains(fixture.Body)) + affectedBodies.Add(fixture.Body); + } + + return true; + }, ref aabb); + + return ApplyImpulse(pos, radius, force, maxForce, affectedBodies); + } + + private Dictionary ApplyImpulse(Vector2 pos, float radius, float force, float maxForce, HashSet overlappingBodies) + { + Dictionary forces = new Dictionary(overlappingBodies.Count); + + foreach (Body overlappingBody in overlappingBodies) + { + if (IsActiveOn(overlappingBody)) + { + float distance = Vector2.Distance(pos, overlappingBody.Position); + float forcePercent = GetPercent(distance, radius); + + Vector2 forceVector = pos - overlappingBody.Position; + forceVector *= 1f / (float)Math.Sqrt(forceVector.X * forceVector.X + forceVector.Y * forceVector.Y); + forceVector *= MathHelper.Min(force * forcePercent, maxForce); + forceVector *= -1; + + overlappingBody.ApplyLinearImpulse(forceVector); + forces.Add(overlappingBody, forceVector); + } + } + + return forces; + } + + private float GetPercent(float distance, float radius) + { + //(1-(distance/radius))^power-1 + float percent = (float)Math.Pow(1 - ((distance - radius) / radius), Power) - 1; + + if (float.IsNaN(percent)) + return 0f; + + return MathHelper.Clamp(percent, 0f, 1f); + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/PolygonManipulation/CuttingTools.cs b/Farseer Physics Engine 3.5/Common/PolygonManipulation/CuttingTools.cs new file mode 100644 index 000000000..d3cc188e8 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/PolygonManipulation/CuttingTools.cs @@ -0,0 +1,217 @@ +using System.Collections.Generic; +using System.Diagnostics; +using FarseerPhysics.Collision.Shapes; +using FarseerPhysics.Dynamics; +using FarseerPhysics.Factories; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common.PolygonManipulation +{ + public static class CuttingTools + { + //Cutting a shape into two is based on the work of Daid and his prototype BoxCutter: http://www.box2d.org/forum/viewtopic.php?f=3&t=1473 + + /// + /// Split a fixture into 2 vertice collections using the given entry and exit-point. + /// + /// The Fixture to split + /// The entry point - The start point + /// The exit point - The end point + /// The first collection of vertexes + /// The second collection of vertexes + public static void SplitShape(Fixture fixture, Vector2 entryPoint, Vector2 exitPoint, out Vertices first, out Vertices second) + { + Vector2 localEntryPoint = fixture.Body.GetLocalPoint(ref entryPoint); + Vector2 localExitPoint = fixture.Body.GetLocalPoint(ref exitPoint); + + PolygonShape shape = fixture.Shape as PolygonShape; + + //We can only cut polygons at the moment + if (shape == null) + { + first = new Vertices(); + second = new Vertices(); + return; + } + + //Offset the entry and exit points if they are too close to the vertices + foreach (Vector2 vertex in shape.Vertices) + { + if (vertex.Equals(localEntryPoint)) + localEntryPoint -= new Vector2(0, Settings.Epsilon); + + if (vertex.Equals(localExitPoint)) + localExitPoint += new Vector2(0, Settings.Epsilon); + } + + Vertices vertices = new Vertices(shape.Vertices); + Vertices[] newPolygon = new Vertices[2]; + + for (int i = 0; i < newPolygon.Length; i++) + { + newPolygon[i] = new Vertices(vertices.Count); + } + + int[] cutAdded = { -1, -1 }; + int last = -1; + for (int i = 0; i < vertices.Count; i++) + { + int n; + //Find out if this vertex is on the old or new shape. + if (Vector2.Dot(MathUtils.Cross(localExitPoint - localEntryPoint, 1), vertices[i] - localEntryPoint) > Settings.Epsilon) + n = 0; + else + n = 1; + + if (last != n) + { + //If we switch from one shape to the other add the cut vertices. + if (last == 0) + { + Debug.Assert(cutAdded[0] == -1); + cutAdded[0] = newPolygon[last].Count; + newPolygon[last].Add(localExitPoint); + newPolygon[last].Add(localEntryPoint); + } + if (last == 1) + { + Debug.Assert(cutAdded[last] == -1); + cutAdded[last] = newPolygon[last].Count; + newPolygon[last].Add(localEntryPoint); + newPolygon[last].Add(localExitPoint); + } + } + + newPolygon[n].Add(vertices[i]); + last = n; + } + + //Add the cut in case it has not been added yet. + if (cutAdded[0] == -1) + { + cutAdded[0] = newPolygon[0].Count; + newPolygon[0].Add(localExitPoint); + newPolygon[0].Add(localEntryPoint); + } + if (cutAdded[1] == -1) + { + cutAdded[1] = newPolygon[1].Count; + newPolygon[1].Add(localEntryPoint); + newPolygon[1].Add(localExitPoint); + } + + for (int n = 0; n < 2; n++) + { + Vector2 offset; + if (cutAdded[n] > 0) + { + offset = (newPolygon[n][cutAdded[n] - 1] - newPolygon[n][cutAdded[n]]); + } + else + { + offset = (newPolygon[n][newPolygon[n].Count - 1] - newPolygon[n][0]); + } + offset.Normalize(); + + if (!offset.IsValid()) + offset = Vector2.One; + + newPolygon[n][cutAdded[n]] += Settings.Epsilon * offset; + + if (cutAdded[n] < newPolygon[n].Count - 2) + { + offset = (newPolygon[n][cutAdded[n] + 2] - newPolygon[n][cutAdded[n] + 1]); + } + else + { + offset = (newPolygon[n][0] - newPolygon[n][newPolygon[n].Count - 1]); + } + offset.Normalize(); + + if (!offset.IsValid()) + offset = Vector2.One; + + newPolygon[n][cutAdded[n] + 1] += Settings.Epsilon * offset; + } + + first = newPolygon[0]; + second = newPolygon[1]; + } + + /// + /// This is a high-level function to cuts fixtures inside the given world, using the start and end points. + /// Note: We don't support cutting when the start or end is inside a shape. + /// + /// The world. + /// The startpoint. + /// The endpoint. + /// True if the cut was performed. + public static bool Cut(World world, Vector2 start, Vector2 end) + { + List fixtures = new List(); + List entryPoints = new List(); + List exitPoints = new List(); + + //We don't support cutting when the start or end is inside a shape. + if (world.TestPoint(start) != null || world.TestPoint(end) != null) + return false; + + //Get the entry points + world.RayCast((f, p, n, fr) => + { + fixtures.Add(f); + entryPoints.Add(p); + return 1; + }, start, end); + + //Reverse the ray to get the exitpoints + world.RayCast((f, p, n, fr) => + { + exitPoints.Add(p); + return 1; + }, end, start); + + //We only have a single point. We need at least 2 + if (entryPoints.Count + exitPoints.Count < 2) + return false; + + for (int i = 0; i < fixtures.Count; i++) + { + // can't cut circles or edges yet ! + if (fixtures[i].Shape.ShapeType != ShapeType.Polygon) + continue; + + if (fixtures[i].Body.BodyType != BodyType.Static) + { + //Split the shape up into two shapes + Vertices first; + Vertices second; + SplitShape(fixtures[i], entryPoints[i], exitPoints[i], out first, out second); + + //Delete the original shape and create two new. Retain the properties of the body. + if (first.CheckPolygon() == PolygonError.NoError) + { + Body firstFixture = BodyFactory.CreatePolygon(world, first, fixtures[i].Shape.Density, fixtures[i].Body.Position); + firstFixture.Rotation = fixtures[i].Body.Rotation; + firstFixture.LinearVelocity = fixtures[i].Body.LinearVelocity; + firstFixture.AngularVelocity = fixtures[i].Body.AngularVelocity; + firstFixture.BodyType = BodyType.Dynamic; + } + + if (second.CheckPolygon() == PolygonError.NoError) + { + Body secondFixture = BodyFactory.CreatePolygon(world, second, fixtures[i].Shape.Density, fixtures[i].Body.Position); + secondFixture.Rotation = fixtures[i].Body.Rotation; + secondFixture.LinearVelocity = fixtures[i].Body.LinearVelocity; + secondFixture.AngularVelocity = fixtures[i].Body.AngularVelocity; + secondFixture.BodyType = BodyType.Dynamic; + } + + world.RemoveBody(fixtures[i].Body); + } + } + + return true; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/PolygonManipulation/SimpleCombiner.cs b/Farseer Physics Engine 3.5/Common/PolygonManipulation/SimpleCombiner.cs new file mode 100644 index 000000000..581ed0680 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/PolygonManipulation/SimpleCombiner.cs @@ -0,0 +1,226 @@ +/* +* C# Version Ported by Matt Bettcher and Ian Qvist 2009-2010 +* +* Original C++ Version Copyright (c) 2007 Eric Jordan +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System.Collections.Generic; +using System.Diagnostics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common.PolygonManipulation +{ + /// + /// Combines a list of triangles into a list of convex polygons. + /// Starts with a seed triangle, keep adding triangles to it until you can't add any more without making the polygon non-convex. + /// + public static class SimpleCombiner + { + /// + /// Combine a list of triangles into a list of convex polygons. + /// + /// Note: This only works on triangles. + /// + ///The triangles. + ///The maximun number of polygons to return. + ///The tolerance + public static List PolygonizeTriangles(List triangles, int maxPolys = int.MaxValue, float tolerance = 0.001f) + { + if (triangles.Count <= 0) + return triangles; + + List polys = new List(); + + bool[] covered = new bool[triangles.Count]; + for (int i = 0; i < triangles.Count; ++i) + { + covered[i] = false; + + //Check here for degenerate triangles + Vertices triangle = triangles[i]; + Vector2 a = triangle[0]; + Vector2 b = triangle[1]; + Vector2 c = triangle[2]; + + if ((a.X == b.X && a.Y == b.Y) || (b.X == c.X && b.Y == c.Y) || (a.X == c.X && a.Y == c.Y)) + covered[i] = true; + } + + int polyIndex = 0; + + bool notDone = true; + while (notDone) + { + int currTri = -1; + for (int i = 0; i < triangles.Count; ++i) + { + if (covered[i]) + continue; + + currTri = i; + break; + } + + if (currTri == -1) + { + notDone = false; + } + else + { + Vertices poly = new Vertices(3); + + for (int i = 0; i < 3; i++) + { + poly.Add(triangles[currTri][i]); + } + + covered[currTri] = true; + int index = 0; + for (int i = 0; i < 2 * triangles.Count; ++i, ++index) + { + while (index >= triangles.Count) index -= triangles.Count; + if (covered[index]) + { + continue; + } + Vertices newP = AddTriangle(triangles[index], poly); + if (newP == null) + continue; // is this right + + if (newP.Count > Settings.MaxPolygonVertices) + continue; + + if (newP.IsConvex()) + { + //Or should it be IsUsable? Maybe re-write IsConvex to apply the angle threshold from Box2d + poly = new Vertices(newP); + covered[index] = true; + } + } + + //We have a maximum of polygons that we need to keep under. + if (polyIndex < maxPolys) + { + SimplifyTools.MergeParallelEdges(poly, tolerance); + + //If identical points are present, a triangle gets + //borked by the MergeParallelEdges function, hence + //the vertex number check + if (poly.Count >= 3) + polys.Add(new Vertices(poly)); + else + Debug.WriteLine("Skipping corrupt poly."); + } + + if (poly.Count >= 3) + polyIndex++; //Must be outside (polyIndex < polysLength) test + } + } + + //TODO: Add sanity check + //Remove empty vertice collections + for (int i = polys.Count - 1; i >= 0; i--) + { + if (polys[i].Count == 0) + polys.RemoveAt(i); + } + + return polys; + } + + private static Vertices AddTriangle(Vertices t, Vertices vertices) + { + // First, find vertices that connect + int firstP = -1; + int firstT = -1; + int secondP = -1; + int secondT = -1; + for (int i = 0; i < vertices.Count; i++) + { + if (t[0].X == vertices[i].X && t[0].Y == vertices[i].Y) + { + if (firstP == -1) + { + firstP = i; + firstT = 0; + } + else + { + secondP = i; + secondT = 0; + } + } + else if (t[1].X == vertices[i].X && t[1].Y == vertices[i].Y) + { + if (firstP == -1) + { + firstP = i; + firstT = 1; + } + else + { + secondP = i; + secondT = 1; + } + } + else if (t[2].X == vertices[i].X && t[2].Y == vertices[i].Y) + { + if (firstP == -1) + { + firstP = i; + firstT = 2; + } + else + { + secondP = i; + secondT = 2; + } + } + } + // Fix ordering if first should be last vertex of poly + if (firstP == 0 && secondP == vertices.Count - 1) + { + firstP = vertices.Count - 1; + secondP = 0; + } + + // Didn't find it + if (secondP == -1) + { + return null; + } + + // Find tip index on triangle + int tipT = 0; + if (tipT == firstT || tipT == secondT) + tipT = 1; + if (tipT == firstT || tipT == secondT) + tipT = 2; + + Vertices result = new Vertices(vertices.Count + 1); + for (int i = 0; i < vertices.Count; i++) + { + result.Add(vertices[i]); + + if (i == firstP) + result.Add(t[tipT]); + } + + return result; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/PolygonManipulation/SimplifyTools.cs b/Farseer Physics Engine 3.5/Common/PolygonManipulation/SimplifyTools.cs new file mode 100644 index 000000000..bed75e012 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/PolygonManipulation/SimplifyTools.cs @@ -0,0 +1,302 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common.PolygonManipulation +{ + /// + /// Provides a set of tools to simplify polygons in various ways. + /// + public static class SimplifyTools + { + /// + /// Removes all collinear points on the polygon. + /// + /// The polygon that needs simplification. + /// The collinearity tolerance. + /// A simplified polygon. + public static Vertices CollinearSimplify(Vertices vertices, float collinearityTolerance = 0) + { + if (vertices.Count <= 3) + return vertices; + + Vertices simplified = new Vertices(vertices.Count); + + for (int i = 0; i < vertices.Count; i++) + { + Vector2 prev = vertices.PreviousVertex(i); + Vector2 current = vertices[i]; + Vector2 next = vertices.NextVertex(i); + + //If they collinear, continue + if (MathUtils.IsCollinear(ref prev, ref current, ref next, collinearityTolerance)) + continue; + + simplified.Add(current); + } + + return simplified; + } + + /// + /// Ramer-Douglas-Peucker polygon simplification algorithm. This is the general recursive version that does not use the + /// speed-up technique by using the Melkman convex hull. + /// + /// If you pass in 0, it will remove all collinear points. + /// + /// The simplified polygon + public static Vertices DouglasPeuckerSimplify(Vertices vertices, float distanceTolerance) + { + if (vertices.Count <= 3) + return vertices; + + bool[] usePoint = new bool[vertices.Count]; + + for (int i = 0; i < vertices.Count; i++) + usePoint[i] = true; + + SimplifySection(vertices, 0, vertices.Count - 1, usePoint, distanceTolerance); + + Vertices simplified = new Vertices(vertices.Count); + + for (int i = 0; i < vertices.Count; i++) + { + if (usePoint[i]) + simplified.Add(vertices[i]); + } + + return simplified; + } + + private static void SimplifySection(Vertices vertices, int i, int j, bool[] usePoint, float distanceTolerance) + { + if ((i + 1) == j) + return; + + Vector2 a = vertices[i]; + Vector2 b = vertices[j]; + + double maxDistance = -1.0; + int maxIndex = i; + for (int k = i + 1; k < j; k++) + { + Vector2 point = vertices[k]; + + double distance = LineTools.DistanceBetweenPointAndLineSegment(ref point, ref a, ref b); + + if (distance > maxDistance) + { + maxDistance = distance; + maxIndex = k; + } + } + + if (maxDistance <= distanceTolerance) + { + for (int k = i + 1; k < j; k++) + { + usePoint[k] = false; + } + } + else + { + SimplifySection(vertices, i, maxIndex, usePoint, distanceTolerance); + SimplifySection(vertices, maxIndex, j, usePoint, distanceTolerance); + } + } + + /// + /// Merges all parallel edges in the list of vertices + /// + /// The vertices. + /// The tolerance. + public static Vertices MergeParallelEdges(Vertices vertices, float tolerance) + { + //From Eric Jordan's convex decomposition library + + if (vertices.Count <= 3) + return vertices; //Can't do anything useful here to a triangle + + bool[] mergeMe = new bool[vertices.Count]; + int newNVertices = vertices.Count; + + //Gather points to process + for (int i = 0; i < vertices.Count; ++i) + { + int lower = (i == 0) ? (vertices.Count - 1) : (i - 1); + int middle = i; + int upper = (i == vertices.Count - 1) ? (0) : (i + 1); + + float dx0 = vertices[middle].X - vertices[lower].X; + float dy0 = vertices[middle].Y - vertices[lower].Y; + float dx1 = vertices[upper].Y - vertices[middle].X; + float dy1 = vertices[upper].Y - vertices[middle].Y; + float norm0 = (float)Math.Sqrt(dx0 * dx0 + dy0 * dy0); + float norm1 = (float)Math.Sqrt(dx1 * dx1 + dy1 * dy1); + + if (!(norm0 > 0.0f && norm1 > 0.0f) && newNVertices > 3) + { + //Merge identical points + mergeMe[i] = true; + --newNVertices; + } + + dx0 /= norm0; + dy0 /= norm0; + dx1 /= norm1; + dy1 /= norm1; + float cross = dx0 * dy1 - dx1 * dy0; + float dot = dx0 * dx1 + dy0 * dy1; + + if (Math.Abs(cross) < tolerance && dot > 0 && newNVertices > 3) + { + mergeMe[i] = true; + --newNVertices; + } + else + mergeMe[i] = false; + } + + if (newNVertices == vertices.Count || newNVertices == 0) + return vertices; + + int currIndex = 0; + + //Copy the vertices to a new list and clear the old + Vertices newVertices = new Vertices(newNVertices); + + for (int i = 0; i < vertices.Count; ++i) + { + if (mergeMe[i] || newNVertices == 0 || currIndex == newNVertices) + continue; + + Debug.Assert(currIndex < newNVertices); + + newVertices.Add(vertices[i]); + ++currIndex; + } + + return newVertices; + } + + /// + /// Merges the identical points in the polygon. + /// + /// The vertices. + public static Vertices MergeIdenticalPoints(Vertices vertices) + { + HashSet unique = new HashSet(); + + foreach (Vector2 vertex in vertices) + { + unique.Add(vertex); + } + + return new Vertices(unique); + } + + /// + /// Reduces the polygon by distance. + /// + /// The vertices. + /// The distance between points. Points closer than this will be removed. + public static Vertices ReduceByDistance(Vertices vertices, float distance) + { + if (vertices.Count <= 3) + return vertices; + + float distance2 = distance * distance; + + Vertices simplified = new Vertices(vertices.Count); + + for (int i = 0; i < vertices.Count; i++) + { + Vector2 current = vertices[i]; + Vector2 next = vertices.NextVertex(i); + + //If they are closer than the distance, continue + if ((next - current).LengthSquared() <= distance2) + continue; + + simplified.Add(current); + } + + return simplified; + } + + /// + /// Reduces the polygon by removing the Nth vertex in the vertices list. + /// + /// The vertices. + /// The Nth point to remove. Example: 5. + /// + public static Vertices ReduceByNth(Vertices vertices, int nth) + { + if (vertices.Count <= 3) + return vertices; + + if (nth == 0) + return vertices; + + Vertices simplified = new Vertices(vertices.Count); + + for (int i = 0; i < vertices.Count; i++) + { + if (i % nth == 0) + continue; + + simplified.Add(vertices[i]); + } + + return simplified; + } + + /// + /// Simplify the polygon by removing all points that in pairs of 3 have an area less than the tolerance. + /// + /// Pass in 0 as tolerance, and it will only remove collinear points. + /// + /// + /// + /// + public static Vertices ReduceByArea(Vertices vertices, float areaTolerance) + { + //From physics2d.net + + if (vertices.Count <= 3) + return vertices; + + if (areaTolerance < 0) + throw new ArgumentOutOfRangeException("areaTolerance", "must be equal to or greater than zero."); + + Vertices simplified = new Vertices(vertices.Count); + Vector2 v3; + Vector2 v1 = vertices[vertices.Count - 2]; + Vector2 v2 = vertices[vertices.Count - 1]; + areaTolerance *= 2; + + for (int i = 0; i < vertices.Count; ++i, v2 = v3) + { + v3 = i == vertices.Count - 1 ? simplified[0] : vertices[i]; + + float old1; + MathUtils.Cross(ref v1, ref v2, out old1); + + float old2; + MathUtils.Cross(ref v2, ref v3, out old2); + + float new1; + MathUtils.Cross(ref v1, ref v3, out new1); + + if (Math.Abs(new1 - (old1 + old2)) > areaTolerance) + { + simplified.Add(v2); + v1 = v2; + } + } + + return simplified; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/PolygonManipulation/YuPengClipper.cs b/Farseer Physics Engine 3.5/Common/PolygonManipulation/YuPengClipper.cs new file mode 100644 index 000000000..5da98b34b --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/PolygonManipulation/YuPengClipper.cs @@ -0,0 +1,514 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common.PolygonManipulation +{ + internal enum PolyClipType + { + Intersect, + Union, + Difference + } + + public enum PolyClipError + { + None, + DegeneratedOutput, + NonSimpleInput, + BrokenResult + } + + //Clipper contributed by Helge Backhaus + + public static class YuPengClipper + { + private const float ClipperEpsilonSquared = 1.192092896e-07f; + + public static List Union(Vertices polygon1, Vertices polygon2, out PolyClipError error) + { + return Execute(polygon1, polygon2, PolyClipType.Union, out error); + } + + public static List Difference(Vertices polygon1, Vertices polygon2, out PolyClipError error) + { + return Execute(polygon1, polygon2, PolyClipType.Difference, out error); + } + + public static List Intersect(Vertices polygon1, Vertices polygon2, out PolyClipError error) + { + return Execute(polygon1, polygon2, PolyClipType.Intersect, out error); + } + + /// + /// Implements "A new algorithm for Boolean operations on general polygons" + /// available here: http://liama.ia.ac.cn/wiki/_media/user:dong:dong_cg_05.pdf + /// Merges two polygons, a subject and a clip with the specified operation. Polygons may not be + /// self-intersecting. + /// + /// Warning: May yield incorrect results or even crash if polygons contain collinear points. + /// + /// The subject polygon. + /// The clip polygon, which is added, + /// substracted or intersected with the subject + /// The operation to be performed. Either + /// Union, Difference or Intersection. + /// The error generated (if any) + /// A list of closed polygons, which make up the result of the clipping operation. + /// Outer contours are ordered counter clockwise, holes are ordered clockwise. + private static List Execute(Vertices subject, Vertices clip, PolyClipType clipType, out PolyClipError error) + { + Debug.Assert(subject.IsSimple() && clip.IsSimple(), "Non simple input!", "Input polygons must be simple (cannot intersect themselves)."); + + // Copy polygons + Vertices slicedSubject; + Vertices slicedClip; + // Calculate the intersection and touch points between + // subject and clip and add them to both + CalculateIntersections(subject, clip, out slicedSubject, out slicedClip); + + // Translate polygons into upper right quadrant + // as the algorithm depends on it + Vector2 lbSubject = subject.GetAABB().LowerBound; + Vector2 lbClip = clip.GetAABB().LowerBound; + Vector2 translate; + Vector2.Min(ref lbSubject, ref lbClip, out translate); + translate = Vector2.One - translate; + if (translate != Vector2.Zero) + { + slicedSubject.Translate(ref translate); + slicedClip.Translate(ref translate); + } + + // Enforce counterclockwise contours + slicedSubject.ForceCounterClockWise(); + slicedClip.ForceCounterClockWise(); + + List subjectSimplices; + List subjectCoeff; + List clipSimplices; + List clipCoeff; + // Build simplical chains from the polygons and calculate the + // the corresponding coefficients + CalculateSimplicalChain(slicedSubject, out subjectCoeff, out subjectSimplices); + CalculateSimplicalChain(slicedClip, out clipCoeff, out clipSimplices); + + List resultSimplices; + + // Determine the characteristics function for all non-original edges + // in subject and clip simplical chain and combine the edges contributing + // to the result, depending on the clipType + CalculateResultChain(subjectCoeff, subjectSimplices, clipCoeff, clipSimplices, clipType, + out resultSimplices); + + List result; + // Convert result chain back to polygon(s) + error = BuildPolygonsFromChain(resultSimplices, out result); + + // Reverse the polygon translation from the beginning + // and remove collinear points from output + translate *= -1f; + for (int i = 0; i < result.Count; ++i) + { + result[i].Translate(ref translate); + SimplifyTools.CollinearSimplify(result[i]); + } + return result; + } + + /// + /// Calculates all intersections between two polygons. + /// + /// The first polygon. + /// The second polygon. + /// Returns the first polygon with added intersection points. + /// Returns the second polygon with added intersection points. + private static void CalculateIntersections(Vertices polygon1, Vertices polygon2, + out Vertices slicedPoly1, out Vertices slicedPoly2) + { + slicedPoly1 = new Vertices(polygon1); + slicedPoly2 = new Vertices(polygon2); + + // Iterate through polygon1's edges + for (int i = 0; i < polygon1.Count; i++) + { + // Get edge vertices + Vector2 a = polygon1[i]; + Vector2 b = polygon1[polygon1.NextIndex(i)]; + + // Get intersections between this edge and polygon2 + for (int j = 0; j < polygon2.Count; j++) + { + Vector2 c = polygon2[j]; + Vector2 d = polygon2[polygon2.NextIndex(j)]; + + Vector2 intersectionPoint; + // Check if the edges intersect + if (LineTools.LineIntersect(a, b, c, d, out intersectionPoint)) + { + // calculate alpha values for sorting multiple intersections points on a edge + float alpha; + // Insert intersection point into first polygon + alpha = GetAlpha(a, b, intersectionPoint); + if (alpha > 0f && alpha < 1f) + { + int index = slicedPoly1.IndexOf(a) + 1; + while (index < slicedPoly1.Count && + GetAlpha(a, b, slicedPoly1[index]) <= alpha) + { + ++index; + } + slicedPoly1.Insert(index, intersectionPoint); + } + // Insert intersection point into second polygon + alpha = GetAlpha(c, d, intersectionPoint); + if (alpha > 0f && alpha < 1f) + { + int index = slicedPoly2.IndexOf(c) + 1; + while (index < slicedPoly2.Count && + GetAlpha(c, d, slicedPoly2[index]) <= alpha) + { + ++index; + } + slicedPoly2.Insert(index, intersectionPoint); + } + } + } + } + // Check for very small edges + for (int i = 0; i < slicedPoly1.Count; ++i) + { + int iNext = slicedPoly1.NextIndex(i); + //If they are closer than the distance remove vertex + if ((slicedPoly1[iNext] - slicedPoly1[i]).LengthSquared() <= ClipperEpsilonSquared) + { + slicedPoly1.RemoveAt(i); + --i; + } + } + for (int i = 0; i < slicedPoly2.Count; ++i) + { + int iNext = slicedPoly2.NextIndex(i); + //If they are closer than the distance remove vertex + if ((slicedPoly2[iNext] - slicedPoly2[i]).LengthSquared() <= ClipperEpsilonSquared) + { + slicedPoly2.RemoveAt(i); + --i; + } + } + } + + /// + /// Calculates the simplical chain corresponding to the input polygon. + /// + /// Used by method Execute(). + private static void CalculateSimplicalChain(Vertices poly, out List coeff, + out List simplicies) + { + simplicies = new List(); + coeff = new List(); + for (int i = 0; i < poly.Count; ++i) + { + simplicies.Add(new Edge(poly[i], poly[poly.NextIndex(i)])); + coeff.Add(CalculateSimplexCoefficient(Vector2.Zero, poly[i], poly[poly.NextIndex(i)])); + } + } + + /// + /// Calculates the characteristics function for all edges of + /// the given simplical chains and builds the result chain. + /// + /// Used by method Execute(). + private static void CalculateResultChain(List poly1Coeff, List poly1Simplicies, + List poly2Coeff, List poly2Simplicies, + PolyClipType clipType, out List resultSimplices) + { + resultSimplices = new List(); + + for (int i = 0; i < poly1Simplicies.Count; ++i) + { + float edgeCharacter = 0; + if (poly2Simplicies.Contains(poly1Simplicies[i])) + { + edgeCharacter = 1f; + } + else if (poly2Simplicies.Contains(-poly1Simplicies[i]) && clipType == PolyClipType.Union) + { + edgeCharacter = 1f; + } + else + { + for (int j = 0; j < poly2Simplicies.Count; ++j) + { + if (!poly2Simplicies.Contains(-poly1Simplicies[i])) + { + edgeCharacter += CalculateBeta(poly1Simplicies[i].GetCenter(), + poly2Simplicies[j], poly2Coeff[j]); + } + } + } + if (clipType == PolyClipType.Intersect) + { + if (edgeCharacter == 1f) + { + resultSimplices.Add(poly1Simplicies[i]); + } + } + else + { + if (edgeCharacter == 0f) + { + resultSimplices.Add(poly1Simplicies[i]); + } + } + } + for (int i = 0; i < poly2Simplicies.Count; ++i) + { + float edgeCharacter = 0f; + if (!resultSimplices.Contains(poly2Simplicies[i]) && + !resultSimplices.Contains(-poly2Simplicies[i])) + { + if (poly1Simplicies.Contains(-poly2Simplicies[i]) && clipType == PolyClipType.Union) + { + edgeCharacter = 1f; + } + else + { + edgeCharacter = 0f; + for (int j = 0; j < poly1Simplicies.Count; ++j) + { + if (!poly1Simplicies.Contains(poly2Simplicies[i]) && !poly1Simplicies.Contains(-poly2Simplicies[i])) + { + edgeCharacter += CalculateBeta(poly2Simplicies[i].GetCenter(), + poly1Simplicies[j], poly1Coeff[j]); + } + } + if (clipType == PolyClipType.Intersect || clipType == PolyClipType.Difference) + { + if (edgeCharacter == 1f) + { + resultSimplices.Add(-poly2Simplicies[i]); + } + } + else + { + if (edgeCharacter == 0f) + { + resultSimplices.Add(poly2Simplicies[i]); + } + } + } + } + } + } + + /// + /// Calculates the polygon(s) from the result simplical chain. + /// + /// Used by method Execute(). + private static PolyClipError BuildPolygonsFromChain(List simplicies, out List result) + { + result = new List(); + PolyClipError errVal = PolyClipError.None; + + while (simplicies.Count > 0) + { + Vertices output = new Vertices(); + output.Add(simplicies[0].EdgeStart); + output.Add(simplicies[0].EdgeEnd); + simplicies.RemoveAt(0); + bool closed = false; + int index = 0; + int count = simplicies.Count; // Needed to catch infinite loops + while (!closed && simplicies.Count > 0) + { + if (VectorEqual(output[output.Count - 1], simplicies[index].EdgeStart)) + { + if (VectorEqual(simplicies[index].EdgeEnd, output[0])) + { + closed = true; + } + else + { + output.Add(simplicies[index].EdgeEnd); + } + simplicies.RemoveAt(index); + --index; + } + else if (VectorEqual(output[output.Count - 1], simplicies[index].EdgeEnd)) + { + if (VectorEqual(simplicies[index].EdgeStart, output[0])) + { + closed = true; + } + else + { + output.Add(simplicies[index].EdgeStart); + } + simplicies.RemoveAt(index); + --index; + } + if (!closed) + { + if (++index == simplicies.Count) + { + if (count == simplicies.Count) + { + result = new List(); + Debug.WriteLine("Undefined error while building result polygon(s)."); + return PolyClipError.BrokenResult; + } + index = 0; + count = simplicies.Count; + } + } + } + if (output.Count < 3) + { + errVal = PolyClipError.DegeneratedOutput; + Debug.WriteLine("Degenerated output polygon produced (vertices < 3)."); + } + result.Add(output); + } + return errVal; + } + + /// + /// Needed to calculate the characteristics function of a simplex. + /// + /// Used by method CalculateEdgeCharacter(). + private static float CalculateBeta(Vector2 point, Edge e, float coefficient) + { + float result = 0f; + if (PointInSimplex(point, e)) + { + result = coefficient; + } + if (PointOnLineSegment(Vector2.Zero, e.EdgeStart, point) || + PointOnLineSegment(Vector2.Zero, e.EdgeEnd, point)) + { + result = .5f * coefficient; + } + return result; + } + + /// + /// Needed for sorting multiple intersections points on the same edge. + /// + /// Used by method CalculateIntersections(). + private static float GetAlpha(Vector2 start, Vector2 end, Vector2 point) + { + return (point - start).LengthSquared() / (end - start).LengthSquared(); + } + + /// + /// Returns the coefficient of a simplex. + /// + /// Used by method CalculateSimplicalChain(). + private static float CalculateSimplexCoefficient(Vector2 a, Vector2 b, Vector2 c) + { + float isLeft = MathUtils.Area(ref a, ref b, ref c); + if (isLeft < 0f) + { + return -1f; + } + + if (isLeft > 0f) + { + return 1f; + } + + return 0f; + } + + /// + /// Winding number test for a point in a simplex. + /// + /// The point to be tested. + /// The edge that the point is tested against. + /// False if the winding number is even and the point is outside + /// the simplex and True otherwise. + private static bool PointInSimplex(Vector2 point, Edge edge) + { + Vertices polygon = new Vertices(); + polygon.Add(Vector2.Zero); + polygon.Add(edge.EdgeStart); + polygon.Add(edge.EdgeEnd); + return (polygon.PointInPolygon(ref point) == 1); + } + + /// + /// Tests if a point lies on a line segment. + /// + /// Used by method CalculateBeta(). + private static bool PointOnLineSegment(Vector2 start, Vector2 end, Vector2 point) + { + Vector2 segment = end - start; + return MathUtils.Area(ref start, ref end, ref point) == 0f && + Vector2.Dot(point - start, segment) >= 0f && + Vector2.Dot(point - end, segment) <= 0f; + } + + private static bool VectorEqual(Vector2 vec1, Vector2 vec2) + { + return (vec2 - vec1).LengthSquared() <= ClipperEpsilonSquared; + } + + #region Nested type: Edge + + /// Specifies an Edge. Edges are used to represent simplicies in simplical chains + private sealed class Edge + { + public Edge(Vector2 edgeStart, Vector2 edgeEnd) + { + EdgeStart = edgeStart; + EdgeEnd = edgeEnd; + } + + public Vector2 EdgeStart { get; private set; } + public Vector2 EdgeEnd { get; private set; } + + public Vector2 GetCenter() + { + return (EdgeStart + EdgeEnd) / 2f; + } + + public static Edge operator -(Edge e) + { + return new Edge(e.EdgeEnd, e.EdgeStart); + } + + public override bool Equals(Object obj) + { + // If parameter is null return false. + if (obj == null) + { + return false; + } + + // If parameter cannot be cast to Point return false. + return Equals(obj as Edge); + } + + public bool Equals(Edge e) + { + // If parameter is null return false: + if (e == null) + { + return false; + } + + // Return true if the fields match + return VectorEqual(EdgeStart, e.EdgeStart) && VectorEqual(EdgeEnd, e.EdgeEnd); + } + + public override int GetHashCode() + { + return EdgeStart.GetHashCode() ^ EdgeEnd.GetHashCode(); + } + } + + #endregion + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/PolygonTools.cs b/Farseer Physics Engine 3.5/Common/PolygonTools.cs new file mode 100644 index 000000000..13ef4a450 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/PolygonTools.cs @@ -0,0 +1,360 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using FarseerPhysics.Common.TextureTools; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common +{ + public static class PolygonTools + { + /// + /// Build vertices to represent an axis-aligned box. + /// + /// the half-width. + /// the half-height. + public static Vertices CreateRectangle(float hx, float hy) + { + Vertices vertices = new Vertices(4); + vertices.Add(new Vector2(-hx, -hy)); + vertices.Add(new Vector2(hx, -hy)); + vertices.Add(new Vector2(hx, hy)); + vertices.Add(new Vector2(-hx, hy)); + + return vertices; + } + + /// + /// Build vertices to represent an oriented box. + /// + /// the half-width. + /// the half-height. + /// the center of the box in local coordinates. + /// the rotation of the box in local coordinates. + public static Vertices CreateRectangle(float hx, float hy, Vector2 center, float angle) + { + Vertices vertices = CreateRectangle(hx, hy); + + Transform xf = new Transform(); + xf.p = center; + xf.q.Set(angle); + + // Transform vertices + for (int i = 0; i < 4; ++i) + { + vertices[i] = MathUtils.Mul(ref xf, vertices[i]); + } + + return vertices; + } + + //Rounded rectangle contributed by Jonathan Smars - jsmars@gmail.com + + /// + /// Creates a rounded rectangle with the specified width and height. + /// + /// The width. + /// The height. + /// The rounding X radius. + /// The rounding Y radius. + /// The number of segments to subdivide the edges. + /// + public static Vertices CreateRoundedRectangle(float width, float height, float xRadius, float yRadius, + int segments) + { + if (yRadius > height / 2 || xRadius > width / 2) + throw new Exception("Rounding amount can't be more than half the height and width respectively."); + if (segments < 0) + throw new Exception("Segments must be zero or more."); + + //We need at least 8 vertices to create a rounded rectangle + Debug.Assert(Settings.MaxPolygonVertices >= 8); + + Vertices vertices = new Vertices(); + if (segments == 0) + { + vertices.Add(new Vector2(width * .5f - xRadius, -height * .5f)); + vertices.Add(new Vector2(width * .5f, -height * .5f + yRadius)); + + vertices.Add(new Vector2(width * .5f, height * .5f - yRadius)); + vertices.Add(new Vector2(width * .5f - xRadius, height * .5f)); + + vertices.Add(new Vector2(-width * .5f + xRadius, height * .5f)); + vertices.Add(new Vector2(-width * .5f, height * .5f - yRadius)); + + vertices.Add(new Vector2(-width * .5f, -height * .5f + yRadius)); + vertices.Add(new Vector2(-width * .5f + xRadius, -height * .5f)); + } + else + { + int numberOfEdges = (segments * 4 + 8); + + float stepSize = MathHelper.TwoPi / (numberOfEdges - 4); + int perPhase = numberOfEdges / 4; + + Vector2 posOffset = new Vector2(width / 2 - xRadius, height / 2 - yRadius); + vertices.Add(posOffset + new Vector2(xRadius, -yRadius + yRadius)); + short phase = 0; + for (int i = 1; i < numberOfEdges; i++) + { + if (i - perPhase == 0 || i - perPhase * 3 == 0) + { + posOffset.X *= -1; + phase--; + } + else if (i - perPhase * 2 == 0) + { + posOffset.Y *= -1; + phase--; + } + + vertices.Add(posOffset + new Vector2(xRadius * (float)Math.Cos(stepSize * -(i + phase)), + -yRadius * (float)Math.Sin(stepSize * -(i + phase)))); + } + } + + return vertices; + } + + /// + /// Set this as a single edge. + /// + /// The first point. + /// The second point. + public static Vertices CreateLine(Vector2 start, Vector2 end) + { + Vertices vertices = new Vertices(2); + vertices.Add(start); + vertices.Add(end); + + return vertices; + } + + /// + /// Creates a circle with the specified radius and number of edges. + /// + /// The radius. + /// The number of edges. The more edges, the more it resembles a circle + /// + public static Vertices CreateCircle(float radius, int numberOfEdges) + { + return CreateEllipse(radius, radius, numberOfEdges); + } + + /// + /// Creates a ellipse with the specified width, height and number of edges. + /// + /// Width of the ellipse. + /// Height of the ellipse. + /// The number of edges. The more edges, the more it resembles an ellipse + /// + public static Vertices CreateEllipse(float xRadius, float yRadius, int numberOfEdges) + { + Vertices vertices = new Vertices(); + + float stepSize = MathHelper.TwoPi / numberOfEdges; + + vertices.Add(new Vector2(xRadius, 0)); + for (int i = numberOfEdges - 1; i > 0; --i) + vertices.Add(new Vector2(xRadius * (float)Math.Cos(stepSize * i), + -yRadius * (float)Math.Sin(stepSize * i))); + + return vertices; + } + + public static Vertices CreateArc(float radians, int sides, float radius) + { + Debug.Assert(radians > 0, "The arc needs to be larger than 0"); + Debug.Assert(sides > 1, "The arc needs to have more than 1 sides"); + Debug.Assert(radius > 0, "The arc needs to have a radius larger than 0"); + + Vertices vertices = new Vertices(); + + float stepSize = radians / sides; + for (int i = sides - 1; i > 0; i--) + { + vertices.Add(new Vector2(radius * (float)Math.Cos(stepSize * i), + radius * (float)Math.Sin(stepSize * i))); + } + + return vertices; + } + + //Capsule contributed by Yobiv + + /// + /// Creates an capsule with the specified height, radius and number of edges. + /// A capsule has the same form as a pill capsule. + /// + /// Height (inner height + 2 * radius) of the capsule. + /// Radius of the capsule ends. + /// The number of edges of the capsule ends. The more edges, the more it resembles an capsule + /// + public static Vertices CreateCapsule(float height, float endRadius, int edges) + { + if (endRadius >= height / 2) + throw new ArgumentException( + "The radius must be lower than height / 2. Higher values of radius would create a circle, and not a half circle.", + "endRadius"); + + return CreateCapsule(height, endRadius, edges, endRadius, edges); + } + + /// + /// Creates an capsule with the specified height, radius and number of edges. + /// A capsule has the same form as a pill capsule. + /// + /// Height (inner height + radii) of the capsule. + /// Radius of the top. + /// The number of edges of the top. The more edges, the more it resembles an capsule + /// Radius of bottom. + /// The number of edges of the bottom. The more edges, the more it resembles an capsule + /// + public static Vertices CreateCapsule(float height, float topRadius, int topEdges, float bottomRadius, + int bottomEdges) + { + if (height <= 0) + throw new ArgumentException("Height must be longer than 0", "height"); + + if (topRadius <= 0) + throw new ArgumentException("The top radius must be more than 0", "topRadius"); + + if (topEdges <= 0) + throw new ArgumentException("Top edges must be more than 0", "topEdges"); + + if (bottomRadius <= 0) + throw new ArgumentException("The bottom radius must be more than 0", "bottomRadius"); + + if (bottomEdges <= 0) + throw new ArgumentException("Bottom edges must be more than 0", "bottomEdges"); + + if (topRadius >= height / 2) + throw new ArgumentException( + "The top radius must be lower than height / 2. Higher values of top radius would create a circle, and not a half circle.", + "topRadius"); + + if (bottomRadius >= height / 2) + throw new ArgumentException( + "The bottom radius must be lower than height / 2. Higher values of bottom radius would create a circle, and not a half circle.", + "bottomRadius"); + + Vertices vertices = new Vertices(); + + float newHeight = (height - topRadius - bottomRadius) * 0.5f; + + // top + vertices.Add(new Vector2(topRadius, newHeight)); + + float stepSize = MathHelper.Pi / topEdges; + for (int i = 1; i < topEdges; i++) + { + vertices.Add(new Vector2(topRadius * (float)Math.Cos(stepSize * i), + topRadius * (float)Math.Sin(stepSize * i) + newHeight)); + } + + vertices.Add(new Vector2(-topRadius, newHeight)); + + // bottom + vertices.Add(new Vector2(-bottomRadius, -newHeight)); + + stepSize = MathHelper.Pi / bottomEdges; + for (int i = 1; i < bottomEdges; i++) + { + vertices.Add(new Vector2(-bottomRadius * (float)Math.Cos(stepSize * i), + -bottomRadius * (float)Math.Sin(stepSize * i) - newHeight)); + } + + vertices.Add(new Vector2(bottomRadius, -newHeight)); + + return vertices; + } + + /// + /// Creates a gear shape with the specified radius and number of teeth. + /// + /// The radius. + /// The number of teeth. + /// The tip percentage. + /// Height of the tooth. + /// + public static Vertices CreateGear(float radius, int numberOfTeeth, float tipPercentage, float toothHeight) + { + Vertices vertices = new Vertices(); + + float stepSize = MathHelper.TwoPi / numberOfTeeth; + tipPercentage /= 100f; + MathHelper.Clamp(tipPercentage, 0f, 1f); + float toothTipStepSize = (stepSize / 2f) * tipPercentage; + + float toothAngleStepSize = (stepSize - (toothTipStepSize * 2f)) / 2f; + + for (int i = numberOfTeeth - 1; i >= 0; --i) + { + if (toothTipStepSize > 0f) + { + vertices.Add( + new Vector2(radius * + (float)Math.Cos(stepSize * i + toothAngleStepSize * 2f + toothTipStepSize), + -radius * + (float)Math.Sin(stepSize * i + toothAngleStepSize * 2f + toothTipStepSize))); + + vertices.Add( + new Vector2((radius + toothHeight) * + (float)Math.Cos(stepSize * i + toothAngleStepSize + toothTipStepSize), + -(radius + toothHeight) * + (float)Math.Sin(stepSize * i + toothAngleStepSize + toothTipStepSize))); + } + + vertices.Add(new Vector2((radius + toothHeight) * + (float)Math.Cos(stepSize * i + toothAngleStepSize), + -(radius + toothHeight) * + (float)Math.Sin(stepSize * i + toothAngleStepSize))); + + vertices.Add(new Vector2(radius * (float)Math.Cos(stepSize * i), + -radius * (float)Math.Sin(stepSize * i))); + } + + return vertices; + } + + /// + /// Detects the vertices by analyzing the texture data. + /// + /// The texture data. + /// The texture width. + /// + public static Vertices CreatePolygon(uint[] data, int width) + { + return TextureConverter.DetectVertices(data, width); + } + + /// + /// Detects the vertices by analyzing the texture data. + /// + /// The texture data. + /// The texture width. + /// if set to true it will perform hole detection. + /// + public static Vertices CreatePolygon(uint[] data, int width, bool holeDetection) + { + return TextureConverter.DetectVertices(data, width, holeDetection); + } + + /// + /// Detects the vertices by analyzing the texture data. + /// + /// The texture data. + /// The texture width. + /// The hull tolerance. + /// The alpha tolerance. + /// if set to true it will perform multi part detection. + /// if set to true it will perform hole detection. + /// + public static List CreatePolygon(uint[] data, int width, float hullTolerance, + byte alphaTolerance, bool multiPartDetection, bool holeDetection) + { + return TextureConverter.DetectVertices(data, width, hullTolerance, alphaTolerance, + multiPartDetection, holeDetection); + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Serialization.cs b/Farseer Physics Engine 3.5/Common/Serialization.cs new file mode 100644 index 000000000..f47bae4fc --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Serialization.cs @@ -0,0 +1,1480 @@ +using System; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Text; +using System.Xml; +using System.Xml.Serialization; +using FarseerPhysics.Collision.Shapes; +using FarseerPhysics.Dynamics; +using FarseerPhysics.Dynamics.Joints; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common +{ + /// + /// Serialize the world into an XML file + /// + public static class WorldSerializer + { + /// + /// Serialize the world to an XML file + /// + /// + /// + public static void Serialize(World world, string filename) + { + using (FileStream fs = new FileStream(filename, FileMode.Create)) + { + WorldXmlSerializer.Serialize(world, fs); + } + } + + /// + /// Deserialize the world from an XML file + /// + /// + public static World Deserialize(string filename) + { + using (FileStream fs = new FileStream(filename, FileMode.Open)) + { + return WorldXmlDeserializer.Deserialize(fs); + } + } + } + + internal static class WorldXmlSerializer + { + private static XmlWriter _writer; + + private static void SerializeShape(Shape shape) + { + _writer.WriteStartElement("Shape"); + _writer.WriteAttributeString("Type", shape.ShapeType.ToString()); + _writer.WriteAttributeString("Density", shape.Density.ToString()); + + switch (shape.ShapeType) + { + case ShapeType.Circle: + { + CircleShape circle = (CircleShape)shape; + + _writer.WriteElementString("Radius", circle.Radius.ToString()); + + WriteElement("Position", circle.Position); + } + break; + case ShapeType.Polygon: + { + PolygonShape poly = (PolygonShape)shape; + + _writer.WriteStartElement("Vertices"); + foreach (Vector2 v in poly.Vertices) + WriteElement("Vertex", v); + _writer.WriteEndElement(); + + WriteElement("Centroid", poly.MassData.Centroid); + } + break; + case ShapeType.Edge: + { + EdgeShape poly = (EdgeShape)shape; + WriteElement("Vertex1", poly.Vertex1); + WriteElement("Vertex2", poly.Vertex2); + } + break; + case ShapeType.Chain: + { + ChainShape chain = (ChainShape)shape; + + _writer.WriteStartElement("Vertices"); + foreach (Vector2 v in chain.Vertices) + WriteElement("Vertex", v); + _writer.WriteEndElement(); + + WriteElement("NextVertex", chain.NextVertex); + WriteElement("PrevVertex", chain.PrevVertex); + } + break; + default: + throw new Exception(); + } + + _writer.WriteEndElement(); + } + + private static void SerializeFixture(Fixture fixture) + { + _writer.WriteStartElement("Fixture"); + _writer.WriteAttributeString("Id", fixture.FixtureId.ToString()); + + _writer.WriteStartElement("FilterData"); + _writer.WriteElementString("CategoryBits", ((int)fixture.CollisionCategories).ToString()); + _writer.WriteElementString("MaskBits", ((int)fixture.CollidesWith).ToString()); + _writer.WriteElementString("GroupIndex", fixture.CollisionGroup.ToString()); + _writer.WriteElementString("CollisionIgnores", Join("|", fixture._collisionIgnores)); + _writer.WriteEndElement(); + + _writer.WriteElementString("Friction", fixture.Friction.ToString()); + _writer.WriteElementString("IsSensor", fixture.IsSensor.ToString()); + _writer.WriteElementString("Restitution", fixture.Restitution.ToString()); + + if (fixture.UserData != null) + { + _writer.WriteStartElement("UserData"); + WriteDynamicType(fixture.UserData.GetType(), fixture.UserData); + _writer.WriteEndElement(); + } + + _writer.WriteEndElement(); + } + + private static void SerializeBody(List fixtures, List shapes, Body body) + { + _writer.WriteStartElement("Body"); + _writer.WriteAttributeString("Type", body.BodyType.ToString()); + _writer.WriteElementString("Active", body.Enabled.ToString()); + _writer.WriteElementString("AllowSleep", body.SleepingAllowed.ToString()); + _writer.WriteElementString("Angle", body.Rotation.ToString()); + _writer.WriteElementString("AngularDamping", body.AngularDamping.ToString()); + _writer.WriteElementString("AngularVelocity", body.AngularVelocity.ToString()); + _writer.WriteElementString("Awake", body.Awake.ToString()); + _writer.WriteElementString("Bullet", body.IsBullet.ToString()); + _writer.WriteElementString("FixedRotation", body.FixedRotation.ToString()); + _writer.WriteElementString("LinearDamping", body.LinearDamping.ToString()); + WriteElement("LinearVelocity", body.LinearVelocity); + WriteElement("Position", body.Position); + + if (body.UserData != null) + { + _writer.WriteStartElement("UserData"); + WriteDynamicType(body.UserData.GetType(), body.UserData); + _writer.WriteEndElement(); + } + + _writer.WriteStartElement("Bindings"); + for (int i = 0; i < body.FixtureList.Count; i++) + { + _writer.WriteStartElement("Pair"); + _writer.WriteAttributeString("FixtureId", FindIndex(fixtures, body.FixtureList[i]).ToString()); + _writer.WriteAttributeString("ShapeId", FindIndex(shapes, body.FixtureList[i].Shape).ToString()); + _writer.WriteEndElement(); + } + + _writer.WriteEndElement(); + _writer.WriteEndElement(); + } + + private static void SerializeJoint(List bodies, Joint joint) + { + _writer.WriteStartElement("Joint"); + _writer.WriteAttributeString("Type", joint.JointType.ToString()); + + WriteElement("BodyA", FindIndex(bodies, joint.BodyA)); + WriteElement("BodyB", FindIndex(bodies, joint.BodyB)); + + WriteElement("CollideConnected", joint.CollideConnected); + + WriteElement("Breakpoint", joint.Breakpoint); + + if (joint.UserData != null) + { + _writer.WriteStartElement("UserData"); + WriteDynamicType(joint.UserData.GetType(), joint.UserData); + _writer.WriteEndElement(); + } + + switch (joint.JointType) + { + case JointType.Distance: + { + DistanceJoint distanceJoint = (DistanceJoint)joint; + WriteElement("DampingRatio", distanceJoint.DampingRatio); + WriteElement("FrequencyHz", distanceJoint.Frequency); + WriteElement("Length", distanceJoint.Length); + WriteElement("LocalAnchorA", distanceJoint.LocalAnchorA); + WriteElement("LocalAnchorB", distanceJoint.LocalAnchorB); + } + break; + case JointType.Friction: + { + FrictionJoint frictionJoint = (FrictionJoint)joint; + WriteElement("LocalAnchorA", frictionJoint.LocalAnchorA); + WriteElement("LocalAnchorB", frictionJoint.LocalAnchorB); + WriteElement("MaxForce", frictionJoint.MaxForce); + WriteElement("MaxTorque", frictionJoint.MaxTorque); + } + break; + case JointType.Gear: + throw new Exception("Gear joint not supported by serialization"); + case JointType.Wheel: + { + WheelJoint wheelJoint = (WheelJoint)joint; + WriteElement("EnableMotor", wheelJoint.MotorEnabled); + WriteElement("LocalAnchorA", wheelJoint.LocalAnchorA); + WriteElement("LocalAnchorB", wheelJoint.LocalAnchorB); + WriteElement("MotorSpeed", wheelJoint.MotorSpeed); + WriteElement("DampingRatio", wheelJoint.DampingRatio); + WriteElement("MaxMotorTorque", wheelJoint.MaxMotorTorque); + WriteElement("FrequencyHz", wheelJoint.Frequency); + WriteElement("Axis", wheelJoint.Axis); + } + break; + case JointType.Prismatic: + { + //NOTE: Does not conform with Box2DScene + + PrismaticJoint prismaticJoint = (PrismaticJoint)joint; + WriteElement("EnableLimit", prismaticJoint.LimitEnabled); + WriteElement("EnableMotor", prismaticJoint.MotorEnabled); + WriteElement("LocalAnchorA", prismaticJoint.LocalAnchorA); + WriteElement("LocalAnchorB", prismaticJoint.LocalAnchorB); + WriteElement("Axis", prismaticJoint.Axis); + WriteElement("LowerTranslation", prismaticJoint.LowerLimit); + WriteElement("UpperTranslation", prismaticJoint.UpperLimit); + WriteElement("MaxMotorForce", prismaticJoint.MaxMotorForce); + WriteElement("MotorSpeed", prismaticJoint.MotorSpeed); + } + break; + case JointType.Pulley: + { + PulleyJoint pulleyJoint = (PulleyJoint)joint; + WriteElement("WorldAnchorA", pulleyJoint.WorldAnchorA); + WriteElement("WorldAnchorB", pulleyJoint.WorldAnchorB); + WriteElement("LengthA", pulleyJoint.LengthA); + WriteElement("LengthB", pulleyJoint.LengthB); + WriteElement("LocalAnchorA", pulleyJoint.LocalAnchorA); + WriteElement("LocalAnchorB", pulleyJoint.LocalAnchorB); + WriteElement("Ratio", pulleyJoint.Ratio); + WriteElement("Constant", pulleyJoint.Constant); + } + break; + case JointType.Revolute: + { + RevoluteJoint revoluteJoint = (RevoluteJoint)joint; + WriteElement("EnableLimit", revoluteJoint.LimitEnabled); + WriteElement("EnableMotor", revoluteJoint.MotorEnabled); + WriteElement("LocalAnchorA", revoluteJoint.LocalAnchorA); + WriteElement("LocalAnchorB", revoluteJoint.LocalAnchorB); + WriteElement("LowerAngle", revoluteJoint.LowerLimit); + WriteElement("MaxMotorTorque", revoluteJoint.MaxMotorTorque); + WriteElement("MotorSpeed", revoluteJoint.MotorSpeed); + WriteElement("ReferenceAngle", revoluteJoint.ReferenceAngle); + WriteElement("UpperAngle", revoluteJoint.UpperLimit); + } + break; + case JointType.Weld: + { + WeldJoint weldJoint = (WeldJoint)joint; + WriteElement("LocalAnchorA", weldJoint.LocalAnchorA); + WriteElement("LocalAnchorB", weldJoint.LocalAnchorB); + } + break; + // + // Not part of Box2DScene + // + case JointType.Rope: + { + RopeJoint ropeJoint = (RopeJoint)joint; + WriteElement("LocalAnchorA", ropeJoint.LocalAnchorA); + WriteElement("LocalAnchorB", ropeJoint.LocalAnchorB); + WriteElement("MaxLength", ropeJoint.MaxLength); + } + break; + case JointType.Angle: + { + AngleJoint angleJoint = (AngleJoint)joint; + WriteElement("BiasFactor", angleJoint.BiasFactor); + WriteElement("MaxImpulse", angleJoint.MaxImpulse); + WriteElement("Softness", angleJoint.Softness); + WriteElement("TargetAngle", angleJoint.TargetAngle); + } + break; + case JointType.Motor: + { + MotorJoint motorJoint = (MotorJoint)joint; + WriteElement("AngularOffset", motorJoint.AngularOffset); + WriteElement("LinearOffset", motorJoint.LinearOffset); + WriteElement("MaxForce", motorJoint.MaxForce); + WriteElement("MaxTorque", motorJoint.MaxTorque); + WriteElement("CorrectionFactor", motorJoint.CorrectionFactor); + } + break; + default: + throw new Exception("Joint not supported"); + } + + _writer.WriteEndElement(); + } + + private static void WriteDynamicType(Type type, object val) + { + _writer.WriteElementString("Type", type.AssemblyQualifiedName); + + _writer.WriteStartElement("Value"); + XmlSerializer serializer = new XmlSerializer(type); + XmlSerializerNamespaces xmlnsEmpty = new XmlSerializerNamespaces(); + xmlnsEmpty.Add("", ""); + serializer.Serialize(_writer, val, xmlnsEmpty); + _writer.WriteEndElement(); + } + + private static void WriteElement(string name, Vector2 vec) + { + _writer.WriteElementString(name, vec.X + " " + vec.Y); + } + + private static void WriteElement(string name, int val) + { + _writer.WriteElementString(name, val.ToString()); + } + + private static void WriteElement(string name, bool val) + { + _writer.WriteElementString(name, val.ToString()); + } + + private static void WriteElement(string name, float val) + { + _writer.WriteElementString(name, val.ToString()); + } + + private static int FindIndex(List list, Body item) + { + for (int i = 0; i < list.Count; ++i) + if (list[i] == item) + return i; + + return -1; + } + + private static int FindIndex(List list, Fixture item) + { + for (int i = 0; i < list.Count; ++i) + if (list[i].CompareTo(item)) + return i; + + return -1; + } + + private static int FindIndex(List list, Shape item) + { + for (int i = 0; i < list.Count; ++i) + if (list[i].CompareTo(item)) + return i; + + return -1; + } + + private static String Join(String separator, IEnumerable values) + { + using (IEnumerator en = values.GetEnumerator()) + { + if (!en.MoveNext()) + return String.Empty; + + StringBuilder result = new StringBuilder(); + if (en.Current != null) + { + // handle the case that the enumeration has null entries + // and the case where their ToString() override is broken + string value = en.Current.ToString(); + if (value != null) + result.Append(value); + } + + while (en.MoveNext()) + { + result.Append(separator); + if (en.Current != null) + { + // handle the case that the enumeration has null entries + // and the case where their ToString() override is broken + string value = en.Current.ToString(); + if (value != null) + result.Append(value); + } + } + return result.ToString(); + } + } + + internal static void Serialize(World world, Stream stream) + { + List bodies = new List(); + List fixtures = new List(); + List shapes = new List(); + + XmlWriterSettings settings = new XmlWriterSettings(); + settings.Indent = true; + settings.NewLineOnAttributes = false; + settings.OmitXmlDeclaration = true; + + _writer = XmlWriter.Create(stream, settings); + + _writer.WriteStartElement("World"); + _writer.WriteAttributeString("Version", "3"); + WriteElement("Gravity", world.Gravity); + + _writer.WriteStartElement("Shapes"); + + foreach (Body body in world.BodyList) + { + foreach (Fixture fixture in body.FixtureList) + { + if (!shapes.Any(s2 => fixture.Shape.CompareTo(s2))) + { + SerializeShape(fixture.Shape); + shapes.Add(fixture.Shape); + } + } + } + + _writer.WriteEndElement(); + _writer.WriteStartElement("Fixtures"); + + foreach (Body body in world.BodyList) + { + foreach (Fixture fixture in body.FixtureList) + { + if (!fixtures.Any(f2 => fixture.CompareTo(f2))) + { + SerializeFixture(fixture); + fixtures.Add(fixture); + } + } + } + + _writer.WriteEndElement(); + _writer.WriteStartElement("Bodies"); + + foreach (Body body in world.BodyList) + { + bodies.Add(body); + SerializeBody(fixtures, shapes, body); + } + + _writer.WriteEndElement(); + _writer.WriteStartElement("Joints"); + + foreach (Joint joint in world.JointList) + { + SerializeJoint(bodies, joint); + } + + _writer.WriteEndElement(); + _writer.WriteEndElement(); + + _writer.Flush(); + _writer.Close(); + } + } + + internal static class WorldXmlDeserializer + { + internal static World Deserialize(Stream stream) + { + World world = new World(Vector2.Zero); + Deserialize(world, stream); + return world; + } + + private static void Deserialize(World world, Stream stream) + { + List bodies = new List(); + List fixtures = new List(); + List joints = new List(); + List shapes = new List(); + + XMLFragmentElement root = XMLFragmentParser.LoadFromStream(stream); + + if (root.Name.ToLower() != "world") + throw new Exception(); + + //Read gravity + foreach (XMLFragmentElement element in root.Elements) + { + if (element.Name.ToLower() == "gravity") + { + world.Gravity = ReadVector(element); + break; + } + } + + //Read shapes + foreach (XMLFragmentElement shapeElement in root.Elements) + { + if (shapeElement.Name.ToLower() == "shapes") + { + foreach (XMLFragmentElement element in shapeElement.Elements) + { + if (element.Name.ToLower() != "shape") + throw new Exception(); + + ShapeType type = (ShapeType)Enum.Parse(typeof(ShapeType), element.Attributes[0].Value, true); + float density = float.Parse(element.Attributes[1].Value); + + switch (type) + { + case ShapeType.Circle: + { + CircleShape shape = new CircleShape(); + shape._density = density; + + foreach (XMLFragmentElement sn in element.Elements) + { + switch (sn.Name.ToLower()) + { + case "radius": + shape.Radius = float.Parse(sn.Value); + break; + case "position": + shape.Position = ReadVector(sn); + break; + default: + throw new Exception(); + } + } + + shapes.Add(shape); + } + break; + case ShapeType.Polygon: + { + PolygonShape shape = new PolygonShape(); + shape._density = density; + + foreach (XMLFragmentElement sn in element.Elements) + { + switch (sn.Name.ToLower()) + { + case "vertices": + { + List verts = new List(sn.Elements.Count); + + foreach (XMLFragmentElement vert in sn.Elements) + verts.Add(ReadVector(vert)); + + shape.Vertices = new Vertices(verts); + } + break; + case "centroid": + shape.MassData.Centroid = ReadVector(sn); + break; + } + } + + shapes.Add(shape); + } + break; + case ShapeType.Edge: + { + EdgeShape shape = new EdgeShape(); + shape._density = density; + + foreach (XMLFragmentElement sn in element.Elements) + { + switch (sn.Name.ToLower()) + { + case "hasvertex0": + shape.HasVertex0 = bool.Parse(sn.Value); + break; + case "hasvertex3": + shape.HasVertex0 = bool.Parse(sn.Value); + break; + case "vertex0": + shape.Vertex0 = ReadVector(sn); + break; + case "vertex1": + shape.Vertex1 = ReadVector(sn); + break; + case "vertex2": + shape.Vertex2 = ReadVector(sn); + break; + case "vertex3": + shape.Vertex3 = ReadVector(sn); + break; + default: + throw new Exception(); + } + } + shapes.Add(shape); + } + break; + case ShapeType.Chain: + { + ChainShape shape = new ChainShape(); + shape._density = density; + + foreach (XMLFragmentElement sn in element.Elements) + { + switch (sn.Name.ToLower()) + { + case "vertices": + { + List verts = new List(sn.Elements.Count); + + foreach (XMLFragmentElement vert in sn.Elements) + verts.Add(ReadVector(vert)); + + shape.Vertices = new Vertices(verts); + } + break; + case "nextvertex": + shape.NextVertex = ReadVector(sn); + break; + case "prevvertex": + shape.PrevVertex = ReadVector(sn); + break; + + default: + throw new Exception(); + } + } + shapes.Add(shape); + } + break; + } + } + } + } + + //Read fixtures + foreach (XMLFragmentElement fixtureElement in root.Elements) + { + if (fixtureElement.Name.ToLower() == "fixtures") + { + foreach (XMLFragmentElement element in fixtureElement.Elements) + { + Fixture fixture = new Fixture(); + + if (element.Name.ToLower() != "fixture") + throw new Exception(); + + fixture.FixtureId = int.Parse(element.Attributes[0].Value); + + foreach (XMLFragmentElement sn in element.Elements) + { + switch (sn.Name.ToLower()) + { + case "filterdata": + foreach (XMLFragmentElement ssn in sn.Elements) + { + switch (ssn.Name.ToLower()) + { + case "categorybits": + fixture._collisionCategories = (Category)int.Parse(ssn.Value); + break; + case "maskbits": + fixture._collidesWith = (Category)int.Parse(ssn.Value); + break; + case "groupindex": + fixture._collisionGroup = short.Parse(ssn.Value); + break; + case "CollisionIgnores": + string[] split = ssn.Value.Split('|'); + foreach (string s in split) + { + fixture._collisionIgnores.Add(int.Parse(s)); + } + break; + } + } + + break; + case "friction": + fixture.Friction = float.Parse(sn.Value); + break; + case "issensor": + fixture.IsSensor = bool.Parse(sn.Value); + break; + case "restitution": + fixture.Restitution = float.Parse(sn.Value); + break; + case "userdata": + fixture.UserData = ReadSimpleType(sn, null, false); + break; + } + } + + fixtures.Add(fixture); + } + } + } + + //Read bodies + foreach (XMLFragmentElement bodyElement in root.Elements) + { + if (bodyElement.Name.ToLower() == "bodies") + { + foreach (XMLFragmentElement element in bodyElement.Elements) + { + Body body = new Body(world); + + if (element.Name.ToLower() != "body") + throw new Exception(); + + body.BodyType = (BodyType)Enum.Parse(typeof(BodyType), element.Attributes[0].Value, true); + + foreach (XMLFragmentElement sn in element.Elements) + { + switch (sn.Name.ToLower()) + { + case "active": + body._enabled = bool.Parse(sn.Value); + break; + case "allowsleep": + body.SleepingAllowed = bool.Parse(sn.Value); + break; + case "angle": + { + Vector2 position = body.Position; + body.SetTransformIgnoreContacts(ref position, float.Parse(sn.Value)); + } + break; + case "angulardamping": + body.AngularDamping = float.Parse(sn.Value); + break; + case "angularvelocity": + body.AngularVelocity = float.Parse(sn.Value); + break; + case "awake": + body.Awake = bool.Parse(sn.Value); + break; + case "bullet": + body.IsBullet = bool.Parse(sn.Value); + break; + case "fixedrotation": + body.FixedRotation = bool.Parse(sn.Value); + break; + case "lineardamping": + body.LinearDamping = float.Parse(sn.Value); + break; + case "linearvelocity": + body.LinearVelocity = ReadVector(sn); + break; + case "position": + { + float rotation = body.Rotation; + Vector2 position = ReadVector(sn); + body.SetTransformIgnoreContacts(ref position, rotation); + } + break; + case "userdata": + body.UserData = ReadSimpleType(sn, null, false); + break; + case "bindings": + { + foreach (XMLFragmentElement pair in sn.Elements) + { + Fixture fix = fixtures[int.Parse(pair.Attributes[0].Value)]; + fix.Shape = shapes[int.Parse(pair.Attributes[1].Value)].Clone(); + fix.CloneOnto(body); + } + break; + } + } + } + + bodies.Add(body); + } + } + } + + //Read joints + foreach (XMLFragmentElement jointElement in root.Elements) + { + if (jointElement.Name.ToLower() == "joints") + { + foreach (XMLFragmentElement n in jointElement.Elements) + { + Joint joint; + + if (n.Name.ToLower() != "joint") + throw new Exception(); + + JointType type = (JointType)Enum.Parse(typeof(JointType), n.Attributes[0].Value, true); + + int bodyAIndex = -1, bodyBIndex = -1; + bool collideConnected = false; + object userData = null; + + foreach (XMLFragmentElement sn in n.Elements) + { + switch (sn.Name.ToLower()) + { + case "bodya": + bodyAIndex = int.Parse(sn.Value); + break; + case "bodyb": + bodyBIndex = int.Parse(sn.Value); + break; + case "collideconnected": + collideConnected = bool.Parse(sn.Value); + break; + case "userdata": + userData = ReadSimpleType(sn, null, false); + break; + } + } + + Body bodyA = bodies[bodyAIndex]; + Body bodyB = bodies[bodyBIndex]; + + switch (type) + { + //case JointType.FixedMouse: + // joint = new FixedMouseJoint(); + // break; + //case JointType.FixedRevolute: + // break; + //case JointType.FixedDistance: + // break; + //case JointType.FixedLine: + // break; + //case JointType.FixedPrismatic: + // break; + //case JointType.FixedAngle: + // break; + //case JointType.FixedFriction: + // break; + case JointType.Distance: + joint = new DistanceJoint(); + break; + case JointType.Friction: + joint = new FrictionJoint(); + break; + case JointType.Wheel: + joint = new WheelJoint(); + break; + case JointType.Prismatic: + joint = new PrismaticJoint(); + break; + case JointType.Pulley: + joint = new PulleyJoint(); + break; + case JointType.Revolute: + joint = new RevoluteJoint(); + break; + case JointType.Weld: + joint = new WeldJoint(); + break; + case JointType.Rope: + joint = new RopeJoint(); + break; + case JointType.Angle: + joint = new AngleJoint(); + break; + case JointType.Motor: + joint = new MotorJoint(); + break; + case JointType.Gear: + throw new Exception("GearJoint is not supported."); + default: + throw new Exception("Invalid or unsupported joint."); + } + + joint.CollideConnected = collideConnected; + joint.UserData = userData; + joint.BodyA = bodyA; + joint.BodyB = bodyB; + joints.Add(joint); + world.AddJoint(joint); + + foreach (XMLFragmentElement sn in n.Elements) + { + // check for specific nodes + switch (type) + { + case JointType.Distance: + { + switch (sn.Name.ToLower()) + { + case "dampingratio": + ((DistanceJoint)joint).DampingRatio = float.Parse(sn.Value); + break; + case "frequencyhz": + ((DistanceJoint)joint).Frequency = float.Parse(sn.Value); + break; + case "length": + ((DistanceJoint)joint).Length = float.Parse(sn.Value); + break; + case "localanchora": + ((DistanceJoint)joint).LocalAnchorA = ReadVector(sn); + break; + case "localanchorb": + ((DistanceJoint)joint).LocalAnchorB = ReadVector(sn); + break; + } + } + break; + case JointType.Friction: + { + switch (sn.Name.ToLower()) + { + case "localanchora": + ((FrictionJoint)joint).LocalAnchorA = ReadVector(sn); + break; + case "localanchorb": + ((FrictionJoint)joint).LocalAnchorB = ReadVector(sn); + break; + case "maxforce": + ((FrictionJoint)joint).MaxForce = float.Parse(sn.Value); + break; + case "maxtorque": + ((FrictionJoint)joint).MaxTorque = float.Parse(sn.Value); + break; + } + } + break; + case JointType.Wheel: + { + switch (sn.Name.ToLower()) + { + case "enablemotor": + ((WheelJoint)joint).MotorEnabled = bool.Parse(sn.Value); + break; + case "localanchora": + ((WheelJoint)joint).LocalAnchorA = ReadVector(sn); + break; + case "localanchorb": + ((WheelJoint)joint).LocalAnchorB = ReadVector(sn); + break; + case "motorspeed": + ((WheelJoint)joint).MotorSpeed = float.Parse(sn.Value); + break; + case "dampingratio": + ((WheelJoint)joint).DampingRatio = float.Parse(sn.Value); + break; + case "maxmotortorque": + ((WheelJoint)joint).MaxMotorTorque = float.Parse(sn.Value); + break; + case "frequencyhz": + ((WheelJoint)joint).Frequency = float.Parse(sn.Value); + break; + case "axis": + ((WheelJoint)joint).Axis = ReadVector(sn); + break; + } + } + break; + case JointType.Prismatic: + { + switch (sn.Name.ToLower()) + { + case "enablelimit": + ((PrismaticJoint)joint).LimitEnabled = bool.Parse(sn.Value); + break; + case "enablemotor": + ((PrismaticJoint)joint).MotorEnabled = bool.Parse(sn.Value); + break; + case "localanchora": + ((PrismaticJoint)joint).LocalAnchorA = ReadVector(sn); + break; + case "localanchorb": + ((PrismaticJoint)joint).LocalAnchorB = ReadVector(sn); + break; + case "axis": + ((PrismaticJoint)joint).Axis = ReadVector(sn); + break; + case "maxmotorforce": + ((PrismaticJoint)joint).MaxMotorForce = float.Parse(sn.Value); + break; + case "motorspeed": + ((PrismaticJoint)joint).MotorSpeed = float.Parse(sn.Value); + break; + case "lowertranslation": + ((PrismaticJoint)joint).LowerLimit = float.Parse(sn.Value); + break; + case "uppertranslation": + ((PrismaticJoint)joint).UpperLimit = float.Parse(sn.Value); + break; + case "referenceangle": + ((PrismaticJoint)joint).ReferenceAngle = float.Parse(sn.Value); + break; + } + } + break; + case JointType.Pulley: + { + switch (sn.Name.ToLower()) + { + case "worldanchora": + ((PulleyJoint)joint).WorldAnchorA = ReadVector(sn); + break; + case "worldanchorb": + ((PulleyJoint)joint).WorldAnchorB = ReadVector(sn); + break; + case "lengtha": + ((PulleyJoint)joint).LengthA = float.Parse(sn.Value); + break; + case "lengthb": + ((PulleyJoint)joint).LengthB = float.Parse(sn.Value); + break; + case "localanchora": + ((PulleyJoint)joint).LocalAnchorA = ReadVector(sn); + break; + case "localanchorb": + ((PulleyJoint)joint).LocalAnchorB = ReadVector(sn); + break; + case "ratio": + ((PulleyJoint)joint).Ratio = float.Parse(sn.Value); + break; + case "constant": + ((PulleyJoint)joint).Constant = float.Parse(sn.Value); + break; + } + } + break; + case JointType.Revolute: + { + switch (sn.Name.ToLower()) + { + case "enablelimit": + ((RevoluteJoint)joint).LimitEnabled = bool.Parse(sn.Value); + break; + case "enablemotor": + ((RevoluteJoint)joint).MotorEnabled = bool.Parse(sn.Value); + break; + case "localanchora": + ((RevoluteJoint)joint).LocalAnchorA = ReadVector(sn); + break; + case "localanchorb": + ((RevoluteJoint)joint).LocalAnchorB = ReadVector(sn); + break; + case "maxmotortorque": + ((RevoluteJoint)joint).MaxMotorTorque = float.Parse(sn.Value); + break; + case "motorspeed": + ((RevoluteJoint)joint).MotorSpeed = float.Parse(sn.Value); + break; + case "lowerangle": + ((RevoluteJoint)joint).LowerLimit = float.Parse(sn.Value); + break; + case "upperangle": + ((RevoluteJoint)joint).UpperLimit = float.Parse(sn.Value); + break; + case "referenceangle": + ((RevoluteJoint)joint).ReferenceAngle = float.Parse(sn.Value); + break; + } + } + break; + case JointType.Weld: + { + switch (sn.Name.ToLower()) + { + case "localanchora": + ((WeldJoint)joint).LocalAnchorA = ReadVector(sn); + break; + case "localanchorb": + ((WeldJoint)joint).LocalAnchorB = ReadVector(sn); + break; + } + } + break; + case JointType.Rope: + { + switch (sn.Name.ToLower()) + { + case "localanchora": + ((RopeJoint)joint).LocalAnchorA = ReadVector(sn); + break; + case "localanchorb": + ((RopeJoint)joint).LocalAnchorB = ReadVector(sn); + break; + case "maxlength": + ((RopeJoint)joint).MaxLength = float.Parse(sn.Value); + break; + } + } + break; + case JointType.Gear: + throw new Exception("Gear joint is unsupported"); + case JointType.Angle: + { + switch (sn.Name.ToLower()) + { + case "biasfactor": + ((AngleJoint)joint).BiasFactor = float.Parse(sn.Value); + break; + case "maximpulse": + ((AngleJoint)joint).MaxImpulse = float.Parse(sn.Value); + break; + case "softness": + ((AngleJoint)joint).Softness = float.Parse(sn.Value); + break; + case "targetangle": + ((AngleJoint)joint).TargetAngle = float.Parse(sn.Value); + break; + } + } + break; + case JointType.Motor: + switch (sn.Name.ToLower()) + { + case "angularoffset": + ((MotorJoint)joint).AngularOffset = float.Parse(sn.Value); + break; + case "linearoffset": + ((MotorJoint)joint).LinearOffset = ReadVector(sn); + break; + case "maxforce": + ((MotorJoint)joint).MaxForce = float.Parse(sn.Value); + break; + case "maxtorque": + ((MotorJoint)joint).MaxTorque = float.Parse(sn.Value); + break; + case "correctionfactor": + ((MotorJoint)joint).CorrectionFactor = float.Parse(sn.Value); + break; + } + break; + } + } + } + } + } + + world.ProcessChanges(); + } + + private static Vector2 ReadVector(XMLFragmentElement node) + { + string[] values = node.Value.Split(' '); + return new Vector2(float.Parse(values[0]), float.Parse(values[1])); + } + + private static object ReadSimpleType(XMLFragmentElement node, Type type, bool outer) + { + if (type == null) + return ReadSimpleType(node.Elements[1], Type.GetType(node.Elements[0].Value), outer); + + XmlSerializer serializer = new XmlSerializer(type); + XmlSerializerNamespaces xmlnsEmpty = new XmlSerializerNamespaces(); + xmlnsEmpty.Add("", ""); + + using (MemoryStream stream = new MemoryStream()) + { + StreamWriter writer = new StreamWriter(stream); + { + writer.Write((outer) ? node.OuterXml : node.InnerXml); + writer.Flush(); + stream.Position = 0; + } + XmlReaderSettings settings = new XmlReaderSettings(); + settings.ConformanceLevel = ConformanceLevel.Fragment; + + return serializer.Deserialize(XmlReader.Create(stream, settings)); + } + } + } + + #region XMLFragment + + internal class XMLFragmentAttribute + { + public string Name { get; set; } + public string Value { get; set; } + } + + internal class XMLFragmentElement + { + private List _attributes = new List(); + private List _elements = new List(); + + public IList Elements + { + get { return _elements; } + } + + public IList Attributes + { + get { return _attributes; } + } + + public string Name { get; set; } + public string Value { get; set; } + public string OuterXml { get; set; } + public string InnerXml { get; set; } + } + + internal class XMLFragmentException : Exception + { + public XMLFragmentException(string message) + : base(message) + { + } + } + + internal class FileBuffer + { + public FileBuffer(Stream stream) + { + using (StreamReader sr = new StreamReader(stream)) + Buffer = sr.ReadToEnd(); + + Position = 0; + } + + public string Buffer { get; set; } + + public int Position { get; set; } + + private int Length + { + get { return Buffer.Length; } + } + + public char Next + { + get + { + char c = Buffer[Position]; + Position++; + return c; + } + } + + public bool EndOfBuffer + { + get { return Position == Length; } + } + } + + internal class XMLFragmentParser + { + private static List _punctuation = new List { '/', '<', '>', '=' }; + private FileBuffer _buffer; + private XMLFragmentElement _rootNode; + + public XMLFragmentParser(Stream stream) + { + Load(stream); + } + + public XMLFragmentParser(string fileName) + { + using (FileStream fs = new FileStream(fileName, FileMode.Open, FileAccess.Read)) + Load(fs); + } + + public XMLFragmentElement RootNode + { + get { return _rootNode; } + } + + public void Load(Stream stream) + { + _buffer = new FileBuffer(stream); + } + + public static XMLFragmentElement LoadFromStream(Stream stream) + { + XMLFragmentParser x = new XMLFragmentParser(stream); + x.Parse(); + return x.RootNode; + } + + private string NextToken() + { + string str = ""; + bool _done = false; + + while (true) + { + char c = _buffer.Next; + + if (_punctuation.Contains(c)) + { + if (str != "") + { + _buffer.Position--; + break; + } + + _done = true; + } + else if (char.IsWhiteSpace(c)) + { + if (str != "") + break; + else + continue; + } + + str += c; + + if (_done) + break; + } + + str = TrimControl(str); + + // Trim quotes from start and end + if (str[0] == '\"') + str = str.Remove(0, 1); + + if (str[str.Length - 1] == '\"') + str = str.Remove(str.Length - 1, 1); + + return str; + } + + private string PeekToken() + { + int oldPos = _buffer.Position; + string str = NextToken(); + _buffer.Position = oldPos; + return str; + } + + private string ReadUntil(char c) + { + string str = ""; + + while (true) + { + char ch = _buffer.Next; + + if (ch == c) + { + _buffer.Position--; + break; + } + + str += ch; + } + + // Trim quotes from start and end + if (str[0] == '\"') + str = str.Remove(0, 1); + + if (str[str.Length - 1] == '\"') + str = str.Remove(str.Length - 1, 1); + + return str; + } + + private string TrimControl(string str) + { + string newStr = str; + + // Trim control characters + int i = 0; + while (true) + { + if (i == newStr.Length) + break; + + if (char.IsControl(newStr[i])) + newStr = newStr.Remove(i, 1); + else + i++; + } + + return newStr; + } + + private string TrimTags(string outer) + { + int start = outer.IndexOf('>') + 1; + int end = outer.LastIndexOf('<'); + + return TrimControl(outer.Substring(start, end - start)); + } + + public XMLFragmentElement TryParseNode() + { + if (_buffer.EndOfBuffer) + return null; + + int startOuterXml = _buffer.Position; + string token = NextToken(); + + if (token != "<") + throw new XMLFragmentException("Expected \"<\", got " + token); + + XMLFragmentElement element = new XMLFragmentElement(); + element.Name = NextToken(); + + while (true) + { + token = NextToken(); + + if (token == ">") + break; + else if (token == "/") // quick-exit case + { + NextToken(); + + element.OuterXml = + TrimControl(_buffer.Buffer.Substring(startOuterXml, _buffer.Position - startOuterXml)).Trim(); + element.InnerXml = ""; + + return element; + } + else + { + XMLFragmentAttribute attribute = new XMLFragmentAttribute(); + attribute.Name = token; + if ((token = NextToken()) != "=") + throw new XMLFragmentException("Expected \"=\", got " + token); + attribute.Value = NextToken(); + + element.Attributes.Add(attribute); + } + } + + while (true) + { + int oldPos = _buffer.Position; // for restoration below + token = NextToken(); + + if (token == "<") + { + token = PeekToken(); + + if (token == "/") // finish element + { + NextToken(); // skip the / again + token = NextToken(); + NextToken(); // skip > + + element.OuterXml = TrimControl(_buffer.Buffer.Substring(startOuterXml, _buffer.Position - startOuterXml)).Trim(); + element.InnerXml = TrimTags(element.OuterXml); + + if (token != element.Name) + throw new XMLFragmentException("Mismatched element pairs: \"" + element.Name + "\" vs \"" + + token + "\""); + + break; + } + else + { + _buffer.Position = oldPos; + element.Elements.Add(TryParseNode()); + } + } + else + { + // value, probably + _buffer.Position = oldPos; + element.Value = ReadUntil('<'); + } + } + + return element; + } + + private void Parse() + { + _rootNode = TryParseNode(); + + if (_rootNode == null) + throw new XMLFragmentException("Unable to load root node"); + } + } + + #endregion +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Stopwatch.cs b/Farseer Physics Engine 3.5/Common/Stopwatch.cs new file mode 100644 index 000000000..64c72a217 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Stopwatch.cs @@ -0,0 +1,112 @@ +#if SILVERLIGHT +using System; + +namespace FarseerPhysics.Common +{ + // Source: http://www.wiredprairie.us/blog/index.php/archives/723 + + /// + /// An emulation for the Stopwatch class for Windows Phone and Silverlight. + /// + public sealed class Stopwatch + { + private long _startTick; + private long _elapsed; + private bool _isRunning; + + /// + /// Creates a new instance of the class and starts the watch immediately. + /// + /// An instance of Stopwatch, running. + public static Stopwatch StartNew() + { + Stopwatch sw = new Stopwatch(); + sw.Start(); + return sw; + } + + /// + /// Creates an instance of the Stopwatch class. + /// + public Stopwatch() { } + + /// + /// Completely resets and deactivates the timer. + /// + public void Reset() + { + _elapsed = 0; + _isRunning = false; + _startTick = 0; + } + + /// + /// Begins the timer. + /// + public void Start() + { + if (!_isRunning) + { + _startTick = GetCurrentTicks(); + _isRunning = true; + } + } + + /// + /// Stops the current timer. + /// + public void Stop() + { + if (_isRunning) + { + _elapsed += GetCurrentTicks() - _startTick; + _isRunning = false; + } + } + + /// + /// Gets a value indicating whether the instance is currently recording. + /// + public bool IsRunning + { + get { return _isRunning; } + } + + /// + /// Gets the Elapsed time as a Timespan. + /// + public TimeSpan Elapsed + { + get { return TimeSpan.FromMilliseconds(ElapsedMilliseconds); } + } + + /// + /// Gets the Elapsed time as the total number of milliseconds. + /// + public long ElapsedMilliseconds + { + get { return GetCurrentElapsedTicks() / TimeSpan.TicksPerMillisecond; } + } + + /// + /// Gets the Elapsed time as the total number of ticks (which is faked + /// as Silverlight doesn't have a way to get at the actual "Ticks") + /// + public long ElapsedTicks + { + get { return GetCurrentElapsedTicks(); } + } + + private long GetCurrentElapsedTicks() + { + return _elapsed + (IsRunning ? (GetCurrentTicks() - _startTick) : 0); + } + + private long GetCurrentTicks() + { + // TickCount: Gets the number of milliseconds elapsed since the system started. + return Environment.TickCount * TimeSpan.TicksPerMillisecond; + } + } +} +#endif \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/TextureTools/MarchingSquares.cs b/Farseer Physics Engine 3.5/Common/TextureTools/MarchingSquares.cs new file mode 100644 index 000000000..ca587213b --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/TextureTools/MarchingSquares.cs @@ -0,0 +1,800 @@ +using System.Collections.Generic; +using FarseerPhysics.Collision; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common.TextureTools +{ + // Ported by Matthew Bettcher - Feb 2011 + + /* + Copyright (c) 2010, Luca Deltodesco + All rights reserved. + + Redistribution and use in source and binary forms, with or without modification, are permitted + provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this list of conditions + and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, this list of + conditions and the following disclaimer in the documentation and/or other materials provided + with the distribution. + * Neither the name of the nape project nor the names of its contributors may be used to endorse + or promote products derived from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR + IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND + FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER + IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT + OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + + public static class MarchingSquares + { + /// + /// Marching squares over the given domain using the mesh defined via the dimensions + /// (wid,hei) to build a set of polygons such that f(x,y) less than 0, using the given number + /// 'bin' for recursive linear inteprolation along cell boundaries. + /// + /// if 'comb' is true, then the polygons will also be composited into larger possible concave + /// polygons. + /// + /// + /// + /// + /// + /// + /// + /// + public static List DetectSquares(AABB domain, float cellWidth, float cellHeight, sbyte[,] f, + int lerpCount, bool combine) + { + CxFastList ret = new CxFastList(); + + List verticesList = new List(); + + //NOTE: removed assignments as they were not used. + List polyList; + GeomPoly gp; + + int xn = (int)(domain.Extents.X * 2 / cellWidth); + bool xp = xn == (domain.Extents.X * 2 / cellWidth); + int yn = (int)(domain.Extents.Y * 2 / cellHeight); + bool yp = yn == (domain.Extents.Y * 2 / cellHeight); + if (!xp) xn++; + if (!yp) yn++; + + sbyte[,] fs = new sbyte[xn + 1, yn + 1]; + GeomPolyVal[,] ps = new GeomPolyVal[xn + 1, yn + 1]; + + //populate shared function lookups. + for (int x = 0; x < xn + 1; x++) + { + int x0; + if (x == xn) x0 = (int)domain.UpperBound.X; + else x0 = (int)(x * cellWidth + domain.LowerBound.X); + for (int y = 0; y < yn + 1; y++) + { + int y0; + if (y == yn) y0 = (int)domain.UpperBound.Y; + else y0 = (int)(y * cellHeight + domain.LowerBound.Y); + fs[x, y] = f[x0, y0]; + } + } + + //generate sub-polys and combine to scan lines + for (int y = 0; y < yn; y++) + { + float y0 = y * cellHeight + domain.LowerBound.Y; + float y1; + if (y == yn - 1) y1 = domain.UpperBound.Y; + else y1 = y0 + cellHeight; + GeomPoly pre = null; + for (int x = 0; x < xn; x++) + { + float x0 = x * cellWidth + domain.LowerBound.X; + float x1; + if (x == xn - 1) x1 = domain.UpperBound.X; + else x1 = x0 + cellWidth; + + gp = new GeomPoly(); + + int key = MarchSquare(f, fs, ref gp, x, y, x0, y0, x1, y1, lerpCount); + if (gp.Length != 0) + { + if (combine && pre != null && (key & 9) != 0) + { + combLeft(ref pre, ref gp); + gp = pre; + } + else + ret.Add(gp); + ps[x, y] = new GeomPolyVal(gp, key); + } + else + gp = null; + pre = gp; + } + } + if (!combine) + { + polyList = ret.GetListOfElements(); + + foreach (GeomPoly poly in polyList) + { + verticesList.Add(new Vertices(poly.Points.GetListOfElements())); + } + + return verticesList; + } + + //combine scan lines together + for (int y = 1; y < yn; y++) + { + int x = 0; + while (x < xn) + { + GeomPolyVal p = ps[x, y]; + + //skip along scan line if no polygon exists at this point + if (p == null) + { + x++; + continue; + } + + //skip along if current polygon cannot be combined above. + if ((p.Key & 12) == 0) + { + x++; + continue; + } + + //skip along if no polygon exists above. + GeomPolyVal u = ps[x, y - 1]; + if (u == null) + { + x++; + continue; + } + + //skip along if polygon above cannot be combined with. + if ((u.Key & 3) == 0) + { + x++; + continue; + } + + float ax = x * cellWidth + domain.LowerBound.X; + float ay = y * cellHeight + domain.LowerBound.Y; + + CxFastList bp = p.GeomP.Points; + CxFastList ap = u.GeomP.Points; + + //skip if it's already been combined with above polygon + if (u.GeomP == p.GeomP) + { + x++; + continue; + } + + //combine above (but disallow the hole thingies + CxFastListNode bi = bp.Begin(); + while (Square(bi.Elem().Y - ay) > Settings.Epsilon || bi.Elem().X < ax) bi = bi.Next(); + + //NOTE: Unused + //Vector2 b0 = bi.elem(); + Vector2 b1 = bi.Next().Elem(); + if (Square(b1.Y - ay) > Settings.Epsilon) + { + x++; + continue; + } + + bool brk = true; + CxFastListNode ai = ap.Begin(); + while (ai != ap.End()) + { + if (VecDsq(ai.Elem(), b1) < Settings.Epsilon) + { + brk = false; + break; + } + ai = ai.Next(); + } + if (brk) + { + x++; + continue; + } + + CxFastListNode bj = bi.Next().Next(); + if (bj == bp.End()) bj = bp.Begin(); + while (bj != bi) + { + ai = ap.Insert(ai, bj.Elem()); // .clone() + bj = bj.Next(); + if (bj == bp.End()) bj = bp.Begin(); + u.GeomP.Length++; + } + //u.p.simplify(float.Epsilon,float.Epsilon); + // + ax = x + 1; + while (ax < xn) + { + GeomPolyVal p2 = ps[(int)ax, y]; + if (p2 == null || p2.GeomP != p.GeomP) + { + ax++; + continue; + } + p2.GeomP = u.GeomP; + ax++; + } + ax = x - 1; + while (ax >= 0) + { + GeomPolyVal p2 = ps[(int)ax, y]; + if (p2 == null || p2.GeomP != p.GeomP) + { + ax--; + continue; + } + p2.GeomP = u.GeomP; + ax--; + } + ret.Remove(p.GeomP); + p.GeomP = u.GeomP; + + x = (int)((bi.Next().Elem().X - domain.LowerBound.X) / cellWidth) + 1; + //x++; this was already commented out! + } + } + + polyList = ret.GetListOfElements(); + + foreach (GeomPoly poly in polyList) + { + verticesList.Add(new Vertices(poly.Points.GetListOfElements())); + } + + return verticesList; + } + + #region Private Methods + + //- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + /** Linearly interpolate between (x0 to x1) given a value at these coordinates (v0 and v1) + such as to approximate value(return) = 0 + **/ + + private static int[] _lookMarch = { + 0x00, 0xE0, 0x38, 0xD8, 0x0E, 0xEE, 0x36, 0xD6, 0x83, 0x63, 0xBB, 0x5B, 0x8D, + 0x6D, 0xB5, 0x55 + }; + + private static float Lerp(float x0, float x1, float v0, float v1) + { + float dv = v0 - v1; + float t; + if (dv * dv < Settings.Epsilon) + t = 0.5f; + else t = v0 / dv; + return x0 + t * (x1 - x0); + } + + //- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + /** Recursive linear interpolation for use in marching squares **/ + + private static float Xlerp(float x0, float x1, float y, float v0, float v1, sbyte[,] f, int c) + { + float xm = Lerp(x0, x1, v0, v1); + if (c == 0) + return xm; + + sbyte vm = f[(int)xm, (int)y]; + + if (v0 * vm < 0) + return Xlerp(x0, xm, y, v0, vm, f, c - 1); + + return Xlerp(xm, x1, y, vm, v1, f, c - 1); + } + + /** Recursive linear interpolation for use in marching squares **/ + + private static float Ylerp(float y0, float y1, float x, float v0, float v1, sbyte[,] f, int c) + { + float ym = Lerp(y0, y1, v0, v1); + if (c == 0) + return ym; + + sbyte vm = f[(int)x, (int)ym]; + + if (v0 * vm < 0) + return Ylerp(y0, ym, x, v0, vm, f, c - 1); + + return Ylerp(ym, y1, x, vm, v1, f, c - 1); + } + + /** Square value for use in marching squares **/ + + private static float Square(float x) + { + return x * x; + } + + private static float VecDsq(Vector2 a, Vector2 b) + { + Vector2 d = a - b; + return d.X * d.X + d.Y * d.Y; + } + + private static float VecCross(Vector2 a, Vector2 b) + { + return a.X * b.Y - a.Y * b.X; + } + + /** Look-up table to relate polygon key with the vertices that should be used for + the sub polygon in marching squares + **/ + + /** Perform a single celled marching square for for the given cell defined by (x0,y0) (x1,y1) + using the function f for recursive interpolation, given the look-up table 'fs' of + the values of 'f' at cell vertices with the result to be stored in 'poly' given the actual + coordinates of 'ax' 'ay' in the marching squares mesh. + **/ + + private static int MarchSquare(sbyte[,] f, sbyte[,] fs, ref GeomPoly poly, int ax, int ay, float x0, float y0, + float x1, float y1, int bin) + { + //key lookup + int key = 0; + sbyte v0 = fs[ax, ay]; + if (v0 < 0) key |= 8; + sbyte v1 = fs[ax + 1, ay]; + if (v1 < 0) key |= 4; + sbyte v2 = fs[ax + 1, ay + 1]; + if (v2 < 0) key |= 2; + sbyte v3 = fs[ax, ay + 1]; + if (v3 < 0) key |= 1; + + int val = _lookMarch[key]; + if (val != 0) + { + CxFastListNode pi = null; + for (int i = 0; i < 8; i++) + { + Vector2 p; + if ((val & (1 << i)) != 0) + { + if (i == 7 && (val & 1) == 0) + poly.Points.Add(p = new Vector2(x0, Ylerp(y0, y1, x0, v0, v3, f, bin))); + else + { + if (i == 0) p = new Vector2(x0, y0); + else if (i == 2) p = new Vector2(x1, y0); + else if (i == 4) p = new Vector2(x1, y1); + else if (i == 6) p = new Vector2(x0, y1); + + else if (i == 1) p = new Vector2(Xlerp(x0, x1, y0, v0, v1, f, bin), y0); + else if (i == 5) p = new Vector2(Xlerp(x0, x1, y1, v3, v2, f, bin), y1); + + else if (i == 3) p = new Vector2(x1, Ylerp(y0, y1, x1, v1, v2, f, bin)); + else p = new Vector2(x0, Ylerp(y0, y1, x0, v0, v3, f, bin)); + + pi = poly.Points.Insert(pi, p); + } + poly.Length++; + } + } + //poly.simplify(float.Epsilon,float.Epsilon); + } + return key; + } + + /** Used in polygon composition to composit polygons into scan lines + Combining polya and polyb into one super-polygon stored in polya. + **/ + + private static void combLeft(ref GeomPoly polya, ref GeomPoly polyb) + { + CxFastList ap = polya.Points; + CxFastList bp = polyb.Points; + CxFastListNode ai = ap.Begin(); + CxFastListNode bi = bp.Begin(); + + Vector2 b = bi.Elem(); + CxFastListNode prea = null; + while (ai != ap.End()) + { + Vector2 a = ai.Elem(); + if (VecDsq(a, b) < Settings.Epsilon) + { + //ignore shared vertex if parallel + if (prea != null) + { + Vector2 a0 = prea.Elem(); + b = bi.Next().Elem(); + + Vector2 u = a - a0; + //vec_new(u); vec_sub(a.p.p, a0.p.p, u); + Vector2 v = b - a; + //vec_new(v); vec_sub(b.p.p, a.p.p, v); + float dot = VecCross(u, v); + if (dot * dot < Settings.Epsilon) + { + ap.Erase(prea, ai); + polya.Length--; + ai = prea; + } + } + + //insert polyb into polya + bool fst = true; + CxFastListNode preb = null; + while (!bp.Empty()) + { + Vector2 bb = bp.Front(); + bp.Pop(); + if (!fst && !bp.Empty()) + { + ai = ap.Insert(ai, bb); + polya.Length++; + preb = ai; + } + fst = false; + } + + //ignore shared vertex if parallel + ai = ai.Next(); + Vector2 a1 = ai.Elem(); + ai = ai.Next(); + if (ai == ap.End()) ai = ap.Begin(); + Vector2 a2 = ai.Elem(); + Vector2 a00 = preb.Elem(); + Vector2 uu = a1 - a00; + //vec_new(u); vec_sub(a1.p, a0.p, u); + Vector2 vv = a2 - a1; + //vec_new(v); vec_sub(a2.p, a1.p, v); + float dot1 = VecCross(uu, vv); + if (dot1 * dot1 < Settings.Epsilon) + { + ap.Erase(preb, preb.Next()); + polya.Length--; + } + + return; + } + prea = ai; + ai = ai.Next(); + } + } + + #endregion + + #region CxFastList from nape physics + + #region Nested type: CxFastList + + /// + /// Designed as a complete port of CxFastList from CxStd. + /// + internal class CxFastList + { + // first node in the list + private CxFastListNode _head; + private int _count; + + /// + /// Iterator to start of list (O(1)) + /// + public CxFastListNode Begin() + { + return _head; + } + + /// + /// Iterator to end of list (O(1)) + /// + public CxFastListNode End() + { + return null; + } + + /// + /// Returns first element of list (O(1)) + /// + public T Front() + { + return _head.Elem(); + } + + /// + /// add object to list (O(1)) + /// + public CxFastListNode Add(T value) + { + CxFastListNode newNode = new CxFastListNode(value); + if (_head == null) + { + newNode._next = null; + _head = newNode; + _count++; + return newNode; + } + newNode._next = _head; + _head = newNode; + + _count++; + + return newNode; + } + + /// + /// remove object from list, returns true if an element was removed (O(n)) + /// + public bool Remove(T value) + { + CxFastListNode head = _head; + CxFastListNode prev = _head; + + EqualityComparer comparer = EqualityComparer.Default; + + if (head != null) + { + if (value != null) + { + do + { + // if we are on the value to be removed + if (comparer.Equals(head._elt, value)) + { + // then we need to patch the list + // check to see if we are removing the _head + if (head == _head) + { + _head = head._next; + _count--; + return true; + } + else + { + // were not at the head + prev._next = head._next; + _count--; + return true; + } + } + // cache the current as the previous for the next go around + prev = head; + head = head._next; + } while (head != null); + } + } + return false; + } + + /// + /// pop element from head of list (O(1)) Note: this does not return the object popped! + /// There is good reason to this, and it regards the Alloc list variants which guarantee + /// objects are released to the object pool. You do not want to retrieve an element + /// through pop or else that object may suddenly be used by another piece of code which + /// retrieves it from the object pool. + /// + public CxFastListNode Pop() + { + return Erase(null, _head); + } + + /// + /// insert object after 'node' returning an iterator to the inserted object. + /// + public CxFastListNode Insert(CxFastListNode node, T value) + { + if (node == null) + { + return Add(value); + } + CxFastListNode newNode = new CxFastListNode(value); + CxFastListNode nextNode = node._next; + newNode._next = nextNode; + node._next = newNode; + + _count++; + + return newNode; + } + + /// + /// removes the element pointed to by 'node' with 'prev' being the previous iterator, + /// returning an iterator to the element following that of 'node' (O(1)) + /// + public CxFastListNode Erase(CxFastListNode prev, CxFastListNode node) + { + // cache the node after the node to be removed + CxFastListNode nextNode = node._next; + if (prev != null) + prev._next = nextNode; + else if (_head != null) + _head = _head._next; + else + return null; + + _count--; + return nextNode; + } + + /// + /// whether the list is empty (O(1)) + /// + public bool Empty() + { + if (_head == null) + return true; + return false; + } + + /// + /// computes size of list (O(n)) + /// + public int Size() + { + CxFastListNode i = Begin(); + int count = 0; + + do + { + count++; + } while (i.Next() != null); + + return count; + } + + /// + /// empty the list (O(1) if CxMixList, O(n) otherwise) + /// + public void Clear() + { + CxFastListNode head = _head; + while (head != null) + { + CxFastListNode node2 = head; + head = head._next; + node2._next = null; + } + _head = null; + _count = 0; + } + + /// + /// returns true if 'value' is an element of the list (O(n)) + /// + public bool Has(T value) + { + return (Find(value) != null); + } + + // Non CxFastList Methods + public CxFastListNode Find(T value) + { + // start at head + CxFastListNode head = _head; + EqualityComparer comparer = EqualityComparer.Default; + if (head != null) + { + if (value != null) + { + do + { + if (comparer.Equals(head._elt, value)) + { + return head; + } + head = head._next; + } while (head != _head); + } + else + { + do + { + if (head._elt == null) + { + return head; + } + head = head._next; + } while (head != _head); + } + } + return null; + } + + public List GetListOfElements() + { + List list = new List(); + + CxFastListNode iter = Begin(); + + if (iter != null) + { + do + { + list.Add(iter._elt); + iter = iter._next; + } while (iter != null); + } + return list; + } + } + + #endregion + + #region Nested type: CxFastListNode + + internal class CxFastListNode + { + internal T _elt; + internal CxFastListNode _next; + + public CxFastListNode(T obj) + { + _elt = obj; + } + + public T Elem() + { + return _elt; + } + + public CxFastListNode Next() + { + return _next; + } + } + + #endregion + + #endregion + + #region Internal Stuff + + #region Nested type: GeomPoly + + internal class GeomPoly + { + public int Length; + public CxFastList Points; + + public GeomPoly() + { + Points = new CxFastList(); + Length = 0; + } + } + + #endregion + + #region Nested type: GeomPolyVal + + private class GeomPolyVal + { + /** Associated polygon at coordinate **/ + /** Key of original sub-polygon **/ + public int Key; + public GeomPoly GeomP; + + public GeomPolyVal(GeomPoly geomP, int K) + { + GeomP = geomP; + Key = K; + } + } + + #endregion + + #endregion + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/TextureTools/Terrain.cs b/Farseer Physics Engine 3.5/Common/TextureTools/Terrain.cs new file mode 100644 index 000000000..a817b3284 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/TextureTools/Terrain.cs @@ -0,0 +1,264 @@ +using System.Collections.Generic; +using FarseerPhysics.Collision; +using FarseerPhysics.Common.Decomposition; +using FarseerPhysics.Common.PolygonManipulation; +using FarseerPhysics.Dynamics; +using FarseerPhysics.Factories; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common.TextureTools +{ + /// + /// Simple class to maintain a terrain. It can keep track + /// + public class Terrain + { + /// + /// World to manage terrain in. + /// + public World World; + + /// + /// Center of terrain in world units. + /// + public Vector2 Center; + + /// + /// Width of terrain in world units. + /// + public float Width; + + /// + /// Height of terrain in world units. + /// + public float Height; + + /// + /// Points per each world unit used to define the terrain in the point cloud. + /// + public int PointsPerUnit; + + /// + /// Points per cell. + /// + public int CellSize; + + /// + /// Points per sub cell. + /// + public int SubCellSize; + + /// + /// Number of iterations to perform in the Marching Squares algorithm. + /// Note: More then 3 has almost no effect on quality. + /// + public int Iterations = 2; + + /// + /// Decomposer to use when regenerating terrain. Can be changed on the fly without consequence. + /// Note: Some decomposerers are unstable. + /// + public TriangulationAlgorithm Decomposer; + + /// + /// Point cloud defining the terrain. + /// + private sbyte[,] _terrainMap; + + /// + /// Generated bodies. + /// + private List[,] _bodyMap; + + private float _localWidth; + private float _localHeight; + private int _xnum; + private int _ynum; + private AABB _dirtyArea; + private Vector2 _topLeft; + + /// + /// Creates a new terrain. + /// + /// The World + /// The area of the terrain. + public Terrain(World world, AABB area) + { + World = world; + Width = area.Width; + Height = area.Height; + Center = area.Center; + } + + /// + /// Creates a new terrain + /// + /// The World + /// The position (center) of the terrain. + /// The width of the terrain. + /// The height of the terrain. + public Terrain(World world, Vector2 position, float width, float height) + { + World = world; + Width = width; + Height = height; + Center = position; + } + + /// + /// Initialize the terrain for use. + /// + public void Initialize() + { + // find top left of terrain in world space + _topLeft = new Vector2(Center.X - (Width * 0.5f), Center.Y - (-Height * 0.5f)); + + // convert the terrains size to a point cloud size + _localWidth = Width * PointsPerUnit; + _localHeight = Height * PointsPerUnit; + + _terrainMap = new sbyte[(int)_localWidth + 1, (int)_localHeight + 1]; + + for (int x = 0; x < _localWidth; x++) + { + for (int y = 0; y < _localHeight; y++) + { + _terrainMap[x, y] = 1; + } + } + + _xnum = (int)(_localWidth / CellSize); + _ynum = (int)(_localHeight / CellSize); + _bodyMap = new List[_xnum, _ynum]; + + // make sure to mark the dirty area to an infinitely small box + _dirtyArea = new AABB(new Vector2(float.MaxValue, float.MaxValue), new Vector2(float.MinValue, float.MinValue)); + } + + /// + /// Apply the specified texture data to the terrain. + /// + /// + /// + public void ApplyData(sbyte[,] data, Vector2 offset = default(Vector2)) + { + for (int x = 0; x < data.GetUpperBound(0); x++) + { + for (int y = 0; y < data.GetUpperBound(1); y++) + { + if (x + offset.X >= 0 && x + offset.X < _localWidth && y + offset.Y >= 0 && y + offset.Y < _localHeight) + { + _terrainMap[(int)(x + offset.X), (int)(y + offset.Y)] = data[x, y]; + } + } + } + + RemoveOldData(0, _xnum, 0, _ynum); + } + + /// + /// Modify a single point in the terrain. + /// + /// World location to modify. Automatically clipped. + /// -1 = inside terrain, 1 = outside terrain + public void ModifyTerrain(Vector2 location, sbyte value) + { + // find local position + // make position local to map space + Vector2 p = location - _topLeft; + + // find map position for each axis + p.X = p.X * _localWidth / Width; + p.Y = p.Y * -_localHeight / Height; + + if (p.X >= 0 && p.X < _localWidth && p.Y >= 0 && p.Y < _localHeight) + { + _terrainMap[(int)p.X, (int)p.Y] = value; + + // expand dirty area + if (p.X < _dirtyArea.LowerBound.X) _dirtyArea.LowerBound.X = p.X; + if (p.X > _dirtyArea.UpperBound.X) _dirtyArea.UpperBound.X = p.X; + + if (p.Y < _dirtyArea.LowerBound.Y) _dirtyArea.LowerBound.Y = p.Y; + if (p.Y > _dirtyArea.UpperBound.Y) _dirtyArea.UpperBound.Y = p.Y; + } + } + + /// + /// Regenerate the terrain. + /// + public void RegenerateTerrain() + { + //iterate effected cells + int xStart = (int)(_dirtyArea.LowerBound.X / CellSize); + if (xStart < 0) xStart = 0; + + int xEnd = (int)(_dirtyArea.UpperBound.X / CellSize) + 1; + if (xEnd > _xnum) xEnd = _xnum; + + int yStart = (int)(_dirtyArea.LowerBound.Y / CellSize); + if (yStart < 0) yStart = 0; + + int yEnd = (int)(_dirtyArea.UpperBound.Y / CellSize) + 1; + if (yEnd > _ynum) yEnd = _ynum; + + RemoveOldData(xStart, xEnd, yStart, yEnd); + + _dirtyArea = new AABB(new Vector2(float.MaxValue, float.MaxValue), new Vector2(float.MinValue, float.MinValue)); + } + + private void RemoveOldData(int xStart, int xEnd, int yStart, int yEnd) + { + for (int x = xStart; x < xEnd; x++) + { + for (int y = yStart; y < yEnd; y++) + { + //remove old terrain object at grid cell + if (_bodyMap[x, y] != null) + { + for (int i = 0; i < _bodyMap[x, y].Count; i++) + { + World.RemoveBody(_bodyMap[x, y][i]); + } + } + + _bodyMap[x, y] = null; + + //generate new one + GenerateTerrain(x, y); + } + } + } + + private void GenerateTerrain(int gx, int gy) + { + float ax = gx * CellSize; + float ay = gy * CellSize; + + List polys = MarchingSquares.DetectSquares(new AABB(new Vector2(ax, ay), new Vector2(ax + CellSize, ay + CellSize)), SubCellSize, SubCellSize, _terrainMap, Iterations, true); + if (polys.Count == 0) return; + + _bodyMap[gx, gy] = new List(); + + // create the scale vector + Vector2 scale = new Vector2(1f / PointsPerUnit, 1f / -PointsPerUnit); + + // create physics object for this grid cell + foreach (Vertices item in polys) + { + // does this need to be negative? + item.Scale(ref scale); + item.Translate(ref _topLeft); + Vertices simplified = SimplifyTools.CollinearSimplify(item); + + List decompPolys = Triangulate.ConvexPartition(simplified, Decomposer); + + foreach (Vertices poly in decompPolys) + { + if (poly.Count > 2) + _bodyMap[gx, gy].Add(BodyFactory.CreatePolygon(World, poly, 1)); + } + } + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/TextureTools/TextureConverter.cs b/Farseer Physics Engine 3.5/Common/TextureTools/TextureConverter.cs new file mode 100644 index 000000000..9c4eecd97 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/TextureTools/TextureConverter.cs @@ -0,0 +1,1258 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common.TextureTools +{ + // User contribution from Sickbattery aka David Reschke. + + /// + /// The detection type affects the resulting polygon data. + /// + public enum VerticesDetectionType + { + /// + /// Holes are integrated into the main polygon. + /// + Integrated = 0, + + /// + /// The data of the main polygon and hole polygons is returned separately. + /// + Separated = 1 + } + + public sealed class TextureConverter + { + private const int ClosepixelsLength = 8; + + /// + /// This array is ment to be readonly. + /// It's not because it is accessed very frequently. + /// + private static int[,] _closePixels = new[,] { { -1, -1 }, { 0, -1 }, { 1, -1 }, { 1, 0 }, { 1, 1 }, { 0, 1 }, { -1, 1 }, { -1, 0 } }; + + private uint[] _data; + private int _dataLength; + private int _width; + private int _height; + + private VerticesDetectionType _polygonDetectionType; + + private uint _alphaTolerance; + private float _hullTolerance; + + private bool _holeDetection; + private bool _multipartDetection; + private bool _pixelOffsetOptimization; + + private Matrix _transform = Matrix.Identity; + + #region Properties + /// + /// Get or set the polygon detection type. + /// + public VerticesDetectionType PolygonDetectionType + { + get { return _polygonDetectionType; } + set { _polygonDetectionType = value; } + } + + /// + /// Will detect texture 'holes' if set to true. Slows down the detection. Default is false. + /// + public bool HoleDetection + { + get { return _holeDetection; } + set { _holeDetection = value; } + } + + /// + /// Will detect texture multiple 'solid' isles if set to true. Slows down the detection. Default is false. + /// + public bool MultipartDetection + { + get { return _multipartDetection; } + set { _multipartDetection = value; } + } + + /// + /// Will optimize the vertex positions along the interpolated normal between two edges about a half pixel (post processing). Default is false. + /// + public bool PixelOffsetOptimization + { + get { return _pixelOffsetOptimization; } + set { _pixelOffsetOptimization = value; } + } + + /// + /// Can be used for scaling. + /// + public Matrix Transform + { + get { return _transform; } + set { _transform = value; } + } + + /// + /// Alpha (coverage) tolerance. Default is 20: Every pixel with a coverage value equal or greater to 20 will be counts as solid. + /// + public byte AlphaTolerance + { + get { return (byte)(_alphaTolerance >> 24); } + set { _alphaTolerance = (uint)value << 24; } + } + + /// + /// Default is 1.5f. + /// + public float HullTolerance + { + get { return _hullTolerance; } + set + { + if (value > 4f) + { + _hullTolerance = 4f; + } + else if (value < 0.9f) + { + _hullTolerance = 0.9f; + } + else + { + _hullTolerance = value; + } + } + } + #endregion + + #region Constructors + public TextureConverter() + { + Initialize(null, null, null, null, null, null, null, null); + } + + public TextureConverter(byte? alphaTolerance, float? hullTolerance, + bool? holeDetection, bool? multipartDetection, bool? pixelOffsetOptimization, Matrix? transform) + { + Initialize(null, null, alphaTolerance, hullTolerance, holeDetection, + multipartDetection, pixelOffsetOptimization, transform); + } + + public TextureConverter(uint[] data, int width) + { + Initialize(data, width, null, null, null, null, null, null); + } + + public TextureConverter(uint[] data, int width, byte? alphaTolerance, + float? hullTolerance, bool? holeDetection, bool? multipartDetection, + bool? pixelOffsetOptimization, Matrix? transform) + { + Initialize(data, width, alphaTolerance, hullTolerance, holeDetection, + multipartDetection, pixelOffsetOptimization, transform); + } + #endregion + + #region Initialization + private void Initialize(uint[] data, int? width, byte? alphaTolerance, + float? hullTolerance, bool? holeDetection, bool? multipartDetection, + bool? pixelOffsetOptimization, Matrix? transform) + { + if (data != null && !width.HasValue) + throw new ArgumentNullException("width", "'width' can't be null if 'data' is set."); + + if (data == null && width.HasValue) + throw new ArgumentNullException("data", "'data' can't be null if 'width' is set."); + + if (data != null && width.HasValue) + SetTextureData(data, width.Value); + + if (alphaTolerance.HasValue) + AlphaTolerance = alphaTolerance.Value; + else + AlphaTolerance = 20; + + if (hullTolerance.HasValue) + HullTolerance = hullTolerance.Value; + else + HullTolerance = 1.5f; + + if (holeDetection.HasValue) + HoleDetection = holeDetection.Value; + else + HoleDetection = false; + + if (multipartDetection.HasValue) + MultipartDetection = multipartDetection.Value; + else + MultipartDetection = false; + + if (pixelOffsetOptimization.HasValue) + PixelOffsetOptimization = pixelOffsetOptimization.Value; + else + PixelOffsetOptimization = false; + + if (transform.HasValue) + Transform = transform.Value; + else + Transform = Matrix.Identity; + } + #endregion + + private void SetTextureData(uint[] data, int width) + { + if (data == null) + throw new ArgumentNullException("data", "'data' can't be null."); + + if (data.Length < 4) + throw new ArgumentOutOfRangeException("data", "'data' length can't be less then 4. Your texture must be at least 2 x 2 pixels in size."); + + if (width < 2) + throw new ArgumentOutOfRangeException("width", "'width' can't be less then 2. Your texture must be at least 2 x 2 pixels in size."); + + if (data.Length % width != 0) + throw new ArgumentException("'width' has an invalid value."); + + _data = data; + _dataLength = _data.Length; + _width = width; + _height = _dataLength / width; + } + + /// + /// Detects the vertices of the supplied texture data. (PolygonDetectionType.Integrated) + /// + /// The texture data. + /// The texture width. + /// + public static Vertices DetectVertices(uint[] data, int width) + { + TextureConverter tc = new TextureConverter(data, width); + + List detectedVerticesList = tc.DetectVertices(); + + return detectedVerticesList[0]; + } + + /// + /// Detects the vertices of the supplied texture data. + /// + /// The texture data. + /// The texture width. + /// if set to true it will perform hole detection. + /// + public static Vertices DetectVertices(uint[] data, int width, bool holeDetection) + { + TextureConverter tc = + new TextureConverter(data, width) + { + HoleDetection = holeDetection + }; + + List detectedVerticesList = tc.DetectVertices(); + + return detectedVerticesList[0]; + } + + /// + /// Detects the vertices of the supplied texture data. + /// + /// The texture data. + /// The texture width. + /// if set to true it will perform hole detection. + /// The hull tolerance. + /// The alpha tolerance. + /// if set to true it will perform multi part detection. + /// + public static List DetectVertices(uint[] data, int width, float hullTolerance, byte alphaTolerance, bool multiPartDetection, bool holeDetection) + { + TextureConverter tc = + new TextureConverter(data, width) + { + HullTolerance = hullTolerance, + AlphaTolerance = alphaTolerance, + MultipartDetection = multiPartDetection, + HoleDetection = holeDetection + }; + + List detectedVerticesList = tc.DetectVertices(); + List result = new List(); + + for (int i = 0; i < detectedVerticesList.Count; i++) + { + result.Add(detectedVerticesList[i]); + } + + return result; + } + + public List DetectVertices() + { + #region Check TextureConverter setup. + + if (_data == null) + throw new Exception("'_data' can't be null. You have to use SetTextureData(uint[] data, int width) before calling this method."); + + if (_data.Length < 4) + throw new Exception("'_data' length can't be less then 4. Your texture must be at least 2 x 2 pixels in size. " + + "You have to use SetTextureData(uint[] data, int width) before calling this method."); + + if (_width < 2) + throw new Exception("'_width' can't be less then 2. Your texture must be at least 2 x 2 pixels in size. " + + "You have to use SetTextureData(uint[] data, int width) before calling this method."); + + if (_data.Length % _width != 0) + throw new Exception("'_width' has an invalid value. You have to use SetTextureData(uint[] data, int width) before calling this method."); + + #endregion + + List detectedPolygons = new List(); + + Vector2? holeEntrance = null; + Vector2? polygonEntrance = null; + + List blackList = new List(); + + bool searchOn; + do + { + Vertices polygon; + if (detectedPolygons.Count == 0) + { + // First pass / single polygon + polygon = new Vertices(CreateSimplePolygon(Vector2.Zero, Vector2.Zero)); + + if (polygon.Count > 2) + polygonEntrance = GetTopMostVertex(polygon); + } + else if (polygonEntrance.HasValue) + { + // Multi pass / multiple polygons + polygon = new Vertices(CreateSimplePolygon(polygonEntrance.Value, new Vector2(polygonEntrance.Value.X - 1f, polygonEntrance.Value.Y))); + } + else + break; + + searchOn = false; + + + if (polygon.Count > 2) + { + if (_holeDetection) + { + do + { + holeEntrance = SearchHoleEntrance(polygon, holeEntrance); + + if (holeEntrance.HasValue) + { + if (!blackList.Contains(holeEntrance.Value)) + { + blackList.Add(holeEntrance.Value); + Vertices holePolygon = CreateSimplePolygon(holeEntrance.Value, + new Vector2(holeEntrance.Value.X + 1, holeEntrance.Value.Y)); + + if (holePolygon != null && holePolygon.Count > 2) + { + switch (_polygonDetectionType) + { + case VerticesDetectionType.Integrated: + + // Add first hole polygon vertex to close the hole polygon. + holePolygon.Add(holePolygon[0]); + + int vertex1Index, vertex2Index; + if (SplitPolygonEdge(polygon, holeEntrance.Value, out vertex1Index, out vertex2Index)) + polygon.InsertRange(vertex2Index, holePolygon); + + break; + + case VerticesDetectionType.Separated: + if (polygon.Holes == null) + polygon.Holes = new List(); + + polygon.Holes.Add(holePolygon); + break; + } + } + } + else + break; + } + else + break; + } + while (true); + } + + detectedPolygons.Add(polygon); + } + + if (_multipartDetection || polygon.Count <= 2) + { + if (SearchNextHullEntrance(detectedPolygons, polygonEntrance.Value, out polygonEntrance)) + searchOn = true; + } + } + while (searchOn); + + if (detectedPolygons == null || (detectedPolygons != null && detectedPolygons.Count == 0)) + throw new Exception("Couldn't detect any vertices."); + + // Post processing. + if (PolygonDetectionType == VerticesDetectionType.Separated) // Only when VerticesDetectionType.Separated? -> Recheck. + ApplyTriangulationCompatibleWinding(ref detectedPolygons); + + if (_transform != Matrix.Identity) + ApplyTransform(ref detectedPolygons); + + return detectedPolygons; + } + + private void ApplyTriangulationCompatibleWinding(ref List detectedPolygons) + { + for (int i = 0; i < detectedPolygons.Count; i++) + { + detectedPolygons[i].Reverse(); + + if (detectedPolygons[i].Holes != null && detectedPolygons[i].Holes.Count > 0) + { + for (int j = 0; j < detectedPolygons[i].Holes.Count; j++) + detectedPolygons[i].Holes[j].Reverse(); + } + } + } + + private void ApplyTransform(ref List detectedPolygons) + { + for (int i = 0; i < detectedPolygons.Count; i++) + detectedPolygons[i].Transform(ref _transform); + } + + #region Data[] functions + private int _tempIsSolidX; + private int _tempIsSolidY; + public bool IsSolid(ref Vector2 v) + { + _tempIsSolidX = (int)v.X; + _tempIsSolidY = (int)v.Y; + + if (_tempIsSolidX >= 0 && _tempIsSolidX < _width && _tempIsSolidY >= 0 && _tempIsSolidY < _height) + return (_data[_tempIsSolidX + _tempIsSolidY * _width] >= _alphaTolerance); + //return ((_data[_tempIsSolidX + _tempIsSolidY * _width] & 0xFF000000) >= _alphaTolerance); + + return false; + } + + public bool IsSolid(ref int x, ref int y) + { + if (x >= 0 && x < _width && y >= 0 && y < _height) + return (_data[x + y * _width] >= _alphaTolerance); + //return ((_data[x + y * _width] & 0xFF000000) >= _alphaTolerance); + + return false; + } + + public bool IsSolid(ref int index) + { + if (index >= 0 && index < _dataLength) + return (_data[index] >= _alphaTolerance); + //return ((_data[index] & 0xFF000000) >= _alphaTolerance); + + return false; + } + + public bool InBounds(ref Vector2 coord) + { + return (coord.X >= 0f && coord.X < _width && coord.Y >= 0f && coord.Y < _height); + } + #endregion + + /// + /// Function to search for an entrance point of a hole in a polygon. It searches the polygon from top to bottom between the polygon edges. + /// + /// The polygon to search in. + /// The last entrance point. + /// The next holes entrance point. Null if ther are no holes. + private Vector2? SearchHoleEntrance(Vertices polygon, Vector2? lastHoleEntrance) + { + if (polygon == null) + throw new ArgumentNullException("'polygon' can't be null."); + + if (polygon.Count < 3) + throw new ArgumentException("'polygon.MainPolygon.Count' can't be less then 3."); + + + List xCoords; + Vector2? entrance; + + int startY; + int endY; + + int lastSolid = 0; + bool foundSolid; + bool foundTransparent; + + // Set start y coordinate. + if (lastHoleEntrance.HasValue) + { + // We need the y coordinate only. + startY = (int)lastHoleEntrance.Value.Y; + } + else + { + // Start from the top of the polygon if last entrance == null. + startY = (int)GetTopMostCoord(polygon); + } + + // Set the end y coordinate. + endY = (int)GetBottomMostCoord(polygon); + + if (startY > 0 && startY < _height && endY > 0 && endY < _height) + { + // go from top to bottom of the polygon + for (int y = startY; y <= endY; y++) + { + // get x-coord of every polygon edge which crosses y + xCoords = SearchCrossingEdges(polygon, y); + + // We need an even number of crossing edges. + // It's always a pair of start and end edge: nothing | polygon | hole | polygon | nothing ... + // If it's not then don't bother, it's probably a peak ... + // ...which should be filtered out by SearchCrossingEdges() anyway. + if (xCoords.Count > 1 && xCoords.Count % 2 == 0) + { + // Ok, this is short, but probably a little bit confusing. + // This part searches from left to right between the edges inside the polygon. + // The problem: We are using the polygon data to search in the texture data. + // That's simply not accurate, but necessary because of performance. + for (int i = 0; i < xCoords.Count; i += 2) + { + foundSolid = false; + foundTransparent = false; + + // We search between the edges inside the polygon. + for (int x = (int)xCoords[i]; x <= (int)xCoords[i + 1]; x++) + { + // First pass: IsSolid might return false. + // In that case the polygon edge doesn't lie on the texture's solid pixel, because of the hull tolearance. + // If the edge lies before the first solid pixel then we need to skip our transparent pixel finds. + + // The algorithm starts to search for a relevant transparent pixel (which indicates a possible hole) + // after it has found a solid pixel. + + // After we've found a solid and a transparent pixel (a hole's left edge) + // we search for a solid pixel again (a hole's right edge). + // When found the distance of that coodrinate has to be greater then the hull tolerance. + + if (IsSolid(ref x, ref y)) + { + if (!foundTransparent) + { + foundSolid = true; + lastSolid = x; + } + + if (foundSolid && foundTransparent) + { + entrance = new Vector2(lastSolid, y); + + if (DistanceToHullAcceptable(polygon, entrance.Value, true)) + return entrance; + + entrance = null; + break; + } + } + else + { + if (foundSolid) + foundTransparent = true; + } + } + } + } + else + { + if (xCoords.Count % 2 == 0) + Debug.WriteLine("SearchCrossingEdges() % 2 != 0"); + } + } + } + + return null; + } + + private bool DistanceToHullAcceptableHoles(Vertices polygon, Vector2 point, bool higherDetail) + { + if (polygon == null) + throw new ArgumentNullException("polygon", "'polygon' can't be null."); + + if (polygon.Count < 3) + throw new ArgumentException("'polygon.MainPolygon.Count' can't be less then 3."); + + // Check the distance to main polygon. + if (DistanceToHullAcceptable(polygon, point, higherDetail)) + { + if (polygon.Holes != null) + { + for (int i = 0; i < polygon.Holes.Count; i++) + { + // If there is one distance not acceptable then return false. + if (!DistanceToHullAcceptable(polygon.Holes[i], point, higherDetail)) + return false; + } + } + + // All distances are larger then _hullTolerance. + return true; + } + + // Default to false. + return false; + } + + private bool DistanceToHullAcceptable(Vertices polygon, Vector2 point, bool higherDetail) + { + if (polygon == null) + throw new ArgumentNullException("polygon", "'polygon' can't be null."); + + if (polygon.Count < 3) + throw new ArgumentException("'polygon.Count' can't be less then 3."); + + + Vector2 edgeVertex2 = polygon[polygon.Count - 1]; + Vector2 edgeVertex1; + + if (higherDetail) + { + for (int i = 0; i < polygon.Count; i++) + { + edgeVertex1 = polygon[i]; + + if (LineTools.DistanceBetweenPointAndLineSegment(ref point, ref edgeVertex1, ref edgeVertex2) <= _hullTolerance || Vector2.Distance(point, edgeVertex1) <= _hullTolerance) + return false; + + edgeVertex2 = polygon[i]; + } + + return true; + } + else + { + for (int i = 0; i < polygon.Count; i++) + { + edgeVertex1 = polygon[i]; + + if (LineTools.DistanceBetweenPointAndLineSegment(ref point, ref edgeVertex1, ref edgeVertex2) <= _hullTolerance) + return false; + + edgeVertex2 = polygon[i]; + } + + return true; + } + } + + private bool InPolygon(Vertices polygon, Vector2 point) + { + bool inPolygon = !DistanceToHullAcceptableHoles(polygon, point, true); + + if (!inPolygon) + { + List xCoords = SearchCrossingEdgesHoles(polygon, (int)point.Y); + + if (xCoords.Count > 0 && xCoords.Count % 2 == 0) + { + for (int i = 0; i < xCoords.Count; i += 2) + { + if (xCoords[i] <= point.X && xCoords[i + 1] >= point.X) + return true; + } + } + + return false; + } + + return true; + } + + private Vector2? GetTopMostVertex(Vertices vertices) + { + float topMostValue = float.MaxValue; + Vector2? topMost = null; + + for (int i = 0; i < vertices.Count; i++) + { + if (topMostValue > vertices[i].Y) + { + topMostValue = vertices[i].Y; + topMost = vertices[i]; + } + } + + return topMost; + } + + private float GetTopMostCoord(Vertices vertices) + { + float returnValue = float.MaxValue; + + for (int i = 0; i < vertices.Count; i++) + { + if (returnValue > vertices[i].Y) + { + returnValue = vertices[i].Y; + } + } + + return returnValue; + } + + private float GetBottomMostCoord(Vertices vertices) + { + float returnValue = float.MinValue; + + for (int i = 0; i < vertices.Count; i++) + { + if (returnValue < vertices[i].Y) + { + returnValue = vertices[i].Y; + } + } + + return returnValue; + } + + private List SearchCrossingEdgesHoles(Vertices polygon, int y) + { + if (polygon == null) + throw new ArgumentNullException("polygon", "'polygon' can't be null."); + + if (polygon.Count < 3) + throw new ArgumentException("'polygon.MainPolygon.Count' can't be less then 3."); + + List result = SearchCrossingEdges(polygon, y); + + if (polygon.Holes != null) + { + for (int i = 0; i < polygon.Holes.Count; i++) + { + result.AddRange(SearchCrossingEdges(polygon.Holes[i], y)); + } + } + + result.Sort(); + return result; + } + + /// + /// Searches the polygon for the x coordinates of the edges that cross the specified y coordinate. + /// + /// Polygon to search in. + /// Y coordinate to check for edges. + /// Descending sorted list of x coordinates of edges that cross the specified y coordinate. + private List SearchCrossingEdges(Vertices polygon, int y) + { + // sick-o-note: + // Used to search the x coordinates of edges in the polygon for a specific y coordinate. + // (Usualy comming from the texture data, that's why it's an int and not a float.) + + List edges = new List(); + + // current edge + Vector2 slope; + Vector2 vertex1; // i + Vector2 vertex2; // i - 1 + + // next edge + Vector2 nextSlope; + Vector2 nextVertex; // i + 1 + + bool addFind; + + if (polygon.Count > 2) + { + // There is a gap between the last and the first vertex in the vertex list. + // We will bridge that by setting the last vertex (vertex2) to the last + // vertex in the list. + vertex2 = polygon[polygon.Count - 1]; + + // We are moving along the polygon edges. + for (int i = 0; i < polygon.Count; i++) + { + vertex1 = polygon[i]; + + // Approx. check if the edge crosses our y coord. + if ((vertex1.Y >= y && vertex2.Y <= y) || + (vertex1.Y <= y && vertex2.Y >= y)) + { + // Ignore edges that are parallel to y. + if (vertex1.Y != vertex2.Y) + { + addFind = true; + slope = vertex2 - vertex1; + + // Special threatment for edges that end at the y coord. + if (vertex1.Y == y) + { + // Create preview of the next edge. + nextVertex = polygon[(i + 1) % polygon.Count]; + nextSlope = vertex1 - nextVertex; + + // Ignore peaks. + // If thwo edges are aligned like this: /\ and the y coordinate lies on the top, + // then we get the same x coord twice and we don't need that. + if (slope.Y > 0) + addFind = (nextSlope.Y <= 0); + else + addFind = (nextSlope.Y >= 0); + } + + if (addFind) + edges.Add((y - vertex1.Y) / slope.Y * slope.X + vertex1.X); // Calculate and add the x coord. + } + } + + // vertex1 becomes vertex2 :). + vertex2 = vertex1; + } + } + + edges.Sort(); + return edges; + } + + private bool SplitPolygonEdge(Vertices polygon, Vector2 coordInsideThePolygon, out int vertex1Index, out int vertex2Index) + { + Vector2 slope; + int nearestEdgeVertex1Index = 0; + int nearestEdgeVertex2Index = 0; + bool edgeFound = false; + + float shortestDistance = float.MaxValue; + + bool edgeCoordFound = false; + Vector2 foundEdgeCoord = Vector2.Zero; + + List xCoords = SearchCrossingEdges(polygon, (int)coordInsideThePolygon.Y); + + vertex1Index = 0; + vertex2Index = 0; + + foundEdgeCoord.Y = coordInsideThePolygon.Y; + + if (xCoords != null && xCoords.Count > 1 && xCoords.Count % 2 == 0) + { + float distance; + for (int i = 0; i < xCoords.Count; i++) + { + if (xCoords[i] < coordInsideThePolygon.X) + { + distance = coordInsideThePolygon.X - xCoords[i]; + + if (distance < shortestDistance) + { + shortestDistance = distance; + foundEdgeCoord.X = xCoords[i]; + + edgeCoordFound = true; + } + } + } + + if (edgeCoordFound) + { + shortestDistance = float.MaxValue; + + int edgeVertex2Index = polygon.Count - 1; + + int edgeVertex1Index; + for (edgeVertex1Index = 0; edgeVertex1Index < polygon.Count; edgeVertex1Index++) + { + Vector2 tempVector1 = polygon[edgeVertex1Index]; + Vector2 tempVector2 = polygon[edgeVertex2Index]; + distance = LineTools.DistanceBetweenPointAndLineSegment(ref foundEdgeCoord, + ref tempVector1, ref tempVector2); + if (distance < shortestDistance) + { + shortestDistance = distance; + + nearestEdgeVertex1Index = edgeVertex1Index; + nearestEdgeVertex2Index = edgeVertex2Index; + + edgeFound = true; + } + + edgeVertex2Index = edgeVertex1Index; + } + + if (edgeFound) + { + slope = polygon[nearestEdgeVertex2Index] - polygon[nearestEdgeVertex1Index]; + slope.Normalize(); + + Vector2 tempVector = polygon[nearestEdgeVertex1Index]; + distance = Vector2.Distance(tempVector, foundEdgeCoord); + + vertex1Index = nearestEdgeVertex1Index; + vertex2Index = nearestEdgeVertex1Index + 1; + + polygon.Insert(nearestEdgeVertex1Index, distance * slope + polygon[vertex1Index]); + polygon.Insert(nearestEdgeVertex1Index, distance * slope + polygon[vertex2Index]); + + return true; + } + } + } + + return false; + } + + /// + /// + /// + /// + /// + /// + private Vertices CreateSimplePolygon(Vector2 entrance, Vector2 last) + { + bool entranceFound = false; + bool endOfHull = false; + + Vertices polygon = new Vertices(32); + Vertices hullArea = new Vertices(32); + Vertices endOfHullArea = new Vertices(32); + + Vector2 current = Vector2.Zero; + + #region Entrance check + + // Get the entrance point. //todo: alle möglichkeiten testen + if (entrance == Vector2.Zero || !InBounds(ref entrance)) + { + entranceFound = SearchHullEntrance(out entrance); + + if (entranceFound) + { + current = new Vector2(entrance.X - 1f, entrance.Y); + } + } + else + { + if (IsSolid(ref entrance)) + { + if (IsNearPixel(ref entrance, ref last)) + { + current = last; + entranceFound = true; + } + else + { + Vector2 temp; + if (SearchNearPixels(false, ref entrance, out temp)) + { + current = temp; + entranceFound = true; + } + else + { + entranceFound = false; + } + } + } + } + + #endregion + + if (entranceFound) + { + polygon.Add(entrance); + hullArea.Add(entrance); + + Vector2 next = entrance; + + do + { + // Search in the pre vision list for an outstanding point. + Vector2 outstanding; + if (SearchForOutstandingVertex(hullArea, out outstanding)) + { + if (endOfHull) + { + // We have found the next pixel, but is it on the last bit of the hull? + if (endOfHullArea.Contains(outstanding)) + { + // Indeed. + polygon.Add(outstanding); + } + + // That's enough, quit. + break; + } + + // Add it and remove all vertices that don't matter anymore + // (all the vertices before the outstanding). + polygon.Add(outstanding); + hullArea.RemoveRange(0, hullArea.IndexOf(outstanding)); + } + + // Last point gets current and current gets next. Our little spider is moving forward on the hull ;). + last = current; + current = next; + + // Get the next point on hull. + if (GetNextHullPoint(ref last, ref current, out next)) + { + // Add the vertex to a hull pre vision list. + hullArea.Add(next); + } + else + { + // Quit + break; + } + + if (next == entrance && !endOfHull) + { + // It's the last bit of the hull, search on and exit at next found vertex. + endOfHull = true; + endOfHullArea.AddRange(hullArea); + + // We don't want the last vertex to be the same as the first one, because it causes the triangulation code to crash. + if (endOfHullArea.Contains(entrance)) + endOfHullArea.Remove(entrance); + } + + } while (true); + } + + return polygon; + } + + private bool SearchNearPixels(bool searchingForSolidPixel, ref Vector2 current, out Vector2 foundPixel) + { + for (int i = 0; i < ClosepixelsLength; i++) + { + int x = (int)current.X + _closePixels[i, 0]; + int y = (int)current.Y + _closePixels[i, 1]; + + if (!searchingForSolidPixel ^ IsSolid(ref x, ref y)) + { + foundPixel = new Vector2(x, y); + return true; + } + } + + // Nothing found. + foundPixel = Vector2.Zero; + return false; + } + + private bool IsNearPixel(ref Vector2 current, ref Vector2 near) + { + for (int i = 0; i < ClosepixelsLength; i++) + { + int x = (int)current.X + _closePixels[i, 0]; + int y = (int)current.Y + _closePixels[i, 1]; + + if (x >= 0 && x <= _width && y >= 0 && y <= _height) + { + if (x == (int)near.X && y == (int)near.Y) + { + return true; + } + } + } + + return false; + } + + private bool SearchHullEntrance(out Vector2 entrance) + { + // Search for first solid pixel. + for (int y = 0; y <= _height; y++) + { + for (int x = 0; x <= _width; x++) + { + if (IsSolid(ref x, ref y)) + { + entrance = new Vector2(x, y); + return true; + } + } + } + + // If there are no solid pixels. + entrance = Vector2.Zero; + return false; + } + + /// + /// Searches for the next shape. + /// + /// Already detected polygons. + /// Search start coordinate. + /// Returns the found entrance coordinate. Null if no other shapes found. + /// True if a new shape was found. + private bool SearchNextHullEntrance(List detectedPolygons, Vector2 start, out Vector2? entrance) + { + int x; + + bool foundTransparent = false; + bool inPolygon = false; + + for (int i = (int)start.X + (int)start.Y * _width; i <= _dataLength; i++) + { + if (IsSolid(ref i)) + { + if (foundTransparent) + { + x = i % _width; + entrance = new Vector2(x, (i - x) / (float)_width); + + inPolygon = false; + for (int polygonIdx = 0; polygonIdx < detectedPolygons.Count; polygonIdx++) + { + if (InPolygon(detectedPolygons[polygonIdx], entrance.Value)) + { + inPolygon = true; + break; + } + } + + if (inPolygon) + foundTransparent = false; + else + return true; + } + } + else + foundTransparent = true; + } + + entrance = null; + return false; + } + + private bool GetNextHullPoint(ref Vector2 last, ref Vector2 current, out Vector2 next) + { + int x; + int y; + + int indexOfFirstPixelToCheck = GetIndexOfFirstPixelToCheck(ref last, ref current); + int indexOfPixelToCheck; + + for (int i = 0; i < ClosepixelsLength; i++) + { + indexOfPixelToCheck = (indexOfFirstPixelToCheck + i) % ClosepixelsLength; + + x = (int)current.X + _closePixels[indexOfPixelToCheck, 0]; + y = (int)current.Y + _closePixels[indexOfPixelToCheck, 1]; + + if (x >= 0 && x < _width && y >= 0 && y <= _height) + { + if (IsSolid(ref x, ref y)) + { + next = new Vector2(x, y); + return true; + } + } + } + + next = Vector2.Zero; + return false; + } + + private bool SearchForOutstandingVertex(Vertices hullArea, out Vector2 outstanding) + { + Vector2 outstandingResult = Vector2.Zero; + bool found = false; + + if (hullArea.Count > 2) + { + int hullAreaLastPoint = hullArea.Count - 1; + + Vector2 tempVector1; + Vector2 tempVector2 = hullArea[0]; + Vector2 tempVector3 = hullArea[hullAreaLastPoint]; + + // Search between the first and last hull point. + for (int i = 1; i < hullAreaLastPoint; i++) + { + tempVector1 = hullArea[i]; + + // Check if the distance is over the one that's tolerable. + if (LineTools.DistanceBetweenPointAndLineSegment(ref tempVector1, ref tempVector2, ref tempVector3) >= _hullTolerance) + { + outstandingResult = hullArea[i]; + found = true; + break; + } + } + } + + outstanding = outstandingResult; + return found; + } + + private int GetIndexOfFirstPixelToCheck(ref Vector2 last, ref Vector2 current) + { + // .: pixel + // l: last position + // c: current position + // f: first pixel for next search + + // f . . + // l c . + // . . . + + //Calculate in which direction the last move went and decide over the next pixel to check. + switch ((int)(current.X - last.X)) + { + case 1: + switch ((int)(current.Y - last.Y)) + { + case 1: + return 1; + + case 0: + return 0; + + case -1: + return 7; + } + break; + + case 0: + switch ((int)(current.Y - last.Y)) + { + case 1: + return 2; + + case -1: + return 6; + } + break; + + case -1: + switch ((int)(current.Y - last.Y)) + { + case 1: + return 3; + + case 0: + return 4; + + case -1: + return 5; + } + break; + } + + return 0; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Vector2.cs b/Farseer Physics Engine 3.5/Common/Vector2.cs new file mode 100644 index 000000000..4bfe35167 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Vector2.cs @@ -0,0 +1,588 @@ +#if !XNA && !WINDOWS_PHONE && !XBOX && !ANDROID + +#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.Runtime.InteropServices; +using System.Text; + +namespace Microsoft.Xna.Framework +{ + [StructLayout(LayoutKind.Sequential)] + public struct Vector2 : IEquatable + { +#region Private Fields + + private static Vector2 zeroVector = new Vector2(0f, 0f); + private static Vector2 unitVector = new Vector2(1f, 1f); + private static Vector2 unitXVector = new Vector2(1f, 0f); + private static Vector2 unitYVector = new Vector2(0f, 1f); + + #endregion Private Fields + +#region Public Fields + + public float X; + public float Y; + + #endregion Public Fields + +#region Properties + + public static Vector2 Zero + { + get { return zeroVector; } + } + + public static Vector2 One + { + get { return unitVector; } + } + + public static Vector2 UnitX + { + get { return unitXVector; } + } + + public static Vector2 UnitY + { + get { return unitYVector; } + } + + #endregion Properties + +#region Constructors + + /// + /// Constructor foe standard 2D vector. + /// + /// + /// A + /// + /// + /// A + /// + public Vector2(float x, float y) + { + X = x; + Y = y; + } + + /// + /// Constructor for "square" vector. + /// + /// + /// A + /// + public Vector2(float value) + { + X = value; + Y = value; + } + + #endregion Constructors + +#region Public Methods + + public static void Reflect(ref Vector2 vector, ref Vector2 normal, out Vector2 result) + { + float dot = Dot(vector, normal); + result.X = vector.X - ((2f*dot)*normal.X); + result.Y = vector.Y - ((2f*dot)*normal.Y); + } + + public static Vector2 Reflect(Vector2 vector, Vector2 normal) + { + Vector2 result; + Reflect(ref vector, ref normal, out result); + return result; + } + + public static Vector2 Add(Vector2 value1, Vector2 value2) + { + value1.X += value2.X; + value1.Y += value2.Y; + return value1; + } + + public static void Add(ref Vector2 value1, ref Vector2 value2, out Vector2 result) + { + result.X = value1.X + value2.X; + result.Y = value1.Y + value2.Y; + } + + public static Vector2 Barycentric(Vector2 value1, Vector2 value2, Vector2 value3, float amount1, float amount2) + { + return new Vector2( + MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2), + MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2)); + } + + public static void Barycentric(ref Vector2 value1, ref Vector2 value2, ref Vector2 value3, float amount1, + float amount2, out Vector2 result) + { + result = new Vector2( + MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2), + MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2)); + } + + public static Vector2 CatmullRom(Vector2 value1, Vector2 value2, Vector2 value3, Vector2 value4, float amount) + { + return new Vector2( + MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount), + MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount)); + } + + public static void CatmullRom(ref Vector2 value1, ref Vector2 value2, ref Vector2 value3, ref Vector2 value4, + float amount, out Vector2 result) + { + result = new Vector2( + MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount), + MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount)); + } + + public static Vector2 Clamp(Vector2 value1, Vector2 min, Vector2 max) + { + return new Vector2( + MathHelper.Clamp(value1.X, min.X, max.X), + MathHelper.Clamp(value1.Y, min.Y, max.Y)); + } + + public static void Clamp(ref Vector2 value1, ref Vector2 min, ref Vector2 max, out Vector2 result) + { + result = new Vector2( + MathHelper.Clamp(value1.X, min.X, max.X), + MathHelper.Clamp(value1.Y, min.Y, max.Y)); + } + + /// + /// Returns float precison distanve between two vectors + /// + /// + /// A + /// + /// + /// A + /// + /// + /// A + /// + public static float Distance(Vector2 value1, Vector2 value2) + { + float result; + DistanceSquared(ref value1, ref value2, out result); + return (float) Math.Sqrt(result); + } + + + public static void Distance(ref Vector2 value1, ref Vector2 value2, out float result) + { + DistanceSquared(ref value1, ref value2, out result); + result = (float) Math.Sqrt(result); + } + + public static float DistanceSquared(Vector2 value1, Vector2 value2) + { + float result; + DistanceSquared(ref value1, ref value2, out result); + return result; + } + + public static void DistanceSquared(ref Vector2 value1, ref Vector2 value2, out float result) + { + result = (value1.X - value2.X)*(value1.X - value2.X) + (value1.Y - value2.Y)*(value1.Y - value2.Y); + } + + /// + /// Devide first vector with the secund vector + /// + /// + /// A + /// + /// + /// A + /// + /// + /// A + /// + public static Vector2 Divide(Vector2 value1, Vector2 value2) + { + value1.X /= value2.X; + value1.Y /= value2.Y; + return value1; + } + + public static void Divide(ref Vector2 value1, ref Vector2 value2, out Vector2 result) + { + result.X = value1.X/value2.X; + result.Y = value1.Y/value2.Y; + } + + public static Vector2 Divide(Vector2 value1, float divider) + { + float factor = 1/divider; + value1.X *= factor; + value1.Y *= factor; + return value1; + } + + public static void Divide(ref Vector2 value1, float divider, out Vector2 result) + { + float factor = 1/divider; + result.X = value1.X*factor; + result.Y = value1.Y*factor; + } + + public static float Dot(Vector2 value1, Vector2 value2) + { + return value1.X*value2.X + value1.Y*value2.Y; + } + + public static void Dot(ref Vector2 value1, ref Vector2 value2, out float result) + { + result = value1.X*value2.X + value1.Y*value2.Y; + } + + public override bool Equals(object obj) + { + return (obj is Vector2) ? this == ((Vector2) obj) : false; + } + + public bool Equals(Vector2 other) + { + return this == other; + } + + public override int GetHashCode() + { + return (int) (X + Y); + } + + public static Vector2 Hermite(Vector2 value1, Vector2 tangent1, Vector2 value2, Vector2 tangent2, float amount) + { + Vector2 result = new Vector2(); + Hermite(ref value1, ref tangent1, ref value2, ref tangent2, amount, out result); + return result; + } + + public static void Hermite(ref Vector2 value1, ref Vector2 tangent1, ref Vector2 value2, ref Vector2 tangent2, + float amount, out Vector2 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); + } + + 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 Vector2 Lerp(Vector2 value1, Vector2 value2, float amount) + { + return new Vector2( + MathHelper.Lerp(value1.X, value2.X, amount), + MathHelper.Lerp(value1.Y, value2.Y, amount)); + } + + public static void Lerp(ref Vector2 value1, ref Vector2 value2, float amount, out Vector2 result) + { + result = new Vector2( + MathHelper.Lerp(value1.X, value2.X, amount), + MathHelper.Lerp(value1.Y, value2.Y, amount)); + } + + public static Vector2 Max(Vector2 value1, Vector2 value2) + { + return new Vector2( + MathHelper.Max(value1.X, value2.X), + MathHelper.Max(value1.Y, value2.Y)); + } + + public static void Max(ref Vector2 value1, ref Vector2 value2, out Vector2 result) + { + result = new Vector2( + MathHelper.Max(value1.X, value2.X), + MathHelper.Max(value1.Y, value2.Y)); + } + + public static Vector2 Min(Vector2 value1, Vector2 value2) + { + return new Vector2( + MathHelper.Min(value1.X, value2.X), + MathHelper.Min(value1.Y, value2.Y)); + } + + public static void Min(ref Vector2 value1, ref Vector2 value2, out Vector2 result) + { + result = new Vector2( + MathHelper.Min(value1.X, value2.X), + MathHelper.Min(value1.Y, value2.Y)); + } + + public static Vector2 Multiply(Vector2 value1, Vector2 value2) + { + value1.X *= value2.X; + value1.Y *= value2.Y; + return value1; + } + + public static Vector2 Multiply(Vector2 value1, float scaleFactor) + { + value1.X *= scaleFactor; + value1.Y *= scaleFactor; + return value1; + } + + public static void Multiply(ref Vector2 value1, float scaleFactor, out Vector2 result) + { + result.X = value1.X*scaleFactor; + result.Y = value1.Y*scaleFactor; + } + + public static void Multiply(ref Vector2 value1, ref Vector2 value2, out Vector2 result) + { + result.X = value1.X*value2.X; + result.Y = value1.Y*value2.Y; + } + + public static Vector2 Negate(Vector2 value) + { + value.X = -value.X; + value.Y = -value.Y; + return value; + } + + public static void Negate(ref Vector2 value, out Vector2 result) + { + result.X = -value.X; + result.Y = -value.Y; + } + + public void Normalize() + { + Normalize(ref this, out this); + } + + public static Vector2 Normalize(Vector2 value) + { + Normalize(ref value, out value); + return value; + } + + public static void Normalize(ref Vector2 value, out Vector2 result) + { + float factor; + DistanceSquared(ref value, ref zeroVector, out factor); + factor = 1f/(float) Math.Sqrt(factor); + result.X = value.X*factor; + result.Y = value.Y*factor; + } + + public static Vector2 SmoothStep(Vector2 value1, Vector2 value2, float amount) + { + return new Vector2( + MathHelper.SmoothStep(value1.X, value2.X, amount), + MathHelper.SmoothStep(value1.Y, value2.Y, amount)); + } + + public static void SmoothStep(ref Vector2 value1, ref Vector2 value2, float amount, out Vector2 result) + { + result = new Vector2( + MathHelper.SmoothStep(value1.X, value2.X, amount), + MathHelper.SmoothStep(value1.Y, value2.Y, amount)); + } + + public static Vector2 Subtract(Vector2 value1, Vector2 value2) + { + value1.X -= value2.X; + value1.Y -= value2.Y; + return value1; + } + + public static void Subtract(ref Vector2 value1, ref Vector2 value2, out Vector2 result) + { + result.X = value1.X - value2.X; + result.Y = value1.Y - value2.Y; + } + + public static Vector2 Transform(Vector2 position, Matrix matrix) + { + Transform(ref position, ref matrix, out position); + return position; + } + + public static void Transform(ref Vector2 position, ref Matrix matrix, out Vector2 result) + { + result = new Vector2((position.X*matrix.M11) + (position.Y*matrix.M21) + matrix.M41, + (position.X*matrix.M12) + (position.Y*matrix.M22) + matrix.M42); + } + + public static void Transform(Vector2[] sourceArray, ref Matrix matrix, Vector2[] destinationArray) + { + throw new NotImplementedException(); + } + + public static void Transform(Vector2[] sourceArray, int sourceIndex, ref Matrix matrix, + Vector2[] destinationArray, int destinationIndex, int length) + { + throw new NotImplementedException(); + } + + public static Vector2 TransformNormal(Vector2 normal, Matrix matrix) + { + TransformNormal(ref normal, ref matrix, out normal); + return normal; + } + + public static void TransformNormal(ref Vector2 normal, ref Matrix matrix, out Vector2 result) + { + result = new Vector2((normal.X*matrix.M11) + (normal.Y*matrix.M21), + (normal.X*matrix.M12) + (normal.Y*matrix.M22)); + } + + public static void TransformNormal(Vector2[] sourceArray, ref Matrix matrix, Vector2[] destinationArray) + { + throw new NotImplementedException(); + } + + public static void TransformNormal(Vector2[] sourceArray, int sourceIndex, ref Matrix matrix, + Vector2[] destinationArray, int destinationIndex, int length) + { + throw new NotImplementedException(); + } + + public override string ToString() + { + StringBuilder sb = new StringBuilder(24); + sb.Append("{X:"); + sb.Append(X); + sb.Append(" Y:"); + sb.Append(Y); + sb.Append("}"); + return sb.ToString(); + } + + #endregion Public Methods + +#region Operators + + public static Vector2 operator -(Vector2 value) + { + value.X = -value.X; + value.Y = -value.Y; + return value; + } + + + public static bool operator ==(Vector2 value1, Vector2 value2) + { + return value1.X == value2.X && value1.Y == value2.Y; + } + + + public static bool operator !=(Vector2 value1, Vector2 value2) + { + return value1.X != value2.X || value1.Y != value2.Y; + } + + + public static Vector2 operator +(Vector2 value1, Vector2 value2) + { + value1.X += value2.X; + value1.Y += value2.Y; + return value1; + } + + + public static Vector2 operator -(Vector2 value1, Vector2 value2) + { + value1.X -= value2.X; + value1.Y -= value2.Y; + return value1; + } + + + public static Vector2 operator *(Vector2 value1, Vector2 value2) + { + value1.X *= value2.X; + value1.Y *= value2.Y; + return value1; + } + + + public static Vector2 operator *(Vector2 value, float scaleFactor) + { + value.X *= scaleFactor; + value.Y *= scaleFactor; + return value; + } + + + public static Vector2 operator *(float scaleFactor, Vector2 value) + { + value.X *= scaleFactor; + value.Y *= scaleFactor; + return value; + } + + + public static Vector2 operator /(Vector2 value1, Vector2 value2) + { + value1.X /= value2.X; + value1.Y /= value2.Y; + return value1; + } + + + public static Vector2 operator /(Vector2 value1, float divider) + { + float factor = 1/divider; + value1.X *= factor; + value1.Y *= factor; + return value1; + } + + #endregion Operators + } +} + +#endif \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Vector3.cs b/Farseer Physics Engine 3.5/Common/Vector3.cs new file mode 100644 index 000000000..296ac14c8 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Vector3.cs @@ -0,0 +1,648 @@ +#if !XNA && !WINDOWS_PHONE && !XBOX && !ANDROID + +#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.Runtime.InteropServices; +using System.Text; + +namespace Microsoft.Xna.Framework +{ + [StructLayout(LayoutKind.Sequential)] + 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) + { + X = x; + Y = y; + Z = z; + } + + + public Vector3(float value) + { + X = value; + Y = value; + Z = value; + } + + + public Vector3(Vector2 value, float z) + { + X = value.X; + Y = value.Y; + 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) (X + Y + 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) + { + Vector3 result; + Reflect(ref vector, ref normal, out result); + return result; + } + + public static void Reflect(ref Vector3 vector, ref Vector3 normal, out Vector3 result) + { + float dot = Dot(vector, normal); + result.X = vector.X - ((2f*dot)*normal.X); + result.Y = vector.Y - ((2f*dot)*normal.Y); + result.Z = vector.Z - ((2f*dot)*normal.Z); + } + + 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(X); + sb.Append(" Y:"); + sb.Append(Y); + sb.Append(" Z:"); + sb.Append(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 void Transform(Vector3[] sourceArray, ref Matrix matrix, Vector3[] destinationArray) + { + throw new NotImplementedException(); + } + + public static void Transform(Vector3[] sourceArray, int sourceIndex, ref Matrix matrix, + Vector3[] destinationArray, int destinationIndex, int length) + { + throw new NotImplementedException(); + } + + public static void TransformNormal(Vector3[] sourceArray, ref Matrix matrix, Vector3[] destinationArray) + { + throw new NotImplementedException(); + } + + public static void TransformNormal(Vector3[] sourceArray, int sourceIndex, ref Matrix matrix, + Vector3[] destinationArray, int destinationIndex, int length) + { + throw new NotImplementedException(); + } + + 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 + } +} + +#endif \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Common/Vertices.cs b/Farseer Physics Engine 3.5/Common/Vertices.cs new file mode 100644 index 000000000..5c3c33575 --- /dev/null +++ b/Farseer Physics Engine 3.5/Common/Vertices.cs @@ -0,0 +1,582 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +*/ + +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Text; +using FarseerPhysics.Collision; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common +{ + public enum PolygonError + { + /// + /// There were no errors in the polygon + /// + NoError, + + /// + /// Polygon must have between 3 and Settings.MaxPolygonVertices vertices. + /// + InvalidAmountOfVertices, + + /// + /// Polygon must be simple. This means no overlapping edges. + /// + NotSimple, + + /// + /// Polygon must have a counter clockwise winding. + /// + NotCounterClockWise, + + /// + /// The polygon is concave, it needs to be convex. + /// + NotConvex, + + /// + /// Polygon area is too small. + /// + AreaTooSmall, + + /// + /// The polygon has a side that is too short. + /// + SideTooSmall + } + +#if !(XBOX360) + [DebuggerDisplay("Count = {Count} Vertices = {ToString()}")] +#endif + public class Vertices : List + { + public Vertices() { } + + public Vertices(int capacity) : base(capacity) { } + + public Vertices(IEnumerable vertices) + { + AddRange(vertices); + } + + internal bool AttachedToBody { get; set; } + + /// + /// You can add holes to this collection. + /// It will get respected by some of the triangulation algoithms, but otherwise not used. + /// + public List Holes { get; set; } + + /// + /// Gets the next index. Used for iterating all the edges with wrap-around. + /// + /// The current index + public int NextIndex(int index) + { + return (index + 1 > Count - 1) ? 0 : index + 1; + } + + /// + /// Gets the next vertex. Used for iterating all the edges with wrap-around. + /// + /// The current index + public Vector2 NextVertex(int index) + { + return this[NextIndex(index)]; + } + + /// + /// Gets the previous index. Used for iterating all the edges with wrap-around. + /// + /// The current index + public int PreviousIndex(int index) + { + return index - 1 < 0 ? Count - 1 : index - 1; + } + + /// + /// Gets the previous vertex. Used for iterating all the edges with wrap-around. + /// + /// The current index + public Vector2 PreviousVertex(int index) + { + return this[PreviousIndex(index)]; + } + + /// + /// Gets the signed area. + /// If the area is less than 0, it indicates that the polygon is clockwise winded. + /// + /// The signed area + public float GetSignedArea() + { + //The simplest polygon which can exist in the Euclidean plane has 3 sides. + if (Count < 3) + return 0; + + int i; + float area = 0; + + for (i = 0; i < Count; i++) + { + int j = (i + 1) % Count; + + Vector2 vi = this[i]; + Vector2 vj = this[j]; + + area += vi.X * vj.Y; + area -= vi.Y * vj.X; + } + area /= 2.0f; + return area; + } + + /// + /// Gets the area. + /// + /// + public float GetArea() + { + float area = GetSignedArea(); + return (area < 0 ? -area : area); + } + + /// + /// Gets the centroid. + /// + /// + public Vector2 GetCentroid() + { + //The simplest polygon which can exist in the Euclidean plane has 3 sides. + if (Count < 3) + return new Vector2(float.NaN, float.NaN); + + // Same algorithm is used by Box2D + Vector2 c = Vector2.Zero; + float area = 0.0f; + const float inv3 = 1.0f / 3.0f; + + for (int i = 0; i < Count; ++i) + { + // Triangle vertices. + Vector2 current = this[i]; + Vector2 next = (i + 1 < Count ? this[i + 1] : this[0]); + + float triangleArea = 0.5f * (current.X * next.Y - current.Y * next.X); + area += triangleArea; + + // Area weighted centroid + c += triangleArea * inv3 * (current + next); + } + + // Centroid + c *= 1.0f / area; + return c; + } + + /// + /// Returns an AABB that fully contains this polygon. + /// + public AABB GetAABB() + { + AABB aabb; + Vector2 lowerBound = new Vector2(float.MaxValue, float.MaxValue); + Vector2 upperBound = new Vector2(float.MinValue, float.MinValue); + + for (int i = 0; i < Count; ++i) + { + if (this[i].X < lowerBound.X) + { + lowerBound.X = this[i].X; + } + if (this[i].X > upperBound.X) + { + upperBound.X = this[i].X; + } + + if (this[i].Y < lowerBound.Y) + { + lowerBound.Y = this[i].Y; + } + if (this[i].Y > upperBound.Y) + { + upperBound.Y = this[i].Y; + } + } + + aabb.LowerBound = lowerBound; + aabb.UpperBound = upperBound; + + return aabb; + } + + /// + /// Translates the vertices with the specified vector. + /// + /// The value. + public void Translate(Vector2 value) + { + Translate(ref value); + } + + /// + /// Translates the vertices with the specified vector. + /// + /// The vector. + public void Translate(ref Vector2 value) + { + Debug.Assert(!AttachedToBody, "Translating vertices that are used by a Body can result in unstable behavior. Use Body.Position instead."); + + for (int i = 0; i < Count; i++) + this[i] = Vector2.Add(this[i], value); + + if (Holes != null && Holes.Count > 0) + { + foreach (Vertices hole in Holes) + { + hole.Translate(ref value); + } + } + } + + /// + /// Scales the vertices with the specified vector. + /// + /// The Value. + public void Scale(Vector2 value) + { + Scale(ref value); + } + + /// + /// Scales the vertices with the specified vector. + /// + /// The Value. + public void Scale(ref Vector2 value) + { + Debug.Assert(!AttachedToBody, "Scaling vertices that are used by a Body can result in unstable behavior."); + + for (int i = 0; i < Count; i++) + this[i] = Vector2.Multiply(this[i], value); + + if (Holes != null && Holes.Count > 0) + { + foreach (Vertices hole in Holes) + { + hole.Scale(ref value); + } + } + } + + /// + /// Rotate the vertices with the defined value in radians. + /// + /// Warning: Using this method on an active set of vertices of a Body, + /// will cause problems with collisions. Use Body.Rotation instead. + /// + /// The amount to rotate by in radians. + public void Rotate(float value) + { + Debug.Assert(!AttachedToBody, "Rotating vertices that are used by a Body can result in unstable behavior."); + + float num1 = (float)Math.Cos(value); + float num2 = (float)Math.Sin(value); + + for (int i = 0; i < Count; i++) + { + Vector2 position = this[i]; + this[i] = new Vector2((position.X * num1 + position.Y * -num2), (position.X * num2 + position.Y * num1)); + } + + if (Holes != null && Holes.Count > 0) + { + foreach (Vertices hole in Holes) + { + hole.Rotate(value); + } + } + } + + /// + /// Determines whether the polygon is convex. + /// O(n^2) running time. + /// + /// Assumptions: + /// - The polygon is in counter clockwise order + /// - The polygon has no overlapping edges + /// + /// + /// true if it is convex; otherwise, false. + /// + public bool IsConvex() + { + //The simplest polygon which can exist in the Euclidean plane has 3 sides. + if (Count < 3) + return false; + + //Triangles are always convex + if (Count == 3) + return true; + + // Checks the polygon is convex and the interior is to the left of each edge. + for (int i = 0; i < Count; ++i) + { + int next = i + 1 < Count ? i + 1 : 0; + Vector2 edge = this[next] - this[i]; + + for (int j = 0; j < Count; ++j) + { + // Don't check vertices on the current edge. + if (j == i || j == next) + continue; + + Vector2 r = this[j] - this[i]; + + float s = edge.X * r.Y - edge.Y * r.X; + + if (s <= 0.0f) + return false; + } + } + return true; + } + + /// + /// Indicates if the vertices are in counter clockwise order. + /// Warning: If the area of the polygon is 0, it is unable to determine the winding. + /// + public bool IsCounterClockWise() + { + //The simplest polygon which can exist in the Euclidean plane has 3 sides. + if (Count < 3) + return false; + + return (GetSignedArea() > 0.0f); + } + + /// + /// Forces the vertices to be counter clock wise order. + /// + public void ForceCounterClockWise() + { + //The simplest polygon which can exist in the Euclidean plane has 3 sides. + if (Count < 3) + return; + + if (!IsCounterClockWise()) + Reverse(); + } + + /// + /// Checks if the vertices forms an simple polygon by checking for edge crossings. + /// + public bool IsSimple() + { + //The simplest polygon which can exist in the Euclidean plane has 3 sides. + if (Count < 3) + return false; + + for (int i = 0; i < Count; ++i) + { + Vector2 a1 = this[i]; + Vector2 a2 = NextVertex(i); + for (int j = i + 1; j < Count; ++j) + { + Vector2 b1 = this[j]; + Vector2 b2 = NextVertex(j); + + Vector2 temp; + + if (LineTools.LineIntersect2(ref a1, ref a2, ref b1, ref b2, out temp)) + return false; + } + } + return true; + } + + /// + /// Checks if the polygon is valid for use in the engine. + /// + /// Performs a full check, for simplicity, convexity, + /// orientation, minimum angle, and volume. + /// + /// From Eric Jordan's convex decomposition library + /// + /// PolygonError.NoError if there were no error. + public PolygonError CheckPolygon() + { + if (Count < 3 || Count > Settings.MaxPolygonVertices) + return PolygonError.InvalidAmountOfVertices; + + if (!IsSimple()) + return PolygonError.NotSimple; + + if (GetArea() <= Settings.Epsilon) + return PolygonError.AreaTooSmall; + + if (!IsConvex()) + return PolygonError.NotConvex; + + //Check if the sides are of adequate length. + for (int i = 0; i < Count; ++i) + { + int next = i + 1 < Count ? i + 1 : 0; + Vector2 edge = this[next] - this[i]; + if (edge.LengthSquared() <= Settings.Epsilon*Settings.Epsilon) + { + return PolygonError.SideTooSmall; + } + } + + if (!IsCounterClockWise()) + return PolygonError.NotCounterClockWise; + + return PolygonError.NoError; + } + + /// + /// Projects to axis. + /// + /// The axis. + /// The min. + /// The max. + public void ProjectToAxis(ref Vector2 axis, out float min, out float max) + { + // To project a point on an axis use the dot product + float dotProduct = Vector2.Dot(axis, this[0]); + min = dotProduct; + max = dotProduct; + + for (int i = 0; i < Count; i++) + { + dotProduct = Vector2.Dot(this[i], axis); + if (dotProduct < min) + { + min = dotProduct; + } + else + { + if (dotProduct > max) + { + max = dotProduct; + } + } + } + } + + /// + /// Winding number test for a point in a polygon. + /// + /// See more info about the algorithm here: http://softsurfer.com/Archive/algorithm_0103/algorithm_0103.htm + /// The point to be tested. + /// -1 if the winding number is zero and the point is outside + /// the polygon, 1 if the point is inside the polygon, and 0 if the point + /// is on the polygons edge. + public int PointInPolygon(ref Vector2 point) + { + // Winding number + int wn = 0; + + // Iterate through polygon's edges + for (int i = 0; i < Count; i++) + { + // Get points + Vector2 p1 = this[i]; + Vector2 p2 = this[NextIndex(i)]; + + // Test if a point is directly on the edge + Vector2 edge = p2 - p1; + float area = MathUtils.Area(ref p1, ref p2, ref point); + if (area == 0f && Vector2.Dot(point - p1, edge) >= 0f && Vector2.Dot(point - p2, edge) <= 0f) + { + return 0; + } + // Test edge for intersection with ray from point + if (p1.Y <= point.Y) + { + if (p2.Y > point.Y && area > 0f) + { + ++wn; + } + } + else + { + if (p2.Y <= point.Y && area < 0f) + { + --wn; + } + } + } + return (wn == 0 ? -1 : 1); + } + + /// + /// Compute the sum of the angles made between the test point and each pair of points making up the polygon. + /// If this sum is 2pi then the point is an interior point, if 0 then the point is an exterior point. + /// ref: http://ozviz.wasp.uwa.edu.au/~pbourke/geometry/insidepoly/ - Solution 2 + /// + public bool PointInPolygonAngle(ref Vector2 point) + { + double angle = 0; + + // Iterate through polygon's edges + for (int i = 0; i < Count; i++) + { + // Get points + Vector2 p1 = this[i] - point; + Vector2 p2 = this[NextIndex(i)] - point; + + angle += MathUtils.VectorAngle(ref p1, ref p2); + } + + if (Math.Abs(angle) < Math.PI) + { + return false; + } + + return true; + } + + /// + /// Transforms the polygon using the defined matrix. + /// + /// The matrix to use as transformation. + public void Transform(ref Matrix transform) + { + // Transform main polygon + for (int i = 0; i < Count; i++) + this[i] = Vector2.Transform(this[i], transform); + + // Transform holes + if (Holes != null && Holes.Count > 0) + { + for (int i = 0; i < Holes.Count; i++) + { + Vector2[] temp = Holes[i].ToArray(); + Vector2.Transform(temp, ref transform, temp); + + Holes[i] = new Vertices(temp); + } + } + } + + public override string ToString() + { + StringBuilder builder = new StringBuilder(); + for (int i = 0; i < Count; i++) + { + builder.Append(this[i].ToString()); + if (i < Count - 1) + { + builder.Append(" "); + } + } + return builder.ToString(); + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Controllers/AbstractForceController.cs b/Farseer Physics Engine 3.5/Controllers/AbstractForceController.cs new file mode 100644 index 000000000..4782624a2 --- /dev/null +++ b/Farseer Physics Engine 3.5/Controllers/AbstractForceController.cs @@ -0,0 +1,323 @@ +using System; +using FarseerPhysics.Dynamics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Controllers +{ + public abstract class AbstractForceController : Controller + { + #region DecayModes enum + + /// + /// Modes for Decay. Actual Decay must be implemented in inheriting + /// classes + /// + public enum DecayModes + { + None, + Step, + Linear, + InverseSquare, + Curve + } + + #endregion + + #region ForceTypes enum + + /// + /// Forcetypes are used in the decay math to properly get the distance. + /// They are also used to draw a representation in DebugView + /// + public enum ForceTypes + { + Point, + Line, + Area + } + + #endregion + + #region TimingModes enum + + /// + /// Timing Modes + /// Switched: Standard on/off mode using the baseclass enabled property + /// Triggered: When the Trigger() method is called the force is active + /// for a specified Impulse Length + /// Curve: Still to be defined. The basic idea is having a Trigger + /// combined with a curve for the strength + /// + public enum TimingModes + { + Switched, + Triggered, + Curve + } + + #endregion + + /// + /// Curve to be used for Decay in Curve mode + /// + public Curve DecayCurve; + + /// + /// The Forcetype of the instance + /// + public ForceTypes ForceType; + + /// + /// Provided for reuse to provide Variation functionality in + /// inheriting classes + /// + protected Random Randomize; + + /// + /// Curve used by Curve Mode as an animated multiplier for the force + /// strength. + /// Only positions between 0 and 1 are considered as that range is + /// stretched to have ImpulseLength. + /// + public Curve StrengthCurve; + + /// + /// Constructor + /// + public AbstractForceController() + : base(ControllerType.AbstractForceController) + { + Enabled = true; + + Strength = 1.0f; + Position = new Vector2(0, 0); + MaximumSpeed = 100.0f; + TimingMode = TimingModes.Switched; + ImpulseTime = 0.0f; + ImpulseLength = 1.0f; + Triggered = false; + StrengthCurve = new Curve(); + Variation = 0.0f; + Randomize = new Random(1234); + DecayMode = DecayModes.None; + DecayCurve = new Curve(); + DecayStart = 0.0f; + DecayEnd = 0.0f; + + StrengthCurve.Keys.Add(new CurveKey(0, 5)); + StrengthCurve.Keys.Add(new CurveKey(0.1f, 5)); + StrengthCurve.Keys.Add(new CurveKey(0.2f, -4)); + StrengthCurve.Keys.Add(new CurveKey(1f, 0)); + } + + /// + /// Overloaded Contstructor with supplying Timing Mode + /// + /// + public AbstractForceController(TimingModes mode) + : base(ControllerType.AbstractForceController) + { + TimingMode = mode; + switch (mode) + { + case TimingModes.Switched: + Enabled = true; + break; + case TimingModes.Triggered: + Enabled = false; + break; + case TimingModes.Curve: + Enabled = false; + break; + } + } + + /// + /// Global Strength of the force to be applied + /// + public float Strength { get; set; } + + /// + /// Position of the Force. Can be ignored (left at (0,0) for forces + /// that are not position-dependent + /// + public Vector2 Position { get; set; } + + /// + /// Maximum speed of the bodies. Bodies that are travelling faster are + /// supposed to be ignored + /// + public float MaximumSpeed { get; set; } + + /// + /// Maximum Force to be applied. As opposed to Maximum Speed this is + /// independent of the velocity of + /// the affected body + /// + public float MaximumForce { get; set; } + + /// + /// Timing Mode of the force instance + /// + public TimingModes TimingMode { get; set; } + + /// + /// Time of the current impulse. Incremented in update till + /// ImpulseLength is reached + /// + public float ImpulseTime { get; private set; } + + /// + /// Length of a triggered impulse. Used in both Triggered and Curve Mode + /// + public float ImpulseLength { get; set; } + + /// + /// Indicating if we are currently during an Impulse + /// (Triggered and Curve Mode) + /// + public bool Triggered { get; private set; } + + /// + /// Variation of the force applied to each body affected + /// !! Must be used in inheriting classes properly !! + /// + public float Variation { get; set; } + + /// + /// See DecayModes + /// + public DecayModes DecayMode { get; set; } + + /// + /// Start of the distance based Decay. To set a non decaying area + /// + public float DecayStart { get; set; } + + /// + /// Maximum distance a force should be applied + /// + public float DecayEnd { get; set; } + + /// + /// Calculate the Decay for a given body. Meant to ease force + /// development and stick to the DRY principle and provide unified and + /// predictable decay math. + /// + /// The body to calculate decay for + /// A multiplier to multiply the force with to add decay + /// support in inheriting classes + protected float GetDecayMultiplier(Body body) + { + //TODO: Consider ForceType in distance calculation! + float distance = (body.Position - Position).Length(); + switch (DecayMode) + { + case DecayModes.None: + { + return 1.0f; + } + case DecayModes.Step: + { + if (distance < DecayEnd) + return 1.0f; + else + return 0.0f; + } + case DecayModes.Linear: + { + if (distance < DecayStart) + return 1.0f; + if (distance > DecayEnd) + return 0.0f; + return (DecayEnd - DecayStart / distance - DecayStart); + } + case DecayModes.InverseSquare: + { + if (distance < DecayStart) + return 1.0f; + else + return 1.0f / ((distance - DecayStart) * (distance - DecayStart)); + } + case DecayModes.Curve: + { + if (distance < DecayStart) + return 1.0f; + else + return DecayCurve.Evaluate(distance - DecayStart); + } + default: + return 1.0f; + } + } + + /// + /// Triggers the trigger modes (Trigger and Curve) + /// + public void Trigger() + { + Triggered = true; + ImpulseTime = 0; + } + + /// + /// Inherited from Controller + /// Depending on the TimingMode perform timing logic and call ApplyForce() + /// + /// + public override void Update(float dt) + { + switch (TimingMode) + { + case TimingModes.Switched: + { + if (Enabled) + { + ApplyForce(dt, Strength); + } + break; + } + case TimingModes.Triggered: + { + if (Enabled && Triggered) + { + if (ImpulseTime < ImpulseLength) + { + ApplyForce(dt, Strength); + ImpulseTime += dt; + } + else + { + Triggered = false; + } + } + break; + } + case TimingModes.Curve: + { + if (Enabled && Triggered) + { + if (ImpulseTime < ImpulseLength) + { + ApplyForce(dt, Strength * StrengthCurve.Evaluate(ImpulseTime)); + ImpulseTime += dt; + } + else + { + Triggered = false; + } + } + break; + } + } + } + + /// + /// Apply the force supplying strength (wich is modified in Update() + /// according to the TimingMode + /// + /// + /// The strength + public abstract void ApplyForce(float dt, float strength); + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Controllers/BuoyancyController.cs b/Farseer Physics Engine 3.5/Controllers/BuoyancyController.cs new file mode 100644 index 000000000..82eb59f59 --- /dev/null +++ b/Farseer Physics Engine 3.5/Controllers/BuoyancyController.cs @@ -0,0 +1,134 @@ +using System.Collections.Generic; +using FarseerPhysics.Collision; +using FarseerPhysics.Collision.Shapes; +using FarseerPhysics.Dynamics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Controllers +{ + public sealed class BuoyancyController : Controller + { + /// + /// Controls the rotational drag that the fluid exerts on the bodies within it. Use higher values will simulate thick fluid, like honey, lower values to + /// simulate water-like fluids. + /// + public float AngularDragCoefficient; + + /// + /// Density of the fluid. Higher values will make things more buoyant, lower values will cause things to sink. + /// + public float Density; + + /// + /// Controls the linear drag that the fluid exerts on the bodies within it. Use higher values will simulate thick fluid, like honey, lower values to + /// simulate water-like fluids. + /// + public float LinearDragCoefficient; + + /// + /// Acts like waterflow. Defaults to 0,0. + /// + public Vector2 Velocity; + + private AABB _container; + + private Vector2 _gravity; + private Vector2 _normal; + private float _offset; + private Dictionary _uniqueBodies = new Dictionary(); + + /// + /// Initializes a new instance of the class. + /// + /// Only bodies inside this AABB will be influenced by the controller + /// Density of the fluid + /// Linear drag coefficient of the fluid + /// Rotational drag coefficient of the fluid + /// The direction gravity acts. Buoyancy force will act in opposite direction of gravity. + public BuoyancyController(AABB container, float density, float linearDragCoefficient, float rotationalDragCoefficient, Vector2 gravity) + : base(ControllerType.BuoyancyController) + { + Container = container; + _normal = new Vector2(0, 1); + Density = density; + LinearDragCoefficient = linearDragCoefficient; + AngularDragCoefficient = rotationalDragCoefficient; + _gravity = gravity; + } + + public AABB Container + { + get { return _container; } + set + { + _container = value; + _offset = _container.UpperBound.Y; + } + } + + public override void Update(float dt) + { + _uniqueBodies.Clear(); + World.QueryAABB(fixture => + { + if (fixture.Body.IsStatic || !fixture.Body.Awake) + return true; + + if (!_uniqueBodies.ContainsKey(fixture.Body.BodyId)) + _uniqueBodies.Add(fixture.Body.BodyId, fixture.Body); + + return true; + }, ref _container); + + foreach (KeyValuePair kv in _uniqueBodies) + { + Body body = kv.Value; + + Vector2 areac = Vector2.Zero; + Vector2 massc = Vector2.Zero; + float area = 0; + float mass = 0; + + for (int j = 0; j < body.FixtureList.Count; j++) + { + Fixture fixture = body.FixtureList[j]; + + if (fixture.Shape.ShapeType != ShapeType.Polygon && fixture.Shape.ShapeType != ShapeType.Circle) + continue; + + Shape shape = fixture.Shape; + + Vector2 sc; + float sarea = shape.ComputeSubmergedArea(ref _normal, _offset, ref body._xf, out sc); + area += sarea; + areac.X += sarea * sc.X; + areac.Y += sarea * sc.Y; + + mass += sarea * shape.Density; + massc.X += sarea * sc.X * shape.Density; + massc.Y += sarea * sc.Y * shape.Density; + } + + areac.X /= area; + areac.Y /= area; + massc.X /= mass; + massc.Y /= mass; + + if (area < Settings.Epsilon) + continue; + + //Buoyancy + Vector2 buoyancyForce = -Density * area * _gravity; + body.ApplyForce(buoyancyForce, massc); + + //Linear drag + Vector2 dragForce = body.GetLinearVelocityFromWorldPoint(areac) - Velocity; + dragForce *= -LinearDragCoefficient * area; + body.ApplyForce(dragForce, areac); + + //Angular drag + body.ApplyTorque(-body.Inertia / body.Mass * area * body.AngularVelocity * AngularDragCoefficient); + } + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Controllers/Controller.cs b/Farseer Physics Engine 3.5/Controllers/Controller.cs new file mode 100644 index 000000000..f09274bb8 --- /dev/null +++ b/Farseer Physics Engine 3.5/Controllers/Controller.cs @@ -0,0 +1,72 @@ +using System; +using FarseerPhysics.Common.PhysicsLogic; +using FarseerPhysics.Dynamics; + +namespace FarseerPhysics.Controllers +{ + [Flags] + public enum ControllerType + { + GravityController = (1 << 0), + VelocityLimitController = (1 << 1), + AbstractForceController = (1 << 2), + BuoyancyController = (1 << 3), + } + + public struct ControllerFilter + { + public ControllerType ControllerFlags; + + /// + /// Ignores the controller. The controller has no effect on this body. + /// + /// The controller type. + public void IgnoreController(ControllerType controller) + { + ControllerFlags |= controller; + } + + /// + /// Restore the controller. The controller affects this body. + /// + /// The controller type. + public void RestoreController(ControllerType controller) + { + ControllerFlags &= ~controller; + } + + /// + /// Determines whether this body ignores the the specified controller. + /// + /// The controller type. + /// + /// true if the body has the specified flag; otherwise, false. + /// + public bool IsControllerIgnored(ControllerType controller) + { + return (ControllerFlags & controller) == controller; + } + } + + public abstract class Controller : FilterData + { + public bool Enabled; + public World World; + private ControllerType _type; + + public Controller(ControllerType controllerType) + { + _type = controllerType; + } + + public override bool IsActiveOn(Body body) + { + if (body.ControllerFilter.IsControllerIgnored(_type)) + return false; + + return base.IsActiveOn(body); + } + + public abstract void Update(float dt); + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Controllers/GravityController.cs b/Farseer Physics Engine 3.5/Controllers/GravityController.cs new file mode 100644 index 000000000..4f359ccb2 --- /dev/null +++ b/Farseer Physics Engine 3.5/Controllers/GravityController.cs @@ -0,0 +1,110 @@ +using System; +using System.Collections.Generic; +using FarseerPhysics.Dynamics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Controllers +{ + public enum GravityType + { + Linear, + DistanceSquared + } + + public class GravityController : Controller + { + public GravityController(float strength) + : base(ControllerType.GravityController) + { + Strength = strength; + MaxRadius = float.MaxValue; + GravityType = GravityType.DistanceSquared; + Points = new List(); + Bodies = new List(); + } + + public GravityController(float strength, float maxRadius, float minRadius) + : base(ControllerType.GravityController) + { + MinRadius = minRadius; + MaxRadius = maxRadius; + Strength = strength; + GravityType = GravityType.DistanceSquared; + Points = new List(); + Bodies = new List(); + } + + public float MinRadius { get; set; } + public float MaxRadius { get; set; } + public float Strength { get; set; } + public GravityType GravityType { get; set; } + public List Bodies { get; set; } + public List Points { get; set; } + + public override void Update(float dt) + { + Vector2 f = Vector2.Zero; + + foreach (Body worldBody in World.BodyList) + { + if (!IsActiveOn(worldBody)) + continue; + + foreach (Body controllerBody in Bodies) + { + if (worldBody == controllerBody || (worldBody.IsStatic && controllerBody.IsStatic) || !controllerBody.Enabled) + continue; + + Vector2 d = controllerBody.Position - worldBody.Position; + float r2 = d.LengthSquared(); + + if (r2 <= Settings.Epsilon || r2 > MaxRadius * MaxRadius || r2 < MinRadius * MinRadius) + continue; + + switch (GravityType) + { + case GravityType.DistanceSquared: + f = Strength / r2 * worldBody.Mass * controllerBody.Mass * d; + break; + case GravityType.Linear: + f = Strength / (float)Math.Sqrt(r2) * worldBody.Mass * controllerBody.Mass * d; + break; + } + + worldBody.ApplyForce(ref f); + } + + foreach (Vector2 point in Points) + { + Vector2 d = point - worldBody.Position; + float r2 = d.LengthSquared(); + + if (r2 <= Settings.Epsilon || r2 > MaxRadius * MaxRadius || r2 < MinRadius * MinRadius) + continue; + + switch (GravityType) + { + case GravityType.DistanceSquared: + f = Strength / r2 * worldBody.Mass * d; + break; + case GravityType.Linear: + f = Strength / (float)Math.Sqrt(r2) * worldBody.Mass * d; + break; + } + + worldBody.ApplyForce(ref f); + } + } + } + + public void AddBody(Body body) + { + Bodies.Add(body); + } + + public void AddPoint(Vector2 point) + { + Points.Add(point); + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Controllers/SimpleWindForce.cs b/Farseer Physics Engine 3.5/Controllers/SimpleWindForce.cs new file mode 100644 index 000000000..541d7ff53 --- /dev/null +++ b/Farseer Physics Engine 3.5/Controllers/SimpleWindForce.cs @@ -0,0 +1,75 @@ +using FarseerPhysics.Dynamics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Controllers +{ + /// + /// Reference implementation for forces based on AbstractForceController + /// It supports all features provided by the base class and illustrates proper + /// usage as an easy to understand example. + /// As a side-effect it is a nice and easy to use wind force for your projects + /// + public class SimpleWindForce : AbstractForceController + { + /// + /// Direction of the windforce + /// + public Vector2 Direction { get; set; } + + /// + /// The amount of Direction randomization. Allowed range is 0-1. + /// + public float Divergence { get; set; } + + /// + /// Ignore the position and apply the force. If off only in the "front" (relative to position and direction) + /// will be affected + /// + public bool IgnorePosition { get; set; } + + + public override void ApplyForce(float dt, float strength) + { + foreach (Body body in World.BodyList) + { + //TODO: Consider Force Type + float decayMultiplier = GetDecayMultiplier(body); + + if (decayMultiplier != 0) + { + Vector2 forceVector; + + if (ForceType == ForceTypes.Point) + { + forceVector = body.Position - Position; + } + else + { + Direction.Normalize(); + + forceVector = Direction; + + if (forceVector.Length() == 0) + forceVector = new Vector2(0, 1); + } + + //TODO: Consider Divergence: + //forceVector = Vector2.Transform(forceVector, Matrix.CreateRotationZ((MathHelper.Pi - MathHelper.Pi/2) * (float)Randomize.NextDouble())); + + // Calculate random Variation + if (Variation != 0) + { + float strengthVariation = (float)Randomize.NextDouble() * MathHelper.Clamp(Variation, 0, 1); + forceVector.Normalize(); + body.ApplyForce(forceVector * strength * decayMultiplier * strengthVariation); + } + else + { + forceVector.Normalize(); + body.ApplyForce(forceVector * strength * decayMultiplier); + } + } + } + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Controllers/VelocityLimitController.cs b/Farseer Physics Engine 3.5/Controllers/VelocityLimitController.cs new file mode 100644 index 000000000..13d2eb28b --- /dev/null +++ b/Farseer Physics Engine 3.5/Controllers/VelocityLimitController.cs @@ -0,0 +1,129 @@ +using System; +using System.Collections.Generic; +using FarseerPhysics.Dynamics; + +namespace FarseerPhysics.Controllers +{ + /// + /// Put a limit on the linear (translation - the movespeed) and angular (rotation) velocity + /// of bodies added to this controller. + /// + public class VelocityLimitController : Controller + { + public bool LimitAngularVelocity = true; + public bool LimitLinearVelocity = true; + private List _bodies = new List(); + private float _maxAngularSqared; + private float _maxAngularVelocity; + private float _maxLinearSqared; + private float _maxLinearVelocity; + + /// + /// Initializes a new instance of the class. + /// Sets the max linear velocity to Settings.MaxTranslation + /// Sets the max angular velocity to Settings.MaxRotation + /// + public VelocityLimitController() + : base(ControllerType.VelocityLimitController) + { + MaxLinearVelocity = Settings.MaxTranslation; + MaxAngularVelocity = Settings.MaxRotation; + } + + /// + /// Initializes a new instance of the class. + /// Pass in 0 or float.MaxValue to disable the limit. + /// maxAngularVelocity = 0 will disable the angular velocity limit. + /// + /// The max linear velocity. + /// The max angular velocity. + public VelocityLimitController(float maxLinearVelocity, float maxAngularVelocity) + : base(ControllerType.VelocityLimitController) + { + if (maxLinearVelocity == 0 || maxLinearVelocity == float.MaxValue) + LimitLinearVelocity = false; + + if (maxAngularVelocity == 0 || maxAngularVelocity == float.MaxValue) + LimitAngularVelocity = false; + + MaxLinearVelocity = maxLinearVelocity; + MaxAngularVelocity = maxAngularVelocity; + } + + /// + /// Gets or sets the max angular velocity. + /// + /// The max angular velocity. + public float MaxAngularVelocity + { + get { return _maxAngularVelocity; } + set + { + _maxAngularVelocity = value; + _maxAngularSqared = _maxAngularVelocity * _maxAngularVelocity; + } + } + + /// + /// Gets or sets the max linear velocity. + /// + /// The max linear velocity. + public float MaxLinearVelocity + { + get { return _maxLinearVelocity; } + set + { + _maxLinearVelocity = value; + _maxLinearSqared = _maxLinearVelocity * _maxLinearVelocity; + } + } + + public override void Update(float dt) + { + foreach (Body body in _bodies) + { + if (!IsActiveOn(body)) + continue; + + if (LimitLinearVelocity) + { + //Translation + // Check for large velocities. + float translationX = dt * body._linearVelocity.X; + float translationY = dt * body._linearVelocity.Y; + float result = translationX * translationX + translationY * translationY; + + if (result > dt * _maxLinearSqared) + { + float sq = (float)Math.Sqrt(result); + + float ratio = _maxLinearVelocity / sq; + body._linearVelocity.X *= ratio; + body._linearVelocity.Y *= ratio; + } + } + + if (LimitAngularVelocity) + { + //Rotation + float rotation = dt * body._angularVelocity; + if (rotation * rotation > _maxAngularSqared) + { + float ratio = _maxAngularVelocity / Math.Abs(rotation); + body._angularVelocity *= ratio; + } + } + } + } + + public void AddBody(Body body) + { + _bodies.Add(body); + } + + public void RemoveBody(Body body) + { + _bodies.Remove(body); + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/ConvertUnits.cs b/Farseer Physics Engine 3.5/ConvertUnits.cs new file mode 100644 index 000000000..1b6f51b3d --- /dev/null +++ b/Farseer Physics Engine 3.5/ConvertUnits.cs @@ -0,0 +1,108 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +*/ + +using Microsoft.Xna.Framework; + +namespace FarseerPhysics +{ + /// + /// Convert units between display and simulation units. + /// + public static class ConvertUnits + { + private static float _displayUnitsToSimUnitsRatio = 100f; + private static float _simUnitsToDisplayUnitsRatio = 1 / _displayUnitsToSimUnitsRatio; + + public static void SetDisplayUnitToSimUnitRatio(float displayUnitsPerSimUnit) + { + _displayUnitsToSimUnitsRatio = displayUnitsPerSimUnit; + _simUnitsToDisplayUnitsRatio = 1 / displayUnitsPerSimUnit; + } + + public static float ToDisplayUnits(float simUnits) + { + return simUnits * _displayUnitsToSimUnitsRatio; + } + + public static float ToDisplayUnits(int simUnits) + { + return simUnits * _displayUnitsToSimUnitsRatio; + } + + public static Vector2 ToDisplayUnits(Vector2 simUnits) + { + return simUnits * _displayUnitsToSimUnitsRatio; + } + + public static void ToDisplayUnits(ref Vector2 simUnits, out Vector2 displayUnits) + { + Vector2.Multiply(ref simUnits, _displayUnitsToSimUnitsRatio, out displayUnits); + } + + public static Vector3 ToDisplayUnits(Vector3 simUnits) + { + return simUnits * _displayUnitsToSimUnitsRatio; + } + + public static Vector2 ToDisplayUnits(float x, float y) + { + return new Vector2(x, y) * _displayUnitsToSimUnitsRatio; + } + + public static void ToDisplayUnits(float x, float y, out Vector2 displayUnits) + { + displayUnits = Vector2.Zero; + displayUnits.X = x * _displayUnitsToSimUnitsRatio; + displayUnits.Y = y * _displayUnitsToSimUnitsRatio; + } + + public static float ToSimUnits(float displayUnits) + { + return displayUnits * _simUnitsToDisplayUnitsRatio; + } + + public static float ToSimUnits(double displayUnits) + { + return (float)displayUnits * _simUnitsToDisplayUnitsRatio; + } + + public static float ToSimUnits(int displayUnits) + { + return displayUnits * _simUnitsToDisplayUnitsRatio; + } + + public static Vector2 ToSimUnits(Vector2 displayUnits) + { + return displayUnits * _simUnitsToDisplayUnitsRatio; + } + + public static Vector3 ToSimUnits(Vector3 displayUnits) + { + return displayUnits * _simUnitsToDisplayUnitsRatio; + } + + public static void ToSimUnits(ref Vector2 displayUnits, out Vector2 simUnits) + { + Vector2.Multiply(ref displayUnits, _simUnitsToDisplayUnitsRatio, out simUnits); + } + + public static Vector2 ToSimUnits(float x, float y) + { + return new Vector2(x, y) * _simUnitsToDisplayUnitsRatio; + } + + public static Vector2 ToSimUnits(double x, double y) + { + return new Vector2((float)x, (float)y) * _simUnitsToDisplayUnitsRatio; + } + + public static void ToSimUnits(float x, float y, out Vector2 simUnits) + { + simUnits = Vector2.Zero; + simUnits.X = x * _simUnitsToDisplayUnitsRatio; + simUnits.Y = y * _simUnitsToDisplayUnitsRatio; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/DebugViewBase.cs b/Farseer Physics Engine 3.5/DebugViewBase.cs new file mode 100644 index 000000000..3fb7821e4 --- /dev/null +++ b/Farseer Physics Engine 3.5/DebugViewBase.cs @@ -0,0 +1,165 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +*/ + +using System; +using FarseerPhysics.Common; +using FarseerPhysics.Dynamics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics +{ + [Flags] + public enum DebugViewFlags + { + /// + /// Draw shapes. + /// + Shape = (1 << 0), + + /// + /// Draw joint connections. + /// + Joint = (1 << 1), + + /// + /// Draw axis aligned bounding boxes. + /// + AABB = (1 << 2), + + /// + /// Draw broad-phase pairs. + /// + //Pair = (1 << 3), + + /// + /// Draw center of mass frame. + /// + CenterOfMass = (1 << 4), + + /// + /// Draw useful debug data such as timings and number of bodies, joints, contacts and more. + /// + DebugPanel = (1 << 5), + + /// + /// Draw contact points between colliding bodies. + /// + ContactPoints = (1 << 6), + + /// + /// Draw contact normals. Need ContactPoints to be enabled first. + /// + ContactNormals = (1 << 7), + + /// + /// Draws the vertices of polygons. + /// + PolygonPoints = (1 << 8), + + /// + /// Draws the performance graph. + /// + PerformanceGraph = (1 << 9), + + /// + /// Draws controllers. + /// + Controllers = (1 << 10) + } + + /// Implement and register this class with a World to provide debug drawing of physics + /// entities in your game. + public abstract class DebugViewBase + { + protected DebugViewBase(World world) + { + World = world; + } + + protected World World { get; private set; } + + /// + /// Gets or sets the debug view flags. + /// + /// The flags. + public DebugViewFlags Flags { get; set; } + + /// + /// Append flags to the current flags. + /// + /// The flags. + public void AppendFlags(DebugViewFlags flags) + { + Flags |= flags; + } + + /// + /// Remove flags from the current flags. + /// + /// The flags. + public void RemoveFlags(DebugViewFlags flags) + { + Flags &= ~flags; + } + + /// + /// Draw a closed polygon provided in CCW order. + /// + /// The vertices. + /// The vertex count. + /// The red value. + /// The blue value. + /// The green value. + public abstract void DrawPolygon(Vector2[] vertices, int count, float red, float blue, float green, bool closed = true); + + /// + /// Draw a solid closed polygon provided in CCW order. + /// + /// The vertices. + /// The vertex count. + /// The red value. + /// The blue value. + /// The green value. + public abstract void DrawSolidPolygon(Vector2[] vertices, int count, float red, float blue, float green); + + /// + /// Draw a circle. + /// + /// The center. + /// The radius. + /// The red value. + /// The blue value. + /// The green value. + public abstract void DrawCircle(Vector2 center, float radius, float red, float blue, float green); + + /// + /// Draw a solid circle. + /// + /// The center. + /// The radius. + /// The axis. + /// The red value. + /// The blue value. + /// The green value. + public abstract void DrawSolidCircle(Vector2 center, float radius, Vector2 axis, float red, float blue, + float green); + + /// + /// Draw a line segment. + /// + /// The start. + /// The end. + /// The red value. + /// The blue value. + /// The green value. + public abstract void DrawSegment(Vector2 start, Vector2 end, float red, float blue, float green); + + /// + /// Draw a transform. Choose your own length scale. + /// + /// The transform. + public abstract void DrawTransform(ref Transform transform); + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Dynamics/Body.cs b/Farseer Physics Engine 3.5/Dynamics/Body.cs new file mode 100644 index 000000000..febc7d8f8 --- /dev/null +++ b/Farseer Physics Engine 3.5/Dynamics/Body.cs @@ -0,0 +1,1362 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ +//#define USE_AWAKE_BODY_SET + +using System; +using System.Collections.Generic; +using System.Diagnostics; +using FarseerPhysics.Collision; +using FarseerPhysics.Collision.Shapes; +using FarseerPhysics.Common; +using FarseerPhysics.Common.PhysicsLogic; +using FarseerPhysics.Controllers; +using FarseerPhysics.Dynamics.Contacts; +using FarseerPhysics.Dynamics.Joints; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics +{ + /// + /// The body type. + /// + public enum BodyType + { + /// + /// Zero velocity, may be manually moved. Note: even static bodies have mass. + /// + Static, + /// + /// Zero mass, non-zero velocity set by user, moved by solver + /// + Kinematic, + /// + /// Positive mass, non-zero velocity determined by forces, moved by solver + /// + Dynamic, + } + + public class Body : IDisposable + { + [ThreadStatic] + private static int _bodyIdCounter; + + private float _angularDamping; + private BodyType _bodyType; + private float _inertia; + private float _linearDamping; + private float _mass; + private bool _sleepingAllowed = true; + private bool _awake = true; + private bool _fixedRotation; + + internal bool _enabled = true; + internal float _angularVelocity; + internal Vector2 _linearVelocity; + internal Vector2 _force; + internal float _invI; + internal float _invMass; + internal float _sleepTime; + internal Sweep _sweep; // the swept motion for CCD + internal float _torque; + internal World _world; + internal Transform _xf; // the body origin transform + internal bool _island; + + public PhysicsLogicFilter PhysicsLogicFilter; + public ControllerFilter ControllerFilter; + + public Body(World world, Vector2? position = null, float rotation = 0, object userdata = null) + { + FixtureList = new List(); + BodyId = _bodyIdCounter++; + _world = world; + + UserData = userdata; + GravityScale = 1.0f; + BodyType = BodyType.Static; + Enabled = true; //FPE note: Also creates proxies in the broadphase + + _xf.q.Set(rotation); + + if (position.HasValue) + { + _xf.p = position.Value; + _sweep.C0 = _xf.p; + _sweep.C = _xf.p; + _sweep.A0 = rotation; + _sweep.A = rotation; + } + + world.AddBody(this); //FPE note: bodies can't live without a World + } + + /// + /// A unique id for this body. + /// + public int BodyId { get; private set; } + + public int IslandIndex { get; set; } + + /// + /// Scale the gravity applied to this body. + /// Defaults to 1. A value of 2 means double the gravity is applied to this body. + /// + public float GravityScale { get; set; } + + /// + /// Set the user data. Use this to store your application specific data. + /// + /// The user data. + public object UserData { get; set; } + + /// + /// Gets the total number revolutions the body has made. + /// + /// The revolutions. + public float Revolutions + { + get { return Rotation / (float)Math.PI; } + } + + /// + /// Gets or sets the body type. + /// Warning: Calling this mid-update might cause a crash. + /// + /// The type of body. + public BodyType BodyType + { + get { return _bodyType; } + set + { + if (_bodyType == value) + return; + + _bodyType = value; + + ResetMassData(); + + if (_bodyType == BodyType.Static) + { + _linearVelocity = Vector2.Zero; + _angularVelocity = 0.0f; + _sweep.A0 = _sweep.A; + _sweep.C0 = _sweep.C; + SynchronizeFixtures(); + } + + Awake = true; + + _force = Vector2.Zero; + _torque = 0.0f; + + // Delete the attached contacts. + ContactEdge ce = ContactList; + while (ce != null) + { + ContactEdge ce0 = ce; + ce = ce.Next; + _world.ContactManager.Destroy(ce0.Contact); + } + + ContactList = null; + + // Touch the proxies so that new contacts will be created (when appropriate) + IBroadPhase broadPhase = _world.ContactManager.BroadPhase; + foreach (Fixture fixture in FixtureList) + { + int proxyCount = fixture.ProxyCount; + for (int j = 0; j < proxyCount; j++) + { + broadPhase.TouchProxy(fixture.Proxies[j].ProxyId); + } + } + } + } + + /// + /// Get or sets the linear velocity of the center of mass. + /// + /// The linear velocity. + public Vector2 LinearVelocity + { + set + { + Debug.Assert(!float.IsNaN(value.X) && !float.IsNaN(value.Y)); + + if (_bodyType == BodyType.Static) + return; + + if (Vector2.Dot(value, value) > 0.0f) + Awake = true; + + _linearVelocity = value; + } + get { return _linearVelocity; } + } + + /// + /// Gets or sets the angular velocity. Radians/second. + /// + /// The angular velocity. + public float AngularVelocity + { + set + { + Debug.Assert(!float.IsNaN(value)); + + if (_bodyType == BodyType.Static) + return; + + if (value * value > 0.0f) + Awake = true; + + _angularVelocity = value; + } + get { return _angularVelocity; } + } + + /// + /// Gets or sets the linear damping. + /// + /// The linear damping. + public float LinearDamping + { + get { return _linearDamping; } + set + { + Debug.Assert(!float.IsNaN(value)); + + _linearDamping = value; + } + } + + /// + /// Gets or sets the angular damping. + /// + /// The angular damping. + public float AngularDamping + { + get { return _angularDamping; } + set + { + Debug.Assert(!float.IsNaN(value)); + + _angularDamping = value; + } + } + + /// + /// Gets or sets a value indicating whether this body should be included in the CCD solver. + /// + /// true if this instance is included in CCD; otherwise, false. + public bool IsBullet { get; set; } + + /// + /// You can disable sleeping on this body. If you disable sleeping, the + /// body will be woken. + /// + /// true if sleeping is allowed; otherwise, false. + public bool SleepingAllowed + { + set + { + if (!value) + Awake = true; + + _sleepingAllowed = value; + } + get { return _sleepingAllowed; } + } + + /// + /// Set the sleep state of the body. A sleeping body has very + /// low CPU cost. + /// + /// true if awake; otherwise, false. + public bool Awake + { + set + { + if (value) + { + if (!_awake) + { + _sleepTime = 0.0f; + _world.ContactManager.UpdateContacts(ContactList, true); +#if USE_AWAKE_BODY_SET + if (InWorld && !World.AwakeBodySet.Contains(this)) + { + World.AwakeBodySet.Add(this); + } +#endif + } + } + else + { +#if USE_AWAKE_BODY_SET + // Check even for BodyType.Static because if this body had just been changed to Static it will have + // set Awake = false in the process. + if (InWorld && World.AwakeBodySet.Contains(this)) + { + World.AwakeBodySet.Remove(this); + } +#endif + ResetDynamics(); + _sleepTime = 0.0f; + _world.ContactManager.UpdateContacts(ContactList, false); + } + + _awake = value; + } + get { return _awake; } + } + + /// + /// Set the active state of the body. An inactive body is not + /// simulated and cannot be collided with or woken up. + /// If you pass a flag of true, all fixtures will be added to the + /// broad-phase. + /// If you pass a flag of false, all fixtures will be removed from + /// the broad-phase and all contacts will be destroyed. + /// Fixtures and joints are otherwise unaffected. You may continue + /// to create/destroy fixtures and joints on inactive bodies. + /// Fixtures on an inactive body are implicitly inactive and will + /// not participate in collisions, ray-casts, or queries. + /// Joints connected to an inactive body are implicitly inactive. + /// An inactive body is still owned by a b2World object and remains + /// in the body list. + /// + /// true if active; otherwise, false. + public bool Enabled + { + set + { + if (value == _enabled) + return; + + if (value) + { + // Create all proxies. + IBroadPhase broadPhase = _world.ContactManager.BroadPhase; + for (int i = 0; i < FixtureList.Count; i++) + { + FixtureList[i].CreateProxies(broadPhase, ref _xf); + } + + // Contacts are created the next time step. + } + else + { + // Destroy all proxies. + IBroadPhase broadPhase = _world.ContactManager.BroadPhase; + + for (int i = 0; i < FixtureList.Count; i++) + { + FixtureList[i].DestroyProxies(broadPhase); + } + + // Destroy the attached contacts. + ContactEdge ce = ContactList; + while (ce != null) + { + ContactEdge ce0 = ce; + ce = ce.Next; + _world.ContactManager.Destroy(ce0.Contact); + } + ContactList = null; + } + + _enabled = value; + } + get { return _enabled; } + } + + /// + /// Set this body to have fixed rotation. This causes the mass + /// to be reset. + /// + /// true if it has fixed rotation; otherwise, false. + public bool FixedRotation + { + set + { + if (_fixedRotation == value) + return; + + _fixedRotation = value; + + _angularVelocity = 0f; + ResetMassData(); + } + get { return _fixedRotation; } + } + + /// + /// Gets all the fixtures attached to this body. + /// + /// The fixture list. + public List FixtureList { get; internal set; } + + /// + /// Get the list of all joints attached to this body. + /// + /// The joint list. + public JointEdge JointList { get; internal set; } + + /// + /// Get the list of all contacts attached to this body. + /// Warning: this list changes during the time step and you may + /// miss some collisions if you don't use ContactListener. + /// + /// The contact list. + public ContactEdge ContactList { get; internal set; } + + /// + /// Get the world body origin position. + /// + /// Return the world position of the body's origin. + public Vector2 Position + { + get { return _xf.p; } + set + { + Debug.Assert(!float.IsNaN(value.X) && !float.IsNaN(value.Y)); + + SetTransform(ref value, Rotation); + } + } + + /// + /// Get the angle in radians. + /// + /// Return the current world rotation angle in radians. + public float Rotation + { + get { return _sweep.A; } + set + { + Debug.Assert(!float.IsNaN(value)); + + SetTransform(ref _xf.p, value); + } + } + + /// + /// Gets or sets a value indicating whether this body is static. + /// + /// true if this instance is static; otherwise, false. + public bool IsStatic + { + get { return _bodyType == BodyType.Static; } + set { BodyType = value ? BodyType.Static : BodyType.Dynamic; } + } + + /// + /// Gets or sets a value indicating whether this body is kinematic. + /// + /// true if this instance is kinematic; otherwise, false. + public bool IsKinematic + { + get { return _bodyType == BodyType.Kinematic; } + set { BodyType = value ? BodyType.Kinematic : BodyType.Dynamic; } + } + + /// + /// Gets or sets a value indicating whether this body ignores gravity. + /// + /// true if it ignores gravity; otherwise, false. + public bool IgnoreGravity { get; set; } + + /// + /// Get the world position of the center of mass. + /// + /// The world position. + public Vector2 WorldCenter + { + get { return _sweep.C; } + } + + /// + /// Get the local position of the center of mass. + /// + /// The local position. + public Vector2 LocalCenter + { + get { return _sweep.LocalCenter; } + set + { + if (_bodyType != BodyType.Dynamic) + return; + + // Move center of mass. + Vector2 oldCenter = _sweep.C; + _sweep.LocalCenter = value; + _sweep.C0 = _sweep.C = MathUtils.Mul(ref _xf, ref _sweep.LocalCenter); + + // Update center of mass velocity. + Vector2 a = _sweep.C - oldCenter; + _linearVelocity += new Vector2(-_angularVelocity * a.Y, _angularVelocity * a.X); + } + } + + /// + /// Gets or sets the mass. Usually in kilograms (kg). + /// + /// The mass. + public float Mass + { + get { return _mass; } + set + { + Debug.Assert(!float.IsNaN(value)); + + if (_bodyType != BodyType.Dynamic) //Make an assert + return; + + _mass = value; + + if (_mass <= 0.0f) + _mass = 1.0f; + + _invMass = 1.0f / _mass; + } + } + + /// + /// Get or set the rotational inertia of the body about the local origin. usually in kg-m^2. + /// + /// The inertia. + public float Inertia + { + get { return _inertia + Mass * Vector2.Dot(_sweep.LocalCenter, _sweep.LocalCenter); } + set + { + Debug.Assert(!float.IsNaN(value)); + + if (_bodyType != BodyType.Dynamic) //Make an assert + return; + + if (value > 0.0f && !_fixedRotation) //Make an assert + { + _inertia = value - Mass * Vector2.Dot(LocalCenter, LocalCenter); + Debug.Assert(_inertia > 0.0f); + _invI = 1.0f / _inertia; + } + } + } + + public float Restitution + { + get + { + float res = 0; + + for (int i = 0; i < FixtureList.Count; i++) + { + Fixture f = FixtureList[i]; + res += f.Restitution; + } + + return FixtureList.Count > 0 ? res / FixtureList.Count : 0; + } + set + { + for (int i = 0; i < FixtureList.Count; i++) + { + Fixture f = FixtureList[i]; + f.Restitution = value; + } + } + } + + public float Friction + { + get + { + float res = 0; + + for (int i = 0; i < FixtureList.Count; i++) + { + Fixture f = FixtureList[i]; + res += f.Friction; + } + + return FixtureList.Count > 0 ? res / FixtureList.Count : 0; + } + set + { + for (int i = 0; i < FixtureList.Count; i++) + { + Fixture f = FixtureList[i]; + f.Friction = value; + } + } + } + + public Category CollisionCategories + { + set + { + for (int i = 0; i < FixtureList.Count; i++) + { + Fixture f = FixtureList[i]; + f.CollisionCategories = value; + } + } + } + + public Category CollidesWith + { + set + { + for (int i = 0; i < FixtureList.Count; i++) + { + Fixture f = FixtureList[i]; + f.CollidesWith = value; + } + } + } + + /// + /// Body objects can define which categories of bodies they wish to ignore CCD with. + /// This allows certain bodies to be configured to ignore CCD with objects that + /// aren't a penetration problem due to the way content has been prepared. + /// This is compared against the other Body's fixture CollisionCategories within World.SolveTOI(). + /// + public Category IgnoreCCDWith + { + set + { + for (int i = 0; i < FixtureList.Count; i++) + { + Fixture f = FixtureList[i]; + f.IgnoreCCDWith = value; + } + } + } + + public short CollisionGroup + { + set + { + for (int i = 0; i < FixtureList.Count; i++) + { + Fixture f = FixtureList[i]; + f.CollisionGroup = value; + } + } + } + + public bool IsSensor + { + set + { + for (int i = 0; i < FixtureList.Count; i++) + { + Fixture f = FixtureList[i]; + f.IsSensor = value; + } + } + } + + public bool IgnoreCCD { get; set; } + + /// + /// Resets the dynamics of this body. + /// Sets torque, force and linear/angular velocity to 0 + /// + public void ResetDynamics() + { + _torque = 0; + _angularVelocity = 0; + _force = Vector2.Zero; + _linearVelocity = Vector2.Zero; + } + + /// + /// Creates a fixture and attach it to this body. + /// If the density is non-zero, this function automatically updates the mass of the body. + /// Contacts are not created until the next time step. + /// Warning: This function is locked during callbacks. + /// + /// The shape. + /// Application specific data + /// + public Fixture CreateFixture(Shape shape, object userData = null) + { + return new Fixture(this, shape, userData); + } + + /// + /// Destroy a fixture. This removes the fixture from the broad-phase and + /// destroys all contacts associated with this fixture. This will + /// automatically adjust the mass of the body if the body is dynamic and the + /// fixture has positive density. + /// All fixtures attached to a body are implicitly destroyed when the body is destroyed. + /// Warning: This function is locked during callbacks. + /// + /// The fixture to be removed. + public void DestroyFixture(Fixture fixture) + { + Debug.Assert(fixture.Body == this); + + // Remove the fixture from this body's singly linked list. + Debug.Assert(FixtureList.Count > 0); + + // You tried to remove a fixture that not present in the fixturelist. + Debug.Assert(FixtureList.Contains(fixture)); + + // Destroy any contacts associated with the fixture. + ContactEdge edge = ContactList; + while (edge != null) + { + Contact c = edge.Contact; + edge = edge.Next; + + Fixture fixtureA = c.FixtureA; + Fixture fixtureB = c.FixtureB; + + if (fixture == fixtureA || fixture == fixtureB) + { + // This destroys the contact and removes it from + // this body's contact list. + _world.ContactManager.Destroy(c); + } + } + + if (_enabled) + { + IBroadPhase broadPhase = _world.ContactManager.BroadPhase; + fixture.DestroyProxies(broadPhase); + } + + FixtureList.Remove(fixture); + fixture.Destroy(); + fixture.Body = null; + + ResetMassData(); + } + + /// + /// Set the position of the body's origin and rotation. + /// This breaks any contacts and wakes the other bodies. + /// Manipulating a body's transform may cause non-physical behavior. + /// + /// The world position of the body's local origin. + /// The world rotation in radians. + public void SetTransform(ref Vector2 position, float rotation) + { + SetTransformIgnoreContacts(ref position, rotation); + + _world.ContactManager.FindNewContacts(); + } + + /// + /// Set the position of the body's origin and rotation. + /// This breaks any contacts and wakes the other bodies. + /// Manipulating a body's transform may cause non-physical behavior. + /// + /// The world position of the body's local origin. + /// The world rotation in radians. + public void SetTransform(Vector2 position, float rotation) + { + SetTransform(ref position, rotation); + } + + /// + /// For teleporting a body without considering new contacts immediately. + /// + /// The position. + /// The angle. + public void SetTransformIgnoreContacts(ref Vector2 position, float angle) + { + _xf.q.Set(angle); + _xf.p = position; + + _sweep.C = MathUtils.Mul(ref _xf, _sweep.LocalCenter); + _sweep.A = angle; + + _sweep.C0 = _sweep.C; + _sweep.A0 = angle; + + IBroadPhase broadPhase = _world.ContactManager.BroadPhase; + for (int i = 0; i < FixtureList.Count; i++) + { + FixtureList[i].Synchronize(broadPhase, ref _xf, ref _xf); + } + } + + /// + /// Get the body transform for the body's origin. + /// + /// The transform of the body's origin. + public void GetTransform(out Transform transform) + { + transform = _xf; + } + + /// + /// Apply a force at a world point. If the force is not + /// applied at the center of mass, it will generate a torque and + /// affect the angular velocity. This wakes up the body. + /// + /// The world force vector, usually in Newtons (N). + /// The world position of the point of application. + public void ApplyForce(Vector2 force, Vector2 point) + { + ApplyForce(ref force, ref point); + } + + /// + /// Applies a force at the center of mass. + /// + /// The force. + public void ApplyForce(ref Vector2 force) + { + ApplyForce(ref force, ref _xf.p); + } + + /// + /// Applies a force at the center of mass. + /// + /// The force. + public void ApplyForce(Vector2 force) + { + ApplyForce(ref force, ref _xf.p); + } + + /// + /// Apply a force at a world point. If the force is not + /// applied at the center of mass, it will generate a torque and + /// affect the angular velocity. This wakes up the body. + /// + /// The world force vector, usually in Newtons (N). + /// The world position of the point of application. + public void ApplyForce(ref Vector2 force, ref Vector2 point) + { + Debug.Assert(!float.IsNaN(force.X)); + Debug.Assert(!float.IsNaN(force.Y)); + Debug.Assert(!float.IsNaN(point.X)); + Debug.Assert(!float.IsNaN(point.Y)); + + if (_bodyType == BodyType.Dynamic) + { + if (Awake == false) + { + Awake = true; + } + + _force += force; + _torque += (point.X - _sweep.C.X) * force.Y - (point.Y - _sweep.C.Y) * force.X; + } + } + + /// + /// Apply a torque. This affects the angular velocity + /// without affecting the linear velocity of the center of mass. + /// This wakes up the body. + /// + /// The torque about the z-axis (out of the screen), usually in N-m. + public void ApplyTorque(float torque) + { + Debug.Assert(!float.IsNaN(torque)); + + if (_bodyType == BodyType.Dynamic) + { + if (Awake == false) + { + Awake = true; + } + + _torque += torque; + } + } + + /// + /// Apply an impulse at a point. This immediately modifies the velocity. + /// This wakes up the body. + /// + /// The world impulse vector, usually in N-seconds or kg-m/s. + public void ApplyLinearImpulse(Vector2 impulse) + { + ApplyLinearImpulse(ref impulse); + } + + /// + /// Apply an impulse at a point. This immediately modifies the velocity. + /// It also modifies the angular velocity if the point of application + /// is not at the center of mass. + /// This wakes up the body. + /// + /// The world impulse vector, usually in N-seconds or kg-m/s. + /// The world position of the point of application. + public void ApplyLinearImpulse(Vector2 impulse, Vector2 point) + { + ApplyLinearImpulse(ref impulse, ref point); + } + + /// + /// Apply an impulse at a point. This immediately modifies the velocity. + /// This wakes up the body. + /// + /// The world impulse vector, usually in N-seconds or kg-m/s. + public void ApplyLinearImpulse(ref Vector2 impulse) + { + if (_bodyType != BodyType.Dynamic) + { + return; + } + if (Awake == false) + { + Awake = true; + } + _linearVelocity += _invMass * impulse; + } + + /// + /// Apply an impulse at a point. This immediately modifies the velocity. + /// It also modifies the angular velocity if the point of application + /// is not at the center of mass. + /// This wakes up the body. + /// + /// The world impulse vector, usually in N-seconds or kg-m/s. + /// The world position of the point of application. + public void ApplyLinearImpulse(ref Vector2 impulse, ref Vector2 point) + { + if (_bodyType != BodyType.Dynamic) + return; + + if (Awake == false) + Awake = true; + + _linearVelocity += _invMass * impulse; + _angularVelocity += _invI * ((point.X - _sweep.C.X) * impulse.Y - (point.Y - _sweep.C.Y) * impulse.X); + } + + /// + /// Apply an angular impulse. + /// + /// The angular impulse in units of kg*m*m/s. + public void ApplyAngularImpulse(float impulse) + { + if (_bodyType != BodyType.Dynamic) + { + return; + } + + if (Awake == false) + { + Awake = true; + } + + _angularVelocity += _invI * impulse; + } + + /// + /// This resets the mass properties to the sum of the mass properties of the fixtures. + /// This normally does not need to be called unless you called SetMassData to override + /// the mass and you later want to reset the mass. + /// + public void ResetMassData() + { + // Compute mass data from shapes. Each shape has its own density. + _mass = 0.0f; + _invMass = 0.0f; + _inertia = 0.0f; + _invI = 0.0f; + _sweep.LocalCenter = Vector2.Zero; + + // Kinematic bodies have zero mass. + if (BodyType == BodyType.Kinematic) + { + _sweep.C0 = _xf.p; + _sweep.C = _xf.p; + _sweep.A0 = _sweep.A; + return; + } + + Debug.Assert(BodyType == BodyType.Dynamic || BodyType == BodyType.Static); + + // Accumulate mass over all fixtures. + Vector2 localCenter = Vector2.Zero; + foreach (Fixture f in FixtureList) + { + if (f.Shape._density == 0) + { + continue; + } + + MassData massData = f.Shape.MassData; + _mass += massData.Mass; + localCenter += massData.Mass * massData.Centroid; + _inertia += massData.Inertia; + } + + //Static bodies only have mass, they don't have other properties. A little hacky tho... + if (BodyType == BodyType.Static) + { + _sweep.C0 = _sweep.C = _xf.p; + return; + } + + // Compute center of mass. + if (_mass > 0.0f) + { + _invMass = 1.0f / _mass; + localCenter *= _invMass; + } + else + { + // Force all dynamic bodies to have a positive mass. + _mass = 1.0f; + _invMass = 1.0f; + } + + if (_inertia > 0.0f && !_fixedRotation) + { + // Center the inertia about the center of mass. + _inertia -= _mass * Vector2.Dot(localCenter, localCenter); + + Debug.Assert(_inertia > 0.0f); + _invI = 1.0f / _inertia; + } + else + { + _inertia = 0.0f; + _invI = 0.0f; + } + + // Move center of mass. + Vector2 oldCenter = _sweep.C; + _sweep.LocalCenter = localCenter; + _sweep.C0 = _sweep.C = MathUtils.Mul(ref _xf, ref _sweep.LocalCenter); + + // Update center of mass velocity. + Vector2 a = _sweep.C - oldCenter; + _linearVelocity += new Vector2(-_angularVelocity * a.Y, _angularVelocity * a.X); + } + + /// + /// Get the world coordinates of a point given the local coordinates. + /// + /// A point on the body measured relative the the body's origin. + /// The same point expressed in world coordinates. + public Vector2 GetWorldPoint(ref Vector2 localPoint) + { + return MathUtils.Mul(ref _xf, ref localPoint); + } + + /// + /// Get the world coordinates of a point given the local coordinates. + /// + /// A point on the body measured relative the the body's origin. + /// The same point expressed in world coordinates. + public Vector2 GetWorldPoint(Vector2 localPoint) + { + return GetWorldPoint(ref localPoint); + } + + /// + /// Get the world coordinates of a vector given the local coordinates. + /// Note that the vector only takes the rotation into account, not the position. + /// + /// A vector fixed in the body. + /// The same vector expressed in world coordinates. + public Vector2 GetWorldVector(ref Vector2 localVector) + { + return MathUtils.Mul(_xf.q, localVector); + } + + /// + /// Get the world coordinates of a vector given the local coordinates. + /// + /// A vector fixed in the body. + /// The same vector expressed in world coordinates. + public Vector2 GetWorldVector(Vector2 localVector) + { + return GetWorldVector(ref localVector); + } + + /// + /// Gets a local point relative to the body's origin given a world point. + /// Note that the vector only takes the rotation into account, not the position. + /// + /// A point in world coordinates. + /// The corresponding local point relative to the body's origin. + public Vector2 GetLocalPoint(ref Vector2 worldPoint) + { + return MathUtils.MulT(ref _xf, worldPoint); + } + + /// + /// Gets a local point relative to the body's origin given a world point. + /// + /// A point in world coordinates. + /// The corresponding local point relative to the body's origin. + public Vector2 GetLocalPoint(Vector2 worldPoint) + { + return GetLocalPoint(ref worldPoint); + } + + /// + /// Gets a local vector given a world vector. + /// Note that the vector only takes the rotation into account, not the position. + /// + /// A vector in world coordinates. + /// The corresponding local vector. + public Vector2 GetLocalVector(ref Vector2 worldVector) + { + return MathUtils.MulT(_xf.q, worldVector); + } + + /// + /// Gets a local vector given a world vector. + /// Note that the vector only takes the rotation into account, not the position. + /// + /// A vector in world coordinates. + /// The corresponding local vector. + public Vector2 GetLocalVector(Vector2 worldVector) + { + return GetLocalVector(ref worldVector); + } + + /// + /// Get the world linear velocity of a world point attached to this body. + /// + /// A point in world coordinates. + /// The world velocity of a point. + public Vector2 GetLinearVelocityFromWorldPoint(Vector2 worldPoint) + { + return GetLinearVelocityFromWorldPoint(ref worldPoint); + } + + /// + /// Get the world linear velocity of a world point attached to this body. + /// + /// A point in world coordinates. + /// The world velocity of a point. + public Vector2 GetLinearVelocityFromWorldPoint(ref Vector2 worldPoint) + { + return _linearVelocity + + new Vector2(-_angularVelocity * (worldPoint.Y - _sweep.C.Y), + _angularVelocity * (worldPoint.X - _sweep.C.X)); + } + + /// + /// Get the world velocity of a local point. + /// + /// A point in local coordinates. + /// The world velocity of a point. + public Vector2 GetLinearVelocityFromLocalPoint(Vector2 localPoint) + { + return GetLinearVelocityFromLocalPoint(ref localPoint); + } + + /// + /// Get the world velocity of a local point. + /// + /// A point in local coordinates. + /// The world velocity of a point. + public Vector2 GetLinearVelocityFromLocalPoint(ref Vector2 localPoint) + { + return GetLinearVelocityFromWorldPoint(GetWorldPoint(ref localPoint)); + } + + internal void SynchronizeFixtures() + { + Transform xf1 = new Transform(); + xf1.q.Set(_sweep.A0); + xf1.p = _sweep.C0 - MathUtils.Mul(xf1.q, _sweep.LocalCenter); + + IBroadPhase broadPhase = _world.ContactManager.BroadPhase; + for (int i = 0; i < FixtureList.Count; i++) + { + FixtureList[i].Synchronize(broadPhase, ref xf1, ref _xf); + } + } + + internal void SynchronizeTransform() + { + _xf.q.Set(_sweep.A); + _xf.p = _sweep.C - MathUtils.Mul(_xf.q, _sweep.LocalCenter); + } + + /// + /// This is used to prevent connected bodies from colliding. + /// It may lie, depending on the collideConnected flag. + /// + /// The other body. + /// + internal bool ShouldCollide(Body other) + { + // At least one body should be dynamic. + if (_bodyType != BodyType.Dynamic && other._bodyType != BodyType.Dynamic) + { + return false; + } + + // Does a joint prevent collision? + for (JointEdge jn = JointList; jn != null; jn = jn.Next) + { + if (jn.Other == other) + { + if (jn.Joint.CollideConnected == false) + { + return false; + } + } + } + + return true; + } + + internal void Advance(float alpha) + { + // Advance to the new safe time. This doesn't sync the broad-phase. + _sweep.Advance(alpha); + _sweep.C = _sweep.C0; + _sweep.A = _sweep.A0; + _xf.q.Set(_sweep.A); + _xf.p = _sweep.C - MathUtils.Mul(_xf.q, _sweep.LocalCenter); + } + + public event OnCollisionEventHandler OnCollision + { + add + { + for (int i = 0; i < FixtureList.Count; i++) + { + FixtureList[i].OnCollision += value; + } + } + remove + { + for (int i = 0; i < FixtureList.Count; i++) + { + FixtureList[i].OnCollision -= value; + } + } + } + + public event OnSeparationEventHandler OnSeparation + { + add + { + for (int i = 0; i < FixtureList.Count; i++) + { + FixtureList[i].OnSeparation += value; + } + } + remove + { + for (int i = 0; i < FixtureList.Count; i++) + { + FixtureList[i].OnSeparation -= value; + } + } + } + + public void IgnoreCollisionWith(Body other) + { + for (int i = 0; i < FixtureList.Count; i++) + { + for (int j = 0; j < other.FixtureList.Count; j++) + { + FixtureList[i].IgnoreCollisionWith(other.FixtureList[j]); + } + } + } + + public void RestoreCollisionWith(Body other) + { + for (int i = 0; i < FixtureList.Count; i++) + { + for (int j = 0; j < other.FixtureList.Count; j++) + { + FixtureList[i].RestoreCollisionWith(other.FixtureList[j]); + } + } + } + + #region IDisposable Members + + public bool IsDisposed { get; set; } + + public void Dispose() + { + if (!IsDisposed) + { + _world.RemoveBody(this); + IsDisposed = true; + GC.SuppressFinalize(this); + } + } + + #endregion + + /// + /// Makes a clone of the body. Fixtures and therefore shapes are not included. + /// Use DeepClone() to clone the body, as well as fixtures and shapes. + /// + /// + /// + public Body Clone(World world = null) + { + Body body = new Body(world ?? _world, Position, Rotation, UserData); + body._bodyType = _bodyType; + body._linearVelocity = _linearVelocity; + body._angularVelocity = _angularVelocity; + body.GravityScale = GravityScale; + body.UserData = UserData; + body._enabled = _enabled; + body._fixedRotation = _fixedRotation; + body._sleepingAllowed = _sleepingAllowed; + body._linearDamping = _linearDamping; + body._angularDamping = _angularDamping; + body._awake = _awake; + body.IsBullet = IsBullet; + body.IgnoreCCD = IgnoreCCD; + body.IgnoreGravity = IgnoreGravity; + body._torque = _torque; + + return body; + } + + /// + /// Clones the body and all attached fixtures and shapes. Simply said, it makes a complete copy of the body. + /// + /// + /// + public Body DeepClone(World world = null) + { + Body body = Clone(world ?? _world); + + int count = FixtureList.Count; //Make a copy of the count. Otherwise it causes an infinite loop. + for (int i = 0; i < count; i++) + { + FixtureList[i].CloneOnto(body); + } + + return body; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Dynamics/BreakableBody.cs b/Farseer Physics Engine 3.5/Dynamics/BreakableBody.cs new file mode 100644 index 000000000..b63a776bc --- /dev/null +++ b/Farseer Physics Engine 3.5/Dynamics/BreakableBody.cs @@ -0,0 +1,148 @@ +using System; +using System.Collections.Generic; +using FarseerPhysics.Collision.Shapes; +using FarseerPhysics.Common; +using FarseerPhysics.Dynamics.Contacts; +using FarseerPhysics.Factories; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics +{ + /// + /// A type of body that supports multiple fixtures that can break apart. + /// + public class BreakableBody + { + private float[] _angularVelocitiesCache = new float[8]; + private bool _break; + private Vector2[] _velocitiesCache = new Vector2[8]; + private World _world; + + public BreakableBody(IEnumerable vertices, World world, float density) + { + _world = world; + _world.ContactManager.PostSolve += PostSolve; + MainBody = new Body(_world); + MainBody.BodyType = BodyType.Dynamic; + + foreach (Vertices part in vertices) + { + PolygonShape polygonShape = new PolygonShape(part, density); + Fixture fixture = MainBody.CreateFixture(polygonShape); + Parts.Add(fixture); + } + } + + public BreakableBody(IEnumerable shapes, World world) + { + _world = world; + _world.ContactManager.PostSolve += PostSolve; + MainBody = new Body(_world); + MainBody.BodyType = BodyType.Dynamic; + + foreach (Shape part in shapes) + { + Fixture fixture = MainBody.CreateFixture(part); + Parts.Add(fixture); + } + } + + public bool Broken; + public Body MainBody; + public List Parts = new List(8); + + /// + /// The force needed to break the body apart. + /// Default: 500 + /// + public float Strength = 500.0f; + + private void PostSolve(Contact contact, ContactVelocityConstraint impulse) + { + if (!Broken) + { + if (Parts.Contains(contact.FixtureA) || Parts.Contains(contact.FixtureB)) + { + float maxImpulse = 0.0f; + int count = contact.Manifold.PointCount; + + for (int i = 0; i < count; ++i) + { + maxImpulse = Math.Max(maxImpulse, impulse.points[i].normalImpulse); + } + + if (maxImpulse > Strength) + { + // Flag the body for breaking. + _break = true; + } + } + } + } + + public void Update() + { + if (_break) + { + Decompose(); + Broken = true; + _break = false; + } + + // Cache velocities to improve movement on breakage. + if (Broken == false) + { + //Enlarge the cache if needed + if (Parts.Count > _angularVelocitiesCache.Length) + { + _velocitiesCache = new Vector2[Parts.Count]; + _angularVelocitiesCache = new float[Parts.Count]; + } + + //Cache the linear and angular velocities. + for (int i = 0; i < Parts.Count; i++) + { + _velocitiesCache[i] = Parts[i].Body.LinearVelocity; + _angularVelocitiesCache[i] = Parts[i].Body.AngularVelocity; + } + } + } + + private void Decompose() + { + //Unsubsribe from the PostSolve delegate + _world.ContactManager.PostSolve -= PostSolve; + + for (int i = 0; i < Parts.Count; i++) + { + Fixture oldFixture = Parts[i]; + + Shape shape = oldFixture.Shape.Clone(); + object userData = oldFixture.UserData; + + MainBody.DestroyFixture(oldFixture); + + Body body = BodyFactory.CreateBody(_world); + body.BodyType = BodyType.Dynamic; + body.Position = MainBody.Position; + body.Rotation = MainBody.Rotation; + body.UserData = MainBody.UserData; + + Fixture newFixture = body.CreateFixture(shape); + newFixture.UserData = userData; + Parts[i] = newFixture; + + body.AngularVelocity = _angularVelocitiesCache[i]; + body.LinearVelocity = _velocitiesCache[i]; + } + + _world.RemoveBody(MainBody); + _world.RemoveBreakableBody(this); + } + + public void Break() + { + _break = true; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Dynamics/ContactManager.cs b/Farseer Physics Engine 3.5/Dynamics/ContactManager.cs new file mode 100644 index 000000000..12cd38c80 --- /dev/null +++ b/Farseer Physics Engine 3.5/Dynamics/ContactManager.cs @@ -0,0 +1,445 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ +//#define USE_ACTIVE_CONTACT_SET + +using System.Collections.Generic; +using FarseerPhysics.Collision; +using FarseerPhysics.Dynamics.Contacts; + +namespace FarseerPhysics.Dynamics +{ + public class ContactManager + { + /// + /// Fires when a contact is created + /// + public BeginContactDelegate BeginContact; + + public IBroadPhase BroadPhase; + + /// + /// The filter used by the contact manager. + /// + public CollisionFilterDelegate ContactFilter; + + public List ContactList = new List(128); + +#if USE_ACTIVE_CONTACT_SET + /// + /// The set of active contacts. + /// + public HashSet ActiveContacts = new HashSet(); + + /// + /// A temporary copy of active contacts that is used during updates so + /// the hash set can have members added/removed during the update. + /// This list is cleared after every update. + /// + List ActiveList = new List(); +#endif + + /// + /// Fires when a contact is deleted + /// + public EndContactDelegate EndContact; + + /// + /// Fires when the broadphase detects that two Fixtures are close to each other. + /// + public BroadphaseDelegate OnBroadphaseCollision; + + /// + /// Fires after the solver has run + /// + public PostSolveDelegate PostSolve; + + /// + /// Fires before the solver runs + /// + public PreSolveDelegate PreSolve; + + internal ContactManager(IBroadPhase broadPhase) + { + BroadPhase = broadPhase; + OnBroadphaseCollision = AddPair; + } + + // Broad-phase callback. + private void AddPair(ref FixtureProxy proxyA, ref FixtureProxy proxyB) + { + Fixture fixtureA = proxyA.Fixture; + Fixture fixtureB = proxyB.Fixture; + + int indexA = proxyA.ChildIndex; + int indexB = proxyB.ChildIndex; + + Body bodyA = fixtureA.Body; + Body bodyB = fixtureB.Body; + + // Are the fixtures on the same body? + if (bodyA == bodyB) + { + return; + } + + // Does a contact already exist? + ContactEdge edge = bodyB.ContactList; + while (edge != null) + { + if (edge.Other == bodyA) + { + Fixture fA = edge.Contact.FixtureA; + Fixture fB = edge.Contact.FixtureB; + int iA = edge.Contact.ChildIndexA; + int iB = edge.Contact.ChildIndexB; + + if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB) + { + // A contact already exists. + return; + } + + if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA) + { + // A contact already exists. + return; + } + } + + edge = edge.Next; + } + + // Does a joint override collision? Is at least one body dynamic? + if (bodyB.ShouldCollide(bodyA) == false) + return; + + //Check default filter + if (ShouldCollide(fixtureA, fixtureB) == false) + return; + + // Check user filtering. + if (ContactFilter != null && ContactFilter(fixtureA, fixtureB) == false) + return; + + //FPE feature: BeforeCollision delegate + if (fixtureA.BeforeCollision != null && fixtureA.BeforeCollision(fixtureA, fixtureB) == false) + return; + + if (fixtureB.BeforeCollision != null && fixtureB.BeforeCollision(fixtureB, fixtureA) == false) + return; + + // Call the factory. + Contact c = Contact.Create(fixtureA, indexA, fixtureB, indexB); + + if (c == null) + return; + + // Contact creation may swap fixtures. + fixtureA = c.FixtureA; + fixtureB = c.FixtureB; + bodyA = fixtureA.Body; + bodyB = fixtureB.Body; + + // Insert into the world. + ContactList.Add(c); + +#if USE_ACTIVE_CONTACT_SET + ActiveContacts.Add(c); +#endif + // Connect to island graph. + + // Connect to body A + c._nodeA.Contact = c; + c._nodeA.Other = bodyB; + + c._nodeA.Prev = null; + c._nodeA.Next = bodyA.ContactList; + if (bodyA.ContactList != null) + { + bodyA.ContactList.Prev = c._nodeA; + } + bodyA.ContactList = c._nodeA; + + // Connect to body B + c._nodeB.Contact = c; + c._nodeB.Other = bodyA; + + c._nodeB.Prev = null; + c._nodeB.Next = bodyB.ContactList; + if (bodyB.ContactList != null) + { + bodyB.ContactList.Prev = c._nodeB; + } + bodyB.ContactList = c._nodeB; + + // Wake up the bodies + if (fixtureA.IsSensor == false && fixtureB.IsSensor == false) + { + bodyA.Awake = true; + bodyB.Awake = true; + } + } + + internal void FindNewContacts() + { + BroadPhase.UpdatePairs(OnBroadphaseCollision); + } + + internal void Destroy(Contact contact) + { + Fixture fixtureA = contact.FixtureA; + Fixture fixtureB = contact.FixtureB; + Body bodyA = fixtureA.Body; + Body bodyB = fixtureB.Body; + + if (contact.IsTouching) + { + //Report the separation to both participants: + if (fixtureA != null && fixtureA.OnSeparation != null) + fixtureA.OnSeparation(fixtureA, fixtureB); + + //Reverse the order of the reported fixtures. The first fixture is always the one that the + //user subscribed to. + if (fixtureB != null && fixtureB.OnSeparation != null) + fixtureB.OnSeparation(fixtureB, fixtureA); + + if (EndContact != null) + EndContact(contact); + } + + // Remove from the world. + ContactList.Remove(contact); + + // Remove from body 1 + if (contact._nodeA.Prev != null) + { + contact._nodeA.Prev.Next = contact._nodeA.Next; + } + + if (contact._nodeA.Next != null) + { + contact._nodeA.Next.Prev = contact._nodeA.Prev; + } + + if (contact._nodeA == bodyA.ContactList) + { + bodyA.ContactList = contact._nodeA.Next; + } + + // Remove from body 2 + if (contact._nodeB.Prev != null) + { + contact._nodeB.Prev.Next = contact._nodeB.Next; + } + + if (contact._nodeB.Next != null) + { + contact._nodeB.Next.Prev = contact._nodeB.Prev; + } + + if (contact._nodeB == bodyB.ContactList) + { + bodyB.ContactList = contact._nodeB.Next; + } + +#if USE_ACTIVE_CONTACT_SET + if (ActiveContacts.Contains(contact)) + { + ActiveContacts.Remove(contact); + } +#endif + contact.Destroy(); + } + + internal void Collide() + { + // Update awake contacts. +#if USE_ACTIVE_CONTACT_SET + ActiveList.AddRange(ActiveContacts); + + foreach (var c in ActiveList) + { +#else + for (int i = 0; i < ContactList.Count; i++) + { + Contact c = ContactList[i]; +#endif + Fixture fixtureA = c.FixtureA; + Fixture fixtureB = c.FixtureB; + int indexA = c.ChildIndexA; + int indexB = c.ChildIndexB; + Body bodyA = fixtureA.Body; + Body bodyB = fixtureB.Body; + + //Do no try to collide disabled bodies + if (!bodyA.Enabled || !bodyB.Enabled) + continue; + + // Is this contact flagged for filtering? + if (c.FilterFlag) + { + // Should these bodies collide? + if (bodyB.ShouldCollide(bodyA) == false) + { + Contact cNuke = c; + Destroy(cNuke); + continue; + } + + // Check default filtering + if (ShouldCollide(fixtureA, fixtureB) == false) + { + Contact cNuke = c; + Destroy(cNuke); + continue; + } + + // Check user filtering. + if (ContactFilter != null && ContactFilter(fixtureA, fixtureB) == false) + { + Contact cNuke = c; + Destroy(cNuke); + continue; + } + + // Clear the filtering flag. + c.FilterFlag = false; + } + + bool activeA = bodyA.Awake && bodyA.BodyType != BodyType.Static; + bool activeB = bodyB.Awake && bodyB.BodyType != BodyType.Static; + + // At least one body must be awake and it must be dynamic or kinematic. + if (activeA == false && activeB == false) + { +#if USE_ACTIVE_CONTACT_SET + ActiveContacts.Remove(c); +#endif + continue; + } + + int proxyIdA = fixtureA.Proxies[indexA].ProxyId; + int proxyIdB = fixtureB.Proxies[indexB].ProxyId; + + bool overlap = BroadPhase.TestOverlap(proxyIdA, proxyIdB); + + // Here we destroy contacts that cease to overlap in the broad-phase. + if (overlap == false) + { + Contact cNuke = c; + Destroy(cNuke); + continue; + } + + // The contact persists. + c.Update(this); + } + +#if USE_ACTIVE_CONTACT_SET + ActiveList.Clear(); +#endif + } + + private static bool ShouldCollide(Fixture fixtureA, Fixture fixtureB) + { + if (Settings.UseFPECollisionCategories) + { + if ((fixtureA.CollisionGroup == fixtureB.CollisionGroup) && + fixtureA.CollisionGroup != 0 && fixtureB.CollisionGroup != 0) + return false; + + if (((fixtureA.CollisionCategories & fixtureB.CollidesWith) == + Category.None) & + ((fixtureB.CollisionCategories & fixtureA.CollidesWith) == + Category.None)) + return false; + + if (fixtureA.IsFixtureIgnored(fixtureB) || + fixtureB.IsFixtureIgnored(fixtureA)) + return false; + + return true; + } + + if (fixtureA.CollisionGroup == fixtureB.CollisionGroup && + fixtureA.CollisionGroup != 0) + { + return fixtureA.CollisionGroup > 0; + } + + bool collide = (fixtureA.CollidesWith & fixtureB.CollisionCategories) != 0 && + (fixtureA.CollisionCategories & fixtureB.CollidesWith) != 0; + + if (collide) + { + if (fixtureA.IsFixtureIgnored(fixtureB) || + fixtureB.IsFixtureIgnored(fixtureA)) + { + return false; + } + } + + return collide; + } + + internal void UpdateContacts(ContactEdge contactEdge, bool value) + { +#if USE_ACTIVE_CONTACT_SET + if(value) + { + while(contactEdge != null) + { + var c = contactEdge.Contact; + if (!ActiveContacts.Contains(c)) + { + ActiveContacts.Add(c); + } + contactEdge = contactEdge.Next; + } + } + else + { + while (contactEdge != null) + { + var c = contactEdge.Contact; + if (!contactEdge.Other.Awake) + { + if (ActiveContacts.Contains(c)) + { + ActiveContacts.Remove(c); + } + } + contactEdge = contactEdge.Next; + } + } +#endif + } + +#if USE_ACTIVE_CONTACT_SET + internal void RemoveActiveContact(Contact contact) + { + if (ActiveContacts.Contains(contact)) + ActiveContacts.Remove(contact); + } +#endif + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Dynamics/Contacts/Contact.cs b/Farseer Physics Engine 3.5/Dynamics/Contacts/Contact.cs new file mode 100644 index 000000000..dfd0f7754 --- /dev/null +++ b/Farseer Physics Engine 3.5/Dynamics/Contacts/Contact.cs @@ -0,0 +1,480 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ +//#define USE_ACTIVE_CONTACT_SET + +using System.Collections.Generic; +using System.Diagnostics; +using FarseerPhysics.Collision; +using FarseerPhysics.Collision.Shapes; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Contacts +{ + /// + /// A contact edge is used to connect bodies and contacts together + /// in a contact graph where each body is a node and each contact + /// is an edge. A contact edge belongs to a doubly linked list + /// maintained in each attached body. Each contact has two contact + /// nodes, one for each attached body. + /// + public sealed class ContactEdge + { + /// + /// The contact + /// + public Contact Contact; + + /// + /// The next contact edge in the body's contact list + /// + public ContactEdge Next; + + /// + /// Provides quick access to the other body attached. + /// + public Body Other; + + /// + /// The previous contact edge in the body's contact list + /// + public ContactEdge Prev; + } + + /// + /// The class manages contact between two shapes. A contact exists for each overlapping + /// AABB in the broad-phase (except if filtered). Therefore a contact object may exist + /// that has no contact points. + /// + public class Contact + { + private ContactType _type; + + private static EdgeShape _edge = new EdgeShape(); + + private static ContactType[,] _registers = new[,] + { + { + ContactType.Circle, + ContactType.EdgeAndCircle, + ContactType.PolygonAndCircle, + ContactType.ChainAndCircle, + }, + { + ContactType.EdgeAndCircle, + ContactType.NotSupported, + // 1,1 is invalid (no ContactType.Edge) + ContactType.EdgeAndPolygon, + ContactType.NotSupported, + // 1,3 is invalid (no ContactType.EdgeAndLoop) + }, + { + ContactType.PolygonAndCircle, + ContactType.EdgeAndPolygon, + ContactType.Polygon, + ContactType.ChainAndPolygon, + }, + { + ContactType.ChainAndCircle, + ContactType.NotSupported, + // 3,1 is invalid (no ContactType.EdgeAndLoop) + ContactType.ChainAndPolygon, + ContactType.NotSupported, + // 3,3 is invalid (no ContactType.Loop) + }, + }; + // Nodes for connecting bodies. + internal ContactEdge _nodeA = new ContactEdge(); + internal ContactEdge _nodeB = new ContactEdge(); + internal int _toiCount; + internal float _toi; + + public Fixture FixtureA; + public Fixture FixtureB; + public float Friction { get; set; } + public float Restitution { get; set; } + + /// + /// Get the contact manifold. Do not modify the manifold unless you understand the + /// internals of Box2D. + /// + public Manifold Manifold; + + /// Get or set the desired tangent speed for a conveyor belt behavior. In meters per second. + public float TangentSpeed { get; set; } + + /// Enable/disable this contact. This can be used inside the pre-solve + /// contact listener. The contact is only disabled for the current + /// time step (or sub-step in continuous collisions). + /// NOTE: If you are setting Enabled to a constant true or false, + /// use the explicit Enable() or Disable() functions instead to + /// save the CPU from doing a branch operation. + public bool Enabled { get; set; } + + /// + /// Get the child primitive index for fixture A. + /// + /// The child index A. + public int ChildIndexA { get; internal set; } + + /// + /// Get the child primitive index for fixture B. + /// + /// The child index B. + public int ChildIndexB { get; internal set; } + + /// + /// Determines whether this contact is touching. + /// + /// + /// true if this instance is touching; otherwise, false. + /// + public bool IsTouching { get; set; } + + internal bool IslandFlag { get; set; } + internal bool TOIFlag { get; set; } + internal bool FilterFlag { get; set; } + + public void ResetRestitution() + { + Restitution = Settings.MixRestitution(FixtureA.Restitution, FixtureB.Restitution); + } + + public void ResetFriction() + { + Friction = Settings.MixFriction(FixtureA.Friction, FixtureB.Friction); + } + + private Contact(Fixture fA, int indexA, Fixture fB, int indexB) + { + Reset(fA, indexA, fB, indexB); + } + + /// + /// Gets the world manifold. + /// + public void GetWorldManifold(out Vector2 normal, out FixedArray2 points) + { + Body bodyA = FixtureA.Body; + Body bodyB = FixtureB.Body; + Shape shapeA = FixtureA.Shape; + Shape shapeB = FixtureB.Shape; + + ContactSolver.WorldManifold.Initialize(ref Manifold, ref bodyA._xf, shapeA.Radius, ref bodyB._xf, shapeB.Radius, out normal, out points); + } + + private void Reset(Fixture fA, int indexA, Fixture fB, int indexB) + { + Enabled = true; + IsTouching = false; + IslandFlag = false; + FilterFlag = false; + TOIFlag = false; + + FixtureA = fA; + FixtureB = fB; + + ChildIndexA = indexA; + ChildIndexB = indexB; + + Manifold.PointCount = 0; + + _nodeA.Contact = null; + _nodeA.Prev = null; + _nodeA.Next = null; + _nodeA.Other = null; + + _nodeB.Contact = null; + _nodeB.Prev = null; + _nodeB.Next = null; + _nodeB.Other = null; + + _toiCount = 0; + + //FPE: We only set the friction and restitution if we are not destroying the contact + if (FixtureA != null && FixtureB != null) + { + Friction = Settings.MixFriction(FixtureA.Friction, FixtureB.Friction); + Restitution = Settings.MixRestitution(FixtureA.Restitution, FixtureB.Restitution); + } + + TangentSpeed = 0; + } + + /// + /// Update the contact manifold and touching status. + /// Note: do not assume the fixture AABBs are overlapping or are valid. + /// + /// The contact manager. + internal void Update(ContactManager contactManager) + { + Body bodyA = FixtureA.Body; + Body bodyB = FixtureB.Body; + + if (FixtureA == null || FixtureB == null) + return; + + Manifold oldManifold = Manifold; + + // Re-enable this contact. + Enabled = true; + + bool touching; + bool wasTouching = IsTouching; + + bool sensor = FixtureA.IsSensor || FixtureB.IsSensor; + + // Is this contact a sensor? + if (sensor) + { + Shape shapeA = FixtureA.Shape; + Shape shapeB = FixtureB.Shape; + touching = Collision.Collision.TestOverlap(shapeA, ChildIndexA, shapeB, ChildIndexB, ref bodyA._xf, ref bodyB._xf); + + // Sensors don't generate manifolds. + Manifold.PointCount = 0; + } + else + { + Evaluate(ref Manifold, ref bodyA._xf, ref bodyB._xf); + touching = Manifold.PointCount > 0; + + // Match old contact ids to new contact ids and copy the + // stored impulses to warm start the solver. + for (int i = 0; i < Manifold.PointCount; ++i) + { + ManifoldPoint mp2 = Manifold.Points[i]; + mp2.NormalImpulse = 0.0f; + mp2.TangentImpulse = 0.0f; + ContactID id2 = mp2.Id; + + for (int j = 0; j < oldManifold.PointCount; ++j) + { + ManifoldPoint mp1 = oldManifold.Points[j]; + + if (mp1.Id.Key == id2.Key) + { + mp2.NormalImpulse = mp1.NormalImpulse; + mp2.TangentImpulse = mp1.TangentImpulse; + break; + } + } + + Manifold.Points[i] = mp2; + } + + if (touching != wasTouching) + { + bodyA.Awake = true; + bodyB.Awake = true; + } + } + + IsTouching = touching; + + if (wasTouching == false) + { + if (touching) + { + if (Settings.AllCollisionCallbacksAgree) + { + bool enabledA = true, enabledB = true; + + // Report the collision to both participants. Track which ones returned true so we can + // later call OnSeparation if the contact is disabled for a different reason. + if (FixtureA.OnCollision != null) + foreach (OnCollisionEventHandler handler in FixtureA.OnCollision.GetInvocationList()) + enabledA = handler(FixtureA, FixtureB, this) && enabledA; + + // Reverse the order of the reported fixtures. The first fixture is always the one that the + // user subscribed to. + if (FixtureB.OnCollision != null) + foreach (OnCollisionEventHandler handler in FixtureB.OnCollision.GetInvocationList()) + enabledB = handler(FixtureB, FixtureA, this) && enabledB; + + Enabled = enabledA && enabledB; + + // BeginContact can also return false and disable the contact + if (enabledA && enabledB && contactManager.BeginContact != null) + Enabled = contactManager.BeginContact(this); + } + else + { + //Report the collision to both participants: + if (FixtureA.OnCollision != null) + foreach (OnCollisionEventHandler handler in FixtureA.OnCollision.GetInvocationList()) + Enabled = handler(FixtureA, FixtureB, this); + + //Reverse the order of the reported fixtures. The first fixture is always the one that the + //user subscribed to. + if (FixtureB.OnCollision != null) + foreach (OnCollisionEventHandler handler in FixtureB.OnCollision.GetInvocationList()) + Enabled = handler(FixtureB, FixtureA, this); + + //BeginContact can also return false and disable the contact + if (contactManager.BeginContact != null) + Enabled = contactManager.BeginContact(this); + } + + // If the user disabled the contact (needed to exclude it in TOI solver) at any point by + // any of the callbacks, we need to mark it as not touching and call any separation + // callbacks for fixtures that didn't explicitly disable the collision. + if (!Enabled) + IsTouching = false; + } + } + else + { + if (touching == false) + { + //Report the separation to both participants: + if (FixtureA != null && FixtureA.OnSeparation != null) + FixtureA.OnSeparation(FixtureA, FixtureB); + + //Reverse the order of the reported fixtures. The first fixture is always the one that the + //user subscribed to. + if (FixtureB != null && FixtureB.OnSeparation != null) + FixtureB.OnSeparation(FixtureB, FixtureA); + + if (contactManager.EndContact != null) + contactManager.EndContact(this); + } + } + + if (sensor) + return; + + if (contactManager.PreSolve != null) + contactManager.PreSolve(this, ref oldManifold); + } + + /// + /// Evaluate this contact with your own manifold and transforms. + /// + /// The manifold. + /// The first transform. + /// The second transform. + private void Evaluate(ref Manifold manifold, ref Transform transformA, ref Transform transformB) + { + switch (_type) + { + case ContactType.Polygon: + Collision.Collision.CollidePolygons(ref manifold, (PolygonShape)FixtureA.Shape, ref transformA, (PolygonShape)FixtureB.Shape, ref transformB); + break; + case ContactType.PolygonAndCircle: + Collision.Collision.CollidePolygonAndCircle(ref manifold, (PolygonShape)FixtureA.Shape, ref transformA, (CircleShape)FixtureB.Shape, ref transformB); + break; + case ContactType.EdgeAndCircle: + Collision.Collision.CollideEdgeAndCircle(ref manifold, (EdgeShape)FixtureA.Shape, ref transformA, (CircleShape)FixtureB.Shape, ref transformB); + break; + case ContactType.EdgeAndPolygon: + Collision.Collision.CollideEdgeAndPolygon(ref manifold, (EdgeShape)FixtureA.Shape, ref transformA, (PolygonShape)FixtureB.Shape, ref transformB); + break; + case ContactType.ChainAndCircle: + ChainShape chain = (ChainShape)FixtureA.Shape; + chain.GetChildEdge(_edge, ChildIndexA); + Collision.Collision.CollideEdgeAndCircle(ref manifold, _edge, ref transformA, (CircleShape)FixtureB.Shape, ref transformB); + break; + case ContactType.ChainAndPolygon: + ChainShape loop2 = (ChainShape)FixtureA.Shape; + loop2.GetChildEdge(_edge, ChildIndexA); + Collision.Collision.CollideEdgeAndPolygon(ref manifold, _edge, ref transformA, (PolygonShape)FixtureB.Shape, ref transformB); + break; + case ContactType.Circle: + Collision.Collision.CollideCircles(ref manifold, (CircleShape)FixtureA.Shape, ref transformA, (CircleShape)FixtureB.Shape, ref transformB); + break; + } + } + + internal static Contact Create(Fixture fixtureA, int indexA, Fixture fixtureB, int indexB) + { + ShapeType type1 = fixtureA.Shape.ShapeType; + ShapeType type2 = fixtureB.Shape.ShapeType; + + Debug.Assert(ShapeType.Unknown < type1 && type1 < ShapeType.TypeCount); + Debug.Assert(ShapeType.Unknown < type2 && type2 < ShapeType.TypeCount); + + Contact c; + Queue pool = fixtureA.Body._world._contactPool; + if (pool.Count > 0) + { + c = pool.Dequeue(); + if ((type1 >= type2 || (type1 == ShapeType.Edge && type2 == ShapeType.Polygon)) && !(type2 == ShapeType.Edge && type1 == ShapeType.Polygon)) + { + c.Reset(fixtureA, indexA, fixtureB, indexB); + } + else + { + c.Reset(fixtureB, indexB, fixtureA, indexA); + } + } + else + { + // Edge+Polygon is non-symetrical due to the way Erin handles collision type registration. + if ((type1 >= type2 || (type1 == ShapeType.Edge && type2 == ShapeType.Polygon)) && !(type2 == ShapeType.Edge && type1 == ShapeType.Polygon)) + { + c = new Contact(fixtureA, indexA, fixtureB, indexB); + } + else + { + c = new Contact(fixtureB, indexB, fixtureA, indexA); + } + } + + c._type = _registers[(int)type1, (int)type2]; + + return c; + } + + internal void Destroy() + { +#if USE_ACTIVE_CONTACT_SET + FixtureA.Body.World.ContactManager.RemoveActiveContact(this); +#endif + FixtureA.Body._world._contactPool.Enqueue(this); + + if (Manifold.PointCount > 0 && FixtureA.IsSensor == false && FixtureB.IsSensor == false) + { + FixtureA.Body.Awake = true; + FixtureB.Body.Awake = true; + } + + Reset(null, 0, null, 0); + } + + #region Nested type: ContactType + + private enum ContactType + { + NotSupported, + Polygon, + PolygonAndCircle, + Circle, + EdgeAndPolygon, + EdgeAndCircle, + ChainAndPolygon, + ChainAndCircle, + } + + #endregion + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Dynamics/Contacts/ContactSolver.cs b/Farseer Physics Engine 3.5/Dynamics/Contacts/ContactSolver.cs new file mode 100644 index 000000000..f74937e70 --- /dev/null +++ b/Farseer Physics Engine 3.5/Dynamics/Contacts/ContactSolver.cs @@ -0,0 +1,979 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using FarseerPhysics.Collision; +using FarseerPhysics.Collision.Shapes; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Contacts +{ + public sealed class ContactPositionConstraint + { + public Vector2[] localPoints = new Vector2[Settings.MaxManifoldPoints]; + public Vector2 localNormal; + public Vector2 localPoint; + public int indexA; + public int indexB; + public float invMassA, invMassB; + public Vector2 localCenterA, localCenterB; + public float invIA, invIB; + public ManifoldType type; + public float radiusA, radiusB; + public int pointCount; + } + + public sealed class VelocityConstraintPoint + { + public Vector2 rA; + public Vector2 rB; + public float normalImpulse; + public float tangentImpulse; + public float normalMass; + public float tangentMass; + public float velocityBias; + } + + public sealed class ContactVelocityConstraint + { + public VelocityConstraintPoint[] points = new VelocityConstraintPoint[Settings.MaxManifoldPoints]; + public Vector2 normal; + public Mat22 normalMass; + public Mat22 K; + public int indexA; + public int indexB; + public float invMassA, invMassB; + public float invIA, invIB; + public float friction; + public float restitution; + public float tangentSpeed; + public int pointCount; + public int contactIndex; + + public ContactVelocityConstraint() + { + for (int i = 0; i < Settings.MaxManifoldPoints; i++) + { + points[i] = new VelocityConstraintPoint(); + } + } + } + + public class ContactSolver + { + public TimeStep _step; + public Position[] _positions; + public Velocity[] _velocities; + public ContactPositionConstraint[] _positionConstraints; + public ContactVelocityConstraint[] _velocityConstraints; + public Contact[] _contacts; + public int _count; + + public void Reset(TimeStep step, int count, Contact[] contacts, Position[] positions, Velocity[] velocities, bool warmstarting = Settings.EnableWarmstarting) + { + _step = step; + _count = count; + _positions = positions; + _velocities = velocities; + _contacts = contacts; + + // grow the array + if (_velocityConstraints == null || _velocityConstraints.Length < count) + { + _velocityConstraints = new ContactVelocityConstraint[count * 2]; + _positionConstraints = new ContactPositionConstraint[count * 2]; + + for (int i = 0; i < _velocityConstraints.Length; i++) + { + _velocityConstraints[i] = new ContactVelocityConstraint(); + } + + for (int i = 0; i < _positionConstraints.Length; i++) + { + _positionConstraints[i] = new ContactPositionConstraint(); + } + } + + // Initialize position independent portions of the constraints. + for (int i = 0; i < _count; ++i) + { + Contact contact = contacts[i]; + + Fixture fixtureA = contact.FixtureA; + Fixture fixtureB = contact.FixtureB; + Shape shapeA = fixtureA.Shape; + Shape shapeB = fixtureB.Shape; + float radiusA = shapeA.Radius; + float radiusB = shapeB.Radius; + Body bodyA = fixtureA.Body; + Body bodyB = fixtureB.Body; + Manifold manifold = contact.Manifold; + + int pointCount = manifold.PointCount; + Debug.Assert(pointCount > 0); + + ContactVelocityConstraint vc = _velocityConstraints[i]; + vc.friction = contact.Friction; + vc.restitution = contact.Restitution; + vc.tangentSpeed = contact.TangentSpeed; + vc.indexA = bodyA.IslandIndex; + vc.indexB = bodyB.IslandIndex; + vc.invMassA = bodyA._invMass; + vc.invMassB = bodyB._invMass; + vc.invIA = bodyA._invI; + vc.invIB = bodyB._invI; + vc.contactIndex = i; + vc.pointCount = pointCount; + vc.K.SetZero(); + vc.normalMass.SetZero(); + + ContactPositionConstraint pc = _positionConstraints[i]; + pc.indexA = bodyA.IslandIndex; + pc.indexB = bodyB.IslandIndex; + pc.invMassA = bodyA._invMass; + pc.invMassB = bodyB._invMass; + pc.localCenterA = bodyA._sweep.LocalCenter; + pc.localCenterB = bodyB._sweep.LocalCenter; + pc.invIA = bodyA._invI; + pc.invIB = bodyB._invI; + pc.localNormal = manifold.LocalNormal; + pc.localPoint = manifold.LocalPoint; + pc.pointCount = pointCount; + pc.radiusA = radiusA; + pc.radiusB = radiusB; + pc.type = manifold.Type; + + for (int j = 0; j < pointCount; ++j) + { + ManifoldPoint cp = manifold.Points[j]; + VelocityConstraintPoint vcp = vc.points[j]; + + if (Settings.EnableWarmstarting) + { + vcp.normalImpulse = _step.dtRatio * cp.NormalImpulse; + vcp.tangentImpulse = _step.dtRatio * cp.TangentImpulse; + } + else + { + vcp.normalImpulse = 0.0f; + vcp.tangentImpulse = 0.0f; + } + + vcp.rA = Vector2.Zero; + vcp.rB = Vector2.Zero; + vcp.normalMass = 0.0f; + vcp.tangentMass = 0.0f; + vcp.velocityBias = 0.0f; + + pc.localPoints[j] = cp.LocalPoint; + } + } + } + + public void InitializeVelocityConstraints() + { + for (int i = 0; i < _count; ++i) + { + ContactVelocityConstraint vc = _velocityConstraints[i]; + ContactPositionConstraint pc = _positionConstraints[i]; + + float radiusA = pc.radiusA; + float radiusB = pc.radiusB; + Manifold manifold = _contacts[vc.contactIndex].Manifold; + + int indexA = vc.indexA; + int indexB = vc.indexB; + + float mA = vc.invMassA; + float mB = vc.invMassB; + float iA = vc.invIA; + float iB = vc.invIB; + Vector2 localCenterA = pc.localCenterA; + Vector2 localCenterB = pc.localCenterB; + + Vector2 cA = _positions[indexA].c; + float aA = _positions[indexA].a; + Vector2 vA = _velocities[indexA].v; + float wA = _velocities[indexA].w; + + Vector2 cB = _positions[indexB].c; + float aB = _positions[indexB].a; + Vector2 vB = _velocities[indexB].v; + float wB = _velocities[indexB].w; + + Debug.Assert(manifold.PointCount > 0); + + Transform xfA = new Transform(); + Transform xfB = new Transform(); + xfA.q.Set(aA); + xfB.q.Set(aB); + xfA.p = cA - MathUtils.Mul(xfA.q, localCenterA); + xfB.p = cB - MathUtils.Mul(xfB.q, localCenterB); + + Vector2 normal; + FixedArray2 points; + WorldManifold.Initialize(ref manifold, ref xfA, radiusA, ref xfB, radiusB, out normal, out points); + + vc.normal = normal; + + int pointCount = vc.pointCount; + for (int j = 0; j < pointCount; ++j) + { + VelocityConstraintPoint vcp = vc.points[j]; + + vcp.rA = points[j] - cA; + vcp.rB = points[j] - cB; + + float rnA = MathUtils.Cross(vcp.rA, vc.normal); + float rnB = MathUtils.Cross(vcp.rB, vc.normal); + + float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB; + + vcp.normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; + + Vector2 tangent = MathUtils.Cross(vc.normal, 1.0f); + + float rtA = MathUtils.Cross(vcp.rA, tangent); + float rtB = MathUtils.Cross(vcp.rB, tangent); + + float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB; + + vcp.tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; + + // Setup a velocity bias for restitution. + vcp.velocityBias = 0.0f; + float vRel = Vector2.Dot(vc.normal, vB + MathUtils.Cross(wB, vcp.rB) - vA - MathUtils.Cross(wA, vcp.rA)); + if (vRel < -Settings.VelocityThreshold) + { + vcp.velocityBias = -vc.restitution * vRel; + } + } + + // If we have two points, then prepare the block solver. + if (vc.pointCount == 2) + { + VelocityConstraintPoint vcp1 = vc.points[0]; + VelocityConstraintPoint vcp2 = vc.points[1]; + + float rn1A = MathUtils.Cross(vcp1.rA, vc.normal); + float rn1B = MathUtils.Cross(vcp1.rB, vc.normal); + float rn2A = MathUtils.Cross(vcp2.rA, vc.normal); + float rn2B = MathUtils.Cross(vcp2.rB, vc.normal); + + float k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B; + float k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B; + float k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B; + + // Ensure a reasonable condition number. + const float k_maxConditionNumber = 1000.0f; + if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) + { + // K is safe to invert. + vc.K.ex = new Vector2(k11, k12); + vc.K.ey = new Vector2(k12, k22); + vc.normalMass = vc.K.Inverse; + } + else + { + // The constraints are redundant, just use one. + // TODO_ERIN use deepest? + vc.pointCount = 1; + } + } + } + } + + public void WarmStart() + { + // Warm start. + for (int i = 0; i < _count; ++i) + { + ContactVelocityConstraint vc = _velocityConstraints[i]; + + int indexA = vc.indexA; + int indexB = vc.indexB; + float mA = vc.invMassA; + float iA = vc.invIA; + float mB = vc.invMassB; + float iB = vc.invIB; + int pointCount = vc.pointCount; + + Vector2 vA = _velocities[indexA].v; + float wA = _velocities[indexA].w; + Vector2 vB = _velocities[indexB].v; + float wB = _velocities[indexB].w; + + Vector2 normal = vc.normal; + Vector2 tangent = MathUtils.Cross(normal, 1.0f); + + for (int j = 0; j < pointCount; ++j) + { + VelocityConstraintPoint vcp = vc.points[j]; + Vector2 P = vcp.normalImpulse * normal + vcp.tangentImpulse * tangent; + wA -= iA * MathUtils.Cross(vcp.rA, P); + vA -= mA * P; + wB += iB * MathUtils.Cross(vcp.rB, P); + vB += mB * P; + } + + _velocities[indexA].v = vA; + _velocities[indexA].w = wA; + _velocities[indexB].v = vB; + _velocities[indexB].w = wB; + } + } + + public void SolveVelocityConstraints() + { + for (int i = 0; i < _count; ++i) + { + ContactVelocityConstraint vc = _velocityConstraints[i]; + + int indexA = vc.indexA; + int indexB = vc.indexB; + float mA = vc.invMassA; + float iA = vc.invIA; + float mB = vc.invMassB; + float iB = vc.invIB; + int pointCount = vc.pointCount; + + Vector2 vA = _velocities[indexA].v; + float wA = _velocities[indexA].w; + Vector2 vB = _velocities[indexB].v; + float wB = _velocities[indexB].w; + + Vector2 normal = vc.normal; + Vector2 tangent = MathUtils.Cross(normal, 1.0f); + float friction = vc.friction; + + Debug.Assert(pointCount == 1 || pointCount == 2); + + // Solve tangent constraints first because non-penetration is more important + // than friction. + for (int j = 0; j < pointCount; ++j) + { + VelocityConstraintPoint vcp = vc.points[j]; + + // Relative velocity at contact + Vector2 dv = vB + MathUtils.Cross(wB, vcp.rB) - vA - MathUtils.Cross(wA, vcp.rA); + + // Compute tangent force + float vt = Vector2.Dot(dv, tangent) - vc.tangentSpeed; + float lambda = vcp.tangentMass * (-vt); + + // b2Clamp the accumulated force + float maxFriction = friction * vcp.normalImpulse; + float newImpulse = MathUtils.Clamp(vcp.tangentImpulse + lambda, -maxFriction, maxFriction); + lambda = newImpulse - vcp.tangentImpulse; + vcp.tangentImpulse = newImpulse; + + // Apply contact impulse + Vector2 P = lambda * tangent; + + vA -= mA * P; + wA -= iA * MathUtils.Cross(vcp.rA, P); + + vB += mB * P; + wB += iB * MathUtils.Cross(vcp.rB, P); + } + + // Solve normal constraints + if (vc.pointCount == 1) + { + VelocityConstraintPoint vcp = vc.points[0]; + + // Relative velocity at contact + Vector2 dv = vB + MathUtils.Cross(wB, vcp.rB) - vA - MathUtils.Cross(wA, vcp.rA); + + // Compute normal impulse + float vn = Vector2.Dot(dv, normal); + float lambda = -vcp.normalMass * (vn - vcp.velocityBias); + + // b2Clamp the accumulated impulse + float newImpulse = Math.Max(vcp.normalImpulse + lambda, 0.0f); + lambda = newImpulse - vcp.normalImpulse; + vcp.normalImpulse = newImpulse; + + // Apply contact impulse + Vector2 P = lambda * normal; + vA -= mA * P; + wA -= iA * MathUtils.Cross(vcp.rA, P); + + vB += mB * P; + wB += iB * MathUtils.Cross(vcp.rB, P); + } + else + { + // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite). + // Build the mini LCP for this contact patch + // + // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2 + // + // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n ) + // b = vn0 - velocityBias + // + // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i + // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases + // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid + // solution that satisfies the problem is chosen. + // + // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires + // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i). + // + // Substitute: + // + // x = a + d + // + // a := old total impulse + // x := new total impulse + // d := incremental impulse + // + // For the current iteration we extend the formula for the incremental impulse + // to compute the new total impulse: + // + // vn = A * d + b + // = A * (x - a) + b + // = A * x + b - A * a + // = A * x + b' + // b' = b - A * a; + + VelocityConstraintPoint cp1 = vc.points[0]; + VelocityConstraintPoint cp2 = vc.points[1]; + + Vector2 a = new Vector2(cp1.normalImpulse, cp2.normalImpulse); + Debug.Assert(a.X >= 0.0f && a.Y >= 0.0f); + + // Relative velocity at contact + Vector2 dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA); + Vector2 dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA); + + // Compute normal velocity + float vn1 = Vector2.Dot(dv1, normal); + float vn2 = Vector2.Dot(dv2, normal); + + Vector2 b = new Vector2(); + b.X = vn1 - cp1.velocityBias; + b.Y = vn2 - cp2.velocityBias; + + // Compute b' + b -= MathUtils.Mul(ref vc.K, a); + + const float k_errorTol = 1e-3f; + //B2_NOT_USED(k_errorTol); + + for (; ; ) + { + // + // Case 1: vn = 0 + // + // 0 = A * x + b' + // + // Solve for x: + // + // x = - inv(A) * b' + // + Vector2 x = -MathUtils.Mul(ref vc.normalMass, b); + + if (x.X >= 0.0f && x.Y >= 0.0f) + { + // Get the incremental impulse + Vector2 d = x - a; + + // Apply incremental impulse + Vector2 P1 = d.X * normal; + Vector2 P2 = d.Y * normal; + vA -= mA * (P1 + P2); + wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2)); + + vB += mB * (P1 + P2); + wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2)); + + // Accumulate + cp1.normalImpulse = x.X; + cp2.normalImpulse = x.Y; + +#if B2_DEBUG_SOLVER + // Postconditions + dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA); + dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA); + + // Compute normal velocity + vn1 = Vector2.Dot(dv1, normal); + vn2 = Vector2.Dot(dv2, normal); + + b2Assert(b2Abs(vn1 - cp1.velocityBias) < k_errorTol); + b2Assert(b2Abs(vn2 - cp2.velocityBias) < k_errorTol); +#endif + break; + } + + // + // Case 2: vn1 = 0 and x2 = 0 + // + // 0 = a11 * x1 + a12 * 0 + b1' + // vn2 = a21 * x1 + a22 * 0 + b2' + // + x.X = -cp1.normalMass * b.X; + x.Y = 0.0f; + vn1 = 0.0f; + vn2 = vc.K.ex.Y * x.X + b.Y; + + if (x.X >= 0.0f && vn2 >= 0.0f) + { + // Get the incremental impulse + Vector2 d = x - a; + + // Apply incremental impulse + Vector2 P1 = d.X * normal; + Vector2 P2 = d.Y * normal; + vA -= mA * (P1 + P2); + wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2)); + + vB += mB * (P1 + P2); + wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2)); + + // Accumulate + cp1.normalImpulse = x.X; + cp2.normalImpulse = x.Y; + +#if B2_DEBUG_SOLVER + // Postconditions + dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA); + + // Compute normal velocity + vn1 = Vector2.Dot(dv1, normal); + + b2Assert(b2Abs(vn1 - cp1.velocityBias) < k_errorTol); +#endif + break; + } + + + // + // Case 3: vn2 = 0 and x1 = 0 + // + // vn1 = a11 * 0 + a12 * x2 + b1' + // 0 = a21 * 0 + a22 * x2 + b2' + // + x.X = 0.0f; + x.Y = -cp2.normalMass * b.Y; + vn1 = vc.K.ey.X * x.Y + b.X; + vn2 = 0.0f; + + if (x.Y >= 0.0f && vn1 >= 0.0f) + { + // Resubstitute for the incremental impulse + Vector2 d = x - a; + + // Apply incremental impulse + Vector2 P1 = d.X * normal; + Vector2 P2 = d.Y * normal; + vA -= mA * (P1 + P2); + wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2)); + + vB += mB * (P1 + P2); + wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2)); + + // Accumulate + cp1.normalImpulse = x.X; + cp2.normalImpulse = x.Y; + +#if B2_DEBUG_SOLVER + // Postconditions + dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA); + + // Compute normal velocity + vn2 = Vector2.Dot(dv2, normal); + + b2Assert(b2Abs(vn2 - cp2.velocityBias) < k_errorTol); +#endif + break; + } + + // + // Case 4: x1 = 0 and x2 = 0 + // + // vn1 = b1 + // vn2 = b2; + x.X = 0.0f; + x.Y = 0.0f; + vn1 = b.X; + vn2 = b.Y; + + if (vn1 >= 0.0f && vn2 >= 0.0f) + { + // Resubstitute for the incremental impulse + Vector2 d = x - a; + + // Apply incremental impulse + Vector2 P1 = d.X * normal; + Vector2 P2 = d.Y * normal; + vA -= mA * (P1 + P2); + wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2)); + + vB += mB * (P1 + P2); + wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2)); + + // Accumulate + cp1.normalImpulse = x.X; + cp2.normalImpulse = x.Y; + + break; + } + + // No solution, give up. This is hit sometimes, but it doesn't seem to matter. + break; + } + } + + _velocities[indexA].v = vA; + _velocities[indexA].w = wA; + _velocities[indexB].v = vB; + _velocities[indexB].w = wB; + } + } + + public void StoreImpulses() + { + for (int i = 0; i < _count; ++i) + { + ContactVelocityConstraint vc = _velocityConstraints[i]; + Manifold manifold = _contacts[vc.contactIndex].Manifold; + + for (int j = 0; j < vc.pointCount; ++j) + { + ManifoldPoint point = manifold.Points[j]; + point.NormalImpulse = vc.points[j].normalImpulse; + point.TangentImpulse = vc.points[j].tangentImpulse; + manifold.Points[j] = point; + } + + _contacts[vc.contactIndex].Manifold = manifold; + } + } + + public bool SolvePositionConstraints() + { + float minSeparation = 0.0f; + + for (int i = 0; i < _count; ++i) + { + ContactPositionConstraint pc = _positionConstraints[i]; + + int indexA = pc.indexA; + int indexB = pc.indexB; + Vector2 localCenterA = pc.localCenterA; + float mA = pc.invMassA; + float iA = pc.invIA; + Vector2 localCenterB = pc.localCenterB; + float mB = pc.invMassB; + float iB = pc.invIB; + int pointCount = pc.pointCount; + + Vector2 cA = _positions[indexA].c; + float aA = _positions[indexA].a; + + Vector2 cB = _positions[indexB].c; + float aB = _positions[indexB].a; + + // Solve normal constraints + for (int j = 0; j < pointCount; ++j) + { + Transform xfA = new Transform(); + Transform xfB = new Transform(); + xfA.q.Set(aA); + xfB.q.Set(aB); + xfA.p = cA - MathUtils.Mul(xfA.q, localCenterA); + xfB.p = cB - MathUtils.Mul(xfB.q, localCenterB); + + Vector2 normal; + Vector2 point; + float separation; + + PositionSolverManifold.Initialize(pc, xfA, xfB, j, out normal, out point, out separation); + + Vector2 rA = point - cA; + Vector2 rB = point - cB; + + // Track max constraint error. + minSeparation = Math.Min(minSeparation, separation); + + // Prevent large corrections and allow slop. + float C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f); + + // Compute the effective mass. + float rnA = MathUtils.Cross(rA, normal); + float rnB = MathUtils.Cross(rB, normal); + float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; + + // Compute normal impulse + float impulse = K > 0.0f ? -C / K : 0.0f; + + Vector2 P = impulse * normal; + + cA -= mA * P; + aA -= iA * MathUtils.Cross(rA, P); + + cB += mB * P; + aB += iB * MathUtils.Cross(rB, P); + } + + _positions[indexA].c = cA; + _positions[indexA].a = aA; + + _positions[indexB].c = cB; + _positions[indexB].a = aB; + } + + // We can't expect minSpeparation >= -b2_linearSlop because we don't + // push the separation above -b2_linearSlop. + return minSeparation >= -3.0f * Settings.LinearSlop; + } + + // Sequential position solver for position constraints. + public bool SolveTOIPositionConstraints(int toiIndexA, int toiIndexB) + { + float minSeparation = 0.0f; + + for (int i = 0; i < _count; ++i) + { + ContactPositionConstraint pc = _positionConstraints[i]; + + int indexA = pc.indexA; + int indexB = pc.indexB; + Vector2 localCenterA = pc.localCenterA; + Vector2 localCenterB = pc.localCenterB; + int pointCount = pc.pointCount; + + float mA = 0.0f; + float iA = 0.0f; + if (indexA == toiIndexA || indexA == toiIndexB) + { + mA = pc.invMassA; + iA = pc.invIA; + } + + float mB = 0.0f; + float iB = 0.0f; + if (indexB == toiIndexA || indexB == toiIndexB) + { + mB = pc.invMassB; + iB = pc.invIB; + } + + Vector2 cA = _positions[indexA].c; + float aA = _positions[indexA].a; + + Vector2 cB = _positions[indexB].c; + float aB = _positions[indexB].a; + + // Solve normal constraints + for (int j = 0; j < pointCount; ++j) + { + Transform xfA = new Transform(); + Transform xfB = new Transform(); + xfA.q.Set(aA); + xfB.q.Set(aB); + xfA.p = cA - MathUtils.Mul(xfA.q, localCenterA); + xfB.p = cB - MathUtils.Mul(xfB.q, localCenterB); + + Vector2 normal; + Vector2 point; + float separation; + + PositionSolverManifold.Initialize(pc, xfA, xfB, j, out normal, out point, out separation); + + Vector2 rA = point - cA; + Vector2 rB = point - cB; + + // Track max constraint error. + minSeparation = Math.Min(minSeparation, separation); + + // Prevent large corrections and allow slop. + float C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f); + + // Compute the effective mass. + float rnA = MathUtils.Cross(rA, normal); + float rnB = MathUtils.Cross(rB, normal); + float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; + + // Compute normal impulse + float impulse = K > 0.0f ? -C / K : 0.0f; + + Vector2 P = impulse * normal; + + cA -= mA * P; + aA -= iA * MathUtils.Cross(rA, P); + + cB += mB * P; + aB += iB * MathUtils.Cross(rB, P); + } + + _positions[indexA].c = cA; + _positions[indexA].a = aA; + + _positions[indexB].c = cB; + _positions[indexB].a = aB; + } + + // We can't expect minSpeparation >= -b2_linearSlop because we don't + // push the separation above -b2_linearSlop. + return minSeparation >= -1.5f * Settings.LinearSlop; + } + + public static class WorldManifold + { + /// + /// Evaluate the manifold with supplied transforms. This assumes + /// modest motion from the original state. This does not change the + /// point count, impulses, etc. The radii must come from the Shapes + /// that generated the manifold. + /// + /// The manifold. + /// The transform for A. + /// The radius for A. + /// The transform for B. + /// The radius for B. + /// World vector pointing from A to B + /// Torld contact point (point of intersection). + public static void Initialize(ref Manifold manifold, ref Transform xfA, float radiusA, ref Transform xfB, float radiusB, out Vector2 normal, out FixedArray2 points) + { + normal = Vector2.Zero; + points = new FixedArray2(); + + if (manifold.PointCount == 0) + { + return; + } + + switch (manifold.Type) + { + case ManifoldType.Circles: + { + normal = new Vector2(1.0f, 0.0f); + Vector2 pointA = MathUtils.Mul(ref xfA, manifold.LocalPoint); + Vector2 pointB = MathUtils.Mul(ref xfB, manifold.Points[0].LocalPoint); + if (Vector2.DistanceSquared(pointA, pointB) > Settings.Epsilon * Settings.Epsilon) + { + normal = pointB - pointA; + normal.Normalize(); + } + + Vector2 cA = pointA + radiusA * normal; + Vector2 cB = pointB - radiusB * normal; + points[0] = 0.5f * (cA + cB); + } + break; + + case ManifoldType.FaceA: + { + normal = MathUtils.Mul(xfA.q, manifold.LocalNormal); + Vector2 planePoint = MathUtils.Mul(ref xfA, manifold.LocalPoint); + + for (int i = 0; i < manifold.PointCount; ++i) + { + Vector2 clipPoint = MathUtils.Mul(ref xfB, manifold.Points[i].LocalPoint); + Vector2 cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, normal)) * normal; + Vector2 cB = clipPoint - radiusB * normal; + points[i] = 0.5f * (cA + cB); + } + } + break; + + case ManifoldType.FaceB: + { + normal = MathUtils.Mul(xfB.q, manifold.LocalNormal); + Vector2 planePoint = MathUtils.Mul(ref xfB, manifold.LocalPoint); + + for (int i = 0; i < manifold.PointCount; ++i) + { + Vector2 clipPoint = MathUtils.Mul(ref xfA, manifold.Points[i].LocalPoint); + Vector2 cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, normal)) * normal; + Vector2 cA = clipPoint - radiusA * normal; + points[i] = 0.5f * (cA + cB); + } + + // Ensure normal points from A to B. + normal = -normal; + } + break; + } + } + } + + private static class PositionSolverManifold + { + public static void Initialize(ContactPositionConstraint pc, Transform xfA, Transform xfB, int index, out Vector2 normal, out Vector2 point, out float separation) + { + Debug.Assert(pc.pointCount > 0); + + + switch (pc.type) + { + case ManifoldType.Circles: + { + Vector2 pointA = MathUtils.Mul(ref xfA, pc.localPoint); + Vector2 pointB = MathUtils.Mul(ref xfB, pc.localPoints[0]); + normal = pointB - pointA; + normal.Normalize(); + point = 0.5f * (pointA + pointB); + separation = Vector2.Dot(pointB - pointA, normal) - pc.radiusA - pc.radiusB; + } + break; + + case ManifoldType.FaceA: + { + normal = MathUtils.Mul(xfA.q, pc.localNormal); + Vector2 planePoint = MathUtils.Mul(ref xfA, pc.localPoint); + + Vector2 clipPoint = MathUtils.Mul(ref xfB, pc.localPoints[index]); + separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB; + point = clipPoint; + } + break; + + case ManifoldType.FaceB: + { + normal = MathUtils.Mul(xfB.q, pc.localNormal); + Vector2 planePoint = MathUtils.Mul(ref xfB, pc.localPoint); + + Vector2 clipPoint = MathUtils.Mul(ref xfA, pc.localPoints[index]); + separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB; + point = clipPoint; + + // Ensure normal points from A to B + normal = -normal; + } + break; + default: + normal = Vector2.Zero; + point = Vector2.Zero; + separation = 0; + break; + + } + } + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Dynamics/Fixture.cs b/Farseer Physics Engine 3.5/Dynamics/Fixture.cs new file mode 100644 index 000000000..5983c38ad --- /dev/null +++ b/Farseer Physics Engine 3.5/Dynamics/Fixture.cs @@ -0,0 +1,619 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ +//#define USE_IGNORE_CCD_CATEGORIES + +using System; +using System.Collections.Generic; +using System.Diagnostics; +using FarseerPhysics.Collision; +using FarseerPhysics.Collision.Shapes; +using FarseerPhysics.Common; +using FarseerPhysics.Dynamics.Contacts; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics +{ + [Flags] + public enum Category + { + None = 0, + All = int.MaxValue, + Cat1 = 1, + Cat2 = 2, + Cat3 = 4, + Cat4 = 8, + Cat5 = 16, + Cat6 = 32, + Cat7 = 64, + Cat8 = 128, + Cat9 = 256, + Cat10 = 512, + Cat11 = 1024, + Cat12 = 2048, + Cat13 = 4096, + Cat14 = 8192, + Cat15 = 16384, + Cat16 = 32768, + Cat17 = 65536, + Cat18 = 131072, + Cat19 = 262144, + Cat20 = 524288, + Cat21 = 1048576, + Cat22 = 2097152, + Cat23 = 4194304, + Cat24 = 8388608, + Cat25 = 16777216, + Cat26 = 33554432, + Cat27 = 67108864, + Cat28 = 134217728, + Cat29 = 268435456, + Cat30 = 536870912, + Cat31 = 1073741824 + } + + /// + /// This proxy is used internally to connect fixtures to the broad-phase. + /// + public struct FixtureProxy + { + public AABB AABB; + public int ChildIndex; + public Fixture Fixture; + public int ProxyId; + } + + /// + /// A fixture is used to attach a Shape to a body for collision detection. A fixture + /// inherits its transform from its parent. Fixtures hold additional non-geometric data + /// such as friction, collision filters, etc. + /// Fixtures are created via Body.CreateFixture. + /// Warning: You cannot reuse fixtures. + /// + public class Fixture : IDisposable + { + [ThreadStatic] + private static int _fixtureIdCounter; + private bool _isSensor; + private float _friction; + private float _restitution; + + internal Category _collidesWith; + internal Category _collisionCategories; + internal short _collisionGroup; + internal HashSet _collisionIgnores; + + public FixtureProxy[] Proxies; + public int ProxyCount; + public Category IgnoreCCDWith; + + /// + /// Fires after two shapes has collided and are solved. This gives you a chance to get the impact force. + /// + public AfterCollisionEventHandler AfterCollision; + + /// + /// Fires when two fixtures are close to each other. + /// Due to how the broadphase works, this can be quite inaccurate as shapes are approximated using AABBs. + /// + public BeforeCollisionEventHandler BeforeCollision; + + /// + /// Fires when two shapes collide and a contact is created between them. + /// Note that the first fixture argument is always the fixture that the delegate is subscribed to. + /// + public OnCollisionEventHandler OnCollision; + + /// + /// Fires when two shapes separate and a contact is removed between them. + /// Note: This can in some cases be called multiple times, as a fixture can have multiple contacts. + /// Note The first fixture argument is always the fixture that the delegate is subscribed to. + /// + public OnSeparationEventHandler OnSeparation; + + internal Fixture() + { + FixtureId = _fixtureIdCounter++; + + _collisionCategories = Settings.DefaultFixtureCollisionCategories; + _collidesWith = Settings.DefaultFixtureCollidesWith; + _collisionGroup = 0; + _collisionIgnores = new HashSet(); + + IgnoreCCDWith = Settings.DefaultFixtureIgnoreCCDWith; + + //Fixture defaults + Friction = 0.2f; + Restitution = 0; + } + + internal Fixture(Body body, Shape shape, object userData = null) + : this() + { +#if DEBUG + if (shape.ShapeType == ShapeType.Polygon) + ((PolygonShape)shape).Vertices.AttachedToBody = true; +#endif + + Body = body; + UserData = userData; + Shape = shape.Clone(); + + RegisterFixture(); + } + + /// + /// Defaults to 0 + /// + /// If Settings.UseFPECollisionCategories is set to false: + /// Collision groups allow a certain group of objects to never collide (negative) + /// or always collide (positive). Zero means no collision group. Non-zero group + /// filtering always wins against the mask bits. + /// + /// If Settings.UseFPECollisionCategories is set to true: + /// If 2 fixtures are in the same collision group, they will not collide. + /// + public short CollisionGroup + { + set + { + if (_collisionGroup == value) + return; + + _collisionGroup = value; + Refilter(); + } + get { return _collisionGroup; } + } + + /// + /// Defaults to Category.All + /// + /// The collision mask bits. This states the categories that this + /// fixture would accept for collision. + /// Use Settings.UseFPECollisionCategories to change the behavior. + /// + public Category CollidesWith + { + get { return _collidesWith; } + + set + { + if (_collidesWith == value) + return; + + _collidesWith = value; + Refilter(); + } + } + + /// + /// The collision categories this fixture is a part of. + /// + /// If Settings.UseFPECollisionCategories is set to false: + /// Defaults to Category.Cat1 + /// + /// If Settings.UseFPECollisionCategories is set to true: + /// Defaults to Category.All + /// + public Category CollisionCategories + { + get { return _collisionCategories; } + + set + { + if (_collisionCategories == value) + return; + + _collisionCategories = value; + Refilter(); + } + } + + /// + /// Get the child Shape. You can modify the child Shape, however you should not change the + /// number of vertices because this will crash some collision caching mechanisms. + /// + /// The shape. + public Shape Shape { get; internal set; } + + /// + /// Gets or sets a value indicating whether this fixture is a sensor. + /// + /// true if this instance is a sensor; otherwise, false. + public bool IsSensor + { + get { return _isSensor; } + set + { + if (Body != null) + Body.Awake = true; + + _isSensor = value; + } + } + + /// + /// Get the parent body of this fixture. This is null if the fixture is not attached. + /// + /// The body. + public Body Body { get; internal set; } + + /// + /// Set the user data. Use this to store your application specific data. + /// + /// The user data. + public object UserData { get; set; } + + /// + /// Set the coefficient of friction. This will _not_ change the friction of + /// existing contacts. + /// + /// The friction. + public float Friction + { + get { return _friction; } + set + { + Debug.Assert(!float.IsNaN(value)); + + _friction = value; + } + } + + /// + /// Set the coefficient of restitution. This will not change the restitution of + /// existing contacts. + /// + /// The restitution. + public float Restitution + { + get { return _restitution; } + set + { + Debug.Assert(!float.IsNaN(value)); + + _restitution = value; + } + } + + /// + /// Gets a unique ID for this fixture. + /// + /// The fixture id. + public int FixtureId { get; internal set; } + + #region IDisposable Members + + public bool IsDisposed { get; set; } + + public void Dispose() + { + if (!IsDisposed) + { + Body.DestroyFixture(this); + IsDisposed = true; + GC.SuppressFinalize(this); + } + } + + #endregion + + /// + /// Restores collisions between this fixture and the provided fixture. + /// + /// The fixture. + public void RestoreCollisionWith(Fixture fixture) + { + if (_collisionIgnores.Contains(fixture.FixtureId)) + { + _collisionIgnores.Remove(fixture.FixtureId); + Refilter(); + } + } + + /// + /// Ignores collisions between this fixture and the provided fixture. + /// + /// The fixture. + public void IgnoreCollisionWith(Fixture fixture) + { + if (!_collisionIgnores.Contains(fixture.FixtureId)) + { + _collisionIgnores.Add(fixture.FixtureId); + Refilter(); + } + } + + /// + /// Determines whether collisions are ignored between this fixture and the provided fixture. + /// + /// The fixture. + /// + /// true if the fixture is ignored; otherwise, false. + /// + public bool IsFixtureIgnored(Fixture fixture) + { + return _collisionIgnores.Contains(fixture.FixtureId); + } + + /// + /// Contacts are persistant and will keep being persistant unless they are + /// flagged for filtering. + /// This methods flags all contacts associated with the body for filtering. + /// + private void Refilter() + { + // Flag associated contacts for filtering. + ContactEdge edge = Body.ContactList; + while (edge != null) + { + Contact contact = edge.Contact; + Fixture fixtureA = contact.FixtureA; + Fixture fixtureB = contact.FixtureB; + if (fixtureA == this || fixtureB == this) + { + contact.FilterFlag = true; + } + + edge = edge.Next; + } + + World world = Body._world; + + if (world == null) + { + return; + } + + // Touch each proxy so that new pairs may be created + IBroadPhase broadPhase = world.ContactManager.BroadPhase; + for (int i = 0; i < ProxyCount; ++i) + { + broadPhase.TouchProxy(Proxies[i].ProxyId); + } + } + + private void RegisterFixture() + { + // Reserve proxy space + Proxies = new FixtureProxy[Shape.ChildCount]; + ProxyCount = 0; + + if (Body.Enabled) + { + IBroadPhase broadPhase = Body._world.ContactManager.BroadPhase; + CreateProxies(broadPhase, ref Body._xf); + } + + Body.FixtureList.Add(this); + + // Adjust mass properties if needed. + if (Shape._density > 0.0f) + { + Body.ResetMassData(); + } + + // Let the world know we have a new fixture. This will cause new contacts + // to be created at the beginning of the next time step. + Body._world._worldHasNewFixture = true; + + if (Body._world.FixtureAdded != null) + { + Body._world.FixtureAdded(this); + } + } + + /// + /// Test a point for containment in this fixture. + /// + /// A point in world coordinates. + /// + public bool TestPoint(ref Vector2 point) + { + return Shape.TestPoint(ref Body._xf, ref point); + } + + /// + /// Cast a ray against this Shape. + /// + /// The ray-cast results. + /// The ray-cast input parameters. + /// Index of the child. + /// + public bool RayCast(out RayCastOutput output, ref RayCastInput input, int childIndex) + { + return Shape.RayCast(out output, ref input, ref Body._xf, childIndex); + } + + /// + /// Get the fixture's AABB. This AABB may be enlarge and/or stale. + /// If you need a more accurate AABB, compute it using the Shape and + /// the body transform. + /// + /// The aabb. + /// Index of the child. + public void GetAABB(out AABB aabb, int childIndex) + { + Debug.Assert(0 <= childIndex && childIndex < ProxyCount); + aabb = Proxies[childIndex].AABB; + } + + internal void Destroy() + { +#if DEBUG + if (Shape.ShapeType == ShapeType.Polygon) + ((PolygonShape)Shape).Vertices.AttachedToBody = false; +#endif + + // The proxies must be destroyed before calling this. + Debug.Assert(ProxyCount == 0); + + // Free the proxy array. + Proxies = null; + Shape = null; + + //FPE: We set the userdata to null here to help prevent bugs related to stale references in GC + UserData = null; + + BeforeCollision = null; + OnCollision = null; + OnSeparation = null; + AfterCollision = null; + + if (Body._world.FixtureRemoved != null) + { + Body._world.FixtureRemoved(this); + } + + Body._world.FixtureAdded = null; + Body._world.FixtureRemoved = null; + OnSeparation = null; + OnCollision = null; + } + + // These support body activation/deactivation. + internal void CreateProxies(IBroadPhase broadPhase, ref Transform xf) + { + Debug.Assert(ProxyCount == 0); + + // Create proxies in the broad-phase. + ProxyCount = Shape.ChildCount; + + for (int i = 0; i < ProxyCount; ++i) + { + FixtureProxy proxy = new FixtureProxy(); + Shape.ComputeAABB(out proxy.AABB, ref xf, i); + proxy.Fixture = this; + proxy.ChildIndex = i; + + //FPE note: This line needs to be after the previous two because FixtureProxy is a struct + proxy.ProxyId = broadPhase.AddProxy(ref proxy); + + Proxies[i] = proxy; + } + } + + internal void DestroyProxies(IBroadPhase broadPhase) + { + // Destroy proxies in the broad-phase. + for (int i = 0; i < ProxyCount; ++i) + { + broadPhase.RemoveProxy(Proxies[i].ProxyId); + Proxies[i].ProxyId = -1; + } + + ProxyCount = 0; + } + + internal void Synchronize(IBroadPhase broadPhase, ref Transform transform1, ref Transform transform2) + { + if (ProxyCount == 0) + { + return; + } + + for (int i = 0; i < ProxyCount; ++i) + { + FixtureProxy proxy = Proxies[i]; + + // Compute an AABB that covers the swept Shape (may miss some rotation effect). + AABB aabb1, aabb2; + Shape.ComputeAABB(out aabb1, ref transform1, proxy.ChildIndex); + Shape.ComputeAABB(out aabb2, ref transform2, proxy.ChildIndex); + + proxy.AABB.Combine(ref aabb1, ref aabb2); + + Vector2 displacement = transform2.p - transform1.p; + + broadPhase.MoveProxy(proxy.ProxyId, ref proxy.AABB, displacement); + } + } + + /// + /// Only compares the values of this fixture, and not the attached shape or body. + /// This is used for deduplication in serialization only. + /// + internal bool CompareTo(Fixture fixture) + { + return (_collidesWith == fixture._collidesWith && + _collisionCategories == fixture._collisionCategories && + _collisionGroup == fixture._collisionGroup && + Friction == fixture.Friction && + IsSensor == fixture.IsSensor && + Restitution == fixture.Restitution && + UserData == fixture.UserData && + IgnoreCCDWith == fixture.IgnoreCCDWith && + SequenceEqual(_collisionIgnores, fixture._collisionIgnores)); + } + + private bool SequenceEqual(HashSet first, HashSet second) + { + if (first.Count != second.Count) + return false; + + using (IEnumerator enumerator1 = first.GetEnumerator()) + { + using (IEnumerator enumerator2 = second.GetEnumerator()) + { + while (enumerator1.MoveNext()) + { + if (!enumerator2.MoveNext() || !Equals(enumerator1.Current, enumerator2.Current)) + return false; + } + + if (enumerator2.MoveNext()) + return false; + } + } + + return true; + } + + /// + /// Clones the fixture and attached shape onto the specified body. + /// + /// The body you wish to clone the fixture onto. + /// The cloned fixture. + public Fixture CloneOnto(Body body) + { + Fixture fixture = new Fixture(); + fixture.Body = body; + fixture.Shape = Shape.Clone(); + fixture.UserData = UserData; + fixture.Restitution = Restitution; + fixture.Friction = Friction; + fixture.IsSensor = IsSensor; + fixture._collisionGroup = _collisionGroup; + fixture._collisionCategories = _collisionCategories; + fixture._collidesWith = _collidesWith; + fixture.IgnoreCCDWith = IgnoreCCDWith; + + foreach (int ignore in _collisionIgnores) + { + fixture._collisionIgnores.Add(ignore); + } + + fixture.RegisterFixture(); + return fixture; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Dynamics/Island.cs b/Farseer Physics Engine 3.5/Dynamics/Island.cs new file mode 100644 index 000000000..6c4429a73 --- /dev/null +++ b/Farseer Physics Engine 3.5/Dynamics/Island.cs @@ -0,0 +1,449 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using FarseerPhysics.Common; +using FarseerPhysics.Dynamics.Contacts; +using FarseerPhysics.Dynamics.Joints; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics +{ + /// + /// This is an internal class. + /// + public class Island + { + private ContactManager _contactManager; + private ContactSolver _contactSolver = new ContactSolver(); + private Contact[] _contacts; + private Joint[] _joints; + + private const float LinTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance; + private const float AngTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance; + private Stopwatch _watch = new Stopwatch(); + + public Body[] Bodies; + public int BodyCount; + public int ContactCount; + public int JointCount; + + public Velocity[] _velocities; + public Position[] _positions; + + public int BodyCapacity; + public int ContactCapacity; + public int JointCapacity; + public float JointUpdateTime; + + public void Reset(int bodyCapacity, int contactCapacity, int jointCapacity, ContactManager contactManager) + { + BodyCapacity = bodyCapacity; + ContactCapacity = contactCapacity; + JointCapacity = jointCapacity; + BodyCount = 0; + ContactCount = 0; + JointCount = 0; + + _contactManager = contactManager; + + if (Bodies == null || Bodies.Length < bodyCapacity) + { + Bodies = new Body[bodyCapacity]; + _velocities = new Velocity[bodyCapacity]; + _positions = new Position[bodyCapacity]; + } + + if (_contacts == null || _contacts.Length < contactCapacity) + { + _contacts = new Contact[contactCapacity * 2]; + } + + if (_joints == null || _joints.Length < jointCapacity) + { + _joints = new Joint[jointCapacity * 2]; + } + } + + public void Clear() + { + BodyCount = 0; + ContactCount = 0; + JointCount = 0; + } + + public void Solve(ref TimeStep step, ref Vector2 gravity) + { + float h = step.dt; + + // Integrate velocities and apply damping. Initialize the body state. + for (int i = 0; i < BodyCount; ++i) + { + Body b = Bodies[i]; + + Vector2 c = b._sweep.C; + float a = b._sweep.A; + Vector2 v = b._linearVelocity; + float w = b._angularVelocity; + + // Store positions for continuous collision. + b._sweep.C0 = b._sweep.C; + b._sweep.A0 = b._sweep.A; + + if (b.BodyType == BodyType.Dynamic) + { + // Integrate velocities. + + // FPE: Only apply gravity if the body wants it. + if (b.IgnoreGravity) + v += h * (b._invMass * b._force); + else + v += h * (b.GravityScale * gravity + b._invMass * b._force); + + w += h * b._invI * b._torque; + + // Apply damping. + // ODE: dv/dt + c * v = 0 + // Solution: v(t) = v0 * exp(-c * t) + // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) + // v2 = exp(-c * dt) * v1 + // Taylor expansion: + // v2 = (1.0f - c * dt) * v1 + v *= MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f); + w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f); + } + + _positions[i].c = c; + _positions[i].a = a; + _velocities[i].v = v; + _velocities[i].w = w; + } + + // Solver data + SolverData solverData = new SolverData(); + solverData.step = step; + solverData.positions = _positions; + solverData.velocities = _velocities; + + _contactSolver.Reset(step, ContactCount, _contacts, _positions, _velocities); + _contactSolver.InitializeVelocityConstraints(); + + if (Settings.EnableWarmstarting) + { + _contactSolver.WarmStart(); + } + + if (Settings.EnableDiagnostics) + _watch.Start(); + + for (int i = 0; i < JointCount; ++i) + { + if (_joints[i].Enabled) + _joints[i].InitVelocityConstraints(ref solverData); + } + + if (Settings.EnableDiagnostics) + _watch.Stop(); + + // Solve velocity constraints. + for (int i = 0; i < Settings.VelocityIterations; ++i) + { + for (int j = 0; j < JointCount; ++j) + { + Joint joint = _joints[j]; + + if (!joint.Enabled) + continue; + + if (Settings.EnableDiagnostics) + _watch.Start(); + + joint.SolveVelocityConstraints(ref solverData); + joint.Validate(step.inv_dt); + + if (Settings.EnableDiagnostics) + _watch.Stop(); + } + + _contactSolver.SolveVelocityConstraints(); + } + + // Store impulses for warm starting. + _contactSolver.StoreImpulses(); + + // Integrate positions + for (int i = 0; i < BodyCount; ++i) + { + Vector2 c = _positions[i].c; + float a = _positions[i].a; + Vector2 v = _velocities[i].v; + float w = _velocities[i].w; + + // Check for large velocities + Vector2 translation = h * v; + if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared) + { + float ratio = Settings.MaxTranslation / translation.Length(); + v *= ratio; + } + + float rotation = h * w; + if (rotation * rotation > Settings.MaxRotationSquared) + { + float ratio = Settings.MaxRotation / Math.Abs(rotation); + w *= ratio; + } + + // Integrate + c += h * v; + a += h * w; + + _positions[i].c = c; + _positions[i].a = a; + _velocities[i].v = v; + _velocities[i].w = w; + } + + + // Solve position constraints + bool positionSolved = false; + for (int i = 0; i < Settings.PositionIterations; ++i) + { + bool contactsOkay = _contactSolver.SolvePositionConstraints(); + + bool jointsOkay = true; + for (int j = 0; j < JointCount; ++j) + { + Joint joint = _joints[j]; + + if (!joint.Enabled) + continue; + + if (Settings.EnableDiagnostics) + _watch.Start(); + + bool jointOkay = joint.SolvePositionConstraints(ref solverData); + + if (Settings.EnableDiagnostics) + _watch.Stop(); + + jointsOkay = jointsOkay && jointOkay; + } + + if (contactsOkay && jointsOkay) + { + // Exit early if the position errors are small. + positionSolved = true; + break; + } + } + + if (Settings.EnableDiagnostics) + { + JointUpdateTime = _watch.ElapsedTicks; + _watch.Reset(); + } + + // Copy state buffers back to the bodies + for (int i = 0; i < BodyCount; ++i) + { + Body body = Bodies[i]; + body._sweep.C = _positions[i].c; + body._sweep.A = _positions[i].a; + body._linearVelocity = _velocities[i].v; + body._angularVelocity = _velocities[i].w; + body.SynchronizeTransform(); + } + + Report(_contactSolver._velocityConstraints); + + if (Settings.AllowSleep) + { + float minSleepTime = Settings.MaxFloat; + + for (int i = 0; i < BodyCount; ++i) + { + Body b = Bodies[i]; + + if (b.BodyType == BodyType.Static) + continue; + + if (!b.SleepingAllowed || b._angularVelocity * b._angularVelocity > AngTolSqr || Vector2.Dot(b._linearVelocity, b._linearVelocity) > LinTolSqr) + { + b._sleepTime = 0.0f; + minSleepTime = 0.0f; + } + else + { + b._sleepTime += h; + minSleepTime = Math.Min(minSleepTime, b._sleepTime); + } + } + + if (minSleepTime >= Settings.TimeToSleep && positionSolved) + { + for (int i = 0; i < BodyCount; ++i) + { + Body b = Bodies[i]; + b.Awake = false; + } + } + } + } + + internal void SolveTOI(ref TimeStep subStep, int toiIndexA, int toiIndexB, bool warmstarting) + { + Debug.Assert(toiIndexA < BodyCount); + Debug.Assert(toiIndexB < BodyCount); + + // Initialize the body state. + for (int i = 0; i < BodyCount; ++i) + { + Body b = Bodies[i]; + _positions[i].c = b._sweep.C; + _positions[i].a = b._sweep.A; + _velocities[i].v = b._linearVelocity; + _velocities[i].w = b._angularVelocity; + } + + _contactSolver.Reset(subStep, ContactCount, _contacts, _positions, _velocities, warmstarting); + + // Solve position constraints. + for (int i = 0; i < Settings.TOIPositionIterations; ++i) + { + bool contactsOkay = _contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB); + if (contactsOkay) + { + break; + } + } + + // Leap of faith to new safe state. + Bodies[toiIndexA]._sweep.C0 = _positions[toiIndexA].c; + Bodies[toiIndexA]._sweep.A0 = _positions[toiIndexA].a; + Bodies[toiIndexB]._sweep.C0 = _positions[toiIndexB].c; + Bodies[toiIndexB]._sweep.A0 = _positions[toiIndexB].a; + + // No warm starting is needed for TOI events because warm + // starting impulses were applied in the discrete solver. + _contactSolver.InitializeVelocityConstraints(); + + // Solve velocity constraints. + for (int i = 0; i < Settings.TOIVelocityIterations; ++i) + { + _contactSolver.SolveVelocityConstraints(); + } + + // Don't store the TOI contact forces for warm starting + // because they can be quite large. + + float h = subStep.dt; + + // Integrate positions. + for (int i = 0; i < BodyCount; ++i) + { + Vector2 c = _positions[i].c; + float a = _positions[i].a; + Vector2 v = _velocities[i].v; + float w = _velocities[i].w; + + // Check for large velocities + Vector2 translation = h * v; + if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared) + { + float ratio = Settings.MaxTranslation / translation.Length(); + v *= ratio; + } + + float rotation = h * w; + if (rotation * rotation > Settings.MaxRotationSquared) + { + float ratio = Settings.MaxRotation / Math.Abs(rotation); + w *= ratio; + } + + // Integrate + c += h * v; + a += h * w; + + _positions[i].c = c; + _positions[i].a = a; + _velocities[i].v = v; + _velocities[i].w = w; + + // Sync bodies + Body body = Bodies[i]; + body._sweep.C = c; + body._sweep.A = a; + body._linearVelocity = v; + body._angularVelocity = w; + body.SynchronizeTransform(); + } + + Report(_contactSolver._velocityConstraints); + } + + public void Add(Body body) + { + Debug.Assert(BodyCount < BodyCapacity); + body.IslandIndex = BodyCount; + Bodies[BodyCount++] = body; + } + + public void Add(Contact contact) + { + Debug.Assert(ContactCount < ContactCapacity); + _contacts[ContactCount++] = contact; + } + + public void Add(Joint joint) + { + Debug.Assert(JointCount < JointCapacity); + _joints[JointCount++] = joint; + } + + private void Report(ContactVelocityConstraint[] constraints) + { + if (_contactManager == null) + return; + + for (int i = 0; i < ContactCount; ++i) + { + Contact c = _contacts[i]; + + //FPE optimization: We don't store the impulses and send it to the delegate. We just send the whole contact. + //FPE feature: added after collision + if (c.FixtureA.AfterCollision != null) + c.FixtureA.AfterCollision(c.FixtureA, c.FixtureB, c, constraints[i]); + + if (c.FixtureB.AfterCollision != null) + c.FixtureB.AfterCollision(c.FixtureB, c.FixtureA, c, constraints[i]); + + if (_contactManager.PostSolve != null) + { + _contactManager.PostSolve(c, constraints[i]); + } + } + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Dynamics/Joints/AngleJoint.cs b/Farseer Physics Engine 3.5/Dynamics/Joints/AngleJoint.cs new file mode 100644 index 000000000..29d96e32e --- /dev/null +++ b/Farseer Physics Engine 3.5/Dynamics/Joints/AngleJoint.cs @@ -0,0 +1,128 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +*/ + +using System; +using System.Diagnostics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + /// + /// Maintains a fixed angle between two bodies + /// + public class AngleJoint : Joint + { + private float _bias; + private float _jointError; + private float _massFactor; + private float _targetAngle; + + internal AngleJoint() + { + JointType = JointType.Angle; + } + + /// + /// Constructor for AngleJoint + /// + /// The first body + /// The second body + public AngleJoint(Body bodyA, Body bodyB) + : base(bodyA, bodyB) + { + JointType = JointType.Angle; + BiasFactor = .2f; + MaxImpulse = float.MaxValue; + } + + public override Vector2 WorldAnchorA + { + get { return BodyA.Position; } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.Position; } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + /// + /// The desired angle between BodyA and BodyB + /// + public float TargetAngle + { + get { return _targetAngle; } + set + { + if (value != _targetAngle) + { + _targetAngle = value; + WakeBodies(); + } + } + } + + /// + /// Gets or sets the bias factor. + /// Defaults to 0.2 + /// + public float BiasFactor { get; set; } + + /// + /// Gets or sets the maximum impulse + /// Defaults to float.MaxValue + /// + public float MaxImpulse { get; set; } + + /// + /// Gets or sets the softness of the joint + /// Defaults to 0 + /// + public float Softness { get; set; } + + public override Vector2 GetReactionForce(float invDt) + { + //TODO + //return _inv_dt * _impulse; + return Vector2.Zero; + } + + public override float GetReactionTorque(float invDt) + { + return 0; + } + + internal override void InitVelocityConstraints(ref SolverData data) + { + int indexA = BodyA.IslandIndex; + int indexB = BodyB.IslandIndex; + + float aW = data.positions[indexA].a; + float bW = data.positions[indexB].a; + + _jointError = (bW - aW - TargetAngle); + _bias = -BiasFactor * data.step.inv_dt * _jointError; + _massFactor = (1 - Softness) / (BodyA._invI + BodyB._invI); + } + + internal override void SolveVelocityConstraints(ref SolverData data) + { + int indexA = BodyA.IslandIndex; + int indexB = BodyB.IslandIndex; + + float p = (_bias - data.velocities[indexB].w + data.velocities[indexA].w) * _massFactor; + + data.velocities[indexA].w -= BodyA._invI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse); + data.velocities[indexB].w += BodyB._invI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse); + } + + internal override bool SolvePositionConstraints(ref SolverData data) + { + //no position solving for this joint + return true; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Dynamics/Joints/DistanceJoint.cs b/Farseer Physics Engine 3.5/Dynamics/Joints/DistanceJoint.cs new file mode 100644 index 000000000..e4d1de0b6 --- /dev/null +++ b/Farseer Physics Engine 3.5/Dynamics/Joints/DistanceJoint.cs @@ -0,0 +1,331 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + // 1-D rained system + // m (v2 - v1) = lambda + // v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass. + // x2 = x1 + h * v2 + + // 1-D mass-damper-spring system + // m (v2 - v1) + h * d * v2 + h * k * + + // C = norm(p2 - p1) - L + // u = (p2 - p1) / norm(p2 - p1) + // Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1)) + // J = [-u -cross(r1, u) u cross(r2, u)] + // K = J * invM * JT + // = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2 + + /// + /// A distance joint rains two points on two bodies + /// to remain at a fixed distance from each other. You can view + /// this as a massless, rigid rod. + /// + public class DistanceJoint : Joint + { + // Solver shared + private float _bias; + private float _gamma; + private float _impulse; + + // Solver temp + private int _indexA; + private int _indexB; + private Vector2 _u; + private Vector2 _rA; + private Vector2 _rB; + private Vector2 _localCenterA; + private Vector2 _localCenterB; + private float _invMassA; + private float _invMassB; + private float _invIA; + private float _invIB; + private float _mass; + + internal DistanceJoint() + { + JointType = JointType.Distance; + } + + /// + /// This requires defining an + /// anchor point on both bodies and the non-zero length of the + /// distance joint. If you don't supply a length, the local anchor points + /// is used so that the initial configuration can violate the constraint + /// slightly. This helps when saving and loading a game. + /// Warning Do not use a zero or short length. + /// + /// The first body + /// The second body + /// The first body anchor + /// The second body anchor + /// Set to true if you are using world coordinates as anchors. + public DistanceJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false) + : base(bodyA, bodyB) + { + JointType = JointType.Distance; + + if (useWorldCoordinates) + { + LocalAnchorA = bodyA.GetLocalPoint(ref anchorA); + LocalAnchorB = bodyB.GetLocalPoint(ref anchorB); + Length = (anchorB - anchorA).Length(); + } + else + { + LocalAnchorA = anchorA; + LocalAnchorB = anchorB; + Length = (BodyB.GetWorldPoint(ref anchorB) - BodyA.GetWorldPoint(ref anchorA)).Length(); + } + } + + /// + /// The local anchor point relative to bodyA's origin. + /// + public Vector2 LocalAnchorA { get; set; } + + /// + /// The local anchor point relative to bodyB's origin. + /// + public Vector2 LocalAnchorB { get; set; } + + public override sealed Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + public override sealed Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + /// + /// The natural length between the anchor points. + /// Manipulating the length can lead to non-physical behavior when the frequency is zero. + /// + public float Length { get; set; } + + /// + /// The mass-spring-damper frequency in Hertz. A value of 0 + /// disables softness. + /// + public float Frequency { get; set; } + + /// + /// The damping ratio. 0 = no damping, 1 = critical damping. + /// + public float DampingRatio { get; set; } + + /// + /// Get the reaction force given the inverse time step. Unit is N. + /// + /// + /// + public override Vector2 GetReactionForce(float invDt) + { + Vector2 F = (invDt * _impulse) * _u; + return F; + } + + /// + /// Get the reaction torque given the inverse time step. + /// Unit is N*m. This is always zero for a distance joint. + /// + /// + /// + public override float GetReactionTorque(float invDt) + { + return 0.0f; + } + + internal override void InitVelocityConstraints(ref SolverData data) + { + _indexA = BodyA.IslandIndex; + _indexB = BodyB.IslandIndex; + _localCenterA = BodyA._sweep.LocalCenter; + _localCenterB = BodyB._sweep.LocalCenter; + _invMassA = BodyA._invMass; + _invMassB = BodyB._invMass; + _invIA = BodyA._invI; + _invIB = BodyB._invI; + + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + _u = cB + _rB - cA - _rA; + + // Handle singularity. + float length = _u.Length(); + if (length > Settings.LinearSlop) + { + _u *= 1.0f / length; + } + else + { + _u = Vector2.Zero; + } + + float crAu = MathUtils.Cross(_rA, _u); + float crBu = MathUtils.Cross(_rB, _u); + float invMass = _invMassA + _invIA * crAu * crAu + _invMassB + _invIB * crBu * crBu; + + // Compute the effective mass matrix. + _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; + + if (Frequency > 0.0f) + { + float C = length - Length; + + // Frequency + float omega = 2.0f * Settings.Pi * Frequency; + + // Damping coefficient + float d = 2.0f * _mass * DampingRatio * omega; + + // Spring stiffness + float k = _mass * omega * omega; + + // magic formulas + float h = data.step.dt; + _gamma = h * (d + h * k); + _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f; + _bias = C * h * k * _gamma; + + invMass += _gamma; + _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; + } + else + { + _gamma = 0.0f; + _bias = 0.0f; + } + + if (Settings.EnableWarmstarting) + { + // Scale the impulse to support a variable time step. + _impulse *= data.step.dtRatio; + + Vector2 P = _impulse * _u; + vA -= _invMassA * P; + wA -= _invIA * MathUtils.Cross(_rA, P); + vB += _invMassB * P; + wB += _invIB * MathUtils.Cross(_rB, P); + } + else + { + _impulse = 0.0f; + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override void SolveVelocityConstraints(ref SolverData data) + { + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + // Cdot = dot(u, v + cross(w, r)) + Vector2 vpA = vA + MathUtils.Cross(wA, _rA); + Vector2 vpB = vB + MathUtils.Cross(wB, _rB); + float Cdot = Vector2.Dot(_u, vpB - vpA); + + float impulse = -_mass * (Cdot + _bias + _gamma * _impulse); + _impulse += impulse; + + Vector2 P = impulse * _u; + vA -= _invMassA * P; + wA -= _invIA * MathUtils.Cross(_rA, P); + vB += _invMassB * P; + wB += _invIB * MathUtils.Cross(_rB, P); + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + + } + + internal override bool SolvePositionConstraints(ref SolverData data) + { + if (Frequency > 0.0f) + { + // There is no position correction for soft distance constraints. + return true; + } + + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + Vector2 u = cB + rB - cA - rA; + + float length = u.Length(); u.Normalize(); + float C = length - Length; + C = MathUtils.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); + + float impulse = -_mass * C; + Vector2 P = impulse * u; + + cA -= _invMassA * P; + aA -= _invIA * MathUtils.Cross(rA, P); + cB += _invMassB * P; + aB += _invIB * MathUtils.Cross(rB, P); + + data.positions[_indexA].c = cA; + data.positions[_indexA].a = aA; + data.positions[_indexB].c = cB; + data.positions[_indexB].a = aB; + + return Math.Abs(C) < Settings.LinearSlop; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Dynamics/Joints/FixedMouseJoint.cs b/Farseer Physics Engine 3.5/Dynamics/Joints/FixedMouseJoint.cs new file mode 100644 index 000000000..ba7e6846b --- /dev/null +++ b/Farseer Physics Engine 3.5/Dynamics/Joints/FixedMouseJoint.cs @@ -0,0 +1,261 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + // p = attached point, m = mouse point + // C = p - m + // Cdot = v + // = v + cross(w, r) + // J = [I r_skew] + // Identity used: + // w k % (rx i + ry j) = w * (-ry i + rx j) + + /// + /// A mouse joint is used to make a point on a body track a + /// specified world point. This a soft constraint with a maximum + /// force. This allows the constraint to stretch and without + /// applying huge forces. + /// NOTE: this joint is not documented in the manual because it was + /// developed to be used in the testbed. If you want to learn how to + /// use the mouse joint, look at the testbed. + /// + public class FixedMouseJoint : Joint + { + private Vector2 _worldAnchor; + private float _frequency; + private float _dampingRatio; + private float _beta; + + // Solver shared + private Vector2 _impulse; + private float _maxForce; + private float _gamma; + + // Solver temp + private int _indexA; + private Vector2 _rA; + private Vector2 _localCenterA; + private float _invMassA; + private float _invIA; + private Mat22 _mass; + private Vector2 _C; + + /// + /// This requires a world target point, + /// tuning parameters, and the time step. + /// + /// The body. + /// The target. + public FixedMouseJoint(Body body, Vector2 worldAnchor) + : base(body) + { + JointType = JointType.FixedMouse; + Frequency = 5.0f; + DampingRatio = 0.7f; + MaxForce = 1000 * body.Mass; + + Debug.Assert(worldAnchor.IsValid()); + + _worldAnchor = worldAnchor; + LocalAnchorA = MathUtils.MulT(BodyA._xf, worldAnchor); + } + + /// + /// The local anchor point on BodyA + /// + public Vector2 LocalAnchorA { get; set; } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + set { LocalAnchorA = BodyA.GetLocalPoint(value); } + } + + public override Vector2 WorldAnchorB + { + get { return _worldAnchor; } + set + { + WakeBodies(); + _worldAnchor = value; + } + } + + /// + /// The maximum constraint force that can be exerted + /// to move the candidate body. Usually you will express + /// as some multiple of the weight (multiplier * mass * gravity). + /// + public float MaxForce + { + get { return _maxForce; } + set + { + Debug.Assert(MathUtils.IsValid(value) && value >= 0.0f); + _maxForce = value; + } + } + + /// + /// The response speed. + /// + public float Frequency + { + get { return _frequency; } + set + { + Debug.Assert(MathUtils.IsValid(value) && value >= 0.0f); + _frequency = value; + } + } + + /// + /// The damping ratio. 0 = no damping, 1 = critical damping. + /// + public float DampingRatio + { + get { return _dampingRatio; } + set + { + Debug.Assert(MathUtils.IsValid(value) && value >= 0.0f); + _dampingRatio = value; + } + } + + public override Vector2 GetReactionForce(float invDt) + { + return invDt * _impulse; + } + + public override float GetReactionTorque(float invDt) + { + return invDt * 0.0f; + } + + internal override void InitVelocityConstraints(ref SolverData data) + { + _indexA = BodyA.IslandIndex; + _localCenterA = BodyA._sweep.LocalCenter; + _invMassA = BodyA._invMass; + _invIA = BodyA._invI; + + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + + Rot qA = new Rot(aA); + + float mass = BodyA.Mass; + + // Frequency + float omega = 2.0f * Settings.Pi * Frequency; + + // Damping coefficient + float d = 2.0f * mass * DampingRatio * omega; + + // Spring stiffness + float k = mass * (omega * omega); + + // magic formulas + // gamma has units of inverse mass. + // beta has units of inverse time. + float h = data.step.dt; + Debug.Assert(d + h * k > Settings.Epsilon); + _gamma = h * (d + h * k); + if (_gamma != 0.0f) + { + _gamma = 1.0f / _gamma; + } + + _beta = h * k * _gamma; + + // Compute the effective mass matrix. + _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] + // = [1/m1+1/m2 0 ] + invI1 * [r1.Y*r1.Y -r1.X*r1.Y] + invI2 * [r1.Y*r1.Y -r1.X*r1.Y] + // [ 0 1/m1+1/m2] [-r1.X*r1.Y r1.X*r1.X] [-r1.X*r1.Y r1.X*r1.X] + Mat22 K = new Mat22(); + K.ex.X = _invMassA + _invIA * _rA.Y * _rA.Y + _gamma; + K.ex.Y = -_invIA * _rA.X * _rA.Y; + K.ey.X = K.ex.Y; + K.ey.Y = _invMassA + _invIA * _rA.X * _rA.X + _gamma; + + _mass = K.Inverse; + + _C = cA + _rA - _worldAnchor; + _C *= _beta; + + // Cheat with some damping + wA *= 0.98f; + + if (Settings.EnableWarmstarting) + { + _impulse *= data.step.dtRatio; + vA += _invMassA * _impulse; + wA += _invIA * MathUtils.Cross(_rA, _impulse); + } + else + { + _impulse = Vector2.Zero; + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + } + + internal override void SolveVelocityConstraints(ref SolverData data) + { + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + + // Cdot = v + cross(w, r) + Vector2 Cdot = vA + MathUtils.Cross(wA, _rA); + Vector2 impulse = MathUtils.Mul(ref _mass, -(Cdot + _C + _gamma * _impulse)); + + Vector2 oldImpulse = _impulse; + _impulse += impulse; + float maxImpulse = data.step.dt * MaxForce; + if (_impulse.LengthSquared() > maxImpulse * maxImpulse) + { + _impulse *= maxImpulse / _impulse.Length(); + } + impulse = _impulse - oldImpulse; + + vA += _invMassA * impulse; + wA += _invIA * MathUtils.Cross(_rA, impulse); + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + } + + internal override bool SolvePositionConstraints(ref SolverData data) + { + return true; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Dynamics/Joints/FrictionJoint.cs b/Farseer Physics Engine 3.5/Dynamics/Joints/FrictionJoint.cs new file mode 100644 index 000000000..61ffd3585 --- /dev/null +++ b/Farseer Physics Engine 3.5/Dynamics/Joints/FrictionJoint.cs @@ -0,0 +1,272 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + // Point-to-point constraint + // Cdot = v2 - v1 + // = v2 + cross(w2, r2) - v1 - cross(w1, r1) + // J = [-I -r1_skew I r2_skew ] + // Identity used: + // w k % (rx i + ry j) = w * (-ry i + rx j) + + // Angle constraint + // Cdot = w2 - w1 + // J = [0 0 -1 0 0 1] + // K = invI1 + invI2 + + /// + /// Friction joint. This is used for top-down friction. + /// It provides 2D translational friction and angular friction. + /// + public class FrictionJoint : Joint + { + // Solver shared + private Vector2 _linearImpulse; + private float _angularImpulse; + + // Solver temp + private int _indexA; + private int _indexB; + private Vector2 _rA; + private Vector2 _rB; + private Vector2 _localCenterA; + private Vector2 _localCenterB; + private float _invMassA; + private float _invMassB; + private float _invIA; + private float _invIB; + private float _angularMass; + private Mat22 _linearMass; + + internal FrictionJoint() + { + JointType = JointType.Friction; + } + + /// + /// Constructor for FrictionJoint. + /// + /// + /// + /// + /// Set to true if you are using world coordinates as anchors. + public FrictionJoint(Body bodyA, Body bodyB, Vector2 anchor, bool useWorldCoordinates = false) + : base(bodyA, bodyB) + { + JointType = JointType.Friction; + + if (useWorldCoordinates) + { + LocalAnchorA = BodyA.GetLocalPoint(anchor); + LocalAnchorB = BodyB.GetLocalPoint(anchor); + } + else + { + LocalAnchorA = anchor; + LocalAnchorB = anchor; + } + } + + /// + /// The local anchor point on BodyA + /// + public Vector2 LocalAnchorA { get; set; } + + /// + /// The local anchor point on BodyB + /// + public Vector2 LocalAnchorB { get; set; } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + set { LocalAnchorA = BodyA.GetLocalPoint(value); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { LocalAnchorB = BodyB.GetLocalPoint(value); } + } + + /// + /// The maximum friction force in N. + /// + public float MaxForce { get; set; } + + /// + /// The maximum friction torque in N-m. + /// + public float MaxTorque { get; set; } + + public override Vector2 GetReactionForce(float invDt) + { + return invDt * _linearImpulse; + } + + public override float GetReactionTorque(float invDt) + { + return invDt * _angularImpulse; + } + + internal override void InitVelocityConstraints(ref SolverData data) + { + _indexA = BodyA.IslandIndex; + _indexB = BodyB.IslandIndex; + _localCenterA = BodyA._sweep.LocalCenter; + _localCenterB = BodyB._sweep.LocalCenter; + _invMassA = BodyA._invMass; + _invMassB = BodyB._invMass; + _invIA = BodyA._invI; + _invIB = BodyB._invI; + + float aA = data.positions[_indexA].a; + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + + float aB = data.positions[_indexB].a; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + // Compute the effective mass matrix. + _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + + // J = [-I -r1_skew I r2_skew] + // [ 0 -1 0 1] + // r_skew = [-ry; rx] + + // Matlab + // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] + // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] + // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] + + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + Mat22 K = new Mat22(); + K.ex.X = mA + mB + iA * _rA.Y * _rA.Y + iB * _rB.Y * _rB.Y; + K.ex.Y = -iA * _rA.X * _rA.Y - iB * _rB.X * _rB.Y; + K.ey.X = K.ex.Y; + K.ey.Y = mA + mB + iA * _rA.X * _rA.X + iB * _rB.X * _rB.X; + + _linearMass = K.Inverse; + + _angularMass = iA + iB; + if (_angularMass > 0.0f) + { + _angularMass = 1.0f / _angularMass; + } + + if (Settings.EnableWarmstarting) + { + // Scale impulses to support a variable time step. + _linearImpulse *= data.step.dtRatio; + _angularImpulse *= data.step.dtRatio; + + Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y); + vA -= mA * P; + wA -= iA * (MathUtils.Cross(_rA, P) + _angularImpulse); + vB += mB * P; + wB += iB * (MathUtils.Cross(_rB, P) + _angularImpulse); + } + else + { + _linearImpulse = Vector2.Zero; + _angularImpulse = 0.0f; + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override void SolveVelocityConstraints(ref SolverData data) + { + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + float h = data.step.dt; + + // Solve angular friction + { + float Cdot = wB - wA; + float impulse = -_angularMass * Cdot; + + float oldImpulse = _angularImpulse; + float maxImpulse = h * MaxTorque; + _angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse); + impulse = _angularImpulse - oldImpulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + // Solve linear friction + { + Vector2 Cdot = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA); + + Vector2 impulse = -MathUtils.Mul(ref _linearMass, Cdot); + Vector2 oldImpulse = _linearImpulse; + _linearImpulse += impulse; + + float maxImpulse = h * MaxForce; + + if (_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) + { + _linearImpulse.Normalize(); + _linearImpulse *= maxImpulse; + } + + impulse = _linearImpulse - oldImpulse; + + vA -= mA * impulse; + wA -= iA * MathUtils.Cross(_rA, impulse); + + vB += mB * impulse; + wB += iB * MathUtils.Cross(_rB, impulse); + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override bool SolvePositionConstraints(ref SolverData data) + { + return true; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Dynamics/Joints/GearJoint.cs b/Farseer Physics Engine 3.5/Dynamics/Joints/GearJoint.cs new file mode 100644 index 000000000..8cd7c5f8d --- /dev/null +++ b/Farseer Physics Engine 3.5/Dynamics/Joints/GearJoint.cs @@ -0,0 +1,478 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + // Gear Joint: + // C0 = (coordinate1 + ratio * coordinate2)_initial + // C = (coordinate1 + ratio * coordinate2) - C0 = 0 + // J = [J1 ratio * J2] + // K = J * invM * JT + // = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T + // + // Revolute: + // coordinate = rotation + // Cdot = angularVelocity + // J = [0 0 1] + // K = J * invM * JT = invI + // + // Prismatic: + // coordinate = dot(p - pg, ug) + // Cdot = dot(v + cross(w, r), ug) + // J = [ug cross(r, ug)] + // K = J * invM * JT = invMass + invI * cross(r, ug)^2 + + /// + /// A gear joint is used to connect two joints together. + /// Either joint can be a revolute or prismatic joint. + /// You specify a gear ratio to bind the motions together: + /// + /// The ratio can be negative or positive. If one joint is a revolute joint + /// and the other joint is a prismatic joint, then the ratio will have units + /// of length or units of 1/length. + /// + /// Warning: You have to manually destroy the gear joint if jointA or jointB is destroyed. + /// + public class GearJoint : Joint + { + private JointType _typeA; + private JointType _typeB; + + private Body _bodyA; + private Body _bodyB; + private Body _bodyC; + private Body _bodyD; + + // Solver shared + private Vector2 _localAnchorA; + private Vector2 _localAnchorB; + private Vector2 _localAnchorC; + private Vector2 _localAnchorD; + + private Vector2 _localAxisC; + private Vector2 _localAxisD; + + private float _referenceAngleA; + private float _referenceAngleB; + + private float _constant; + private float _ratio; + + private float _impulse; + + // Solver temp + private int _indexA, _indexB, _indexC, _indexD; + private Vector2 _lcA, _lcB, _lcC, _lcD; + private float _mA, _mB, _mC, _mD; + private float _iA, _iB, _iC, _iD; + private Vector2 _JvAC, _JvBD; + private float _JwA, _JwB, _JwC, _JwD; + private float _mass; + + /// + /// Requires two existing revolute or prismatic joints (any combination will work). + /// The provided joints must attach a dynamic body to a static body. + /// + /// The first joint. + /// The second joint. + /// The ratio. + /// The first body + /// The second body + public GearJoint(Body bodyA, Body bodyB, Joint jointA, Joint jointB, float ratio = 1f) + { + JointType = JointType.Gear; + BodyA = bodyA; + BodyB = bodyB; + JointA = jointA; + JointB = jointB; + Ratio = ratio; + + _typeA = jointA.JointType; + _typeB = jointB.JointType; + + Debug.Assert(_typeA == JointType.Revolute || _typeA == JointType.Prismatic || _typeA == JointType.FixedRevolute || _typeA == JointType.FixedPrismatic); + Debug.Assert(_typeB == JointType.Revolute || _typeB == JointType.Prismatic || _typeB == JointType.FixedRevolute || _typeB == JointType.FixedPrismatic); + + float coordinateA, coordinateB; + + // TODO_ERIN there might be some problem with the joint edges in b2Joint. + + _bodyC = JointA.BodyA; + _bodyA = JointA.BodyB; + + // Get geometry of joint1 + Transform xfA = _bodyA._xf; + float aA = _bodyA._sweep.A; + Transform xfC = _bodyC._xf; + float aC = _bodyC._sweep.A; + + if (_typeA == JointType.Revolute) + { + RevoluteJoint revolute = (RevoluteJoint)jointA; + _localAnchorC = revolute.LocalAnchorA; + _localAnchorA = revolute.LocalAnchorB; + _referenceAngleA = revolute.ReferenceAngle; + _localAxisC = Vector2.Zero; + + coordinateA = aA - aC - _referenceAngleA; + } + else + { + PrismaticJoint prismatic = (PrismaticJoint)jointA; + _localAnchorC = prismatic.LocalAnchorA; + _localAnchorA = prismatic.LocalAnchorB; + _referenceAngleA = prismatic.ReferenceAngle; + _localAxisC = prismatic.LocalXAxis; + + Vector2 pC = _localAnchorC; + Vector2 pA = MathUtils.MulT(xfC.q, MathUtils.Mul(xfA.q, _localAnchorA) + (xfA.p - xfC.p)); + coordinateA = Vector2.Dot(pA - pC, _localAxisC); + } + + _bodyD = JointB.BodyA; + _bodyB = JointB.BodyB; + + // Get geometry of joint2 + Transform xfB = _bodyB._xf; + float aB = _bodyB._sweep.A; + Transform xfD = _bodyD._xf; + float aD = _bodyD._sweep.A; + + if (_typeB == JointType.Revolute) + { + RevoluteJoint revolute = (RevoluteJoint)jointB; + _localAnchorD = revolute.LocalAnchorA; + _localAnchorB = revolute.LocalAnchorB; + _referenceAngleB = revolute.ReferenceAngle; + _localAxisD = Vector2.Zero; + + coordinateB = aB - aD - _referenceAngleB; + } + else + { + PrismaticJoint prismatic = (PrismaticJoint)jointB; + _localAnchorD = prismatic.LocalAnchorA; + _localAnchorB = prismatic.LocalAnchorB; + _referenceAngleB = prismatic.ReferenceAngle; + _localAxisD = prismatic.LocalXAxis; + + Vector2 pD = _localAnchorD; + Vector2 pB = MathUtils.MulT(xfD.q, MathUtils.Mul(xfB.q, _localAnchorB) + (xfB.p - xfD.p)); + coordinateB = Vector2.Dot(pB - pD, _localAxisD); + } + + _ratio = ratio; + _constant = coordinateA + _ratio * coordinateB; + _impulse = 0.0f; + } + + public override Vector2 WorldAnchorA + { + get { return _bodyA.GetWorldPoint(_localAnchorA); } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + public override Vector2 WorldAnchorB + { + get { return _bodyB.GetWorldPoint(_localAnchorB); } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + /// + /// The gear ratio. + /// + public float Ratio + { + get { return _ratio; } + set + { + Debug.Assert(MathUtils.IsValid(value)); + _ratio = value; + } + } + + /// + /// The first revolute/prismatic joint attached to the gear joint. + /// + public Joint JointA { get; private set; } + + /// + /// The second revolute/prismatic joint attached to the gear joint. + /// + public Joint JointB { get; private set; } + + public override Vector2 GetReactionForce(float invDt) + { + Vector2 P = _impulse * _JvAC; + return invDt * P; + } + + public override float GetReactionTorque(float invDt) + { + float L = _impulse * _JwA; + return invDt * L; + } + + internal override void InitVelocityConstraints(ref SolverData data) + { + _indexA = _bodyA.IslandIndex; + _indexB = _bodyB.IslandIndex; + _indexC = _bodyC.IslandIndex; + _indexD = _bodyD.IslandIndex; + _lcA = _bodyA._sweep.LocalCenter; + _lcB = _bodyB._sweep.LocalCenter; + _lcC = _bodyC._sweep.LocalCenter; + _lcD = _bodyD._sweep.LocalCenter; + _mA = _bodyA._invMass; + _mB = _bodyB._invMass; + _mC = _bodyC._invMass; + _mD = _bodyD._invMass; + _iA = _bodyA._invI; + _iB = _bodyB._invI; + _iC = _bodyC._invI; + _iD = _bodyD._invI; + + float aA = data.positions[_indexA].a; + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + + float aB = data.positions[_indexB].a; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + float aC = data.positions[_indexC].a; + Vector2 vC = data.velocities[_indexC].v; + float wC = data.velocities[_indexC].w; + + float aD = data.positions[_indexD].a; + Vector2 vD = data.velocities[_indexD].v; + float wD = data.velocities[_indexD].w; + + Rot qA = new Rot(aA), qB = new Rot(aB), qC = new Rot(aC), qD = new Rot(aD); + + _mass = 0.0f; + + if (_typeA == JointType.Revolute) + { + _JvAC = Vector2.Zero; + _JwA = 1.0f; + _JwC = 1.0f; + _mass += _iA + _iC; + } + else + { + Vector2 u = MathUtils.Mul(qC, _localAxisC); + Vector2 rC = MathUtils.Mul(qC, _localAnchorC - _lcC); + Vector2 rA = MathUtils.Mul(qA, _localAnchorA - _lcA); + _JvAC = u; + _JwC = MathUtils.Cross(rC, u); + _JwA = MathUtils.Cross(rA, u); + _mass += _mC + _mA + _iC * _JwC * _JwC + _iA * _JwA * _JwA; + } + + if (_typeB == JointType.Revolute) + { + _JvBD = Vector2.Zero; + _JwB = _ratio; + _JwD = _ratio; + _mass += _ratio * _ratio * (_iB + _iD); + } + else + { + Vector2 u = MathUtils.Mul(qD, _localAxisD); + Vector2 rD = MathUtils.Mul(qD, _localAnchorD - _lcD); + Vector2 rB = MathUtils.Mul(qB, _localAnchorB - _lcB); + _JvBD = _ratio * u; + _JwD = _ratio * MathUtils.Cross(rD, u); + _JwB = _ratio * MathUtils.Cross(rB, u); + _mass += _ratio * _ratio * (_mD + _mB) + _iD * _JwD * _JwD + _iB * _JwB * _JwB; + } + + // Compute effective mass. + _mass = _mass > 0.0f ? 1.0f / _mass : 0.0f; + + if (Settings.EnableWarmstarting) + { + vA += (_mA * _impulse) * _JvAC; + wA += _iA * _impulse * _JwA; + vB += (_mB * _impulse) * _JvBD; + wB += _iB * _impulse * _JwB; + vC -= (_mC * _impulse) * _JvAC; + wC -= _iC * _impulse * _JwC; + vD -= (_mD * _impulse) * _JvBD; + wD -= _iD * _impulse * _JwD; + } + else + { + _impulse = 0.0f; + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + data.velocities[_indexC].v = vC; + data.velocities[_indexC].w = wC; + data.velocities[_indexD].v = vD; + data.velocities[_indexD].w = wD; + } + + internal override void SolveVelocityConstraints(ref SolverData data) + { + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + Vector2 vC = data.velocities[_indexC].v; + float wC = data.velocities[_indexC].w; + Vector2 vD = data.velocities[_indexD].v; + float wD = data.velocities[_indexD].w; + + float Cdot = Vector2.Dot(_JvAC, vA - vC) + Vector2.Dot(_JvBD, vB - vD); + Cdot += (_JwA * wA - _JwC * wC) + (_JwB * wB - _JwD * wD); + + float impulse = -_mass * Cdot; + _impulse += impulse; + + vA += (_mA * impulse) * _JvAC; + wA += _iA * impulse * _JwA; + vB += (_mB * impulse) * _JvBD; + wB += _iB * impulse * _JwB; + vC -= (_mC * impulse) * _JvAC; + wC -= _iC * impulse * _JwC; + vD -= (_mD * impulse) * _JvBD; + wD -= _iD * impulse * _JwD; + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + data.velocities[_indexC].v = vC; + data.velocities[_indexC].w = wC; + data.velocities[_indexD].v = vD; + data.velocities[_indexD].w = wD; + } + + internal override bool SolvePositionConstraints(ref SolverData data) + { + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + Vector2 cC = data.positions[_indexC].c; + float aC = data.positions[_indexC].a; + Vector2 cD = data.positions[_indexD].c; + float aD = data.positions[_indexD].a; + + Rot qA = new Rot(aA), qB = new Rot(aB), qC = new Rot(aC), qD = new Rot(aD); + + const float linearError = 0.0f; + + float coordinateA, coordinateB; + + Vector2 JvAC, JvBD; + float JwA, JwB, JwC, JwD; + float mass = 0.0f; + + if (_typeA == JointType.Revolute) + { + JvAC = Vector2.Zero; + JwA = 1.0f; + JwC = 1.0f; + mass += _iA + _iC; + + coordinateA = aA - aC - _referenceAngleA; + } + else + { + Vector2 u = MathUtils.Mul(qC, _localAxisC); + Vector2 rC = MathUtils.Mul(qC, _localAnchorC - _lcC); + Vector2 rA = MathUtils.Mul(qA, _localAnchorA - _lcA); + JvAC = u; + JwC = MathUtils.Cross(rC, u); + JwA = MathUtils.Cross(rA, u); + mass += _mC + _mA + _iC * JwC * JwC + _iA * JwA * JwA; + + Vector2 pC = _localAnchorC - _lcC; + Vector2 pA = MathUtils.MulT(qC, rA + (cA - cC)); + coordinateA = Vector2.Dot(pA - pC, _localAxisC); + } + + if (_typeB == JointType.Revolute) + { + JvBD = Vector2.Zero; + JwB = _ratio; + JwD = _ratio; + mass += _ratio * _ratio * (_iB + _iD); + + coordinateB = aB - aD - _referenceAngleB; + } + else + { + Vector2 u = MathUtils.Mul(qD, _localAxisD); + Vector2 rD = MathUtils.Mul(qD, _localAnchorD - _lcD); + Vector2 rB = MathUtils.Mul(qB, _localAnchorB - _lcB); + JvBD = _ratio * u; + JwD = _ratio * MathUtils.Cross(rD, u); + JwB = _ratio * MathUtils.Cross(rB, u); + mass += _ratio * _ratio * (_mD + _mB) + _iD * JwD * JwD + _iB * JwB * JwB; + + Vector2 pD = _localAnchorD - _lcD; + Vector2 pB = MathUtils.MulT(qD, rB + (cB - cD)); + coordinateB = Vector2.Dot(pB - pD, _localAxisD); + } + + float C = (coordinateA + _ratio * coordinateB) - _constant; + + float impulse = 0.0f; + if (mass > 0.0f) + { + impulse = -C / mass; + } + + cA += _mA * impulse * JvAC; + aA += _iA * impulse * JwA; + cB += _mB * impulse * JvBD; + aB += _iB * impulse * JwB; + cC -= _mC * impulse * JvAC; + aC -= _iC * impulse * JwC; + cD -= _mD * impulse * JvBD; + aD -= _iD * impulse * JwD; + + data.positions[_indexA].c = cA; + data.positions[_indexA].a = aA; + data.positions[_indexB].c = cB; + data.positions[_indexB].a = aB; + data.positions[_indexC].c = cC; + data.positions[_indexC].a = aC; + data.positions[_indexD].c = cD; + data.positions[_indexD].a = aD; + + // TODO_ERIN not implemented + return linearError < Settings.LinearSlop; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Dynamics/Joints/Joint.cs b/Farseer Physics Engine 3.5/Dynamics/Joints/Joint.cs new file mode 100644 index 000000000..74e890467 --- /dev/null +++ b/Farseer Physics Engine 3.5/Dynamics/Joints/Joint.cs @@ -0,0 +1,253 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + public enum JointType + { + Unknown, + Revolute, + Prismatic, + Distance, + Pulley, + //Mouse, <- We have fixed mouse + Gear, + Wheel, + Weld, + Friction, + Rope, + Motor, + + //FPE note: From here on and down, it is only FPE joints + Angle, + FixedMouse, + FixedRevolute, + FixedDistance, + FixedLine, + FixedPrismatic, + FixedAngle, + FixedFriction, + } + + public enum LimitState + { + Inactive, + AtLower, + AtUpper, + Equal, + } + + /// + /// A joint edge is used to connect bodies and joints together + /// in a joint graph where each body is a node and each joint + /// is an edge. A joint edge belongs to a doubly linked list + /// maintained in each attached body. Each joint has two joint + /// nodes, one for each attached body. + /// + public sealed class JointEdge + { + /// + /// The joint. + /// + public Joint Joint; + + /// + /// The next joint edge in the body's joint list. + /// + public JointEdge Next; + + /// + /// Provides quick access to the other body attached. + /// + public Body Other; + + /// + /// The previous joint edge in the body's joint list. + /// + public JointEdge Prev; + } + + public abstract class Joint + { + private float _breakpoint; + private double _breakpointSquared; + + /// + /// Indicate if this join is enabled or not. Disabling a joint + /// means it is still in the simulation, but inactive. + /// + public bool Enabled = true; + + internal JointEdge EdgeA = new JointEdge(); + internal JointEdge EdgeB = new JointEdge(); + internal bool IslandFlag; + + protected Joint() + { + Breakpoint = float.MaxValue; + + //Connected bodies should not collide by default + CollideConnected = false; + } + + protected Joint(Body bodyA, Body bodyB) : this() + { + //Can't connect a joint to the same body twice. + Debug.Assert(bodyA != bodyB); + + BodyA = bodyA; + BodyB = bodyB; + } + + /// + /// Constructor for fixed joint + /// + protected Joint(Body body) : this() + { + BodyA = body; + } + + /// + /// Gets or sets the type of the joint. + /// + /// The type of the joint. + public JointType JointType { get; protected set; } + + /// + /// Get the first body attached to this joint. + /// + public Body BodyA { get; internal set; } + + /// + /// Get the second body attached to this joint. + /// + public Body BodyB { get; internal set; } + + /// + /// Get the anchor point on bodyA in world coordinates. + /// On some joints, this value indicate the anchor point within the world. + /// + public abstract Vector2 WorldAnchorA { get; set; } + + /// + /// Get the anchor point on bodyB in world coordinates. + /// On some joints, this value indicate the anchor point within the world. + /// + public abstract Vector2 WorldAnchorB { get; set; } + + /// + /// Set the user data pointer. + /// + /// The data. + public object UserData { get; set; } + + /// + /// Set this flag to true if the attached bodies should collide. + /// + public bool CollideConnected { get; set; } + + /// + /// The Breakpoint simply indicates the maximum Value the JointError can be before it breaks. + /// The default value is float.MaxValue, which means it never breaks. + /// + public float Breakpoint + { + get { return _breakpoint; } + set + { + _breakpoint = value; + _breakpointSquared = _breakpoint * _breakpoint; + } + } + + /// + /// Fires when the joint is broken. + /// + public event Action Broke; + + /// + /// Get the reaction force on body at the joint anchor in Newtons. + /// + /// The inverse delta time. + public abstract Vector2 GetReactionForce(float invDt); + + /// + /// Get the reaction torque on the body at the joint anchor in N*m. + /// + /// The inverse delta time. + public abstract float GetReactionTorque(float invDt); + + protected void WakeBodies() + { + if (BodyA != null) + BodyA.Awake = true; + + if (BodyB != null) + BodyB.Awake = true; + } + + /// + /// Return true if the joint is a fixed type. + /// + public bool IsFixedType() + { + return JointType == JointType.FixedRevolute || + JointType == JointType.FixedDistance || + JointType == JointType.FixedPrismatic || + JointType == JointType.FixedLine || + JointType == JointType.FixedMouse || + JointType == JointType.FixedAngle || + JointType == JointType.FixedFriction; + } + + internal abstract void InitVelocityConstraints(ref SolverData data); + + internal void Validate(float invDt) + { + if (!Enabled) + return; + + float jointErrorSquared = GetReactionForce(invDt).LengthSquared(); + + if (Math.Abs(jointErrorSquared) <= _breakpointSquared) + return; + + Enabled = false; + + if (Broke != null) + Broke(this, (float)Math.Sqrt(jointErrorSquared)); + } + + internal abstract void SolveVelocityConstraints(ref SolverData data); + + /// + /// Solves the position constraints. + /// + /// + /// returns true if the position errors are within tolerance. + internal abstract bool SolvePositionConstraints(ref SolverData data); + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Dynamics/Joints/MotorJoint.cs b/Farseer Physics Engine 3.5/Dynamics/Joints/MotorJoint.cs new file mode 100644 index 000000000..9c9bf8eaa --- /dev/null +++ b/Farseer Physics Engine 3.5/Dynamics/Joints/MotorJoint.cs @@ -0,0 +1,320 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + /// + /// A motor joint is used to control the relative motion + /// between two bodies. A typical usage is to control the movement + /// of a dynamic body with respect to the ground. + /// + public class MotorJoint : Joint + { + // Solver shared + private Vector2 _linearOffset; + private float _angularOffset; + private Vector2 _linearImpulse; + private float _angularImpulse; + private float _maxForce; + private float _maxTorque; + + // Solver temp + private int _indexA; + private int _indexB; + private Vector2 _rA; + private Vector2 _rB; + private Vector2 _localCenterA; + private Vector2 _localCenterB; + private Vector2 _linearError; + private float _angularError; + private float _invMassA; + private float _invMassB; + private float _invIA; + private float _invIB; + private Mat22 _linearMass; + private float _angularMass; + + internal MotorJoint() + { + JointType = JointType.Motor; + } + + /// + /// Constructor for MotorJoint. + /// + /// The first body + /// The second body + /// Set to true if you are using world coordinates as anchors. + public MotorJoint(Body bodyA, Body bodyB, bool useWorldCoordinates = false) + : base(bodyA, bodyB) + { + JointType = JointType.Motor; + + Vector2 xB = BodyB.Position; + + if (useWorldCoordinates) + _linearOffset = BodyA.GetLocalPoint(xB); + else + _linearOffset = xB; + + //Defaults + _angularOffset = 0.0f; + _maxForce = 1.0f; + _maxTorque = 1.0f; + CorrectionFactor = 0.3f; + + _angularOffset = BodyB.Rotation - BodyA.Rotation; + } + + public override Vector2 WorldAnchorA + { + get { return BodyA.Position; } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.Position; } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + /// + /// The maximum amount of force that can be applied to BodyA + /// + public float MaxForce + { + set + { + Debug.Assert(MathUtils.IsValid(value) && value >= 0.0f); + _maxForce = value; + } + get { return _maxForce; } + } + + /// + /// The maximum amount of torque that can be applied to BodyA + /// + public float MaxTorque + { + set + { + Debug.Assert(MathUtils.IsValid(value) && value >= 0.0f); + _maxTorque = value; + } + get { return _maxTorque; } + } + + /// + /// The linear (translation) offset. + /// + public Vector2 LinearOffset + { + set + { + if (_linearOffset.X != value.X || _linearOffset.Y != value.Y) + { + WakeBodies(); + _linearOffset = value; + } + } + get { return _linearOffset; } + } + + /// + /// Get or set the angular offset. + /// + public float AngularOffset + { + set + { + if (_angularOffset != value) + { + WakeBodies(); + _angularOffset = value; + } + } + get { return _angularOffset; } + } + + //FPE note: Used for serialization. + internal float CorrectionFactor { get; set; } + + public override Vector2 GetReactionForce(float invDt) + { + return invDt * _linearImpulse; + } + + public override float GetReactionTorque(float invDt) + { + return invDt * _angularImpulse; + } + + internal override void InitVelocityConstraints(ref SolverData data) + { + _indexA = BodyA.IslandIndex; + _indexB = BodyB.IslandIndex; + _localCenterA = BodyA._sweep.LocalCenter; + _localCenterB = BodyB._sweep.LocalCenter; + _invMassA = BodyA._invMass; + _invMassB = BodyB._invMass; + _invIA = BodyA._invI; + _invIB = BodyB._invI; + + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + Rot qA = new Rot(aA); + Rot qB = new Rot(aB); + + // Compute the effective mass matrix. + _rA = MathUtils.Mul(qA, -_localCenterA); + _rB = MathUtils.Mul(qB, -_localCenterB); + + // J = [-I -r1_skew I r2_skew] + // [ 0 -1 0 1] + // r_skew = [-ry; rx] + + // Matlab + // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] + // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] + // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] + + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + Mat22 K = new Mat22(); + K.ex.X = mA + mB + iA * _rA.Y * _rA.Y + iB * _rB.Y * _rB.Y; + K.ex.Y = -iA * _rA.X * _rA.Y - iB * _rB.X * _rB.Y; + K.ey.X = K.ex.Y; + K.ey.Y = mA + mB + iA * _rA.X * _rA.X + iB * _rB.X * _rB.X; + + _linearMass = K.Inverse; + + _angularMass = iA + iB; + if (_angularMass > 0.0f) + { + _angularMass = 1.0f / _angularMass; + } + + _linearError = cB + _rB - cA - _rA - MathUtils.Mul(qA, _linearOffset); + _angularError = aB - aA - _angularOffset; + + if (Settings.EnableWarmstarting) + { + // Scale impulses to support a variable time step. + _linearImpulse *= data.step.dtRatio; + _angularImpulse *= data.step.dtRatio; + + Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y); + + vA -= mA * P; + wA -= iA * (MathUtils.Cross(_rA, P) + _angularImpulse); + vB += mB * P; + wB += iB * (MathUtils.Cross(_rB, P) + _angularImpulse); + } + else + { + _linearImpulse = Vector2.Zero; + _angularImpulse = 0.0f; + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override void SolveVelocityConstraints(ref SolverData data) + { + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + float h = data.step.dt; + float inv_h = data.step.inv_dt; + + // Solve angular friction + { + float Cdot = wB - wA + inv_h * CorrectionFactor * _angularError; + float impulse = -_angularMass * Cdot; + + float oldImpulse = _angularImpulse; + float maxImpulse = h * _maxTorque; + _angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse); + impulse = _angularImpulse - oldImpulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + // Solve linear friction + { + Vector2 Cdot = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA) + inv_h * CorrectionFactor * _linearError; + + Vector2 impulse = -MathUtils.Mul(ref _linearMass, ref Cdot); + Vector2 oldImpulse = _linearImpulse; + _linearImpulse += impulse; + + float maxImpulse = h * _maxForce; + + if (_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) + { + _linearImpulse.Normalize(); + _linearImpulse *= maxImpulse; + } + + impulse = _linearImpulse - oldImpulse; + + vA -= mA * impulse; + wA -= iA * MathUtils.Cross(_rA, impulse); + + vB += mB * impulse; + wB += iB * MathUtils.Cross(_rB, impulse); + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override bool SolvePositionConstraints(ref SolverData data) + { + return true; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Dynamics/Joints/PrismaticJoint.cs b/Farseer Physics Engine 3.5/Dynamics/Joints/PrismaticJoint.cs new file mode 100644 index 000000000..b45a34e5f --- /dev/null +++ b/Farseer Physics Engine 3.5/Dynamics/Joints/PrismaticJoint.cs @@ -0,0 +1,768 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + // Linear constraint (point-to-line) + // d = p2 - p1 = x2 + r2 - x1 - r1 + // C = dot(perp, d) + // Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1)) + // = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2) + // J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)] + // + // Angular constraint + // C = a2 - a1 + a_initial + // Cdot = w2 - w1 + // J = [0 0 -1 0 0 1] + // + // K = J * invM * JT + // + // J = [-a -s1 a s2] + // [0 -1 0 1] + // a = perp + // s1 = cross(d + r1, a) = cross(p2 - x1, a) + // s2 = cross(r2, a) = cross(p2 - x2, a) + // Motor/Limit linear constraint + // C = dot(ax1, d) + // Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2) + // J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)] + // Block Solver + // We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even + // when the mass has poor distribution (leading to large torques about the joint anchor points). + // + // The Jacobian has 3 rows: + // J = [-uT -s1 uT s2] // linear + // [0 -1 0 1] // angular + // [-vT -a1 vT a2] // limit + // + // u = perp + // v = axis + // s1 = cross(d + r1, u), s2 = cross(r2, u) + // a1 = cross(d + r1, v), a2 = cross(r2, v) + // M * (v2 - v1) = JT * df + // J * v2 = bias + // + // v2 = v1 + invM * JT * df + // J * (v1 + invM * JT * df) = bias + // K * df = bias - J * v1 = -Cdot + // K = J * invM * JT + // Cdot = J * v1 - bias + // + // Now solve for f2. + // df = f2 - f1 + // K * (f2 - f1) = -Cdot + // f2 = invK * (-Cdot) + f1 + // + // Clamp accumulated limit impulse. + // lower: f2(3) = max(f2(3), 0) + // upper: f2(3) = min(f2(3), 0) + // + // Solve for correct f2(1:2) + // K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1 + // = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3) + // K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2) + // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) + // + // Now compute impulse to be applied: + // df = f2 - f1 + + /// + /// A prismatic joint. This joint provides one degree of freedom: translation + /// along an axis fixed in bodyA. Relative rotation is prevented. You can + /// use a joint limit to restrict the range of motion and a joint motor to + /// drive the motion or to model joint friction. + /// + public class PrismaticJoint : Joint + { + private Vector2 _localYAxisA; + private Vector3 _impulse; + private float _lowerTranslation; + private float _upperTranslation; + private float _maxMotorForce; + private float _motorSpeed; + private bool _enableLimit; + private bool _enableMotor; + private LimitState _limitState; + + // Solver temp + private int _indexA; + private int _indexB; + private Vector2 _localCenterA; + private Vector2 _localCenterB; + private float _invMassA; + private float _invMassB; + private float _invIA; + private float _invIB; + private Vector2 _axis, _perp; + private float _s1, _s2; + private float _a1, _a2; + private Mat33 _K; + private float _motorMass; + private Vector2 _axis1; + + internal PrismaticJoint() + { + JointType = JointType.Prismatic; + } + + /// + /// This requires defining a line of + /// motion using an axis and an anchor point. The definition uses local + /// anchor points and a local axis so that the initial configuration + /// can violate the constraint slightly. The joint translation is zero + /// when the local anchor points coincide in world space. Using local + /// anchors and a local axis helps when saving and loading a game. + /// + /// The first body. + /// The second body. + /// The first body anchor. + /// The second body anchor. + /// The axis. + /// Set to true if you are using world coordinates as anchors. + public PrismaticJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, Vector2 axis, bool useWorldCoordinates = false) + : base(bodyA, bodyB) + { + Initialize(anchorA, anchorB, axis, useWorldCoordinates); + } + + public PrismaticJoint(Body bodyA, Body bodyB, Vector2 anchor, Vector2 axis, bool useWorldCoordinates = false) + : base(bodyA, bodyB) + { + Initialize(anchor, anchor, axis, useWorldCoordinates); + } + + private void Initialize(Vector2 localAnchorA, Vector2 localAnchorB, Vector2 axis, bool useWorldCoordinates) + { + JointType = JointType.Prismatic; + + if (useWorldCoordinates) + { + LocalAnchorA = BodyA.GetLocalPoint(localAnchorA); + LocalAnchorB = BodyB.GetLocalPoint(localAnchorB); + } + else + { + LocalAnchorA = localAnchorA; + LocalAnchorB = localAnchorB; + } + + Axis = axis; //FPE only: store the orignal value for use in Serialization + ReferenceAngle = BodyB.Rotation - BodyA.Rotation; + + _limitState = LimitState.Inactive; + } + + /// + /// The local anchor point on BodyA + /// + public Vector2 LocalAnchorA { get; set; } + + /// + /// The local anchor point on BodyB + /// + public Vector2 LocalAnchorB { get; set; } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + set { LocalAnchorA = BodyA.GetLocalPoint(value); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { LocalAnchorB = BodyB.GetLocalPoint(value); } + } + + /// + /// Get the current joint translation, usually in meters. + /// + /// + public float JointTranslation + { + get + { + Vector2 d = BodyB.GetWorldPoint(LocalAnchorB) - BodyA.GetWorldPoint(LocalAnchorA); + Vector2 axis = BodyA.GetWorldVector(LocalXAxis); + + return Vector2.Dot(d, axis); + } + } + + /// + /// Get the current joint translation speed, usually in meters per second. + /// + /// + public float JointSpeed + { + get + { + Transform xf1, xf2; + BodyA.GetTransform(out xf1); + BodyB.GetTransform(out xf2); + + Vector2 r1 = MathUtils.Mul(ref xf1.q, LocalAnchorA - BodyA.LocalCenter); + Vector2 r2 = MathUtils.Mul(ref xf2.q, LocalAnchorB - BodyB.LocalCenter); + Vector2 p1 = BodyA._sweep.C + r1; + Vector2 p2 = BodyB._sweep.C + r2; + Vector2 d = p2 - p1; + Vector2 axis = BodyA.GetWorldVector(LocalXAxis); + + Vector2 v1 = BodyA._linearVelocity; + Vector2 v2 = BodyB._linearVelocity; + float w1 = BodyA._angularVelocity; + float w2 = BodyB._angularVelocity; + + float speed = Vector2.Dot(d, MathUtils.Cross(w1, axis)) + Vector2.Dot(axis, v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1)); + return speed; + } + } + + /// + /// Is the joint limit enabled? + /// + /// true if [limit enabled]; otherwise, false. + public bool LimitEnabled + { + get { return _enableLimit; } + set + { + Debug.Assert(BodyA.FixedRotation == false || BodyB.FixedRotation == false, "Warning: limits does currently not work with fixed rotation"); + + if (value != _enableLimit) + { + WakeBodies(); + _enableLimit = value; + _impulse.Z = 0; + } + } + } + + /// + /// Get the lower joint limit, usually in meters. + /// + /// + public float LowerLimit + { + get { return _lowerTranslation; } + set + { + if (value != _lowerTranslation) + { + WakeBodies(); + _lowerTranslation = value; + _impulse.Z = 0.0f; + } + } + } + + /// + /// Get the upper joint limit, usually in meters. + /// + /// + public float UpperLimit + { + get { return _upperTranslation; } + set + { + if (value != _upperTranslation) + { + WakeBodies(); + _upperTranslation = value; + _impulse.Z = 0.0f; + } + } + } + + /// + /// Set the joint limits, usually in meters. + /// + /// The lower limit + /// The upper limit + public void SetLimits(float lower, float upper) + { + if (upper != _upperTranslation || lower != _lowerTranslation) + { + WakeBodies(); + _upperTranslation = upper; + _lowerTranslation = lower; + _impulse.Z = 0.0f; + } + } + + /// + /// Is the joint motor enabled? + /// + /// true if [motor enabled]; otherwise, false. + public bool MotorEnabled + { + get { return _enableMotor; } + set + { + WakeBodies(); + _enableMotor = value; + } + } + + /// + /// Set the motor speed, usually in meters per second. + /// + /// The speed. + public float MotorSpeed + { + set + { + WakeBodies(); + _motorSpeed = value; + } + get { return _motorSpeed; } + } + + /// + /// Set the maximum motor force, usually in N. + /// + /// The force. + public float MaxMotorForce + { + get { return _maxMotorForce; } + set + { + WakeBodies(); + _maxMotorForce = value; + } + } + + /// + /// Get the current motor impulse, usually in N. + /// + /// + public float MotorImpulse { get; set; } + + /// + /// Gets the motor force. + /// + /// The inverse delta time + public float GetMotorForce(float invDt) + { + return invDt * MotorImpulse; + } + + /// + /// The axis at which the joint moves. + /// + public Vector2 Axis + { + get { return _axis1; } + set + { + _axis1 = value; + LocalXAxis = BodyA.GetLocalVector(_axis1); + LocalXAxis.Normalize(); + _localYAxisA = MathUtils.Cross(1.0f, LocalXAxis); + } + } + + /// + /// The axis in local coordinates relative to BodyA + /// + public Vector2 LocalXAxis { get; private set; } + + /// + /// The reference angle. + /// + public float ReferenceAngle { get; set; } + + public override Vector2 GetReactionForce(float invDt) + { + return invDt * (_impulse.X * _perp + (MotorImpulse + _impulse.Z) * _axis); + } + + public override float GetReactionTorque(float invDt) + { + return invDt * _impulse.Y; + } + + internal override void InitVelocityConstraints(ref SolverData data) + { + _indexA = BodyA.IslandIndex; + _indexB = BodyB.IslandIndex; + _localCenterA = BodyA._sweep.LocalCenter; + _localCenterB = BodyB._sweep.LocalCenter; + _invMassA = BodyA._invMass; + _invMassB = BodyB._invMass; + _invIA = BodyA._invI; + _invIB = BodyB._invI; + + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + // Compute the effective masses. + Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + Vector2 d = (cB - cA) + rB - rA; + + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + // Compute motor Jacobian and effective mass. + { + _axis = MathUtils.Mul(qA, LocalXAxis); + _a1 = MathUtils.Cross(d + rA, _axis); + _a2 = MathUtils.Cross(rB, _axis); + + _motorMass = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2; + if (_motorMass > 0.0f) + { + _motorMass = 1.0f / _motorMass; + } + } + + // Prismatic constraint. + { + _perp = MathUtils.Mul(qA, _localYAxisA); + + _s1 = MathUtils.Cross(d + rA, _perp); + _s2 = MathUtils.Cross(rB, _perp); + + float k11 = mA + mB + iA * _s1 * _s1 + iB * _s2 * _s2; + float k12 = iA * _s1 + iB * _s2; + float k13 = iA * _s1 * _a1 + iB * _s2 * _a2; + float k22 = iA + iB; + if (k22 == 0.0f) + { + // For bodies with fixed rotation. + k22 = 1.0f; + } + float k23 = iA * _a1 + iB * _a2; + float k33 = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2; + + _K.ex = new Vector3(k11, k12, k13); + _K.ey = new Vector3(k12, k22, k23); + _K.ez = new Vector3(k13, k23, k33); + } + + // Compute motor and limit terms. + if (_enableLimit) + { + float jointTranslation = Vector2.Dot(_axis, d); + if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) + { + _limitState = LimitState.Equal; + } + else if (jointTranslation <= _lowerTranslation) + { + if (_limitState != LimitState.AtLower) + { + _limitState = LimitState.AtLower; + _impulse.Z = 0.0f; + } + } + else if (jointTranslation >= _upperTranslation) + { + if (_limitState != LimitState.AtUpper) + { + _limitState = LimitState.AtUpper; + _impulse.Z = 0.0f; + } + } + else + { + _limitState = LimitState.Inactive; + _impulse.Z = 0.0f; + } + } + else + { + _limitState = LimitState.Inactive; + _impulse.Z = 0.0f; + } + + if (_enableMotor == false) + { + MotorImpulse = 0.0f; + } + + if (Settings.EnableWarmstarting) + { + // Account for variable time step. + _impulse *= data.step.dtRatio; + MotorImpulse *= data.step.dtRatio; + + Vector2 P = _impulse.X * _perp + (MotorImpulse + _impulse.Z) * _axis; + float LA = _impulse.X * _s1 + _impulse.Y + (MotorImpulse + _impulse.Z) * _a1; + float LB = _impulse.X * _s2 + _impulse.Y + (MotorImpulse + _impulse.Z) * _a2; + + vA -= mA * P; + wA -= iA * LA; + + vB += mB * P; + wB += iB * LB; + } + else + { + _impulse = Vector3.Zero; + MotorImpulse = 0.0f; + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override void SolveVelocityConstraints(ref SolverData data) + { + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + // Solve linear motor constraint. + if (_enableMotor && _limitState != LimitState.Equal) + { + float Cdot = Vector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA; + float impulse = _motorMass * (_motorSpeed - Cdot); + float oldImpulse = MotorImpulse; + float maxImpulse = data.step.dt * _maxMotorForce; + MotorImpulse = MathUtils.Clamp(MotorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = MotorImpulse - oldImpulse; + + Vector2 P = impulse * _axis; + float LA = impulse * _a1; + float LB = impulse * _a2; + + vA -= mA * P; + wA -= iA * LA; + + vB += mB * P; + wB += iB * LB; + } + + Vector2 Cdot1 = new Vector2(); + Cdot1.X = Vector2.Dot(_perp, vB - vA) + _s2 * wB - _s1 * wA; + Cdot1.Y = wB - wA; + + if (_enableLimit && _limitState != LimitState.Inactive) + { + // Solve prismatic and limit constraint in block form. + float Cdot2; + Cdot2 = Vector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA; + Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); + + Vector3 f1 = _impulse; + Vector3 df = _K.Solve33(-Cdot); + _impulse += df; + + if (_limitState == LimitState.AtLower) + { + _impulse.Z = Math.Max(_impulse.Z, 0.0f); + } + else if (_limitState == LimitState.AtUpper) + { + _impulse.Z = Math.Min(_impulse.Z, 0.0f); + } + + // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) + Vector2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vector2(_K.ez.X, _K.ez.Y); + Vector2 f2r = _K.Solve22(b) + new Vector2(f1.X, f1.Y); + _impulse.X = f2r.X; + _impulse.Y = f2r.Y; + + df = _impulse - f1; + + Vector2 P = df.X * _perp + df.Z * _axis; + float LA = df.X * _s1 + df.Y + df.Z * _a1; + float LB = df.X * _s2 + df.Y + df.Z * _a2; + + vA -= mA * P; + wA -= iA * LA; + + vB += mB * P; + wB += iB * LB; + } + else + { + // Limit is inactive, just solve the prismatic constraint in block form. + Vector2 df = _K.Solve22(-Cdot1); + _impulse.X += df.X; + _impulse.Y += df.Y; + + Vector2 P = df.X * _perp; + float LA = df.X * _s1 + df.Y; + float LB = df.X * _s2 + df.Y; + + vA -= mA * P; + wA -= iA * LA; + + vB += mB * P; + wB += iB * LB; + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override bool SolvePositionConstraints(ref SolverData data) + { + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + // Compute fresh Jacobians + Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + Vector2 d = cB + rB - cA - rA; + + Vector2 axis = MathUtils.Mul(qA, LocalXAxis); + float a1 = MathUtils.Cross(d + rA, axis); + float a2 = MathUtils.Cross(rB, axis); + Vector2 perp = MathUtils.Mul(qA, _localYAxisA); + + float s1 = MathUtils.Cross(d + rA, perp); + float s2 = MathUtils.Cross(rB, perp); + + Vector3 impulse; + Vector2 C1 = new Vector2(); + C1.X = Vector2.Dot(perp, d); + C1.Y = aB - aA - ReferenceAngle; + + float linearError = Math.Abs(C1.X); + float angularError = Math.Abs(C1.Y); + + bool active = false; + float C2 = 0.0f; + if (_enableLimit) + { + float translation = Vector2.Dot(axis, d); + if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) + { + // Prevent large angular corrections + C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); + linearError = Math.Max(linearError, Math.Abs(translation)); + active = true; + } + else if (translation <= _lowerTranslation) + { + // Prevent large linear corrections and allow some slop. + C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); + linearError = Math.Max(linearError, _lowerTranslation - translation); + active = true; + } + else if (translation >= _upperTranslation) + { + // Prevent large linear corrections and allow some slop. + C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection); + linearError = Math.Max(linearError, translation - _upperTranslation); + active = true; + } + } + + if (active) + { + float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; + float k12 = iA * s1 + iB * s2; + float k13 = iA * s1 * a1 + iB * s2 * a2; + float k22 = iA + iB; + if (k22 == 0.0f) + { + // For fixed rotation + k22 = 1.0f; + } + float k23 = iA * a1 + iB * a2; + float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2; + + Mat33 K = new Mat33(); + K.ex = new Vector3(k11, k12, k13); + K.ey = new Vector3(k12, k22, k23); + K.ez = new Vector3(k13, k23, k33); + + Vector3 C = new Vector3(); + C.X = C1.X; + C.Y = C1.Y; + C.Z = C2; + + impulse = K.Solve33(-C); + } + else + { + float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; + float k12 = iA * s1 + iB * s2; + float k22 = iA + iB; + if (k22 == 0.0f) + { + k22 = 1.0f; + } + + Mat22 K = new Mat22(); + K.ex = new Vector2(k11, k12); + K.ey = new Vector2(k12, k22); + + Vector2 impulse1 = K.Solve(-C1); + impulse = new Vector3(); + impulse.X = impulse1.X; + impulse.Y = impulse1.Y; + impulse.Z = 0.0f; + } + + Vector2 P = impulse.X * perp + impulse.Z * axis; + float LA = impulse.X * s1 + impulse.Y + impulse.Z * a1; + float LB = impulse.X * s2 + impulse.Y + impulse.Z * a2; + + cA -= mA * P; + aA -= iA * LA; + cB += mB * P; + aB += iB * LB; + + data.positions[_indexA].c = cA; + data.positions[_indexA].a = aA; + data.positions[_indexB].c = cB; + data.positions[_indexB].a = aB; + + return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Dynamics/Joints/PulleyJoint.cs b/Farseer Physics Engine 3.5/Dynamics/Joints/PulleyJoint.cs new file mode 100644 index 000000000..15d73c109 --- /dev/null +++ b/Farseer Physics Engine 3.5/Dynamics/Joints/PulleyJoint.cs @@ -0,0 +1,396 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + // Pulley: + // length1 = norm(p1 - s1) + // length2 = norm(p2 - s2) + // C0 = (length1 + ratio * length2)_initial + // C = C0 - (length1 + ratio * length2) + // u1 = (p1 - s1) / norm(p1 - s1) + // u2 = (p2 - s2) / norm(p2 - s2) + // Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2)) + // J = -[u1 cross(r1, u1) ratio * u2 ratio * cross(r2, u2)] + // K = J * invM * JT + // = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2) + + /// + /// The pulley joint is connected to two bodies and two fixed world points. + /// The pulley supports a ratio such that: + /// + /// Yes, the force transmitted is scaled by the ratio. + /// + /// Warning: the pulley joint can get a bit squirrelly by itself. They often + /// work better when combined with prismatic joints. You should also cover the + /// the anchor points with static shapes to prevent one side from going to zero length. + /// + public class PulleyJoint : Joint + { + // Solver shared + private float _impulse; + + // Solver temp + private int _indexA; + private int _indexB; + private Vector2 _uA; + private Vector2 _uB; + private Vector2 _rA; + private Vector2 _rB; + private Vector2 _localCenterA; + private Vector2 _localCenterB; + private float _invMassA; + private float _invMassB; + private float _invIA; + private float _invIB; + private float _mass; + + internal PulleyJoint() + { + JointType = JointType.Pulley; + } + + /// + /// Constructor for PulleyJoint. + /// + /// The first body. + /// The second body. + /// The anchor on the first body. + /// The anchor on the second body. + /// The world anchor for the first body. + /// The world anchor for the second body. + /// The ratio. + /// Set to true if you are using world coordinates as anchors. + public PulleyJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, Vector2 worldAnchorA, Vector2 worldAnchorB, float ratio, bool useWorldCoordinates = false) + : base(bodyA, bodyB) + { + JointType = JointType.Pulley; + + WorldAnchorA = worldAnchorA; + WorldAnchorB = worldAnchorB; + + if (useWorldCoordinates) + { + LocalAnchorA = BodyA.GetLocalPoint(anchorA); + LocalAnchorB = BodyB.GetLocalPoint(anchorB); + + Vector2 dA = anchorA - worldAnchorA; + LengthA = dA.Length(); + Vector2 dB = anchorB - worldAnchorB; + LengthB = dB.Length(); + } + else + { + LocalAnchorA = anchorA; + LocalAnchorB = anchorB; + + Vector2 dA = anchorA - BodyA.GetLocalPoint(worldAnchorA); + LengthA = dA.Length(); + Vector2 dB = anchorB - BodyB.GetLocalPoint(worldAnchorB); + LengthB = dB.Length(); + } + + Debug.Assert(ratio != 0.0f); + Debug.Assert(ratio > Settings.Epsilon); + + Ratio = ratio; + Constant = LengthA + ratio * LengthB; + _impulse = 0.0f; + } + + /// + /// The local anchor point on BodyA + /// + public Vector2 LocalAnchorA { get; set; } + + /// + /// The local anchor point on BodyB + /// + public Vector2 LocalAnchorB { get; set; } + + /// + /// Get the first world anchor. + /// + /// + public override sealed Vector2 WorldAnchorA { get; set; } + + /// + /// Get the second world anchor. + /// + /// + public override sealed Vector2 WorldAnchorB { get; set; } + + /// + /// Get the current length of the segment attached to body1. + /// + /// + public float LengthA { get; set; } + + /// + /// Get the current length of the segment attached to body2. + /// + /// + public float LengthB { get; set; } + + /// + /// The current length between the anchor point on BodyA and WorldAnchorA + /// + public float CurrentLengthA + { + get + { + Vector2 p = BodyA.GetWorldPoint(LocalAnchorA); + Vector2 s = WorldAnchorA; + Vector2 d = p - s; + return d.Length(); + } + } + + /// + /// The current length between the anchor point on BodyB and WorldAnchorB + /// + public float CurrentLengthB + { + get + { + Vector2 p = BodyB.GetWorldPoint(LocalAnchorB); + Vector2 s = WorldAnchorB; + Vector2 d = p - s; + return d.Length(); + } + } + + /// + /// Get the pulley ratio. + /// + /// + public float Ratio { get; set; } + + //FPE note: Only used for serialization. + internal float Constant { get; set; } + + public override Vector2 GetReactionForce(float invDt) + { + Vector2 P = _impulse * _uB; + return invDt * P; + } + + public override float GetReactionTorque(float invDt) + { + return 0.0f; + } + + internal override void InitVelocityConstraints(ref SolverData data) + { + _indexA = BodyA.IslandIndex; + _indexB = BodyB.IslandIndex; + _localCenterA = BodyA._sweep.LocalCenter; + _localCenterB = BodyB._sweep.LocalCenter; + _invMassA = BodyA._invMass; + _invMassB = BodyB._invMass; + _invIA = BodyA._invI; + _invIB = BodyB._invI; + + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + + // Get the pulley axes. + _uA = cA + _rA - WorldAnchorA; + _uB = cB + _rB - WorldAnchorB; + + float lengthA = _uA.Length(); + float lengthB = _uB.Length(); + + if (lengthA > 10.0f * Settings.LinearSlop) + { + _uA *= 1.0f / lengthA; + } + else + { + _uA = Vector2.Zero; + } + + if (lengthB > 10.0f * Settings.LinearSlop) + { + _uB *= 1.0f / lengthB; + } + else + { + _uB = Vector2.Zero; + } + + // Compute effective mass. + float ruA = MathUtils.Cross(_rA, _uA); + float ruB = MathUtils.Cross(_rB, _uB); + + float mA = _invMassA + _invIA * ruA * ruA; + float mB = _invMassB + _invIB * ruB * ruB; + + _mass = mA + Ratio * Ratio * mB; + + if (_mass > 0.0f) + { + _mass = 1.0f / _mass; + } + + if (Settings.EnableWarmstarting) + { + // Scale impulses to support variable time steps. + _impulse *= data.step.dtRatio; + + // Warm starting. + Vector2 PA = -(_impulse) * _uA; + Vector2 PB = (-Ratio * _impulse) * _uB; + + vA += _invMassA * PA; + wA += _invIA * MathUtils.Cross(_rA, PA); + vB += _invMassB * PB; + wB += _invIB * MathUtils.Cross(_rB, PB); + } + else + { + _impulse = 0.0f; + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override void SolveVelocityConstraints(ref SolverData data) + { + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + Vector2 vpA = vA + MathUtils.Cross(wA, _rA); + Vector2 vpB = vB + MathUtils.Cross(wB, _rB); + + float Cdot = -Vector2.Dot(_uA, vpA) - Ratio * Vector2.Dot(_uB, vpB); + float impulse = -_mass * Cdot; + _impulse += impulse; + + Vector2 PA = -impulse * _uA; + Vector2 PB = -Ratio * impulse * _uB; + vA += _invMassA * PA; + wA += _invIA * MathUtils.Cross(_rA, PA); + vB += _invMassB * PB; + wB += _invIB * MathUtils.Cross(_rB, PB); + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override bool SolvePositionConstraints(ref SolverData data) + { + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + + // Get the pulley axes. + Vector2 uA = cA + rA - WorldAnchorA; + Vector2 uB = cB + rB - WorldAnchorB; + + float lengthA = uA.Length(); + float lengthB = uB.Length(); + + if (lengthA > 10.0f * Settings.LinearSlop) + { + uA *= 1.0f / lengthA; + } + else + { + uA = Vector2.Zero; + } + + if (lengthB > 10.0f * Settings.LinearSlop) + { + uB *= 1.0f / lengthB; + } + else + { + uB = Vector2.Zero; + } + + // Compute effective mass. + float ruA = MathUtils.Cross(rA, uA); + float ruB = MathUtils.Cross(rB, uB); + + float mA = _invMassA + _invIA * ruA * ruA; + float mB = _invMassB + _invIB * ruB * ruB; + + float mass = mA + Ratio * Ratio * mB; + + if (mass > 0.0f) + { + mass = 1.0f / mass; + } + + float C = Constant - lengthA - Ratio * lengthB; + float linearError = Math.Abs(C); + + float impulse = -mass * C; + + Vector2 PA = -impulse * uA; + Vector2 PB = -Ratio * impulse * uB; + + cA += _invMassA * PA; + aA += _invIA * MathUtils.Cross(rA, PA); + cB += _invMassB * PB; + aB += _invIB * MathUtils.Cross(rB, PB); + + data.positions[_indexA].c = cA; + data.positions[_indexA].a = aA; + data.positions[_indexB].c = cB; + data.positions[_indexB].a = aB; + + return linearError < Settings.LinearSlop; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Dynamics/Joints/RevoluteJoint.cs b/Farseer Physics Engine 3.5/Dynamics/Joints/RevoluteJoint.cs new file mode 100644 index 000000000..fa93c90db --- /dev/null +++ b/Farseer Physics Engine 3.5/Dynamics/Joints/RevoluteJoint.cs @@ -0,0 +1,619 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + /// + /// A revolute joint constrains to bodies to share a common point while they + /// are free to rotate about the point. The relative rotation about the shared + /// point is the joint angle. You can limit the relative rotation with + /// a joint limit that specifies a lower and upper angle. You can use a motor + /// to drive the relative rotation about the shared point. A maximum motor torque + /// is provided so that infinite forces are not generated. + /// + public class RevoluteJoint : Joint + { + // Solver shared + private Vector3 _impulse; + private float _motorImpulse; + + private bool _enableMotor; + private float _maxMotorTorque; + private float _motorSpeed; + + private bool _enableLimit; + private float _referenceAngle; + private float _lowerAngle; + private float _upperAngle; + + // Solver temp + private int _indexA; + private int _indexB; + private Vector2 _rA; + private Vector2 _rB; + private Vector2 _localCenterA; + private Vector2 _localCenterB; + private float _invMassA; + private float _invMassB; + private float _invIA; + private float _invIB; + private Mat33 _mass; // effective mass for point-to-point constraint. + private float _motorMass; // effective mass for motor/limit angular constraint. + private LimitState _limitState; + + internal RevoluteJoint() + { + JointType = JointType.Revolute; + } + + /// + /// Constructor of RevoluteJoint. + /// + /// The first body. + /// The second body. + /// The first body anchor. + /// The second anchor. + /// Set to true if you are using world coordinates as anchors. + public RevoluteJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false) + : base(bodyA, bodyB) + { + JointType = JointType.Revolute; + + if (useWorldCoordinates) + { + LocalAnchorA = BodyA.GetLocalPoint(anchorA); + LocalAnchorB = BodyB.GetLocalPoint(anchorB); + } + else + { + LocalAnchorA = anchorA; + LocalAnchorB = anchorB; + } + + ReferenceAngle = BodyB.Rotation - BodyA.Rotation; + + _impulse = Vector3.Zero; + _limitState = LimitState.Inactive; + } + + /// + /// Constructor of RevoluteJoint. + /// + /// The first body. + /// The second body. + /// The shared anchor. + /// + public RevoluteJoint(Body bodyA, Body bodyB, Vector2 anchor, bool useWorldCoordinates = false) + : this(bodyA, bodyB, anchor, anchor, useWorldCoordinates) + { + } + + /// + /// The local anchor point on BodyA + /// + public Vector2 LocalAnchorA { get; set; } + + /// + /// The local anchor point on BodyB + /// + public Vector2 LocalAnchorB { get; set; } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + set { LocalAnchorA = BodyA.GetLocalPoint(value); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { LocalAnchorB = BodyB.GetLocalPoint(value); } + } + + /// + /// The referance angle computed as BodyB angle minus BodyA angle. + /// + public float ReferenceAngle + { + get { return _referenceAngle; } + set + { + WakeBodies(); + _referenceAngle = value; + } + } + + /// + /// Get the current joint angle in radians. + /// + public float JointAngle + { + get { return BodyB._sweep.A - BodyA._sweep.A - ReferenceAngle; } + } + + /// + /// Get the current joint angle speed in radians per second. + /// + public float JointSpeed + { + get { return BodyB._angularVelocity - BodyA._angularVelocity; } + } + + /// + /// Is the joint limit enabled? + /// + /// true if [limit enabled]; otherwise, false. + public bool LimitEnabled + { + get { return _enableLimit; } + set + { + if (_enableLimit != value) + { + WakeBodies(); + _enableLimit = value; + _impulse.Z = 0.0f; + } + } + } + + /// + /// Get the lower joint limit in radians. + /// + public float LowerLimit + { + get { return _lowerAngle; } + set + { + if (_lowerAngle != value) + { + WakeBodies(); + _lowerAngle = value; + _impulse.Z = 0.0f; + } + } + } + + /// + /// Get the upper joint limit in radians. + /// + public float UpperLimit + { + get { return _upperAngle; } + set + { + if (_upperAngle != value) + { + WakeBodies(); + _upperAngle = value; + _impulse.Z = 0.0f; + } + } + } + + /// + /// Set the joint limits, usually in meters. + /// + /// The lower limit + /// The upper limit + public void SetLimits(float lower, float upper) + { + if (lower != _lowerAngle || upper != _upperAngle) + { + WakeBodies(); + _upperAngle = upper; + _lowerAngle = lower; + _impulse.Z = 0.0f; + } + } + + /// + /// Is the joint motor enabled? + /// + /// true if [motor enabled]; otherwise, false. + public bool MotorEnabled + { + get { return _enableMotor; } + set + { + WakeBodies(); + _enableMotor = value; + } + } + + /// + /// Get or set the motor speed in radians per second. + /// + public float MotorSpeed + { + set + { + WakeBodies(); + _motorSpeed = value; + } + get { return _motorSpeed; } + } + + /// + /// Get or set the maximum motor torque, usually in N-m. + /// + public float MaxMotorTorque + { + set + { + WakeBodies(); + _maxMotorTorque = value; + } + get { return _maxMotorTorque; } + } + + /// + /// Get or set the current motor impulse, usually in N-m. + /// + public float MotorImpulse + { + get { return _motorImpulse; } + set + { + WakeBodies(); + _motorImpulse = value; + } + } + + /// + /// Gets the motor torque in N-m. + /// + /// The inverse delta time + public float GetMotorTorque(float invDt) + { + return invDt * _motorImpulse; + } + + public override Vector2 GetReactionForce(float invDt) + { + Vector2 p = new Vector2(_impulse.X, _impulse.Y); + return invDt * p; + } + + public override float GetReactionTorque(float invDt) + { + return invDt * _impulse.Z; + } + + internal override void InitVelocityConstraints(ref SolverData data) + { + _indexA = BodyA.IslandIndex; + _indexB = BodyB.IslandIndex; + _localCenterA = BodyA._sweep.LocalCenter; + _localCenterB = BodyB._sweep.LocalCenter; + _invMassA = BodyA._invMass; + _invMassB = BodyB._invMass; + _invIA = BodyA._invI; + _invIB = BodyB._invI; + + float aA = data.positions[_indexA].a; + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + + float aB = data.positions[_indexB].a; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + + // J = [-I -r1_skew I r2_skew] + // [ 0 -1 0 1] + // r_skew = [-ry; rx] + + // Matlab + // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] + // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] + // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] + + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + bool fixedRotation = (iA + iB == 0.0f); + + _mass.ex.X = mA + mB + _rA.Y * _rA.Y * iA + _rB.Y * _rB.Y * iB; + _mass.ey.X = -_rA.Y * _rA.X * iA - _rB.Y * _rB.X * iB; + _mass.ez.X = -_rA.Y * iA - _rB.Y * iB; + _mass.ex.Y = _mass.ey.X; + _mass.ey.Y = mA + mB + _rA.X * _rA.X * iA + _rB.X * _rB.X * iB; + _mass.ez.Y = _rA.X * iA + _rB.X * iB; + _mass.ex.Z = _mass.ez.X; + _mass.ey.Z = _mass.ez.Y; + _mass.ez.Z = iA + iB; + + _motorMass = iA + iB; + if (_motorMass > 0.0f) + { + _motorMass = 1.0f / _motorMass; + } + + if (_enableMotor == false || fixedRotation) + { + _motorImpulse = 0.0f; + } + + if (_enableLimit && fixedRotation == false) + { + float jointAngle = aB - aA - ReferenceAngle; + if (Math.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop) + { + _limitState = LimitState.Equal; + } + else if (jointAngle <= _lowerAngle) + { + if (_limitState != LimitState.AtLower) + { + _impulse.Z = 0.0f; + } + _limitState = LimitState.AtLower; + } + else if (jointAngle >= _upperAngle) + { + if (_limitState != LimitState.AtUpper) + { + _impulse.Z = 0.0f; + } + _limitState = LimitState.AtUpper; + } + else + { + _limitState = LimitState.Inactive; + _impulse.Z = 0.0f; + } + } + else + { + _limitState = LimitState.Inactive; + } + + if (Settings.EnableWarmstarting) + { + // Scale impulses to support a variable time step. + _impulse *= data.step.dtRatio; + _motorImpulse *= data.step.dtRatio; + + Vector2 P = new Vector2(_impulse.X, _impulse.Y); + + vA -= mA * P; + wA -= iA * (MathUtils.Cross(_rA, P) + MotorImpulse + _impulse.Z); + + vB += mB * P; + wB += iB * (MathUtils.Cross(_rB, P) + MotorImpulse + _impulse.Z); + } + else + { + _impulse = Vector3.Zero; + _motorImpulse = 0.0f; + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override void SolveVelocityConstraints(ref SolverData data) + { + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + bool fixedRotation = (iA + iB == 0.0f); + + // Solve motor constraint. + if (_enableMotor && _limitState != LimitState.Equal && fixedRotation == false) + { + float Cdot = wB - wA - _motorSpeed; + float impulse = _motorMass * (-Cdot); + float oldImpulse = _motorImpulse; + float maxImpulse = data.step.dt * _maxMotorTorque; + _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = _motorImpulse - oldImpulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + // Solve limit constraint. + if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false) + { + Vector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA); + float Cdot2 = wB - wA; + Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); + + Vector3 impulse = -_mass.Solve33(Cdot); + + if (_limitState == LimitState.Equal) + { + _impulse += impulse; + } + else if (_limitState == LimitState.AtLower) + { + float newImpulse = _impulse.Z + impulse.Z; + if (newImpulse < 0.0f) + { + Vector2 rhs = -Cdot1 + _impulse.Z * new Vector2(_mass.ez.X, _mass.ez.Y); + Vector2 reduced = _mass.Solve22(rhs); + impulse.X = reduced.X; + impulse.Y = reduced.Y; + impulse.Z = -_impulse.Z; + _impulse.X += reduced.X; + _impulse.Y += reduced.Y; + _impulse.Z = 0.0f; + } + else + { + _impulse += impulse; + } + } + else if (_limitState == LimitState.AtUpper) + { + float newImpulse = _impulse.Z + impulse.Z; + if (newImpulse > 0.0f) + { + Vector2 rhs = -Cdot1 + _impulse.Z * new Vector2(_mass.ez.X, _mass.ez.Y); + Vector2 reduced = _mass.Solve22(rhs); + impulse.X = reduced.X; + impulse.Y = reduced.Y; + impulse.Z = -_impulse.Z; + _impulse.X += reduced.X; + _impulse.Y += reduced.Y; + _impulse.Z = 0.0f; + } + else + { + _impulse += impulse; + } + } + + Vector2 P = new Vector2(impulse.X, impulse.Y); + + vA -= mA * P; + wA -= iA * (MathUtils.Cross(_rA, P) + impulse.Z); + + vB += mB * P; + wB += iB * (MathUtils.Cross(_rB, P) + impulse.Z); + } + else + { + // Solve point-to-point constraint + Vector2 Cdot = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA); + Vector2 impulse = _mass.Solve22(-Cdot); + + _impulse.X += impulse.X; + _impulse.Y += impulse.Y; + + vA -= mA * impulse; + wA -= iA * MathUtils.Cross(_rA, impulse); + + vB += mB * impulse; + wB += iB * MathUtils.Cross(_rB, impulse); + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override bool SolvePositionConstraints(ref SolverData data) + { + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + float angularError = 0.0f; + float positionError; + + bool fixedRotation = (_invIA + _invIB == 0.0f); + + // Solve angular limit constraint. + if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false) + { + float angle = aB - aA - ReferenceAngle; + float limitImpulse = 0.0f; + + if (_limitState == LimitState.Equal) + { + // Prevent large angular corrections + float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection); + limitImpulse = -_motorMass * C; + angularError = Math.Abs(C); + } + else if (_limitState == LimitState.AtLower) + { + float C = angle - _lowerAngle; + angularError = -C; + + // Prevent large angular corrections and allow some slop. + C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f); + limitImpulse = -_motorMass * C; + } + else if (_limitState == LimitState.AtUpper) + { + float C = angle - _upperAngle; + angularError = C; + + // Prevent large angular corrections and allow some slop. + C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection); + limitImpulse = -_motorMass * C; + } + + aA -= _invIA * limitImpulse; + aB += _invIB * limitImpulse; + } + + // Solve point-to-point constraint. + { + qA.Set(aA); + qB.Set(aB); + Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + + Vector2 C = cB + rB - cA - rA; + positionError = C.Length(); + + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + Mat22 K = new Mat22(); + K.ex.X = mA + mB + iA * rA.Y * rA.Y + iB * rB.Y * rB.Y; + K.ex.Y = -iA * rA.X * rA.Y - iB * rB.X * rB.Y; + K.ey.X = K.ex.Y; + K.ey.Y = mA + mB + iA * rA.X * rA.X + iB * rB.X * rB.X; + + Vector2 impulse = -K.Solve(C); + + cA -= mA * impulse; + aA -= iA * MathUtils.Cross(rA, impulse); + + cB += mB * impulse; + aB += iB * MathUtils.Cross(rB, impulse); + } + + data.positions[_indexA].c = cA; + data.positions[_indexA].a = aA; + data.positions[_indexB].c = cB; + data.positions[_indexB].a = aB; + + return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Dynamics/Joints/RopeJoint.cs b/Farseer Physics Engine 3.5/Dynamics/Joints/RopeJoint.cs new file mode 100644 index 000000000..8bcd00968 --- /dev/null +++ b/Farseer Physics Engine 3.5/Dynamics/Joints/RopeJoint.cs @@ -0,0 +1,291 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + // Limit: + // C = norm(pB - pA) - L + // u = (pB - pA) / norm(pB - pA) + // Cdot = dot(u, vB + cross(wB, rB) - vA - cross(wA, rA)) + // J = [-u -cross(rA, u) u cross(rB, u)] + // K = J * invM * JT + // = invMassA + invIA * cross(rA, u)^2 + invMassB + invIB * cross(rB, u)^2 + + /// + /// A rope joint enforces a maximum distance between two points on two bodies. It has no other effect. + /// It can be used on ropes that are made up of several connected bodies, and if there is a need to support a heavy body. + /// This joint is used for stabiliation of heavy objects on soft constraint joints. + /// + /// Warning: if you attempt to change the maximum length during the simulation you will get some non-physical behavior. + /// Use the DistanceJoint instead if you want to dynamically control the length. + /// + public class RopeJoint : Joint + { + // Solver shared + private float _impulse; + private float _length; + + // Solver temp + private int _indexA; + private int _indexB; + private Vector2 _localCenterA; + private Vector2 _localCenterB; + private float _invMassA; + private float _invMassB; + private float _invIA; + private float _invIB; + private float _mass; + private Vector2 _rA, _rB; + private Vector2 _u; + + internal RopeJoint() + { + JointType = JointType.Rope; + } + + /// + /// Constructor for RopeJoint. + /// + /// The first body + /// The second body + /// The anchor on the first body + /// The anchor on the second body + /// Set to true if you are using world coordinates as anchors. + public RopeJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false) + : base(bodyA, bodyB) + { + JointType = JointType.Rope; + + if (useWorldCoordinates) + { + LocalAnchorA = bodyA.GetLocalPoint(anchorA); + LocalAnchorB = bodyB.GetLocalPoint(anchorB); + } + else + { + LocalAnchorA = anchorA; + LocalAnchorB = anchorB; + } + + //FPE feature: Setting default MaxLength + Vector2 d = WorldAnchorB - WorldAnchorA; + MaxLength = d.Length(); + } + + /// + /// The local anchor point on BodyA + /// + public Vector2 LocalAnchorA { get; set; } + + /// + /// The local anchor point on BodyB + /// + public Vector2 LocalAnchorB { get; set; } + + public override sealed Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + set { LocalAnchorA = BodyA.GetLocalPoint(value); } + } + + public override sealed Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { LocalAnchorB = BodyB.GetLocalPoint(value); } + } + + /// + /// Get or set the maximum length of the rope. + /// By default, it is the distance between the two anchor points. + /// + public float MaxLength { get; set; } + + /// + /// Gets the state of the joint. + /// + public LimitState State { get; private set; } + + public override Vector2 GetReactionForce(float invDt) + { + return (invDt * _impulse) * _u; + } + + public override float GetReactionTorque(float invDt) + { + return 0; + } + + internal override void InitVelocityConstraints(ref SolverData data) + { + _indexA = BodyA.IslandIndex; + _indexB = BodyB.IslandIndex; + _localCenterA = BodyA._sweep.LocalCenter; + _localCenterB = BodyB._sweep.LocalCenter; + _invMassA = BodyA._invMass; + _invMassB = BodyB._invMass; + _invIA = BodyA._invI; + _invIB = BodyB._invI; + + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + _u = cB + _rB - cA - _rA; + + _length = _u.Length(); + + float C = _length - MaxLength; + if (C > 0.0f) + { + State = LimitState.AtUpper; + } + else + { + State = LimitState.Inactive; + } + + if (_length > Settings.LinearSlop) + { + _u *= 1.0f / _length; + } + else + { + _u = Vector2.Zero; + _mass = 0.0f; + _impulse = 0.0f; + return; + } + + // Compute effective mass. + float crA = MathUtils.Cross(_rA, _u); + float crB = MathUtils.Cross(_rB, _u); + float invMass = _invMassA + _invIA * crA * crA + _invMassB + _invIB * crB * crB; + + _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; + + if (Settings.EnableWarmstarting) + { + // Scale the impulse to support a variable time step. + _impulse *= data.step.dtRatio; + + Vector2 P = _impulse * _u; + vA -= _invMassA * P; + wA -= _invIA * MathUtils.Cross(_rA, P); + vB += _invMassB * P; + wB += _invIB * MathUtils.Cross(_rB, P); + } + else + { + _impulse = 0.0f; + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override void SolveVelocityConstraints(ref SolverData data) + { + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + // Cdot = dot(u, v + cross(w, r)) + Vector2 vpA = vA + MathUtils.Cross(wA, _rA); + Vector2 vpB = vB + MathUtils.Cross(wB, _rB); + float C = _length - MaxLength; + float Cdot = Vector2.Dot(_u, vpB - vpA); + + // Predictive constraint. + if (C < 0.0f) + { + Cdot += data.step.inv_dt * C; + } + + float impulse = -_mass * Cdot; + float oldImpulse = _impulse; + _impulse = Math.Min(0.0f, _impulse + impulse); + impulse = _impulse - oldImpulse; + + Vector2 P = impulse * _u; + vA -= _invMassA * P; + wA -= _invIA * MathUtils.Cross(_rA, P); + vB += _invMassB * P; + wB += _invIB * MathUtils.Cross(_rB, P); + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override bool SolvePositionConstraints(ref SolverData data) + { + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + Vector2 u = cB + rB - cA - rA; + + float length = u.Length(); u.Normalize(); + float C = length - MaxLength; + + C = MathUtils.Clamp(C, 0.0f, Settings.MaxLinearCorrection); + + float impulse = -_mass * C; + Vector2 P = impulse * u; + + cA -= _invMassA * P; + aA -= _invIA * MathUtils.Cross(rA, P); + cB += _invMassB * P; + aB += _invIB * MathUtils.Cross(rB, P); + + data.positions[_indexA].c = cA; + data.positions[_indexA].a = aA; + data.positions[_indexB].c = cB; + data.positions[_indexB].a = aB; + + return length - MaxLength < Settings.LinearSlop; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Dynamics/Joints/WeldJoint.cs b/Farseer Physics Engine 3.5/Dynamics/Joints/WeldJoint.cs new file mode 100644 index 000000000..1975d0cd2 --- /dev/null +++ b/Farseer Physics Engine 3.5/Dynamics/Joints/WeldJoint.cs @@ -0,0 +1,388 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + // Point-to-point constraint + // C = p2 - p1 + // Cdot = v2 - v1 + // = v2 + cross(w2, r2) - v1 - cross(w1, r1) + // J = [-I -r1_skew I r2_skew ] + // Identity used: + // w k % (rx i + ry j) = w * (-ry i + rx j) + + // Angle constraint + // C = angle2 - angle1 - referenceAngle + // Cdot = w2 - w1 + // J = [0 0 -1 0 0 1] + // K = invI1 + invI2 + + /// + /// A weld joint essentially glues two bodies together. A weld joint may + /// distort somewhat because the island constraint solver is approximate. + /// + /// The joint is soft constraint based, which means the two bodies will move + /// relative to each other, when a force is applied. To combine two bodies + /// in a rigid fashion, combine the fixtures to a single body instead. + /// + public class WeldJoint : Joint + { + // Solver shared + private Vector3 _impulse; + private float _gamma; + private float _bias; + + // Solver temp + private int _indexA; + private int _indexB; + private Vector2 _rA; + private Vector2 _rB; + private Vector2 _localCenterA; + private Vector2 _localCenterB; + private float _invMassA; + private float _invMassB; + private float _invIA; + private float _invIB; + private Mat33 _mass; + + internal WeldJoint() + { + JointType = JointType.Weld; + } + + /// + /// You need to specify an anchor point where they are attached. + /// The position of the anchor point is important for computing the reaction torque. + /// + /// The first body + /// The second body + /// The first body anchor. + /// The second body anchor. + /// Set to true if you are using world coordinates as anchors. + public WeldJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false) + : base(bodyA, bodyB) + { + JointType = JointType.Weld; + + if (useWorldCoordinates) + { + LocalAnchorA = bodyA.GetLocalPoint(anchorA); + LocalAnchorB = bodyB.GetLocalPoint(anchorB); + } + else + { + LocalAnchorA = anchorA; + LocalAnchorB = anchorB; + } + + ReferenceAngle = BodyB.Rotation - BodyA.Rotation; + } + + /// + /// The local anchor point on BodyA + /// + public Vector2 LocalAnchorA { get; set; } + + /// + /// The local anchor point on BodyB + /// + public Vector2 LocalAnchorB { get; set; } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + set { LocalAnchorA = BodyA.GetLocalPoint(value); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { LocalAnchorB = BodyB.GetLocalPoint(value); } + } + + /// + /// The bodyB angle minus bodyA angle in the reference state (radians). + /// + public float ReferenceAngle { get; set; } + + /// + /// The frequency of the joint. A higher frequency means a stiffer joint, but + /// a too high value can cause the joint to oscillate. + /// Default is 0, which means the joint does no spring calculations. + /// + public float FrequencyHz { get; set; } + + /// + /// The damping on the joint. The damping is only used when + /// the joint has a frequency (> 0). A higher value means more damping. + /// + public float DampingRatio { get; set; } + + public override Vector2 GetReactionForce(float invDt) + { + return invDt * new Vector2(_impulse.X, _impulse.Y); + } + + public override float GetReactionTorque(float invDt) + { + return invDt * _impulse.Z; + } + + internal override void InitVelocityConstraints(ref SolverData data) + { + _indexA = BodyA.IslandIndex; + _indexB = BodyB.IslandIndex; + _localCenterA = BodyA._sweep.LocalCenter; + _localCenterB = BodyB._sweep.LocalCenter; + _invMassA = BodyA._invMass; + _invMassB = BodyB._invMass; + _invIA = BodyA._invI; + _invIB = BodyB._invI; + + float aA = data.positions[_indexA].a; + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + + float aB = data.positions[_indexB].a; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + + // J = [-I -r1_skew I r2_skew] + // [ 0 -1 0 1] + // r_skew = [-ry; rx] + + // Matlab + // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] + // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] + // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] + + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + Mat33 K = new Mat33(); + K.ex.X = mA + mB + _rA.Y * _rA.Y * iA + _rB.Y * _rB.Y * iB; + K.ey.X = -_rA.Y * _rA.X * iA - _rB.Y * _rB.X * iB; + K.ez.X = -_rA.Y * iA - _rB.Y * iB; + K.ex.Y = K.ey.X; + K.ey.Y = mA + mB + _rA.X * _rA.X * iA + _rB.X * _rB.X * iB; + K.ez.Y = _rA.X * iA + _rB.X * iB; + K.ex.Z = K.ez.X; + K.ey.Z = K.ez.Y; + K.ez.Z = iA + iB; + + if (FrequencyHz > 0.0f) + { + K.GetInverse22(ref _mass); + + float invM = iA + iB; + float m = invM > 0.0f ? 1.0f / invM : 0.0f; + + float C = aB - aA - ReferenceAngle; + + // Frequency + float omega = 2.0f * Settings.Pi * FrequencyHz; + + // Damping coefficient + float d = 2.0f * m * DampingRatio * omega; + + // Spring stiffness + float k = m * omega * omega; + + // magic formulas + float h = data.step.dt; + _gamma = h * (d + h * k); + _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f; + _bias = C * h * k * _gamma; + + invM += _gamma; + _mass.ez.Z = invM != 0.0f ? 1.0f / invM : 0.0f; + } + else + { + K.GetSymInverse33(ref _mass); + _gamma = 0.0f; + _bias = 0.0f; + } + + if (Settings.EnableWarmstarting) + { + // Scale impulses to support a variable time step. + _impulse *= data.step.dtRatio; + + Vector2 P = new Vector2(_impulse.X, _impulse.Y); + + vA -= mA * P; + wA -= iA * (MathUtils.Cross(_rA, P) + _impulse.Z); + + vB += mB * P; + wB += iB * (MathUtils.Cross(_rB, P) + _impulse.Z); + } + else + { + _impulse = Vector3.Zero; + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override void SolveVelocityConstraints(ref SolverData data) + { + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + if (FrequencyHz > 0.0f) + { + float Cdot2 = wB - wA; + + float impulse2 = -_mass.ez.Z * (Cdot2 + _bias + _gamma * _impulse.Z); + _impulse.Z += impulse2; + + wA -= iA * impulse2; + wB += iB * impulse2; + + Vector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA); + + Vector2 impulse1 = -MathUtils.Mul22(_mass, Cdot1); + _impulse.X += impulse1.X; + _impulse.Y += impulse1.Y; + + Vector2 P = impulse1; + + vA -= mA * P; + wA -= iA * MathUtils.Cross(_rA, P); + + vB += mB * P; + wB += iB * MathUtils.Cross(_rB, P); + } + else + { + Vector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA); + float Cdot2 = wB - wA; + Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); + + Vector3 impulse = -MathUtils.Mul(_mass, Cdot); + _impulse += impulse; + + Vector2 P = new Vector2(impulse.X, impulse.Y); + + vA -= mA * P; + wA -= iA * (MathUtils.Cross(_rA, P) + impulse.Z); + + vB += mB * P; + wB += iB * (MathUtils.Cross(_rB, P) + impulse.Z); + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override bool SolvePositionConstraints(ref SolverData data) + { + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + + float positionError, angularError; + + Mat33 K = new Mat33(); + K.ex.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB; + K.ey.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB; + K.ez.X = -rA.Y * iA - rB.Y * iB; + K.ex.Y = K.ey.X; + K.ey.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB; + K.ez.Y = rA.X * iA + rB.X * iB; + K.ex.Z = K.ez.X; + K.ey.Z = K.ez.Y; + K.ez.Z = iA + iB; + + if (FrequencyHz > 0.0f) + { + Vector2 C1 = cB + rB - cA - rA; + + positionError = C1.Length(); + angularError = 0.0f; + + Vector2 P = -K.Solve22(C1); + + cA -= mA * P; + aA -= iA * MathUtils.Cross(rA, P); + + cB += mB * P; + aB += iB * MathUtils.Cross(rB, P); + } + else + { + Vector2 C1 = cB + rB - cA - rA; + float C2 = aB - aA - ReferenceAngle; + + positionError = C1.Length(); + angularError = Math.Abs(C2); + + Vector3 C = new Vector3(C1.X, C1.Y, C2); + + Vector3 impulse = -K.Solve33(C); + Vector2 P = new Vector2(impulse.X, impulse.Y); + + cA -= mA * P; + aA -= iA * (MathUtils.Cross(rA, P) + impulse.Z); + + cB += mB * P; + aB += iB * (MathUtils.Cross(rB, P) + impulse.Z); + } + + data.positions[_indexA].c = cA; + data.positions[_indexA].a = aA; + data.positions[_indexB].c = cB; + data.positions[_indexB].a = aB; + + return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Dynamics/Joints/WheelJoint.cs b/Farseer Physics Engine 3.5/Dynamics/Joints/WheelJoint.cs new file mode 100644 index 000000000..9e5a5aefe --- /dev/null +++ b/Farseer Physics Engine 3.5/Dynamics/Joints/WheelJoint.cs @@ -0,0 +1,513 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + // Linear constraint (point-to-line) + // d = pB - pA = xB + rB - xA - rA + // C = dot(ay, d) + // Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA)) + // = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB) + // J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)] + + // Spring linear constraint + // C = dot(ax, d) + // Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB) + // J = [-ax -cross(d+rA, ax) ax cross(rB, ax)] + + // Motor rotational constraint + // Cdot = wB - wA + // J = [0 0 -1 0 0 1] + + /// + /// A wheel joint. This joint provides two degrees of freedom: translation + /// along an axis fixed in bodyA and rotation in the plane. You can use a + /// joint limit to restrict the range of motion and a joint motor to drive + /// the rotation or to model rotational friction. + /// This joint is designed for vehicle suspensions. + /// + public class WheelJoint : Joint + { + // Solver shared + private Vector2 _localYAxis; + + private float _impulse; + private float _motorImpulse; + private float _springImpulse; + + private float _maxMotorTorque; + private float _motorSpeed; + private bool _enableMotor; + + // Solver temp + private int _indexA; + private int _indexB; + private Vector2 _localCenterA; + private Vector2 _localCenterB; + private float _invMassA; + private float _invMassB; + private float _invIA; + private float _invIB; + + private Vector2 _ax, _ay; + private float _sAx, _sBx; + private float _sAy, _sBy; + + private float _mass; + private float _motorMass; + private float _springMass; + + private float _bias; + private float _gamma; + private Vector2 _axis; + + internal WheelJoint() + { + JointType = JointType.Wheel; + } + + /// + /// Constructor for WheelJoint + /// + /// The first body + /// The second body + /// The anchor point + /// The axis + /// Set to true if you are using world coordinates as anchors. + public WheelJoint(Body bodyA, Body bodyB, Vector2 anchor, Vector2 axis, bool useWorldCoordinates = false) + : base(bodyA, bodyB) + { + JointType = JointType.Wheel; + + if (useWorldCoordinates) + { + LocalAnchorA = bodyA.GetLocalPoint(anchor); + LocalAnchorB = bodyB.GetLocalPoint(anchor); + } + else + { + LocalAnchorA = bodyA.GetLocalPoint(bodyB.GetWorldPoint(anchor)); + LocalAnchorB = anchor; + } + + Axis = axis; //FPE only: We maintain the original value as it is supposed to. + } + + /// + /// The local anchor point on BodyA + /// + public Vector2 LocalAnchorA { get; set; } + + /// + /// The local anchor point on BodyB + /// + public Vector2 LocalAnchorB { get; set; } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + set { LocalAnchorA = BodyA.GetLocalPoint(value); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { LocalAnchorB = BodyB.GetLocalPoint(value); } + } + + /// + /// The axis at which the suspension moves. + /// + public Vector2 Axis + { + get { return _axis; } + set + { + _axis = value; + LocalXAxis = BodyA.GetLocalVector(_axis); + _localYAxis = MathUtils.Cross(1.0f, LocalXAxis); + } + } + + /// + /// The axis in local coordinates relative to BodyA + /// + public Vector2 LocalXAxis { get; private set; } + + /// + /// The desired motor speed in radians per second. + /// + public float MotorSpeed + { + get { return _motorSpeed; } + set + { + WakeBodies(); + _motorSpeed = value; + } + } + + /// + /// The maximum motor torque, usually in N-m. + /// + public float MaxMotorTorque + { + get { return _maxMotorTorque; } + set + { + WakeBodies(); + _maxMotorTorque = value; + } + } + + /// + /// Suspension frequency, zero indicates no suspension + /// + public float Frequency { get; set; } + + /// + /// Suspension damping ratio, one indicates critical damping + /// + public float DampingRatio { get; set; } + + /// + /// Gets the translation along the axis + /// + public float JointTranslation + { + get + { + Body bA = BodyA; + Body bB = BodyB; + + Vector2 pA = bA.GetWorldPoint(LocalAnchorA); + Vector2 pB = bB.GetWorldPoint(LocalAnchorB); + Vector2 d = pB - pA; + Vector2 axis = bA.GetWorldVector(LocalXAxis); + + float translation = Vector2.Dot(d, axis); + return translation; + } + } + + /// + /// Gets the angular velocity of the joint + /// + public float JointSpeed + { + get + { + float wA = BodyA.AngularVelocity; + float wB = BodyB.AngularVelocity; + return wB - wA; + } + } + + /// + /// Enable/disable the joint motor. + /// + public bool MotorEnabled + { + get { return _enableMotor; } + set + { + WakeBodies(); + _enableMotor = value; + } + } + + /// + /// Gets the torque of the motor + /// + /// inverse delta time + public float GetMotorTorque(float invDt) + { + return invDt * _motorImpulse; + } + + public override Vector2 GetReactionForce(float invDt) + { + return invDt * (_impulse * _ay + _springImpulse * _ax); + } + + public override float GetReactionTorque(float invDt) + { + return invDt * _motorImpulse; + } + + internal override void InitVelocityConstraints(ref SolverData data) + { + _indexA = BodyA.IslandIndex; + _indexB = BodyB.IslandIndex; + _localCenterA = BodyA._sweep.LocalCenter; + _localCenterB = BodyB._sweep.LocalCenter; + _invMassA = BodyA._invMass; + _invMassB = BodyB._invMass; + _invIA = BodyA._invI; + _invIB = BodyB._invI; + + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + // Compute the effective masses. + Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + Vector2 d1 = cB + rB - cA - rA; + + // Point to line constraint + { + _ay = MathUtils.Mul(qA, _localYAxis); + _sAy = MathUtils.Cross(d1 + rA, _ay); + _sBy = MathUtils.Cross(rB, _ay); + + _mass = mA + mB + iA * _sAy * _sAy + iB * _sBy * _sBy; + + if (_mass > 0.0f) + { + _mass = 1.0f / _mass; + } + } + + // Spring constraint + _springMass = 0.0f; + _bias = 0.0f; + _gamma = 0.0f; + if (Frequency > 0.0f) + { + _ax = MathUtils.Mul(qA, LocalXAxis); + _sAx = MathUtils.Cross(d1 + rA, _ax); + _sBx = MathUtils.Cross(rB, _ax); + + float invMass = mA + mB + iA * _sAx * _sAx + iB * _sBx * _sBx; + + if (invMass > 0.0f) + { + _springMass = 1.0f / invMass; + + float C = Vector2.Dot(d1, _ax); + + // Frequency + float omega = 2.0f * Settings.Pi * Frequency; + + // Damping coefficient + float d = 2.0f * _springMass * DampingRatio * omega; + + // Spring stiffness + float k = _springMass * omega * omega; + + // magic formulas + float h = data.step.dt; + _gamma = h * (d + h * k); + if (_gamma > 0.0f) + { + _gamma = 1.0f / _gamma; + } + + _bias = C * h * k * _gamma; + + _springMass = invMass + _gamma; + if (_springMass > 0.0f) + { + _springMass = 1.0f / _springMass; + } + } + } + else + { + _springImpulse = 0.0f; + } + + // Rotational motor + if (_enableMotor) + { + _motorMass = iA + iB; + if (_motorMass > 0.0f) + { + _motorMass = 1.0f / _motorMass; + } + } + else + { + _motorMass = 0.0f; + _motorImpulse = 0.0f; + } + + if (Settings.EnableWarmstarting) + { + // Account for variable time step. + _impulse *= data.step.dtRatio; + _springImpulse *= data.step.dtRatio; + _motorImpulse *= data.step.dtRatio; + + Vector2 P = _impulse * _ay + _springImpulse * _ax; + float LA = _impulse * _sAy + _springImpulse * _sAx + _motorImpulse; + float LB = _impulse * _sBy + _springImpulse * _sBx + _motorImpulse; + + vA -= _invMassA * P; + wA -= _invIA * LA; + + vB += _invMassB * P; + wB += _invIB * LB; + } + else + { + _impulse = 0.0f; + _springImpulse = 0.0f; + _motorImpulse = 0.0f; + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override void SolveVelocityConstraints(ref SolverData data) + { + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + // Solve spring constraint + { + float Cdot = Vector2.Dot(_ax, vB - vA) + _sBx * wB - _sAx * wA; + float impulse = -_springMass * (Cdot + _bias + _gamma * _springImpulse); + _springImpulse += impulse; + + Vector2 P = impulse * _ax; + float LA = impulse * _sAx; + float LB = impulse * _sBx; + + vA -= mA * P; + wA -= iA * LA; + + vB += mB * P; + wB += iB * LB; + } + + // Solve rotational motor constraint + { + float Cdot = wB - wA - _motorSpeed; + float impulse = -_motorMass * Cdot; + + float oldImpulse = _motorImpulse; + float maxImpulse = data.step.dt * _maxMotorTorque; + _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = _motorImpulse - oldImpulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + // Solve point to line constraint + { + float Cdot = Vector2.Dot(_ay, vB - vA) + _sBy * wB - _sAy * wA; + float impulse = -_mass * Cdot; + _impulse += impulse; + + Vector2 P = impulse * _ay; + float LA = impulse * _sAy; + float LB = impulse * _sBy; + + vA -= mA * P; + wA -= iA * LA; + + vB += mB * P; + wB += iB * LB; + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override bool SolvePositionConstraints(ref SolverData data) + { + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + Vector2 d = (cB - cA) + rB - rA; + + Vector2 ay = MathUtils.Mul(qA, _localYAxis); + + float sAy = MathUtils.Cross(d + rA, ay); + float sBy = MathUtils.Cross(rB, ay); + + float C = Vector2.Dot(d, ay); + + float k = _invMassA + _invMassB + _invIA * _sAy * _sAy + _invIB * _sBy * _sBy; + + float impulse; + if (k != 0.0f) + { + impulse = -C / k; + } + else + { + impulse = 0.0f; + } + + Vector2 P = impulse * ay; + float LA = impulse * sAy; + float LB = impulse * sBy; + + cA -= _invMassA * P; + aA -= _invIA * LA; + cB += _invMassB * P; + aB += _invIB * LB; + + data.positions[_indexA].c = cA; + data.positions[_indexA].a = aA; + data.positions[_indexB].c = cB; + data.positions[_indexB].a = aB; + + return Math.Abs(C) <= Settings.LinearSlop; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Dynamics/TimeStep.cs b/Farseer Physics Engine 3.5/Dynamics/TimeStep.cs new file mode 100644 index 000000000..82277b446 --- /dev/null +++ b/Farseer Physics Engine 3.5/Dynamics/TimeStep.cs @@ -0,0 +1,66 @@ +/* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics +{ + /// + /// This is an internal structure. + /// + public struct TimeStep + { + /// + /// Time step (Delta time) + /// + public float dt; + + /// + /// dt * inv_dt0 + /// + public float dtRatio; + + /// + /// Inverse time step (0 if dt == 0). + /// + public float inv_dt; + } + + /// This is an internal structure. + public struct Position + { + public Vector2 c; + public float a; + } + + /// This is an internal structure. + public struct Velocity + { + public Vector2 v; + public float w; + } + + /// Solver Data + public struct SolverData + { + public TimeStep step; + public Position[] positions; + public Velocity[] velocities; + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Dynamics/World.cs b/Farseer Physics Engine 3.5/Dynamics/World.cs new file mode 100644 index 000000000..633b362b0 --- /dev/null +++ b/Farseer Physics Engine 3.5/Dynamics/World.cs @@ -0,0 +1,1510 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ +//#define USE_ACTIVE_CONTACT_SET +//#define USE_AWAKE_BODY_SET +//#define USE_ISLAND_SET +//#define OPTIMIZE_TOI +//#define USE_IGNORE_CCD_CATEGORIES + +using System; +using System.Collections.Generic; +using System.Diagnostics; +using FarseerPhysics.Collision; +using FarseerPhysics.Common; +using FarseerPhysics.Controllers; +using FarseerPhysics.Dynamics.Contacts; +using FarseerPhysics.Dynamics.Joints; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics +{ + /// + /// The world class manages all physics entities, dynamic simulation, + /// and asynchronous queries. + /// + public class World + { + private float _invDt0; + private Body[] _stack = new Body[64]; + private bool _stepComplete; + private HashSet _bodyAddList = new HashSet(); + private HashSet _bodyRemoveList = new HashSet(); + private HashSet _jointAddList = new HashSet(); + private HashSet _jointRemoveList = new HashSet(); + private Func _queryAABBCallback; + private Func _queryAABBCallbackWrapper; + private TOIInput _input = new TOIInput(); + private Fixture _myFixture; + private Vector2 _point1; + private Vector2 _point2; + private List _testPointAllFixtures; + private Stopwatch _watch = new Stopwatch(); + private Func _rayCastCallback; + private Func _rayCastCallbackWrapper; + + internal Queue _contactPool = new Queue(256); + internal bool _worldHasNewFixture; + + /// + /// Fires whenever a body has been added + /// + public BodyDelegate BodyAdded; + + /// + /// Fires whenever a body has been removed + /// + public BodyDelegate BodyRemoved; + + /// + /// Fires whenever a fixture has been added + /// + public FixtureDelegate FixtureAdded; + + /// + /// Fires whenever a fixture has been removed + /// + public FixtureDelegate FixtureRemoved; + + /// + /// Fires whenever a joint has been added + /// + public JointDelegate JointAdded; + + /// + /// Fires whenever a joint has been removed + /// + public JointDelegate JointRemoved; + + /// + /// Fires every time a controller is added to the World. + /// + public ControllerDelegate ControllerAdded; + + /// + /// Fires every time a controlelr is removed form the World. + /// + public ControllerDelegate ControllerRemoved; + + /// + /// Initializes a new instance of the class. + /// + public World(Vector2 gravity) + { + Island = new Island(); + Enabled = true; + ControllerList = new List(); + BreakableBodyList = new List(); + BodyList = new List(32); + JointList = new List(32); + +#if USE_AWAKE_BODY_SET + AwakeBodySet = new HashSet(); + AwakeBodyList = new List(32); +#endif +#if USE_ISLAND_SET + IslandSet = new HashSet(); +#endif +#if OPTIMIZE_TOI + TOISet = new HashSet(); +#endif + + _queryAABBCallbackWrapper = QueryAABBCallbackWrapper; + _rayCastCallbackWrapper = RayCastCallbackWrapper; + + ContactManager = new ContactManager(new DynamicTreeBroadPhase()); + Gravity = gravity; + } + + private void ProcessRemovedJoints() + { + if (_jointRemoveList.Count > 0) + { + foreach (Joint joint in _jointRemoveList) + { + bool collideConnected = joint.CollideConnected; + + // Remove from the world list. + JointList.Remove(joint); + + // Disconnect from island graph. + Body bodyA = joint.BodyA; + Body bodyB = joint.BodyB; + + // Wake up connected bodies. + bodyA.Awake = true; + + // WIP David + if (!joint.IsFixedType()) + { + bodyB.Awake = true; + } + + // Remove from body 1. + if (joint.EdgeA.Prev != null) + { + joint.EdgeA.Prev.Next = joint.EdgeA.Next; + } + + if (joint.EdgeA.Next != null) + { + joint.EdgeA.Next.Prev = joint.EdgeA.Prev; + } + + if (joint.EdgeA == bodyA.JointList) + { + bodyA.JointList = joint.EdgeA.Next; + } + + joint.EdgeA.Prev = null; + joint.EdgeA.Next = null; + + // WIP David + if (!joint.IsFixedType()) + { + // Remove from body 2 + if (joint.EdgeB.Prev != null) + { + joint.EdgeB.Prev.Next = joint.EdgeB.Next; + } + + if (joint.EdgeB.Next != null) + { + joint.EdgeB.Next.Prev = joint.EdgeB.Prev; + } + + if (joint.EdgeB == bodyB.JointList) + { + bodyB.JointList = joint.EdgeB.Next; + } + + joint.EdgeB.Prev = null; + joint.EdgeB.Next = null; + } + + // WIP David + if (!joint.IsFixedType()) + { + // If the joint prevents collisions, then flag any contacts for filtering. + if (collideConnected == false) + { + ContactEdge edge = bodyB.ContactList; + while (edge != null) + { + if (edge.Other == bodyA) + { + // Flag the contact for filtering at the next time step (where either + // body is awake). + edge.Contact.FilterFlag = true; + } + + edge = edge.Next; + } + } + } + + if (JointRemoved != null) + { + JointRemoved(joint); + } + } + + _jointRemoveList.Clear(); + } + } + + private void ProcessAddedJoints() + { + if (_jointAddList.Count > 0) + { + foreach (Joint joint in _jointAddList) + { + // Connect to the world list. + JointList.Add(joint); + + // Connect to the bodies' doubly linked lists. + joint.EdgeA.Joint = joint; + joint.EdgeA.Other = joint.BodyB; + joint.EdgeA.Prev = null; + joint.EdgeA.Next = joint.BodyA.JointList; + + if (joint.BodyA.JointList != null) + joint.BodyA.JointList.Prev = joint.EdgeA; + + joint.BodyA.JointList = joint.EdgeA; + + // WIP David + if (!joint.IsFixedType()) + { + joint.EdgeB.Joint = joint; + joint.EdgeB.Other = joint.BodyA; + joint.EdgeB.Prev = null; + joint.EdgeB.Next = joint.BodyB.JointList; + + if (joint.BodyB.JointList != null) + joint.BodyB.JointList.Prev = joint.EdgeB; + + joint.BodyB.JointList = joint.EdgeB; + + Body bodyA = joint.BodyA; + Body bodyB = joint.BodyB; + + // If the joint prevents collisions, then flag any contacts for filtering. + if (joint.CollideConnected == false) + { + ContactEdge edge = bodyB.ContactList; + while (edge != null) + { + if (edge.Other == bodyA) + { + // Flag the contact for filtering at the next time step (where either + // body is awake). + edge.Contact.FilterFlag = true; + } + + edge = edge.Next; + } + } + } + + if (JointAdded != null) + JointAdded(joint); + + // Note: creating a joint doesn't wake the bodies. + } + + _jointAddList.Clear(); + } + } + + private void ProcessAddedBodies() + { + if (_bodyAddList.Count > 0) + { + foreach (Body body in _bodyAddList) + { +#if USE_AWAKE_BODY_SET + Debug.Assert(!body.IsDisposed); + if (body.Awake) + { + if (!AwakeBodySet.Contains(body)) + { + AwakeBodySet.Add(body); + } + } + else + { + if (AwakeBodySet.Contains(body)) + { + AwakeBodySet.Remove(body); + } + } +#endif + // Add to world list. + BodyList.Add(body); + + if (BodyAdded != null) + BodyAdded(body); + } + + _bodyAddList.Clear(); + } + } + + private void ProcessRemovedBodies() + { + if (_bodyRemoveList.Count > 0) + { + foreach (Body body in _bodyRemoveList) + { + Debug.Assert(BodyList.Count > 0); + + // You tried to remove a body that is not contained in the BodyList. + // Are you removing the body more than once? + System.Diagnostics.Debug.WriteLine(body.UserData); + Debug.Assert(BodyList.Contains(body)); + +#if USE_AWAKE_BODY_SET + Debug.Assert(!AwakeBodySet.Contains(body)); +#endif + // Delete the attached joints. + JointEdge je = body.JointList; + while (je != null) + { + JointEdge je0 = je; + je = je.Next; + + RemoveJoint(je0.Joint, false); + } + body.JointList = null; + + // Delete the attached contacts. + ContactEdge ce = body.ContactList; + while (ce != null) + { + ContactEdge ce0 = ce; + ce = ce.Next; + ContactManager.Destroy(ce0.Contact); + } + body.ContactList = null; + + // Delete the attached fixtures. This destroys broad-phase proxies. + for (int i = 0; i < body.FixtureList.Count; i++) + { + body.FixtureList[i].DestroyProxies(ContactManager.BroadPhase); + body.FixtureList[i].Destroy(); + } + + body.FixtureList = null; + + // Remove world body list. + BodyList.Remove(body); + + if (BodyRemoved != null) + BodyRemoved(body); + +#if USE_AWAKE_BODY_SET + Debug.Assert(!AwakeBodySet.Contains(body)); +#endif + } + + _bodyRemoveList.Clear(); + } + } + + private bool QueryAABBCallbackWrapper(int proxyId) + { + FixtureProxy proxy = ContactManager.BroadPhase.GetProxy(proxyId); + return _queryAABBCallback(proxy.Fixture); + } + + private float RayCastCallbackWrapper(RayCastInput rayCastInput, int proxyId) + { + FixtureProxy proxy = ContactManager.BroadPhase.GetProxy(proxyId); + Fixture fixture = proxy.Fixture; + int index = proxy.ChildIndex; + RayCastOutput output; + bool hit = fixture.RayCast(out output, ref rayCastInput, index); + + if (hit) + { + float fraction = output.Fraction; + Vector2 point = (1.0f - fraction) * rayCastInput.Point1 + fraction * rayCastInput.Point2; + return _rayCastCallback(fixture, point, output.Normal, fraction); + } + + return rayCastInput.MaxFraction; + } + + private void Solve(ref TimeStep step) + { + // Size the island for the worst case. + Island.Reset(BodyList.Count, + ContactManager.ContactList.Count, + JointList.Count, + ContactManager); + + // Clear all the island flags. +#if USE_ISLAND_SET + Debug.Assert(IslandSet.Count == 0); +#else + foreach (Body b in BodyList) + { + b._island = false; + } +#endif + +#if USE_ACTIVE_CONTACT_SET + foreach (var c in ContactManager.ActiveContacts) + { + c.Flags &= ~ContactFlags.Island; + } +#else + foreach (Contact c in ContactManager.ContactList) + { + c.IslandFlag = false; + } +#endif + foreach (Joint j in JointList) + { + j.IslandFlag = false; + } + + // Build and simulate all awake islands. + int stackSize = BodyList.Count; + if (stackSize > _stack.Length) + _stack = new Body[Math.Max(_stack.Length * 2, stackSize)]; + +#if USE_AWAKE_BODY_SET + + // If AwakeBodyList is empty, the Island code will not have a chance + // to update the diagnostics timer so reset the timer here. + Island.JointUpdateTime = 0; + + Debug.Assert(AwakeBodyList.Count == 0); + AwakeBodyList.AddRange(AwakeBodySet); + + foreach (var seed in AwakeBodyList) + { +#else + for (int index = BodyList.Count - 1; index >= 0; index--) + { + Body seed = BodyList[index]; +#endif + if (seed._island) + { + continue; + } + + if (seed.Awake == false || seed.Enabled == false) + { + continue; + } + + // The seed can be dynamic or kinematic. + if (seed.BodyType == BodyType.Static) + { + continue; + } + + // Reset island and stack. + Island.Clear(); + int stackCount = 0; + _stack[stackCount++] = seed; + +#if USE_ISLAND_SET + if (!IslandSet.Contains(body)) + IslandSet.Add(body); +#endif + seed._island = true; + + // Perform a depth first search (DFS) on the constraint graph. + while (stackCount > 0) + { + // Grab the next body off the stack and add it to the island. + Body b = _stack[--stackCount]; + Debug.Assert(b.Enabled); + Island.Add(b); + + // Make sure the body is awake. + b.Awake = true; + + // To keep islands as small as possible, we don't + // propagate islands across static bodies. + if (b.BodyType == BodyType.Static) + { + continue; + } + + // Search all contacts connected to this body. + for (ContactEdge ce = b.ContactList; ce != null; ce = ce.Next) + { + Contact contact = ce.Contact; + + // Has this contact already been added to an island? + if (contact.IslandFlag) + { + continue; + } + + // Is this contact solid and touching? + if (ce.Contact.Enabled == false || ce.Contact.IsTouching == false) + { + continue; + } + + // Skip sensors. + bool sensorA = contact.FixtureA.IsSensor; + bool sensorB = contact.FixtureB.IsSensor; + if (sensorA || sensorB) + { + continue; + } + + Island.Add(contact); + contact.IslandFlag = true; + + Body other = ce.Other; + + // Was the other body already added to this island? + if (other._island) + { + continue; + } + + Debug.Assert(stackCount < stackSize); + _stack[stackCount++] = other; + +#if USE_ISLAND_SET + if (!IslandSet.Contains(body)) + IslandSet.Add(body); +#endif + other._island = true; + } + + // Search all joints connect to this body. + for (JointEdge je = b.JointList; je != null; je = je.Next) + { + if (je.Joint.IslandFlag) + { + continue; + } + + Body other = je.Other; + + // WIP David + //Enter here when it's a non-fixed joint. Non-fixed joints have a other body. + if (other != null) + { + // Don't simulate joints connected to inactive bodies. + if (other.Enabled == false) + { + continue; + } + + Island.Add(je.Joint); + je.Joint.IslandFlag = true; + + if (other._island) + { + continue; + } + + Debug.Assert(stackCount < stackSize); + _stack[stackCount++] = other; +#if USE_ISLAND_SET + if (!IslandSet.Contains(body)) + IslandSet.Add(body); +#endif + other._island = true; + } + else + { + Island.Add(je.Joint); + je.Joint.IslandFlag = true; + } + } + } + + Island.Solve(ref step, ref Gravity); + + // Post solve cleanup. + for (int i = 0; i < Island.BodyCount; ++i) + { + // Allow static bodies to participate in other islands. + Body b = Island.Bodies[i]; + if (b.BodyType == BodyType.Static) + { + b._island = false; + } + } + } + + // Synchronize fixtures, check for out of range bodies. +#if USE_ISLAND_SET + foreach (var b in IslandSet) +#else + foreach (Body b in BodyList) +#endif + { + // If a body was not in an island then it did not move. + if (!b._island) + { + continue; + } +#if USE_ISLAND_SET + Debug.Assert(b.BodyType != BodyType.Static); +#else + if (b.BodyType == BodyType.Static) + { + continue; + } +#endif + + // Update fixtures (for broad-phase). + b.SynchronizeFixtures(); + } +#if OPTIMIZE_TOI + foreach (var b in IslandSet) + { + if (!TOISet.Contains(b)) + { + TOISet.Add(b); + } + } +#endif +#if USE_ISLAND_SET + IslandSet.Clear(); +#endif + + // Look for new contacts. + ContactManager.FindNewContacts(); + +#if USE_AWAKE_BODY_SET + AwakeBodyList.Clear(); +#endif + } + + private void SolveTOI(ref TimeStep step) + { + Island.Reset(2 * Settings.MaxTOIContacts, Settings.MaxTOIContacts, 0, ContactManager); + +#if OPTIMIZE_TOI + bool wasStepComplete = _stepComplete; +#endif + if (_stepComplete) + { +#if OPTIMIZE_TOI + foreach (var b in TOISet) + { + b.Flags &= ~BodyFlags.Island; + b.Sweep.Alpha0 = 0.0f; + } +#else + for (int i = 0; i < BodyList.Count; i++) + { + BodyList[i]._island = false; + BodyList[i]._sweep.Alpha0 = 0.0f; + } +#endif +#if USE_ACTIVE_CONTACT_SET + foreach (var c in ContactManager.ActiveContacts) + { +#else + for (int i = 0; i < ContactManager.ContactList.Count; i++) + { + Contact c = ContactManager.ContactList[i]; +#endif + // Invalidate TOI + c.IslandFlag = false; + c.TOIFlag = false; + c._toiCount = 0; + c._toi = 1.0f; + } + } + + // Find TOI events and solve them. + for (; ; ) + { + // Find the first TOI. + Contact minContact = null; + float minAlpha = 1.0f; + +#if USE_ACTIVE_CONTACT_SET + foreach (var c in ContactManager.ActiveContacts) + { +#else + for (int i = 0; i < ContactManager.ContactList.Count; i++) + { + Contact c = ContactManager.ContactList[i]; +#endif + + // Is this contact disabled? + if (c.Enabled == false) + { + continue; + } + + // Prevent excessive sub-stepping. + if (c._toiCount > Settings.MaxSubSteps) + { + continue; + } + + float alpha; + if (c.TOIFlag) + { + // This contact has a valid cached TOI. + alpha = c._toi; + } + else + { + Fixture fA = c.FixtureA; + Fixture fB = c.FixtureB; + + // Is there a sensor? + if (fA.IsSensor || fB.IsSensor) + { + continue; + } + + Body bA = fA.Body; + Body bB = fB.Body; + + BodyType typeA = bA.BodyType; + BodyType typeB = bB.BodyType; + Debug.Assert(typeA == BodyType.Dynamic || typeB == BodyType.Dynamic); + + bool activeA = bA.Awake && typeA != BodyType.Static; + bool activeB = bB.Awake && typeB != BodyType.Static; + + // Is at least one body active (awake and dynamic or kinematic)? + if (activeA == false && activeB == false) + { + continue; + } + + bool collideA = (bA.IsBullet || typeA != BodyType.Dynamic) && ((fA.IgnoreCCDWith & fB.CollisionCategories) == 0) && !bA.IgnoreCCD; + bool collideB = (bB.IsBullet || typeB != BodyType.Dynamic) && ((fB.IgnoreCCDWith & fA.CollisionCategories) == 0) && !bB.IgnoreCCD; + + // Are these two non-bullet dynamic bodies? + if (collideA == false && collideB == false) + { + continue; + } + +#if OPTIMIZE_TOI + if (_stepComplete) + { + if (!TOISet.Contains(bA)) + { + TOISet.Add(bA); + bA.Flags &= ~BodyFlags.Island; + bA.Sweep.Alpha0 = 0.0f; + } + if (!TOISet.Contains(bB)) + { + TOISet.Add(bB); + bB.Flags &= ~BodyFlags.Island; + bB.Sweep.Alpha0 = 0.0f; + } + } +#endif + // Compute the TOI for this contact. + // Put the sweeps onto the same time interval. + float alpha0 = bA._sweep.Alpha0; + + if (bA._sweep.Alpha0 < bB._sweep.Alpha0) + { + alpha0 = bB._sweep.Alpha0; + bA._sweep.Advance(alpha0); + } + else if (bB._sweep.Alpha0 < bA._sweep.Alpha0) + { + alpha0 = bA._sweep.Alpha0; + bB._sweep.Advance(alpha0); + } + + Debug.Assert(alpha0 < 1.0f); + + // Compute the time of impact in interval [0, minTOI] + _input.ProxyA.Set(fA.Shape, c.ChildIndexA); + _input.ProxyB.Set(fB.Shape, c.ChildIndexB); + _input.SweepA = bA._sweep; + _input.SweepB = bB._sweep; + _input.TMax = 1.0f; + + TOIOutput output; + TimeOfImpact.CalculateTimeOfImpact(out output, _input); + + // Beta is the fraction of the remaining portion of the . + float beta = output.T; + if (output.State == TOIOutputState.Touching) + { + alpha = Math.Min(alpha0 + (1.0f - alpha0) * beta, 1.0f); + } + else + { + alpha = 1.0f; + } + + c._toi = alpha; + c.TOIFlag = true; + } + + if (alpha < minAlpha) + { + // This is the minimum TOI found so far. + minContact = c; + minAlpha = alpha; + } + } + + if (minContact == null || 1.0f - 10.0f * Settings.Epsilon < minAlpha) + { + // No more TOI events. Done! + _stepComplete = true; + break; + } + + // Advance the bodies to the TOI. + Fixture fA1 = minContact.FixtureA; + Fixture fB1 = minContact.FixtureB; + Body bA0 = fA1.Body; + Body bB0 = fB1.Body; + + Sweep backup1 = bA0._sweep; + Sweep backup2 = bB0._sweep; + + bA0.Advance(minAlpha); + bB0.Advance(minAlpha); + + // The TOI contact likely has some new contact points. + minContact.Update(ContactManager); + minContact.TOIFlag = false; + ++minContact._toiCount; + + // Is the contact solid? + if (minContact.Enabled == false || minContact.IsTouching == false) + { + // Restore the sweeps. + minContact.Enabled = false; + bA0._sweep = backup1; + bB0._sweep = backup2; + bA0.SynchronizeTransform(); + bB0.SynchronizeTransform(); + continue; + } + + bA0.Awake = true; + bB0.Awake = true; + + // Build the island + Island.Clear(); + Island.Add(bA0); + Island.Add(bB0); + Island.Add(minContact); + + bA0._island = true; + bB0._island = true; + minContact.IslandFlag = true; + + // Get contacts on bodyA and bodyB. + Body[] bodies = { bA0, bB0 }; + for (int i = 0; i < 2; ++i) + { + Body body = bodies[i]; + if (body.BodyType == BodyType.Dynamic) + { + for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next) + { + Contact contact = ce.Contact; + + if (Island.BodyCount == Island.BodyCapacity) + { + break; + } + + if (Island.ContactCount == Island.ContactCapacity) + { + break; + } + + // Has this contact already been added to the island? + if (contact.IslandFlag) + { + continue; + } + + // Only add static, kinematic, or bullet bodies. + Body other = ce.Other; + if (other.BodyType == BodyType.Dynamic && + body.IsBullet == false && other.IsBullet == false) + { + continue; + } + + // Skip sensors. + if (contact.FixtureA.IsSensor || contact.FixtureB.IsSensor) + { + continue; + } + + // Tentatively advance the body to the TOI. + Sweep backup = other._sweep; + if (!other._island) + { + other.Advance(minAlpha); + } + + // Update the contact points + contact.Update(ContactManager); + + // Was the contact disabled by the user? + if (contact.Enabled == false) + { + other._sweep = backup; + other.SynchronizeTransform(); + continue; + } + + // Are there contact points? + if (contact.IsTouching == false) + { + other._sweep = backup; + other.SynchronizeTransform(); + continue; + } + + // Add the contact to the island + contact.IslandFlag = true; + Island.Add(contact); + + // Has the other body already been added to the island? + if (other._island) + { + continue; + } + + // Add the other body to the island. + other._island = true; + + if (other.BodyType != BodyType.Static) + { + other.Awake = true; + } +#if OPTIMIZE_TOI + if (_stepComplete) + { + if (!TOISet.Contains(other)) + { + TOISet.Add(other); + other.Sweep.Alpha0 = 0.0f; + } + } +#endif + Island.Add(other); + } + } + } + + TimeStep subStep; + subStep.dt = (1.0f - minAlpha) * step.dt; + subStep.inv_dt = 1.0f / subStep.dt; + subStep.dtRatio = 1.0f; + Island.SolveTOI(ref subStep, bA0.IslandIndex, bB0.IslandIndex, false); + + // Reset island flags and synchronize broad-phase proxies. + for (int i = 0; i < Island.BodyCount; ++i) + { + Body body = Island.Bodies[i]; + body._island = false; + + if (body.BodyType != BodyType.Dynamic) + { + continue; + } + + body.SynchronizeFixtures(); + + // Invalidate all contact TOIs on this displaced body. + for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next) + { + ce.Contact.TOIFlag = false; + ce.Contact.IslandFlag = false; + } + } + + // Commit fixture proxy movements to the broad-phase so that new contacts are created. + // Also, some contacts can be destroyed. + ContactManager.FindNewContacts(); + + if (Settings.EnableSubStepping) + { + _stepComplete = false; + break; + } + } +#if OPTIMIZE_TOI + if (wasStepComplete) + { + TOISet.Clear(); + } +#endif + } + + public List ControllerList { get; private set; } + + public List BreakableBodyList { get; private set; } + + public float UpdateTime { get; private set; } + + public float ContinuousPhysicsTime { get; private set; } + + public float ControllersUpdateTime { get; private set; } + + public float AddRemoveTime { get; private set; } + + public float NewContactsTime { get; private set; } + + public float ContactsUpdateTime { get; private set; } + + public float SolveUpdateTime { get; private set; } + + /// + /// Get the number of broad-phase proxies. + /// + /// The proxy count. + public int ProxyCount + { + get { return ContactManager.BroadPhase.ProxyCount; } + } + + /// + /// Change the global gravity vector. + /// + /// The gravity. + public Vector2 Gravity; + + /// + /// Get the contact manager for testing. + /// + /// The contact manager. + public ContactManager ContactManager { get; private set; } + + /// + /// Get the world body list. + /// + /// Thehead of the world body list. + public List BodyList { get; private set; } + +#if USE_AWAKE_BODY_SET + public HashSet AwakeBodySet { get; private set; } + List AwakeBodyList; +#endif +#if USE_ISLAND_SET + HashSet IslandSet; +#endif +#if OPTIMIZE_TOI + HashSet TOISet; +#endif + + /// + /// Get the world joint list. + /// + /// The joint list. + public List JointList { get; private set; } + + /// + /// Get the world contact list. With the returned contact, use Contact.GetNext to get + /// the next contact in the world list. A null contact indicates the end of the list. + /// + /// The head of the world contact list. + public List ContactList + { + get { return ContactManager.ContactList; } + } + + /// + /// If false, the whole simulation stops. It still processes added and removed geometries. + /// + public bool Enabled { get; set; } + + public Island Island { get; private set; } + + /// + /// Add a rigid body. + /// + /// + internal void AddBody(Body body) + { + Debug.Assert(!_bodyAddList.Contains(body), "You are adding the same body more than once."); + + if (!_bodyAddList.Contains(body)) + _bodyAddList.Add(body); + } + + /// + /// Destroy a rigid body. + /// Warning: This automatically deletes all associated shapes and joints. + /// + /// The body. + public void RemoveBody(Body body) + { + Debug.Assert(!_bodyRemoveList.Contains(body), + "The body is already marked for removal. You are removing the body more than once."); + + if (!_bodyRemoveList.Contains(body)) + _bodyRemoveList.Add(body); + +#if USE_AWAKE_BODY_SET + if (AwakeBodySet.Contains(body)) + { + AwakeBodySet.Remove(body); + } +#endif + } + + /// + /// Create a joint to constrain bodies together. This may cause the connected bodies to cease colliding. + /// + /// The joint. + public void AddJoint(Joint joint) + { + Debug.Assert(!_jointAddList.Contains(joint), "You are adding the same joint more than once."); + + if (!_jointAddList.Contains(joint)) + _jointAddList.Add(joint); + } + + private void RemoveJoint(Joint joint, bool doCheck) + { + if (doCheck) + { + Debug.Assert(!_jointRemoveList.Contains(joint), + "The joint is already marked for removal. You are removing the joint more than once."); + } + + if (!_jointRemoveList.Contains(joint)) + _jointRemoveList.Add(joint); + } + + /// + /// Destroy a joint. This may cause the connected bodies to begin colliding. + /// + /// The joint. + public void RemoveJoint(Joint joint) + { + RemoveJoint(joint, true); + } + + /// + /// All adds and removes are cached by the World duing a World step. + /// To process the changes before the world updates again, call this method. + /// + public void ProcessChanges() + { + ProcessAddedBodies(); + ProcessAddedJoints(); + + ProcessRemovedBodies(); + ProcessRemovedJoints(); +#if DEBUG && USE_AWAKE_BODY_SET + foreach (var b in AwakeBodySet) + { + Debug.Assert(BodyList.Contains(b)); + } +#endif + } + + /// + /// Take a time step. This performs collision detection, integration, + /// and consraint solution. + /// + /// The amount of time to simulate, this should not vary. + public void Step(float dt) + { + if (!Enabled) + return; + + if (Settings.EnableDiagnostics) + _watch.Start(); + + ProcessChanges(); + + if (Settings.EnableDiagnostics) + AddRemoveTime = _watch.ElapsedTicks; + + // If new fixtures were added, we need to find the new contacts. + if (_worldHasNewFixture) + { + ContactManager.FindNewContacts(); + _worldHasNewFixture = false; + } + + if (Settings.EnableDiagnostics) + NewContactsTime = _watch.ElapsedTicks - AddRemoveTime; + + //FPE only: moved position and velocity iterations into Settings.cs + TimeStep step; + step.inv_dt = dt > 0.0f ? 1.0f / dt : 0.0f; + step.dt = dt; + step.dtRatio = _invDt0 * dt; + + //Update controllers + for (int i = 0; i < ControllerList.Count; i++) + { + ControllerList[i].Update(dt); + } + + if (Settings.EnableDiagnostics) + ControllersUpdateTime = _watch.ElapsedTicks - (AddRemoveTime + NewContactsTime); + + // Update contacts. This is where some contacts are destroyed. + ContactManager.Collide(); + + if (Settings.EnableDiagnostics) + ContactsUpdateTime = _watch.ElapsedTicks - (AddRemoveTime + NewContactsTime + ControllersUpdateTime); + + // Integrate velocities, solve velocity raints, and integrate positions. + Solve(ref step); + + if (Settings.EnableDiagnostics) + SolveUpdateTime = _watch.ElapsedTicks - (AddRemoveTime + NewContactsTime + ControllersUpdateTime + ContactsUpdateTime); + + // Handle TOI events. + if (Settings.ContinuousPhysics) + { + SolveTOI(ref step); + } + + if (Settings.EnableDiagnostics) + ContinuousPhysicsTime = _watch.ElapsedTicks - (AddRemoveTime + NewContactsTime + ControllersUpdateTime + ContactsUpdateTime + SolveUpdateTime); + + if (Settings.AutoClearForces) + ClearForces(); + + for (int i = 0; i < BreakableBodyList.Count; i++) + { + BreakableBodyList[i].Update(); + } + + _invDt0 = step.inv_dt; + + if (Settings.EnableDiagnostics) + { + _watch.Stop(); + UpdateTime = _watch.ElapsedTicks; + _watch.Reset(); + } + } + + /// + /// Call this after you are done with time steps to clear the forces. You normally + /// call this after each call to Step, unless you are performing sub-steps. By default, + /// forces will be automatically cleared, so you don't need to call this function. + /// + public void ClearForces() + { + for (int i = 0; i < BodyList.Count; i++) + { + Body body = BodyList[i]; + body._force = Vector2.Zero; + body._torque = 0.0f; + } + } + + /// + /// Query the world for all fixtures that potentially overlap the provided AABB. + /// + /// Inside the callback: + /// Return true: Continues the query + /// Return false: Terminate the query + /// + /// A user implemented callback class. + /// The aabb query box. + public void QueryAABB(Func callback, ref AABB aabb) + { + _queryAABBCallback = callback; + ContactManager.BroadPhase.Query(_queryAABBCallbackWrapper, ref aabb); + _queryAABBCallback = null; + } + + /// + /// Query the world for all fixtures that potentially overlap the provided AABB. + /// Use the overload with a callback for filtering and better performance. + /// + /// The aabb query box. + /// A list of fixtures that were in the affected area. + public List QueryAABB(ref AABB aabb) + { + List affected = new List(); + + QueryAABB(fixture => + { + affected.Add(fixture); + return true; + }, ref aabb); + + return affected; + } + + /// + /// Ray-cast the world for all fixtures in the path of the ray. Your callback + /// controls whether you get the closest point, any point, or n-points. + /// The ray-cast ignores shapes that contain the starting point. + /// + /// Inside the callback: + /// return -1: ignore this fixture and continue + /// return 0: terminate the ray cast + /// return fraction: clip the ray to this point + /// return 1: don't clip the ray and continue + /// + /// A user implemented callback class. + /// The ray starting point. + /// The ray ending point. + public void RayCast(Func callback, Vector2 point1, Vector2 point2) + { + RayCastInput input = new RayCastInput(); + input.MaxFraction = 1.0f; + input.Point1 = point1; + input.Point2 = point2; + + _rayCastCallback = callback; + ContactManager.BroadPhase.RayCast(_rayCastCallbackWrapper, ref input); + _rayCastCallback = null; + } + + public List RayCast(Vector2 point1, Vector2 point2) + { + List affected = new List(); + + RayCast((f, p, n, fr) => + { + affected.Add(f); + return 1; + }, point1, point2); + + return affected; + } + + public void AddController(Controller controller) + { + Debug.Assert(!ControllerList.Contains(controller), "You are adding the same controller more than once."); + + controller.World = this; + ControllerList.Add(controller); + + if (ControllerAdded != null) + ControllerAdded(controller); + } + + public void RemoveController(Controller controller) + { + Debug.Assert(ControllerList.Contains(controller), + "You are removing a controller that is not in the simulation."); + + if (ControllerList.Contains(controller)) + { + ControllerList.Remove(controller); + + if (ControllerRemoved != null) + ControllerRemoved(controller); + } + } + + public void AddBreakableBody(BreakableBody breakableBody) + { + BreakableBodyList.Add(breakableBody); + } + + public void RemoveBreakableBody(BreakableBody breakableBody) + { + //The breakable body list does not contain the body you tried to remove. + Debug.Assert(BreakableBodyList.Contains(breakableBody)); + + BreakableBodyList.Remove(breakableBody); + } + + public Fixture TestPoint(Vector2 point) + { + AABB aabb; + Vector2 d = new Vector2(Settings.Epsilon, Settings.Epsilon); + aabb.LowerBound = point - d; + aabb.UpperBound = point + d; + + _myFixture = null; + _point1 = point; + + // Query the world for overlapping shapes. + QueryAABB(TestPointCallback, ref aabb); + + return _myFixture; + } + + private bool TestPointCallback(Fixture fixture) + { + bool inside = fixture.TestPoint(ref _point1); + if (inside) + { + _myFixture = fixture; + return false; + } + + // Continue the query. + return true; + } + + /// + /// Returns a list of fixtures that are at the specified point. + /// + /// The point. + /// + public List TestPointAll(Vector2 point) + { + AABB aabb; + Vector2 d = new Vector2(Settings.Epsilon, Settings.Epsilon); + aabb.LowerBound = point - d; + aabb.UpperBound = point + d; + + _point2 = point; + _testPointAllFixtures = new List(); + + // Query the world for overlapping shapes. + QueryAABB(TestPointAllCallback, ref aabb); + + return _testPointAllFixtures; + } + + private bool TestPointAllCallback(Fixture fixture) + { + bool inside = fixture.TestPoint(ref _point2); + if (inside) + _testPointAllFixtures.Add(fixture); + + // Continue the query. + return true; + } + + /// Shift the world origin. Useful for large worlds. + /// The body shift formula is: position -= newOrigin + /// @param newOrigin the new origin with respect to the old origin + /// Warning: Calling this method mid-update might cause a crash. + public void ShiftOrigin(Vector2 newOrigin) + { + foreach (Body b in BodyList) + { + b._xf.p -= newOrigin; + b._sweep.C0 -= newOrigin; + b._sweep.C -= newOrigin; + } + + foreach (Joint joint in JointList) + { + //joint.ShiftOrigin(newOrigin); //TODO: uncomment + } + + ContactManager.BroadPhase.ShiftOrigin(newOrigin); + } + + public void Clear() + { + ProcessChanges(); + + for (int i = BodyList.Count - 1; i >= 0; i--) + { + RemoveBody(BodyList[i]); + } + + for (int i = ControllerList.Count - 1; i >= 0; i--) + { + RemoveController(ControllerList[i]); + } + + for (int i = BreakableBodyList.Count - 1; i >= 0; i--) + { + RemoveBreakableBody(BreakableBodyList[i]); + } + + ProcessChanges(); + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Dynamics/WorldCallbacks.cs b/Farseer Physics Engine 3.5/Dynamics/WorldCallbacks.cs new file mode 100644 index 000000000..8349d89a1 --- /dev/null +++ b/Farseer Physics Engine 3.5/Dynamics/WorldCallbacks.cs @@ -0,0 +1,63 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using FarseerPhysics.Collision; +using FarseerPhysics.Controllers; +using FarseerPhysics.Dynamics.Contacts; +using FarseerPhysics.Dynamics.Joints; + +namespace FarseerPhysics.Dynamics +{ + /// + /// This delegate is called when a contact is deleted + /// + public delegate void EndContactDelegate(Contact contact); + + /// + /// This delegate is called when a contact is created + /// + public delegate bool BeginContactDelegate(Contact contact); + + public delegate void PreSolveDelegate(Contact contact, ref Manifold oldManifold); + + public delegate void PostSolveDelegate(Contact contact, ContactVelocityConstraint impulse); + + public delegate void FixtureDelegate(Fixture fixture); + + public delegate void JointDelegate(Joint joint); + + public delegate void BodyDelegate(Body body); + + public delegate void ControllerDelegate(Controller controller); + + public delegate bool CollisionFilterDelegate(Fixture fixtureA, Fixture fixtureB); + + public delegate void BroadphaseDelegate(ref FixtureProxy proxyA, ref FixtureProxy proxyB); + + public delegate bool BeforeCollisionEventHandler(Fixture fixtureA, Fixture fixtureB); + + public delegate bool OnCollisionEventHandler(Fixture fixtureA, Fixture fixtureB, Contact contact); + + public delegate void AfterCollisionEventHandler(Fixture fixtureA, Fixture fixtureB, Contact contact, ContactVelocityConstraint impulse); + + public delegate void OnSeparationEventHandler(Fixture fixtureA, Fixture fixtureB); +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Factories/BodyFactory.cs b/Farseer Physics Engine 3.5/Factories/BodyFactory.cs new file mode 100644 index 000000000..8ef734a6f --- /dev/null +++ b/Farseer Physics Engine 3.5/Factories/BodyFactory.cs @@ -0,0 +1,281 @@ +using System; +using System.Collections.Generic; +using FarseerPhysics.Collision.Shapes; +using FarseerPhysics.Common; +using FarseerPhysics.Common.Decomposition; +using FarseerPhysics.Dynamics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Factories +{ + public static class BodyFactory + { + public static Body CreateBody(World world, object userData = null) + { + Body body = new Body(world, null, 0, userData); + return body; + } + + public static Body CreateBody(World world, Vector2 position, float rotation = 0, object userData = null) + { + Body body = new Body(world, position, rotation, userData); + return body; + } + + public static Body CreateEdge(World world, Vector2 start, Vector2 end, object userData = null) + { + Body body = CreateBody(world); + FixtureFactory.AttachEdge(start, end, body, userData); + return body; + } + + public static Body CreateChainShape(World world, Vertices vertices, object userData = null) + { + return CreateChainShape(world, vertices, Vector2.Zero, userData); + } + + public static Body CreateChainShape(World world, Vertices vertices, Vector2 position, object userData = null) + { + Body body = CreateBody(world, position); + FixtureFactory.AttachChainShape(vertices, body, userData); + return body; + } + + public static Body CreateLoopShape(World world, Vertices vertices, object userData = null) + { + return CreateLoopShape(world, vertices, Vector2.Zero, userData); + } + + public static Body CreateLoopShape(World world, Vertices vertices, Vector2 position, object userData = null) + { + Body body = CreateBody(world, position); + FixtureFactory.AttachLoopShape(vertices, body, userData); + return body; + } + + public static Body CreateRectangle(World world, float width, float height, float density, object userData = null) + { + return CreateRectangle(world, width, height, density, Vector2.Zero, userData); + } + + public static Body CreateRectangle(World world, float width, float height, float density, Vector2 position, object userData = null) + { + if (width <= 0) + throw new ArgumentOutOfRangeException("width", "Width must be more than 0 meters"); + + if (height <= 0) + throw new ArgumentOutOfRangeException("height", "Height must be more than 0 meters"); + + Body newBody = CreateBody(world, position); + newBody.UserData = userData; + + Vertices rectangleVertices = PolygonTools.CreateRectangle(width / 2, height / 2); + PolygonShape rectangleShape = new PolygonShape(rectangleVertices, density); + newBody.CreateFixture(rectangleShape); + + return newBody; + } + + public static Body CreateCircle(World world, float radius, float density, object userData = null) + { + return CreateCircle(world, radius, density, Vector2.Zero, userData); + } + + public static Body CreateCircle(World world, float radius, float density, Vector2 position, object userData = null) + { + Body body = CreateBody(world, position); + FixtureFactory.AttachCircle(radius, density, body, userData); + return body; + } + + public static Body CreateEllipse(World world, float xRadius, float yRadius, int edges, float density, object userData = null) + { + return CreateEllipse(world, xRadius, yRadius, edges, density, Vector2.Zero, userData); + } + + public static Body CreateEllipse(World world, float xRadius, float yRadius, int edges, float density, + Vector2 position, object userData = null) + { + Body body = CreateBody(world, position); + FixtureFactory.AttachEllipse(xRadius, yRadius, edges, density, body, userData); + return body; + } + + public static Body CreatePolygon(World world, Vertices vertices, float density, object userData = null) + { + return CreatePolygon(world, vertices, density, Vector2.Zero, userData); + } + + public static Body CreatePolygon(World world, Vertices vertices, float density, Vector2 position, object userData = null) + { + Body body = CreateBody(world, position); + FixtureFactory.AttachPolygon(vertices, density, body, userData); + return body; + } + + public static Body CreateCompoundPolygon(World world, List list, float density, object userData = null) + { + return CreateCompoundPolygon(world, list, density, Vector2.Zero, userData); + } + + public static Body CreateCompoundPolygon(World world, List list, float density, Vector2 position, object userData = null) + { + //We create a single body + Body polygonBody = CreateBody(world, position); + FixtureFactory.AttachCompoundPolygon(list, density, polygonBody, userData); + return polygonBody; + } + + public static Body CreateGear(World world, float radius, int numberOfTeeth, float tipPercentage, float toothHeight, float density, object userData = null) + { + Vertices gearPolygon = PolygonTools.CreateGear(radius, numberOfTeeth, tipPercentage, toothHeight); + + //Gears can in some cases be convex + if (!gearPolygon.IsConvex()) + { + //Decompose the gear: + List list = Triangulate.ConvexPartition(gearPolygon, TriangulationAlgorithm.Earclip); + + return CreateCompoundPolygon(world, list, density, userData); + } + + return CreatePolygon(world, gearPolygon, density, userData); + } + + /// + /// Creates a capsule. + /// Note: Automatically decomposes the capsule if it contains too many vertices (controlled by Settings.MaxPolygonVertices) + /// + /// + public static Body CreateCapsule(World world, float height, float topRadius, int topEdges, float bottomRadius, int bottomEdges, float density, Vector2 position, object userData = null) + { + Vertices verts = PolygonTools.CreateCapsule(height, topRadius, topEdges, bottomRadius, bottomEdges); + + Body body; + + //There are too many vertices in the capsule. We decompose it. + if (verts.Count >= Settings.MaxPolygonVertices) + { + List vertList = Triangulate.ConvexPartition(verts, TriangulationAlgorithm.Earclip); + body = CreateCompoundPolygon(world, vertList, density, userData); + body.Position = position; + + return body; + } + + body = CreatePolygon(world, verts, density, userData); + body.Position = position; + + return body; + } + + public static Body CreateCapsule(World world, float height, float endRadius, float density, + object userData = null) + { + //Create the middle rectangle + Vertices rectangle = PolygonTools.CreateRectangle(endRadius, height / 2); + + List list = new List(); + list.Add(rectangle); + + Body body = CreateCompoundPolygon(world, list, density, userData); + body.UserData = userData; + + //Create the two circles + CircleShape topCircle = new CircleShape(endRadius, density); + topCircle.Position = new Vector2(0, height / 2); + body.CreateFixture(topCircle); + + CircleShape bottomCircle = new CircleShape(endRadius, density); + bottomCircle.Position = new Vector2(0, -(height / 2)); + body.CreateFixture(bottomCircle); + return body; + } + + /// + /// Creates a rounded rectangle. + /// Note: Automatically decomposes the capsule if it contains too many vertices (controlled by Settings.MaxPolygonVertices) + /// + /// The world. + /// The width. + /// The height. + /// The x radius. + /// The y radius. + /// The segments. + /// The density. + /// The position. + /// + public static Body CreateRoundedRectangle(World world, float width, float height, float xRadius, float yRadius, int segments, float density, Vector2 position, object userData = null) + { + Vertices verts = PolygonTools.CreateRoundedRectangle(width, height, xRadius, yRadius, segments); + + //There are too many vertices in the capsule. We decompose it. + if (verts.Count >= Settings.MaxPolygonVertices) + { + List vertList = Triangulate.ConvexPartition(verts, TriangulationAlgorithm.Earclip); + Body body = CreateCompoundPolygon(world, vertList, density, userData); + body.Position = position; + return body; + } + + return CreatePolygon(world, verts, density); + } + + public static Body CreateRoundedRectangle(World world, float width, float height, float xRadius, float yRadius, int segments, float density, object userData = null) + { + return CreateRoundedRectangle(world, width, height, xRadius, yRadius, segments, density, Vector2.Zero, userData); + } + + public static BreakableBody CreateBreakableBody(World world, Vertices vertices, float density) + { + return CreateBreakableBody(world, vertices, density, Vector2.Zero); + } + + public static BreakableBody CreateBreakableBody(World world, IEnumerable shapes) + { + return CreateBreakableBody(world, shapes, Vector2.Zero); + } + + /// + /// Creates a breakable body. You would want to remove collinear points before using this. + /// + /// The world. + /// The vertices. + /// The density. + /// The position. + /// + public static BreakableBody CreateBreakableBody(World world, Vertices vertices, float density, Vector2 position) + { + List triangles = Triangulate.ConvexPartition(vertices, TriangulationAlgorithm.Earclip); + + BreakableBody breakableBody = new BreakableBody(triangles, world, density); + breakableBody.MainBody.Position = position; + world.AddBreakableBody(breakableBody); + + return breakableBody; + } + + public static BreakableBody CreateBreakableBody(World world, IEnumerable shapes, Vector2 position) + { + BreakableBody breakableBody = new BreakableBody(shapes, world); + breakableBody.MainBody.Position = position; + world.AddBreakableBody(breakableBody); + + return breakableBody; + } + + public static Body CreateLineArc(World world, float radians, int sides, float radius, Vector2 position, float angle, bool closed) + { + Body body = CreateBody(world); + FixtureFactory.AttachLineArc(radians, sides, radius, position, angle, closed, body); + return body; + } + + public static Body CreateSolidArc(World world, float density, float radians, int sides, float radius, Vector2 position, float angle) + { + Body body = CreateBody(world); + FixtureFactory.AttachSolidArc(density, radians, sides, radius, position, angle, body); + return body; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Factories/FixtureFactory.cs b/Farseer Physics Engine 3.5/Factories/FixtureFactory.cs new file mode 100644 index 000000000..ba299a6ac --- /dev/null +++ b/Farseer Physics Engine 3.5/Factories/FixtureFactory.cs @@ -0,0 +1,129 @@ +using System; +using System.Collections.Generic; +using FarseerPhysics.Collision.Shapes; +using FarseerPhysics.Common; +using FarseerPhysics.Common.Decomposition; +using FarseerPhysics.Dynamics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Factories +{ + /// + /// An easy to use factory for creating bodies + /// + public static class FixtureFactory + { + public static Fixture AttachEdge(Vector2 start, Vector2 end, Body body, object userData = null) + { + EdgeShape edgeShape = new EdgeShape(start, end); + return body.CreateFixture(edgeShape, userData); + } + + public static Fixture AttachChainShape(Vertices vertices, Body body, object userData = null) + { + ChainShape shape = new ChainShape(vertices); + return body.CreateFixture(shape, userData); + } + + public static Fixture AttachLoopShape(Vertices vertices, Body body, object userData = null) + { + ChainShape shape = new ChainShape(vertices, true); + return body.CreateFixture(shape, userData); + } + + public static Fixture AttachRectangle(float width, float height, float density, Vector2 offset, Body body, object userData = null) + { + Vertices rectangleVertices = PolygonTools.CreateRectangle(width / 2, height / 2); + rectangleVertices.Translate(ref offset); + PolygonShape rectangleShape = new PolygonShape(rectangleVertices, density); + return body.CreateFixture(rectangleShape, userData); + } + + public static Fixture AttachCircle(float radius, float density, Body body, object userData = null) + { + if (radius <= 0) + throw new ArgumentOutOfRangeException("radius", "Radius must be more than 0 meters"); + + CircleShape circleShape = new CircleShape(radius, density); + return body.CreateFixture(circleShape, userData); + } + + public static Fixture AttachCircle(float radius, float density, Body body, Vector2 offset, object userData = null) + { + if (radius <= 0) + throw new ArgumentOutOfRangeException("radius", "Radius must be more than 0 meters"); + + CircleShape circleShape = new CircleShape(radius, density); + circleShape.Position = offset; + return body.CreateFixture(circleShape, userData); + } + + public static Fixture AttachPolygon(Vertices vertices, float density, Body body, object userData = null) + { + if (vertices.Count <= 1) + throw new ArgumentOutOfRangeException("vertices", "Too few points to be a polygon"); + + PolygonShape polygon = new PolygonShape(vertices, density); + return body.CreateFixture(polygon, userData); + } + + public static Fixture AttachEllipse(float xRadius, float yRadius, int edges, float density, Body body, object userData = null) + { + if (xRadius <= 0) + throw new ArgumentOutOfRangeException("xRadius", "X-radius must be more than 0"); + + if (yRadius <= 0) + throw new ArgumentOutOfRangeException("yRadius", "Y-radius must be more than 0"); + + Vertices ellipseVertices = PolygonTools.CreateEllipse(xRadius, yRadius, edges); + PolygonShape polygonShape = new PolygonShape(ellipseVertices, density); + return body.CreateFixture(polygonShape, userData); + } + + public static List AttachCompoundPolygon(List list, float density, Body body, object userData = null) + { + List res = new List(list.Count); + + //Then we create several fixtures using the body + foreach (Vertices vertices in list) + { + if (vertices.Count == 2) + { + EdgeShape shape = new EdgeShape(vertices[0], vertices[1]); + res.Add(body.CreateFixture(shape, userData)); + } + else + { + PolygonShape shape = new PolygonShape(vertices, density); + res.Add(body.CreateFixture(shape, userData)); + } + } + + return res; + } + + public static Fixture AttachLineArc(float radians, int sides, float radius, Vector2 position, float angle, bool closed, Body body) + { + Vertices arc = PolygonTools.CreateArc(radians, sides, radius); + arc.Rotate((MathHelper.Pi - radians) / 2 + angle); + arc.Translate(ref position); + + return closed ? AttachLoopShape(arc, body) : AttachChainShape(arc, body); + } + + public static List AttachSolidArc(float density, float radians, int sides, float radius, Vector2 position, float angle, Body body) + { + Vertices arc = PolygonTools.CreateArc(radians, sides, radius); + arc.Rotate((MathHelper.Pi - radians) / 2 + angle); + + arc.Translate(ref position); + + //Close the arc + arc.Add(arc[0]); + + List triangles = Triangulate.ConvexPartition(arc, TriangulationAlgorithm.Earclip); + + return AttachCompoundPolygon(triangles, density, body); + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Factories/JointFactory.cs b/Farseer Physics Engine 3.5/Factories/JointFactory.cs new file mode 100644 index 000000000..ea8b46a87 --- /dev/null +++ b/Farseer Physics Engine 3.5/Factories/JointFactory.cs @@ -0,0 +1,168 @@ +using FarseerPhysics.Dynamics; +using FarseerPhysics.Dynamics.Joints; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Factories +{ + /// + /// An easy to use factory for using joints. + /// + public static class JointFactory + { + #region Motor Joint + + public static MotorJoint CreateMotorJoint(World world, Body bodyA, Body bodyB, bool useWorldCoordinates = false) + { + MotorJoint joint = new MotorJoint(bodyA, bodyB, useWorldCoordinates); + world.AddJoint(joint); + return joint; + } + + #endregion + + #region Revolute Joint + + public static RevoluteJoint CreateRevoluteJoint(World world, Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false) + { + RevoluteJoint joint = new RevoluteJoint(bodyA, bodyB, anchorA, anchorB, useWorldCoordinates); + world.AddJoint(joint); + return joint; + } + + public static RevoluteJoint CreateRevoluteJoint(World world, Body bodyA, Body bodyB, Vector2 anchor) + { + Vector2 localanchorA = bodyA.GetLocalPoint(bodyB.GetWorldPoint(anchor)); + RevoluteJoint joint = new RevoluteJoint(bodyA, bodyB, localanchorA, anchor); + world.AddJoint(joint); + return joint; + } + + + #endregion + + #region Rope Joint + + public static RopeJoint CreateRopeJoint(World world, Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false) + { + RopeJoint ropeJoint = new RopeJoint(bodyA, bodyB, anchorA, anchorB, useWorldCoordinates); + world.AddJoint(ropeJoint); + return ropeJoint; + } + + #endregion + + #region Weld Joint + + public static WeldJoint CreateWeldJoint(World world, Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false) + { + WeldJoint weldJoint = new WeldJoint(bodyA, bodyB, anchorA, anchorB, useWorldCoordinates); + world.AddJoint(weldJoint); + return weldJoint; + } + + #endregion + + #region Prismatic Joint + + public static PrismaticJoint CreatePrismaticJoint(World world, Body bodyA, Body bodyB, Vector2 anchor, Vector2 axis, bool useWorldCoordinates = false) + { + PrismaticJoint joint = new PrismaticJoint(bodyA, bodyB, anchor, axis, useWorldCoordinates); + world.AddJoint(joint); + return joint; + } + + #endregion + + #region Wheel Joint + + public static WheelJoint CreateWheelJoint(World world, Body bodyA, Body bodyB, Vector2 anchor, Vector2 axis, bool useWorldCoordinates = false) + { + WheelJoint joint = new WheelJoint(bodyA, bodyB, anchor, axis, useWorldCoordinates); + world.AddJoint(joint); + return joint; + } + + public static WheelJoint CreateWheelJoint(World world, Body bodyA, Body bodyB, Vector2 axis) + { + return CreateWheelJoint(world, bodyA, bodyB, Vector2.Zero, axis); + } + + #endregion + + #region Angle Joint + + public static AngleJoint CreateAngleJoint(World world, Body bodyA, Body bodyB) + { + AngleJoint angleJoint = new AngleJoint(bodyA, bodyB); + world.AddJoint(angleJoint); + return angleJoint; + } + + #endregion + + #region Distance Joint + + public static DistanceJoint CreateDistanceJoint(World world, Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false) + { + DistanceJoint distanceJoint = new DistanceJoint(bodyA, bodyB, anchorA, anchorB, useWorldCoordinates); + world.AddJoint(distanceJoint); + return distanceJoint; + } + + public static DistanceJoint CreateDistanceJoint(World world, Body bodyA, Body bodyB) + { + return CreateDistanceJoint(world, bodyA, bodyB, Vector2.Zero, Vector2.Zero); + } + + #endregion + + #region Friction Joint + + public static FrictionJoint CreateFrictionJoint(World world, Body bodyA, Body bodyB, Vector2 anchor, bool useWorldCoordinates = false) + { + FrictionJoint frictionJoint = new FrictionJoint(bodyA, bodyB, anchor, useWorldCoordinates); + world.AddJoint(frictionJoint); + return frictionJoint; + } + + public static FrictionJoint CreateFrictionJoint(World world, Body bodyA, Body bodyB) + { + return CreateFrictionJoint(world, bodyA, bodyB, Vector2.Zero); + } + + #endregion + + #region Gear Joint + + public static GearJoint CreateGearJoint(World world, Body bodyA, Body bodyB, Joint jointA, Joint jointB, float ratio) + { + GearJoint gearJoint = new GearJoint(bodyA, bodyB, jointA, jointB, ratio); + world.AddJoint(gearJoint); + return gearJoint; + } + + #endregion + + #region Pulley Joint + + public static PulleyJoint CreatePulleyJoint(World world, Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, Vector2 worldAnchorA, Vector2 worldAnchorB, float ratio, bool useWorldCoordinates = false) + { + PulleyJoint pulleyJoint = new PulleyJoint(bodyA, bodyB, anchorA, anchorB, worldAnchorA, worldAnchorB, ratio, useWorldCoordinates); + world.AddJoint(pulleyJoint); + return pulleyJoint; + } + + #endregion + + #region MouseJoint + + public static FixedMouseJoint CreateFixedMouseJoint(World world, Body body, Vector2 worldAnchor) + { + FixedMouseJoint joint = new FixedMouseJoint(body, worldAnchor); + world.AddJoint(joint); + return joint; + } + + #endregion + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Factories/LinkFactory.cs b/Farseer Physics Engine 3.5/Factories/LinkFactory.cs new file mode 100644 index 000000000..3b3078506 --- /dev/null +++ b/Farseer Physics Engine 3.5/Factories/LinkFactory.cs @@ -0,0 +1,64 @@ +using System.Collections.Generic; +using System.Diagnostics; +using FarseerPhysics.Collision.Shapes; +using FarseerPhysics.Common; +using FarseerPhysics.Dynamics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Factories +{ + public static class LinkFactory + { + /// + /// Creates a chain. + /// + /// The world. + /// The start. + /// The end. + /// The width. + /// The height. + /// The number of links. + /// The link density. + /// Creates a rope joint between start and end. This enforces the length of the rope. Said in another way: it makes the rope less bouncy. + /// + public static Path CreateChain(World world, Vector2 start, Vector2 end, float linkWidth, float linkHeight, int numberOfLinks, float linkDensity, bool attachRopeJoint) + { + Debug.Assert(numberOfLinks >= 2); + + //Chain start / end + Path path = new Path(); + path.Add(start); + path.Add(end); + + //A single chainlink + PolygonShape shape = new PolygonShape(PolygonTools.CreateRectangle(linkWidth, linkHeight), linkDensity); + + //Use PathManager to create all the chainlinks based on the chainlink created before. + List chainLinks = PathManager.EvenlyDistributeShapesAlongPath(world, path, shape, BodyType.Dynamic, numberOfLinks); + + //TODO + //if (fixStart) + //{ + // //Fix the first chainlink to the world + // JointFactory.CreateFixedRevoluteJoint(world, chainLinks[0], new Vector2(0, -(linkHeight / 2)), + // chainLinks[0].Position); + //} + + //if (fixEnd) + //{ + // //Fix the last chainlink to the world + // JointFactory.CreateFixedRevoluteJoint(world, chainLinks[chainLinks.Count - 1], + // new Vector2(0, (linkHeight / 2)), + // chainLinks[chainLinks.Count - 1].Position); + //} + + //Attach all the chainlinks together with a revolute joint + PathManager.AttachBodiesWithRevoluteJoint(world, chainLinks, new Vector2(0, -linkHeight), new Vector2(0, linkHeight), false, false); + + if (attachRopeJoint) + JointFactory.CreateRopeJoint(world, chainLinks[0], chainLinks[chainLinks.Count - 1], Vector2.Zero, Vector2.Zero); + + return (path); + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Farseer Physics Engine.sln b/Farseer Physics Engine 3.5/Farseer Physics Engine.sln new file mode 100644 index 000000000..2ee158511 --- /dev/null +++ b/Farseer Physics Engine 3.5/Farseer Physics Engine.sln @@ -0,0 +1,124 @@ + +Microsoft Visual Studio Solution File, Format Version 11.00 +# Visual Studio 2010 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Farseer Physics MonoGame", "Farseer Physics MonoGame.csproj", "{0AAD36E3-51A5-4A07-AB60-5C8A66BD38B7}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Farseer Physics Silverlight", "Farseer Physics Silverlight.csproj", "{786DC838-D746-4107-93FF-19A902A78CFC}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Farseer Physics WP7", "Farseer Physics WP7.csproj", "{4DA8DC0C-EC1E-4C31-8B00-9C0DB87C5D0A}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Farseer Physics Xbox360", "Farseer Physics Xbox360.csproj", "{448C63FB-8177-4CC6-B2AD-1D44031B3C2E}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Farseer Physics XNA", "Farseer Physics XNA.csproj", "{0A850700-1ABB-4288-A391-6726B90EBEB9}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Farseer Physics", "Farseer Physics.csproj", "{A4610E4C-DD34-428B-BABB-779CA0B5993A}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|Mixed Platforms = Debug|Mixed Platforms + Debug|Windows Phone = Debug|Windows Phone + Debug|x86 = Debug|x86 + Debug|Xbox 360 = Debug|Xbox 360 + Release|Any CPU = Release|Any CPU + Release|Mixed Platforms = Release|Mixed Platforms + Release|Windows Phone = Release|Windows Phone + Release|x86 = Release|x86 + Release|Xbox 360 = Release|Xbox 360 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {0AAD36E3-51A5-4A07-AB60-5C8A66BD38B7}.Debug|Any CPU.ActiveCfg = Debug|x86 + {0AAD36E3-51A5-4A07-AB60-5C8A66BD38B7}.Debug|Mixed Platforms.ActiveCfg = Debug|x86 + {0AAD36E3-51A5-4A07-AB60-5C8A66BD38B7}.Debug|Mixed Platforms.Build.0 = Debug|x86 + {0AAD36E3-51A5-4A07-AB60-5C8A66BD38B7}.Debug|Windows Phone.ActiveCfg = Debug|x86 + {0AAD36E3-51A5-4A07-AB60-5C8A66BD38B7}.Debug|x86.ActiveCfg = Debug|x86 + {0AAD36E3-51A5-4A07-AB60-5C8A66BD38B7}.Debug|x86.Build.0 = Debug|x86 + {0AAD36E3-51A5-4A07-AB60-5C8A66BD38B7}.Debug|Xbox 360.ActiveCfg = Debug|x86 + {0AAD36E3-51A5-4A07-AB60-5C8A66BD38B7}.Release|Any CPU.ActiveCfg = Release|x86 + {0AAD36E3-51A5-4A07-AB60-5C8A66BD38B7}.Release|Mixed Platforms.ActiveCfg = Release|x86 + {0AAD36E3-51A5-4A07-AB60-5C8A66BD38B7}.Release|Mixed Platforms.Build.0 = Release|x86 + {0AAD36E3-51A5-4A07-AB60-5C8A66BD38B7}.Release|Windows Phone.ActiveCfg = Release|x86 + {0AAD36E3-51A5-4A07-AB60-5C8A66BD38B7}.Release|x86.ActiveCfg = Release|x86 + {0AAD36E3-51A5-4A07-AB60-5C8A66BD38B7}.Release|x86.Build.0 = Release|x86 + {0AAD36E3-51A5-4A07-AB60-5C8A66BD38B7}.Release|Xbox 360.ActiveCfg = Release|x86 + {786DC838-D746-4107-93FF-19A902A78CFC}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {786DC838-D746-4107-93FF-19A902A78CFC}.Debug|Any CPU.Build.0 = Debug|Any CPU + {786DC838-D746-4107-93FF-19A902A78CFC}.Debug|Mixed Platforms.ActiveCfg = Debug|Any CPU + {786DC838-D746-4107-93FF-19A902A78CFC}.Debug|Mixed Platforms.Build.0 = Debug|Any CPU + {786DC838-D746-4107-93FF-19A902A78CFC}.Debug|Windows Phone.ActiveCfg = Debug|Any CPU + {786DC838-D746-4107-93FF-19A902A78CFC}.Debug|x86.ActiveCfg = Debug|Any CPU + {786DC838-D746-4107-93FF-19A902A78CFC}.Debug|Xbox 360.ActiveCfg = Debug|Any CPU + {786DC838-D746-4107-93FF-19A902A78CFC}.Release|Any CPU.ActiveCfg = Release|Any CPU + {786DC838-D746-4107-93FF-19A902A78CFC}.Release|Any CPU.Build.0 = Release|Any CPU + {786DC838-D746-4107-93FF-19A902A78CFC}.Release|Mixed Platforms.ActiveCfg = Release|Any CPU + {786DC838-D746-4107-93FF-19A902A78CFC}.Release|Mixed Platforms.Build.0 = Release|Any CPU + {786DC838-D746-4107-93FF-19A902A78CFC}.Release|Windows Phone.ActiveCfg = Release|Any CPU + {786DC838-D746-4107-93FF-19A902A78CFC}.Release|x86.ActiveCfg = Release|Any CPU + {786DC838-D746-4107-93FF-19A902A78CFC}.Release|Xbox 360.ActiveCfg = Release|Any CPU + {4DA8DC0C-EC1E-4C31-8B00-9C0DB87C5D0A}.Debug|Any CPU.ActiveCfg = Debug|Windows Phone + {4DA8DC0C-EC1E-4C31-8B00-9C0DB87C5D0A}.Debug|Mixed Platforms.ActiveCfg = Debug|Windows Phone + {4DA8DC0C-EC1E-4C31-8B00-9C0DB87C5D0A}.Debug|Mixed Platforms.Build.0 = Debug|Windows Phone + {4DA8DC0C-EC1E-4C31-8B00-9C0DB87C5D0A}.Debug|Windows Phone.ActiveCfg = Debug|Windows Phone + {4DA8DC0C-EC1E-4C31-8B00-9C0DB87C5D0A}.Debug|Windows Phone.Build.0 = Debug|Windows Phone + {4DA8DC0C-EC1E-4C31-8B00-9C0DB87C5D0A}.Debug|x86.ActiveCfg = Debug|Windows Phone + {4DA8DC0C-EC1E-4C31-8B00-9C0DB87C5D0A}.Debug|x86.Build.0 = Debug|Windows Phone + {4DA8DC0C-EC1E-4C31-8B00-9C0DB87C5D0A}.Debug|Xbox 360.ActiveCfg = Debug|Windows Phone + {4DA8DC0C-EC1E-4C31-8B00-9C0DB87C5D0A}.Debug|Xbox 360.Build.0 = Debug|Windows Phone + {4DA8DC0C-EC1E-4C31-8B00-9C0DB87C5D0A}.Release|Any CPU.ActiveCfg = Release|Windows Phone + {4DA8DC0C-EC1E-4C31-8B00-9C0DB87C5D0A}.Release|Mixed Platforms.ActiveCfg = Release|Windows Phone + {4DA8DC0C-EC1E-4C31-8B00-9C0DB87C5D0A}.Release|Mixed Platforms.Build.0 = Release|Windows Phone + {4DA8DC0C-EC1E-4C31-8B00-9C0DB87C5D0A}.Release|Windows Phone.ActiveCfg = Release|Windows Phone + {4DA8DC0C-EC1E-4C31-8B00-9C0DB87C5D0A}.Release|Windows Phone.Build.0 = Release|Windows Phone + {4DA8DC0C-EC1E-4C31-8B00-9C0DB87C5D0A}.Release|x86.ActiveCfg = Release|Windows Phone + {4DA8DC0C-EC1E-4C31-8B00-9C0DB87C5D0A}.Release|x86.Build.0 = Release|Windows Phone + {4DA8DC0C-EC1E-4C31-8B00-9C0DB87C5D0A}.Release|Xbox 360.ActiveCfg = Release|Windows Phone + {4DA8DC0C-EC1E-4C31-8B00-9C0DB87C5D0A}.Release|Xbox 360.Build.0 = Release|Windows Phone + {448C63FB-8177-4CC6-B2AD-1D44031B3C2E}.Debug|Any CPU.ActiveCfg = Debug|Xbox 360 + {448C63FB-8177-4CC6-B2AD-1D44031B3C2E}.Debug|Mixed Platforms.ActiveCfg = Debug|Xbox 360 + {448C63FB-8177-4CC6-B2AD-1D44031B3C2E}.Debug|Mixed Platforms.Build.0 = Debug|Xbox 360 + {448C63FB-8177-4CC6-B2AD-1D44031B3C2E}.Debug|Windows Phone.ActiveCfg = Debug|Xbox 360 + {448C63FB-8177-4CC6-B2AD-1D44031B3C2E}.Debug|x86.ActiveCfg = Debug|Xbox 360 + {448C63FB-8177-4CC6-B2AD-1D44031B3C2E}.Debug|x86.Build.0 = Debug|Xbox 360 + {448C63FB-8177-4CC6-B2AD-1D44031B3C2E}.Debug|Xbox 360.ActiveCfg = Debug|Xbox 360 + {448C63FB-8177-4CC6-B2AD-1D44031B3C2E}.Debug|Xbox 360.Build.0 = Debug|Xbox 360 + {448C63FB-8177-4CC6-B2AD-1D44031B3C2E}.Release|Any CPU.ActiveCfg = Release|Xbox 360 + {448C63FB-8177-4CC6-B2AD-1D44031B3C2E}.Release|Mixed Platforms.ActiveCfg = Release|Xbox 360 + {448C63FB-8177-4CC6-B2AD-1D44031B3C2E}.Release|Mixed Platforms.Build.0 = Release|Xbox 360 + {448C63FB-8177-4CC6-B2AD-1D44031B3C2E}.Release|Windows Phone.ActiveCfg = Release|Xbox 360 + {448C63FB-8177-4CC6-B2AD-1D44031B3C2E}.Release|x86.ActiveCfg = Release|Xbox 360 + {448C63FB-8177-4CC6-B2AD-1D44031B3C2E}.Release|x86.Build.0 = Release|Xbox 360 + {448C63FB-8177-4CC6-B2AD-1D44031B3C2E}.Release|Xbox 360.ActiveCfg = Release|Xbox 360 + {448C63FB-8177-4CC6-B2AD-1D44031B3C2E}.Release|Xbox 360.Build.0 = Release|Xbox 360 + {0A850700-1ABB-4288-A391-6726B90EBEB9}.Debug|Any CPU.ActiveCfg = Debug|x86 + {0A850700-1ABB-4288-A391-6726B90EBEB9}.Debug|Mixed Platforms.ActiveCfg = Debug|x86 + {0A850700-1ABB-4288-A391-6726B90EBEB9}.Debug|Mixed Platforms.Build.0 = Debug|x86 + {0A850700-1ABB-4288-A391-6726B90EBEB9}.Debug|Windows Phone.ActiveCfg = Debug|x86 + {0A850700-1ABB-4288-A391-6726B90EBEB9}.Debug|x86.ActiveCfg = Debug|x86 + {0A850700-1ABB-4288-A391-6726B90EBEB9}.Debug|x86.Build.0 = Debug|x86 + {0A850700-1ABB-4288-A391-6726B90EBEB9}.Debug|Xbox 360.ActiveCfg = Debug|x86 + {0A850700-1ABB-4288-A391-6726B90EBEB9}.Release|Any CPU.ActiveCfg = Release|x86 + {0A850700-1ABB-4288-A391-6726B90EBEB9}.Release|Mixed Platforms.ActiveCfg = Release|x86 + {0A850700-1ABB-4288-A391-6726B90EBEB9}.Release|Mixed Platforms.Build.0 = Release|x86 + {0A850700-1ABB-4288-A391-6726B90EBEB9}.Release|Windows Phone.ActiveCfg = Release|x86 + {0A850700-1ABB-4288-A391-6726B90EBEB9}.Release|x86.ActiveCfg = Release|x86 + {0A850700-1ABB-4288-A391-6726B90EBEB9}.Release|x86.Build.0 = Release|x86 + {0A850700-1ABB-4288-A391-6726B90EBEB9}.Release|Xbox 360.ActiveCfg = Release|x86 + {A4610E4C-DD34-428B-BABB-779CA0B5993A}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {A4610E4C-DD34-428B-BABB-779CA0B5993A}.Debug|Any CPU.Build.0 = Debug|Any CPU + {A4610E4C-DD34-428B-BABB-779CA0B5993A}.Debug|Mixed Platforms.ActiveCfg = Debug|Any CPU + {A4610E4C-DD34-428B-BABB-779CA0B5993A}.Debug|Mixed Platforms.Build.0 = Debug|Any CPU + {A4610E4C-DD34-428B-BABB-779CA0B5993A}.Debug|Windows Phone.ActiveCfg = Debug|Any CPU + {A4610E4C-DD34-428B-BABB-779CA0B5993A}.Debug|x86.ActiveCfg = Debug|Any CPU + {A4610E4C-DD34-428B-BABB-779CA0B5993A}.Debug|Xbox 360.ActiveCfg = Debug|Any CPU + {A4610E4C-DD34-428B-BABB-779CA0B5993A}.Release|Any CPU.ActiveCfg = Release|Any CPU + {A4610E4C-DD34-428B-BABB-779CA0B5993A}.Release|Any CPU.Build.0 = Release|Any CPU + {A4610E4C-DD34-428B-BABB-779CA0B5993A}.Release|Mixed Platforms.ActiveCfg = Release|Any CPU + {A4610E4C-DD34-428B-BABB-779CA0B5993A}.Release|Mixed Platforms.Build.0 = Release|Any CPU + {A4610E4C-DD34-428B-BABB-779CA0B5993A}.Release|Windows Phone.ActiveCfg = Release|Any CPU + {A4610E4C-DD34-428B-BABB-779CA0B5993A}.Release|x86.ActiveCfg = Release|Any CPU + {A4610E4C-DD34-428B-BABB-779CA0B5993A}.Release|Xbox 360.ActiveCfg = Release|Any CPU + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/Farseer Physics Engine 3.5/Farseer Physics MonoGame.csproj b/Farseer Physics Engine 3.5/Farseer Physics MonoGame.csproj new file mode 100644 index 000000000..9e4d977f5 --- /dev/null +++ b/Farseer Physics Engine 3.5/Farseer Physics MonoGame.csproj @@ -0,0 +1,180 @@ + + + + Debug + x86 + 8.0.30703 + 2.0 + {0AAD36E3-51A5-4A07-AB60-5C8A66BD38B7} + Library + Properties + FarseerPhysics + FarseerPhysics MonoGame + 512 + + + + + + + + + v4.0 + Client + + + x86 + true + full + false + bin\WindowsGL\Debug\ + DEBUG;TRACE;WINDOWS + prompt + 4 + + + x86 + pdbonly + true + bin\WindowsGL\Release\ + TRACE;WINDOWS + prompt + 4 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + $(MSBuildExtensionsPath)\..\MonoGame\v3.0\Assemblies\WindowsGL\MonoGame.Framework.dll + + + + + + + + \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Farseer Physics Silverlight.csproj b/Farseer Physics Engine 3.5/Farseer Physics Silverlight.csproj new file mode 100644 index 000000000..75ff440c7 --- /dev/null +++ b/Farseer Physics Engine 3.5/Farseer Physics Silverlight.csproj @@ -0,0 +1,533 @@ + + + + Debug + AnyCPU + 8.0.50727 + 2.0 + {786DC838-D746-4107-93FF-19A902A78CFC} + {A1591282-1198-4647-A2B1-27E5FF5F6F3B};{fae04ec0-301f-11d3-bf4b-00c04f79efbc} + Library + Properties + FarseerPhysics + FarseerPhysics Silverlight + Silverlight + v5.0 + $(TargetFrameworkVersion) + false + true + true + + + + + + + + + + + + + v3.5 + + + true + full + false + Bin\Debug + DEBUG;TRACE;SILVERLIGHT + true + true + prompt + 4 + Bin\Debug\FarseerPhysics Silverlight.xml + false + default + + + pdbonly + true + Bin\Release + TRACE;SILVERLIGHT + true + true + prompt + 4 + Bin\Release\FarseerPhysics Silverlight.xml + false + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Farseer Physics WP7.csproj b/Farseer Physics Engine 3.5/Farseer Physics WP7.csproj new file mode 100644 index 000000000..bf418ec1d --- /dev/null +++ b/Farseer Physics Engine 3.5/Farseer Physics WP7.csproj @@ -0,0 +1,265 @@ + + + + {4DA8DC0C-EC1E-4C31-8B00-9C0DB87C5D0A} + {6D335F3A-9D43-41b4-9D22-F6F17C4BE596};{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC} + Debug + Windows Phone + Library + Properties + FarseerPhysics + FarseerPhysics WP7 + v4.0 + v4.0 + Windows Phone + 32c529b3-4d9c-486f-a24f-ba66de6fc565 + false + + + 4.0 + + + Client + Library + Reach + + publish\ + true + Disk + false + Foreground + 7 + Days + false + false + true + 0 + 1.0.0.%2a + false + true + GameThumbnail.png + 1 + $(AssemblyName).xap + Properties\AppManifest.xml + Properties\WMAppManifest.xml + Background.png + $(AssemblyName) + + + + + + + + + + + + bin\Windows Phone\Release + prompt + 4 + true + false + pdbonly + true + TRACE;WINDOWS_PHONE + false + default + + + bin\Windows Phone\Debug + prompt + 4 + true + false + true + full + false + DEBUG;TRACE;WINDOWS_PHONE + false + default + + + + + 129ef00a-26e6-4835-8db3-848cb89276da + False + + + + + False + .NET Framework 3.5 SP1 Client Profile + false + + + False + .NET Framework 2.0 %28x86%29 + false + + + False + .NET Framework 3.0 %28x86%29 + false + + + False + .NET Framework 3.5 + true + + + False + .NET Framework 3.5 SP1 + false + + + False + Windows Installer 3.1 + true + + + False + Microsoft XNA Framework Redistributable 3.1 + true + + + False + Microsoft XNA Framework Redistributable 4.0 + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Farseer Physics XNA.csproj b/Farseer Physics Engine 3.5/Farseer Physics XNA.csproj new file mode 100644 index 000000000..ab0e0567c --- /dev/null +++ b/Farseer Physics Engine 3.5/Farseer Physics XNA.csproj @@ -0,0 +1,266 @@ + + + + {0A850700-1ABB-4288-A391-6726B90EBEB9} + {6D335F3A-9D43-41b4-9D22-F6F17C4BE596};{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC} + Debug + x86 + Library + Properties + FarseerPhysics + FarseerPhysics XNA + v4.0 + v4.0 + Windows + 32c529b3-4d9c-486f-a24f-ba66de6fc565 + false + + + + + + + + + + + 4.0 + + + Client + Library + HiDef + + publish\ + true + Disk + false + Foreground + 7 + Days + false + false + true + 0 + 1.0.0.%2a + false + true + + + true + full + false + bin\x86\Debug + TRACE;DEBUG;WINDOWS;XNA + prompt + 4 + true + false + x86 + false + AllRules.ruleset + bin\x86\Debug\FarseerPhysics XNA.xml + + + pdbonly + true + bin\x86\Release + TRACE;WINDOWS;XNA + prompt + 4 + true + false + x86 + True + AllRules.ruleset + bin\x86\Release\FarseerPhysics XNA.xml + + + + + + False + + + False + + + False + + + 3.5 + False + + + + + 129ef00a-26e6-4835-8db3-848cb89276da + False + + + + + False + .NET Framework 3.5 SP1 Client Profile + false + + + False + .NET Framework 2.0 %28x86%29 + false + + + False + .NET Framework 3.0 %28x86%29 + false + + + False + .NET Framework 3.5 + true + + + False + .NET Framework 3.5 SP1 + false + + + False + Windows Installer 3.1 + true + + + False + Microsoft XNA Framework Redistributable 3.1 + true + + + False + Microsoft XNA Framework Redistributable 4.0 + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Farseer Physics Xbox360.csproj b/Farseer Physics Engine 3.5/Farseer Physics Xbox360.csproj new file mode 100644 index 000000000..6c90b2c1a --- /dev/null +++ b/Farseer Physics Engine 3.5/Farseer Physics Xbox360.csproj @@ -0,0 +1,256 @@ + + + + {448C63FB-8177-4CC6-B2AD-1D44031B3C2E} + {6D335F3A-9D43-41b4-9D22-F6F17C4BE596};{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC} + Debug + Xbox 360 + Library + Properties + FarseerPhysics + FarseerPhysics Xbox360 + v4.0 + v4.0 + Xbox 360 + 32c529b3-4d9c-486f-a24f-ba66de6fc565 + false + + + 4.0 + + + Client + Library + HiDef + + publish\ + true + Disk + false + Foreground + 7 + Days + false + false + true + 0 + 1.0.0.%2a + false + true + GameThumbnail.png + + + + + + + + + + + bin\Xbox 360\Release + prompt + 4 + true + false + pdbonly + true + TRACE;XBOX;XBOX360 + true + default + + + bin\Xbox 360\Debug + prompt + 4 + true + false + true + full + false + DEBUG;TRACE;XBOX;XBOX360 + true + default + + + + + 129ef00a-26e6-4835-8db3-848cb89276da + False + + + + + False + .NET Framework 3.5 SP1 Client Profile + false + + + False + .NET Framework 2.0 %28x86%29 + false + + + False + .NET Framework 3.0 %28x86%29 + false + + + False + .NET Framework 3.5 + true + + + False + .NET Framework 3.5 SP1 + false + + + False + Windows Installer 3.1 + true + + + False + Microsoft XNA Framework Redistributable 3.1 + true + + + False + Microsoft XNA Framework Redistributable 4.0 + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Farseer Physics.csproj b/Farseer Physics Engine 3.5/Farseer Physics.csproj new file mode 100644 index 000000000..6676e8f81 --- /dev/null +++ b/Farseer Physics Engine 3.5/Farseer Physics.csproj @@ -0,0 +1,218 @@ + + + + Debug + AnyCPU + 9.0.30729 + 2.0 + {A4610E4C-DD34-428B-BABB-779CA0B5993A} + Library + Properties + FarseerPhysics + FarseerPhysics + v3.5 + 512 + + + + + + + + + + + 3.5 + + publish\ + true + Disk + false + Foreground + 7 + Days + false + false + true + 0 + 1.0.0.%2a + false + false + true + + + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + AllRules.ruleset + bin\Debug\FarseerPhysics.XML + + + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + AllRules.ruleset + bin\Release\FarseerPhysics.XML + + + + + 3.5 + + + + + + False + .NET Framework 3.5 SP1 Client Profile + false + + + False + .NET Framework 3.5 SP1 + true + + + False + Windows Installer 3.1 + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/Properties/AssemblyInfo.cs b/Farseer Physics Engine 3.5/Properties/AssemblyInfo.cs new file mode 100644 index 000000000..6b62a8340 --- /dev/null +++ b/Farseer Physics Engine 3.5/Properties/AssemblyInfo.cs @@ -0,0 +1,34 @@ +using System.Reflection; +using System.Runtime.InteropServices; + +// General Information about an assembly is controlled through the following +// set of attributes. Change these attribute values to modify the information +// associated with an assembly. +[assembly: AssemblyTitle("Farseer Physics Engine")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("Farseer Physics Engine")] +[assembly: AssemblyProduct("Farseer Physics Engine")] +[assembly: AssemblyCopyright("Copyright Ian Qvist © 2013")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// Setting ComVisible to false makes the types in this assembly not visible +// to COM components. If you need to access a type in this assembly from +// COM, set the ComVisible attribute to true on that type. +[assembly: ComVisible(false)] + +// The following GUID is for the ID of the typelib if this project is exposed to COM +[assembly: Guid("62807528-18aa-4260-9a0f-b7b9f436bc07")] + +// Version information for an assembly consists of the following four values: +// +// Major Version +// Minor Version +// Build Number +// Revision +// +// You can specify all the values or you can default the Revision and Build Numbers +// by using the '*' as shown below: +[assembly: AssemblyVersion("3.5.0.*")] +[assembly: AssemblyFileVersion("3.5.0.*")] diff --git a/Farseer Physics Engine 3.5/Settings.cs b/Farseer Physics Engine 3.5/Settings.cs new file mode 100644 index 000000000..6968d46ee --- /dev/null +++ b/Farseer Physics Engine 3.5/Settings.cs @@ -0,0 +1,285 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using FarseerPhysics.Dynamics; + +namespace FarseerPhysics +{ + public static class Settings + { + public const float MaxFloat = 3.402823466e+38f; + public const float Epsilon = 1.192092896e-07f; + public const float Pi = 3.14159265359f; + + // Common + + /// + /// If true, all collision callbacks have to return the same value, and agree + /// if there was a collision or not. Swtich this to false to revert to the + /// collision agreement used in FPE 3.3.x + /// + public const bool AllCollisionCallbacksAgree = true; + + /// + /// Enabling diagnistics causes the engine to gather timing information. + /// You can see how much time it took to solve the contacts, solve CCD + /// and update the controllers. + /// NOTE: If you are using a debug view that shows performance counters, + /// you might want to enable this. + /// + public const bool EnableDiagnostics = true; + + /// + /// Set this to true to skip sanity checks in the engine. This will speed up the + /// tools by removing the overhead of the checks, but you will need to handle checks + /// yourself where it is needed. + /// + public const bool SkipSanityChecks = false; + + /// + /// The number of velocity iterations used in the solver. + /// + public static int VelocityIterations = 8; + + /// + /// The number of position iterations used in the solver. + /// + public static int PositionIterations = 3; + + /// + /// Enable/Disable Continuous Collision Detection (CCD) + /// + public static bool ContinuousPhysics = true; + + /// + /// If true, it will run a GiftWrap convex hull on all polygon inputs. + /// This makes for a more stable engine when given random input, + /// but if speed of the creation of polygons are more important, + /// you might want to set this to false. + /// + public static bool UseConvexHullPolygons = true; + + /// + /// The number of velocity iterations in the TOI solver + /// + public static int TOIVelocityIterations = VelocityIterations; + + /// + /// The number of position iterations in the TOI solver + /// + public static int TOIPositionIterations = 20; + + /// + /// Maximum number of sub-steps per contact in continuous physics simulation. + /// + public const int MaxSubSteps = 8; + + /// + /// Enable/Disable warmstarting + /// + public const bool EnableWarmstarting = true; + + /// + /// Enable/Disable sleeping + /// + public static bool AllowSleep = true; + + /// + /// The maximum number of vertices on a convex polygon. + /// + public static int MaxPolygonVertices = 8; + + /// + /// Farseer Physics Engine has a different way of filtering fixtures than Box2d. + /// We have both FPE and Box2D filtering in the engine. If you are upgrading + /// from earlier versions of FPE, set this to true and DefaultFixtureCollisionCategories + /// to Category.All. + /// + public static bool UseFPECollisionCategories; + + /// + /// This is used by the Fixture constructor as the default value + /// for Fixture.CollisionCategories member. Note that you may need to change this depending + /// on the setting of UseFPECollisionCategories, above. + /// + public static Category DefaultFixtureCollisionCategories = Category.Cat1; + + /// + /// This is used by the Fixture constructor as the default value + /// for Fixture.CollidesWith member. + /// + public static Category DefaultFixtureCollidesWith = Category.All; + + + /// + /// This is used by the Fixture constructor as the default value + /// for Fixture.IgnoreCCDWith member. + /// + public static Category DefaultFixtureIgnoreCCDWith = Category.None; + + /// + /// The maximum number of contact points between two convex shapes. + /// DO NOT CHANGE THIS VALUE! + /// + public const int MaxManifoldPoints = 2; + + /// + /// This is used to fatten AABBs in the dynamic tree. This allows proxies + /// to move by a small amount without triggering a tree adjustment. + /// This is in meters. + /// + public const float AABBExtension = 0.1f; + + /// + /// This is used to fatten AABBs in the dynamic tree. This is used to predict + /// the future position based on the current displacement. + /// This is a dimensionless multiplier. + /// + public const float AABBMultiplier = 2.0f; + + /// + /// A small length used as a collision and constraint tolerance. Usually it is + /// chosen to be numerically significant, but visually insignificant. + /// + public const float LinearSlop = 0.005f; + + /// + /// A small angle used as a collision and constraint tolerance. Usually it is + /// chosen to be numerically significant, but visually insignificant. + /// + public const float AngularSlop = (2.0f / 180.0f * Pi); + + /// + /// The radius of the polygon/edge shape skin. This should not be modified. Making + /// this smaller means polygons will have an insufficient buffer for continuous collision. + /// Making it larger may create artifacts for vertex collision. + /// + public const float PolygonRadius = (2.0f * LinearSlop); + + // Dynamics + + /// + /// Maximum number of contacts to be handled to solve a TOI impact. + /// + public const int MaxTOIContacts = 32; + + /// + /// A velocity threshold for elastic collisions. Any collision with a relative linear + /// velocity below this threshold will be treated as inelastic. + /// + public const float VelocityThreshold = 1.0f; + + /// + /// The maximum linear position correction used when solving constraints. This helps to + /// prevent overshoot. + /// + public const float MaxLinearCorrection = 0.2f; + + /// + /// The maximum angular position correction used when solving constraints. This helps to + /// prevent overshoot. + /// + public const float MaxAngularCorrection = (8.0f / 180.0f * Pi); + + /// + /// This scale factor controls how fast overlap is resolved. Ideally this would be 1 so + /// that overlap is removed in one time step. However using values close to 1 often lead + /// to overshoot. + /// + public const float Baumgarte = 0.2f; + + // Sleep + /// + /// The time that a body must be still before it will go to sleep. + /// + public const float TimeToSleep = 0.5f; + + /// + /// A body cannot sleep if its linear velocity is above this tolerance. + /// + public const float LinearSleepTolerance = 0.01f; + + /// + /// A body cannot sleep if its angular velocity is above this tolerance. + /// + public const float AngularSleepTolerance = (2.0f / 180.0f * Pi); + + /// + /// The maximum linear velocity of a body. This limit is very large and is used + /// to prevent numerical problems. You shouldn't need to adjust this. + /// + public const float MaxTranslation = 2.0f; + + public const float MaxTranslationSquared = (MaxTranslation * MaxTranslation); + + /// + /// The maximum angular velocity of a body. This limit is very large and is used + /// to prevent numerical problems. You shouldn't need to adjust this. + /// + public const float MaxRotation = (0.5f * Pi); + + public const float MaxRotationSquared = (MaxRotation * MaxRotation); + + /// + /// Defines the maximum number of iterations made by the GJK algorithm. + /// + public const int MaxGJKIterations = 20; + + /// + /// This is only for debugging the solver + /// + public const bool EnableSubStepping = false; + + /// + /// By default, forces are cleared automatically after each call to Step. + /// The default behavior is modified with this setting. + /// The purpose of this setting is to support sub-stepping. Sub-stepping is often used to maintain + /// a fixed sized time step under a variable frame-rate. + /// When you perform sub-stepping you should disable auto clearing of forces and instead call + /// ClearForces after all sub-steps are complete in one pass of your game loop. + /// + public const bool AutoClearForces = true; + + /// + /// Friction mixing law. Feel free to customize this. + /// + /// The friction1. + /// The friction2. + /// + public static float MixFriction(float friction1, float friction2) + { + return (float)Math.Sqrt(friction1 * friction2); + } + + /// + /// Restitution mixing law. Feel free to customize this. + /// + /// The restitution1. + /// The restitution2. + /// + public static float MixRestitution(float restitution1, float restitution2) + { + return restitution1 > restitution2 ? restitution1 : restitution2; + } + } +} \ No newline at end of file diff --git a/Farseer Physics Engine 3.5/StyleCop.Cache b/Farseer Physics Engine 3.5/StyleCop.Cache new file mode 100644 index 000000000..67b8f419a --- /dev/null +++ b/Farseer Physics Engine 3.5/StyleCop.Cache @@ -0,0 +1,6 @@ + + 12 + + DEBUG;TRACE;WINDOWS + + \ No newline at end of file diff --git a/Subsurface/Characters/CharacterInfo.cs b/Subsurface/Characters/CharacterInfo.cs index bb894014f..161bde969 100644 --- a/Subsurface/Characters/CharacterInfo.cs +++ b/Subsurface/Characters/CharacterInfo.cs @@ -24,7 +24,7 @@ namespace Subsurface // return gender.ToString(); //} - public CharacterInfo(string file, string name = "", Gender gender = Gender.None, Job job = null) + public CharacterInfo(string file, string name = "", Gender gender = Gender.None, JobPrefab jobPrefab = null) { this.File = file; @@ -62,7 +62,7 @@ namespace Subsurface HeadSpriteId = Rand.Range((int)headSpriteRange.X, (int)headSpriteRange.Y + 1); } - this.Job = (job == null) ? Job.Random() : job; + this.Job = (jobPrefab == null) ? Job.Random() : new Job(jobPrefab); if (!string.IsNullOrEmpty(name)) { @@ -72,14 +72,14 @@ namespace Subsurface if (doc.Root.Element("name") != null) { - string firstNamePath = (ToolBox.GetAttributeString(doc.Root.Element("name"), "firstname", "")); + string firstNamePath = ToolBox.GetAttributeString(doc.Root.Element("name"), "firstname", ""); if (firstNamePath != "") { firstNamePath = firstNamePath.Replace("[GENDER]", (this.Gender == Gender.Female) ? "f" : ""); this.Name = ToolBox.GetRandomLine(firstNamePath); } - string lastNamePath = (ToolBox.GetAttributeString(doc.Root.Element("name"), "lastname", "")); + string lastNamePath = ToolBox.GetAttributeString(doc.Root.Element("name"), "lastname", ""); if (lastNamePath != "") { lastNamePath = lastNamePath.Replace("[GENDER]", (this.Gender == Gender.Female) ? "f" : ""); @@ -101,21 +101,31 @@ namespace Subsurface Salary = ToolBox.GetAttributeInt(element, "salary", 1000); HeadSpriteId = ToolBox.GetAttributeInt(element, "headspriteid", 1); + + foreach (XElement subElement in element.Elements()) + { + if (subElement.Name.ToString().ToLower() != "job") continue; + + Job = new Job(subElement); + break; + } } public virtual XElement Save(XElement parentElement) { - XElement componentElement = new XElement("character"); + XElement charElement = new XElement("character"); - componentElement.Add( + charElement.Add( new XAttribute("name", Name), new XAttribute("file", File), new XAttribute("gender", Gender == Gender.Male ? "male" : "female"), new XAttribute("salary", Salary), new XAttribute("headspriteid", HeadSpriteId)); - parentElement.Add(componentElement); - return componentElement; + Job.Save(charElement); + + parentElement.Add(charElement); + return charElement; } } } diff --git a/Subsurface/Characters/HumanoidAnimController.cs b/Subsurface/Characters/HumanoidAnimController.cs index 757900651..201a801c8 100644 --- a/Subsurface/Characters/HumanoidAnimController.cs +++ b/Subsurface/Characters/HumanoidAnimController.cs @@ -98,9 +98,7 @@ namespace Subsurface { floorY = rayStart.Y + (rayEnd.Y - rayStart.Y) * closestFraction; } - - System.Diagnostics.Debug.WriteLine(floorY+" - "+inWater); - + IgnorePlatforms = (TargetMovement.Y < 0.0f); diff --git a/Subsurface/Characters/Job.cs b/Subsurface/Characters/Job.cs deleted file mode 100644 index e1973c788..000000000 --- a/Subsurface/Characters/Job.cs +++ /dev/null @@ -1,6 +0,0 @@ -namespace Subsurface.Characters -{ - class Job - { - } -} diff --git a/Subsurface/Characters/Jobs/Job.cs b/Subsurface/Characters/Jobs/Job.cs index 3780b87b6..4cb711608 100644 --- a/Subsurface/Characters/Jobs/Job.cs +++ b/Subsurface/Characters/Jobs/Job.cs @@ -3,6 +3,7 @@ using System; using System.Collections.Generic; using System.Linq; using System.Text; +using System.Xml.Linq; namespace Subsurface { @@ -23,17 +24,29 @@ namespace Subsurface get { return prefab.Description; } } + public Job(JobPrefab jobPrefab) { prefab = jobPrefab; skills = new Dictionary(); - foreach (KeyValuePair skill in prefab.skills) + foreach (KeyValuePair skill in prefab.Skills) { skills.Add(skill.Key, Rand.Range(skill.Value.X, skill.Value.Y, false)); } } + public Job(XElement element) + { + string name = ToolBox.GetAttributeString(element, "name", "").ToLower(); + prefab = JobPrefab.List.Find(jp => jp.Name.ToLower() == name); + + foreach (XElement subElement in element.Elements()) + { + skills.Add(subElement.Name.ToString(), ToolBox.GetAttributeFloat(subElement, "level", 0.0f)); + } + } + public static Job Random() { JobPrefab prefab = JobPrefab.List[Rand.Int(JobPrefab.List.Count-1, false)]; @@ -48,5 +61,20 @@ namespace Subsurface return skillLevel; } + + public virtual XElement Save(XElement parentElement) + { + XElement jobElement = new XElement("job"); + + jobElement.Add(new XAttribute("name", Name)); + + foreach (KeyValuePair skill in skills) + { + jobElement.Add(new XElement(skill.Key, new XAttribute("level", skill.Value))); + } + + parentElement.Add(jobElement); + return jobElement; + } } } diff --git a/Subsurface/Characters/Jobs/JobPrefab.cs b/Subsurface/Characters/Jobs/JobPrefab.cs index 830ff1eb9..2ab9a2416 100644 --- a/Subsurface/Characters/Jobs/JobPrefab.cs +++ b/Subsurface/Characters/Jobs/JobPrefab.cs @@ -11,11 +11,25 @@ namespace Subsurface string name; string description; + + //how many crew members can have the job (only one captain etc) + private int maxNumber; + + //how many crew members are REQUIRED to have a job + //(i.e. if one captain is required, one captain is chosen even if all the players have set captain to lowest preference) + private int minNumber; + + //if set to true, a client that has chosen this as their preferred job will get it no matter what + public bool AllowAlways + { + get; + private set; + } //names of the items the character spawns with - public List itemNames; + public List ItemNames; - public Dictionary skills; + public Dictionary Skills; public string Name { @@ -27,19 +41,15 @@ namespace Subsurface get { return description; } } - //public float GetSkill(string skillName) - //{ - // float skillLevel = 0.0f; - // if (skills.TryGetValue(skillName.ToLower(), out skillLevel)) - // { - // return skillLevel; - // } - // else - // { - // DebugConsole.ThrowError("Skill ''"+skillName+" not found!"); - // return skillLevel; - // } - //} + public int MaxNumber + { + get { return maxNumber; } + } + + public int MinNumber + { + get { return minNumber; } + } public JobPrefab(XElement element) { @@ -47,9 +57,14 @@ namespace Subsurface description = ToolBox.GetAttributeString(element, "description", ""); - itemNames = new List(); + minNumber = ToolBox.GetAttributeInt(element, "minnumber", 0); + maxNumber = ToolBox.GetAttributeInt(element, "maxnumber", 10); - skills = new Dictionary(); + AllowAlways = ToolBox.GetAttributeBool(element, "allowalways", false); + + ItemNames = new List(); + + Skills = new Dictionary(); foreach (XElement subElement in element.Elements()) { @@ -57,7 +72,7 @@ namespace Subsurface { case "item": string itemName = ToolBox.GetAttributeString(subElement, "name", ""); - if (!string.IsNullOrEmpty(itemName)) itemNames.Add(itemName); + if (!string.IsNullOrEmpty(itemName)) ItemNames.Add(itemName); break; case "skills": LoadSkills(subElement); @@ -70,18 +85,18 @@ namespace Subsurface { foreach (XElement subElement in element.Elements()) { - string skillName = subElement.Name.ToString().ToLower(); - if (skills.ContainsKey(skillName)) continue; + string skillName = subElement.Name.ToString(); + if (Skills.ContainsKey(skillName)) continue; var levelAttribute = subElement.Attribute("level").ToString(); if (levelAttribute.Contains("'")) { - skills.Add(skillName, ToolBox.ParseToVector2(levelAttribute, false)); + Skills.Add(skillName, ToolBox.ParseToVector2(levelAttribute, false)); } else { float skillLevel = float.Parse(levelAttribute, CultureInfo.InvariantCulture); - skills.Add(skillName, new Vector2(skillLevel, skillLevel)); + Skills.Add(skillName, new Vector2(skillLevel, skillLevel)); } } diff --git a/Subsurface/DebugConsole.cs b/Subsurface/DebugConsole.cs index 9491fa361..eeeb67e3e 100644 --- a/Subsurface/DebugConsole.cs +++ b/Subsurface/DebugConsole.cs @@ -146,7 +146,7 @@ namespace Subsurface if (commands[1].ToLower()=="human") { - WayPoint spawnPoint = WayPoint.GetRandom(WayPoint.SpawnType.Human); + WayPoint spawnPoint = WayPoint.GetRandom(SpawnType.Human); Character.Controlled = new Character("Content/Characters/Human/human.xml", (spawnPoint == null) ? Vector2.Zero : spawnPoint.SimPosition); if (Game1.GameSession != null) { @@ -158,7 +158,7 @@ namespace Subsurface } else { - WayPoint spawnPoint = WayPoint.GetRandom(WayPoint.SpawnType.Enemy); + WayPoint spawnPoint = WayPoint.GetRandom(SpawnType.Enemy); new Character("Content/Characters/" + commands[1] + "/" + commands[1] + ".xml", (spawnPoint == null) ? Vector2.Zero : spawnPoint.SimPosition); } diff --git a/Subsurface/Events/MonsterEvent.cs b/Subsurface/Events/MonsterEvent.cs index b6fd61ec6..0b2981616 100644 --- a/Subsurface/Events/MonsterEvent.cs +++ b/Subsurface/Events/MonsterEvent.cs @@ -23,7 +23,7 @@ namespace Subsurface protected override void Start() { - WayPoint randomWayPoint = WayPoint.GetRandom(WayPoint.SpawnType.Enemy); + WayPoint randomWayPoint = WayPoint.GetRandom(SpawnType.Enemy); int amount = Rand.Range(minAmount, maxAmount, false); diff --git a/Subsurface/Events/ScriptedEvent.cs b/Subsurface/Events/ScriptedEvent.cs index b02b84e0e..3dd7c7c04 100644 --- a/Subsurface/Events/ScriptedEvent.cs +++ b/Subsurface/Events/ScriptedEvent.cs @@ -92,11 +92,13 @@ namespace Subsurface } - public static ScriptedEvent LoadRandom() + public static ScriptedEvent LoadRandom(string seed) { XDocument doc = ToolBox.TryLoadXml(configFile); if (doc == null) return null; + Random rand = new Random(seed.GetHashCode()); + int eventCount = doc.Root.Elements().Count(); //int[] commonness = new int[eventCount]; float[] eventProbability = new float[eventCount]; @@ -123,9 +125,9 @@ namespace Subsurface probabilitySum += eventProbability[i]; i++; - } + } - float randomNumber = Rand.Range(0.0f,probabilitySum); + float randomNumber = (float)rand.NextDouble() * probabilitySum; i = 0; foreach (XElement element in doc.Root.Elements()) diff --git a/Subsurface/Events/TaskManager.cs b/Subsurface/Events/TaskManager.cs index a6bccadfd..531d20f78 100644 --- a/Subsurface/Events/TaskManager.cs +++ b/Subsurface/Events/TaskManager.cs @@ -45,9 +45,9 @@ namespace Subsurface tasks.Add(newTask); } - public void StartShift(int scriptedEventCount) + public void StartShift(Level level) { - CreateScriptedEvents(scriptedEventCount); + CreateScriptedEvents(level); taskListBox.ClearChildren(); } @@ -59,13 +59,11 @@ namespace Subsurface tasks.Clear(); } - private void CreateScriptedEvents(int scriptedEventCount) + private void CreateScriptedEvents(Level level) { - for (int i = 0; i < scriptedEventCount; i++) - { - ScriptedEvent scriptedEvent = ScriptedEvent.LoadRandom(); - AddTask(new ScriptedTask(scriptedEvent)); - } + ScriptedEvent scriptedEvent = ScriptedEvent.LoadRandom(level.Seed); + AddTask(new ScriptedTask(scriptedEvent)); + } public void TaskStarted(Task task) diff --git a/Subsurface/GUI/GUI.cs b/Subsurface/GUI/GUI.cs index 86303ad09..f31ba8eaf 100644 --- a/Subsurface/GUI/GUI.cs +++ b/Subsurface/GUI/GUI.cs @@ -264,15 +264,18 @@ namespace Subsurface public static void Draw(float deltaTime, SpriteBatch spriteBatch, Camera cam) { - //spriteBatch.DrawString(font, - // "FPS: " + (int)Game1.frameCounter.AverageFramesPerSecond - // + " - render: " + Game1.renderTimeElapsed, - // new Vector2(10, 10), Color.White); + spriteBatch.DrawString(font, + "FPS: " + (int)Game1.frameCounter.AverageFramesPerSecond, + new Vector2(10, 10), Color.White); - //spriteBatch.DrawString(font, - // "Physics: " + Game1.world.UpdateTime - // + " - bodies: " + Game1.world.BodyList.Count, - // new Vector2(10, 30), Color.White); + spriteBatch.DrawString(font, + "Physics: " + Game1.World.UpdateTime + + " - bodies: " + Game1.World.BodyList.Count, + new Vector2(10, 30), Color.White); + + spriteBatch.DrawString(font, + "Camera pos: " + Game1.GameScreen.Cam.Position, + new Vector2(10, 50), Color.White); if (Character.Controlled != null && cam!=null) Character.Controlled.DrawHud(spriteBatch, cam); diff --git a/Subsurface/GUI/GUIComponent.cs b/Subsurface/GUI/GUIComponent.cs index 40063086e..c033071c8 100644 --- a/Subsurface/GUI/GUIComponent.cs +++ b/Subsurface/GUI/GUIComponent.cs @@ -142,6 +142,8 @@ namespace Subsurface sprites = new List(); children = new List(); + + if (style!=null) style.Apply(this); } diff --git a/Subsurface/GUI/GUITextBox.cs b/Subsurface/GUI/GUITextBox.cs index 9967b3c6c..f5f334500 100644 --- a/Subsurface/GUI/GUITextBox.cs +++ b/Subsurface/GUI/GUITextBox.cs @@ -89,9 +89,11 @@ namespace Subsurface public GUITextBox(Rectangle rect, Color? color, Color? textColor, Alignment alignment, Alignment textAlignment = Alignment.Left, GUIStyle style = null, GUIComponent parent = null) : base(style) { + Enabled = true; + this.rect = rect; - if (color!=null) this.color = (Color)color; + if (color != null) this.color = (Color)color; this.alignment = alignment; diff --git a/Subsurface/GameSession/CrewManager.cs b/Subsurface/GameSession/CrewManager.cs index 3e1d0c008..a8837b431 100644 --- a/Subsurface/GameSession/CrewManager.cs +++ b/Subsurface/GameSession/CrewManager.cs @@ -119,7 +119,7 @@ namespace Subsurface foreach (CharacterInfo ci in characterInfos) { - WayPoint randomWayPoint = WayPoint.GetRandom(WayPoint.SpawnType.Human); + WayPoint randomWayPoint = WayPoint.GetRandom(SpawnType.Human); Vector2 position = (randomWayPoint == null) ? Vector2.Zero : randomWayPoint.SimPosition; Character character = new Character(ci.File, position, ci); diff --git a/Subsurface/GameSession/GameSession.cs b/Subsurface/GameSession/GameSession.cs index b11ad8bb0..7ab807ebe 100644 --- a/Subsurface/GameSession/GameSession.cs +++ b/Subsurface/GameSession/GameSession.cs @@ -111,14 +111,14 @@ namespace Subsurface this.savePath = savePath; } - public void StartShift(TimeSpan duration, string levelSeed, int scriptedEventCount = 1) + public void StartShift(TimeSpan duration, string levelSeed) { Level level = Level.CreateRandom(levelSeed); - StartShift(duration, level, scriptedEventCount); + StartShift(duration, level); } - public void StartShift(TimeSpan duration, Level level, int scriptedEventCount = 1) + public void StartShift(TimeSpan duration, Level level) { //if (crewManager.characterInfos.Count == 0) return; @@ -135,7 +135,7 @@ namespace Subsurface } //crewManager.StartShift(); - taskManager.StartShift(scriptedEventCount); + taskManager.StartShift(level); } public void EndShift(string endMessage) diff --git a/Subsurface/Items/Item.cs b/Subsurface/Items/Item.cs index f66989118..34e0330f2 100644 --- a/Subsurface/Items/Item.cs +++ b/Subsurface/Items/Item.cs @@ -285,7 +285,7 @@ namespace Subsurface { base.Move(amount); - if (itemList != null && body!=null) + if (itemList != null && body != null) { amount = ConvertUnits.ToSimUnits(amount); //Vector2 pos = new Vector2(rect.X + rect.Width / 2.0f, rect.Y - rect.Height / 2.0f); @@ -296,7 +296,7 @@ namespace Subsurface ic.Move(amount); } - FindHull(); + if (body != null) FindHull(); } public Rectangle TransformTrigger(Rectangle trigger) diff --git a/Subsurface/Map/Level.cs b/Subsurface/Map/Level.cs index a864b8bee..0ef88cee5 100644 --- a/Subsurface/Map/Level.cs +++ b/Subsurface/Map/Level.cs @@ -51,6 +51,11 @@ namespace Subsurface get { return ConvertUnits.ToDisplayUnits(cells[0].body.Position); } } + public string Seed + { + get { return seed; } + } + public Level(string seed, int width, int height, int siteInterval) { this.seed = seed; @@ -66,7 +71,7 @@ namespace Subsurface { seed = Rand.Range(0, int.MaxValue, false).ToString(); } - return new Level((string)seed, 100000, 40000, 2000); + return new Level(seed, 100000, 40000, 2000); } public void Generate(float minWidth) @@ -86,7 +91,10 @@ namespace Subsurface Voronoi voronoi = new Voronoi(1.0); List sites = new List(); - Random rand = new Random(ToolBox.SeedToInt(seed)); + + int aa = seed.GetHashCode(); + + Random rand = new Random(seed.GetHashCode()); float siteVariance = siteInterval * 0.8f; for (int x = siteInterval / 2; x < borders.Width; x += siteInterval) @@ -153,21 +161,29 @@ namespace Subsurface //generate a path from the left edge of the map to right edge Rectangle pathBorders = new Rectangle( - borders.X + (int)minWidth, borders.Y + (int)minWidth, - borders.Right - (int)minWidth, borders.Y + borders.Height - (int)minWidth); + borders.X + (int)minWidth * 2, borders.Y + (int)minWidth * 2, + borders.Right - (int)minWidth * 4, borders.Y + borders.Height - (int)minWidth * 4); List pathCells = GeneratePath(rand, - new Vector2((int)minWidth, rand.Next((int)minWidth, borders.Height - (int)minWidth)), - new Vector2(borders.Width - (int)minWidth, rand.Next((int)minWidth, borders.Height - (int)minWidth)), + new Vector2((int)minWidth * 2, rand.Next((int)minWidth * 2, borders.Height - (int)minWidth * 2)), + new Vector2(borders.Width - (int)minWidth * 2, rand.Next((int)minWidth * 2, borders.Height - (int)minWidth * 2)), cells, pathBorders, minWidth); + for (int i = 0; i <3 ; i++ ) + { + Vector2 position = pathCells[rand.Next((int)(pathCells.Count*0.5f), pathCells.Count - 2)].Center; + WayPoint wayPoint = new WayPoint(new Rectangle((int)position.X, (int)position.Y, 10, 10)); + wayPoint.MoveWithLevel = true; + wayPoint.SpawnType = SpawnType.Enemy; + } + //generate a couple of random paths for (int i = 0; i < rand.Next() % 3; i++) { - pathBorders = new Rectangle( - borders.X + siteInterval * 2, borders.Y - siteInterval * 2, - borders.Right - siteInterval * 2, borders.Y + borders.Height - siteInterval * 2); + //pathBorders = new Rectangle( + //borders.X + siteInterval * 2, borders.Y - siteInterval * 2, + //borders.Right - siteInterval * 2, borders.Y + borders.Height - siteInterval * 2); Vector2 start = pathCells[rand.Next(1, pathCells.Count - 2)].Center; @@ -236,7 +252,6 @@ namespace Subsurface private List GeneratePath(Random rand, Vector2 start, Vector2 end, List cells, Microsoft.Xna.Framework.Rectangle limits, float minWidth, float wanderAmount = 0.3f) { - Stopwatch sw2 = new Stopwatch(); sw2.Start(); @@ -503,6 +518,13 @@ namespace Subsurface bodyPoints[i] = ConvertUnits.ToSimUnits(bodyPoints[i]); } + for (int i = bodyPoints.Count-1; i >0 ; i--) + { + if (Vector2.Distance(bodyPoints[i], bodyPoints[i - 1]) < 0.1f) bodyPoints.RemoveAt(i); + } + + if (bodyPoints.Count < 2) continue; + Vertices bodyVertices = new Vertices(bodyPoints); Body edgeBody = BodyFactory.CreateLoopShape(Game1.World, bodyVertices); @@ -528,12 +550,28 @@ namespace Subsurface public void SetPosition(Vector2 pos) { - Vector2 amount = ConvertUnits.ToSimUnits(pos - Position); + Vector2 amount = pos - Position; + Vector2 simAmount = ConvertUnits.ToSimUnits(amount); foreach (VoronoiCell cell in cells) { if (cell.body == null) continue; cell.body.SleepingAllowed = false; - cell.body.SetTransform(cell.body.Position + amount, cell.body.Rotation); + cell.body.SetTransform(cell.body.Position + simAmount, cell.body.Rotation); + } + + foreach (MapEntity mapEntity in MapEntity.mapEntityList) + { + Item item = mapEntity as Item; + if (item == null) + { + if (!mapEntity.MoveWithLevel) continue; + mapEntity.Move(amount); + } + else if (item.body != null) + { + if (item.CurrentHull != null) continue; + item.SetTransform(item.SimPosition+amount, item.body.Rotation); + } } } @@ -564,25 +602,27 @@ namespace Subsurface } else { - if (limb.type == LimbType.LeftFoot || limb.type == LimbType.RightFoot) continue; - limb.body.ApplyForce((simVelocity - prevVelocity) * 10.0f * limb.Mass); + //if (limb.type == LimbType.LeftFoot || limb.type == LimbType.RightFoot) continue; + //limb.body.ApplyForce((simVelocity - prevVelocity) * 10.0f * limb.Mass); } } } - foreach (Item item in Item.itemList) - { - if (item.CurrentHull != null) continue; - if (item.body == null) + foreach (MapEntity mapEntity in MapEntity.mapEntityList) + { + Item item = mapEntity as Item; + if (item == null) { - item.Move(velocity); + if (!mapEntity.MoveWithLevel) continue; + mapEntity.Move(velocity); } - else + else if (item.body!=null) { + if (item.CurrentHull != null) continue; item.body.LinearVelocity += simVelocity; } } - + prevVelocity = simVelocity; } diff --git a/Subsurface/Map/MapEntity.cs b/Subsurface/Map/MapEntity.cs index 13cf61fa1..62f369eed 100644 --- a/Subsurface/Map/MapEntity.cs +++ b/Subsurface/Map/MapEntity.cs @@ -36,6 +36,12 @@ namespace Subsurface protected bool isHighlighted; protected bool isSelected; + + public bool MoveWithLevel + { + get; + set; + } //the position and dimensions of the entity protected Rectangle rect; diff --git a/Subsurface/Map/Submarine.cs b/Subsurface/Map/Submarine.cs index e7ea328aa..ac8e5ede7 100644 --- a/Subsurface/Map/Submarine.cs +++ b/Subsurface/Map/Submarine.cs @@ -148,6 +148,8 @@ namespace Subsurface //this.mapHash = new MapHash(md5Hash); } + base.Remove(); + } private List GenerateConvexHull() @@ -410,7 +412,7 @@ namespace Subsurface if (targetPosition != Vector2.Zero && Vector2.Distance(targetPosition, Position) > 5.0f) { - translateAmount += (targetPosition - Position)*0.1f; + translateAmount += (targetPosition - Position) * 0.05f; } else { @@ -433,7 +435,7 @@ namespace Subsurface //hullBodies[0].body.LinearVelocity = -hullBodies[0].body.Position; //hullBody.SetTransform(Vector2.Zero , 0.0f); - hullBody.LinearVelocity = -hullBody.Position; + hullBody.LinearVelocity = -hullBody.Position/(float)Physics.step; if (collidingCell == null) { @@ -515,13 +517,18 @@ namespace Subsurface VoronoiCell cell = f2.Body.UserData as VoronoiCell; if (cell==null) return true; - Vector2 normal = contact.Manifold.LocalNormal; - float impact = Vector2.Dot(ConvertUnits.ToSimUnits(speed), normal); + Vector2 normal = -contact.Manifold.LocalNormal; + Vector2 simSpeed = ConvertUnits.ToSimUnits(speed); + float impact = -Vector2.Dot(simSpeed, normal); + + Vector2 u = Vector2.Dot(simSpeed, normal)*normal; + Vector2 w = simSpeed - u; + + speed = ConvertUnits.ToDisplayUnits(w*f2.Body.Friction - u*0.5f); System.Diagnostics.Debug.WriteLine("IMPACT:"+impact); if (impact < 5.0f) return true; - collisionRigidness = 0.8f; collidingCell = cell; @@ -540,6 +547,9 @@ namespace Subsurface message.Write(Position.X); message.Write(Position.Y); + message.Write(speed.X); + message.Write(speed.Y); + } public override void ReadNetworkData(Networking.NetworkEventType type, NetIncomingMessage message) @@ -548,15 +558,16 @@ namespace Subsurface if (sendingTime <= lastNetworkUpdate) return; - Vector2 newPosition = new Vector2(message.ReadFloat(), message.ReadFloat()); - if (newPosition == Position) return; - if ((newPosition - Position).Length() > 500.0f) - { - System.Diagnostics.Debug.WriteLine("Submarine has moved over 500 pixels since last update"); - return; - } + //Vector2 newPosition = + //if (newPosition == Position) return; + //if ((newPosition - Position).Length() > 500.0f) + //{ + // System.Diagnostics.Debug.WriteLine("Submarine has moved over 500 pixels since last update"); + // return; + //} - targetPosition = Position; + targetPosition = new Vector2(message.ReadFloat(), message.ReadFloat()); + speed = new Vector2(message.ReadFloat(), message.ReadFloat()); lastNetworkUpdate = sendingTime; } @@ -809,6 +820,8 @@ namespace Subsurface } } + ID = 1; + loaded = this; } @@ -825,6 +838,10 @@ namespace Subsurface public static void Unload() { if (loaded == null) return; + + + loaded.Remove(); + loaded.Clear(); loaded = null; } diff --git a/Subsurface/Map/WayPoint.cs b/Subsurface/Map/WayPoint.cs index bf137ce0f..6f0da80f3 100644 --- a/Subsurface/Map/WayPoint.cs +++ b/Subsurface/Map/WayPoint.cs @@ -10,12 +10,19 @@ using System.Collections.ObjectModel; namespace Subsurface { + public enum SpawnType { None, Human, Enemy }; class WayPoint : MapEntity { - public enum SpawnType { None, Human, Enemy }; + private SpawnType spawnType; + public SpawnType SpawnType + { + get { return spawnType; } + set { spawnType = value; } + } + public override Vector2 SimPosition { get { return ConvertUnits.ToSimUnits(new Vector2(rect.X, rect.Y)); } @@ -31,7 +38,7 @@ namespace Subsurface public override void Draw(SpriteBatch spriteBatch, bool editing) { - if (!editing) return; + //if (!editing) return; Color clr = (isSelected) ? Color.Red : Color.LightGreen; GUI.DrawRectangle(spriteBatch, new Rectangle(rect.X, -rect.Y, rect.Width, rect.Height), clr, true); diff --git a/Subsurface/Networking/GameClient.cs b/Subsurface/Networking/GameClient.cs index 2174b4335..45634ff2c 100644 --- a/Subsurface/Networking/GameClient.cs +++ b/Subsurface/Networking/GameClient.cs @@ -2,6 +2,7 @@ using System.Diagnostics; using Lidgren.Network; using Microsoft.Xna.Framework; +using System.Collections.Generic; namespace Subsurface.Networking { @@ -13,6 +14,8 @@ namespace Subsurface.Networking private Character myCharacter; private CharacterInfo characterInfo; + + List otherClients; public Character Character { @@ -30,6 +33,8 @@ namespace Subsurface.Networking name = newName; characterInfo = new CharacterInfo("Content/Characters/Human/human.xml", name); + + otherClients = new List(); } public bool ConnectToServer(string hostIP) @@ -75,7 +80,7 @@ namespace Subsurface.Networking // Funtion that waits for connection approval info from server WaitForStartingInfo(); - if (Client.ConnectionStatus!=NetConnectionStatus.Connected) + if (Client.ConnectionStatus != NetConnectionStatus.Connected) { DebugConsole.ThrowError("Couldn't connect to server"); return false; @@ -115,17 +120,21 @@ namespace Subsurface.Networking case NetIncomingMessageType.Data: if (inc.ReadByte() == (byte)PacketTypes.LoggedIn) { + int myID = inc.ReadInt32(); + //add the names of other connected clients to the lobby screen int existingClients = inc.ReadInt32(); for (int i = 1; i <= existingClients; i++) { - Game1.NetLobbyScreen.AddPlayer(inc.ReadString()); + Client otherClient = new Client(inc.ReadString(), inc.ReadInt32()); + + Game1.NetLobbyScreen.AddPlayer(otherClient); } //add the name of own client to the lobby screen - Game1.NetLobbyScreen.AddPlayer(name); + Game1.NetLobbyScreen.AddPlayer(new Client(name, myID)); - CanStart = true; + CanStart = true; } else if (inc.ReadByte() == (byte)PacketTypes.KickedOut) { @@ -249,19 +258,18 @@ namespace Subsurface.Networking break; case (byte)PacketTypes.PlayerJoined: - Client otherClient = new Client(); - otherClient.name = inc.ReadString(); + Client otherClient = new Client(inc.ReadString(), inc.ReadInt32()); - Game1.NetLobbyScreen.AddPlayer(otherClient.name); + Game1.NetLobbyScreen.AddPlayer(otherClient); AddChatMessage(otherClient.name + " has joined the server", ChatMessageType.Server); break; case (byte)PacketTypes.PlayerLeft: - string leavingName = inc.ReadString(); + int leavingID = inc.ReadInt32(); AddChatMessage(inc.ReadString(), ChatMessageType.Server); - Game1.NetLobbyScreen.RemovePlayer(leavingName); + Game1.NetLobbyScreen.RemovePlayer(otherClients.Find(c => c.ID==leavingID)); break; case (byte)PacketTypes.KickedOut: @@ -330,8 +338,15 @@ namespace Subsurface.Networking msg.Write(characterInfo.Gender == Gender.Male); msg.Write(characterInfo.HeadSpriteId); - Client.SendMessage(msg, NetDeliveryMethod.ReliableUnordered); + var jobPreferences = Game1.NetLobbyScreen.JobPreferences; + int count = Math.Min(jobPreferences.Count, 3); + msg.Write(count); + for (int i = 0; i < count; i++ ) + { + msg.Write(jobPreferences[i].Name); + } + Client.SendMessage(msg, NetDeliveryMethod.ReliableUnordered); } private Character ReadCharacterData(NetIncomingMessage inc) @@ -340,9 +355,17 @@ namespace Subsurface.Networking int ID = inc.ReadInt32(); bool isFemale = inc.ReadBoolean(); int inventoryID = inc.ReadInt32(); + + int headSpriteID = inc.ReadInt32(); + + + string jobName = inc.ReadString(); + JobPrefab jobPrefab = JobPrefab.List.Find(jp => jp.Name == jobName); + Vector2 position = new Vector2(inc.ReadFloat(), inc.ReadFloat()); - CharacterInfo ch = new CharacterInfo("Content/Characters/Human/human.xml", newName, isFemale ? Gender.Female : Gender.Male); + CharacterInfo ch = new CharacterInfo("Content/Characters/Human/human.xml", newName, isFemale ? Gender.Female : Gender.Male, jobPrefab); + ch.HeadSpriteId = headSpriteID; Character character = new Character(ch, position); character.ID = ID; character.Inventory.ID = inventoryID; diff --git a/Subsurface/Networking/GameServer.cs b/Subsurface/Networking/GameServer.cs index 1b1e80d96..33113f133 100644 --- a/Subsurface/Networking/GameServer.cs +++ b/Subsurface/Networking/GameServer.cs @@ -14,8 +14,8 @@ namespace Subsurface.Networking public List connectedClients = new List(); - const int SparseUpdateInterval = 150; - int sparseUpdateTimer; + TimeSpan SparseUpdateInterval = new TimeSpan(0, 0, 0, 1); + DateTime sparseUpdateTimer; Client myClient; @@ -61,16 +61,15 @@ namespace Subsurface.Networking if (inc != null) ReadMessage(inc); // if 30ms has passed - if ((updateTimer) < DateTime.Now) + if (updateTimer < DateTime.Now) { if (Server.ConnectionsCount > 0) { - if (sparseUpdateTimer <= 0) SparseUpdate(); + if (sparseUpdateTimer < DateTime.Now) SparseUpdate(); SendNetworkEvents(); } - sparseUpdateTimer -= 1; updateTimer = DateTime.Now + updateInterval; } } @@ -88,10 +87,18 @@ namespace Subsurface.Networking //Character ch = new Character("Content/Characters/Human/human.xml"); - Client newClient = new Client(); - newClient.version = inc.ReadString(); - newClient.name = inc.ReadString(); + string version = inc.ReadString(); + string name = inc.ReadString(); + + int id = 1; + while (connectedClients.Find(c=>c.ID==id)!=null) + { + id++; + } + + Client newClient = new Client(name, id); newClient.Connection = inc.SenderConnection; + newClient.version = version; connectedClients.Add(newClient); @@ -101,8 +108,6 @@ namespace Subsurface.Networking Debug.WriteLine(inc.SenderConnection + " status changed. " + (NetConnectionStatus)inc.SenderConnection.Status); if (inc.SenderConnection.Status == NetConnectionStatus.Connected) { - - Client sender = connectedClients.Find(x => x.Connection == inc.SenderConnection); if (sender == null) break; @@ -115,20 +120,24 @@ namespace Subsurface.Networking } else { + AssignJobs(); - Game1.NetLobbyScreen.AddPlayer(sender.name); + Game1.NetLobbyScreen.AddPlayer(sender); // Notify the client that they have logged in outmsg = Server.CreateMessage(); outmsg.Write((byte)PacketTypes.LoggedIn); + outmsg.Write(sender.ID); + //notify the client about other clients already logged in outmsg.Write((myClient == null) ? connectedClients.Count - 1 : connectedClients.Count); foreach (Client c in connectedClients) { if (c.Connection == inc.SenderConnection) continue; outmsg.Write(c.name); + outmsg.Write(c.ID); } if (myClient != null) outmsg.Write(myClient.name); @@ -141,6 +150,7 @@ namespace Subsurface.Networking outmsg.Write((byte)PacketTypes.PlayerJoined); outmsg.Write(sender.name); + outmsg.Write(sender.ID); //send the message to everyone except the client who just logged in SendMessage(outmsg, NetDeliveryMethod.ReliableUnordered, inc.SenderConnection); @@ -223,7 +233,9 @@ namespace Subsurface.Networking } } - sparseUpdateTimer = SparseUpdateInterval; + new NetworkEvent(Submarine.Loaded.ID, false); + + sparseUpdateTimer = DateTime.Now + SparseUpdateInterval; } private void SendMessage(NetOutgoingMessage msg, NetDeliveryMethod deliveryMethod, NetConnection excludedConnection) @@ -256,8 +268,12 @@ namespace Subsurface.Networking networkEvent.FillData(message); - Server.SendMessage(message, Server.Connections, - (networkEvent.IsImportant) ? NetDeliveryMethod.Unreliable : NetDeliveryMethod.ReliableUnordered, 0); + if (Server.ConnectionsCount>0) + { + Server.SendMessage(message, Server.Connections, + (networkEvent.IsImportant) ? NetDeliveryMethod.Unreliable : NetDeliveryMethod.ReliableUnordered, 0); + } + } NetworkEvent.events.Clear(); } @@ -273,14 +289,14 @@ namespace Subsurface.Networking //selectedMap.Load(); Game1.GameSession = new GameSession(selectedMap, Game1.NetLobbyScreen.SelectedMode); - Game1.GameSession.StartShift(Game1.NetLobbyScreen.GameDuration, Game1.NetLobbyScreen.LevelSeed, 1); + Game1.GameSession.StartShift(Game1.NetLobbyScreen.GameDuration, Game1.NetLobbyScreen.LevelSeed); //EventManager.SelectEvent(Game1.netLobbyScreen.SelectedEvent); foreach (Client client in connectedClients) { client.inGame = true; - WayPoint spawnPoint = WayPoint.GetRandom(WayPoint.SpawnType.Human); + WayPoint spawnPoint = WayPoint.GetRandom(SpawnType.Human); if (client.characterInfo==null) { @@ -292,7 +308,7 @@ namespace Subsurface.Networking if (myClient != null) { - WayPoint spawnPoint = WayPoint.GetRandom(WayPoint.SpawnType.Human); + WayPoint spawnPoint = WayPoint.GetRandom(SpawnType.Human); CharacterInfo ch = new CharacterInfo("Content/Characters/Human/human.xml", myClient.name); myClient.character = new Character(ch, (spawnPoint == null) ? Vector2.Zero : spawnPoint.SimPosition); } @@ -390,10 +406,10 @@ namespace Subsurface.Networking outmsg = Server.CreateMessage(); outmsg.Write((byte)PacketTypes.PlayerLeft); - outmsg.Write(client.name); + outmsg.Write(client.ID); outmsg.Write(msg); - Game1.NetLobbyScreen.RemovePlayer(client.name); + Game1.NetLobbyScreen.RemovePlayer(client); if (Server.Connections.Count > 0) { @@ -482,11 +498,24 @@ namespace Subsurface.Networking Gender gender = message.ReadBoolean() ? Gender.Male : Gender.Female; int headSpriteId = message.ReadInt32(); + + List jobPreferences = new List(); + int count = message.ReadInt32(); + for (int i = 0; i < count; i++) + { + string jobName = message.ReadString(); + JobPrefab jobPrefab = JobPrefab.List.Find(jp => jp.Name == jobName); + if (jobPrefab != null) jobPreferences.Add(jobPrefab); + } + foreach (Client c in connectedClients) { if (c.Connection != message.SenderConnection) continue; + c.characterInfo = new CharacterInfo("Content/Characters/Human/human.xml", name, gender); c.characterInfo.HeadSpriteId = headSpriteId; + c.jobPreferences = jobPreferences; + break; } } @@ -496,19 +525,116 @@ namespace Subsurface.Networking message.Write(character.ID); message.Write(character.Info.Gender == Gender.Female); message.Write(character.Inventory.ID); + + message.Write(character.Info.HeadSpriteId); + message.Write(character.SimPosition.X); message.Write(character.SimPosition.Y); + + message.Write(character.Info.Job.Name); + } + + private void AssignJobs() + { + List unassigned = new List(connectedClients); + + int[] assignedClientCount = new int[JobPrefab.List.Count]; + + //if any of the players has chosen a job that is Always Allowed, give them that job + for (int i = unassigned.Count - 1; i >= 0; i--) + { + if (!unassigned[i].jobPreferences[0].AllowAlways) continue; + unassigned[i].assignedJob = unassigned[i].jobPreferences[0]; + unassigned.RemoveAt(i); + } + + //go throught the jobs whose MinNumber>0 (i.e. at least one crew member has to have the job) + bool unassignedJobsFound = true; + while (unassignedJobsFound && unassigned.Count > 0) + { + unassignedJobsFound = false; + for (int i = 0; i < JobPrefab.List.Count; i++) + { + if (unassigned.Count == 0) break; + if (JobPrefab.List[i].MinNumber < 1 || assignedClientCount[i] >= JobPrefab.List[i].MinNumber) continue; + + //find the client that wants the job the most, or force it to random client if none of them want it + Client assignedClient = FindClientWithJobPreference(unassigned, JobPrefab.List[i], true); + + assignedClient.assignedJob = JobPrefab.List[i]; + + assignedClientCount[i]++; + unassigned.Remove(assignedClient); + + //the job still needs more crew members, set unassignedJobsFound to true to keep the while loop running + if (assignedClientCount[i] < JobPrefab.List[i].MinNumber) unassignedJobsFound = true; + } + } + + for (int preferenceIndex = 0; preferenceIndex < 3; preferenceIndex++) + { + for (int i = unassigned.Count - 1; i >= 0; i--) + { + int jobIndex = JobPrefab.List.FindIndex(jp => jp == unassigned[i].jobPreferences[preferenceIndex]); + + //if there's enough crew members assigned to the job already, continue + if (assignedClientCount[jobIndex] >= JobPrefab.List[jobIndex].MaxNumber) continue; + + unassigned[i].assignedJob = JobPrefab.List[i]; + + assignedClientCount[jobIndex]++; + unassigned.RemoveAt(i); + } + } + + UpdateNetLobby(null); + + } + + private Client FindClientWithJobPreference(List clients, JobPrefab job, bool forceAssign = false) + { + int bestPreference = 0; + Client preferredClient = null; + foreach (Client c in clients) + { + int index = c.jobPreferences.FindIndex(jp => jp == job); + if (preferredClient == null || index < bestPreference) + { + bestPreference = index; + preferredClient = c; + } + } + + //none of the clients wants the job + if (forceAssign && preferredClient == null) + { + preferredClient = clients[Rand.Int(clients.Count)]; + } + + return preferredClient; } } class Client { public string name; + public int ID; public Character character; public CharacterInfo characterInfo; public NetConnection Connection { get; set; } public string version; public bool inGame; + + public List jobPreferences; + public JobPrefab assignedJob; + + public Client(string name, int ID) + { + this.name = name; + this.ID = ID; + + jobPreferences = new List(JobPrefab.List.GetRange(0,3)); + } } } diff --git a/Subsurface/Screens/NetLobbyScreen.cs b/Subsurface/Screens/NetLobbyScreen.cs index 4a8c6f4c4..bbac16c6d 100644 --- a/Subsurface/Screens/NetLobbyScreen.cs +++ b/Subsurface/Screens/NetLobbyScreen.cs @@ -7,6 +7,7 @@ using FarseerPhysics; using FarseerPhysics.Factories; using FarseerPhysics.Dynamics; using System.IO; +using System.Collections.Generic; namespace Subsurface { @@ -17,8 +18,13 @@ namespace Subsurface private GUIListBox playerList; private GUIListBox subList, modeList, chatBox; + + private GUIListBox jobList; + private GUITextBox textBox; + private GUITextBox seedBox; + private GUIScrollBar durationBar; private GUIFrame playerFrame; @@ -50,10 +56,34 @@ namespace Subsurface } } + public List JobPreferences + { + get + { + List jobPreferences = new List(); + foreach (GUIComponent child in jobList.children) + { + JobPrefab jobPrefab = child.UserData as JobPrefab; + if (jobPrefab == null) continue; + jobPreferences.Add(jobPrefab); + } + return jobPreferences; + } + } + + private string levelSeed; + public string LevelSeed { - get; - private set; + get + { + return levelSeed; + } + private set + { + levelSeed = value; + seedBox.Text = levelSeed; + } } public string DurationText() @@ -99,7 +129,7 @@ namespace Subsurface (int)(panelRect.Width * 0.4f - 20), (int)(panelRect.Height * 0.4f - 20)), GUI.style, menu); - playerList = new GUIListBox(new Rectangle(0,0,0,0), Color.White, null, playerListFrame); + playerList = new GUIListBox(new Rectangle(0,0,0,0), null, GUI.style, playerListFrame); } public override void Deselect() @@ -133,7 +163,7 @@ namespace Subsurface new GUITextBlock(new Rectangle(0, 30, 0, 30), "Selected submarine:", null, null, Alignment.Left, null, infoFrame); subList = new GUIListBox(new Rectangle(0, 60, 200, 200), Color.White, GUI.style, infoFrame); subList.OnSelected = SelectMap; - subList.Enabled = (Game1.Server!=null); + subList.Enabled = (Game1.Server != null); if (Submarine.SavedSubmarines.Count > 0) { @@ -180,17 +210,18 @@ namespace Subsurface GUI.style, 0.1f, infoFrame); durationBar.BarSize = 0.1f; durationBar.Enabled = (Game1.Server != null); - LevelSeed = ToolBox.RandomSeed(8); + new GUITextBlock(new Rectangle((int)(modeList.Rect.Right + 20 - 80), 100, 100, 20), "Level Seed: ", GUI.style, Alignment.Left, Alignment.TopLeft, infoFrame); - GUITextBox seedBox = new GUITextBox(new Rectangle((int)(modeList.Rect.Right + 20 - 80), 130, 180, 20), + seedBox = new GUITextBox(new Rectangle((int)(modeList.Rect.Right + 20 - 80), 130, 180, 20), Alignment.TopLeft, GUI.style, infoFrame); seedBox.OnEnter = SelectSeed; seedBox.Enabled = (Game1.Server != null); - - if (IsServer && Game1.Server!=null) + LevelSeed = ToolBox.RandomSeed(8); + + if (IsServer && Game1.Server != null) { GUIButton startButton = new GUIButton(new Rectangle(0, 0, 200, 30), "Start", GUI.style, infoFrame); startButton.OnClicked = Game1.Server.StartGame; @@ -225,11 +256,13 @@ namespace Subsurface new GUITextBlock(new Rectangle(0, 150, 200, 30), "Job preferences:", GUI.style, playerFrame); - GUIListBox jobList = new GUIListBox(new Rectangle(0,180,200,0), GUI.style, playerFrame); + jobList = new GUIListBox(new Rectangle(0, 180, 200, 0), GUI.style, playerFrame); foreach (JobPrefab job in JobPrefab.List) { GUITextBlock jobText = new GUITextBlock(new Rectangle(0,0,0,20), job.Name, GUI.style, jobList); + jobText.UserData = job; + GUIButton upButton = new GUIButton(new Rectangle(jobText.Rect.Width - 40, 0, 20, 20), "u", GUI.style, jobText); upButton.UserData = -1; upButton.OnClicked += ChangeJobPreference; @@ -250,33 +283,32 @@ namespace Subsurface { if (Game1.Server != null) Game1.Server.UpdateNetLobby(obj); - Submarine map = (Submarine)obj; + Submarine sub = (Submarine)obj; - //map already loaded - if (Submarine.Loaded!=null && map.FilePath == Submarine.Loaded.FilePath) return true; + //submarine already loaded + if (Submarine.Loaded != null && sub.FilePath == Submarine.Loaded.FilePath) return true; - map.Load(); + sub.Load(); return true; } - public void AddPlayer(string name) + public void AddPlayer(Client client) { GUITextBlock textBlock = new GUITextBlock( new Rectangle(0, 0, 0, 25), - name, - GUI.style, - Alignment.Left, - Alignment.Left, + client.name + ((client.assignedJob==null) ? "" : " (" + client.assignedJob.Name + ")"), + GUI.style, Alignment.Left, Alignment.Left, playerList); textBlock.Padding = new Vector4(10.0f, 0.0f, 0.0f, 0.0f); - textBlock.UserData = name; + textBlock.UserData = client; } - public void RemovePlayer(string name) + public void RemovePlayer(Client client) { - playerList.RemoveChild(playerList.GetChild(name)); + if (client == null) return; + playerList.RemoveChild(playerList.GetChild(client)); } public override void Update(double deltaTime) @@ -378,15 +410,15 @@ namespace Subsurface Game1.Client.Character = character; character.AnimController.IsStanding = true; - - if (previewPlatform==null) + + if (previewPlatform == null) { Body platform = BodyFactory.CreateRectangle(Game1.World, 3.0f, 1.0f, 5.0f); platform.SetTransform(new Vector2(pos.X, pos.Y - 2.5f), 0.0f); platform.IsStatic = true; } - if (previewHull==null) + if (previewHull == null) { pos = ConvertUnits.ToDisplayUnits(pos); previewHull = new Hull(new Rectangle((int)pos.X - 100, (int)pos.Y + 100, 200, 200)); @@ -400,6 +432,8 @@ namespace Subsurface character.AnimController.UpdateAnim((float)Physics.step); Game1.World.Step((float)Physics.step); } + + Game1.Client.SendCharacterData(); } private bool SwitchGender(GUIButton button, object obj) @@ -420,15 +454,16 @@ namespace Subsurface private bool SelectSeed(GUITextBox textBox, string seed) { - if (string.IsNullOrWhiteSpace(seed)) - { - textBox.Text = LevelSeed; - } - else + if (!string.IsNullOrWhiteSpace(seed)) { LevelSeed = seed; } + //textBox.Text = LevelSeed; + textBox.Selected = false; + + if (Game1.Server != null) Game1.Server.UpdateNetLobby(null); + return true; } @@ -473,6 +508,8 @@ namespace Subsurface listBox.children[i].Color = color; } + + Game1.Client.SendCharacterData(); } public bool TrySelectMap(string mapName, string md5Hash) @@ -518,9 +555,18 @@ namespace Subsurface msg.Write(selectedMap.Hash.MD5Hash); } - msg.Write(modeList.SelectedIndex); + msg.Write(modeList.SelectedIndex-1); msg.Write(durationBar.BarScroll); msg.Write(LevelSeed); + + msg.Write(playerList.CountChildren - 1); + for (int i = 1; i < playerList.CountChildren; i++) + { + Client client = playerList.children[i].UserData as Client; + msg.Write(client.ID); + msg.Write(client.assignedJob==null ? "" : client.assignedJob.Name); + } + } @@ -539,6 +585,32 @@ namespace Subsurface durationBar.BarScroll = msg.ReadFloat(); LevelSeed = msg.ReadString(); + + int playerCount = msg.ReadInt32(); + for (int i = 0; i < playerCount; i++) + { + int clientID = msg.ReadInt32(); + string jobName = msg.ReadString(); + + + Client client = null; + GUITextBlock textBlock = null; + foreach (GUIComponent child in playerList.children) + { + Client tempClient = child.UserData as Client; + if (tempClient == null || tempClient.ID != clientID) continue; + + client = tempClient; + textBlock = child as GUITextBlock; + break; + } + if (client == null) continue; + + client.assignedJob = JobPrefab.List.Find(jp => jp.Name == jobName); + + textBlock.Text = client.name + ((client.assignedJob==null) ? "" : " (" + client.assignedJob.Name + ")"); + } } + } } diff --git a/Subsurface/StyleCop.Cache b/Subsurface/StyleCop.Cache index 98890f375..b1a473fae 100644 --- a/Subsurface/StyleCop.Cache +++ b/Subsurface/StyleCop.Cache @@ -1,8 +1,5 @@ 12 - - DEBUG;TRACE;WINDOWS - 2014.04.01 10:18:24.000 @@ -305,52 +302,6 @@ - - - 2014.04.01 10:18:24.000 - 2015.07.02 21:22:42.115 - 2015.07.02 21:02:52.124 - 2014.04.01 10:18:24.000 - 2014.04.01 10:18:24.000 - -1945363787 - 2014.04.01 10:18:24.000 - 0 - 2014.04.01 10:18:24.000 - 0 - 2014.04.01 10:18:24.000 - 0 - 2014.04.01 10:18:24.000 - 0 - 2014.04.01 10:18:24.000 - 0 - 2014.04.01 10:18:24.000 - 0 - - - - The line contains unnecessary parenthesis. - 73 - 2090 - 2160 - 73 - 40 - 73 - 110 - False - - - The line contains unnecessary parenthesis. - 80 - 2470 - 2539 - 80 - 39 - 80 - 108 - False - - - 2014.04.01 10:18:24.000 @@ -3993,35 +3944,6 @@ - - - 2014.04.01 10:18:24.000 - 2015.07.02 21:22:42.115 - 2015.07.02 21:17:30.557 - 2014.04.01 10:18:24.000 - 2014.04.01 10:18:24.000 - -1945363787 - 2014.04.01 10:18:24.000 - 0 - 2014.04.01 10:18:24.000 - 0 - 2014.04.01 10:18:24.000 - 0 - 2014.04.01 10:18:24.000 - 0 - 2014.04.01 10:18:24.000 - 0 - 2014.04.01 10:18:24.000 - 0 - - - - Constants must start with an upper-case letter: gridCellWidth. - 27 - False - - - 2014.04.01 10:18:24.000 @@ -6489,4 +6411,76 @@ + + + 2014.04.01 10:18:24.000 + 2015.07.02 21:22:42.115 + 2015.07.06 23:42:06.929 + 2014.04.01 10:18:24.000 + 2014.04.01 10:18:24.000 + -1945363787 + 2014.04.01 10:18:24.000 + 0 + 2014.04.01 10:18:24.000 + 0 + 2014.04.01 10:18:24.000 + 0 + 2014.04.01 10:18:24.000 + 0 + 2014.04.01 10:18:24.000 + 0 + 2014.04.01 10:18:24.000 + 0 + + + + The line contains unnecessary parenthesis. + 75 + 2171 + 2241 + 75 + 40 + 75 + 110 + False + + + The line contains unnecessary parenthesis. + 82 + 2551 + 2620 + 82 + 39 + 82 + 108 + False + + + + + DEBUG;TRACE;WINDOWS + + + + 2014.04.01 10:18:24.000 + 2015.07.02 21:22:42.115 + 2015.07.07 16:37:45.662 + 2014.04.01 10:18:24.000 + 2014.04.01 10:18:24.000 + -1945363787 + 2014.04.01 10:18:24.000 + 0 + 2014.04.01 10:18:24.000 + 0 + 2014.04.01 10:18:24.000 + 0 + 2014.04.01 10:18:24.000 + 0 + 2014.04.01 10:18:24.000 + 0 + 2014.04.01 10:18:24.000 + 0 + + + \ No newline at end of file diff --git a/Subsurface/Subsurface.csproj b/Subsurface/Subsurface.csproj index 25d44f6a8..6b58a20d6 100644 --- a/Subsurface/Subsurface.csproj +++ b/Subsurface/Subsurface.csproj @@ -59,7 +59,6 @@ - @@ -198,10 +197,6 @@ - - False - bin\Windows\Debug\FarseerPhysics MonoGame.dll - False .\Lidgren.Network.dll @@ -720,6 +715,10 @@ + + {0aad36e3-51a5-4a07-ab60-5c8a66bd38b7} + Farseer Physics MonoGame + {1e6bf44d-6e31-40cc-8321-3d5958c983e7} Subsurface_content diff --git a/Subsurface/ToolBox.cs b/Subsurface/ToolBox.cs index d19d51758..637f19f90 100644 --- a/Subsurface/ToolBox.cs +++ b/Subsurface/ToolBox.cs @@ -255,17 +255,7 @@ namespace Subsurface .Select(s => s[Rand.Int(s.Length)]) .ToArray()); } - - public static int SeedToInt(string seed) - { - StringBuilder sb = new StringBuilder(); - for (int i = 0; i < seed.Length; i++) - { - sb.Append((int)seed[i]); - } - return int.Parse(sb.ToString()) % int.MaxValue; - } - + public static string WrapText(string text, float lineWidth) { if (GUI.font.MeasureString(text).X < lineWidth) return text; diff --git a/Subsurface_Solution.sln b/Subsurface_Solution.sln index 71e54b9b9..6d56c3236 100644 --- a/Subsurface_Solution.sln +++ b/Subsurface_Solution.sln @@ -14,7 +14,7 @@ Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "Solution Items", "Solution Performance1.psess = Performance1.psess EndProjectSection EndProject -Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Farseer Physics MonoGame", "..\Lataukset\Selain-lataukset\Farseer Physics Engine 3.5\Farseer Physics Engine 3.5\Farseer Physics MonoGame.csproj", "{0AAD36E3-51A5-4A07-AB60-5C8A66BD38B7}" +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Farseer Physics MonoGame", "Farseer Physics Engine 3.5\Farseer Physics MonoGame.csproj", "{0AAD36E3-51A5-4A07-AB60-5C8A66BD38B7}" EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution diff --git a/Subsurface_Solution.v12.suo b/Subsurface_Solution.v12.suo index b47011268c13c26ea7fad27b594829d6c0bdcc33..c4d398f2a83b6611c024372d5f10e27a9c1e2b84 100644 GIT binary patch delta 29132 zcmeHw31C!3wsu$DdqWloO-LYw5IUiW0YXS1gvEf_gn$9Uz8D}o1VR#$4zfuaMMOjl zT<8Oc5D>uyctDVg2-++<>L}pGsDlnJqvM94$heaC)$Q(dCj_@Q&YS;#w0^03>(+AW z)TvXaR;(-9T2HjCj*a66=g(*`+{+Ax=6JnQS63%|B_Ia)C$J2-58u7^=J;_XJEr`^ zH|lJDC(~-b;eIUnQ+)mj(Of@~gzG2#Gw?kD5Po5B-vr744HyPk)nvJ2kO&!yx3R#J zz(c?WwNmcHUoof~;|R)eSsN3Brq9x6xaYP0j2@d4F+|A;#1|1^rG;7 zBXAdRJ5U9`nQ-%g<6!npa8rPIAP`6d3W3i6KZJcBt`(RHyom5?;a&!=020363HLSl zt8fdo9}E;GpoJ5`*~K5D4(mMT$Lg!LsW`L=juukzCT+jBDrKl z|8wg5GNsDQ0-9|Cp&4+Gl);r|HS zM}fzH$AO&y#U5vYlyscQLA7|@4~U=#obHuXv%9I@ezB{pvFdccb*p_X|GwNduCkih zDw>_Z+qYOPWD-~-WL6#dRcjwX*r|uNo3xK+!HgteL9!h~P zfvS2({4cYh%CO+2uk&n;^-2d)Z;vkPnT#;92>3&-tHM#;gGP~ z)p@C1)NO}C)qywp)~0WKvzcm2l~walllLJ6exgByzKF12j}T!CS5t57$rS^Ihp~=o zc7X}B`O}HFSfc*o@Q`(nt=pTgUT9KHSt#I%vux{rzPcCMLQj)=g2$?so|)cnf=?d+ z^#!TD)H8ZyJ+q8IHs!b56Po_ku^cfiDJ zO(%J<8j%|*@AV{FwdCr2P!@pr`_y@P-BfdkN#27nYG$GtRC}wIJOl}hQ!nKiY3Tzz zu{P-+_Et8|&zk2WQa>OO5Snp})4j>*3O}2~*US_T)100~aA!E5i{Z|4K9@V)*-p2@ z=~lv>2iykCb^3|)7U6jzaJ$p53htefL96?Lt!FHs*8jw&9q7x_n77cYuXys0EdAID z)q1bF*1YX3V|S>`T0|`$U<0*BIGZ4`p~MPV0(E_r2huHpN&*EOkYnkkb+UQ&OfQ<# zpLJ$ah>d0GR2Cq&iFoYkfsx_gv@&%1M{QAUMure=UCV(;$8UxRJd~`Ao9MGua?o~f z#dsZ`LOQG4PlVJC zj9<>=_MQb%_QE{!*1l`WnsL@;!fJuL{qDY`TL+ zGh~C2(YUQ*IfoHmm^Bp=tR0plz zB=;odtpw5Yo0XPcj@WcztI{qg0D8>v>uL|fa~Hg%(d-RMlxjV1*38lDte@>$_E6ZL zSmBj(_8ex!qRa2TAp&W{f9vBZR))K z-8AziwvEZc9HB z2Qrz7tELcH-PXe-e}&I(Qm^Q_tCy~1dJ7vy*bg3Y>sCMw(zc;2Q?qnsnaukfzQ5}k z#xE1qUoUZE8RG{oFbfO)76ktdl6?+%4-jJZfzzexi)^;WCb7C^Ki^)`kkhYgFHvYO z`B1>NC^nZr=uPvZ*igMY>Fr4@U5a9vy0@5Is4pj3y08_L9K<`&g{S2ZtSL;w_9GM~%nTPt{f2-aMex9n-usHrq?#*I0A1)S z2Wr(NtW07VLX-y5xkn^_Ev6=4LWm9xUX<4~*`Lc@#OhbnSZ>G3-9^~CW7XK}XMP7xh zzq%(up`^ufLCamw*7Vpd3H&+tGELVg!( zF06G_n*O7X8A>-gJ6!QmE~NUPbgyJ8H1%wa1&K8P-1D)I;Fc%Fyq=xbQJ-^hcY}1 znMiwx!nY`W4%9F+M8{w7I4)Z1pmo^JK2XRy7!BJzh;3FJX122z)-DZ#0Zgy-mU^gV zi@mhoyV;*5TRKnkK7eGmHY7BiTiI^P+2Z}Fcxnx}~TTP=eC6eY! zCFuL}haU|4VxZpRS&-g1s++1DrHi-8X1ZWg+R4)@i}Ws3?{AH?B3KRxDtdA9j{Etm z>HW_A^`j-PREXh>p^mI1TUS|I(x=UM8?yFi?X{$0W@nUqTJElOKg3>REuanDKT7(T z_tKII5vEG`3KsPTLwCXThWjntBw#t7MHB1@_XfDl;XVu2euU>mfQT!4=954$z6Ain zPpD$yieI?XPcS^w=^qI{VHQ6qCMHE7G3pU+3_QCyBXn}Q!v8rj5!0G8MtgU%HtOl- zn7^r(wnI+$hJ)LOBjhM8(auh=13$8L^=8lsXkjufWl{2Bq~#&fOQDnw!YdTn)_W^r zisUYbCA%R~KH?cgDTI=#qcNo446N+`RVF z*1A|^A;Jo}n8tgPb&K4KvXkX7qtExZ4CjQsYVf;X780uWeMKy*Nxz9Atvvq5c0_ep)L4+r^85Y zZ<1^A%~eRh_wh0hMB=BRw?SXjlP5#&*0Y|Jy$M$Io@K$tus?sZc*%R~69ZfC{rZbV zPd!SrA7vfL-is%ZbtIc?+_cF)^XtCfh1Y%i&dC+=#zCU!>aiaOlD%49VEi)l?)%z% zUF_Pf{q1|lZtpb}Uju1sYbBhO5o^b8BK!Svl5HLPn{i&@jIH-)Axx1a`OaX6-tYDv9`2( z0;@FU9C_yZcfxPjcj>^Et`{#x81+mCPh|aAHZ2~>Zel4S@BS1!gDb0MCuvFN|2LbG zXd(aerli$<%mSJ5#$!)s_xg0e&HjPIYfTYr3k4U$OeN4|rMeb#sKrU2Hfz5tF)U-v zz||K={h)5VmgU8_HKX^}^7%ABm`$ax zy?AH363mK>uRYN$v`@~)O4A$3yZe82F7p!O>Ix&T(#z5j%BD>eLgT+aZKEdEa-u`nb4*uX|9^JvE?$==zApN zll6aV>-FA=koo7|>QX3j)wEnTgxM1KemMrxNe2^n{MbZ14FCoLHvvgNGLQnK0%^d_ zz#w2SkPZw5gdE)hS2WgPaEAjUfJ|Vl!xEUlXm$r)wzHp z>kaWM##c)w&2*WcW406+k)pXodJ2!%ZoCEfiKaEwI|k1Uo9m?%n3#@M<>%3=w7R40 zS!OCn_zFPGLkkTu2M|S=>vZYEI=)|Bw=>izd#Y`VUWO2Q0qbmq>_$3W&CS~8Xr9B=eP?qC(i|dTx|`x&;>m06%3!0{u+;~j zF+4oGs3;=5uFsB<`uK)o?v!7p_itcftE|3%#PW^}L^ieRB{ZvR)k|>XI3Q+KUP3Ls z5BFI>j6%KxP6MyPU+}}ZyfdT8Rj&&O^e5bU79?{cx2IOM*cTq z@ESRRN725$Qi_&R&Yx!L11|(>xfA#%M)oDraJGaNPiLiC*&JTYXw*cWPtRxbFzs{} zk7e}fL=1KiYz;k@E-h;JLBcNIhetLW`gnHM^1>s1wa>db$Q>eR=}4C>d>q;Dl>$s+ zR9A*I?ow=w*pp#iaNpLG3>8rSqk$RHHUbr4tSi-JNF8bZol*kroWT1x8=R6c1a(7u zKa+yUWRWo1dW47Q-eC^!P91N?hKXqy?lm4sinzTHDEu4VEjiVYW*CE=9BBwxi~(mQ zhK@NfrLz$2Hux2QMDcZu$GgMw63LjI$;=v!fJMkpBqE}U5$H_k$kjSymjq#jdV<8d{ zEjrr3gBQD@8KhUFOvY6=i>o?$Tlic$G= zkFo`##>9|%7XB+mmRW`jy;KEYS}Fc3MoHY%hhWswsvwB=JtT$c^#u>wd_@YHR*pP! zz_lDhu1;VI;^reycU)BMFLE&%6in0OH^n;)upKg!y-*6)vlm5P6(Nt-Ut*o*uWDF+ zQ6D9MMRyCF^&{#nM=z=9=HY{}}!W5(Krs;_e)3Yjq+*HCh2bmP<{#k~J@X174qRt#bTZT}A2kz864!SD& zhOBdonS-h2h~BtjFeM*dc!`AZQiuR*ao{0ea!j~K;ZM^nNk zBpV8U`;kcG2b!>`Q<@BOsnKQ zbQyw~r8j;>mmzw^$lA7d)YDkK#)>_N!}@|AwvlLz(K#pQ+2g5W9KqH2givE+l4ZB@b%mERR-qH)ot#XtD4VghIjS zjPK@Z=p%Gi=&By9bhRuuJ7HQoYRVPCRhe0k09S{Tj6_76nTIVM?xOTsOC;$v3eDs*7ZO}WUIH{fLqV~7+%R=cmx>E<$WqSUC zDWcNc<#K8H_O49aEU8a|RuxMfs#-T$sY5`rP-7(xhQR=`$GT~F$ALW`$Kt!c_FQYa=GWk)IC9TWfF7rvk*{S;rOKLGwWa!y^@7? zAnYY5q8oMbav1=0autu#eFi#fWW0-6KVKt1*Rx#RaU?ktzwy^4USB(g5n#U4zN7G1 z-cBdAhcmSxS85GsWCr~{+`0^tbHFLszF!De^V{Sof@QtZuy#b z)`>ym5gkqk(cvUR=~W_ac^~ zoAy+ZE^|OwzM*<<7(9FGZczl?ADxKq-*OOSPISO|5FB*gdLi`?7+C8eCVH>{Bu~KJ z=*Hc=Se5rqvc z7%uu@p+@tXBq0FhnS6`XN#~uY2VGq|4BnY;HSKCgJzS#bPT(%fX=$!&a&R@Dyy48Ld3*0x1+rX zg)YWK*Jc~0kUeEz*ew|5h%_82SrA`X;BKO!szuvuC91m)!qIWz2su(DKanf$U6RNVyirf^(?k0$*97sYE8(g=RoXt zn&H{tg4-l6!Z31PlTA7JDiTDUiAIy7E0_7sh9fxdW{_wjJxo%Z&P-6KS5+@ptv6I4 zj>h30H@H)Asdf+Bw1b0fZn31^uL{l1$t4 zq(sS8jt2dP5WK2}mIf5hRS7oTq8!#(u`Vy=kSI~NIUa08k$z1|&Glw0%nZ~C<(!e* z-6#rnwIz40mhCAD1$qM0L;_-h_UBapwMTev$#N{b&q815Xw#l185kv6Bt5;7#pqqd zud${YcaTk~{$FF*k4GyKGd;x)jUNE{^=x_xPN5y_656XRpvFF893+~BYbwGmngg7P zjlFha*PeK~S~Ggm`5Wf|8qR55OI5lgy}rc`fo^aY&NZHQ&DppM>e9*sTm}94rruQB zblK_k)}gyUbQ@wRV6+7mR#O8Q*9fprLp>l0;MiK?8l?&aCPdg%Q(r@gOl-n1GNMuk zyw5xOq}`k`L^PM##IpGtv~mq5WKS-ZBIw&JevVE~gSP7QoLfgFf+?JINyvDmH^Hj}^4)@jTpeJ!zZbnzsXHnGdpvExcYE7dNpmtOOu zTpN$4)0LR{#n)hE4a0^v3XhT=rR++cs6Dn_`ZkoFUd-w>@Rgs0-SF>9L>bqA9@Pd_(I*@E|?Osj8^RL*A zS4QOpQl$3P=hEBEc9zwsH9G_KZKF>dln(P>+!%=H$9RSo{H3(N!S)%l4#HxSV?9Fj z!S%)VT#wM|e&RBelqH!D}+TD%EH;KT6rn=*M#ItND7#$Gj+afE?jmp`pw+(gm@boYH&q zzVyZvIgr-0mCCL!R$O1K_+5(?1!6M)4_~ZMlLrqW({{Oy*0qJaRly!|eZA}d)3pms zOgY!$XlgZYOUC!OtbG(9FJyGb86HL!BTKl|F-#QWb75P#7M6tVcCEF$pPP|#n=8(7 zK(|H{+E-74{LVoZ##sL+);hXfW38jX=mC8gy6|cWUyhCE*zzmR-q^IMf$}s);pbSJ zsw`MzqE7#pRz;|wk~;>hE(_9Q-1#5BCK8FpA;#cM4Tn%3S543>u~G-s{)w;ld64|J zMEi^^!lRh~uPl}L)ECn|`*wYivU!iEd_!MH_u3>iQyy+qkah%&*%9cYTor!#($4{Q4q^W8Y-s z^-#C%?&j71(M1v??OV>H$#;entoeq>hy7Y$MnV6H^_PFa+6fk4+EYXj&aERBlO4&B7o{@!$MI!-HuWk~)$m%G2XvfIJaEVki?V6QI@{|O(o#0=>K zYbN$O;77@mq>k#q(+h6QxAjI9=fwo z&B9TFp+!<(3LcH5HjcvPxpOn57`i+bAEFl^oN)@$n=?_0rGujoBx#D&pRN>3h4gI^ z!r@N<)!ZxxlYJJJ@jy_#NGghUiYF8z)OG}E>U#zp+0H?>Z_h-I&ySNLaqu=dTe?Ag zX%jBy6Z;Cxc~VbDz879+CW^Qfshk-PGLl9^mQIhw(}!80rSD8pggH1@;%`Qw6DFdl z_D(#M%(+r7y;z0xmy8D)OY*SnxNt1~7zfD#^k$J{B>PzW!3KsTn!f-+9;-s!ow-sP z{Wu<^4w@tl#JStJ9H|Rg#ggA6DQOg*#>|J`;X1GmiN^@~5^2+|C?I$eU>hid z#>uGYQB~3eM|{1cSzQEqMfi|CR`wT^6w}56(0!~BnN6A?Rntq;kZ|k*9zd~0U<+PH zi3G;c?qVsEwoe3Gc($~V@@FC11r$I8=}S{)AaK}pRB!)hK*jDU;K;aJkr9{?LdH4? z3!nKyq81=<)&|L+GUua^OLL?QQILkjI44h<0l>GZLSSCrI5S`YK;aVj*-fTWYUb`-CV$ zJ9eg3)1=N0ZVeiPTsBOVqFHwj-=)P64@k2=oiBjMBu+jzuLr@?s2!v@(|unELY46#dVgudIg1Sa<_c#DIr5C>PR{fp76*pDE#88;3m_Q8nV z*7_E6T6MqFo?@4yy#9-&_qCVqly-AA(QztCtS!)@dm-;D??;`N+=*JVIu?<0 zu>GtTuAVKx-|o0WBMv^j@e~g?*3bt}V*5h^p2gQZ-1VA+w*>f!czxma(brjpvJ~8h zvg||eTBTSV=eYWO);fkOce42|I&?RxuDe5nzPJ=ev%bGu+Ri9_x%APs$xe~PhO3KK z9xe`98Oe8r6xd`oxKL^ZHVL@XkJJ|3Cj~H^)OXPrNS9YaQ~j~kqM=%PD?zc~oLGy* zg;S`=O2bBU=S^-uV7a`mC6(=vqP1uXFJmNEq9xStMBr-}?fHcFCDY?lf35jkK3r^L z2tUDln;Y*r7n}1uF9gBWi;T`aE|t@Z3-~hJLxSarUUcCJmsgbe}fnU=Yb@9`6t z<%DXX%lQ;XMgo+QE=OzeC;3Z3WM9jRsVr0u z)|Pw(CbLGXH&k^7A}5+rNYhJnV$FxD+sn(T<{vy4CllWbmu6}4edU+H^wm6uVsDT_ zwV{dffp&B_75c%lnGe^p*T^-Dj3ZE&{kO@_HKVdVa(}WvAT?L3?>B2RcFQ)2PTwYV z)w(_{pFv8!=p1cd>Oni$a(<8VZPUwslD;x_}4M)>m-|6v7mG;B!95 zi|mDR8+v=X971K&W#fTqGMdcl7hrmC`ikdDR2`1))m4ijbS_esDffF^)nuP85AyTt z*|%40@A!m1@wXF=^j3O^i*xiJvAHN9uJGy5thiuKL2*nTBu7r7QDr=WVk5DvzWaOr zJoAgk{w@4(@G+3M8Y$mrR!&)#9>Z~0+xw(gyejm&+H?=LO%FziU&-E5Nr8oY{gi~( ze_?6PyQh+q(|RS`+^c&B+8Zl(rVG0vvU|GAF(m)Qf27#fcomJ5M#++SMDMud-reJq zWBYVZPDzOE-aEB#aza8%TCct-eQsYkD5t!lprG88IelJ5QC@}V=8|beB?YElF}<@I ze;QU=QaU(i7A+>FZSO*7yfSCJ0(ZQ;(&FNxilWkztcvM5Wd#*kdDC->O2jj5`5HI0 zJm4tXh@a#*vaVB#DSNZx*9@h+t+ae5ZCS5$R?l@!*PfQ8gG_sFy)p#%=B!r=jJPBz z3&<;-RaRP3P+}3B>gPi)rjYS5o}sHqis>#l*UorJT_uXG#F^|xGgzpWZIWt&$ePQ& z$$OwwLhL~$t5H4V`xw%$X7Ya+|3M8ig%V zufqadiR;#=Y^*G6#e<|Z%+_5_&=Q79FEV=nQ3i}@we zQa`d+NZEwVCSkZ`NA!gOqo948WWMEP-{ge#Qn$06n;Nf{-?rln`2 z(QiETbKF@dZal>1;yAQ{#y1(Z5?3KRfA)5GZchH&l%giZt6QsxjcHua<mXWFio(}B!=Yv>I_rt~41*rgy>tRvCDo^7u!}RT22r%VdQkEqxl5b1^#MBD zAI7szVUxjUV;YLi}{LzI0ap2cSKG*2~Bvp{Y} zcVCfPur7|mK6OQoY}ZzFs`}5{-UH7icoqfbT6bUNHC$nZp0zb4KdeL_cvP`+&3;+7 za=K%qoJb2Fk|UTzf19Fg*^#tRmQ?$XNo=c{+`USRxh%cPY~M>U>K$jTnste6Wo(ju z7a*Ce+@#$aqCD(FTQ6~|em$WE6CjDvbJO?|I^+eTu11!#8O6@#p>*;TjGg!>7$~;0 zd;*odD*C6>Xh(BmJKz1HFr08*Srb6ahVbbA>FcaHA7vGk* zP)L@1o@#346}115l7g88x!o_A6s_s#6gj$;m%*^{YO5m9ZB_h<>o)*?zKw?(h27=Z zI_c;fbboeVC5g%4Wn0@uIZ%N1cqz@P<_X!G7PnR+d|e`wQA$Z01H_iuuk z5@C@-w2(yQBWBy9M6#BYyie|?joPDJVZP!z84=+oT<|tWiP4trQ|2nP@sPY0;946pGp2z;vzrS&46P2SN&;55C5ipqj2b0KV1*oN*wgAFqoXjc^t>Rgikd5 za$$LiX?qhwIkx$ULp)-|4`(jD^s80d(ki>W1vaoJ{b`_Em=VJ5^q)#UwK$~gU>S~4 zrF}1^G573Z#uh@ciE0#XKb>|ASY{uT+uf9bvb)SYaW+q!#uD4-1y7=IR!AK95^pAb z&A3EAP@j+Y={lnVbw-izP-To`fBDHTxR;N}N$B=cM^3BtoalVI(nfuEb(>aVU?v9j z797sfjbfU*U%83-I+$MQpuJ`xk8dIFz!QlHH_nmRiu%MLFbjEbkZ2adRuu;?#j`j} z7DZeB%!9g^;N>_QChnS;tJl8c0G4BWp-H#K5rIa|1lK%r)(1&GqPCNP2uIfGjk0d# z=AAQRYI#U`*fX6LSnAM)M!4ZkClhp4G6*XRX3V42e}hizdr&UMi11zsW49c+FRjSM z4ch(>HMgV>R7>9|<@iOR_o#yM-NMuF9zxPV$eJ|NruVLtnqxx8uuO7JTTHreC7>yRPfDWh?eyxO%oY%z!zxaz!m)>e#V1u+K`BlL~?3J}*e>9@}Ci`K0{Urq{})@Ikk(D94V zjn_=Pr2Wqf;+Eo2kK<>wiwjBhd!jEa@oJ_2J71zVaT%s`F&}eU80ogCpHfhd{3TX3 z&6a857+i|=x|mI{KY~Jx8i#4+S!0=M`xX}n2IA<8mhmmmV#b+xECyx)4+3=XJD&cp z`xlm4c>a4%_;;LodHg$0wsrDiHG5VwO5cc1!M+Z(T4CR5N$X)q@31B*%+`S=9e5Cz zN0YTiwjbDxCtH}ZiOITY%`(N6(wcn>Y-rk3f|U&W0L7~I>>APFzPrV?#mufAJDb&7 z2%V<^dI817g?O*go*gn)1LC;J8_G}rVa8NxOH<=;Bd@pvEs)m6@oxG@IuXYcg`G)B zW+mEbWr_<5Ma`uBjFq{X(d=VN9F09LE^2#aIk{PG#Wf%aO_*bZ#u3 z^d8MX4=%_xMG>AkD)vDT7+g{oS;O}1*?MdqaxBx#}yNy8~m1b U->QsJ#}{-_v+?h}{5G`vZwiL*FaQ7m delta 18057 zcmd^n3tUyj_CGUw4{z|uTS7eI5m5mF36ana5Al&H;v?#siHffj0ny6m(ag-u&~0Za znII~fnW7-O%=fDxjZryJE|DWIgzx{mA%$}J& zvu4d&Yu2p2(NZ65SrJ?s7w^(^8C+ZzFc%kZ++VwX{kkm1txGQ84m<+<2?a}b=Xr6_ zFlOf7V6BKh%r@3s;a*H^L-miVih?`KXMY5eP|ykO!;vlk3IQFs6PTl=sT~5>Am<5; z1YQJ=0n4>nYA5c?v@$h3a1jd9fJ49#z=G~7wYStB3m?Z_C{P4+1|onZsBMpQCeQ&0 z1f~H#z$V}!zz9qM9s_0ralo6v9l%at43G>=1>_{|MYzrj^p-(ELv66MVsuie&KUqZ3mW&aE555QSqA?_tT=aIgPdzqJ4cC)-6`g#WU;mjq--XIzUZ2;MD7%FD0 z$_w_$YORkHm;JeQ#T7}(!kN=bKs4qj)0T7#=Udq$fT zw2)2Lt^^%dgHfYpr<$}l?(VS#)N$Ax7wt!n2masYl^bR$`6RDKUft7~Hs8X+YWfDa z^8bKqt`4qgb8e3!Q-XXxNi%Nmr(^A=;}vo=T>(p)%t& zP;r<+$^Gk3b*?dmoef-Fm+5{Q1r5!g)=KKKwR#>`bGa^-X`A3x0Ka@MqnVguyZ zz2@6nyM$;J^=gf&zMOe5In9$Tdb#{sA{kZ1K$o6Zf@+3j@9`l^A~$KK$`0DA;}dAs z5O&OiRpXhLn?FY-L)i*4cjA^B%h(F0881XNjjWYi7>Xw#c(s6YwqhA(_JsCrdUwsV zDqAc0+fd5r#S^qEe``~tP6&^ziSRqe_N*}a))>b%u+{A%G}D)1THg+yH3Qb4cH3)Y z4NSAdnQBZ`XT25<1TCY15J1H!azl_aZ(Pzt;NB%{rCq!~y{xyv}0PjQ2G*S(Q`2kZta0Dm-CiPRT30bB&G z0tW!ez+#{t*bnpvP61y6hk<#(qrhY6W3hHVA=vN9#;M4WUP8@hE}Cc0(8&R4P=ed{ zfzN?3AQE^7cpX>@d=G2@7}|b|RId6_Kz4OkV=D6xA}^I+=Ia{M1_w)ba6((Zli7CP}TQ)%9+{jl$XVLVDY1onj}1=>*!QHi@m~R6BV7hUko#j#A6WH_VAfh; zHf$O*GdtuQTu5-tYh)R!QA1PH*iA8jlvMM|b9X7rXVsIio?qgbv%mh3>u^G&5M zM6t>0N&6FBS(Ls06z<+@svq5zZDxzLv<*|}M6nV|ZEA%{-^N+4vbqiy#t&0i&yG9n z-!p8eU}r#ZlNs^;9t$cRHhERKGaiHCo!1c*&H|RA(Hx}9kUj=12ObCZQp|TO*k?7e z8n6ad#7b<7QZ|Dg)!cO7;?%NjC+s zvx?Q1zi+toz5k=L$6tOgcUjK(fLYg}aK z(c^9SQ<|}>s+&UDSs&{}_8>p+N^>T%LGF7&Pc$acOj0I7QaYokON-)?No<8Rf*Cbq zxk{S{F{3Jdn5Ko%OGdD4=6)7M=bAqzva#-uYv1y&dPFok=cQTdVXdzjnY-lV+s&J5 zCD&I}r7+K**m2pixgD2|`){%1%P^W;h+k#L<$(X=?6^D zvg6<2{#V&?ZT^%lWZEI!n@f1+E5IpT`sE$W8+-OTNWs3dY;{*i^&m)foi{th44EIT zDtmo?r#`RirkzXf-8Ei{wHOYa9;aU)$~to0_#QJcH4!~F2_BXF1#WH*rS;{B+V;O> zXvQKK+)A;8O(W*U22kN^JcxE~Q+;XrCZSc0V%+*Udz8-Su?|}Gzg%g@=j?$tQID-X ztIvD;rH=z^?|m5>cC$O`OjiE*VV7Ke${mjPb z{88RZ?=_jdp^*7Gb}N;pFfZNw0;;MCn5tQ>oXbi{^ecc_p_a^w6>jW4lP>BCeq+-N`O9h344TD53>PkjWmAFEve)%49z|i z#eisMvA6(pI!pUDq^lM&E>e|l%#FHdG0S}EF4E@LS+>t}xP3%U8CNzf3K8kFQs=(< zuw`sl8|&ZMUk%428$O%5%;m_xqSpQS`{iw;Y4>&3nT}2s(V&i9USepNoQJ161`o+gURTw1nDvR*gLFFzuU6luZ{IC)lz=7@PB@&^*$ZE%F16pU??y4b**kTS{Lk2Q_!}R*cswGm(=|~xW>iJCU5PGr;f6gwtYu8o*8Mn% zEx?0A^r6?-ZdR4R`Reb^nv$`ay@6Cf5;Fp6AK>=J`w2)_f4^q>M5L2|$-op~DsVqA z4VVrT0L6gpqo^@`2x$p0tMUE;qz^Xc=OCR6%md~F3xI{d!@y+1Bu7x;ITk>$x}Eiv znXH)6-`!vneO>uiZq}RFWZ~Pc>YZfUr5NPuQkqL=RbBKC+?V3H6aeX^q>H?QhIX5vSA$E#@oJ zDvPI{J&sp1YZk9{`1&to@x+nw7`zYUGXv@ENaaqEhP3INl8L;uKUqlo0|S6T03_l1 zUX%|;{!U;y@&|!hU?de~aep4nXjFe59&le{^Ss7%JW{gt=jK(GQ6{2nwfQt?; zR{HC4OO$=AE=W1T)F^vlW8_f`z=^Y*zTM64p!y$JDp~eG@2Am~J#3-tDom>~i00Jb z#3H$4Z2qM8{dlrbtX%#GKSS9440(v5%3D}R5Bmabh5R|?luA+*@H0Qfj7hT z^kRO1QFSLCLf(57Uw!czWj3QVv(zLq^kK1ViQJuL=!LU+Dbo*~Rqjz_Wg=BP&wc4w zr4mKH2e~VK+Mq0B7Rq~Hy_d3AsV3_ouIghxRHD49mT^wkj&h@3^@VbeI~h;&IdtL+ z9!i$Kz*yB?RR&@FqbyPGfF?cn2c?)(`c!qYwVH?Nw(E*nrEEnVC%1!WeWj~9^_IFY zrJC8d)&j`e44@Dw0*V1T(q1tJOP!vFJE_wPkV>6?n7h#K&dP9oP=s11s=BEvEnlDp z=mBx+=N>fnF#3;9RgVV#E^d;i+$c6+WS9U)n8B?@*n%t(;9J#;d?JClLHe8=)yhcy zT8Xg6C_^ajF*Sg)3l)_%jKt)tXDDHM*cyZ-sNxi#OhaB#jVz4H7pQmJA17O(;;Jv- zqRw_DLxtilJ!PXFwNo9Ztd2l5b}}N&gZUw5-7E&!EuTJkvlz|nVIN6o6X=C4VgMbw zRn4@{SHkJ@-Jqgj7mu*ka6diqfO?cs={Jh1tB2I3tS&^^&!X%bBDgD4>A}h1=FL1f z`gmBQ)z#MTFzGg5rI!DqyHH4rKjcP=>!u{o*;iH5Q-xxXKIkxXlPo8A5;eSnQ^DBp zae5j1rrHf|v%g8(9A$Lh0+rg-T&=W{GMox{ymncpWeD2Gvux?yWpQkF(xa5u%|c1? zfG$Hx%K$6dv;%%qD5>yqlw&vVJvOUWd40yq4&jnb9)DV|8S58%%ndrRFPNCl57A^NrZ zu|57yhMkswq|T<&Gb~w8zM$p^;v2cGwGr2fvXXS$XX=~GY0GmwmiBUNEXnr0gDMX4 zSZ(Y-{m85-z8(cbh8Fq0aDw$|+E^ke;v|pMtG`gsF!HYy;Z&h1A+DD{?3`b{S3jUB z^^Crl1Kl*f|5u&?bwK) zJzJCs6tx*0T&jUZ`tAqSn-VuD-RQk_N)jD?3eVyRyWNkV>h&;n^ERXB6*@Oi<~p?N z@mJGsEQt3-U>tLp=1iqbp9zA+jqw(OK zAZg47@E5a-l=KIXZ&%fiQmavB=jG1NFynyT7^6})@h(#a(b6&{i{9M@;=XzoW7u|s zIL}JtcHW_eQP>txV3dnCXb(7JudsE(oXqPn#r4~id9s&Cy7VqHQg#hEgEa*6b|@Cg zU8@9LmRK+`}Uw51vyC3GcqhK!<$g&%95=-c>>Uk__P!+nz ztv3}AdYQUY*-7_Q;aN;3m8z`DI5LdFxQ6F2+NEw_&g8AiD2FU|OT-#la!`A15BfWB zo`pMRImoJvrfG6W^H%U>z&h{`GG?D;9DP~?GO*TuG^QQa(#{t&PNwQ-F<(0^Wlw{a zzdwymx2;oRnV(ZfT(RXIIZ!=&k1|%OLb42~h0xJ5 zWgp+^wgP?kx|PtBtPW)?!m#i}bMq(l1)KjA@p zj|r7&<`dzxwpLjxNlK8M8%4$);Gs8x!$&q_Vpu14I#dDD=apj;c6F@S3;BQ+N06}s z^uG(~_Q>ovc%U>P2~D;rno3?mpC7-j9HtW+c^DhjRAWEAz?a;wtY#yc3Zv=xdW5qk zOyLGvvroy9-mSM@qANQ&q8N?mo@hG!GN$%Ty>gN+O~*@adn5M)N)==MsA#FulSa*f z42`P;-*&#DT=?1k(-oYQsqI0f2hBUE1l)|cZe%*7q&6E|w;ocG8O{k#YSel(Xf<$% z{$9NjD)a-sY6@ZyI@`{_R_IVi2*g-#{OsJW_>$orxMoLp@)2I-_knUjpY{^J8pdW* z{4RbcReZ*Msc?f5st7gsZ9^`yD^2P~lfRPLETS6GFSR%b**)l}n6l zU*J^uWJfrF)zJua5B5`P87+@fC+ZW8$}yErG7(GnRWP`=K?3f56{Ek zRoqhbfPTOv0vL_;RV(y&yC`Gbww~-PxSrTUIqFK6ywm}7w!a#xkL#;!5LB^R4bp?M z6bszH;i`qg?od7^<1y^aHrQi5a)@$9Awvno5tyr}dO$jjL6YPg;_Pl4me zw{sI++QEaip6ntxMf6aI=*zxQ9)$Zp6T*FB8B5Yfe5dSS6#f=OFho@&ww~;YmWMv& z_t4mnc#!5hp|Ad_s)k_K+>J#l+=3?RMLn}N*V%z-D z<0t18uf_ldbmHz(L&IKHd(eGLL>R626ejv{gYcmzSBiMD)vIo7=S}JZDeV)rJFC4( zRmaG@r18BH6Z4W|;_?&oViG5gPl`!SPRfgkOG->g?iqJ$Vp6Zf`7hk$Q6Iy^DKjVM z6-=2venvq_@r-HH$kL6)ux+i|KH(u!*mJE*3y!FL*e+*jsBT*-ES$rNA1QwQ58>zzM(dS2n^{DP7xB@f9-wa1a*;O7a{_YT!ZxA+Mgrw`sx!Tq^7EoP&sEyvdeEx6W&4Q`A`B?&S=%<>JdM|3`a;C$(-Ow}lIhp4W2)h3w3#8#*sK5wb)TdNL2FUM#^j7y-EQ%{$9s3ZgWATUlldev=zKjCp^1eZd;Tiv@x~;xi}NSujf)*WGo7-% zL=Y7{4OezuEt^F5lw-v$6(TICZxc;O+XB9lEW7AMQC_SXNKfsBG#spBh*Wxt_Ox-b z+KD=yW&g`GZsx4wiFxB4%91y}B)@p(=$lGsa9|dMHVs0X(I~X}#ULt65*2CZkl#-m z*q0?}ax?1WaarnP=FNn?KTC3O2!iiN@3@MfATF(f{fe^rATQquWZn?|R;ug}`!Mee zXuf?ips>&Ql+m+h%6UzlF{2=lUb%>1>L4k*Nwl^*cKkbB@v8HmHZ_>8Z@@akB6vu; z1KAwM>sskm8_u;aJ(|uRy%?W*YSyGqu^kbT57II&bzoB)-{}(Ep3mH{*me7uKbd`) zJH33Jwew2{!S?sXcpR^x?UVh9YIgVX=x5hc5Z}|zUe7zByN)sk zM|(I}g5ORy$r1TVGV^AJqRZBlVwE#QSRaNh#^xGLGwRqd+Fg&MP?@XnX1(PIv2?^$ zbnGz2*=<*wjBUubpfa2WvgJi>Ger-$6bEaeF*co@`NuoOzRJu4+dXgvVrG3R@CK)438cbq{Pj5_9&Q_*Sk*T6PEi;RA_1=E}bWO)wpDbN10be^Lt;{~2>ovViWTmWtIy@x%u z56P;jXo6_xk%5oI^Y9s79u4zXy69sqIQn)Sk27sP3S;s0c0Q^n;G6b*ls$l5uoxd9 zWumMEh@#!Qly-q5@H(*&4U8ysEv5V zs`a`WtyeBq*D?jK)rhTB13Ub;gycF1S$ue)AJ~^O7zJHNvl${#4QfLvr-=N*; zYA@?rwXa^g1kpJvo&XWvvr7r>n1yfIrs1EH6^$$!ia=cnKE9tp^9PEqO#J+?bY_6a zqQqdfQ~z|S8sY=VQ?vDKtGYm;rR{hd+SFdOheT&kMj4dI={{*uJb>OVQ=^@Jl$-DG z=_VCp;!?0B9Bz_b&h75mFZoHdy8>Sm*owrcu692v#^Fuf9-dPHy=8@ zUrq6<#XcvkwwudOc5Qegp5*Bzk|<=Ua5x)2`J*S~kDE0K-p$Mz)5!dZ8vQfA%^s&0 z<1pmG`J*T0P0yd1KXc}k83m&!Pbtoyo>!1J$>AUU&K_JAU3^3Jb@+XOjZRd@BX}EN z-=v-MQr&*CSq1;pd9rxGQnmJ)Y#umwBsq4-FFi!0F}u-{;{Bvez9*A=b>>#5gYxS= zDtfzKO}7(z6J~d8lPTGceH!e~$1ZYfM#|`s_APLb+yZBg{v{7fHx9)2p=?15txWLw z6y(Y`u`=!BFb@UNT(uXAF_|Cf?JCi0l_j@j@PFk55lVx`slGANrsd-s)BCX@N;5Xw z{(C2WXX0NTv{UAyT)LwB&XjkDc!;X6sq+XXr;c_jBFMY>yc|2>DYR|6K13z}Xml-a z_9o{IhEgRSgyWz0OuNRxH z5W6z~bL+hPkh8m!tyIkSh6l%HI{g%X>BI-#aOCVex_T3*ILJ8wGM6b#BcT^+eKz>?Q|v4Ng@0Ln_iR3ad23(kKs)RUseMV z%lJK9u(s*w)#-xCVW?j>cAIocnEfo&eXH49*nNSPPLkON9?DJJ9bLLR9<96#GQA*T z{|9*(#8;{T|Mm9$ANmpPexFhQGpB)qLUD|3IEJ&+@8lxra1jcAy?N~Qo6Mi|&suwt z8wQC~rboP_j$xFTg0oBM90bKeQ^kc}@kyMNa!xrmF%ItPs+W~p=|r@M^O3$@7L-D! z{hRrGztdWckcRw@x-h5`J{^PZ8BgK;O~Xk17}CTjDi|Q9-Ne)TuNF;5r+8CzVmyu= z2<7_0to+>8_1o44TN8WG5gV#-x-h;~gK=ujX)BWHYI|{nY;AFD8C!>)r}gnmntigk zvUq>Tmw)oq@qqp80q2hL%%Y3@9@Nk6Qie3TD5=C5B|9DQwC2_*j;FTimzWk~XDgZ^ z#6tI7#Fs!Jr6L3&+K$}cHNSXZ!NeK%ty1nS&dVVE9W!fX!$8-sIw=Y4_L63qI!;P# zjs4g{m1Ys340S1>j*Gef$S4Tb6nxiM0_VFJo4XNzxf+je`;9j#Pq3F~!1I)UijoI6 zp#<&5x44(vk*mXV|AP-@QX%A_><>_ge`7NLKsEgT!emOdye{ocpk4d}nYX;@3coxp zW+C~#tL9LSL40XfDlBm3Ca3PYdrNtS_Sr9r=OWH~{In_gjT(q=1J^?-?<;3*fBI)n zmU0%?>Og?-#`U3yW_XgrusMlY6Pb^cV*-NLXqlA${?-Gww*jaIkR-Q&&Wc~*t zfbNUu9rago#a|dL$VHo8Ys6LB`6rP>0ejUE`iNS!%KgqnP&n5m%_RXk)DMUCMEu4h zQj>1a_e3rp|1xp3i$N<{mxi1So~HpB2%yE`N%=3sr6&!2N$pBIm*DN6_seRE0*{>7 z4jxa1k1zvu%oV|QYIRWZ2J=e9aYdd2Jzds18F#Dal#=}EGe^r`he2#!*D~hZomD!X z8@f^qY>#uvym@nfCJag)mxvk}zRnMBejsd?cMP`i1|f$0*$8jX96cS!9)vOTXH$=I z7OQ(5QTsC1Xr(c5QXhmb(Qx#vDWhkWJcRSngVU#hkgT+x8JRuf`^EH5?v);snAkHd zrgwUBd`x_DQhZ!uzvRrcjQIKU%5IEDhBs6DVfcwX|*QJf{l^X zHdnO!Nw5({zVT{!MTTSSN30vFGmGvmXQSF4Yii`;V!MKT2pQbPlz+Q=2H$2!Cws6H z6ABG#7653z-ZH`;bip}FXWKW(ORdfP`9<;Z>OXQ|7M4h!f};kGfz z%h+G$hGsHVctpKT#$|mRYl~Zd2PG4oPa19V16{cgng3S$XDFhF;ZBD#^pmOqvU=FN z^>3{ysC1mBU7e$Z*i6WGmZLP&o&Nug?sz!WEyhkpfPDMaS`+EG5JBw%opglRB#+ty za;Z6OOvlR}^X>Lv3ygXj8Q&~$>5G%I{cNiZ`)5bH@sj^FhW$bXb=ZXPRG&?-X68+> zvrRvN&ay!KHG`Wzz*{`X8>7wAce5FV*%24QU$l-Re2N|FH5d*}qr*k#lhs@7rK9~6{RjhEU@Vc}G!`EJ1{2WcY2898*9Ze%w$UccT? z%-}8D^(LMEgS?X*hkNyZ`hb8x{izkV)2bX1LF>+l4Kk`v=kLam?Z)Z-8*;@ird#UaUjJ`p<0Fdz