natalie
/
axiosengine
1
0
Fork 0
Code Issues Pull Requests Releases 1 Wiki Activity

Adding initial files

master
nathan@daedalus 2012-03-19 18:57:59 -05:00
commit 5bdc5db408
162 changed files with 43840 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
axios.sln 100644
View File

@ -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

BIN
axios.suo 100644
View File

Binary file not shown.

118
axios/AxiosEngine.cd 100644
View File

@ -0,0 +1,118 @@
<?xml version="1.0" encoding="utf-8"?>
<ClassDiagram MajorVersion="1" MinorVersion="1">
<Class Name="Axios.Engine.AxiosEvents">
<Position X="11" Y="0.5" Width="1.5" />
<NestedTypes>
<Delegate Name="Axios.Engine.AxiosEvents.AxiosHandler" Collapsed="true">
<TypeIdentifier>
<NewMemberFileName>Engine\AxiosEvents.cs</NewMemberFileName>
</TypeIdentifier>
</Delegate>
<Delegate Name="Axios.Engine.AxiosEvents.AxiosGameObjectHandler" Collapsed="true">
<TypeIdentifier>
<NewMemberFileName>Engine\AxiosEvents.cs</NewMemberFileName>
</TypeIdentifier>
</Delegate>
</NestedTypes>
<TypeIdentifier>
<HashCode>AAQAACAIAAAAAkAAAkkAAAAAAEEAGAIAIQQAABgCAAA=</HashCode>
<FileName>Engine\AxiosEvents.cs</FileName>
</TypeIdentifier>
</Class>
<Class Name="Axios.Engine.AxiosGameObject">
<Position X="5.75" Y="0.5" Width="1.5" />
<TypeIdentifier>
<HashCode>AAQAMACAAAAAAAAEACIIBAQAIAAAAAAAAAAAAAAAACQ=</HashCode>
<FileName>Engine\AxiosGameObject.cs</FileName>
</TypeIdentifier>
<Lollipop Position="0.2" />
</Class>
<Class Name="Axios.Engine.ComplexAxiosGameObject&lt;T&gt;">
<Position X="2.5" Y="0.5" Width="2.25" />
<TypeIdentifier>
<HashCode>AAAAAAAEAAAAAAAAAAAAAAAAAAAACAAAAAAAAAAAAAQ=</HashCode>
<FileName>Engine\ComplexAxiosGameObject.cs</FileName>
</TypeIdentifier>
</Class>
<Class Name="Axios.Engine.SimpleAxiosGameObject">
<Position X="8.5" Y="1.25" Width="2" />
<TypeIdentifier>
<HashCode>AEQAEAABAgAAAAAgAAAAAAAAAAAAAAAAAAAAAAgAAAA=</HashCode>
<FileName>Engine\SimpleAxiosGameObject.cs</FileName>
</TypeIdentifier>
</Class>
<Class Name="Axios.Engine.SimpleDrawableAxiosGameObject">
<Position X="5.25" Y="4.25" Width="2.75" />
<TypeIdentifier>
<HashCode>AAAAAIAAAAAAAgAAAAAAAAAAAAIACAAgAAIAIAAAAAQ=</HashCode>
<FileName>Engine\SimpleDrawableAxiosGameObject.cs</FileName>
</TypeIdentifier>
<Lollipop Position="0.2" />
</Class>
<Class Name="Axios.Engine.AxiosGameScreen">
<Position X="0.5" Y="0.5" Width="1.5" />
<TypeIdentifier>
<HashCode>AARoEQCCAAACAggBACAAAgAICAAAAAAAIAAAAIIAAAQ=</HashCode>
<FileName>Engine\AxiosGameScreen.cs</FileName>
</TypeIdentifier>
</Class>
<Class Name="Axios.Engine.AxiosTimer">
<Position X="0.5" Y="5.75" Width="1.5" />
<TypeIdentifier>
<HashCode>AAQAEACAAAAAAIAAAKAAAACAAAAAAAAQAAAAACAAQAQ=</HashCode>
<FileName>Engine\AxiosTimer.cs</FileName>
</TypeIdentifier>
<Lollipop Position="0.2" />
</Class>
<Class Name="Axios.Engine.Extenions.AxiosExtensions_String">
<Position X="11" Y="5.75" Width="2" />
<TypeIdentifier>
<HashCode>AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAACAAAA=</HashCode>
<FileName>Engine\Extensions\String.cs</FileName>
</TypeIdentifier>
</Class>
<Class Name="Axios.Engine.Extenions.AxiosExtensions_Texture2D">
<Position X="11" Y="7" Width="2.25" />
<TypeIdentifier>
<HashCode>AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAQAAAAAQAAAAA=</HashCode>
<FileName>Engine\Extensions\Texture2D.cs</FileName>
</TypeIdentifier>
</Class>
<Class Name="Axios.Engine.UI.AxiosUIObject">
<Position X="13" Y="2" Width="1.5" />
<TypeIdentifier>
<HashCode>AAAAAAAABAAAAAAAAAAAAEAAAAAAAAAAAAAAAAAAAAA=</HashCode>
<FileName>Engine\UI\AxiosUIObject.cs</FileName>
</TypeIdentifier>
</Class>
<Class Name="Axios.Engine.DrawableAxiosGameObject">
<Position X="15" Y="0.5" Width="2.25" />
<TypeIdentifier>
<HashCode>AAAAAIAAAAAAAgAgAAAABAAAAAAACAAgAAIAIAgAAAQ=</HashCode>
<FileName>Engine\DrawableAxiosGameObject.cs</FileName>
</TypeIdentifier>
<Lollipop Position="0.2" />
</Class>
<Class Name="Axios.Engine.UI.AxiosButton">
<Position X="13.5" Y="3.75" Width="1.5" />
<TypeIdentifier>
<HashCode>AAAAgAAIQAAIAAAAACAAgAAAAAAASAAAIAAAAAgCAAQ=</HashCode>
<FileName>Engine\UI\AxiosButton.cs</FileName>
</TypeIdentifier>
</Class>
<Interface Name="Axios.Engine.Interfaces.IAxiosGameObject">
<Position X="8.5" Y="4.25" Width="2.25" />
<TypeIdentifier>
<HashCode>AAQAEACAAAAAAAAAACAAAAAAAAAAAAAAAAAAAAAAAAQ=</HashCode>
<FileName>Engine\Interfaces\IAxiosGameObject.cs</FileName>
</TypeIdentifier>
</Interface>
<Interface Name="Axios.Engine.Interfaces.IDrawableAxiosGameObject">
<Position X="8.5" Y="6" Width="2.25" />
<TypeIdentifier>
<HashCode>AAAAAAAAAAAAAgAAAAAAAAAAAAAAAAAgAAAAAAAAAAA=</HashCode>
<FileName>Engine\Interfaces\IDrawableAxiosGameObject.cs</FileName>
</TypeIdentifier>
</Interface>
<Font Name="Tahoma" Size="8.25" />
</ClassDiagram>

240
axios/Axios_WP7.csproj 100644
View File

@ -0,0 +1,240 @@
<?xml version="1.0" encoding="utf-8"?>
<Project DefaultTargets="Build" ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
<PropertyGroup>
<ProjectGuid>{C09D9005-76AC-4F1A-9479-2787BB3DB158}</ProjectGuid>
<ProjectTypeGuids>{6D335F3A-9D43-41b4-9D22-F6F17C4BE596};{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}</ProjectTypeGuids>
<Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
<Platform Condition=" '$(Platform)' == '' ">Windows Phone</Platform>
<OutputType>Library</OutputType>
<AppDesignerFolder>Properties</AppDesignerFolder>
<RootNamespace>Axios</RootNamespace>
<AssemblyName>Axios.WP7</AssemblyName>
<TargetFrameworkVersion>v4.0</TargetFrameworkVersion>
<TargetFrameworkProfile>Client</TargetFrameworkProfile>
<XnaFrameworkVersion>v4.0</XnaFrameworkVersion>
<XnaPlatform>Windows Phone</XnaPlatform>
<XnaProfile>Reach</XnaProfile>
<XnaCrossPlatformGroupID>cd94d25d-3fe9-470c-b921-7a5abdd2494c</XnaCrossPlatformGroupID>
<XnaOutputType>Library</XnaOutputType>
<XnaRefreshLevel>1</XnaRefreshLevel>
<XapFilename Condition="$(XnaOutputType)=='Game'">$(AssemblyName).xap</XapFilename>
<SilverlightManifestTemplate Condition="$(XnaOutputType)=='Game'">Properties\AppManifest.xml</SilverlightManifestTemplate>
<XnaWindowsPhoneManifestTemplate Condition="$(XnaOutputType)=='Game'">Properties\WMAppManifest.xml</XnaWindowsPhoneManifestTemplate>
<TileImage Condition="$(XnaOutputType)=='Game'">Background.png</TileImage>
<TileTitle Condition="$(XnaOutputType)=='Game'">$(AssemblyName)</TileTitle>
<Thumbnail Condition="$(XnaOutputType)=='Game'">PhoneGameThumb.png</Thumbnail>
<SupportedCultures />
</PropertyGroup>
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|Windows Phone' ">
<OutputPath>bin\Windows Phone\Release</OutputPath>
<ErrorReport>prompt</ErrorReport>
<WarningLevel>4</WarningLevel>
<NoStdLib>true</NoStdLib>
<UseVSHostingProcess>false</UseVSHostingProcess>
<DebugType>pdbonly</DebugType>
<Optimize>true</Optimize>
<DefineConstants>TRACE;WINDOWS_PHONE</DefineConstants>
<XnaCompressContent>false</XnaCompressContent>
</PropertyGroup>
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|Windows Phone' ">
<OutputPath>bin\Windows Phone\Debug</OutputPath>
<ErrorReport>prompt</ErrorReport>
<WarningLevel>4</WarningLevel>
<NoStdLib>true</NoStdLib>
<UseVSHostingProcess>false</UseVSHostingProcess>
<DebugSymbols>true</DebugSymbols>
<DebugType>full</DebugType>
<Optimize>false</Optimize>
<DefineConstants>TRACE;DEBUG;WINDOWS_PHONE</DefineConstants>
<XnaCompressContent>false</XnaCompressContent>
</PropertyGroup>
<ItemGroup>
<None Include="AxiosEngine.cd" />
</ItemGroup>
<ItemGroup>
<Compile Include="Axios_settings.cs" />
<Compile Include="Collision\Collision.cs" />
<Compile Include="Collision\Distance.cs" />
<Compile Include="Collision\DynamicTree.cs" />
<Compile Include="Collision\DynamicTreeBroadPhase.cs" />
<Compile Include="Collision\IBroadPhase.cs" />
<Compile Include="Collision\QuadTree.cs" />
<Compile Include="Collision\QuadTreeBroadPhase.cs" />
<Compile Include="Collision\Shapes\CircleShape.cs" />
<Compile Include="Collision\Shapes\EdgeShape.cs" />
<Compile Include="Collision\Shapes\LoopShape.cs" />
<Compile Include="Collision\Shapes\PolygonShape.cs" />
<Compile Include="Collision\Shapes\Shape.cs" />
<Compile Include="Collision\TimeOfImpact.cs" />
<Compile Include="Common\ConvexHull\ChainHull.cs" />
<Compile Include="Common\ConvexHull\GiftWrap.cs" />
<Compile Include="Common\ConvexHull\Melkman.cs" />
<Compile Include="Common\Decomposition\BayazitDecomposer.cs" />
<Compile Include="Common\Decomposition\CDTDecomposer.cs" />
<Compile Include="Common\Decomposition\CDT\Delaunay\DelaunayTriangle.cs" />
<Compile Include="Common\Decomposition\CDT\Delaunay\Sweep\AdvancingFront.cs" />
<Compile Include="Common\Decomposition\CDT\Delaunay\Sweep\AdvancingFrontNode.cs" />
<Compile Include="Common\Decomposition\CDT\Delaunay\Sweep\DTSweep.cs" />
<Compile Include="Common\Decomposition\CDT\Delaunay\Sweep\DTSweepConstraint.cs" />
<Compile Include="Common\Decomposition\CDT\Delaunay\Sweep\DTSweepContext.cs" />
<Compile Include="Common\Decomposition\CDT\Delaunay\Sweep\DTSweepPointComparator.cs" />
<Compile Include="Common\Decomposition\CDT\Delaunay\Sweep\PointOnEdgeException.cs" />
<Compile Include="Common\Decomposition\CDT\ITriangulatable.cs" />
<Compile Include="Common\Decomposition\CDT\Orientation.cs" />
<Compile Include="Common\Decomposition\CDT\Polygon\Polygon.cs" />
<Compile Include="Common\Decomposition\CDT\Polygon\PolygonPoint.cs" />
<Compile Include="Common\Decomposition\CDT\Polygon\PolygonSet.cs" />
<Compile Include="Common\Decomposition\CDT\Sets\ConstrainedPointSet.cs" />
<Compile Include="Common\Decomposition\CDT\Sets\PointSet.cs" />
<Compile Include="Common\Decomposition\CDT\TriangulationConstraint.cs" />
<Compile Include="Common\Decomposition\CDT\TriangulationContext.cs" />
<Compile Include="Common\Decomposition\CDT\TriangulationMode.cs" />
<Compile Include="Common\Decomposition\CDT\TriangulationPoint.cs" />
<Compile Include="Common\Decomposition\CDT\TriangulationUtil.cs" />
<Compile Include="Common\Decomposition\CDT\Util\FixedArray3.cs" />
<Compile Include="Common\Decomposition\CDT\Util\FixedBitArray3.cs" />
<Compile Include="Common\Decomposition\CDT\Util\PointGenerator.cs" />
<Compile Include="Common\Decomposition\CDT\Util\PolygonGenerator.cs" />
<Compile Include="Common\Decomposition\EarclipDecomposer.cs" />
<Compile Include="Common\Decomposition\FlipcodeDecomposer.cs" />
<Compile Include="Common\Decomposition\SeidelDecomposer.cs" />
<Compile Include="Common\FixedArray.cs" />
<Compile Include="Common\HashSet.cs" />
<Compile Include="Common\LineTools.cs" />
<Compile Include="Common\Math.cs" />
<Compile Include="Common\Path.cs" />
<Compile Include="Common\PathManager.cs" />
<Compile Include="Common\PhysicsLogic\Explosion.cs" />
<Compile Include="Common\PhysicsLogic\PhysicsLogic.cs" />
<Compile Include="Common\PolygonManipulation\CuttingTools.cs" />
<Compile Include="Common\PolygonManipulation\SimplifyTools.cs" />
<Compile Include="Common\PolygonManipulation\YuPengClipper.cs" />
<Compile Include="Common\PolygonTools.cs" />
<Compile Include="Common\Serialization.cs" />
<Compile Include="Common\TextureTools\MarchingSquares.cs" />
<Compile Include="Common\TextureTools\MSTerrain.cs" />
<Compile Include="Common\TextureTools\TextureConverter.cs" />
<Compile Include="Common\Vertices.cs" />
<Compile Include="Controllers\AbstractForceController.cs" />
<Compile Include="Controllers\BuoyancyController.cs" />
<Compile Include="Controllers\Controller.cs" />
<Compile Include="Controllers\GravityController.cs" />
<Compile Include="Controllers\SimpleWindForce.cs" />
<Compile Include="Controllers\VelocityLimitController.cs" />
<Compile Include="DebugView.cs" />
<Compile Include="DebugViewXNA.cs" />
<Compile Include="DrawingSystem\AssetCreator.cs" />
<Compile Include="DrawingSystem\LineBatch.cs" />
<Compile Include="DrawingSystem\Sprite.cs" />
<Compile Include="Dynamics\Body.cs" />
<Compile Include="Dynamics\BreakableBody.cs" />
<Compile Include="Dynamics\ContactManager.cs" />
<Compile Include="Dynamics\Contacts\Contact.cs" />
<Compile Include="Dynamics\Contacts\ContactSolver.cs" />
<Compile Include="Dynamics\Fixture.cs" />
<Compile Include="Dynamics\Island.cs" />
<Compile Include="Dynamics\Joints\AngleJoint.cs" />
<Compile Include="Dynamics\Joints\DistanceJoint.cs" />
<Compile Include="Dynamics\Joints\FixedAngleJoint.cs" />
<Compile Include="Dynamics\Joints\FixedDistanceJoint.cs" />
<Compile Include="Dynamics\Joints\FixedFrictionJoint.cs" />
<Compile Include="Dynamics\Joints\FixedLineJoint.cs" />
<Compile Include="Dynamics\Joints\FixedMouseJoint.cs" />
<Compile Include="Dynamics\Joints\FixedPrismaticJoint.cs" />
<Compile Include="Dynamics\Joints\FixedRevoluteJoint.cs" />
<Compile Include="Dynamics\Joints\FrictionJoint.cs" />
<Compile Include="Dynamics\Joints\GearJoint.cs" />
<Compile Include="Dynamics\Joints\Joint.cs" />
<Compile Include="Dynamics\Joints\LineJoint.cs" />
<Compile Include="Dynamics\Joints\PrismaticJoint.cs" />
<Compile Include="Dynamics\Joints\PulleyJoint.cs" />
<Compile Include="Dynamics\Joints\RevoluteJoint.cs" />
<Compile Include="Dynamics\Joints\RopeJoint.cs" />
<Compile Include="Dynamics\Joints\SliderJoint.cs" />
<Compile Include="Dynamics\Joints\WeldJoint.cs" />
<Compile Include="Dynamics\TimeStep.cs" />
<Compile Include="Dynamics\World.cs" />
<Compile Include="Dynamics\WorldCallbacks.cs" />
<Compile Include="Engine\AxiosEvents.cs" />
<Compile Include="Engine\AxiosGameObject.cs" />
<Compile Include="Engine\AxiosGameScreen.cs" />
<Compile Include="Engine\AxiosTimer.cs" />
<Compile Include="Engine\AxiosBreakableGameObject.cs" />
<Compile Include="Engine\ComplexAxiosGameObject.cs" />
<Compile Include="Engine\Data\AxiosCSV.cs" />
<Compile Include="Engine\Data\DataEvents.cs" />
<Compile Include="Engine\Data\AxiosDataTable.cs" />
<Compile Include="Engine\DrawableAxiosGameObject.cs" />
<Compile Include="Engine\DrawableBreakableAxiosGameObject.cs" />
<Compile Include="Engine\Extensions\String.cs" />
<Compile Include="Engine\Extensions\Texture2D.cs" />
<Compile Include="Engine\File\AxiosFile.cs" />
<Compile Include="Engine\File\AxiosIsolatedFile.cs" />
<Compile Include="Engine\File\AxiosRegularFile.cs" />
<Compile Include="Engine\File\AxiosTitleFile.cs" />
<Compile Include="Engine\Interfaces\IAxiosFile.cs" />
<Compile Include="Engine\Interfaces\IAxiosGameObject.cs" />
<Compile Include="Engine\Interfaces\IDrawableAxiosGameObject.cs" />
<Compile Include="Engine\Log\AxiosLog.cs" />
<Compile Include="Engine\SimpleAxiosGameObject.cs" />
<Compile Include="Engine\SimpleDrawableAxiosGameObject.cs" />
<Compile Include="Engine\Singleton.cs" />
<Compile Include="Engine\UI\AxiosButton.cs" />
<Compile Include="Engine\UI\AxiosUIObject.cs" />
<Compile Include="Factories\BodyFactory.cs" />
<Compile Include="Factories\FixtureFactory.cs" />
<Compile Include="Factories\JointFactory.cs" />
<Compile Include="Factories\LinkFactory.cs" />
<Compile Include="PrimitiveBatch.cs" />
<Compile Include="Properties\AssemblyInfo.cs" />
<Compile Include="ScreenSystem\BackgroundScreen.cs" />
<Compile Include="ScreenSystem\Camera2D.cs" />
<Compile Include="ScreenSystem\ConvertUnits.cs" />
<Compile Include="ScreenSystem\FramerateCounterComponent.cs" />
<Compile Include="ScreenSystem\GameScreen.cs" />
<Compile Include="ScreenSystem\IDemoScreen.cs" />
<Compile Include="ScreenSystem\InputHelper.cs" />
<Compile Include="ScreenSystem\LogoScreen.cs" />
<Compile Include="ScreenSystem\MenuButton.cs" />
<Compile Include="ScreenSystem\MenuEntry.cs" />
<Compile Include="ScreenSystem\MenuScreen.cs" />
<Compile Include="ScreenSystem\MessageBoxScreen.cs" />
<Compile Include="ScreenSystem\PhysicsGameScreen.cs" />
<Compile Include="ScreenSystem\ScreenManagerComponent.cs" />
<Compile Include="ScreenSystem\SpriteFonts.cs" />
<Compile Include="ScreenSystem\VirtualButton.cs" />
<Compile Include="ScreenSystem\VirtualStick.cs" />
<Compile Include="Settings.cs" />
</ItemGroup>
<ItemGroup>
<Reference Include="Microsoft.Phone, Version=7.0.0.0, Culture=neutral, PublicKeyToken=24eec0d8c86cda1e, processorArchitecture=MSIL" />
<Reference Include="Microsoft.Xna.Framework" />
<Reference Include="Microsoft.Xna.Framework.Avatar" />
<Reference Include="Microsoft.Xna.Framework.Game" />
<Reference Include="Microsoft.Xna.Framework.GamerServices" />
<Reference Include="Microsoft.Xna.Framework.Graphics" />
<Reference Include="Microsoft.Xna.Framework.Input.Touch, Version=4.0.0.0, Culture=neutral, PublicKeyToken=842cf8be1de50553, processorArchitecture=MSIL" />
<Reference Include="mscorlib" />
<Reference Include="System" />
<Reference Include="System.Core" />
<Reference Include="system.data.linq, Version=7.0.0.0, Culture=neutral, PublicKeyToken=24eec0d8c86cda1e, processorArchitecture=MSIL" />
<Reference Include="System.Net" />
<Reference Include="System.Xml" />
<Reference Include="System.Xml.Linq" />
<Reference Include="System.Xml.Serialization, Version=2.0.5.0, Culture=neutral, PublicKeyToken=31bf3856ad364e35, processorArchitecture=MSIL" />
</ItemGroup>
<ItemGroup />
<Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" />
<Import Project="$(MSBuildExtensionsPath)\Microsoft\XNA Game Studio\Microsoft.Xna.GameStudio.targets" />
<PropertyGroup>
<PostBuildEvent>copy "$(TargetPath)" ..\..\Combined</PostBuildEvent>
</PropertyGroup>
<!--
To modify your build process, add your task inside one of the targets below and uncomment it.
Other similar extension points exist, see Microsoft.Common.targets.
<Target Name="BeforeBuild">
</Target>
<Target Name="AfterBuild">
</Target>
-->
</Project>

6
axios/Axios_WP7.csproj.user 100644
View File

@ -0,0 +1,6 @@
<?xml version="1.0" encoding="utf-8"?>
<Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
<PropertyGroup>
<XNASelectedDeployDevice>5E7661DF-D928-40ff-B747-A4B1957194F9</XNASelectedDeployDevice>
</PropertyGroup>
</Project>

271
axios/Axios_Windows.csproj 100644
View File

