Adding initial files
This commit is contained in:
commit
5bdc5db408
76
axios.sln
Normal file
76
axios.sln
Normal 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
|
118
axios/AxiosEngine.cd
Normal file
118
axios/AxiosEngine.cd
Normal 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<T>">
|
||||
<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
Normal file
240
axios/Axios_WP7.csproj
Normal 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
Normal file
6
axios/Axios_WP7.csproj.user
Normal 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
Normal file
271
axios/Axios_Windows.csproj
Normal 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
Normal file
6
axios/Axios_Windows.csproj.user
Normal 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
Normal file
235
axios/Axios_Xbox_360.csproj
Normal 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
Normal file
155
axios/Axios_settings.cs
Normal 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
Normal file
1945
axios/Collision/Collision.cs
Normal file
File diff suppressed because it is too large
Load Diff
780
axios/Collision/Distance.cs
Normal file
780
axios/Collision/Distance.cs
Normal 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
Normal file
654
axios/Collision/DynamicTree.cs
Normal 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
Normal file
324
axios/Collision/DynamicTreeBroadPhase.cs
Normal 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
Normal file
30
axios/Collision/IBroadPhase.cs
Normal 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
Normal file
267
axios/Collision/QuadTree.cs
Normal 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
Normal file
249
axios/Collision/QuadTreeBroadPhase.cs
Normal 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
Normal file
207
axios/Collision/Shapes/CircleShape.cs
Normal 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
Normal file
266
axios/Collision/Shapes/EdgeShape.cs
Normal 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
Normal file
188
axios/Collision/Shapes/LoopShape.cs
Normal 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
Normal file
556
axios/Collision/Shapes/PolygonShape.cs
Normal 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
Normal file
222
axios/Collision/Shapes/Shape.cs
Normal 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
Normal file
500
axios/Collision/TimeOfImpact.cs
Normal 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
Normal file
126
axios/Common/ConvexHull/ChainHull.cs
Normal 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
Normal file
99
axios/Common/ConvexHull/GiftWrap.cs
Normal 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
Normal file
122
axios/Common/ConvexHull/Melkman.cs
Normal 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
Normal file
253
axios/Common/Decomposition/BayazitDecomposer.cs
Normal 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
Normal file
420
axios/Common/Decomposition/CDT/Delaunay/DelaunayTriangle.cs
Normal 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
Normal file
180
axios/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFront.cs
Normal 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;
|
||||
}
|
||||
}
|
||||
}
|
@ -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
Normal file
1132
axios/Common/Decomposition/CDT/Delaunay/Sweep/DTSweep.cs
Normal file
File diff suppressed because it is too large
Load Diff
@ -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
Normal file
236
axios/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepContext.cs
Normal 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
|
||||
}
|
||||
}
|
@ -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
|
||||
}
|
||||
}
|
@ -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
Normal file
48
axios/Common/Decomposition/CDT/ITriangulatable.cs
Normal 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
Normal file
40
axios/Common/Decomposition/CDT/Orientation.cs
Normal 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
Normal file
272
axios/Common/Decomposition/CDT/Polygon/Polygon.cs
Normal 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
Normal file
48
axios/Common/Decomposition/CDT/Polygon/PolygonPoint.cs
Normal 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
Normal file
65
axios/Common/Decomposition/CDT/Polygon/PolygonSet.cs
Normal 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
Normal file
114
axios/Common/Decomposition/CDT/Sets/ConstrainedPointSet.cs
Normal 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
Normal file
84
axios/Common/Decomposition/CDT/Sets/PointSet.cs
Normal 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
Normal file
46
axios/Common/Decomposition/CDT/TriangulationConstraint.cs
Normal 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
Normal file
87
axios/Common/Decomposition/CDT/TriangulationContext.cs
Normal 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
Normal file
40
axios/Common/Decomposition/CDT/TriangulationMode.cs
Normal 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
Normal file
82
axios/Common/Decomposition/CDT/TriangulationPoint.cs
Normal 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
Normal file
160
axios/Common/Decomposition/CDT/TriangulationUtil.cs
Normal 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
Normal file
118
axios/Common/Decomposition/CDT/Util/FixedArray3.cs
Normal 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
Normal file
118
axios/Common/Decomposition/CDT/Util/FixedBitArray3.cs
Normal 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
Normal file
38
axios/Common/Decomposition/CDT/Util/PointGenerator.cs
Normal 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
Normal file
98
axios/Common/Decomposition/CDT/Util/PolygonGenerator.cs
Normal 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
Normal file
110
axios/Common/Decomposition/CDTDecomposer.cs
Normal 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
Normal file
691
axios/Common/Decomposition/EarclipDecomposer.cs
Normal 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
Normal file
160
axios/Common/Decomposition/FlipcodeDecomposer.cs
Normal 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
Normal file
1057
axios/Common/Decomposition/SeidelDecomposer.cs
Normal file
File diff suppressed because it is too large
Load Diff
227
axios/Common/FixedArray.cs
Normal file
227
axios/Common/FixedArray.cs
Normal 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
Normal file
81
axios/Common/HashSet.cs
Normal 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
Normal file
308
axios/Common/LineTools.cs
Normal 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
Normal file
638
axios/Common/Math.cs
Normal 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
Normal file
341
axios/Common/Path.cs
Normal 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
Normal file
240
axios/Common/PathManager.cs
Normal 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
Normal file
464
axios/Common/PhysicsLogic/Explosion.cs
Normal 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
Normal file
66
axios/Common/PhysicsLogic/PhysicsLogic.cs
Normal 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
Normal file
246
axios/Common/PolygonManipulation/CuttingTools.cs
Normal 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
Normal file
359
axios/Common/PolygonManipulation/SimplifyTools.cs
Normal 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
Normal file
513
axios/Common/PolygonManipulation/YuPengClipper.cs
Normal 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
Normal file
368
axios/Common/PolygonTools.cs
Normal 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
Normal file
1453
axios/Common/Serialization.cs
Normal file
File diff suppressed because it is too large
Load Diff
367
axios/Common/TextureTools/MSTerrain.cs
Normal file
367
axios/Common/TextureTools/MSTerrain.cs
Normal 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
Normal file
800
axios/Common/TextureTools/MarchingSquares.cs
Normal 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
Normal file
1338
axios/Common/TextureTools/TextureConverter.cs
Normal file
File diff suppressed because it is too large
Load Diff
955
axios/Common/Vertices.cs
Normal file
955
axios/Common/Vertices.cs
Normal 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
Normal file
323
axios/Controllers/AbstractForceController.cs
Normal 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
Normal file
135
axios/Controllers/BuoyancyController.cs
Normal 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
Normal file
71
axios/Controllers/Controller.cs
Normal 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
Normal file
117
axios/Controllers/GravityController.cs
Normal 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
Normal file
75
axios/Controllers/SimpleWindForce.cs
Normal 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
Normal file
129
axios/Controllers/VelocityLimitController.cs
Normal 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
Normal file
185
axios/DebugView.cs
Normal 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
Normal file
875
axios/DebugViewXNA.cs
Normal 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
Normal file
240
axios/DrawingSystem/AssetCreator.cs
Normal 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
Normal file
183
axios/DrawingSystem/LineBatch.cs
Normal 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
Normal file
23
axios/DrawingSystem/Sprite.cs
Normal 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
Normal file
1388
axios/Dynamics/Body.cs
Normal file
File diff suppressed because it is too large
Load Diff
142
axios/Dynamics/BreakableBody.cs
Normal file
142
axios/Dynamics/BreakableBody.cs
Normal 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
Normal file
340
axios/Dynamics/ContactManager.cs
Normal 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
Normal file
502
axios/Dynamics/Contacts/Contact.cs
Normal 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
Normal file
794
axios/Dynamics/Contacts/ContactSolver.cs
Normal 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
Normal file
611
axios/Dynamics/Fixture.cs
Normal 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
Normal file
484
axios/Dynamics/Island.cs
Normal 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
Normal file
93
axios/Dynamics/Joints/AngleJoint.cs
Normal 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
Normal file
286
axios/Dynamics/Joints/DistanceJoint.cs
Normal 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
Normal file
84
axios/Dynamics/Joints/FixedAngleJoint.cs
Normal 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
Normal file
255
axios/Dynamics/Joints/FixedDistanceJoint.cs
Normal 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
Normal file
227
axios/Dynamics/Joints/FixedFrictionJoint.cs
Normal 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
Normal file
413
axios/Dynamics/Joints/FixedLineJoint.cs
Normal 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
Normal file
209
axios/Dynamics/Joints/FixedMouseJoint.cs
Normal 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
Normal file
636
axios/Dynamics/Joints/FixedPrismaticJoint.cs
Normal 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
Normal file
541
axios/Dynamics/Joints/FixedRevoluteJoint.cs
Normal 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
Normal file
249
axios/Dynamics/Joints/FrictionJoint.cs
Normal 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
Normal file
350
axios/Dynamics/Joints/GearJoint.cs
Normal 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
Loading…
Reference in New Issue
Block a user