From 5bdc5db4084be263ae33228fd943f3da2f6749b9 Mon Sep 17 00:00:00 2001 From: "nathan@daedalus" Date: Mon, 19 Mar 2012 18:57:59 -0500 Subject: [PATCH] Adding initial files --- axios.sln | 76 + axios.suo | Bin 0 -> 116224 bytes axios/AxiosEngine.cd | 118 + axios/Axios_WP7.csproj | 240 ++ axios/Axios_WP7.csproj.user | 6 + axios/Axios_Windows.csproj | 271 +++ axios/Axios_Windows.csproj.user | 6 + axios/Axios_Xbox_360.csproj | 235 ++ axios/Axios_settings.cs | 155 ++ axios/Collision/Collision.cs | 1945 +++++++++++++++++ axios/Collision/Distance.cs | 780 +++++++ axios/Collision/DynamicTree.cs | 654 ++++++ axios/Collision/DynamicTreeBroadPhase.cs | 324 +++ axios/Collision/IBroadPhase.cs | 30 + axios/Collision/QuadTree.cs | 267 +++ axios/Collision/QuadTreeBroadPhase.cs | 249 +++ axios/Collision/Shapes/CircleShape.cs | 207 ++ axios/Collision/Shapes/EdgeShape.cs | 266 +++ axios/Collision/Shapes/LoopShape.cs | 188 ++ axios/Collision/Shapes/PolygonShape.cs | 556 +++++ axios/Collision/Shapes/Shape.cs | 222 ++ axios/Collision/TimeOfImpact.cs | 500 +++++ axios/Common/ConvexHull/ChainHull.cs | 126 ++ axios/Common/ConvexHull/GiftWrap.cs | 99 + axios/Common/ConvexHull/Melkman.cs | 122 ++ .../Common/Decomposition/BayazitDecomposer.cs | 253 +++ .../CDT/Delaunay/DelaunayTriangle.cs | 420 ++++ .../CDT/Delaunay/Sweep/AdvancingFront.cs | 180 ++ .../CDT/Delaunay/Sweep/AdvancingFrontNode.cs | 64 + .../CDT/Delaunay/Sweep/DTSweep.cs | 1132 ++++++++++ .../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 + axios/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 | 87 + .../Decomposition/CDT/TriangulationMode.cs | 40 + .../Decomposition/CDT/TriangulationPoint.cs | 82 + .../Decomposition/CDT/TriangulationUtil.cs | 160 ++ .../Decomposition/CDT/Util/FixedArray3.cs | 118 + .../Decomposition/CDT/Util/FixedBitArray3.cs | 118 + .../Decomposition/CDT/Util/PointGenerator.cs | 38 + .../CDT/Util/PolygonGenerator.cs | 98 + axios/Common/Decomposition/CDTDecomposer.cs | 110 + .../Common/Decomposition/EarclipDecomposer.cs | 691 ++++++ .../Decomposition/FlipcodeDecomposer.cs | 160 ++ .../Common/Decomposition/SeidelDecomposer.cs | 1057 +++++++++ axios/Common/FixedArray.cs | 227 ++ axios/Common/HashSet.cs | 81 + axios/Common/LineTools.cs | 308 +++ axios/Common/Math.cs | 638 ++++++ axios/Common/Path.cs | 341 +++ axios/Common/PathManager.cs | 240 ++ axios/Common/PhysicsLogic/Explosion.cs | 464 ++++ axios/Common/PhysicsLogic/PhysicsLogic.cs | 66 + .../PolygonManipulation/CuttingTools.cs | 246 +++ .../PolygonManipulation/SimplifyTools.cs | 359 +++ .../PolygonManipulation/YuPengClipper.cs | 513 +++++ axios/Common/PolygonTools.cs | 368 ++++ axios/Common/Serialization.cs | 1453 ++++++++++++ axios/Common/TextureTools/MSTerrain.cs | 367 ++++ axios/Common/TextureTools/MarchingSquares.cs | 800 +++++++ axios/Common/TextureTools/TextureConverter.cs | 1338 ++++++++++++ axios/Common/Vertices.cs | 955 ++++++++ axios/Controllers/AbstractForceController.cs | 323 +++ axios/Controllers/BuoyancyController.cs | 135 ++ axios/Controllers/Controller.cs | 71 + axios/Controllers/GravityController.cs | 117 + axios/Controllers/SimpleWindForce.cs | 75 + axios/Controllers/VelocityLimitController.cs | 129 ++ axios/DebugView.cs | 185 ++ axios/DebugViewXNA.cs | 875 ++++++++ axios/DrawingSystem/AssetCreator.cs | 240 ++ axios/DrawingSystem/LineBatch.cs | 183 ++ axios/DrawingSystem/Sprite.cs | 23 + axios/Dynamics/Body.cs | 1388 ++++++++++++ axios/Dynamics/BreakableBody.cs | 142 ++ axios/Dynamics/ContactManager.cs | 340 +++ axios/Dynamics/Contacts/Contact.cs | 502 +++++ axios/Dynamics/Contacts/ContactSolver.cs | 794 +++++++ axios/Dynamics/Fixture.cs | 611 ++++++ axios/Dynamics/Island.cs | 484 ++++ axios/Dynamics/Joints/AngleJoint.cs | 93 + axios/Dynamics/Joints/DistanceJoint.cs | 286 +++ axios/Dynamics/Joints/FixedAngleJoint.cs | 84 + axios/Dynamics/Joints/FixedDistanceJoint.cs | 255 +++ axios/Dynamics/Joints/FixedFrictionJoint.cs | 227 ++ axios/Dynamics/Joints/FixedLineJoint.cs | 413 ++++ axios/Dynamics/Joints/FixedMouseJoint.cs | 209 ++ axios/Dynamics/Joints/FixedPrismaticJoint.cs | 636 ++++++ axios/Dynamics/Joints/FixedRevoluteJoint.cs | 541 +++++ axios/Dynamics/Joints/FrictionJoint.cs | 249 +++ axios/Dynamics/Joints/GearJoint.cs | 350 +++ axios/Dynamics/Joints/Joint.cs | 282 +++ axios/Dynamics/Joints/LineJoint.cs | 436 ++++ axios/Dynamics/Joints/PrismaticJoint.cs | 677 ++++++ axios/Dynamics/Joints/PulleyJoint.cs | 507 +++++ axios/Dynamics/Joints/RevoluteJoint.cs | 595 +++++ axios/Dynamics/Joints/RopeJoint.cs | 239 ++ axios/Dynamics/Joints/SliderJoint.cs | 298 +++ axios/Dynamics/Joints/WeldJoint.cs | 263 +++ axios/Dynamics/TimeStep.cs | 45 + axios/Dynamics/World.cs | 1456 ++++++++++++ axios/Dynamics/WorldCallbacks.cs | 74 + axios/Engine/AxiosBreakableGameObject.cs | 126 ++ axios/Engine/AxiosEvents.cs | 138 ++ axios/Engine/AxiosGameObject.cs | 88 + axios/Engine/AxiosGameScreen.cs | 397 ++++ axios/Engine/AxiosTimer.cs | 80 + axios/Engine/BreakableAxiosGameObject.cs | 44 + axios/Engine/ComplexAxiosGameObject.cs | 49 + axios/Engine/Data/AxiosCSV.cs | 18 + axios/Engine/Data/AxiosDataTable.cs | 252 +++ axios/Engine/Data/DataEvents.cs | 43 + axios/Engine/DrawableAxiosGameObject.cs | 175 ++ .../DrawableBreakableAxiosGameObject.cs | 70 + axios/Engine/Extensions/String.cs | 24 + axios/Engine/Extensions/Texture2D.cs | 199 ++ axios/Engine/File/AxiosFile.cs | 35 + axios/Engine/File/AxiosIsolatedFile.cs | 59 + axios/Engine/File/AxiosRegularFile.cs | 44 + axios/Engine/File/AxiosTitleFile.cs | 35 + axios/Engine/Interfaces/IAxiosFile.cs | 14 + axios/Engine/Interfaces/IAxiosGameObject.cs | 19 + .../Interfaces/IDrawableAxiosGameObject.cs | 18 + axios/Engine/Log/AxiosLog.cs | 52 + axios/Engine/SimpleAxiosGameObject.cs | 66 + axios/Engine/SimpleDrawableAxiosGameObject.cs | 92 + axios/Engine/Singleton.cs | 32 + axios/Engine/UI/AxiosButton.cs | 97 + axios/Engine/UI/AxiosUIObject.cs | 25 + axios/Factories/BodyFactory.cs | 399 ++++ axios/Factories/FixtureFactory.cs | 179 ++ axios/Factories/JointFactory.cs | 288 +++ axios/Factories/LinkFactory.cs | 62 + axios/PrimitiveBatch.cs | 196 ++ axios/Properties/AssemblyInfo.cs | 40 + axios/ScreenSystem/BackgroundScreen.cs | 74 + axios/ScreenSystem/Camera2D.cs | 369 ++++ axios/ScreenSystem/ConvertUnits.cs | 103 + .../ScreenSystem/FramerateCounterComponent.cs | 57 + axios/ScreenSystem/GameScreen.cs | 285 +++ axios/ScreenSystem/IDemoScreen.cs | 8 + axios/ScreenSystem/InputHelper.cs | 469 ++++ axios/ScreenSystem/LogoScreen.cs | 89 + axios/ScreenSystem/MenuButton.cs | 118 + axios/ScreenSystem/MenuEntry.cs | 193 ++ axios/ScreenSystem/MenuScreen.cs | 338 +++ axios/ScreenSystem/MessageBoxScreen.cs | 99 + axios/ScreenSystem/PhysicsGameScreen.cs | 332 +++ axios/ScreenSystem/ScreenManagerComponent.cs | 303 +++ axios/ScreenSystem/SpriteFonts.cs | 19 + axios/ScreenSystem/VirtualButton.cs | 46 + axios/ScreenSystem/VirtualStick.cs | 70 + axios/Settings.cs | 236 ++ 162 files changed, 43840 insertions(+) create mode 100644 axios.sln create mode 100644 axios.suo create mode 100644 axios/AxiosEngine.cd create mode 100644 axios/Axios_WP7.csproj create mode 100644 axios/Axios_WP7.csproj.user create mode 100644 axios/Axios_Windows.csproj create mode 100644 axios/Axios_Windows.csproj.user create mode 100644 axios/Axios_Xbox_360.csproj create mode 100644 axios/Axios_settings.cs create mode 100644 axios/Collision/Collision.cs create mode 100644 axios/Collision/Distance.cs create mode 100644 axios/Collision/DynamicTree.cs create mode 100644 axios/Collision/DynamicTreeBroadPhase.cs create mode 100644 axios/Collision/IBroadPhase.cs create mode 100644 axios/Collision/QuadTree.cs create mode 100644 axios/Collision/QuadTreeBroadPhase.cs create mode 100644 axios/Collision/Shapes/CircleShape.cs create mode 100644 axios/Collision/Shapes/EdgeShape.cs create mode 100644 axios/Collision/Shapes/LoopShape.cs create mode 100644 axios/Collision/Shapes/PolygonShape.cs create mode 100644 axios/Collision/Shapes/Shape.cs create mode 100644 axios/Collision/TimeOfImpact.cs create mode 100644 axios/Common/ConvexHull/ChainHull.cs create mode 100644 axios/Common/ConvexHull/GiftWrap.cs create mode 100644 axios/Common/ConvexHull/Melkman.cs create mode 100644 axios/Common/Decomposition/BayazitDecomposer.cs create mode 100644 axios/Common/Decomposition/CDT/Delaunay/DelaunayTriangle.cs create mode 100644 axios/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFront.cs create mode 100644 axios/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFrontNode.cs create mode 100644 axios/Common/Decomposition/CDT/Delaunay/Sweep/DTSweep.cs create mode 100644 axios/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepConstraint.cs create mode 100644 axios/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepContext.cs create mode 100644 axios/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepPointComparator.cs create mode 100644 axios/Common/Decomposition/CDT/Delaunay/Sweep/PointOnEdgeException.cs create mode 100644 axios/Common/Decomposition/CDT/ITriangulatable.cs create mode 100644 axios/Common/Decomposition/CDT/Orientation.cs create mode 100644 axios/Common/Decomposition/CDT/Polygon/Polygon.cs create mode 100644 axios/Common/Decomposition/CDT/Polygon/PolygonPoint.cs create mode 100644 axios/Common/Decomposition/CDT/Polygon/PolygonSet.cs create mode 100644 axios/Common/Decomposition/CDT/Sets/ConstrainedPointSet.cs create mode 100644 axios/Common/Decomposition/CDT/Sets/PointSet.cs create mode 100644 axios/Common/Decomposition/CDT/TriangulationConstraint.cs create mode 100644 axios/Common/Decomposition/CDT/TriangulationContext.cs create mode 100644 axios/Common/Decomposition/CDT/TriangulationMode.cs create mode 100644 axios/Common/Decomposition/CDT/TriangulationPoint.cs create mode 100644 axios/Common/Decomposition/CDT/TriangulationUtil.cs create mode 100644 axios/Common/Decomposition/CDT/Util/FixedArray3.cs create mode 100644 axios/Common/Decomposition/CDT/Util/FixedBitArray3.cs create mode 100644 axios/Common/Decomposition/CDT/Util/PointGenerator.cs create mode 100644 axios/Common/Decomposition/CDT/Util/PolygonGenerator.cs create mode 100644 axios/Common/Decomposition/CDTDecomposer.cs create mode 100644 axios/Common/Decomposition/EarclipDecomposer.cs create mode 100644 axios/Common/Decomposition/FlipcodeDecomposer.cs create mode 100644 axios/Common/Decomposition/SeidelDecomposer.cs create mode 100644 axios/Common/FixedArray.cs create mode 100644 axios/Common/HashSet.cs create mode 100644 axios/Common/LineTools.cs create mode 100644 axios/Common/Math.cs create mode 100644 axios/Common/Path.cs create mode 100644 axios/Common/PathManager.cs create mode 100644 axios/Common/PhysicsLogic/Explosion.cs create mode 100644 axios/Common/PhysicsLogic/PhysicsLogic.cs create mode 100644 axios/Common/PolygonManipulation/CuttingTools.cs create mode 100644 axios/Common/PolygonManipulation/SimplifyTools.cs create mode 100644 axios/Common/PolygonManipulation/YuPengClipper.cs create mode 100644 axios/Common/PolygonTools.cs create mode 100644 axios/Common/Serialization.cs create mode 100644 axios/Common/TextureTools/MSTerrain.cs create mode 100644 axios/Common/TextureTools/MarchingSquares.cs create mode 100644 axios/Common/TextureTools/TextureConverter.cs create mode 100644 axios/Common/Vertices.cs create mode 100644 axios/Controllers/AbstractForceController.cs create mode 100644 axios/Controllers/BuoyancyController.cs create mode 100644 axios/Controllers/Controller.cs create mode 100644 axios/Controllers/GravityController.cs create mode 100644 axios/Controllers/SimpleWindForce.cs create mode 100644 axios/Controllers/VelocityLimitController.cs create mode 100644 axios/DebugView.cs create mode 100644 axios/DebugViewXNA.cs create mode 100644 axios/DrawingSystem/AssetCreator.cs create mode 100644 axios/DrawingSystem/LineBatch.cs create mode 100644 axios/DrawingSystem/Sprite.cs create mode 100644 axios/Dynamics/Body.cs create mode 100644 axios/Dynamics/BreakableBody.cs create mode 100644 axios/Dynamics/ContactManager.cs create mode 100644 axios/Dynamics/Contacts/Contact.cs create mode 100644 axios/Dynamics/Contacts/ContactSolver.cs create mode 100644 axios/Dynamics/Fixture.cs create mode 100644 axios/Dynamics/Island.cs create mode 100644 axios/Dynamics/Joints/AngleJoint.cs create mode 100644 axios/Dynamics/Joints/DistanceJoint.cs create mode 100644 axios/Dynamics/Joints/FixedAngleJoint.cs create mode 100644 axios/Dynamics/Joints/FixedDistanceJoint.cs create mode 100644 axios/Dynamics/Joints/FixedFrictionJoint.cs create mode 100644 axios/Dynamics/Joints/FixedLineJoint.cs create mode 100644 axios/Dynamics/Joints/FixedMouseJoint.cs create mode 100644 axios/Dynamics/Joints/FixedPrismaticJoint.cs create mode 100644 axios/Dynamics/Joints/FixedRevoluteJoint.cs create mode 100644 axios/Dynamics/Joints/FrictionJoint.cs create mode 100644 axios/Dynamics/Joints/GearJoint.cs create mode 100644 axios/Dynamics/Joints/Joint.cs create mode 100644 axios/Dynamics/Joints/LineJoint.cs create mode 100644 axios/Dynamics/Joints/PrismaticJoint.cs create mode 100644 axios/Dynamics/Joints/PulleyJoint.cs create mode 100644 axios/Dynamics/Joints/RevoluteJoint.cs create mode 100644 axios/Dynamics/Joints/RopeJoint.cs create mode 100644 axios/Dynamics/Joints/SliderJoint.cs create mode 100644 axios/Dynamics/Joints/WeldJoint.cs create mode 100644 axios/Dynamics/TimeStep.cs create mode 100644 axios/Dynamics/World.cs create mode 100644 axios/Dynamics/WorldCallbacks.cs create mode 100644 axios/Engine/AxiosBreakableGameObject.cs create mode 100644 axios/Engine/AxiosEvents.cs create mode 100644 axios/Engine/AxiosGameObject.cs create mode 100644 axios/Engine/AxiosGameScreen.cs create mode 100644 axios/Engine/AxiosTimer.cs create mode 100644 axios/Engine/BreakableAxiosGameObject.cs create mode 100644 axios/Engine/ComplexAxiosGameObject.cs create mode 100644 axios/Engine/Data/AxiosCSV.cs create mode 100644 axios/Engine/Data/AxiosDataTable.cs create mode 100644 axios/Engine/Data/DataEvents.cs create mode 100644 axios/Engine/DrawableAxiosGameObject.cs create mode 100644 axios/Engine/DrawableBreakableAxiosGameObject.cs create mode 100644 axios/Engine/Extensions/String.cs create mode 100644 axios/Engine/Extensions/Texture2D.cs create mode 100644 axios/Engine/File/AxiosFile.cs create mode 100644 axios/Engine/File/AxiosIsolatedFile.cs create mode 100644 axios/Engine/File/AxiosRegularFile.cs create mode 100644 axios/Engine/File/AxiosTitleFile.cs create mode 100644 axios/Engine/Interfaces/IAxiosFile.cs create mode 100644 axios/Engine/Interfaces/IAxiosGameObject.cs create mode 100644 axios/Engine/Interfaces/IDrawableAxiosGameObject.cs create mode 100644 axios/Engine/Log/AxiosLog.cs create mode 100644 axios/Engine/SimpleAxiosGameObject.cs create mode 100644 axios/Engine/SimpleDrawableAxiosGameObject.cs create mode 100644 axios/Engine/Singleton.cs create mode 100644 axios/Engine/UI/AxiosButton.cs create mode 100644 axios/Engine/UI/AxiosUIObject.cs create mode 100644 axios/Factories/BodyFactory.cs create mode 100644 axios/Factories/FixtureFactory.cs create mode 100644 axios/Factories/JointFactory.cs create mode 100644 axios/Factories/LinkFactory.cs create mode 100644 axios/PrimitiveBatch.cs create mode 100644 axios/Properties/AssemblyInfo.cs create mode 100644 axios/ScreenSystem/BackgroundScreen.cs create mode 100644 axios/ScreenSystem/Camera2D.cs create mode 100644 axios/ScreenSystem/ConvertUnits.cs create mode 100644 axios/ScreenSystem/FramerateCounterComponent.cs create mode 100644 axios/ScreenSystem/GameScreen.cs create mode 100644 axios/ScreenSystem/IDemoScreen.cs create mode 100644 axios/ScreenSystem/InputHelper.cs create mode 100644 axios/ScreenSystem/LogoScreen.cs create mode 100644 axios/ScreenSystem/MenuButton.cs create mode 100644 axios/ScreenSystem/MenuEntry.cs create mode 100644 axios/ScreenSystem/MenuScreen.cs create mode 100644 axios/ScreenSystem/MessageBoxScreen.cs create mode 100644 axios/ScreenSystem/PhysicsGameScreen.cs create mode 100644 axios/ScreenSystem/ScreenManagerComponent.cs create mode 100644 axios/ScreenSystem/SpriteFonts.cs create mode 100644 axios/ScreenSystem/VirtualButton.cs create mode 100644 axios/ScreenSystem/VirtualStick.cs create mode 100644 axios/Settings.cs diff --git a/axios.sln b/axios.sln new file mode 100644 index 0000000..f0d0e45 --- /dev/null +++ b/axios.sln @@ -0,0 +1,76 @@ + +Microsoft Visual Studio Solution File, Format Version 11.00 +# Visual Studio 2010 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Axios_Windows", "axios\Axios_Windows.csproj", "{742C938F-997D-4EFD-95D2-BB09CDADCD2E}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Axios_Xbox_360", "axios\Axios_Xbox_360.csproj", "{B5664516-72B7-4BA3-9F72-25CAA90867D8}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Axios_WP7", "axios\Axios_WP7.csproj", "{C09D9005-76AC-4F1A-9479-2787BB3DB158}" +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 + {742C938F-997D-4EFD-95D2-BB09CDADCD2E}.Debug|Any CPU.ActiveCfg = Debug|x86 + {742C938F-997D-4EFD-95D2-BB09CDADCD2E}.Debug|Mixed Platforms.ActiveCfg = Debug|x86 + {742C938F-997D-4EFD-95D2-BB09CDADCD2E}.Debug|Mixed Platforms.Build.0 = Debug|x86 + {742C938F-997D-4EFD-95D2-BB09CDADCD2E}.Debug|Windows Phone.ActiveCfg = Debug|x86 + {742C938F-997D-4EFD-95D2-BB09CDADCD2E}.Debug|Windows Phone.Build.0 = Debug|x86 + {742C938F-997D-4EFD-95D2-BB09CDADCD2E}.Debug|x86.ActiveCfg = Debug|x86 + {742C938F-997D-4EFD-95D2-BB09CDADCD2E}.Debug|x86.Build.0 = Debug|x86 + {742C938F-997D-4EFD-95D2-BB09CDADCD2E}.Debug|Xbox 360.ActiveCfg = Debug|x86 + {742C938F-997D-4EFD-95D2-BB09CDADCD2E}.Debug|Xbox 360.Build.0 = Debug|x86 + {742C938F-997D-4EFD-95D2-BB09CDADCD2E}.Release|Any CPU.ActiveCfg = Release|x86 + {742C938F-997D-4EFD-95D2-BB09CDADCD2E}.Release|Mixed Platforms.ActiveCfg = Release|x86 + {742C938F-997D-4EFD-95D2-BB09CDADCD2E}.Release|Mixed Platforms.Build.0 = Release|x86 + {742C938F-997D-4EFD-95D2-BB09CDADCD2E}.Release|Windows Phone.ActiveCfg = Release|x86 + {742C938F-997D-4EFD-95D2-BB09CDADCD2E}.Release|Windows Phone.Build.0 = Release|x86 + {742C938F-997D-4EFD-95D2-BB09CDADCD2E}.Release|x86.ActiveCfg = Release|x86 + {742C938F-997D-4EFD-95D2-BB09CDADCD2E}.Release|x86.Build.0 = Release|x86 + {742C938F-997D-4EFD-95D2-BB09CDADCD2E}.Release|Xbox 360.ActiveCfg = Release|x86 + {742C938F-997D-4EFD-95D2-BB09CDADCD2E}.Release|Xbox 360.Build.0 = Release|x86 + {B5664516-72B7-4BA3-9F72-25CAA90867D8}.Debug|Any CPU.ActiveCfg = Debug|Xbox 360 + {B5664516-72B7-4BA3-9F72-25CAA90867D8}.Debug|Mixed Platforms.ActiveCfg = Debug|Xbox 360 + {B5664516-72B7-4BA3-9F72-25CAA90867D8}.Debug|Mixed Platforms.Build.0 = Debug|Xbox 360 + {B5664516-72B7-4BA3-9F72-25CAA90867D8}.Debug|Windows Phone.ActiveCfg = Debug|Xbox 360 + {B5664516-72B7-4BA3-9F72-25CAA90867D8}.Debug|Windows Phone.Build.0 = Debug|Xbox 360 + {B5664516-72B7-4BA3-9F72-25CAA90867D8}.Debug|x86.ActiveCfg = Debug|Xbox 360 + {B5664516-72B7-4BA3-9F72-25CAA90867D8}.Debug|Xbox 360.ActiveCfg = Debug|Xbox 360 + {B5664516-72B7-4BA3-9F72-25CAA90867D8}.Debug|Xbox 360.Build.0 = Debug|Xbox 360 + {B5664516-72B7-4BA3-9F72-25CAA90867D8}.Release|Any CPU.ActiveCfg = Release|Xbox 360 + {B5664516-72B7-4BA3-9F72-25CAA90867D8}.Release|Mixed Platforms.ActiveCfg = Release|Xbox 360 + {B5664516-72B7-4BA3-9F72-25CAA90867D8}.Release|Mixed Platforms.Build.0 = Release|Xbox 360 + {B5664516-72B7-4BA3-9F72-25CAA90867D8}.Release|Windows Phone.ActiveCfg = Release|Xbox 360 + {B5664516-72B7-4BA3-9F72-25CAA90867D8}.Release|Windows Phone.Build.0 = Release|Xbox 360 + {B5664516-72B7-4BA3-9F72-25CAA90867D8}.Release|x86.ActiveCfg = Release|Xbox 360 + {B5664516-72B7-4BA3-9F72-25CAA90867D8}.Release|Xbox 360.ActiveCfg = Release|Xbox 360 + {B5664516-72B7-4BA3-9F72-25CAA90867D8}.Release|Xbox 360.Build.0 = Release|Xbox 360 + {C09D9005-76AC-4F1A-9479-2787BB3DB158}.Debug|Any CPU.ActiveCfg = Debug|Windows Phone + {C09D9005-76AC-4F1A-9479-2787BB3DB158}.Debug|Mixed Platforms.ActiveCfg = Debug|Windows Phone + {C09D9005-76AC-4F1A-9479-2787BB3DB158}.Debug|Mixed Platforms.Build.0 = Debug|Windows Phone + {C09D9005-76AC-4F1A-9479-2787BB3DB158}.Debug|Windows Phone.ActiveCfg = Debug|Windows Phone + {C09D9005-76AC-4F1A-9479-2787BB3DB158}.Debug|Windows Phone.Build.0 = Debug|Windows Phone + {C09D9005-76AC-4F1A-9479-2787BB3DB158}.Debug|x86.ActiveCfg = Debug|Windows Phone + {C09D9005-76AC-4F1A-9479-2787BB3DB158}.Debug|Xbox 360.ActiveCfg = Debug|Windows Phone + {C09D9005-76AC-4F1A-9479-2787BB3DB158}.Release|Any CPU.ActiveCfg = Release|Windows Phone + {C09D9005-76AC-4F1A-9479-2787BB3DB158}.Release|Mixed Platforms.ActiveCfg = Release|Windows Phone + {C09D9005-76AC-4F1A-9479-2787BB3DB158}.Release|Mixed Platforms.Build.0 = Release|Windows Phone + {C09D9005-76AC-4F1A-9479-2787BB3DB158}.Release|Windows Phone.ActiveCfg = Release|Windows Phone + {C09D9005-76AC-4F1A-9479-2787BB3DB158}.Release|Windows Phone.Build.0 = Release|Windows Phone + {C09D9005-76AC-4F1A-9479-2787BB3DB158}.Release|x86.ActiveCfg = Release|Windows Phone + {C09D9005-76AC-4F1A-9479-2787BB3DB158}.Release|Xbox 360.ActiveCfg = Release|Windows Phone + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/axios.suo b/axios.suo new file mode 100644 index 0000000000000000000000000000000000000000..4aa991cd97ea3c47b577de700711bbc497455c0b GIT binary patch literal 116224 zcmeHw4V+v>m2VFQC4y)qARyrIEhZTz$z&!0f|;3~A%T3s3?#^~v8ShJm}JsDboV4P zBxrO+J{AE1MV5suqJRnrJQNgpBD?M?!os?{KHxn+_WgMJcGvy6x<7yFzMt^^|65&C zRrjlJclCVqq0)GUa^U!8T0}ckf4Zt=(1n_pie88cAcLNRs zyaRAJ;GF=lKw<&l2*6Q*g@7Xg?*SYOI0iu8crVT;0FDD35BNL4i-1PJiGY&;Cj(9a zoC-J%a5~`qfHMFe04xG51}p(I0nP+81C|1o0hR+=04o5kfHuHNz$(C5fU^PT09FIe z1$+>&2G9=Z0CWP9fG$8c;5@)uz&gPBfC~WY0UH1t0h<7u0T%);0`vfS0T%-<0c-(m z1zZaF5TFl0T3?Ryp8&Q2J`6|!`T=Rc03ZVx1PlSL01N}R19kvL06PI$Kn^epxDt>D z6aYoQ7+@D*H((qv0k{gV2e21#HQ*zFj{^1qF2b8~_+=w%p2a!u`~qJ@JdOg_NLFV% zh8@p7vFO|_|L=u{x8R|DY)l-RQDP1?T!+#Janrr|ErCyA`8PG)DXS;?S zD8J!EJivY$wf^()Ec-VP_RoWGW}EO@0_6ao;d?0uC<6`yyaRAJfc>A(vj0;y98vl# zf6p^z3S|Ig0cC*f|C9yq12h8o9Qj86ZmRk}-_QQfvu*$9JJ|nuj@th(!}F8@Q`P^8 ziw?5?^PcSgyq5i+*ZY9ayyn(x9`5_a+B@E8d+XB+?|L1#DmwC=5U+h;+Fj^8ePHHs zFmeu``@qb3bgnFz+q|zo(*wbm2g7IZTn|JKyFdm)t`7n@g>wdX)e}v)E+l?_;)eN! zcOQJwx8Hm67sr42^Vd0Cpf=7y+}EOi>7PUE^B_ons@#v3zEAHZxYtr$!bxZXW9&xda65MT6xa1N-rJ90aHQ*@b3I z6z6}^LdoU8DP~r~!9YVQFAel$O3}mJ(a11&= zfbyv;P@YFE|E+j-2ZbJvUX(D3vRUT>t_1^Tow_G+cNa8_W=H!F&%6J!e3mN@_xFCE z9b8Lau#A3mlOdxR8H`Vd+#1#?N}Z$Y%CXeQaB)Ju;KNI;dq^nO&M-Q^#7l{JyqJw>vZXnK z`y6iha~SLJCzlkGnf|e%&G}3rlP#twfHtSnJ5obvb}3kvI&vjYvgmYcUOu>Dux)VZ z(t#DtspYA+Il^vI;*D(>PUmxl++eZklHmdxu%|dSFq~_0P#DVOCog()xsffy*@4{d zLQ^tTO!W?r7MeB>XS3u-=R?nY~)8P@Yq zWzYJMCdw^Vq6JX-Feu(GZnoWA7!P~f8FJiQdwAHJ*fQ{Xi;tqMEMOYm%S@$`NVf4*$+o>`4eb4j>xq7f)Cc8 z<;o-YN%akNKlzEHJT0&SU=VHHh_RTX;21b(1fOUPje#BK&wGuqcD-5XAedK&)GmDa(EgvW4mTRGuxxY%EoJK?invC z^P!>4_vOB({kj2^7y%azJKUafczVpy3WaZiUL&d}^deE#?}2xz1zx94c$bz#mUcs1 zU4?&b=*>&;O&9L)Y8AXuNj$d%9;OcTnpOBk5+0|db6$q$u7)pb0KOXbGF7h7eDzXk zZJW_lIuA81QS7UqCqkOsaO@{js7=|~xbQ`zH0 zJkeWv%3Z$y_@iTDzP<(J)6Z}`AZqz1<5?;HIo{CZ%%e|G;;E6+q;Mx_!m*Hssy-r~ zh4R@t$5l|iE`O7FZyNkZQ;FSM`yVtKaD?&Z*^alg zQjQ9HV-x(18BjR`TGP+9x1@_SL3(%Kp0Ip6@WcRcQ@!qNe5x%TitnQ0AbNfW`gAM) zwcxxI=OyU%%Yb_uu3B(zheTk{?{@Al!FMar+uQMP6(j)lE=q}&k$e6cr{=XRRj>Kh zlFi=;tXqL!xpwHNIW}&7s1zg7DxHpWO*%Ag2F2Mbt)O|Q(<V&tMlpi^K2q~1858E|Sp;z$P04&)&x3mAn z`qX#UKK9y|UpnH8Ti&@o@$xr+z4f}2zR~s2S^t#&-0#LYG~nn-tQ=hU+#AoGu>K2o z4?J?m3BOtu^dzKWb8E}8mCKt~w6wOhTn!xq|D9ua%>xa|vzqa5Pg~2f&Q;4VY@{ud zwsL}s*MG?JEd~5ux>5^GT^)|Dn`!duv+K z7~U`69JgQf(*Pz<%+>b8Z14DRuF%)czuSZ(3xehCH{Z6nc=ers-1wOvK0W`%hhL;S z#W{}Z8vNRm`TH)|pFj2aFC8-UioAdDEq6cr(5_=Q-9B;pwNI~pY^%KQJ_o&5d+un? z-R*-cwwc7~GI}&M<(;sesO61A;>!skIia%&I>&Z=N4rH#=!hA`(VAj$F=mwew9-4E zV;Av{2dpOu~{Y2ykzdl3C-2zSZJ@*BU>852~w|ML{Q z>g$|@#37{729ze+U$^lufkja6!2D+e<;Xlh)cVt!WsDGyPSl>ZaKZ7rX3%egJPqP` z_up#bPjvc|AN(XT{dcyUr}s3o_tVRiMf>Vc$i7BO-cr-PCi}G*k=*hzT2G{Z#a067 zwe_b5`>Ke+lGLDYZF^0rq3YY^*kLTOFe&NSmv5?RdHR@G9qs9>`?w`e!L+A!&A=1R zLKH&6PDG4>Ef1K|Sha=Jj&wXF577ABYgvD@T!bN_s;ncQf#jBqzCow!3W$ zm>5*Y2|{RdeopwL8xb?wf$JRRD|bSU=5eOS96_C(J{IaiVzz<)?(l>0 zhaC6bi^rmlf2;9q6U97^iy`q?Y6@*ie~-jR*atzg4&t}`%kh`x%0qnEB?spo_ zy!r25D|}Mw)q1rUT$w7_)%z^*{}@O{e*9CMKT*2=B+l2CuCK@WhSK$oIDe{ieKXFt zl&){Z`L@#aXK?;(>H2dx-&wl83+KNmUEhQA{?fHxFa7!feE*=&GY{k5Bc=O~;{2G; zGx~et>u?H~{9Dbayx`2Pvma>r#lm^-*l_=A|GB7}9u0Sy!TzfoE&12PUl$@pt1~o$qGz!XJIJ^$+Wx-?wl6b7%bi zzEi2;xLZVoXfx_o#5joYwilnqpg|QU*JE(TCCmoL{Il_H6jk=n$G@=k7fFoG`7&># z{&57Jc^`mvEc3YY^=ABiq7(q&)*hqkJ+hqRP(Jl@Nnd$C>`blZ{iO%wIuE)2qsN)+ zV^kHR0`y1&|2I}+|1(~sRQ<$9YB_$J!(WsU{FI~iefHuLuAX<=FVFdx{hiD2p@J_- zDbJvSw1m7~iP2i(9|hz56L)_8?!Ve{=Yp3Xef*fOU-~V&NhPN8Jn8dR<21)?T_cye z`V~`yzt^-M{K%q(*Im~5<&(brc=!MQ@e!Sza+UbXv*s_C_NZ6s61*er(J7|CPBOs+ z|GxkALykD{?*|Wl;+IcuUGjf^ezaDPJ&4kJ1)u*Bm3vXU4>=;_oz$Ax8e-(g){_2i z=c;$o;lZ!G`rY|g-1GX8zuR-h)_a%!S3!`|r8_ zW4He18DE(bfUf6uza9SwiOGOFuAaKYF*@n+X4tWG%Kh8PM{WPscYgNUkKgvl3lD#L zyzML?A$i3-Oa8yk&Ho;>CvSA4&v(H~!l=bg_)3-_9)>;>#xyR)za&0yo757#qa)(q z3}hx>9CE?2df-xynp|rerTktzw*mO2;G-~o0|hrAH(z?`ecOnO8wS-Rn7SSX}si%r5AN{_H6d^U!B*{)5+K>JN8xs z>NX>?QL^@EN&7r@*NZ>?>K|TcST$`WHaU7OeLRc)<)BA?`s)`OK#5Lj%DNv=tduB` z%2~HIh~JIi-$LB%MKmL0%(~!ROyX|`yjkbrdJDAYjkvQ3S3S;;IgfWX+Oo~*Tk=Zq z)|+o~{zy=QbH~)q%!9fbS19)bTlzRBCch@XHR4mwnN#lrzc9kEPpgAI2j1jqg8`>5 z>sH{}j!R4-u6V|)#IU>$>+&dc*@j%_AmW2au?dJ^u9l(j zr5(gYskFa@Z<3>-h*=eC2r=P0V!N7~;-8UGGq4TJ%z#Y8e>2! zgmH#)MX2njtgU_D3~J|KO^4;STi4Mn*KK1fehbILzO_v}Dw~5LB{=Zg@tcW3G9gs zjwWf;h?3om=BEWhY0i~6T!ZX>l9W<-oj;9ub(b(A-kb*s$IM7;Xmz2;F) z;&A4h^WdbppR}>>d#2{$ulvxuK&ilXQ7vw-_2_-tY;PHd<}dB+HP7IZqk7jUb?F1J z<9VgyZgcc4MnVfZ>l%Kmbvb@g$9qq;?WHv-u`-E6sl=E|##*G%`{|e8jeFd4fEwul zEHELF1%Z7#1bzV+Z>E%;WFsjo}X4#LS!M&HBNUqpr?NyvyZ8W6dQrDrCN7430 zcml}7!kSWCX|EA``zm_}{SPdQx~cl4%H;~E>Fy2Ty_mX%>eb=>gR{ZYjeWVk%<-QZ zv7+&YS%sz4+7y2Fo+xj>H|+vdmi@TwElWudayh*f)Xin=L)uru*vmRoEAm>OYTOR@ zO|7-&4igh0@#PvWsWDfweu%Qr-e+NA<;J6WdfE3T6EjL?_SrnVSF~@1tfW=R5kjrS zirSuwtlN{j`1xyj_H{S)fhbm9u)Ny<-Gz3C>fPPebvN{eraobiH`ZG-A|)k&ApJH919>+F;@5XFJa+jXORTq05QUlDeew#wjeitZ7kB z_A33(?H^GxTH#~w4`6-Wo)V?ya5P{%b`Bblx?Ah=l?5ZH&;1efIiT~Kvw zK~;yD(D{ia=eUM&X0VU>S=B*dSsSmjk}cd}w+L&_Tn(hkv-fS9Eh}~srq6=j#jBjs zg~vjbd0vgVDR{Iuzfp3Q^N}2J6;8F$K>SA9cE~d6r{OFJ*BXgECIvo0qb?&w>}*u8 zq+16}*r&MSNY&k5s_64Ka)w8Z-1@h=-^kuuLkhabakJLcLh0vIwVlq{Uas_-09=K> zP%Ja2nzC+hvnNIw)x%}1=}}SAN|ZB-dQoF_&zAJgKs|)zL%mJ(DcRrX%@r#Q(!1TJ3ABsbKaxwuI{c z{1ip6|DZJ%rgo!foPpoE{GK$Vg#FkQj6=1a0ed;AOJFIVMcmg8j7MsAUYE}2Sht3* zOPX_VqLjn-37e7Ih&m1!Bf9-usfqtuP^bW%**#V?)tF8k9jKct-m~{| zi=Jibl$5`m?an#6NEWSM&f_z6+Azl;uKiCtJ$}N;fyDS*Mqh{-SAQSWnbx!?SDIdh zt%%re*QjqVXUmO(YD`pZZZBucl`15?|BUI>m%hbm@c~UAP<6NOx!$^O0UzbTW4eW7 zUrSR$2wru2>?qwwO1qNC9p@-tYpr8Ok*}s6>h?US7T9RruT09=~Uc?(je6FefK>cru^kGB3#}W^|KC^#Hok(RMP$k>Pcz7k1q=vA)M4uG&5qHF%n8G4fnAlr{y38>tAZ=0l zD`+q1w!gis4lY;!#+;Q9eRI^|n~Z~%qJ;fST=bqVv;emzW6l@Mgif~(UP5m?sEp82 zw6gbMRZ6Qqta~M9qSa{Cy61dq(VO+vn(RSJ(`RfZN5Fo^x551Y6*cUwI^9Diy=QWb zkp7+CgY07z^nPOYTxRQ{-%RA|@{eOrPmbtvM_*Em(a-caa0ZBR&!hM!cbpS34{3*z zeNIFQeMNns2)e(iNd_Nj0!&D4YZ^t9Swq3Z$u zT07S%%Do}wdTG_8>|^{)`C`^Pc+RBL;SITp!B1;xp5*#!lM{PKZrRgS{ii~fks=(2 zg*IT0i>hS%QP5kzARCJPowl8n_gTE;uGSJ2RsBK^Wau{3$E@afdU+MPi!Gfu;uYsd2z@DIw;r63xdFd-- zi2UU4OnAxsbUmR}QC`xf(bo#pS|5!nlP?9&Zg$4?q0$&nox;?W?OU%L-?lqaBxqHT z{j5s4@|EXF+LeAiXga0$MQDR^>&fjstHC{5X^SF8gt@=OUdJNxN@xJ1PG6T=p|clz zO`;Kk@uJns{U=2J5035Q0rS8YTk8^{j+Ly8rp;-_Ji71bQi1%g#^Q@+1-@zp7Gpm2 z6x5k z{Ru(mWEbMGP|H1&{3g5#QF7cbHvO*^0g^(OEj9r8Gf zh~TJnpcY`(pb=gleg7rt&+=kVdEoZ=gn5H>%9-9D&Oa&d4`UzI{u@n`{->Z?UpXgh zy%nCc5pafoOBXAyv$y>kOPoG?IKCY`PX8liYEaE{71w-r@;GzWrXhbAi9)YtP<<=! z)Ai8OQlco$sR0c`Usqfum8=%4ju78eCN8!Sj|k`}{g?YybF)>7=}&Zi*eK zzTeX@d{UoDq>v92oRaJ|*E#@X|v*;ikud=3`sn;S@<3<&SgIsB(+C z9{P~r6lOuxdyc#Am1{cnP&q z$5QkaFD=CPZdz1s+t&kMNLskPw@jw3OAvZ4MP!wHQl}67y9#S78|E5~av<+DGW42)&d6kzVTWd3B92hMyiXNUzY)>rf z`GJ!QSI3pPX(@MFMF06zur zcmG^E<8nrht;kC(IAN>CN0cuzOYzdlt7W*$EZ1+tU%{>3e8mUhyXA>P@K?mTany2W zEIt+A%8bRbFGwy9&Rr~@lzm1>LcGd3{7RfBzjU2xjdb9iU5}<-?_R@e# z#EFEG>v`-YsTnJ;DUB`ydCXq?xWaV<8r0`CwF*I^$y45+#2J- zwcuP0!+c|61tM3A^x}@nv0-M!vR^M?wm_Vz5BJ?A(g%eGi$NEzu6o!lua(@opgIc)w0XFmS(v@W0e;l|M|FW*7#Iz`-W@# zYUj>P7>DJKqx3PwS&Q|#D478xXdmRZ8n>+2fh*2h%-!d>pF8)4kFyqwRnpuUL7&~R z_tyB0f_SS~q zcqpx#IBT)#&C|Vnrgai$E$+6K#H6TSd{>;cn6VCezKL_JX>Dq}v^mU~4I%0McOako z(&Mbf7g)EjDW9mdr_sNtKL%;`rXCt+Emj(O6zb87!Mr(wZlcGEdup`aGy|>b=Y@^4 z7E_+Gt(0dv+Fb4$>lW1YI8)P$vlf?=P>PpLSrrxuy4JeIqDxZt_c&{DA9~gGr(VT8A_3 z*xMBzOZ^GmWUq4!ain0Hj^_WKk? z*Zty5zif#()34mC$=>1#9w~yO$7SA?4^7Go&V}R>Gn?tx+JR3Px zpVd=4V*1x)bWXLFo|`C$Ke55*9O6&8|9ntwx6kU@`g3a7dJKOqu6&nluN;^?GRNEn z*)4l!S1txy`5HBYZf;idv6q{|M-`Q6BdFhQoz?X<`>lVUOw~~h+`C2xwaVNYJEU$G zRNH#ea>8Zim9v@`E0rE*hNGa?8=F&g=4#;RnOiTP)pa+eC;dQN@8{Q!(lVa=TKfCY zVwLs-UE9acCy%*mg&vI>Rz{;qlHh_=ncL&n@fFi2IP>U%=EXV9f_7ftkelxg44dV| zs%RLVY4kuE#ZRe%e*SuaoB!srf8e+(s3x>!HoLG>=;zi|N-=)q z%dwDXbk-o^DZ8XYdzPZo`Z1<^0sWmyEmcUJ;(vMIk6Vk8?gf*5BA)t z%e+UsUS{44u2FCfKG{-2$`D^3aqd0l;Nc2W5yiI)TrrMT9dlM6@$MCfhGp!wyr<&( z>~|n%nJRu$Mk1^Ic~x2VeaYONO7*Nb_nu!4z&Q8bS?CkhuD%wrRHKe$<~oTC-o(8q z3;4&ZSN+Z|b@ccQq6gHH%h{vKts7M3*~inHJ=nc!H_p8$b}4kntoyCZ7xI^D`$Y?y zE1vClC-cTq$GP`prhz#3p1ud4{aEWKh1`3~D=9QJJJGIla!cl*y$m=SZ_1Iqs??d? zK}fBL3)^vJH8fN^TP4oD$C1}uG2DX@QtU00*S=AlVZUzF)HUPWd*-Tr8Lg$Suu#;G zbMH;xdbc?Dp1*#kY*MwmfXcU_cy&5gxW&2mYON9bX(?tN8udxFx$>-6YaK?nnW|q4 z8t2~AJxgJBsibl6?getMJjDa{o-eO8CM1o_$*sz2uYnHe&bediL-ba}x%cASd%=2# zsn61H73bdLP7rbKy(IXqhVg((NBnGFpuV_WpqKI!~N? zPtJ$Mx%X`5K+G{i&9{YH|Moi$ndJtJRbG7J+dO#ZSKb9-T_~EKb35& zjn`RW8Ry;`hn6-5-CpcumBmidw4jCNrszI(*+$~rd)=koG2`5OX3h-LlcOT}W~WC* zNh_h*#<}-|*VNSWO^F}8|DxJ~KF+--cdN4BFWS_46piEDdvWeP`hppCB=V{c!D9EH zd7H{T)8gEF($7roc?5a^cNR?{BX1vCV>j+G_c>$42B4><@Lk%8x*URsfDu9TxNXFF z0M9Ai&3>-bl;Lsiy{XfNR}IbN4u+-4N|5PS`=1C;i(L;;93?&ETbz3@&b=4s-bGJkQVxE-B(4y$(ZI(VoM<5opT_Gy9zYyiuHS?mb3?WqsGxv{-Y3Im@A9IC{~_ zJE7O>Ib!YoJHa-NDTlbL7W=^tjkoNzNH1?GFP5;+#Txw#e8}n>#ohMxFNeWJXlYtg zex=*5p$evcoO_S_#$HDIRz#@8%O#PuoYrmFvIClNRUR zW0X_|J<9ZD#ku!t850;j^GP|TWL3pI)EwjhKT2$cq)1^L@#;g&O=IQ;38GcyxoYZ7 zcN4pH2i5=TImjyo_@|ZbI^bxjmMdkfQSQbJ2@`;#60eb7LF=Zfi z1^0NfS_Sdb_#C8~Y4>nOn|6K>1?K(;BzM;L>TS}4a^UK2%*Ws-qm`D22PTi$qe5?2 z6MSV{u~YPDy>kn|DCInPZeBW2=03`0 zo1FVT1=Fj|JaDGhMSSn(kGW45)7Rpqi}>D6m$~SFG5JB##T|V>|4aCjZd%3BK3r=f zM*Ui6(d_E%*{suhwq0*>R``f{B+exbfp_SIqE^H?dhRG0cASzpsNqfR2zyq3_fcHj zU;0q4eCF&5J6Qgyc!1g((Fl81em8-OdrKe6mA@2`W?{!^D4$lz!GNgcv+ny#FD+L- z^E!nctUtW~vI)YTmEV0F7mt@d>}#;gjbyGSl9`fHPIkU%`=%{8v+L{!T7I!`-a9tj z|Jr{p>ZWxfYa`DRbMJX?Ez#E(m#c+Qatg@1x$P~le5f(Kr{}U4*WU82YkqtEtC9&M z+B|zvHtt+iOZ@$UYWffooJ+n6rHViyq@{-eX5JLuI%UViE;`&<9(yB|K~U*8VI zbj%}QpGe$TQ~uE#pc-C!YX7=xpE>-fxBT>oZ{PXv5A6Tm;bp6ox1)dX0;}N7+9n=a z@#6XSJapIV5B=@;j=lAv6-Sgs(%=~L!>A?x<>U|78-^W}KPTXUw*VqB44!)ka0h@# zx$;}ZV}XBBJ|c}1iIV|Q%l|5#{S1Idx$@h@V}XBJzTRm9AJo3M9oM&&K9?(hDWaFd z4vs$`zyr*|9JT(t@a&`VrD*MHeg7RmtyJh$PPC`pZ_@40Ks{rBGcn}55o_g%ld z>G5^Pl?twu2&M9~cKmA&KNrJ&$nF1vQ&K+ol~=zz|B8EFKk|2b&e(eI(w|US)-jJt zLP@>4KfgWkuSizDAn|BJ@~me3+tb#vtaH`!mEB8Lt!hgyY3b@tE?KoAxok;CNAs%A zWP7qRxvcBzy{YlxT%oU>f46NJ&JN^u7n;(A(R^q-V)>fd?vNSj!C{c&8U~SyZY|<;4~v#J*Hon2~0D)FmQ0xj5^@W7(FW~W>`NY<$-GFg?rOd#*Fw~dUQ~H-rkgWM$$B%xWd-C} zRPCMeBhDuQIBA!xTZ>(*jB_dKsCPg*#W~TLtXEUnzl!?Z58?x7Sd#T&Kk2CRTC~}~ zyOCv({rt2j3r3+E1)3w{2LE-1*j8&>)ja*-S{ z?JYPBeXUlL+E(JUXE|VV)5~O?8GHUUTdv*Sg6lI_v$}B&XvjO4f(P}x@5YBC0n^v9 z45Nm&ZlwmKjy|tNrKkkU<0xEkN^kl)mZwrR5*M$_DkTni_P4ku`^ATw0_EqQ^=|C` zbs~ZJXJZ1@`-JxnV>6#9-7a7L3hxWFJDmUKr0o*ii#mCG4W50f^p5i7hetrL{)_Q0 za#z&yufwzVMJ}KD>4J}H=+7O@q#<0s{>y?3$2X+^BAnk3h+6-f@GRGs@hD$@cmzl_ k^=Iq}WAJ#CD}Pz}_`izz|8A6fty6k!{*dnSsPv8hAEIbC<^TWy literal 0 HcmV?d00001 diff --git a/axios/AxiosEngine.cd b/axios/AxiosEngine.cd new file mode 100644 index 0000000..489e67e --- /dev/null +++ b/axios/AxiosEngine.cd @@ -0,0 +1,118 @@ + + + + + + + + Engine\AxiosEvents.cs + + + + + Engine\AxiosEvents.cs + + + + + AAQAACAIAAAAAkAAAkkAAAAAAEEAGAIAIQQAABgCAAA= + Engine\AxiosEvents.cs + + + + + + AAQAMACAAAAAAAAEACIIBAQAIAAAAAAAAAAAAAAAACQ= + Engine\AxiosGameObject.cs + + + + + + + AAAAAAAEAAAAAAAAAAAAAAAAAAAACAAAAAAAAAAAAAQ= + Engine\ComplexAxiosGameObject.cs + + + + + + AEQAEAABAgAAAAAgAAAAAAAAAAAAAAAAAAAAAAgAAAA= + Engine\SimpleAxiosGameObject.cs + + + + + + AAAAAIAAAAAAAgAAAAAAAAAAAAIACAAgAAIAIAAAAAQ= + Engine\SimpleDrawableAxiosGameObject.cs + + + + + + + AARoEQCCAAACAggBACAAAgAICAAAAAAAIAAAAIIAAAQ= + Engine\AxiosGameScreen.cs + + + + + + AAQAEACAAAAAAIAAAKAAAACAAAAAAAAQAAAAACAAQAQ= + Engine\AxiosTimer.cs + + + + + + + AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAACAAAA= + Engine\Extensions\String.cs + + + + + + AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAQAAAAAQAAAAA= + Engine\Extensions\Texture2D.cs + + + + + + AAAAAAAABAAAAAAAAAAAAEAAAAAAAAAAAAAAAAAAAAA= + Engine\UI\AxiosUIObject.cs + + + + + + AAAAAIAAAAAAAgAgAAAABAAAAAAACAAgAAIAIAgAAAQ= + Engine\DrawableAxiosGameObject.cs + + + + + + + AAAAgAAIQAAIAAAAACAAgAAAAAAASAAAIAAAAAgCAAQ= + Engine\UI\AxiosButton.cs + + + + + + AAQAEACAAAAAAAAAACAAAAAAAAAAAAAAAAAAAAAAAAQ= + Engine\Interfaces\IAxiosGameObject.cs + + + + + + AAAAAAAAAAAAAgAAAAAAAAAAAAAAAAAgAAAAAAAAAAA= + Engine\Interfaces\IDrawableAxiosGameObject.cs + + + + \ No newline at end of file diff --git a/axios/Axios_WP7.csproj b/axios/Axios_WP7.csproj new file mode 100644 index 0000000..aa7e0e0 --- /dev/null +++ b/axios/Axios_WP7.csproj @@ -0,0 +1,240 @@ + + + + {C09D9005-76AC-4F1A-9479-2787BB3DB158} + {6D335F3A-9D43-41b4-9D22-F6F17C4BE596};{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC} + Debug + Windows Phone + Library + Properties + Axios + Axios.WP7 + v4.0 + Client + v4.0 + Windows Phone + Reach + cd94d25d-3fe9-470c-b921-7a5abdd2494c + Library + 1 + $(AssemblyName).xap + Properties\AppManifest.xml + Properties\WMAppManifest.xml + Background.png + $(AssemblyName) + PhoneGameThumb.png + + + + bin\Windows Phone\Release + prompt + 4 + true + false + pdbonly + true + TRACE;WINDOWS_PHONE + false + + + bin\Windows Phone\Debug + prompt + 4 + true + false + true + full + false + TRACE;DEBUG;WINDOWS_PHONE + false + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + copy "$(TargetPath)" ..\..\Combined + + + \ No newline at end of file diff --git a/axios/Axios_WP7.csproj.user b/axios/Axios_WP7.csproj.user new file mode 100644 index 0000000..84869fb --- /dev/null +++ b/axios/Axios_WP7.csproj.user @@ -0,0 +1,6 @@ + + + + 5E7661DF-D928-40ff-B747-A4B1957194F9 + + \ No newline at end of file diff --git a/axios/Axios_Windows.csproj b/axios/Axios_Windows.csproj new file mode 100644 index 0000000..c62d807 --- /dev/null +++ b/axios/Axios_Windows.csproj @@ -0,0 +1,271 @@ + + + + {742C938F-997D-4EFD-95D2-BB09CDADCD2E} + {6D335F3A-9D43-41b4-9D22-F6F17C4BE596};{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC} + Debug + x86 + Library + Properties + Axios + Axios.Windows + v4.0 + Client + v4.0 + Windows + HiDef + cd94d25d-3fe9-470c-b921-7a5abdd2494c + Library + + + true + full + false + bin\x86\Debug + TRACE;DEBUG;WINDOWS + prompt + 4 + true + false + x86 + false + false + + + pdbonly + true + bin\x86\Release + TRACE;WINDOWS + prompt + 4 + true + false + x86 + true + + + + False + + + False + + + False + + + False + + + + False + + + False + + + False + + + False + + + False + + + False + + + False + + + + + + False + + + 4.0 + False + + + 4.0 + False + + + False + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + copy "$(TargetPath)" ..\..\Combined + + + \ No newline at end of file diff --git a/axios/Axios_Windows.csproj.user b/axios/Axios_Windows.csproj.user new file mode 100644 index 0000000..76fe5a5 --- /dev/null +++ b/axios/Axios_Windows.csproj.user @@ -0,0 +1,6 @@ + + + + ProjectFiles + + \ No newline at end of file diff --git a/axios/Axios_Xbox_360.csproj b/axios/Axios_Xbox_360.csproj new file mode 100644 index 0000000..d2dc579 --- /dev/null +++ b/axios/Axios_Xbox_360.csproj @@ -0,0 +1,235 @@ + + + + {B5664516-72B7-4BA3-9F72-25CAA90867D8} + {6D335F3A-9D43-41b4-9D22-F6F17C4BE596};{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC} + Debug + Xbox 360 + Library + Properties + Axios + Axios.Xbox360 + v4.0 + Client + v4.0 + Xbox 360 + HiDef + cd94d25d-3fe9-470c-b921-7a5abdd2494c + Library + GameThumbnail.png + + + bin\Xbox 360\Release + prompt + 4 + true + false + pdbonly + true + TRACE;XBOX;XBOX360 + true + + + bin\Xbox 360\Debug + prompt + 4 + true + false + true + full + false + DEBUG;TRACE;XBOX;XBOX360 + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + copy "$(TargetPath)" ..\..\Combined + + + \ No newline at end of file diff --git a/axios/Axios_settings.cs b/axios/Axios_settings.cs new file mode 100644 index 0000000..9e9f22b --- /dev/null +++ b/axios/Axios_settings.cs @@ -0,0 +1,155 @@ +/* + * Axios Engine + * + * By: Nathan Adams + * + * CHANGELOG + * + * 1.0.0.0 + * - Initial Version + * + * 1.0.0.1 + * - Adding staic function SetResolution + * + * 1.0.0.2 + * - Adding flag when removing object from Farseer to prevent it from getting removed twice + * + * 1.0.0.3 + * - Axios.Engine.File namespace + * - Adding title file reading support + * - Adding iosloated file storage support + * + * 1.0.0.4 - 3/9/2012 + * - Condensing AddGameObject into a single method + * + * 1.0.0.5 - 3/9/2012 + * - Adding checks in MenuScreen to make sure screen doesn't get struck in transition + * + * 1.0.0.6 - 3/10/2012 + * - Added Singleton class + * - Added Logging class + * - Added LoggingFlag flags + * - Added static loglevel setting + * - Moving some enums out of classes + * - Adding AxiosRegularFile class + * + * 1.0.0.7 - 3/11/2012 + * - Adding IAxiosFile interface + * + * 1.0.0.8 - 3/15/2012 + * - Adding code for breakable bodies + * + * 1.0.0.9 - 3/16/2012 + * - Changeing the complex objects alot - now they are more like "chained" objects + * + */ + +using System; +using System.Collections.Generic; +using System.Linq; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Audio; +using Microsoft.Xna.Framework.Content; +using Microsoft.Xna.Framework.GamerServices; +using Microsoft.Xna.Framework.Graphics; +using Microsoft.Xna.Framework.Input; +using Microsoft.Xna.Framework.Media; +using System.Reflection; +using Axios.Engine.Extenions; +using Axios.Engine.Log; + +namespace Axios +{ + public enum ResolutionSetting + { + Windows, + Xbox360, + WP7_Portrait, + WP7_Landscape + } + public static class Settings + { + + + public static LoggingFlag Loglevel = LoggingFlag.ALL; + +#if WINDOWS + public static string Version = "Axios Engine " + Assembly.GetExecutingAssembly().GetName().Version.ToString(); +#elif XBOX360 || WINDOWS_PHONE + private static AssemblyName assemblyref = new AssemblyName(Assembly.GetExecutingAssembly().FullName); + public static string Version = "Axios Engine " + Settings.assemblyref.Version; +#endif + + public static bool ScreenSaver = false; + + private static ResolutionSetting _ressetting; + /// + /// We should have two seperate resolutions for seperate devices. + /// This way you can have one source to preform calculations on world size depending on the device. + /// + public static void SetResolution(GraphicsDeviceManager graphics, ResolutionSetting setting) + { + //height is first + graphics.PreferredBackBufferHeight = GetResolution(setting)[0]; + graphics.PreferredBackBufferWidth = GetResolution(setting)[1]; + _ressetting = setting; + } + + private static int[] GetResolution(ResolutionSetting setting) + { + int[] screendim = new int[2]; + screendim[0] = 0; + screendim[1] = 0; + if (setting == ResolutionSetting.Windows || setting == ResolutionSetting.Xbox360) + { + screendim[0] = 720; + screendim[1] = 1280; + } + + if (setting == ResolutionSetting.WP7_Landscape) + { + screendim[0] = 480; + screendim[1] = 800; + + } else if (setting == ResolutionSetting.WP7_Portrait) + { + screendim[0] = 800; + screendim[1] = 480; + } + + return screendim; + } + + public static float GetHeightScale() + { + if (_ressetting == ResolutionSetting.WP7_Landscape || _ressetting == ResolutionSetting.WP7_Portrait) + { + return (float)GetResolution(_ressetting)[0] / (float)GetResolution(ResolutionSetting.Windows)[0]; + } + else + { + return 1f; + } + } + + public static float GetWidthScale() + { + if (_ressetting == ResolutionSetting.WP7_Landscape || _ressetting == ResolutionSetting.WP7_Portrait) + { + return (float)GetResolution(_ressetting)[1] / (float)GetResolution(ResolutionSetting.Windows)[1]; + } + else + { + return 1f; + } + } + + public static float GetScale() + { + return GetHeightScale() / GetWidthScale(); + } + + + public static float DisplayUnitToSimUnitRatio = 24f; + } +} \ No newline at end of file diff --git a/axios/Collision/Collision.cs b/axios/Collision/Collision.cs new file mode 100644 index 0000000..c78b9bc --- /dev/null +++ b/axios/Collision/Collision.cs @@ -0,0 +1,1945 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not 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 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; + + public Vector2 LocalPoint; + + public float NormalImpulse; + + 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. The ray extends from p1 to p1 + maxFraction * (p2 - p1). + /// + public struct RayCastInput + { + public float MaxFraction; + public Vector2 Point1, Point2; + } + + /// + /// Ray-cast output data. The ray hits at p1 + fraction * (p2 - p1), where p1 and p2 + /// come from RayCastInput. + /// + public struct RayCastOutput + { + public float Fraction; + public Vector2 Normal; + } + + /// + /// An axis aligned bounding box. + /// + public struct AABB + { + private static DistanceInput _input = new DistanceInput(); + + /// + /// 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); + } + + /// + /// 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(); + vertices.Add(LowerBound); + vertices.Add(new Vector2(LowerBound.X, UpperBound.Y)); + vertices.Add(UpperBound); + vertices.Add(new Vector2(UpperBound.X, LowerBound.Y)); + return vertices; + } + } + + /// + /// first quadrant + /// + public AABB Q1 + { + get { return new AABB(Center, UpperBound); } + } + + public AABB Q2 + { + get + { + return new AABB(new Vector2(LowerBound.X, Center.Y), new Vector2(Center.X, UpperBound.Y)); + ; + } + } + + public AABB Q3 + { + get { return new AABB(LowerBound, Center); } + } + + public AABB Q4 + { + get { return new AABB(new Vector2(Center.X, LowerBound.Y), new Vector2(UpperBound.X, Center.Y)); } + } + + public Vector2[] GetVertices() + { + Vector2 p1 = UpperBound; + Vector2 p2 = new Vector2(UpperBound.X, LowerBound.Y); + Vector2 p3 = LowerBound; + Vector2 p4 = new Vector2(LowerBound.X, UpperBound.Y); + return new[] { p1, p2, p3, p4 }; + } + + /// + /// Verify that the bounds are sorted. + /// + /// + /// 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. + if ((point.X > (LowerBound.X + Settings.Epsilon) && point.X < (UpperBound.X - Settings.Epsilon) && + (point.Y > (LowerBound.Y + Settings.Epsilon) && point.Y < (UpperBound.Y - Settings.Epsilon)))) + { + return true; + } + return false; + } + + public static bool TestOverlap(AABB a, AABB b) + { + return TestOverlap(ref a, ref b); + } + + 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; + } + + public static bool TestOverlap(Shape shapeA, int indexA, + Shape shapeB, int indexB, + ref Transform xfA, ref Transform xfB) + { + _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; + } + + + // From Real-time Collision Detection, p179. + public bool RayCast(out RayCastOutput output, ref RayCastInput input) + { + 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 (tmin < 0.0f || input.MaxFraction < tmin) + { + return false; + } + + // Intersection. + output.Fraction = tmin; + output.Normal = normal; + return true; + } + } + + /// + /// Edge shape plus more stuff. + /// + public struct FatEdge + { + public bool HasVertex0, HasVertex3; + public Vector2 Normal; + public Vector2 V0, V1, V2, V3; + } + + /// + /// This lets us treate and edge shape and a polygon in the same + /// way in the SAT collider. + /// + public class EPProxy + { + public Vector2 Centroid; + public int Count; + public Vector2[] Normals = new Vector2[Settings.MaxPolygonVertices]; + public Vector2[] Vertices = new Vector2[Settings.MaxPolygonVertices]; + } + + public struct EPAxis + { + public int Index; + public float Separation; + public EPAxisType Type; + } + + public enum EPAxisType + { + Unknown, + EdgeA, + EdgeB, + } + + public static class Collision + { + private static FatEdge _edgeA; + + private static EPProxy _proxyA = new EPProxy(); + private static EPProxy _proxyB = new EPProxy(); + + private static Transform _xf; + private static Vector2 _limit11, _limit12; + private static Vector2 _limit21, _limit22; + private static float _radius; + private static Vector2[] _tmpNormals = new Vector2[2]; + + /// + /// 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 GetWorldManifold(ref Manifold manifold, + ref Transform transformA, float radiusA, + ref Transform transformB, float radiusB, out Vector2 normal, + out FixedArray2 points) + { + points = new FixedArray2(); + normal = Vector2.Zero; + + if (manifold.PointCount == 0) + { + normal = Vector2.UnitY; + return; + } + + switch (manifold.Type) + { + case ManifoldType.Circles: + { + Vector2 tmp = manifold.Points[0].LocalPoint; + float pointAx = transformA.Position.X + transformA.R.Col1.X * manifold.LocalPoint.X + + transformA.R.Col2.X * manifold.LocalPoint.Y; + + float pointAy = transformA.Position.Y + transformA.R.Col1.Y * manifold.LocalPoint.X + + transformA.R.Col2.Y * manifold.LocalPoint.Y; + + float pointBx = transformB.Position.X + transformB.R.Col1.X * tmp.X + + transformB.R.Col2.X * tmp.Y; + + float pointBy = transformB.Position.Y + transformB.R.Col1.Y * tmp.X + + transformB.R.Col2.Y * tmp.Y; + + normal.X = 1; + normal.Y = 0; + + float result = (pointAx - pointBx) * (pointAx - pointBx) + + (pointAy - pointBy) * (pointAy - pointBy); + if (result > Settings.Epsilon * Settings.Epsilon) + { + float tmpNormalx = pointBx - pointAx; + float tmpNormaly = pointBy - pointAy; + float factor = 1f / (float)Math.Sqrt(tmpNormalx * tmpNormalx + tmpNormaly * tmpNormaly); + normal.X = tmpNormalx * factor; + normal.Y = tmpNormaly * factor; + } + + Vector2 c = Vector2.Zero; + c.X = (pointAx + radiusA * normal.X) + (pointBx - radiusB * normal.X); + c.Y = (pointAy + radiusA * normal.Y) + (pointBy - radiusB * normal.Y); + + points[0] = 0.5f * c; + } + break; + + case ManifoldType.FaceA: + { + normal.X = transformA.R.Col1.X * manifold.LocalNormal.X + + transformA.R.Col2.X * manifold.LocalNormal.Y; + normal.Y = transformA.R.Col1.Y * manifold.LocalNormal.X + + transformA.R.Col2.Y * manifold.LocalNormal.Y; + + float planePointx = transformA.Position.X + transformA.R.Col1.X * manifold.LocalPoint.X + + transformA.R.Col2.X * manifold.LocalPoint.Y; + + float planePointy = transformA.Position.Y + transformA.R.Col1.Y * manifold.LocalPoint.X + + transformA.R.Col2.Y * manifold.LocalPoint.Y; + + for (int i = 0; i < manifold.PointCount; ++i) + { + Vector2 tmp = manifold.Points[i].LocalPoint; + + float clipPointx = transformB.Position.X + transformB.R.Col1.X * tmp.X + + transformB.R.Col2.X * tmp.Y; + + float clipPointy = transformB.Position.Y + transformB.R.Col1.Y * tmp.X + + transformB.R.Col2.Y * tmp.Y; + + float value = (clipPointx - planePointx) * normal.X + (clipPointy - planePointy) * normal.Y; + + Vector2 c = Vector2.Zero; + c.X = (clipPointx + (radiusA - value) * normal.X) + (clipPointx - radiusB * normal.X); + c.Y = (clipPointy + (radiusA - value) * normal.Y) + (clipPointy - radiusB * normal.Y); + + points[i] = 0.5f * c; + } + } + break; + + case ManifoldType.FaceB: + { + normal.X = transformB.R.Col1.X * manifold.LocalNormal.X + + transformB.R.Col2.X * manifold.LocalNormal.Y; + normal.Y = transformB.R.Col1.Y * manifold.LocalNormal.X + + transformB.R.Col2.Y * manifold.LocalNormal.Y; + + float planePointx = transformB.Position.X + transformB.R.Col1.X * manifold.LocalPoint.X + + transformB.R.Col2.X * manifold.LocalPoint.Y; + + float planePointy = transformB.Position.Y + transformB.R.Col1.Y * manifold.LocalPoint.X + + transformB.R.Col2.Y * manifold.LocalPoint.Y; + + for (int i = 0; i < manifold.PointCount; ++i) + { + Vector2 tmp = manifold.Points[i].LocalPoint; + + float clipPointx = transformA.Position.X + transformA.R.Col1.X * tmp.X + + transformA.R.Col2.X * tmp.Y; + + float clipPointy = transformA.Position.Y + transformA.R.Col1.Y * tmp.X + + transformA.R.Col2.Y * tmp.Y; + + float value = (clipPointx - planePointx) * normal.X + (clipPointy - planePointy) * normal.Y; + + Vector2 c = Vector2.Zero; + c.X = (clipPointx - radiusA * normal.X) + (clipPointx + (radiusB - value) * normal.X); + c.Y = (clipPointy - radiusA * normal.Y) + (clipPointy + (radiusB - value) * normal.Y); + + points[i] = 0.5f * c; + } + // Ensure normal points from A to B. + normal *= -1; + } + break; + default: + normal = Vector2.UnitY; + break; + } + } + + 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; + + float pAx = xfA.Position.X + xfA.R.Col1.X * circleA.Position.X + xfA.R.Col2.X * circleA.Position.Y; + float pAy = xfA.Position.Y + xfA.R.Col1.Y * circleA.Position.X + xfA.R.Col2.Y * circleA.Position.Y; + float pBx = xfB.Position.X + xfB.R.Col1.X * circleB.Position.X + xfB.R.Col2.X * circleB.Position.Y; + float pBy = xfB.Position.Y + xfB.R.Col1.Y * circleB.Position.X + xfB.R.Col2.Y * circleB.Position.Y; + + float distSqr = (pBx - pAx) * (pBx - pAx) + (pBy - pAy) * (pBy - pAy); + 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 transformA, + CircleShape circleB, ref Transform transformB) + { + manifold.PointCount = 0; + + // Compute circle position in the frame of the polygon. + Vector2 c = + new Vector2( + transformB.Position.X + transformB.R.Col1.X * circleB.Position.X + + transformB.R.Col2.X * circleB.Position.Y, + transformB.Position.Y + transformB.R.Col1.Y * circleB.Position.X + + transformB.R.Col2.Y * circleB.Position.Y); + Vector2 cLocal = + new Vector2( + (c.X - transformA.Position.X) * transformA.R.Col1.X + + (c.Y - transformA.Position.Y) * transformA.R.Col1.Y, + (c.X - transformA.Position.X) * transformA.R.Col2.X + + (c.Y - transformA.Position.Y) * transformA.R.Col2.Y); + + // 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]; + + float localTangentX = v12.X - v11.X; + float localTangentY = v12.Y - v11.Y; + + float factor = 1f / (float)Math.Sqrt(localTangentX * localTangentX + localTangentY * localTangentY); + localTangentX = localTangentX * factor; + localTangentY = localTangentY * factor; + + Vector2 localNormal = new Vector2(localTangentY, -localTangentX); + Vector2 planePoint = 0.5f * (v11 + v12); + + Vector2 tangent = new Vector2(xf1.R.Col1.X * localTangentX + xf1.R.Col2.X * localTangentY, + xf1.R.Col1.Y * localTangentX + xf1.R.Col2.Y * localTangentY); + float normalx = tangent.Y; + float normaly = -tangent.X; + + v11 = new Vector2(xf1.Position.X + xf1.R.Col1.X * v11.X + xf1.R.Col2.X * v11.Y, + xf1.Position.Y + xf1.R.Col1.Y * v11.X + xf1.R.Col2.Y * v11.Y); + v12 = new Vector2(xf1.Position.X + xf1.R.Col1.X * v12.X + xf1.R.Col2.X * v12.Y, + xf1.Position.Y + xf1.R.Col1.Y * v12.X + xf1.R.Col2.Y * v12.Y); + + // 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]; + Vector2 tmp = clipPoints2[i].V; + float tmp1X = tmp.X - xf2.Position.X; + float tmp1Y = tmp.Y - xf2.Position.Y; + cp.LocalPoint.X = tmp1X * xf2.R.Col1.X + tmp1Y * xf2.R.Col1.Y; + cp.LocalPoint.Y = tmp1X * xf2.R.Col2.X + tmp1Y * xf2.R.Col2.Y; + 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.MultiplyT(ref transformA, MathUtils.Multiply(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) + { + MathUtils.MultiplyT(ref xfA, ref xfB, out _xf); + + // Edge geometry + _edgeA.V0 = edgeA.Vertex0; + _edgeA.V1 = edgeA.Vertex1; + _edgeA.V2 = edgeA.Vertex2; + _edgeA.V3 = edgeA.Vertex3; + Vector2 e = _edgeA.V2 - _edgeA.V1; + + // Normal points outwards in CCW order. + _edgeA.Normal = new Vector2(e.Y, -e.X); + _edgeA.Normal.Normalize(); + _edgeA.HasVertex0 = edgeA.HasVertex0; + _edgeA.HasVertex3 = edgeA.HasVertex3; + + // Proxy for edge + _proxyA.Vertices[0] = _edgeA.V1; + _proxyA.Vertices[1] = _edgeA.V2; + _proxyA.Normals[0] = _edgeA.Normal; + _proxyA.Normals[1] = -_edgeA.Normal; + _proxyA.Centroid = 0.5f * (_edgeA.V1 + _edgeA.V2); + _proxyA.Count = 2; + + // Proxy for polygon + _proxyB.Count = polygonB.Vertices.Count; + _proxyB.Centroid = MathUtils.Multiply(ref _xf, ref polygonB.MassData.Centroid); + for (int i = 0; i < polygonB.Vertices.Count; ++i) + { + _proxyB.Vertices[i] = MathUtils.Multiply(ref _xf, polygonB.Vertices[i]); + _proxyB.Normals[i] = MathUtils.Multiply(ref _xf.R, polygonB.Normals[i]); + } + + _radius = 2.0f * Settings.PolygonRadius; + + _limit11 = Vector2.Zero; + _limit12 = Vector2.Zero; + _limit21 = Vector2.Zero; + _limit22 = Vector2.Zero; + + //Collide(ref manifold); inline start + manifold.PointCount = 0; + + //ComputeAdjacency(); inline start + Vector2 v0 = _edgeA.V0; + Vector2 v1 = _edgeA.V1; + Vector2 v2 = _edgeA.V2; + Vector2 v3 = _edgeA.V3; + + // Determine allowable the normal regions based on adjacency. + // Note: it may be possible that no normal is admissable. + Vector2 centerB = _proxyB.Centroid; + if (_edgeA.HasVertex0) + { + Vector2 e0 = v1 - v0; + Vector2 e1 = v2 - v1; + Vector2 n0 = new Vector2(e0.Y, -e0.X); + Vector2 n1 = new Vector2(e1.Y, -e1.X); + n0.Normalize(); + n1.Normalize(); + + bool convex = MathUtils.Cross(n0, n1) >= 0.0f; + bool front0 = Vector2.Dot(n0, centerB - v0) >= 0.0f; + bool front1 = Vector2.Dot(n1, centerB - v1) >= 0.0f; + + if (convex) + { + if (front0 || front1) + { + _limit11 = n1; + _limit12 = n0; + } + else + { + _limit11 = -n1; + _limit12 = -n0; + } + } + else + { + if (front0 && front1) + { + _limit11 = n0; + _limit12 = n1; + } + else + { + _limit11 = -n0; + _limit12 = -n1; + } + } + } + else + { + _limit11 = Vector2.Zero; + _limit12 = Vector2.Zero; + } + + if (_edgeA.HasVertex3) + { + Vector2 e1 = v2 - v1; + Vector2 e2 = v3 - v2; + Vector2 n1 = new Vector2(e1.Y, -e1.X); + Vector2 n2 = new Vector2(e2.Y, -e2.X); + n1.Normalize(); + n2.Normalize(); + + bool convex = MathUtils.Cross(n1, n2) >= 0.0f; + bool front1 = Vector2.Dot(n1, centerB - v1) >= 0.0f; + bool front2 = Vector2.Dot(n2, centerB - v2) >= 0.0f; + + if (convex) + { + if (front1 || front2) + { + _limit21 = n2; + _limit22 = n1; + } + else + { + _limit21 = -n2; + _limit22 = -n1; + } + } + else + { + if (front1 && front2) + { + _limit21 = n1; + _limit22 = n2; + } + else + { + _limit21 = -n1; + _limit22 = -n2; + } + } + } + else + { + _limit21 = Vector2.Zero; + _limit22 = Vector2.Zero; + } + + //ComputeAdjacency(); inline end + + //EPAxis edgeAxis = ComputeEdgeSeparation(); inline start + EPAxis edgeAxis = ComputeEdgeSeparation(); + + // If no valid normal can be found than this edge should not collide. + // This can happen on the middle edge of a 3-edge zig-zag chain. + 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; + } + + EPProxy proxy1; + EPProxy proxy2; + FixedArray2 incidentEdge = new FixedArray2(); + if (primaryAxis.Type == EPAxisType.EdgeA) + { + proxy1 = _proxyA; + proxy2 = _proxyB; + manifold.Type = ManifoldType.FaceA; + } + else + { + proxy1 = _proxyB; + proxy2 = _proxyA; + manifold.Type = ManifoldType.FaceB; + } + + int edge1 = primaryAxis.Index; + + FindIncidentEdge(ref incidentEdge, proxy1, primaryAxis.Index, proxy2); + int count1 = proxy1.Count; + + int iv1 = edge1; + int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; + + Vector2 v11 = proxy1.Vertices[iv1]; + Vector2 v12 = proxy1.Vertices[iv2]; + + Vector2 tangent = v12 - v11; + tangent.Normalize(); + + Vector2 normal = MathUtils.Cross(tangent, 1.0f); + Vector2 planePoint = 0.5f * (v11 + v12); + + // Face offset. + float frontOffset = Vector2.Dot(normal, v11); + + // Side offsets, extended by polytope skin thickness. + float sideOffset1 = -Vector2.Dot(tangent, v11) + _radius; + float sideOffset2 = Vector2.Dot(tangent, v12) + _radius; + + // Clip incident edge against extruded edge1 side edges. + FixedArray2 clipPoints1; + FixedArray2 clipPoints2; + int np; + + // Clip to box side 1 + np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1, iv1); + + if (np < Settings.MaxManifoldPoints) + { + return; + } + + // Clip to negative box side 1 + np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2, iv2); + + if (np < Settings.MaxManifoldPoints) + { + return; + } + + // Now clipPoints2 contains the clipped points. + if (primaryAxis.Type == EPAxisType.EdgeA) + { + manifold.LocalNormal = normal; + manifold.LocalPoint = planePoint; + } + else + { + manifold.LocalNormal = MathUtils.MultiplyT(ref _xf.R, ref normal); + manifold.LocalPoint = MathUtils.MultiplyT(ref _xf, ref planePoint); + } + + int pointCount = 0; + for (int i1 = 0; i1 < Settings.MaxManifoldPoints; ++i1) + { + float separation = Vector2.Dot(normal, clipPoints2[i1].V) - frontOffset; + + if (separation <= _radius) + { + ManifoldPoint cp = manifold.Points[pointCount]; + + if (primaryAxis.Type == EPAxisType.EdgeA) + { + cp.LocalPoint = MathUtils.MultiplyT(ref _xf, clipPoints2[i1].V); + cp.Id = clipPoints2[i1].ID; + } + else + { + cp.LocalPoint = clipPoints2[i1].V; + cp.Id.Features.TypeA = clipPoints2[i1].ID.Features.TypeB; + cp.Id.Features.TypeB = clipPoints2[i1].ID.Features.TypeA; + cp.Id.Features.IndexA = clipPoints2[i1].ID.Features.IndexB; + cp.Id.Features.IndexB = clipPoints2[i1].ID.Features.IndexA; + } + + manifold.Points[pointCount] = cp; + + ++pointCount; + } + } + + manifold.PointCount = pointCount; + + //Collide(ref manifold); inline end + } + + private static EPAxis ComputeEdgeSeparation() + { + // EdgeA separation + EPAxis bestAxis; + bestAxis.Type = EPAxisType.Unknown; + bestAxis.Index = -1; + bestAxis.Separation = -Settings.MaxFloat; + _tmpNormals[0] = _edgeA.Normal; + _tmpNormals[1] = -_edgeA.Normal; + + for (int i = 0; i < 2; ++i) + { + Vector2 n = _tmpNormals[i]; + + // Adjacency + bool valid1 = MathUtils.Cross(n, _limit11) >= -Settings.AngularSlop && + MathUtils.Cross(_limit12, n) >= -Settings.AngularSlop; + bool valid2 = MathUtils.Cross(n, _limit21) >= -Settings.AngularSlop && + MathUtils.Cross(_limit22, n) >= -Settings.AngularSlop; + + if (valid1 == false || valid2 == false) + { + continue; + } + + EPAxis axis; + axis.Type = EPAxisType.EdgeA; + axis.Index = i; + axis.Separation = Settings.MaxFloat; + + for (int j = 0; j < _proxyB.Count; ++j) + { + float s = Vector2.Dot(n, _proxyB.Vertices[j] - _edgeA.V1); + if (s < axis.Separation) + { + axis.Separation = s; + } + } + + if (axis.Separation > _radius) + { + return axis; + } + + if (axis.Separation > bestAxis.Separation) + { + bestAxis = axis; + } + } + + return bestAxis; + } + + private static EPAxis ComputePolygonSeparation() + { + EPAxis axis; + axis.Type = EPAxisType.Unknown; + axis.Index = -1; + axis.Separation = -Settings.MaxFloat; + for (int i = 0; i < _proxyB.Count; ++i) + { + Vector2 n = -_proxyB.Normals[i]; + + // Adjacency + bool valid1 = MathUtils.Cross(n, _limit11) >= -Settings.AngularSlop && + MathUtils.Cross(_limit12, n) >= -Settings.AngularSlop; + bool valid2 = MathUtils.Cross(n, _limit21) >= -Settings.AngularSlop && + MathUtils.Cross(_limit22, n) >= -Settings.AngularSlop; + + if (valid1 == false && valid2 == false) + { + continue; + } + + float s1 = Vector2.Dot(n, _proxyB.Vertices[i] - _edgeA.V1); + float s2 = Vector2.Dot(n, _proxyB.Vertices[i] - _edgeA.V2); + float s = Math.Min(s1, s2); + + if (s > _radius) + { + axis.Type = EPAxisType.EdgeB; + axis.Index = i; + axis.Separation = s; + } + + if (s > axis.Separation) + { + axis.Type = EPAxisType.EdgeB; + axis.Index = i; + axis.Separation = s; + } + } + + return axis; + } + + private static void FindIncidentEdge(ref FixedArray2 c, EPProxy proxy1, int edge1, EPProxy proxy2) + { + int count2 = proxy2.Count; + + Debug.Assert(0 <= edge1 && edge1 < proxy1.Count); + + // Get the normal of the reference edge in proxy2's frame. + Vector2 normal1 = proxy1.Normals[edge1]; + + // Find the incident edge on proxy2. + int index = 0; + float minDot = float.MaxValue; + for (int i = 0; i < count2; ++i) + { + float dot = Vector2.Dot(normal1, proxy2.Normals[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 cTemp = new ClipVertex(); + cTemp.V = proxy2.Vertices[i1]; + cTemp.ID.Features.IndexA = (byte)edge1; + cTemp.ID.Features.IndexB = (byte)i1; + cTemp.ID.Features.TypeA = (byte)ContactFeatureType.Face; + cTemp.ID.Features.TypeB = (byte)ContactFeatureType.Vertex; + c[0] = cTemp; + + cTemp.V = proxy2.Vertices[i2]; + cTemp.ID.Features.IndexA = (byte)edge1; + cTemp.ID.Features.IndexB = (byte)i2; + cTemp.ID.Features.TypeA = (byte)ContactFeatureType.Face; + cTemp.ID.Features.TypeB = (byte)ContactFeatureType.Vertex; + c[1] = cTemp; + } + + /// + /// 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) + { + int count2 = poly2.Vertices.Count; + + Debug.Assert(0 <= edge1 && edge1 < poly1.Vertices.Count); + + // Convert normal from poly1's frame into poly2's frame. + Vector2 p1n = poly1.Normals[edge1]; + + float normalWorldx = xf1.R.Col1.X * p1n.X + xf1.R.Col2.X * p1n.Y; + float normalWorldy = xf1.R.Col1.Y * p1n.X + xf1.R.Col2.Y * p1n.Y; + + Vector2 normal = new Vector2(normalWorldx * xf2.R.Col1.X + normalWorldy * xf2.R.Col1.Y, + normalWorldx * xf2.R.Col2.X + normalWorldy * xf2.R.Col2.Y); + + // 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(poly2.Vertices[i], normal); + + if (dot < minDot) + { + minDot = dot; + index = i; + } + } + + Vector2 p1ve = poly1.Vertices[edge1]; + Vector2 p2vi = poly2.Vertices[index]; + + return ((xf2.Position.X + xf2.R.Col1.X * p2vi.X + xf2.R.Col2.X * p2vi.Y) - + (xf1.Position.X + xf1.R.Col1.X * p1ve.X + xf1.R.Col2.X * p1ve.Y)) * normalWorldx + + ((xf2.Position.Y + xf2.R.Col1.Y * p2vi.X + xf2.R.Col2.Y * p2vi.Y) - + (xf1.Position.Y + xf1.R.Col1.Y * p1ve.X + xf1.R.Col2.Y * p1ve.Y)) * normalWorldy; + } + + /// + /// 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; + + // Vector pointing from the centroid of poly1 to the centroid of poly2. + float dx = (xf2.Position.X + xf2.R.Col1.X * poly2.MassData.Centroid.X + + xf2.R.Col2.X * poly2.MassData.Centroid.Y) - + (xf1.Position.X + xf1.R.Col1.X * poly1.MassData.Centroid.X + + xf1.R.Col2.X * poly1.MassData.Centroid.Y); + float dy = (xf2.Position.Y + xf2.R.Col1.Y * poly2.MassData.Centroid.X + + xf2.R.Col2.Y * poly2.MassData.Centroid.Y) - + (xf1.Position.Y + xf1.R.Col1.Y * poly1.MassData.Centroid.X + + xf1.R.Col2.Y * poly1.MassData.Centroid.Y); + Vector2 dLocal1 = new Vector2(dx * xf1.R.Col1.X + dy * xf1.R.Col1.Y, dx * xf1.R.Col2.X + dy * xf1.R.Col2.Y); + + // 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(poly1.Normals[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(); + + int count2 = poly2.Vertices.Count; + + Debug.Assert(0 <= edge1 && edge1 < poly1.Vertices.Count); + + // Get the normal of the reference edge in poly2's frame. + Vector2 v = poly1.Normals[edge1]; + float tmpx = xf1.R.Col1.X * v.X + xf1.R.Col2.X * v.Y; + float tmpy = xf1.R.Col1.Y * v.X + xf1.R.Col2.Y * v.Y; + Vector2 normal1 = new Vector2(tmpx * xf2.R.Col1.X + tmpy * xf2.R.Col1.Y, + tmpx * xf2.R.Col2.X + tmpy * xf2.R.Col2.Y); + + // 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, poly2.Normals[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]; + + Vector2 v1 = poly2.Vertices[i1]; + cv0.V.X = xf2.Position.X + xf2.R.Col1.X * v1.X + xf2.R.Col2.X * v1.Y; + cv0.V.Y = xf2.Position.Y + xf2.R.Col1.Y * v1.X + xf2.R.Col2.Y * v1.Y; + 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]; + Vector2 v2 = poly2.Vertices[i2]; + cv1.V.X = xf2.Position.X + xf2.R.Col1.X * v2.X + xf2.R.Col2.X * v2.Y; + cv1.V.Y = xf2.Position.Y + xf2.R.Col1.Y * v2.X + xf2.R.Col2.Y * v2.Y; + 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/axios/Collision/Distance.cs b/axios/Collision/Distance.cs new file mode 100644 index 0000000..354a459 --- /dev/null +++ b/axios/Collision/Distance.cs @@ -0,0 +1,780 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not 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(); + + /// + /// 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.Loop: + { + LoopShape loop = (LoopShape)shape; + Debug.Assert(0 <= index && index < loop.Vertices.Count); + Vertices.Clear(); + Vertices.Add(loop.Vertices[index]); + Vertices.Add(index + 1 < loop.Vertices.Count ? loop.Vertices[index + 1] : loop.Vertices[0]); + + Radius = loop.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 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 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.Multiply(ref transformA, wALocal); + v.WB = MathUtils.Multiply(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.Multiply(ref transformA, wALocal); + v.WB = MathUtils.Multiply(ref transformB, wBLocal); + v.W = v.WB - v.WA; + 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; + } + } + + public static class Distance + { + public static int GJKCalls, GJKIters, GJKMaxIters; + + public static void ComputeDistance(out DistanceOutput output, + out SimplexCache cache, + DistanceInput input) + { + cache = new SimplexCache(); + ++GJKCalls; + + // Initialize the simplex. + Simplex simplex = new Simplex(); + simplex.ReadCache(ref cache, input.ProxyA, ref input.TransformA, input.ProxyB, ref input.TransformB); + + // Get simplex vertices as an array. + const int k_maxIters = 20; + + // 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(); + + Vector2 closestPoint = simplex.GetClosestPoint(); + float distanceSqr1 = closestPoint.LengthSquared(); + float distanceSqr2 = distanceSqr1; + + // Main iteration loop. + int iter = 0; + while (iter < k_maxIters) + { + // 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; + } + + // Compute closest point. + Vector2 p = simplex.GetClosestPoint(); + 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.MultiplyT(ref input.TransformA.R, -d)); + vertex.WA = MathUtils.Multiply(ref input.TransformA, input.ProxyA.Vertices[vertex.IndexA]); + + vertex.IndexB = input.ProxyB.GetSupport(MathUtils.MultiplyT(ref input.TransformB.R, d)); + vertex.WB = MathUtils.Multiply(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; + ++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; + } + + 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/axios/Collision/DynamicTree.cs b/axios/Collision/DynamicTree.cs new file mode 100644 index 0000000..aba6d7c --- /dev/null +++ b/axios/Collision/DynamicTree.cs @@ -0,0 +1,654 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being 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 struct DynamicTreeNode + { + /// + /// This is the fattened AABB. + /// + internal AABB AABB; + + internal int Child1; + internal int Child2; + + internal int LeafCount; + 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 + { + internal const int NullNode = -1; + private static Stack _stack = new Stack(256); + private int _freeList; + private int _insertionCount; + private int _nodeCapacity; + private int _nodeCount; + private DynamicTreeNode[] _nodes; + + /// + /// This is used incrementally traverse the tree for re-balancing. + /// + private int _path; + + private int _root; + + /// + /// Constructing the tree initializes the node pool. + /// + public DynamicTree() + { + _root = NullNode; + + _nodeCapacity = 16; + _nodes = new DynamicTreeNode[_nodeCapacity]; + + // Build a linked list for the free list. + for (int i = 0; i < _nodeCapacity - 1; ++i) + { + _nodes[i].ParentOrNext = i + 1; + } + _nodes[_nodeCapacity - 1].ParentOrNext = NullNode; + } + + /// + /// 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].LeafCount = 1; + + 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; + } + + /// + /// Perform some iterations to re-balance the tree. + /// + /// The iterations. + public void Rebalance(int iterations) + { + if (_root == NullNode) + { + return; + } + + // Rebalance the tree by removing and re-inserting leaves. + for (int i = 0; i < iterations; ++i) + { + int node = _root; + + int bit = 0; + while (_nodes[node].IsLeaf() == false) + { + // Child selector based on a bit in the path + int selector = (_path >> bit) & 1; + + // Select the child nod + node = (selector == 0) ? _nodes[node].Child1 : _nodes[node].Child2; + + // Keep bit between 0 and 31 because _path has 32 bits + // bit = (bit + 1) % 31 + bit = (bit + 1) & 0x1F; + } + ++_path; + + RemoveLeaf(node); + InsertLeaf(node); + } + } + + /// + /// 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; + } + + /// + /// Compute the height of the binary tree in O(N) time. Should not be + /// called often. + /// + /// + public int ComputeHeight() + { + return ComputeHeight(_root); + } + + /// + /// 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) + { + _stack.Clear(); + _stack.Push(_root); + + while (_stack.Count > 0) + { + int nodeId = _stack.Pop(); + if (nodeId == NullNode) + { + continue; + } + + DynamicTreeNode node = _nodes[nodeId]; + + if (AABB.TestOverlap(ref node.AABB, ref aabb)) + { + if (node.IsLeaf()) + { + bool proceed = callback(nodeId); + if (proceed == false) + { + return; + } + } + else + { + _stack.Push(node.Child1); + _stack.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)); + + // 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); + } + + _stack.Clear(); + _stack.Push(_root); + + while (_stack.Count > 0) + { + int nodeId = _stack.Pop(); + if (nodeId == NullNode) + { + continue; + } + + DynamicTreeNode 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 + { + _stack.Push(node.Child1); + _stack.Push(node.Child2); + } + } + } + + private int CountLeaves(int nodeId) + { + if (nodeId == NullNode) + { + return 0; + } + + Debug.Assert(0 <= nodeId && nodeId < _nodeCapacity); + DynamicTreeNode node = _nodes[nodeId]; + + if (node.IsLeaf()) + { + Debug.Assert(node.LeafCount == 1); + return 1; + } + + int count1 = CountLeaves(node.Child1); + int count2 = CountLeaves(node.Child2); + int count = count1 + count2; + Debug.Assert(count == node.LeafCount); + return count; + } + + private void Validate() + { + CountLeaves(_root); + } + + 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. + DynamicTreeNode[] oldNodes = _nodes; + _nodeCapacity *= 2; + _nodes = new DynamicTreeNode[_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].ParentOrNext = i + 1; + } + _nodes[_nodeCapacity - 1].ParentOrNext = NullNode; + _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].LeafCount = 0; + ++_nodeCount; + return nodeId; + } + + private void FreeNode(int nodeId) + { + Debug.Assert(0 <= nodeId && nodeId < _nodeCapacity); + Debug.Assert(0 < _nodeCount); + _nodes[nodeId].ParentOrNext = _freeList; + _freeList = nodeId; + --_nodeCount; + } + + private void InsertLeaf(int leaf) + { + ++_insertionCount; + + if (_root == NullNode) + { + _root = leaf; + _nodes[_root].ParentOrNext = NullNode; + return; + } + + // Find the best sibling for this node + AABB leafAABB = _nodes[leaf].AABB; + int sibling = _root; + while (_nodes[sibling].IsLeaf() == false) + { + int child1 = _nodes[sibling].Child1; + int child2 = _nodes[sibling].Child2; + + // Expand the node's AABB. + _nodes[sibling].AABB.Combine(ref leafAABB); + _nodes[sibling].LeafCount += 1; + + float siblingArea = _nodes[sibling].AABB.Perimeter; + AABB parentAABB = new AABB(); + parentAABB.Combine(ref _nodes[sibling].AABB, ref leafAABB); + float parentArea = parentAABB.Perimeter; + float cost1 = 2.0f * parentArea; + + float inheritanceCost = 2.0f * (parentArea - siblingArea); + + float cost2; + if (_nodes[child1].IsLeaf()) + { + AABB aabb = new AABB(); + aabb.Combine(ref leafAABB, ref _nodes[child1].AABB); + cost2 = 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; + cost2 = (newArea - oldArea) + inheritanceCost; + } + + float cost3; + if (_nodes[child2].IsLeaf()) + { + AABB aabb = new AABB(); + aabb.Combine(ref leafAABB, ref _nodes[child2].AABB); + cost3 = 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; + cost3 = newArea - oldArea + inheritanceCost; + } + + // Descend according to the minimum cost. + if (cost1 < cost2 && cost1 < cost3) + { + break; + } + + // Expand the node's AABB to account for the new leaf. + _nodes[sibling].AABB.Combine(ref leafAABB); + + // Descend + if (cost2 < cost3) + { + sibling = child1; + } + else + { + sibling = child2; + } + } + + // Create a new parent for the siblings. + 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].LeafCount = _nodes[sibling].LeafCount + 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; + } + } + + 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. + parent = grandParent; + while (parent != NullNode) + { + _nodes[parent].AABB.Combine(ref _nodes[_nodes[parent].Child1].AABB, + ref _nodes[_nodes[parent].Child2].AABB); + + Debug.Assert(_nodes[parent].LeafCount > 0); + _nodes[parent].LeafCount -= 1; + + parent = _nodes[parent].ParentOrNext; + } + } + else + { + _root = sibling; + _nodes[sibling].ParentOrNext = NullNode; + FreeNode(parent); + } + } + + private int ComputeHeight(int nodeId) + { + if (nodeId == NullNode) + { + return 0; + } + + Debug.Assert(0 <= nodeId && nodeId < _nodeCapacity); + DynamicTreeNode node = _nodes[nodeId]; + int height1 = ComputeHeight(node.Child1); + int height2 = ComputeHeight(node.Child2); + return 1 + Math.Max(height1, height2); + } + } +} \ No newline at end of file diff --git a/axios/Collision/DynamicTreeBroadPhase.cs b/axios/Collision/DynamicTreeBroadPhase.cs new file mode 100644 index 0000000..88a2cc7 --- /dev/null +++ b/axios/Collision/DynamicTreeBroadPhase.cs @@ -0,0 +1,324 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must 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 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(); + + public DynamicTreeBroadPhase() + { + _queryCallback = new Func(QueryCallback); + + _pairCapacity = 16; + _pairBuffer = new Pair[_pairCapacity]; + + _moveCapacity = 16; + _moveBuffer = new int[_moveCapacity]; + } + + #region IBroadPhase Members + + /// + /// 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 aabb. + /// 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); + } + } + + /// + /// 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 == -1) + { + 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 TouchProxy(int proxyId) + { + BufferMove(proxyId); + } + + #endregion + + /// + /// Compute the height of the embedded tree. + /// + /// + public int ComputeHeight() + { + return _tree.ComputeHeight(); + } + + 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] = -1; + return; + } + } + } + + 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; + } + } +} \ No newline at end of file diff --git a/axios/Collision/IBroadPhase.cs b/axios/Collision/IBroadPhase.cs new file mode 100644 index 0000000..231a0f6 --- /dev/null +++ b/axios/Collision/IBroadPhase.cs @@ -0,0 +1,30 @@ +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); + } +} \ No newline at end of file diff --git a/axios/Collision/QuadTree.cs b/axios/Collision/QuadTree.cs new file mode 100644 index 0000000..e7c968c --- /dev/null +++ b/axios/Collision/QuadTree.cs @@ -0,0 +1,267 @@ +using System; +using System.Collections.Generic; +using FarseerPhysics.Collision; +using Microsoft.Xna.Framework; + +public class Element +{ + public QuadTree Parent; + public AABB Span; + public T Value; + + public Element(T value, AABB span) + { + Span = span; + Value = value; + Parent = null; + } +} + +public class QuadTree +{ + public int MaxBucket; + public int MaxDepth; + public List> Nodes; + public AABB Span; + public QuadTree[] SubTrees; + + public QuadTree(AABB span, int maxbucket, int maxdepth) + { + Span = span; + Nodes = new List>(); + + MaxBucket = maxbucket; + MaxDepth = maxdepth; + } + + public bool IsPartitioned + { + get { return SubTrees != null; } + } + + /// + /// returns the quadrant of span that entirely contains test. if none, return 0. + /// + /// + /// + /// + private int Partition(AABB span, AABB test) + { + if (span.Q1.Contains(ref test)) return 1; + if (span.Q2.Contains(ref test)) return 2; + if (span.Q3.Contains(ref test)) return 3; + if (span.Q4.Contains(ref test)) return 4; + + return 0; + } + + public void AddNode(Element node) + { + if (!IsPartitioned) + { + if (Nodes.Count >= MaxBucket && MaxDepth > 0) //bin is full and can still subdivide + { + // + //partition into quadrants and sort existing nodes amonst quads. + // + Nodes.Add(node); //treat new node just like other nodes for partitioning + + SubTrees = new QuadTree[4]; + SubTrees[0] = new QuadTree(Span.Q1, MaxBucket, MaxDepth - 1); + SubTrees[1] = new QuadTree(Span.Q2, MaxBucket, MaxDepth - 1); + SubTrees[2] = new QuadTree(Span.Q3, MaxBucket, MaxDepth - 1); + SubTrees[3] = new QuadTree(Span.Q4, MaxBucket, MaxDepth - 1); + + List> remNodes = new List>(); + //nodes that are not fully contained by any quadrant + + foreach (Element n in Nodes) + { + switch (Partition(Span, n.Span)) + { + case 1: //quadrant 1 + SubTrees[0].AddNode(n); + break; + case 2: + SubTrees[1].AddNode(n); + break; + case 3: + SubTrees[2].AddNode(n); + break; + case 4: + SubTrees[3].AddNode(n); + break; + default: + n.Parent = this; + remNodes.Add(n); + break; + } + } + + Nodes = remNodes; + } + else + { + node.Parent = this; + Nodes.Add(node); + //if bin is not yet full or max depth has been reached, just add the node without subdividing + } + } + else //we already have children nodes + { + // + //add node to specific sub-tree + // + switch (Partition(Span, node.Span)) + { + case 1: //quadrant 1 + SubTrees[0].AddNode(node); + break; + case 2: + SubTrees[1].AddNode(node); + break; + case 3: + SubTrees[2].AddNode(node); + break; + case 4: + SubTrees[3].AddNode(node); + break; + default: + node.Parent = this; + Nodes.Add(node); + break; + } + } + } + + /// + /// tests if ray intersects AABB + /// + /// + /// + public static bool RayCastAABB(AABB aabb, Vector2 p1, Vector2 p2) + { + AABB segmentAABB = new AABB(); + { + Vector2.Min(ref p1, ref p2, out segmentAABB.LowerBound); + Vector2.Max(ref p1, ref p2, out segmentAABB.UpperBound); + } + if (!AABB.TestOverlap(aabb, segmentAABB)) return false; + + Vector2 rayDir = p2 - p1; + Vector2 rayPos = p1; + + Vector2 norm = new Vector2(-rayDir.Y, rayDir.X); //normal to ray + if (norm.Length() == 0.0) + return true; //if ray is just a point, return true (iff point is within aabb, as tested earlier) + norm.Normalize(); + + float dPos = Vector2.Dot(rayPos, norm); + + Vector2[] verts = aabb.GetVertices(); + float d0 = Vector2.Dot(verts[0], norm) - dPos; + for (int i = 1; i < 4; i++) + { + float d = Vector2.Dot(verts[i], norm) - dPos; + if (Math.Sign(d) != Math.Sign(d0)) + //return true if the ray splits the vertices (ie: sign of dot products with normal are not all same) + return true; + } + + return false; + } + + public void QueryAABB(Func, bool> callback, ref AABB searchR) + { + Stack> stack = new Stack>(); + stack.Push(this); + + while (stack.Count > 0) + { + QuadTree qt = stack.Pop(); + if (!AABB.TestOverlap(ref searchR, ref qt.Span)) + continue; + + foreach (Element n in qt.Nodes) + if (AABB.TestOverlap(ref searchR, ref n.Span)) + { + if (!callback(n)) return; + } + + if (qt.IsPartitioned) + foreach (QuadTree st in qt.SubTrees) + stack.Push(st); + } + } + + public void RayCast(Func, float> callback, ref RayCastInput input) + { + Stack> stack = new Stack>(); + stack.Push(this); + + float maxFraction = input.MaxFraction; + Vector2 p1 = input.Point1; + Vector2 p2 = p1 + (input.Point2 - input.Point1) * maxFraction; + + while (stack.Count > 0) + { + QuadTree qt = stack.Pop(); + + if (!RayCastAABB(qt.Span, p1, p2)) + continue; + + foreach (Element n in qt.Nodes) + { + if (!RayCastAABB(n.Span, p1, p2)) + continue; + + RayCastInput subInput; + subInput.Point1 = input.Point1; + subInput.Point2 = input.Point2; + subInput.MaxFraction = maxFraction; + + float value = callback(subInput, n); + if (value == 0.0f) + return; // the client has terminated the raycast. + + if (value <= 0.0f) + continue; + + maxFraction = value; + p2 = p1 + (input.Point2 - input.Point1) * maxFraction; //update segment endpoint + } + if (IsPartitioned) + foreach (QuadTree st in qt.SubTrees) + stack.Push(st); + } + } + + public void GetAllNodesR(ref List> nodes) + { + nodes.AddRange(Nodes); + + if (IsPartitioned) + foreach (QuadTree st in SubTrees) st.GetAllNodesR(ref nodes); + } + + public void RemoveNode(Element node) + { + node.Parent.Nodes.Remove(node); + } + + public void Reconstruct() + { + List> allNodes = new List>(); + GetAllNodesR(ref allNodes); + + Clear(); + + allNodes.ForEach(AddNode); + } + + public void Clear() + { + Nodes.Clear(); + SubTrees = null; + } +} \ No newline at end of file diff --git a/axios/Collision/QuadTreeBroadPhase.cs b/axios/Collision/QuadTreeBroadPhase.cs new file mode 100644 index 0000000..322170a --- /dev/null +++ b/axios/Collision/QuadTreeBroadPhase.cs @@ -0,0 +1,249 @@ +using System; +using System.Collections.Generic; +using FarseerPhysics; +using FarseerPhysics.Collision; +using FarseerPhysics.Dynamics; +using Microsoft.Xna.Framework; + +public class QuadTreeBroadPhase : IBroadPhase +{ + private const int TreeUpdateThresh = 10000; + private int _currID; + private Dictionary> _idRegister; + private List> _moveBuffer; + private List _pairBuffer; + private QuadTree _quadTree; + private int _treeMoveNum; + + /// + /// Creates a new quad tree broadphase with the specified span. + /// + /// the maximum span of the tree (world size) + public QuadTreeBroadPhase(AABB span) + { + _quadTree = new QuadTree(span, 5, 10); + _idRegister = new Dictionary>(); + _moveBuffer = new List>(); + _pairBuffer = new List(); + } + + #region IBroadPhase Members + + /// + /// The number of proxies + /// + public int ProxyCount + { + get { return _idRegister.Count; } + } + + public void GetFatAABB(int proxyID, out AABB aabb) + { + if (_idRegister.ContainsKey(proxyID)) + aabb = _idRegister[proxyID].Span; + else + throw new KeyNotFoundException("proxyID not found in register"); + } + + public void UpdatePairs(BroadphaseDelegate callback) + { + _pairBuffer.Clear(); + foreach (Element qtnode in _moveBuffer) + { + // Query tree, create pairs and add them pair buffer. + Query(proxyID => PairBufferQueryCallback(proxyID, qtnode.Value.ProxyId), ref qtnode.Span); + } + _moveBuffer.Clear(); + + // Sort the pair buffer to expose duplicates. + _pairBuffer.Sort(); + + // Send the pairs back to the client. + int i = 0; + while (i < _pairBuffer.Count) + { + Pair primaryPair = _pairBuffer[i]; + FixtureProxy userDataA = GetProxy(primaryPair.ProxyIdA); + FixtureProxy userDataB = GetProxy(primaryPair.ProxyIdB); + + callback(ref userDataA, ref userDataB); + ++i; + + // Skip any duplicate pairs. + while (i < _pairBuffer.Count && _pairBuffer[i].ProxyIdA == primaryPair.ProxyIdA && + _pairBuffer[i].ProxyIdB == primaryPair.ProxyIdB) + ++i; + } + } + + /// + /// Test overlap of fat AABBs. + /// + /// The proxy id A. + /// The proxy id B. + /// + public bool TestOverlap(int proxyIdA, int proxyIdB) + { + AABB aabb1; + AABB aabb2; + GetFatAABB(proxyIdA, out aabb1); + GetFatAABB(proxyIdB, out aabb2); + return AABB.TestOverlap(ref aabb1, ref aabb2); + } + + public int AddProxy(ref FixtureProxy proxy) + { + int proxyID = _currID++; + proxy.ProxyId = proxyID; + AABB aabb = Fatten(ref proxy.AABB); + Element qtnode = new Element(proxy, aabb); + + _idRegister.Add(proxyID, qtnode); + _quadTree.AddNode(qtnode); + + return proxyID; + } + + public void RemoveProxy(int proxyId) + { + if (_idRegister.ContainsKey(proxyId)) + { + Element qtnode = _idRegister[proxyId]; + UnbufferMove(qtnode); + _idRegister.Remove(proxyId); + _quadTree.RemoveNode(qtnode); + } + else + throw new KeyNotFoundException("proxyID not found in register"); + } + + public void MoveProxy(int proxyId, ref AABB aabb, Vector2 displacement) + { + AABB fatAABB; + GetFatAABB(proxyId, out fatAABB); + + //exit if movement is within fat aabb + if (fatAABB.Contains(ref aabb)) + return; + + // 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; + + + Element qtnode = _idRegister[proxyId]; + qtnode.Value.AABB = b; //not neccesary for QTree, but might be accessed externally + qtnode.Span = b; + + ReinsertNode(qtnode); + + BufferMove(qtnode); + } + + public FixtureProxy GetProxy(int proxyId) + { + if (_idRegister.ContainsKey(proxyId)) + return _idRegister[proxyId].Value; + else + throw new KeyNotFoundException("proxyID not found in register"); + } + + public void TouchProxy(int proxyId) + { + if (_idRegister.ContainsKey(proxyId)) + BufferMove(_idRegister[proxyId]); + else + throw new KeyNotFoundException("proxyID not found in register"); + } + + public void Query(Func callback, ref AABB query) + { + _quadTree.QueryAABB(TransformPredicate(callback), ref query); + } + + public void RayCast(Func callback, ref RayCastInput input) + { + _quadTree.RayCast(TransformRayCallback(callback), ref input); + } + + #endregion + + private AABB Fatten(ref AABB aabb) + { + Vector2 r = new Vector2(Settings.AABBExtension, Settings.AABBExtension); + return new AABB(aabb.LowerBound - r, aabb.UpperBound + r); + } + + private Func, bool> TransformPredicate(Func idPredicate) + { + Func, bool> qtPred = qtnode => idPredicate(qtnode.Value.ProxyId); + return qtPred; + } + + private Func, float> TransformRayCallback( + Func callback) + { + Func, float> newCallback = + (input, qtnode) => callback(input, qtnode.Value.ProxyId); + return newCallback; + } + + private bool PairBufferQueryCallback(int proxyID, int baseID) + { + // A proxy cannot form a pair with itself. + if (proxyID == baseID) + return true; + + Pair p = new Pair(); + p.ProxyIdA = Math.Min(proxyID, baseID); + p.ProxyIdB = Math.Max(proxyID, baseID); + _pairBuffer.Add(p); + + return true; + } + + private void ReconstructTree() + { + //this is faster than _quadTree.Reconstruct(), since the quadtree method runs a recusive query to find all nodes. + _quadTree.Clear(); + foreach (Element elem in _idRegister.Values) + _quadTree.AddNode(elem); + } + + private void ReinsertNode(Element qtnode) + { + _quadTree.RemoveNode(qtnode); + _quadTree.AddNode(qtnode); + + if (++_treeMoveNum > TreeUpdateThresh) + { + ReconstructTree(); + _treeMoveNum = 0; + } + } + + private void BufferMove(Element proxy) + { + _moveBuffer.Add(proxy); + } + + private void UnbufferMove(Element proxy) + { + _moveBuffer.Remove(proxy); + } +} \ No newline at end of file diff --git a/axios/Collision/Shapes/CircleShape.cs b/axios/Collision/Shapes/CircleShape.cs new file mode 100644 index 0000000..5fad0d0 --- /dev/null +++ b/axios/Collision/Shapes/CircleShape.cs @@ -0,0 +1,207 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must 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.Collision.Shapes +{ + public class CircleShape : Shape + { + internal Vector2 _position; + + public CircleShape(float radius, float density) + : base(density) + { + ShapeType = ShapeType.Circle; + _radius = radius; + _position = Vector2.Zero; + ComputeProperties(); + } + + internal CircleShape() + : base(0) + { + ShapeType = ShapeType.Circle; + _radius = 0.0f; + _position = Vector2.Zero; + } + + public override int ChildCount + { + get { return 1; } + } + + public Vector2 Position + { + get { return _position; } + set + { + _position = value; + ComputeProperties(); + } + } + + public override Shape Clone() + { + CircleShape shape = new CircleShape(); + shape._radius = Radius; + shape._density = _density; + shape._position = _position; + shape.ShapeType = ShapeType; + shape.MassData = MassData; + return shape; + } + + /// + /// Test a point for containment in this shape. This only works for convex shapes. + /// + /// The shape world transform. + /// a point in world coordinates. + /// True if the point is inside the shape + public override bool TestPoint(ref Transform transform, ref Vector2 point) + { + Vector2 center = transform.Position + MathUtils.Multiply(ref transform.R, Position); + Vector2 d = point - center; + return Vector2.Dot(d, d) <= Radius * Radius; + } + + /// + /// 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 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.Position + MathUtils.Multiply(ref transform.R, Position); + Vector2 s = input.Point1 - position; + float b = Vector2.Dot(s, s) - Radius * Radius; + + // 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; + Vector2 norm = (s + a * r); + norm.Normalize(); + output.Normal = norm; + 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 p = transform.Position + MathUtils.Multiply(ref transform.R, Position); + aabb.LowerBound = new Vector2(p.X - Radius, p.Y - Radius); + aabb.UpperBound = new Vector2(p.X + Radius, p.Y + Radius); + } + + /// + /// Compute the mass properties of this shape using its dimensions and density. + /// The inertia tensor is computed about the local origin, not the centroid. + /// + public override sealed void ComputeProperties() + { + float area = Settings.Pi * Radius * Radius; + MassData.Area = area; + MassData.Mass = Density * area; + MassData.Centroid = Position; + + // inertia about the local origin + MassData.Inertia = MassData.Mass * (0.5f * Radius * Radius + Vector2.Dot(Position, Position)); + } + + public bool CompareTo(CircleShape shape) + { + return (Radius == shape.Radius && + Position == shape.Position); + } + + public override float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 sc) + { + sc = Vector2.Zero; + + Vector2 p = MathUtils.Multiply(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 * Radius * Radius; + } + + //Magic + float r2 = Radius * Radius; + float l2 = l * l; + float area = r2 * (float)((Math.Asin(l / Radius) + Settings.Pi / 2) + l * Math.Sqrt(r2 - l2)); + float com = -2.0f / 3.0f * (float)Math.Pow(r2 - l2, 1.5f) / area; + + sc.X = p.X + normal.X * com; + sc.Y = p.Y + normal.Y * com; + + return area; + } + } +} \ No newline at end of file diff --git a/axios/Collision/Shapes/EdgeShape.cs b/axios/Collision/Shapes/EdgeShape.cs new file mode 100644 index 0000000..34a1506 --- /dev/null +++ b/axios/Collision/Shapes/EdgeShape.cs @@ -0,0 +1,266 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, 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 + { + public bool HasVertex0, HasVertex3; + + /// + /// Optional adjacent vertices. These are used for smooth collision. + /// + public Vector2 Vertex0; + + /// + /// Optional adjacent vertices. These are used for smooth collision. + /// + public Vector2 Vertex3; + + /// + /// Edge start vertex + /// + private Vector2 _vertex1; + + /// + /// Edge end vertex + /// + private Vector2 _vertex2; + + internal EdgeShape() + : base(0) + { + ShapeType = ShapeType.Edge; + _radius = Settings.PolygonRadius; + } + + public EdgeShape(Vector2 start, Vector2 end) + : base(0) + { + ShapeType = ShapeType.Edge; + _radius = Settings.PolygonRadius; + Set(start, end); + } + + public override int ChildCount + { + get { return 1; } + } + + /// + /// 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 Shape Clone() + { + EdgeShape edge = new EdgeShape(); + edge._radius = _radius; + edge._density = _density; + edge.HasVertex0 = HasVertex0; + edge.HasVertex3 = HasVertex3; + edge.Vertex0 = Vertex0; + edge._vertex1 = _vertex1; + edge._vertex2 = _vertex2; + edge.Vertex3 = Vertex3; + edge.MassData = MassData; + return edge; + } + + /// + /// Test a point for containment in this shape. This only works for convex shapes. + /// + /// The shape world transform. + /// a point in world coordinates. + /// True if the point is inside the shape + public override bool TestPoint(ref Transform transform, ref Vector2 point) + { + return false; + } + + /// + /// 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 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.MultiplyT(ref transform.R, input.Point1 - transform.Position); + Vector2 p2 = MathUtils.MultiplyT(ref transform.R, input.Point2 - transform.Position); + Vector2 d = p2 - p1; + + Vector2 v1 = _vertex1; + Vector2 v2 = _vertex2; + Vector2 e = v2 - v1; + Vector2 normal = new Vector2(e.Y, -e.X); + 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 || 1.0f < 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; + } + + /// + /// 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 v1 = MathUtils.Multiply(ref transform, _vertex1); + Vector2 v2 = MathUtils.Multiply(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; + } + + /// + /// Compute the mass properties of this shape using its dimensions and density. + /// The inertia tensor is computed about the local origin, not the centroid. + /// + public override void ComputeProperties() + { + MassData.Centroid = 0.5f * (_vertex1 + _vertex2); + } + + public override float ComputeSubmergedArea(Vector2 normal, float offset, 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); + } + } +} \ No newline at end of file diff --git a/axios/Collision/Shapes/LoopShape.cs b/axios/Collision/Shapes/LoopShape.cs new file mode 100644 index 0000000..dd4f04f --- /dev/null +++ b/axios/Collision/Shapes/LoopShape.cs @@ -0,0 +1,188 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and 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 loop Shape is a free form sequence of line segments that form a circular list. + /// The loop may cross upon itself, but this is not recommended for smooth collision. + /// The loop has double sided collision, so you can use inside and outside collision. + /// Therefore, you may use any winding order. + /// + public class LoopShape : Shape + { + private static EdgeShape _edgeShape = new EdgeShape(); + + /// + /// The vertices. These are not owned/freed by the loop Shape. + /// + public Vertices Vertices; + + private LoopShape() + : base(0) + { + ShapeType = ShapeType.Loop; + _radius = Settings.PolygonRadius; + } + + public LoopShape(Vertices vertices) + : base(0) + { + ShapeType = ShapeType.Loop; + _radius = Settings.PolygonRadius; + +#pragma warning disable 162 + if (Settings.ConserveMemory) + Vertices = vertices; + else + // Copy vertices. + Vertices = new Vertices(vertices); +#pragma warning restore 162 + } + + public override int ChildCount + { + get { return Vertices.Count; } + } + + public override Shape Clone() + { + LoopShape loop = new LoopShape(); + loop._density = _density; + loop._radius = _radius; + loop.Vertices = Vertices; + loop.MassData = MassData; + return loop; + } + + /// + /// Get a child edge. + /// + /// The edge. + /// The index. + public void GetChildEdge(ref EdgeShape edge, int index) + { + Debug.Assert(2 <= Vertices.Count); + Debug.Assert(0 <= index && index < Vertices.Count); + edge.ShapeType = ShapeType.Edge; + edge._radius = _radius; + edge.HasVertex0 = true; + edge.HasVertex3 = true; + + int i0 = index - 1 >= 0 ? index - 1 : Vertices.Count - 1; + int i1 = index; + int i2 = index + 1 < Vertices.Count ? index + 1 : 0; + int i3 = index + 2; + while (i3 >= Vertices.Count) + { + i3 -= Vertices.Count; + } + + edge.Vertex0 = Vertices[i0]; + edge.Vertex1 = Vertices[i1]; + edge.Vertex2 = Vertices[i2]; + edge.Vertex3 = Vertices[i3]; + } + + /// + /// Test a point for containment in this shape. This only works for convex shapes. + /// + /// The shape world transform. + /// a point in world coordinates. + /// True if the point is inside the shape + public override bool TestPoint(ref Transform transform, ref Vector2 point) + { + return false; + } + + /// + /// 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 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); + } + + /// + /// 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) + { + Debug.Assert(childIndex < Vertices.Count); + + int i1 = childIndex; + int i2 = childIndex + 1; + if (i2 == Vertices.Count) + { + i2 = 0; + } + + Vector2 v1 = MathUtils.Multiply(ref transform, Vertices[i1]); + Vector2 v2 = MathUtils.Multiply(ref transform, Vertices[i2]); + + aabb.LowerBound = Vector2.Min(v1, v2); + aabb.UpperBound = Vector2.Max(v1, v2); + } + + /// + /// Chains have zero mass. + /// + public override void ComputeProperties() + { + //Does nothing. Loop shapes don't have properties. + } + + public override float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 sc) + { + sc = Vector2.Zero; + return 0; + } + } +} \ No newline at end of file diff --git a/axios/Collision/Shapes/PolygonShape.cs b/axios/Collision/Shapes/PolygonShape.cs new file mode 100644 index 0000000..7661b7d --- /dev/null +++ b/axios/Collision/Shapes/PolygonShape.cs @@ -0,0 +1,556 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and 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.Decomposition; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Collision.Shapes +{ + /// + /// Represents a simple non-selfintersecting convex polygon. + /// If you want to have concave polygons, you will have to use the or the + /// to decompose the concave polygon into 2 or more convex polygons. + /// + public class PolygonShape : Shape + { + public Vertices Normals; + public Vertices Vertices; + + /// + /// 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; + + Set(vertices); + } + + public PolygonShape(float density) + : base(density) + { + ShapeType = ShapeType.Polygon; + _radius = Settings.PolygonRadius; + Normals = new Vertices(); + Vertices = new Vertices(); + } + + internal PolygonShape() + : base(0) + { + ShapeType = ShapeType.Polygon; + _radius = Settings.PolygonRadius; + Normals = new Vertices(); + Vertices = new Vertices(); + } + + public override int ChildCount + { + get { return 1; } + } + + public override Shape Clone() + { + PolygonShape clone = new PolygonShape(); + clone.ShapeType = ShapeType; + clone._radius = _radius; + clone._density = _density; + + if (Settings.ConserveMemory) + { +#pragma warning disable 162 + clone.Vertices = Vertices; + clone.Normals = Normals; +#pragma warning restore 162 + } + else + { + clone.Vertices = new Vertices(Vertices); + clone.Normals = new Vertices(Normals); + } + + clone.MassData = MassData; + return clone; + } + + /// + /// Copy vertices. This assumes the vertices define a convex polygon. + /// It is assumed that the exterior is the the right of each edge. + /// + /// The vertices. + public void Set(Vertices vertices) + { + Debug.Assert(vertices.Count >= 3 && vertices.Count <= Settings.MaxPolygonVertices); + +#pragma warning disable 162 + if (Settings.ConserveMemory) + Vertices = vertices; + else + // Copy vertices. + Vertices = new Vertices(vertices); +#pragma warning restore 162 + + Normals = new Vertices(vertices.Count); + + // Compute normals. Ensure the edges have non-zero length. + for (int i = 0; i < vertices.Count; ++i) + { + int i1 = i; + int i2 = i + 1 < vertices.Count ? i + 1 : 0; + Vector2 edge = Vertices[i2] - Vertices[i1]; + Debug.Assert(edge.LengthSquared() > Settings.Epsilon * Settings.Epsilon); + + Vector2 temp = new Vector2(edge.Y, -edge.X); + temp.Normalize(); + Normals.Add(temp); + } + +#if DEBUG + // Ensure the polygon is convex and the interior + // is to the left of each edge. + for (int i = 0; i < Vertices.Count; ++i) + { + int i1 = i; + int i2 = i + 1 < Vertices.Count ? i + 1 : 0; + Vector2 edge = Vertices[i2] - Vertices[i1]; + + for (int j = 0; j < vertices.Count; ++j) + { + // Don't check vertices on the current edge. + if (j == i1 || j == i2) + { + continue; + } + + Vector2 r = Vertices[j] - Vertices[i1]; + + // Your polygon is non-convex (it has an indentation) or + // has colinear edges. + float s = edge.X * r.Y - edge.Y * r.X; + + Debug.Assert(s > 0.0f); + } + } +#endif + + // Compute the polygon mass data + ComputeProperties(); + } + + /// + /// Compute the mass properties of this shape using its dimensions and density. + /// The inertia tensor is computed about the local origin, not the centroid. + /// + public 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); + + if (_density <= 0) + return; + + 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 pRef = Vector2.Zero; + +#if false + // This code would put the reference point inside the polygon. + for (int i = 0; i < count; ++i) + { + pRef += vs[i]; + } + pRef *= 1.0f / count; +#endif + + const float inv3 = 1.0f / 3.0f; + + for (int i = 0; i < Vertices.Count; ++i) + { + // Triangle vertices. + Vector2 p1 = pRef; + Vector2 p2 = Vertices[i]; + Vector2 p3 = i + 1 < Vertices.Count ? Vertices[i + 1] : Vertices[0]; + + Vector2 e1 = p2 - p1; + Vector2 e2 = p3 - p1; + + float d; + MathUtils.Cross(ref e1, ref e2, out d); + + float triangleArea = 0.5f * d; + area += triangleArea; + + // Area weighted centroid + center += triangleArea * inv3 * (p1 + p2 + p3); + + float px = p1.X, py = p1.Y; + float ex1 = e1.X, ey1 = e1.Y; + float ex2 = e2.X, ey2 = e2.Y; + + float intx2 = inv3 * (0.25f * (ex1 * ex1 + ex2 * ex1 + ex2 * ex2) + (px * ex1 + px * ex2)) + + 0.5f * px * px; + float inty2 = inv3 * (0.25f * (ey1 * ey1 + ey2 * ey1 + ey2 * ey2) + (py * ey1 + py * ey2)) + + 0.5f * py * py; + + I += 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; + + // Inertia tensor relative to the local origin. + MassData.Inertia = _density * I; + } + + /// + /// Build vertices to represent an axis-aligned box. + /// + /// The half-width. + /// The half-height. + public void SetAsBox(float halfWidth, float halfHeight) + { + Set(PolygonTools.CreateRectangle(halfWidth, halfHeight)); + } + + /// + /// 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 void SetAsBox(float halfWidth, float halfHeight, Vector2 center, float angle) + { + Set(PolygonTools.CreateRectangle(halfWidth, halfHeight, center, angle)); + } + + /// + /// Test a point for containment in this shape. This only works for convex shapes. + /// + /// The shape world transform. + /// a point in world coordinates. + /// True if the point is inside the shape + public override bool TestPoint(ref Transform transform, ref Vector2 point) + { + Vector2 pLocal = MathUtils.MultiplyT(ref transform.R, point - transform.Position); + + 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; + } + + /// + /// 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 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.MultiplyT(ref transform.R, input.Point1 - transform.Position); + Vector2 p2 = MathUtils.MultiplyT(ref transform.R, input.Point2 - transform.Position); + 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.Multiply(ref transform.R, 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.Multiply(ref transform, Vertices[0]); + Vector2 upper = lower; + + for (int i = 1; i < Vertices.Count; ++i) + { + Vector2 v = MathUtils.Multiply(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 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 float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 sc) + { + sc = Vector2.Zero; + + //Transform plane into shape co-ordinates + Vector2 normalL = MathUtils.MultiplyT(ref xf.R, normal); + float offsetL = offset - Vector2.Dot(normal, xf.Position); + + 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.Multiply(ref xf, MassData.Centroid); + return MassData.Mass / Density; + } + else + { + //Completely dry + return 0; + } +#pragma warning disable 162 + break; +#pragma warning restore 162 + 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]; + Vector2 p3; + + 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; + 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.Multiply(ref xf, center); + + return area; + } + } +} \ No newline at end of file diff --git a/axios/Collision/Shapes/Shape.cs b/axios/Collision/Shapes/Shape.cs new file mode 100644 index 0000000..8d8e1cf --- /dev/null +++ b/axios/Collision/Shapes/Shape.cs @@ -0,0 +1,222 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must 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.Collision.Shapes +{ + /// + /// This holds the mass data computed for a shape. + /// + public struct MassData : IEquatable + { + /// + /// The area of the shape + /// + public float Area; + + /// + /// The position of the shape's centroid relative to the shape's origin. + /// + public Vector2 Centroid; + + /// + /// The rotational inertia of the shape about the local origin. + /// + public float Inertia; + + /// + /// The mass of the shape, usually in kilograms. + /// + public float Mass; + + #region IEquatable Members + + public bool Equals(MassData other) + { + return this == other; + } + + #endregion + + 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); + } + + public static bool operator !=(MassData left, MassData right) + { + return !(left == right); + } + + 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, + Loop = 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 + { + private static int _shapeIdCounter; + public MassData MassData; + public int ShapeId; + + internal float _density; + internal float _radius; + + protected Shape(float density) + { + _density = density; + ShapeType = ShapeType.Unknown; + ShapeId = _shapeIdCounter++; + } + + /// + /// 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. + /// + /// The density. + public float Density + { + get { return _density; } + set + { + _density = value; + ComputeProperties(); + } + } + + /// + /// Radius of the Shape + /// + public float Radius + { + get { return _radius; } + set + { + _radius = value; + ComputeProperties(); + } + } + + /// + /// Clone the concrete shape + /// + /// A clone of the shape + public abstract Shape Clone(); + + /// + /// Test a point for containment in this shape. 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. + /// + public abstract void ComputeProperties(); + + 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); + + return false; + } + + public abstract float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 sc); + } +} \ No newline at end of file diff --git a/axios/Collision/TimeOfImpact.cs b/axios/Collision/TimeOfImpact.cs new file mode 100644 index 0000000..57ef88d --- /dev/null +++ b/axios/Collision/TimeOfImpact.cs @@ -0,0 +1,500 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not 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 + { + private static Vector2 _axis; + private static Vector2 _localPoint; + private static DistanceProxy _proxyA = new DistanceProxy(); + private static DistanceProxy _proxyB = new DistanceProxy(); + private static Sweep _sweepA, _sweepB; + 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.Multiply(ref xfA, localPointA); + Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB); + _axis = pointB - pointA; + _axis.Normalize(); + return; + } + 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.Multiply(ref xfB.R, _axis); + + _localPoint = 0.5f * (localPointB1 + localPointB2); + Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint); + + Vector2 localPointA = proxyA.Vertices[cache.IndexA[0]]; + Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA); + + float s = Vector2.Dot(pointA - pointB, normal); + if (s < 0.0f) + { + _axis = -_axis; + s = -s; + } + return; + } + 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.Multiply(ref xfA.R, _axis); + + _localPoint = 0.5f * (localPointA1 + localPointA2); + Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint); + + Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]]; + Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB); + + float s = Vector2.Dot(pointB - pointA, normal); + if (s < 0.0f) + { + _axis = -_axis; + s = -s; + } + return; + } + } + + 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.MultiplyT(ref xfA.R, _axis); + Vector2 axisB = MathUtils.MultiplyT(ref xfB.R, -_axis); + + indexA = _proxyA.GetSupport(axisA); + indexB = _proxyB.GetSupport(axisB); + + Vector2 localPointA = _proxyA.Vertices[indexA]; + Vector2 localPointB = _proxyB.Vertices[indexB]; + + Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA); + Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB); + + float separation = Vector2.Dot(pointB - pointA, _axis); + return separation; + } + + case SeparationFunctionType.FaceA: + { + Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis); + Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint); + + Vector2 axisB = MathUtils.MultiplyT(ref xfB.R, -normal); + + indexA = -1; + indexB = _proxyB.GetSupport(axisB); + + Vector2 localPointB = _proxyB.Vertices[indexB]; + Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB); + + float separation = Vector2.Dot(pointB - pointA, normal); + return separation; + } + + case SeparationFunctionType.FaceB: + { + Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis); + Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint); + + Vector2 axisA = MathUtils.MultiplyT(ref xfA.R, -normal); + + indexB = -1; + indexA = _proxyA.GetSupport(axisA); + + Vector2 localPointA = _proxyA.Vertices[indexA]; + Vector2 pointA = MathUtils.Multiply(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 axisA = MathUtils.MultiplyT(ref xfA.R, _axis); + Vector2 axisB = MathUtils.MultiplyT(ref xfB.R, -_axis); + + Vector2 localPointA = _proxyA.Vertices[indexA]; + Vector2 localPointB = _proxyB.Vertices[indexB]; + + Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA); + Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB); + float separation = Vector2.Dot(pointB - pointA, _axis); + + return separation; + } + + case SeparationFunctionType.FaceA: + { + Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis); + Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint); + + Vector2 axisB = MathUtils.MultiplyT(ref xfB.R, -normal); + + Vector2 localPointB = _proxyB.Vertices[indexB]; + Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB); + + float separation = Vector2.Dot(pointB - pointA, normal); + return separation; + } + + case SeparationFunctionType.FaceB: + { + Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis); + Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint); + + Vector2 axisA = MathUtils.MultiplyT(ref xfA.R, -normal); + + Vector2 localPointA = _proxyA.Vertices[indexA]; + Vector2 pointA = MathUtils.Multiply(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. + + public static int TOICalls, TOIIters, TOIMaxIters; + public static int TOIRootIters, TOIMaxRootIters; + private static DistanceInput _distanceInput = new 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) + { + ++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. + SimplexCache cache; + _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; + 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); + } + + 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; + } + + ++rootIterCount; + ++TOIRootIters; + + if (rootIterCount == 50) + { + break; + } + } + + TOIMaxRootIters = Math.Max(TOIMaxRootIters, rootIterCount); + + ++pushBackIter; + + if (pushBackIter == Settings.MaxPolygonVertices) + { + break; + } + } + + ++iter; + ++TOIIters; + + if (done) + { + break; + } + + if (iter == k_maxIterations) + { + // Root finder got stuck. Semi-victory. + output.State = TOIOutputState.Failed; + output.T = t1; + break; + } + } + + TOIMaxIters = Math.Max(TOIMaxIters, iter); + } + } +} \ No newline at end of file diff --git a/axios/Common/ConvexHull/ChainHull.cs b/axios/Common/ConvexHull/ChainHull.cs new file mode 100644 index 0000000..ec33432 --- /dev/null +++ b/axios/Common/ConvexHull/ChainHull.cs @@ -0,0 +1,126 @@ +using System.Collections.Generic; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common.ConvexHull +{ + public static class ChainHull + { + //Andrew's monotone chain 2D convex hull algorithm. + //Copyright 2001, softSurfer (www.softsurfer.com) + + /// + /// Gets the convex hull. + /// + /// + /// http://www.softsurfer.com/Archive/algorithm_0109/algorithm_0109.htm + /// + /// + public static Vertices GetConvexHull(Vertices P) + { + P.Sort(new PointComparer()); + + Vector2[] H = new Vector2[P.Count]; + Vertices res = new Vertices(); + + int n = P.Count; + + int bot, 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 + int minmin = 0, minmax; + float xmin = P[0].X; + for (i = 1; i < n; i++) + if (P[i].X != xmin) break; + minmax = i - 1; + if (minmax == n - 1) + { + // degenerate case: all x-coords == xmin + H[++top] = P[minmin]; + if (P[minmax].Y != P[minmin].Y) // a nontrivial segment + H[++top] = P[minmax]; + H[++top] = P[minmin]; // add polygon endpoint + + for (int j = 0; j < top + 1; j++) + { + res.Add(H[j]); + } + + return res; + } + + top = res.Count - 1; + + // Get the indices of points with max x-coord and min|max y-coord + int maxmin, maxmax = n - 1; + float xmax = P[n - 1].X; + for (i = n - 2; i >= 0; i--) + if (P[i].X != xmax) break; + maxmin = i + 1; + + // Compute the lower hull on the stack H + H[++top] = P[minmin]; // push minmin point onto stack + i = minmax; + while (++i <= maxmin) + { + // the lower line joins P[minmin] with P[maxmin] + if (MathUtils.Area(P[minmin], P[maxmin], P[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], P[i]) > 0) + break; // P[i] is a new hull vertex + else + top--; // pop top point off stack + } + H[++top] = P[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] = P[maxmax]; // push maxmax point onto stack + bot = top; // the bottom point of the upper hull stack + i = maxmin; + while (--i >= minmax) + { + // the upper line joins P[maxmax] with P[minmax] + if (MathUtils.Area(P[maxmax], P[minmax], P[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], P[i]) > 0) + break; // P[i] is a new hull vertex + else + top--; // pop top point off stack + } + H[++top] = P[i]; // push P[i] onto stack + } + if (minmax != minmin) + H[++top] = P[minmin]; // push joining endpoint onto stack + + for (int j = 0; j < top + 1; j++) + { + res.Add(H[j]); + } + + return res; + } + + #region Nested type: PointComparer + + public 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); + } + } + + #endregion + } +} \ No newline at end of file diff --git a/axios/Common/ConvexHull/GiftWrap.cs b/axios/Common/ConvexHull/GiftWrap.cs new file mode 100644 index 0000000..7138040 --- /dev/null +++ b/axios/Common/ConvexHull/GiftWrap.cs @@ -0,0 +1,99 @@ +using System; + +namespace FarseerPhysics.Common.ConvexHull +{ + public static class GiftWrap + { + // From Eric Jordan's convex decomposition library (box2D rev 32) + + /// + /// Find the convex hull of a point cloud using "Gift-wrap" algorithm - start + /// with an extremal point, and walk around the outside edge by testing + /// angles. + /// + /// Runs in O(N*S) time where S is number of sides of resulting polygon. + /// Worst case: point cloud is all vertices of convex polygon: O(N^2). + /// There may be faster algorithms to do this, should you need one - + /// this is just the simplest. You can get O(N log N) expected time if you + /// try, I think, and O(N) if you restrict inputs to simple polygons. + /// Returns null if number of vertices passed is less than 3. + /// Results should be passed through convex decomposition afterwards + /// to ensure that each shape has few enough points to be used in Box2d. + /// + /// Warning: May be buggy with colinear points on hull. + /// + /// The vertices. + /// + public static Vertices GetConvexHull(Vertices vertices) + { + if (vertices.Count < 3) + return vertices; + + int[] edgeList = new int[vertices.Count]; + int numEdges = 0; + + float minY = float.MaxValue; + int minYIndex = vertices.Count; + for (int i = 0; i < vertices.Count; ++i) + { + if (vertices[i].Y < minY) + { + minY = vertices[i].Y; + minYIndex = i; + } + } + + int startIndex = minYIndex; + int winIndex = -1; + float dx = -1.0f; + float dy = 0.0f; + while (winIndex != minYIndex) + { + float maxDot = -2.0f; + float nrm; + + for (int i = 0; i < vertices.Count; ++i) + { + if (i == startIndex) + continue; + float newdx = vertices[i].X - vertices[startIndex].X; + float newdy = vertices[i].Y - vertices[startIndex].Y; + nrm = (float)Math.Sqrt(newdx * newdx + newdy * newdy); + nrm = (nrm == 0.0f) ? 1.0f : nrm; + newdx /= nrm; + newdy /= nrm; + + //Dot products act as proxy for angle + //without requiring inverse trig. + float newDot = newdx * dx + newdy * dy; + if (newDot > maxDot) + { + maxDot = newDot; + winIndex = i; + } + } + edgeList[numEdges++] = winIndex; + dx = vertices[winIndex].X - vertices[startIndex].X; + dy = vertices[winIndex].Y - vertices[startIndex].Y; + nrm = (float)Math.Sqrt(dx * dx + dy * dy); + nrm = (nrm == 0.0f) ? 1.0f : nrm; + dx /= nrm; + dy /= nrm; + startIndex = winIndex; + } + + Vertices returnVal = new Vertices(numEdges); + + for (int i = 0; i < numEdges; i++) + { + returnVal.Add(vertices[edgeList[i]]); + //Debug.WriteLine(string.Format("{0}, {1}", vertices[edgeList[i]].X, vertices[edgeList[i]].Y)); + } + + //Not sure if we need this + //returnVal.MergeParallelEdges(Settings.b2_angularSlop); + + return returnVal; + } + } +} \ No newline at end of file diff --git a/axios/Common/ConvexHull/Melkman.cs b/axios/Common/ConvexHull/Melkman.cs new file mode 100644 index 0000000..e56e8c3 --- /dev/null +++ b/axios/Common/ConvexHull/Melkman.cs @@ -0,0 +1,122 @@ +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common.ConvexHull +{ + public static class Melkman + { + //Melkman based convex hull algorithm contributed by Cowdozer + + /// + /// 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 + /// + /// A convex hull in counterclockwise winding order. + public static Vertices GetConvexHull(Vertices vertices) + { + //With less than 3 vertices, this is about the best we can do for a convex hull + 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 + int qfm1, qbm1; //qfm1 = second element, qbm1 = second last element + + //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]; + } + } + + qfm1 = qf == 0 ? deque.Length - 1 : qf - 1; //qfm1 = qf - 1; + qbm1 = qb == deque.Length - 1 ? 0 : qb + 1; //qbm1 = 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 + Vertices convexHull = new Vertices(vertices.Count + 1); + if (qb < qf) + for (int i = qb; i < qf; i++) + convexHull.Add(deque[i]); + else + { + 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/axios/Common/Decomposition/BayazitDecomposer.cs b/axios/Common/Decomposition/BayazitDecomposer.cs new file mode 100644 index 0000000..6303f51 --- /dev/null +++ b/axios/Common/Decomposition/BayazitDecomposer.cs @@ -0,0 +1,253 @@ +using System.Collections.Generic; +using FarseerPhysics.Common.PolygonManipulation; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common.Decomposition +{ + //From phed rev 36 + + /// + /// Convex decomposition algorithm created by Mark Bayazit (http://mnbayazit.com/) + /// For more information about this algorithm, see http://mnbayazit.com/406/bayazit + /// + public static class BayazitDecomposer + { + private static Vector2 At(int i, Vertices vertices) + { + int s = vertices.Count; + return vertices[i < 0 ? s - (-i % s) : i % s]; + } + + private static Vertices Copy(int i, int j, Vertices vertices) + { + Vertices p = new Vertices(); + while (j < i) j += vertices.Count; + //p.reserve(j - i + 1); + for (; i <= j; ++i) + { + p.Add(At(i, vertices)); + } + return p; + } + + /// + /// 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. + /// Precondition: Counter Clockwise polygon + /// + /// + /// + public static List ConvexPartition(Vertices vertices) + { + //We force it to CCW as it is a precondition in this algorithm. + vertices.ForceCounterClockWise(); + + List list = new List(); + float d, lowerDist, upperDist; + Vector2 p; + 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)) + { + lowerDist = upperDist = float.MaxValue; // std::numeric_limits::max(); + for (int j = 0; j < vertices.Count; ++j) + { + // if line intersects with an edge + 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 sp = ((lowerInt + upperInt) / 2); + + lowerPoly = Copy(i, upperIndex, vertices); + lowerPoly.Add(sp); + upperPoly = Copy(lowerIndex, i, vertices); + upperPoly.Add(sp); + } + 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(ConvexPartition(lowerPoly)); + list.AddRange(ConvexPartition(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(ConvexPartition(lowerPoly)); + list.AddRange(ConvexPartition(upperPoly)); + } + else + list.Add(vertices); + + //The polygons are not guaranteed to be without collinear points. We remove + //them to be sure. + for (int i = 0; i < list.Count; i++) + { + list[i] = SimplifyTools.CollinearSimplify(list[i], 0); + } + + //Remove empty vertice collections + for (int i = list.Count - 1; i >= 0; i--) + { + if (list[i].Count == 0) + list.RemoveAt(i); + } + + return list; + } + + 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; + } + + // precondition: ccw + 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/axios/Common/Decomposition/CDT/Delaunay/DelaunayTriangle.cs b/axios/Common/Decomposition/CDT/Delaunay/DelaunayTriangle.cs new file mode 100644 index 0000000..210461b --- /dev/null +++ b/axios/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 Poly2Tri.Triangulation.Delaunay.Sweep; +using Poly2Tri.Triangulation.Util; + +namespace Poly2Tri.Triangulation.Delaunay +{ + public 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 FixedArray3 Neighbors; + + /** Has this triangle been marked as an interior triangle? */ + + public 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/axios/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFront.cs b/axios/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFront.cs new file mode 100644 index 0000000..fdcc44a --- /dev/null +++ b/axios/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 Poly2Tri.Triangulation.Delaunay.Sweep +{ + /** + * @author Thomas Åhlen (thahlen@gmail.com) + */ + + public 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/axios/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFrontNode.cs b/axios/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFrontNode.cs new file mode 100644 index 0000000..1552909 --- /dev/null +++ b/axios/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 Poly2Tri.Triangulation.Delaunay.Sweep +{ + public 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/axios/Common/Decomposition/CDT/Delaunay/Sweep/DTSweep.cs b/axios/Common/Decomposition/CDT/Delaunay/Sweep/DTSweep.cs new file mode 100644 index 0000000..3d7a835 --- /dev/null +++ b/axios/Common/Decomposition/CDT/Delaunay/Sweep/DTSweep.cs @@ -0,0 +1,1132 @@ +/* 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; +using FarseerPhysics.Common.Decomposition.CDT; + +namespace Poly2Tri.Triangulation.Delaunay.Sweep +{ + public 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; + TriangulationPoint point; + AdvancingFrontNode node; + + for (int i = 1; i < points.Count; i++) + { + point = points[i]; + + 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) + { + AdvancingFrontNode n1, n2; + DelaunayTriangle t1, t2; + TriangulationPoint first, p1; + + n1 = tcx.aFront.Head.Next; + n2 = n1.Next; + first = n1.Point; + + 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 + first = tcx.aFront.Head.Point; + n2 = tcx.aFront.Tail.Prev; + t1 = n2.Triangle; + 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, newNode; + + node = tcx.LocateNode(point); + 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) + { + AdvancingFrontNode newNode; + DelaunayTriangle triangle; + + triangle = new DelaunayTriangle(point, node.Point, node.Next.Point); + triangle.MarkNeighbor(node.Triangle); + tcx.Triangles.Add(triangle); + + 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(String.Format("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; + } + } + } + + //TODO: Port note: There were some structural differences here. + private static bool IsEdgeSideOfTriangle(DelaunayTriangle triangle, TriangulationPoint ep, TriangulationPoint eq) + { + int index; + 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) + { + TriangulationPoint p1, p2; + + if (IsEdgeSideOfTriangle(triangle, ep, eq)) + { + return; + } + + 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; + } + + 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) + { + TriangulationPoint op, newP; + DelaunayTriangle ot; + bool inScanArea; + + ot = t.NeighborAcross(p); + 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"); + } + + 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 + { + 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; + TriangulationPoint op, newP; + bool inScanArea; + + ot = t.NeighborAcross(p); + 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"); + } + + 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 + { + 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) + { + AdvancingFrontNode node; + double angle; + + // Fill right holes + node = n.Next; + while (node.HasNext) + { + angle = HoleAngle(node); + if (angle > PI_div2 || angle < -PI_div2) + { + break; + } + Fill(tcx, node); + node = node.Next; + } + + // Fill left holes + node = n.Prev; + while (node.HasPrev) + { + 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); + } + } + } + + /// + /// 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) + { + int oi; + bool inside; + TriangulationPoint p, op; + DelaunayTriangle ot; + + // 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; + } + + ot = t.Neighbors[i]; + if (ot != null) + { + p = t.Points[i]; + op = ot.OppositePoint(t, p); + 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; + } + + inside = TriangulationUtil.SmartIncircle(p, + t.PointCCW(p), + t.PointCW(p), + op); + + if (inside) + { + bool notLegalized; + + // 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 + 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, n2, n3, n4; + n1 = t.NeighborCCW(p); + n2 = t.NeighborCW(p); + n3 = ot.NeighborCCW(op); + n4 = ot.NeighborCW(op); + + bool ce1, ce2, ce3, ce4; + ce1 = t.GetConstrainedEdgeCCW(p); + ce2 = t.GetConstrainedEdgeCW(p); + ce3 = ot.GetConstrainedEdgeCCW(op); + ce4 = ot.GetConstrainedEdgeCW(op); + + bool de1, de2, de3, de4; + de1 = t.GetDelaunayEdgeCCW(p); + de2 = t.GetDelaunayEdgeCW(p); + de3 = ot.GetDelaunayEdgeCCW(op); + 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/axios/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepConstraint.cs b/axios/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepConstraint.cs new file mode 100644 index 0000000..f1fa5c4 --- /dev/null +++ b/axios/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 Poly2Tri.Triangulation.Delaunay.Sweep +{ + public 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/axios/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepContext.cs b/axios/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepContext.cs new file mode 100644 index 0000000..c34db0c --- /dev/null +++ b/axios/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 Poly2Tri.Triangulation.Delaunay.Sweep +{ + /** + * + * @author Thomas Åhlén, thahlen@gmail.com + * + */ + + public 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/axios/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepPointComparator.cs b/axios/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepPointComparator.cs new file mode 100644 index 0000000..7985f8c --- /dev/null +++ b/axios/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 Poly2Tri.Triangulation.Delaunay.Sweep +{ + public 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/axios/Common/Decomposition/CDT/Delaunay/Sweep/PointOnEdgeException.cs b/axios/Common/Decomposition/CDT/Delaunay/Sweep/PointOnEdgeException.cs new file mode 100644 index 0000000..8ccd95f --- /dev/null +++ b/axios/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 Poly2Tri.Triangulation.Delaunay.Sweep +{ + public class PointOnEdgeException : NotImplementedException + { + public PointOnEdgeException(string message) + : base(message) + { + } + } +} \ No newline at end of file diff --git a/axios/Common/Decomposition/CDT/ITriangulatable.cs b/axios/Common/Decomposition/CDT/ITriangulatable.cs new file mode 100644 index 0000000..3f141d7 --- /dev/null +++ b/axios/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 Poly2Tri.Triangulation.Delaunay; + +namespace Poly2Tri.Triangulation +{ + public 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/axios/Common/Decomposition/CDT/Orientation.cs b/axios/Common/Decomposition/CDT/Orientation.cs new file mode 100644 index 0000000..0cf3100 --- /dev/null +++ b/axios/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 +{ + public enum Orientation + { + CW, + CCW, + Collinear + } +} \ No newline at end of file diff --git a/axios/Common/Decomposition/CDT/Polygon/Polygon.cs b/axios/Common/Decomposition/CDT/Polygon/Polygon.cs new file mode 100644 index 0000000..94932f9 --- /dev/null +++ b/axios/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 Poly2Tri.Triangulation.Delaunay; + +namespace Poly2Tri.Triangulation.Polygon +{ + public 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/axios/Common/Decomposition/CDT/Polygon/PolygonPoint.cs b/axios/Common/Decomposition/CDT/Polygon/PolygonPoint.cs new file mode 100644 index 0000000..b8d7117 --- /dev/null +++ b/axios/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 Poly2Tri.Triangulation.Polygon +{ + public 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/axios/Common/Decomposition/CDT/Polygon/PolygonSet.cs b/axios/Common/Decomposition/CDT/Polygon/PolygonSet.cs new file mode 100644 index 0000000..59cb9ee --- /dev/null +++ b/axios/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 Poly2Tri.Triangulation.Polygon +{ + public 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/axios/Common/Decomposition/CDT/Sets/ConstrainedPointSet.cs b/axios/Common/Decomposition/CDT/Sets/ConstrainedPointSet.cs new file mode 100644 index 0000000..73e6e67 --- /dev/null +++ b/axios/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 Poly2Tri.Triangulation.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 + */ + + public class ConstrainedPointSet : PointSet + { + private List _constrainedPointList = null; + + 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/axios/Common/Decomposition/CDT/Sets/PointSet.cs b/axios/Common/Decomposition/CDT/Sets/PointSet.cs new file mode 100644 index 0000000..00ea21c --- /dev/null +++ b/axios/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 Poly2Tri.Triangulation.Delaunay; + +namespace Poly2Tri.Triangulation.Sets +{ + public 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/axios/Common/Decomposition/CDT/TriangulationConstraint.cs b/axios/Common/Decomposition/CDT/TriangulationConstraint.cs new file mode 100644 index 0000000..16a81ec --- /dev/null +++ b/axios/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 Poly2Tri.Triangulation +{ + public class TriangulationConstraint + { + public TriangulationPoint P; + public TriangulationPoint Q; + } +} \ No newline at end of file diff --git a/axios/Common/Decomposition/CDT/TriangulationContext.cs b/axios/Common/Decomposition/CDT/TriangulationContext.cs new file mode 100644 index 0000000..f92f21c --- /dev/null +++ b/axios/Common/Decomposition/CDT/TriangulationContext.cs @@ -0,0 +1,87 @@ +/* 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 Poly2Tri.Triangulation.Delaunay; + +namespace Poly2Tri.Triangulation +{ + public abstract class TriangulationContext + { + public readonly List Points = new List(200); + public readonly List Triangles = new List(); + +#pragma warning disable 414 + private int _stepTime = -1; +#pragma warning restore 414 + + 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/axios/Common/Decomposition/CDT/TriangulationMode.cs b/axios/Common/Decomposition/CDT/TriangulationMode.cs new file mode 100644 index 0000000..b928401 --- /dev/null +++ b/axios/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 Poly2Tri.Triangulation +{ + public enum TriangulationMode + { + Unconstrained, + Constrained, + Polygon + } +} \ No newline at end of file diff --git a/axios/Common/Decomposition/CDT/TriangulationPoint.cs b/axios/Common/Decomposition/CDT/TriangulationPoint.cs new file mode 100644 index 0000000..afa5915 --- /dev/null +++ b/axios/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 Poly2Tri.Triangulation.Delaunay.Sweep; + +namespace Poly2Tri.Triangulation +{ + public 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/axios/Common/Decomposition/CDT/TriangulationUtil.cs b/axios/Common/Decomposition/CDT/TriangulationUtil.cs new file mode 100644 index 0000000..01bbed1 --- /dev/null +++ b/axios/Common/Decomposition/CDT/TriangulationUtil.cs @@ -0,0 +1,160 @@ +/* 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 FarseerPhysics.Common.Decomposition.CDT; + +namespace Poly2Tri.Triangulation +{ + /** + * @author Thomas Åhlén, thahlen@gmail.com + */ + + public 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; + } + + /// 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/axios/Common/Decomposition/CDT/Util/FixedArray3.cs b/axios/Common/Decomposition/CDT/Util/FixedArray3.cs new file mode 100644 index 0000000..c5b06df --- /dev/null +++ b/axios/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 Poly2Tri.Triangulation.Util +{ + public 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/axios/Common/Decomposition/CDT/Util/FixedBitArray3.cs b/axios/Common/Decomposition/CDT/Util/FixedBitArray3.cs new file mode 100644 index 0000000..f38e4ee --- /dev/null +++ b/axios/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 Poly2Tri.Triangulation.Util +{ + public 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/axios/Common/Decomposition/CDT/Util/PointGenerator.cs b/axios/Common/Decomposition/CDT/Util/PointGenerator.cs new file mode 100644 index 0000000..52b43af --- /dev/null +++ b/axios/Common/Decomposition/CDT/Util/PointGenerator.cs @@ -0,0 +1,38 @@ +using System; +using System.Collections.Generic; + +namespace Poly2Tri.Triangulation.Util +{ + public 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/axios/Common/Decomposition/CDT/Util/PolygonGenerator.cs b/axios/Common/Decomposition/CDT/Util/PolygonGenerator.cs new file mode 100644 index 0000000..4668659 --- /dev/null +++ b/axios/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 Poly2Tri.Triangulation.Polygon; + +namespace Poly2Tri.Triangulation.Util +{ + public 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/axios/Common/Decomposition/CDTDecomposer.cs b/axios/Common/Decomposition/CDTDecomposer.cs new file mode 100644 index 0000000..32ead06 --- /dev/null +++ b/axios/Common/Decomposition/CDTDecomposer.cs @@ -0,0 +1,110 @@ +/* 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 Microsoft.Xna.Framework; +using Poly2Tri.Triangulation; +using Poly2Tri.Triangulation.Delaunay; +using Poly2Tri.Triangulation.Delaunay.Sweep; +using Poly2Tri.Triangulation.Polygon; + +using System.Linq; + +namespace FarseerPhysics.Common.Decomposition +{ + public static class CDTDecomposer + { + public static List ConvexPartition(Vertices vertices) + { + Polygon poly = new Polygon(); + + foreach (Vector2 vertex in vertices) + { + poly.Points.Add(new TriangulationPoint(vertex.X, vertex.Y)); + } + + 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; + } + + public static List ConvexPartition(DetectedVertices vertices) + { + Polygon poly = new Polygon(); + foreach (var vertex in vertices) + poly.Points.Add(new TriangulationPoint(vertex.X, vertex.Y)); + + if (vertices.Holes != null) + { + foreach (var holeVertices in vertices.Holes) + { + Polygon hole = new Polygon(); + foreach (var 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/axios/Common/Decomposition/EarclipDecomposer.cs b/axios/Common/Decomposition/EarclipDecomposer.cs new file mode 100644 index 0000000..6dffbb5 --- /dev/null +++ b/axios/Common/Decomposition/EarclipDecomposer.cs @@ -0,0 +1,691 @@ +/* + * 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 FarseerPhysics.Common.PolygonManipulation; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common.Decomposition +{ + /// + /// Ported from jBox2D. Original author: ewjordan + /// Triangulates a polygon using simple ear-clipping algorithm. + /// + /// Only works on simple polygons. + /// + /// Triangles may be degenerate, especially if you have identical points + /// in the input to the algorithm. Check this before you use them. + /// + public static class EarclipDecomposer + { + //box2D rev 32 - for details, see http://www.box2d.org/forum/viewtopic.php?f=4&t=83&start=50 + + private const float Tol = .001f; + + /// + /// Decomposes a non-convex polygon into a number of convex polygons, up + /// to maxPolys (remaining pieces are thrown out). + /// + /// Each resulting polygon will have no more than Settings.MaxPolygonVertices + /// vertices. + /// + /// Warning: Only works on simple polygons + /// + /// The vertices. + /// + public static List ConvexPartition(Vertices vertices) + { + return ConvexPartition(vertices, int.MaxValue, 0); + } + + /// + /// Decomposes a non-convex polygon into a number of convex polygons, up + /// to maxPolys (remaining pieces are thrown out). + /// Each resulting polygon will have no more than Settings.MaxPolygonVertices + /// vertices. + /// Warning: Only works on simple polygons + /// + /// The vertices. + /// The maximum number of polygons. + /// The tolerance. + /// + public static List ConvexPartition(Vertices vertices, int maxPolys, float tolerance) + { + if (vertices.Count < 3) + return new List { vertices }; + /* + if (vertices.IsConvex() && vertices.Count <= Settings.MaxPolygonVertices) + { + if (vertices.IsCounterClockWise()) + { + Vertices tempP = new Vertices(vertices); + tempP.Reverse(); + tempP = SimplifyTools.CollinearSimplify(tempP); + tempP.ForceCounterClockWise(); + return new List { tempP }; + } + vertices = SimplifyTools.CollinearSimplify(vertices); + vertices.ForceCounterClockWise(); + return new List { vertices }; + } + */ + List triangulated; + + if (vertices.IsCounterClockWise()) + { + Vertices tempP = new Vertices(vertices); + tempP.Reverse(); + triangulated = TriangulatePolygon(tempP); + } + else + { + triangulated = TriangulatePolygon(vertices); + } + if (triangulated.Count < 1) + { + //Still no luck? Oh well... + throw new Exception("Can't triangulate your polygon."); + } + + List polygonizedTriangles = PolygonizeTriangles(triangulated, maxPolys, tolerance); + + //The polygonized triangles are not guaranteed to be without collinear points. We remove + //them to be sure. + for (int i = 0; i < polygonizedTriangles.Count; i++) + { + polygonizedTriangles[i] = SimplifyTools.CollinearSimplify(polygonizedTriangles[i], 0); + } + + //Remove empty vertice collections + for (int i = polygonizedTriangles.Count - 1; i >= 0; i--) + { + if (polygonizedTriangles[i].Count == 0) + polygonizedTriangles.RemoveAt(i); + } + + return polygonizedTriangles; + } + + /// + /// Turns a list of triangles into a list of convex polygons. Very simple + /// method - start with a seed triangle, keep adding triangles to it until + /// you can't add any more without making the polygon non-convex. + /// + /// Returns an integer telling how many polygons were created. Will fill + /// polys array up to polysLength entries, which may be smaller or larger + /// than the return value. + /// + /// Takes O(N///P) where P is the number of resultant polygons, N is triangle + /// count. + /// + /// The final polygon list will not necessarily be minimal, though in + /// practice it works fairly well. + /// + /// The triangulated. + ///The maximun number of polygons + ///The tolerance + /// + public static List PolygonizeTriangles(List triangulated, int maxPolys, float tolerance) + { + List polys = new List(50); + + int polyIndex = 0; + + if (triangulated.Count <= 0) + { + //return empty polygon list + return polys; + } + + bool[] covered = new bool[triangulated.Count]; + for (int i = 0; i < triangulated.Count; ++i) + { + covered[i] = false; + + //Check here for degenerate triangles + if (((triangulated[i].X[0] == triangulated[i].X[1]) && (triangulated[i].Y[0] == triangulated[i].Y[1])) + || + ((triangulated[i].X[1] == triangulated[i].X[2]) && (triangulated[i].Y[1] == triangulated[i].Y[2])) + || + ((triangulated[i].X[0] == triangulated[i].X[2]) && (triangulated[i].Y[0] == triangulated[i].Y[2]))) + { + covered[i] = true; + } + } + + bool notDone = true; + while (notDone) + { + int currTri = -1; + for (int i = 0; i < triangulated.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(new Vector2(triangulated[currTri].X[i], triangulated[currTri].Y[i])); + } + + covered[currTri] = true; + int index = 0; + for (int i = 0; i < 2 * triangulated.Count; ++i, ++index) + { + while (index >= triangulated.Count) index -= triangulated.Count; + if (covered[index]) + { + continue; + } + Vertices newP = AddTriangle(triangulated[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 + // printf("Skipping corrupt poly\n"); + } + if (poly.Count >= 3) + polyIndex++; //Must be outside (polyIndex < polysLength) test + } + } + + return polys; + } + + /// + /// 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. + /// + /// Warning: Only works on simple polygons. + /// + /// + public static List TriangulatePolygon(Vertices vertices) + { + List results = new List(); + if (vertices.Count < 3) + return new List(); + + //Recurse and split on pinch points + Vertices pA, pB; + Vertices pin = new Vertices(vertices); + if (ResolvePinchPoint(pin, out pA, out pB)) + { + List mergeA = TriangulatePolygon(pA); + List mergeB = TriangulatePolygon(pB); + + 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 Triangle(mergeA[i])); + } + for (int i = 0; i < mergeB.Count; ++i) + { + results.Add(new Triangle(mergeB[i])); + } + + return results; + } + + Triangle[] buffer = new Triangle[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(new Triangle(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 Triangle(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) + { + 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) < Tol && Math.Abs(pin[i].Y - pin[j].Y) < Tol && 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; + } + + private static Vertices AddTriangle(Triangle 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.X[0] == vertices[i].X && t.Y[0] == vertices[i].Y) + { + if (firstP == -1) + { + firstP = i; + firstT = 0; + } + else + { + secondP = i; + secondT = 0; + } + } + else if (t.X[1] == vertices[i].X && t.Y[1] == vertices[i].Y) + { + if (firstP == -1) + { + firstP = i; + firstT = 1; + } + else + { + secondP = i; + secondT = 1; + } + } + else if (t.X[2] == vertices[i].X && t.Y[2] == 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(new Vector2(t.X[tipT], t.Y[tipT])); + } + + return result; + } + + /// + /// Checks if vertex i is the tip of an ear in polygon defined by xv[] and + /// yv[]. + /// + /// Assumes clockwise orientation of polygon...ick + /// + /// The i. + /// The xv. + /// The yv. + /// Length of the xv. + /// + /// 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; + } + } + + public class Triangle + { + public float[] X; + public float[] Y; + + //Constructor automatically fixes orientation to ccw + public Triangle(float x1, float y1, float x2, float y2, float x3, float y3) + { + X = new float[3]; + Y = new float[3]; + float dx1 = x2 - x1; + float dx2 = x3 - x1; + float dy1 = y2 - y1; + float dy2 = y3 - y1; + float cross = dx1 * dy2 - dx2 * dy1; + bool ccw = (cross > 0); + if (ccw) + { + X[0] = x1; + X[1] = x2; + X[2] = x3; + Y[0] = y1; + Y[1] = y2; + Y[2] = y3; + } + else + { + X[0] = x1; + X[1] = x3; + X[2] = x2; + Y[0] = y1; + Y[1] = y3; + Y[2] = y2; + } + } + + public Triangle(Triangle t) + { + X = new float[3]; + Y = new float[3]; + + X[0] = t.X[0]; + X[1] = t.X[1]; + X[2] = t.X[2]; + Y[0] = t.Y[0]; + Y[1] = t.Y[1]; + Y[2] = t.Y[2]; + } + + public bool IsInside(float x, float y) + { + if (x < X[0] && x < X[1] && x < X[2]) return false; + if (x > X[0] && x > X[1] && x > X[2]) return false; + if (y < Y[0] && y < Y[1] && y < Y[2]) return false; + if (y > Y[0] && y > Y[1] && y > Y[2]) return false; + + float vx2 = x - X[0]; + float vy2 = y - Y[0]; + float vx1 = X[1] - X[0]; + float vy1 = Y[1] - Y[0]; + float vx0 = X[2] - X[0]; + float vy0 = Y[2] - Y[0]; + + 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/axios/Common/Decomposition/FlipcodeDecomposer.cs b/axios/Common/Decomposition/FlipcodeDecomposer.cs new file mode 100644 index 0000000..4bb2bbb --- /dev/null +++ b/axios/Common/Decomposition/FlipcodeDecomposer.cs @@ -0,0 +1,160 @@ +using System.Collections.Generic; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common.Decomposition +{ + // Original code can be found here: http://www.flipcode.com/archives/Efficient_Polygon_Triangulation.shtml + + /// + /// Triangulates a polygon into triangles. + /// Doesn't handle holes. + /// + public static class FlipcodeDecomposer + { + private static Vector2 _tmpA; + private static Vector2 _tmpB; + private static Vector2 _tmpC; + + /// + /// 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; + } + + /// + /// Decompose the polygon into triangles + /// + /// The list of points describing the polygon + /// + public static List ConvexPartition(Vertices contour) + { + int n = contour.Count; + if (n < 3) + return new List(); + + int[] V = new int[n]; + + // We want a counter-clockwise polygon in V + if (contour.IsCounterClockWise()) + { + for (int v = 0; v < n; v++) + V[v] = v; + } + else + { + for (int v = 0; v < n; v++) + V[v] = (n - 1) - v; + } + + int nv = n; + + // 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 = contour[V[u]]; + _tmpB = contour[V[v]]; + _tmpC = contour[V[w]]; + + if (Snip(contour, u, v, w, nv, V)) + { + 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++) + { + V[s] = V[t]; + } + nv--; + + // Reset error detection counter + count = 2 * nv; + } + } + + return result; + } + } +} \ No newline at end of file diff --git a/axios/Common/Decomposition/SeidelDecomposer.cs b/axios/Common/Decomposition/SeidelDecomposer.cs new file mode 100644 index 0000000..93273f1 --- /dev/null +++ b/axios/Common/Decomposition/SeidelDecomposer.cs @@ -0,0 +1,1057 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common.Decomposition +{ + //From the Poly2Tri project by Mason Green: http://code.google.com/p/poly2tri/source/browse?repo=archive#hg/scala/src/org/poly2tri/seidel + + /// + /// Convex decomposition algorithm based on 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 + /// + public static class SeidelDecomposer + { + /// + /// Decompose the polygon into several smaller non-concave polygon. + /// + /// The polygon to decompose. + /// The sheer to use. If you get bad results, try using a higher value. The default value is 0.001 + /// A list of triangles + public static List ConvexPartition(Vertices vertices, float sheer) + { + 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 verts = new Vertices(triangle.Count); + + foreach (Point point in triangle) + { + verts.Add(new Vector2(point.X, point.Y)); + } + + list.Add(verts); + } + + return list; + } + + /// + /// Decompose the polygon into several smaller non-concave polygon. + /// + /// The polygon to decompose. + /// The sheer to use. If you get bad results, try using a higher value. The default value is 0.001 + /// A list of trapezoids + public static List ConvexPartitionTrapezoid(Vertices vertices, float sheer) + { + 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.Vertices(); + foreach (Point point in points) + { + verts.Add(new Vector2(point.X, point.Y)); + } + + list.Add(verts); + } + + return list; + } + } + + internal class MonotoneMountain + { + private const float PiSlop = 3.1f; + public List> Triangles; + private HashSet _convexPoints; + private Point _head; + + // Monotone mountain points + private List _monoPoly; + + // Triangles that constitute the mountain + + // Used to track which side of the line we are on + private bool _positive; + private int _size; + private Point _tail; + + // Almost Pi! + + 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.0) + 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; + } + } + + // 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); + } + } + + // 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); + } + } + + 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; + } + } + + // See "Computational Geometry", 3rd edition, by Mark de Berg et al, Chapter 6.2 + internal class TrapezoidalMap + { + // Trapezoid container + public HashSet Map; + + // AABB margin + + // Bottom segment that spans multiple trapezoids + private Edge _bCross; + + // Top segment that spans multiple trapezoids + private Edge _cross; + 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); + } + } + + 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; + } + } + + internal class Edge + { + // Pointers used for building trapezoidal map + public Trapezoid Above; + public float B; + public Trapezoid Below; + + // Equation of a line: y = m*x + b + // Slope of the line (m) + + // Montone mountain points + public HashSet MPoints; + public Point P; + public Point Q; + public float Slope; + + // Y intercept + + 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); + } + } + + 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 Vertices() + { + 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); + } + } + } + + 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) + // Move to the right in the graph + return RightChild.Locate(edge); + // Move to the left in the graph + return LeftChild.Locate(edge); + } + } + + 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)) + // Move down the graph + return RightChild.Locate(edge); + + if (_edge.IsBelow(edge.P)) + // Move up the graph + return LeftChild.Locate(edge); + + // s and segment share the same endpoint, p + if (edge.Slope < _edge.Slope) + // Move down the graph + return RightChild.Locate(edge); + + // Move up the graph + return LeftChild.Locate(edge); + } + } + + 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/axios/Common/FixedArray.cs b/axios/Common/FixedArray.cs new file mode 100644 index 0000000..e294350 --- /dev/null +++ b/axios/Common/FixedArray.cs @@ -0,0 +1,227 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and 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/axios/Common/HashSet.cs b/axios/Common/HashSet.cs new file mode 100644 index 0000000..d853772 --- /dev/null +++ b/axios/Common/HashSet.cs @@ -0,0 +1,81 @@ + +#if WINDOWS_PHONE || XBOX + +//TODO: FIX + +using System; +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(); + } + + // Methods + +#region ICollection Members + + public void Add(T item) + { + // We don't care for the value in dictionary, Keys matter. + _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) + { + throw new NotImplementedException(); + } + + 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/axios/Common/LineTools.cs b/axios/Common/LineTools.cs new file mode 100644 index 0000000..4c72c2a --- /dev/null +++ b/axios/Common/LineTools.cs @@ -0,0 +1,308 @@ +using System; +using System.Collections.Generic; +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 DistanceBetweenPointAndPoint(ref Vector2 point1, ref Vector2 point2) + { + Vector2 v; + Vector2.Subtract(ref point1, ref point2, out v); + return v.Length(); + } + + public static float DistanceBetweenPointAndLineSegment(ref Vector2 point, ref Vector2 lineEndPoint1, + ref Vector2 lineEndPoint2) + { + Vector2 v = Vector2.Subtract(lineEndPoint2, lineEndPoint1); + Vector2 w = Vector2.Subtract(point, lineEndPoint1); + + float c1 = Vector2.Dot(w, v); + if (c1 <= 0) return DistanceBetweenPointAndPoint(ref point, ref lineEndPoint1); + + float c2 = Vector2.Dot(v, v); + if (c2 <= c1) return DistanceBetweenPointAndPoint(ref point, ref lineEndPoint2); + + float b = c1 / c2; + Vector2 pointOnLine = Vector2.Add(lineEndPoint1, Vector2.Multiply(v, b)); + return DistanceBetweenPointAndPoint(ref point, ref 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(Vector2 a0, Vector2 a1, Vector2 b0, 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 + /// An list of intersection points. Any intersection points + /// found will be added to this list. + public static void LineSegmentVerticesIntersect(ref Vector2 point1, ref Vector2 point2, Vertices vertices, + ref List intersectionPoints) + { + 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); + } + } + } + + /// + /// 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. + /// An list of intersection points. Any intersection points found will be added to this list. + public static void LineSegmentAABBIntersect(ref Vector2 point1, ref Vector2 point2, AABB aabb, + ref List intersectionPoints) + { + LineSegmentVerticesIntersect(ref point1, ref point2, aabb.Vertices, ref intersectionPoints); + } + } +} \ No newline at end of file diff --git a/axios/Common/Math.cs b/axios/Common/Math.cs new file mode 100644 index 0000000..8e2274f --- /dev/null +++ b/axios/Common/Math.cs @@ -0,0 +1,638 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not 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(Vector2 a, Vector2 b) + { + return 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 Multiply(ref Mat22 A, Vector2 v) + { + return Multiply(ref A, ref v); + } + + public static Vector2 Multiply(ref Mat22 A, ref Vector2 v) + { + return new Vector2(A.Col1.X * v.X + A.Col2.X * v.Y, A.Col1.Y * v.X + A.Col2.Y * v.Y); + } + + public static Vector2 MultiplyT(ref Mat22 A, Vector2 v) + { + return MultiplyT(ref A, ref v); + } + + public static Vector2 MultiplyT(ref Mat22 A, ref Vector2 v) + { + return new Vector2(v.X * A.Col1.X + v.Y * A.Col1.Y, v.X * A.Col2.X + v.Y * A.Col2.Y); + } + + public static Vector2 Multiply(ref Transform T, Vector2 v) + { + return Multiply(ref T, ref v); + } + + public static Vector2 Multiply(ref Transform T, ref Vector2 v) + { + return new Vector2(T.Position.X + T.R.Col1.X * v.X + T.R.Col2.X * v.Y, + T.Position.Y + T.R.Col1.Y * v.X + T.R.Col2.Y * v.Y); + } + + public static Vector2 MultiplyT(ref Transform T, Vector2 v) + { + return MultiplyT(ref T, ref v); + } + + public static Vector2 MultiplyT(ref Transform T, ref Vector2 v) + { + Vector2 tmp = Vector2.Zero; + tmp.X = v.X - T.Position.X; + tmp.Y = v.Y - T.Position.Y; + return MultiplyT(ref T.R, ref tmp); + } + + // A^T * B + public static void MultiplyT(ref Mat22 A, ref Mat22 B, out Mat22 C) + { + C = new Mat22(); + C.Col1.X = A.Col1.X * B.Col1.X + A.Col1.Y * B.Col1.Y; + C.Col1.Y = A.Col2.X * B.Col1.X + A.Col2.Y * B.Col1.Y; + C.Col2.X = A.Col1.X * B.Col2.X + A.Col1.Y * B.Col2.Y; + C.Col2.Y = A.Col2.X * B.Col2.X + A.Col2.Y * B.Col2.Y; + } + + // v2 = A.R' * (B.R * v1 + B.p - A.p) = (A.R' * B.R) * v1 + (B.p - A.p) + public static void MultiplyT(ref Transform A, ref Transform B, out Transform C) + { + C = new Transform(); + MultiplyT(ref A.R, ref B.R, out C.R); + C.Position.X = B.Position.X - A.Position.X; + C.Position.Y = B.Position.Y - A.Position.Y; + } + + public static void Swap(ref T a, ref T b) + { + T tmp = a; + a = b; + b = tmp; + } + + /// + /// 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); + } + + 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 + /// + public static bool Collinear(ref Vector2 a, ref Vector2 b, ref Vector2 c) + { + return Collinear(ref a, ref b, ref c, 0); + } + + public static bool Collinear(ref Vector2 a, ref Vector2 b, ref Vector2 c, float tolerance) + { + 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 + } + + /// + /// A 2-by-2 matrix. Stored in column-major order. + /// + public struct Mat22 + { + public Vector2 Col1, Col2; + + /// + /// Construct this matrix using columns. + /// + /// The c1. + /// The c2. + public Mat22(Vector2 c1, Vector2 c2) + { + Col1 = c1; + Col2 = c2; + } + + /// + /// Construct this matrix using scalars. + /// + /// The a11. + /// The a12. + /// The a21. + /// The a22. + public Mat22(float a11, float a12, float a21, float a22) + { + Col1 = new Vector2(a11, a21); + Col2 = new Vector2(a12, a22); + } + + /// + /// Construct this matrix using an angle. This matrix becomes + /// an orthonormal rotation matrix. + /// + /// The angle. + public Mat22(float angle) + { + // TODO_ERIN compute sin+cos together. + float c = (float)Math.Cos(angle), s = (float)Math.Sin(angle); + Col1 = new Vector2(c, s); + Col2 = new Vector2(-s, c); + } + + /// + /// Extract the angle from this matrix (assumed to be + /// a rotation matrix). + /// + /// + public float Angle + { + get { return (float)Math.Atan2(Col1.Y, Col1.X); } + } + + public Mat22 Inverse + { + get + { + float a = Col1.X, b = Col2.X, c = Col1.Y, d = Col2.Y; + float det = a * d - b * c; + if (det != 0.0f) + { + det = 1.0f / det; + } + + Mat22 result = new Mat22(); + result.Col1.X = det * d; + result.Col1.Y = -det * c; + + result.Col2.X = -det * b; + result.Col2.Y = det * a; + + return result; + } + } + + /// + /// Initialize this matrix using columns. + /// + /// The c1. + /// The c2. + public void Set(Vector2 c1, Vector2 c2) + { + Col1 = c1; + Col2 = c2; + } + + /// + /// Initialize this matrix using an angle. This matrix becomes + /// an orthonormal rotation matrix. + /// + /// The angle. + public void Set(float angle) + { + float c = (float)Math.Cos(angle), s = (float)Math.Sin(angle); + Col1.X = c; + Col2.X = -s; + Col1.Y = s; + Col2.Y = c; + } + + /// + /// Set this to the identity matrix. + /// + public void SetIdentity() + { + Col1.X = 1.0f; + Col2.X = 0.0f; + Col1.Y = 0.0f; + Col2.Y = 1.0f; + } + + /// + /// Set this matrix to all zeros. + /// + public void SetZero() + { + Col1.X = 0.0f; + Col2.X = 0.0f; + Col1.Y = 0.0f; + Col2.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 = Col1.X, a12 = Col2.X, a21 = Col1.Y, a22 = Col2.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.Col1 = A.Col1 + B.Col1; + R.Col2 = A.Col2 + B.Col2; + } + } + + /// + /// A 3-by-3 matrix. Stored in column-major order. + /// + public struct Mat33 + { + public Vector3 Col1, Col2, Col3; + + /// + /// Construct this matrix using columns. + /// + /// The c1. + /// The c2. + /// The c3. + public Mat33(Vector3 c1, Vector3 c2, Vector3 c3) + { + Col1 = c1; + Col2 = c2; + Col3 = c3; + } + + /// + /// Set this matrix to all zeros. + /// + public void SetZero() + { + Col1 = Vector3.Zero; + Col2 = Vector3.Zero; + Col3 = 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(Col1, Vector3.Cross(Col2, Col3)); + if (det != 0.0f) + { + det = 1.0f / det; + } + + return new Vector3(det * Vector3.Dot(b, Vector3.Cross(Col2, Col3)), + det * Vector3.Dot(Col1, Vector3.Cross(b, Col3)), + det * Vector3.Dot(Col1, Vector3.Cross(Col2, 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 = Col1.X, a12 = Col2.X, a21 = Col1.Y, a22 = Col2.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)); + } + } + + /// + /// A transform contains translation and rotation. It is used to represent + /// the position and orientation of rigid frames. + /// + public struct Transform + { + public Vector2 Position; + public Mat22 R; + + /// + /// Initialize using a position vector and a rotation matrix. + /// + /// The position. + /// The r. + public Transform(ref Vector2 position, ref Mat22 r) + { + Position = position; + R = r; + } + + /// + /// Calculate the angle that the rotation matrix represents. + /// + /// + public float Angle + { + get { return (float)Math.Atan2(R.Col1.Y, R.Col1.X); } + } + + /// + /// Set this to the identity transform. + /// + public void SetIdentity() + { + Position = Vector2.Zero; + R.SetIdentity(); + } + + /// + /// Set this based on the position and angle. + /// + /// The position. + /// The angle. + public void Set(Vector2 position, float angle) + { + Position = position; + R.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 xf, float beta) + { + xf = new Transform(); + xf.Position.X = (1.0f - beta) * C0.X + beta * C.X; + xf.Position.Y = (1.0f - beta) * C0.Y + beta * C.Y; + float angle = (1.0f - beta) * A0 + beta * A; + xf.R.Set(angle); + + // Shift to origin + xf.Position -= MathUtils.Multiply(ref xf.R, ref 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.X = (1.0f - beta) * C0.X + beta * C.X; + C0.Y = (1.0f - beta) * C0.Y + beta * C.Y; + A0 = (1.0f - beta) * A0 + beta * A; + 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/axios/Common/Path.cs b/axios/Common/Path.cs new file mode 100644 index 0000000..e81e2c6 --- /dev/null +++ b/axios/Common/Path.cs @@ -0,0 +1,341 @@ +using System; +using System.Collections.Generic; +using System.Text; +using System.Xml.Serialization; +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. + /// + [XmlRoot("Path")] + public class Path + { + /// + /// All the points that makes up the curve + /// + [XmlElement("ControlPoints")] + 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. + [XmlElement("Closed")] + 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/axios/Common/PathManager.cs b/axios/Common/PathManager.cs new file mode 100644 index 0000000..c245f57 --- /dev/null +++ b/axios/Common/PathManager.cs @@ -0,0 +1,240 @@ +using System; +using System.Collections.Generic; +using FarseerPhysics.Collision.Shapes; +using FarseerPhysics.Common; +using FarseerPhysics.Common.Decomposition; +using FarseerPhysics.Dynamics; +using FarseerPhysics.Dynamics.Joints; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Factories +{ + /// + /// 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) + { + LoopShape loop = new LoopShape(verts); + body.CreateFixture(loop); + } + 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 = EarclipDecomposer.ConvexPartition(new Vertices(verts)); + //List decomposedVerts = BayazitDecomposer.ConvexPartition(new Vertices(verts)); + + 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) + { + 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; + + foreach (Shape shape in shapes) + { + b.CreateFixture(shape, userData); + } + + bodyList.Add(b); + } + + return bodyList; + } + + public static List EvenlyDistributeShapesAlongPath(World world, Path path, IEnumerable shapes, + BodyType type, int copies) + { + return EvenlyDistributeShapesAlongPath(world, path, shapes, type, copies, null); + } + + + /// + /// 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); + } + + //TODO: Comment better + /// + /// Moves the body on the 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; + } + + /// + /// 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]. + /// Minimum length of the slider joint. + /// Maximum length of the slider joint. + /// + public static List AttachBodiesWithSliderJoint(World world, List bodies, Vector2 localAnchorA, + Vector2 localAnchorB, bool connectFirstAndLast, + bool collideConnected, float minLength, + float maxLength) + { + List joints = new List(bodies.Count + 1); + + for (int i = 1; i < bodies.Count; i++) + { + SliderJoint joint = new SliderJoint(bodies[i], bodies[i - 1], localAnchorA, localAnchorB, minLength, + maxLength); + joint.CollideConnected = collideConnected; + world.AddJoint(joint); + joints.Add(joint); + } + + if (connectFirstAndLast) + { + SliderJoint lastjoint = new SliderJoint(bodies[0], bodies[bodies.Count - 1], localAnchorA, localAnchorB, + minLength, maxLength); + lastjoint.CollideConnected = collideConnected; + world.AddJoint(lastjoint); + joints.Add(lastjoint); + } + + return joints; + } + } +} \ No newline at end of file diff --git a/axios/Common/PhysicsLogic/Explosion.cs b/axios/Common/PhysicsLogic/Explosion.cs new file mode 100644 index 0000000..5bbc882 --- /dev/null +++ b/axios/Common/PhysicsLogic/Explosion.cs @@ -0,0 +1,464 @@ +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 +{ + 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 + */ + + /// + /// This is an explosive... it explodes. + /// + /// + /// 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 + /// + public sealed class Explosion : 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 Dictionary> _exploded; + private RayDataComparer _rdc; + + public Explosion(World world) + : base(world, PhysicsLogicType.Explosion) + { + _exploded = new Dictionary>(); + _rdc = new RayDataComparer(); + _data = new List(); + } + + /// + /// This makes the explosive explode + /// + /// + /// 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 dictionnary containing all the "exploded" fixtures + /// with a list of the applied impulses + /// + public Dictionary> Activate(Vector2 pos, float radius, float maxForce) + { + _exploded.Clear(); + + 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; + else + containedShapes[containedShapeCount++] = fixture; + } + else + { + shapes[shapeCount++] = fixture; + } + + // Continue the query. + return true; + }, ref aabb); + + if (exit) + { + return _exploded; + } + + // 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) + throw new ArgumentException("OMG!"); + // 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 shape = 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; + + if (body.UserData != null) + { + int index = (int)body.UserData; + if (index == 0) + { + // filter + return -1.0f; + } + } + + hitClosest = true; + shape = f; + return fr; + }, p1, p2); + + //draws radius points + if ((hitClosest) && (shape.Body.BodyType == BodyType.Dynamic)) + { + if ((_data.Count() > 0) && (_data.Last().Body == shape.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 = shape.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 + Vector2 val = Vector2.Zero; + List vectorList; + if (_exploded.TryGetValue(f, out vectorList)) + { + val.X += Math.Abs(vectImp.X); + val.Y += Math.Abs(vectImp.Y); + + vectorList.Add(val); + } + else + { + vectorList = new List(); + val.X = Math.Abs(vectImp.X); + val.Y = Math.Abs(vectImp.Y); + + vectorList.Add(val); + _exploded.Add(f, vectorList); + } + + 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); + + List vectorList = new List(); + vectorList.Add(vectImp); + + fix.Body.ApplyLinearImpulse(ref vectImp, ref hitPoint); + + if (!_exploded.ContainsKey(fix)) + _exploded.Add(fix, vectorList); + } + + return _exploded; + } + } +} \ No newline at end of file diff --git a/axios/Common/PhysicsLogic/PhysicsLogic.cs b/axios/Common/PhysicsLogic/PhysicsLogic.cs new file mode 100644 index 0000000..3a4e81b --- /dev/null +++ b/axios/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/axios/Common/PolygonManipulation/CuttingTools.cs b/axios/Common/PolygonManipulation/CuttingTools.cs new file mode 100644 index 0000000..432b49f --- /dev/null +++ b/axios/Common/PolygonManipulation/CuttingTools.cs @@ -0,0 +1,246 @@ +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 size of the split. Think of this as the laser-width + /// The first collection of vertexes + /// The second collection of vertexes + public static void SplitShape(Fixture fixture, Vector2 entryPoint, Vector2 exitPoint, float splitSize, + 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; + + if (shape == null) + { + first = new Vertices(); + second = new Vertices(); + return; + } + + 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(); + + newPolygon[n][cutAdded[n]] += splitSize * 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(); + + newPolygon[n][cutAdded[n] + 1] += splitSize * 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. + /// The thickness of the cut + public static void Cut(World world, Vector2 start, Vector2 end, float thickness) + { + 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; + + //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; + + for (int i = 0; i < fixtures.Count; i++) + { + // can't cut circles 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], thickness, out first, out second); + + //Delete the original shape and create two new. Retain the properties of the body. + if (SanityCheck(first)) + { + 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 (SanityCheck(second)) + { + 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); + } + } + } + + private static bool SanityCheck(Vertices vertices) + { + if (vertices.Count < 3) + return false; + + if (vertices.GetArea() < 0.00001f) + return false; + + for (int i = 0; i < vertices.Count; ++i) + { + int i1 = i; + int i2 = i + 1 < vertices.Count ? i + 1 : 0; + Vector2 edge = vertices[i2] - vertices[i1]; + if (edge.LengthSquared() < Settings.Epsilon * Settings.Epsilon) + return false; + } + + for (int i = 0; i < vertices.Count; ++i) + { + int i1 = i; + int i2 = i + 1 < vertices.Count ? i + 1 : 0; + Vector2 edge = vertices[i2] - vertices[i1]; + + for (int j = 0; j < vertices.Count; ++j) + { + // Don't check vertices on the current edge. + if (j == i1 || j == i2) + { + continue; + } + + Vector2 r = vertices[j] - vertices[i1]; + + // Your polygon is non-convex (it has an indentation) or + // has colinear edges. + float s = edge.X * r.Y - edge.Y * r.X; + + if (s < 0.0f) + return false; + } + } + + return true; + } + } +} \ No newline at end of file diff --git a/axios/Common/PolygonManipulation/SimplifyTools.cs b/axios/Common/PolygonManipulation/SimplifyTools.cs new file mode 100644 index 0000000..36cb40d --- /dev/null +++ b/axios/Common/PolygonManipulation/SimplifyTools.cs @@ -0,0 +1,359 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common.PolygonManipulation +{ + public static class SimplifyTools + { + private static bool[] _usePt; + private static double _distanceTolerance; + + /// + /// 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) + { + //We can't simplify polygons under 3 vertices + if (vertices.Count < 3) + return vertices; + + Vertices simplified = new Vertices(); + + for (int i = 0; i < vertices.Count; i++) + { + int prevId = vertices.PreviousIndex(i); + int nextId = vertices.NextIndex(i); + + Vector2 prev = vertices[prevId]; + Vector2 current = vertices[i]; + Vector2 next = vertices[nextId]; + + //If they collinear, continue + if (MathUtils.Collinear(ref prev, ref current, ref next, collinearityTolerance)) + continue; + + simplified.Add(current); + } + + return simplified; + } + + /// + /// Removes all collinear points on the polygon. + /// Has a default bias of 0 + /// + /// The polygon that needs simplification. + /// A simplified polygon. + public static Vertices CollinearSimplify(Vertices vertices) + { + return CollinearSimplify(vertices, 0); + } + + /// + /// 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) + { + _distanceTolerance = distanceTolerance; + + _usePt = new bool[vertices.Count]; + for (int i = 0; i < vertices.Count; i++) + _usePt[i] = true; + + SimplifySection(vertices, 0, vertices.Count - 1); + Vertices result = new Vertices(); + + for (int i = 0; i < vertices.Count; i++) + if (_usePt[i]) + result.Add(vertices[i]); + + return result; + } + + private static void SimplifySection(Vertices vertices, int i, int j) + { + 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++) + { + double distance = DistancePointLine(vertices[k], A, B); + + if (distance > maxDistance) + { + maxDistance = distance; + maxIndex = k; + } + } + if (maxDistance <= _distanceTolerance) + for (int k = i + 1; k < j; k++) + _usePt[k] = false; + else + { + SimplifySection(vertices, i, maxIndex); + SimplifySection(vertices, maxIndex, j); + } + } + + private static double DistancePointPoint(Vector2 p, Vector2 p2) + { + double dx = p.X - p2.X; + double dy = p.Y - p2.X; + return Math.Sqrt(dx * dx + dy * dy); + } + + private static double DistancePointLine(Vector2 p, Vector2 A, Vector2 B) + { + // if start == end, then use point-to-point distance + if (A.X == B.X && A.Y == B.Y) + return DistancePointPoint(p, A); + + // otherwise use comp.graphics.algorithms Frequently Asked Questions method + /*(1) AC dot AB + r = --------- + ||AB||^2 + + r has the following meaning: + r=0 Point = A + r=1 Point = B + r<0 Point is on the backward extension of AB + r>1 Point is on the forward extension of AB + 0= 1.0) return DistancePointPoint(p, B); + + + /*(2) + (Ay-Cy)(Bx-Ax)-(Ax-Cx)(By-Ay) + s = ----------------------------- + Curve^2 + + Then the distance from C to Point = |s|*Curve. + */ + + double s = ((A.Y - p.Y) * (B.X - A.X) - (A.X - p.X) * (B.Y - A.Y)) + / + ((B.X - A.X) * (B.X - A.X) + (B.Y - A.Y) * (B.Y - A.Y)); + + return Math.Abs(s) * Math.Sqrt(((B.X - A.X) * (B.X - A.X) + (B.Y - A.Y) * (B.Y - A.Y))); + } + + //From physics2d.net + public static Vertices ReduceByArea(Vertices vertices, float areaTolerance) + { + if (vertices.Count <= 3) + return vertices; + + if (areaTolerance < 0) + { + throw new ArgumentOutOfRangeException("areaTolerance", "must be equal to or greater then zero."); + } + + Vertices result = new Vertices(); + Vector2 v1, v2, v3; + float old1, old2, new1; + v1 = vertices[vertices.Count - 2]; + v2 = vertices[vertices.Count - 1]; + areaTolerance *= 2; + for (int index = 0; index < vertices.Count; ++index, v2 = v3) + { + if (index == vertices.Count - 1) + { + if (result.Count == 0) + { + throw new ArgumentOutOfRangeException("areaTolerance", "The tolerance is too high!"); + } + v3 = result[0]; + } + else + { + v3 = vertices[index]; + } + MathUtils.Cross(ref v1, ref v2, out old1); + MathUtils.Cross(ref v2, ref v3, out old2); + MathUtils.Cross(ref v1, ref v3, out new1); + if (Math.Abs(new1 - (old1 + old2)) > areaTolerance) + { + result.Add(v2); + v1 = v2; + } + } + return result; + } + + //From Eric Jordan's convex decomposition library + + /// + /// Merges all parallel edges in the list of vertices + /// + /// The vertices. + /// The tolerance. + public static void MergeParallelEdges(Vertices vertices, float tolerance) + { + if (vertices.Count <= 3) + return; //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; + + int currIndex = 0; + + //Copy the vertices to a new list and clear the old + Vertices oldVertices = new Vertices(vertices); + vertices.Clear(); + + for (int i = 0; i < oldVertices.Count; ++i) + { + if (mergeMe[i] || newNVertices == 0 || currIndex == newNVertices) + continue; + + Debug.Assert(currIndex < newNVertices); + + vertices.Add(oldVertices[i]); + ++currIndex; + } + } + + //Misc + + /// + /// Merges the identical points in the polygon. + /// + /// The vertices. + /// + public static Vertices MergeIdenticalPoints(Vertices vertices) + { + //We use a dictonary here because HashSet is not avaliable on all platforms. + HashSet results = new HashSet(); + + for (int i = 0; i < vertices.Count; i++) + { + results.Add(vertices[i]); + } + + Vertices returnResults = new Vertices(); + foreach (Vector2 v in results) + { + returnResults.Add(v); + } + + return returnResults; + } + + /// + /// Reduces the polygon by distance. + /// + /// The vertices. + /// The distance between points. Points closer than this will be 'joined'. + /// + public static Vertices ReduceByDistance(Vertices vertices, float distance) + { + //We can't simplify polygons under 3 vertices + if (vertices.Count < 3) + return vertices; + + Vertices simplified = new Vertices(); + + 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() <= distance) + 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) + { + //We can't simplify polygons under 3 vertices + if (vertices.Count < 3) + return vertices; + + if (nth == 0) + return vertices; + + Vertices result = new Vertices(vertices.Count); + + for (int i = 0; i < vertices.Count; i++) + { + if (i % nth == 0) + continue; + + result.Add(vertices[i]); + } + + return result; + } + } +} \ No newline at end of file diff --git a/axios/Common/PolygonManipulation/YuPengClipper.cs b/axios/Common/PolygonManipulation/YuPengClipper.cs new file mode 100644 index 0000000..9c25475 --- /dev/null +++ b/axios/Common/PolygonManipulation/YuPengClipper.cs @@ -0,0 +1,513 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using FarseerPhysics.Collision.Shapes; +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.GetCollisionBox().LowerBound; + Vector2 lbClip = clip.GetCollisionBox().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 = 0f; + if (poly2Simplicies.Contains(poly1Simplicies[i]) || + (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) + { + if (!resultSimplices.Contains(poly2Simplicies[i]) && + !resultSimplices.Contains(-poly2Simplicies[i])) + { + float edgeCharacter = 0f; + if (poly1Simplicies.Contains(poly2Simplicies[i]) || + (poly1Simplicies.Contains(-poly2Simplicies[i]) && clipType == PolyClipType.Union)) + { + edgeCharacter = 1f; + } + else + { + for (int j = 0; j < poly1Simplicies.Count; ++j) + { + if (!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/axios/Common/PolygonTools.cs b/axios/Common/PolygonTools.cs new file mode 100644 index 0000000..4f3ed18 --- /dev/null +++ b/axios/Common/PolygonTools.cs @@ -0,0 +1,368 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +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; + } + + public static Vertices CreateTriangle(float hx, float hy) + { + Vertices vertices = new Vertices(3); + vertices.Add(new Vector2(hx/2, hy)); + vertices.Add(new Vector2(hx, 0)); + vertices.Add(new Vector2(0, 0)); + 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.Position = center; + xf.R.Set(angle); + + // Transform vertices + for (int i = 0; i < 4; ++i) + { + vertices[i] = MathUtils.Multiply(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/axios/Common/Serialization.cs b/axios/Common/Serialization.cs new file mode 100644 index 0000000..2905eab --- /dev/null +++ b/axios/Common/Serialization.cs @@ -0,0 +1,1453 @@ +using System; +using System.Collections.Generic; +using System.IO; +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 +{ + public static class WorldSerializer + { + public static void Serialize(World world, string filename) + { + using (FileStream fs = new FileStream(filename, FileMode.Create)) + { + new WorldXmlSerializer().Serialize(world, fs); + } + } + + public static void Deserialize(World world, string filename) + { + using (FileStream fs = new FileStream(filename, FileMode.Open)) + { + new WorldXmlDeserializer().Deserialize(world, fs); + } + } + + public static World Deserialize(string filename) + { + using (FileStream fs = new FileStream(filename, FileMode.Open)) + { + return new WorldXmlDeserializer().Deserialize(fs); + } + } + } + + /// + /// + public class WorldXmlSerializer + { + private List _bodies = new List(); + private List _serializedFixtures = new List(); + private List _serializedShapes = new List(); + private XmlWriter _writer; + + private void SerializeShape(Shape shape) + { + _writer.WriteStartElement("Shape"); + _writer.WriteAttributeString("Type", shape.ShapeType.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; + default: + throw new Exception(); + } + + _writer.WriteEndElement(); + } + + private void SerializeFixture(Fixture fixture) + { + _writer.WriteStartElement("Fixture"); + _writer.WriteElementString("Shape", FindShapeIndex(fixture.Shape).ToString()); + _writer.WriteElementString("Density", fixture.Shape.Density.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.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 void SerializeBody(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("Fixtures"); + for (int i = 0; i < body.FixtureList.Count; i++) + { + _writer.WriteElementString("ID", FindFixtureIndex(body.FixtureList[i]).ToString()); + } + + _writer.WriteEndElement(); + _writer.WriteEndElement(); + } + + private void SerializeJoint(Joint joint) + { + if (joint.IsFixedType()) + return; + + _writer.WriteStartElement("Joint"); + + _writer.WriteAttributeString("Type", joint.JointType.ToString()); + + WriteElement("BodyA", FindBodyIndex(joint.BodyA)); + WriteElement("BodyB", FindBodyIndex(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 djd = (DistanceJoint)joint; + + WriteElement("DampingRatio", djd.DampingRatio); + WriteElement("FrequencyHz", djd.Frequency); + WriteElement("Length", djd.Length); + WriteElement("LocalAnchorA", djd.LocalAnchorA); + WriteElement("LocalAnchorB", djd.LocalAnchorB); + } + break; + case JointType.Friction: + { + FrictionJoint fjd = (FrictionJoint)joint; + WriteElement("LocalAnchorA", fjd.LocalAnchorA); + WriteElement("LocalAnchorB", fjd.LocalAnchorB); + WriteElement("MaxForce", fjd.MaxForce); + WriteElement("MaxTorque", fjd.MaxTorque); + } + break; + case JointType.Gear: + throw new Exception("Gear joint not supported by serialization"); + case JointType.Line: + { + LineJoint ljd = (LineJoint)joint; + + WriteElement("EnableMotor", ljd.MotorEnabled); + WriteElement("LocalAnchorA", ljd.LocalAnchorA); + WriteElement("LocalAnchorB", ljd.LocalAnchorB); + WriteElement("MotorSpeed", ljd.MotorSpeed); + WriteElement("DampingRatio", ljd.DampingRatio); + WriteElement("MaxMotorTorque", ljd.MaxMotorTorque); + WriteElement("FrequencyHz", ljd.Frequency); + WriteElement("LocalXAxis", ljd.LocalXAxis); + } + break; + case JointType.Prismatic: + { + PrismaticJoint pjd = (PrismaticJoint)joint; + + //NOTE: Does not conform with Box2DScene + + WriteElement("EnableLimit", pjd.LimitEnabled); + WriteElement("EnableMotor", pjd.MotorEnabled); + WriteElement("LocalAnchorA", pjd.LocalAnchorA); + WriteElement("LocalAnchorB", pjd.LocalAnchorB); + WriteElement("LocalXAxis1", pjd.LocalXAxis1); + WriteElement("LowerTranslation", pjd.LowerLimit); + WriteElement("UpperTranslation", pjd.UpperLimit); + WriteElement("MaxMotorForce", pjd.MaxMotorForce); + WriteElement("MotorSpeed", pjd.MotorSpeed); + } + break; + case JointType.Pulley: + { + PulleyJoint pjd = (PulleyJoint)joint; + + WriteElement("GroundAnchorA", pjd.GroundAnchorA); + WriteElement("GroundAnchorB", pjd.GroundAnchorB); + WriteElement("LengthA", pjd.LengthA); + WriteElement("LengthB", pjd.LengthB); + WriteElement("LocalAnchorA", pjd.LocalAnchorA); + WriteElement("LocalAnchorB", pjd.LocalAnchorB); + WriteElement("MaxLengthA", pjd.MaxLengthA); + WriteElement("MaxLengthB", pjd.MaxLengthB); + WriteElement("Ratio", pjd.Ratio); + } + break; + case JointType.Revolute: + { + RevoluteJoint rjd = (RevoluteJoint)joint; + + WriteElement("EnableLimit", rjd.LimitEnabled); + WriteElement("EnableMotor", rjd.MotorEnabled); + WriteElement("LocalAnchorA", rjd.LocalAnchorA); + WriteElement("LocalAnchorB", rjd.LocalAnchorB); + WriteElement("LowerAngle", rjd.LowerLimit); + WriteElement("MaxMotorTorque", rjd.MaxMotorTorque); + WriteElement("MotorSpeed", rjd.MotorSpeed); + WriteElement("ReferenceAngle", rjd.ReferenceAngle); + WriteElement("UpperAngle", rjd.UpperLimit); + } + break; + case JointType.Weld: + { + WeldJoint wjd = (WeldJoint)joint; + + WriteElement("LocalAnchorA", wjd.LocalAnchorA); + WriteElement("LocalAnchorB", wjd.LocalAnchorB); + } + break; + // + // Not part of Box2DScene + // + case JointType.Rope: + { + RopeJoint rjd = (RopeJoint)joint; + + WriteElement("LocalAnchorA", rjd.LocalAnchorA); + WriteElement("LocalAnchorB", rjd.LocalAnchorB); + WriteElement("MaxLength", rjd.MaxLength); + } + break; + case JointType.Angle: + { + AngleJoint aj = (AngleJoint)joint; + WriteElement("BiasFactor", aj.BiasFactor); + WriteElement("MaxImpulse", aj.MaxImpulse); + WriteElement("Softness", aj.Softness); + WriteElement("TargetAngle", aj.TargetAngle); + } + break; + case JointType.Slider: + { + SliderJoint sliderJoint = (SliderJoint)joint; + WriteElement("DampingRatio", sliderJoint.DampingRatio); + WriteElement("FrequencyHz", sliderJoint.Frequency); + WriteElement("MaxLength", sliderJoint.MaxLength); + WriteElement("MinLength", sliderJoint.MinLength); + WriteElement("LocalAnchorA", sliderJoint.LocalAnchorA); + WriteElement("LocalAnchorB", sliderJoint.LocalAnchorB); + } + break; + default: + throw new Exception("Joint not supported"); + } + + _writer.WriteEndElement(); + } + + private void WriteDynamicType(Type type, object val) + { + _writer.WriteElementString("Type", type.FullName); + + _writer.WriteStartElement("Value"); + XmlSerializer serializer = new XmlSerializer(type); + XmlSerializerNamespaces xmlnsEmpty = new XmlSerializerNamespaces(); + xmlnsEmpty.Add("", ""); + serializer.Serialize(_writer, val, xmlnsEmpty); + _writer.WriteEndElement(); + } + + private void WriteElement(string name, Vector2 vec) + { + _writer.WriteElementString(name, vec.X + " " + vec.Y); + } + + private void WriteElement(string name, int val) + { + _writer.WriteElementString(name, val.ToString()); + } + + private void WriteElement(string name, bool val) + { + _writer.WriteElementString(name, val.ToString()); + } + + private void WriteElement(string name, float val) + { + _writer.WriteElementString(name, val.ToString()); + } + + public void Serialize(World world, Stream stream) + { + XmlWriterSettings settings = new XmlWriterSettings(); + settings.Indent = true; + settings.NewLineOnAttributes = false; + settings.OmitXmlDeclaration = true; + + _writer = XmlWriter.Create(stream, settings); + + _writer.WriteStartElement("World"); + _writer.WriteAttributeString("Version", "2"); + WriteElement("Gravity", world.Gravity); + + _writer.WriteStartElement("Shapes"); + + for (int i = 0; i < world.BodyList.Count; i++) + { + Body body = world.BodyList[i]; + for (int j = 0; j < body.FixtureList.Count; j++) + { + Fixture fixture = body.FixtureList[j]; + + bool alreadyThere = false; + for (int k = 0; k < _serializedShapes.Count; k++) + { + Shape s2 = _serializedShapes[k]; + if (fixture.Shape.CompareTo(s2)) + { + alreadyThere = true; + break; + } + } + + if (!alreadyThere) + { + SerializeShape(fixture.Shape); + _serializedShapes.Add(fixture.Shape); + } + } + } + + _writer.WriteEndElement(); + _writer.WriteStartElement("Fixtures"); + + + for (int i = 0; i < world.BodyList.Count; i++) + { + Body body = world.BodyList[i]; + for (int j = 0; j < body.FixtureList.Count; j++) + { + Fixture fixture = body.FixtureList[j]; + bool alreadyThere = false; + for (int k = 0; k < _serializedFixtures.Count; k++) + { + Fixture f2 = _serializedFixtures[k]; + if (fixture.CompareTo(f2)) + { + alreadyThere = true; + break; + } + } + + if (!alreadyThere) + { + SerializeFixture(fixture); + _serializedFixtures.Add(fixture); + } + } + } + + _writer.WriteEndElement(); + _writer.WriteStartElement("Bodies"); + + for (int i = 0; i < world.BodyList.Count; i++) + { + Body body = world.BodyList[i]; + _bodies.Add(body); + SerializeBody(body); + } + + _writer.WriteEndElement(); + _writer.WriteStartElement("Joints"); + + for (int i = 0; i < world.JointList.Count; i++) + { + Joint joint = world.JointList[i]; + SerializeJoint(joint); + } + + _writer.WriteEndElement(); + _writer.WriteEndElement(); + + _writer.Flush(); + _writer.Close(); + } + + private int FindBodyIndex(Body body) + { + for (int i = 0; i < _bodies.Count; ++i) + if (_bodies[i] == body) + return i; + + return -1; + } + + private int FindFixtureIndex(Fixture fixture) + { + for (int i = 0; i < _serializedFixtures.Count; ++i) + { + if (_serializedFixtures[i].CompareTo(fixture)) + return i; + } + + return -1; + } + + private int FindShapeIndex(Shape shape) + { + for (int i = 0; i < _serializedShapes.Count; ++i) + { + if (_serializedShapes[i].CompareTo(shape)) + return i; + } + + return -1; + } + } + + public class WorldXmlDeserializer + { + private List _bodies = new List(); + private List _fixtures = new List(); + private List _joints = new List(); + private List _shapes = new List(); + + public World Deserialize(Stream stream) + { + World world = new World(Vector2.Zero); + Deserialize(world, stream); + return world; + } + + public void Deserialize(World world, Stream stream) + { + world.Clear(); + + XMLFragmentElement root = XMLFragmentParser.LoadFromStream(stream); + + if (root.Name.ToLower() != "world") + throw new Exception(); + + foreach (XMLFragmentElement main in root.Elements) + { + if (main.Name.ToLower() == "gravity") + { + world.Gravity = ReadVector(main); + } + } + + foreach (XMLFragmentElement shapeElement in root.Elements) + { + if (shapeElement.Name.ToLower() == "shapes") + { + foreach (XMLFragmentElement n in shapeElement.Elements) + { + if (n.Name.ToLower() != "shape") + throw new Exception(); + + ShapeType type = (ShapeType)Enum.Parse(typeof(ShapeType), n.Attributes[0].Value, true); + + switch (type) + { + case ShapeType.Circle: + { + CircleShape shape = new CircleShape(); + + foreach (XMLFragmentElement sn in n.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(); + + foreach (XMLFragmentElement sn in n.Elements) + { + switch (sn.Name.ToLower()) + { + case "vertices": + { + List verts = new List(); + + foreach (XMLFragmentElement vert in sn.Elements) + verts.Add(ReadVector(vert)); + + shape.Set(new Vertices(verts.ToArray())); + } + break; + case "centroid": + shape.MassData.Centroid = ReadVector(sn); + break; + } + } + + _shapes.Add(shape); + } + break; + case ShapeType.Edge: + { + EdgeShape shape = new EdgeShape(); + foreach (XMLFragmentElement sn in n.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; + } + } + } + } + + foreach (XMLFragmentElement fixtureElement in root.Elements) + { + if (fixtureElement.Name.ToLower() == "fixtures") + { + foreach (XMLFragmentElement n in fixtureElement.Elements) + { + Fixture fixture = new Fixture(); + + if (n.Name.ToLower() != "fixture") + throw new Exception(); + + foreach (XMLFragmentElement sn in n.Elements) + { + switch (sn.Name.ToLower()) + { + case "shape": + fixture.Shape = _shapes[int.Parse(sn.Value)]; + break; + case "density": + fixture.Shape.Density = float.Parse(sn.Value); + break; + 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; + } + } + + 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); + } + } + } + + foreach (XMLFragmentElement bodyElement in root.Elements) + { + if (bodyElement.Name.ToLower() == "bodies") + { + foreach (XMLFragmentElement n in bodyElement.Elements) + { + Body body = new Body(world); + + if (n.Name.ToLower() != "body") + throw new Exception(); + + body.BodyType = (BodyType)Enum.Parse(typeof(BodyType), n.Attributes[0].Value, true); + + foreach (XMLFragmentElement sn in n.Elements) + { + switch (sn.Name.ToLower()) + { + case "active": + if (bool.Parse(sn.Value)) + body.Flags |= BodyFlags.Enabled; + else + body.Flags &= ~BodyFlags.Enabled; + 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 "fixtures": + { + foreach (XMLFragmentElement v in sn.Elements) + { + Fixture blueprint = _fixtures[int.Parse(v.Value)]; + Fixture f = new Fixture(body, blueprint.Shape); + f.Restitution = blueprint.Restitution; + f.UserData = blueprint.UserData; + f.Friction = blueprint.Friction; + f.CollidesWith = blueprint.CollidesWith; + f.CollisionCategories = blueprint.CollisionCategories; + f.CollisionGroup = blueprint.CollisionGroup; + } + break; + } + } + } + + _bodies.Add(body); + } + } + } + + 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.Distance: + joint = new DistanceJoint(); + break; + case JointType.Friction: + joint = new FrictionJoint(); + break; + case JointType.Line: + joint = new LineJoint(); + 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.Slider: + joint = new SliderJoint(); + 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.Line: + { + switch (sn.Name.ToLower()) + { + case "enablemotor": + ((LineJoint)joint).MotorEnabled = bool.Parse(sn.Value); + break; + case "localanchora": + ((LineJoint)joint).LocalAnchorA = ReadVector(sn); + break; + case "localanchorb": + ((LineJoint)joint).LocalAnchorB = ReadVector(sn); + break; + case "motorspeed": + ((LineJoint)joint).MotorSpeed = float.Parse(sn.Value); + break; + case "dampingratio": + ((LineJoint)joint).DampingRatio = float.Parse(sn.Value); + break; + case "maxmotortorque": + ((LineJoint)joint).MaxMotorTorque = float.Parse(sn.Value); + break; + case "frequencyhz": + ((LineJoint)joint).Frequency = float.Parse(sn.Value); + break; + case "localxaxis": + ((LineJoint)joint).LocalXAxis = 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 "local1axis1": + ((PrismaticJoint)joint).LocalXAxis1 = 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 "groundanchora": + ((PulleyJoint)joint).GroundAnchorA = ReadVector(sn); + break; + case "groundanchorb": + ((PulleyJoint)joint).GroundAnchorB = 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 "maxlengtha": + ((PulleyJoint)joint).MaxLengthA = float.Parse(sn.Value); + break; + case "maxlengthb": + ((PulleyJoint)joint).MaxLengthB = float.Parse(sn.Value); + break; + case "ratio": + ((PulleyJoint)joint).Ratio = 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.Slider: + { + switch (sn.Name.ToLower()) + { + case "dampingratio": + ((SliderJoint)joint).DampingRatio = float.Parse(sn.Value); + break; + case "frequencyhz": + ((SliderJoint)joint).Frequency = float.Parse(sn.Value); + break; + case "maxlength": + ((SliderJoint)joint).MaxLength = float.Parse(sn.Value); + break; + case "minlength": + ((SliderJoint)joint).MinLength = float.Parse(sn.Value); + break; + case "localanchora": + ((SliderJoint)joint).LocalAnchorA = ReadVector(sn); + break; + case "localanchorb": + ((SliderJoint)joint).LocalAnchorB = ReadVector(sn); + break; + } + } + break; + } + } + } + } + } + } + + private Vector2 ReadVector(XMLFragmentElement node) + { + string[] values = node.Value.Split(' '); + return new Vector2(float.Parse(values[0]), float.Parse(values[1])); + } + + private 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 + + public class XMLFragmentAttribute + { + public string Name { get; set; } + + public string Value { get; set; } + } + + public 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; } + } + + public class XMLFragmentException : Exception + { + public XMLFragmentException() + { + } + + public XMLFragmentException(string message) + : base(message) + { + } + + public XMLFragmentException(string message, Exception inner) + : base(message, inner) + { + } + } + + public 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; } + + public int Length + { + get { return Buffer.Length; } + } + + public char Next + { + get + { + char c = Buffer[Position]; + Position++; + return c; + } + } + + public char Peek + { + get { return Buffer[Position]; } + } + + public bool EndOfBuffer + { + get { return Position == Length; } + } + } + + public 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 LoadFromFile(string fileName) + { + XMLFragmentParser x = new XMLFragmentParser(fileName); + x.Parse(); + return x.RootNode; + } + + 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; + } + + public 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/axios/Common/TextureTools/MSTerrain.cs b/axios/Common/TextureTools/MSTerrain.cs new file mode 100644 index 0000000..af0a795 --- /dev/null +++ b/axios/Common/TextureTools/MSTerrain.cs @@ -0,0 +1,367 @@ +using System.Collections.Generic; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Graphics; +using FarseerPhysics.Dynamics; +using FarseerPhysics.Collision; +using FarseerPhysics.Factories; + +namespace FarseerPhysics.Common +{ + public enum Decomposer + { + Bayazit, + CDT, + Earclip, + Flipcode, + Seidel, + } + + /// + /// Return true if the specified color is inside the terrain. + /// + public delegate bool TerrainTester(Color color); + + /// + /// Simple class to maintain a terrain. + /// + public class MSTerrain + { + /// + /// 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 Decomposer 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; + + public MSTerrain(World world, AABB area) + { + World = world; + Width = area.Extents.X * 2; + Height = area.Extents.Y * 2; + Center = area.Center; + } + + /// + /// 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 a texture to the terrain using the specified TerrainTester. + /// + /// Texture to apply. + /// Top left position of the texture relative to the terrain. + /// Delegate method used to determine what colors should be included in the terrain. + public void ApplyTexture(Texture2D texture, Vector2 position, TerrainTester tester) + { + Color[] colorData = new Color[texture.Width * texture.Height]; + + texture.GetData(colorData); + + for (int y = (int)position.Y; y < texture.Height + (int)position.Y; y++) + { + for (int x = (int)position.X; x < texture.Width + (int)position.X; x++) + { + if (x >= 0 && x < _localWidth && y >= 0 && y < _localHeight) + { + bool inside = tester(colorData[((y - (int)position.Y) * texture.Width) + (x - (int)position.X)]); + + if (!inside) + _terrainMap[x, y] = 1; + else + _terrainMap[x, y] = -1; + } + } + } + + // generate terrain + for (int gy = 0; gy < _ynum; gy++) + { + for (int gx = 0; gx < _xnum; gx++) + { + //remove old terrain object at grid cell + if (_bodyMap[gx, gy] != null) + { + for (int i = 0; i < _bodyMap[gx, gy].Count; i++) + { + World.RemoveBody(_bodyMap[gx, gy][i]); + } + } + + _bodyMap[gx, gy] = null; + + //generate new one + GenerateTerrain(gx, gy); + } + } + } + + /// + /// Apply a texture to the terrain using the specified TerrainTester. + /// + /// Top left position of the texture relative to the terrain. + public void ApplyData(sbyte[,] data, Vector2 position) + { + for (int y = (int)position.Y; y < data.GetUpperBound(1) + (int)position.Y; y++) + { + for (int x = (int)position.X; x < data.GetUpperBound(0) + (int)position.X; x++) + { + if (x >= 0 && x < _localWidth && y >= 0 && y < _localHeight) + { + _terrainMap[x, y] = data[x, y]; + } + } + } + + // generate terrain + for (int gy = 0; gy < _ynum; gy++) + { + for (int gx = 0; gx < _xnum; gx++) + { + //remove old terrain object at grid cell + if (_bodyMap[gx, gy] != null) + { + for (int i = 0; i < _bodyMap[gx, gy].Count; i++) + { + World.RemoveBody(_bodyMap[gx, gy][i]); + } + } + + _bodyMap[gx, gy] = null; + + //generate new one + GenerateTerrain(gx, gy); + } + } + } + + /// + /// Convert a texture to an sbtye array compatible with ApplyData(). + /// + /// Texture to convert. + /// + /// + public static sbyte[,] ConvertTextureToData(Texture2D texture, TerrainTester tester) + { + sbyte[,] data = new sbyte[texture.Width, texture.Height]; + Color[] colorData = new Color[texture.Width * texture.Height]; + + texture.GetData(colorData); + + for (int y = 0; y < texture.Height; y++) + { + for (int x = 0; x < texture.Width; x++) + { + bool inside = tester(colorData[(y * texture.Width) + x]); + + if (!inside) + data[x, y] = 1; + else + data[x, y] = -1; + } + } + + return data; + } + + /// + /// 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 + var gx0 = (int)(_dirtyArea.LowerBound.X / CellSize); + var gx1 = (int)(_dirtyArea.UpperBound.X / CellSize) + 1; + if (gx0 < 0) gx0 = 0; + if (gx1 > _xnum) gx1 = _xnum; + var gy0 = (int)(_dirtyArea.LowerBound.Y / CellSize); + var gy1 = (int)(_dirtyArea.UpperBound.Y / CellSize) + 1; + if (gy0 < 0) gy0 = 0; + if (gy1 > _ynum) gy1 = _ynum; + + for (int gx = gx0; gx < gx1; gx++) + { + for (int gy = gy0; gy < gy1; gy++) + { + //remove old terrain object at grid cell + if (_bodyMap[gx, gy] != null) + { + for (int i = 0; i < _bodyMap[gx, gy].Count; i++) + { + World.RemoveBody(_bodyMap[gx, gy][i]); + } + } + + _bodyMap[gx, gy] = null; + + //generate new one + GenerateTerrain(gx, gy); + } + } + + _dirtyArea = new AABB(new Vector2(float.MaxValue, float.MaxValue), new Vector2(float.MinValue, float.MinValue)); + } + + 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 (var item in polys) + { + // does this need to be negative? + item.Scale(ref scale); + item.Translate(ref _topLeft); + item.ForceCounterClockWise(); + Vertices p = FarseerPhysics.Common.PolygonManipulation.SimplifyTools.CollinearSimplify(item); + List decompPolys = new List(); + + switch (Decomposer) + { + case Decomposer.Bayazit: + decompPolys = Decomposition.BayazitDecomposer.ConvexPartition(p); + break; + case Decomposer.CDT: + decompPolys = Decomposition.CDTDecomposer.ConvexPartition(p); + break; + case Decomposer.Earclip: + decompPolys = Decomposition.EarclipDecomposer.ConvexPartition(p); + break; + case Decomposer.Flipcode: + decompPolys = Decomposition.FlipcodeDecomposer.ConvexPartition(p); + break; + case Decomposer.Seidel: + decompPolys = Decomposition.SeidelDecomposer.ConvexPartition(p, 0.001f); + break; + default: + break; + } + + foreach (Vertices poly in decompPolys) + { + if (poly.Count > 2) + _bodyMap[gx, gy].Add(BodyFactory.CreatePolygon(World, poly, 1)); + } + } + } + } +} diff --git a/axios/Common/TextureTools/MarchingSquares.cs b/axios/Common/TextureTools/MarchingSquares.cs new file mode 100644 index 0000000..d553d58 --- /dev/null +++ b/axios/Common/TextureTools/MarchingSquares.cs @@ -0,0 +1,800 @@ +using System.Collections.Generic; +using FarseerPhysics.Collision; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common +{ + // 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/axios/Common/TextureTools/TextureConverter.cs b/axios/Common/TextureTools/TextureConverter.cs new file mode 100644 index 0000000..73dd53a --- /dev/null +++ b/axios/Common/TextureTools/TextureConverter.cs @@ -0,0 +1,1338 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common +{ + // User contribution from Sickbattery aka David Reschke :). + + #region ToDo: Create a new file for each ... + /// + /// 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 + } + + /// + /// Detected vertices of a single polygon. + /// + public class DetectedVertices : Vertices + { + private List _holes; + + public List Holes + { + get { return _holes; } + set { _holes = value; } + } + + public DetectedVertices() + : base() + { + } + + public DetectedVertices(Vertices vertices) + : base(vertices) + { + } + + public void Transform(Matrix transform) + { + // Transform main polygon + for (int i = 0; i < this.Count; i++) + this[i] = Vector2.Transform(this[i], transform); + + // Transform holes + Vector2[] temp = null; + if (_holes != null && _holes.Count > 0) + { + for (int i = 0; i < _holes.Count; i++) + { + temp = _holes[i].ToArray(); + Vector2.Transform(temp, ref transform, temp); + + _holes[i] = new Vertices(temp); + } + } + } + } + #endregion + + /// + /// + /// + public sealed class TextureConverter + { + private const int _CLOSEPIXELS_LENGTH = 8; + + /// + /// This array is ment to be readonly. + /// It's not because it is accessed very frequently. + /// + private static /*readonly*/ int[,] ClosePixels = + new int[_CLOSEPIXELS_LENGTH, 2] { { -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(); + + DetectedVertices polygon; + Vertices holePolygon; + + Vector2? holeEntrance = null; + Vector2? polygonEntrance = null; + + List blackList = new List(); + + bool searchOn; + do + { + if (detectedPolygons.Count == 0) + { + // First pass / single polygon + polygon = new DetectedVertices(CreateSimplePolygon(Vector2.Zero, Vector2.Zero)); + + if (polygon.Count > 2) + polygonEntrance = GetTopMostVertex(polygon); + } + else if (polygonEntrance.HasValue) + { + // Multi pass / multiple polygons + polygon = new DetectedVertices(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); + 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 (_pixelOffsetOptimization) + ApplyPixelOffsetOptimization(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 ApplyPixelOffsetOptimization(ref List detectedPolygons) + { + + } + + private void ApplyTransform(ref List detectedPolygons) + { + for (int i = 0; i < detectedPolygons.Count; i++) + detectedPolygons[i].Transform(_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 DistanceToHullAcceptable(DetectedVertices 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((Vertices)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 || + LineTools.DistanceBetweenPointAndPoint(ref point, ref 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(DetectedVertices polygon, Vector2 point) + { + bool inPolygon = !DistanceToHullAcceptable(polygon, point, true); + + if (!inPolygon) + { + List xCoords = SearchCrossingEdges(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 SearchCrossingEdges(DetectedVertices 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((Vertices)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 = LineTools.DistanceBetweenPointAndPoint(ref tempVector, ref 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 < _CLOSEPIXELS_LENGTH; 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 < _CLOSEPIXELS_LENGTH; 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 < _CLOSEPIXELS_LENGTH; i++) + { + indexOfPixelToCheck = (indexOfFirstPixelToCheck + i) % _CLOSEPIXELS_LENGTH; + + 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/axios/Common/Vertices.cs b/axios/Common/Vertices.cs new file mode 100644 index 0000000..83655bf --- /dev/null +++ b/axios/Common/Vertices.cs @@ -0,0 +1,955 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Text; +using FarseerPhysics.Collision; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Common +{ +#if !(XBOX360) + [DebuggerDisplay("Count = {Count} Vertices = {ToString()}")] +#endif + public class Vertices : List + { + public Vertices() + { + } + + public Vertices(int capacity) + { + Capacity = capacity; + } + + public Vertices(Vector2[] vector2) + { + for (int i = 0; i < vector2.Length; i++) + { + Add(vector2[i]); + } + } + + public Vertices(IList vertices) + { + for (int i = 0; i < vertices.Count; i++) + { + Add(vertices[i]); + } + } + + /// + /// Nexts the index. + /// + /// The index. + /// + public int NextIndex(int index) + { + if (index == Count - 1) + { + return 0; + } + return index + 1; + } + + public Vector2 NextVertex(int index) + { + return this[NextIndex(index)]; + } + + /// + /// Gets the previous index. + /// + /// The index. + /// + public int PreviousIndex(int index) + { + if (index == 0) + { + return Count - 1; + } + return index - 1; + } + + public Vector2 PreviousVertex(int index) + { + return this[PreviousIndex(index)]; + } + + /// + /// Gets the signed area. + /// + /// + public float GetSignedArea() + { + int i; + float area = 0; + + for (i = 0; i < Count; i++) + { + int j = (i + 1) % Count; + area += this[i].X * this[j].Y; + area -= this[i].Y * this[j].X; + } + area /= 2.0f; + return area; + } + + /// + /// Gets the area. + /// + /// + public float GetArea() + { + int i; + float area = 0; + + for (i = 0; i < Count; i++) + { + int j = (i + 1) % Count; + area += this[i].X * this[j].Y; + area -= this[i].Y * this[j].X; + } + area /= 2.0f; + return (area < 0 ? -area : area); + } + + /// + /// Gets the centroid. + /// + /// + public Vector2 GetCentroid() + { + // Same algorithm is used by Box2D + + Vector2 c = Vector2.Zero; + float area = 0.0f; + + const float inv3 = 1.0f / 3.0f; + Vector2 pRef = Vector2.Zero; + for (int i = 0; i < Count; ++i) + { + // Triangle vertices. + Vector2 p1 = pRef; + Vector2 p2 = this[i]; + Vector2 p3 = i + 1 < Count ? this[i + 1] : this[0]; + + Vector2 e1 = p2 - p1; + Vector2 e2 = p3 - p1; + + float D = MathUtils.Cross(e1, e2); + + float triangleArea = 0.5f * D; + area += triangleArea; + + // Area weighted centroid + c += triangleArea * inv3 * (p1 + p2 + p3); + } + + // Centroid + c *= 1.0f / area; + return c; + } + + /// + /// Gets the radius based on area. + /// + /// + public float GetRadius() + { + float area = GetSignedArea(); + + double radiusSqrd = (double)area / MathHelper.Pi; + if (radiusSqrd < 0) + { + radiusSqrd *= -1; + } + + return (float)Math.Sqrt(radiusSqrd); + } + + /// + /// Returns an AABB for vertex. + /// + /// + public AABB GetCollisionBox() + { + 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; + } + + public void Translate(Vector2 vector) + { + Translate(ref vector); + } + + /// + /// Translates the vertices with the specified vector. + /// + /// The vector. + public void Translate(ref Vector2 vector) + { + for (int i = 0; i < Count; i++) + this[i] = Vector2.Add(this[i], vector); + } + + /// + /// Scales the vertices with the specified vector. + /// + /// The Value. + public void Scale(ref Vector2 value) + { + for (int i = 0; i < Count; i++) + this[i] = Vector2.Multiply(this[i], value); + } + + /// + /// Rotate the vertices with 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 < Count; i++) + this[i] = Vector2.Transform(this[i], rotationMatrix); + } + + /// + /// Assuming the polygon is simple; determines whether the polygon is convex. + /// NOTE: It will also return false if the input contains colinear edges. + /// + /// + /// true if it is convex; otherwise, false. + /// + public bool IsConvex() + { + // Ensure the polygon is convex and the interior + // is to the left of each edge. + for (int i = 0; i < Count; ++i) + { + int i1 = i; + int i2 = i + 1 < Count ? i + 1 : 0; + Vector2 edge = this[i2] - this[i1]; + + for (int j = 0; j < Count; ++j) + { + // Don't check vertices on the current edge. + if (j == i1 || j == i2) + { + continue; + } + + Vector2 r = this[j] - this[i1]; + + float s = edge.X * r.Y - edge.Y * r.X; + + if (s <= 0.0f) + return false; + } + } + return true; + } + + public bool IsCounterClockWise() + { + //We just return true for lines + if (Count < 3) + return true; + + return (GetSignedArea() > 0.0f); + } + + /// + /// Forces counter clock wise order. + /// + public void ForceCounterClockWise() + { + if (!IsCounterClockWise()) + { + Reverse(); + } + } + + /// + /// Check for edge crossings + /// + /// + public bool IsSimple() + { + for (int i = 0; i < Count; ++i) + { + int iplus = (i + 1 > Count - 1) ? 0 : i + 1; + Vector2 a1 = new Vector2(this[i].X, this[i].Y); + Vector2 a2 = new Vector2(this[iplus].X, this[iplus].Y); + for (int j = i + 1; j < Count; ++j) + { + int jplus = (j + 1 > Count - 1) ? 0 : j + 1; + Vector2 b1 = new Vector2(this[j].X, this[j].Y); + Vector2 b2 = new Vector2(this[jplus].X, this[jplus].Y); + + Vector2 temp; + + if (LineTools.LineIntersect2(a1, a2, b1, b2, out temp)) + { + return false; + } + } + } + return true; + } + + //TODO: Test + //Implementation found here: http://www.gamedev.net/community/forums/topic.asp?topic_id=548477 + public bool IsSimple2() + { + for (int i = 0; i < Count; ++i) + { + if (i < Count - 1) + { + for (int h = i + 1; h < Count; ++h) + { + // Do two vertices lie on top of one another? + if (this[i] == this[h]) + { + return true; + } + } + } + + int j = (i + 1) % Count; + Vector2 iToj = this[j] - this[i]; + Vector2 iTojNormal = new Vector2(iToj.Y, -iToj.X); + + // i is the first vertex and j is the second + int startK = (j + 1) % Count; + int endK = (i - 1 + Count) % Count; + endK += startK < endK ? 0 : startK + 1; + int k = startK; + Vector2 iTok = this[k] - this[i]; + bool onLeftSide = Vector2.Dot(iTok, iTojNormal) >= 0; + Vector2 prevK = this[k]; + ++k; + for (; k <= endK; ++k) + { + int modK = k % Count; + iTok = this[modK] - this[i]; + if (onLeftSide != Vector2.Dot(iTok, iTojNormal) >= 0) + { + Vector2 prevKtoK = this[modK] - prevK; + Vector2 prevKtoKNormal = new Vector2(prevKtoK.Y, -prevKtoK.X); + if ((Vector2.Dot(this[i] - prevK, prevKtoKNormal) >= 0) != + (Vector2.Dot(this[j] - prevK, prevKtoKNormal) >= 0)) + { + return true; + } + } + onLeftSide = Vector2.Dot(iTok, iTojNormal) > 0; + prevK = this[modK]; + } + } + return false; + } + + // From Eric Jordan's convex decomposition library + + /// + /// Checks if polygon is valid for use in Box2d engine. + /// Last ditch effort to ensure no invalid polygons are + /// added to world geometry. + /// + /// Performs a full check, for simplicity, convexity, + /// orientation, minimum angle, and volume. This won't + /// be very efficient, and a lot of it is redundant when + /// other tools in this section are used. + /// + /// + public bool CheckPolygon() + { + int error = -1; + if (Count < 3 || Count > Settings.MaxPolygonVertices) + { + error = 0; + } + if (!IsConvex()) + { + error = 1; + } + if (!IsSimple()) + { + error = 2; + } + if (GetArea() < Settings.Epsilon) + { + error = 3; + } + + //Compute normals + Vector2[] normals = new Vector2[Count]; + Vertices vertices = new Vertices(Count); + for (int i = 0; i < Count; ++i) + { + vertices.Add(new Vector2(this[i].X, this[i].Y)); + int i1 = i; + int i2 = i + 1 < Count ? i + 1 : 0; + Vector2 edge = new Vector2(this[i2].X - this[i1].X, this[i2].Y - this[i1].Y); + normals[i] = MathUtils.Cross(edge, 1.0f); + normals[i].Normalize(); + } + + //Required side checks + for (int i = 0; i < Count; ++i) + { + int iminus = (i == 0) ? Count - 1 : i - 1; + + //Parallel sides check + float cross = MathUtils.Cross(normals[iminus], normals[i]); + cross = MathUtils.Clamp(cross, -1.0f, 1.0f); + float angle = (float)Math.Asin(cross); + if (angle <= Settings.AngularSlop) + { + error = 4; + break; + } + + //Too skinny check + for (int j = 0; j < Count; ++j) + { + if (j == i || j == (i + 1) % Count) + { + continue; + } + float s = Vector2.Dot(normals[i], vertices[j] - vertices[i]); + if (s >= -Settings.LinearSlop) + { + error = 5; + } + } + + + Vector2 centroid = vertices.GetCentroid(); + Vector2 n1 = normals[iminus]; + Vector2 n2 = normals[i]; + Vector2 v = vertices[i] - centroid; + + Vector2 d = new Vector2(); + d.X = Vector2.Dot(n1, v); // - toiSlop; + d.Y = Vector2.Dot(n2, v); // - toiSlop; + + // Shifting the edge inward by toiSlop should + // not cause the plane to pass the centroid. + if ((d.X < 0.0f) || (d.Y < 0.0f)) + { + error = 6; + } + } + + if (error != -1) + { + Debug.WriteLine("Found invalid polygon, "); + switch (error) + { + case 0: + Debug.WriteLine(string.Format("must have between 3 and {0} vertices.\n", + Settings.MaxPolygonVertices)); + break; + case 1: + Debug.WriteLine("must be convex.\n"); + break; + case 2: + Debug.WriteLine("must be simple (cannot intersect itself).\n"); + break; + case 3: + Debug.WriteLine("area is too small.\n"); + break; + case 4: + Debug.WriteLine("sides are too close to parallel.\n"); + break; + case 5: + Debug.WriteLine("polygon is too thin.\n"); + break; + case 6: + Debug.WriteLine("core shape generation would move edge past centroid (too thin).\n"); + break; + default: + Debug.WriteLine("don't know why.\n"); + break; + } + } + return error != -1; + } + + // From Eric Jordan's convex decomposition library + + /// + /// Trace the edge of a non-simple polygon and return a simple polygon. + /// + /// Method: + /// Start at vertex with minimum y (pick maximum x one if there are two). + /// We aim our "lastDir" vector at (1.0, 0) + /// We look at the two rays going off from our start vertex, and follow whichever + /// has the smallest angle (in -Pi . Pi) wrt lastDir ("rightest" turn) + /// Loop until we hit starting vertex: + /// We add our current vertex to the list. + /// We check the seg from current vertex to next vertex for intersections + /// - if no intersections, follow to next vertex and continue + /// - if intersections, pick one with minimum distance + /// - if more than one, pick one with "rightest" next point (two possibilities for each) + /// + /// The vertices. + /// + public Vertices TraceEdge(Vertices verts) + { + PolyNode[] nodes = new PolyNode[verts.Count * verts.Count]; + //overkill, but sufficient (order of mag. is right) + int nNodes = 0; + + //Add base nodes (raw outline) + for (int i = 0; i < verts.Count; ++i) + { + Vector2 pos = new Vector2(verts[i].X, verts[i].Y); + nodes[i].Position = pos; + ++nNodes; + int iplus = (i == verts.Count - 1) ? 0 : i + 1; + int iminus = (i == 0) ? verts.Count - 1 : i - 1; + nodes[i].AddConnection(nodes[iplus]); + nodes[i].AddConnection(nodes[iminus]); + } + + //Process intersection nodes - horribly inefficient + bool dirty = true; + int counter = 0; + while (dirty) + { + dirty = false; + for (int i = 0; i < nNodes; ++i) + { + for (int j = 0; j < nodes[i].NConnected; ++j) + { + for (int k = 0; k < nNodes; ++k) + { + if (k == i || nodes[k] == nodes[i].Connected[j]) continue; + for (int l = 0; l < nodes[k].NConnected; ++l) + { + if (nodes[k].Connected[l] == nodes[i].Connected[j] || + nodes[k].Connected[l] == nodes[i]) continue; + + //Check intersection + Vector2 intersectPt; + + bool crosses = LineTools.LineIntersect(nodes[i].Position, nodes[i].Connected[j].Position, + nodes[k].Position, nodes[k].Connected[l].Position, + out intersectPt); + if (crosses) + { + dirty = true; + //Destroy and re-hook connections at crossing point + PolyNode connj = nodes[i].Connected[j]; + PolyNode connl = nodes[k].Connected[l]; + nodes[i].Connected[j].RemoveConnection(nodes[i]); + nodes[i].RemoveConnection(connj); + nodes[k].Connected[l].RemoveConnection(nodes[k]); + nodes[k].RemoveConnection(connl); + nodes[nNodes] = new PolyNode(intersectPt); + nodes[nNodes].AddConnection(nodes[i]); + nodes[i].AddConnection(nodes[nNodes]); + nodes[nNodes].AddConnection(nodes[k]); + nodes[k].AddConnection(nodes[nNodes]); + nodes[nNodes].AddConnection(connj); + connj.AddConnection(nodes[nNodes]); + nodes[nNodes].AddConnection(connl); + connl.AddConnection(nodes[nNodes]); + ++nNodes; + goto SkipOut; + } + } + } + } + } + SkipOut: + ++counter; + } + + //Collapse duplicate points + bool foundDupe = true; + int nActive = nNodes; + while (foundDupe) + { + foundDupe = false; + for (int i = 0; i < nNodes; ++i) + { + if (nodes[i].NConnected == 0) continue; + for (int j = i + 1; j < nNodes; ++j) + { + if (nodes[j].NConnected == 0) continue; + Vector2 diff = nodes[i].Position - nodes[j].Position; + if (diff.LengthSquared() <= Settings.Epsilon * Settings.Epsilon) + { + if (nActive <= 3) + return new Vertices(); + + //printf("Found dupe, %d left\n",nActive); + --nActive; + foundDupe = true; + PolyNode inode = nodes[i]; + PolyNode jnode = nodes[j]; + //Move all of j's connections to i, and orphan j + int njConn = jnode.NConnected; + for (int k = 0; k < njConn; ++k) + { + PolyNode knode = jnode.Connected[k]; + Debug.Assert(knode != jnode); + if (knode != inode) + { + inode.AddConnection(knode); + knode.AddConnection(inode); + } + knode.RemoveConnection(jnode); + } + jnode.NConnected = 0; + } + } + } + } + + //Now walk the edge of the list + + //Find node with minimum y value (max x if equal) + float minY = float.MaxValue; + float maxX = -float.MaxValue; + int minYIndex = -1; + for (int i = 0; i < nNodes; ++i) + { + if (nodes[i].Position.Y < minY && nodes[i].NConnected > 1) + { + minY = nodes[i].Position.Y; + minYIndex = i; + maxX = nodes[i].Position.X; + } + else if (nodes[i].Position.Y == minY && nodes[i].Position.X > maxX && nodes[i].NConnected > 1) + { + minYIndex = i; + maxX = nodes[i].Position.X; + } + } + + Vector2 origDir = new Vector2(1.0f, 0.0f); + Vector2[] resultVecs = new Vector2[4 * nNodes]; + // nodes may be visited more than once, unfortunately - change to growable array! + int nResultVecs = 0; + PolyNode currentNode = nodes[minYIndex]; + PolyNode startNode = currentNode; + Debug.Assert(currentNode.NConnected > 0); + PolyNode nextNode = currentNode.GetRightestConnection(origDir); + if (nextNode == null) + { + Vertices vertices = new Vertices(nResultVecs); + + for (int i = 0; i < nResultVecs; ++i) + { + vertices.Add(resultVecs[i]); + } + + return vertices; + } + + // Borked, clean up our mess and return + resultVecs[0] = startNode.Position; + ++nResultVecs; + while (nextNode != startNode) + { + if (nResultVecs > 4 * nNodes) + { + Debug.Assert(false); + } + resultVecs[nResultVecs++] = nextNode.Position; + PolyNode oldNode = currentNode; + currentNode = nextNode; + nextNode = currentNode.GetRightestConnection(oldNode); + if (nextNode == null) + { + Vertices vertices = new Vertices(nResultVecs); + for (int i = 0; i < nResultVecs; ++i) + { + vertices.Add(resultVecs[i]); + } + return vertices; + } + // There was a problem, so jump out of the loop and use whatever garbage we've generated so far + } + + return new Vertices(); + } + + private class PolyNode + { + private const int MaxConnected = 32; + + /* + * Given sines and cosines, tells if A's angle is less than B's on -Pi, Pi + * (in other words, is A "righter" than B) + */ + public PolyNode[] Connected = new PolyNode[MaxConnected]; + public int NConnected; + public Vector2 Position; + + public PolyNode(Vector2 pos) + { + Position = pos; + NConnected = 0; + } + + private bool IsRighter(float sinA, float cosA, float sinB, float cosB) + { + if (sinA < 0) + { + if (sinB > 0 || cosA <= cosB) return true; + else return false; + } + else + { + if (sinB < 0 || cosA <= cosB) return false; + else return true; + } + } + + public void AddConnection(PolyNode toMe) + { + Debug.Assert(NConnected < MaxConnected); + + // Ignore duplicate additions + for (int i = 0; i < NConnected; ++i) + { + if (Connected[i] == toMe) return; + } + Connected[NConnected] = toMe; + ++NConnected; + } + + public void RemoveConnection(PolyNode fromMe) + { + bool isFound = false; + int foundIndex = -1; + for (int i = 0; i < NConnected; ++i) + { + if (fromMe == Connected[i]) + { + //.position == connected[i].position){ + isFound = true; + foundIndex = i; + break; + } + } + Debug.Assert(isFound); + --NConnected; + for (int i = foundIndex; i < NConnected; ++i) + { + Connected[i] = Connected[i + 1]; + } + } + + public PolyNode GetRightestConnection(PolyNode incoming) + { + if (NConnected == 0) Debug.Assert(false); // This means the connection graph is inconsistent + if (NConnected == 1) + { + //b2Assert(false); + // Because of the possibility of collapsing nearby points, + // we may end up with "spider legs" dangling off of a region. + // The correct behavior here is to turn around. + return incoming; + } + Vector2 inDir = Position - incoming.Position; + + float inLength = inDir.Length(); + inDir.Normalize(); + + Debug.Assert(inLength > Settings.Epsilon); + + PolyNode result = null; + for (int i = 0; i < NConnected; ++i) + { + if (Connected[i] == incoming) continue; + Vector2 testDir = Connected[i].Position - Position; + float testLengthSqr = testDir.LengthSquared(); + testDir.Normalize(); + Debug.Assert(testLengthSqr >= Settings.Epsilon * Settings.Epsilon); + float myCos = Vector2.Dot(inDir, testDir); + float mySin = MathUtils.Cross(inDir, testDir); + if (result != null) + { + Vector2 resultDir = result.Position - Position; + resultDir.Normalize(); + float resCos = Vector2.Dot(inDir, resultDir); + float resSin = MathUtils.Cross(inDir, resultDir); + if (IsRighter(mySin, myCos, resSin, resCos)) + { + result = Connected[i]; + } + } + else + { + result = Connected[i]; + } + } + + Debug.Assert(result != null); + + return result; + } + + public PolyNode GetRightestConnection(Vector2 incomingDir) + { + Vector2 diff = Position - incomingDir; + PolyNode temp = new PolyNode(diff); + PolyNode res = GetRightestConnection(temp); + Debug.Assert(res != null); + return res; + } + } + + 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(); + } + + /// + /// 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; + } + } +} \ No newline at end of file diff --git a/axios/Controllers/AbstractForceController.cs b/axios/Controllers/AbstractForceController.cs new file mode 100644 index 0000000..47adcc7 --- /dev/null +++ b/axios/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/axios/Controllers/BuoyancyController.cs b/axios/Controllers/BuoyancyController.cs new file mode 100644 index 0000000..ee3393c --- /dev/null +++ b/axios/Controllers/BuoyancyController.cs @@ -0,0 +1,135 @@ +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(_normal, _offset, 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/axios/Controllers/Controller.cs b/axios/Controllers/Controller.cs new file mode 100644 index 0000000..f285eff --- /dev/null +++ b/axios/Controllers/Controller.cs @@ -0,0 +1,71 @@ +using System; +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/axios/Controllers/GravityController.cs b/axios/Controllers/GravityController.cs new file mode 100644 index 0000000..efba5f0 --- /dev/null +++ b/axios/Controllers/GravityController.cs @@ -0,0 +1,117 @@ +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 List Bodies = new List(); + public List Points = new List(); + + public GravityController(float strength) + : base(ControllerType.GravityController) + { + Strength = strength; + MaxRadius = float.MaxValue; + } + + public GravityController(float strength, float maxRadius, float minRadius) + : base(ControllerType.GravityController) + { + MinRadius = minRadius; + MaxRadius = maxRadius; + Strength = strength; + } + + public float MinRadius { get; set; } + public float MaxRadius { get; set; } + public float Strength { get; set; } + public GravityType GravityType { get; set; } + + public override void Update(float dt) + { + Vector2 f = Vector2.Zero; + + foreach (Body body1 in World.BodyList) + { + if (!IsActiveOn(body1)) + continue; + + foreach (Body body2 in Bodies) + { + if (body1 == body2 || (body1.IsStatic && body2.IsStatic) || !body2.Enabled) + continue; + + Vector2 d = body2.WorldCenter - body1.WorldCenter; + float r2 = d.LengthSquared(); + + if (r2 < Settings.Epsilon) + continue; + + float r = d.Length(); + + if (r >= MaxRadius || r <= MinRadius) + continue; + + switch (GravityType) + { + case GravityType.DistanceSquared: + f = Strength / r2 / (float)Math.Sqrt(r2) * body1.Mass * body2.Mass * d; + break; + case GravityType.Linear: + f = Strength / r2 * body1.Mass * body2.Mass * d; + break; + } + + body1.ApplyForce(ref f); + Vector2.Negate(ref f, out f); + body2.ApplyForce(ref f); + } + + foreach (Vector2 point in Points) + { + Vector2 d = point - body1.Position; + float r2 = d.LengthSquared(); + + if (r2 < Settings.Epsilon) + continue; + + float r = d.Length(); + + if (r >= MaxRadius || r <= MinRadius) + continue; + + switch (GravityType) + { + case GravityType.DistanceSquared: + f = Strength / r2 / (float)Math.Sqrt(r2) * body1.Mass * d; + break; + case GravityType.Linear: + f = Strength / r2 * body1.Mass * d; + break; + } + + body1.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/axios/Controllers/SimpleWindForce.cs b/axios/Controllers/SimpleWindForce.cs new file mode 100644 index 0000000..d783d81 --- /dev/null +++ b/axios/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/axios/Controllers/VelocityLimitController.cs b/axios/Controllers/VelocityLimitController.cs new file mode 100644 index 0000000..4348cce --- /dev/null +++ b/axios/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.LinearVelocityInternal.X; + float translationY = dt * body.LinearVelocityInternal.Y; + float result = translationX * translationX + translationY * translationY; + + if (result > dt * _maxLinearSqared) + { + float sq = (float)Math.Sqrt(result); + + float ratio = _maxLinearVelocity / sq; + body.LinearVelocityInternal.X *= ratio; + body.LinearVelocityInternal.Y *= ratio; + } + } + + if (LimitAngularVelocity) + { + //Rotation + float rotation = dt * body.AngularVelocityInternal; + if (rotation * rotation > _maxAngularSqared) + { + float ratio = _maxAngularVelocity / Math.Abs(rotation); + body.AngularVelocityInternal *= 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/axios/DebugView.cs b/axios/DebugView.cs new file mode 100644 index 0000000..6e099e2 --- /dev/null +++ b/axios/DebugView.cs @@ -0,0 +1,185 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must 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 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 DebugView + { + protected DebugView(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); + + /// + /// 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/axios/DebugViewXNA.cs b/axios/DebugViewXNA.cs new file mode 100644 index 0000000..d21a577 --- /dev/null +++ b/axios/DebugViewXNA.cs @@ -0,0 +1,875 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Linq; +using FarseerPhysics.Collision; +using FarseerPhysics.Collision.Shapes; +using FarseerPhysics.Common; +using FarseerPhysics.Controllers; +using FarseerPhysics.Dynamics; +using FarseerPhysics.Dynamics.Contacts; +using FarseerPhysics.Dynamics.Joints; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Content; +using Microsoft.Xna.Framework.Graphics; + +namespace FarseerPhysics.DebugViews +{ + /// + /// A debug view that works in XNA. + /// A debug view shows you what happens inside the physics engine. You can view + /// bodies, joints, fixtures and more. + /// + public class DebugViewXNA : DebugView, IDisposable + { + //Drawing + private PrimitiveBatch _primitiveBatch; + private SpriteBatch _batch; + private SpriteFont _font; + private GraphicsDevice _device; + private Vector2[] _tempVertices = new Vector2[Settings.MaxPolygonVertices]; + private List _stringData; + + private Matrix _localProjection; + private Matrix _localView; + + //Shapes + public Color DefaultShapeColor = new Color(0.9f, 0.7f, 0.7f); + public Color InactiveShapeColor = new Color(0.5f, 0.5f, 0.3f); + public Color KinematicShapeColor = new Color(0.5f, 0.5f, 0.9f); + public Color SleepingShapeColor = new Color(0.6f, 0.6f, 0.6f); + public Color StaticShapeColor = new Color(0.5f, 0.9f, 0.5f); + public Color TextColor = Color.White; + + //Contacts + private int _pointCount; + private const int MaxContactPoints = 2048; + private ContactPoint[] _points = new ContactPoint[MaxContactPoints]; + + //Debug panel +#if XBOX + public Vector2 DebugPanelPosition = new Vector2(55, 100); +#else + public Vector2 DebugPanelPosition = new Vector2(40, 100); +#endif + private int _max; + private int _avg; + private int _min; + + //Performance graph + public bool AdaptiveLimits = true; + public int ValuesToGraph = 500; + public int MinimumValue; + public int MaximumValue = 1000; + private List _graphValues = new List(); + +#if XBOX + public Rectangle PerformancePanelBounds = new Rectangle(265, 100, 200, 100); +#else + public Rectangle PerformancePanelBounds = new Rectangle(250, 100, 200, 100); +#endif + private Vector2[] _background = new Vector2[4]; + public bool Enabled = true; + +#if XBOX || WINDOWS_PHONE + public const int CircleSegments = 16; +#else + public const int CircleSegments = 32; +#endif + + public DebugViewXNA(World world) + : base(world) + { + world.ContactManager.PreSolve += PreSolve; + + //Default flags + AppendFlags(DebugViewFlags.Shape); + AppendFlags(DebugViewFlags.Controllers); + AppendFlags(DebugViewFlags.Joint); + } + + public void BeginCustomDraw(ref Matrix projection, ref Matrix view) + { + _primitiveBatch.Begin(ref projection, ref view); + } + + public void EndCustomDraw() + { + _primitiveBatch.End(); + } + + #region IDisposable Members + + public void Dispose() + { + World.ContactManager.PreSolve -= PreSolve; + } + + #endregion + + private void PreSolve(Contact contact, ref Manifold oldManifold) + { + if ((Flags & DebugViewFlags.ContactPoints) == DebugViewFlags.ContactPoints) + { + Manifold manifold = contact.Manifold; + + if (manifold.PointCount == 0) + { + return; + } + + Fixture fixtureA = contact.FixtureA; + + FixedArray2 state1, state2; + Collision.Collision.GetPointStates(out state1, out state2, ref oldManifold, ref manifold); + + FixedArray2 points; + Vector2 normal; + contact.GetWorldManifold(out normal, out points); + + for (int i = 0; i < manifold.PointCount && _pointCount < MaxContactPoints; ++i) + { + if (fixtureA == null) + { + _points[i] = new ContactPoint(); + } + ContactPoint cp = _points[_pointCount]; + cp.Position = points[i]; + cp.Normal = normal; + cp.State = state2[i]; + _points[_pointCount] = cp; + ++_pointCount; + } + } + } + + /// + /// Call this to draw shapes and other debug draw data. + /// + private void DrawDebugData() + { + if ((Flags & DebugViewFlags.Shape) == DebugViewFlags.Shape) + { + foreach (Body b in World.BodyList) + { + Transform xf; + b.GetTransform(out xf); + foreach (Fixture f in b.FixtureList) + { + if (b.Enabled == false) + { + DrawShape(f, xf, InactiveShapeColor); + } + else if (b.BodyType == BodyType.Static) + { + DrawShape(f, xf, StaticShapeColor); + } + else if (b.BodyType == BodyType.Kinematic) + { + DrawShape(f, xf, KinematicShapeColor); + } + else if (b.Awake == false) + { + DrawShape(f, xf, SleepingShapeColor); + } + else + { + DrawShape(f, xf, DefaultShapeColor); + } + } + } + } + if ((Flags & DebugViewFlags.ContactPoints) == DebugViewFlags.ContactPoints) + { + const float axisScale = 0.3f; + + for (int i = 0; i < _pointCount; ++i) + { + ContactPoint point = _points[i]; + + if (point.State == PointState.Add) + { + // Add + DrawPoint(point.Position, 0.1f, new Color(0.3f, 0.95f, 0.3f)); + } + else if (point.State == PointState.Persist) + { + // Persist + DrawPoint(point.Position, 0.1f, new Color(0.3f, 0.3f, 0.95f)); + } + + if ((Flags & DebugViewFlags.ContactNormals) == DebugViewFlags.ContactNormals) + { + Vector2 p1 = point.Position; + Vector2 p2 = p1 + axisScale * point.Normal; + DrawSegment(p1, p2, new Color(0.4f, 0.9f, 0.4f)); + } + } + _pointCount = 0; + } + if ((Flags & DebugViewFlags.PolygonPoints) == DebugViewFlags.PolygonPoints) + { + foreach (Body body in World.BodyList) + { + foreach (Fixture f in body.FixtureList) + { + PolygonShape polygon = f.Shape as PolygonShape; + if (polygon != null) + { + Transform xf; + body.GetTransform(out xf); + + for (int i = 0; i < polygon.Vertices.Count; i++) + { + Vector2 tmp = MathUtils.Multiply(ref xf, polygon.Vertices[i]); + DrawPoint(tmp, 0.1f, Color.Red); + } + } + } + } + } + if ((Flags & DebugViewFlags.Joint) == DebugViewFlags.Joint) + { + foreach (Joint j in World.JointList) + { + DrawJoint(j); + } + } + if ((Flags & DebugViewFlags.Pair) == DebugViewFlags.Pair) + { + Color color = new Color(0.3f, 0.9f, 0.9f); + for (int i = 0; i < World.ContactManager.ContactList.Count; i++) + { + Contact c = World.ContactManager.ContactList[i]; + Fixture fixtureA = c.FixtureA; + Fixture fixtureB = c.FixtureB; + + AABB aabbA; + fixtureA.GetAABB(out aabbA, 0); + AABB aabbB; + fixtureB.GetAABB(out aabbB, 0); + + Vector2 cA = aabbA.Center; + Vector2 cB = aabbB.Center; + + DrawSegment(cA, cB, color); + } + } + if ((Flags & DebugViewFlags.AABB) == DebugViewFlags.AABB) + { + Color color = new Color(0.9f, 0.3f, 0.9f); + IBroadPhase bp = World.ContactManager.BroadPhase; + + foreach (Body b in World.BodyList) + { + if (b.Enabled == false) + { + continue; + } + + foreach (Fixture f in b.FixtureList) + { + for (int t = 0; t < f.ProxyCount; ++t) + { + FixtureProxy proxy = f.Proxies[t]; + AABB aabb; + bp.GetFatAABB(proxy.ProxyId, out aabb); + + DrawAABB(ref aabb, color); + } + } + } + } + if ((Flags & DebugViewFlags.CenterOfMass) == DebugViewFlags.CenterOfMass) + { + foreach (Body b in World.BodyList) + { + Transform xf; + b.GetTransform(out xf); + xf.Position = b.WorldCenter; + DrawTransform(ref xf); + } + } + if ((Flags & DebugViewFlags.Controllers) == DebugViewFlags.Controllers) + { + for (int i = 0; i < World.ControllerList.Count; i++) + { + Controller controller = World.ControllerList[i]; + + BuoyancyController buoyancy = controller as BuoyancyController; + if (buoyancy != null) + { + AABB container = buoyancy.Container; + DrawAABB(ref container, Color.LightBlue); + } + } + } + if ((Flags & DebugViewFlags.DebugPanel) == DebugViewFlags.DebugPanel) + { + DrawDebugPanel(); + } + } + + private void DrawPerformanceGraph() + { + _graphValues.Add(World.UpdateTime); + + if (_graphValues.Count > ValuesToGraph + 1) + _graphValues.RemoveAt(0); + + float x = PerformancePanelBounds.X; + float deltaX = PerformancePanelBounds.Width / (float)ValuesToGraph; + float yScale = PerformancePanelBounds.Bottom - (float)PerformancePanelBounds.Top; + + // we must have at least 2 values to start rendering + if (_graphValues.Count > 2) + { + _max = (int)_graphValues.Max(); + _avg = (int)_graphValues.Average(); + _min = (int)_graphValues.Min(); + + if (AdaptiveLimits) + { + MaximumValue = _max; + MinimumValue = 0; + } + + // start at last value (newest value added) + // continue until no values are left + for (int i = _graphValues.Count - 1; i > 0; i--) + { + float y1 = PerformancePanelBounds.Bottom - + ((_graphValues[i] / (MaximumValue - MinimumValue)) * yScale); + float y2 = PerformancePanelBounds.Bottom - + ((_graphValues[i - 1] / (MaximumValue - MinimumValue)) * yScale); + + Vector2 x1 = + new Vector2(MathHelper.Clamp(x, PerformancePanelBounds.Left, PerformancePanelBounds.Right), + MathHelper.Clamp(y1, PerformancePanelBounds.Top, PerformancePanelBounds.Bottom)); + + Vector2 x2 = + new Vector2( + MathHelper.Clamp(x + deltaX, PerformancePanelBounds.Left, PerformancePanelBounds.Right), + MathHelper.Clamp(y2, PerformancePanelBounds.Top, PerformancePanelBounds.Bottom)); + + DrawSegment(x1, x2, Color.LightGreen); + + x += deltaX; + } + } + + DrawString(PerformancePanelBounds.Right + 10, PerformancePanelBounds.Top, "Max: " + _max); + DrawString(PerformancePanelBounds.Right + 10, PerformancePanelBounds.Center.Y - 7, "Avg: " + _avg); + DrawString(PerformancePanelBounds.Right + 10, PerformancePanelBounds.Bottom - 15, "Min: " + _min); + + //Draw background. + _background[0] = new Vector2(PerformancePanelBounds.X, PerformancePanelBounds.Y); + _background[1] = new Vector2(PerformancePanelBounds.X, + PerformancePanelBounds.Y + PerformancePanelBounds.Height); + _background[2] = new Vector2(PerformancePanelBounds.X + PerformancePanelBounds.Width, + PerformancePanelBounds.Y + PerformancePanelBounds.Height); + _background[3] = new Vector2(PerformancePanelBounds.X + PerformancePanelBounds.Width, + PerformancePanelBounds.Y); + + DrawSolidPolygon(_background, 4, Color.DarkGray, true); + } + + private void DrawDebugPanel() + { + int fixtures = 0; + for (int i = 0; i < World.BodyList.Count; i++) + { + fixtures += World.BodyList[i].FixtureList.Count; + } + + int x = (int)DebugPanelPosition.X; + int y = (int)DebugPanelPosition.Y; + + DrawString(x, y, "Objects:" + + "\n- Bodies: " + World.BodyList.Count + + "\n- Fixtures: " + fixtures + + "\n- Contacts: " + World.ContactList.Count + + "\n- Joints: " + World.JointList.Count + + "\n- Controllers: " + World.ControllerList.Count + + "\n- Proxies: " + World.ProxyCount); + + DrawString(x + 110, y, "Update time:" + + "\n- Body: " + World.SolveUpdateTime + + "\n- Contact: " + World.ContactsUpdateTime + + "\n- CCD: " + World.ContinuousPhysicsTime + + "\n- Joint: " + World.Island.JointUpdateTime + + "\n- Controller: " + World.ControllersUpdateTime + + "\n- Total: " + World.UpdateTime); + } + + public void DrawAABB(ref AABB aabb, Color color) + { + Vector2[] verts = new Vector2[4]; + verts[0] = new Vector2(aabb.LowerBound.X, aabb.LowerBound.Y); + verts[1] = new Vector2(aabb.UpperBound.X, aabb.LowerBound.Y); + verts[2] = new Vector2(aabb.UpperBound.X, aabb.UpperBound.Y); + verts[3] = new Vector2(aabb.LowerBound.X, aabb.UpperBound.Y); + + DrawPolygon(verts, 4, color); + } + + private void DrawJoint(Joint joint) + { + if (!joint.Enabled) + return; + + Body b1 = joint.BodyA; + Body b2 = joint.BodyB; + Transform xf1, xf2; + b1.GetTransform(out xf1); + + Vector2 x2 = Vector2.Zero; + + // WIP David + if (!joint.IsFixedType()) + { + b2.GetTransform(out xf2); + x2 = xf2.Position; + } + + Vector2 p1 = joint.WorldAnchorA; + Vector2 p2 = joint.WorldAnchorB; + Vector2 x1 = xf1.Position; + + Color color = new Color(0.5f, 0.8f, 0.8f); + + switch (joint.JointType) + { + case JointType.Distance: + DrawSegment(p1, p2, color); + break; + case JointType.Pulley: + PulleyJoint pulley = (PulleyJoint)joint; + Vector2 s1 = pulley.GroundAnchorA; + Vector2 s2 = pulley.GroundAnchorB; + DrawSegment(s1, p1, color); + DrawSegment(s2, p2, color); + DrawSegment(s1, s2, color); + break; + case JointType.FixedMouse: + DrawPoint(p1, 0.5f, new Color(0.0f, 1.0f, 0.0f)); + DrawSegment(p1, p2, new Color(0.8f, 0.8f, 0.8f)); + break; + case JointType.Revolute: + //DrawSegment(x2, p1, color); + DrawSegment(p2, p1, color); + DrawSolidCircle(p2, 0.1f, Vector2.Zero, Color.Red); + DrawSolidCircle(p1, 0.1f, Vector2.Zero, Color.Blue); + break; + case JointType.FixedAngle: + //Should not draw anything. + break; + case JointType.FixedRevolute: + DrawSegment(x1, p1, color); + DrawSolidCircle(p1, 0.1f, Vector2.Zero, Color.Pink); + break; + case JointType.FixedLine: + DrawSegment(x1, p1, color); + DrawSegment(p1, p2, color); + break; + case JointType.FixedDistance: + DrawSegment(x1, p1, color); + DrawSegment(p1, p2, color); + break; + case JointType.FixedPrismatic: + DrawSegment(x1, p1, color); + DrawSegment(p1, p2, color); + break; + case JointType.Gear: + DrawSegment(x1, x2, color); + break; + //case JointType.Weld: + // break; + default: + DrawSegment(x1, p1, color); + DrawSegment(p1, p2, color); + DrawSegment(x2, p2, color); + break; + } + } + + public void DrawShape(Fixture fixture, Transform xf, Color color) + { + switch (fixture.ShapeType) + { + case ShapeType.Circle: + { + CircleShape circle = (CircleShape)fixture.Shape; + + Vector2 center = MathUtils.Multiply(ref xf, circle.Position); + float radius = circle.Radius; + Vector2 axis = xf.R.Col1; + + DrawSolidCircle(center, radius, axis, color); + } + break; + + case ShapeType.Polygon: + { + PolygonShape poly = (PolygonShape)fixture.Shape; + int vertexCount = poly.Vertices.Count; + Debug.Assert(vertexCount <= Settings.MaxPolygonVertices); + + for (int i = 0; i < vertexCount; ++i) + { + _tempVertices[i] = MathUtils.Multiply(ref xf, poly.Vertices[i]); + } + + DrawSolidPolygon(_tempVertices, vertexCount, color); + } + break; + + + case ShapeType.Edge: + { + EdgeShape edge = (EdgeShape)fixture.Shape; + Vector2 v1 = MathUtils.Multiply(ref xf, edge.Vertex1); + Vector2 v2 = MathUtils.Multiply(ref xf, edge.Vertex2); + DrawSegment(v1, v2, color); + } + break; + + case ShapeType.Loop: + { + LoopShape loop = (LoopShape)fixture.Shape; + int count = loop.Vertices.Count; + + Vector2 v1 = MathUtils.Multiply(ref xf, loop.Vertices[count - 1]); + DrawCircle(v1, 0.05f, color); + for (int i = 0; i < count; ++i) + { + Vector2 v2 = MathUtils.Multiply(ref xf, loop.Vertices[i]); + DrawSegment(v1, v2, color); + v1 = v2; + } + } + break; + } + } + + public override void DrawPolygon(Vector2[] vertices, int count, float red, float green, float blue) + { + DrawPolygon(vertices, count, new Color(red, green, blue)); + } + + public void DrawPolygon(Vector2[] vertices, int count, Color color) + { + if (!_primitiveBatch.IsReady()) + { + throw new InvalidOperationException("BeginCustomDraw must be called before drawing anything."); + } + for (int i = 0; i < count - 1; i++) + { + _primitiveBatch.AddVertex(vertices[i], color, PrimitiveType.LineList); + _primitiveBatch.AddVertex(vertices[i + 1], color, PrimitiveType.LineList); + } + + _primitiveBatch.AddVertex(vertices[count - 1], color, PrimitiveType.LineList); + _primitiveBatch.AddVertex(vertices[0], color, PrimitiveType.LineList); + } + + public override void DrawSolidPolygon(Vector2[] vertices, int count, float red, float green, float blue) + { + DrawSolidPolygon(vertices, count, new Color(red, green, blue), true); + } + + public void DrawSolidPolygon(Vector2[] vertices, int count, Color color) + { + DrawSolidPolygon(vertices, count, color, true); + } + + public void DrawSolidPolygon(Vector2[] vertices, int count, Color color, bool outline) + { + if (!_primitiveBatch.IsReady()) + { + throw new InvalidOperationException("BeginCustomDraw must be called before drawing anything."); + } + if (count == 2) + { + DrawPolygon(vertices, count, color); + return; + } + + Color colorFill = color * (outline ? 0.5f : 1.0f); + + for (int i = 1; i < count - 1; i++) + { + _primitiveBatch.AddVertex(vertices[0], colorFill, PrimitiveType.TriangleList); + _primitiveBatch.AddVertex(vertices[i], colorFill, PrimitiveType.TriangleList); + _primitiveBatch.AddVertex(vertices[i + 1], colorFill, PrimitiveType.TriangleList); + } + + if (outline) + { + DrawPolygon(vertices, count, color); + } + } + + public override void DrawCircle(Vector2 center, float radius, float red, float green, float blue) + { + DrawCircle(center, radius, new Color(red, green, blue)); + } + + public void DrawCircle(Vector2 center, float radius, Color color) + { + if (!_primitiveBatch.IsReady()) + { + throw new InvalidOperationException("BeginCustomDraw must be called before drawing anything."); + } + const double increment = Math.PI * 2.0 / CircleSegments; + double theta = 0.0; + + for (int i = 0; i < CircleSegments; i++) + { + Vector2 v1 = center + radius * new Vector2((float)Math.Cos(theta), (float)Math.Sin(theta)); + Vector2 v2 = center + + radius * + new Vector2((float)Math.Cos(theta + increment), (float)Math.Sin(theta + increment)); + + _primitiveBatch.AddVertex(v1, color, PrimitiveType.LineList); + _primitiveBatch.AddVertex(v2, color, PrimitiveType.LineList); + + theta += increment; + } + } + + public override void DrawSolidCircle(Vector2 center, float radius, Vector2 axis, float red, float green, + float blue) + { + DrawSolidCircle(center, radius, axis, new Color(red, green, blue)); + } + + public void DrawSolidCircle(Vector2 center, float radius, Vector2 axis, Color color) + { + if (!_primitiveBatch.IsReady()) + { + throw new InvalidOperationException("BeginCustomDraw must be called before drawing anything."); + } + const double increment = Math.PI * 2.0 / CircleSegments; + double theta = 0.0; + + Color colorFill = color * 0.5f; + + Vector2 v0 = center + radius * new Vector2((float)Math.Cos(theta), (float)Math.Sin(theta)); + theta += increment; + + for (int i = 1; i < CircleSegments - 1; i++) + { + Vector2 v1 = center + radius * new Vector2((float)Math.Cos(theta), (float)Math.Sin(theta)); + Vector2 v2 = center + + radius * + new Vector2((float)Math.Cos(theta + increment), (float)Math.Sin(theta + increment)); + + _primitiveBatch.AddVertex(v0, colorFill, PrimitiveType.TriangleList); + _primitiveBatch.AddVertex(v1, colorFill, PrimitiveType.TriangleList); + _primitiveBatch.AddVertex(v2, colorFill, PrimitiveType.TriangleList); + + theta += increment; + } + DrawCircle(center, radius, color); + + DrawSegment(center, center + axis * radius, color); + } + + public override void DrawSegment(Vector2 start, Vector2 end, float red, float green, float blue) + { + DrawSegment(start, end, new Color(red, green, blue)); + } + + public void DrawSegment(Vector2 start, Vector2 end, Color color) + { + if (!_primitiveBatch.IsReady()) + { + throw new InvalidOperationException("BeginCustomDraw must be called before drawing anything."); + } + _primitiveBatch.AddVertex(start, color, PrimitiveType.LineList); + _primitiveBatch.AddVertex(end, color, PrimitiveType.LineList); + } + + public override void DrawTransform(ref Transform transform) + { + const float axisScale = 0.4f; + Vector2 p1 = transform.Position; + + Vector2 p2 = p1 + axisScale * transform.R.Col1; + DrawSegment(p1, p2, Color.Red); + + p2 = p1 + axisScale * transform.R.Col2; + DrawSegment(p1, p2, Color.Green); + } + + public void DrawPoint(Vector2 p, float size, Color color) + { + Vector2[] verts = new Vector2[4]; + float hs = size / 2.0f; + verts[0] = p + new Vector2(-hs, -hs); + verts[1] = p + new Vector2(hs, -hs); + verts[2] = p + new Vector2(hs, hs); + verts[3] = p + new Vector2(-hs, hs); + + DrawSolidPolygon(verts, 4, color, true); + } + + public void DrawString(int x, int y, string s, params object[] args) + { + _stringData.Add(new StringData(x, y, s, args, TextColor)); + } + + public void DrawArrow(Vector2 start, Vector2 end, float length, float width, bool drawStartIndicator, + Color color) + { + // Draw connection segment between start- and end-point + DrawSegment(start, end, color); + + // Precalculate halfwidth + float halfWidth = width / 2; + + // Create directional reference + Vector2 rotation = (start - end); + rotation.Normalize(); + + // Calculate angle of directional vector + float angle = (float)Math.Atan2(rotation.X, -rotation.Y); + // Create matrix for rotation + Matrix rotMatrix = Matrix.CreateRotationZ(angle); + // Create translation matrix for end-point + Matrix endMatrix = Matrix.CreateTranslation(end.X, end.Y, 0); + + // Setup arrow end shape + Vector2[] verts = new Vector2[3]; + verts[0] = new Vector2(0, 0); + verts[1] = new Vector2(-halfWidth, -length); + verts[2] = new Vector2(halfWidth, -length); + + // Rotate end shape + Vector2.Transform(verts, ref rotMatrix, verts); + // Translate end shape + Vector2.Transform(verts, ref endMatrix, verts); + + // Draw arrow end shape + DrawSolidPolygon(verts, 3, color, false); + + if (drawStartIndicator) + { + // Create translation matrix for start + Matrix startMatrix = Matrix.CreateTranslation(start.X, start.Y, 0); + // Setup arrow start shape + Vector2[] baseVerts = new Vector2[4]; + baseVerts[0] = new Vector2(-halfWidth, length / 4); + baseVerts[1] = new Vector2(halfWidth, length / 4); + baseVerts[2] = new Vector2(halfWidth, 0); + baseVerts[3] = new Vector2(-halfWidth, 0); + + // Rotate start shape + Vector2.Transform(baseVerts, ref rotMatrix, baseVerts); + // Translate start shape + Vector2.Transform(baseVerts, ref startMatrix, baseVerts); + // Draw start shape + DrawSolidPolygon(baseVerts, 4, color, false); + } + } + + public void RenderDebugData(ref Matrix projection, ref Matrix view) + { + if (!Enabled) + { + return; + } + + //Nothing is enabled - don't draw the debug view. + if (Flags == 0) + return; + + _device.RasterizerState = RasterizerState.CullNone; + _device.DepthStencilState = DepthStencilState.Default; + + _primitiveBatch.Begin(ref projection, ref view); + DrawDebugData(); + _primitiveBatch.End(); + + if ((Flags & DebugViewFlags.PerformanceGraph) == DebugViewFlags.PerformanceGraph) + { + _primitiveBatch.Begin(ref _localProjection, ref _localView); + DrawPerformanceGraph(); + _primitiveBatch.End(); + } + + // begin the sprite batch effect + _batch.Begin(SpriteSortMode.Deferred, BlendState.AlphaBlend); + + // draw any strings we have + for (int i = 0; i < _stringData.Count; i++) + { + _batch.DrawString(_font, string.Format(_stringData[i].S, _stringData[i].Args), + new Vector2(_stringData[i].X + 1f, _stringData[i].Y + 1f), Color.Black); + _batch.DrawString(_font, string.Format(_stringData[i].S, _stringData[i].Args), + new Vector2(_stringData[i].X, _stringData[i].Y), _stringData[i].Color); + } + // end the sprite batch effect + _batch.End(); + + _stringData.Clear(); + } + + public void RenderDebugData(ref Matrix projection) + { + if (!Enabled) + { + return; + } + Matrix view = Matrix.Identity; + RenderDebugData(ref projection, ref view); + } + + public void LoadContent(GraphicsDevice device, ContentManager content) + { + // Create a new SpriteBatch, which can be used to draw textures. + _device = device; + _batch = new SpriteBatch(_device); + _primitiveBatch = new PrimitiveBatch(_device, 1000); + _font = content.Load("font"); + _stringData = new List(); + + _localProjection = Matrix.CreateOrthographicOffCenter(0f, _device.Viewport.Width, _device.Viewport.Height, + 0f, 0f, 1f); + _localView = Matrix.Identity; + } + + #region Nested type: ContactPoint + + private struct ContactPoint + { + public Vector2 Normal; + public Vector2 Position; + public PointState State; + } + + #endregion + + #region Nested type: StringData + + private struct StringData + { + public object[] Args; + public Color Color; + public string S; + public int X, Y; + + public StringData(int x, int y, string s, object[] args, Color color) + { + X = x; + Y = y; + S = s; + Args = args; + Color = color; + } + } + + #endregion + } +} \ No newline at end of file diff --git a/axios/DrawingSystem/AssetCreator.cs b/axios/DrawingSystem/AssetCreator.cs new file mode 100644 index 0000000..9c81443 --- /dev/null +++ b/axios/DrawingSystem/AssetCreator.cs @@ -0,0 +1,240 @@ +using System; +using System.Collections.Generic; +using FarseerPhysics.Collision; +using FarseerPhysics.Collision.Shapes; +using FarseerPhysics.Common; +using FarseerPhysics.Common.Decomposition; +using FarseerPhysics.Dynamics; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Content; +using Microsoft.Xna.Framework.Graphics; + +namespace FarseerPhysics.SamplesFramework +{ + public enum MaterialType + { + Blank, + Dots, + Squares, + Waves, + Pavement + } + + public class AssetCreator + { + private const int CircleSegments = 32; + + private GraphicsDevice _device; + private BasicEffect _effect; + private Dictionary _materials = new Dictionary(); + + public AssetCreator(GraphicsDevice device) + { + _device = device; + _effect = new BasicEffect(_device); + } + + public static Vector2 CalculateOrigin(Body b) + { + Vector2 lBound = new Vector2(float.MaxValue); + AABB bounds; + Transform trans; + b.GetTransform(out trans); + + for (int i = 0; i < b.FixtureList.Count; ++i) + { + for (int j = 0; j < b.FixtureList[i].Shape.ChildCount; ++j) + { + b.FixtureList[i].Shape.ComputeAABB(out bounds, ref trans, j); + Vector2.Min(ref lBound, ref bounds.LowerBound, out lBound); + } + } + // calculate body offset from its center and add a 1 pixel border + // because we generate the textures a little bigger than the actual body's fixtures + return ConvertUnits.ToDisplayUnits(b.Position - lBound) + new Vector2(1f); + } + + public void LoadContent(ContentManager contentManager) + { + _materials[MaterialType.Blank] = contentManager.Load("Materials/blank"); + _materials[MaterialType.Dots] = contentManager.Load("Materials/dots"); + _materials[MaterialType.Squares] = contentManager.Load("Materials/squares"); + _materials[MaterialType.Waves] = contentManager.Load("Materials/waves"); + _materials[MaterialType.Pavement] = contentManager.Load("Materials/pavement"); + } + + public Texture2D TextureFromShape(Shape shape, MaterialType type, Color color, float materialScale) + { + switch (shape.ShapeType) + { + case ShapeType.Circle: + return CircleTexture(shape.Radius, type, color, materialScale); + case ShapeType.Polygon: + return TextureFromVertices(((PolygonShape) shape).Vertices, type, color, materialScale); + default: + throw new NotSupportedException("The specified shape type is not supported."); + } + } + + public Texture2D TextureFromVertices(Vertices vertices, MaterialType type, Color color, float materialScale) + { + // copy vertices + Vertices verts = new Vertices(vertices); + + // scale to display units (i.e. pixels) for rendering to texture + Vector2 scale = ConvertUnits.ToDisplayUnits(Vector2.One); + verts.Scale(ref scale); + + // translate the boundingbox center to the texture center + // because we use an orthographic projection for rendering later + AABB vertsBounds = verts.GetCollisionBox(); + verts.Translate(-vertsBounds.Center); + + List decomposedVerts; + if (!verts.IsConvex()) + { + decomposedVerts = EarclipDecomposer.ConvexPartition(verts); + } + else + { + decomposedVerts = new List(); + decomposedVerts.Add(verts); + } + List verticesFill = + new List(decomposedVerts.Count); + + materialScale /= _materials[type].Width; + + for (int i = 0; i < decomposedVerts.Count; ++i) + { + verticesFill.Add(new VertexPositionColorTexture[3 * (decomposedVerts[i].Count - 2)]); + for (int j = 0; j < decomposedVerts[i].Count - 2; ++j) + { + // fill vertices + verticesFill[i][3 * j].Position = new Vector3(decomposedVerts[i][0], 0f); + verticesFill[i][3 * j + 1].Position = new Vector3(decomposedVerts[i].NextVertex(j), 0f); + verticesFill[i][3 * j + 2].Position = new Vector3(decomposedVerts[i].NextVertex(j + 1), 0f); + verticesFill[i][3 * j].TextureCoordinate = decomposedVerts[i][0] * materialScale; + verticesFill[i][3 * j + 1].TextureCoordinate = decomposedVerts[i].NextVertex(j) * materialScale; + verticesFill[i][3 * j + 2].TextureCoordinate = decomposedVerts[i].NextVertex(j + 1) * materialScale; + verticesFill[i][3 * j].Color = + verticesFill[i][3 * j + 1].Color = verticesFill[i][3 * j + 2].Color = color; + } + } + + // calculate outline + VertexPositionColor[] verticesOutline = new VertexPositionColor[2 * verts.Count]; + for (int i = 0; i < verts.Count; ++i) + { + verticesOutline[2 * i].Position = new Vector3(verts[i], 0f); + verticesOutline[2 * i + 1].Position = new Vector3(verts.NextVertex(i), 0f); + verticesOutline[2 * i].Color = verticesOutline[2 * i + 1].Color = Color.Black; + } + + Vector2 vertsSize = new Vector2(vertsBounds.UpperBound.X - vertsBounds.LowerBound.X, + vertsBounds.UpperBound.Y - vertsBounds.LowerBound.Y); + return RenderTexture((int)vertsSize.X, (int)vertsSize.Y, + _materials[type], verticesFill, verticesOutline); + } + + public Texture2D CircleTexture(float radius, MaterialType type, Color color, float materialScale) + { + return EllipseTexture(radius, radius, type, color, materialScale); + } + + public Texture2D EllipseTexture(float radiusX, float radiusY, MaterialType type, Color color, + float materialScale) + { + VertexPositionColorTexture[] verticesFill = new VertexPositionColorTexture[3 * (CircleSegments - 2)]; + VertexPositionColor[] verticesOutline = new VertexPositionColor[2 * CircleSegments]; + const float segmentSize = MathHelper.TwoPi / CircleSegments; + float theta = segmentSize; + + radiusX = ConvertUnits.ToDisplayUnits(radiusX); + radiusY = ConvertUnits.ToDisplayUnits(radiusY); + materialScale /= _materials[type].Width; + + Vector2 start = new Vector2(radiusX, 0f); + + for (int i = 0; i < CircleSegments - 2; ++i) + { + Vector2 p1 = new Vector2(radiusX * (float)Math.Cos(theta), radiusY * (float)Math.Sin(theta)); + Vector2 p2 = new Vector2(radiusX * (float)Math.Cos(theta + segmentSize), + radiusY * (float)Math.Sin(theta + segmentSize)); + // fill vertices + verticesFill[3 * i].Position = new Vector3(start, 0f); + verticesFill[3 * i + 1].Position = new Vector3(p1, 0f); + verticesFill[3 * i + 2].Position = new Vector3(p2, 0f); + verticesFill[3 * i].TextureCoordinate = start * materialScale; + verticesFill[3 * i + 1].TextureCoordinate = p1 * materialScale; + verticesFill[3 * i + 2].TextureCoordinate = p2 * materialScale; + verticesFill[3 * i].Color = verticesFill[3 * i + 1].Color = verticesFill[3 * i + 2].Color = color; + + // outline vertices + if (i == 0) + { + verticesOutline[0].Position = new Vector3(start, 0f); + verticesOutline[1].Position = new Vector3(p1, 0f); + verticesOutline[0].Color = verticesOutline[1].Color = Color.Black; + } + if (i == CircleSegments - 3) + { + verticesOutline[2 * CircleSegments - 2].Position = new Vector3(p2, 0f); + verticesOutline[2 * CircleSegments - 1].Position = new Vector3(start, 0f); + verticesOutline[2 * CircleSegments - 2].Color = + verticesOutline[2 * CircleSegments - 1].Color = Color.Black; + } + verticesOutline[2 * i + 2].Position = new Vector3(p1, 0f); + verticesOutline[2 * i + 3].Position = new Vector3(p2, 0f); + verticesOutline[2 * i + 2].Color = verticesOutline[2 * i + 3].Color = Color.Black; + + theta += segmentSize; + } + + return RenderTexture((int)(radiusX * 2f), (int)(radiusY * 2f), + _materials[type], verticesFill, verticesOutline); + } + + private Texture2D RenderTexture(int width, int height, Texture2D material, + VertexPositionColorTexture[] verticesFill, + VertexPositionColor[] verticesOutline) + { + List fill = new List(1); + fill.Add(verticesFill); + return RenderTexture(width, height, material, fill, verticesOutline); + } + + private Texture2D RenderTexture(int width, int height, Texture2D material, + List verticesFill, + VertexPositionColor[] verticesOutline) + { + Matrix halfPixelOffset = Matrix.CreateTranslation(-0.5f, -0.5f, 0f); + PresentationParameters pp = _device.PresentationParameters; + RenderTarget2D texture = new RenderTarget2D(_device, width + 2, height + 2, false, SurfaceFormat.Color, + DepthFormat.None, pp.MultiSampleCount, + RenderTargetUsage.DiscardContents); + _device.RasterizerState = RasterizerState.CullNone; + _device.SamplerStates[0] = SamplerState.LinearWrap; + + _device.SetRenderTarget(texture); + _device.Clear(Color.Transparent); + _effect.Projection = Matrix.CreateOrthographic(width + 2f, -height - 2f, 0f, 1f); + _effect.View = halfPixelOffset; + // render shape; + _effect.TextureEnabled = true; + _effect.Texture = material; + _effect.VertexColorEnabled = true; + _effect.Techniques[0].Passes[0].Apply(); + for (int i = 0; i < verticesFill.Count; ++i) + { + _device.DrawUserPrimitives(PrimitiveType.TriangleList, verticesFill[i], 0, verticesFill[i].Length / 3); + } + // render outline; + _effect.TextureEnabled = false; + _effect.Techniques[0].Passes[0].Apply(); + _device.DrawUserPrimitives(PrimitiveType.LineList, verticesOutline, 0, verticesOutline.Length / 2); + _device.SetRenderTarget(null); + return texture; + } + } +} \ No newline at end of file diff --git a/axios/DrawingSystem/LineBatch.cs b/axios/DrawingSystem/LineBatch.cs new file mode 100644 index 0000000..4230fb1 --- /dev/null +++ b/axios/DrawingSystem/LineBatch.cs @@ -0,0 +1,183 @@ +using System; +using FarseerPhysics.Collision.Shapes; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Graphics; + +namespace FarseerPhysics.SamplesFramework +{ + public class LineBatch : IDisposable + { + private const int DefaultBufferSize = 500; + + // a basic effect, which contains the shaders that we will use to draw our + // primitives. + private BasicEffect _basicEffect; + + // the device that we will issue draw calls to. + private GraphicsDevice _device; + + // hasBegun is flipped to true once Begin is called, and is used to make + // sure users don't call End before Begin is called. + private bool _hasBegun; + + private bool _isDisposed; + private VertexPositionColor[] _lineVertices; + private int _lineVertsCount; + + public LineBatch(GraphicsDevice graphicsDevice) + : this(graphicsDevice, DefaultBufferSize) + { + } + + public LineBatch(GraphicsDevice graphicsDevice, int bufferSize) + { + if (graphicsDevice == null) + { + throw new ArgumentNullException("graphicsDevice"); + } + _device = graphicsDevice; + + _lineVertices = new VertexPositionColor[bufferSize - bufferSize % 2]; + + // set up a new basic effect, and enable vertex colors. + _basicEffect = new BasicEffect(graphicsDevice); + _basicEffect.VertexColorEnabled = true; + } + + #region IDisposable Members + + public void Dispose() + { + Dispose(true); + GC.SuppressFinalize(this); + } + + #endregion + + protected virtual void Dispose(bool disposing) + { + if (disposing && !_isDisposed) + { + if (_basicEffect != null) + _basicEffect.Dispose(); + + _isDisposed = true; + } + } + + public void Begin(Matrix projection, Matrix view) + { + if (_hasBegun) + { + throw new InvalidOperationException("End must be called before Begin can be called again."); + } + + _device.SamplerStates[0] = SamplerState.AnisotropicClamp; + //tell our basic effect to begin. + _basicEffect.Projection = projection; + _basicEffect.View = view; + _basicEffect.CurrentTechnique.Passes[0].Apply(); + + // flip the error checking boolean. It's now ok to call DrawLineShape, Flush, + // and End. + _hasBegun = true; + } + + public void DrawLineShape(Shape shape) + { + DrawLineShape(shape, Color.Black); + } + + public void DrawLineShape(Shape shape, Color color) + { + if (!_hasBegun) + { + throw new InvalidOperationException("Begin must be called before DrawLineShape can be called."); + } + if (shape.ShapeType != ShapeType.Edge && + shape.ShapeType != ShapeType.Loop) + { + throw new NotSupportedException("The specified shapeType is not supported by LineBatch."); + } + if (shape.ShapeType == ShapeType.Edge) + { + if (_lineVertsCount >= _lineVertices.Length) + { + Flush(); + } + EdgeShape edge = (EdgeShape)shape; + _lineVertices[_lineVertsCount].Position = new Vector3(edge.Vertex1, 0f); + _lineVertices[_lineVertsCount + 1].Position = new Vector3(edge.Vertex2, 0f); + _lineVertices[_lineVertsCount].Color = _lineVertices[_lineVertsCount + 1].Color = color; + _lineVertsCount += 2; + } + else if (shape.ShapeType == ShapeType.Loop) + { + LoopShape loop = (LoopShape)shape; + for (int i = 0; i < loop.Vertices.Count; ++i) + { + if (_lineVertsCount >= _lineVertices.Length) + { + Flush(); + } + _lineVertices[_lineVertsCount].Position = new Vector3(loop.Vertices[i], 0f); + _lineVertices[_lineVertsCount + 1].Position = new Vector3(loop.Vertices.NextVertex(i), 0f); + _lineVertices[_lineVertsCount].Color = _lineVertices[_lineVertsCount + 1].Color = color; + _lineVertsCount += 2; + } + } + } + + public void DrawLine(Vector2 v1, Vector2 v2) + { + DrawLine(v1, v2, Color.Black); + } + + public void DrawLine(Vector2 v1, Vector2 v2, Color color) + { + if (!_hasBegun) + { + throw new InvalidOperationException("Begin must be called before DrawLineShape can be called."); + } + if (_lineVertsCount >= _lineVertices.Length) + { + Flush(); + } + _lineVertices[_lineVertsCount].Position = new Vector3(v1, 0f); + _lineVertices[_lineVertsCount + 1].Position = new Vector3(v2, 0f); + _lineVertices[_lineVertsCount].Color = _lineVertices[_lineVertsCount + 1].Color = color; + _lineVertsCount += 2; + } + + // End is called once all the primitives have been drawn using AddVertex. + // it will call Flush to actually submit the draw call to the graphics card, and + // then tell the basic effect to end. + public void End() + { + if (!_hasBegun) + { + throw new InvalidOperationException("Begin must be called before End can be called."); + } + + // Draw whatever the user wanted us to draw + Flush(); + + _hasBegun = false; + } + + private void Flush() + { + if (!_hasBegun) + { + throw new InvalidOperationException("Begin must be called before Flush can be called."); + } + if (_lineVertsCount >= 2) + { + int primitiveCount = _lineVertsCount / 2; + // submit the draw call to the graphics card + _device.DrawUserPrimitives(PrimitiveType.LineList, _lineVertices, 0, primitiveCount); + _lineVertsCount -= primitiveCount * 2; + } + } + } +} \ No newline at end of file diff --git a/axios/DrawingSystem/Sprite.cs b/axios/DrawingSystem/Sprite.cs new file mode 100644 index 0000000..27c76f1 --- /dev/null +++ b/axios/DrawingSystem/Sprite.cs @@ -0,0 +1,23 @@ +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Graphics; + +namespace FarseerPhysics.SamplesFramework +{ + public struct Sprite + { + public Vector2 Origin; + public Texture2D Texture; + + public Sprite(Texture2D texture, Vector2 origin) + { + this.Texture = texture; + this.Origin = origin; + } + + public Sprite(Texture2D sprite) + { + Texture = sprite; + Origin = new Vector2(sprite.Width / 2f, sprite.Height / 2f); + } + } +} \ No newline at end of file diff --git a/axios/Dynamics/Body.cs b/axios/Dynamics/Body.cs new file mode 100644 index 0000000..a3f857a --- /dev/null +++ b/axios/Dynamics/Body.cs @@ -0,0 +1,1388 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being 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.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, + } + + [Flags] + public enum BodyFlags + { + None = 0, + Island = (1 << 0), + Awake = (1 << 1), + AutoSleep = (1 << 2), + Bullet = (1 << 3), + FixedRotation = (1 << 4), + Enabled = (1 << 5), + IgnoreGravity = (1 << 6), + IgnoreCCD = (1 << 7), + } + + public class Body : IDisposable + { + private static int _bodyIdCounter; + internal float AngularVelocityInternal; + public int BodyId; + public ControllerFilter ControllerFilter; + internal BodyFlags Flags; + internal Vector2 Force; + internal float InvI; + internal float InvMass; + internal Vector2 LinearVelocityInternal; + public PhysicsLogicFilter PhysicsLogicFilter; + internal float SleepTime; + internal Sweep Sweep; // the swept motion for CCD + internal float Torque; + internal World World; + internal Transform Xf; // the body origin transform + private float _angularDamping; + private BodyType _bodyType; + private float _inertia; + private float _linearDamping; + private float _mass; + + internal Body() + { + FixtureList = new List(32); + } + + public Body(World world) + : this(world, null) + { + } + + public Body(World world, object userData) + { + FixtureList = new List(32); + BodyId = _bodyIdCounter++; + + World = world; + UserData = userData; + + FixedRotation = false; + IsBullet = false; + SleepingAllowed = true; + Awake = true; + BodyType = BodyType.Static; + Enabled = true; + + Xf.R.Set(0); + + world.AddBody(this); + } + + /// + /// 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. + /// + /// The type of body. + public BodyType BodyType + { + get { return _bodyType; } + set + { + if (_bodyType == value) + { + return; + } + + _bodyType = value; + + ResetMassData(); + + if (_bodyType == BodyType.Static) + { + LinearVelocityInternal = Vector2.Zero; + AngularVelocityInternal = 0.0f; + } + + Awake = true; + + Force = Vector2.Zero; + Torque = 0.0f; + + // Since the body type changed, we need to flag contacts for filtering. + for (int i = 0; i < FixtureList.Count; i++) + { + Fixture f = FixtureList[i]; + f.Refilter(); + } + } + } + + /// + /// 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; + + LinearVelocityInternal = value; + } + get { return LinearVelocityInternal; } + } + + /// + /// 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; + + AngularVelocityInternal = value; + } + get { return AngularVelocityInternal; } + } + + /// + /// 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 + { + set + { + if (value) + { + Flags |= BodyFlags.Bullet; + } + else + { + Flags &= ~BodyFlags.Bullet; + } + } + get { return (Flags & BodyFlags.Bullet) == BodyFlags.Bullet; } + } + + /// + /// 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) + { + Flags |= BodyFlags.AutoSleep; + } + else + { + Flags &= ~BodyFlags.AutoSleep; + Awake = true; + } + } + get { return (Flags & BodyFlags.AutoSleep) == BodyFlags.AutoSleep; } + } + + /// + /// 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 ((Flags & BodyFlags.Awake) == 0) + { + Flags |= BodyFlags.Awake; + SleepTime = 0.0f; + } + } + else + { + Flags &= ~BodyFlags.Awake; + SleepTime = 0.0f; + LinearVelocityInternal = Vector2.Zero; + AngularVelocityInternal = 0.0f; + Force = Vector2.Zero; + Torque = 0.0f; + } + } + get { return (Flags & BodyFlags.Awake) == BodyFlags.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) + { + Flags |= BodyFlags.Enabled; + + // 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 + { + Flags &= ~BodyFlags.Enabled; + + // 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; + } + } + get { return (Flags & BodyFlags.Enabled) == BodyFlags.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 (value) + { + Flags |= BodyFlags.FixedRotation; + } + else + { + Flags &= ~BodyFlags.FixedRotation; + } + + ResetMassData(); + } + get { return (Flags & BodyFlags.FixedRotation) == BodyFlags.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; } + + /// + /// Set the user data. Use this to store your application specific data. + /// + /// The user data. + private object _userdata; + public object UserData + { + get + { + return this._userdata; + } + set + { + this._userdata = value; + foreach (Fixture f in this.FixtureList) + f.UserData = value; + } + } + + /// + /// Get the world body origin position. + /// + /// Return the world position of the body's origin. + public Vector2 Position + { + get { return Xf.Position; } + 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.Position, 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 + { + if (value) + BodyType = BodyType.Static; + else + BodyType = BodyType.Dynamic; + } + } + + /// + /// Gets or sets a value indicating whether this body ignores gravity. + /// + /// true if it ignores gravity; otherwise, false. + public bool IgnoreGravity + { + get { return (Flags & BodyFlags.IgnoreGravity) == BodyFlags.IgnoreGravity; } + set + { + if (value) + Flags |= BodyFlags.IgnoreGravity; + else + Flags &= ~BodyFlags.IgnoreGravity; + } + } + + /// + /// 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.Multiply(ref Xf, ref Sweep.LocalCenter); + + // Update center of mass velocity. + Vector2 a = Sweep.C - oldCenter; + LinearVelocityInternal += new Vector2(-AngularVelocityInternal * a.Y, AngularVelocityInternal * 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) + 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) + return; + + if (value > 0.0f && (Flags & BodyFlags.FixedRotation) == 0) + { + _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 res / FixtureList.Count; + } + 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 res / FixtureList.Count; + } + 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; + } + } + } + + 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 { return (Flags & BodyFlags.IgnoreCCD) == BodyFlags.IgnoreCCD; } + set + { + if (value) + Flags |= BodyFlags.IgnoreCCD; + else + Flags &= ~BodyFlags.IgnoreCCD; + } + } + + #region IDisposable Members + + public bool IsDisposed { get; set; } + + public void Dispose() + { + if (!IsDisposed) + { + + World.RemoveBody(this); + IsDisposed = true; + GC.SuppressFinalize(this); + } + } + + #endregion + + /// + /// Resets the dynamics of this body. + /// Sets torque, force and linear/angular velocity to 0 + /// + public void ResetDynamics() + { + Torque = 0; + AngularVelocityInternal = 0; + Force = Vector2.Zero; + LinearVelocityInternal = 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. + /// + public Fixture CreateFixture(Shape shape) + { + return new Fixture(this, shape); + } + + /// + /// 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) + { + 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 ((Flags & BodyFlags.Enabled) == BodyFlags.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.R.Set(angle); + Xf.Position = position; + + Sweep.C0 = + Sweep.C = + new Vector2(Xf.Position.X + Xf.R.Col1.X * Sweep.LocalCenter.X + Xf.R.Col2.X * Sweep.LocalCenter.Y, + Xf.Position.Y + Xf.R.Col1.Y * Sweep.LocalCenter.X + Xf.R.Col2.Y * Sweep.LocalCenter.Y); + Sweep.A0 = Sweep.A = 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.Position); + } + + /// + /// Applies a force at the center of mass. + /// + /// The force. + public void ApplyForce(Vector2 force) + { + ApplyForce(ref force, ref Xf.Position); + } + + /// + /// 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; + } + LinearVelocityInternal += 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; + + LinearVelocityInternal += InvMass * impulse; + AngularVelocityInternal += 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; + } + + AngularVelocityInternal += 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 = Sweep.C = Xf.Position; + return; + } + + Debug.Assert(BodyType == BodyType.Dynamic || BodyType == BodyType.Static); + + // Accumulate mass over all fixtures. + Vector2 center = Vector2.Zero; + foreach (Fixture f in FixtureList) + { + if (f.Shape._density == 0) + { + continue; + } + + MassData massData = f.Shape.MassData; + _mass += massData.Mass; + center += 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.Position; + return; + } + + // Compute center of mass. + if (_mass > 0.0f) + { + InvMass = 1.0f / _mass; + center *= InvMass; + } + else + { + // Force all dynamic bodies to have a positive mass. + _mass = 1.0f; + InvMass = 1.0f; + } + + if (_inertia > 0.0f && (Flags & BodyFlags.FixedRotation) == 0) + { + // Center the inertia about the center of mass. + _inertia -= _mass * Vector2.Dot(center, center); + + 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 = center; + Sweep.C0 = Sweep.C = MathUtils.Multiply(ref Xf, ref Sweep.LocalCenter); + + // Update center of mass velocity. + Vector2 a = Sweep.C - oldCenter; + LinearVelocityInternal += new Vector2(-AngularVelocityInternal * a.Y, AngularVelocityInternal * 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 new Vector2(Xf.Position.X + Xf.R.Col1.X * localPoint.X + Xf.R.Col2.X * localPoint.Y, + Xf.Position.Y + Xf.R.Col1.Y * localPoint.X + Xf.R.Col2.Y * localPoint.Y); + } + + /// + /// 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 new Vector2(Xf.R.Col1.X * localVector.X + Xf.R.Col2.X * localVector.Y, + Xf.R.Col1.Y * localVector.X + Xf.R.Col2.Y * localVector.Y); + } + + /// + /// 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 + new Vector2((worldPoint.X - Xf.Position.X) * Xf.R.Col1.X + (worldPoint.Y - Xf.Position.Y) * Xf.R.Col1.Y, + (worldPoint.X - Xf.Position.X) * Xf.R.Col2.X + (worldPoint.Y - Xf.Position.Y) * Xf.R.Col2.Y); + } + + /// + /// 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 new Vector2(worldVector.X * Xf.R.Col1.X + worldVector.Y * Xf.R.Col1.Y, + worldVector.X * Xf.R.Col2.X + worldVector.Y * Xf.R.Col2.Y); + } + + /// + /// 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 LinearVelocityInternal + + new Vector2(-AngularVelocityInternal * (worldPoint.Y - Sweep.C.Y), + AngularVelocityInternal * (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)); + } + + public Body DeepClone() + { + Body body = Clone(); + + for (int i = 0; i < FixtureList.Count; i++) + { + FixtureList[i].Clone(body); + } + + return body; + } + + public Body Clone() + { + Body body = new Body(); + body.World = World; + body.UserData = UserData; + body.LinearDamping = LinearDamping; + body.LinearVelocityInternal = LinearVelocityInternal; + body.AngularDamping = AngularDamping; + body.AngularVelocityInternal = AngularVelocityInternal; + body.Position = Position; + body.Rotation = Rotation; + body._bodyType = _bodyType; + body.Flags = Flags; + + World.AddBody(body); + + return body; + } + + internal void SynchronizeFixtures() + { + Transform xf1 = new Transform(); + float c = (float)Math.Cos(Sweep.A0), s = (float)Math.Sin(Sweep.A0); + xf1.R.Col1.X = c; + xf1.R.Col2.X = -s; + xf1.R.Col1.Y = s; + xf1.R.Col2.Y = c; + + xf1.Position.X = Sweep.C0.X - (xf1.R.Col1.X * Sweep.LocalCenter.X + xf1.R.Col2.X * Sweep.LocalCenter.Y); + xf1.Position.Y = Sweep.C0.Y - (xf1.R.Col1.Y * Sweep.LocalCenter.X + xf1.R.Col2.Y * Sweep.LocalCenter.Y); + + 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.R.Set(Sweep.A); + + float vx = Xf.R.Col1.X * Sweep.LocalCenter.X + Xf.R.Col2.X * Sweep.LocalCenter.Y; + float vy = Xf.R.Col1.Y * Sweep.LocalCenter.X + Xf.R.Col2.Y * Sweep.LocalCenter.Y; + + Xf.Position.X = Sweep.C.X - vx; + Xf.Position.Y = Sweep.C.Y - vy; + } + + /// + /// 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. + Sweep.Advance(alpha); + Sweep.C = Sweep.C0; + Sweep.A = Sweep.A0; + SynchronizeTransform(); + } + + 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++) + { + Fixture f = FixtureList[i]; + for (int j = 0; j < other.FixtureList.Count; j++) + { + Fixture f2 = other.FixtureList[j]; + + f.IgnoreCollisionWith(f2); + } + } + } + + public void RestoreCollisionWith(Body other) + { + for (int i = 0; i < FixtureList.Count; i++) + { + Fixture f = FixtureList[i]; + for (int j = 0; j < other.FixtureList.Count; j++) + { + Fixture f2 = other.FixtureList[j]; + + f.RestoreCollisionWith(f2); + } + } + } + } +} \ No newline at end of file diff --git a/axios/Dynamics/BreakableBody.cs b/axios/Dynamics/BreakableBody.cs new file mode 100644 index 0000000..393c148 --- /dev/null +++ b/axios/Dynamics/BreakableBody.cs @@ -0,0 +1,142 @@ +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 + { + 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 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) + : this(vertices, world, density, null) + { + } + + public BreakableBody() + { + + } + + public BreakableBody(IEnumerable vertices, World world, float density, object userData) + { + _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, userData); + Parts.Add(fixture); + } + } + + private void PostSolve(Contact contact, ContactConstraint 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 fixture = Parts[i]; + + Shape shape = fixture.Shape.Clone(); + + object userdata = fixture.UserData; + MainBody.DestroyFixture(fixture); + + Body body = BodyFactory.CreateBody(_world); + body.BodyType = BodyType.Dynamic; + body.Position = MainBody.Position; + body.Rotation = MainBody.Rotation; + body.UserData = MainBody.UserData; + + body.CreateFixture(shape, userdata); + + 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/axios/Dynamics/ContactManager.cs b/axios/Dynamics/ContactManager.cs new file mode 100644 index 0000000..4d30e96 --- /dev/null +++ b/axios/Dynamics/ContactManager.cs @@ -0,0 +1,340 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and 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 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); + + /// + /// 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; + + 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); + + // Contact creation may swap fixtures. + fixtureA = c.FixtureA; + fixtureB = c.FixtureB; + bodyA = fixtureA.Body; + bodyB = fixtureB.Body; + + // Insert into the world. + ContactList.Add(c); + + // 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; + } + + 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 (EndContact != null && contact.IsTouching()) + { + 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; + } + + contact.Destroy(); + } + + internal void Collide() + { + // Update awake contacts. + for (int i = 0; i < ContactList.Count; i++) + { + Contact c = ContactList[i]; + Fixture fixtureA = c.FixtureA; + Fixture fixtureB = c.FixtureB; + int indexA = c.ChildIndexA; + int indexB = c.ChildIndexB; + Body bodyA = fixtureA.Body; + Body bodyB = fixtureB.Body; + + if (bodyA.Awake == false && bodyB.Awake == false) + { + continue; + } + + // Is this contact flagged for filtering? + if ((c.Flags & ContactFlags.Filter) == ContactFlags.Filter) + { + // 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.Flags &= ~ContactFlags.Filter; + } + + 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); + } + } + + 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; + } + } +} \ No newline at end of file diff --git a/axios/Dynamics/Contacts/Contact.cs b/axios/Dynamics/Contacts/Contact.cs new file mode 100644 index 0000000..35634a9 --- /dev/null +++ b/axios/Dynamics/Contacts/Contact.cs @@ -0,0 +1,502 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being 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.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; + } + + [Flags] + public enum ContactFlags + { + None = 0, + + /// + /// Used when crawling contact graph when forming islands. + /// + Island = 0x0001, + + /// + /// Set when the shapes are touching. + /// + Touching = 0x0002, + + /// + /// This contact can be disabled (by user) + /// + Enabled = 0x0004, + + /// + /// This contact needs filtering because a fixture filter was changed. + /// + Filter = 0x0008, + + /// + /// This bullet contact had a TOI event + /// + BulletHit = 0x0010, + + /// + /// This contact has a valid TOI i the field TOI + /// + TOI = 0x0020 + } + + /// + /// 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 static EdgeShape _edge = new EdgeShape(); + + private static ContactType[,] _registers = new[,] + { + { + ContactType.Circle, + ContactType.EdgeAndCircle, + ContactType.PolygonAndCircle, + ContactType.LoopAndCircle, + }, + { + 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.LoopAndPolygon, + }, + { + ContactType.LoopAndCircle, + ContactType.NotSupported, + // 3,1 is invalid (no ContactType.EdgeAndLoop) + ContactType.LoopAndPolygon, + ContactType.NotSupported, + // 3,3 is invalid (no ContactType.Loop) + }, + }; + + public Fixture FixtureA; + public Fixture FixtureB; + internal ContactFlags Flags; + + public Manifold Manifold; + + // Nodes for connecting bodies. + internal ContactEdge NodeA = new ContactEdge(); + internal ContactEdge NodeB = new ContactEdge(); + public float TOI; + internal int TOICount; + private ContactType _type; + + private Contact(Fixture fA, int indexA, Fixture fB, int indexB) + { + Reset(fA, indexA, fB, indexB); + } + + /// 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). + public bool Enabled + { + set + { + if (value) + { + Flags |= ContactFlags.Enabled; + } + else + { + Flags &= ~ContactFlags.Enabled; + } + } + + get { return (Flags & ContactFlags.Enabled) == ContactFlags.Enabled; } + } + + /// + /// 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; } + + /// + /// Get the contact manifold. Do not modify the manifold unless you understand the + /// internals of Box2D. + /// + /// The manifold. + public void GetManifold(out Manifold manifold) + { + manifold = Manifold; + } + + /// + /// 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; + + Collision.Collision.GetWorldManifold(ref Manifold, ref bodyA.Xf, shapeA.Radius, ref bodyB.Xf, shapeB.Radius, + out normal, out points); + } + + /// + /// Determines whether this contact is touching. + /// + /// + /// true if this instance is touching; otherwise, false. + /// + public bool IsTouching() + { + return (Flags & ContactFlags.Touching) == ContactFlags.Touching; + } + + /// + /// Flag this contact for filtering. Filtering will occur the next time step. + /// + public void FlagForFiltering() + { + Flags |= ContactFlags.Filter; + } + + private void Reset(Fixture fA, int indexA, Fixture fB, int indexB) + { + Flags = ContactFlags.Enabled; + + 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; + } + + /// + /// 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) + { + Manifold oldManifold = Manifold; + + // Re-enable this contact. + Flags |= ContactFlags.Enabled; + + bool touching; + bool wasTouching = (Flags & ContactFlags.Touching) == ContactFlags.Touching; + + bool sensor = FixtureA.IsSensor || FixtureB.IsSensor; + + Body bodyA = FixtureA.Body; + Body bodyB = FixtureB.Body; + + // Is this contact a sensor? + if (sensor) + { + Shape shapeA = FixtureA.Shape; + Shape shapeB = FixtureB.Shape; + touching = AABB.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; + bool found = false; + + 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; + found = true; + break; + } + } + if (found == false) + { + mp2.NormalImpulse = 0.0f; + mp2.TangentImpulse = 0.0f; + } + + Manifold.Points[i] = mp2; + } + + if (touching != wasTouching) + { + bodyA.Awake = true; + bodyB.Awake = true; + } + } + + if (touching) + { + Flags |= ContactFlags.Touching; + } + else + { + Flags &= ~ContactFlags.Touching; + } + + if (wasTouching == false && touching) + { + //Report the collision to both participants: + if (FixtureA.OnCollision != null) + Enabled = FixtureA.OnCollision(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) + Enabled = FixtureB.OnCollision(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), we also need to mark + //it as not touching. + if (Enabled == false) + Flags &= ~ContactFlags.Touching; + } + + if (wasTouching && 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.LoopAndCircle: + LoopShape loop = (LoopShape)FixtureA.Shape; + loop.GetChildEdge(ref _edge, ChildIndexA); + Collision.Collision.CollideEdgeAndCircle(ref manifold, _edge, ref transformA, + (CircleShape)FixtureB.Shape, ref transformB); + break; + case ContactType.LoopAndPolygon: + LoopShape loop2 = (LoopShape)FixtureA.Shape; + loop2.GetChildEdge(ref _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.ShapeType; + ShapeType type2 = fixtureB.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() + { + FixtureA.Body.World.ContactPool.Enqueue(this); + Reset(null, 0, null, 0); + } + + #region Nested type: ContactType + + private enum ContactType + { + NotSupported, + Polygon, + PolygonAndCircle, + Circle, + EdgeAndPolygon, + EdgeAndCircle, + LoopAndPolygon, + LoopAndCircle, + } + + #endregion + } +} \ No newline at end of file diff --git a/axios/Dynamics/Contacts/ContactSolver.cs b/axios/Dynamics/Contacts/ContactSolver.cs new file mode 100644 index 0000000..441abc2 --- /dev/null +++ b/axios/Dynamics/Contacts/ContactSolver.cs @@ -0,0 +1,794 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not 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 ContactConstraintPoint + { + public Vector2 LocalPoint; + public float NormalImpulse; + public float NormalMass; + public float TangentImpulse; + public float TangentMass; + public float VelocityBias; + public Vector2 rA; + public Vector2 rB; + } + + public sealed class ContactConstraint + { + public Body BodyA; + public Body BodyB; + public float Friction; + public Mat22 K; + public Vector2 LocalNormal; + public Vector2 LocalPoint; + public Manifold Manifold; + public Vector2 Normal; + public Mat22 NormalMass; + public int PointCount; + public ContactConstraintPoint[] Points = new ContactConstraintPoint[Settings.MaxPolygonVertices]; + public float RadiusA; + public float RadiusB; + public float Restitution; + public ManifoldType Type; + + public ContactConstraint() + { + for (int i = 0; i < Settings.MaxManifoldPoints; i++) + { + Points[i] = new ContactConstraintPoint(); + } + } + } + + public class ContactSolver + { + public ContactConstraint[] Constraints; + private int _constraintCount; // collection can be bigger. + private Contact[] _contacts; + + public void Reset(Contact[] contacts, int contactCount, float impulseRatio, bool warmstarting) + { + _contacts = contacts; + + _constraintCount = contactCount; + + // grow the array + if (Constraints == null || Constraints.Length < _constraintCount) + { + Constraints = new ContactConstraint[_constraintCount * 2]; + + for (int i = 0; i < Constraints.Length; i++) + { + Constraints[i] = new ContactConstraint(); + } + } + + // Initialize position independent portions of the constraints. + for (int i = 0; i < _constraintCount; ++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; + + Debug.Assert(manifold.PointCount > 0); + + ContactConstraint cc = Constraints[i]; + cc.Friction = Settings.MixFriction(fixtureA.Friction, fixtureB.Friction); + cc.Restitution = Settings.MixRestitution(fixtureA.Restitution, fixtureB.Restitution); + cc.BodyA = bodyA; + cc.BodyB = bodyB; + cc.Manifold = manifold; + cc.Normal = Vector2.Zero; + cc.PointCount = manifold.PointCount; + + cc.LocalNormal = manifold.LocalNormal; + cc.LocalPoint = manifold.LocalPoint; + cc.RadiusA = radiusA; + cc.RadiusB = radiusB; + cc.Type = manifold.Type; + + for (int j = 0; j < cc.PointCount; ++j) + { + ManifoldPoint cp = manifold.Points[j]; + ContactConstraintPoint ccp = cc.Points[j]; + + if (warmstarting) + { + ccp.NormalImpulse = impulseRatio * cp.NormalImpulse; + ccp.TangentImpulse = impulseRatio * cp.TangentImpulse; + } + else + { + ccp.NormalImpulse = 0.0f; + ccp.TangentImpulse = 0.0f; + } + + ccp.LocalPoint = cp.LocalPoint; + ccp.rA = Vector2.Zero; + ccp.rB = Vector2.Zero; + ccp.NormalMass = 0.0f; + ccp.TangentMass = 0.0f; + ccp.VelocityBias = 0.0f; + } + + cc.K.SetZero(); + cc.NormalMass.SetZero(); + } + } + + public void InitializeVelocityConstraints() + { + for (int i = 0; i < _constraintCount; ++i) + { + ContactConstraint cc = Constraints[i]; + + float radiusA = cc.RadiusA; + float radiusB = cc.RadiusB; + Body bodyA = cc.BodyA; + Body bodyB = cc.BodyB; + Manifold manifold = cc.Manifold; + + Vector2 vA = bodyA.LinearVelocity; + Vector2 vB = bodyB.LinearVelocity; + float wA = bodyA.AngularVelocity; + float wB = bodyB.AngularVelocity; + + Debug.Assert(manifold.PointCount > 0); + FixedArray2 points; + + Collision.Collision.GetWorldManifold(ref manifold, ref bodyA.Xf, radiusA, ref bodyB.Xf, radiusB, + out cc.Normal, out points); + Vector2 tangent = new Vector2(cc.Normal.Y, -cc.Normal.X); + + for (int j = 0; j < cc.PointCount; ++j) + { + ContactConstraintPoint ccp = cc.Points[j]; + + ccp.rA = points[j] - bodyA.Sweep.C; + ccp.rB = points[j] - bodyB.Sweep.C; + + float rnA = ccp.rA.X * cc.Normal.Y - ccp.rA.Y * cc.Normal.X; + float rnB = ccp.rB.X * cc.Normal.Y - ccp.rB.Y * cc.Normal.X; + rnA *= rnA; + rnB *= rnB; + + float kNormal = bodyA.InvMass + bodyB.InvMass + bodyA.InvI * rnA + bodyB.InvI * rnB; + + Debug.Assert(kNormal > Settings.Epsilon); + ccp.NormalMass = 1.0f / kNormal; + + float rtA = ccp.rA.X * tangent.Y - ccp.rA.Y * tangent.X; + float rtB = ccp.rB.X * tangent.Y - ccp.rB.Y * tangent.X; + + rtA *= rtA; + rtB *= rtB; + float kTangent = bodyA.InvMass + bodyB.InvMass + bodyA.InvI * rtA + bodyB.InvI * rtB; + + Debug.Assert(kTangent > Settings.Epsilon); + ccp.TangentMass = 1.0f / kTangent; + + // Setup a velocity bias for restitution. + ccp.VelocityBias = 0.0f; + float vRel = cc.Normal.X * (vB.X + -wB * ccp.rB.Y - vA.X - -wA * ccp.rA.Y) + + cc.Normal.Y * (vB.Y + wB * ccp.rB.X - vA.Y - wA * ccp.rA.X); + if (vRel < -Settings.VelocityThreshold) + { + ccp.VelocityBias = -cc.Restitution * vRel; + } + } + + // If we have two points, then prepare the block solver. + if (cc.PointCount == 2) + { + ContactConstraintPoint ccp1 = cc.Points[0]; + ContactConstraintPoint ccp2 = cc.Points[1]; + + float invMassA = bodyA.InvMass; + float invIA = bodyA.InvI; + float invMassB = bodyB.InvMass; + float invIB = bodyB.InvI; + + float rn1A = ccp1.rA.X * cc.Normal.Y - ccp1.rA.Y * cc.Normal.X; + float rn1B = ccp1.rB.X * cc.Normal.Y - ccp1.rB.Y * cc.Normal.X; + float rn2A = ccp2.rA.X * cc.Normal.Y - ccp2.rA.Y * cc.Normal.X; + float rn2B = ccp2.rB.X * cc.Normal.Y - ccp2.rB.Y * cc.Normal.X; + + float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B; + float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B; + float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B; + + // Ensure a reasonable condition number. + const float k_maxConditionNumber = 100.0f; + if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) + { + // K is safe to invert. + cc.K.Col1.X = k11; + cc.K.Col1.Y = k12; + cc.K.Col2.X = k12; + cc.K.Col2.Y = k22; + + float a = cc.K.Col1.X, b = cc.K.Col2.X, c = cc.K.Col1.Y, d = cc.K.Col2.Y; + float det = a * d - b * c; + if (det != 0.0f) + { + det = 1.0f / det; + } + + cc.NormalMass.Col1.X = det * d; + cc.NormalMass.Col1.Y = -det * c; + cc.NormalMass.Col2.X = -det * b; + cc.NormalMass.Col2.Y = det * a; + } + else + { + // The constraints are redundant, just use one. + // TODO_ERIN use deepest? + cc.PointCount = 1; + } + } + } + } + + public void WarmStart() + { + // Warm start. + for (int i = 0; i < _constraintCount; ++i) + { + ContactConstraint c = Constraints[i]; + + float tangentx = c.Normal.Y; + float tangenty = -c.Normal.X; + + for (int j = 0; j < c.PointCount; ++j) + { + ContactConstraintPoint ccp = c.Points[j]; + float px = ccp.NormalImpulse * c.Normal.X + ccp.TangentImpulse * tangentx; + float py = ccp.NormalImpulse * c.Normal.Y + ccp.TangentImpulse * tangenty; + c.BodyA.AngularVelocityInternal -= c.BodyA.InvI * (ccp.rA.X * py - ccp.rA.Y * px); + c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * px; + c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * py; + c.BodyB.AngularVelocityInternal += c.BodyB.InvI * (ccp.rB.X * py - ccp.rB.Y * px); + c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * px; + c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * py; + } + } + } + + public void SolveVelocityConstraints() + { + for (int i = 0; i < _constraintCount; ++i) + { + ContactConstraint c = Constraints[i]; + float wA = c.BodyA.AngularVelocityInternal; + float wB = c.BodyB.AngularVelocityInternal; + + float tangentx = c.Normal.Y; + float tangenty = -c.Normal.X; + + float friction = c.Friction; + + Debug.Assert(c.PointCount == 1 || c.PointCount == 2); + + // Solve tangent constraints + for (int j = 0; j < c.PointCount; ++j) + { + ContactConstraintPoint ccp = c.Points[j]; + float lambda = ccp.TangentMass * + -((c.BodyB.LinearVelocityInternal.X + (-wB * ccp.rB.Y) - + c.BodyA.LinearVelocityInternal.X - (-wA * ccp.rA.Y)) * tangentx + + (c.BodyB.LinearVelocityInternal.Y + (wB * ccp.rB.X) - + c.BodyA.LinearVelocityInternal.Y - (wA * ccp.rA.X)) * tangenty); + + // MathUtils.Clamp the accumulated force + float maxFriction = friction * ccp.NormalImpulse; + float newImpulse = Math.Max(-maxFriction, Math.Min(ccp.TangentImpulse + lambda, maxFriction)); + lambda = newImpulse - ccp.TangentImpulse; + + // Apply contact impulse + float px = lambda * tangentx; + float py = lambda * tangenty; + + c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * px; + c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * py; + wA -= c.BodyA.InvI * (ccp.rA.X * py - ccp.rA.Y * px); + + c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * px; + c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * py; + wB += c.BodyB.InvI * (ccp.rB.X * py - ccp.rB.Y * px); + + ccp.TangentImpulse = newImpulse; + } + + // Solve normal constraints + if (c.PointCount == 1) + { + ContactConstraintPoint ccp = c.Points[0]; + + // Relative velocity at contact + // Compute normal impulse + float lambda = -ccp.NormalMass * + ((c.BodyB.LinearVelocityInternal.X + (-wB * ccp.rB.Y) - + c.BodyA.LinearVelocityInternal.X - (-wA * ccp.rA.Y)) * c.Normal.X + + (c.BodyB.LinearVelocityInternal.Y + (wB * ccp.rB.X) - + c.BodyA.LinearVelocityInternal.Y - + (wA * ccp.rA.X)) * c.Normal.Y - ccp.VelocityBias); + + // Clamp the accumulated impulse + float newImpulse = Math.Max(ccp.NormalImpulse + lambda, 0.0f); + lambda = newImpulse - ccp.NormalImpulse; + + // Apply contact impulse + float px = lambda * c.Normal.X; + float py = lambda * c.Normal.Y; + + c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * px; + c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * py; + wA -= c.BodyA.InvI * (ccp.rA.X * py - ccp.rA.Y * px); + + c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * px; + c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * py; + wB += c.BodyB.InvI * (ccp.rB.X * py - ccp.rB.Y * px); + + ccp.NormalImpulse = newImpulse; + } + 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 = vn_0 - 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 = x' - a + // + // Plug into above equation: + // + // vn = A * x + b + // = A * (x' - a) + b + // = A * x' + b - A * a + // = A * x' + b' + // b' = b - A * a; + + ContactConstraintPoint cp1 = c.Points[0]; + ContactConstraintPoint cp2 = c.Points[1]; + + float ax = cp1.NormalImpulse; + float ay = cp2.NormalImpulse; + Debug.Assert(ax >= 0.0f && ay >= 0.0f); + + // Relative velocity at contact + // Compute normal velocity + float vn1 = (c.BodyB.LinearVelocityInternal.X + (-wB * cp1.rB.Y) - c.BodyA.LinearVelocityInternal.X - + (-wA * cp1.rA.Y)) * c.Normal.X + + (c.BodyB.LinearVelocityInternal.Y + (wB * cp1.rB.X) - c.BodyA.LinearVelocityInternal.Y - + (wA * cp1.rA.X)) * c.Normal.Y; + float vn2 = (c.BodyB.LinearVelocityInternal.X + (-wB * cp2.rB.Y) - c.BodyA.LinearVelocityInternal.X - + (-wA * cp2.rA.Y)) * c.Normal.X + + (c.BodyB.LinearVelocityInternal.Y + (wB * cp2.rB.X) - c.BodyA.LinearVelocityInternal.Y - + (wA * cp2.rA.X)) * c.Normal.Y; + + float bx = vn1 - cp1.VelocityBias - (c.K.Col1.X * ax + c.K.Col2.X * ay); + float by = vn2 - cp2.VelocityBias - (c.K.Col1.Y * ax + c.K.Col2.Y * ay); + + float xx = -(c.NormalMass.Col1.X * bx + c.NormalMass.Col2.X * by); + float xy = -(c.NormalMass.Col1.Y * bx + c.NormalMass.Col2.Y * by); + + while (true) + { + // + // Case 1: vn = 0 + // + // 0 = A * x' + b' + // + // Solve for x': + // + // x' = - inv(A) * b' + // + if (xx >= 0.0f && xy >= 0.0f) + { + // Resubstitute for the incremental impulse + float dx = xx - ax; + float dy = xy - ay; + + // Apply incremental impulse + float p1x = dx * c.Normal.X; + float p1y = dx * c.Normal.Y; + + float p2x = dy * c.Normal.X; + float p2y = dy * c.Normal.Y; + + float p12x = p1x + p2x; + float p12y = p1y + p2y; + + c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x; + c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y; + wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x)); + + c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x; + c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y; + wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x)); + + // Accumulate + cp1.NormalImpulse = xx; + cp2.NormalImpulse = xy; + +#if B2_DEBUG_SOLVER + + float k_errorTol = 1e-3f; + + // 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); + + Debug.Assert(MathUtils.Abs(vn1 - cp1.velocityBias) < k_errorTol); + Debug.Assert(MathUtils.Abs(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' + // + xx = -cp1.NormalMass * bx; + xy = 0.0f; + vn1 = 0.0f; + vn2 = c.K.Col1.Y * xx + by; + + if (xx >= 0.0f && vn2 >= 0.0f) + { + // Resubstitute for the incremental impulse + float dx = xx - ax; + float dy = xy - ay; + + // Apply incremental impulse + float p1x = dx * c.Normal.X; + float p1y = dx * c.Normal.Y; + + float p2x = dy * c.Normal.X; + float p2y = dy * c.Normal.Y; + + float p12x = p1x + p2x; + float p12y = p1y + p2y; + + c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x; + c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y; + wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x)); + + c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x; + c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y; + wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x)); + + // Accumulate + cp1.NormalImpulse = xx; + cp2.NormalImpulse = xy; + +#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); + + Debug.Assert(MathUtils.Abs(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' + // + xx = 0.0f; + xy = -cp2.NormalMass * by; + vn1 = c.K.Col2.X * xy + bx; + vn2 = 0.0f; + + if (xy >= 0.0f && vn1 >= 0.0f) + { + // Resubstitute for the incremental impulse + float dx = xx - ax; + float dy = xy - ay; + + // Apply incremental impulse + float p1x = dx * c.Normal.X; + float p1y = dx * c.Normal.Y; + + float p2x = dy * c.Normal.X; + float p2y = dy * c.Normal.Y; + + float p12x = p1x + p2x; + float p12y = p1y + p2y; + + c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x; + c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y; + wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x)); + + c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x; + c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y; + wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x)); + + // Accumulate + cp1.NormalImpulse = xx; + cp2.NormalImpulse = xy; + +#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); + + Debug.Assert(MathUtils.Abs(vn2 - cp2.velocityBias) < k_errorTol); +#endif + break; + } + + // + // Case 4: x1 = 0 and x2 = 0 + // + // vn1 = b1 + // vn2 = b2; + xx = 0.0f; + xy = 0.0f; + vn1 = bx; + vn2 = by; + + if (vn1 >= 0.0f && vn2 >= 0.0f) + { + // Resubstitute for the incremental impulse + float dx = xx - ax; + float dy = xy - ay; + + // Apply incremental impulse + float p1x = dx * c.Normal.X; + float p1y = dx * c.Normal.Y; + + float p2x = dy * c.Normal.X; + float p2y = dy * c.Normal.Y; + + float p12x = p1x + p2x; + float p12y = p1y + p2y; + + c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x; + c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y; + wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x)); + + c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x; + c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y; + wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x)); + + // Accumulate + cp1.NormalImpulse = xx; + cp2.NormalImpulse = xy; + + break; + } + + // No solution, give up. This is hit sometimes, but it doesn't seem to matter. + break; + } + } + + c.BodyA.AngularVelocityInternal = wA; + c.BodyB.AngularVelocityInternal = wB; + } + } + + public void StoreImpulses() + { + for (int i = 0; i < _constraintCount; ++i) + { + ContactConstraint c = Constraints[i]; + Manifold m = c.Manifold; + + for (int j = 0; j < c.PointCount; ++j) + { + ManifoldPoint pj = m.Points[j]; + ContactConstraintPoint cp = c.Points[j]; + + pj.NormalImpulse = cp.NormalImpulse; + pj.TangentImpulse = cp.TangentImpulse; + + m.Points[j] = pj; + } + + c.Manifold = m; + _contacts[i].Manifold = m; + } + } + + public bool SolvePositionConstraints(float baumgarte) + { + float minSeparation = 0.0f; + + for (int i = 0; i < _constraintCount; ++i) + { + ContactConstraint c = Constraints[i]; + + Body bodyA = c.BodyA; + Body bodyB = c.BodyB; + + float invMassA = bodyA.Mass * bodyA.InvMass; + float invIA = bodyA.Mass * bodyA.InvI; + float invMassB = bodyB.Mass * bodyB.InvMass; + float invIB = bodyB.Mass * bodyB.InvI; + + // Solve normal constraints + for (int j = 0; j < c.PointCount; ++j) + { + Vector2 normal; + Vector2 point; + float separation; + + Solve(c, j, out normal, out point, out separation); + + float rax = point.X - bodyA.Sweep.C.X; + float ray = point.Y - bodyA.Sweep.C.Y; + + float rbx = point.X - bodyB.Sweep.C.X; + float rby = point.Y - bodyB.Sweep.C.Y; + + // Track max constraint error. + minSeparation = Math.Min(minSeparation, separation); + + // Prevent large corrections and allow slop. + float C = Math.Max(-Settings.MaxLinearCorrection, + Math.Min(baumgarte * (separation + Settings.LinearSlop), 0.0f)); + + // Compute the effective mass. + float rnA = rax * normal.Y - ray * normal.X; + float rnB = rbx * normal.Y - rby * normal.X; + float K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB; + + // Compute normal impulse + float impulse = K > 0.0f ? -C / K : 0.0f; + + float px = impulse * normal.X; + float py = impulse * normal.Y; + + bodyA.Sweep.C.X -= invMassA * px; + bodyA.Sweep.C.Y -= invMassA * py; + bodyA.Sweep.A -= invIA * (rax * py - ray * px); + + bodyB.Sweep.C.X += invMassB * px; + bodyB.Sweep.C.Y += invMassB * py; + bodyB.Sweep.A += invIB * (rbx * py - rby * px); + + bodyA.SynchronizeTransform(); + bodyB.SynchronizeTransform(); + } + } + + // We can't expect minSpeparation >= -Settings.b2_linearSlop because we don't + // push the separation above -Settings.b2_linearSlop. + return minSeparation >= -1.5f * Settings.LinearSlop; + } + + private static void Solve(ContactConstraint cc, int index, out Vector2 normal, out Vector2 point, + out float separation) + { + Debug.Assert(cc.PointCount > 0); + + normal = Vector2.Zero; + + switch (cc.Type) + { + case ManifoldType.Circles: + { + Vector2 pointA = cc.BodyA.GetWorldPoint(ref cc.LocalPoint); + Vector2 pointB = cc.BodyB.GetWorldPoint(ref cc.Points[0].LocalPoint); + float a = (pointA.X - pointB.X) * (pointA.X - pointB.X) + + (pointA.Y - pointB.Y) * (pointA.Y - pointB.Y); + if (a > Settings.Epsilon * Settings.Epsilon) + { + Vector2 normalTmp = pointB - pointA; + float factor = 1f / (float)Math.Sqrt(normalTmp.X * normalTmp.X + normalTmp.Y * normalTmp.Y); + normal.X = normalTmp.X * factor; + normal.Y = normalTmp.Y * factor; + } + else + { + normal.X = 1; + normal.Y = 0; + } + + point = 0.5f * (pointA + pointB); + separation = (pointB.X - pointA.X) * normal.X + (pointB.Y - pointA.Y) * normal.Y - cc.RadiusA - + cc.RadiusB; + } + break; + + case ManifoldType.FaceA: + { + normal = cc.BodyA.GetWorldVector(ref cc.LocalNormal); + Vector2 planePoint = cc.BodyA.GetWorldPoint(ref cc.LocalPoint); + Vector2 clipPoint = cc.BodyB.GetWorldPoint(ref cc.Points[index].LocalPoint); + separation = (clipPoint.X - planePoint.X) * normal.X + (clipPoint.Y - planePoint.Y) * normal.Y - + cc.RadiusA - cc.RadiusB; + point = clipPoint; + } + break; + + case ManifoldType.FaceB: + { + normal = cc.BodyB.GetWorldVector(ref cc.LocalNormal); + Vector2 planePoint = cc.BodyB.GetWorldPoint(ref cc.LocalPoint); + + Vector2 clipPoint = cc.BodyA.GetWorldPoint(ref cc.Points[index].LocalPoint); + separation = (clipPoint.X - planePoint.X) * normal.X + (clipPoint.Y - planePoint.Y) * normal.Y - + cc.RadiusA - cc.RadiusB; + point = clipPoint; + + // Ensure normal points from A to B + normal = -normal; + } + break; + default: + point = Vector2.Zero; + separation = 0.0f; + break; + } + } + } +} \ No newline at end of file diff --git a/axios/Dynamics/Fixture.cs b/axios/Dynamics/Fixture.cs new file mode 100644 index 0000000..9adcc33 --- /dev/null +++ b/axios/Dynamics/Fixture.cs @@ -0,0 +1,611 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being 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.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 + { + private static int _fixtureIdCounter; + + /// + /// 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 that the first fixture argument is always the fixture that the delegate is subscribed to. + /// + public OnSeparationEventHandler OnSeparation; + + public FixtureProxy[] Proxies; + public int ProxyCount; + internal Category _collidesWith; + internal Category _collisionCategories; + internal short _collisionGroup; + internal Dictionary _collisionIgnores; + private float _friction; + private float _restitution; + + internal Fixture() + { + } + + public Fixture(Body body, Shape shape) + : this(body, shape, null) + { + } + + public Fixture(Body body, Shape shape, object userData) + { + if (Settings.UseFPECollisionCategories) + _collisionCategories = Category.All; + else + _collisionCategories = Category.Cat1; + + _collidesWith = Category.All; + _collisionGroup = 0; + + //Fixture defaults + Friction = 0.2f; + Restitution = 0; + + IsSensor = false; + + Body = body; + UserData = userData; + +#pragma warning disable 162 + if (Settings.ConserveMemory) + Shape = shape; + else + Shape = shape.Clone(); +#pragma warning restore 162 + + 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 type of the child Shape. You can use this to down cast to the concrete Shape. + /// + /// The type of the shape. + public ShapeType ShapeType + { + get { return Shape.ShapeType; } + } + + /// + /// 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; set; } + + /// + /// 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; } + + /// + /// Get or set the coefficient of friction. + /// + /// The friction. + public float Friction + { + get { return _friction; } + set + { + Debug.Assert(!float.IsNaN(value)); + + _friction = value; + } + } + + /// + /// Get or set the coefficient of restitution. + /// + /// 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; private 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 == null) + return; + + if (_collisionIgnores.ContainsKey(fixture.FixtureId)) + { + _collisionIgnores[fixture.FixtureId] = false; + Refilter(); + } + } + + /// + /// Ignores collisions between this fixture and the provided fixture. + /// + /// The fixture. + public void IgnoreCollisionWith(Fixture fixture) + { + if (_collisionIgnores == null) + _collisionIgnores = new Dictionary(); + + if (_collisionIgnores.ContainsKey(fixture.FixtureId)) + _collisionIgnores[fixture.FixtureId] = true; + else + _collisionIgnores.Add(fixture.FixtureId, true); + + 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) + { + if (_collisionIgnores == null) + return false; + + if (_collisionIgnores.ContainsKey(fixture.FixtureId)) + return _collisionIgnores[fixture.FixtureId]; + + return false; + } + + /// + /// 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. + /// + internal 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.FlagForFiltering(); + } + + 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; + + FixtureId = _fixtureIdCounter++; + + if ((Body.Flags & BodyFlags.Enabled) == BodyFlags.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.Flags |= WorldFlags.NewFixture; + + 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; + } + + public Fixture Clone(Body body) + { + Fixture fixture = new Fixture(); + fixture.Body = body; + +#pragma warning disable 162 + if (Settings.ConserveMemory) + fixture.Shape = Shape; + else + fixture.Shape = Shape.Clone(); +#pragma warning restore 162 + + fixture.UserData = UserData; + fixture.Restitution = Restitution; + fixture.Friction = Friction; + fixture.IsSensor = IsSensor; + fixture._collisionGroup = CollisionGroup; + fixture._collisionCategories = CollisionCategories; + fixture._collidesWith = CollidesWith; + + if (_collisionIgnores != null) + { + fixture._collisionIgnores = new Dictionary(); + + foreach (KeyValuePair pair in _collisionIgnores) + { + fixture._collisionIgnores.Add(pair.Key, pair.Value); + } + } + + fixture.RegisterFixture(); + return fixture; + } + + public Fixture DeepClone() + { + Fixture fix = Clone(Body.Clone()); + return fix; + } + + internal void Destroy() + { + // The proxies must be destroyed before calling this. + Debug.Assert(ProxyCount == 0); + + // Free the proxy array. + Proxies = null; + Shape = 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; + 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.Position - transform1.Position; + + broadPhase.MoveProxy(proxy.ProxyId, ref proxy.AABB, displacement); + } + } + + internal bool CompareTo(Fixture fixture) + { + return ( + CollidesWith == fixture.CollidesWith && + CollisionCategories == fixture.CollisionCategories && + CollisionGroup == fixture.CollisionGroup && + Friction == fixture.Friction && + IsSensor == fixture.IsSensor && + Restitution == fixture.Restitution && + Shape.CompareTo(fixture.Shape) && + UserData == fixture.UserData); + } + } +} \ No newline at end of file diff --git a/axios/Dynamics/Island.cs b/axios/Dynamics/Island.cs new file mode 100644 index 0000000..3eb7e31 --- /dev/null +++ b/axios/Dynamics/Island.cs @@ -0,0 +1,484 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not 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 + { + public Body[] Bodies; + public int BodyCount; + public int ContactCount; + public int JointCount; + private int _bodyCapacity; + private int _contactCapacity; + private ContactManager _contactManager; + private ContactSolver _contactSolver = new ContactSolver(); + private Contact[] _contacts; + private int _jointCapacity; + private Joint[] _joints; + public float JointUpdateTime; + + private const float LinTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance; + private const float AngTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance; + +#if (!SILVERLIGHT) + private Stopwatch _watch = new Stopwatch(); +#endif + + 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]; + } + + 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; + } + + private float _tmpTime; + + public void Solve(ref TimeStep step, ref Vector2 gravity) + { + // Integrate velocities and apply damping. + for (int i = 0; i < BodyCount; ++i) + { + Body b = Bodies[i]; + + if (b.BodyType != BodyType.Dynamic) + { + continue; + } + + // Integrate velocities. + // FPE 3 only - Only apply gravity if the body wants it. + if (b.IgnoreGravity) + { + b.LinearVelocityInternal.X += step.dt * (b.InvMass * b.Force.X); + b.LinearVelocityInternal.Y += step.dt * (b.InvMass * b.Force.Y); + b.AngularVelocityInternal += step.dt * b.InvI * b.Torque; + } + else + { + b.LinearVelocityInternal.X += step.dt * (gravity.X + b.InvMass * b.Force.X); + b.LinearVelocityInternal.Y += step.dt * (gravity.Y + b.InvMass * b.Force.Y); + b.AngularVelocityInternal += step.dt * 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 + b.LinearVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.LinearDamping, 0.0f, 1.0f); + b.AngularVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.AngularDamping, 0.0f, 1.0f); + } + + // Partition contacts so that contacts with static bodies are solved last. + int i1 = -1; + for (int i2 = 0; i2 < ContactCount; ++i2) + { + Fixture fixtureA = _contacts[i2].FixtureA; + Fixture fixtureB = _contacts[i2].FixtureB; + Body bodyA = fixtureA.Body; + Body bodyB = fixtureB.Body; + bool nonStatic = bodyA.BodyType != BodyType.Static && bodyB.BodyType != BodyType.Static; + if (nonStatic) + { + ++i1; + + //TODO: Only swap if they are not the same? see http://code.google.com/p/box2d/issues/detail?id=162 + Contact tmp = _contacts[i1]; + _contacts[i1] = _contacts[i2]; + _contacts[i2] = tmp; + } + } + + // Initialize velocity constraints. + _contactSolver.Reset(_contacts, ContactCount, step.dtRatio, Settings.EnableWarmstarting); + _contactSolver.InitializeVelocityConstraints(); + + if (Settings.EnableWarmstarting) + { + _contactSolver.WarmStart(); + } + +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + { + _watch.Start(); + _tmpTime = 0; + } +#endif + + for (int i = 0; i < JointCount; ++i) + { + if (_joints[i].Enabled) + _joints[i].InitVelocityConstraints(ref step); + } + +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + { + _tmpTime += _watch.ElapsedTicks; + } +#endif + + // Solve velocity constraints. + for (int i = 0; i < Settings.VelocityIterations; ++i) + { +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + _watch.Start(); +#endif + for (int j = 0; j < JointCount; ++j) + { + Joint joint = _joints[j]; + + if (!joint.Enabled) + continue; + + joint.SolveVelocityConstraints(ref step); + joint.Validate(step.inv_dt); + } + +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + { + _watch.Stop(); + _tmpTime += _watch.ElapsedTicks; + _watch.Reset(); + } +#endif + + _contactSolver.SolveVelocityConstraints(); + } + + // Post-solve (store impulses for warm starting). + _contactSolver.StoreImpulses(); + + // Integrate positions. + for (int i = 0; i < BodyCount; ++i) + { + Body b = Bodies[i]; + + if (b.BodyType == BodyType.Static) + { + continue; + } + + // Check for large velocities. + float translationX = step.dt * b.LinearVelocityInternal.X; + float translationY = step.dt * b.LinearVelocityInternal.Y; + float result = translationX * translationX + translationY * translationY; + + if (result > Settings.MaxTranslationSquared) + { + float sq = (float)Math.Sqrt(result); + + float ratio = Settings.MaxTranslation / sq; + b.LinearVelocityInternal.X *= ratio; + b.LinearVelocityInternal.Y *= ratio; + } + + float rotation = step.dt * b.AngularVelocityInternal; + if (rotation * rotation > Settings.MaxRotationSquared) + { + float ratio = Settings.MaxRotation / Math.Abs(rotation); + b.AngularVelocityInternal *= ratio; + } + + // Store positions for continuous collision. + b.Sweep.C0.X = b.Sweep.C.X; + b.Sweep.C0.Y = b.Sweep.C.Y; + b.Sweep.A0 = b.Sweep.A; + + // Integrate + b.Sweep.C.X += step.dt * b.LinearVelocityInternal.X; + b.Sweep.C.Y += step.dt * b.LinearVelocityInternal.Y; + b.Sweep.A += step.dt * b.AngularVelocityInternal; + + // Compute new transform + b.SynchronizeTransform(); + + // Note: shapes are synchronized later. + } + + // Iterate over constraints. + for (int i = 0; i < Settings.PositionIterations; ++i) + { + bool contactsOkay = _contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte); + bool jointsOkay = true; + +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + _watch.Start(); +#endif + for (int j = 0; j < JointCount; ++j) + { + Joint joint = _joints[j]; + if (!joint.Enabled) + continue; + + bool jointOkay = joint.SolvePositionConstraints(); + jointsOkay = jointsOkay && jointOkay; + } + +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + { + _watch.Stop(); + _tmpTime += _watch.ElapsedTicks; + _watch.Reset(); + } +#endif + if (contactsOkay && jointsOkay) + { + // Exit early if the position errors are small. + break; + } + } + +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + { + JointUpdateTime = _tmpTime; + } +#endif + + Report(_contactSolver.Constraints); + + 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.Flags & BodyFlags.AutoSleep) == 0) + { + b.SleepTime = 0.0f; + minSleepTime = 0.0f; + } + + if ((b.Flags & BodyFlags.AutoSleep) == 0 || + b.AngularVelocityInternal * b.AngularVelocityInternal > AngTolSqr || + Vector2.Dot(b.LinearVelocityInternal, b.LinearVelocityInternal) > LinTolSqr) + { + b.SleepTime = 0.0f; + minSleepTime = 0.0f; + } + else + { + b.SleepTime += step.dt; + minSleepTime = Math.Min(minSleepTime, b.SleepTime); + } + } + + if (minSleepTime >= Settings.TimeToSleep) + { + for (int i = 0; i < BodyCount; ++i) + { + Body b = Bodies[i]; + b.Awake = false; + } + } + } + } + + internal void SolveTOI(ref TimeStep subStep) + { + _contactSolver.Reset(_contacts, ContactCount, subStep.dtRatio, false); + + // Solve position constraints. + const float kTOIBaumgarte = 0.75f; + for (int i = 0; i < Settings.TOIPositionIterations; ++i) + { + bool contactsOkay = _contactSolver.SolvePositionConstraints(kTOIBaumgarte); + if (contactsOkay) + { + break; + } + + if (i == Settings.TOIPositionIterations - 1) + { + i += 0; + } + } + + // Leap of faith to new safe state. + for (int i = 0; i < BodyCount; ++i) + { + Body body = Bodies[i]; + body.Sweep.A0 = body.Sweep.A; + body.Sweep.C0 = body.Sweep.C; + } + + // 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. + + // Integrate positions. + for (int i = 0; i < BodyCount; ++i) + { + Body b = Bodies[i]; + + if (b.BodyType == BodyType.Static) + { + continue; + } + + // Check for large velocities. + float translationx = subStep.dt * b.LinearVelocityInternal.X; + float translationy = subStep.dt * b.LinearVelocityInternal.Y; + float dot = translationx * translationx + translationy * translationy; + if (dot > Settings.MaxTranslationSquared) + { + float norm = 1f / (float)Math.Sqrt(dot); + float value = Settings.MaxTranslation * subStep.inv_dt; + b.LinearVelocityInternal.X = value * (translationx * norm); + b.LinearVelocityInternal.Y = value * (translationy * norm); + } + + float rotation = subStep.dt * b.AngularVelocity; + if (rotation * rotation > Settings.MaxRotationSquared) + { + if (rotation < 0.0) + { + b.AngularVelocityInternal = -subStep.inv_dt * Settings.MaxRotation; + } + else + { + b.AngularVelocityInternal = subStep.inv_dt * Settings.MaxRotation; + } + } + + // Integrate + b.Sweep.C.X += subStep.dt * b.LinearVelocityInternal.X; + b.Sweep.C.Y += subStep.dt * b.LinearVelocityInternal.Y; + b.Sweep.A += subStep.dt * b.AngularVelocityInternal; + + // Compute new transform + b.SynchronizeTransform(); + + // Note: shapes are synchronized later. + } + + Report(_contactSolver.Constraints); + } + + public void Add(Body body) + { + Debug.Assert(BodyCount < _bodyCapacity); + 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(ContactConstraint[] constraints) + { + if (_contactManager == null) + return; + + for (int i = 0; i < ContactCount; ++i) + { + Contact c = _contacts[i]; + + if (c.FixtureA.AfterCollision != null) + c.FixtureA.AfterCollision(c.FixtureA, c.FixtureB, c); + + if (c.FixtureB.AfterCollision != null) + c.FixtureB.AfterCollision(c.FixtureB, c.FixtureA, c); + + if (_contactManager.PostSolve != null) + { + ContactConstraint cc = constraints[i]; + + _contactManager.PostSolve(c, cc); + } + } + } + } +} \ No newline at end of file diff --git a/axios/Dynamics/Joints/AngleJoint.cs b/axios/Dynamics/Joints/AngleJoint.cs new file mode 100644 index 0000000..3e05a4d --- /dev/null +++ b/axios/Dynamics/Joints/AngleJoint.cs @@ -0,0 +1,93 @@ +using System; +using System.Diagnostics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + /// + /// Maintains a fixed angle between two bodies + /// + public class AngleJoint : Joint + { + public float BiasFactor; + public float MaxImpulse; + public float Softness; + private float _bias; + private float _jointError; + private float _massFactor; + private float _targetAngle; + + internal AngleJoint() + { + JointType = JointType.Angle; + } + + public AngleJoint(Body bodyA, Body bodyB) + : base(bodyA, bodyB) + { + JointType = JointType.Angle; + TargetAngle = 0; + BiasFactor = .2f; + Softness = 0f; + MaxImpulse = float.MaxValue; + } + + public float TargetAngle + { + get { return _targetAngle; } + set + { + if (value != _targetAngle) + { + _targetAngle = value; + WakeBodies(); + } + } + } + + public override Vector2 WorldAnchorA + { + get { return BodyA.Position; } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.Position; } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + public override Vector2 GetReactionForce(float inv_dt) + { + //TODO + //return _inv_dt * _impulse; + return Vector2.Zero; + } + + public override float GetReactionTorque(float inv_dt) + { + return 0; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + _jointError = (BodyB.Sweep.A - BodyA.Sweep.A - TargetAngle); + + _bias = -BiasFactor * step.inv_dt * _jointError; + + _massFactor = (1 - Softness) / (BodyA.InvI + BodyB.InvI); + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + float p = (_bias - BodyB.AngularVelocity + BodyA.AngularVelocity) * _massFactor; + BodyA.AngularVelocity -= BodyA.InvI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse); + BodyB.AngularVelocity += BodyB.InvI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse); + } + + internal override bool SolvePositionConstraints() + { + //no position solving for this joint + return true; + } + } +} \ No newline at end of file diff --git a/axios/Dynamics/Joints/DistanceJoint.cs b/axios/Dynamics/Joints/DistanceJoint.cs new file mode 100644 index 0000000..80966df --- /dev/null +++ b/axios/Dynamics/Joints/DistanceJoint.cs @@ -0,0 +1,286 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not 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 + { + /// + /// The local anchor point relative to bodyA's origin. + /// + public Vector2 LocalAnchorA; + + /// + /// The local anchor point relative to bodyB's origin. + /// + public Vector2 LocalAnchorB; + + private float _bias; + private float _gamma; + private float _impulse; + private float _mass; + private float _tmpFloat1; + private Vector2 _tmpVector1; + private Vector2 _u; + + 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 + public DistanceJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB) + : base(bodyA, bodyB) + { + JointType = JointType.Distance; + + LocalAnchorA = localAnchorA; + LocalAnchorB = localAnchorB; + + Vector2 d = WorldAnchorB - WorldAnchorA; + Length = d.Length(); + } + + /// + /// 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. + /// + public float Frequency { get; set; } + + /// + /// The damping ratio. 0 = no damping, 1 = critical damping. + /// + public float DampingRatio { get; set; } + + public override sealed Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + } + + 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."); } + } + + public override Vector2 GetReactionForce(float inv_dt) + { + Vector2 F = (inv_dt * _impulse) * _u; + return F; + } + + public override float GetReactionTorque(float inv_dt) + { + return 0.0f; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + Body b2 = BodyB; + + // Compute the effective mass matrix. + Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter); + _u = b2.Sweep.C + r2 - b1.Sweep.C - r1; + + // Handle singularity. + float length = _u.Length(); + if (length > Settings.LinearSlop) + { + _u *= 1.0f / length; + } + else + { + _u = Vector2.Zero; + } + + float cr1u, cr2u; + MathUtils.Cross(ref r1, ref _u, out cr1u); + MathUtils.Cross(ref r2, ref _u, out cr2u); + float invMass = b1.InvMass + b1.InvI * cr1u * cr1u + b2.InvMass + b2.InvI * cr2u * cr2u; + Debug.Assert(invMass > Settings.Epsilon); + _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 + _gamma = step.dt * (d + step.dt * k); + _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f; + _bias = C * step.dt * k * _gamma; + + _mass = invMass + _gamma; + _mass = _mass != 0.0f ? 1.0f / _mass : 0.0f; + } + + if (Settings.EnableWarmstarting) + { + // Scale the impulse to support a variable time step. + _impulse *= step.dtRatio; + + Vector2 P = _impulse * _u; + b1.LinearVelocityInternal -= b1.InvMass * P; + MathUtils.Cross(ref r1, ref P, out _tmpFloat1); + b1.AngularVelocityInternal -= b1.InvI * /* r1 x P */ _tmpFloat1; + b2.LinearVelocityInternal += b2.InvMass * P; + MathUtils.Cross(ref r2, ref P, out _tmpFloat1); + b2.AngularVelocityInternal += b2.InvI * /* r2 x P */ _tmpFloat1; + } + else + { + _impulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + Body b2 = BodyB; + + Transform xf1, xf2; + b1.GetTransform(out xf1); + b2.GetTransform(out xf2); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter); + + // Cdot = dot(u, v + cross(w, r)) + MathUtils.Cross(b1.AngularVelocityInternal, ref r1, out _tmpVector1); + Vector2 v1 = b1.LinearVelocityInternal + _tmpVector1; + MathUtils.Cross(b2.AngularVelocityInternal, ref r2, out _tmpVector1); + Vector2 v2 = b2.LinearVelocityInternal + _tmpVector1; + float Cdot = Vector2.Dot(_u, v2 - v1); + + float impulse = -_mass * (Cdot + _bias + _gamma * _impulse); + _impulse += impulse; + + Vector2 P = impulse * _u; + b1.LinearVelocityInternal -= b1.InvMass * P; + MathUtils.Cross(ref r1, ref P, out _tmpFloat1); + b1.AngularVelocityInternal -= b1.InvI * _tmpFloat1; + b2.LinearVelocityInternal += b2.InvMass * P; + MathUtils.Cross(ref r2, ref P, out _tmpFloat1); + b2.AngularVelocityInternal += b2.InvI * _tmpFloat1; + } + + internal override bool SolvePositionConstraints() + { + if (Frequency > 0.0f) + { + // There is no position correction for soft distance constraints. + return true; + } + + Body b1 = BodyA; + Body b2 = BodyB; + + Transform xf1, xf2; + b1.GetTransform(out xf1); + b2.GetTransform(out xf2); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter); + + Vector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1; + + float length = d.Length(); + + if (length == 0.0f) + return true; + + d /= length; + float C = length - Length; + C = MathUtils.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); + + float impulse = -_mass * C; + _u = d; + Vector2 P = impulse * _u; + + b1.Sweep.C -= b1.InvMass * P; + MathUtils.Cross(ref r1, ref P, out _tmpFloat1); + b1.Sweep.A -= b1.InvI * _tmpFloat1; + b2.Sweep.C += b2.InvMass * P; + MathUtils.Cross(ref r2, ref P, out _tmpFloat1); + b2.Sweep.A += b2.InvI * _tmpFloat1; + + b1.SynchronizeTransform(); + b2.SynchronizeTransform(); + + return Math.Abs(C) < Settings.LinearSlop; + } + } +} \ No newline at end of file diff --git a/axios/Dynamics/Joints/FixedAngleJoint.cs b/axios/Dynamics/Joints/FixedAngleJoint.cs new file mode 100644 index 0000000..267fb0d --- /dev/null +++ b/axios/Dynamics/Joints/FixedAngleJoint.cs @@ -0,0 +1,84 @@ +using System; +using System.Diagnostics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + public class FixedAngleJoint : Joint + { + public float BiasFactor; + public float MaxImpulse; + public float Softness; + private float _bias; + private float _jointError; + private float _massFactor; + private float _targetAngle; + + public FixedAngleJoint(Body bodyA) + : base(bodyA) + { + JointType = JointType.FixedAngle; + TargetAngle = 0; + BiasFactor = .2f; + Softness = 0f; + MaxImpulse = float.MaxValue; + } + + public float TargetAngle + { + get { return _targetAngle; } + set + { + if (value != _targetAngle) + { + _targetAngle = value; + WakeBodies(); + } + } + } + + public override Vector2 WorldAnchorA + { + get { return BodyA.Position; } + } + + public override Vector2 WorldAnchorB + { + get { return BodyA.Position; } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + public override Vector2 GetReactionForce(float inv_dt) + { + //TODO + //return _inv_dt * _impulse; + return Vector2.Zero; + } + + public override float GetReactionTorque(float inv_dt) + { + return 0; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + _jointError = BodyA.Sweep.A - TargetAngle; + + _bias = -BiasFactor * step.inv_dt * _jointError; + + _massFactor = (1 - Softness) / (BodyA.InvI); + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + float p = (_bias - BodyA.AngularVelocity) * _massFactor; + BodyA.AngularVelocity += BodyA.InvI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse); + } + + internal override bool SolvePositionConstraints() + { + //no position solving for this joint + return true; + } + } +} \ No newline at end of file diff --git a/axios/Dynamics/Joints/FixedDistanceJoint.cs b/axios/Dynamics/Joints/FixedDistanceJoint.cs new file mode 100644 index 0000000..ce8e89b --- /dev/null +++ b/axios/Dynamics/Joints/FixedDistanceJoint.cs @@ -0,0 +1,255 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not 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 FixedDistanceJoint : Joint + { + /// + /// The local anchor point relative to bodyA's origin. + /// + public Vector2 LocalAnchorA; + + private float _bias; + private float _gamma; + private float _impulse; + private float _mass; + private Vector2 _u; + private Vector2 _worldAnchorB; + + /// + /// 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 body. + /// The body anchor. + /// The world anchor. + public FixedDistanceJoint(Body body, Vector2 bodyAnchor, Vector2 worldAnchor) + : base(body) + { + JointType = JointType.FixedDistance; + + LocalAnchorA = bodyAnchor; + _worldAnchorB = worldAnchor; + + //Calculate the length + Vector2 d = WorldAnchorB - WorldAnchorA; + Length = d.Length(); + } + + /// + /// 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. + /// + public float Frequency { get; set; } + + /// + /// The damping ratio. 0 = no damping, 1 = critical damping. + /// + public float DampingRatio { get; set; } + + public override sealed Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + } + + public override sealed Vector2 WorldAnchorB + { + get { return _worldAnchorB; } + set { _worldAnchorB = value; } + } + + public override Vector2 GetReactionForce(float invDt) + { + return (invDt * _impulse) * _u; + } + + public override float GetReactionTorque(float invDt) + { + return 0.0f; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + + Transform xf1; + b1.GetTransform(out xf1); + + // Compute the effective mass matrix. + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = _worldAnchorB; + _u = r2 - b1.Sweep.C - r1; + + // Handle singularity. + float length = _u.Length(); + if (length > Settings.LinearSlop) + { + _u *= 1.0f / length; + } + else + { + _u = Vector2.Zero; + } + + float cr1u = MathUtils.Cross(r1, _u); + float cr2u = MathUtils.Cross(r2, _u); + float invMass = b1.InvMass + b1.InvI * cr1u * cr1u + 0 * cr2u * cr2u; + Debug.Assert(invMass > Settings.Epsilon); + _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 + _gamma = step.dt * (d + step.dt * k); + _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f; + _bias = C * step.dt * k * _gamma; + + _mass = invMass + _gamma; + _mass = _mass != 0.0f ? 1.0f / _mass : 0.0f; + } + + if (Settings.EnableWarmstarting) + { + // Scale the impulse to support a variable time step. + _impulse *= step.dtRatio; + + Vector2 P = _impulse * _u; + b1.LinearVelocityInternal -= b1.InvMass * P; + b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P); + } + else + { + _impulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + + Transform xf1; + b1.GetTransform(out xf1); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + + // Cdot = dot(u, v + cross(w, r)) + Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1); + Vector2 v2 = Vector2.Zero; + float Cdot = Vector2.Dot(_u, v2 - v1); + + float impulse = -_mass * (Cdot + _bias + _gamma * _impulse); + _impulse += impulse; + + Vector2 P = impulse * _u; + b1.LinearVelocityInternal -= b1.InvMass * P; + b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P); + } + + internal override bool SolvePositionConstraints() + { + if (Frequency > 0.0f) + { + // There is no position correction for soft distance constraints. + return true; + } + + Body b1 = BodyA; + + Transform xf1; + b1.GetTransform(out xf1); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = _worldAnchorB; + + Vector2 d = r2 - b1.Sweep.C - r1; + + float length = d.Length(); + + if (length == 0.0f) + return true; + + d /= length; + float C = length - Length; + C = MathUtils.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); + + float impulse = -_mass * C; + _u = d; + Vector2 P = impulse * _u; + + b1.Sweep.C -= b1.InvMass * P; + b1.Sweep.A -= b1.InvI * MathUtils.Cross(r1, P); + + b1.SynchronizeTransform(); + + return Math.Abs(C) < Settings.LinearSlop; + } + } +} \ No newline at end of file diff --git a/axios/Dynamics/Joints/FixedFrictionJoint.cs b/axios/Dynamics/Joints/FixedFrictionJoint.cs new file mode 100644 index 0000000..9334a9a --- /dev/null +++ b/axios/Dynamics/Joints/FixedFrictionJoint.cs @@ -0,0 +1,227 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not 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 +{ + // 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 FixedFrictionJoint : Joint + { + public Vector2 LocalAnchorA; + + /// + /// The maximum friction force in N. + /// + public float MaxForce; + + /// + /// The maximum friction torque in N-m. + /// + public float MaxTorque; + + private float _angularImpulse; + private float _angularMass; + private Vector2 _linearImpulse; + private Mat22 _linearMass; + + public FixedFrictionJoint(Body body, Vector2 localAnchorA) + : base(body) + { + JointType = JointType.FixedFriction; + LocalAnchorA = localAnchorA; + + //Setting default max force and max torque + const float gravity = 10.0f; + + // For a circle: I = 0.5 * m * r * r ==> r = sqrt(2 * I / m) + float radius = (float)Math.Sqrt(2.0 * (body.Inertia / body.Mass)); + + MaxForce = body.Mass * gravity; + MaxTorque = body.Mass * radius * gravity; + } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + } + + public override Vector2 WorldAnchorB + { + get { return Vector2.Zero; } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + public override Vector2 GetReactionForce(float invDT) + { + return invDT * _linearImpulse; + } + + public override float GetReactionTorque(float invDT) + { + return invDT * _angularImpulse; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body bA = BodyA; + + Transform xfA; + bA.GetTransform(out xfA); + + // Compute the effective mass matrix. + Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter); + + // 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 = bA.InvMass; + float iA = bA.InvI; + + Mat22 K1 = new Mat22(); + K1.Col1.X = mA; + K1.Col2.X = 0.0f; + K1.Col1.Y = 0.0f; + K1.Col2.Y = mA; + + Mat22 K2 = new Mat22(); + K2.Col1.X = iA * rA.Y * rA.Y; + K2.Col2.X = -iA * rA.X * rA.Y; + K2.Col1.Y = -iA * rA.X * rA.Y; + K2.Col2.Y = iA * rA.X * rA.X; + + Mat22 K12; + Mat22.Add(ref K1, ref K2, out K12); + + _linearMass = K12.Inverse; + + _angularMass = iA; + if (_angularMass > 0.0f) + { + _angularMass = 1.0f / _angularMass; + } + + if (Settings.EnableWarmstarting) + { + // Scale impulses to support a variable time step. + _linearImpulse *= step.dtRatio; + _angularImpulse *= step.dtRatio; + + Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y); + + bA.LinearVelocityInternal -= mA * P; + bA.AngularVelocityInternal -= iA * (MathUtils.Cross(rA, P) + _angularImpulse); + } + else + { + _linearImpulse = Vector2.Zero; + _angularImpulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body bA = BodyA; + + Vector2 vA = bA.LinearVelocityInternal; + float wA = bA.AngularVelocityInternal; + + float mA = bA.InvMass; + float iA = bA.InvI; + + Transform xfA; + bA.GetTransform(out xfA); + + Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter); + + // Solve angular friction + { + float Cdot = -wA; + float impulse = -_angularMass * Cdot; + + float oldImpulse = _angularImpulse; + float maxImpulse = step.dt * MaxTorque; + _angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse); + impulse = _angularImpulse - oldImpulse; + + wA -= iA * impulse; + } + + // Solve linear friction + { + Vector2 Cdot = -vA - MathUtils.Cross(wA, rA); + + Vector2 impulse = -MathUtils.Multiply(ref _linearMass, Cdot); + Vector2 oldImpulse = _linearImpulse; + _linearImpulse += impulse; + + float maxImpulse = step.dt * MaxForce; + + if (_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) + { + _linearImpulse.Normalize(); + _linearImpulse *= maxImpulse; + } + + impulse = _linearImpulse - oldImpulse; + + vA -= mA * impulse; + wA -= iA * MathUtils.Cross(rA, impulse); + } + + bA.LinearVelocityInternal = vA; + bA.AngularVelocityInternal = wA; + } + + internal override bool SolvePositionConstraints() + { + return true; + } + } +} \ No newline at end of file diff --git a/axios/Dynamics/Joints/FixedLineJoint.cs b/axios/Dynamics/Joints/FixedLineJoint.cs new file mode 100644 index 0000000..d01a81d --- /dev/null +++ b/axios/Dynamics/Joints/FixedLineJoint.cs @@ -0,0 +1,413 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not 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 +{ + public class FixedLineJoint : Joint + { + private Vector2 _ax, _ay; + private float _bias; + private bool _enableMotor; + private float _gamma; + private float _impulse; + private Vector2 _localXAxis; + private Vector2 _localYAxisA; + private float _mass; + private float _maxMotorTorque; + private float _motorImpulse; + private float _motorMass; + private float _motorSpeed; + + private float _sAx; + private float _sAy; + private float _sBx; + private float _sBy; + + private float _springImpulse; + private float _springMass; + + // 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] + + internal FixedLineJoint() { JointType = JointType.FixedLine; } + + public FixedLineJoint(Body body, Vector2 worldAnchor, Vector2 axis) + : base(body) + { + JointType = JointType.FixedLine; + + BodyB = BodyA; + + LocalAnchorA = worldAnchor; + LocalAnchorB = BodyB.GetLocalPoint(worldAnchor); + LocalXAxis = axis; + } + + public Vector2 LocalAnchorA { get; set; } + + public Vector2 LocalAnchorB { get; set; } + + public override Vector2 WorldAnchorA + { + get { return LocalAnchorA; } + } + + public override Vector2 WorldAnchorB + { + get { return BodyA.GetWorldPoint(LocalAnchorB); } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + 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; + } + } + + public float JointSpeed + { + get + { + float wA = BodyA.AngularVelocityInternal; + float wB = BodyB.AngularVelocityInternal; + return wB - wA; + } + } + + public bool MotorEnabled + { + get { return _enableMotor; } + set + { + BodyA.Awake = true; + BodyB.Awake = true; + _enableMotor = value; + } + } + + public float MotorSpeed + { + set + { + BodyA.Awake = true; + BodyB.Awake = true; + _motorSpeed = value; + } + get { return _motorSpeed; } + } + + public float MaxMotorTorque + { + set + { + BodyA.Awake = true; + BodyB.Awake = true; + _maxMotorTorque = value; + } + get { return _maxMotorTorque; } + } + + public float Frequency { get; set; } + + public float DampingRatio { get; set; } + + public Vector2 LocalXAxis + { + get { return _localXAxis; } + set + { + _localXAxis = value; + _localYAxisA = MathUtils.Cross(1.0f, _localXAxis); + } + } + + 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 TimeStep step) + { + Body bB = BodyB; + + LocalCenterA = Vector2.Zero; + LocalCenterB = bB.LocalCenter; + + Transform xfB; + bB.GetTransform(out xfB); + + // Compute the effective masses. + Vector2 rA = LocalAnchorA; + Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - LocalCenterB); + Vector2 d = bB.Sweep.C + rB - rA; + + InvMassA = 0.0f; + InvIA = 0.0f; + InvMassB = bB.InvMass; + InvIB = bB.InvI; + + // Point to line constraint + { + _ay = _localYAxisA; + _sAy = MathUtils.Cross(d + rA, _ay); + _sBy = MathUtils.Cross(rB, _ay); + + _mass = InvMassA + InvMassB + InvIA * _sAy * _sAy + InvIB * _sBy * _sBy; + + if (_mass > 0.0f) + { + _mass = 1.0f / _mass; + } + } + + // Spring constraint + _springMass = 0.0f; + if (Frequency > 0.0f) + { + _ax = LocalXAxis; + _sAx = MathUtils.Cross(d + rA, _ax); + _sBx = MathUtils.Cross(rB, _ax); + + float invMass = InvMassA + InvMassB + InvIA * _sAx * _sAx + InvIB * _sBx * _sBx; + + if (invMass > 0.0f) + { + _springMass = 1.0f / invMass; + + float C = Vector2.Dot(d, _ax); + + // Frequency + float omega = 2.0f * Settings.Pi * Frequency; + + // Damping coefficient + float da = 2.0f * _springMass * DampingRatio * omega; + + // Spring stiffness + float k = _springMass * omega * omega; + + // magic formulas + _gamma = step.dt * (da + step.dt * k); + if (_gamma > 0.0f) + { + _gamma = 1.0f / _gamma; + } + + _bias = C * step.dt * k * _gamma; + + _springMass = invMass + _gamma; + if (_springMass > 0.0f) + { + _springMass = 1.0f / _springMass; + } + } + } + else + { + _springImpulse = 0.0f; + _springMass = 0.0f; + } + + // Rotational motor + if (_enableMotor) + { + _motorMass = InvIA + InvIB; + if (_motorMass > 0.0f) + { + _motorMass = 1.0f / _motorMass; + } + } + else + { + _motorMass = 0.0f; + _motorImpulse = 0.0f; + } + + if (Settings.EnableWarmstarting) + { + // Account for variable time step. + _impulse *= step.dtRatio; + _springImpulse *= step.dtRatio; + _motorImpulse *= step.dtRatio; + + Vector2 P = _impulse * _ay + _springImpulse * _ax; + float LB = _impulse * _sBy + _springImpulse * _sBx + _motorImpulse; + + bB.LinearVelocityInternal += InvMassB * P; + bB.AngularVelocityInternal += InvIB * LB; + } + else + { + _impulse = 0.0f; + _springImpulse = 0.0f; + _motorImpulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body bB = BodyB; + + Vector2 vA = Vector2.Zero; + float wA = 0.0f; + Vector2 vB = bB.LinearVelocityInternal; + float wB = bB.AngularVelocityInternal; + + // 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 -= InvMassA * P; + wA -= InvIA * LA; + + vB += InvMassB * P; + wB += InvIB * LB; + } + + // Solve rotational motor constraint + { + float Cdot = wB - wA - _motorSpeed; + float impulse = -_motorMass * Cdot; + + float oldImpulse = _motorImpulse; + float maxImpulse = step.dt * _maxMotorTorque; + _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = _motorImpulse - oldImpulse; + + wA -= InvIA * impulse; + wB += InvIB * 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 LB = impulse * _sBy; + + vB += InvMassB * P; + wB += InvIB * LB; + } + + bB.LinearVelocityInternal = vB; + bB.AngularVelocityInternal = wB; + } + + internal override bool SolvePositionConstraints() + { + Body bB = BodyB; + + Vector2 xA = Vector2.Zero; + const float angleA = 0.0f; + + Vector2 xB = bB.Sweep.C; + float angleB = bB.Sweep.A; + + Mat22 RA = new Mat22(angleA); + Mat22 RB = new Mat22(angleB); + + Vector2 rA = MathUtils.Multiply(ref RA, LocalAnchorA - LocalCenterA); + Vector2 rB = MathUtils.Multiply(ref RB, LocalAnchorB - LocalCenterB); + Vector2 d = xB + rB - xA - rA; + + Vector2 ay = MathUtils.Multiply(ref RA, _localYAxisA); + + 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 LB = impulse * sBy; + + xB += InvMassB * P; + angleB += InvIB * LB; + + // TODO_ERIN remove need for this. + bB.Sweep.C = xB; + bB.Sweep.A = angleB; + bB.SynchronizeTransform(); + + return Math.Abs(C) <= Settings.LinearSlop; + } + + public float GetMotorTorque(float invDt) + { + return invDt * _motorImpulse; + } + } +} \ No newline at end of file diff --git a/axios/Dynamics/Joints/FixedMouseJoint.cs b/axios/Dynamics/Joints/FixedMouseJoint.cs new file mode 100644 index 0000000..eeebb72 --- /dev/null +++ b/axios/Dynamics/Joints/FixedMouseJoint.cs @@ -0,0 +1,209 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and 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 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 + { + public Vector2 LocalAnchorA; + private Vector2 _C; // position error + private float _beta; + private float _gamma; + private Vector2 _impulse; + private Mat22 _mass; // effective mass for point-to-point constraint. + + private Vector2 _worldAnchor; + + /// + /// 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; + + Debug.Assert(worldAnchor.IsValid()); + + Transform xf1; + BodyA.GetTransform(out xf1); + + _worldAnchor = worldAnchor; + LocalAnchorA = BodyA.GetLocalPoint(worldAnchor); + } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + } + + public override Vector2 WorldAnchorB + { + get { return _worldAnchor; } + set + { + BodyA.Awake = true; + _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; set; } + + /// + /// The response speed. + /// + public float Frequency { get; set; } + + /// + /// The damping ratio. 0 = no damping, 1 = critical damping. + /// + public float DampingRatio { get; set; } + + public override Vector2 GetReactionForce(float inv_dt) + { + return inv_dt * _impulse; + } + + public override float GetReactionTorque(float inv_dt) + { + return inv_dt * 0.0f; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body b = BodyA; + + float mass = b.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. + Debug.Assert(d + step.dt * k > Settings.Epsilon); + + _gamma = step.dt * (d + step.dt * k); + if (_gamma != 0.0f) + { + _gamma = 1.0f / _gamma; + } + + _beta = step.dt * k * _gamma; + + // Compute the effective mass matrix. + Transform xf1; + b.GetTransform(out xf1); + Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b.LocalCenter); + + // 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] + float invMass = b.InvMass; + float invI = b.InvI; + + Mat22 K1 = new Mat22(new Vector2(invMass, 0.0f), new Vector2(0.0f, invMass)); + Mat22 K2 = new Mat22(new Vector2(invI * r.Y * r.Y, -invI * r.X * r.Y), + new Vector2(-invI * r.X * r.Y, invI * r.X * r.X)); + + Mat22 K; + Mat22.Add(ref K1, ref K2, out K); + + K.Col1.X += _gamma; + K.Col2.Y += _gamma; + + _mass = K.Inverse; + + _C = b.Sweep.C + r - _worldAnchor; + + // Cheat with some damping + b.AngularVelocityInternal *= 0.98f; + + // Warm starting. + _impulse *= step.dtRatio; + b.LinearVelocityInternal += invMass * _impulse; + b.AngularVelocityInternal += invI * MathUtils.Cross(r, _impulse); + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body b = BodyA; + + Transform xf1; + b.GetTransform(out xf1); + + Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b.LocalCenter); + + // Cdot = v + cross(w, r) + Vector2 Cdot = b.LinearVelocityInternal + MathUtils.Cross(b.AngularVelocityInternal, r); + Vector2 impulse = MathUtils.Multiply(ref _mass, -(Cdot + _beta * _C + _gamma * _impulse)); + + Vector2 oldImpulse = _impulse; + _impulse += impulse; + float maxImpulse = step.dt * MaxForce; + if (_impulse.LengthSquared() > maxImpulse * maxImpulse) + { + _impulse *= maxImpulse / _impulse.Length(); + } + impulse = _impulse - oldImpulse; + + b.LinearVelocityInternal += b.InvMass * impulse; + b.AngularVelocityInternal += b.InvI * MathUtils.Cross(r, impulse); + } + + internal override bool SolvePositionConstraints() + { + return true; + } + } +} \ No newline at end of file diff --git a/axios/Dynamics/Joints/FixedPrismaticJoint.cs b/axios/Dynamics/Joints/FixedPrismaticJoint.cs new file mode 100644 index 0000000..54d4f25 --- /dev/null +++ b/axios/Dynamics/Joints/FixedPrismaticJoint.cs @@ -0,0 +1,636 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not 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 body1. 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 FixedPrismaticJoint : Joint + { + private Mat33 _K; + private float _a1, _a2; + private Vector2 _axis; + private bool _enableLimit; + private bool _enableMotor; + private Vector3 _impulse; + private LimitState _limitState; + private Vector2 _localXAxis1; + private Vector2 _localYAxis1; + private float _lowerTranslation; + private float _maxMotorForce; + private float _motorMass; // effective mass for motor/limit translational constraint. + private float _motorSpeed; + private Vector2 _perp; + private float _refAngle; + private float _s1, _s2; + private float _upperTranslation; + + /// + /// 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 body. + /// The anchor. + /// The axis. + public FixedPrismaticJoint(Body body, Vector2 worldAnchor, Vector2 axis) + : base(body) + { + JointType = JointType.FixedPrismatic; + + BodyB = BodyA; + + LocalAnchorA = worldAnchor; + LocalAnchorB = BodyB.GetLocalPoint(worldAnchor); + + _localXAxis1 = axis; + _localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1); + _refAngle = BodyB.Rotation; + + _limitState = LimitState.Inactive; + } + + public Vector2 LocalAnchorA { get; set; } + + public Vector2 LocalAnchorB { get; set; } + + public override Vector2 WorldAnchorA + { + get { return LocalAnchorA; } + } + + public override Vector2 WorldAnchorB + { + get { return BodyA.GetWorldPoint(LocalAnchorB); } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + /// + /// Get the current joint translation, usually in meters. + /// + /// + public float JointTranslation + { + get + { + Vector2 d = BodyB.GetWorldPoint(LocalAnchorB) - LocalAnchorA; + Vector2 axis = _localXAxis1; + + return Vector2.Dot(d, axis); + } + } + + /// + /// Get the current joint translation speed, usually in meters per second. + /// + /// + public float JointSpeed + { + get + { + Transform xf2; + BodyB.GetTransform(out xf2); + + Vector2 r1 = LocalAnchorA; + Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - BodyB.LocalCenter); + Vector2 p1 = r1; + Vector2 p2 = BodyB.Sweep.C + r2; + Vector2 d = p2 - p1; + Vector2 axis = _localXAxis1; + + Vector2 v1 = Vector2.Zero; + Vector2 v2 = BodyB.LinearVelocityInternal; + const float w1 = 0.0f; + float w2 = BodyB.AngularVelocityInternal; + + 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, "Warning: limits does currently not work with fixed rotation"); + + WakeBodies(); + _enableLimit = value; + } + } + + /// + /// Get the lower joint limit, usually in meters. + /// + /// + public float LowerLimit + { + get { return _lowerTranslation; } + set + { + WakeBodies(); + _lowerTranslation = value; + } + } + + /// + /// Get the upper joint limit, usually in meters. + /// + /// + public float UpperLimit + { + get { return _upperTranslation; } + set + { + WakeBodies(); + _upperTranslation = value; + } + } + + /// + /// 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 + { + set + { + WakeBodies(); + _maxMotorForce = value; + } + } + + /// + /// Get the current motor force, usually in N. + /// + /// + public float MotorForce { get; set; } + + public Vector2 LocalXAxis1 + { + get { return _localXAxis1; } + set + { + _localXAxis1 = value; + _localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1); + } + } + + public override Vector2 GetReactionForce(float inv_dt) + { + return inv_dt * (_impulse.X * _perp + (MotorForce + _impulse.Z) * _axis); + } + + public override float GetReactionTorque(float inv_dt) + { + return inv_dt * _impulse.Y; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body bB = BodyB; + + LocalCenterA = Vector2.Zero; + LocalCenterB = bB.LocalCenter; + + Transform xf2; + bB.GetTransform(out xf2); + + // Compute the effective masses. + Vector2 r1 = LocalAnchorA; + Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - LocalCenterB); + Vector2 d = bB.Sweep.C + r2 - /* b1._sweep.Center - */ r1; + + InvMassA = 0.0f; + InvIA = 0.0f; + InvMassB = bB.InvMass; + InvIB = bB.InvI; + + // Compute motor Jacobian and effective mass. + { + _axis = _localXAxis1; + _a1 = MathUtils.Cross(d + r1, _axis); + _a2 = MathUtils.Cross(r2, _axis); + + _motorMass = InvMassA + InvMassB + InvIA * _a1 * _a1 + InvIB * _a2 * _a2; + + if (_motorMass > Settings.Epsilon) + { + _motorMass = 1.0f / _motorMass; + } + } + + // Prismatic constraint. + { + _perp = _localYAxis1; + + _s1 = MathUtils.Cross(d + r1, _perp); + _s2 = MathUtils.Cross(r2, _perp); + + float m1 = InvMassA, m2 = InvMassB; + float i1 = InvIA, i2 = InvIB; + + float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; + float k12 = i1 * _s1 + i2 * _s2; + float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2; + float k22 = i1 + i2; + float k23 = i1 * _a1 + i2 * _a2; + float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2; + + _K.Col1 = new Vector3(k11, k12, k13); + _K.Col2 = new Vector3(k12, k22, k23); + _K.Col3 = 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; + } + + if (_enableMotor == false) + { + MotorForce = 0.0f; + } + + if (Settings.EnableWarmstarting) + { + // Account for variable time step. + _impulse *= step.dtRatio; + MotorForce *= step.dtRatio; + + Vector2 P = _impulse.X * _perp + (MotorForce + _impulse.Z) * _axis; + float L2 = _impulse.X * _s2 + _impulse.Y + (MotorForce + _impulse.Z) * _a2; + + bB.LinearVelocityInternal += InvMassB * P; + bB.AngularVelocityInternal += InvIB * L2; + } + else + { + _impulse = Vector3.Zero; + MotorForce = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body bB = BodyB; + + Vector2 v1 = Vector2.Zero; + float w1 = 0.0f; + Vector2 v2 = bB.LinearVelocityInternal; + float w2 = bB.AngularVelocityInternal; + + // Solve linear motor constraint. + if (_enableMotor && _limitState != LimitState.Equal) + { + float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; + float impulse = _motorMass * (_motorSpeed - Cdot); + float oldImpulse = MotorForce; + float maxImpulse = step.dt * _maxMotorForce; + MotorForce = MathUtils.Clamp(MotorForce + impulse, -maxImpulse, maxImpulse); + impulse = MotorForce - oldImpulse; + + Vector2 P = impulse * _axis; + float L1 = impulse * _a1; + float L2 = impulse * _a2; + + v1 -= InvMassA * P; + w1 -= InvIA * L1; + + v2 += InvMassB * P; + w2 += InvIB * L2; + } + + Vector2 Cdot1 = new Vector2(Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1, w2 - w1); + + if (_enableLimit && _limitState != LimitState.Inactive) + { + // Solve prismatic and limit constraint in block form. + float Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; + 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.Col3.X, _K.Col3.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 L2 = df.X * _s2 + df.Y + df.Z * _a2; + + v2 += InvMassB * P; + w2 += InvIB * L2; + } + 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 L2 = df.X * _s2 + df.Y; + + v2 += InvMassB * P; + w2 += InvIB * L2; + } + + bB.LinearVelocityInternal = v2; + bB.AngularVelocityInternal = w2; + } + + internal override bool SolvePositionConstraints() + { + //Body b1 = BodyA; + Body b2 = BodyB; + + Vector2 c1 = Vector2.Zero; // b1._sweep.Center; + float a1 = 0.0f; // b1._sweep.Angle; + + Vector2 c2 = b2.Sweep.C; + float a2 = b2.Sweep.A; + + // Solve linear limit constraint. + float linearError = 0.0f; + bool active = false; + float C2 = 0.0f; + + Mat22 R1 = new Mat22(a1); + Mat22 R2 = new Mat22(a2); + + Vector2 r1 = MathUtils.Multiply(ref R1, LocalAnchorA - LocalCenterA); + Vector2 r2 = MathUtils.Multiply(ref R2, LocalAnchorB - LocalCenterB); + Vector2 d = c2 + r2 - c1 - r1; + + if (_enableLimit) + { + _axis = MathUtils.Multiply(ref R1, _localXAxis1); + + _a1 = MathUtils.Cross(d + r1, _axis); + _a2 = MathUtils.Cross(r2, _axis); + + 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.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 = _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 = translation - _upperTranslation; + active = true; + } + } + + _perp = MathUtils.Multiply(ref R1, _localYAxis1); + + _s1 = MathUtils.Cross(d + r1, _perp); + _s2 = MathUtils.Cross(r2, _perp); + + Vector3 impulse; + Vector2 C1 = new Vector2(Vector2.Dot(_perp, d), a2 - a1 - _refAngle); + + linearError = Math.Max(linearError, Math.Abs(C1.X)); + float angularError = Math.Abs(C1.Y); + + if (active) + { + float m1 = InvMassA, m2 = InvMassB; + float i1 = InvIA, i2 = InvIB; + + float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; + float k12 = i1 * _s1 + i2 * _s2; + float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2; + float k22 = i1 + i2; + float k23 = i1 * _a1 + i2 * _a2; + float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2; + + _K.Col1 = new Vector3(k11, k12, k13); + _K.Col2 = new Vector3(k12, k22, k23); + _K.Col3 = new Vector3(k13, k23, k33); + + Vector3 C = new Vector3(-C1.X, -C1.Y, -C2); + impulse = _K.Solve33(C); // negated above + } + else + { + float m1 = InvMassA, m2 = InvMassB; + float i1 = InvIA, i2 = InvIB; + + float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; + float k12 = i1 * _s1 + i2 * _s2; + float k22 = i1 + i2; + + _K.Col1 = new Vector3(k11, k12, 0.0f); + _K.Col2 = new Vector3(k12, k22, 0.0f); + + Vector2 impulse1 = _K.Solve22(-C1); + impulse.X = impulse1.X; + impulse.Y = impulse1.Y; + impulse.Z = 0.0f; + } + + Vector2 P = impulse.X * _perp + impulse.Z * _axis; + float L2 = impulse.X * _s2 + impulse.Y + impulse.Z * _a2; + + c2 += InvMassB * P; + a2 += InvIB * L2; + + // TODO_ERIN remove need for this. + b2.Sweep.C = c2; + b2.Sweep.A = a2; + b2.SynchronizeTransform(); + + return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; + } + } +} \ No newline at end of file diff --git a/axios/Dynamics/Joints/FixedRevoluteJoint.cs b/axios/Dynamics/Joints/FixedRevoluteJoint.cs new file mode 100644 index 0000000..c8b7de3 --- /dev/null +++ b/axios/Dynamics/Joints/FixedRevoluteJoint.cs @@ -0,0 +1,541 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not 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 +{ + /// + /// A revolute joint rains 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 FixedRevoluteJoint : Joint + { + private bool _enableLimit; + private bool _enableMotor; + private Vector3 _impulse; + private LimitState _limitState; + private float _lowerAngle; + private Mat33 _mass; // effective mass for point-to-point constraint. + private float _maxMotorTorque; + private float _motorImpulse; + private float _motorMass; // effective mass for motor/limit angular constraint. + private float _motorSpeed; + private float _upperAngle; + private Vector2 _worldAnchor; + + /// + /// Initialize the bodies, anchors, and reference angle using the world + /// anchor. + /// This requires defining an + /// anchor point where the bodies are joined. The definition + /// uses local anchor points so that the initial configuration + /// can violate the constraint slightly. You also need to + /// specify the initial relative angle for joint limits. This + /// helps when saving and loading a game. + /// The local anchor points are measured from the body's origin + /// rather than the center of mass because: + /// 1. you might not know where the center of mass will be. + /// 2. if you add/remove shapes from a body and recompute the mass, + /// the joints will be broken. + /// + /// The body. + /// The body anchor. + /// The world anchor. + public FixedRevoluteJoint(Body body, Vector2 bodyAnchor, Vector2 worldAnchor) + : base(body) + { + JointType = JointType.FixedRevolute; + + // Changed to local coordinates. + LocalAnchorA = bodyAnchor; + _worldAnchor = worldAnchor; + + ReferenceAngle = -BodyA.Rotation; + + _impulse = Vector3.Zero; + + _limitState = LimitState.Inactive; + } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + } + + public override Vector2 WorldAnchorB + { + get { return _worldAnchor; } + set { _worldAnchor = value; } + } + + public Vector2 LocalAnchorA { get; set; } + + public float ReferenceAngle { get; set; } + + /// + /// Get the current joint angle in radians. + /// + /// + public float JointAngle + { + get { return BodyA.Sweep.A - ReferenceAngle; } + } + + /// + /// Get the current joint angle speed in radians per second. + /// + /// + public float JointSpeed + { + get { return BodyA.AngularVelocityInternal; } + } + + /// + /// Is the joint limit enabled? + /// + /// true if [limit enabled]; otherwise, false. + public bool LimitEnabled + { + get { return _enableLimit; } + set + { + WakeBodies(); + _enableLimit = value; + } + } + + /// + /// Get the lower joint limit in radians. + /// + /// + public float LowerLimit + { + get { return _lowerAngle; } + set + { + WakeBodies(); + _lowerAngle = value; + } + } + + /// + /// Get the upper joint limit in radians. + /// + /// + public float UpperLimit + { + get { return _upperAngle; } + set + { + WakeBodies(); + _upperAngle = value; + } + } + + /// + /// 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 in radians per second. + /// + /// The speed. + public float MotorSpeed + { + set + { + WakeBodies(); + _motorSpeed = value; + } + get { return _motorSpeed; } + } + + /// + /// Set the maximum motor torque, usually in N-m. + /// + /// The torque. + public float MaxMotorTorque + { + set + { + WakeBodies(); + _maxMotorTorque = value; + } + get { return _maxMotorTorque; } + } + + /// + /// Get the current motor torque, usually in N-m. + /// + /// + public float MotorTorque + { + get { return _motorImpulse; } + set + { + WakeBodies(); + _motorImpulse = value; + } + } + + public override Vector2 GetReactionForce(float inv_dt) + { + return inv_dt * new Vector2(_impulse.X, _impulse.Y); + } + + public override float GetReactionTorque(float inv_dt) + { + return inv_dt * _impulse.Z; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + + if (_enableMotor || _enableLimit) + { + // You cannot create a rotation limit between bodies that + // both have fixed rotation. + Debug.Assert(b1.InvI > 0.0f /* || b2._invI > 0.0f*/); + } + + // Compute the effective mass matrix. + Transform xf1; + b1.GetTransform(out xf1); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = _worldAnchor; // MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter); + + // J = [-I -r1_skew I r2_skew] + // [ 0 -1 0 1] + // r_skew = [-ry; rx] + + // Matlab + // K = [ m1+r1y^2*i1+m2+r2y^2*i2, -r1y*i1*r1x-r2y*i2*r2x, -r1y*i1-r2y*i2] + // [ -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2, r1x*i1+r2x*i2] + // [ -r1y*i1-r2y*i2, r1x*i1+r2x*i2, i1+i2] + + float m1 = b1.InvMass; + const float m2 = 0; + float i1 = b1.InvI; + const float i2 = 0; + + _mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2; + _mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2; + _mass.Col3.X = -r1.Y * i1 - r2.Y * i2; + _mass.Col1.Y = _mass.Col2.X; + _mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2; + _mass.Col3.Y = r1.X * i1 + r2.X * i2; + _mass.Col1.Z = _mass.Col3.X; + _mass.Col2.Z = _mass.Col3.Y; + _mass.Col3.Z = i1 + i2; + + _motorMass = i1 + i2; + if (_motorMass > 0.0f) + { + _motorMass = 1.0f / _motorMass; + } + + if (_enableMotor == false) + { + _motorImpulse = 0.0f; + } + + if (_enableLimit) + { + float jointAngle = 0 - b1.Sweep.A - 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 *= step.dtRatio; + _motorImpulse *= step.dtRatio; + + Vector2 P = new Vector2(_impulse.X, _impulse.Y); + + b1.LinearVelocityInternal -= m1 * P; + b1.AngularVelocityInternal -= i1 * (MathUtils.Cross(r1, P) + _motorImpulse + _impulse.Z); + } + else + { + _impulse = Vector3.Zero; + _motorImpulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + + Vector2 v1 = b1.LinearVelocityInternal; + float w1 = b1.AngularVelocityInternal; + Vector2 v2 = Vector2.Zero; + const float w2 = 0; + + float m1 = b1.InvMass; + float i1 = b1.InvI; + + // Solve motor constraint. + if (_enableMotor && _limitState != LimitState.Equal) + { + float Cdot = w2 - w1 - _motorSpeed; + float impulse = _motorMass * (-Cdot); + float oldImpulse = _motorImpulse; + float maxImpulse = step.dt * _maxMotorTorque; + _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = _motorImpulse - oldImpulse; + + w1 -= i1 * impulse; + } + + // Solve limit constraint. + if (_enableLimit && _limitState != LimitState.Inactive) + { + Transform xf1; + b1.GetTransform(out xf1); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = _worldAnchor; + + // Solve point-to-point constraint + Vector2 Cdot1 = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1); + float Cdot2 = w2 - w1; + 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 reduced = _mass.Solve22(-Cdot1); + 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 if (_limitState == LimitState.AtUpper) + { + float newImpulse = _impulse.Z + impulse.Z; + if (newImpulse > 0.0f) + { + Vector2 reduced = _mass.Solve22(-Cdot1); + impulse.X = reduced.X; + impulse.Y = reduced.Y; + impulse.Z = -_impulse.Z; + _impulse.X += reduced.X; + _impulse.Y += reduced.Y; + _impulse.Z = 0.0f; + } + } + + Vector2 P = new Vector2(impulse.X, impulse.Y); + + v1 -= m1 * P; + w1 -= i1 * (MathUtils.Cross(r1, P) + impulse.Z); + } + else + { + Transform xf1; + b1.GetTransform(out xf1); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = _worldAnchor; + + // Solve point-to-point constraint + Vector2 Cdot = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1); + Vector2 impulse = _mass.Solve22(-Cdot); + + _impulse.X += impulse.X; + _impulse.Y += impulse.Y; + + v1 -= m1 * impulse; + w1 -= i1 * MathUtils.Cross(r1, impulse); + } + + b1.LinearVelocityInternal = v1; + b1.AngularVelocityInternal = w1; + } + + internal override bool SolvePositionConstraints() + { + // TODO_ERIN block solve with limit. COME ON ERIN + + Body b1 = BodyA; + + float angularError = 0.0f; + float positionError; + + // Solve angular limit constraint. + if (_enableLimit && _limitState != LimitState.Inactive) + { + float angle = 0 - b1.Sweep.A - 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; + } + + b1.Sweep.A -= b1.InvI * limitImpulse; + + b1.SynchronizeTransform(); + } + + // Solve point-to-point constraint. + { + Transform xf1; + b1.GetTransform(out xf1); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = _worldAnchor; + + Vector2 C = Vector2.Zero + r2 - b1.Sweep.C - r1; + positionError = C.Length(); + + float invMass1 = b1.InvMass; + const float invMass2 = 0; + float invI1 = b1.InvI; + const float invI2 = 0; + + // Handle large detachment. + const float k_allowedStretch = 10.0f * Settings.LinearSlop; + if (C.LengthSquared() > k_allowedStretch * k_allowedStretch) + { + // Use a particle solution (no rotation). + Vector2 u = C; + u.Normalize(); + float k = invMass1 + invMass2; + Debug.Assert(k > Settings.Epsilon); + float m = 1.0f / k; + Vector2 impulse2 = m * (-C); + const float k_beta = 0.5f; + b1.Sweep.C -= k_beta * invMass1 * impulse2; + + C = Vector2.Zero + r2 - b1.Sweep.C - r1; + } + + Mat22 K1 = new Mat22(new Vector2(invMass1 + invMass2, 0.0f), new Vector2(0.0f, invMass1 + invMass2)); + Mat22 K2 = new Mat22(new Vector2(invI1 * r1.Y * r1.Y, -invI1 * r1.X * r1.Y), + new Vector2(-invI1 * r1.X * r1.Y, invI1 * r1.X * r1.X)); + Mat22 K3 = new Mat22(new Vector2(invI2 * r2.Y * r2.Y, -invI2 * r2.X * r2.Y), + new Vector2(-invI2 * r2.X * r2.Y, invI2 * r2.X * r2.X)); + + Mat22 Ka; + Mat22.Add(ref K1, ref K2, out Ka); + + Mat22 K; + Mat22.Add(ref Ka, ref K3, out K); + + Vector2 impulse = K.Solve(-C); + + b1.Sweep.C -= b1.InvMass * impulse; + b1.Sweep.A -= b1.InvI * MathUtils.Cross(r1, impulse); + + b1.SynchronizeTransform(); + } + + return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; + } + } +} \ No newline at end of file diff --git a/axios/Dynamics/Joints/FrictionJoint.cs b/axios/Dynamics/Joints/FrictionJoint.cs new file mode 100644 index 0000000..aba89a6 --- /dev/null +++ b/axios/Dynamics/Joints/FrictionJoint.cs @@ -0,0 +1,249 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and 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 +{ + // 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 + { + public Vector2 LocalAnchorA; + public Vector2 LocalAnchorB; + private float _angularImpulse; + private float _angularMass; + private Vector2 _linearImpulse; + private Mat22 _linearMass; + + internal FrictionJoint() + { + JointType = JointType.Friction; + } + + public FrictionJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB) + : base(bodyA, bodyB) + { + JointType = JointType.Friction; + LocalAnchorA = localAnchorA; + LocalAnchorB = localAnchorB; + } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + } + + 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 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 inv_dt) + { + return inv_dt * _linearImpulse; + } + + public override float GetReactionTorque(float inv_dt) + { + return inv_dt * _angularImpulse; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body bA = BodyA; + Body bB = BodyB; + + Transform xfA, xfB; + bA.GetTransform(out xfA); + bB.GetTransform(out xfB); + + // Compute the effective mass matrix. + Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter); + Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter); + + // 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 = bA.InvMass, mB = bB.InvMass; + float iA = bA.InvI, iB = bB.InvI; + + Mat22 K1 = new Mat22(); + K1.Col1.X = mA + mB; + K1.Col2.X = 0.0f; + K1.Col1.Y = 0.0f; + K1.Col2.Y = mA + mB; + + Mat22 K2 = new Mat22(); + K2.Col1.X = iA * rA.Y * rA.Y; + K2.Col2.X = -iA * rA.X * rA.Y; + K2.Col1.Y = -iA * rA.X * rA.Y; + K2.Col2.Y = iA * rA.X * rA.X; + + Mat22 K3 = new Mat22(); + K3.Col1.X = iB * rB.Y * rB.Y; + K3.Col2.X = -iB * rB.X * rB.Y; + K3.Col1.Y = -iB * rB.X * rB.Y; + K3.Col2.Y = iB * rB.X * rB.X; + + Mat22 K12; + Mat22.Add(ref K1, ref K2, out K12); + + Mat22 K; + Mat22.Add(ref K12, ref K3, out K); + + _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 *= step.dtRatio; + _angularImpulse *= step.dtRatio; + + Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y); + + bA.LinearVelocityInternal -= mA * P; + bA.AngularVelocityInternal -= iA * (MathUtils.Cross(rA, P) + _angularImpulse); + + bB.LinearVelocityInternal += mB * P; + bB.AngularVelocityInternal += iB * (MathUtils.Cross(rB, P) + _angularImpulse); + } + else + { + _linearImpulse = Vector2.Zero; + _angularImpulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body bA = BodyA; + Body bB = BodyB; + + Vector2 vA = bA.LinearVelocityInternal; + float wA = bA.AngularVelocityInternal; + Vector2 vB = bB.LinearVelocityInternal; + float wB = bB.AngularVelocityInternal; + + float mA = bA.InvMass, mB = bB.InvMass; + float iA = bA.InvI, iB = bB.InvI; + + Transform xfA, xfB; + bA.GetTransform(out xfA); + bB.GetTransform(out xfB); + + Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter); + Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter); + + // Solve angular friction + { + float Cdot = wB - wA; + float impulse = -_angularMass * Cdot; + + float oldImpulse = _angularImpulse; + float maxImpulse = step.dt * 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.Multiply(ref _linearMass, Cdot); + Vector2 oldImpulse = _linearImpulse; + _linearImpulse += impulse; + + float maxImpulse = step.dt * 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); + } + + bA.LinearVelocityInternal = vA; + bA.AngularVelocityInternal = wA; + bB.LinearVelocityInternal = vB; + bB.AngularVelocityInternal = wB; + } + + internal override bool SolvePositionConstraints() + { + return true; + } + } +} \ No newline at end of file diff --git a/axios/Dynamics/Joints/GearJoint.cs b/axios/Dynamics/Joints/GearJoint.cs new file mode 100644 index 0000000..bff6cad --- /dev/null +++ b/axios/Dynamics/Joints/GearJoint.cs @@ -0,0 +1,350 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and 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 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: + /// coordinate1 + ratio * coordinate2 = ant + /// 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 The revolute and prismatic joints must be attached to + /// fixed bodies (which must be body1 on those joints). + /// + public class GearJoint : Joint + { + private Jacobian _J; + + private float _ant; + private FixedPrismaticJoint _fixedPrismatic1; + private FixedPrismaticJoint _fixedPrismatic2; + private FixedRevoluteJoint _fixedRevolute1; + private FixedRevoluteJoint _fixedRevolute2; + private float _impulse; + private float _mass; + private PrismaticJoint _prismatic1; + private PrismaticJoint _prismatic2; + private RevoluteJoint _revolute1; + private RevoluteJoint _revolute2; + + /// + /// 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. + public GearJoint(Joint jointA, Joint jointB, float ratio) + : base(jointA.BodyA, jointA.BodyB) + { + JointType = JointType.Gear; + JointA = jointA; + JointB = jointB; + Ratio = ratio; + + JointType type1 = jointA.JointType; + JointType type2 = jointB.JointType; + + // Make sure its the right kind of joint + Debug.Assert(type1 == JointType.Revolute || + type1 == JointType.Prismatic || + type1 == JointType.FixedRevolute || + type1 == JointType.FixedPrismatic); + Debug.Assert(type2 == JointType.Revolute || + type2 == JointType.Prismatic || + type2 == JointType.FixedRevolute || + type2 == JointType.FixedPrismatic); + + // In the case of a prismatic and revolute joint, the first body must be static. + if (type1 == JointType.Revolute || type1 == JointType.Prismatic) + Debug.Assert(jointA.BodyA.BodyType == BodyType.Static); + if (type2 == JointType.Revolute || type2 == JointType.Prismatic) + Debug.Assert(jointB.BodyA.BodyType == BodyType.Static); + + float coordinate1 = 0.0f, coordinate2 = 0.0f; + + switch (type1) + { + case JointType.Revolute: + BodyA = jointA.BodyB; + _revolute1 = (RevoluteJoint)jointA; + LocalAnchor1 = _revolute1.LocalAnchorB; + coordinate1 = _revolute1.JointAngle; + break; + case JointType.Prismatic: + BodyA = jointA.BodyB; + _prismatic1 = (PrismaticJoint)jointA; + LocalAnchor1 = _prismatic1.LocalAnchorB; + coordinate1 = _prismatic1.JointTranslation; + break; + case JointType.FixedRevolute: + BodyA = jointA.BodyA; + _fixedRevolute1 = (FixedRevoluteJoint)jointA; + LocalAnchor1 = _fixedRevolute1.LocalAnchorA; + coordinate1 = _fixedRevolute1.JointAngle; + break; + case JointType.FixedPrismatic: + BodyA = jointA.BodyA; + _fixedPrismatic1 = (FixedPrismaticJoint)jointA; + LocalAnchor1 = _fixedPrismatic1.LocalAnchorA; + coordinate1 = _fixedPrismatic1.JointTranslation; + break; + } + + switch (type2) + { + case JointType.Revolute: + BodyB = jointB.BodyB; + _revolute2 = (RevoluteJoint)jointB; + LocalAnchor2 = _revolute2.LocalAnchorB; + coordinate2 = _revolute2.JointAngle; + break; + case JointType.Prismatic: + BodyB = jointB.BodyB; + _prismatic2 = (PrismaticJoint)jointB; + LocalAnchor2 = _prismatic2.LocalAnchorB; + coordinate2 = _prismatic2.JointTranslation; + break; + case JointType.FixedRevolute: + BodyB = jointB.BodyA; + _fixedRevolute2 = (FixedRevoluteJoint)jointB; + LocalAnchor2 = _fixedRevolute2.LocalAnchorA; + coordinate2 = _fixedRevolute2.JointAngle; + break; + case JointType.FixedPrismatic: + BodyB = jointB.BodyA; + _fixedPrismatic2 = (FixedPrismaticJoint)jointB; + LocalAnchor2 = _fixedPrismatic2.LocalAnchorA; + coordinate2 = _fixedPrismatic2.JointTranslation; + break; + } + + _ant = coordinate1 + Ratio * coordinate2; + } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchor1); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchor2); } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + /// + /// The gear ratio. + /// + public float Ratio { get; set; } + + /// + /// The first revolute/prismatic joint attached to the gear joint. + /// + public Joint JointA { get; set; } + + /// + /// The second revolute/prismatic joint attached to the gear joint. + /// + public Joint JointB { get; set; } + + public Vector2 LocalAnchor1 { get; private set; } + public Vector2 LocalAnchor2 { get; private set; } + + public override Vector2 GetReactionForce(float inv_dt) + { + Vector2 P = _impulse * _J.LinearB; + return inv_dt * P; + } + + public override float GetReactionTorque(float inv_dt) + { + Transform xf1; + BodyB.GetTransform(out xf1); + + Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchor2 - BodyB.LocalCenter); + Vector2 P = _impulse * _J.LinearB; + float L = _impulse * _J.AngularB - MathUtils.Cross(r, P); + return inv_dt * L; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + Body b2 = BodyB; + + float K = 0.0f; + _J.SetZero(); + + if (_revolute1 != null || _fixedRevolute1 != null) + { + _J.AngularA = -1.0f; + K += b1.InvI; + } + else + { + Vector2 ug; + if (_prismatic1 != null) + ug = _prismatic1.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1); + else + ug = _fixedPrismatic1.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1); + + Transform xf1 /*, xfg1*/; + b1.GetTransform(out xf1); + //g1.GetTransform(out xfg1); + + + Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchor1 - b1.LocalCenter); + float crug = MathUtils.Cross(r, ug); + _J.LinearA = -ug; + _J.AngularA = -crug; + K += b1.InvMass + b1.InvI * crug * crug; + } + + if (_revolute2 != null || _fixedRevolute2 != null) + { + _J.AngularB = -Ratio; + K += Ratio * Ratio * b2.InvI; + } + else + { + Vector2 ug; + if (_prismatic2 != null) + ug = _prismatic2.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1); + else + ug = _fixedPrismatic2.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1); + + Transform /*xfg1,*/ xf2; + //g1.GetTransform(out xfg1); + b2.GetTransform(out xf2); + + Vector2 r = MathUtils.Multiply(ref xf2.R, LocalAnchor2 - b2.LocalCenter); + float crug = MathUtils.Cross(r, ug); + _J.LinearB = -Ratio * ug; + _J.AngularB = -Ratio * crug; + K += Ratio * Ratio * (b2.InvMass + b2.InvI * crug * crug); + } + + // Compute effective mass. + Debug.Assert(K > 0.0f); + _mass = K > 0.0f ? 1.0f / K : 0.0f; + + if (Settings.EnableWarmstarting) + { + // Warm starting. + b1.LinearVelocityInternal += b1.InvMass * _impulse * _J.LinearA; + b1.AngularVelocityInternal += b1.InvI * _impulse * _J.AngularA; + b2.LinearVelocityInternal += b2.InvMass * _impulse * _J.LinearB; + b2.AngularVelocityInternal += b2.InvI * _impulse * _J.AngularB; + } + else + { + _impulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + Body b2 = BodyB; + + float Cdot = _J.Compute(b1.LinearVelocityInternal, b1.AngularVelocityInternal, + b2.LinearVelocityInternal, b2.AngularVelocityInternal); + + float impulse = _mass * (-Cdot); + _impulse += impulse; + + b1.LinearVelocityInternal += b1.InvMass * impulse * _J.LinearA; + b1.AngularVelocityInternal += b1.InvI * impulse * _J.AngularA; + b2.LinearVelocityInternal += b2.InvMass * impulse * _J.LinearB; + b2.AngularVelocityInternal += b2.InvI * impulse * _J.AngularB; + } + + internal override bool SolvePositionConstraints() + { + const float linearError = 0.0f; + + Body b1 = BodyA; + Body b2 = BodyB; + + float coordinate1 = 0.0f, coordinate2 = 0.0f; + if (_revolute1 != null) + { + coordinate1 = _revolute1.JointAngle; + } + else if (_fixedRevolute1 != null) + { + coordinate1 = _fixedRevolute1.JointAngle; + } + else if (_prismatic1 != null) + { + coordinate1 = _prismatic1.JointTranslation; + } + else if (_fixedPrismatic1 != null) + { + coordinate1 = _fixedPrismatic1.JointTranslation; + } + + if (_revolute2 != null) + { + coordinate2 = _revolute2.JointAngle; + } + else if (_fixedRevolute2 != null) + { + coordinate2 = _fixedRevolute2.JointAngle; + } + else if (_prismatic2 != null) + { + coordinate2 = _prismatic2.JointTranslation; + } + else if (_fixedPrismatic2 != null) + { + coordinate2 = _fixedPrismatic2.JointTranslation; + } + + float C = _ant - (coordinate1 + Ratio * coordinate2); + + float impulse = _mass * (-C); + + b1.Sweep.C += b1.InvMass * impulse * _J.LinearA; + b1.Sweep.A += b1.InvI * impulse * _J.AngularA; + b2.Sweep.C += b2.InvMass * impulse * _J.LinearB; + b2.Sweep.A += b2.InvI * impulse * _J.AngularB; + + b1.SynchronizeTransform(); + b2.SynchronizeTransform(); + + // TODO_ERIN not implemented + return linearError < Settings.LinearSlop; + } + } +} \ No newline at end of file diff --git a/axios/Dynamics/Joints/Joint.cs b/axios/Dynamics/Joints/Joint.cs new file mode 100644 index 0000000..04db989 --- /dev/null +++ b/axios/Dynamics/Joints/Joint.cs @@ -0,0 +1,282 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not 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 + { + Revolute, + Prismatic, + Distance, + Pulley, + Gear, + Line, + Weld, + Friction, + Slider, + Angle, + Rope, + FixedMouse, + FixedRevolute, + FixedDistance, + FixedLine, + FixedPrismatic, + FixedAngle, + FixedFriction, + } + + public enum LimitState + { + Inactive, + AtLower, + AtUpper, + Equal, + } + + internal struct Jacobian + { + public float AngularA; + public float AngularB; + public Vector2 LinearA; + public Vector2 LinearB; + + public void SetZero() + { + LinearA = Vector2.Zero; + AngularA = 0.0f; + LinearB = Vector2.Zero; + AngularB = 0.0f; + } + + public void Set(Vector2 x1, float a1, Vector2 x2, float a2) + { + LinearA = x1; + AngularA = a1; + LinearB = x2; + AngularB = a2; + } + + public float Compute(Vector2 x1, float a1, Vector2 x2, float a2) + { + return Vector2.Dot(LinearA, x1) + AngularA * a1 + Vector2.Dot(LinearB, x2) + AngularB * a2; + } + } + + /// + /// 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 + { + /// + /// The Breakpoint simply indicates the maximum Value the JointError can be before it breaks. + /// The default value is float.MaxValue + /// + public float Breakpoint = float.MaxValue; + + internal JointEdge EdgeA = new JointEdge(); + internal JointEdge EdgeB = new JointEdge(); + public bool Enabled = true; + protected float InvIA; + protected float InvIB; + protected float InvMassA; + protected float InvMassB; + internal bool IslandFlag; + protected Vector2 LocalCenterA, LocalCenterB; + + protected Joint() + { + } + + protected Joint(Body body, Body bodyB) + { + Debug.Assert(body != bodyB); + + BodyA = body; + BodyB = bodyB; + + //Connected bodies should not collide by default + CollideConnected = false; + } + + /// + /// Constructor for fixed joint + /// + protected Joint(Body body) + { + BodyA = body; + + //Connected bodies should not collide by default + CollideConnected = false; + } + + /// + /// 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; set; } + + /// + /// Get the second body attached to this joint. + /// + /// + public Body BodyB { get; set; } + + /// + /// Get the anchor point on body1 in world coordinates. + /// + /// + public abstract Vector2 WorldAnchorA { get; } + + /// + /// Get the anchor point on body2 in world coordinates. + /// + /// + public abstract Vector2 WorldAnchorB { get; set; } + + /// + /// Set the user data pointer. + /// + /// The data. + public object UserData { get; set; } + + /// + /// Short-cut function to determine if either body is inactive. + /// + /// true if active; otherwise, false. + public bool Active + { + get { return BodyA.Enabled && BodyB.Enabled; } + } + + /// + /// Set this flag to true if the attached bodies should collide. + /// + public bool CollideConnected { get; set; } + + /// + /// Fires when the joint is broken. + /// + public event Action Broke; + + /// + /// Get the reaction force on body2 at the joint anchor in Newtons. + /// + /// The inv_dt. + /// + public abstract Vector2 GetReactionForce(float inv_dt); + + /// + /// Get the reaction torque on body2 in N*m. + /// + /// The inv_dt. + /// + public abstract float GetReactionTorque(float inv_dt); + + protected void WakeBodies() + { + 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 TimeStep step); + + internal void Validate(float invDT) + { + if (!Enabled) + return; + + float jointError = GetReactionForce(invDT).Length(); + if (Math.Abs(jointError) <= Breakpoint) + return; + + Enabled = false; + + if (Broke != null) + Broke(this, jointError); + } + + internal abstract void SolveVelocityConstraints(ref TimeStep step); + + /// + /// Solves the position constraints. + /// + /// returns true if the position errors are within tolerance. + internal abstract bool SolvePositionConstraints(); + } +} \ No newline at end of file diff --git a/axios/Dynamics/Joints/LineJoint.cs b/axios/Dynamics/Joints/LineJoint.cs new file mode 100644 index 0000000..34393a0 --- /dev/null +++ b/axios/Dynamics/Joints/LineJoint.cs @@ -0,0 +1,436 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not 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 +{ + public class LineJoint : Joint + { + private Vector2 _ax, _ay; + private float _bias; + private bool _enableMotor; + private float _gamma; + private float _impulse; + private Vector2 _localXAxis; + private Vector2 _localYAxisA; + private float _mass; + private float _maxMotorTorque; + private float _motorImpulse; + private float _motorMass; + private float _motorSpeed; + + private float _sAx; + private float _sAy; + private float _sBx; + private float _sBy; + + private float _springImpulse; + private float _springMass; + + // 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] + + internal LineJoint() + { + JointType = JointType.Line; + } + + public LineJoint(Body bA, Body bB, Vector2 anchor, Vector2 axis) + : base(bA, bB) + { + JointType = JointType.Line; + + LocalAnchorA = bA.GetLocalPoint(anchor); + LocalAnchorB = bB.GetLocalPoint(anchor); + LocalXAxis = bA.GetLocalVector(axis); + } + + public Vector2 LocalAnchorA { get; set; } + + public Vector2 LocalAnchorB { get; set; } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + 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; + } + } + + public float JointSpeed + { + get + { + float wA = BodyA.AngularVelocityInternal; + float wB = BodyB.AngularVelocityInternal; + return wB - wA; + } + } + + public bool MotorEnabled + { + get { return _enableMotor; } + set + { + BodyA.Awake = true; + BodyB.Awake = true; + _enableMotor = value; + } + } + + public float MotorSpeed + { + set + { + BodyA.Awake = true; + BodyB.Awake = true; + _motorSpeed = value; + } + get { return _motorSpeed; } + } + + public float MaxMotorTorque + { + set + { + BodyA.Awake = true; + BodyB.Awake = true; + _maxMotorTorque = value; + } + get { return _maxMotorTorque; } + } + + public float Frequency { get; set; } + + public float DampingRatio { get; set; } + + public Vector2 LocalXAxis + { + get { return _localXAxis; } + set + { + _localXAxis = value; + _localYAxisA = MathUtils.Cross(1.0f, _localXAxis); + } + } + + 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 TimeStep step) + { + Body bA = BodyA; + Body bB = BodyB; + + LocalCenterA = bA.LocalCenter; + LocalCenterB = bB.LocalCenter; + + Transform xfA; + bA.GetTransform(out xfA); + Transform xfB; + bB.GetTransform(out xfB); + + // Compute the effective masses. + Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - LocalCenterA); + Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - LocalCenterB); + Vector2 d = bB.Sweep.C + rB - bA.Sweep.C - rA; + + InvMassA = bA.InvMass; + InvIA = bA.InvI; + InvMassB = bB.InvMass; + InvIB = bB.InvI; + + // Point to line constraint + { + _ay = MathUtils.Multiply(ref xfA.R, _localYAxisA); + _sAy = MathUtils.Cross(d + rA, _ay); + _sBy = MathUtils.Cross(rB, _ay); + + _mass = InvMassA + InvMassB + InvIA * _sAy * _sAy + InvIB * _sBy * _sBy; + + if (_mass > 0.0f) + { + _mass = 1.0f / _mass; + } + } + + // Spring constraint + _springMass = 0.0f; + if (Frequency > 0.0f) + { + _ax = MathUtils.Multiply(ref xfA.R, LocalXAxis); + _sAx = MathUtils.Cross(d + rA, _ax); + _sBx = MathUtils.Cross(rB, _ax); + + float invMass = InvMassA + InvMassB + InvIA * _sAx * _sAx + InvIB * _sBx * _sBx; + + if (invMass > 0.0f) + { + _springMass = 1.0f / invMass; + + float C = Vector2.Dot(d, _ax); + + // Frequency + float omega = 2.0f * Settings.Pi * Frequency; + + // Damping coefficient + float da = 2.0f * _springMass * DampingRatio * omega; + + // Spring stiffness + float k = _springMass * omega * omega; + + // magic formulas + _gamma = step.dt * (da + step.dt * k); + if (_gamma > 0.0f) + { + _gamma = 1.0f / _gamma; + } + + _bias = C * step.dt * k * _gamma; + + _springMass = invMass + _gamma; + if (_springMass > 0.0f) + { + _springMass = 1.0f / _springMass; + } + } + } + else + { + _springImpulse = 0.0f; + _springMass = 0.0f; + } + + // Rotational motor + if (_enableMotor) + { + _motorMass = InvIA + InvIB; + if (_motorMass > 0.0f) + { + _motorMass = 1.0f / _motorMass; + } + } + else + { + _motorMass = 0.0f; + _motorImpulse = 0.0f; + } + + if (Settings.EnableWarmstarting) + { + // Account for variable time step. + _impulse *= step.dtRatio; + _springImpulse *= step.dtRatio; + _motorImpulse *= step.dtRatio; + + Vector2 P = _impulse * _ay + _springImpulse * _ax; + float LA = _impulse * _sAy + _springImpulse * _sAx + _motorImpulse; + float LB = _impulse * _sBy + _springImpulse * _sBx + _motorImpulse; + + bA.LinearVelocityInternal -= InvMassA * P; + bA.AngularVelocityInternal -= InvIA * LA; + + bB.LinearVelocityInternal += InvMassB * P; + bB.AngularVelocityInternal += InvIB * LB; + } + else + { + _impulse = 0.0f; + _springImpulse = 0.0f; + _motorImpulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body bA = BodyA; + Body bB = BodyB; + + Vector2 vA = bA.LinearVelocity; + float wA = bA.AngularVelocityInternal; + Vector2 vB = bB.LinearVelocityInternal; + float wB = bB.AngularVelocityInternal; + + // 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 -= InvMassA * P; + wA -= InvIA * LA; + + vB += InvMassB * P; + wB += InvIB * LB; + } + + // Solve rotational motor constraint + { + float Cdot = wB - wA - _motorSpeed; + float impulse = -_motorMass * Cdot; + + float oldImpulse = _motorImpulse; + float maxImpulse = step.dt * _maxMotorTorque; + _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = _motorImpulse - oldImpulse; + + wA -= InvIA * impulse; + wB += InvIB * 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 -= InvMassA * P; + wA -= InvIA * LA; + + vB += InvMassB * P; + wB += InvIB * LB; + } + + bA.LinearVelocityInternal = vA; + bA.AngularVelocityInternal = wA; + bB.LinearVelocityInternal = vB; + bB.AngularVelocityInternal = wB; + } + + internal override bool SolvePositionConstraints() + { + Body bA = BodyA; + Body bB = BodyB; + + Vector2 xA = bA.Sweep.C; + float angleA = bA.Sweep.A; + + Vector2 xB = bB.Sweep.C; + float angleB = bB.Sweep.A; + + Mat22 RA = new Mat22(angleA); + Mat22 RB = new Mat22(angleB); + + Vector2 rA = MathUtils.Multiply(ref RA, LocalAnchorA - LocalCenterA); + Vector2 rB = MathUtils.Multiply(ref RB, LocalAnchorB - LocalCenterB); + Vector2 d = xB + rB - xA - rA; + + Vector2 ay = MathUtils.Multiply(ref RA, _localYAxisA); + + 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; + + xA -= InvMassA * P; + angleA -= InvIA * LA; + xB += InvMassB * P; + angleB += InvIB * LB; + + // TODO_ERIN remove need for this. + bA.Sweep.C = xA; + bA.Sweep.A = angleA; + bB.Sweep.C = xB; + bB.Sweep.A = angleB; + bA.SynchronizeTransform(); + bB.SynchronizeTransform(); + + return Math.Abs(C) <= Settings.LinearSlop; + } + + public float GetMotorTorque(float invDt) + { + return invDt * _motorImpulse; + } + } +} \ No newline at end of file diff --git a/axios/Dynamics/Joints/PrismaticJoint.cs b/axios/Dynamics/Joints/PrismaticJoint.cs new file mode 100644 index 0000000..c854235 --- /dev/null +++ b/axios/Dynamics/Joints/PrismaticJoint.cs @@ -0,0 +1,677 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not 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 body1. 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 + { + public Vector2 LocalAnchorA; + + public Vector2 LocalAnchorB; + private Mat33 _K; + private float _a1, _a2; + private Vector2 _axis; + private bool _enableLimit; + private bool _enableMotor; + private Vector3 _impulse; + private LimitState _limitState; + private Vector2 _localXAxis1; + private Vector2 _localYAxis1; + private float _lowerTranslation; + private float _maxMotorForce; + private float _motorImpulse; + private float _motorMass; // effective mass for motor/limit translational constraint. + private float _motorSpeed; + private Vector2 _perp; + private float _refAngle; + private float _s1, _s2; + private float _upperTranslation; + + 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. + public PrismaticJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB, Vector2 axis) + : base(bodyA, bodyB) + { + JointType = JointType.Prismatic; + + LocalAnchorA = localAnchorA; + LocalAnchorB = localAnchorB; + + _localXAxis1 = BodyA.GetLocalVector(axis); + _localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1); + _refAngle = BodyB.Rotation - BodyA.Rotation; + + _limitState = LimitState.Inactive; + } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + /// + /// Get the current joint translation, usually in meters. + /// + /// + public float JointTranslation + { + get + { + Vector2 d = BodyB.GetWorldPoint(LocalAnchorB) - BodyA.GetWorldPoint(LocalAnchorA); + Vector2 axis = BodyA.GetWorldVector(ref _localXAxis1); + + 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.Multiply(ref xf1.R, LocalAnchorA - BodyA.LocalCenter); + Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - BodyB.LocalCenter); + Vector2 p1 = BodyA.Sweep.C + r1; + Vector2 p2 = BodyB.Sweep.C + r2; + Vector2 d = p2 - p1; + Vector2 axis = BodyA.GetWorldVector(ref _localXAxis1); + + Vector2 v1 = BodyA.LinearVelocityInternal; + Vector2 v2 = BodyB.LinearVelocityInternal; + float w1 = BodyA.AngularVelocityInternal; + float w2 = BodyB.AngularVelocityInternal; + + 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"); + + WakeBodies(); + _enableLimit = value; + } + } + + /// + /// Get the lower joint limit, usually in meters. + /// + /// + public float LowerLimit + { + get { return _lowerTranslation; } + set + { + WakeBodies(); + _lowerTranslation = value; + } + } + + /// + /// Get the upper joint limit, usually in meters. + /// + /// + public float UpperLimit + { + get { return _upperTranslation; } + set + { + WakeBodies(); + _upperTranslation = value; + } + } + + /// + /// 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 force, usually in N. + /// + /// + public float MotorForce + { + get { return _motorImpulse; } + set { _motorImpulse = value; } + } + + public Vector2 LocalXAxis1 + { + get { return _localXAxis1; } + set + { + _localXAxis1 = BodyA.GetLocalVector(value); + _localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1); + } + } + + public float ReferenceAngle + { + get { return _refAngle; } + set { _refAngle = value; } + } + + public override Vector2 GetReactionForce(float inv_dt) + { + return inv_dt * (_impulse.X * _perp + (_motorImpulse + _impulse.Z) * _axis); + } + + public override float GetReactionTorque(float inv_dt) + { + return inv_dt * _impulse.Y; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + Body b2 = BodyB; + + LocalCenterA = b1.LocalCenter; + LocalCenterB = b2.LocalCenter; + + Transform xf1, xf2; + b1.GetTransform(out xf1); + b2.GetTransform(out xf2); + + // Compute the effective masses. + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - LocalCenterA); + Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - LocalCenterB); + Vector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1; + + InvMassA = b1.InvMass; + InvIA = b1.InvI; + InvMassB = b2.InvMass; + InvIB = b2.InvI; + + // Compute motor Jacobian and effective mass. + { + _axis = MathUtils.Multiply(ref xf1.R, _localXAxis1); + _a1 = MathUtils.Cross(d + r1, _axis); + _a2 = MathUtils.Cross(r2, _axis); + + _motorMass = InvMassA + InvMassB + InvIA * _a1 * _a1 + InvIB * _a2 * _a2; + + if (_motorMass > Settings.Epsilon) + { + _motorMass = 1.0f / _motorMass; + } + } + + // Prismatic constraint. + { + _perp = MathUtils.Multiply(ref xf1.R, _localYAxis1); + + _s1 = MathUtils.Cross(d + r1, _perp); + _s2 = MathUtils.Cross(r2, _perp); + + float m1 = InvMassA, m2 = InvMassB; + float i1 = InvIA, i2 = InvIB; + + float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; + float k12 = i1 * _s1 + i2 * _s2; + float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2; + float k22 = i1 + i2; + float k23 = i1 * _a1 + i2 * _a2; + float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2; + + _K.Col1 = new Vector3(k11, k12, k13); + _K.Col2 = new Vector3(k12, k22, k23); + _K.Col3 = 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; + } + + if (_enableMotor == false) + { + _motorImpulse = 0.0f; + } + + if (Settings.EnableWarmstarting) + { + // Account for variable time step. + _impulse *= step.dtRatio; + _motorImpulse *= step.dtRatio; + + Vector2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Z) * _axis; + float L1 = _impulse.X * _s1 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a1; + float L2 = _impulse.X * _s2 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a2; + + b1.LinearVelocityInternal -= InvMassA * P; + b1.AngularVelocityInternal -= InvIA * L1; + + b2.LinearVelocityInternal += InvMassB * P; + b2.AngularVelocityInternal += InvIB * L2; + } + else + { + _impulse = Vector3.Zero; + _motorImpulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + Body b2 = BodyB; + + Vector2 v1 = b1.LinearVelocityInternal; + float w1 = b1.AngularVelocityInternal; + Vector2 v2 = b2.LinearVelocityInternal; + float w2 = b2.AngularVelocityInternal; + + // Solve linear motor constraint. + if (_enableMotor && _limitState != LimitState.Equal) + { + float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; + float impulse = _motorMass * (_motorSpeed - Cdot); + float oldImpulse = _motorImpulse; + float maxImpulse = step.dt * _maxMotorForce; + _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = _motorImpulse - oldImpulse; + + Vector2 P = impulse * _axis; + float L1 = impulse * _a1; + float L2 = impulse * _a2; + + v1 -= InvMassA * P; + w1 -= InvIA * L1; + + v2 += InvMassB * P; + w2 += InvIB * L2; + } + + Vector2 Cdot1 = new Vector2(Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1, w2 - w1); + + if (_enableLimit && _limitState != LimitState.Inactive) + { + // Solve prismatic and limit constraint in block form. + float Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; + 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.Col3.X, _K.Col3.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 L1 = df.X * _s1 + df.Y + df.Z * _a1; + float L2 = df.X * _s2 + df.Y + df.Z * _a2; + + v1 -= InvMassA * P; + w1 -= InvIA * L1; + + v2 += InvMassB * P; + w2 += InvIB * L2; + } + 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 L1 = df.X * _s1 + df.Y; + float L2 = df.X * _s2 + df.Y; + + v1 -= InvMassA * P; + w1 -= InvIA * L1; + + v2 += InvMassB * P; + w2 += InvIB * L2; + } + + b1.LinearVelocityInternal = v1; + b1.AngularVelocityInternal = w1; + b2.LinearVelocityInternal = v2; + b2.AngularVelocityInternal = w2; + } + + internal override bool SolvePositionConstraints() + { + Body b1 = BodyA; + Body b2 = BodyB; + + Vector2 c1 = b1.Sweep.C; + float a1 = b1.Sweep.A; + + Vector2 c2 = b2.Sweep.C; + float a2 = b2.Sweep.A; + + // Solve linear limit constraint. + float linearError = 0.0f; + bool active = false; + float C2 = 0.0f; + + Mat22 R1 = new Mat22(a1); + Mat22 R2 = new Mat22(a2); + + Vector2 r1 = MathUtils.Multiply(ref R1, LocalAnchorA - LocalCenterA); + Vector2 r2 = MathUtils.Multiply(ref R2, LocalAnchorB - LocalCenterB); + Vector2 d = c2 + r2 - c1 - r1; + + if (_enableLimit) + { + _axis = MathUtils.Multiply(ref R1, _localXAxis1); + + _a1 = MathUtils.Cross(d + r1, _axis); + _a2 = MathUtils.Cross(r2, _axis); + + 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.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 = _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 = translation - _upperTranslation; + active = true; + } + } + + _perp = MathUtils.Multiply(ref R1, _localYAxis1); + + _s1 = MathUtils.Cross(d + r1, _perp); + _s2 = MathUtils.Cross(r2, _perp); + + Vector3 impulse; + Vector2 C1 = new Vector2(Vector2.Dot(_perp, d), a2 - a1 - ReferenceAngle); + + linearError = Math.Max(linearError, Math.Abs(C1.X)); + float angularError = Math.Abs(C1.Y); + + if (active) + { + float m1 = InvMassA, m2 = InvMassB; + float i1 = InvIA, i2 = InvIB; + + float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; + float k12 = i1 * _s1 + i2 * _s2; + float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2; + float k22 = i1 + i2; + float k23 = i1 * _a1 + i2 * _a2; + float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2; + + _K.Col1 = new Vector3(k11, k12, k13); + _K.Col2 = new Vector3(k12, k22, k23); + _K.Col3 = new Vector3(k13, k23, k33); + + Vector3 C = new Vector3(-C1.X, -C1.Y, -C2); + impulse = _K.Solve33(C); // negated above + } + else + { + float m1 = InvMassA, m2 = InvMassB; + float i1 = InvIA, i2 = InvIB; + + float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; + float k12 = i1 * _s1 + i2 * _s2; + float k22 = i1 + i2; + + _K.Col1 = new Vector3(k11, k12, 0.0f); + _K.Col2 = new Vector3(k12, k22, 0.0f); + + Vector2 impulse1 = _K.Solve22(-C1); + impulse.X = impulse1.X; + impulse.Y = impulse1.Y; + impulse.Z = 0.0f; + } + + Vector2 P = impulse.X * _perp + impulse.Z * _axis; + float L1 = impulse.X * _s1 + impulse.Y + impulse.Z * _a1; + float L2 = impulse.X * _s2 + impulse.Y + impulse.Z * _a2; + + c1 -= InvMassA * P; + a1 -= InvIA * L1; + c2 += InvMassB * P; + a2 += InvIB * L2; + + // TODO_ERIN remove need for this. + b1.Sweep.C = c1; + b1.Sweep.A = a1; + b2.Sweep.C = c2; + b2.Sweep.A = a2; + b1.SynchronizeTransform(); + b2.SynchronizeTransform(); + + return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; + } + } +} \ No newline at end of file diff --git a/axios/Dynamics/Joints/PulleyJoint.cs b/axios/Dynamics/Joints/PulleyJoint.cs new file mode 100644 index 0000000..00a8088 --- /dev/null +++ b/axios/Dynamics/Joints/PulleyJoint.cs @@ -0,0 +1,507 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not 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 +{ + /// + /// The pulley joint is connected to two bodies and two fixed ground points. + /// The pulley supports a ratio such that: + /// length1 + ratio * length2 = ant + /// Yes, the force transmitted is scaled by the ratio. + /// The pulley also enforces a maximum length limit on both sides. This is + /// useful to prevent one side of the pulley hitting the top. + /// + public class PulleyJoint : Joint + { + /// + /// Get the first ground anchor. + /// + /// + public Vector2 GroundAnchorA; + + /// + /// Get the second ground anchor. + /// + /// + public Vector2 GroundAnchorB; + + public Vector2 LocalAnchorA; + public Vector2 LocalAnchorB; + + public float MinPulleyLength = 2.0f; + private float _ant; + private float _impulse; + private float _lengthA; + private float _lengthB; + private float _limitImpulse1; + private float _limitImpulse2; + private float _limitMass1; + private float _limitMass2; + private LimitState _limitState1; + private LimitState _limitState2; + private float _maxLengthA; + private float _maxLengthB; + + // Effective masses + private float _pulleyMass; + private LimitState _state; + private Vector2 _u1; + private Vector2 _u2; + + internal PulleyJoint() + { + JointType = JointType.Pulley; + } + + /// + /// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors. + /// This requires two ground anchors, + /// two dynamic body anchor points, max lengths for each side, + /// and a pulley ratio. + /// + /// The first body. + /// The second body. + /// The ground anchor for the first body. + /// The ground anchor for the second body. + /// The first body anchor. + /// The second body anchor. + /// The ratio. + public PulleyJoint(Body bodyA, Body bodyB, + Vector2 groundAnchorA, Vector2 groundAnchorB, + Vector2 localAnchorA, Vector2 localAnchorB, + float ratio) + : base(bodyA, bodyB) + { + JointType = JointType.Pulley; + + GroundAnchorA = groundAnchorA; + GroundAnchorB = groundAnchorB; + LocalAnchorA = localAnchorA; + LocalAnchorB = localAnchorB; + + Vector2 d1 = BodyA.GetWorldPoint(localAnchorA) - groundAnchorA; + _lengthA = d1.Length(); + + Vector2 d2 = BodyB.GetWorldPoint(localAnchorB) - groundAnchorB; + _lengthB = d2.Length(); + + Debug.Assert(ratio != 0.0f); + Debug.Assert(ratio > Settings.Epsilon); + Ratio = ratio; + + float C = _lengthA + Ratio * _lengthB; + + MaxLengthA = C - Ratio * MinPulleyLength; + MaxLengthB = (C - MinPulleyLength) / Ratio; + + _ant = _lengthA + Ratio * _lengthB; + + MaxLengthA = Math.Min(MaxLengthA, _ant - Ratio * MinPulleyLength); + MaxLengthB = Math.Min(MaxLengthB, (_ant - MinPulleyLength) / Ratio); + + _impulse = 0.0f; + _limitImpulse1 = 0.0f; + _limitImpulse2 = 0.0f; + } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + /// + /// Get the current length of the segment attached to body1. + /// + /// + public float LengthA + { + get + { + Vector2 d = BodyA.GetWorldPoint(LocalAnchorA) - GroundAnchorA; + return d.Length(); + } + set { _lengthA = value; } + } + + /// + /// Get the current length of the segment attached to body2. + /// + /// + public float LengthB + { + get + { + Vector2 d = BodyB.GetWorldPoint(LocalAnchorB) - GroundAnchorB; + return d.Length(); + } + set { _lengthB = value; } + } + + /// + /// Get the pulley ratio. + /// + /// + public float Ratio { get; set; } + + public float MaxLengthA + { + get { return _maxLengthA; } + set { _maxLengthA = value; } + } + + public float MaxLengthB + { + get { return _maxLengthB; } + set { _maxLengthB = value; } + } + + public override Vector2 GetReactionForce(float inv_dt) + { + Vector2 P = _impulse * _u2; + return inv_dt * P; + } + + public override float GetReactionTorque(float inv_dt) + { + return 0.0f; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + Body b2 = BodyB; + + Transform xf1, xf2; + b1.GetTransform(out xf1); + b2.GetTransform(out xf2); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter); + + Vector2 p1 = b1.Sweep.C + r1; + Vector2 p2 = b2.Sweep.C + r2; + + Vector2 s1 = GroundAnchorA; + Vector2 s2 = GroundAnchorB; + + // Get the pulley axes. + _u1 = p1 - s1; + _u2 = p2 - s2; + + float length1 = _u1.Length(); + float length2 = _u2.Length(); + + if (length1 > Settings.LinearSlop) + { + _u1 *= 1.0f / length1; + } + else + { + _u1 = Vector2.Zero; + } + + if (length2 > Settings.LinearSlop) + { + _u2 *= 1.0f / length2; + } + else + { + _u2 = Vector2.Zero; + } + + float C = _ant - length1 - Ratio * length2; + if (C > 0.0f) + { + _state = LimitState.Inactive; + _impulse = 0.0f; + } + else + { + _state = LimitState.AtUpper; + } + + if (length1 < MaxLengthA) + { + _limitState1 = LimitState.Inactive; + _limitImpulse1 = 0.0f; + } + else + { + _limitState1 = LimitState.AtUpper; + } + + if (length2 < MaxLengthB) + { + _limitState2 = LimitState.Inactive; + _limitImpulse2 = 0.0f; + } + else + { + _limitState2 = LimitState.AtUpper; + } + + // Compute effective mass. + float cr1u1 = MathUtils.Cross(r1, _u1); + float cr2u2 = MathUtils.Cross(r2, _u2); + + _limitMass1 = b1.InvMass + b1.InvI * cr1u1 * cr1u1; + _limitMass2 = b2.InvMass + b2.InvI * cr2u2 * cr2u2; + _pulleyMass = _limitMass1 + Ratio * Ratio * _limitMass2; + Debug.Assert(_limitMass1 > Settings.Epsilon); + Debug.Assert(_limitMass2 > Settings.Epsilon); + Debug.Assert(_pulleyMass > Settings.Epsilon); + _limitMass1 = 1.0f / _limitMass1; + _limitMass2 = 1.0f / _limitMass2; + _pulleyMass = 1.0f / _pulleyMass; + + if (Settings.EnableWarmstarting) + { + // Scale impulses to support variable time steps. + _impulse *= step.dtRatio; + _limitImpulse1 *= step.dtRatio; + _limitImpulse2 *= step.dtRatio; + + // Warm starting. + Vector2 P1 = -(_impulse + _limitImpulse1) * _u1; + Vector2 P2 = (-Ratio * _impulse - _limitImpulse2) * _u2; + b1.LinearVelocityInternal += b1.InvMass * P1; + b1.AngularVelocityInternal += b1.InvI * MathUtils.Cross(r1, P1); + b2.LinearVelocityInternal += b2.InvMass * P2; + b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P2); + } + else + { + _impulse = 0.0f; + _limitImpulse1 = 0.0f; + _limitImpulse2 = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + Body b2 = BodyB; + + Transform xf1, xf2; + b1.GetTransform(out xf1); + b2.GetTransform(out xf2); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter); + + if (_state == LimitState.AtUpper) + { + Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1); + Vector2 v2 = b2.LinearVelocityInternal + MathUtils.Cross(b2.AngularVelocityInternal, r2); + + float Cdot = -Vector2.Dot(_u1, v1) - Ratio * Vector2.Dot(_u2, v2); + float impulse = _pulleyMass * (-Cdot); + float oldImpulse = _impulse; + _impulse = Math.Max(0.0f, _impulse + impulse); + impulse = _impulse - oldImpulse; + + Vector2 P1 = -impulse * _u1; + Vector2 P2 = -Ratio * impulse * _u2; + b1.LinearVelocityInternal += b1.InvMass * P1; + b1.AngularVelocityInternal += b1.InvI * MathUtils.Cross(r1, P1); + b2.LinearVelocityInternal += b2.InvMass * P2; + b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P2); + } + + if (_limitState1 == LimitState.AtUpper) + { + Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1); + + float Cdot = -Vector2.Dot(_u1, v1); + float impulse = -_limitMass1 * Cdot; + float oldImpulse = _limitImpulse1; + _limitImpulse1 = Math.Max(0.0f, _limitImpulse1 + impulse); + impulse = _limitImpulse1 - oldImpulse; + + Vector2 P1 = -impulse * _u1; + b1.LinearVelocityInternal += b1.InvMass * P1; + b1.AngularVelocityInternal += b1.InvI * MathUtils.Cross(r1, P1); + } + + if (_limitState2 == LimitState.AtUpper) + { + Vector2 v2 = b2.LinearVelocityInternal + MathUtils.Cross(b2.AngularVelocityInternal, r2); + + float Cdot = -Vector2.Dot(_u2, v2); + float impulse = -_limitMass2 * Cdot; + float oldImpulse = _limitImpulse2; + _limitImpulse2 = Math.Max(0.0f, _limitImpulse2 + impulse); + impulse = _limitImpulse2 - oldImpulse; + + Vector2 P2 = -impulse * _u2; + b2.LinearVelocityInternal += b2.InvMass * P2; + b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P2); + } + } + + internal override bool SolvePositionConstraints() + { + Body b1 = BodyA; + Body b2 = BodyB; + + Vector2 s1 = GroundAnchorA; + Vector2 s2 = GroundAnchorB; + + float linearError = 0.0f; + + if (_state == LimitState.AtUpper) + { + Transform xf1, xf2; + b1.GetTransform(out xf1); + b2.GetTransform(out xf2); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter); + + Vector2 p1 = b1.Sweep.C + r1; + Vector2 p2 = b2.Sweep.C + r2; + + // Get the pulley axes. + _u1 = p1 - s1; + _u2 = p2 - s2; + + float length1 = _u1.Length(); + float length2 = _u2.Length(); + + if (length1 > Settings.LinearSlop) + { + _u1 *= 1.0f / length1; + } + else + { + _u1 = Vector2.Zero; + } + + if (length2 > Settings.LinearSlop) + { + _u2 *= 1.0f / length2; + } + else + { + _u2 = Vector2.Zero; + } + + float C = _ant - length1 - Ratio * length2; + linearError = Math.Max(linearError, -C); + + C = MathUtils.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); + float impulse = -_pulleyMass * C; + + Vector2 P1 = -impulse * _u1; + Vector2 P2 = -Ratio * impulse * _u2; + + b1.Sweep.C += b1.InvMass * P1; + b1.Sweep.A += b1.InvI * MathUtils.Cross(r1, P1); + b2.Sweep.C += b2.InvMass * P2; + b2.Sweep.A += b2.InvI * MathUtils.Cross(r2, P2); + + b1.SynchronizeTransform(); + b2.SynchronizeTransform(); + } + + if (_limitState1 == LimitState.AtUpper) + { + Transform xf1; + b1.GetTransform(out xf1); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 p1 = b1.Sweep.C + r1; + + _u1 = p1 - s1; + float length1 = _u1.Length(); + + if (length1 > Settings.LinearSlop) + { + _u1 *= 1.0f / length1; + } + else + { + _u1 = Vector2.Zero; + } + + float C = MaxLengthA - length1; + linearError = Math.Max(linearError, -C); + C = MathUtils.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); + float impulse = -_limitMass1 * C; + + Vector2 P1 = -impulse * _u1; + b1.Sweep.C += b1.InvMass * P1; + b1.Sweep.A += b1.InvI * MathUtils.Cross(r1, P1); + + b1.SynchronizeTransform(); + } + + if (_limitState2 == LimitState.AtUpper) + { + Transform xf2; + b2.GetTransform(out xf2); + + Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter); + Vector2 p2 = b2.Sweep.C + r2; + + _u2 = p2 - s2; + float length2 = _u2.Length(); + + if (length2 > Settings.LinearSlop) + { + _u2 *= 1.0f / length2; + } + else + { + _u2 = Vector2.Zero; + } + + float C = MaxLengthB - length2; + linearError = Math.Max(linearError, -C); + C = MathUtils.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); + float impulse = -_limitMass2 * C; + + Vector2 P2 = -impulse * _u2; + b2.Sweep.C += b2.InvMass * P2; + b2.Sweep.A += b2.InvI * MathUtils.Cross(r2, P2); + + b2.SynchronizeTransform(); + } + + return linearError < Settings.LinearSlop; + } + } +} \ No newline at end of file diff --git a/axios/Dynamics/Joints/RevoluteJoint.cs b/axios/Dynamics/Joints/RevoluteJoint.cs new file mode 100644 index 0000000..17b5e7a --- /dev/null +++ b/axios/Dynamics/Joints/RevoluteJoint.cs @@ -0,0 +1,595 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not 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 +{ + /// + /// A revolute joint rains 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 + { + public Vector2 LocalAnchorA; + + public Vector2 LocalAnchorB; + private bool _enableLimit; + private bool _enableMotor; + private Vector3 _impulse; + private LimitState _limitState; + private float _lowerAngle; + private Mat33 _mass; // effective mass for point-to-point constraint. + private float _maxMotorTorque; + private float _motorImpulse; + private float _motorMass; // effective mass for motor/limit angular constraint. + private float _motorSpeed; + private float _referenceAngle; + private float _tmpFloat1; + private Vector2 _tmpVector1, _tmpVector2; + private float _upperAngle; + + internal RevoluteJoint() + { + JointType = JointType.Revolute; + } + + /// + /// Initialize the bodies and local anchor. + /// This requires defining an + /// anchor point where the bodies are joined. The definition + /// uses local anchor points so that the initial configuration + /// can violate the constraint slightly. You also need to + /// specify the initial relative angle for joint limits. This + /// helps when saving and loading a game. + /// The local anchor points are measured from the body's origin + /// rather than the center of mass because: + /// 1. you might not know where the center of mass will be. + /// 2. if you add/remove shapes from a body and recompute the mass, + /// the joints will be broken. + /// + /// The first body. + /// The second body. + /// The first body anchor. + /// The second anchor. + public RevoluteJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB) + : base(bodyA, bodyB) + { + JointType = JointType.Revolute; + + // Changed to local coordinates. + LocalAnchorA = localAnchorA; + LocalAnchorB = localAnchorB; + + ReferenceAngle = BodyB.Rotation - BodyA.Rotation; + + _impulse = Vector3.Zero; + + _limitState = LimitState.Inactive; + } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + 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.AngularVelocityInternal - BodyA.AngularVelocityInternal; } + } + + /// + /// Is the joint limit enabled? + /// + /// true if [limit enabled]; otherwise, false. + public bool LimitEnabled + { + get { return _enableLimit; } + set + { + WakeBodies(); + _enableLimit = value; + } + } + + /// + /// Get the lower joint limit in radians. + /// + /// + public float LowerLimit + { + get { return _lowerAngle; } + set + { + WakeBodies(); + _lowerAngle = value; + } + } + + /// + /// Get the upper joint limit in radians. + /// + /// + public float UpperLimit + { + get { return _upperAngle; } + set + { + WakeBodies(); + _upperAngle = value; + } + } + + /// + /// 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 in radians per second. + /// + /// The speed. + public float MotorSpeed + { + set + { + WakeBodies(); + _motorSpeed = value; + } + get { return _motorSpeed; } + } + + /// + /// Set the maximum motor torque, usually in N-m. + /// + /// The torque. + public float MaxMotorTorque + { + set + { + WakeBodies(); + _maxMotorTorque = value; + } + get { return _maxMotorTorque; } + } + + /// + /// Get the current motor torque, usually in N-m. + /// + /// + public float MotorTorque + { + get { return _motorImpulse; } + set + { + WakeBodies(); + _motorImpulse = value; + } + } + + public override Vector2 GetReactionForce(float inv_dt) + { + Vector2 P = new Vector2(_impulse.X, _impulse.Y); + return inv_dt * P; + } + + public override float GetReactionTorque(float inv_dt) + { + return inv_dt * _impulse.Z; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + Body b2 = BodyB; + + if (_enableMotor || _enableLimit) + { + // You cannot create a rotation limit between bodies that + // both have fixed rotation. + Debug.Assert(b1.InvI > 0.0f || b2.InvI > 0.0f); + } + + // Compute the effective mass matrix. + /*Transform xf1, xf2; + b1.GetTransform(out xf1); + b2.GetTransform(out xf2);*/ + + Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter); + + // J = [-I -r1_skew I r2_skew] + // [ 0 -1 0 1] + // r_skew = [-ry; rx] + + // Matlab + // K = [ m1+r1y^2*i1+m2+r2y^2*i2, -r1y*i1*r1x-r2y*i2*r2x, -r1y*i1-r2y*i2] + // [ -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2, r1x*i1+r2x*i2] + // [ -r1y*i1-r2y*i2, r1x*i1+r2x*i2, i1+i2] + + float m1 = b1.InvMass, m2 = b2.InvMass; + float i1 = b1.InvI, i2 = b2.InvI; + + _mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2; + _mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2; + _mass.Col3.X = -r1.Y * i1 - r2.Y * i2; + _mass.Col1.Y = _mass.Col2.X; + _mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2; + _mass.Col3.Y = r1.X * i1 + r2.X * i2; + _mass.Col1.Z = _mass.Col3.X; + _mass.Col2.Z = _mass.Col3.Y; + _mass.Col3.Z = i1 + i2; + + _motorMass = i1 + i2; + if (_motorMass > 0.0f) + { + _motorMass = 1.0f / _motorMass; + } + + if (_enableMotor == false) + { + _motorImpulse = 0.0f; + } + + if (_enableLimit) + { + float jointAngle = b2.Sweep.A - b1.Sweep.A - 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 *= step.dtRatio; + _motorImpulse *= step.dtRatio; + + Vector2 P = new Vector2(_impulse.X, _impulse.Y); + + b1.LinearVelocityInternal -= m1 * P; + MathUtils.Cross(ref r1, ref P, out _tmpFloat1); + b1.AngularVelocityInternal -= i1 * ( /* r1 x P */_tmpFloat1 + _motorImpulse + _impulse.Z); + + b2.LinearVelocityInternal += m2 * P; + MathUtils.Cross(ref r2, ref P, out _tmpFloat1); + b2.AngularVelocityInternal += i2 * ( /* r2 x P */_tmpFloat1 + _motorImpulse + _impulse.Z); + } + else + { + _impulse = Vector3.Zero; + _motorImpulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + Body b2 = BodyB; + + Vector2 v1 = b1.LinearVelocityInternal; + float w1 = b1.AngularVelocityInternal; + Vector2 v2 = b2.LinearVelocityInternal; + float w2 = b2.AngularVelocityInternal; + + float m1 = b1.InvMass, m2 = b2.InvMass; + float i1 = b1.InvI, i2 = b2.InvI; + + // Solve motor constraint. + if (_enableMotor && _limitState != LimitState.Equal) + { + float Cdot = w2 - w1 - _motorSpeed; + float impulse = _motorMass * (-Cdot); + float oldImpulse = _motorImpulse; + float maxImpulse = step.dt * _maxMotorTorque; + _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = _motorImpulse - oldImpulse; + + w1 -= i1 * impulse; + w2 += i2 * impulse; + } + + // Solve limit constraint. + if (_enableLimit && _limitState != LimitState.Inactive) + { + /*Transform xf1, xf2; + b1.GetTransform(out xf1); + b2.GetTransform(out xf2);*/ + + Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter); + + // Solve point-to-point constraint + MathUtils.Cross(w2, ref r2, out _tmpVector2); + MathUtils.Cross(w1, ref r1, out _tmpVector1); + Vector2 Cdot1 = v2 + /* w2 x r2 */ _tmpVector2 - v1 - /* w1 x r1 */ _tmpVector1; + float Cdot2 = w2 - w1; + 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 reduced = _mass.Solve22(-Cdot1); + 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 if (_limitState == LimitState.AtUpper) + { + float newImpulse = _impulse.Z + impulse.Z; + if (newImpulse > 0.0f) + { + Vector2 reduced = _mass.Solve22(-Cdot1); + impulse.X = reduced.X; + impulse.Y = reduced.Y; + impulse.Z = -_impulse.Z; + _impulse.X += reduced.X; + _impulse.Y += reduced.Y; + _impulse.Z = 0.0f; + } + } + + Vector2 P = new Vector2(impulse.X, impulse.Y); + + v1 -= m1 * P; + MathUtils.Cross(ref r1, ref P, out _tmpFloat1); + w1 -= i1 * ( /* r1 x P */_tmpFloat1 + impulse.Z); + + v2 += m2 * P; + MathUtils.Cross(ref r2, ref P, out _tmpFloat1); + w2 += i2 * ( /* r2 x P */_tmpFloat1 + impulse.Z); + } + else + { + /*Transform xf1, xf2; + b1.GetTransform(out xf1); + b2.GetTransform(out xf2);*/ + + _tmpVector1 = LocalAnchorA - b1.LocalCenter; + _tmpVector2 = LocalAnchorB - b2.LocalCenter; + Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, ref _tmpVector1); + Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, ref _tmpVector2); + + // Solve point-to-point constraint + MathUtils.Cross(w2, ref r2, out _tmpVector2); + MathUtils.Cross(w1, ref r1, out _tmpVector1); + Vector2 Cdot = v2 + /* w2 x r2 */ _tmpVector2 - v1 - /* w1 x r1 */ _tmpVector1; + Vector2 impulse = _mass.Solve22(-Cdot); + + _impulse.X += impulse.X; + _impulse.Y += impulse.Y; + + v1 -= m1 * impulse; + MathUtils.Cross(ref r1, ref impulse, out _tmpFloat1); + w1 -= i1 * /* r1 x impulse */ _tmpFloat1; + + v2 += m2 * impulse; + MathUtils.Cross(ref r2, ref impulse, out _tmpFloat1); + w2 += i2 * /* r2 x impulse */ _tmpFloat1; + } + + b1.LinearVelocityInternal = v1; + b1.AngularVelocityInternal = w1; + b2.LinearVelocityInternal = v2; + b2.AngularVelocityInternal = w2; + } + + internal override bool SolvePositionConstraints() + { + // TODO_ERIN block solve with limit. COME ON ERIN + + Body b1 = BodyA; + Body b2 = BodyB; + + float angularError = 0.0f; + float positionError; + + // Solve angular limit constraint. + if (_enableLimit && _limitState != LimitState.Inactive) + { + float angle = b2.Sweep.A - b1.Sweep.A - 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; + } + + b1.Sweep.A -= b1.InvI * limitImpulse; + b2.Sweep.A += b2.InvI * limitImpulse; + + b1.SynchronizeTransform(); + b2.SynchronizeTransform(); + } + + // Solve point-to-point constraint. + { + /*Transform xf1, xf2; + b1.GetTransform(out xf1); + b2.GetTransform(out xf2);*/ + + Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter); + + Vector2 C = b2.Sweep.C + r2 - b1.Sweep.C - r1; + positionError = C.Length(); + + float invMass1 = b1.InvMass, invMass2 = b2.InvMass; + float invI1 = b1.InvI, invI2 = b2.InvI; + + // Handle large detachment. + const float k_allowedStretch = 10.0f * Settings.LinearSlop; + if (C.LengthSquared() > k_allowedStretch * k_allowedStretch) + { + // Use a particle solution (no rotation). + Vector2 u = C; + u.Normalize(); + float k = invMass1 + invMass2; + Debug.Assert(k > Settings.Epsilon); + float m = 1.0f / k; + Vector2 impulse2 = m * (-C); + const float k_beta = 0.5f; + b1.Sweep.C -= k_beta * invMass1 * impulse2; + b2.Sweep.C += k_beta * invMass2 * impulse2; + + C = b2.Sweep.C + r2 - b1.Sweep.C - r1; + } + + Mat22 K1 = new Mat22(new Vector2(invMass1 + invMass2, 0.0f), new Vector2(0.0f, invMass1 + invMass2)); + Mat22 K2 = new Mat22(new Vector2(invI1 * r1.Y * r1.Y, -invI1 * r1.X * r1.Y), + new Vector2(-invI1 * r1.X * r1.Y, invI1 * r1.X * r1.X)); + Mat22 K3 = new Mat22(new Vector2(invI2 * r2.Y * r2.Y, -invI2 * r2.X * r2.Y), + new Vector2(-invI2 * r2.X * r2.Y, invI2 * r2.X * r2.X)); + + Mat22 Ka; + Mat22.Add(ref K1, ref K2, out Ka); + + Mat22 K; + Mat22.Add(ref Ka, ref K3, out K); + + + Vector2 impulse = K.Solve(-C); + + b1.Sweep.C -= b1.InvMass * impulse; + MathUtils.Cross(ref r1, ref impulse, out _tmpFloat1); + b1.Sweep.A -= b1.InvI * /* r1 x impulse */ _tmpFloat1; + + b2.Sweep.C += b2.InvMass * impulse; + MathUtils.Cross(ref r2, ref impulse, out _tmpFloat1); + b2.Sweep.A += b2.InvI * /* r2 x impulse */ _tmpFloat1; + + b1.SynchronizeTransform(); + b2.SynchronizeTransform(); + } + + return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; + } + } +} \ No newline at end of file diff --git a/axios/Dynamics/Joints/RopeJoint.cs b/axios/Dynamics/Joints/RopeJoint.cs new file mode 100644 index 0000000..84bf3c9 --- /dev/null +++ b/axios/Dynamics/Joints/RopeJoint.cs @@ -0,0 +1,239 @@ +/* +* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not 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 +{ + // 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. + /// Warning: if you attempt to change the maximum length during + /// the simulation you will get some non-physical behavior. + /// A model that would allow you to dynamically modify the length + /// would have some sponginess, so I chose not to implement it + /// that way. See b2DistanceJoint if you want to dynamically + /// control length. + /// + public class RopeJoint : Joint + { + public Vector2 LocalAnchorA; + public Vector2 LocalAnchorB; + + private float _impulse; + private float _length; + + private float _mass; + private Vector2 _rA, _rB; + private LimitState _state; + private Vector2 _u; + + internal RopeJoint() + { + JointType = JointType.Rope; + } + + public RopeJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB) + : base(bodyA, bodyB) + { + JointType = JointType.Rope; + LocalAnchorA = localAnchorA; + LocalAnchorB = localAnchorB; + + Vector2 d = WorldAnchorB - WorldAnchorA; + MaxLength = d.Length(); + + _mass = 0.0f; + _impulse = 0.0f; + _state = LimitState.Inactive; + _length = 0.0f; + } + + /// Get the maximum length of the rope. + public float MaxLength { get; set; } + + public LimitState State + { + get { return _state; } + } + + public override sealed Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + } + + 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."); } + } + + public override Vector2 GetReactionForce(float invDt) + { + return (invDt * _impulse) * _u; + } + + public override float GetReactionTorque(float invDt) + { + return 0; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body bA = BodyA; + Body bB = BodyB; + + Transform xf1; + bA.GetTransform(out xf1); + + Transform xf2; + bB.GetTransform(out xf2); + + _rA = MathUtils.Multiply(ref xf1.R, LocalAnchorA - bA.LocalCenter); + _rB = MathUtils.Multiply(ref xf2.R, LocalAnchorB - bB.LocalCenter); + + // Rope axis + _u = bB.Sweep.C + _rB - bA.Sweep.C - _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 = bA.InvMass + bA.InvI * crA * crA + bB.InvMass + bB.InvI * crB * crB; + + _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; + + if (Settings.EnableWarmstarting) + { + // Scale the impulse to support a variable time step. + _impulse *= step.dtRatio; + + Vector2 P = _impulse * _u; + bA.LinearVelocity -= bA.InvMass * P; + bA.AngularVelocity -= bA.InvI * MathUtils.Cross(_rA, P); + bB.LinearVelocity += bB.InvMass * P; + bB.AngularVelocity += bB.InvI * MathUtils.Cross(_rB, P); + } + else + { + _impulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body bA = BodyA; + Body bB = BodyB; + + // Cdot = dot(u, v + cross(w, r)) + Vector2 vA = bA.LinearVelocity + MathUtils.Cross(bA.AngularVelocity, _rA); + Vector2 vB = bB.LinearVelocity + MathUtils.Cross(bB.AngularVelocity, _rB); + float C = _length - MaxLength; + float Cdot = Vector2.Dot(_u, vB - vA); + + // Predictive constraint. + if (C < 0.0f) + { + Cdot += 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; + bA.LinearVelocity -= bA.InvMass * P; + bA.AngularVelocity -= bA.InvI * MathUtils.Cross(_rA, P); + bB.LinearVelocity += bB.InvMass * P; + bB.AngularVelocity += bB.InvI * MathUtils.Cross(_rB, P); + } + + internal override bool SolvePositionConstraints() + { + Body bA = BodyA; + Body bB = BodyB; + + Transform xf1; + bA.GetTransform(out xf1); + + Transform xf2; + bB.GetTransform(out xf2); + + Vector2 rA = MathUtils.Multiply(ref xf1.R, LocalAnchorA - bA.LocalCenter); + Vector2 rB = MathUtils.Multiply(ref xf2.R, LocalAnchorB - bB.LocalCenter); + + Vector2 u = bB.Sweep.C + rB - bA.Sweep.C - 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; + + bA.Sweep.C -= bA.InvMass * P; + bA.Sweep.A -= bA.InvI * MathUtils.Cross(rA, P); + bB.Sweep.C += bB.InvMass * P; + bB.Sweep.A += bB.InvI * MathUtils.Cross(rB, P); + + bA.SynchronizeTransform(); + bB.SynchronizeTransform(); + + return length - MaxLength < Settings.LinearSlop; + } + } +} \ No newline at end of file diff --git a/axios/Dynamics/Joints/SliderJoint.cs b/axios/Dynamics/Joints/SliderJoint.cs new file mode 100644 index 0000000..3bda643 --- /dev/null +++ b/axios/Dynamics/Joints/SliderJoint.cs @@ -0,0 +1,298 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not 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 +{ + /// + /// A distance joint contrains 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 SliderJoint : Joint + { + // 1-D constrained 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 + + public Vector2 LocalAnchorA; + + public Vector2 LocalAnchorB; + private float _bias; + private float _gamma; + private float _impulse; + private float _mass; + private Vector2 _u; + + internal SliderJoint() + { + JointType = JointType.Slider; + } + + /// + /// Initializes a new instance of the class. + /// Warning: Do not use a zero or short length. + /// + /// The first body. + /// The second body. + /// The first body anchor. + /// The second body anchor. + /// The minimum length between anchorpoints + /// The maximum length between anchorpoints. + public SliderJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB, float minLength, + float maxlength) + : base(bodyA, bodyB) + { + JointType = JointType.Slider; + + LocalAnchorA = localAnchorA; + LocalAnchorB = localAnchorB; + MaxLength = maxlength; + MinLength = minLength; + } + + /// + /// The maximum length between the anchor points. + /// + /// The length. + public float MaxLength { get; set; } + + /// + /// The minimal length between the anchor points. + /// + /// The length. + public float MinLength { get; set; } + + /// + /// The mass-spring-damper frequency in Hertz. + /// + /// The frequency. + public float Frequency { get; set; } + + /// + /// The damping ratio. 0 = no damping, 1 = critical damping. + /// + /// The damping ratio. + public float DampingRatio { get; set; } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + public override Vector2 GetReactionForce(float inv_dt) + { + Vector2 F = (inv_dt * _impulse) * _u; + return F; + } + + public override float GetReactionTorque(float inv_dt) + { + return 0.0f; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + Body b2 = BodyB; + + Transform xf1, xf2; + b1.GetTransform(out xf1); + b2.GetTransform(out xf2); + + // Compute the effective mass matrix. + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter); + _u = b2.Sweep.C + r2 - b1.Sweep.C - r1; + + // Handle singularity. + float length = _u.Length(); + + if (length < MaxLength && length > MinLength) + { + return; + } + + if (length > Settings.LinearSlop) + { + _u *= 1.0f / length; + } + else + { + _u = Vector2.Zero; + } + + float cr1u = MathUtils.Cross(r1, _u); + float cr2u = MathUtils.Cross(r2, _u); + float invMass = b1.InvMass + b1.InvI * cr1u * cr1u + b2.InvMass + b2.InvI * cr2u * cr2u; + Debug.Assert(invMass > Settings.Epsilon); + _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; + + if (Frequency > 0.0f) + { + float C = length - MaxLength; + + // 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 = step.dt * (d + step.dt * k); + _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f; + _bias = C * step.dt * k * _gamma; + + _mass = invMass + _gamma; + _mass = _mass != 0.0f ? 1.0f / _mass : 0.0f; + } + + if (Settings.EnableWarmstarting) + { + // Scale the impulse to support a variable time step. + _impulse *= step.dtRatio; + + Vector2 P = _impulse * _u; + b1.LinearVelocityInternal -= b1.InvMass * P; + b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P); + b2.LinearVelocityInternal += b2.InvMass * P; + b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P); + } + else + { + _impulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + Body b2 = BodyB; + + Transform xf1, xf2; + b1.GetTransform(out xf1); + b2.GetTransform(out xf2); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter); + + Vector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1; + + float length = d.Length(); + + if (length < MaxLength && length > MinLength) + { + return; + } + + // Cdot = dot(u, v + cross(w, r)) + Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1); + Vector2 v2 = b2.LinearVelocityInternal + MathUtils.Cross(b2.AngularVelocityInternal, r2); + float Cdot = Vector2.Dot(_u, v2 - v1); + + float impulse = -_mass * (Cdot + _bias + _gamma * _impulse); + _impulse += impulse; + + Vector2 P = impulse * _u; + b1.LinearVelocityInternal -= b1.InvMass * P; + b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P); + b2.LinearVelocityInternal += b2.InvMass * P; + b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P); + } + + internal override bool SolvePositionConstraints() + { + if (Frequency > 0.0f) + { + // There is no position correction for soft distance constraints. + return true; + } + + Body b1 = BodyA; + Body b2 = BodyB; + + Transform xf1, xf2; + b1.GetTransform(out xf1); + b2.GetTransform(out xf2); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter); + + Vector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1; + + float length = d.Length(); + + if (length < MaxLength && length > MinLength) + { + return true; + } + + if (length == 0.0f) + return true; + + d /= length; + float C = length - MaxLength; + C = MathUtils.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); + + float impulse = -_mass * C; + _u = d; + Vector2 P = impulse * _u; + + b1.Sweep.C -= b1.InvMass * P; + b1.Sweep.A -= b1.InvI * MathUtils.Cross(r1, P); + b2.Sweep.C += b2.InvMass * P; + b2.Sweep.A += b2.InvI * MathUtils.Cross(r2, P); + + b1.SynchronizeTransform(); + b2.SynchronizeTransform(); + + return Math.Abs(C) < Settings.LinearSlop; + } + } +} \ No newline at end of file diff --git a/axios/Dynamics/Joints/WeldJoint.cs b/axios/Dynamics/Joints/WeldJoint.cs new file mode 100644 index 0000000..5951cec --- /dev/null +++ b/axios/Dynamics/Joints/WeldJoint.cs @@ -0,0 +1,263 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not 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 +{ + // 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. + /// + public class WeldJoint : Joint + { + public Vector2 LocalAnchorA; + public Vector2 LocalAnchorB; + private Vector3 _impulse; + private Mat33 _mass; + + internal WeldJoint() + { + JointType = JointType.Weld; + } + + /// + /// You need to specify a local anchor point + /// where they are attached and the relative body angle. The position + /// of the anchor point is important for computing the reaction torque. + /// You can change the anchor points relative to bodyA or bodyB by changing LocalAnchorA + /// and/or LocalAnchorB. + /// + /// The first body + /// The second body + /// The first body anchor. + /// The second body anchor. + public WeldJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB) + : base(bodyA, bodyB) + { + JointType = JointType.Weld; + + LocalAnchorA = localAnchorA; + LocalAnchorB = localAnchorB; + ReferenceAngle = BodyB.Rotation - BodyA.Rotation; + } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + } + + 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 body2 angle minus body1 angle in the reference state (radians). + /// + public float ReferenceAngle { get; private set; } + + public override Vector2 GetReactionForce(float inv_dt) + { + return inv_dt * new Vector2(_impulse.X, _impulse.Y); + } + + public override float GetReactionTorque(float inv_dt) + { + return inv_dt * _impulse.Z; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body bA = BodyA; + Body bB = BodyB; + + Transform xfA, xfB; + bA.GetTransform(out xfA); + bB.GetTransform(out xfB); + + // Compute the effective mass matrix. + Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter); + Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter); + + // 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 = bA.InvMass, mB = bB.InvMass; + float iA = bA.InvI, iB = bB.InvI; + + _mass.Col1.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB; + _mass.Col2.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB; + _mass.Col3.X = -rA.Y * iA - rB.Y * iB; + _mass.Col1.Y = _mass.Col2.X; + _mass.Col2.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB; + _mass.Col3.Y = rA.X * iA + rB.X * iB; + _mass.Col1.Z = _mass.Col3.X; + _mass.Col2.Z = _mass.Col3.Y; + _mass.Col3.Z = iA + iB; + + if (Settings.EnableWarmstarting) + { + // Scale impulses to support a variable time step. + _impulse *= step.dtRatio; + + Vector2 P = new Vector2(_impulse.X, _impulse.Y); + + bA.LinearVelocityInternal -= mA * P; + bA.AngularVelocityInternal -= iA * (MathUtils.Cross(rA, P) + _impulse.Z); + + bB.LinearVelocityInternal += mB * P; + bB.AngularVelocityInternal += iB * (MathUtils.Cross(rB, P) + _impulse.Z); + } + else + { + _impulse = Vector3.Zero; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body bA = BodyA; + Body bB = BodyB; + + Vector2 vA = bA.LinearVelocityInternal; + float wA = bA.AngularVelocityInternal; + Vector2 vB = bB.LinearVelocityInternal; + float wB = bB.AngularVelocityInternal; + + float mA = bA.InvMass, mB = bB.InvMass; + float iA = bA.InvI, iB = bB.InvI; + + Transform xfA, xfB; + bA.GetTransform(out xfA); + bB.GetTransform(out xfB); + + Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter); + Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter); + + // Solve point-to-point constraint + 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); + _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); + + bA.LinearVelocityInternal = vA; + bA.AngularVelocityInternal = wA; + bB.LinearVelocityInternal = vB; + bB.AngularVelocityInternal = wB; + } + + internal override bool SolvePositionConstraints() + { + Body bA = BodyA; + Body bB = BodyB; + + float mA = bA.InvMass, mB = bB.InvMass; + float iA = bA.InvI, iB = bB.InvI; + + Transform xfA; + Transform xfB; + bA.GetTransform(out xfA); + bB.GetTransform(out xfB); + + Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter); + Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter); + + Vector2 C1 = bB.Sweep.C + rB - bA.Sweep.C - rA; + float C2 = bB.Sweep.A - bA.Sweep.A - ReferenceAngle; + + // Handle large detachment. + const float k_allowedStretch = 10.0f * Settings.LinearSlop; + float positionError = C1.Length(); + float angularError = Math.Abs(C2); + if (positionError > k_allowedStretch) + { + iA *= 1.0f; + iB *= 1.0f; + } + + _mass.Col1.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB; + _mass.Col2.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB; + _mass.Col3.X = -rA.Y * iA - rB.Y * iB; + _mass.Col1.Y = _mass.Col2.X; + _mass.Col2.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB; + _mass.Col3.Y = rA.X * iA + rB.X * iB; + _mass.Col1.Z = _mass.Col3.X; + _mass.Col2.Z = _mass.Col3.Y; + _mass.Col3.Z = iA + iB; + + Vector3 C = new Vector3(C1.X, C1.Y, C2); + + Vector3 impulse = _mass.Solve33(-C); + + Vector2 P = new Vector2(impulse.X, impulse.Y); + + bA.Sweep.C -= mA * P; + bA.Sweep.A -= iA * (MathUtils.Cross(rA, P) + impulse.Z); + + bB.Sweep.C += mB * P; + bB.Sweep.A += iB * (MathUtils.Cross(rB, P) + impulse.Z); + + bA.SynchronizeTransform(); + bB.SynchronizeTransform(); + + return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; + } + } +} \ No newline at end of file diff --git a/axios/Dynamics/TimeStep.cs b/axios/Dynamics/TimeStep.cs new file mode 100644 index 0000000..8a2a117 --- /dev/null +++ b/axios/Dynamics/TimeStep.cs @@ -0,0 +1,45 @@ +/* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +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; + } +} \ No newline at end of file diff --git a/axios/Dynamics/World.cs b/axios/Dynamics/World.cs new file mode 100644 index 0000000..bfee485 --- /dev/null +++ b/axios/Dynamics/World.cs @@ -0,0 +1,1456 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being 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.Collision; +using FarseerPhysics.Common; +using FarseerPhysics.Controllers; +using FarseerPhysics.Dynamics.Contacts; +using FarseerPhysics.Dynamics.Joints; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics +{ + /// + /// Contains filter data that can determine whether an object should be processed or not. + /// + public abstract class FilterData + { + public Category DisabledOnCategories = Category.None; + + public int DisabledOnGroup; + public Category EnabledOnCategories = Category.All; + 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 IsInEnabledCategory(Category category) + { + return (EnabledOnCategories & category) == category; + } + } + + [Flags] + public enum WorldFlags + { + /// + /// Flag that indicates a new fixture has been added to the world. + /// + NewFixture = (1 << 0), + + /// + /// Flag that clear the forces after each time step. + /// + ClearForces = (1 << 2), + + SubStepping = (1 << 4), + } + + /// + /// The world class manages all physics entities, dynamic simulation, + /// and asynchronous queries. + /// + public class World + { + /// + /// Fires whenever a body has been added + /// + public BodyDelegate BodyAdded; + + /// + /// Fires whenever a body has been removed + /// + public BodyDelegate BodyRemoved; + + internal Queue ContactPool = new Queue(256); + + /// + /// Fires whenever a fixture has been added + /// + public FixtureDelegate FixtureAdded; + + /// + /// Fires whenever a fixture has been removed + /// + public FixtureDelegate FixtureRemoved; + + internal WorldFlags Flags; + + /// + /// Fires whenever a joint has been added + /// + public JointDelegate JointAdded; + + /// + /// Fires whenever a joint has been removed + /// + public JointDelegate JointRemoved; + + public ControllerDelegate ControllerAdded; + + public ControllerDelegate ControllerRemoved; + + private float _invDt0; + public Island Island = new Island(); + 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 TOIInput _input = new TOIInput(); + + /// + /// If false, the whole simulation stops. It still processes added and removed geometries. + /// + public bool Enabled = true; + +#if (!SILVERLIGHT) + private Stopwatch _watch = new Stopwatch(); +#endif + + /// + /// Initializes a new instance of the class. + /// + private World() + { + Flags = WorldFlags.ClearForces; + + ControllerList = new List(); + BreakableBodyList = new List(); + BodyList = new List(32); + JointList = new List(32); + } + + public World(Vector2 gravity, AABB span) + : this() + { + Gravity = gravity; + ContactManager = new ContactManager(new QuadTreeBroadPhase(span)); + } + + /// + /// Initializes a new instance of the class. + /// + /// The gravity. + public World(Vector2 gravity) + : this() + { + ContactManager = new ContactManager(new DynamicTreeBroadPhase()); + Gravity = gravity; + } + + 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 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; + + /// + /// Set flag to control automatic clearing of forces after each time step. + /// + /// true if it should auto clear forces; otherwise, false. + public bool AutoClearForces + { + set + { + if (value) + { + Flags |= WorldFlags.ClearForces; + } + else + { + Flags &= ~WorldFlags.ClearForces; + } + } + get { return (Flags & WorldFlags.ClearForces) == WorldFlags.ClearForces; } + } + + /// + /// 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; } + + /// + /// 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; } + } + + /// + /// Enable/disable single stepped continuous physics. For testing. + /// + public bool EnableSubStepping + { + set + { + if (value) + { + Flags |= WorldFlags.SubStepping; + } + else + { + Flags &= ~WorldFlags.SubStepping; + } + } + get { return (Flags & WorldFlags.SubStepping) == WorldFlags.SubStepping; } + } + + /// + /// 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); + } + + /// + /// 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(); + } + + 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.FlagForFiltering(); + } + + 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.FlagForFiltering(); + } + + 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) + { + // 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? + Debug.Assert(BodyList.Contains(body)); + + // 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); + } + + _bodyRemoveList.Clear(); + } + } + + /// + /// 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 (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + _watch.Start(); +#endif + + ProcessChanges(); + +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + AddRemoveTime = _watch.ElapsedTicks; +#endif + //If there is no change in time, no need to calculate anything. + if (dt == 0 || !Enabled) + { +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + { + _watch.Stop(); + _watch.Reset(); + } +#endif + return; + } + + // If new fixtures were added, we need to find the new contacts. + if ((Flags & WorldFlags.NewFixture) == WorldFlags.NewFixture) + { + ContactManager.FindNewContacts(); + Flags &= ~WorldFlags.NewFixture; + } + + TimeStep step; + step.inv_dt = 1.0f / dt; + step.dt = dt; + step.dtRatio = _invDt0 * dt; + + //Update controllers + for (int i = 0; i < ControllerList.Count; i++) + { + ControllerList[i].Update(dt); + } + +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + ControllersUpdateTime = _watch.ElapsedTicks - AddRemoveTime; +#endif + + // Update contacts. This is where some contacts are destroyed. + ContactManager.Collide(); + +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + ContactsUpdateTime = _watch.ElapsedTicks - (AddRemoveTime + ControllersUpdateTime); +#endif + // Integrate velocities, solve velocity raints, and integrate positions. + Solve(ref step); + +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + SolveUpdateTime = _watch.ElapsedTicks - (AddRemoveTime + ControllersUpdateTime + ContactsUpdateTime); +#endif + + // Handle TOI events. + if (Settings.ContinuousPhysics) + { + SolveTOI(ref step); + } + +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + ContinuousPhysicsTime = _watch.ElapsedTicks - + (AddRemoveTime + ControllersUpdateTime + ContactsUpdateTime + SolveUpdateTime); +#endif + _invDt0 = step.inv_dt; + + if ((Flags & WorldFlags.ClearForces) != 0) + { + ClearForces(); + } + + for (int i = 0; i < BreakableBodyList.Count; i++) + { + BreakableBodyList[i].Update(); + } + +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + { + _watch.Stop(); + //AddRemoveTime = 1000 * AddRemoveTime / Stopwatch.Frequency; + + UpdateTime = _watch.ElapsedTicks; + _watch.Reset(); + } +#endif + } + + /// + /// 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) + { + ContactManager.BroadPhase.Query(proxyId => + { + FixtureProxy proxy = ContactManager.BroadPhase.GetProxy(proxyId); + return callback(proxy.Fixture); + }, ref aabb); + } + + /// + /// 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(RayCastCallback callback, Vector2 point1, Vector2 point2) + { + RayCastInput input = new RayCastInput(); + input.MaxFraction = 1.0f; + input.Point1 = point1; + input.Point2 = point2; + + ContactManager.BroadPhase.RayCast((rayCastInput, 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) * input.Point1 + + fraction * input.Point2; + return callback(fixture, point, output.Normal, fraction); + } + + return input.MaxFraction; + }, ref input); + } + + 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. + foreach (Body b in BodyList) + { + b.Flags &= ~BodyFlags.Island; + } + + for (int i = 0; i < ContactManager.ContactList.Count; i++) + { + Contact c = ContactManager.ContactList[i]; + c.Flags &= ~ContactFlags.Island; + } + 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)]; + + for (int index = BodyList.Count - 1; index >= 0; index--) + { + Body seed = BodyList[index]; + if ((seed.Flags & (BodyFlags.Island)) != BodyFlags.None) + { + 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; + seed.Flags |= BodyFlags.Island; + + // 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.Flags & ContactFlags.Island) != ContactFlags.None) + { + continue; + } + + // Is this contact solid and touching? + if (!ce.Contact.Enabled || !ce.Contact.IsTouching()) + { + continue; + } + + // Skip sensors. + bool sensorA = contact.FixtureA.IsSensor; + bool sensorB = contact.FixtureB.IsSensor; + if (sensorA || sensorB) + { + continue; + } + + Island.Add(contact); + contact.Flags |= ContactFlags.Island; + + Body other = ce.Other; + + // Was the other body already added to this island? + if ((other.Flags & BodyFlags.Island) != BodyFlags.None) + { + continue; + } + + Debug.Assert(stackCount < stackSize); + _stack[stackCount++] = other; + other.Flags |= BodyFlags.Island; + } + + // 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.Flags & BodyFlags.Island) != BodyFlags.None) + { + continue; + } + + Debug.Assert(stackCount < stackSize); + _stack[stackCount++] = other; + other.Flags |= BodyFlags.Island; + } + 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.Flags &= ~BodyFlags.Island; + } + } + } + + // Synchronize fixtures, check for out of range bodies. + foreach (Body b in BodyList) + { + // If a body was not in an island then it did not move. + if ((b.Flags & BodyFlags.Island) != BodyFlags.Island) + { + continue; + } + + if (b.BodyType == BodyType.Static) + { + continue; + } + + // Update fixtures (for broad-phase). + b.SynchronizeFixtures(); + } + + // Look for new contacts. + ContactManager.FindNewContacts(); + } + + /// + /// Find TOI contacts and solve them. + /// + /// The step. + private void SolveTOI(ref TimeStep step) + { + Island.Reset(2 * Settings.MaxTOIContacts, Settings.MaxTOIContacts, 0, ContactManager); + + if (_stepComplete) + { + for (int i = 0; i < BodyList.Count; i++) + { + BodyList[i].Flags &= ~BodyFlags.Island; + BodyList[i].Sweep.Alpha0 = 0.0f; + } + + for (int i = 0; i < ContactManager.ContactList.Count; i++) + { + Contact c = ContactManager.ContactList[i]; + + // Invalidate TOI + c.Flags &= ~(ContactFlags.TOI | ContactFlags.Island); + 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; + + for (int i = 0; i < ContactManager.ContactList.Count; i++) + { + Contact c = ContactManager.ContactList[i]; + + // Is this contact disabled? + if (c.Enabled == false) + { + continue; + } + + // Prevent excessive sub-stepping. + if (c.TOICount > Settings.MaxSubSteps) + { + continue; + } + + float alpha; + if ((c.Flags & ContactFlags.TOI) == ContactFlags.TOI) + { + // 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 awakeA = bA.Awake && typeA != BodyType.Static; + bool awakeB = bB.Awake && typeB != BodyType.Static; + + // Is at least one body awake? + if (awakeA == false && awakeB == false) + { + continue; + } + + bool collideA = (bA.IsBullet || typeA != BodyType.Dynamic) && !bA.IgnoreCCD; + bool collideB = (bB.IsBullet || typeB != BodyType.Dynamic) && !bB.IgnoreCCD; + + // Are these two non-bullet dynamic bodies? + if (collideA == false && collideB == false) + { + continue; + } + + // 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.Flags |= ContactFlags.TOI; + } + + 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 bA1 = fA1.Body; + Body bB1 = fB1.Body; + + Sweep backup1 = bA1.Sweep; + Sweep backup2 = bB1.Sweep; + + bA1.Advance(minAlpha); + bB1.Advance(minAlpha); + + // The TOI contact likely has some new contact points. + minContact.Update(ContactManager); + minContact.Flags &= ~ContactFlags.TOI; + ++minContact.TOICount; + + // Is the contact solid? + if (minContact.Enabled == false || minContact.IsTouching() == false) + { + // Restore the sweeps. + minContact.Enabled = false; + bA1.Sweep = backup1; + bB1.Sweep = backup2; + bA1.SynchronizeTransform(); + bB1.SynchronizeTransform(); + continue; + } + + bA1.Awake = true; + bB1.Awake = true; + + // Build the island + Island.Clear(); + Island.Add(bA1); + Island.Add(bB1); + Island.Add(minContact); + + bA1.Flags |= BodyFlags.Island; + bB1.Flags |= BodyFlags.Island; + minContact.Flags |= ContactFlags.Island; + + // Get contacts on bodyA and bodyB. + Body[] bodies = { bA1, bB1 }; + for (int i = 0; i < 2; ++i) + { + Body body = bodies[i]; + if (body.BodyType == BodyType.Dynamic) + { + // for (ContactEdge ce = body.ContactList; ce && Island.BodyCount < Settings.MaxTOIContacts; ce = ce.Next) + for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next) + { + Contact contact = ce.Contact; + + // Has this contact already been added to the island? + if ((contact.Flags & ContactFlags.Island) == ContactFlags.Island) + { + 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.Flags & BodyFlags.Island) == 0) + { + 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.Flags |= ContactFlags.Island; + Island.Add(contact); + + // Has the other body already been added to the island? + if ((other.Flags & BodyFlags.Island) == BodyFlags.Island) + { + continue; + } + + // Add the other body to the island. + other.Flags |= BodyFlags.Island; + + if (other.BodyType != BodyType.Static) + { + other.Awake = true; + } + + Island.Add(other); + } + } + } + + TimeStep subStep; + subStep.dt = (1.0f - minAlpha) * step.dt; + subStep.inv_dt = 1.0f / subStep.dt; + subStep.dtRatio = 1.0f; + //subStep.positionIterations = 20; + //subStep.velocityIterations = step.velocityIterations; + //subStep.warmStarting = false; + Island.SolveTOI(ref subStep); + + // Reset island flags and synchronize broad-phase proxies. + for (int i = 0; i < Island.BodyCount; ++i) + { + Body body = Island.Bodies[i]; + body.Flags &= ~BodyFlags.Island; + + 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.Flags &= ~(ContactFlags.TOI | ContactFlags.Island); + } + } + + // Commit fixture proxy movements to the broad-phase so that new contacts are created. + // Also, some contacts can be destroyed. + ContactManager.FindNewContacts(); + + if (EnableSubStepping) + { + _stepComplete = false; + break; + } + } + } + + 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; + + Fixture myFixture = null; + + // Query the world for overlapping shapes. + QueryAABB( + fixture => + { + bool inside = fixture.TestPoint(ref point); + if (inside) + { + myFixture = fixture; + return false; + } + + // Continue the query. + return true; + }, ref aabb); + + return myFixture; + } + + /// + /// 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; + + List fixtures = new List(); + + // Query the world for overlapping shapes. + QueryAABB( + fixture => + { + bool inside = fixture.TestPoint(ref point); + if (inside) + fixtures.Add(fixture); + + // Continue the query. + return true; + }, ref aabb); + + return fixtures; + } + + 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/axios/Dynamics/WorldCallbacks.cs b/axios/Dynamics/WorldCallbacks.cs new file mode 100644 index 0000000..9a536f1 --- /dev/null +++ b/axios/Dynamics/WorldCallbacks.cs @@ -0,0 +1,74 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, 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; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics +{ + /// + /// Called for each fixture found in the query. You control how the ray cast + /// proceeds by returning a float: + /// -1 to filter, 0 to terminate, fraction to clip the ray for closest hit, 1 to continue + /// + public delegate float RayCastCallback(Fixture fixture, Vector2 point, Vector2 normal, float fraction); + + /// + /// 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, ContactConstraint 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); + + public delegate void OnSeparationEventHandler(Fixture fixtureA, Fixture fixtureB); +} \ No newline at end of file diff --git a/axios/Engine/AxiosBreakableGameObject.cs b/axios/Engine/AxiosBreakableGameObject.cs new file mode 100644 index 0000000..25ca675 --- /dev/null +++ b/axios/Engine/AxiosBreakableGameObject.cs @@ -0,0 +1,126 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +using FarseerPhysics.Dynamics; + +using Axios.Engine.Interfaces; + +namespace Axios.Engine +{ + //I think using a template here would be good + //It would solve the problem of having to repeat methods in DrawableBreakableAxiosGameObject + abstract class AxiosBreakableGameObject : AxiosGameObject + + { + + /// + /// BodyParts is what the body will break into + /// Body is what will be used to show the object as a whole + /// + protected List BodyParts = new List(); + protected SimpleAxiosGameObject BodyPart = null; + + + public delegate void BodyBroken(AxiosBreakableGameObject body); + + public event BodyBroken OnBodyBreak; + + protected bool _calledBodyBroken = false; + + protected bool _isbroken = false; + + private int _draworder; + + public bool Broken + { + get { return _isbroken; } + set { _isbroken = true; Break(); } + } + + public override void LoadContent(AxiosGameScreen gameScreen) + { + base.LoadContent(gameScreen); + + BodyParts = new List(); + + + CreateBodyPart(gameScreen); + CreateBodyParts(gameScreen); + + gameScreen.AddGameObject(BodyPart); + BodyPart.BodyPart.Enabled = true; + foreach (SimpleAxiosGameObject obj in BodyParts) + { + gameScreen.AddGameObject(obj); + obj.BodyPart.Enabled = false; + } + + } + + //The developer will have to define the BodyPart creation in an overriden method + public abstract void CreateBodyPart(AxiosGameScreen gameScreen); + + //The developer will have to define the BodyParts creation in an overriden method + public abstract void CreateBodyParts(AxiosGameScreen gameScreen); + + public void Break() + { + OnBodyBreak(this); + _isbroken = true; + + BodyPart.BodyPart.Enabled = false; + foreach (SimpleAxiosGameObject s in BodyParts) + s.BodyPart.Enabled = true; + } + + public override void Update(AxiosGameScreen gameScreen, Microsoft.Xna.Framework.GameTime gameTime, bool otherScreenHasFocus, bool coveredByOtherScreen) + { + base.Update(gameScreen, gameTime, otherScreenHasFocus, coveredByOtherScreen); + + + } + + protected override void OnRemove(AxiosGameObject gameObject) + { + base.OnRemove(gameObject); + + if (BodyPart != null) + BodyPart.Remove(); + } + + + + public int DrawOrder + { + get + { + return _draworder; + } + set + { + _draworder = value; + } + } + + + public void Draw(AxiosGameScreen gameScreen, Microsoft.Xna.Framework.GameTime gameTime) + { + if (_isbroken) + { + if (BodyParts.Count > 0 && BodyParts[0] is IDrawableAxiosGameObject) + { + foreach (SimpleAxiosGameObject b in BodyParts) + ((IDrawableAxiosGameObject)b).Draw(gameScreen, gameTime); + } + } + else + { + if (BodyPart != null && BodyPart is IDrawableAxiosGameObject) + ((IDrawableAxiosGameObject)BodyPart).Draw(gameScreen, gameTime); + } + + } + } +} diff --git a/axios/Engine/AxiosEvents.cs b/axios/Engine/AxiosEvents.cs new file mode 100644 index 0000000..a8d9902 --- /dev/null +++ b/axios/Engine/AxiosEvents.cs @@ -0,0 +1,138 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using FarseerPhysics.SamplesFramework; + +namespace Axios.Engine +{ + public abstract class AxiosEvents + { + protected Boolean _hasFocus; + + public bool HasFocus + { + get + { + return this._hasFocus; + } + set + { + this._hasFocus = value; + } + } + + public delegate void AxiosHandler(object sender, AxiosGameScreen gameScreen, InputHelper input); + + public delegate void AxiosGameObjectHandler(AxiosGameObject sender); + + #region GameObjectEventMethods + + public virtual void OnFocusEnter(AxiosGameScreen gameScreen, InputHelper input) + { + this.HasFocus = true; + this.OnEvent(FocusEnter, gameScreen, input); + } + + public virtual void OnFocusLeave(AxiosGameScreen gameScreen, InputHelper input) + { + this.HasFocus = false; + this.OnEvent(FocusLeave, gameScreen, input); + } + + public virtual void OnMouseHover(AxiosGameScreen gameScreen, InputHelper input) + { + this.OnEvent(MouseHover, gameScreen, input); + } + + public virtual void OnMouseLeave(AxiosGameScreen gameScreen, InputHelper input) + { + this.OnEvent(MouseLeave, gameScreen, input); + } + + public virtual void OnValueChange(AxiosGameScreen gameScreen, InputHelper input) + { + this.OnEvent(ValueChange, gameScreen, input); + } + + public virtual void OnMouseDown(AxiosGameScreen gameScreen, InputHelper input) + { + this.OnEvent(MouseDown, gameScreen, input); + } + + public virtual void OnMouseUp(AxiosGameScreen gameScreen, InputHelper input) + { + this.OnEvent(MouseUp, gameScreen, input); + } + + public virtual void OnScaleChange(AxiosGameObject gameObject) + { + if (this.ScaleChanged != null) + this.ScaleChanged(gameObject); + } + + private void OnEvent(AxiosHandler e, AxiosGameScreen gameScreen, InputHelper input) + { + AxiosHandler handle = e; + if (handle != null) + handle(this, gameScreen, input); + } + + #endregion + + #region GameObjectEvents + + /// + /// This event is fired when the the object looses focus + /// + /// The object sending the event + /// The gamescreen that this happened on + /// The current version of the kosmos world + public event AxiosHandler FocusLeave; + + public event AxiosHandler MouseHover; + + public event AxiosHandler MouseLeave; + + public event AxiosHandler MouseDown; + + public event AxiosHandler MouseUp; + + /// + /// This event is fired when the the object gains focus + /// + /// The object sending the event + /// The gamescreen that this happened on + /// The current version of the kosmos world + public event AxiosHandler FocusEnter; + + /// + /// This event is fired when the object's value changes + /// + /// The object sending the event + /// The gamescreen that this happened on + /// The current version of the kosmos world + public event AxiosHandler ValueChange; + + public event AxiosGameObjectHandler RemoveObject; + + public event AxiosGameObjectHandler ScaleChanged; + + #endregion + + protected virtual void OnRemove(AxiosGameObject gameObject) + { + RemoveObject(gameObject); + } + + protected void RemoveEvents() + { + this.MouseDown = null; + this.MouseHover = null; + this.MouseLeave = null; + this.MouseUp = null; + this.FocusEnter = null; + this.FocusLeave = null; + } + } +} diff --git a/axios/Engine/AxiosGameObject.cs b/axios/Engine/AxiosGameObject.cs new file mode 100644 index 0000000..bb387af --- /dev/null +++ b/axios/Engine/AxiosGameObject.cs @@ -0,0 +1,88 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using Axios.Engine.Interfaces; +using FarseerPhysics.SamplesFramework; +using Microsoft.Xna.Framework; +using FarseerPhysics.Dynamics; + +namespace Axios.Engine +{ + public abstract class AxiosGameObject : AxiosEvents, IAxiosGameObject + { + protected float _scale = 1f; + protected bool removing = false; + + public float Scale + { + get { return _scale; } + set + { + if (value != _scale) + { + _scale = value; + OnScaleChange(this); + } + } + } + + private string _name; + + public string Name + { + get + { + return this._name; + } + set + { + this._name = value; + } + } + public virtual void Update(AxiosGameScreen gameScreen, Microsoft.Xna.Framework.GameTime gameTime, bool otherScreenHasFocus, bool coveredByOtherScreen) + { + + } + + public virtual void LoadContent(AxiosGameScreen gameScreen) + { + + } + + public virtual void HandleInput(AxiosGameScreen gameScreen, InputHelper input, GameTime gameTime) + { + + } + + public virtual void HandleCursor(AxiosGameScreen gameScreen, InputHelper input) + { + + } + + public virtual void UnloadContent(AxiosGameScreen gameScreen) + { + RemoveEvents(); + } + + public void Remove() + { + this.OnRemove(this); + } + + protected void SetCollideWithAll(Body b) + { + if (b != null) + { + b.CollidesWith = Category.All; + b.CollisionCategories = Category.All; + } + } + + public override string ToString() + { + return this._name; + } + + } +} diff --git a/axios/Engine/AxiosGameScreen.cs b/axios/Engine/AxiosGameScreen.cs new file mode 100644 index 0000000..9fb7d5b --- /dev/null +++ b/axios/Engine/AxiosGameScreen.cs @@ -0,0 +1,397 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Audio; +using Microsoft.Xna.Framework.Content; +using Microsoft.Xna.Framework.GamerServices; +using Microsoft.Xna.Framework.Graphics; +using Microsoft.Xna.Framework.Input; +using Microsoft.Xna.Framework.Media; +using FarseerPhysics.Dynamics; +using FarseerPhysics.Dynamics.Joints; +using FarseerPhysics.Dynamics.Contacts; +using FarseerPhysics.Factories; +using FarseerPhysics.Common; +using FarseerPhysics.SamplesFramework; +using Axios.Engine.Interfaces; +using Axios.Engine.UI; +using Axios.Engine.Log; +using Axios.Engine.File; +using System.IO; + +namespace Axios.Engine +{ + + public abstract class AxiosGameScreen : PhysicsGameScreen + { + private List _gameObjects; + private AxiosGameObject prevobj; + private AxiosGameObject prevfocusobj; + + #region DebugTextVariables +#if DEBUG + public SpriteFont DebugSpriteFont; + public String DebugTextFont = "Fonts/helptext"; + public Color DebugTextColor = Color.Red; +#endif + #endregion + + private List _timers; + private List _uiobjects; + + private AxiosUIObject prevuiobj; + private AxiosUIObject prevuifocusobj; + + public AxiosGameScreen() + : base() + { + this._gameObjects = new List(); + _timers = new List(); + prevobj = null; + prevfocusobj = null; + this._uiobjects = new List(); + prevuiobj = null; + prevuifocusobj = null; + } + + /*public void AddGameObject(T gameobject) + { + if (gameobject is AxiosGameObject || gameobject is AxiosUIObject) + gameobject.LoadContent(this); + + if (gameobject is AxiosGameObject || gameobject is AxiosUIObject) + gameobject.RemoveObject += new AxiosGameObject.RemoveAxiosGameObjectHandler(RemoveGameObject); + + if (gameobject is AxiosGameObject) + { + this._gameObjects.Add(gameobject); + } + else if (gameobject is AxiosUIObject) + { + this._uiobjects.Add(gameobject); + } + }*/ + + /*public void AddGameObject(AxiosGameObject gameobject) + { + gameobject.LoadContent(this); + gameobject.RemoveObject += new AxiosGameObject.AxiosGameObjectHandler(RemoveGameObject); + this._gameObjects.Add(gameobject); + } + + public void AddGameObject(AxiosTimer timer) + { + timer.LoadContent(this); + _timers.Add(timer); + } + + public void AddGameObject(AxiosUIObject uiobject) + { + uiobject.LoadContent(this); + uiobject.RemoveObject += new AxiosEvents.AxiosGameObjectHandler(RemoveGameObject); + _uiobjects.Add(uiobject); + }*/ + + public void AddGameObject(object obj) + { + + if (obj is AxiosGameObject || obj is AxiosUIObject || obj is AxiosTimer) + { + AxiosGameObject tmp = obj as AxiosGameObject; + tmp.LoadContent(this); + + if (obj is AxiosGameObject || obj is AxiosUIObject) + tmp.RemoveObject += new AxiosEvents.AxiosGameObjectHandler(RemoveGameObject); + if (obj is AxiosGameObject && !(obj is AxiosUIObject)) + { + _gameObjects.Add(tmp); + } + else if (obj is AxiosUIObject) + { + _uiobjects.Add(obj as AxiosUIObject); + } + else if (obj is AxiosTimer) + { + _timers.Add(obj as AxiosTimer); + } + } + + } + + public void RemoveGameObject(AxiosTimer timer) + { + _timers.Remove(timer); + } + + public void RemoveGameObject(AxiosUIObject uiobject) + { + uiobject.RemoveObject -= new AxiosGameObject.AxiosGameObjectHandler(RemoveGameObject); + uiobject.UnloadContent(this); + _uiobjects.Remove(uiobject); + } + + public void RemoveGameObject(AxiosGameObject gameobject) + { + gameobject.RemoveObject -= new AxiosGameObject.AxiosGameObjectHandler(RemoveGameObject); + try + { + gameobject.UnloadContent(this); + this._gameObjects.Remove(gameobject); + } + catch (Exception) + { + //Not sure what is going on - but in certain cases an exception will be triggered that the body has already been marked for removal + } + + } + + public void RemoveAll() + { + AxiosLog.Instance.AddLine("Memory usage before cleanup: " + GC.GetTotalMemory(true).ToString(), LoggingFlag.DEBUG); + foreach (AxiosGameObject g in _gameObjects) + g.UnloadContent(this); + foreach (AxiosUIObject ui in _uiobjects) + ui.UnloadContent(this); + this.World.Clear(); + this._gameObjects.Clear(); + _timers.Clear(); + _uiobjects.Clear(); + AxiosLog.Instance.AddLine("Memory usage after cleanup: ", LoggingFlag.DEBUG); + } + + public override void ExitScreen() + { + base.ExitScreen(); + + } + + public override void LoadContent() + { + base.LoadContent(); + +#if DEBUG + if (!Axios.Settings.ScreenSaver) + this.DebugSpriteFont = this.ScreenManager.Content.Load(this.DebugTextFont); +#endif + } + + public override void Draw(GameTime gameTime) + { + base.Draw(gameTime); + + foreach (AxiosGameObject g in (from x in (from i in _gameObjects where i is IDrawableAxiosGameObject select (IDrawableAxiosGameObject)i) orderby x.DrawOrder select x)) + ((IDrawableAxiosGameObject)g).Draw(this, gameTime); + + foreach(AxiosUIObject g in (from x in _uiobjects orderby x.DrawOrder select x)) + ((IDrawableAxiosGameObject)g).Draw(this, gameTime); + } + + public override void Update(GameTime gameTime, bool otherScreenHasFocus, bool coveredByOtherScreen) + { + base.Update(gameTime, otherScreenHasFocus, coveredByOtherScreen); + + foreach (AxiosGameObject g in _gameObjects) + g.Update(this, gameTime, otherScreenHasFocus, coveredByOtherScreen); + + foreach (AxiosTimer t in _timers) + t.Update(this, gameTime, otherScreenHasFocus, coveredByOtherScreen); + + foreach(AxiosUIObject g in _uiobjects) + g.Update(this, gameTime, otherScreenHasFocus, coveredByOtherScreen); + + } + + public override void HandleCursor(InputHelper input) + { + base.HandleCursor(input); + HandleMouseEvents(input); + + foreach (AxiosGameObject g in _gameObjects) + g.HandleCursor(this, input); + } + + private void HandleMouseEvents(InputHelper input) + { + Vector2 position = this.Camera.ConvertScreenToWorld(input.Cursor); + Fixture fix = this.World.TestPoint(position); + AxiosGameObject gobj; + if (fix != null && fix.UserData != null && fix.UserData is AxiosGameObject) + { + gobj = (AxiosGameObject)fix.UserData; + + if (gobj != null && gobj != prevobj) + { + gobj.OnMouseHover(this, input); + + + if (prevobj != gobj && prevobj != null) + prevobj.OnMouseLeave(this, input); + } + else if (gobj != null) + { + if (input.IsNewMouseButtonRelease(MouseButtons.LeftButton)) + { + if (prevobj != null) + prevobj.OnFocusLeave(this, input); + gobj.OnFocusEnter(this, input); + gobj.OnMouseUp(this, input); + prevfocusobj = gobj; + //prevobj = gobj; + } + + if (input.IsNewMouseButtonPress(MouseButtons.LeftButton)) + gobj.OnMouseDown(this, input); + } + + if (gobj != null) + prevobj = gobj; + } + else + { + if (prevobj != null) + prevobj.OnMouseLeave(this, input); + if (input.IsNewMouseButtonPress(MouseButtons.LeftButton) && prevfocusobj != null) + { + prevfocusobj.OnFocusLeave(this, input); + prevfocusobj = null; + } + prevobj = null; + } + + Vector2 uiobjpos; + Rectangle uirect; + bool foundobject = false; + Vector2 mousepos = ConvertUnits.ToSimUnits(input.Cursor); + Vector2 objpos; + //System.Diagnostics.Debugger.Break(); + foreach(AxiosUIObject uiobject in _uiobjects) + { + uiobjpos = uiobject.Position; + objpos = this.Camera.ConvertScreenToWorld(uiobjpos); + + uirect = new Rectangle((int)uiobjpos.X, (int)uiobjpos.Y, (int)ConvertUnits.ToSimUnits(uiobject.Width), (int)ConvertUnits.ToSimUnits(uiobject.Height)); + + if (uirect.Contains((int)position.X, (int)position.Y)) + { + + if (input.IsNewMouseButtonPress(MouseButtons.LeftButton)) + { + uiobject.OnMouseDown(this, input); + } + + if (input.IsNewMouseButtonRelease(MouseButtons.LeftButton)) + { + //System.Diagnostics.Debugger.Break(); + if (prevuifocusobj != uiobject) + { + uiobject.OnFocusEnter(this, input); + if (prevuifocusobj != null) + prevuifocusobj.OnFocusLeave(this, input); + prevuifocusobj = uiobject; + } + uiobject.OnMouseUp(this, input); + } + + if (prevuiobj != uiobject) + { + //System.Diagnostics.Debugger.Break(); + uiobject.OnMouseHover(this, input); + if (prevuiobj != null) + prevuiobj.OnMouseLeave(this, input); + prevuiobj = uiobject; + } + foundobject = true; + break; + + } + } + if (!foundobject && prevuiobj != null) + { + //mouse moved away from object + prevuiobj.OnMouseLeave(this, input); + prevuiobj = null; + } + + if (input.IsNewMouseButtonRelease(MouseButtons.LeftButton)) + { + if (!foundobject && prevuifocusobj != null) + { + + prevuifocusobj.OnFocusLeave(this, input); + prevuifocusobj = null; + } + } + } + + public override void HandleInput(InputHelper input, GameTime gameTime) + { + base.HandleInput(input, gameTime); + + foreach (AxiosGameObject g in _gameObjects) + g.HandleInput(this, input, gameTime); + + foreach (AxiosUIObject g in _uiobjects) + g.HandleInput(this, input, gameTime); + } + + public override void UnloadContent() + { + base.UnloadContent(); + //AxiosLog.Instance.AddLine("Memory usage before cleanup: " + GC.GetTotalMemory(true).ToString(), LoggingFlag.DEBUG); + foreach (AxiosGameObject g in _gameObjects) + g.UnloadContent(this); + + foreach (AxiosUIObject g in _uiobjects) + g.UnloadContent(this); + + this._gameObjects.Clear(); + this._uiobjects.Clear(); + this.World.Clear(); + _timers.Clear(); + _uiobjects.Clear(); + //AxiosLog.Instance.AddLine("Memory usage after cleanup: " + GC.GetTotalMemory(true).ToString(), LoggingFlag.DEBUG); + //AxiosRegularFile f = new AxiosRegularFile("log.log"); + //f.WriteData(AxiosLog.Instance.GetLog(), FileMode.Append); + //AxiosIsolatedFile f = new AxiosIsolatedFile("log.log"); + //f.WriteData(AxiosLog.Instance.GetLog(), FileMode.Append); + //CleanUp(); + } + +#if WINDOWS +// System.Drawing is NOT avaiable on WP7 or Xbox + /* + * http://stackoverflow.com/a/7394185/195722 + * + * + * + */ + public Texture2D GetTexture(System.Drawing.Bitmap bitmap) + { + BlendState oldstate = ScreenManager.GraphicsDevice.BlendState; + ScreenManager.GraphicsDevice.BlendState = BlendState.AlphaBlend; + Texture2D tex = new Texture2D(this.ScreenManager.GraphicsDevice, bitmap.Width, bitmap.Height, true, SurfaceFormat.Color); + + System.Drawing.Imaging.BitmapData data = bitmap.LockBits(new System.Drawing.Rectangle(0, 0, bitmap.Width, bitmap.Height), System.Drawing.Imaging.ImageLockMode.ReadOnly, bitmap.PixelFormat); + + int bufferSize = data.Height * data.Stride; + + //create data buffer + byte[] bytes = new byte[bufferSize]; + + // copy bitmap data into buffer + System.Runtime.InteropServices.Marshal.Copy(data.Scan0, bytes, 0, bytes.Length); + + // copy our buffer to the texture + tex.SetData(bytes); + + // unlock the bitmap data + bitmap.UnlockBits(data); + + this.ScreenManager.GraphicsDevice.BlendState = oldstate; + return tex; + } +#endif + } +} diff --git a/axios/Engine/AxiosTimer.cs b/axios/Engine/AxiosTimer.cs new file mode 100644 index 0000000..7463787 --- /dev/null +++ b/axios/Engine/AxiosTimer.cs @@ -0,0 +1,80 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +using Axios.Engine.Interfaces; +using Microsoft.Xna.Framework; + +namespace Axios.Engine +{ + /* + * Modeled after Nicks' implemenentation + * Source: http://www.gamedev.net/topic/473544-how-to-make-a-timer-using-xna/page__view__findpost__p__4107032 + * + */ + public class AxiosTimer : IAxiosGameObject + { + TimeSpan interval = new TimeSpan(0, 0, 1); + TimeSpan lastTick = new TimeSpan(); + private bool _enabled = false; + + public event EventHandler Tick; + + public TimeSpan Interval + { + get { return interval; } + set { interval = value; } + } + + public Boolean Enabled + { + get { return _enabled; } + set { _enabled = value; } + } + + public AxiosTimer() + { + + } + + public void Update(AxiosGameScreen gameScreen, GameTime gameTime, bool otherScreenHasFocus, bool coveredByOtherScreen) + { + + if (_enabled) + { + if (gameTime.TotalGameTime - lastTick >= interval) + { + if (Tick != null) + Tick(this, null); + + lastTick = gameTime.TotalGameTime; + } + } + else + { + lastTick = gameTime.TotalGameTime; + } + } + + public virtual void LoadContent(AxiosGameScreen gameScreen) + { + + } + + public void HandleInput(AxiosGameScreen gameScreen, FarseerPhysics.SamplesFramework.InputHelper input, Microsoft.Xna.Framework.GameTime gameTime) + { + + } + + public void HandleCursor(AxiosGameScreen gameScreen, FarseerPhysics.SamplesFramework.InputHelper input) + { + + } + + public void UnloadContent(AxiosGameScreen gameScreen) + { + + } + } +} diff --git a/axios/Engine/BreakableAxiosGameObject.cs b/axios/Engine/BreakableAxiosGameObject.cs new file mode 100644 index 0000000..3fbfe23 --- /dev/null +++ b/axios/Engine/BreakableAxiosGameObject.cs @@ -0,0 +1,44 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +using FarseerPhysics.Dynamics; + +namespace Axios.Engine +{ + class BreakableAxiosGameObject : AxiosGameObject + { + + public BreakableBody Body; + + public delegate void BodyBroken(BreakableAxiosGameObject body); + + public event BodyBroken OnBodyBreak; + + protected bool _calledBodyBroken = false; + + public override void LoadContent(AxiosGameScreen gameScreen) + { + base.LoadContent(gameScreen); + + Body = new BreakableBody(); + } + + protected virtual void LoadSimpleBreakableBody() + { + + } + + public override void Update(AxiosGameScreen gameScreen, Microsoft.Xna.Framework.GameTime gameTime, bool otherScreenHasFocus, bool coveredByOtherScreen) + { + base.Update(gameScreen, gameTime, otherScreenHasFocus, coveredByOtherScreen); + + if (!_calledBodyBroken) + { + if (Body.Broken == true) + OnBodyBreak(this); + } + } + } +} diff --git a/axios/Engine/ComplexAxiosGameObject.cs b/axios/Engine/ComplexAxiosGameObject.cs new file mode 100644 index 0000000..4b8cd50 --- /dev/null +++ b/axios/Engine/ComplexAxiosGameObject.cs @@ -0,0 +1,49 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using FarseerPhysics.Dynamics; +using Microsoft.Xna.Framework; + +using Axios.Engine; +using Axios.Engine.Interfaces; +using FarseerPhysics.SamplesFramework; + +namespace Axios.Engine +{ + + public abstract class ComplexAxiosGameObject : AxiosGameObject + { + + public List GameObjects; + + public ComplexAxiosGameObject() + { + + } + + public override void LoadContent(AxiosGameScreen gameScreen) + { + base.LoadContent(gameScreen); + + CreateObjects(gameScreen); + + foreach (SimpleAxiosGameObject obj in GameObjects) + { + gameScreen.AddGameObject(obj); + } + + } + + protected override void OnRemove(AxiosGameObject gameObject) + { + base.OnRemove(gameObject); + + foreach (SimpleAxiosGameObject g in GameObjects) + g.Remove(); + } + + public abstract void CreateObjects(AxiosGameScreen gameScreen); + } + +} diff --git a/axios/Engine/Data/AxiosCSV.cs b/axios/Engine/Data/AxiosCSV.cs new file mode 100644 index 0000000..4a1758f --- /dev/null +++ b/axios/Engine/Data/AxiosCSV.cs @@ -0,0 +1,18 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +using Axios.Engine.File; + +namespace Axios.Engine.Data +{ + class AxiosCSV + { + private AxiosFile _file; + public AxiosCSV(AxiosFile file) + { + _file = file; + } + } +} diff --git a/axios/Engine/Data/AxiosDataTable.cs b/axios/Engine/Data/AxiosDataTable.cs new file mode 100644 index 0000000..4326f7d --- /dev/null +++ b/axios/Engine/Data/AxiosDataTable.cs @@ -0,0 +1,252 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.ComponentModel; +using System.Collections; + +namespace Axios.Engine.Data +{ +#if USECUSTOMDATATABLE + public enum CollectionChangeAction + { + Add, + Remove, + Refresh + } + + + + public delegate void CollectionChangeEventHandler( + Object sender, + CollectionChangeEventArgs e + ); + + class DataTable + { + private DataColumnCollection _columnCollection = new DataColumnCollection(); + private DataRowCollection _rowCollection = new DataRowCollection(); + public DataTable() + { + + } + + public DataColumnCollection Columns + { + get + { + return this._columnCollection; + } + private set + { + this._columnCollection = value; + } + } + + public int Count + { + get + { + return _columnCollection.Count(); + } + } + + public DataRowCollection Rows + { + get + { + return this._rowCollection; + } + set + { + this._rowCollection = value; + } + } + + public DataRow NewRow() + { + DataRow r = new DataRow(); + r.Table = this; + return r; + } + + } + + class DataRowCollection + { + private List _rows = new List(); + + public DataRowCollection() + { + + } + + /*public AxiosDataRow Add(params Object[] values) + { + AxiosDataRow row = new AxiosDataRow(); + //row.Table + foreach (object obj in values) + { + + } + }*/ + + public void Add(DataRow row) + { + _rows.Add(row); + } + } + + class DataRow + { + private DataTable _table; + private Dictionary _row = new Dictionary(); + + public DataRow() + { + + } + + public Object this[string columnName] + { + get + { + if (_row.ContainsKey(columnName)) + return _row[columnName]; + else + throw new ArgumentException("The column specified by " + columnName + " cannot be found."); + } + set + { + if (_row.ContainsKey(columnName)) + _row[columnName] = value; + else + throw new ArgumentException("The column specified by " + columnName + " cannot be found."); + } + } + + //Does this really reference a list of ints rather than a list of strings? + public Object this[int columnIndex] + { + get + { + return _row.ElementAt(columnIndex).Value; + } + + set + { + _row[_row.ElementAt(columnIndex).Key] = value; + } + } + + public DataTable Table + { + get + { + return _table; + } + set + { + _row.Clear(); + foreach (DataColumn col in _table.Columns) + _row[col.ColumnName] = ""; + this._table = value; + } + } + } + + class DataColumn + { + private string _columnName; + private Type _datatype; + public DataColumn(string columnName, Type dataType) + { + _columnName = columnName; + _datatype = dataType; + } + + public string ColumnName + { + get + { + return _columnName; + } + set + { + _columnName = value; + } + } + } + + class DataColumnCollection : IEnumerable + { + List _datacolumns = new List(); + + public event CollectionChangeEventHandler CollectionChanged; + + public DataColumnCollection() + { + + } + + public DataColumn Add(string columnName) + { + DataColumn col = new DataColumn(columnName, typeof(string)); + _datacolumns.Add(col); + CollectionChanged(this, new CollectionChangeEventArgs(CollectionChangeAction.Add, col)); + return col; + } + + public DataColumn Add(string columnName, Type dataType) + { + DataColumn col = new DataColumn(columnName, dataType); + _datacolumns.Add(col); + return col; + } + + public void Add(DataColumn column) + { + _datacolumns.Add(column); + } + + public void Remove(DataColumn column) + { + _datacolumns.Remove(column); + } + + public void Remove(string column) + { + for (int i = 0; i < _datacolumns.Count; i++) + { + if (_datacolumns[i].ColumnName == column) + { + _datacolumns.RemoveAt(i); + break; + } + } + } + + public void RemoveAt(int index) + { + _datacolumns.RemoveAt(index); + } + + public int Count() + { + return _datacolumns.Count(); + } + + + + public IEnumerator GetEnumerator() + { + return GetEnumerator(); + } + + IEnumerator IEnumerable.GetEnumerator() + { + return _datacolumns.GetEnumerator(); + } + } +#endif +} diff --git a/axios/Engine/Data/DataEvents.cs b/axios/Engine/Data/DataEvents.cs new file mode 100644 index 0000000..767c932 --- /dev/null +++ b/axios/Engine/Data/DataEvents.cs @@ -0,0 +1,43 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +namespace Axios.Engine.Data +{ +#if USECUSTOMDATATABLE + public class CollectionChangeEventArgs : EventArgs + { + private object _element; + private CollectionChangeAction _action; + public CollectionChangeEventArgs(CollectionChangeAction action, Object element) + { + + } + + public object Element + { + get + { + return _element; + } + private set + { + _element = value; + } + } + + public CollectionChangeAction Action + { + get + { + return _action; + } + set + { + _action = value; + } + } + } +#endif +} diff --git a/axios/Engine/DrawableAxiosGameObject.cs b/axios/Engine/DrawableAxiosGameObject.cs new file mode 100644 index 0000000..130cde0 --- /dev/null +++ b/axios/Engine/DrawableAxiosGameObject.cs @@ -0,0 +1,175 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Graphics; +using FarseerPhysics.Dynamics; +using FarseerPhysics.SamplesFramework; +using Axios.Engine.Interfaces; + +namespace Axios.Engine +{ + public class DrawableAxiosGameObject : AxiosGameObject, IDrawableAxiosGameObject + { + protected int _draworder; + protected Texture2D Texture; + //protected float _scale = 1f; + public Vector2 Position = new Vector2(); + public Vector2 Origin = new Vector2(); + + protected Boolean _adjustunits = true; + protected Boolean _relativetocamera = true; + + public override void LoadContent(AxiosGameScreen gameScreen) + { + base.LoadContent(gameScreen); + + + } + + /*public float Scale + { + get { return _scale; } + set { _scale = value; } + }*/ + + public virtual void Draw(AxiosGameScreen gameScreen, GameTime gameTime) + { +/*#if DEBUG + System.Diagnostics.Debugger.Break(); +#endif*/ + if (_relativetocamera) + gameScreen.ScreenManager.SpriteBatch.Begin(0, null, null, null, null, null, gameScreen.Camera.View); + else + gameScreen.ScreenManager.SpriteBatch.Begin(); + if (_adjustunits) + gameScreen.ScreenManager.SpriteBatch.Draw(Texture, ConvertUnits.ToDisplayUnits(Position), null, Color.White, 0, Origin, _scale, SpriteEffects.None, 0); + else + gameScreen.ScreenManager.SpriteBatch.Draw(Texture, Position, null, Color.White, 0, Origin, _scale, SpriteEffects.None, 0); + gameScreen.ScreenManager.SpriteBatch.End(); + } + + public int DrawOrder + { + get + { + return this._draworder; + } + set + { + this._draworder = value; + } + } + + //Copied/adapted from http://create.msdn.com/en-US/education/catalog/tutorial/collision_2d_perpixel + /// + /// This method is a very simple collision detection based on textures. + /// While Farseer (Box2D) is an excellent physics engine - it doesn't know, or care, about the textures. + /// This method does a AABB test and if that is true - it tests the individual pixels in the textures. + /// + /// Object to test against + /// true if the object is colliding, false if it isn't + public bool CollidesWith(DrawableAxiosGameObject obj) + { + Rectangle thisobj = new Rectangle((int)this.Position.X, (int)this.Position.Y, this.Texture.Width, this.Texture.Height); + Rectangle otherobj = new Rectangle((int)obj.Position.X, (int)obj.Position.Y, obj.Texture.Width, obj.Texture.Height); + + if (thisobj.Intersects(otherobj)) + { + + int top = Math.Max(thisobj.Top, otherobj.Top); + int bottom = Math.Min(thisobj.Bottom, otherobj.Bottom); + int left = Math.Max(thisobj.Left, otherobj.Left); + int right = Math.Min(thisobj.Right, otherobj.Right); + + Color[] thisobjcolor = new Color[this.Texture.Width * this.Texture.Height]; + Color[] otherobjcolor = new Color[obj.Texture.Width * obj.Texture.Height]; + + Texture.GetData(thisobjcolor); + obj.Texture.GetData(otherobjcolor); + + // Check every point within the intersection bounds + for (int y = top; y < bottom; y++) + { + for (int x = left; x < right; x++) + { + // Get the color of both pixels at this point + Color colorA = thisobjcolor[(x - thisobj.Left) + + (y - thisobj.Top) * thisobj.Width]; + Color colorB = otherobjcolor[(x - otherobj.Left) + + (y - otherobj.Top) * otherobj.Width]; + + // If both pixels are not completely transparent, + if (colorA.A != 0 && colorB.A != 0) + { + // then an intersection has been found + return true; + } + } + } + } + + return false; + } + + //Copied/adapted from http://create.msdn.com/en-US/education/catalog/tutorial/collision_2d_perpixel + /// + /// This method is a very simple collision detection based on textures. + /// While Farseer (Box2D) is an excellent physics engine - it doesn't know, or care, about the textures. + /// This method does a AABB test and if that is true - it tests the individual pixels in the textures. + /// + /// Object to test against + /// true if the object is colliding, false if it isn't + public bool CollidesWith(Vector2 pos, Rectangle rect) + { + Rectangle thisobj = new Rectangle((int)this.Position.X, (int)this.Position.Y, this.Texture.Width, this.Texture.Height); + Rectangle otherobj = new Rectangle((int)pos.X, (int)pos.Y, rect.Width, rect.Height); + + Texture2D obj = new Texture2D(Texture.GraphicsDevice, rect.Width, rect.Height); + Color[] arr = new Color[rect.Width * rect.Height]; + for (int i = 0; i < rect.Width * rect.Height; ++i) + arr[i] = Color.Black; + + obj.SetData(arr); + + if (thisobj.Intersects(otherobj)) + { + + int top = Math.Max(thisobj.Top, otherobj.Top); + int bottom = Math.Min(thisobj.Bottom, otherobj.Bottom); + int left = Math.Max(thisobj.Left, otherobj.Left); + int right = Math.Min(thisobj.Right, otherobj.Right); + + Color[] thisobjcolor = new Color[this.Texture.Width * this.Texture.Height]; + Color[] otherobjcolor = new Color[obj.Width * obj.Height]; + + Texture.GetData(thisobjcolor); + obj.GetData(otherobjcolor); + + // Check every point within the intersection bounds + for (int y = top; y < bottom; y++) + { + for (int x = left; x < right; x++) + { + // Get the color of both pixels at this point + Color colorA = thisobjcolor[(x - thisobj.Left) + + (y - thisobj.Top) * thisobj.Width]; + Color colorB = otherobjcolor[(x - otherobj.Left) + + (y - otherobj.Top) * otherobj.Width]; + + // If both pixels are not completely transparent, + if (colorA.A != 0 && colorB.A != 0) + { + // then an intersection has been found + return true; + } + } + } + } + + return false; + } + } +} diff --git a/axios/Engine/DrawableBreakableAxiosGameObject.cs b/axios/Engine/DrawableBreakableAxiosGameObject.cs new file mode 100644 index 0000000..f1b4ded --- /dev/null +++ b/axios/Engine/DrawableBreakableAxiosGameObject.cs @@ -0,0 +1,70 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +using Axios.Engine.Interfaces; +using Microsoft.Xna.Framework.Graphics; +using Microsoft.Xna.Framework; +using FarseerPhysics.Dynamics; +using FarseerPhysics.SamplesFramework; + +namespace Axios.Engine +{ +#if DFDSF + class DrawableBreakableAxiosGameObject : AxiosBreakableGameObject, IDrawableAxiosGameObject + { + protected int _draworder; + + protected new List BodyParts = new List(); + protected new SimpleDrawableAxiosGameObject BodyPart = null; + + protected Boolean _adjustunits = true; + protected Boolean _relativetocamera = true; + + public int DrawOrder + { + get + { + return this._draworder; + } + set + { + this._draworder = value; + } + } + + public override void LoadContent(AxiosGameScreen gameScreen) + { + base.LoadContent(gameScreen); + + + } + + public virtual void Draw(AxiosGameScreen gameScreen, GameTime gameTime) + { + + /*for(int i = 0; i < Body.Parts.Count; i++) + { + if (_relativetocamera) + gameScreen.ScreenManager.SpriteBatch.Begin(0, null, null, null, null, null, gameScreen.Camera.View); + else + gameScreen.ScreenManager.SpriteBatch.Begin(); + if (_adjustunits) + DrawObject(gameScreen.ScreenManager.SpriteBatch, Textures[i], Body.Parts[i].Body, Origins[i], true); + else + DrawObject(gameScreen.ScreenManager.SpriteBatch, Textures[i], Body.Parts[i].Body, Origins[i]); + gameScreen.ScreenManager.SpriteBatch.End(); + }*/ + if (_isbroken) + if (BodyParts.Count > 0) + foreach (SimpleDrawableAxiosGameObject obj in BodyParts) + obj.Draw(gameScreen, gameTime); + else + if (BodyPart != null) + BodyPart.Draw(gameScreen, gameTime); + } + + } +#endif +} diff --git a/axios/Engine/Extensions/String.cs b/axios/Engine/Extensions/String.cs new file mode 100644 index 0000000..ccd9c71 --- /dev/null +++ b/axios/Engine/Extensions/String.cs @@ -0,0 +1,24 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +namespace Axios.Engine.Extenions +{ + public static class AxiosExtensions_String + { + /// + /// Get the string slice between the two indexes. + /// Inclusive for start index, exclusive for end index. + /// + public static string Slice(this string source, int start, int end) + { + if (end < 0) // Keep this for negative end support + { + end = source.Length + end; + } + int len = end - start; // Calculate length + return source.Substring(start, len); // Return Substring of length + } + } +} \ No newline at end of file diff --git a/axios/Engine/Extensions/Texture2D.cs b/axios/Engine/Extensions/Texture2D.cs new file mode 100644 index 0000000..967b574 --- /dev/null +++ b/axios/Engine/Extensions/Texture2D.cs @@ -0,0 +1,199 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +using Microsoft.Xna.Framework.Graphics; +using Microsoft.Xna.Framework; + + +namespace Axios.Engine.Extenions +{ + public enum TextureUnionLocation { + Right, + Bottom, + Top, + Left + } + public static class AxiosExtensions_Texture2D + { + /// http://gamedev.stackexchange.com/questions/11584/xna-splitting-one-large-texture-into-an-array-of-smaller-textures + /// + /// Splits a texture into an array of smaller textures of the specified size. + /// + /// The texture to be split into smaller textures + /// The width of each of the smaller textures that will be contained in the returned array. + /// The height of each of the smaller textures that will be contained in the returned array. + /// A multidimensional array represting the rows/coulmns in the texture. + public static Texture2D[,] Split(this Texture2D original, int partWidth, int partHeight, out int xCount, out int yCount) + { + yCount = original.Height / partHeight; //+ (partHeight % original.Height == 0 ? 0 : 1);//The number of textures in each horizontal row + xCount = original.Width / partWidth; //+(partWidth % original.Width == 0 ? 0 : 1);//The number of textures in each vertical column + Texture2D[,] r = new Texture2D[yCount,xCount];//Number of parts = (area of original) / (area of each part). + int dataPerPart = partWidth * partHeight;//Number of pixels in each of the split parts + + //Get the pixel data from the original texture: + Color[] originalData = new Color[original.Width * original.Height]; + original.GetData(originalData); + + //int index = 0; + int currxidx = 0; + int curryidx = 0; + for (int y = 0; y < yCount * partHeight; y += partHeight) + { + for (int x = 0; x < xCount * partWidth; x += partWidth) + { + //The texture at coordinate {x, y} from the top-left of the original texture + Texture2D part = new Texture2D(original.GraphicsDevice, partWidth, partHeight); + //The data for part + Color[] partData = new Color[dataPerPart]; + + //Fill the part data with colors from the original texture + for (int py = 0; py < partHeight; py++) + for (int px = 0; px < partWidth; px++) + { + int partIndex = px + py * partWidth; + //If a part goes outside of the source texture, then fill the overlapping part with Color.Transparent + if (y + py >= original.Height || x + px >= original.Width) + partData[partIndex] = Color.Transparent; + else + partData[partIndex] = originalData[(x + px) + (y + py) * original.Width]; + } + + //Fill the part with the extracted data + part.SetData(partData); + //Stick the part in the return array: + r[curryidx, currxidx] = part; + curryidx++; + } + curryidx = 0; + curryidx++; + } + //Return the array of parts. + return r; + } + + // http://gamedev.stackexchange.com/questions/11584/xna-splitting-one-large-texture-into-an-array-of-smaller-textures + /// + /// Splits a texture into an array of smaller textures of the specified size. + /// + /// The texture to be split into smaller textures + /// The width of each of the smaller textures that will be contained in the returned array. + /// The height of each of the smaller textures that will be contained in the returned array. + /// The width offset whitespace to ignore + /// The height offset whitespace to ignore + /// The number of textures per row + /// The number of texture per column + /// A multidimensional array represting the rows/coulmns in the texture. + public static Texture2D[,] Split(this Texture2D original, int partWidth, int partHeight, int offsetWidth, int offsetHeight, out int xCount, out int yCount) + { + yCount = original.Height / partHeight; //+ (partHeight % original.Height == 0 ? 0 : 1);//The number of textures in each horizontal row + xCount = original.Width / partWidth; //+(partWidth % original.Width == 0 ? 0 : 1);//The number of textures in each vertical column + + //xCount -= (xCount % offsetWidth); + //yCount -= (yCount % offsetHeight); + Texture2D[,] r = new Texture2D[yCount, xCount];//Number of parts = (area of original) / (area of each part). + int dataPerPart = partWidth * partHeight;//Number of pixels in each of the split parts + + //Get the pixel data from the original texture: + Color[] originalData = new Color[original.Width * original.Height]; + original.GetData(originalData); + + //int index = 0; + int currxidx = 0; + int curryidx = 0; + for (int y = 0; y < yCount * partHeight; y += (partHeight + offsetHeight)) + { + for (int x = 0; x < xCount * partWidth; x += (partWidth + offsetWidth)) + { + //The texture at coordinate {x, y} from the top-left of the original texture + Texture2D part = new Texture2D(original.GraphicsDevice, partWidth, partHeight); + //The data for part + Color[] partData = new Color[dataPerPart]; + + //Fill the part data with colors from the original texture + for (int py = 0; py < partHeight; py++) + for (int px = 0; px < partWidth; px++) + { + int partIndex = px + py * partWidth; + //If a part goes outside of the source texture, then fill the overlapping part with Color.Transparent + if (y + py >= original.Height || x + px >= original.Width) + partData[partIndex] = Color.Transparent; + else + partData[partIndex] = originalData[(x + px) + (y + py) * original.Width]; + } + + //Fill the part with the extracted data + part.SetData(partData); + //Stick the part in the return array: + r[curryidx, currxidx] = part; + currxidx++; + } + currxidx = 0; + curryidx++; + } + //Return the array of parts. + return r; + } + + /// http://forums.create.msdn.com/forums/t/79258.aspx + /// + /// Combines one texture with another + /// + /// The first texture + /// The second texture + /// The location where to put the texture in reference to the first + /// + public static Texture2D Union(this Texture2D original, Texture2D texturetoadd, TextureUnionLocation loc) + { + int newWidth = 0; + int newHeight = 0; + if (loc == TextureUnionLocation.Right || loc == TextureUnionLocation.Left) + { + newWidth = original.Width + texturetoadd.Width; + newHeight = original.Height; + } + else if (loc == TextureUnionLocation.Bottom || loc == TextureUnionLocation.Top) + { + newWidth = original.Width; + newHeight = original.Height + texturetoadd.Height; + } + Texture2D r = new Texture2D(original.GraphicsDevice, newWidth, newHeight); + Color[] originaldata = new Color[original.Width * original.Height]; + Color[] texturetoadddata = new Color[texturetoadd.Width * texturetoadd.Height]; + Color[] newtexturedata = new Color[newHeight * newWidth]; + + original.GetData(originaldata); + texturetoadd.GetData(texturetoadddata); + + if (loc == TextureUnionLocation.Right) + { + r.SetData(0, new Rectangle(0, 0, original.Width, original.Height), originaldata, 0, original.Width * original.Height); + + r.SetData(0, new Rectangle(original.Width, 0, texturetoadd.Width, texturetoadd.Height), texturetoadddata, 0, texturetoadd.Width * texturetoadd.Height); + } + else if (loc == TextureUnionLocation.Bottom) + { + r.SetData(0, new Rectangle(0, 0, original.Width, original.Height), originaldata, 0, original.Width * original.Height); + + r.SetData(0, new Rectangle(0, original.Height, texturetoadd.Width, texturetoadd.Height), texturetoadddata, 0, texturetoadd.Width * texturetoadd.Height); + } + else if (loc == TextureUnionLocation.Left) + { + r.SetData(0, new Rectangle(0, 0, texturetoadd.Width, texturetoadd.Height), texturetoadddata, 0, texturetoadd.Width * texturetoadd.Height); + + r.SetData(0, new Rectangle(texturetoadd.Width, 0, original.Width, original.Height), originaldata, 0, original.Width * original.Height); + } + else if (loc == TextureUnionLocation.Top) + { + r.SetData(0, new Rectangle(0, 0, texturetoadd.Width, texturetoadd.Height), texturetoadddata, 0, texturetoadd.Width * texturetoadd.Height); + + r.SetData(0, new Rectangle(0, texturetoadd.Height, original.Width, original.Height), originaldata, 0, original.Width * original.Height); + } + + + return r; + } + + } +} \ No newline at end of file diff --git a/axios/Engine/File/AxiosFile.cs b/axios/Engine/File/AxiosFile.cs new file mode 100644 index 0000000..40aef9d --- /dev/null +++ b/axios/Engine/File/AxiosFile.cs @@ -0,0 +1,35 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.IO; + +using Axios.Engine.Interfaces; + +namespace Axios.Engine.File +{ + public class AxiosFile : IAxiosFile + { + protected string _content; + + public String Content + { + get { return _content; } + protected set { this._content = value; } + } + + protected string _filename; + + + + public virtual void WriteData(string data, FileMode mode) + { + throw new NotImplementedException(); + } + + public virtual string ReadData() + { + throw new NotImplementedException(); + } + } +} diff --git a/axios/Engine/File/AxiosIsolatedFile.cs b/axios/Engine/File/AxiosIsolatedFile.cs new file mode 100644 index 0000000..2d304a0 --- /dev/null +++ b/axios/Engine/File/AxiosIsolatedFile.cs @@ -0,0 +1,59 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.IO.IsolatedStorage; +using System.IO; + +using Axios.Engine.Interfaces; + +namespace Axios.Engine.File +{ + public class AxiosIsolatedFile : AxiosFile, IAxiosFile + { + + public AxiosIsolatedFile(string filename) + { + this._filename = filename; + } + + public override void WriteData(string data, FileMode mode) + { + //Make sure that a proper mode is passed + if (mode == FileMode.Append + || mode == FileMode.Create + || mode == FileMode.CreateNew + || mode == FileMode.Truncate) + { +#if WINDOWS + IsolatedStorageFile savegameStorage = IsolatedStorageFile.GetUserStoreForDomain(); +#else + IsolatedStorageFile savegameStorage = IsolatedStorageFile.GetUserStoreForApplication(); +#endif + IsolatedStorageFileStream fs = null; + fs = savegameStorage.OpenFile(_filename, mode); + StreamWriter sw = new StreamWriter(fs); + sw.Write(data); + sw.Close(); + this.Content = data; + } + } + + public override string ReadData() + { + string ret = ""; +#if WINDOWS + IsolatedStorageFile savegameStorage = IsolatedStorageFile.GetUserStoreForDomain(); +#else + IsolatedStorageFile savegameStorage = IsolatedStorageFile.GetUserStoreForApplication(); +#endif + IsolatedStorageFileStream fs = null; + fs = savegameStorage.OpenFile(_filename, System.IO.FileMode.Open); + StreamReader sr = new StreamReader(fs); + ret = sr.ReadToEnd(); + sr.Close(); + Content = ret; + return ret; + } + } +} diff --git a/axios/Engine/File/AxiosRegularFile.cs b/axios/Engine/File/AxiosRegularFile.cs new file mode 100644 index 0000000..f860204 --- /dev/null +++ b/axios/Engine/File/AxiosRegularFile.cs @@ -0,0 +1,44 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.IO; + +using Axios.Engine.Interfaces; + +namespace Axios.Engine.File +{ + public class AxiosRegularFile : AxiosFile, IAxiosFile + { + public AxiosRegularFile(string file) + { + _filename = file; + } + + public override void WriteData(string data, FileMode mode) + { + //Make sure that a proper mode is passed + if (mode == FileMode.Append + || mode == FileMode.Create + || mode == FileMode.CreateNew + || mode == FileMode.Truncate) + { + FileStream fs = new FileStream(_filename, mode); + StreamWriter sw = new StreamWriter(fs); + sw.Write(data); + sw.Close(); + + } + } + + public override string ReadData() + { + string ret = ""; + FileStream fs = new FileStream(_filename, FileMode.Open); + StreamReader sr = new StreamReader(fs); + ret = sr.ReadToEnd(); + sr.Close(); + return ret; + } + } +} diff --git a/axios/Engine/File/AxiosTitleFile.cs b/axios/Engine/File/AxiosTitleFile.cs new file mode 100644 index 0000000..3f24cc8 --- /dev/null +++ b/axios/Engine/File/AxiosTitleFile.cs @@ -0,0 +1,35 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.IO; +using Microsoft.Xna.Framework; +using Axios.Engine.File; +using Axios.Engine.Interfaces; + +namespace Axios.Engine.File +{ + public class AxiosTitleFile : AxiosFile, IAxiosFile + { + public AxiosTitleFile(string filename) + { + //Title Files can only be opened for reading! + + this._filename = filename; + } + + + public override void WriteData(string data, FileMode mode) + { + throw new NotImplementedException(); + } + + public override string ReadData() + { + StreamReader sr = new StreamReader(TitleContainer.OpenStream(_filename)); + this.Content = sr.ReadToEnd(); + sr.Close(); + return this.Content; + } + } +} diff --git a/axios/Engine/Interfaces/IAxiosFile.cs b/axios/Engine/Interfaces/IAxiosFile.cs new file mode 100644 index 0000000..7e0b669 --- /dev/null +++ b/axios/Engine/Interfaces/IAxiosFile.cs @@ -0,0 +1,14 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.IO; + +namespace Axios.Engine.Interfaces +{ + interface IAxiosFile + { + void WriteData(string data, FileMode mode); + string ReadData(); + } +} diff --git a/axios/Engine/Interfaces/IAxiosGameObject.cs b/axios/Engine/Interfaces/IAxiosGameObject.cs new file mode 100644 index 0000000..3af34d3 --- /dev/null +++ b/axios/Engine/Interfaces/IAxiosGameObject.cs @@ -0,0 +1,19 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using Microsoft.Xna.Framework; +using FarseerPhysics.SamplesFramework; + + +namespace Axios.Engine.Interfaces +{ + interface IAxiosGameObject + { + void Update(AxiosGameScreen gameScreen, GameTime gameTime, bool otherScreenHasFocus, bool coveredByOtherScreen); + void LoadContent(AxiosGameScreen gameScreen); + void HandleInput(AxiosGameScreen gameScreen, InputHelper input, GameTime gameTime); + void HandleCursor(AxiosGameScreen gameScreen, InputHelper input); + void UnloadContent(AxiosGameScreen gameScreen); + } +} diff --git a/axios/Engine/Interfaces/IDrawableAxiosGameObject.cs b/axios/Engine/Interfaces/IDrawableAxiosGameObject.cs new file mode 100644 index 0000000..dd558aa --- /dev/null +++ b/axios/Engine/Interfaces/IDrawableAxiosGameObject.cs @@ -0,0 +1,18 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using Microsoft.Xna.Framework; + +namespace Axios.Engine.Interfaces +{ + interface IDrawableAxiosGameObject + { + int DrawOrder + { + get; + set; + } + void Draw(AxiosGameScreen gameScreen, GameTime gameTime); + } +} diff --git a/axios/Engine/Log/AxiosLog.cs b/axios/Engine/Log/AxiosLog.cs new file mode 100644 index 0000000..d32751f --- /dev/null +++ b/axios/Engine/Log/AxiosLog.cs @@ -0,0 +1,52 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +using Axios.Engine; + +namespace Axios.Engine.Log +{ + [Flags] + public enum LoggingFlag + { + NONE = 0, + DEBUG = 1, + INFO = 2, + WARN = 4, + ERROR = 8, + FATAL = 16, + ALL = 32 + } + public class AxiosLog : Singleton + { + private List _log; + + public AxiosLog() + { + _log = new List(); + } + + public void AddLine(string line, LoggingFlag flag) + { + + if (flag <= Settings.Loglevel) + _log.Add("[" + DateTime.Now.ToString("M/d/yyyy H:mm:ss") + " - " + flag.ToString() + "]" + line); + } + + public List GetLogList() + { + return _log; + } + + public string GetLog(string seperator) + { + return String.Join(seperator, _log.ToArray()) + seperator; + } + + public string GetLog() + { + return GetLog("\r\n"); + } + } +} diff --git a/axios/Engine/SimpleAxiosGameObject.cs b/axios/Engine/SimpleAxiosGameObject.cs new file mode 100644 index 0000000..2392323 --- /dev/null +++ b/axios/Engine/SimpleAxiosGameObject.cs @@ -0,0 +1,66 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using FarseerPhysics.Dynamics; +using Microsoft.Xna.Framework; + +using Axios.Engine.Log; + +namespace Axios.Engine +{ + public abstract class SimpleAxiosGameObject : AxiosGameObject + { + public Body BodyPart; + public Vector2 Position; + public Vector2 Origin; + + public bool ApplyConstantVelocity = false; + public Vector2 ConstantVelocity; + + public SimpleAxiosGameObject() + { + AxiosLog.Instance.AddLine("[Axios Engine] - Creating SimpleAxiosGameObject " + Name, LoggingFlag.DEBUG); + Position = new Vector2(); + } + + public override void UnloadContent(AxiosGameScreen gameScreen) + { + AxiosLog.Instance.AddLine("[Axios Engine] - Unloading SimpleAxiosGameObject " + Name, LoggingFlag.DEBUG); + base.UnloadContent(gameScreen); + gameScreen.World.RemoveBody(BodyPart); + } + + public override void Update(AxiosGameScreen gameScreen, GameTime gameTime, bool otherScreenHasFocus, bool coveredByOtherScreen) + { + base.Update(gameScreen, gameTime, otherScreenHasFocus, coveredByOtherScreen); + + if (ApplyConstantVelocity) + { + if (Math.Abs(BodyPart.LinearVelocity.X) > ConstantVelocity.X || Math.Abs(BodyPart.LinearVelocity.X) < ConstantVelocity.X) + { + //Figure which direction it's going and adjust + + if (Math.Abs(BodyPart.LinearVelocity.X) > BodyPart.LinearVelocity.X) //negative + BodyPart.LinearVelocity = new Vector2(-ConstantVelocity.X, BodyPart.LinearVelocity.Y); + else + BodyPart.LinearVelocity = new Vector2(ConstantVelocity.X, BodyPart.LinearVelocity.Y); + + } + + if (Math.Abs(BodyPart.LinearVelocity.Y) > ConstantVelocity.Y || Math.Abs(BodyPart.LinearVelocity.Y) < ConstantVelocity.Y) + { + //Figure which direction it's going and adjust + + if (Math.Abs(BodyPart.LinearVelocity.Y) > BodyPart.LinearVelocity.Y) //negative + BodyPart.LinearVelocity = new Vector2(BodyPart.LinearVelocity.X, -ConstantVelocity.Y); + else + BodyPart.LinearVelocity = new Vector2(BodyPart.LinearVelocity.X, ConstantVelocity.Y); + } + } + } + + + + } +} diff --git a/axios/Engine/SimpleDrawableAxiosGameObject.cs b/axios/Engine/SimpleDrawableAxiosGameObject.cs new file mode 100644 index 0000000..8bcdd2c --- /dev/null +++ b/axios/Engine/SimpleDrawableAxiosGameObject.cs @@ -0,0 +1,92 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Graphics; +using FarseerPhysics.Dynamics; +using FarseerPhysics.SamplesFramework; +using Axios.Engine.Interfaces; + + +namespace Axios.Engine +{ + public abstract class SimpleDrawableAxiosGameObject : SimpleAxiosGameObject, IDrawableAxiosGameObject + { + protected Texture2D Texture; + + protected Boolean _adjustunits = true; + protected Boolean _relativetocamera = true; + protected int _draworder; + + + + public SimpleDrawableAxiosGameObject() + { + this.BodyPart = new Body(); + this.Position = new Vector2(); + this.Origin = new Vector2(); + + } + + public override void LoadContent(AxiosGameScreen gameScreen) + { + base.LoadContent(gameScreen); + + //this.Texture = new Texture2D(gameScreen.ScreenManager.GraphicsDevice, 1, 1); + } + + public virtual void Draw(AxiosGameScreen gameScreen, GameTime gameTime) + { + + /*#if DEBUG + System.Diagnostics.Debugger.Break(); + #endif*/ + if (_relativetocamera) + gameScreen.ScreenManager.SpriteBatch.Begin(0, null, null, null, null, null, gameScreen.Camera.View); + else + gameScreen.ScreenManager.SpriteBatch.Begin(); + if (_adjustunits) + DrawObject(gameScreen.ScreenManager.SpriteBatch, Texture, BodyPart, Origin, true, _scale); + else + DrawObject(gameScreen.ScreenManager.SpriteBatch, Texture, BodyPart, Origin, _scale); + gameScreen.ScreenManager.SpriteBatch.End(); + } + + protected void DrawObject(SpriteBatch sb, Texture2D texture, Body body, Vector2 origin) + { + DrawObject(sb, texture, body, origin, false, _scale); + } + + protected void DrawObject(SpriteBatch sb, Texture2D texture, Body body, Vector2 origin, float scale) + { + DrawObject(sb, texture, body, origin, false, scale); + } + + protected void DrawObject(SpriteBatch sb, Texture2D texture, Body body, Vector2 origin, bool Convertunits, float scale) + { + if (Convertunits) + sb.Draw(texture, ConvertUnits.ToDisplayUnits(body.Position), + null, + Color.White, body.Rotation, origin, scale, + SpriteEffects.None, 0); + else + sb.Draw(texture, body.Position, + null, + Color.White, body.Rotation, origin, scale, + SpriteEffects.None, 0f); + } + + public int DrawOrder + { + get + { + return this._draworder; + } + set + { + this._draworder = value; + } + } + } +} diff --git a/axios/Engine/Singleton.cs b/axios/Engine/Singleton.cs new file mode 100644 index 0000000..51ce2d2 --- /dev/null +++ b/axios/Engine/Singleton.cs @@ -0,0 +1,32 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +namespace Axios.Engine +{ + //http://ralch.wordpress.com/2008/11/22/the-singleton-pattern-how-to-make-it-reusable/ + //http://msdn.microsoft.com/en-us/library/ff650316.aspx + public abstract class Singleton where T: new() + { + private static T instance; + private static object syncRoot = new Object(); + + public static T Instance + { + get + { + if (instance == null) + { + lock (syncRoot) + { + if (instance == null) + instance = new T(); + } + } + + return instance; + } + } + } +} diff --git a/axios/Engine/UI/AxiosButton.cs b/axios/Engine/UI/AxiosButton.cs new file mode 100644 index 0000000..5faf6ec --- /dev/null +++ b/axios/Engine/UI/AxiosButton.cs @@ -0,0 +1,97 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +using Microsoft.Xna.Framework.Graphics; +using FarseerPhysics.SamplesFramework; +using FarseerPhysics.Factories; +using FarseerPhysics.Dynamics; + +namespace Axios.Engine.UI +{ + public class AxiosButton : AxiosUIObject + { + protected Texture2D _hovertexture; + protected Texture2D _clicktexture; + protected Texture2D _normaltexture; + + /// + /// HoverTexture is the texture that will be set when the mouse hovers over the button + /// + public Texture2D HoverTexture + { + get { return this._hovertexture; } + set { this._hovertexture = value; } + } + + /// + /// The ClickTexture is the texture that will be set when the user clicks on the button + /// + public Texture2D ClickTexture + { + get { return this._clicktexture; } + set { this._clicktexture = value; } + } + + /// + /// The normal texture is the texture when the button is not active + /// + public Texture2D NormalTexture + { + get { return this._normaltexture; } + set { this._normaltexture = value; this.Texture = value; } + } + + public AxiosButton() + { + + } + + public override void LoadContent(AxiosGameScreen gameScreen) + { + base.LoadContent(gameScreen); + + } + + public override void OnMouseHover(AxiosGameScreen gameScreen, InputHelper input) + { + base.OnMouseHover(gameScreen, input); + + this.Texture = _hovertexture; + + } + + public override void OnMouseLeave(AxiosGameScreen gameScreen, InputHelper input) + { + base.OnMouseLeave(gameScreen, input); + + this.Texture = _normaltexture; + } + + public override void OnMouseDown(AxiosGameScreen gameScreen, InputHelper input) + { + base.OnMouseDown(gameScreen, input); + + this.Texture = _clicktexture; + } + + public override void OnMouseUp(AxiosGameScreen gameScreen, InputHelper input) + { + base.OnMouseUp(gameScreen, input); + + this.Texture = _hovertexture; + + + } + + + + public override void HandleCursor(AxiosGameScreen gameScreen, InputHelper input) + { + base.HandleCursor(gameScreen, input); + + + } + } +} diff --git a/axios/Engine/UI/AxiosUIObject.cs b/axios/Engine/UI/AxiosUIObject.cs new file mode 100644 index 0000000..f874a46 --- /dev/null +++ b/axios/Engine/UI/AxiosUIObject.cs @@ -0,0 +1,25 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +using Axios.Engine; + +namespace Axios.Engine.UI +{ + public class AxiosUIObject : DrawableAxiosGameObject + { + public int Width + { + get { return this.Texture.Width; } + private set { } + } + + public int Height + { + get { return this.Texture.Height; } + private set {} + } + + } +} diff --git a/axios/Factories/BodyFactory.cs b/axios/Factories/BodyFactory.cs new file mode 100644 index 0000000..3a2b655 --- /dev/null +++ b/axios/Factories/BodyFactory.cs @@ -0,0 +1,399 @@ +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) + { + return CreateBody(world, null); + } + + public static Body CreateBody(World world, object userData) + { + Body body = new Body(world, userData); + return body; + } + + public static Body CreateBody(World world, Vector2 position) + { + return CreateBody(world, position, null); + } + + public static Body CreateBody(World world, Vector2 position, object userData) + { + Body body = CreateBody(world, userData); + body.Position = position; + return body; + } + + public static Body CreateEdge(World world, Vector2 start, Vector2 end) + { + return CreateEdge(world, start, end, null); + } + + public static Body CreateEdge(World world, Vector2 start, Vector2 end, object userData) + { + Body body = CreateBody(world); + FixtureFactory.AttachEdge(start, end, body, userData); + return body; + } + + public static Body CreateLoopShape(World world, Vertices vertices) + { + return CreateLoopShape(world, vertices, null); + } + + public static Body CreateLoopShape(World world, Vertices vertices, object userData) + { + return CreateLoopShape(world, vertices, Vector2.Zero, userData); + } + + public static Body CreateLoopShape(World world, Vertices vertices, Vector2 position) + { + return CreateLoopShape(world, vertices, position, null); + } + + public static Body CreateLoopShape(World world, Vertices vertices, Vector2 position, + object userData) + { + Body body = CreateBody(world, position); + FixtureFactory.AttachLoopShape(vertices, body, userData); + return body; + } + + public static Body CreateRectangle(World world, float width, float height, float density) + { + return CreateRectangle(world, width, height, density, null); + } + + public static Body CreateRectangle(World world, float width, float height, float density, object userData) + { + return CreateRectangle(world, width, height, density, Vector2.Zero, userData); + } + + public static Body CreateRectangle(World world, float width, float height, float density, Vector2 position) + { + return CreateRectangle(world, width, height, density, position, null); + } + + public static Body CreateRectangle(World world, float width, float height, float density, Vector2 position, + object userData) + { + 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, userData); + Vertices rectangleVertices = PolygonTools.CreateRectangle(width / 2, height / 2); + PolygonShape rectangleShape = new PolygonShape(rectangleVertices, density); + newBody.CreateFixture(rectangleShape, userData); + + return newBody; + } + + public static Body CreateCircle(World world, float radius, float density) + { + return CreateCircle(world, radius, density, null); + } + + public static Body CreateCircle(World world, float radius, float density, object userData) + { + return CreateCircle(world, radius, density, Vector2.Zero, userData); + } + + public static Body CreateCircle(World world, float radius, float density, Vector2 position) + { + return CreateCircle(world, radius, density, position, null); + } + + public static Body CreateCircle(World world, float radius, float density, Vector2 position, object userData) + { + 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) + { + return CreateEllipse(world, xRadius, yRadius, edges, density, null); + } + + public static Body CreateEllipse(World world, float xRadius, float yRadius, int edges, float density, + object userData) + { + 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) + { + return CreateEllipse(world, xRadius, yRadius, edges, density, position, null); + } + + public static Body CreateEllipse(World world, float xRadius, float yRadius, int edges, float density, + Vector2 position, object userData) + { + 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) + { + return CreatePolygon(world, vertices, density, null); + } + + public static Body CreatePolygon(World world, Vertices vertices, float density, object userData) + { + return CreatePolygon(world, vertices, density, Vector2.Zero, userData); + } + + public static Body CreatePolygon(World world, Vertices vertices, float density, Vector2 position) + { + return CreatePolygon(world, vertices, density, position, null); + } + + public static Body CreatePolygon(World world, Vertices vertices, float density, Vector2 position, + object userData) + { + Body body = CreateBody(world, position); + FixtureFactory.AttachPolygon(vertices, density, body, userData); + return body; + } + + public static Body CreateCompoundPolygon(World world, List list, float density) + { + return CreateCompoundPolygon(world, list, density, BodyType.Static); + } + + public static Body CreateCompoundPolygon(World world, List list, float density, + object userData) + { + return CreateCompoundPolygon(world, list, density, Vector2.Zero, userData); + } + + public static Body CreateCompoundPolygon(World world, List list, float density, + Vector2 position) + { + return CreateCompoundPolygon(world, list, density, position, null); + } + + public static Body CreateCompoundPolygon(World world, List list, float density, + Vector2 position, object userData) + { + //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) + { + return CreateGear(world, radius, numberOfTeeth, tipPercentage, toothHeight, density, null); + } + + public static Body CreateGear(World world, float radius, int numberOfTeeth, float tipPercentage, + float toothHeight, float density, object userData) + { + Vertices gearPolygon = PolygonTools.CreateGear(radius, numberOfTeeth, tipPercentage, toothHeight); + + //Gears can in some cases be convex + if (!gearPolygon.IsConvex()) + { + //Decompose the gear: + List list = EarclipDecomposer.ConvexPartition(gearPolygon); + + 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) + /// + /// The world. + /// The height. + /// The top radius. + /// The top edges. + /// The bottom radius. + /// The bottom edges. + /// The density. + /// The position. + /// + public static Body CreateCapsule(World world, float height, float topRadius, int topEdges, + float bottomRadius, + int bottomEdges, float density, Vector2 position, object userData) + { + 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 = EarclipDecomposer.ConvexPartition(verts); + 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 topRadius, int topEdges, + float bottomRadius, + int bottomEdges, float density, Vector2 position) + { + return CreateCapsule(world, height, topRadius, topEdges, bottomRadius, bottomEdges, density, position, null); + } + + public static Body CreateCapsule(World world, float height, float endRadius, float density) + { + return CreateCapsule(world, height, endRadius, density, null); + } + + public static Body CreateCapsule(World world, float height, float endRadius, float density, + object userData) + { + //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); + + //Create the two circles + CircleShape topCircle = new CircleShape(endRadius, density); + topCircle.Position = new Vector2(0, height / 2); + body.CreateFixture(topCircle, userData); + + CircleShape bottomCircle = new CircleShape(endRadius, density); + bottomCircle.Position = new Vector2(0, -(height / 2)); + body.CreateFixture(bottomCircle, userData); + 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) + { + 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 = EarclipDecomposer.ConvexPartition(verts); + 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, Vector2 position) + { + return CreateRoundedRectangle(world, width, height, xRadius, yRadius, segments, density, position, null); + } + + public static Body CreateRoundedRectangle(World world, float width, float height, float xRadius, + float yRadius, + int segments, float density) + { + return CreateRoundedRectangle(world, width, height, xRadius, yRadius, segments, density, null); + } + + public static Body CreateRoundedRectangle(World world, float width, float height, float xRadius, + float yRadius, + int segments, float density, object userData) + { + 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, null); + } + + public static BreakableBody CreateBreakableBody(World world, Vertices vertices, float density, object userData) + { + return CreateBreakableBody(world, vertices, density, Vector2.Zero, userData); + } + + /// + /// 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, + object userData) + { + List triangles = EarclipDecomposer.ConvexPartition(vertices); + + BreakableBody breakableBody = new BreakableBody(triangles, world, density, userData); + breakableBody.MainBody.Position = position; + world.AddBreakableBody(breakableBody); + + return breakableBody; + } + + public static BreakableBody CreateBreakableBody(World world, Vertices vertices, float density, Vector2 position) + { + return CreateBreakableBody(world, vertices, density, position, null); + } + + 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/axios/Factories/FixtureFactory.cs b/axios/Factories/FixtureFactory.cs new file mode 100644 index 0000000..3f58e3e --- /dev/null +++ b/axios/Factories/FixtureFactory.cs @@ -0,0 +1,179 @@ +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) + { + return AttachEdge(start, end, body, null); + } + + public static Fixture AttachEdge(Vector2 start, Vector2 end, Body body, object userData) + { + EdgeShape edgeShape = new EdgeShape(start, end); + return body.CreateFixture(edgeShape, userData); + } + + public static Fixture AttachLoopShape(Vertices vertices, Body body) + { + return AttachLoopShape(vertices, body, null); + } + + public static Fixture AttachLoopShape(Vertices vertices, Body body, object userData) + { + LoopShape shape = new LoopShape(vertices); + return body.CreateFixture(shape, userData); + } + + public static Fixture AttachRectangle(float width, float height, float density, Vector2 offset, Body body, + object userData) + { + 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 AttachRectangle(float width, float height, float density, Vector2 offset, Body body) + { + return AttachRectangle(width, height, density, offset, body, null); + } + + public static Fixture AttachCircle(float radius, float density, Body body) + { + return AttachCircle(radius, density, body, null); + } + + public static Fixture AttachCircle(float radius, float density, Body body, object userData) + { + 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) + { + return AttachCircle(radius, density, body, offset, null); + } + + public static Fixture AttachCircle(float radius, float density, Body body, Vector2 offset, object userData) + { + 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) + { + return AttachPolygon(vertices, density, body, null); + } + + public static Fixture AttachPolygon(Vertices vertices, float density, Body body, object userData) + { + 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) + { + return AttachEllipse(xRadius, yRadius, edges, density, body, null); + } + + public static Fixture AttachEllipse(float xRadius, float yRadius, int edges, float density, Body body, + object userData) + { + 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) + { + return AttachCompoundPolygon(list, density, body, null); + } + + public static List AttachCompoundPolygon(List list, float density, Body body, object userData) + { + 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 List 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); + + List fixtures = new List(arc.Count); + + if (closed) + { + fixtures.Add(AttachLoopShape(arc, body)); + } + + for (int i = 1; i < arc.Count; i++) + { + fixtures.Add(AttachEdge(arc[i], arc[i - 1], body)); + } + + return fixtures; + } + + 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 = EarclipDecomposer.ConvexPartition(arc); + + return AttachCompoundPolygon(triangles, density, body); + } + } +} \ No newline at end of file diff --git a/axios/Factories/JointFactory.cs b/axios/Factories/JointFactory.cs new file mode 100644 index 0000000..c270fb0 --- /dev/null +++ b/axios/Factories/JointFactory.cs @@ -0,0 +1,288 @@ +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 Revolute Joint + + /// + /// Creates a revolute joint. + /// + /// + /// + /// The anchor of bodyB in local coordinates + /// + public static RevoluteJoint CreateRevoluteJoint(Body bodyA, Body bodyB, Vector2 localAnchorB) + { + Vector2 localanchorA = bodyA.GetLocalPoint(bodyB.GetWorldPoint(localAnchorB)); + RevoluteJoint joint = new RevoluteJoint(bodyA, bodyB, localanchorA, localAnchorB); + return joint; + } + + /// + /// Creates a revolute joint and adds it to the world + /// + /// + /// + /// + /// + /// + public static RevoluteJoint CreateRevoluteJoint(World world, Body bodyA, Body bodyB, Vector2 anchor) + { + RevoluteJoint joint = CreateRevoluteJoint(bodyA, bodyB, anchor); + world.AddJoint(joint); + return joint; + } + + /// + /// Creates the fixed revolute joint. + /// + /// The world. + /// The body. + /// The body anchor. + /// The world anchor. + /// + public static FixedRevoluteJoint CreateFixedRevoluteJoint(World world, Body body, Vector2 bodyAnchor, + Vector2 worldAnchor) + { + FixedRevoluteJoint fixedRevoluteJoint = new FixedRevoluteJoint(body, bodyAnchor, worldAnchor); + world.AddJoint(fixedRevoluteJoint); + return fixedRevoluteJoint; + } + + #endregion + + #region Weld Joint + + /// + /// Creates a weld joint + /// + /// + /// + /// + /// + public static WeldJoint CreateWeldJoint(Body bodyA, Body bodyB, Vector2 localAnchor) + { + WeldJoint joint = new WeldJoint(bodyA, bodyB, bodyA.GetLocalPoint(localAnchor), + bodyB.GetLocalPoint(localAnchor)); + return joint; + } + + /// + /// Creates a weld joint and adds it to the world + /// + /// + /// + /// + /// + /// + public static WeldJoint CreateWeldJoint(World world, Body bodyA, Body bodyB, Vector2 localanchorB) + { + WeldJoint joint = CreateWeldJoint(bodyA, bodyB, localanchorB); + world.AddJoint(joint); + return joint; + } + + public static WeldJoint CreateWeldJoint(World world, Body bodyA, Body bodyB, Vector2 localAnchorA, + Vector2 localAnchorB) + { + WeldJoint weldJoint = new WeldJoint(bodyA, bodyB, localAnchorA, localAnchorB); + world.AddJoint(weldJoint); + return weldJoint; + } + + #endregion + + #region Prismatic Joint + + /// + /// Creates a prsimatic joint + /// + /// + /// + /// + /// + /// + public static PrismaticJoint CreatePrismaticJoint(Body bodyA, Body bodyB, Vector2 localanchorB, Vector2 axis) + { + Vector2 localanchorA = bodyA.GetLocalPoint(bodyB.GetWorldPoint(localanchorB)); + PrismaticJoint joint = new PrismaticJoint(bodyA, bodyB, localanchorA, localanchorB, axis); + return joint; + } + + /// + /// Creates a prismatic joint and adds it to the world + /// + /// + /// + /// + /// + /// + /// + public static PrismaticJoint CreatePrismaticJoint(World world, Body bodyA, Body bodyB, Vector2 localanchorB, + Vector2 axis) + { + PrismaticJoint joint = CreatePrismaticJoint(bodyA, bodyB, localanchorB, axis); + world.AddJoint(joint); + return joint; + } + + public static FixedPrismaticJoint CreateFixedPrismaticJoint(World world, Body body, Vector2 worldAnchor, + Vector2 axis) + { + FixedPrismaticJoint joint = new FixedPrismaticJoint(body, worldAnchor, axis); + world.AddJoint(joint); + return joint; + } + + #endregion + + #region Line Joint + + /// + /// Creates a line joint + /// + /// + /// + /// + /// + /// + public static LineJoint CreateLineJoint(Body bodyA, Body bodyB, Vector2 anchor, Vector2 axis) + { + LineJoint joint = new LineJoint(bodyA, bodyB, anchor, axis); + return joint; + } + + /// + /// Creates a line joint and adds it to the world + /// + /// + /// + /// + /// + /// + /// + public static LineJoint CreateLineJoint(World world, Body bodyA, Body bodyB, Vector2 localanchorB, Vector2 axis) + { + LineJoint joint = CreateLineJoint(bodyA, bodyB, localanchorB, axis); + world.AddJoint(joint); + return joint; + } + + #endregion + + #region Angle Joint + + /// + /// Creates an angle joint. + /// + /// The world. + /// The first body. + /// The second body. + /// + public static AngleJoint CreateAngleJoint(World world, Body bodyA, Body bodyB) + { + AngleJoint angleJoint = new AngleJoint(bodyA, bodyB); + world.AddJoint(angleJoint); + + return angleJoint; + } + + /// + /// Creates a fixed angle joint. + /// + /// The world. + /// The body. + /// + public static FixedAngleJoint CreateFixedAngleJoint(World world, Body body) + { + FixedAngleJoint angleJoint = new FixedAngleJoint(body); + world.AddJoint(angleJoint); + + return angleJoint; + } + + #endregion + + #region Distance Joint + + public static DistanceJoint CreateDistanceJoint(World world, Body bodyA, Body bodyB, Vector2 anchorA, + Vector2 anchorB) + { + DistanceJoint distanceJoint = new DistanceJoint(bodyA, bodyB, anchorA, anchorB); + world.AddJoint(distanceJoint); + return distanceJoint; + } + + public static FixedDistanceJoint CreateFixedDistanceJoint(World world, Body body, Vector2 localAnchor, + Vector2 worldAnchor) + { + FixedDistanceJoint distanceJoint = new FixedDistanceJoint(body, localAnchor, worldAnchor); + world.AddJoint(distanceJoint); + return distanceJoint; + } + + #endregion + + #region Friction Joint + + public static FrictionJoint CreateFrictionJoint(World world, Body bodyA, Body bodyB, Vector2 anchorA, + Vector2 anchorB) + { + FrictionJoint frictionJoint = new FrictionJoint(bodyA, bodyB, anchorA, anchorB); + world.AddJoint(frictionJoint); + return frictionJoint; + } + + public static FixedFrictionJoint CreateFixedFrictionJoint(World world, Body body, Vector2 bodyAnchor) + { + FixedFrictionJoint frictionJoint = new FixedFrictionJoint(body, bodyAnchor); + world.AddJoint(frictionJoint); + return frictionJoint; + } + + #endregion + + #region Gear Joint + + public static GearJoint CreateGearJoint(World world, Joint jointA, Joint jointB, float ratio) + { + GearJoint gearJoint = new GearJoint(jointA, jointB, ratio); + world.AddJoint(gearJoint); + return gearJoint; + } + + #endregion + + #region Pulley Joint + + public static PulleyJoint CreatePulleyJoint(World world, Body bodyA, Body bodyB, Vector2 groundAnchorA, + Vector2 groundAnchorB, Vector2 anchorA, Vector2 anchorB, float ratio) + { + PulleyJoint pulleyJoint = new PulleyJoint(bodyA, bodyB, groundAnchorA, groundAnchorB, anchorA, anchorB, + ratio); + world.AddJoint(pulleyJoint); + return pulleyJoint; + } + + #endregion + + #region Slider Joint + + public static SliderJoint CreateSliderJoint(World world, Body bodyA, Body bodyB, Vector2 anchorA, + Vector2 anchorB, float minLength, float maxLength) + { + SliderJoint sliderJoint = new SliderJoint(bodyA, bodyB, anchorA, anchorB, minLength, maxLength); + world.AddJoint(sliderJoint); + return sliderJoint; + } + + #endregion + } +} \ No newline at end of file diff --git a/axios/Factories/LinkFactory.cs b/axios/Factories/LinkFactory.cs new file mode 100644 index 0000000..d5a2ebe --- /dev/null +++ b/axios/Factories/LinkFactory.cs @@ -0,0 +1,62 @@ +using System.Collections.Generic; +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. + /// if set to true [fix start]. + /// if set to true [fix end]. + /// The number of links. + /// The link density. + /// + public static Path CreateChain(World world, Vector2 start, Vector2 end, float linkWidth, float linkHeight, + bool fixStart, bool fixEnd, int numberOfLinks, float linkDensity) + { + //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); + + 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); + + return (path); + } + } +} \ No newline at end of file diff --git a/axios/PrimitiveBatch.cs b/axios/PrimitiveBatch.cs new file mode 100644 index 0000000..62442d7 --- /dev/null +++ b/axios/PrimitiveBatch.cs @@ -0,0 +1,196 @@ +using System; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Graphics; + +namespace FarseerPhysics.DebugViews +{ + public class PrimitiveBatch : IDisposable + { + private const int DefaultBufferSize = 500; + + // a basic effect, which contains the shaders that we will use to draw our + // primitives. + private BasicEffect _basicEffect; + + // the device that we will issue draw calls to. + private GraphicsDevice _device; + + // hasBegun is flipped to true once Begin is called, and is used to make + // sure users don't call End before Begin is called. + private bool _hasBegun; + + private bool _isDisposed; + private VertexPositionColor[] _lineVertices; + private int _lineVertsCount; + private VertexPositionColor[] _triangleVertices; + private int _triangleVertsCount; + + + /// + /// the constructor creates a new PrimitiveBatch and sets up all of the internals + /// that PrimitiveBatch will need. + /// + /// The graphics device. + public PrimitiveBatch(GraphicsDevice graphicsDevice) + : this(graphicsDevice, DefaultBufferSize) + { + } + + public PrimitiveBatch(GraphicsDevice graphicsDevice, int bufferSize) + { + if (graphicsDevice == null) + { + throw new ArgumentNullException("graphicsDevice"); + } + _device = graphicsDevice; + + _triangleVertices = new VertexPositionColor[bufferSize - bufferSize % 3]; + _lineVertices = new VertexPositionColor[bufferSize - bufferSize % 2]; + + // set up a new basic effect, and enable vertex colors. + _basicEffect = new BasicEffect(graphicsDevice); + _basicEffect.VertexColorEnabled = true; + } + + #region IDisposable Members + + public void Dispose() + { + Dispose(true); + GC.SuppressFinalize(this); + } + + #endregion + + public void SetProjection(ref Matrix projection) + { + _basicEffect.Projection = projection; + } + + protected virtual void Dispose(bool disposing) + { + if (disposing && !_isDisposed) + { + if (_basicEffect != null) + _basicEffect.Dispose(); + + _isDisposed = true; + } + } + + + /// + /// Begin is called to tell the PrimitiveBatch what kind of primitives will be + /// drawn, and to prepare the graphics card to render those primitives. + /// + /// The projection. + /// The view. + public void Begin(ref Matrix projection, ref Matrix view) + { + if (_hasBegun) + { + throw new InvalidOperationException("End must be called before Begin can be called again."); + } + + //tell our basic effect to begin. + _basicEffect.Projection = projection; + _basicEffect.View = view; + _basicEffect.CurrentTechnique.Passes[0].Apply(); + + // flip the error checking boolean. It's now ok to call AddVertex, Flush, + // and End. + _hasBegun = true; + } + + public bool IsReady() + { + return _hasBegun; + } + + public void AddVertex(Vector2 vertex, Color color, PrimitiveType primitiveType) + { + if (!_hasBegun) + { + throw new InvalidOperationException("Begin must be called before AddVertex can be called."); + } + if (primitiveType == PrimitiveType.LineStrip || + primitiveType == PrimitiveType.TriangleStrip) + { + throw new NotSupportedException("The specified primitiveType is not supported by PrimitiveBatch."); + } + + if (primitiveType == PrimitiveType.TriangleList) + { + if (_triangleVertsCount >= _triangleVertices.Length) + { + FlushTriangles(); + } + _triangleVertices[_triangleVertsCount].Position = new Vector3(vertex, -0.1f); + _triangleVertices[_triangleVertsCount].Color = color; + _triangleVertsCount++; + } + if (primitiveType == PrimitiveType.LineList) + { + if (_lineVertsCount >= _lineVertices.Length) + { + FlushLines(); + } + _lineVertices[_lineVertsCount].Position = new Vector3(vertex, 0f); + _lineVertices[_lineVertsCount].Color = color; + _lineVertsCount++; + } + } + + + /// + /// End is called once all the primitives have been drawn using AddVertex. + /// it will call Flush to actually submit the draw call to the graphics card, and + /// then tell the basic effect to end. + /// + public void End() + { + if (!_hasBegun) + { + throw new InvalidOperationException("Begin must be called before End can be called."); + } + + // Draw whatever the user wanted us to draw + FlushTriangles(); + FlushLines(); + + _hasBegun = false; + } + + private void FlushTriangles() + { + if (!_hasBegun) + { + throw new InvalidOperationException("Begin must be called before Flush can be called."); + } + if (_triangleVertsCount >= 3) + { + int primitiveCount = _triangleVertsCount / 3; + // submit the draw call to the graphics card + _device.SamplerStates[0] = SamplerState.AnisotropicClamp; + _device.DrawUserPrimitives(PrimitiveType.TriangleList, _triangleVertices, 0, primitiveCount); + _triangleVertsCount -= primitiveCount * 3; + } + } + + private void FlushLines() + { + if (!_hasBegun) + { + throw new InvalidOperationException("Begin must be called before Flush can be called."); + } + if (_lineVertsCount >= 2) + { + int primitiveCount = _lineVertsCount / 2; + // submit the draw call to the graphics card + _device.SamplerStates[0] = SamplerState.AnisotropicClamp; + _device.DrawUserPrimitives(PrimitiveType.LineList, _lineVertices, 0, primitiveCount); + _lineVertsCount -= primitiveCount * 2; + } + } + } +} \ No newline at end of file diff --git a/axios/Properties/AssemblyInfo.cs b/axios/Properties/AssemblyInfo.cs new file mode 100644 index 0000000..849ebe0 --- /dev/null +++ b/axios/Properties/AssemblyInfo.cs @@ -0,0 +1,40 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +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("Axios Engine")] +[assembly: AssemblyProduct("Axios")] +[assembly: AssemblyDescription("XNA 2D Game Engine")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyCopyright("Copyright © 2012 - Nathan Adams")] +[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. Only Windows +// assemblies support COM. +[assembly: ComVisible(false)] + +// On Windows, the following GUID is for the ID of the typelib if this +// project is exposed to COM. On other platforms, it unique identifies the +// title storage container when deploying this assembly to the device. +[assembly: Guid("c5288362-176b-4b45-a378-ae5ce3ce37f7")] + +// Version information for an assembly consists of the following four values: +// +// Major Version +// Minor Version +// Build Number +// Revision +// +[assembly: AssemblyVersion("1.0.0.9")] + +#if DEBUG +[assembly: AssemblyConfiguration("Debug")] +#else +[assembly: AssemblyConfiguration("Release")] +#endif \ No newline at end of file diff --git a/axios/ScreenSystem/BackgroundScreen.cs b/axios/ScreenSystem/BackgroundScreen.cs new file mode 100644 index 0000000..f4903a6 --- /dev/null +++ b/axios/ScreenSystem/BackgroundScreen.cs @@ -0,0 +1,74 @@ +using System; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Graphics; + +namespace FarseerPhysics.SamplesFramework +{ + /// + /// The background screen sits behind all the other menu screens. + /// It draws a background image that remains fixed in place regardless + /// of whatever transitions the screens on top of it may be doing. + /// + public class BackgroundScreen : GameScreen + { + private const float LogoScreenHeightRatio = 0.25f; + private const float LogoScreenBorderRatio = 0.0375f; + private const float LogoWidthHeightRatio = 1.4f; + + private Texture2D _backgroundTexture; + private Rectangle _logoDestination; + //private Texture2D _logoTexture; + private Rectangle _viewport; + + /// + /// Constructor. + /// + public BackgroundScreen() + { + TransitionOnTime = TimeSpan.FromSeconds(0.5); + TransitionOffTime = TimeSpan.FromSeconds(0.5); + } + + public override void LoadContent() + { + //_logoTexture = ScreenManager.Content.Load("Common/logo"); + _backgroundTexture = ScreenManager.Content.Load("Common/gradient"); + + Viewport viewport = ScreenManager.GraphicsDevice.Viewport; + Vector2 logoSize = new Vector2(); + logoSize.Y = viewport.Height * LogoScreenHeightRatio; + logoSize.X = logoSize.Y * LogoWidthHeightRatio; + + float border = viewport.Height * LogoScreenBorderRatio; + Vector2 logoPosition = new Vector2(viewport.Width - border - logoSize.X, + viewport.Height - border - logoSize.Y); + _logoDestination = new Rectangle((int)logoPosition.X, (int)logoPosition.Y, (int)logoSize.X, + (int)logoSize.Y); + _viewport = viewport.Bounds; + } + + /// + /// Updates the background screen. Unlike most screens, this should not + /// transition off even if it has been covered by another screen: it is + /// supposed to be covered, after all! This overload forces the + /// coveredByOtherScreen parameter to false in order to stop the base + /// Update method wanting to transition off. + /// + public override void Update(GameTime gameTime, bool otherScreenHasFocus, + bool coveredByOtherScreen) + { + base.Update(gameTime, otherScreenHasFocus, false); + } + + /// + /// Draws the background screen. + /// + public override void Draw(GameTime gameTime) + { + ScreenManager.SpriteBatch.Begin(); + ScreenManager.SpriteBatch.Draw(_backgroundTexture, _viewport, Color.White); + //ScreenManager.SpriteBatch.Draw(_logoTexture, _logoDestination, Color.White * 0.6f); + ScreenManager.SpriteBatch.End(); + } + } +} \ No newline at end of file diff --git a/axios/ScreenSystem/Camera2D.cs b/axios/ScreenSystem/Camera2D.cs new file mode 100644 index 0000000..8c117ad --- /dev/null +++ b/axios/ScreenSystem/Camera2D.cs @@ -0,0 +1,369 @@ +using System; +using FarseerPhysics.Dynamics; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Graphics; + +namespace FarseerPhysics.SamplesFramework +{ + public class Camera2D + { + private const float _minZoom = 0.02f; + private const float _maxZoom = 20f; + private static GraphicsDevice _graphics; + + private Matrix _batchView; + + private Vector2 _currentPosition; + + private float _currentRotation; + + private float _currentZoom; + private Vector2 _maxPosition; + private float _maxRotation; + private Vector2 _minPosition; + private float _minRotation; + private bool _positionTracking; + private Matrix _projection; + private bool _rotationTracking; + private Vector2 _targetPosition; + private float _targetRotation; + private Body _trackingBody; + private Vector2 _translateCenter; + private Matrix _view; + + /// + /// The constructor for the Camera2D class. + /// + /// + public Camera2D(GraphicsDevice graphics) + { + _graphics = graphics; + _projection = Matrix.CreateOrthographicOffCenter(0f, ConvertUnits.ToSimUnits(_graphics.Viewport.Width), + ConvertUnits.ToSimUnits(_graphics.Viewport.Height), 0f, 0f, + 1f); + _view = Matrix.Identity; + _batchView = Matrix.Identity; + + _translateCenter = new Vector2(ConvertUnits.ToSimUnits(_graphics.Viewport.Width / 2f), + ConvertUnits.ToSimUnits(_graphics.Viewport.Height / 2f)); + + ResetCamera(); + } + + public Matrix View + { + get { return _batchView; } + } + + public Matrix SimView + { + get { return _view; } + } + + public Matrix SimProjection + { + get { return _projection; } + } + + /// + /// The current position of the camera. + /// + public Vector2 Position + { + get { return ConvertUnits.ToDisplayUnits(_currentPosition); } + set + { + _targetPosition = ConvertUnits.ToSimUnits(value); + if (_minPosition != _maxPosition) + { + Vector2.Clamp(ref _targetPosition, ref _minPosition, ref _maxPosition, out _targetPosition); + } + } + } + + /// + /// The furthest up, and the furthest left the camera can go. + /// if this value equals maxPosition, then no clamping will be + /// applied (unless you override that function). + /// + public Vector2 MinPosition + { + get { return ConvertUnits.ToDisplayUnits(_minPosition); } + set { _minPosition = ConvertUnits.ToSimUnits(value); } + } + + /// + /// the furthest down, and the furthest right the camera will go. + /// if this value equals minPosition, then no clamping will be + /// applied (unless you override that function). + /// + public Vector2 MaxPosition + { + get { return ConvertUnits.ToDisplayUnits(_maxPosition); } + set { _maxPosition = ConvertUnits.ToSimUnits(value); } + } + + /// + /// The current rotation of the camera in radians. + /// + public float Rotation + { + get { return _currentRotation; } + set + { + _targetRotation = value % MathHelper.TwoPi; + if (_minRotation != _maxRotation) + { + _targetRotation = MathHelper.Clamp(_targetRotation, _minRotation, _maxRotation); + } + } + } + + /// + /// Gets or sets the minimum rotation in radians. + /// + /// The min rotation. + public float MinRotation + { + get { return _minRotation; } + set { _minRotation = MathHelper.Clamp(value, -MathHelper.Pi, 0f); } + } + + /// + /// Gets or sets the maximum rotation in radians. + /// + /// The max rotation. + public float MaxRotation + { + get { return _maxRotation; } + set { _maxRotation = MathHelper.Clamp(value, 0f, MathHelper.Pi); } + } + + /// + /// The current rotation of the camera in radians. + /// + public float Zoom + { + get { return _currentZoom; } + set + { + _currentZoom = value; + _currentZoom = MathHelper.Clamp(_currentZoom, _minZoom, _maxZoom); + } + } + + /// + /// the body that this camera is currently tracking. + /// Null if not tracking any. + /// + public Body TrackingBody + { + get { return _trackingBody; } + set + { + _trackingBody = value; + if (_trackingBody != null) + { + _positionTracking = true; + } + } + } + + public bool EnablePositionTracking + { + get { return _positionTracking; } + set + { + if (value && _trackingBody != null) + { + _positionTracking = true; + } + else + { + _positionTracking = false; + } + } + } + + public bool EnableRotationTracking + { + get { return _rotationTracking; } + set + { + if (value && _trackingBody != null) + { + _rotationTracking = true; + } + else + { + _rotationTracking = false; + } + } + } + + public bool EnableTracking + { + set + { + EnablePositionTracking = value; + EnableRotationTracking = value; + } + } + + public void MoveCamera(Vector2 amount) + { + _currentPosition += amount; + if (_minPosition != _maxPosition) + { + Vector2.Clamp(ref _currentPosition, ref _minPosition, ref _maxPosition, out _currentPosition); + } + _targetPosition = _currentPosition; + _positionTracking = false; + _rotationTracking = false; + } + + public void RotateCamera(float amount) + { + _currentRotation += amount; + if (_minRotation != _maxRotation) + { + _currentRotation = MathHelper.Clamp(_currentRotation, _minRotation, _maxRotation); + } + _targetRotation = _currentRotation; + _positionTracking = false; + _rotationTracking = false; + } + + /// + /// Resets the camera to default values. + /// + public void ResetCamera() + { + _currentPosition = Vector2.Zero; + _targetPosition = Vector2.Zero; + _minPosition = Vector2.Zero; + _maxPosition = Vector2.Zero; + + _currentRotation = 0f; + _targetRotation = 0f; + _minRotation = -MathHelper.Pi; + _maxRotation = MathHelper.Pi; + + _positionTracking = false; + _rotationTracking = false; + + _currentZoom = 1f; + + SetView(); + } + + public void Jump2Target() + { + _currentPosition = _targetPosition; + _currentRotation = _targetRotation; + + SetView(); + } + + private void SetView() + { + Matrix matRotation = Matrix.CreateRotationZ(_currentRotation); + Matrix matZoom = Matrix.CreateScale(_currentZoom); + Vector3 translateCenter = new Vector3(_translateCenter, 0f); + Vector3 translateBody = new Vector3(-_currentPosition, 0f); + + _view = Matrix.CreateTranslation(translateBody) * + matRotation * + matZoom * + Matrix.CreateTranslation(translateCenter); + + translateCenter = ConvertUnits.ToDisplayUnits(translateCenter); + translateBody = ConvertUnits.ToDisplayUnits(translateBody); + + _batchView = Matrix.CreateTranslation(translateBody) * + matRotation * + matZoom * + Matrix.CreateTranslation(translateCenter); + } + + /// + /// Moves the camera forward one timestep. + /// + public void Update(GameTime gameTime) + { + if (_trackingBody != null) + { + if (_positionTracking) + { + _targetPosition = _trackingBody.Position; + if (_minPosition != _maxPosition) + { + Vector2.Clamp(ref _targetPosition, ref _minPosition, ref _maxPosition, out _targetPosition); + } + } + if (_rotationTracking) + { + _targetRotation = -_trackingBody.Rotation % MathHelper.TwoPi; + if (_minRotation != _maxRotation) + { + _targetRotation = MathHelper.Clamp(_targetRotation, _minRotation, _maxRotation); + } + } + } + Vector2 delta = _targetPosition - _currentPosition; + float distance = delta.Length(); + if (distance > 0f) + { + delta /= distance; + } + float inertia; + if (distance < 10f) + { + inertia = (float) Math.Pow(distance / 10.0, 2.0); + } + else + { + inertia = 1f; + } + + float rotDelta = _targetRotation - _currentRotation; + + float rotInertia; + if (Math.Abs(rotDelta) < 5f) + { + rotInertia = (float) Math.Pow(rotDelta / 5.0, 2.0); + } + else + { + rotInertia = 1f; + } + if (Math.Abs(rotDelta) > 0f) + { + rotDelta /= Math.Abs(rotDelta); + } + + _currentPosition += 100f * delta * inertia * (float) gameTime.ElapsedGameTime.TotalSeconds; + _currentRotation += 80f * rotDelta * rotInertia * (float) gameTime.ElapsedGameTime.TotalSeconds; + + SetView(); + } + + public Vector2 ConvertScreenToWorld(Vector2 location) + { + Vector3 t = new Vector3(location, 0); + + t = _graphics.Viewport.Unproject(t, _projection, _view, Matrix.Identity); + + return new Vector2(t.X, t.Y); + } + + public Vector2 ConvertWorldToScreen(Vector2 location) + { + Vector3 t = new Vector3(location, 0); + + t = _graphics.Viewport.Project(t, _projection, _view, Matrix.Identity); + + return new Vector2(t.X, t.Y); + } + } +} \ No newline at end of file diff --git a/axios/ScreenSystem/ConvertUnits.cs b/axios/ScreenSystem/ConvertUnits.cs new file mode 100644 index 0000000..524fe9d --- /dev/null +++ b/axios/ScreenSystem/ConvertUnits.cs @@ -0,0 +1,103 @@ +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.SamplesFramework +{ + /// + /// 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/axios/ScreenSystem/FramerateCounterComponent.cs b/axios/ScreenSystem/FramerateCounterComponent.cs new file mode 100644 index 0000000..da8b0a0 --- /dev/null +++ b/axios/ScreenSystem/FramerateCounterComponent.cs @@ -0,0 +1,57 @@ +using System; +using System.Globalization; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.SamplesFramework +{ + /// + /// Displays the FPS + /// + public class FrameRateCounter : DrawableGameComponent + { + private TimeSpan _elapsedTime = TimeSpan.Zero; + private NumberFormatInfo _format; + private int _frameCounter; + private int _frameRate; + private Vector2 _position; + private ScreenManager _screenManager; + + public FrameRateCounter(ScreenManager screenManager) + : base(screenManager.Game) + { + _screenManager = screenManager; + _format = new NumberFormatInfo(); + _format.NumberDecimalSeparator = "."; +#if XBOX + _position = new Vector2(55, 35); +#else + _position = new Vector2(30, 25); +#endif + } + + public override void Update(GameTime gameTime) + { + _elapsedTime += gameTime.ElapsedGameTime; + + if (_elapsedTime <= TimeSpan.FromSeconds(1)) return; + + _elapsedTime -= TimeSpan.FromSeconds(1); + _frameRate = _frameCounter; + _frameCounter = 0; + } + + public override void Draw(GameTime gameTime) + { + _frameCounter++; + + string fps = string.Format(_format, "{0} fps", _frameRate); + + _screenManager.SpriteBatch.Begin(); + _screenManager.SpriteBatch.DrawString(_screenManager.Fonts.FrameRateCounterFont, fps, + _position + Vector2.One, Color.Black); + _screenManager.SpriteBatch.DrawString(_screenManager.Fonts.FrameRateCounterFont, fps, + _position, Color.White); + _screenManager.SpriteBatch.End(); + } + } +} \ No newline at end of file diff --git a/axios/ScreenSystem/GameScreen.cs b/axios/ScreenSystem/GameScreen.cs new file mode 100644 index 0000000..54e4ae3 --- /dev/null +++ b/axios/ScreenSystem/GameScreen.cs @@ -0,0 +1,285 @@ +#region File Description + +//----------------------------------------------------------------------------- +// PlayerIndexEventArgs.cs +// +// XNA Community Game Platform +// Copyright (C) Microsoft Corporation. All rights reserved. +//----------------------------------------------------------------------------- + +#endregion + +using System; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Input.Touch; + +namespace FarseerPhysics.SamplesFramework +{ + /// + /// Enum describes the screen transition state. + /// + public enum ScreenState + { + TransitionOn, + Active, + TransitionOff, + Hidden, + } + + /// + /// A screen is a single layer that has update and draw logic, and which + /// can be combined with other layers to build up a complex menu system. + /// For instance the main menu, the options menu, the "are you sure you + /// want to quit" message box, and the main game itself are all implemented + /// as screens. + /// + public abstract class GameScreen + { + private GestureType _enabledGestures = GestureType.None; + private bool _otherScreenHasFocus; + + public GameScreen() + { + ScreenState = ScreenState.TransitionOn; + TransitionPosition = 1; + TransitionOffTime = TimeSpan.Zero; + TransitionOnTime = TimeSpan.Zero; + HasCursor = false; + HasVirtualStick = false; + } + + public bool HasCursor { get; set; } + + public bool HasVirtualStick { get; set; } + + /// + /// Normally when one screen is brought up over the top of another, + /// the first screen will transition off to make room for the new + /// one. This property indicates whether the screen is only a small + /// popup, in which case screens underneath it do not need to bother + /// transitioning off. + /// + public bool IsPopup { get; protected set; } + + /// + /// Indicates how long the screen takes to + /// transition on when it is activated. + /// + public TimeSpan TransitionOnTime { get; protected set; } + + /// + /// Indicates how long the screen takes to + /// transition off when it is deactivated. + /// + public TimeSpan TransitionOffTime { get; protected set; } + + /// + /// Gets the current position of the screen transition, ranging + /// from zero (fully active, no transition) to one (transitioned + /// fully off to nothing). + /// + public float TransitionPosition { get; protected set; } + + /// + /// Gets the current alpha of the screen transition, ranging + /// from 1 (fully active, no transition) to 0 (transitioned + /// fully off to nothing). + /// + public float TransitionAlpha + { + get { return 1f - TransitionPosition; } + } + + /// + /// Gets the current screen transition state. + /// + public ScreenState ScreenState { get; protected set; } + + /// + /// There are two possible reasons why a screen might be transitioning + /// off. It could be temporarily going away to make room for another + /// screen that is on top of it, or it could be going away for good. + /// This property indicates whether the screen is exiting for real: + /// if set, the screen will automatically remove itself as soon as the + /// transition finishes. + /// + public bool IsExiting { get; protected internal set; } + + public bool AlwaysHasFocus = false; + + /// + /// Checks whether this screen is active and can respond to user input. + /// + public bool IsActive + { + get + { + return !_otherScreenHasFocus && + (ScreenState == ScreenState.TransitionOn || + ScreenState == ScreenState.Active); + } + } + + /// + /// Gets the manager that this screen belongs to. + /// + public ScreenManager ScreenManager { get; internal set; } + + /// + /// Gets the gestures the screen is interested in. Screens should be as specific + /// as possible with gestures to increase the accuracy of the gesture engine. + /// For example, most menus only need Tap or perhaps Tap and VerticalDrag to operate. + /// These gestures are handled by the ScreenManager when screens change and + /// all gestures are placed in the InputState passed to the HandleInput method. + /// + public GestureType EnabledGestures + { + get { return _enabledGestures; } + protected set + { + _enabledGestures = value; + + // the screen manager handles this during screen changes, but + // if this screen is active and the gesture types are changing, + // we have to update the TouchPanel ourself. + if (ScreenState == ScreenState.Active) + { + TouchPanel.EnabledGestures = value; + } + } + } + + /// + /// Load graphics content for the screen. + /// + public virtual void LoadContent() + { + } + + /// + /// Unload content for the screen. + /// + public virtual void UnloadContent() + { + } + + /// + /// Allows the screen to run logic, such as updating the transition position. + /// Unlike HandleInput, this method is called regardless of whether the screen + /// is active, hidden, or in the middle of a transition. + /// + public virtual void Update(GameTime gameTime, bool otherScreenHasFocus, + bool coveredByOtherScreen) + { + _otherScreenHasFocus = otherScreenHasFocus; + + if (IsExiting) + { + // If the screen is going away to die, it should transition off. + ScreenState = ScreenState.TransitionOff; + + if (!UpdateTransition(gameTime, TransitionOffTime, 1)) + { + // When the transition finishes, remove the screen. + ScreenManager.RemoveScreen(this); + } + } + else if (coveredByOtherScreen) + { + // If the screen is covered by another, it should transition off. + if (UpdateTransition(gameTime, TransitionOffTime, 1)) + { + // Still busy transitioning. + ScreenState = ScreenState.TransitionOff; + } + else + { + // Transition finished! + ScreenState = ScreenState.Hidden; + } + } + else + { + // Otherwise the screen should transition on and become active. + if (UpdateTransition(gameTime, TransitionOnTime, -1)) + { + // Still busy transitioning. + ScreenState = ScreenState.TransitionOn; + } + else + { + // Transition finished! + ScreenState = ScreenState.Active; + } + } + } + + /// + /// Helper for updating the screen transition position. + /// + private bool UpdateTransition(GameTime gameTime, TimeSpan time, int direction) + { + // How much should we move by? + float transitionDelta; + + if (time == TimeSpan.Zero) + { + transitionDelta = 1f; + } + else + { + transitionDelta = (float)(gameTime.ElapsedGameTime.TotalMilliseconds / + time.TotalMilliseconds); + } + + // Update the transition position. + TransitionPosition += transitionDelta * direction; + + // Did we reach the end of the transition? + if (((direction < 0) && (TransitionPosition <= 0)) || + ((direction > 0) && (TransitionPosition >= 1))) + { + TransitionPosition = MathHelper.Clamp(TransitionPosition, 0, 1); + return false; + } + + // Otherwise we are still busy transitioning. + return true; + } + + /// + /// Allows the screen to handle user input. Unlike Update, this method + /// is only called when the screen is active, and not when some other + /// screen has taken the focus. + /// + public virtual void HandleInput(InputHelper input, GameTime gameTime) + { + } + + /// + /// This is called when the screen should draw itself. + /// + public virtual void Draw(GameTime gameTime) + { + } + + /// + /// Tells the screen to go away. Unlike ScreenManager.RemoveScreen, which + /// instantly kills the screen, this method respects the transition timings + /// and will give the screen a chance to gradually transition off. + /// + public virtual void ExitScreen() + { + if (TransitionOffTime == TimeSpan.Zero) + { + // If the screen has a zero transition time, remove it immediately. + ScreenManager.RemoveScreen(this); + } + else + { + // Otherwise flag that it should transition off and then exit. + IsExiting = true; + } + } + } +} \ No newline at end of file diff --git a/axios/ScreenSystem/IDemoScreen.cs b/axios/ScreenSystem/IDemoScreen.cs new file mode 100644 index 0000000..baeb18c --- /dev/null +++ b/axios/ScreenSystem/IDemoScreen.cs @@ -0,0 +1,8 @@ +namespace FarseerPhysics.SamplesFramework +{ + public interface IDemoScreen + { + string GetTitle(); + string GetDetails(); + } +} \ No newline at end of file diff --git a/axios/ScreenSystem/InputHelper.cs b/axios/ScreenSystem/InputHelper.cs new file mode 100644 index 0000000..c3d4e1c --- /dev/null +++ b/axios/ScreenSystem/InputHelper.cs @@ -0,0 +1,469 @@ +using System.Collections.Generic; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Graphics; +using Microsoft.Xna.Framework.Input; +using Microsoft.Xna.Framework.Input.Touch; + +namespace FarseerPhysics.SamplesFramework +{ + /// + /// an enum of all available mouse buttons. + /// + public enum MouseButtons + { + LeftButton, + MiddleButton, + RightButton, + ExtraButton1, + ExtraButton2 + } + + public class InputHelper + { + private readonly List _gestures = new List(); + private GamePadState _currentGamePadState; + private KeyboardState _currentKeyboardState; + private MouseState _currentMouseState; + private GamePadState _currentVirtualState; + + private GamePadState _lastGamePadState; + private KeyboardState _lastKeyboardState; + private MouseState _lastMouseState; + private GamePadState _lastVirtualState; + private bool _handleVirtualStick; + + private Vector2 _cursor; + private bool _cursorIsValid; + private bool _cursorIsVisible; + private bool _cursorMoved; + private Sprite _cursorSprite; + +#if WINDOWS_PHONE + private VirtualStick _phoneStick; + private VirtualButton _phoneA; + private VirtualButton _phoneB; +#endif + + private ScreenManager _manager; + private Viewport _viewport; + + /// + /// Constructs a new input state. + /// + public InputHelper(ScreenManager manager) + { + _currentKeyboardState = new KeyboardState(); + _currentGamePadState = new GamePadState(); + _currentMouseState = new MouseState(); + _currentVirtualState = new GamePadState(); + + _lastKeyboardState = new KeyboardState(); + _lastGamePadState = new GamePadState(); + _lastMouseState = new MouseState(); + _lastVirtualState = new GamePadState(); + + _manager = manager; + + _cursorIsVisible = false; + _cursorMoved = false; +#if WINDOWS_PHONE + _cursorIsValid = false; +#else + _cursorIsValid = true; +#endif + _cursor = Vector2.Zero; + + _handleVirtualStick = false; + } + + public GamePadState GamePadState + { + get { return _currentGamePadState; } + } + + public KeyboardState KeyboardState + { + get { return _currentKeyboardState; } + } + + public MouseState MouseState + { + get { return _currentMouseState; } + } + + public GamePadState VirtualState + { + get { return _currentVirtualState; } + } + + public GamePadState PreviousGamePadState + { + get { return _lastGamePadState; } + } + + public KeyboardState PreviousKeyboardState + { + get { return _lastKeyboardState; } + } + + public MouseState PreviousMouseState + { + get { return _lastMouseState; } + } + + public GamePadState PreviousVirtualState + { + get { return _lastVirtualState; } + } + + public bool ShowCursor + { + get { return _cursorIsVisible && _cursorIsValid; } + set { _cursorIsVisible = value; } + } + + public bool EnableVirtualStick + { + get { return _handleVirtualStick; } + set { _handleVirtualStick = value; } + } + + public Vector2 Cursor + { + get { return _cursor; } + } + + public bool IsCursorMoved + { + get { return _cursorMoved; } + } + + public bool IsCursorValid + { + get { return _cursorIsValid; } + } + + public void LoadContent() + { + _cursorSprite = new Sprite(_manager.Content.Load("Common/cursor")); +#if WINDOWS_PHONE + // virtual stick content + _phoneStick = new VirtualStick(_manager.Content.Load("Common/socket"), + _manager.Content.Load("Common/stick"), new Vector2(80f, 400f)); + + Texture2D temp = _manager.Content.Load("Common/buttons"); + _phoneA = new VirtualButton(temp, new Vector2(695f, 380f), new Rectangle(0, 0, 40, 40), new Rectangle(0, 40, 40, 40)); + _phoneB = new VirtualButton(temp, new Vector2(745f, 360f), new Rectangle(40, 0, 40, 40), new Rectangle(40, 40, 40, 40)); +#endif + _viewport = _manager.GraphicsDevice.Viewport; + } + + /// + /// Reads the latest state of the keyboard and gamepad and mouse/touchpad. + /// + public void Update(GameTime gameTime) + { + _lastKeyboardState = _currentKeyboardState; + _lastGamePadState = _currentGamePadState; + _lastMouseState = _currentMouseState; + if (_handleVirtualStick) + { + _lastVirtualState = _currentVirtualState; + } + + _currentKeyboardState = Keyboard.GetState(); + _currentGamePadState = GamePad.GetState(PlayerIndex.One); + _currentMouseState = Mouse.GetState(); + + if (_handleVirtualStick) + { +#if XBOX + _currentVirtualState= GamePad.GetState(PlayerIndex.One); +#elif WINDOWS + if (GamePad.GetState(PlayerIndex.One).IsConnected) + { + _currentVirtualState = GamePad.GetState(PlayerIndex.One); + } + else + { + _currentVirtualState = HandleVirtualStickWin(); + } +#elif WINDOWS_PHONE + _currentVirtualState = HandleVirtualStickWP7(); +#endif + } + + _gestures.Clear(); + while (TouchPanel.IsGestureAvailable) + { + _gestures.Add(TouchPanel.ReadGesture()); + } + + // Update cursor + Vector2 oldCursor = _cursor; + if (_currentGamePadState.IsConnected && _currentGamePadState.ThumbSticks.Left != Vector2.Zero) + { + Vector2 temp = _currentGamePadState.ThumbSticks.Left; + _cursor += temp * new Vector2(300f, -300f) * (float)gameTime.ElapsedGameTime.TotalSeconds; + Mouse.SetPosition((int)_cursor.X, (int)_cursor.Y); + } + else + { + _cursor.X = _currentMouseState.X; + _cursor.Y = _currentMouseState.Y; + } + _cursor.X = MathHelper.Clamp(_cursor.X, 0f, _viewport.Width); + _cursor.Y = MathHelper.Clamp(_cursor.Y, 0f, _viewport.Height); + + if (_cursorIsValid && oldCursor != _cursor) + { + _cursorMoved = true; + } + else + { + _cursorMoved = false; + } + +#if WINDOWS + if (_viewport.Bounds.Contains(_currentMouseState.X, _currentMouseState.Y)) + { + _cursorIsValid = true; + } + else + { + _cursorIsValid = false; + } +#elif WINDOWS_PHONE + if (_currentMouseState.LeftButton == ButtonState.Pressed) + { + _cursorIsValid = true; + } + else + { + _cursorIsValid = false; + } +#endif + } + + public void Draw() + { + if (_cursorIsVisible && _cursorIsValid) + { + _manager.SpriteBatch.Begin(); + _manager.SpriteBatch.Draw(_cursorSprite.Texture, _cursor, null, Color.White, 0f, _cursorSprite.Origin, 1f, SpriteEffects.None, 0f); + _manager.SpriteBatch.End(); + } +#if WINDOWS_PHONE + if (_handleVirtualStick) + { + _manager.SpriteBatch.Begin(); + _phoneA.Draw(_manager.SpriteBatch); + _phoneB.Draw(_manager.SpriteBatch); + _phoneStick.Draw(_manager.SpriteBatch); + _manager.SpriteBatch.End(); + } +#endif + } + + private GamePadState HandleVirtualStickWin() + { + Vector2 _leftStick = Vector2.Zero; + List _buttons = new List(); + + if (_currentKeyboardState.IsKeyDown(Keys.A)) + { + _leftStick.X -= 1f; + } + if (_currentKeyboardState.IsKeyDown(Keys.S)) + { + _leftStick.Y -= 1f; + } + if (_currentKeyboardState.IsKeyDown(Keys.D)) + { + _leftStick.X += 1f; + } + if (_currentKeyboardState.IsKeyDown(Keys.W)) + { + _leftStick.Y += 1f; + } + if (_currentKeyboardState.IsKeyDown(Keys.Space)) + { + _buttons.Add(Buttons.A); + } + if (_currentKeyboardState.IsKeyDown(Keys.LeftControl)) + { + _buttons.Add(Buttons.B); + } + if (_leftStick != Vector2.Zero) + { + _leftStick.Normalize(); + } + + return new GamePadState(_leftStick, Vector2.Zero, 0f, 0f, _buttons.ToArray()); + } + + private GamePadState HandleVirtualStickWP7() + { + List _buttons = new List(); + Vector2 _stick = Vector2.Zero; +#if WINDOWS_PHONE + _phoneA.Pressed = false; + _phoneB.Pressed = false; + TouchCollection touchLocations = TouchPanel.GetState(); + foreach (TouchLocation touchLocation in touchLocations) + { + _phoneA.Update(touchLocation); + _phoneB.Update(touchLocation); + _phoneStick.Update(touchLocation); + } + if (_phoneA.Pressed) + { + _buttons.Add(Buttons.A); + } + if (_phoneB.Pressed) + { + _buttons.Add(Buttons.B); + } + _stick = _phoneStick.StickPosition; +#endif + return new GamePadState(_stick, Vector2.Zero, 0f, 0f, _buttons.ToArray()); + } + + /// + /// Helper for checking if a key was newly pressed during this update. + /// + public bool IsNewKeyPress(Keys key) + { + return (_currentKeyboardState.IsKeyDown(key) && + _lastKeyboardState.IsKeyUp(key)); + } + + public bool IsNewKeyRelease(Keys key) + { + return (_lastKeyboardState.IsKeyDown(key) && + _currentKeyboardState.IsKeyUp(key)); + } + + public bool IsNewVirtualButtonPress(Buttons button) + { + return (_lastVirtualState.IsButtonUp(button) && + _currentVirtualState.IsButtonDown(button)); + } + + public bool IsNewVirtualButtonRelease(Buttons button) + { + return (_lastVirtualState.IsButtonDown(button) && + _currentVirtualState.IsButtonUp(button)); + } + + /// + /// Helper for checking if a button was newly pressed during this update. + /// + public bool IsNewButtonPress(Buttons button) + { + return (_currentGamePadState.IsButtonDown(button) && + _lastGamePadState.IsButtonUp(button)); + } + + public bool IsNewButtonRelease(Buttons button) + { + return (_lastGamePadState.IsButtonDown(button) && + _currentGamePadState.IsButtonUp(button)); + } + + /// + /// Helper for checking if a mouse button was newly pressed during this update. + /// + public bool IsNewMouseButtonPress(MouseButtons button) + { + switch (button) + { + case MouseButtons.LeftButton: + return (_currentMouseState.LeftButton == ButtonState.Pressed && + _lastMouseState.LeftButton == ButtonState.Released); + case MouseButtons.RightButton: + return (_currentMouseState.RightButton == ButtonState.Pressed && + _lastMouseState.RightButton == ButtonState.Released); + case MouseButtons.MiddleButton: + return (_currentMouseState.MiddleButton == ButtonState.Pressed && + _lastMouseState.MiddleButton == ButtonState.Released); + case MouseButtons.ExtraButton1: + return (_currentMouseState.XButton1 == ButtonState.Pressed && + _lastMouseState.XButton1 == ButtonState.Released); + case MouseButtons.ExtraButton2: + return (_currentMouseState.XButton2 == ButtonState.Pressed && + _lastMouseState.XButton2 == ButtonState.Released); + default: + return false; + } + } + + + /// + /// Checks if the requested mouse button is released. + /// + /// The button. + public bool IsNewMouseButtonRelease(MouseButtons button) + { + switch (button) + { + case MouseButtons.LeftButton: + return (_lastMouseState.LeftButton == ButtonState.Pressed && + _currentMouseState.LeftButton == ButtonState.Released); + case MouseButtons.RightButton: + return (_lastMouseState.RightButton == ButtonState.Pressed && + _currentMouseState.RightButton == ButtonState.Released); + case MouseButtons.MiddleButton: + return (_lastMouseState.MiddleButton == ButtonState.Pressed && + _currentMouseState.MiddleButton == ButtonState.Released); + case MouseButtons.ExtraButton1: + return (_lastMouseState.XButton1 == ButtonState.Pressed && + _currentMouseState.XButton1 == ButtonState.Released); + case MouseButtons.ExtraButton2: + return (_lastMouseState.XButton2 == ButtonState.Pressed && + _currentMouseState.XButton2 == ButtonState.Released); + default: + return false; + } + } + + /// + /// Checks for a "menu select" input action. + /// + public bool IsMenuSelect() + { + return IsNewKeyPress(Keys.Space) || + IsNewKeyPress(Keys.Enter) || + IsNewButtonPress(Buttons.A) || + IsNewButtonPress(Buttons.Start) || + IsNewMouseButtonPress(MouseButtons.LeftButton); + } + + public bool IsMenuPressed() + { + return _currentKeyboardState.IsKeyDown(Keys.Space) || + _currentKeyboardState.IsKeyDown(Keys.Enter) || + _currentGamePadState.IsButtonDown(Buttons.A) || + _currentGamePadState.IsButtonDown(Buttons.Start) || + _currentMouseState.LeftButton == ButtonState.Pressed; + } + + public bool IsMenuReleased() + { + return IsNewKeyRelease(Keys.Space) || + IsNewKeyRelease(Keys.Enter) || + IsNewButtonRelease(Buttons.A) || + IsNewButtonRelease(Buttons.Start) || + IsNewMouseButtonRelease(MouseButtons.LeftButton); + } + + /// + /// Checks for a "menu cancel" input action. + /// + public bool IsMenuCancel() + { + return IsNewKeyPress(Keys.Escape) || + IsNewButtonPress(Buttons.Back); + } + } +} \ No newline at end of file diff --git a/axios/ScreenSystem/LogoScreen.cs b/axios/ScreenSystem/LogoScreen.cs new file mode 100644 index 0000000..f78f3ec --- /dev/null +++ b/axios/ScreenSystem/LogoScreen.cs @@ -0,0 +1,89 @@ +using System; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Content; +using Microsoft.Xna.Framework.Graphics; +using Microsoft.Xna.Framework.Input; + +namespace FarseerPhysics.SamplesFramework +{ + public class LogoScreen : GameScreen + { + private const float LogoScreenHeightRatio = 4f / 6f; + private const float LogoWidthHeightRatio = 1.4f; + + private ContentManager _content; + private Rectangle _destination; + private TimeSpan _duration; + private Texture2D _farseerLogoTexture; + + public LogoScreen(TimeSpan duration) + { + _duration = duration; + TransitionOffTime = TimeSpan.FromSeconds(2.0); + } + + /// + /// Loads graphics content for this screen. The background texture is quite + /// big, so we use our own local ContentManager to load it. This allows us + /// to unload before going from the menus into the game itself, wheras if we + /// used the shared ContentManager provided by the Game class, the content + /// would remain loaded forever. + /// + public override void LoadContent() + { + if (_content == null) + { + _content = new ContentManager(ScreenManager.Game.Services, "Content"); + } + + _farseerLogoTexture = _content.Load("Common/logo"); + + Viewport viewport = ScreenManager.GraphicsDevice.Viewport; + int rectHeight = (int)(viewport.Height * LogoScreenHeightRatio); + int rectWidth = (int)(rectHeight * LogoWidthHeightRatio); + int posX = viewport.Bounds.Center.X - rectWidth / 2; + int posY = viewport.Bounds.Center.Y - rectHeight / 2; + + _destination = new Rectangle(posX, posY, rectWidth, rectHeight); + } + + /// + /// Unloads graphics content for this screen. + /// + public override void UnloadContent() + { + _content.Unload(); + } + + public override void HandleInput(InputHelper input, GameTime gameTime) + { + if (input.KeyboardState.GetPressedKeys().Length > 0 || + input.GamePadState.IsButtonDown(Buttons.A | Buttons.Start | Buttons.Back) || + input.MouseState.LeftButton == ButtonState.Pressed) + { + _duration = TimeSpan.Zero; + } + } + + public override void Update(GameTime gameTime, bool otherScreenHasFocus, + bool coveredByOtherScreen) + { + _duration -= gameTime.ElapsedGameTime; + if (_duration <= TimeSpan.Zero) + { + ExitScreen(); + } + + base.Update(gameTime, otherScreenHasFocus, coveredByOtherScreen); + } + + public override void Draw(GameTime gameTime) + { + ScreenManager.GraphicsDevice.Clear(Color.White); + + ScreenManager.SpriteBatch.Begin(); + ScreenManager.SpriteBatch.Draw(_farseerLogoTexture, _destination, Color.White); + ScreenManager.SpriteBatch.End(); + } + } +} \ No newline at end of file diff --git a/axios/ScreenSystem/MenuButton.cs b/axios/ScreenSystem/MenuButton.cs new file mode 100644 index 0000000..67e8df4 --- /dev/null +++ b/axios/ScreenSystem/MenuButton.cs @@ -0,0 +1,118 @@ +using System; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Graphics; + +namespace FarseerPhysics.SamplesFramework +{ + /// + /// Helper class represents a single entry in a MenuScreen. By default this + /// just draws the entry text string, but it can be customized to display menu + /// entries in different ways. This also provides an event that will be raised + /// when the menu entry is selected. + /// + public sealed class MenuButton + { + private Vector2 _baseOrigin; + private bool _flip; + private bool _hover; + + /// + /// The position at which the entry is drawn. This is set by the MenuScreen + /// each frame in Update. + /// + private Vector2 _position; + + private float _scale; + private GameScreen _screen; + + /// + /// Tracks a fading selection effect on the entry. + /// + /// + /// The entries transition out of the selection effect when they are deselected. + /// + private float _selectionFade; + + private Texture2D _sprite; + + /// + /// Constructs a new menu entry with the specified text. + /// + public MenuButton(Texture2D sprite, bool flip, Vector2 position, GameScreen screen) + { + _screen = screen; + _scale = 1f; + _sprite = sprite; + _baseOrigin = new Vector2(_sprite.Width / 2f, _sprite.Height / 2f); + _hover = false; + _flip = flip; + Position = position; + } + + /// + /// Gets or sets the position at which to draw this menu entry. + /// + public Vector2 Position + { + get { return _position; } + set { _position = value; } + } + + public bool Hover + { + get { return _hover; } + set { _hover = value; } + } + + public GameScreen Screen + { + get { return _screen; } + } + + /// + /// Updates the menu entry. + /// + public void Update(GameTime gameTime) + { + float fadeSpeed = (float)gameTime.ElapsedGameTime.TotalSeconds * 4; + if (_hover) + { + _selectionFade = Math.Min(_selectionFade + fadeSpeed, 1f); + } + else + { + _selectionFade = Math.Max(_selectionFade - fadeSpeed, 0f); + } + _scale = 1f + 0.1f * _selectionFade; + } + + public void Collide(Vector2 position) + { + Rectangle collisonBox = new Rectangle((int)(Position.X - _sprite.Width / 2f), + (int)(Position.Y - _sprite.Height / 2f), + (_sprite.Width), + (_sprite.Height)); + + if (collisonBox.Contains((int)position.X, (int)position.Y)) + { + _hover = true; + } + else + { + _hover = false; + } + } + + /// + /// Draws the menu entry. This can be overridden to customize the appearance. + /// + public void Draw() + { + SpriteBatch batch = _screen.ScreenManager.SpriteBatch; + Color color = Color.Lerp(Color.White, new Color(255, 210, 0), _selectionFade); + + batch.Draw(_sprite, _position - _baseOrigin * _scale, null, color, 0f, Vector2.Zero, + _scale, _flip ? SpriteEffects.FlipVertically : SpriteEffects.None, 0f); + } + } +} \ No newline at end of file diff --git a/axios/ScreenSystem/MenuEntry.cs b/axios/ScreenSystem/MenuEntry.cs new file mode 100644 index 0000000..1f3efc2 --- /dev/null +++ b/axios/ScreenSystem/MenuEntry.cs @@ -0,0 +1,193 @@ +using System; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Graphics; + +namespace FarseerPhysics.SamplesFramework +{ + public enum EntryType + { + Screen, + Separator, + ExitItem, + BackItem + } + + /// + /// Helper class represents a single entry in a MenuScreen. By default this + /// just draws the entry text string, but it can be customized to display menu + /// entries in different ways. This also provides an event that will be raised + /// when the menu entry is selected. + /// + public sealed class MenuEntry + { + private float _alpha; + private Vector2 _baseOrigin; + + private float _height; + private MenuScreen _menu; + + /// + /// The position at which the entry is drawn. This is set by the MenuScreen + /// each frame in Update. + /// + private Vector2 _position; + + private float _scale; + private GameScreen _screen; + + /// + /// Tracks a fading selection effect on the entry. + /// + /// + /// The entries transition out of the selection effect when they are deselected. + /// + private float _selectionFade; + + /// + /// The text rendered for this entry. + /// + private string _text; + + private EntryType _type; + private float _width; + + /// + /// Constructs a new menu entry with the specified text. + /// + public MenuEntry(MenuScreen menu, string text, EntryType type, GameScreen screen) + { + _text = text; + _screen = screen; + _type = type; + _menu = menu; + _scale = 0.9f; + _alpha = 1.0f; + } + + + /// + /// Gets or sets the text of this menu entry. + /// + public string Text + { + get { return _text; } + set { _text = value; } + } + + /// + /// Gets or sets the position at which to draw this menu entry. + /// + public Vector2 Position + { + get { return _position; } + set { _position = value; } + } + + public float Alpha + { + get { return _alpha; } + set { _alpha = value; } + } + + public GameScreen Screen + { + get { return _screen; } + } + + public void Initialize() + { + SpriteFont font = _menu.ScreenManager.Fonts.MenuSpriteFont; + + _baseOrigin = new Vector2(font.MeasureString(Text).X, font.MeasureString("M").Y) * 0.5f; + + _width = font.MeasureString(Text).X * 0.8f; + _height = font.MeasureString("M").Y * 0.8f; + } + + public bool IsExitItem() + { + return _type == EntryType.ExitItem; + } + + public bool IsSelectable() + { + return _type != EntryType.Separator; + } + + public bool IsBackItem() + { + return _type == EntryType.BackItem; + } + + /// + /// Updates the menu entry. + /// + public void Update(bool isSelected, GameTime gameTime) + { + // there is no such thing as a selected item on Windows Phone, so we always + // force isSelected to be false +#if WINDOWS_PHONE + isSelected = false; +#endif + // When the menu selection changes, entries gradually fade between + // their selected and deselected appearance, rather than instantly + // popping to the new state. + if (_type != EntryType.Separator) + { + float fadeSpeed = (float)gameTime.ElapsedGameTime.TotalSeconds * 4; + if (isSelected) + { + _selectionFade = Math.Min(_selectionFade + fadeSpeed, 1f); + } + else + { + _selectionFade = Math.Max(_selectionFade - fadeSpeed, 0f); + } + _scale = 0.7f + 0.1f * _selectionFade; + } + } + + /// + /// Draws the menu entry. This can be overridden to customize the appearance. + /// + public void Draw() + { + SpriteFont font = _menu.ScreenManager.Fonts.MenuSpriteFont; + SpriteBatch batch = _menu.ScreenManager.SpriteBatch; + + Color color; + if (_type == EntryType.Separator) + { + color = Color.DarkOrange; + } + else + { + // Draw the selected entry in yellow, otherwise white + color = Color.Lerp(Color.White, new Color(255, 210, 0), _selectionFade); + } + color *= _alpha; + + // Draw text, centered on the middle of each line. + batch.DrawString(font, _text, _position - _baseOrigin * _scale + Vector2.One, + Color.DarkSlateGray * _alpha * _alpha, 0, Vector2.Zero, _scale, SpriteEffects.None, 0); + batch.DrawString(font, _text, _position - _baseOrigin * _scale, color, 0, Vector2.Zero, _scale, + SpriteEffects.None, 0); + } + + /// + /// Queries how much space this menu entry requires. + /// + public int GetHeight() + { + return (int)_height; + } + + /// + /// Queries how wide the entry is, used for centering on the screen. + /// + public int GetWidth() + { + return (int)_width; + } + } +} \ No newline at end of file diff --git a/axios/ScreenSystem/MenuScreen.cs b/axios/ScreenSystem/MenuScreen.cs new file mode 100644 index 0000000..6cf1bc5 --- /dev/null +++ b/axios/ScreenSystem/MenuScreen.cs @@ -0,0 +1,338 @@ +using System; +using System.Collections.Generic; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Graphics; + +namespace FarseerPhysics.SamplesFramework +{ + /// + /// Base class for screens that contain a menu of options. The user can + /// move up and down to select an entry, or cancel to back out of the screen. + /// + public class MenuScreen : GameScreen + { +#if WINDOWS || XBOX + protected const float NumEntries = 15; +#elif WINDOWS_PHONE + protected const float NumEntries = 9; +#endif + protected List _menuEntries = new List(); + protected string _menuTitle; + protected Vector2 _titlePosition; + protected Vector2 _titleOrigin; + protected int _selectedEntry; + protected float _menuBorderTop; + protected float _menuBorderBottom; + protected float _menuBorderMargin; + protected float _menuOffset; + protected float _maxOffset; + + protected Texture2D _texScrollButton; + protected Texture2D _texSlider; + + protected MenuButton _scrollUp; + protected MenuButton _scrollDown; + protected MenuButton _scrollSlider; + protected bool _scrollLock; + + /// + /// Constructor. + /// + public MenuScreen(string menuTitle) + { + _menuTitle = menuTitle; + + TransitionOnTime = TimeSpan.FromSeconds(0.7); + TransitionOffTime = TimeSpan.FromSeconds(0.7); + HasCursor = true; + } + + public void AddMenuItem(string name, EntryType type, GameScreen screen) + { + MenuEntry entry = new MenuEntry(this, name, type, screen); + _menuEntries.Add(entry); + } + + public void AddMenuItem(MenuEntry me) + { + _menuEntries.Add(me); + } + + public override void LoadContent() + { + base.LoadContent(); + + _texScrollButton = ScreenManager.Content.Load("Common/arrow"); + _texSlider = ScreenManager.Content.Load("Common/slider"); + + //Viewport viewport = ScreenManager.GraphicsDevice.Viewport; + //float scrollBarPos = viewport.Width / 2f; + //scrollBarPos -= _texScrollButton.Width + 2f; + + + + InitMenu(); + + + + } + + public void InitMenu() + { + Viewport viewport = ScreenManager.GraphicsDevice.Viewport; + SpriteFont font = ScreenManager.Fonts.MenuSpriteFont; + float scrollBarPos = viewport.Width / 2f; + + for (int i = 0; i < _menuEntries.Count; ++i) + { + _menuEntries[i].Initialize(); + scrollBarPos = Math.Min(scrollBarPos, + (viewport.Width - _menuEntries[i].GetWidth()) / 2f); + } + + _titleOrigin = font.MeasureString(_menuTitle) / 2f; + _titlePosition = new Vector2(viewport.Width / 2f, font.MeasureString("M").Y / 2f + 10f); + + _menuBorderMargin = font.MeasureString("M").Y * 0.8f; + _menuBorderTop = (viewport.Height - _menuBorderMargin * (NumEntries - 1)) / 2f; + _menuBorderBottom = (viewport.Height + _menuBorderMargin * (NumEntries - 1)) / 2f; + + _menuOffset = 0f; + _maxOffset = Math.Max(0f, (_menuEntries.Count - NumEntries) * _menuBorderMargin); + + _scrollUp = new MenuButton(_texScrollButton, false, + new Vector2(scrollBarPos, _menuBorderTop - _texScrollButton.Height), this); + _scrollDown = new MenuButton(_texScrollButton, true, + new Vector2(scrollBarPos, _menuBorderBottom + _texScrollButton.Height), this); + _scrollSlider = new MenuButton(_texSlider, false, new Vector2(scrollBarPos, _menuBorderTop), this); + + _scrollLock = false; + } + + /// + /// Returns the index of the menu entry at the position of the given mouse state. + /// + /// Index of menu entry if valid, -1 otherwise + private int GetMenuEntryAt(Vector2 position) + { + if (this.TransitionPosition == 0f && this.ScreenState == SamplesFramework.ScreenState.Active) + { + int index = 0; + foreach (MenuEntry entry in _menuEntries) + { + float width = entry.GetWidth(); + float height = entry.GetHeight(); + Rectangle rect = new Rectangle((int)(entry.Position.X - width / 2f), + (int)(entry.Position.Y - height / 2f), + (int)width, (int)height); + if (rect.Contains((int)position.X, (int)position.Y) && entry.Alpha > 0.1f) + { + return index; + } + ++index; + } + } + return -1; + } + + /// + /// Responds to user input, changing the selected entry and accepting + /// or cancelling the menu. + /// + public override void HandleInput(InputHelper input, GameTime gameTime) + { + // Mouse or touch on a menu item + int hoverIndex = GetMenuEntryAt(input.Cursor); + if (hoverIndex > -1 && _menuEntries[hoverIndex].IsSelectable() && !_scrollLock) + { + _selectedEntry = hoverIndex; + } + else + { + _selectedEntry = -1; + } + + _scrollSlider.Hover = false; + if (input.IsCursorValid) + { + _scrollUp.Collide(input.Cursor); + _scrollDown.Collide(input.Cursor); + _scrollSlider.Collide(input.Cursor); + } + else + { + _scrollUp.Hover = false; + _scrollDown.Hover = false; + _scrollLock = false; + } + + // Accept or cancel the menu? + if (input.IsMenuSelect() && _selectedEntry != -1) + { + if (_menuEntries[_selectedEntry].IsExitItem()) + { + ScreenManager.Game.Exit(); + } + else if (_menuEntries[_selectedEntry].IsBackItem()) + { + this.ExitScreen(); + } + else if (_menuEntries[_selectedEntry].Screen != null) + { + ScreenManager.AddScreen(_menuEntries[_selectedEntry].Screen); + if (_menuEntries[_selectedEntry].Screen is IDemoScreen) + { + ScreenManager.AddScreen( + new MessageBoxScreen((_menuEntries[_selectedEntry].Screen as IDemoScreen).GetDetails())); + } + } + } + else if (input.IsMenuCancel()) + { + if (this.ScreenState == SamplesFramework.ScreenState.Active) + { + if (ScreenManager.GetScreens().Length == 2) + ScreenManager.Game.Exit(); + else + this.ExitScreen(); + } + //ScreenManager.Game.Exit(); + } + + if (input.IsMenuPressed()) + { + if (_scrollUp.Hover) + { + _menuOffset = Math.Max(_menuOffset - 200f * (float)gameTime.ElapsedGameTime.TotalSeconds, 0f); + _scrollLock = false; + } + if (_scrollDown.Hover) + { + _menuOffset = Math.Min(_menuOffset + 200f * (float)gameTime.ElapsedGameTime.TotalSeconds, _maxOffset); + _scrollLock = false; + } + if (_scrollSlider.Hover) + { + _scrollLock = true; + } + } + if (input.IsMenuReleased()) + { + _scrollLock = false; + } + if (_scrollLock) + { + _scrollSlider.Hover = true; + _menuOffset = Math.Max(Math.Min(((input.Cursor.Y - _menuBorderTop) / (_menuBorderBottom - _menuBorderTop)) * _maxOffset, _maxOffset), 0f); + } + } + + /// + /// Allows the screen the chance to position the menu entries. By default + /// all menu entries are lined up in a vertical list, centered on the screen. + /// + protected virtual void UpdateMenuEntryLocations() + { + // Make the menu slide into place during transitions, using a + // power curve to make things look more interesting (this makes + // the movement slow down as it nears the end). + float transitionOffset = (float)Math.Pow(TransitionPosition, 2); + + Vector2 position = Vector2.Zero; + position.Y = _menuBorderTop - _menuOffset; + + // update each menu entry's location in turn + for (int i = 0; i < _menuEntries.Count; ++i) + { + position.X = ScreenManager.GraphicsDevice.Viewport.Width / 2f; + if (ScreenState == ScreenState.TransitionOn) + { + position.X -= transitionOffset * 256; + } + else + { + position.X += transitionOffset * 256; + } + + // set the entry's position + _menuEntries[i].Position = position; + + if (position.Y < _menuBorderTop) + { + _menuEntries[i].Alpha = 1f - + Math.Min(_menuBorderTop - position.Y, _menuBorderMargin) / _menuBorderMargin; + } + else if (position.Y > _menuBorderBottom) + { + _menuEntries[i].Alpha = 1f - + Math.Min(position.Y - _menuBorderBottom, _menuBorderMargin) / + _menuBorderMargin; + } + else + { + _menuEntries[i].Alpha = 1f; + } + + // move down for the next entry the size of this entry + position.Y += _menuEntries[i].GetHeight(); + } + Vector2 scrollPos = _scrollSlider.Position; + scrollPos.Y = MathHelper.Lerp(_menuBorderTop, _menuBorderBottom, _menuOffset / _maxOffset); + _scrollSlider.Position = scrollPos; + } + + /// + /// Updates the menu. + /// + public override void Update(GameTime gameTime, bool otherScreenHasFocus, + bool coveredByOtherScreen) + { + base.Update(gameTime, otherScreenHasFocus, coveredByOtherScreen); + + // Update each nested MenuEntry object. + for (int i = 0; i < _menuEntries.Count; ++i) + { + bool isSelected = IsActive && (i == _selectedEntry); + _menuEntries[i].Update(isSelected, gameTime); + } + + _scrollUp.Update(gameTime); + _scrollDown.Update(gameTime); + _scrollSlider.Update(gameTime); + } + + /// + /// Draws the menu. + /// + public override void Draw(GameTime gameTime) + { + // make sure our entries are in the right place before we draw them + UpdateMenuEntryLocations(); + + SpriteBatch spriteBatch = ScreenManager.SpriteBatch; + SpriteFont font = ScreenManager.Fonts.MenuSpriteFont; + + spriteBatch.Begin(); + // Draw each menu entry in turn. + for (int i = 0; i < _menuEntries.Count; ++i) + { + bool isSelected = IsActive && (i == _selectedEntry); + _menuEntries[i].Draw(); + } + + // Make the menu slide into place during transitions, using a + // power curve to make things look more interesting (this makes + // the movement slow down as it nears the end). + Vector2 transitionOffset = new Vector2(0f, (float)Math.Pow(TransitionPosition, 2) * 100f); + + spriteBatch.DrawString(font, _menuTitle, _titlePosition - transitionOffset + Vector2.One * 2f, Color.Black, 0, + _titleOrigin, 1f, SpriteEffects.None, 0); + spriteBatch.DrawString(font, _menuTitle, _titlePosition - transitionOffset, new Color(255, 210, 0), 0, + _titleOrigin, 1f, SpriteEffects.None, 0); + _scrollUp.Draw(); + _scrollSlider.Draw(); + _scrollDown.Draw(); + spriteBatch.End(); + } + } +} \ No newline at end of file diff --git a/axios/ScreenSystem/MessageBoxScreen.cs b/axios/ScreenSystem/MessageBoxScreen.cs new file mode 100644 index 0000000..d3c9826 --- /dev/null +++ b/axios/ScreenSystem/MessageBoxScreen.cs @@ -0,0 +1,99 @@ +using System; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Content; +using Microsoft.Xna.Framework.Graphics; + +namespace FarseerPhysics.SamplesFramework +{ + /// + /// A popup message box screen, used to display "are you sure?" + /// confirmation messages. + /// + public class MessageBoxScreen : GameScreen + { + protected Rectangle _backgroundRectangle; + protected Texture2D _gradientTexture; + protected string _message; + protected Vector2 _textPosition; + + public MessageBoxScreen(string message) + { + _message = message; + + IsPopup = true; + HasCursor = true; + + TransitionOnTime = TimeSpan.FromSeconds(0.4); + TransitionOffTime = TimeSpan.FromSeconds(0.4); + } + + public MessageBoxScreen() + { + IsPopup = true; + } + + /// + /// Loads graphics content for this screen. This uses the shared ContentManager + /// provided by the Game class, so the content will remain loaded forever. + /// Whenever a subsequent MessageBoxScreen tries to load this same content, + /// it will just get back another reference to the already loaded data. + /// + public override void LoadContent() + { + SpriteFont font = ScreenManager.Fonts.DetailsFont; + ContentManager content = ScreenManager.Game.Content; + _gradientTexture = content.Load("Common/popup"); + + // Center the message text in the viewport. + Viewport viewport = ScreenManager.GraphicsDevice.Viewport; + Vector2 viewportSize = new Vector2(viewport.Width, viewport.Height); + Vector2 textSize = font.MeasureString(_message); + _textPosition = (viewportSize - textSize) / 2; + + // The background includes a border somewhat larger than the text itself. + const int hPad = 32; + const int vPad = 16; + + _backgroundRectangle = new Rectangle((int)_textPosition.X - hPad, + (int)_textPosition.Y - vPad, + (int)textSize.X + hPad * 2, + (int)textSize.Y + vPad * 2); + } + + /// + /// Responds to user input, accepting or cancelling the message box. + /// + public override void HandleInput(InputHelper input, GameTime gameTime) + { + + if (input.IsMenuSelect() || input.IsMenuCancel() || + input.IsNewMouseButtonPress(MouseButtons.LeftButton)) + { + ExitScreen(); + } + } + + /// + /// Draws the message box. + /// + public override void Draw(GameTime gameTime) + { + SpriteBatch spriteBatch = ScreenManager.SpriteBatch; + SpriteFont font = ScreenManager.Fonts.DetailsFont; + + // Fade the popup alpha during transitions. + Color color = Color.White * TransitionAlpha * (2f / 3f); + + spriteBatch.Begin(); + + // Draw the background rectangle. + spriteBatch.Draw(_gradientTexture, _backgroundRectangle, color); + + // Draw the message box text. + spriteBatch.DrawString(font, _message, _textPosition + Vector2.One, Color.Black); + spriteBatch.DrawString(font, _message, _textPosition, Color.White); + + spriteBatch.End(); + } + } +} \ No newline at end of file diff --git a/axios/ScreenSystem/PhysicsGameScreen.cs b/axios/ScreenSystem/PhysicsGameScreen.cs new file mode 100644 index 0000000..03b502f --- /dev/null +++ b/axios/ScreenSystem/PhysicsGameScreen.cs @@ -0,0 +1,332 @@ +using System; +using FarseerPhysics; +using FarseerPhysics.DebugViews; +using FarseerPhysics.Dynamics; +using FarseerPhysics.Dynamics.Joints; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Content; +using Microsoft.Xna.Framework.Input; + +namespace FarseerPhysics.SamplesFramework +{ + public class PhysicsGameScreen : GameScreen + { + public Camera2D Camera; + protected DebugViewXNA DebugView; + public World World; + + private float _agentForce; + private float _agentTorque; +#if DEBUG + private FixedMouseJoint _fixedMouseJoint; +#endif + private Body _userAgent; + + protected PhysicsGameScreen() + { + TransitionOnTime = TimeSpan.FromSeconds(0.75); + TransitionOffTime = TimeSpan.FromSeconds(0.75); + HasCursor = true; + EnableCameraControl = true; + _userAgent = null; + World = null; + Camera = null; + DebugView = null; + } + + public bool EnableCameraControl { get; set; } + + protected void SetUserAgent(Body agent, float force, float torque) + { + _userAgent = agent; + _agentForce = force; + _agentTorque = torque; + } + + + + public override void LoadContent() + { + base.LoadContent(); + + //We enable diagnostics to show get values for our performance counters. + Settings.EnableDiagnostics = true; + + if (World == null) + { + World = new World(Vector2.Zero); + } + else + { + World.Clear(); + } + + if (DebugView == null) + { + if (!Axios.Settings.ScreenSaver) + { + DebugView = new DebugViewXNA(World); + DebugView.RemoveFlags(DebugViewFlags.Shape); + DebugView.RemoveFlags(DebugViewFlags.Joint); + DebugView.DefaultShapeColor = Color.White; + DebugView.SleepingShapeColor = Color.LightGray; + DebugView.LoadContent(ScreenManager.GraphicsDevice, ScreenManager.Content); + } + } + + if (Camera == null) + { + Camera = new Camera2D(ScreenManager.GraphicsDevice); + } + else + { + Camera.ResetCamera(); + } + + // Loading may take a while... so prevent the game from "catching up" once we finished loading + ScreenManager.Game.ResetElapsedTime(); + } + + public override void Update(GameTime gameTime, bool otherScreenHasFocus, bool coveredByOtherScreen) + { + if (!coveredByOtherScreen && !otherScreenHasFocus) + { + // variable time step but never less then 30 Hz + World.Step(Math.Min((float)gameTime.ElapsedGameTime.TotalSeconds, (1f / 30f))); + } + else + { + World.Step(0f); + } + Camera.Update(gameTime); + base.Update(gameTime, otherScreenHasFocus, coveredByOtherScreen); + } + + public virtual void CleanUp() + { + + } + public override void HandleInput(InputHelper input, GameTime gameTime) + { + +#if DEBUG + // Control debug view + if (input.IsNewButtonPress(Buttons.Start)) + { + EnableOrDisableFlag(DebugViewFlags.Shape); + EnableOrDisableFlag(DebugViewFlags.DebugPanel); + EnableOrDisableFlag(DebugViewFlags.PerformanceGraph); + EnableOrDisableFlag(DebugViewFlags.Joint); + EnableOrDisableFlag(DebugViewFlags.ContactPoints); + EnableOrDisableFlag(DebugViewFlags.ContactNormals); + EnableOrDisableFlag(DebugViewFlags.Controllers); + } + + if (input.IsNewKeyPress(Keys.F1)) + { + EnableOrDisableFlag(DebugViewFlags.Shape); + } + if (input.IsNewKeyPress(Keys.F2)) + { + EnableOrDisableFlag(DebugViewFlags.DebugPanel); + EnableOrDisableFlag(DebugViewFlags.PerformanceGraph); + } + if (input.IsNewKeyPress(Keys.F3)) + { + EnableOrDisableFlag(DebugViewFlags.Joint); + } + if (input.IsNewKeyPress(Keys.F4)) + { + EnableOrDisableFlag(DebugViewFlags.ContactPoints); + EnableOrDisableFlag(DebugViewFlags.ContactNormals); + } + if (input.IsNewKeyPress(Keys.F5)) + { + EnableOrDisableFlag(DebugViewFlags.PolygonPoints); + } + if (input.IsNewKeyPress(Keys.F6)) + { + EnableOrDisableFlag(DebugViewFlags.Controllers); + } + if (input.IsNewKeyPress(Keys.F7)) + { + EnableOrDisableFlag(DebugViewFlags.CenterOfMass); + } + if (input.IsNewKeyPress(Keys.F8)) + { + EnableOrDisableFlag(DebugViewFlags.AABB); + } + + if (input.IsNewButtonPress(Buttons.Back) || input.IsNewKeyPress(Keys.Escape)) + { + if (this.ScreenState == SamplesFramework.ScreenState.Active && this.TransitionPosition == 0 && this.TransitionAlpha == 1) + { //Give the screens a chance to transition + + CleanUp(); + ExitScreen(); + + } + } + + if (HasCursor) + { + HandleCursor(input); + } + + if (_userAgent != null) + { + HandleUserAgent(input); + } + + if (EnableCameraControl) + { + HandleCamera(input, gameTime); + } +#endif + + base.HandleInput(input, gameTime); + } + + public virtual void HandleCursor(InputHelper input) + { + #if DEBUG + Vector2 position = Camera.ConvertScreenToWorld(input.Cursor); + + if ((input.IsNewButtonPress(Buttons.A) || + input.IsNewMouseButtonPress(MouseButtons.LeftButton)) && + _fixedMouseJoint == null) + { + Fixture savedFixture = World.TestPoint(position); + if (savedFixture != null) + { + Body body = savedFixture.Body; + _fixedMouseJoint = new FixedMouseJoint(body, position); + _fixedMouseJoint.MaxForce = 1000.0f * body.Mass; + World.AddJoint(_fixedMouseJoint); + body.Awake = true; + } + } + + if ((input.IsNewButtonRelease(Buttons.A) || + input.IsNewMouseButtonRelease(MouseButtons.LeftButton)) && + _fixedMouseJoint != null) + { + World.RemoveJoint(_fixedMouseJoint); + _fixedMouseJoint = null; + } + + if (_fixedMouseJoint != null) + { + _fixedMouseJoint.WorldAnchorB = position; + } + #endif + } + + private void HandleCamera(InputHelper input, GameTime gameTime) + { + Vector2 camMove = Vector2.Zero; + +#if DEBUG + if (input.KeyboardState.IsKeyDown(Keys.Up)) + { + camMove.Y -= 10f * (float)gameTime.ElapsedGameTime.TotalSeconds; + } + if (input.KeyboardState.IsKeyDown(Keys.Down)) + { + camMove.Y += 10f * (float)gameTime.ElapsedGameTime.TotalSeconds; + } + if (input.KeyboardState.IsKeyDown(Keys.Left)) + { + camMove.X -= 10f * (float)gameTime.ElapsedGameTime.TotalSeconds; + } + if (input.KeyboardState.IsKeyDown(Keys.Right)) + { + camMove.X += 10f * (float)gameTime.ElapsedGameTime.TotalSeconds; + } + if (input.KeyboardState.IsKeyDown(Keys.PageUp)) + { + Camera.Zoom += 5f * (float)gameTime.ElapsedGameTime.TotalSeconds * Camera.Zoom / 20f; + } + if (input.KeyboardState.IsKeyDown(Keys.PageDown)) + { + Camera.Zoom -= 5f * (float)gameTime.ElapsedGameTime.TotalSeconds * Camera.Zoom / 20f; + } + if (camMove != Vector2.Zero) + { + Camera.MoveCamera(camMove); + } + if (input.IsNewKeyPress(Keys.Home)) + { + Camera.ResetCamera(); + } +#endif + } + + private void HandleUserAgent(InputHelper input) + { +#if DEBUG + Vector2 force = _agentForce * new Vector2(input.GamePadState.ThumbSticks.Right.X, + -input.GamePadState.ThumbSticks.Right.Y); + float torque = _agentTorque * (input.GamePadState.Triggers.Right - input.GamePadState.Triggers.Left); + + _userAgent.ApplyForce(force); + _userAgent.ApplyTorque(torque); + + float forceAmount = _agentForce * 0.6f; + + force = Vector2.Zero; + torque = 0; + + if (input.KeyboardState.IsKeyDown(Keys.A)) + { + force += new Vector2(-forceAmount, 0); + } + if (input.KeyboardState.IsKeyDown(Keys.S)) + { + force += new Vector2(0, forceAmount); + } + if (input.KeyboardState.IsKeyDown(Keys.D)) + { + force += new Vector2(forceAmount, 0); + } + if (input.KeyboardState.IsKeyDown(Keys.W)) + { + force += new Vector2(0, -forceAmount); + } + if (input.KeyboardState.IsKeyDown(Keys.Q)) + { + torque -= _agentTorque; + } + if (input.KeyboardState.IsKeyDown(Keys.E)) + { + torque += _agentTorque; + } + + _userAgent.ApplyForce(force); + _userAgent.ApplyTorque(torque); +#endif + } + + private void EnableOrDisableFlag(DebugViewFlags flag) + { + if ((DebugView.Flags & flag) == flag) + { + DebugView.RemoveFlags(flag); + } + else + { + DebugView.AppendFlags(flag); + } + } + + public override void Draw(GameTime gameTime) + { + Matrix projection = Camera.SimProjection; + Matrix view = Camera.SimView; + + if (!Axios.Settings.ScreenSaver) + DebugView.RenderDebugData(ref projection, ref view); + base.Draw(gameTime); + } + } +} \ No newline at end of file diff --git a/axios/ScreenSystem/ScreenManagerComponent.cs b/axios/ScreenSystem/ScreenManagerComponent.cs new file mode 100644 index 0000000..0218aba --- /dev/null +++ b/axios/ScreenSystem/ScreenManagerComponent.cs @@ -0,0 +1,303 @@ +using System.Collections.Generic; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Content; +using Microsoft.Xna.Framework.Graphics; +using Microsoft.Xna.Framework.Input.Touch; + +namespace FarseerPhysics.SamplesFramework +{ + /// + /// The screen manager is a component which manages one or more GameScreen + /// instances. It maintains a stack of screens, calls their Update and Draw + /// methods at the appropriate times, and automatically routes input to the + /// topmost active screen. + /// + public class ScreenManager : DrawableGameComponent + { + private AssetCreator _assetCreator; + private ContentManager _contentManager; + + private InputHelper _input; + private bool _isInitialized; + private LineBatch _lineBatch; + + private List _screens; + private List _screensToUpdate; + + private SpriteBatch _spriteBatch; + + /// + /// Contains all the fonts avaliable for use. + /// + private SpriteFonts _spriteFonts; + + private List _transitions; + + /// + /// Constructs a new screen manager component. + /// + public ScreenManager(Game game) + : base(game) + { + // we must set EnabledGestures before we can query for them, but + // we don't assume the game wants to read them. + //game.Components. + TouchPanel.EnabledGestures = GestureType.None; + _contentManager = game.Content; + _contentManager.RootDirectory = "Content"; + _input = new InputHelper(this); + + _screens = new List(); + _screensToUpdate = new List(); + _transitions = new List(); + } + + /// + /// A default SpriteBatch shared by all the screens. This saves + /// each screen having to bother creating their own local instance. + /// + public SpriteBatch SpriteBatch + { + get { return _spriteBatch; } + } + + public LineBatch LineBatch + { + get { return _lineBatch; } + } + + public ContentManager Content + { + get { return _contentManager; } + } + + public SpriteFonts Fonts + { + get { return _spriteFonts; } + } + + public AssetCreator Assets + { + get { return _assetCreator; } + } + + /// + /// Initializes the screen manager component. + /// + public override void Initialize() + { + if (!Axios.Settings.ScreenSaver) + _spriteFonts = new SpriteFonts(_contentManager); + base.Initialize(); + + _isInitialized = true; + } + + /// + /// Load your graphics content. + /// + protected override void LoadContent() + { + _spriteBatch = new SpriteBatch(GraphicsDevice); + _lineBatch = new LineBatch(GraphicsDevice); + + if (!Axios.Settings.ScreenSaver) + { + _assetCreator = new AssetCreator(GraphicsDevice); + _assetCreator.LoadContent(_contentManager); + + _input.LoadContent(); + } + // Tell each of the screens to load their content. + foreach (GameScreen screen in _screens) + { + screen.LoadContent(); + } + } + + /// + /// Unload your graphics content. + /// + protected override void UnloadContent() + { + // Tell each of the screens to unload their content. + foreach (GameScreen screen in _screens) + { + screen.UnloadContent(); + } + } + + /// + /// Allows each screen to run logic. + /// + public override void Update(GameTime gameTime) + { + // Read the keyboard and gamepad. + _input.Update(gameTime); + + // Make a copy of the master screen list, to avoid confusion if + // the process of updating one screen adds or removes others. + _screensToUpdate.Clear(); + + if (_screens.Count == 0) + //I'm done, exit + this.Game.Exit(); + + foreach (GameScreen screen in _screens) + { + _screensToUpdate.Add(screen); + } + + bool otherScreenHasFocus = !Game.IsActive; + bool coveredByOtherScreen = false; + + // Loop as long as there are screens waiting to be updated. + while (_screensToUpdate.Count > 0) + { + // Pop the topmost screen off the waiting list. + GameScreen screen = _screensToUpdate[_screensToUpdate.Count - 1]; + + _screensToUpdate.RemoveAt(_screensToUpdate.Count - 1); + + // Update the screen. + screen.Update(gameTime, otherScreenHasFocus, coveredByOtherScreen); + + if (screen.ScreenState == ScreenState.TransitionOn || + screen.ScreenState == ScreenState.Active) + { + // If this is the first active screen we came across, + // give it a chance to handle input. + if (!otherScreenHasFocus || screen.AlwaysHasFocus) + { + + if (!otherScreenHasFocus) + { + _input.ShowCursor = screen.HasCursor; + _input.EnableVirtualStick = screen.HasVirtualStick; + + otherScreenHasFocus = true; + } + screen.HandleInput(_input, gameTime); + } + + // If this is an active non-popup, inform any subsequent + // screens that they are covered by it. + if (!screen.IsPopup) + { + coveredByOtherScreen = true; + } + } + } + } + + /// + /// Tells each screen to draw itself. + /// + public override void Draw(GameTime gameTime) + { + int transitionCount = 0; + foreach (GameScreen screen in _screens) + { + if (screen.ScreenState == ScreenState.TransitionOn || + screen.ScreenState == ScreenState.TransitionOff) + { + ++transitionCount; + if (_transitions.Count < transitionCount) + { + PresentationParameters _pp = GraphicsDevice.PresentationParameters; + _transitions.Add(new RenderTarget2D(GraphicsDevice, _pp.BackBufferWidth, _pp.BackBufferHeight, + false, + SurfaceFormat.Color, _pp.DepthStencilFormat, + _pp.MultiSampleCount, + RenderTargetUsage.DiscardContents)); + } + GraphicsDevice.SetRenderTarget(_transitions[transitionCount - 1]); + GraphicsDevice.Clear(Color.Transparent); + screen.Draw(gameTime); + GraphicsDevice.SetRenderTarget(null); + } + } + + //GraphicsDevice.Clear(Color.Black); + + transitionCount = 0; + foreach (GameScreen screen in _screens) + { + if (screen.ScreenState == ScreenState.Hidden) + { + continue; + } + + if (screen.ScreenState == ScreenState.TransitionOn || + screen.ScreenState == ScreenState.TransitionOff) + { + _spriteBatch.Begin(0, BlendState.AlphaBlend); + _spriteBatch.Draw(_transitions[transitionCount], Vector2.Zero, Color.White * screen.TransitionAlpha); + _spriteBatch.End(); + + ++transitionCount; + } + else + { + screen.Draw(gameTime); + } + } + _input.Draw(); + } + + /// + /// Adds a new screen to the screen manager. + /// + public void AddScreen(GameScreen screen) + { + screen.ScreenManager = this; + screen.IsExiting = false; + + // If we have a graphics device, tell the screen to load content. + if (_isInitialized) + { + screen.LoadContent(); + } + + _screens.Add(screen); + + // update the TouchPanel to respond to gestures this screen is interested in + TouchPanel.EnabledGestures = screen.EnabledGestures; + } + + /// + /// Removes a screen from the screen manager. You should normally + /// use GameScreen.ExitScreen instead of calling this directly, so + /// the screen can gradually transition off rather than just being + /// instantly removed. + /// + public void RemoveScreen(GameScreen screen) + { + // If we have a graphics device, tell the screen to unload content. + if (_isInitialized) + { + screen.UnloadContent(); + } + + _screens.Remove(screen); + _screensToUpdate.Remove(screen); + + // if there is a screen still in the manager, update TouchPanel + // to respond to gestures that screen is interested in. + if (_screens.Count > 0) + { + TouchPanel.EnabledGestures = _screens[_screens.Count - 1].EnabledGestures; + } + } + + /// + /// Expose an array holding all the screens. We return a copy rather + /// than the real master list, because screens should only ever be added + /// or removed using the AddScreen and RemoveScreen methods. + /// + public GameScreen[] GetScreens() + { + return _screens.ToArray(); + } + } +} \ No newline at end of file diff --git a/axios/ScreenSystem/SpriteFonts.cs b/axios/ScreenSystem/SpriteFonts.cs new file mode 100644 index 0000000..749a7e0 --- /dev/null +++ b/axios/ScreenSystem/SpriteFonts.cs @@ -0,0 +1,19 @@ +using Microsoft.Xna.Framework.Content; +using Microsoft.Xna.Framework.Graphics; + +namespace FarseerPhysics.SamplesFramework +{ + public class SpriteFonts + { + public SpriteFont DetailsFont; + public SpriteFont FrameRateCounterFont; + public SpriteFont MenuSpriteFont; + + public SpriteFonts(ContentManager contentManager) + { + MenuSpriteFont = contentManager.Load("Fonts/menuFont"); + FrameRateCounterFont = contentManager.Load("Fonts/frameRateCounterFont"); + DetailsFont = contentManager.Load("Fonts/detailsFont"); + } + } +} \ No newline at end of file diff --git a/axios/ScreenSystem/VirtualButton.cs b/axios/ScreenSystem/VirtualButton.cs new file mode 100644 index 0000000..c23b0a3 --- /dev/null +++ b/axios/ScreenSystem/VirtualButton.cs @@ -0,0 +1,46 @@ +using System; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Graphics; +using Microsoft.Xna.Framework.Input.Touch; + +namespace FarseerPhysics.SamplesFramework +{ + public sealed class VirtualButton + { + private Texture2D _sprite; + private Vector2 _origin; + private Rectangle _normal; + private Rectangle _pressed; + private Vector2 _position; + + public bool Pressed; + + public VirtualButton(Texture2D sprite, Vector2 position, Rectangle normal, Rectangle pressed) + { + _sprite = sprite; + _origin = new Vector2(normal.Width / 2f, normal.Height / 2f); + _normal = normal; + _pressed = pressed; + Pressed = false; + _position = position; + } + + public void Update(TouchLocation touchLocation) + { + if (touchLocation.State == TouchLocationState.Pressed || + touchLocation.State == TouchLocationState.Moved) + { + Vector2 delta = touchLocation.Position - _position; + if (delta.LengthSquared() <= 400f) + { + Pressed = true; + } + } + } + + public void Draw(SpriteBatch batch) + { + batch.Draw(_sprite, _position, Pressed ? _pressed : _normal, Color.White, 0f, _origin, 1f, SpriteEffects.None, 0f); + } + } +} diff --git a/axios/ScreenSystem/VirtualStick.cs b/axios/ScreenSystem/VirtualStick.cs new file mode 100644 index 0000000..a18a50e --- /dev/null +++ b/axios/ScreenSystem/VirtualStick.cs @@ -0,0 +1,70 @@ +using System; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Graphics; +using Microsoft.Xna.Framework.Input.Touch; + +namespace FarseerPhysics.SamplesFramework +{ + public sealed class VirtualStick + { + private Sprite _socketSprite; + private Sprite _stickSprite; + private int _picked; + private Vector2 _position; + private Vector2 _center; + + public Vector2 StickPosition; + + public VirtualStick(Texture2D socket, Texture2D stick, Vector2 position) + { + _socketSprite = new Sprite(socket); + _stickSprite = new Sprite(stick); + _picked = -1; + _center = position; + _position = position; + StickPosition = Vector2.Zero; + } + + public void Update(TouchLocation touchLocation) + { + if (touchLocation.State == TouchLocationState.Pressed && _picked < 0) + { + Vector2 delta = touchLocation.Position - _position; + if (delta.LengthSquared() <= 2025f) + { + _picked = touchLocation.Id; + } + } + if ((touchLocation.State == TouchLocationState.Pressed || + touchLocation.State == TouchLocationState.Moved) && touchLocation.Id == _picked) + { + Vector2 delta = touchLocation.Position - _center; + if (delta != Vector2.Zero) + { + float _length = delta.Length(); + if (_length > 25f) + { + delta *= (25f / _length); + } + StickPosition = delta / 25f; + StickPosition.Y *= -1f; + _position = _center + delta; + } + } + if (touchLocation.State == TouchLocationState.Released && touchLocation.Id == _picked) + { + _picked = -1; + _position = _center; + StickPosition = Vector2.Zero; + } + } + + public void Draw(SpriteBatch batch) + { + batch.Draw(_socketSprite.Texture, _center, null, Color.White, 0f, + _socketSprite.Origin, 1f, SpriteEffects.None, 0f); + batch.Draw(_stickSprite.Texture, _position, null, Color.White, 0f, + _stickSprite.Origin, 1f, SpriteEffects.None, 0f); + } + } +} diff --git a/axios/Settings.cs b/axios/Settings.cs new file mode 100644 index 0000000..1fcf20b --- /dev/null +++ b/axios/Settings.cs @@ -0,0 +1,236 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and 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 +{ + public static class Settings + { + + public static string Version = "Farseer Engine Version 3.3.1 (Patched)"; + + public const float MaxFloat = 3.402823466e+38f; + public const float Epsilon = 1.192092896e-07f; + public const float Pi = 3.14159265359f; + + // Common + + /// + /// 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 static bool EnableDiagnostics = true; + + /// + /// 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; + + /// + /// The number of velocity iterations in the TOI solver + /// + public static int TOIVelocityIterations = 8; + + /// + /// 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 static 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. + /// + public static bool UseFPECollisionCategories; + + /// + /// Conserve memory makes sure that objects are used by reference instead of cloned. + /// When you give a vertices collection to a PolygonShape, it will by default copy the vertices + /// instead of using the original reference. This is to ensure that objects modified outside the engine + /// does not affect the engine itself, however, this uses extra memory. This behavior + /// can be turned off by setting ConserveMemory to true. + /// + public const bool ConserveMemory = false; + + /// + /// The maximum number of contact points between two convex shapes. + /// + 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 ContactBaumgarte = 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); + + /// + /// 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