@ -0,0 +1,271 @@
<?xml version="1.0" encoding="utf-8"?>
<Project DefaultTargets="Build" ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
<PropertyGroup>
<ProjectGuid>{742C938F-997D-4EFD-95D2-BB09CDADCD2E}</ProjectGuid>
<ProjectTypeGuids>{6D335F3A-9D43-41b4-9D22-F6F17C4BE596};{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}</ProjectTypeGuids>
<Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
<Platform Condition=" '$(Platform)' == '' ">x86</Platform>
<OutputType>Library</OutputType>
<AppDesignerFolder>Properties</AppDesignerFolder>
<RootNamespace>Axios</RootNamespace>
<AssemblyName>Axios.Windows</AssemblyName>
<TargetFrameworkVersion>v4.0</TargetFrameworkVersion>
<TargetFrameworkProfile>Client</TargetFrameworkProfile>
<XnaFrameworkVersion>v4.0</XnaFrameworkVersion>
<XnaPlatform>Windows</XnaPlatform>
<XnaProfile>HiDef</XnaProfile>
<XnaCrossPlatformGroupID>cd94d25d-3fe9-470c-b921-7a5abdd2494c</XnaCrossPlatformGroupID>
<XnaOutputType>Library</XnaOutputType>
</PropertyGroup>
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|x86' ">
<DebugSymbols>true</DebugSymbols>
<DebugType>full</DebugType>
<Optimize>false</Optimize>
<OutputPath>bin\x86\Debug</OutputPath>
<DefineConstants>TRACE;DEBUG;WINDOWS</DefineConstants>
<ErrorReport>prompt</ErrorReport>
<WarningLevel>4</WarningLevel>
<NoStdLib>true</NoStdLib>
<UseVSHostingProcess>false</UseVSHostingProcess>
<PlatformTarget>x86</PlatformTarget>
<XnaCompressContent>false</XnaCompressContent>
<RunCodeAnalysis>false</RunCodeAnalysis>
</PropertyGroup>
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|x86' ">
<DebugType>pdbonly</DebugType>
<Optimize>true</Optimize>
<OutputPath>bin\x86\Release</OutputPath>
<DefineConstants>TRACE;WINDOWS</DefineConstants>
<ErrorReport>prompt</ErrorReport>
<WarningLevel>4</WarningLevel>
<NoStdLib>true</NoStdLib>
<UseVSHostingProcess>false</UseVSHostingProcess>
<PlatformTarget>x86</PlatformTarget>
<XnaCompressContent>true</XnaCompressContent>
</PropertyGroup>
<ItemGroup>
<Reference Include="Microsoft.Xna.Framework, Version=4.0.0.0, Culture=neutral, PublicKeyToken=842cf8be1de50553, processorArchitecture=x86">
<Private>False</Private>
</Reference>
<Reference Include="Microsoft.Xna.Framework.Game, Version=4.0.0.0, Culture=neutral, PublicKeyToken=842cf8be1de50553, processorArchitecture=x86">
<Private>False</Private>
</Reference>
<Reference Include="Microsoft.Xna.Framework.Graphics, Version=4.0.0.0, Culture=neutral, PublicKeyToken=842cf8be1de50553, processorArchitecture=x86">
<Private>False</Private>
</Reference>
<Reference Include="Microsoft.Xna.Framework.GamerServices, Version=4.0.0.0, Culture=neutral, PublicKeyToken=842cf8be1de50553, processorArchitecture=x86">
<Private>False</Private>
</Reference>
<Reference Include="Microsoft.Xna.Framework.Input.Touch, Version=4.0.0.0, Culture=neutral, PublicKeyToken=842cf8be1de50553, processorArchitecture=MSIL" />
<Reference Include="Microsoft.Xna.Framework.Xact, Version=4.0.0.0, Culture=neutral, PublicKeyToken=842cf8be1de50553, processorArchitecture=x86">
<Private>False</Private>
</Reference>
<Reference Include="Microsoft.Xna.Framework.Video, Version=4.0.0.0, Culture=neutral, PublicKeyToken=842cf8be1de50553, processorArchitecture=x86">
<Private>False</Private>
</Reference>
<Reference Include="Microsoft.Xna.Framework.Avatar, Version=4.0.0.0, Culture=neutral, PublicKeyToken=842cf8be1de50553, processorArchitecture=x86">
<Private>False</Private>
</Reference>
<Reference Include="Microsoft.Xna.Framework.Net, Version=4.0.0.0, Culture=neutral, PublicKeyToken=842cf8be1de50553, processorArchitecture=x86">
<Private>False</Private>
</Reference>
<Reference Include="Microsoft.Xna.Framework.Storage, Version=4.0.0.0, Culture=neutral, PublicKeyToken=842cf8be1de50553, processorArchitecture=x86">
<Private>False</Private>
</Reference>
<Reference Include="mscorlib">
<Private>False</Private>
</Reference>
<Reference Include="System">
<Private>False</Private>
</Reference>
<Reference Include="System.Data" />
<Reference Include="System.Data.Linq" />
<Reference Include="System.Drawing" />
<Reference Include="System.Xml">
<Private>False</Private>
</Reference>
<Reference Include="System.Core">
<RequiredTargetFramework>4.0</RequiredTargetFramework>
<Private>False</Private>
</Reference>
<Reference Include="System.Xml.Linq">
<RequiredTargetFramework>4.0</RequiredTargetFramework>
<Private>False</Private>
</Reference>
<Reference Include="System.Net">
<Private>False</Private>
</Reference>
</ItemGroup>
<ItemGroup>
<Compile Include="Engine\AxiosBreakableGameObject.cs" />
<Compile Include="Engine\AxiosEvents.cs" />
<Compile Include="Axios_settings.cs" />
<Compile Include="Collision\Collision.cs" />
<Compile Include="Collision\Distance.cs" />
<Compile Include="Collision\DynamicTree.cs" />
<Compile Include="Collision\DynamicTreeBroadPhase.cs" />
<Compile Include="Collision\IBroadPhase.cs" />
<Compile Include="Collision\QuadTree.cs" />
<Compile Include="Collision\QuadTreeBroadPhase.cs" />
<Compile Include="Collision\Shapes\CircleShape.cs" />
<Compile Include="Collision\Shapes\EdgeShape.cs" />
<Compile Include="Collision\Shapes\LoopShape.cs" />
<Compile Include="Collision\Shapes\PolygonShape.cs" />
<Compile Include="Collision\Shapes\Shape.cs" />
<Compile Include="Collision\TimeOfImpact.cs" />
<Compile Include="Common\ConvexHull\ChainHull.cs" />
<Compile Include="Common\ConvexHull\GiftWrap.cs" />
<Compile Include="Common\ConvexHull\Melkman.cs" />
<Compile Include="Common\Decomposition\BayazitDecomposer.cs" />
<Compile Include="Common\Decomposition\CDTDecomposer.cs" />
<Compile Include="Common\Decomposition\CDT\Delaunay\DelaunayTriangle.cs" />
<Compile Include="Common\Decomposition\CDT\Delaunay\Sweep\AdvancingFront.cs" />
<Compile Include="Common\Decomposition\CDT\Delaunay\Sweep\AdvancingFrontNode.cs" />
<Compile Include="Common\Decomposition\CDT\Delaunay\Sweep\DTSweep.cs" />
<Compile Include="Common\Decomposition\CDT\Delaunay\Sweep\DTSweepConstraint.cs" />
<Compile Include="Common\Decomposition\CDT\Delaunay\Sweep\DTSweepContext.cs" />
<Compile Include="Common\Decomposition\CDT\Delaunay\Sweep\DTSweepPointComparator.cs" />
<Compile Include="Common\Decomposition\CDT\Delaunay\Sweep\PointOnEdgeException.cs" />
<Compile Include="Common\Decomposition\CDT\ITriangulatable.cs" />
<Compile Include="Common\Decomposition\CDT\Orientation.cs" />
<Compile Include="Common\Decomposition\CDT\Polygon\Polygon.cs" />
<Compile Include="Common\Decomposition\CDT\Polygon\PolygonPoint.cs" />
<Compile Include="Common\Decomposition\CDT\Polygon\PolygonSet.cs" />
<Compile Include="Common\Decomposition\CDT\Sets\ConstrainedPointSet.cs" />
<Compile Include="Common\Decomposition\CDT\Sets\PointSet.cs" />
<Compile Include="Common\Decomposition\CDT\TriangulationConstraint.cs" />
<Compile Include="Common\Decomposition\CDT\TriangulationContext.cs" />
<Compile Include="Common\Decomposition\CDT\TriangulationMode.cs" />
<Compile Include="Common\Decomposition\CDT\TriangulationPoint.cs" />
<Compile Include="Common\Decomposition\CDT\TriangulationUtil.cs" />
<Compile Include="Common\Decomposition\CDT\Util\FixedArray3.cs" />
<Compile Include="Common\Decomposition\CDT\Util\FixedBitArray3.cs" />
<Compile Include="Common\Decomposition\CDT\Util\PointGenerator.cs" />
<Compile Include="Common\Decomposition\CDT\Util\PolygonGenerator.cs" />
<Compile Include="Common\Decomposition\EarclipDecomposer.cs" />
<Compile Include="Common\Decomposition\FlipcodeDecomposer.cs" />
<Compile Include="Common\Decomposition\SeidelDecomposer.cs" />
<Compile Include="Common\FixedArray.cs" />
<Compile Include="Common\HashSet.cs" />
<Compile Include="Common\LineTools.cs" />
<Compile Include="Common\Math.cs" />
<Compile Include="Common\Path.cs" />
<Compile Include="Common\PathManager.cs" />
<Compile Include="Common\PhysicsLogic\Explosion.cs" />
<Compile Include="Common\PhysicsLogic\PhysicsLogic.cs" />
<Compile Include="Common\PolygonManipulation\CuttingTools.cs" />
<Compile Include="Common\PolygonManipulation\SimplifyTools.cs" />
<Compile Include="Common\PolygonManipulation\YuPengClipper.cs" />
<Compile Include="Common\PolygonTools.cs" />
<Compile Include="Common\Serialization.cs" />
<Compile Include="Common\TextureTools\MarchingSquares.cs" />
<Compile Include="Common\TextureTools\MSTerrain.cs" />
<Compile Include="Common\TextureTools\TextureConverter.cs" />
<Compile Include="Common\Vertices.cs" />
<Compile Include="Controllers\AbstractForceController.cs" />
<Compile Include="Controllers\BuoyancyController.cs" />
<Compile Include="Controllers\Controller.cs" />
<Compile Include="Controllers\GravityController.cs" />
<Compile Include="Controllers\SimpleWindForce.cs" />
<Compile Include="Controllers\VelocityLimitController.cs" />
<Compile Include="DebugView.cs" />
<Compile Include="DebugViewXNA.cs" />
<Compile Include="DrawingSystem\AssetCreator.cs" />
<Compile Include="DrawingSystem\LineBatch.cs" />
<Compile Include="DrawingSystem\Sprite.cs" />
<Compile Include="Dynamics\Body.cs" />
<Compile Include="Dynamics\BreakableBody.cs" />
<Compile Include="Dynamics\ContactManager.cs" />
<Compile Include="Dynamics\Contacts\Contact.cs" />
<Compile Include="Dynamics\Contacts\ContactSolver.cs" />
<Compile Include="Dynamics\Fixture.cs" />
<Compile Include="Dynamics\Island.cs" />
<Compile Include="Dynamics\Joints\AngleJoint.cs" />
<Compile Include="Dynamics\Joints\DistanceJoint.cs" />
<Compile Include="Dynamics\Joints\FixedAngleJoint.cs" />
<Compile Include="Dynamics\Joints\FixedDistanceJoint.cs" />
<Compile Include="Dynamics\Joints\FixedFrictionJoint.cs" />
<Compile Include="Dynamics\Joints\FixedLineJoint.cs" />
<Compile Include="Dynamics\Joints\FixedMouseJoint.cs" />
<Compile Include="Dynamics\Joints\FixedPrismaticJoint.cs" />
<Compile Include="Dynamics\Joints\FixedRevoluteJoint.cs" />
<Compile Include="Dynamics\Joints\FrictionJoint.cs" />
<Compile Include="Dynamics\Joints\GearJoint.cs" />
<Compile Include="Dynamics\Joints\Joint.cs" />
<Compile Include="Dynamics\Joints\LineJoint.cs" />
<Compile Include="Dynamics\Joints\PrismaticJoint.cs" />
<Compile Include="Dynamics\Joints\PulleyJoint.cs" />
<Compile Include="Dynamics\Joints\RevoluteJoint.cs" />
<Compile Include="Dynamics\Joints\RopeJoint.cs" />
<Compile Include="Dynamics\Joints\SliderJoint.cs" />
<Compile Include="Dynamics\Joints\WeldJoint.cs" />
<Compile Include="Dynamics\TimeStep.cs" />
<Compile Include="Dynamics\World.cs" />
<Compile Include="Dynamics\WorldCallbacks.cs" />
<Compile Include="Engine\AxiosGameObject.cs" />
<Compile Include="Engine\ComplexAxiosGameObject.cs" />
<Compile Include="Engine\Data\AxiosCSV.cs" />
<Compile Include="Engine\Data\AxiosDataTable.cs" />
<Compile Include="Engine\Data\DataEvents.cs" />
<Compile Include="Engine\DrawableAxiosGameObject.cs" />
<Compile Include="Engine\DrawableBreakableAxiosGameObject.cs" />
<Compile Include="Engine\Extensions\String.cs" />
<Compile Include="Engine\Extensions\Texture2D.cs" />
<Compile Include="Engine\File\AxiosFile.cs" />
<Compile Include="Engine\File\AxiosIsolatedFile.cs" />
<Compile Include="Engine\File\AxiosRegularFile.cs" />
<Compile Include="Engine\File\AxiosTitleFile.cs" />
<Compile Include="Engine\Interfaces\IAxiosFile.cs" />
<Compile Include="Engine\Interfaces\IAxiosGameObject.cs" />
<Compile Include="Engine\Interfaces\IDrawableAxiosGameObject.cs" />
<Compile Include="Engine\Log\AxiosLog.cs" />
<Compile Include="Engine\SimpleAxiosGameObject.cs" />
<Compile Include="Engine\SimpleDrawableAxiosGameObject.cs" />
<Compile Include="Engine\AxiosTimer.cs" />
<Compile Include="Engine\Singleton.cs" />
<Compile Include="Engine\UI\AxiosButton.cs" />
<Compile Include="Engine\UI\AxiosUIObject.cs" />
<Compile Include="Factories\BodyFactory.cs" />
<Compile Include="Factories\FixtureFactory.cs" />
<Compile Include="Factories\JointFactory.cs" />
<Compile Include="Factories\LinkFactory.cs" />
<Compile Include="PrimitiveBatch.cs" />
<Compile Include="Properties\AssemblyInfo.cs" />
<Compile Include="ScreenSystem\BackgroundScreen.cs" />
<Compile Include="ScreenSystem\Camera2D.cs" />
<Compile Include="ScreenSystem\ConvertUnits.cs" />
<Compile Include="ScreenSystem\FramerateCounterComponent.cs" />
<Compile Include="ScreenSystem\GameScreen.cs" />
<Compile Include="ScreenSystem\IDemoScreen.cs" />
<Compile Include="ScreenSystem\InputHelper.cs" />
<Compile Include="Engine\AxiosGameScreen.cs" />
<Compile Include="ScreenSystem\LogoScreen.cs" />
<Compile Include="ScreenSystem\MenuButton.cs" />
<Compile Include="ScreenSystem\MenuEntry.cs" />
<Compile Include="ScreenSystem\MenuScreen.cs" />
<Compile Include="ScreenSystem\MessageBoxScreen.cs" />
<Compile Include="ScreenSystem\PhysicsGameScreen.cs" />
<Compile Include="ScreenSystem\ScreenManagerComponent.cs" />
<Compile Include="ScreenSystem\SpriteFonts.cs" />
<Compile Include="ScreenSystem\VirtualButton.cs" />
<Compile Include="ScreenSystem\VirtualStick.cs" />
<Compile Include="Settings.cs" />
</ItemGroup>
<ItemGroup>
<None Include="AxiosEngine.cd" />
</ItemGroup>
<ItemGroup />
<Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" />
<Import Project="$(MSBuildExtensionsPath)\Microsoft\XNA Game Studio\Microsoft.Xna.GameStudio.targets" />
<PropertyGroup>
<PostBuildEvent>copy "$(TargetPath)" ..\..\Combined</PostBuildEvent>
</PropertyGroup>
<!--
To modify your build process, add your task inside one of the targets below and uncomment it.
Other similar extension points exist, see Microsoft.Common.targets.
<Target Name="BeforeBuild">
</Target>
<Target Name="AfterBuild">
</Target>
-->
</Project>

6
axios/Axios_Windows.csproj.user 100644
View File

@ -0,0 +1,6 @@
<?xml version="1.0" encoding="utf-8"?>
<Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
<PropertyGroup>
<ProjectView>ProjectFiles</ProjectView>
</PropertyGroup>
</Project>

235
axios/Axios_Xbox_360.csproj 100644
View File

@ -0,0 +1,235 @@
<?xml version="1.0" encoding="utf-8"?>
<Project DefaultTargets="Build" ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
<PropertyGroup>
<ProjectGuid>{B5664516-72B7-4BA3-9F72-25CAA90867D8}</ProjectGuid>
<ProjectTypeGuids>{6D335F3A-9D43-41b4-9D22-F6F17C4BE596};{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}</ProjectTypeGuids>
<Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
<Platform Condition=" '$(Platform)' == '' ">Xbox 360</Platform>
<OutputType>Library</OutputType>
<AppDesignerFolder>Properties</AppDesignerFolder>
<RootNamespace>Axios</RootNamespace>
<AssemblyName>Axios.Xbox360</AssemblyName>
<TargetFrameworkVersion>v4.0</TargetFrameworkVersion>
<TargetFrameworkProfile>Client</TargetFrameworkProfile>
<XnaFrameworkVersion>v4.0</XnaFrameworkVersion>
<XnaPlatform>Xbox 360</XnaPlatform>
<XnaProfile>HiDef</XnaProfile>
<XnaCrossPlatformGroupID>cd94d25d-3fe9-470c-b921-7a5abdd2494c</XnaCrossPlatformGroupID>
<XnaOutputType>Library</XnaOutputType>
<Thumbnail Condition="$(XnaOutputType)=='Game'">GameThumbnail.png</Thumbnail>
</PropertyGroup>
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|Xbox 360' ">
<OutputPath>bin\Xbox 360\Release</OutputPath>
<ErrorReport>prompt</ErrorReport>
<WarningLevel>4</WarningLevel>
<NoStdLib>true</NoStdLib>
<UseVSHostingProcess>false</UseVSHostingProcess>
<DebugType>pdbonly</DebugType>
<Optimize>true</Optimize>
<DefineConstants>TRACE;XBOX;XBOX360</DefineConstants>
<XnaCompressContent>true</XnaCompressContent>
</PropertyGroup>
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|Xbox 360' ">
<OutputPath>bin\Xbox 360\Debug</OutputPath>
<ErrorReport>prompt</ErrorReport>
<WarningLevel>4</WarningLevel>
<NoStdLib>true</NoStdLib>
<UseVSHostingProcess>false</UseVSHostingProcess>
<DebugSymbols>true</DebugSymbols>
<DebugType>full</DebugType>
<Optimize>false</Optimize>
<DefineConstants>DEBUG;TRACE;XBOX;XBOX360</DefineConstants>
<XnaCompressContent>true</XnaCompressContent>
</PropertyGroup>
<ItemGroup>
<None Include="AxiosEngine.cd" />
</ItemGroup>
<ItemGroup>
<Compile Include="Axios_settings.cs" />
<Compile Include="Collision\Collision.cs" />
<Compile Include="Collision\Distance.cs" />
<Compile Include="Collision\DynamicTree.cs" />
<Compile Include="Collision\DynamicTreeBroadPhase.cs" />
<Compile Include="Collision\IBroadPhase.cs" />
<Compile Include="Collision\QuadTree.cs" />
<Compile Include="Collision\QuadTreeBroadPhase.cs" />
<Compile Include="Collision\Shapes\CircleShape.cs" />
<Compile Include="Collision\Shapes\EdgeShape.cs" />
<Compile Include="Collision\Shapes\LoopShape.cs" />
<Compile Include="Collision\Shapes\PolygonShape.cs" />
<Compile Include="Collision\Shapes\Shape.cs" />
<Compile Include="Collision\TimeOfImpact.cs" />
<Compile Include="Common\ConvexHull\ChainHull.cs" />
<Compile Include="Common\ConvexHull\GiftWrap.cs" />
<Compile Include="Common\ConvexHull\Melkman.cs" />
<Compile Include="Common\Decomposition\BayazitDecomposer.cs" />
<Compile Include="Common\Decomposition\CDTDecomposer.cs" />
<Compile Include="Common\Decomposition\CDT\Delaunay\DelaunayTriangle.cs" />
<Compile Include="Common\Decomposition\CDT\Delaunay\Sweep\AdvancingFront.cs" />
<Compile Include="Common\Decomposition\CDT\Delaunay\Sweep\AdvancingFrontNode.cs" />
<Compile Include="Common\Decomposition\CDT\Delaunay\Sweep\DTSweep.cs" />
<Compile Include="Common\Decomposition\CDT\Delaunay\Sweep\DTSweepConstraint.cs" />
<Compile Include="Common\Decomposition\CDT\Delaunay\Sweep\DTSweepContext.cs" />
<Compile Include="Common\Decomposition\CDT\Delaunay\Sweep\DTSweepPointComparator.cs" />
<Compile Include="Common\Decomposition\CDT\Delaunay\Sweep\PointOnEdgeException.cs" />
<Compile Include="Common\Decomposition\CDT\ITriangulatable.cs" />
<Compile Include="Common\Decomposition\CDT\Orientation.cs" />
<Compile Include="Common\Decomposition\CDT\Polygon\Polygon.cs" />
<Compile Include="Common\Decomposition\CDT\Polygon\PolygonPoint.cs" />
<Compile Include="Common\Decomposition\CDT\Polygon\PolygonSet.cs" />
<Compile Include="Common\Decomposition\CDT\Sets\ConstrainedPointSet.cs" />
<Compile Include="Common\Decomposition\CDT\Sets\PointSet.cs" />
<Compile Include="Common\Decomposition\CDT\TriangulationConstraint.cs" />
<Compile Include="Common\Decomposition\CDT\TriangulationContext.cs" />
<Compile Include="Common\Decomposition\CDT\TriangulationMode.cs" />
<Compile Include="Common\Decomposition\CDT\TriangulationPoint.cs" />
<Compile Include="Common\Decomposition\CDT\TriangulationUtil.cs" />
<Compile Include="Common\Decomposition\CDT\Util\FixedArray3.cs" />
<Compile Include="Common\Decomposition\CDT\Util\FixedBitArray3.cs" />
<Compile Include="Common\Decomposition\CDT\Util\PointGenerator.cs" />
<Compile Include="Common\Decomposition\CDT\Util\PolygonGenerator.cs" />
<Compile Include="Common\Decomposition\EarclipDecomposer.cs" />
<Compile Include="Common\Decomposition\FlipcodeDecomposer.cs" />
<Compile Include="Common\Decomposition\SeidelDecomposer.cs" />
<Compile Include="Common\FixedArray.cs" />
<Compile Include="Common\HashSet.cs" />
<Compile Include="Common\LineTools.cs" />
<Compile Include="Common\Math.cs" />
<Compile Include="Common\Path.cs" />
<Compile Include="Common\PathManager.cs" />
<Compile Include="Common\PhysicsLogic\Explosion.cs" />
<Compile Include="Common\PhysicsLogic\PhysicsLogic.cs" />
<Compile Include="Common\PolygonManipulation\CuttingTools.cs" />
<Compile Include="Common\PolygonManipulation\SimplifyTools.cs" />
<Compile Include="Common\PolygonManipulation\YuPengClipper.cs" />
<Compile Include="Common\PolygonTools.cs" />
<Compile Include="Common\Serialization.cs" />
<Compile Include="Common\TextureTools\MarchingSquares.cs" />
<Compile Include="Common\TextureTools\MSTerrain.cs" />
<Compile Include="Common\TextureTools\TextureConverter.cs" />
<Compile Include="Common\Vertices.cs" />
<Compile Include="Controllers\AbstractForceController.cs" />
<Compile Include="Controllers\BuoyancyController.cs" />
<Compile Include="Controllers\Controller.cs" />
<Compile Include="Controllers\GravityController.cs" />
<Compile Include="Controllers\SimpleWindForce.cs" />
<Compile Include="Controllers\VelocityLimitController.cs" />
<Compile Include="DebugView.cs" />
<Compile Include="DebugViewXNA.cs" />
<Compile Include="DrawingSystem\AssetCreator.cs" />
<Compile Include="DrawingSystem\LineBatch.cs" />
<Compile Include="DrawingSystem\Sprite.cs" />
<Compile Include="Dynamics\Body.cs" />
<Compile Include="Dynamics\BreakableBody.cs" />
<Compile Include="Dynamics\ContactManager.cs" />
<Compile Include="Dynamics\Contacts\Contact.cs" />
<Compile Include="Dynamics\Contacts\ContactSolver.cs" />
<Compile Include="Dynamics\Fixture.cs" />
<Compile Include="Dynamics\Island.cs" />
<Compile Include="Dynamics\Joints\AngleJoint.cs" />
<Compile Include="Dynamics\Joints\DistanceJoint.cs" />
<Compile Include="Dynamics\Joints\FixedAngleJoint.cs" />
<Compile Include="Dynamics\Joints\FixedDistanceJoint.cs" />
<Compile Include="Dynamics\Joints\FixedFrictionJoint.cs" />
<Compile Include="Dynamics\Joints\FixedLineJoint.cs" />
<Compile Include="Dynamics\Joints\FixedMouseJoint.cs" />
<Compile Include="Dynamics\Joints\FixedPrismaticJoint.cs" />
<Compile Include="Dynamics\Joints\FixedRevoluteJoint.cs" />
<Compile Include="Dynamics\Joints\FrictionJoint.cs" />
<Compile Include="Dynamics\Joints\GearJoint.cs" />
<Compile Include="Dynamics\Joints\Joint.cs" />
<Compile Include="Dynamics\Joints\LineJoint.cs" />
<Compile Include="Dynamics\Joints\PrismaticJoint.cs" />
<Compile Include="Dynamics\Joints\PulleyJoint.cs" />
<Compile Include="Dynamics\Joints\RevoluteJoint.cs" />
<Compile Include="Dynamics\Joints\RopeJoint.cs" />
<Compile Include="Dynamics\Joints\SliderJoint.cs" />
<Compile Include="Dynamics\Joints\WeldJoint.cs" />
<Compile Include="Dynamics\TimeStep.cs" />
<Compile Include="Dynamics\World.cs" />
<Compile Include="Dynamics\WorldCallbacks.cs" />
<Compile Include="Engine\AxiosBreakableGameObject.cs" />
<Compile Include="Engine\AxiosEvents.cs" />
<Compile Include="Engine\AxiosGameObject.cs" />
<Compile Include="Engine\AxiosGameScreen.cs" />
<Compile Include="Engine\AxiosTimer.cs" />
<Compile Include="Engine\ComplexAxiosGameObject.cs" />
<Compile Include="Engine\Data\AxiosCSV.cs" />
<Compile Include="Engine\Data\AxiosDataTable.cs" />
<Compile Include="Engine\Data\DataEvents.cs" />
<Compile Include="Engine\DrawableAxiosGameObject.cs" />
<Compile Include="Engine\DrawableBreakableAxiosGameObject.cs" />
<Compile Include="Engine\Extensions\String.cs" />
<Compile Include="Engine\Extensions\Texture2D.cs" />
<Compile Include="Engine\File\AxiosFile.cs" />
<Compile Include="Engine\File\AxiosIsolatedFile.cs" />
<Compile Include="Engine\File\AxiosRegularFile.cs" />
<Compile Include="Engine\File\AxiosTitleFile.cs" />
<Compile Include="Engine\Interfaces\IAxiosFile.cs" />
<Compile Include="Engine\Interfaces\IAxiosGameObject.cs" />
<Compile Include="Engine\Interfaces\IDrawableAxiosGameObject.cs" />
<Compile Include="Engine\Log\AxiosLog.cs" />
<Compile Include="Engine\SimpleAxiosGameObject.cs" />
<Compile Include="Engine\SimpleDrawableAxiosGameObject.cs" />
<Compile Include="Engine\Singleton.cs" />
<Compile Include="Engine\UI\AxiosButton.cs" />
<Compile Include="Engine\UI\AxiosUIObject.cs" />
<Compile Include="Factories\BodyFactory.cs" />
<Compile Include="Factories\FixtureFactory.cs" />
<Compile Include="Factories\JointFactory.cs" />
<Compile Include="Factories\LinkFactory.cs" />
<Compile Include="PrimitiveBatch.cs" />
<Compile Include="Properties\AssemblyInfo.cs" />
<Compile Include="ScreenSystem\BackgroundScreen.cs" />
<Compile Include="ScreenSystem\Camera2D.cs" />
<Compile Include="ScreenSystem\ConvertUnits.cs" />
<Compile Include="ScreenSystem\FramerateCounterComponent.cs" />
<Compile Include="ScreenSystem\GameScreen.cs" />
<Compile Include="ScreenSystem\IDemoScreen.cs" />
<Compile Include="ScreenSystem\InputHelper.cs" />
<Compile Include="ScreenSystem\LogoScreen.cs" />
<Compile Include="ScreenSystem\MenuButton.cs" />
<Compile Include="ScreenSystem\MenuEntry.cs" />
<Compile Include="ScreenSystem\MenuScreen.cs" />
<Compile Include="ScreenSystem\MessageBoxScreen.cs" />
<Compile Include="ScreenSystem\PhysicsGameScreen.cs" />
<Compile Include="ScreenSystem\ScreenManagerComponent.cs" />
<Compile Include="ScreenSystem\SpriteFonts.cs" />
<Compile Include="ScreenSystem\VirtualButton.cs" />
<Compile Include="ScreenSystem\VirtualStick.cs" />
<Compile Include="Settings.cs" />
</ItemGroup>
<ItemGroup>
<Reference Include="Microsoft.Xna.Framework" />
<Reference Include="Microsoft.Xna.Framework.Avatar" />
<Reference Include="Microsoft.Xna.Framework.Game" />
<Reference Include="Microsoft.Xna.Framework.GamerServices" />
<Reference Include="Microsoft.Xna.Framework.Graphics" />
<Reference Include="Microsoft.Xna.Framework.Input.Touch, Version=4.0.0.0, Culture=neutral, PublicKeyToken=842cf8be1de50553, processorArchitecture=MSIL" />
<Reference Include="Microsoft.Xna.Framework.Net" />
<Reference Include="Microsoft.Xna.Framework.Storage" />
<Reference Include="Microsoft.Xna.Framework.Video" />
<Reference Include="Microsoft.Xna.Framework.Xact" />
<Reference Include="mscorlib" />
<Reference Include="System" />
<Reference Include="System.Core" />
<Reference Include="System.Net" />
<Reference Include="System.Xml" />
<Reference Include="System.Xml.Linq" />
<Reference Include="System.Xml.Serialization, Version=2.0.5.0, Culture=neutral, PublicKeyToken=31bf3856ad364e35, processorArchitecture=MSIL" />
</ItemGroup>
<ItemGroup />
<Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" />
<Import Project="$(MSBuildExtensionsPath)\Microsoft\XNA Game Studio\Microsoft.Xna.GameStudio.targets" />
<PropertyGroup>
<PostBuildEvent>copy "$(TargetPath)" ..\..\Combined</PostBuildEvent>
</PropertyGroup>
<!--
To modify your build process, add your task inside one of the targets below and uncomment it.
Other similar extension points exist, see Microsoft.Common.targets.
<Target Name="BeforeBuild">
</Target>
<Target Name="AfterBuild">
</Target>
-->
</Project>

155
axios/Axios_settings.cs 100644
View File

@ -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;
/// <summary>
/// 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.
/// </summary>
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;
}
}

1945
axios/Collision/Collision.cs 100644
View File

File diff suppressed because it is too large Load Diff

780
axios/Collision/Distance.cs 100644
View File

@ -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
{
/// <summary>
/// A distance proxy is used by the GJK algorithm.
/// It encapsulates any shape.
/// </summary>
public class DistanceProxy
{
internal float Radius;
internal Vertices Vertices = new Vertices();
/// <summary>
/// Initialize the proxy using the given shape. The shape
/// must remain in scope while the proxy is in use.
/// </summary>
/// <param name="shape">The shape.</param>
/// <param name="index">The index.</param>
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;
}
}
/// <summary>
/// Get the supporting vertex index in the given direction.
/// </summary>
/// <param name="direction">The direction.</param>
/// <returns></returns>
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;
}
/// <summary>
/// Get the supporting vertex in the given direction.
/// </summary>
/// <param name="direction">The direction.</param>
/// <returns></returns>
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];
}
}
/// <summary>
/// Used to warm start ComputeDistance.
/// Set count to zero on first call.
/// </summary>
public struct SimplexCache
{
/// <summary>
/// Length or area
/// </summary>
public ushort Count;
/// <summary>
/// Vertices on shape A
/// </summary>
public FixedArray3<byte> IndexA;
/// <summary>
/// Vertices on shape B
/// </summary>
public FixedArray3<byte> IndexB;
public float Metric;
}
/// <summary>
/// Input for ComputeDistance.
/// You have to option to use the shape radii
/// in the computation.
/// </summary>
public class DistanceInput
{
public DistanceProxy ProxyA = new DistanceProxy();
public DistanceProxy ProxyB = new DistanceProxy();
public Transform TransformA;
public Transform TransformB;
public bool UseRadii;
}
/// <summary>
/// Output for ComputeDistance.
/// </summary>
public struct DistanceOutput
{
public float Distance;
/// <summary>
/// Number of GJK iterations used
/// </summary>
public int Iterations;
/// <summary>
/// Closest point on shapeA
/// </summary>
public Vector2 PointA;
/// <summary>
/// Closest point on shapeB
/// </summary>
public Vector2 PointB;
}
internal struct SimplexVertex
{
/// <summary>
/// Barycentric coordinate for closest point
/// </summary>
public float A;
/// <summary>
/// wA index
/// </summary>
public int IndexA;
/// <summary>
/// wB index
/// </summary>
public int IndexB;
/// <summary>
/// wB - wA
/// </summary>
public Vector2 W;
/// <summary>
/// Support point in proxyA
/// </summary>
public Vector2 WA;
/// <summary>
/// Support point in proxyB
/// </summary>
public Vector2 WB;
}
internal struct Simplex
{
internal int Count;
internal FixedArray3<SimplexVertex> 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<int> saveA = new FixedArray3<int>();
FixedArray3<int> saveB = new FixedArray3<int>();
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;
}
}
}
}
}

654
axios/Collision/DynamicTree.cs 100644
View File

@ -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
{
/// <summary>
/// A node in the dynamic tree. The client does not interact with this directly.
/// </summary>
internal struct DynamicTreeNode<T>
{
/// <summary>
/// This is the fattened AABB.
/// </summary>
internal AABB AABB;
internal int Child1;
internal int Child2;
internal int LeafCount;
internal int ParentOrNext;
internal T UserData;
internal bool IsLeaf()
{
return Child1 == DynamicTree<T>.NullNode;
}
}
/// <summary>
/// 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.
/// </summary>
public class DynamicTree<T>
{
internal const int NullNode = -1;
private static Stack<int> _stack = new Stack<int>(256);
private int _freeList;
private int _insertionCount;
private int _nodeCapacity;
private int _nodeCount;
private DynamicTreeNode<T>[] _nodes;
/// <summary>
/// This is used incrementally traverse the tree for re-balancing.
/// </summary>
private int _path;
private int _root;
/// <summary>
/// Constructing the tree initializes the node pool.
/// </summary>
public DynamicTree()
{
_root = NullNode;
_nodeCapacity = 16;
_nodes = new DynamicTreeNode<T>[_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;
}
/// <summary>
/// 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.
/// /// </summary>
/// <param name="aabb">The aabb.</param>
/// <param name="userData">The user data.</param>
/// <returns>Index of the created proxy</returns>
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;
}
/// <summary>
/// Destroy a proxy. This asserts if the id is invalid.
/// </summary>
/// <param name="proxyId">The proxy id.</param>
public void RemoveProxy(int proxyId)
{
Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity);
Debug.Assert(_nodes[proxyId].IsLeaf());
RemoveLeaf(proxyId);
FreeNode(proxyId);
}
/// <summary>
/// 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.
/// </summary>
/// <param name="proxyId">The proxy id.</param>
/// <param name="aabb">The aabb.</param>
/// <param name="displacement">The displacement.</param>
/// <returns>true if the proxy was re-inserted.</returns>
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;
}
/// <summary>
/// Perform some iterations to re-balance the tree.
/// </summary>
/// <param name="iterations">The iterations.</param>
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);
}
}
/// <summary>
/// Get proxy user data.
/// </summary>
/// <typeparam name="T"></typeparam>
/// <param name="proxyId">The proxy id.</param>
/// <returns>the proxy user data or 0 if the id is invalid.</returns>
public T GetUserData(int proxyId)
{
Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity);
return _nodes[proxyId].UserData;
}
/// <summary>
/// Get the fat AABB for a proxy.
/// </summary>
/// <param name="proxyId">The proxy id.</param>
/// <param name="fatAABB">The fat AABB.</param>
public void GetFatAABB(int proxyId, out AABB fatAABB)
{
Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity);
fatAABB = _nodes[proxyId].AABB;
}
/// <summary>
/// Compute the height of the binary tree in O(N) time. Should not be
/// called often.
/// </summary>
/// <returns></returns>
public int ComputeHeight()
{
return ComputeHeight(_root);
}
/// <summary>
/// Query an AABB for overlapping proxies. The callback class
/// is called for each proxy that overlaps the supplied AABB.
/// </summary>
/// <param name="callback">The callback.</param>
/// <param name="aabb">The aabb.</param>
public void Query(Func<int, bool> callback, ref AABB aabb)
{
_stack.Clear();
_stack.Push(_root);
while (_stack.Count > 0)
{
int nodeId = _stack.Pop();
if (nodeId == NullNode)
{
continue;
}
DynamicTreeNode<T> 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);
}
}
}
}
/// <summary>
/// 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.
/// </summary>
/// <param name="callback">A callback class that is called for each proxy that is hit by the ray.</param>
/// <param name="input">The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param>
public void RayCast(Func<RayCastInput, int, float> 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<T> 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<T> 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<T>[] oldNodes = _nodes;
_nodeCapacity *= 2;
_nodes = new DynamicTreeNode<T>[_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<T> node = _nodes[nodeId];
int height1 = ComputeHeight(node.Child1);
int height2 = ComputeHeight(node.Child2);
return 1 + Math.Max(height1, height2);
}
}
}

324
axios/Collision/DynamicTreeBroadPhase.cs 100644
View File

@ -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<Pair>
{
public int ProxyIdA;
public int ProxyIdB;
#region IComparable<Pair> 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
}
/// <summary>
/// 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.
/// </summary>
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<int, bool> _queryCallback;
private int _queryProxyId;
private DynamicTree<FixtureProxy> _tree = new DynamicTree<FixtureProxy>();
public DynamicTreeBroadPhase()
{
_queryCallback = new Func<int, bool>(QueryCallback);
_pairCapacity = 16;
_pairBuffer = new Pair[_pairCapacity];
_moveCapacity = 16;
_moveBuffer = new int[_moveCapacity];
}
#region IBroadPhase Members
/// <summary>
/// Get the number of proxies.
/// </summary>
/// <value>The proxy count.</value>
public int ProxyCount
{
get { return _proxyCount; }
}
/// <summary>
/// Create a proxy with an initial AABB. Pairs are not reported until
/// UpdatePairs is called.
/// </summary>
/// <param name="aabb">The aabb.</param>
/// <param name="proxy">The user data.</param>
/// <returns></returns>
public int AddProxy(ref FixtureProxy proxy)
{
int proxyId = _tree.AddProxy(ref proxy.AABB, proxy);
++_proxyCount;
BufferMove(proxyId);
return proxyId;
}
/// <summary>
/// Destroy a proxy. It is up to the client to remove any pairs.
/// </summary>
/// <param name="proxyId">The proxy id.</param>
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);
}
}
/// <summary>
/// Get the AABB for a proxy.
/// </summary>
/// <param name="proxyId">The proxy id.</param>
/// <param name="aabb">The aabb.</param>
public void GetFatAABB(int proxyId, out AABB aabb)
{
_tree.GetFatAABB(proxyId, out aabb);
}
/// <summary>
/// Get user data from a proxy. Returns null if the id is invalid.
/// </summary>
/// <param name="proxyId">The proxy id.</param>
/// <returns></returns>
public FixtureProxy GetProxy(int proxyId)
{
return _tree.GetUserData(proxyId);
}
/// <summary>
/// Test overlap of fat AABBs.
/// </summary>
/// <param name="proxyIdA">The proxy id A.</param>
/// <param name="proxyIdB">The proxy id B.</param>
/// <returns></returns>
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);
}
/// <summary>
/// Update the pairs. This results in pair callbacks. This can only add pairs.
/// </summary>
/// <param name="callback">The callback.</param>
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);
}
/// <summary>
/// Query an AABB for overlapping proxies. The callback class
/// is called for each proxy that overlaps the supplied AABB.
/// </summary>
/// <param name="callback">The callback.</param>
/// <param name="aabb">The aabb.</param>
public void Query(Func<int, bool> callback, ref AABB aabb)
{
_tree.Query(callback, ref aabb);
}
/// <summary>
/// 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.
/// </summary>
/// <param name="callback">A callback class that is called for each proxy that is hit by the ray.</param>
/// <param name="input">The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param>
public void RayCast(Func<RayCastInput, int, float> callback, ref RayCastInput input)
{
_tree.RayCast(callback, ref input);
}
public void TouchProxy(int proxyId)
{
BufferMove(proxyId);
}
#endregion
/// <summary>
/// Compute the height of the embedded tree.
/// </summary>
/// <returns></returns>
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;
}
}
}

30
axios/Collision/IBroadPhase.cs 100644
View File

@ -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<int, bool> callback, ref AABB aabb);
void RayCast(Func<RayCastInput, int, float> callback, ref RayCastInput input);
}
}

267
axios/Collision/QuadTree.cs 100644
View File

@ -0,0 +1,267 @@
using System;
using System.Collections.Generic;
using FarseerPhysics.Collision;
using Microsoft.Xna.Framework;
public class Element<T>
{
public QuadTree<T> Parent;
public AABB Span;
public T Value;
public Element(T value, AABB span)
{
Span = span;
Value = value;
Parent = null;
}
}
public class QuadTree<T>
{
public int MaxBucket;
public int MaxDepth;
public List<Element<T>> Nodes;
public AABB Span;
public QuadTree<T>[] SubTrees;
public QuadTree(AABB span, int maxbucket, int maxdepth)
{
Span = span;
Nodes = new List<Element<T>>();
MaxBucket = maxbucket;
MaxDepth = maxdepth;
}
public bool IsPartitioned
{
get { return SubTrees != null; }
}
/// <summary>
/// returns the quadrant of span that entirely contains test. if none, return 0.
/// </summary>
/// <param name="span"></param>
/// <param name="test"></param>
/// <returns></returns>
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<T> 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<T>[4];
SubTrees[0] = new QuadTree<T>(Span.Q1, MaxBucket, MaxDepth - 1);
SubTrees[1] = new QuadTree<T>(Span.Q2, MaxBucket, MaxDepth - 1);
SubTrees[2] = new QuadTree<T>(Span.Q3, MaxBucket, MaxDepth - 1);
SubTrees[3] = new QuadTree<T>(Span.Q4, MaxBucket, MaxDepth - 1);
List<Element<T>> remNodes = new List<Element<T>>();
//nodes that are not fully contained by any quadrant
foreach (Element<T> 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;
}
}
}
/// <summary>
/// tests if ray intersects AABB
/// </summary>
/// <param name="aabb"></param>
/// <returns></returns>
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<Element<T>, bool> callback, ref AABB searchR)
{
Stack<QuadTree<T>> stack = new Stack<QuadTree<T>>();
stack.Push(this);
while (stack.Count > 0)
{
QuadTree<T> qt = stack.Pop();
if (!AABB.TestOverlap(ref searchR, ref qt.Span))
continue;
foreach (Element<T> n in qt.Nodes)
if (AABB.TestOverlap(ref searchR, ref n.Span))
{
if (!callback(n)) return;
}
if (qt.IsPartitioned)
foreach (QuadTree<T> st in qt.SubTrees)
stack.Push(st);
}
}
public void RayCast(Func<RayCastInput, Element<T>, float> callback, ref RayCastInput input)
{
Stack<QuadTree<T>> stack = new Stack<QuadTree<T>>();
stack.Push(this);
float maxFraction = input.MaxFraction;
Vector2 p1 = input.Point1;
Vector2 p2 = p1 + (input.Point2 - input.Point1) * maxFraction;
while (stack.Count > 0)
{
QuadTree<T> qt = stack.Pop();
if (!RayCastAABB(qt.Span, p1, p2))
continue;
foreach (Element<T> 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<T> st in qt.SubTrees)
stack.Push(st);
}
}
public void GetAllNodesR(ref List<Element<T>> nodes)
{
nodes.AddRange(Nodes);
if (IsPartitioned)
foreach (QuadTree<T> st in SubTrees) st.GetAllNodesR(ref nodes);
}
public void RemoveNode(Element<T> node)
{
node.Parent.Nodes.Remove(node);
}
public void Reconstruct()
{
List<Element<T>> allNodes = new List<Element<T>>();
GetAllNodesR(ref allNodes);
Clear();
allNodes.ForEach(AddNode);
}
public void Clear()
{
Nodes.Clear();
SubTrees = null;
}
}

249
axios/Collision/QuadTreeBroadPhase.cs 100644
View File

@ -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<int, Element<FixtureProxy>> _idRegister;
private List<Element<FixtureProxy>> _moveBuffer;
private List<Pair> _pairBuffer;
private QuadTree<FixtureProxy> _quadTree;
private int _treeMoveNum;
/// <summary>
/// Creates a new quad tree broadphase with the specified span.
/// </summary>
/// <param name="span">the maximum span of the tree (world size)</param>
public QuadTreeBroadPhase(AABB span)
{
_quadTree = new QuadTree<FixtureProxy>(span, 5, 10);
_idRegister = new Dictionary<int, Element<FixtureProxy>>();
_moveBuffer = new List<Element<FixtureProxy>>();
_pairBuffer = new List<Pair>();
}
#region IBroadPhase Members
///<summary>
/// The number of proxies
///</summary>
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<FixtureProxy> 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;
}
}
/// <summary>
/// Test overlap of fat AABBs.
/// </summary>
/// <param name="proxyIdA">The proxy id A.</param>
/// <param name="proxyIdB">The proxy id B.</param>
/// <returns></returns>
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<FixtureProxy> qtnode = new Element<FixtureProxy>(proxy, aabb);
_idRegister.Add(proxyID, qtnode);
_quadTree.AddNode(qtnode);
return proxyID;
}
public void RemoveProxy(int proxyId)
{
if (_idRegister.ContainsKey(proxyId))
{
Element<FixtureProxy> 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<FixtureProxy> 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<int, bool> callback, ref AABB query)
{
_quadTree.QueryAABB(TransformPredicate(callback), ref query);
}
public void RayCast(Func<RayCastInput, int, float> 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<Element<FixtureProxy>, bool> TransformPredicate(Func<int, bool> idPredicate)
{
Func<Element<FixtureProxy>, bool> qtPred = qtnode => idPredicate(qtnode.Value.ProxyId);
return qtPred;
}
private Func<RayCastInput, Element<FixtureProxy>, float> TransformRayCallback(
Func<RayCastInput, int, float> callback)
{
Func<RayCastInput, Element<FixtureProxy>, 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<FixtureProxy> elem in _idRegister.Values)
_quadTree.AddNode(elem);
}
private void ReinsertNode(Element<FixtureProxy> qtnode)
{
_quadTree.RemoveNode(qtnode);
_quadTree.AddNode(qtnode);
if (++_treeMoveNum > TreeUpdateThresh)
{
ReconstructTree();
_treeMoveNum = 0;
}
}
private void BufferMove(Element<FixtureProxy> proxy)
{
_moveBuffer.Add(proxy);
}
private void UnbufferMove(Element<FixtureProxy> proxy)
{
_moveBuffer.Remove(proxy);
}
}

207
axios/Collision/Shapes/CircleShape.cs 100644
View File

@ -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;
}
/// <summary>
/// Test a point for containment in this shape. This only works for convex shapes.
/// </summary>
/// <param name="transform">The shape world transform.</param>
/// <param name="point">a point in world coordinates.</param>
/// <returns>True if the point is inside the shape</returns>
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;
}
/// <summary>
/// Cast a ray against a child shape.
/// </summary>
/// <param name="output">The ray-cast results.</param>
/// <param name="input">The ray-cast input parameters.</param>
/// <param name="transform">The transform to be applied to the shape.</param>
/// <param name="childIndex">The child shape index.</param>
/// <returns>True if the ray-cast hits the shape</returns>
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;
}
/// <summary>
/// Given a transform, compute the associated axis aligned bounding box for a child shape.
/// </summary>
/// <param name="aabb">The aabb results.</param>
/// <param name="transform">The world transform of the shape.</param>
/// <param name="childIndex">The child shape index.</param>
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);
}
/// <summary>
/// Compute the mass properties of this shape using its dimensions and density.
/// The inertia tensor is computed about the local origin, not the centroid.
/// </summary>
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;
}
}
}

266
axios/Collision/Shapes/EdgeShape.cs 100644
View File

@ -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
{
/// <summary>
/// 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.
/// </summary>
public class EdgeShape : Shape
{
public bool HasVertex0, HasVertex3;
/// <summary>
/// Optional adjacent vertices. These are used for smooth collision.
/// </summary>
public Vector2 Vertex0;
/// <summary>
/// Optional adjacent vertices. These are used for smooth collision.
/// </summary>
public Vector2 Vertex3;
/// <summary>
/// Edge start vertex
/// </summary>
private Vector2 _vertex1;
/// <summary>
/// Edge end vertex
/// </summary>
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; }
}
/// <summary>
/// These are the edge vertices
/// </summary>
public Vector2 Vertex1
{
get { return _vertex1; }
set
{
_vertex1 = value;
ComputeProperties();
}
}
/// <summary>
/// These are the edge vertices
/// </summary>
public Vector2 Vertex2
{
get { return _vertex2; }
set
{
_vertex2 = value;
ComputeProperties();
}
}
/// <summary>
/// Set this as an isolated edge.
/// </summary>
/// <param name="start">The start.</param>
/// <param name="end">The end.</param>
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;
}
/// <summary>
/// Test a point for containment in this shape. This only works for convex shapes.
/// </summary>
/// <param name="transform">The shape world transform.</param>
/// <param name="point">a point in world coordinates.</param>
/// <returns>True if the point is inside the shape</returns>
public override bool TestPoint(ref Transform transform, ref Vector2 point)
{
return false;
}
/// <summary>
/// Cast a ray against a child shape.
/// </summary>
/// <param name="output">The ray-cast results.</param>
/// <param name="input">The ray-cast input parameters.</param>
/// <param name="transform">The transform to be applied to the shape.</param>
/// <param name="childIndex">The child shape index.</param>
/// <returns>True if the ray-cast hits the shape</returns>
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;
}
/// <summary>
/// Given a transform, compute the associated axis aligned bounding box for a child shape.
/// </summary>
/// <param name="aabb">The aabb results.</param>
/// <param name="transform">The world transform of the shape.</param>
/// <param name="childIndex">The child shape index.</param>
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;
}
/// <summary>
/// Compute the mass properties of this shape using its dimensions and density.
/// The inertia tensor is computed about the local origin, not the centroid.
/// </summary>
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);
}
}
}

188
axios/Collision/Shapes/LoopShape.cs 100644
View File

@ -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
{
/// <summary>
/// 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.
/// </summary>
public class LoopShape : Shape
{
private static EdgeShape _edgeShape = new EdgeShape();
/// <summary>
/// The vertices. These are not owned/freed by the loop Shape.
/// </summary>
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;
}
/// <summary>
/// Get a child edge.
/// </summary>
/// <param name="edge">The edge.</param>
/// <param name="index">The index.</param>
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];
}
/// <summary>
/// Test a point for containment in this shape. This only works for convex shapes.
/// </summary>
/// <param name="transform">The shape world transform.</param>
/// <param name="point">a point in world coordinates.</param>
/// <returns>True if the point is inside the shape</returns>
public override bool TestPoint(ref Transform transform, ref Vector2 point)
{
return false;
}
/// <summary>
/// Cast a ray against a child shape.
/// </summary>
/// <param name="output">The ray-cast results.</param>
/// <param name="input">The ray-cast input parameters.</param>
/// <param name="transform">The transform to be applied to the shape.</param>
/// <param name="childIndex">The child shape index.</param>
/// <returns>True if the ray-cast hits the shape</returns>
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);
}
/// <summary>
/// Given a transform, compute the associated axis aligned bounding box for a child shape.
/// </summary>
/// <param name="aabb">The aabb results.</param>
/// <param name="transform">The world transform of the shape.</param>
/// <param name="childIndex">The child shape index.</param>
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);
}
/// <summary>
/// Chains have zero mass.
/// </summary>
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;
}
}
}

556
axios/Collision/Shapes/PolygonShape.cs 100644
View File

@ -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
{
/// <summary>
/// Represents a simple non-selfintersecting convex polygon.
/// If you want to have concave polygons, you will have to use the <see cref="BayazitDecomposer"/> or the <see cref="EarclipDecomposer"/>
/// to decompose the concave polygon into 2 or more convex polygons.
/// </summary>
public class PolygonShape : Shape
{
public Vertices Normals;
public Vertices Vertices;
/// <summary>
/// Initializes a new instance of the <see cref="PolygonShape"/> class.
/// </summary>
/// <param name="vertices">The vertices.</param>
/// <param name="density">The density.</param>
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;
}
/// <summary>
/// Copy vertices. This assumes the vertices define a convex polygon.
/// It is assumed that the exterior is the the right of each edge.
/// </summary>
/// <param name="vertices">The vertices.</param>
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();
}
/// <summary>
/// Compute the mass properties of this shape using its dimensions and density.
/// The inertia tensor is computed about the local origin, not the centroid.
/// </summary>
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;
}
/// <summary>
/// Build vertices to represent an axis-aligned box.
/// </summary>
/// <param name="halfWidth">The half-width.</param>
/// <param name="halfHeight">The half-height.</param>
public void SetAsBox(float halfWidth, float halfHeight)
{
Set(PolygonTools.CreateRectangle(halfWidth, halfHeight));
}
/// <summary>
/// Build vertices to represent an oriented box.
/// </summary>
/// <param name="halfWidth">The half-width..</param>
/// <param name="halfHeight">The half-height.</param>
/// <param name="center">The center of the box in local coordinates.</param>
/// <param name="angle">The rotation of the box in local coordinates.</param>
public void SetAsBox(float halfWidth, float halfHeight, Vector2 center, float angle)
{
Set(PolygonTools.CreateRectangle(halfWidth, halfHeight, center, angle));
}
/// <summary>
/// Test a point for containment in this shape. This only works for convex shapes.
/// </summary>
/// <param name="transform">The shape world transform.</param>
/// <param name="point">a point in world coordinates.</param>
/// <returns>True if the point is inside the shape</returns>
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;
}
/// <summary>
/// Cast a ray against a child shape.
/// </summary>
/// <param name="output">The ray-cast results.</param>
/// <param name="input">The ray-cast input parameters.</param>
/// <param name="transform">The transform to be applied to the shape.</param>
/// <param name="childIndex">The child shape index.</param>
/// <returns>True if the ray-cast hits the shape</returns>
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;
}
/// <summary>
/// Given a transform, compute the associated axis aligned bounding box for a child shape.
/// </summary>
/// <param name="aabb">The aabb results.</param>
/// <param name="transform">The world transform of the shape.</param>
/// <param name="childIndex">The child shape index.</param>
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;
}
}
}

222
axios/Collision/Shapes/Shape.cs 100644
View File

@ -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
{
/// <summary>
/// This holds the mass data computed for a shape.
/// </summary>
public struct MassData : IEquatable<MassData>
{
/// <summary>
/// The area of the shape
/// </summary>
public float Area;
/// <summary>
/// The position of the shape's centroid relative to the shape's origin.
/// </summary>
public Vector2 Centroid;
/// <summary>
/// The rotational inertia of the shape about the local origin.
/// </summary>
public float Inertia;
/// <summary>
/// The mass of the shape, usually in kilograms.
/// </summary>
public float Mass;
#region IEquatable<MassData> 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,
}
/// <summary>
/// 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.
/// </summary>
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++;
}
/// <summary>
/// Get the type of this shape.
/// </summary>
/// <value>The type of the shape.</value>
public ShapeType ShapeType { get; internal set; }
/// <summary>
/// Get the number of child primitives.
/// </summary>
/// <value></value>
public abstract int ChildCount { get; }
/// <summary>
/// Gets or sets the density.
/// </summary>
/// <value>The density.</value>
public float Density
{
get { return _density; }
set
{
_density = value;
ComputeProperties();
}
}
/// <summary>
/// Radius of the Shape
/// </summary>
public float Radius
{
get { return _radius; }
set
{
_radius = value;
ComputeProperties();
}
}
/// <summary>
/// Clone the concrete shape
/// </summary>
/// <returns>A clone of the shape</returns>
public abstract Shape Clone();
/// <summary>
/// Test a point for containment in this shape. This only works for convex shapes.
/// </summary>
/// <param name="transform">The shape world transform.</param>
/// <param name="point">a point in world coordinates.</param>
/// <returns>True if the point is inside the shape</returns>
public abstract bool TestPoint(ref Transform transform, ref Vector2 point);
/// <summary>
/// Cast a ray against a child shape.
/// </summary>
/// <param name="output">The ray-cast results.</param>
/// <param name="input">The ray-cast input parameters.</param>
/// <param name="transform">The transform to be applied to the shape.</param>
/// <param name="childIndex">The child shape index.</param>
/// <returns>True if the ray-cast hits the shape</returns>
public abstract bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform,
int childIndex);
/// <summary>
/// Given a transform, compute the associated axis aligned bounding box for a child shape.
/// </summary>
/// <param name="aabb">The aabb results.</param>
/// <param name="transform">The world transform of the shape.</param>
/// <param name="childIndex">The child shape index.</param>
public abstract void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex);
/// <summary>
/// Compute the mass properties of this shape using its dimensions and density.
/// The inertia tensor is computed about the local origin, not the centroid.
/// </summary>
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);
}
}

500
axios/Collision/TimeOfImpact.cs 100644
View File

@ -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
{
/// <summary>
/// Input parameters for CalculateTimeOfImpact
/// </summary>
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();
/// <summary>
/// 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.
/// </summary>
/// <param name="output">The output.</param>
/// <param name="input">The input.</param>
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);
}
}
}

126
axios/Common/ConvexHull/ChainHull.cs 100644
View File

@ -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)
/// <summary>
/// Gets the convex hull.
/// </summary>
/// <remarks>
/// http://www.softsurfer.com/Archive/algorithm_0109/algorithm_0109.htm
/// </remarks>
/// <returns></returns>
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<Vector2>
{
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
}
}

99
axios/Common/ConvexHull/GiftWrap.cs 100644
View File

@ -0,0 +1,99 @@
using System;
namespace FarseerPhysics.Common.ConvexHull
{
public static class GiftWrap
{
// From Eric Jordan's convex decomposition library (box2D rev 32)
/// <summary>
/// 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.
/// </summary>
/// <param name="vertices">The vertices.</param>
/// <returns></returns>
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;
}
}
}

122
axios/Common/ConvexHull/Melkman.cs 100644
View File

@ -0,0 +1,122 @@
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Common.ConvexHull
{
public static class Melkman
{
//Melkman based convex hull algorithm contributed by Cowdozer
/// <summary>
/// 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
/// </summary>
/// <remarks>
/// Implemented using Melkman's Convex Hull Algorithm - O(n) time complexity.
/// Reference: http://www.ams.sunysb.edu/~jsbm/courses/345/melkman.pdf
/// </remarks>
/// <returns>A convex hull in counterclockwise winding order.</returns>
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;
}
}
}

253
axios/Common/Decomposition/BayazitDecomposer.cs 100644
View File

@ -0,0 +1,253 @@
using System.Collections.Generic;
using FarseerPhysics.Common.PolygonManipulation;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Common.Decomposition
{
//From phed rev 36
/// <summary>
/// Convex decomposition algorithm created by Mark Bayazit (http://mnbayazit.com/)
/// For more information about this algorithm, see http://mnbayazit.com/406/bayazit
/// </summary>
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;
}
/// <summary>
/// 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
/// </summary>
/// <param name="vertices"></param>
/// <returns></returns>
public static List<Vertices> ConvexPartition(Vertices vertices)
{
//We force it to CCW as it is a precondition in this algorithm.
vertices.ForceCounterClockWise();
List<Vertices> list = new List<Vertices>();
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<qreal>::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;
}
}
}

420
axios/Common/Decomposition/CDT/Delaunay/DelaunayTriangle.cs 100644
View File

@ -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<DelaunayTriangle> Neighbors;
/** Has this triangle been marked as an interior triangle? */
public FixedArray3<TriangulationPoint> 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));
}
/// <summary>
/// Update neighbor pointers
/// </summary>
/// <param name="p1">Point 1 of the shared edge</param>
/// <param name="p2">Point 2 of the shared edge</param>
/// <param name="t">This triangle's new neighbor</param>
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!");
}
}
/// <summary>
/// Exhaustive search to update neighbor pointers
/// </summary>
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;
}
/// <param name="t">Opposite triangle</param>
/// <param name="p">The point in t that isn't shared between the triangles</param>
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;
}
/// <summary>
/// Legalize triangle by rotating clockwise around oPoint
/// </summary>
/// <param name="oPoint">The origin point to rotate around</param>
/// <param name="nPoint">???</param>
public void Legalize(TriangulationPoint oPoint, TriangulationPoint nPoint)
{
RotateCW();
Points[IndexCCW(oPoint)] = nPoint;
}
public override string ToString()
{
return Points[0] + "," + Points[1] + "," + Points[2];
}
/// <summary>
/// Finalize edge marking
/// </summary>
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<DelaunayTriangle> 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);
}
/// <summary>
/// Mark edge as constrained
/// </summary>
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);
}
/// <summary>
/// Get the index of the neighbor that shares this edge (or -1 if it isn't shared)
/// </summary>
/// <returns>index of the shared edge or -1 if edge isn't shared</returns>
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;
}
}
}

180
axios/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFront.cs 100644
View File

@ -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();
}
/// <summary>
/// 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
/// </summary>
private AdvancingFrontNode FindSearchNode(double x)
{
// TODO: implement BST index
return Search;
}
/// <summary>
/// We use a balancing tree to locate a node smaller or equal to given key value
/// </summary>
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;
}
/// <summary>
/// This implementation will use simple node traversal algorithm to find a point on the front
/// </summary>
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;
}
}
}

64
axios/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFrontNode.cs 100644
View File

@ -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; }
}
}
}

1132
axios/Common/Decomposition/CDT/Delaunay/Sweep/DTSweep.cs 100644
View File

File diff suppressed because it is too large Load Diff

66
axios/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepConstraint.cs 100644
View File

@ -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
{
/// <summary>
/// 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
/// </summary>
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);
}
}
}

236
axios/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepContext.cs 100644
View File

@ -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;
}
/// <summary>
/// Try to map a node to all sides of this triangle that don't have
/// a neighbor.
/// </summary>
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
}
}

69
axios/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepPointComparator.cs 100644
View File

@ -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<TriangulationPoint>
{
#region IComparer<TriangulationPoint> 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
}
}

43
axios/Common/Decomposition/CDT/Delaunay/Sweep/PointOnEdgeException.cs 100644
View File

@ -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)
{
}
}
}

48
axios/Common/Decomposition/CDT/ITriangulatable.cs 100644
View File

@ -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<TriangulationPoint> Points { get; } // MM: Neither of these are used via interface (yet?)
IList<DelaunayTriangle> Triangles { get; }
TriangulationMode TriangulationMode { get; }
void PrepareTriangulation(TriangulationContext tcx);
void AddTriangle(DelaunayTriangle t);
void AddTriangles(IEnumerable<DelaunayTriangle> list);
void ClearTriangles();
}
}

40
axios/Common/Decomposition/CDT/Orientation.cs 100644
View File

@ -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
}
}

272
axios/Common/Decomposition/CDT/Polygon/Polygon.cs 100644
View File

@ -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<Polygon> _holes;
protected PolygonPoint _last;
protected List<TriangulationPoint> _points = new List<TriangulationPoint>();
protected List<TriangulationPoint> _steinerPoints;
protected List<DelaunayTriangle> _triangles;
/// <summary>
/// Create a polygon from a list of at least 3 points with no duplicates.
/// </summary>
/// <param name="points">A list of unique points</param>
public Polygon(IList<PolygonPoint> 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<TriangulationPoint>());
}
/// <summary>
/// Create a polygon from a list of at least 3 points with no duplicates.
/// </summary>
/// <param name="points">A list of unique points.</param>
public Polygon(IEnumerable<PolygonPoint> points) : this((points as IList<PolygonPoint>) ?? points.ToArray())
{
}
public Polygon()
{
}
public IList<Polygon> Holes
{
get { return _holes; }
}
#region Triangulatable Members
public TriangulationMode TriangulationMode
{
get { return TriangulationMode.Polygon; }
}
public IList<TriangulationPoint> Points
{
get { return _points; }
}
public IList<DelaunayTriangle> Triangles
{
get { return _triangles; }
}
public void AddTriangle(DelaunayTriangle t)
{
_triangles.Add(t);
}
public void AddTriangles(IEnumerable<DelaunayTriangle> list)
{
_triangles.AddRange(list);
}
public void ClearTriangles()
{
if (_triangles != null) _triangles.Clear();
}
/// <summary>
/// Creates constraints and populates the context with points
/// </summary>
/// <param name="tcx">The context</param>
public void PrepareTriangulation(TriangulationContext tcx)
{
if (_triangles == null)
{
_triangles = new List<DelaunayTriangle>(_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<TriangulationPoint>();
}
_steinerPoints.Add(point);
}
public void AddSteinerPoints(List<TriangulationPoint> points)
{
if (_steinerPoints == null)
{
_steinerPoints = new List<TriangulationPoint>();
}
_steinerPoints.AddRange(points);
}
public void ClearSteinerPoints()
{
if (_steinerPoints != null)
{
_steinerPoints.Clear();
}
}
/// <summary>
/// Add a hole to the polygon.
/// </summary>
/// <param name="poly">A subtraction polygon fully contained inside this polygon.</param>
public void AddHole(Polygon poly)
{
if (_holes == null) _holes = new List<Polygon>();
_holes.Add(poly);
// XXX: tests could be made here to be sure it is fully inside
// addSubtraction( poly.getPoints() );
}
/// <summary>
/// Inserts newPoint after point.
/// </summary>
/// <param name="point">The point to insert after in the polygon</param>
/// <param name="newPoint">The point to insert into the polygon</param>
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);
}
/// <summary>
/// Inserts list (after last point in polygon?)
/// </summary>
/// <param name="list"></param>
public void AddPoints(IEnumerable<PolygonPoint> 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;
}
/// <summary>
/// Adds a point after the last in the polygon.
/// </summary>
/// <param name="p">The point to add</param>
public void AddPoint(PolygonPoint p)
{
p.Previous = _last;
p.Next = _last.Next;
_last.Next = p;
_points.Add(p);
}
/// <summary>
/// Removes a point from the polygon.
/// </summary>
/// <param name="p"></param>
public void RemovePoint(PolygonPoint p)
{
PolygonPoint next, prev;
next = p.Next;
prev = p.Previous;
prev.Next = next;
next.Previous = prev;
_points.Remove(p);
}
}
}

48
axios/Common/Decomposition/CDT/Polygon/PolygonPoint.cs 100644
View File

@ -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; }
}
}

65
axios/Common/Decomposition/CDT/Polygon/PolygonSet.cs 100644
View File

@ -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<Polygon> ?
using System.Collections.Generic;
namespace Poly2Tri.Triangulation.Polygon
{
public class PolygonSet
{
protected List<Polygon> _polygons = new List<Polygon>();
public PolygonSet()
{
}
public PolygonSet(Polygon poly)
{
_polygons.Add(poly);
}
public IEnumerable<Polygon> Polygons
{
get { return _polygons; }
}
public void Add(Polygon p)
{
_polygons.Add(p);
}
}
}

114
axios/Common/Decomposition/CDT/Sets/ConstrainedPointSet.cs 100644
View File

@ -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<br>
* 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.
* <p>
*
*
* @author Thomas Åhlén, thahlen@gmail.com
*/
public class ConstrainedPointSet : PointSet
{
private List<TriangulationPoint> _constrainedPointList = null;
public ConstrainedPointSet(List<TriangulationPoint> 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 <b>must</b> be part of given PointSet!
*/
public ConstrainedPointSet(List<TriangulationPoint> points, IEnumerable<TriangulationPoint> constraints)
: base(points)
{
_constrainedPointList = new List<TriangulationPoint>();
_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<TriangulationPoint>.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<br>
* 1. Check's if there any constraint edges are crossing or collinear<br>
* 2.
* @return
*/
public bool isValid()
{
return true;
}
}
}

84
axios/Common/Decomposition/CDT/Sets/PointSet.cs 100644
View File

@ -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<TriangulationPoint> points)
{
Points = new List<TriangulationPoint>(points);
}
#region Triangulatable Members
public IList<TriangulationPoint> Points { get; private set; }
public IList<DelaunayTriangle> Triangles { get; private set; }
public virtual TriangulationMode TriangulationMode
{
get { return TriangulationMode.Unconstrained; }
}
public void AddTriangle(DelaunayTriangle t)
{
Triangles.Add(t);
}
public void AddTriangles(IEnumerable<DelaunayTriangle> 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<DelaunayTriangle>(Points.Count);
}
else
{
Triangles.Clear();
}
tcx.Points.AddRange(Points);
}
#endregion
}
}

46
axios/Common/Decomposition/CDT/TriangulationConstraint.cs 100644
View File

@ -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;
}
}

87
axios/Common/Decomposition/CDT/TriangulationContext.cs 100644
View File

@ -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<TriangulationPoint> Points = new List<TriangulationPoint>(200);
public readonly List<DelaunayTriangle> Triangles = new List<DelaunayTriangle>();
#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;
}
}
}

40
axios/Common/Decomposition/CDT/TriangulationMode.cs 100644
View File

@ -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
}
}

82
axios/Common/Decomposition/CDT/TriangulationPoint.cs 100644
View File

@ -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<DTSweepConstraint> 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<DTSweepConstraint>();
}
Edges.Add(e);
}
}
}

160
axios/Common/Decomposition/CDT/TriangulationUtil.cs 100644
View File

@ -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;
/// <summary>
/// Requirements:
/// 1. a,b and c form a triangle.
/// 2. a and d is know to be on opposite side of bc
/// <code>
/// a
/// +
/// / \
/// / \
/// b/ \c
/// +-------+
/// / B \
/// / \
/// </code>
/// 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
/// </summary>
/// <param name="pa">triangle point, opposite d</param>
/// <param name="pb">triangle point</param>
/// <param name="pc">triangle point</param>
/// <param name="pd">point opposite a</param>
/// <returns>true if d is inside circle, false if on circle edge</returns>
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;
}
}
}

118
axios/Common/Decomposition/CDT/Util/FixedArray3.cs 100644
View File

@ -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<T> : IEnumerable<T> 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<T> Members
public IEnumerator<T> 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<T> Enumerate()
{
for (int i = 0; i < 3; ++i) yield return this[i];
}
}
}

118
axios/Common/Decomposition/CDT/Util/FixedBitArray3.cs 100644
View File

@ -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<bool>
{
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<bool> Members
public IEnumerator<bool> 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<bool> Enumerate()
{
for (int i = 0; i < 3; ++i) yield return this[i];
}
}
}

38
axios/Common/Decomposition/CDT/Util/PointGenerator.cs 100644
View File

@ -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<TriangulationPoint> UniformDistribution(int n, double scale)
{
List<TriangulationPoint> points = new List<TriangulationPoint>();
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<TriangulationPoint> UniformGrid(int n, double scale)
{
double x = 0;
double size = scale/n;
double halfScale = 0.5*scale;
List<TriangulationPoint> points = new List<TriangulationPoint>();
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;
}
}
}

98
axios/Common/Decomposition/CDT/Util/PolygonGenerator.cs 100644
View File

@ -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);
}
}
}

110
axios/Common/Decomposition/CDTDecomposer.cs 100644
View File

@ -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<Vertices> 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<Vertices> results = new List<Vertices>();
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<Vertices> 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<Vertices> results = new List<Vertices>();
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;
}
}
}

691
axios/Common/Decomposition/EarclipDecomposer.cs 100644
View File

@ -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
{
/// <summary>
/// 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.
/// </summary>
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;
/// <summary>
/// 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
/// </summary>
/// <param name="vertices">The vertices.</param>
/// <returns></returns>
public static List<Vertices> ConvexPartition(Vertices vertices)
{
return ConvexPartition(vertices, int.MaxValue, 0);
}
/// <summary>
/// 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
/// </summary>
/// <param name="vertices">The vertices.</param>
/// <param name="maxPolys">The maximum number of polygons.</param>
/// <param name="tolerance">The tolerance.</param>
/// <returns></returns>
public static List<Vertices> ConvexPartition(Vertices vertices, int maxPolys, float tolerance)
{
if (vertices.Count < 3)
return new List<Vertices> { 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<Vertices> { tempP };
}
vertices = SimplifyTools.CollinearSimplify(vertices);
vertices.ForceCounterClockWise();
return new List<Vertices> { vertices };
}
*/
List<Triangle> 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<Vertices> 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;
}
/// <summary>
/// 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.
/// </summary>
/// <param name="triangulated">The triangulated.</param>
///<param name="maxPolys">The maximun number of polygons</param>
///<param name="tolerance">The tolerance</param>
///<returns></returns>
public static List<Vertices> PolygonizeTriangles(List<Triangle> triangulated, int maxPolys, float tolerance)
{
List<Vertices> polys = new List<Vertices>(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;
}
/// <summary>
/// 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.
/// </summary>
/// <returns></returns>
public static List<Triangle> TriangulatePolygon(Vertices vertices)
{
List<Triangle> results = new List<Triangle>();
if (vertices.Count < 3)
return new List<Triangle>();
//Recurse and split on pinch points
Vertices pA, pB;
Vertices pin = new Vertices(vertices);
if (ResolvePinchPoint(pin, out pA, out pB))
{
List<Triangle> mergeA = TriangulatePolygon(pA);
List<Triangle> 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;
}
/// <summary>
/// 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...
/// </summary>
/// <param name="pin">The pin.</param>
/// <param name="poutA">The pout A.</param>
/// <param name="poutB">The pout B.</param>
/// <returns></returns>
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;
}
/// <summary>
/// Fix for obnoxious behavior for the % operator for negative numbers...
/// </summary>
/// <param name="x">The x.</param>
/// <param name="modulus">The modulus.</param>
/// <returns></returns>
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;
}
/// <summary>
/// Checks if vertex i is the tip of an ear in polygon defined by xv[] and
/// yv[].
///
/// Assumes clockwise orientation of polygon...ick
/// </summary>
/// <param name="i">The i.</param>
/// <param name="xv">The xv.</param>
/// <param name="yv">The yv.</param>
/// <param name="xvLength">Length of the xv.</param>
/// <returns>
/// <c>true</c> if the specified i is ear; otherwise, <c>false</c>.
/// </returns>
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));
}
}
}

160
axios/Common/Decomposition/FlipcodeDecomposer.cs 100644
View File

@ -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
/// <summary>
/// Triangulates a polygon into triangles.
/// Doesn't handle holes.
/// </summary>
public static class FlipcodeDecomposer
{
private static Vector2 _tmpA;
private static Vector2 _tmpB;
private static Vector2 _tmpC;
/// <summary>
/// Check if the point P is inside the triangle defined by
/// the points A, B, C
/// </summary>
/// <param name="a">The A point.</param>
/// <param name="b">The B point.</param>
/// <param name="c">The C point.</param>
/// <param name="p">The point to be tested.</param>
/// <returns>True if the point is inside the triangle</returns>
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));
}
/// <summary>
/// Cut a the contour and add a triangle into V to describe the
/// location of the cut
/// </summary>
/// <param name="contour">The list of points defining the polygon</param>
/// <param name="u">The index of the first point</param>
/// <param name="v">The index of the second point</param>
/// <param name="w">The index of the third point</param>
/// <param name="n">The number of elements in the array.</param>
/// <param name="V">The array to populate with indicies of triangles.</param>
/// <returns>True if a triangle was found</returns>
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;
}
/// <summary>
/// Decompose the polygon into triangles
/// </summary>
/// <param name="contour">The list of points describing the polygon</param>
/// <returns></returns>
public static List<Vertices> ConvexPartition(Vertices contour)
{
int n = contour.Count;
if (n < 3)
return new List<Vertices>();
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<Vertices> result = new List<Vertices>();
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<Vertices>();
}
// Three consecutive vertices in current polygon, <u,v,w>
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;
}
}
}

1057
axios/Common/Decomposition/SeidelDecomposer.cs 100644
View File

File diff suppressed because it is too large Load Diff

227
axios/Common/FixedArray.cs 100644
View File

@ -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<T>
{
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<T>
{
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<T>
{
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<T>
{
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();
}
}
}
}
}

81
axios/Common/HashSet.cs 100644
View File

@ -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<T> : ICollection<T>
{
private Dictionary<T, short> _dict;
public HashSet(int capacity)
{
_dict = new Dictionary<T, short>(capacity);
}
public HashSet()
{
_dict = new Dictionary<T, short>();
}
// Methods
#region ICollection<T> 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<T> 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

308
axios/Common/LineTools.cs 100644
View File

@ -0,0 +1,308 @@
using System;
using System.Collections.Generic;
using FarseerPhysics.Collision;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Common
{
/// <summary>
/// Collection of helper methods for misc collisions.
/// Does float tolerance and line collisions with lines and AABBs.
/// </summary>
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
/// <summary>
///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.
///
/// </summary>
/// <param name="a0"></param>
/// <param name="a1"></param>
/// <param name="b0"></param>
/// <param name="b1"></param>
/// <param name="intersectionPoint"></param>
/// <returns></returns>
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;
}
/// <summary>
/// This method detects if two line segments (or lines) intersect,
/// and, if so, the point of intersection. Use the <paramref name="firstIsSegment"/> and
/// <paramref name="secondIsSegment"/> 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
/// </summary>
/// <param name="point1">The first point of the first line segment.</param>
/// <param name="point2">The second point of the first line segment.</param>
/// <param name="point3">The first point of the second line segment.</param>
/// <param name="point4">The second point of the second line segment.</param>
/// <param name="point">This is set to the intersection
/// point if an intersection is detected.</param>
/// <param name="firstIsSegment">Set this to true to require that the
/// intersection point be on the first line segment.</param>
/// <param name="secondIsSegment">Set this to true to require that the
/// intersection point be on the second line segment.</param>
/// <returns>True if an intersection is detected, false otherwise.</returns>
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;
}
/// <summary>
/// This method detects if two line segments (or lines) intersect,
/// and, if so, the point of intersection. Use the <paramref name="firstIsSegment"/> and
/// <paramref name="secondIsSegment"/> 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
/// </summary>
/// <param name="point1">The first point of the first line segment.</param>
/// <param name="point2">The second point of the first line segment.</param>
/// <param name="point3">The first point of the second line segment.</param>
/// <param name="point4">The second point of the second line segment.</param>
/// <param name="intersectionPoint">This is set to the intersection
/// point if an intersection is detected.</param>
/// <param name="firstIsSegment">Set this to true to require that the
/// intersection point be on the first line segment.</param>
/// <param name="secondIsSegment">Set this to true to require that the
/// intersection point be on the second line segment.</param>
/// <returns>True if an intersection is detected, false otherwise.</returns>
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);
}
/// <summary>
/// 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).
/// </summary>
/// <param name="point1">The first point of the first line segment.</param>
/// <param name="point2">The second point of the first line segment.</param>
/// <param name="point3">The first point of the second line segment.</param>
/// <param name="point4">The second point of the second line segment.</param>
/// <param name="intersectionPoint">This is set to the intersection
/// point if an intersection is detected.</param>
/// <returns>True if an intersection is detected, false otherwise.</returns>
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);
}
/// <summary>
/// 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).
/// </summary>
/// <param name="point1">The first point of the first line segment.</param>
/// <param name="point2">The second point of the first line segment.</param>
/// <param name="point3">The first point of the second line segment.</param>
/// <param name="point4">The second point of the second line segment.</param>
/// <param name="intersectionPoint">This is set to the intersection
/// point if an intersection is detected.</param>
/// <returns>True if an intersection is detected, false otherwise.</returns>
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);
}
/// <summary>
/// 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)
/// </summary>
/// <param name="point1">The first point of the line segment to test</param>
/// <param name="point2">The second point of the line segment to test.</param>
/// <param name="vertices">The vertices, as described above</param>
/// <param name="intersectionPoints">An list of intersection points. Any intersection points
/// found will be added to this list.</param>
public static void LineSegmentVerticesIntersect(ref Vector2 point1, ref Vector2 point2, Vertices vertices,
ref List<Vector2> 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);
}
}
}
/// <summary>
/// Get all intersections between a line segment and an AABB.
/// </summary>
/// <param name="point1">The first point of the line segment to test</param>
/// <param name="point2">The second point of the line segment to test.</param>
/// <param name="aabb">The AABB that is used for testing intersection.</param>
/// <param name="intersectionPoints">An list of intersection points. Any intersection points found will be added to this list.</param>
public static void LineSegmentAABBIntersect(ref Vector2 point1, ref Vector2 point2, AABB aabb,
ref List<Vector2> intersectionPoints)
{
LineSegmentVerticesIntersect(ref point1, ref point2, aabb.Vertices, ref intersectionPoints);
}
}
}

638
axios/Common/Math.cs 100644
View File

@ -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<T>(ref T a, ref T b)
{
T tmp = a;
a = b;
b = tmp;
}
/// <summary>
/// This function is used to ensure that a floating point number is
/// not a NaN or infinity.
/// </summary>
/// <param name="x">The x.</param>
/// <returns>
/// <c>true</c> if the specified x is valid; otherwise, <c>false</c>.
/// </returns>
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);
}
/// <summary>
/// This is a approximate yet fast inverse square-root.
/// </summary>
/// <param name="x">The x.</param>
/// <returns></returns>
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;
}
/// <summary>
/// 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
/// </summary>
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);
}
/// <summary>
/// Returns a positive number if c is to the left of the line going from a to b.
/// </summary>
/// <returns>Positive number if point is left, negative if point is right,
/// and 0 if points are collinear.</returns>
public static float Area(Vector2 a, Vector2 b, Vector2 c)
{
return Area(ref a, ref b, ref c);
}
/// <summary>
/// Returns a positive number if c is to the left of the line going from a to b.
/// </summary>
/// <returns>Positive number if point is left, negative if point is right,
/// and 0 if points are collinear.</returns>
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);
}
/// <summary>
/// Determines if three vertices are collinear (ie. on a straight line)
/// </summary>
/// <param name="a">First vertex</param>
/// <param name="b">Second vertex</param>
/// <param name="c">Third vertex</param>
/// <returns></returns>
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;
}
/// <summary>
/// Checks if a floating point Value is equal to another,
/// within a certain tolerance.
/// </summary>
/// <param name="value1">The first floating point Value.</param>
/// <param name="value2">The second floating point Value.</param>
/// <param name="delta">The floating point tolerance.</param>
/// <returns>True if the values are "equal", false otherwise.</returns>
public static bool FloatEquals(float value1, float value2, float delta)
{
return FloatInRange(value1, value2 - delta, value2 + delta);
}
/// <summary>
/// Checks if a floating point Value is within a specified
/// range of values (inclusive).
/// </summary>
/// <param name="value">The Value to check.</param>
/// <param name="min">The minimum Value.</param>
/// <param name="max">The maximum Value.</param>
/// <returns>True if the Value is within the range specified,
/// false otherwise.</returns>
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
}
/// <summary>
/// A 2-by-2 matrix. Stored in column-major order.
/// </summary>
public struct Mat22
{
public Vector2 Col1, Col2;
/// <summary>
/// Construct this matrix using columns.
/// </summary>
/// <param name="c1">The c1.</param>
/// <param name="c2">The c2.</param>
public Mat22(Vector2 c1, Vector2 c2)
{
Col1 = c1;
Col2 = c2;
}
/// <summary>
/// Construct this matrix using scalars.
/// </summary>
/// <param name="a11">The a11.</param>
/// <param name="a12">The a12.</param>
/// <param name="a21">The a21.</param>
/// <param name="a22">The a22.</param>
public Mat22(float a11, float a12, float a21, float a22)
{
Col1 = new Vector2(a11, a21);
Col2 = new Vector2(a12, a22);
}
/// <summary>
/// Construct this matrix using an angle. This matrix becomes
/// an orthonormal rotation matrix.
/// </summary>
/// <param name="angle">The angle.</param>
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);
}
/// <summary>
/// Extract the angle from this matrix (assumed to be
/// a rotation matrix).
/// </summary>
/// <value></value>
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;
}
}
/// <summary>
/// Initialize this matrix using columns.
/// </summary>
/// <param name="c1">The c1.</param>
/// <param name="c2">The c2.</param>
public void Set(Vector2 c1, Vector2 c2)
{
Col1 = c1;
Col2 = c2;
}
/// <summary>
/// Initialize this matrix using an angle. This matrix becomes
/// an orthonormal rotation matrix.
/// </summary>
/// <param name="angle">The angle.</param>
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;
}
/// <summary>
/// Set this to the identity matrix.
/// </summary>
public void SetIdentity()
{
Col1.X = 1.0f;
Col2.X = 0.0f;
Col1.Y = 0.0f;
Col2.Y = 1.0f;
}
/// <summary>
/// Set this matrix to all zeros.
/// </summary>
public void SetZero()
{
Col1.X = 0.0f;
Col2.X = 0.0f;
Col1.Y = 0.0f;
Col2.Y = 0.0f;
}
/// <summary>
/// Solve A * x = b, where b is a column vector. This is more efficient
/// than computing the inverse in one-shot cases.
/// </summary>
/// <param name="b">The b.</param>
/// <returns></returns>
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;
}
}
/// <summary>
/// A 3-by-3 matrix. Stored in column-major order.
/// </summary>
public struct Mat33
{
public Vector3 Col1, Col2, Col3;
/// <summary>
/// Construct this matrix using columns.
/// </summary>
/// <param name="c1">The c1.</param>
/// <param name="c2">The c2.</param>
/// <param name="c3">The c3.</param>
public Mat33(Vector3 c1, Vector3 c2, Vector3 c3)
{
Col1 = c1;
Col2 = c2;
Col3 = c3;
}
/// <summary>
/// Set this matrix to all zeros.
/// </summary>
public void SetZero()
{
Col1 = Vector3.Zero;
Col2 = Vector3.Zero;
Col3 = Vector3.Zero;
}
/// <summary>
/// Solve A * x = b, where b is a column vector. This is more efficient
/// than computing the inverse in one-shot cases.
/// </summary>
/// <param name="b">The b.</param>
/// <returns></returns>
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)));
}
/// <summary>
/// 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.
/// </summary>
/// <param name="b">The b.</param>
/// <returns></returns>
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));
}
}
/// <summary>
/// A transform contains translation and rotation. It is used to represent
/// the position and orientation of rigid frames.
/// </summary>
public struct Transform
{
public Vector2 Position;
public Mat22 R;
/// <summary>
/// Initialize using a position vector and a rotation matrix.
/// </summary>
/// <param name="position">The position.</param>
/// <param name="r">The r.</param>
public Transform(ref Vector2 position, ref Mat22 r)
{
Position = position;
R = r;
}
/// <summary>
/// Calculate the angle that the rotation matrix represents.
/// </summary>
/// <value></value>
public float Angle
{
get { return (float)Math.Atan2(R.Col1.Y, R.Col1.X); }
}
/// <summary>
/// Set this to the identity transform.
/// </summary>
public void SetIdentity()
{
Position = Vector2.Zero;
R.SetIdentity();
}
/// <summary>
/// Set this based on the position and angle.
/// </summary>
/// <param name="position">The position.</param>
/// <param name="angle">The angle.</param>
public void Set(Vector2 position, float angle)
{
Position = position;
R.Set(angle);
}
}
/// <summary>
/// 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.
/// </summary>
public struct Sweep
{
/// <summary>
/// World angles
/// </summary>
public float A;
public float A0;
/// <summary>
/// Fraction of the current time step in the range [0,1]
/// c0 and a0 are the positions at alpha0.
/// </summary>
public float Alpha0;
/// <summary>
/// Center world positions
/// </summary>
public Vector2 C;
public Vector2 C0;
/// <summary>
/// Local center of mass position
/// </summary>
public Vector2 LocalCenter;
/// <summary>
/// Get the interpolated transform at a specific time.
/// </summary>
/// <param name="xf">The transform.</param>
/// <param name="beta">beta is a factor in [0,1], where 0 indicates alpha0.</param>
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);
}
/// <summary>
/// Advance the sweep forward, yielding a new initial state.
/// </summary>
/// <param name="alpha">new initial time..</param>
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;
}
/// <summary>
/// Normalize the angles.
/// </summary>
public void Normalize()
{
float d = MathHelper.TwoPi * (float)Math.Floor(A0 / MathHelper.TwoPi);
A0 -= d;
A -= d;
}
}
}

341
axios/Common/Path.cs 100644
View File

@ -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
/// <summary>
/// Path:
/// Very similar to Vertices, but this
/// class contains vectors describing
/// control points on a Catmull-Rom
/// curve.
/// </summary>
[XmlRoot("Path")]
public class Path
{
/// <summary>
/// All the points that makes up the curve
/// </summary>
[XmlElement("ControlPoints")]
public List<Vector2> ControlPoints;
private float _deltaT;
/// <summary>
/// Initializes a new instance of the <see cref="Path"/> class.
/// </summary>
public Path()
{
ControlPoints = new List<Vector2>();
}
/// <summary>
/// Initializes a new instance of the <see cref="Path"/> class.
/// </summary>
/// <param name="vertices">The vertices to created the path from.</param>
public Path(Vector2[] vertices)
{
ControlPoints = new List<Vector2>(vertices.Length);
for (int i = 0; i < vertices.Length; i++)
{
Add(vertices[i]);
}
}
/// <summary>
/// Initializes a new instance of the <see cref="Path"/> class.
/// </summary>
/// <param name="vertices">The vertices to created the path from.</param>
public Path(IList<Vector2> vertices)
{
ControlPoints = new List<Vector2>(vertices.Count);
for (int i = 0; i < vertices.Count; i++)
{
Add(vertices[i]);
}
}
/// <summary>
/// True if the curve is closed.
/// </summary>
/// <value><c>true</c> if closed; otherwise, <c>false</c>.</value>
[XmlElement("Closed")]
public bool Closed { get; set; }
/// <summary>
/// Gets the next index of a controlpoint
/// </summary>
/// <param name="index">The index.</param>
/// <returns></returns>
public int NextIndex(int index)
{
if (index == ControlPoints.Count - 1)
{
return 0;
}
return index + 1;
}
/// <summary>
/// Gets the previous index of a controlpoint
/// </summary>
/// <param name="index">The index.</param>
/// <returns></returns>
public int PreviousIndex(int index)
{
if (index == 0)
{
return ControlPoints.Count - 1;
}
return index - 1;
}
/// <summary>
/// Translates the control points by the specified vector.
/// </summary>
/// <param name="vector">The vector.</param>
public void Translate(ref Vector2 vector)
{
for (int i = 0; i < ControlPoints.Count; i++)
ControlPoints[i] = Vector2.Add(ControlPoints[i], vector);
}
/// <summary>
/// Scales the control points by the specified vector.
/// </summary>
/// <param name="value">The Value.</param>
public void Scale(ref Vector2 value)
{
for (int i = 0; i < ControlPoints.Count; i++)
ControlPoints[i] = Vector2.Multiply(ControlPoints[i], value);
}
/// <summary>
/// Rotate the control points by the defined value in radians.
/// </summary>
/// <param name="value">The amount to rotate by in radians.</param>
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();
}
/// <summary>
/// Returns a set of points defining the
/// curve with the specifed number of divisions
/// between each control point.
/// </summary>
/// <param name="divisions">Number of divisions between each control point.</param>
/// <returns></returns>
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;
}
/// <summary>
/// Gets the normal for the given time.
/// </summary>
/// <param name="time">The time</param>
/// <returns>The normal.</returns>
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<Vector2> 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<Vector3> SubdivideEvenly(int divisions)
{
List<Vector3> verts = new List<Vector3>();
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;
}
}
}

240
axios/Common/PathManager.cs 100644
View File

@ -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
{
/// <summary>
/// An easy to use manager for creating paths.
/// </summary>
public static class PathManager
{
#region LinkType enum
public enum LinkType
{
Revolute,
Slider
}
#endregion
//Contributed by Matthew Bettcher
/// <summary>
/// Convert a path into a set of edges and attaches them to the specified body.
/// Note: use only for static edges.
/// </summary>
/// <param name="path">The path.</param>
/// <param name="body">The body.</param>
/// <param name="subdivisions">The subdivisions.</param>
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]));
}
}
}
/// <summary>
/// Convert a closed path into a polygon.
/// Convex decomposition is automatically performed.
/// </summary>
/// <param name="path">The path.</param>
/// <param name="body">The body.</param>
/// <param name="density">The density.</param>
/// <param name="subdivisions">The subdivisions.</param>
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<Vector2> verts = path.GetVertices(subdivisions);
List<Vertices> decomposedVerts = EarclipDecomposer.ConvexPartition(new Vertices(verts));
//List<Vertices> decomposedVerts = BayazitDecomposer.ConvexPartition(new Vertices(verts));
foreach (Vertices item in decomposedVerts)
{
body.CreateFixture(new PolygonShape(item, density));
}
}
/// <summary>
/// Duplicates the given Body along the given path for approximatly the given copies.
/// </summary>
/// <param name="world">The world.</param>
/// <param name="path">The path.</param>
/// <param name="shapes">The shapes.</param>
/// <param name="type">The type.</param>
/// <param name="copies">The copies.</param>
/// <param name="userData"></param>
/// <returns></returns>
public static List<Body> EvenlyDistributeShapesAlongPath(World world, Path path, IEnumerable<Shape> shapes,
BodyType type, int copies, object userData)
{
List<Vector3> centers = path.SubdivideEvenly(copies);
List<Body> bodyList = new List<Body>();
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<Body> EvenlyDistributeShapesAlongPath(World world, Path path, IEnumerable<Shape> shapes,
BodyType type, int copies)
{
return EvenlyDistributeShapesAlongPath(world, path, shapes, type, copies, null);
}
/// <summary>
/// Duplicates the given Body along the given path for approximatly the given copies.
/// </summary>
/// <param name="world">The world.</param>
/// <param name="path">The path.</param>
/// <param name="shape">The shape.</param>
/// <param name="type">The type.</param>
/// <param name="copies">The copies.</param>
/// <param name="userData">The user data.</param>
/// <returns></returns>
public static List<Body> EvenlyDistributeShapesAlongPath(World world, Path path, Shape shape, BodyType type,
int copies, object userData)
{
List<Shape> shapes = new List<Shape>(1);
shapes.Add(shape);
return EvenlyDistributeShapesAlongPath(world, path, shapes, type, copies, userData);
}
public static List<Body> EvenlyDistributeShapesAlongPath(World world, Path path, Shape shape, BodyType type,
int copies)
{
return EvenlyDistributeShapesAlongPath(world, path, shape, type, copies, null);
}
//TODO: Comment better
/// <summary>
/// Moves the body on the path.
/// </summary>
/// <param name="path">The path.</param>
/// <param name="body">The body.</param>
/// <param name="time">The time.</param>
/// <param name="strength">The strength.</param>
/// <param name="timeStep">The time step.</param>
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;
}
/// <summary>
/// Attaches the bodies with revolute joints.
/// </summary>
/// <param name="world">The world.</param>
/// <param name="bodies">The bodies.</param>
/// <param name="localAnchorA">The local anchor A.</param>
/// <param name="localAnchorB">The local anchor B.</param>
/// <param name="connectFirstAndLast">if set to <c>true</c> [connect first and last].</param>
/// <param name="collideConnected">if set to <c>true</c> [collide connected].</param>
public static List<RevoluteJoint> AttachBodiesWithRevoluteJoint(World world, List<Body> bodies,
Vector2 localAnchorA,
Vector2 localAnchorB, bool connectFirstAndLast,
bool collideConnected)
{
List<RevoluteJoint> joints = new List<RevoluteJoint>(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;
}
/// <summary>
/// Attaches the bodies with revolute joints.
/// </summary>
/// <param name="world">The world.</param>
/// <param name="bodies">The bodies.</param>
/// <param name="localAnchorA">The local anchor A.</param>
/// <param name="localAnchorB">The local anchor B.</param>
/// <param name="connectFirstAndLast">if set to <c>true</c> [connect first and last].</param>
/// <param name="collideConnected">if set to <c>true</c> [collide connected].</param>
/// <param name="minLength">Minimum length of the slider joint.</param>
/// <param name="maxLength">Maximum length of the slider joint.</param>
/// <returns></returns>
public static List<SliderJoint> AttachBodiesWithSliderJoint(World world, List<Body> bodies, Vector2 localAnchorA,
Vector2 localAnchorB, bool connectFirstAndLast,
bool collideConnected, float minLength,
float maxLength)
{
List<SliderJoint> joints = new List<SliderJoint>(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;
}
}
}

464
axios/Common/PhysicsLogic/Explosion.cs 100644
View File

@ -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
}
/// <summary>
/// This is a comprarer used for
/// detecting angle difference between rays
/// </summary>
internal class RayDataComparer : IComparer<float>
{
#region IComparer<float> Members
int IComparer<float>.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
*/
/// <summary>
/// This is an explosive... it explodes.
/// </summary>
/// <remarks>
/// 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
/// </remarks>
public sealed class Explosion : PhysicsLogic
{
/// <summary>
/// Two degrees: maximum angle from edges to first ray tested
/// </summary>
private const float MaxEdgeOffset = MathHelper.Pi / 90;
/// <summary>
/// Ratio of arc length to angle from edges to first ray tested.
/// Defaults to 1/40.
/// </summary>
public float EdgeRatio = 1.0f / 40.0f;
/// <summary>
/// Ignore Explosion if it happens inside a shape.
/// Default value is false.
/// </summary>
public bool IgnoreWhenInsideShape = false;
/// <summary>
/// Max angle between rays (used when segment is large).
/// Defaults to 15 degrees
/// </summary>
public float MaxAngle = MathHelper.Pi / 15;
/// <summary>
/// Maximum number of shapes involved in the explosion.
/// Defaults to 100
/// </summary>
public int MaxShapes = 100;
/// <summary>
/// How many rays per shape/body/segment.
/// Defaults to 5
/// </summary>
public int MinRays = 5;
private List<ShapeData> _data = new List<ShapeData>();
private Dictionary<Fixture, List<Vector2>> _exploded;
private RayDataComparer _rdc;
public Explosion(World world)
: base(world, PhysicsLogicType.Explosion)
{
_exploded = new Dictionary<Fixture, List<Vector2>>();
_rdc = new RayDataComparer();
_data = new List<ShapeData>();
}
/// <summary>
/// This makes the explosive explode
/// </summary>
/// <param name="pos">
/// The position where the explosion happens
/// </param>
/// <param name="radius">
/// The explosion radius
/// </param>
/// <param name="maxForce">
/// The explosion force at the explosion point
/// (then is inversely proportional to the square of the distance)
/// </param>
/// <returns>
/// A dictionnary containing all the "exploded" fixtures
/// with a list of the applied impulses
/// </returns>
public Dictionary<Fixture, List<Vector2>> 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<max
{
last.Min = _data.Last().Min - 2 * MathHelper.Pi;
_data[lastPos] = last;
}
rayMissed = false;
}
else
{
rayMissed = true; // raycast did not find a shape
}
}
for (int i = 0; i < _data.Count(); ++i)
{
if (!IsActiveOn(_data[i].Body))
continue;
float arclen = _data[i].Max - _data[i].Min;
float first = MathHelper.Min(MaxEdgeOffset, EdgeRatio * arclen);
int insertedRays = (int)Math.Ceiling(((arclen - 2.0f * first) - (MinRays - 1) * MaxAngle) / MaxAngle);
if (insertedRays < 0)
insertedRays = 0;
float offset = (arclen - first * 2.0f) / ((float)MinRays + insertedRays - 1);
//Note: This loop can go into infinite as it operates on floats.
//Added FloatEquals with a large epsilon.
for (float j = _data[i].Min + first;
j < _data[i].Max || MathUtils.FloatEquals(j, _data[i].Max, 0.0001f);
j += offset)
{
Vector2 p1 = pos;
Vector2 p2 = pos + radius * new Vector2((float)Math.Cos(j), (float)Math.Sin(j));
Vector2 hitpoint = Vector2.Zero;
float minlambda = float.MaxValue;
List<Fixture> 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<Vector2> 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<Vector2>();
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<Vector2> vectorList = new List<Vector2>();
vectorList.Add(vectImp);
fix.Body.ApplyLinearImpulse(ref vectImp, ref hitPoint);
if (!_exploded.ContainsKey(fix))
_exploded.Add(fix, vectorList);
}
return _exploded;
}
}
}

66
axios/Common/PhysicsLogic/PhysicsLogic.cs 100644
View File

@ -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;
/// <summary>
/// Ignores the controller. The controller has no effect on this body.
/// </summary>
/// <param name="type">The logic type.</param>
public void IgnorePhysicsLogic(PhysicsLogicType type)
{
ControllerIgnores |= type;
}
/// <summary>
/// Restore the controller. The controller affects this body.
/// </summary>
/// <param name="type">The logic type.</param>
public void RestorePhysicsLogic(PhysicsLogicType type)
{
ControllerIgnores &= ~type;
}
/// <summary>
/// Determines whether this body ignores the the specified controller.
/// </summary>
/// <param name="type">The logic type.</param>
/// <returns>
/// <c>true</c> if the body has the specified flag; otherwise, <c>false</c>.
/// </returns>
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;
}
}
}

246
axios/Common/PolygonManipulation/CuttingTools.cs 100644
View File

@ -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
/// <summary>
/// Split a fixture into 2 vertice collections using the given entry and exit-point.
/// </summary>
/// <param name="fixture">The Fixture to split</param>
/// <param name="entryPoint">The entry point - The start point</param>
/// <param name="exitPoint">The exit point - The end point</param>
/// <param name="splitSize">The size of the split. Think of this as the laser-width</param>
/// <param name="first">The first collection of vertexes</param>
/// <param name="second">The second collection of vertexes</param>
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];
}
/// <summary>
/// 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.
/// </summary>
/// <param name="world">The world.</param>
/// <param name="start">The startpoint.</param>
/// <param name="end">The endpoint.</param>
/// <param name="thickness">The thickness of the cut</param>
public static void Cut(World world, Vector2 start, Vector2 end, float thickness)
{
List<Fixture> fixtures = new List<Fixture>();
List<Vector2> entryPoints = new List<Vector2>();
List<Vector2> exitPoints = new List<Vector2>();
//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;
}
}
}

359
axios/Common/PolygonManipulation/SimplifyTools.cs 100644
View File

@ -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;
/// <summary>
/// Removes all collinear points on the polygon.
/// </summary>
/// <param name="vertices">The polygon that needs simplification.</param>
/// <param name="collinearityTolerance">The collinearity tolerance.</param>
/// <returns>A simplified polygon.</returns>
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;
}
/// <summary>
/// Removes all collinear points on the polygon.
/// Has a default bias of 0
/// </summary>
/// <param name="vertices">The polygon that needs simplification.</param>
/// <returns>A simplified polygon.</returns>
public static Vertices CollinearSimplify(Vertices vertices)
{
return CollinearSimplify(vertices, 0);
}
/// <summary>
/// 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
/// </summary>
/// <returns>The simplified polygon</returns>
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<r<1 Point is interior to AB
*/
double r = ((p.X - A.X) * (B.X - A.X) + (p.Y - A.Y) * (B.Y - A.Y))
/
((B.X - A.X) * (B.X - A.X) + (B.Y - A.Y) * (B.Y - A.Y));
if (r <= 0.0) return DistancePointPoint(p, A);
if (r >= 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
/// <summary>
/// Merges all parallel edges in the list of vertices
/// </summary>
/// <param name="vertices">The vertices.</param>
/// <param name="tolerance">The tolerance.</param>
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
/// <summary>
/// Merges the identical points in the polygon.
/// </summary>
/// <param name="vertices">The vertices.</param>
/// <returns></returns>
public static Vertices MergeIdenticalPoints(Vertices vertices)
{
//We use a dictonary here because HashSet is not avaliable on all platforms.
HashSet<Vector2> results = new HashSet<Vector2>();
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;
}
/// <summary>
/// Reduces the polygon by distance.
/// </summary>
/// <param name="vertices">The vertices.</param>
/// <param name="distance">The distance between points. Points closer than this will be 'joined'.</param>
/// <returns></returns>
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;
}
/// <summary>
/// Reduces the polygon by removing the Nth vertex in the vertices list.
/// </summary>
/// <param name="vertices">The vertices.</param>
/// <param name="nth">The Nth point to remove. Example: 5.</param>
/// <returns></returns>
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;
}
}
}

513
axios/Common/PolygonManipulation/YuPengClipper.cs 100644
View File

@ -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<Vertices> Union(Vertices polygon1, Vertices polygon2, out PolyClipError error)
{
return Execute(polygon1, polygon2, PolyClipType.Union, out error);
}
public static List<Vertices> Difference(Vertices polygon1, Vertices polygon2, out PolyClipError error)
{
return Execute(polygon1, polygon2, PolyClipType.Difference, out error);
}
public static List<Vertices> Intersect(Vertices polygon1, Vertices polygon2, out PolyClipError error)
{
return Execute(polygon1, polygon2, PolyClipType.Intersect, out error);
}
/// <summary>
/// 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.
/// </summary>
/// <param name="subject">The subject polygon.</param>
/// <param name="clip">The clip polygon, which is added,
/// substracted or intersected with the subject</param>
/// <param name="clipType">The operation to be performed. Either
/// Union, Difference or Intersection.</param>
/// <param name="error">The error generated (if any)</param>
/// <returns>A list of closed polygons, which make up the result of the clipping operation.
/// Outer contours are ordered counter clockwise, holes are ordered clockwise.</returns>
private static List<Vertices> 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<Edge> subjectSimplices;
List<float> subjectCoeff;
List<Edge> clipSimplices;
List<float> 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<Edge> 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<Vertices> 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;
}
/// <summary>
/// Calculates all intersections between two polygons.
/// </summary>
/// <param name="polygon1">The first polygon.</param>
/// <param name="polygon2">The second polygon.</param>
/// <param name="slicedPoly1">Returns the first polygon with added intersection points.</param>
/// <param name="slicedPoly2">Returns the second polygon with added intersection points.</param>
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;
}
}
}
/// <summary>
/// Calculates the simplical chain corresponding to the input polygon.
/// </summary>
/// <remarks>Used by method <c>Execute()</c>.</remarks>
private static void CalculateSimplicalChain(Vertices poly, out List<float> coeff,
out List<Edge> simplicies)
{
simplicies = new List<Edge>();
coeff = new List<float>();
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)]));
}
}
/// <summary>
/// Calculates the characteristics function for all edges of
/// the given simplical chains and builds the result chain.
/// </summary>
/// <remarks>Used by method <c>Execute()</c>.</remarks>
private static void CalculateResultChain(List<float> poly1Coeff, List<Edge> poly1Simplicies,
List<float> poly2Coeff, List<Edge> poly2Simplicies,
PolyClipType clipType, out List<Edge> resultSimplices)
{
resultSimplices = new List<Edge>();
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]);
}
}
}
}
}
/// <summary>
/// Calculates the polygon(s) from the result simplical chain.
/// </summary>
/// <remarks>Used by method <c>Execute()</c>.</remarks>
private static PolyClipError BuildPolygonsFromChain(List<Edge> simplicies, out List<Vertices> result)
{
result = new List<Vertices>();
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<Vertices>();
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;
}
/// <summary>
/// Needed to calculate the characteristics function of a simplex.
/// </summary>
/// <remarks>Used by method <c>CalculateEdgeCharacter()</c>.</remarks>
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;
}
/// <summary>
/// Needed for sorting multiple intersections points on the same edge.
/// </summary>
/// <remarks>Used by method <c>CalculateIntersections()</c>.</remarks>
private static float GetAlpha(Vector2 start, Vector2 end, Vector2 point)
{
return (point - start).LengthSquared() / (end - start).LengthSquared();
}
/// <summary>
/// Returns the coefficient of a simplex.
/// </summary>
/// <remarks>Used by method <c>CalculateSimplicalChain()</c>.</remarks>
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;
}
/// <summary>
/// Winding number test for a point in a simplex.
/// </summary>
/// <param name="point">The point to be tested.</param>
/// <param name="edge">The edge that the point is tested against.</param>
/// <returns>False if the winding number is even and the point is outside
/// the simplex and True otherwise.</returns>
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);
}
/// <summary>
/// Tests if a point lies on a line segment.
/// </summary>
/// <remarks>Used by method <c>CalculateBeta()</c>.</remarks>
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
/// <summary>Specifies an Edge. Edges are used to represent simplicies in simplical chains</summary>
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
}
}

368
axios/Common/PolygonTools.cs 100644
View File

@ -0,0 +1,368 @@
using System;
using System.Collections.Generic;
using System.Diagnostics;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Common
{
public static class PolygonTools
{
/// <summary>
/// Build vertices to represent an axis-aligned box.
/// </summary>
/// <param name="hx">the half-width.</param>
/// <param name="hy">the half-height.</param>
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;
}
/// <summary>
/// Build vertices to represent an oriented box.
/// </summary>
/// <param name="hx">the half-width.</param>
/// <param name="hy">the half-height.</param>
/// <param name="center">the center of the box in local coordinates.</param>
/// <param name="angle">the rotation of the box in local coordinates.</param>
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
/// <summary>
/// Creates a rounded rectangle with the specified width and height.
/// </summary>
/// <param name="width">The width.</param>
/// <param name="height">The height.</param>
/// <param name="xRadius">The rounding X radius.</param>
/// <param name="yRadius">The rounding Y radius.</param>
/// <param name="segments">The number of segments to subdivide the edges.</param>
/// <returns></returns>
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;
}
/// <summary>
/// Set this as a single edge.
/// </summary>
/// <param name="start">The first point.</param>
/// <param name="end">The second point.</param>
public static Vertices CreateLine(Vector2 start, Vector2 end)
{
Vertices vertices = new Vertices(2);
vertices.Add(start);
vertices.Add(end);
return vertices;
}
/// <summary>
/// Creates a circle with the specified radius and number of edges.
/// </summary>
/// <param name="radius">The radius.</param>
/// <param name="numberOfEdges">The number of edges. The more edges, the more it resembles a circle</param>
/// <returns></returns>
public static Vertices CreateCircle(float radius, int numberOfEdges)
{
return CreateEllipse(radius, radius, numberOfEdges);
}
/// <summary>
/// Creates a ellipse with the specified width, height and number of edges.
/// </summary>
/// <param name="xRadius">Width of the ellipse.</param>
/// <param name="yRadius">Height of the ellipse.</param>
/// <param name="numberOfEdges">The number of edges. The more edges, the more it resembles an ellipse</param>
/// <returns></returns>
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
/// <summary>
/// Creates an capsule with the specified height, radius and number of edges.
/// A capsule has the same form as a pill capsule.
/// </summary>
/// <param name="height">Height (inner height + 2 * radius) of the capsule.</param>
/// <param name="endRadius">Radius of the capsule ends.</param>
/// <param name="edges">The number of edges of the capsule ends. The more edges, the more it resembles an capsule</param>
/// <returns></returns>
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);
}
/// <summary>
/// Creates an capsule with the specified height, radius and number of edges.
/// A capsule has the same form as a pill capsule.
/// </summary>
/// <param name="height">Height (inner height + radii) of the capsule.</param>
/// <param name="topRadius">Radius of the top.</param>
/// <param name="topEdges">The number of edges of the top. The more edges, the more it resembles an capsule</param>
/// <param name="bottomRadius">Radius of bottom.</param>
/// <param name="bottomEdges">The number of edges of the bottom. The more edges, the more it resembles an capsule</param>
/// <returns></returns>
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;
}
/// <summary>
/// Creates a gear shape with the specified radius and number of teeth.
/// </summary>
/// <param name="radius">The radius.</param>
/// <param name="numberOfTeeth">The number of teeth.</param>
/// <param name="tipPercentage">The tip percentage.</param>
/// <param name="toothHeight">Height of the tooth.</param>
/// <returns></returns>
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;
}
/// <summary>
/// Detects the vertices by analyzing the texture data.
/// </summary>
/// <param name="data">The texture data.</param>
/// <param name="width">The texture width.</param>
/// <returns></returns>
public static Vertices CreatePolygon(uint[] data, int width)
{
return TextureConverter.DetectVertices(data, width);
}
/// <summary>
/// Detects the vertices by analyzing the texture data.
/// </summary>
/// <param name="data">The texture data.</param>
/// <param name="width">The texture width.</param>
/// <param name="holeDetection">if set to <c>true</c> it will perform hole detection.</param>
/// <returns></returns>
public static Vertices CreatePolygon(uint[] data, int width, bool holeDetection)
{
return TextureConverter.DetectVertices(data, width, holeDetection);
}
/// <summary>
/// Detects the vertices by analyzing the texture data.
/// </summary>
/// <param name="data">The texture data.</param>
/// <param name="width">The texture width.</param>
/// <param name="hullTolerance">The hull tolerance.</param>
/// <param name="alphaTolerance">The alpha tolerance.</param>
/// <param name="multiPartDetection">if set to <c>true</c> it will perform multi part detection.</param>
/// <param name="holeDetection">if set to <c>true</c> it will perform hole detection.</param>
/// <returns></returns>
public static List<Vertices> CreatePolygon(uint[] data, int width, float hullTolerance,
byte alphaTolerance, bool multiPartDetection, bool holeDetection)
{
return TextureConverter.DetectVertices(data, width, hullTolerance, alphaTolerance,
multiPartDetection, holeDetection);
}
}
}

1453
axios/Common/Serialization.cs 100644
View File

File diff suppressed because it is too large Load Diff

367
axios/Common/TextureTools/MSTerrain.cs 100644
View File

@ -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,
}
/// <summary>
/// Return true if the specified color is inside the terrain.
/// </summary>
public delegate bool TerrainTester(Color color);
/// <summary>
/// Simple class to maintain a terrain.
/// </summary>
public class MSTerrain
{
/// <summary>
/// World to manage terrain in.
/// </summary>
public World World;
/// <summary>
/// Center of terrain in world units.
/// </summary>
public Vector2 Center;
/// <summary>
/// Width of terrain in world units.
/// </summary>
public float Width;
/// <summary>
/// Height of terrain in world units.
/// </summary>
public float Height;
/// <summary>
/// Points per each world unit used to define the terrain in the point cloud.
/// </summary>
public int PointsPerUnit;
/// <summary>
/// Points per cell.
/// </summary>
public int CellSize;
/// <summary>
/// Points per sub cell.
/// </summary>
public int SubCellSize;
/// <summary>
/// Number of iterations to perform in the Marching Squares algorithm.
/// Note: More then 3 has almost no effect on quality.
/// </summary>
public int Iterations = 2;
/// <summary>
/// Decomposer to use when regenerating terrain. Can be changed on the fly without consequence.
/// Note: Some decomposerers are unstable.
/// </summary>
public Decomposer Decomposer;
/// <summary>
/// Point cloud defining the terrain.
/// </summary>
private sbyte[,] _terrainMap;
/// <summary>
/// Generated bodies.
/// </summary>
private List<Body>[,] _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;
}
/// <summary>
/// Initialize the terrain for use.
/// </summary>
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<Body>[_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));
}
/// <summary>
/// Apply a texture to the terrain using the specified TerrainTester.
/// </summary>
/// <param name="texture">Texture to apply.</param>
/// <param name="position">Top left position of the texture relative to the terrain.</param>
/// <param name="tester">Delegate method used to determine what colors should be included in the terrain.</param>
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);
}
}
}
/// <summary>
/// Apply a texture to the terrain using the specified TerrainTester.
/// </summary>
/// <param name="position">Top left position of the texture relative to the terrain.</param>
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);
}
}
}
/// <summary>
/// Convert a texture to an sbtye array compatible with ApplyData().
/// </summary>
/// <param name="texture">Texture to convert.</param>
/// <param name="tester"></param>
/// <returns></returns>
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;
}
/// <summary>
/// Modify a single point in the terrain.
/// </summary>
/// <param name="location">World location to modify. Automatically clipped.</param>
/// <param name="value">-1 = inside terrain, 1 = outside terrain</param>
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;
}
}
/// <summary>
/// Regenerate the terrain.
/// </summary>
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<Vertices> 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<Body>();
// 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<Vertices> decompPolys = new List<Vertices>();
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));
}
}
}
}
}

800
axios/Common/TextureTools/MarchingSquares.cs 100644
View File

@ -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
{
/// <summary>
/// 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.
/// </summary>
/// <param name="domain"></param>
/// <param name="cellWidth"></param>
/// <param name="cellHeight"></param>
/// <param name="f"></param>
/// <param name="lerpCount"></param>
/// <param name="combine"></param>
/// <returns></returns>
public static List<Vertices> DetectSquares(AABB domain, float cellWidth, float cellHeight, sbyte[,] f,
int lerpCount, bool combine)
{
CxFastList<GeomPoly> ret = new CxFastList<GeomPoly>();
List<Vertices> verticesList = new List<Vertices>();
//NOTE: removed assignments as they were not used.
List<GeomPoly> 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<Vector2> bp = p.GeomP.Points;
CxFastList<Vector2> 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<Vector2> 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<Vector2> 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<Vector2> 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<Vector2> 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<Vector2> ap = polya.Points;
CxFastList<Vector2> bp = polyb.Points;
CxFastListNode<Vector2> ai = ap.Begin();
CxFastListNode<Vector2> bi = bp.Begin();
Vector2 b = bi.Elem();
CxFastListNode<Vector2> 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<Vector2> 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
/// <summary>
/// Designed as a complete port of CxFastList from CxStd.
/// </summary>
internal class CxFastList<T>
{
// first node in the list
private CxFastListNode<T> _head;
private int _count;
/// <summary>
/// Iterator to start of list (O(1))
/// </summary>
public CxFastListNode<T> Begin()
{
return _head;
}
/// <summary>
/// Iterator to end of list (O(1))
/// </summary>
public CxFastListNode<T> End()
{
return null;
}
/// <summary>
/// Returns first element of list (O(1))
/// </summary>
public T Front()
{
return _head.Elem();
}
/// <summary>
/// add object to list (O(1))
/// </summary>
public CxFastListNode<T> Add(T value)
{
CxFastListNode<T> newNode = new CxFastListNode<T>(value);
if (_head == null)
{
newNode._next = null;
_head = newNode;
_count++;
return newNode;
}
newNode._next = _head;
_head = newNode;
_count++;
return newNode;
}
/// <summary>
/// remove object from list, returns true if an element was removed (O(n))
/// </summary>
public bool Remove(T value)
{
CxFastListNode<T> head = _head;
CxFastListNode<T> prev = _head;
EqualityComparer<T> comparer = EqualityComparer<T>.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;
}
/// <summary>
/// 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.
/// </summary>
public CxFastListNode<T> Pop()
{
return Erase(null, _head);
}
/// <summary>
/// insert object after 'node' returning an iterator to the inserted object.
/// </summary>
public CxFastListNode<T> Insert(CxFastListNode<T> node, T value)
{
if (node == null)
{
return Add(value);
}
CxFastListNode<T> newNode = new CxFastListNode<T>(value);
CxFastListNode<T> nextNode = node._next;
newNode._next = nextNode;
node._next = newNode;
_count++;
return newNode;
}
/// <summary>
/// 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))
/// </summary>
public CxFastListNode<T> Erase(CxFastListNode<T> prev, CxFastListNode<T> node)
{
// cache the node after the node to be removed
CxFastListNode<T> nextNode = node._next;
if (prev != null)
prev._next = nextNode;
else if (_head != null)
_head = _head._next;
else
return null;
_count--;
return nextNode;
}
/// <summary>
/// whether the list is empty (O(1))
/// </summary>
public bool Empty()
{
if (_head == null)
return true;
return false;
}
/// <summary>
/// computes size of list (O(n))
/// </summary>
public int Size()
{
CxFastListNode<T> i = Begin();
int count = 0;
do
{
count++;
} while (i.Next() != null);
return count;
}
/// <summary>
/// empty the list (O(1) if CxMixList, O(n) otherwise)
/// </summary>
public void Clear()
{
CxFastListNode<T> head = _head;
while (head != null)
{
CxFastListNode<T> node2 = head;
head = head._next;
node2._next = null;
}
_head = null;
_count = 0;
}
/// <summary>
/// returns true if 'value' is an element of the list (O(n))
/// </summary>
public bool Has(T value)
{
return (Find(value) != null);
}
// Non CxFastList Methods
public CxFastListNode<T> Find(T value)
{
// start at head
CxFastListNode<T> head = _head;
EqualityComparer<T> comparer = EqualityComparer<T>.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<T> GetListOfElements()
{
List<T> list = new List<T>();
CxFastListNode<T> 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<T>
{
internal T _elt;
internal CxFastListNode<T> _next;
public CxFastListNode(T obj)
{
_elt = obj;
}
public T Elem()
{
return _elt;
}
public CxFastListNode<T> Next()
{
return _next;
}
}
#endregion
#endregion
#region Internal Stuff
#region Nested type: GeomPoly
internal class GeomPoly
{
public int Length;
public CxFastList<Vector2> Points;
public GeomPoly()
{
Points = new CxFastList<Vector2>();
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
}
}

1338
axios/Common/TextureTools/TextureConverter.cs 100644
View File

File diff suppressed because it is too large Load Diff

955
axios/Common/Vertices.cs 100644
View File

@ -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<Vector2>
{
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<Vector2> vertices)
{
for (int i = 0; i < vertices.Count; i++)
{
Add(vertices[i]);
}
}
/// <summary>
/// Nexts the index.
/// </summary>
/// <param name="index">The index.</param>
/// <returns></returns>
public int NextIndex(int index)
{
if (index == Count - 1)
{
return 0;
}
return index + 1;
}
public Vector2 NextVertex(int index)
{
return this[NextIndex(index)];
}
/// <summary>
/// Gets the previous index.
/// </summary>
/// <param name="index">The index.</param>
/// <returns></returns>
public int PreviousIndex(int index)
{
if (index == 0)
{
return Count - 1;
}
return index - 1;
}
public Vector2 PreviousVertex(int index)
{
return this[PreviousIndex(index)];
}
/// <summary>
/// Gets the signed area.
/// </summary>
/// <returns></returns>
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;
}
/// <summary>
/// Gets the area.
/// </summary>
/// <returns></returns>
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);
}
/// <summary>
/// Gets the centroid.
/// </summary>
/// <returns></returns>
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;
}
/// <summary>
/// Gets the radius based on area.
/// </summary>
/// <returns></returns>
public float GetRadius()
{
float area = GetSignedArea();
double radiusSqrd = (double)area / MathHelper.Pi;
if (radiusSqrd < 0)
{
radiusSqrd *= -1;
}
return (float)Math.Sqrt(radiusSqrd);
}
/// <summary>
/// Returns an AABB for vertex.
/// </summary>
/// <returns></returns>
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);
}
/// <summary>
/// Translates the vertices with the specified vector.
/// </summary>
/// <param name="vector">The vector.</param>
public void Translate(ref Vector2 vector)
{
for (int i = 0; i < Count; i++)
this[i] = Vector2.Add(this[i], vector);
}
/// <summary>
/// Scales the vertices with the specified vector.
/// </summary>
/// <param name="value">The Value.</param>
public void Scale(ref Vector2 value)
{
for (int i = 0; i < Count; i++)
this[i] = Vector2.Multiply(this[i], value);
}
/// <summary>
/// Rotate the vertices with the defined value in radians.
/// </summary>
/// <param name="value">The amount to rotate by in radians.</param>
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);
}
/// <summary>
/// Assuming the polygon is simple; determines whether the polygon is convex.
/// NOTE: It will also return false if the input contains colinear edges.
/// </summary>
/// <returns>
/// <c>true</c> if it is convex; otherwise, <c>false</c>.
/// </returns>
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);
}
/// <summary>
/// Forces counter clock wise order.
/// </summary>
public void ForceCounterClockWise()
{
if (!IsCounterClockWise())
{
Reverse();
}
}
/// <summary>
/// Check for edge crossings
/// </summary>
/// <returns></returns>
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
/// <summary>
/// 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.
/// </summary>
/// <returns></returns>
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
/// <summary>
/// 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)
/// </summary>
/// <param name="verts">The vertices.</param>
/// <returns></returns>
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();
}
/// <summary>
/// Projects to axis.
/// </summary>
/// <param name="axis">The axis.</param>
/// <param name="min">The min.</param>
/// <param name="max">The max.</param>
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;
}
}
}
}
/// <summary>
/// Winding number test for a point in a polygon.
/// </summary>
/// See more info about the algorithm here: http://softsurfer.com/Archive/algorithm_0103/algorithm_0103.htm
/// <param name="point">The point to be tested.</param>
/// <returns>-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.</returns>
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);
}
/// <summary>
/// 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
/// </summary>
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;
}
}
}

323
axios/Controllers/AbstractForceController.cs 100644
View File

@ -0,0 +1,323 @@
using System;
using FarseerPhysics.Dynamics;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Controllers
{
public abstract class AbstractForceController : Controller
{
#region DecayModes enum
/// <summary>
/// Modes for Decay. Actual Decay must be implemented in inheriting
/// classes
/// </summary>
public enum DecayModes
{
None,
Step,
Linear,
InverseSquare,
Curve
}
#endregion
#region ForceTypes enum
/// <summary>
/// Forcetypes are used in the decay math to properly get the distance.
/// They are also used to draw a representation in DebugView
/// </summary>
public enum ForceTypes
{
Point,
Line,
Area
}
#endregion
#region TimingModes enum
/// <summary>
/// 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
/// </summary>
public enum TimingModes
{
Switched,
Triggered,
Curve
}
#endregion
/// <summary>
/// Curve to be used for Decay in Curve mode
/// </summary>
public Curve DecayCurve;
/// <summary>
/// The Forcetype of the instance
/// </summary>
public ForceTypes ForceType;
/// <summary>
/// Provided for reuse to provide Variation functionality in
/// inheriting classes
/// </summary>
protected Random Randomize;
/// <summary>
/// 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.
/// </summary>
public Curve StrengthCurve;
/// <summary>
/// Constructor
/// </summary>
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));
}
/// <summary>
/// Overloaded Contstructor with supplying Timing Mode
/// </summary>
/// <param name="mode"></param>
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;
}
}
/// <summary>
/// Global Strength of the force to be applied
/// </summary>
public float Strength { get; set; }
/// <summary>
/// Position of the Force. Can be ignored (left at (0,0) for forces
/// that are not position-dependent
/// </summary>
public Vector2 Position { get; set; }
/// <summary>
/// Maximum speed of the bodies. Bodies that are travelling faster are
/// supposed to be ignored
/// </summary>
public float MaximumSpeed { get; set; }
/// <summary>
/// Maximum Force to be applied. As opposed to Maximum Speed this is
/// independent of the velocity of
/// the affected body
/// </summary>
public float MaximumForce { get; set; }
/// <summary>
/// Timing Mode of the force instance
/// </summary>
public TimingModes TimingMode { get; set; }
/// <summary>
/// Time of the current impulse. Incremented in update till
/// ImpulseLength is reached
/// </summary>
public float ImpulseTime { get; private set; }
/// <summary>
/// Length of a triggered impulse. Used in both Triggered and Curve Mode
/// </summary>
public float ImpulseLength { get; set; }
/// <summary>
/// Indicating if we are currently during an Impulse
/// (Triggered and Curve Mode)
/// </summary>
public bool Triggered { get; private set; }
/// <summary>
/// Variation of the force applied to each body affected
/// !! Must be used in inheriting classes properly !!
/// </summary>
public float Variation { get; set; }
/// <summary>
/// See DecayModes
/// </summary>
public DecayModes DecayMode { get; set; }
/// <summary>
/// Start of the distance based Decay. To set a non decaying area
/// </summary>
public float DecayStart { get; set; }
/// <summary>
/// Maximum distance a force should be applied
/// </summary>
public float DecayEnd { get; set; }
/// <summary>
/// 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.
/// </summary>
/// <param name="body">The body to calculate decay for</param>
/// <returns>A multiplier to multiply the force with to add decay
/// support in inheriting classes</returns>
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;
}
}
/// <summary>
/// Triggers the trigger modes (Trigger and Curve)
/// </summary>
public void Trigger()
{
Triggered = true;
ImpulseTime = 0;
}
/// <summary>
/// Inherited from Controller
/// Depending on the TimingMode perform timing logic and call ApplyForce()
/// </summary>
/// <param name="dt"></param>
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;
}
}
}
/// <summary>
/// Apply the force supplying strength (wich is modified in Update()
/// according to the TimingMode
/// </summary>
/// <param name="dt"></param>
/// <param name="strength">The strength</param>
public abstract void ApplyForce(float dt, float strength);
}
}

135
axios/Controllers/BuoyancyController.cs 100644
View File

@ -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
{
/// <summary>
/// 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.
/// </summary>
public float AngularDragCoefficient;
/// <summary>
/// Density of the fluid. Higher values will make things more buoyant, lower values will cause things to sink.
/// </summary>
public float Density;
/// <summary>
/// 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.
/// </summary>
public float LinearDragCoefficient;
/// <summary>
/// Acts like waterflow. Defaults to 0,0.
/// </summary>
public Vector2 Velocity;
private AABB _container;
private Vector2 _gravity;
private Vector2 _normal;
private float _offset;
private Dictionary<int, Body> _uniqueBodies = new Dictionary<int, Body>();
/// <summary>
/// Initializes a new instance of the <see cref="BuoyancyController"/> class.
/// </summary>
/// <param name="container">Only bodies inside this AABB will be influenced by the controller</param>
/// <param name="density">Density of the fluid</param>
/// <param name="linearDragCoefficient">Linear drag coefficient of the fluid</param>
/// <param name="rotationalDragCoefficient">Rotational drag coefficient of the fluid</param>
/// <param name="gravity">The direction gravity acts. Buoyancy force will act in opposite direction of gravity.</param>
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<int, Body> 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);
}
}
}
}

71
axios/Controllers/Controller.cs 100644
View File

@ -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;
/// <summary>
/// Ignores the controller. The controller has no effect on this body.
/// </summary>
/// <param name="controller">The controller type.</param>
public void IgnoreController(ControllerType controller)
{
ControllerFlags |= controller;
}
/// <summary>
/// Restore the controller. The controller affects this body.
/// </summary>
/// <param name="controller">The controller type.</param>
public void RestoreController(ControllerType controller)
{
ControllerFlags &= ~controller;
}
/// <summary>
/// Determines whether this body ignores the the specified controller.
/// </summary>
/// <param name="controller">The controller type.</param>
/// <returns>
/// <c>true</c> if the body has the specified flag; otherwise, <c>false</c>.
/// </returns>
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);
}
}

117
axios/Controllers/GravityController.cs 100644
View File

@ -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<Body> Bodies = new List<Body>();
public List<Vector2> Points = new List<Vector2>();
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);
}
}
}

75
axios/Controllers/SimpleWindForce.cs 100644
View File

@ -0,0 +1,75 @@
using FarseerPhysics.Dynamics;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Controllers
{
/// <summary>
/// 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
/// </summary>
public class SimpleWindForce : AbstractForceController
{
/// <summary>
/// Direction of the windforce
/// </summary>
public Vector2 Direction { get; set; }
/// <summary>
/// The amount of Direction randomization. Allowed range is 0-1.
/// </summary>
public float Divergence { get; set; }
/// <summary>
/// Ignore the position and apply the force. If off only in the "front" (relative to position and direction)
/// will be affected
/// </summary>
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);
}
}
}
}
}
}

129
axios/Controllers/VelocityLimitController.cs 100644
View File

@ -0,0 +1,129 @@
using System;
using System.Collections.Generic;
using FarseerPhysics.Dynamics;
namespace FarseerPhysics.Controllers
{
/// <summary>
/// Put a limit on the linear (translation - the movespeed) and angular (rotation) velocity
/// of bodies added to this controller.
/// </summary>
public class VelocityLimitController : Controller
{
public bool LimitAngularVelocity = true;
public bool LimitLinearVelocity = true;
private List<Body> _bodies = new List<Body>();
private float _maxAngularSqared;
private float _maxAngularVelocity;
private float _maxLinearSqared;
private float _maxLinearVelocity;
/// <summary>
/// Initializes a new instance of the <see cref="VelocityLimitController"/> class.
/// Sets the max linear velocity to Settings.MaxTranslation
/// Sets the max angular velocity to Settings.MaxRotation
/// </summary>
public VelocityLimitController()
: base(ControllerType.VelocityLimitController)
{
MaxLinearVelocity = Settings.MaxTranslation;
MaxAngularVelocity = Settings.MaxRotation;
}
/// <summary>
/// Initializes a new instance of the <see cref="VelocityLimitController"/> class.
/// Pass in 0 or float.MaxValue to disable the limit.
/// maxAngularVelocity = 0 will disable the angular velocity limit.
/// </summary>
/// <param name="maxLinearVelocity">The max linear velocity.</param>
/// <param name="maxAngularVelocity">The max angular velocity.</param>
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;
}
/// <summary>
/// Gets or sets the max angular velocity.
/// </summary>
/// <value>The max angular velocity.</value>
public float MaxAngularVelocity
{
get { return _maxAngularVelocity; }
set
{
_maxAngularVelocity = value;
_maxAngularSqared = _maxAngularVelocity * _maxAngularVelocity;
}
}
/// <summary>
/// Gets or sets the max linear velocity.
/// </summary>
/// <value>The max linear velocity.</value>
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);
}
}
}

185
axios/DebugView.cs 100644
View File

@ -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
{
/// <summary>
/// Draw shapes.
/// </summary>
Shape = (1 << 0),
/// <summary>
/// Draw joint connections.
/// </summary>
Joint = (1 << 1),
/// <summary>
/// Draw axis aligned bounding boxes.
/// </summary>
AABB = (1 << 2),
/// <summary>
/// Draw broad-phase pairs.
/// </summary>
Pair = (1 << 3),
/// <summary>
/// Draw center of mass frame.
/// </summary>
CenterOfMass = (1 << 4),
/// <summary>
/// Draw useful debug data such as timings and number of bodies, joints, contacts and more.
/// </summary>
DebugPanel = (1 << 5),
/// <summary>
/// Draw contact points between colliding bodies.
/// </summary>
ContactPoints = (1 << 6),
/// <summary>
/// Draw contact normals. Need ContactPoints to be enabled first.
/// </summary>
ContactNormals = (1 << 7),
/// <summary>
/// Draws the vertices of polygons.
/// </summary>
PolygonPoints = (1 << 8),
/// <summary>
/// Draws the performance graph.
/// </summary>
PerformanceGraph = (1 << 9),
/// <summary>
/// Draws controllers.
/// </summary>
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; }
/// <summary>
/// Gets or sets the debug view flags.
/// </summary>
/// <value>The flags.</value>
public DebugViewFlags Flags { get; set; }
/// <summary>
/// Append flags to the current flags.
/// </summary>
/// <param name="flags">The flags.</param>
public void AppendFlags(DebugViewFlags flags)
{
Flags |= flags;
}
/// <summary>
/// Remove flags from the current flags.
/// </summary>
/// <param name="flags">The flags.</param>
public void RemoveFlags(DebugViewFlags flags)
{
Flags &= ~flags;
}
/// <summary>
/// Draw a closed polygon provided in CCW order.
/// </summary>
/// <param name="vertices">The vertices.</param>
/// <param name="count">The vertex count.</param>
/// <param name="red">The red value.</param>
/// <param name="blue">The blue value.</param>
/// <param name="green">The green value.</param>
public abstract void DrawPolygon(Vector2[] vertices, int count, float red, float blue, float green);
/// <summary>
/// Draw a solid closed polygon provided in CCW order.
/// </summary>
/// <param name="vertices">The vertices.</param>
/// <param name="count">The vertex count.</param>
/// <param name="red">The red value.</param>
/// <param name="blue">The blue value.</param>
/// <param name="green">The green value.</param>
public abstract void DrawSolidPolygon(Vector2[] vertices, int count, float red, float blue, float green);
/// <summary>
/// Draw a circle.
/// </summary>
/// <param name="center">The center.</param>
/// <param name="radius">The radius.</param>
/// <param name="red">The red value.</param>
/// <param name="blue">The blue value.</param>
/// <param name="green">The green value.</param>
public abstract void DrawCircle(Vector2 center, float radius, float red, float blue, float green);
/// <summary>
/// Draw a solid circle.
/// </summary>
/// <param name="center">The center.</param>
/// <param name="radius">The radius.</param>
/// <param name="axis">The axis.</param>
/// <param name="red">The red value.</param>
/// <param name="blue">The blue value.</param>
/// <param name="green">The green value.</param>
public abstract void DrawSolidCircle(Vector2 center, float radius, Vector2 axis, float red, float blue,
float green);
/// <summary>
/// Draw a line segment.
/// </summary>
/// <param name="start">The start.</param>
/// <param name="end">The end.</param>
/// <param name="red">The red value.</param>
/// <param name="blue">The blue value.</param>
/// <param name="green">The green value.</param>
public abstract void DrawSegment(Vector2 start, Vector2 end, float red, float blue, float green);
/// <summary>
/// Draw a transform. Choose your own length scale.
/// </summary>
/// <param name="transform">The transform.</param>
public abstract void DrawTransform(ref Transform transform);
}
}

875
axios/DebugViewXNA.cs 100644
View File

@ -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
{
/// <summary>
/// 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.
/// </summary>
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> _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<float> _graphValues = new List<float>();
#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<PointState> state1, state2;
Collision.Collision.GetPointStates(out state1, out state2, ref oldManifold, ref manifold);
FixedArray2<Vector2> 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;
}
}
}
/// <summary>
/// Call this to draw shapes and other debug draw data.
/// </summary>
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<SpriteFont>("font");
_stringData = new List<StringData>();
_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
}
}

240
axios/DrawingSystem/AssetCreator.cs 100644
View File

@ -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<MaterialType, Texture2D> _materials = new Dictionary<MaterialType, Texture2D>();
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<Texture2D>("Materials/blank");
_materials[MaterialType.Dots] = contentManager.Load<Texture2D>("Materials/dots");
_materials[MaterialType.Squares] = contentManager.Load<Texture2D>("Materials/squares");
_materials[MaterialType.Waves] = contentManager.Load<Texture2D>("Materials/waves");
_materials[MaterialType.Pavement] = contentManager.Load<Texture2D>("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<Vertices> decomposedVerts;
if (!verts.IsConvex())
{
decomposedVerts = EarclipDecomposer.ConvexPartition(verts);
}
else
{
decomposedVerts = new List<Vertices>();
decomposedVerts.Add(verts);
}
List<VertexPositionColorTexture[]> verticesFill =
new List<VertexPositionColorTexture[]>(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<VertexPositionColorTexture[]> fill = new List<VertexPositionColorTexture[]>(1);
fill.Add(verticesFill);
return RenderTexture(width, height, material, fill, verticesOutline);
}
private Texture2D RenderTexture(int width, int height, Texture2D material,
List<VertexPositionColorTexture[]> 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;
}
}
}

183
axios/DrawingSystem/LineBatch.cs 100644
View File

@ -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;
}
}
}
}

23
axios/DrawingSystem/Sprite.cs 100644
View File

@ -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);
}
}
}

1388
axios/Dynamics/Body.cs 100644
View File

File diff suppressed because it is too large Load Diff

142
axios/Dynamics/BreakableBody.cs 100644
View File

@ -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
{
/// <summary>
/// A type of body that supports multiple fixtures that can break apart.
/// </summary>
public class BreakableBody
{
public bool Broken;
public Body MainBody;
public List<Fixture> Parts = new List<Fixture>(8);
/// <summary>
/// The force needed to break the body apart.
/// Default: 500
/// </summary>
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> vertices, World world, float density)
: this(vertices, world, density, null)
{
}
public BreakableBody()
{
}
public BreakableBody(IEnumerable<Vertices> 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;
}
}
}

340
axios/Dynamics/ContactManager.cs 100644
View File

@ -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
{
/// <summary>
/// Fires when a contact is created
/// </summary>
public BeginContactDelegate BeginContact;
public IBroadPhase BroadPhase;
/// <summary>
/// The filter used by the contact manager.
/// </summary>
public CollisionFilterDelegate ContactFilter;
public List<Contact> ContactList = new List<Contact>(128);
/// <summary>
/// Fires when a contact is deleted
/// </summary>
public EndContactDelegate EndContact;
/// <summary>
/// Fires when the broadphase detects that two Fixtures are close to each other.
/// </summary>
public BroadphaseDelegate OnBroadphaseCollision;
/// <summary>
/// Fires after the solver has run
/// </summary>
public PostSolveDelegate PostSolve;
/// <summary>
/// Fires before the solver runs
/// </summary>
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;
}
}
}

502
axios/Dynamics/Contacts/Contact.cs 100644
View File

@ -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
{
/// <summary>
/// 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.
/// </summary>
public sealed class ContactEdge
{
/// <summary>
/// The contact
/// </summary>
public Contact Contact;
/// <summary>
/// The next contact edge in the body's contact list
/// </summary>
public ContactEdge Next;
/// <summary>
/// Provides quick access to the other body attached.
/// </summary>
public Body Other;
/// <summary>
/// The previous contact edge in the body's contact list
/// </summary>
public ContactEdge Prev;
}
[Flags]
public enum ContactFlags
{
None = 0,
/// <summary>
/// Used when crawling contact graph when forming islands.
/// </summary>
Island = 0x0001,
/// <summary>
/// Set when the shapes are touching.
/// </summary>
Touching = 0x0002,
/// <summary>
/// This contact can be disabled (by user)
/// </summary>
Enabled = 0x0004,
/// <summary>
/// This contact needs filtering because a fixture filter was changed.
/// </summary>
Filter = 0x0008,
/// <summary>
/// This bullet contact had a TOI event
/// </summary>
BulletHit = 0x0010,
/// <summary>
/// This contact has a valid TOI i the field TOI
/// </summary>
TOI = 0x0020
}
/// <summary>
/// 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.
/// </summary>
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; }
}
/// <summary>
/// Get the child primitive index for fixture A.
/// </summary>
/// <value>The child index A.</value>
public int ChildIndexA { get; internal set; }
/// <summary>
/// Get the child primitive index for fixture B.
/// </summary>
/// <value>The child index B.</value>
public int ChildIndexB { get; internal set; }
/// <summary>
/// Get the contact manifold. Do not modify the manifold unless you understand the
/// internals of Box2D.
/// </summary>
/// <param name="manifold">The manifold.</param>
public void GetManifold(out Manifold manifold)
{
manifold = Manifold;
}
/// <summary>
/// Gets the world manifold.
/// </summary>
public void GetWorldManifold(out Vector2 normal, out FixedArray2<Vector2> 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);
}
/// <summary>
/// Determines whether this contact is touching.
/// </summary>
/// <returns>
/// <c>true</c> if this instance is touching; otherwise, <c>false</c>.
/// </returns>
public bool IsTouching()
{
return (Flags & ContactFlags.Touching) == ContactFlags.Touching;
}
/// <summary>
/// Flag this contact for filtering. Filtering will occur the next time step.
/// </summary>
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;
}
/// <summary>
/// Update the contact manifold and touching status.
/// Note: do not assume the fixture AABBs are overlapping or are valid.
/// </summary>
/// <param name="contactManager">The contact manager.</param>
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);
}
/// <summary>
/// Evaluate this contact with your own manifold and transforms.
/// </summary>
/// <param name="manifold">The manifold.</param>
/// <param name="transformA">The first transform.</param>
/// <param name="transformB">The second transform.</param>
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<Contact> 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
}
}

794
axios/Dynamics/Contacts/ContactSolver.cs 100644
View File

@ -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<Vector2> 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;
}
}
}
}

611
axios/Dynamics/Fixture.cs 100644
View File

@ -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
}
/// <summary>
/// This proxy is used internally to connect fixtures to the broad-phase.
/// </summary>
public struct FixtureProxy
{
public AABB AABB;
public int ChildIndex;
public Fixture Fixture;
public int ProxyId;
}
/// <summary>
/// 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.
/// </summary>
public class Fixture : IDisposable
{
private static int _fixtureIdCounter;
/// <summary>
/// Fires after two shapes has collided and are solved. This gives you a chance to get the impact force.
/// </summary>
public AfterCollisionEventHandler AfterCollision;
/// <summary>
/// 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.
/// </summary>
public BeforeCollisionEventHandler BeforeCollision;
/// <summary>
/// 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.
/// </summary>
public OnCollisionEventHandler OnCollision;
/// <summary>
/// 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.
/// </summary>
public OnSeparationEventHandler OnSeparation;
public FixtureProxy[] Proxies;
public int ProxyCount;
internal Category _collidesWith;
internal Category _collisionCategories;
internal short _collisionGroup;
internal Dictionary<int, bool> _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();
}
/// <summary>
/// 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.
/// </summary>
public short CollisionGroup
{
set
{
if (_collisionGroup == value)
return;
_collisionGroup = value;
Refilter();
}
get { return _collisionGroup; }
}
/// <summary>
/// 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.
/// </summary>
public Category CollidesWith
{
get { return _collidesWith; }
set
{
if (_collidesWith == value)
return;
_collidesWith = value;
Refilter();
}
}
/// <summary>
/// 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
/// </summary>
public Category CollisionCategories
{
get { return _collisionCategories; }
set
{
if (_collisionCategories == value)
return;
_collisionCategories = value;
Refilter();
}
}
/// <summary>
/// Get the type of the child Shape. You can use this to down cast to the concrete Shape.
/// </summary>
/// <value>The type of the shape.</value>
public ShapeType ShapeType
{
get { return Shape.ShapeType; }
}
/// <summary>
/// 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.
/// </summary>
/// <value>The shape.</value>
public Shape Shape { get; internal set; }
/// <summary>
/// Gets or sets a value indicating whether this fixture is a sensor.
/// </summary>
/// <value><c>true</c> if this instance is a sensor; otherwise, <c>false</c>.</value>
public bool IsSensor { get; set; }
/// <summary>
/// Get the parent body of this fixture. This is null if the fixture is not attached.
/// </summary>
/// <value>The body.</value>
public Body Body { get; internal set; }
/// <summary>
/// Set the user data. Use this to store your application specific data.
/// </summary>
/// <value>The user data.</value>
public object UserData { get; set; }
/// <summary>
/// Get or set the coefficient of friction.
/// </summary>
/// <value>The friction.</value>
public float Friction
{
get { return _friction; }
set
{
Debug.Assert(!float.IsNaN(value));
_friction = value;
}
}
/// <summary>
/// Get or set the coefficient of restitution.
/// </summary>
/// <value>The restitution.</value>
public float Restitution
{
get { return _restitution; }
set
{
Debug.Assert(!float.IsNaN(value));
_restitution = value;
}
}
/// <summary>
/// Gets a unique ID for this fixture.
/// </summary>
/// <value>The fixture id.</value>
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
/// <summary>
/// Restores collisions between this fixture and the provided fixture.
/// </summary>
/// <param name="fixture">The fixture.</param>
public void RestoreCollisionWith(Fixture fixture)
{
if (_collisionIgnores == null)
return;
if (_collisionIgnores.ContainsKey(fixture.FixtureId))
{
_collisionIgnores[fixture.FixtureId] = false;
Refilter();
}
}
/// <summary>
/// Ignores collisions between this fixture and the provided fixture.
/// </summary>
/// <param name="fixture">The fixture.</param>
public void IgnoreCollisionWith(Fixture fixture)
{
if (_collisionIgnores == null)
_collisionIgnores = new Dictionary<int, bool>();
if (_collisionIgnores.ContainsKey(fixture.FixtureId))
_collisionIgnores[fixture.FixtureId] = true;
else
_collisionIgnores.Add(fixture.FixtureId, true);
Refilter();
}
/// <summary>
/// Determines whether collisions are ignored between this fixture and the provided fixture.
/// </summary>
/// <param name="fixture">The fixture.</param>
/// <returns>
/// <c>true</c> if the fixture is ignored; otherwise, <c>false</c>.
/// </returns>
public bool IsFixtureIgnored(Fixture fixture)
{
if (_collisionIgnores == null)
return false;
if (_collisionIgnores.ContainsKey(fixture.FixtureId))
return _collisionIgnores[fixture.FixtureId];
return false;
}
/// <summary>
/// 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.
/// </summary>
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);
}
}
/// <summary>
/// Test a point for containment in this fixture.
/// </summary>
/// <param name="point">A point in world coordinates.</param>
/// <returns></returns>
public bool TestPoint(ref Vector2 point)
{
return Shape.TestPoint(ref Body.Xf, ref point);
}
/// <summary>
/// Cast a ray against this Shape.
/// </summary>
/// <param name="output">The ray-cast results.</param>
/// <param name="input">The ray-cast input parameters.</param>
/// <param name="childIndex">Index of the child.</param>
/// <returns></returns>
public bool RayCast(out RayCastOutput output, ref RayCastInput input, int childIndex)
{
return Shape.RayCast(out output, ref input, ref Body.Xf, childIndex);
}
/// <summary>
/// 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.
/// </summary>
/// <param name="aabb">The aabb.</param>
/// <param name="childIndex">Index of the child.</param>
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<int, bool>();
foreach (KeyValuePair<int, bool> 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);
}
}
}

484
axios/Dynamics/Island.cs 100644
View File

@ -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
{
/// <summary>
/// This is an internal class.
/// </summary>
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);
}
}
}
}
}

93
axios/Dynamics/Joints/AngleJoint.cs 100644
View File

@ -0,0 +1,93 @@
using System;
using System.Diagnostics;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
/// <summary>
/// Maintains a fixed angle between two bodies
/// </summary>
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;
}
}
}

286
axios/Dynamics/Joints/DistanceJoint.cs 100644
View File

@ -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
/// <summary>
/// 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.
/// </summary>
public class DistanceJoint : Joint
{
/// <summary>
/// The local anchor point relative to bodyA's origin.
/// </summary>
public Vector2 LocalAnchorA;
/// <summary>
/// The local anchor point relative to bodyB's origin.
/// </summary>
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;
}
/// <summary>
/// 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.
/// </summary>
/// <param name="bodyA">The first body</param>
/// <param name="bodyB">The second body</param>
/// <param name="localAnchorA">The first body anchor</param>
/// <param name="localAnchorB">The second body anchor</param>
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();
}
/// <summary>
/// The natural length between the anchor points.
/// Manipulating the length can lead to non-physical behavior when the frequency is zero.
/// </summary>
public float Length { get; set; }
/// <summary>
/// The mass-spring-damper frequency in Hertz.
/// </summary>
public float Frequency { get; set; }
/// <summary>
/// The damping ratio. 0 = no damping, 1 = critical damping.
/// </summary>
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;
}
}
}

84
axios/Dynamics/Joints/FixedAngleJoint.cs 100644
View File

@ -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;
}
}
}

255
axios/Dynamics/Joints/FixedDistanceJoint.cs 100644
View File

@ -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
/// <summary>
/// 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.
/// </summary>
public class FixedDistanceJoint : Joint
{
/// <summary>
/// The local anchor point relative to bodyA's origin.
/// </summary>
public Vector2 LocalAnchorA;
private float _bias;
private float _gamma;
private float _impulse;
private float _mass;
private Vector2 _u;
private Vector2 _worldAnchorB;
/// <summary>
/// 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.
/// </summary>
/// <param name="body">The body.</param>
/// <param name="bodyAnchor">The body anchor.</param>
/// <param name="worldAnchor">The world anchor.</param>
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();
}
/// <summary>
/// The natural length between the anchor points.
/// Manipulating the length can lead to non-physical behavior when the frequency is zero.
/// </summary>
public float Length { get; set; }
/// <summary>
/// The mass-spring-damper frequency in Hertz.
/// </summary>
public float Frequency { get; set; }
/// <summary>
/// The damping ratio. 0 = no damping, 1 = critical damping.
/// </summary>
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;
}
}
}

227
axios/Dynamics/Joints/FixedFrictionJoint.cs 100644
View File

@ -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
/// <summary>
/// Friction joint. This is used for top-down friction.
/// It provides 2D translational friction and angular friction.
/// </summary>
public class FixedFrictionJoint : Joint
{
public Vector2 LocalAnchorA;
/// <summary>
/// The maximum friction force in N.
/// </summary>
public float MaxForce;
/// <summary>
/// The maximum friction torque in N-m.
/// </summary>
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;
}
}
}

413
axios/Dynamics/Joints/FixedLineJoint.cs 100644
View File

@ -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;
}
}
}

209
axios/Dynamics/Joints/FixedMouseJoint.cs 100644
View File

@ -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
{
/// <summary>
/// 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.
/// </summary>
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;
/// <summary>
/// This requires a world target point,
/// tuning parameters, and the time step.
/// </summary>
/// <param name="body">The body.</param>
/// <param name="worldAnchor">The target.</param>
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;
}
}
/// <summary>
/// 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).
/// </summary>
public float MaxForce { get; set; }
/// <summary>
/// The response speed.
/// </summary>
public float Frequency { get; set; }
/// <summary>
/// The damping ratio. 0 = no damping, 1 = critical damping.
/// </summary>
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;
}
}
}

636
axios/Dynamics/Joints/FixedPrismaticJoint.cs 100644
View File

@ -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
/// <summary>
/// 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.
/// </summary>
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;
/// <summary>
/// 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.
/// </summary>
/// <param name="body">The body.</param>
/// <param name="worldAnchor">The anchor.</param>
/// <param name="axis">The axis.</param>
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."); }
}
/// <summary>
/// Get the current joint translation, usually in meters.
/// </summary>
/// <value></value>
public float JointTranslation
{
get
{
Vector2 d = BodyB.GetWorldPoint(LocalAnchorB) - LocalAnchorA;
Vector2 axis = _localXAxis1;
return Vector2.Dot(d, axis);
}
}
/// <summary>
/// Get the current joint translation speed, usually in meters per second.
/// </summary>
/// <value></value>
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;
}
}
/// <summary>
/// Is the joint limit enabled?
/// </summary>
/// <value><c>true</c> if [limit enabled]; otherwise, <c>false</c>.</value>
public bool LimitEnabled
{
get { return _enableLimit; }
set
{
Debug.Assert(BodyA.FixedRotation == false, "Warning: limits does currently not work with fixed rotation");
WakeBodies();
_enableLimit = value;
}
}
/// <summary>
/// Get the lower joint limit, usually in meters.
/// </summary>
/// <value></value>
public float LowerLimit
{
get { return _lowerTranslation; }
set
{
WakeBodies();
_lowerTranslation = value;
}
}
/// <summary>
/// Get the upper joint limit, usually in meters.
/// </summary>
/// <value></value>
public float UpperLimit
{
get { return _upperTranslation; }
set
{
WakeBodies();
_upperTranslation = value;
}
}
/// <summary>
/// Is the joint motor enabled?
/// </summary>
/// <value><c>true</c> if [motor enabled]; otherwise, <c>false</c>.</value>
public bool MotorEnabled
{
get { return _enableMotor; }
set
{
WakeBodies();
_enableMotor = value;
}
}
/// <summary>
/// Set the motor speed, usually in meters per second.
/// </summary>
/// <value>The speed.</value>
public float MotorSpeed
{
set
{
WakeBodies();
_motorSpeed = value;
}
get { return _motorSpeed; }
}
/// <summary>
/// Set the maximum motor force, usually in N.
/// </summary>
/// <value>The force.</value>
public float MaxMotorForce
{
set
{
WakeBodies();
_maxMotorForce = value;
}
}
/// <summary>
/// Get the current motor force, usually in N.
/// </summary>
/// <value></value>
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;
}
}
}

541
axios/Dynamics/Joints/FixedRevoluteJoint.cs 100644
View File

@ -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
{
/// <summary>
/// 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.
/// </summary>
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;
/// <summary>
/// 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.
/// </summary>
/// <param name="body">The body.</param>
/// <param name="bodyAnchor">The body anchor.</param>
/// <param name="worldAnchor">The world anchor.</param>
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; }
/// <summary>
/// Get the current joint angle in radians.
/// </summary>
/// <value></value>
public float JointAngle
{
get { return BodyA.Sweep.A - ReferenceAngle; }
}
/// <summary>
/// Get the current joint angle speed in radians per second.
/// </summary>
/// <value></value>
public float JointSpeed
{
get { return BodyA.AngularVelocityInternal; }
}
/// <summary>
/// Is the joint limit enabled?
/// </summary>
/// <value><c>true</c> if [limit enabled]; otherwise, <c>false</c>.</value>
public bool LimitEnabled
{
get { return _enableLimit; }
set
{
WakeBodies();
_enableLimit = value;
}
}
/// <summary>
/// Get the lower joint limit in radians.
/// </summary>
/// <value></value>
public float LowerLimit
{
get { return _lowerAngle; }
set
{
WakeBodies();
_lowerAngle = value;
}
}
/// <summary>
/// Get the upper joint limit in radians.
/// </summary>
/// <value></value>
public float UpperLimit
{
get { return _upperAngle; }
set
{
WakeBodies();
_upperAngle = value;
}
}
/// <summary>
/// Is the joint motor enabled?
/// </summary>
/// <value><c>true</c> if [motor enabled]; otherwise, <c>false</c>.</value>
public bool MotorEnabled
{
get { return _enableMotor; }
set
{
WakeBodies();
_enableMotor = value;
}
}
/// <summary>
/// Set the motor speed in radians per second.
/// </summary>
/// <value>The speed.</value>
public float MotorSpeed
{
set
{
WakeBodies();
_motorSpeed = value;
}
get { return _motorSpeed; }
}
/// <summary>
/// Set the maximum motor torque, usually in N-m.
/// </summary>
/// <value>The torque.</value>
public float MaxMotorTorque
{
set
{
WakeBodies();
_maxMotorTorque = value;
}
get { return _maxMotorTorque; }
}
/// <summary>
/// Get the current motor torque, usually in N-m.
/// </summary>
/// <value></value>
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;
}
}
}

249
axios/Dynamics/Joints/FrictionJoint.cs 100644
View File

@ -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
/// <summary>
/// Friction joint. This is used for top-down friction.
/// It provides 2D translational friction and angular friction.
/// </summary>
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."); }
}
/// <summary>
/// The maximum friction force in N.
/// </summary>
public float MaxForce { get; set; }
/// <summary>
/// The maximum friction torque in N-m.
/// </summary>
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;
}
}
}

350
axios/Dynamics/Joints/GearJoint.cs 100644
View File

@ -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
{
/// <summary>
/// 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).
/// </summary>
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;
/// <summary>
/// Requires two existing revolute or prismatic joints (any combination will work).
/// The provided joints must attach a dynamic body to a static body.
/// </summary>
/// <param name="jointA">The first joint.</param>
/// <param name="jointB">The second joint.</param>
/// <param name="ratio">The ratio.</param>
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."); }
}
/// <summary>
/// The gear ratio.
/// </summary>
public float Ratio { get; set; }
/// <summary>
/// The first revolute/prismatic joint attached to the gear joint.
/// </summary>
public Joint JointA { get; set; }
/// <summary>
/// The second revolute/prismatic joint attached to the gear joint.
/// </summary>
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;
}
}
}

Some files were not shown because too many files have changed in this diff Show More