Merge pull request #23 from espes/master

Add a batch of new samples

Audio3D/Audio3D.MacOS.csproj

@@ -0,0 +1,104 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project DefaultTargets="Build" ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <PropertyGroup>
+    <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
+    <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
+    <ProductVersion>10.0.0</ProductVersion>
+    <SchemaVersion>2.0</SchemaVersion>
+    <ProjectGuid>{D5C989FD-6AE3-417F-BCB9-7AE30F195A31}</ProjectGuid>
+    <ProjectTypeGuids>{948B3504-5B70-4649-8FE4-BDE1FB46EC69};{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}</ProjectTypeGuids>
+    <OutputType>Exe</OutputType>
+    <RootNamespace>Audio3D</RootNamespace>
+    <AssemblyName>Audio3D</AssemblyName>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
+    <DebugSymbols>true</DebugSymbols>
+    <DebugType>full</DebugType>
+    <Optimize>false</Optimize>
+    <OutputPath>bin\Debug</OutputPath>
+    <DefineConstants>DEBUG;</DefineConstants>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+    <EnablePackageSigning>false</EnablePackageSigning>
+    <IncludeMonoRuntime>false</IncludeMonoRuntime>
+    <ConsolePause>false</ConsolePause>
+    <EnableCodeSigning>false</EnableCodeSigning>
+    <CreatePackage>false</CreatePackage>
+    <CodeSigningKey>Mac Developer</CodeSigningKey>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
+    <DebugType>none</DebugType>
+    <Optimize>true</Optimize>
+    <OutputPath>bin\Release</OutputPath>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+    <EnablePackageSigning>false</EnablePackageSigning>
+    <IncludeMonoRuntime>false</IncludeMonoRuntime>
+    <LinkMode>Full</LinkMode>
+    <ConsolePause>false</ConsolePause>
+    <EnableCodeSigning>false</EnableCodeSigning>
+    <CreatePackage>false</CreatePackage>
+    <CodeSigningKey>Mac Developer</CodeSigningKey>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'AppStore|AnyCPU' ">
+    <DebugType>none</DebugType>
+    <Optimize>true</Optimize>
+    <OutputPath>bin\AppStore</OutputPath>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+    <PackageSigningKey>3rd Party Mac Developer Installer</PackageSigningKey>
+    <IncludeMonoRuntime>true</IncludeMonoRuntime>
+    <LinkMode>Full</LinkMode>
+    <EnablePackageSigning>true</EnablePackageSigning>
+    <ConsolePause>false</ConsolePause>
+    <EnableCodeSigning>true</EnableCodeSigning>
+    <CreatePackage>true</CreatePackage>
+    <CodeSigningKey>3rd Party Mac Developer Application</CodeSigningKey>
+  </PropertyGroup>
+  <ItemGroup>
+    <Reference Include="System" />
+    <Reference Include="System.Xml" />
+    <Reference Include="System.Core" />
+    <Reference Include="System.Xml.Linq" />
+    <Reference Include="System.Drawing" />
+    <Reference Include="MonoMac" />
+  </ItemGroup>
+  <ItemGroup>
+    <ProjectReference Include="..\..\MonoGame.Framework\MonoGame.Framework.MacOS.csproj">
+      <Project>{36C538E6-C32A-4A8D-A39C-566173D7118E}</Project>
+      <Name>MonoGame.Framework.MacOS</Name>
+    </ProjectReference>
+    <ProjectReference Include="..\..\ThirdParty\Lidgren.Network\Lidgren.Network.MacOS.csproj">
+      <Project>{AE483C29-042E-4226-BA52-D247CE7676DA}</Project>
+      <Name>Lidgren.Network.MacOS</Name>
+    </ProjectReference>
+  </ItemGroup>
+  <ItemGroup>
+    <Compile Include="Main.cs" />
+    <Compile Include="Properties\AssemblyInfo.cs" />
+    <Compile Include="AudioManager.cs" />
+    <Compile Include="Cat.cs" />
+    <Compile Include="Dog.cs" />
+    <Compile Include="Game.cs" />
+    <Compile Include="IAudioEmitter.cs" />
+    <Compile Include="QuadDrawer.cs" />
+    <Compile Include="SpriteEntity.cs" />
+  </ItemGroup>
+  <ItemGroup>
+    <None Include="Info.plist" />
+  </ItemGroup>
+  <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" />
+  <Import Project="$(MSBuildExtensionsPath)\Mono\MonoMac\v0.0\Mono.MonoMac.targets" />
+  <ItemGroup>
+    <Folder Include="Content\" />
+  </ItemGroup>
+  <ItemGroup>
+    <Content Include="Content\CatSound0.xnb" />
+    <Content Include="Content\CatSound1.xnb" />
+    <Content Include="Content\CatSound2.xnb" />
+    <Content Include="Content\CatTexture.xnb" />
+    <Content Include="Content\DogSound.xnb" />
+    <Content Include="Content\DogTexture.xnb" />
+    <Content Include="Content\checker.xnb" />
+  </ItemGroup>
+</Project>

Audio3D/AudioManager.cs

@@ -0,0 +1,195 @@
+#region File Description
+//-----------------------------------------------------------------------------
+// AudioManager.cs
+//
+// Microsoft XNA Community Game Platform
+// Copyright (C) Microsoft Corporation. All rights reserved.
+//-----------------------------------------------------------------------------
+#endregion
+
+#region Using Statements
+using System.Collections.Generic;
+using Microsoft.Xna.Framework;
+using Microsoft.Xna.Framework.Audio;
+#endregion
+
+namespace Audio3D
+{
+    /// <summary>
+    /// Audio manager keeps track of what 3D sounds are playing, updating
+    /// their settings as the camera and entities move around the world, and
+    /// automatically disposing sound effect instances after they finish playing.
+    /// </summary>
+    public class AudioManager : Microsoft.Xna.Framework.GameComponent
+    {
+        #region Fields
+
+
+        // List of all the sound effects that will be loaded into this manager.
+        static string[] soundNames =
+        {
+            "CatSound0",
+            "CatSound1",
+            "CatSound2",
+            "DogSound",
+        };
+
+
+        // The listener describes the ear which is hearing 3D sounds.
+        // This is usually set to match the camera.
+        public AudioListener Listener
+        {
+            get { return listener; }
+        }
+
+        AudioListener listener = new AudioListener();
+
+
+        // The emitter describes an entity which is making a 3D sound.
+        AudioEmitter emitter = new AudioEmitter();
+
+
+        // Store all the sound effects that are available to be played.
+        Dictionary<string, SoundEffect> soundEffects = new Dictionary<string, SoundEffect>();
+
+        
+        // Keep track of all the 3D sounds that are currently playing.
+        List<ActiveSound> activeSounds = new List<ActiveSound>();
+
+
+        #endregion
+
+
+        public AudioManager(Game game)
+            : base(game)
+        { }
+
+
+        /// <summary>
+        /// Initializes the audio manager.
+        /// </summary>
+        public override void Initialize()
+        {
+            // Set the scale for 3D audio so it matches the scale of our game world.
+            // DistanceScale controls how much sounds change volume as you move further away.
+            // DopplerScale controls how much sounds change pitch as you move past them.
+            SoundEffect.DistanceScale = 2000;
+            SoundEffect.DopplerScale = 0.1f;
+
+            // Load all the sound effects.
+            foreach (string soundName in soundNames)
+            {
+                soundEffects.Add(soundName, Game.Content.Load<SoundEffect>(soundName));
+            }
+
+            base.Initialize();
+        }
+
+
+        /// <summary>
+        /// Unloads the sound effect data.
+        /// </summary>
+        protected override void Dispose(bool disposing)
+        {
+            try
+            {
+                if (disposing)
+                {
+                    foreach (SoundEffect soundEffect in soundEffects.Values)
+                    {
+                        soundEffect.Dispose();
+                    }
+
+                    soundEffects.Clear();
+                }
+            }
+            finally
+            {
+                base.Dispose(disposing);
+            }
+        }
+
+        
+        /// <summary>
+        /// Updates the state of the 3D audio system.
+        /// </summary>
+        public override void Update(GameTime gameTime)
+        {
+            // Loop over all the currently playing 3D sounds.
+            int index = 0;
+
+            while (index < activeSounds.Count)
+            {
+                ActiveSound activeSound = activeSounds[index];
+
+                if (activeSound.Instance.State == SoundState.Stopped)
+                {
+                    // If the sound has stopped playing, dispose it.
+                    activeSound.Instance.Dispose();
+
+                    // Remove it from the active list.
+                    activeSounds.RemoveAt(index);
+                }
+                else
+                {
+                    // If the sound is still playing, update its 3D settings.
+                    Apply3D(activeSound);
+
+                    index++;
+                }
+            }
+
+            base.Update(gameTime);
+        }
+
+
+        /// <summary>
+        /// Triggers a new 3D sound.
+        /// </summary>
+        public SoundEffectInstance Play3DSound(string soundName, bool isLooped, IAudioEmitter emitter)
+        {
+            ActiveSound activeSound = new ActiveSound();
+
+            // Fill in the instance and emitter fields.
+            activeSound.Instance = soundEffects[soundName].CreateInstance();
+            activeSound.Instance.IsLooped = isLooped;
+
+            activeSound.Emitter = emitter;
+
+            // Set the 3D position of this sound, and then play it.
+            Apply3D(activeSound);
+
+            activeSound.Instance.Play();
+
+            // Remember that this sound is now active.
+            activeSounds.Add(activeSound);
+
+            return activeSound.Instance;
+        }
+
+
+        /// <summary>
+        /// Updates the position and velocity settings of a 3D sound.
+        /// </summary>
+        private void Apply3D(ActiveSound activeSound)
+        {
+            emitter.Position = activeSound.Emitter.Position;
+            emitter.Forward = activeSound.Emitter.Forward;
+            emitter.Up = activeSound.Emitter.Up;
+            emitter.Velocity = activeSound.Emitter.Velocity;
+
+            activeSound.Instance.Apply3D(listener, emitter);
+        }
+
+
+        /// <summary>
+        /// Internal helper class for keeping track of an active 3D sound,
+        /// and remembering which emitter object it is attached to.
+        /// </summary>
+        private class ActiveSound
+        {
+            public SoundEffectInstance Instance;
+            public IAudioEmitter Emitter;
+        }
+    }
+}

Audio3D/Cat.cs

@@ -0,0 +1,74 @@
+#region File Description
+//-----------------------------------------------------------------------------
+// Cat.cs
+//
+// Microsoft XNA Community Game Platform
+// Copyright (C) Microsoft Corporation. All rights reserved.
+//-----------------------------------------------------------------------------
+#endregion
+
+#region Using Statements
+using System;
+using Microsoft.Xna.Framework;
+#endregion
+
+namespace Audio3D
+{
+    /// <summary>
+    /// Entity class which moves in a circle and plays cat sounds.
+    /// This uses a single-shot sound, which will stop automatically
+    /// when it finishes playing. See the Dog class for an example of
+    /// using a looping sound.
+    /// </summary>
+    class Cat : SpriteEntity
+    {
+        #region Fields
+
+        // How long until we should play the next sound.
+        TimeSpan timeDelay = TimeSpan.Zero;
+
+        // Random number generator for choosing between sound variations.
+        static Random random = new Random();
+
+        #endregion
+
+
+        /// <summary>
+        /// Updates the position of the cat, and plays sounds.
+        /// </summary>
+        public override void Update(GameTime gameTime, AudioManager audioManager)
+        {
+            // Move the cat in a big circle.
+            double time = gameTime.TotalGameTime.TotalSeconds;
+
+            float dx = (float)-Math.Cos(time);
+            float dz = (float)-Math.Sin(time);
+
+            Vector3 newPosition = new Vector3(dx, 0, dz) * 6000;
+
+            // Update entity position and velocity.
+            Velocity = newPosition - Position;
+            Position = newPosition;
+            if (Velocity == Vector3.Zero)
+                Forward = Vector3.Forward;
+            else
+                Forward = Vector3.Normalize(Velocity);
+
+            Up = Vector3.Up;
+
+            // If the time delay has run out, trigger another single-shot sound.
+            timeDelay -= gameTime.ElapsedGameTime;
+
+            if (timeDelay < TimeSpan.Zero)
+            {
+                // For variety, randomly choose between three slightly different
+                // variants of the sound (CatSound0, CatSound1, and CatSound2).
+                string soundName = "CatSound" + random.Next(3);
+
+                audioManager.Play3DSound(soundName, false, this);
+
+                timeDelay += TimeSpan.FromSeconds(1.25f);
+            }
+        }
+    }
+}

Audio3D/Dog.cs

@@ -0,0 +1,71 @@
+#region File Description
+//-----------------------------------------------------------------------------
+// Dog.cs
+//
+// Microsoft XNA Community Game Platform
+// Copyright (C) Microsoft Corporation. All rights reserved.
+//-----------------------------------------------------------------------------
+#endregion
+
+#region Using Statements
+using System;
+using Microsoft.Xna.Framework;
+using Microsoft.Xna.Framework.Audio;
+#endregion
+
+namespace Audio3D
+{
+    /// Entity class which sits in one place and plays dog sounds.
+    /// This uses a looping sound, which must be explicitly stopped
+    /// to prevent it going on forever. See the Cat class for an
+    /// example of using a single-shot sound.
+    class Dog : SpriteEntity
+    {
+        #region Fields
+
+        // How long until we should start or stop the sound.
+        TimeSpan timeDelay = TimeSpan.Zero;
+
+        // The sound which is currently playing, if any.
+        SoundEffectInstance activeSound = null;
+
+        #endregion
+
+
+        /// <summary>
+        /// Updates the position of the dog, and plays sounds.
+        /// </summary>
+        public override void Update(GameTime gameTime, AudioManager audioManager)
+        {
+            // Set the entity to a fixed position.
+            Position = new Vector3(0, 0, -4000);
+            Forward = Vector3.Forward;
+            Up = Vector3.Up;
+            Velocity = Vector3.Zero;
+
+            // If the time delay has run out, start or stop the looping sound.
+            // This would normally go on forever, but we stop it after a six
+            // second delay, then start it up again after four more seconds.
+            timeDelay -= gameTime.ElapsedGameTime;
+
+            if (timeDelay < TimeSpan.Zero)
+            {
+                if (activeSound == null)
+                {
+                    // If no sound is currently playing, trigger one.
+                    activeSound = audioManager.Play3DSound("DogSound", true, this);
+
+                    timeDelay += TimeSpan.FromSeconds(6);
+                }
+                else
+                {
+                    // Otherwise stop the current sound.
+                    activeSound.Stop(false);
+                    activeSound = null;
+
+                    timeDelay += TimeSpan.FromSeconds(4);
+                }
+            }
+        }
+    }
+}

Audio3D/Game.cs

@@ -0,0 +1,204 @@
+#region File Description
+//-----------------------------------------------------------------------------
+// Game.cs
+//
+// Microsoft XNA Community Game Platform
+// Copyright (C) Microsoft Corporation. All rights reserved.
+//-----------------------------------------------------------------------------
+#endregion
+
+#region Using Statements
+using Microsoft.Xna.Framework;
+using Microsoft.Xna.Framework.Content;
+using Microsoft.Xna.Framework.Graphics;
+using Microsoft.Xna.Framework.Input;
+#endregion
+
+namespace Audio3D
+{
+    /// <summary>
+    /// Sample showing how to implement 3D audio.
+    /// </summary>
+    public class Audio3DGame : Microsoft.Xna.Framework.Game
+    {
+        #region Fields
+
+        GraphicsDeviceManager graphics;
+
+        AudioManager audioManager;
+
+        SpriteEntity cat;
+        SpriteEntity dog;
+
+        Texture2D checkerTexture;
+
+        QuadDrawer quadDrawer;
+
+        Vector3 cameraPosition = new Vector3(0, 512, 0);
+        Vector3 cameraForward = Vector3.Forward;
+        Vector3 cameraUp = Vector3.Up;
+        Vector3 cameraVelocity = Vector3.Zero;
+
+        KeyboardState currentKeyboardState;
+        GamePadState currentGamePadState;
+
+        #endregion
+
+        #region Initialization
+
+
+        public Audio3DGame()
+        {
+            Content.RootDirectory = "Content";
+
+            graphics = new GraphicsDeviceManager(this);
+
+            audioManager = new AudioManager(this);
+
+            Components.Add(audioManager);
+
+            cat = new Cat();
+            dog = new Dog();
+        }
+
+
+        /// <summary>
+        /// Load your graphics content.
+        /// </summary>
+        protected override void LoadContent()
+        {
+            cat.Texture = Content.Load<Texture2D>("CatTexture");
+            dog.Texture = Content.Load<Texture2D>("DogTexture");
+
+            checkerTexture = Content.Load<Texture2D>("checker");
+
+            quadDrawer = new QuadDrawer(graphics.GraphicsDevice);
+        }
+
+
+        #endregion
+
+        #region Update and Draw
+
+
+        /// <summary>
+        /// Allows the game to run logic.
+        /// </summary>
+        protected override void Update(GameTime gameTime)
+        {
+            HandleInput();
+
+            UpdateCamera();
+
+            // Tell the AudioManager about the new camera position.
+            audioManager.Listener.Position = cameraPosition;
+            audioManager.Listener.Forward = cameraForward;
+            audioManager.Listener.Up = cameraUp;
+            audioManager.Listener.Velocity = cameraVelocity;
+
+            // Tell our game entities to move around and play sounds.
+            cat.Update(gameTime, audioManager);
+            dog.Update(gameTime, audioManager);
+
+            base.Update(gameTime);
+        }
+
+
+        /// <summary>
+        /// This is called when the game should draw itself.
+        /// </summary>
+        protected override void Draw(GameTime gameTime)
+        {
+            GraphicsDevice device = graphics.GraphicsDevice;
+
+            device.Clear(Color.CornflowerBlue);
+
+            device.BlendState = BlendState.AlphaBlend;
+
+            // Compute camera matrices.
+            Matrix view = Matrix.CreateLookAt(cameraPosition,
+                                              cameraPosition + cameraForward,
+                                              cameraUp);
+
+            Matrix projection = Matrix.CreatePerspectiveFieldOfView(1, device.Viewport.AspectRatio,
+                                                                    1, 100000);
+
+            // Draw the checkered ground polygon.
+            Matrix groundTransform = Matrix.CreateScale(20000) *
+                                     Matrix.CreateRotationX(MathHelper.PiOver2);
+
+            quadDrawer.DrawQuad(checkerTexture, 32, groundTransform, view, projection);
+
+            // Draw the game entities.
+            cat.Draw(quadDrawer, cameraPosition, view, projection);
+            dog.Draw(quadDrawer, cameraPosition, view, projection);
+
+            base.Draw(gameTime);
+        }
+
+
+        #endregion
+
+        #region Handle Input
+
+
+        /// <summary>
+        /// Handles input for quitting the game.
+        /// </summary>
+        void HandleInput()
+        {
+            currentKeyboardState = Keyboard.GetState();
+            currentGamePadState = GamePad.GetState(PlayerIndex.One);
+
+            // Check for exit.
+            if (currentKeyboardState.IsKeyDown(Keys.Escape) ||
+                currentGamePadState.Buttons.Back == ButtonState.Pressed)
+            {
+                Exit();
+            }
+        }
+
+
+        /// <summary>
+        /// Handles input for moving the camera.
+        /// </summary>
+        void UpdateCamera()
+        {
+            const float turnSpeed = 0.05f;
+            const float accelerationSpeed = 4;
+            const float frictionAmount = 0.98f;
+
+            // Turn left or right.
+            float turn = -currentGamePadState.ThumbSticks.Left.X * turnSpeed;
+
+            if (currentKeyboardState.IsKeyDown(Keys.Left))
+                turn += turnSpeed;
+
+            if (currentKeyboardState.IsKeyDown(Keys.Right))
+                turn -= turnSpeed;
+
+            cameraForward = Vector3.TransformNormal(cameraForward,
+                                                    Matrix.CreateRotationY(turn));
+
+            // Accelerate forward or backward.
+            float accel = currentGamePadState.ThumbSticks.Left.Y * accelerationSpeed;
+
+            if (currentKeyboardState.IsKeyDown(Keys.Up))
+                accel += accelerationSpeed;
+
+            if (currentKeyboardState.IsKeyDown(Keys.Down))
+                accel -= accelerationSpeed;
+
+            cameraVelocity += cameraForward * accel;
+
+            // Add velocity to the current position.
+            cameraPosition += cameraVelocity;
+
+            // Apply the friction force.
+            cameraVelocity *= frictionAmount;
+        }
+
+
+        #endregion
+    }
+}

Audio3D/IAudioEmitter.cs

@@ -0,0 +1,27 @@
+#region File Description
+//-----------------------------------------------------------------------------
+// IAudioEmitter.cs
+//
+// Microsoft XNA Community Game Platform
+// Copyright (C) Microsoft Corporation. All rights reserved.
+//-----------------------------------------------------------------------------
+#endregion
+
+#region Using Statements
+using Microsoft.Xna.Framework;
+#endregion 
+
+namespace Audio3D
+{
+    /// <summary>
+    /// Interface used by the AudioManager to look up the position
+    /// and velocity of entities that can emit 3D sounds.
+    /// </summary>
+    public interface IAudioEmitter
+    {
+        Vector3 Position { get; }
+        Vector3 Forward { get; }
+        Vector3 Up { get; }
+        Vector3 Velocity { get; }
+    }
+}

Audio3D/Info.plist

@@ -0,0 +1,16 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+<plist version="1.0">
+<dict>
+	<key>CFBundleIdentifier</key>
+	<string>com.yourcompany.Audio3D</string>
+	<key>CFBundleName</key>
+	<string>Audio3D</string>
+	<key>CFBundleVersion</key>
+	<string>1</string>
+	<key>LSMinimumSystemVersion</key>
+	<string>10.6</string>
+	<key>NSPrincipalClass</key>
+	<string>NSApplication</string>
+</dict>
+</plist>

Audio3D/Main.cs

@@ -0,0 +1,45 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+
+using MonoMac.AppKit;
+using MonoMac.Foundation;
+
+namespace Audio3D
+{
+	static class Program
+	{
+		/// <summary>
+		/// The main entry point for the application.
+		/// </summary>
+		static void Main (string[] args)
+		{
+			NSApplication.Init ();
+			
+			using (var p = new NSAutoreleasePool ()) {
+				NSApplication.SharedApplication.Delegate = new AppDelegate ();
+				NSApplication.Main (args);
+			}
+
+
+		}
+	}
+
+	class AppDelegate : NSApplicationDelegate
+	{
+		Audio3DGame game;
+
+		public override void FinishedLaunching (MonoMac.Foundation.NSObject notification)
+		{
+			game = new Audio3DGame ();
+			game.Run ();
+		}
+		
+		public override bool ApplicationShouldTerminateAfterLastWindowClosed (NSApplication sender)
+		{
+			return true;
+		}
+	}  
+}
+
+

Audio3D/Properties/AssemblyInfo.cs

@@ -0,0 +1,36 @@
+#region File Description
+//-----------------------------------------------------------------------------
+// AssemblyInfo.cs
+//
+// Microsoft XNA Community Game Platform
+// Copyright (C) Microsoft Corporation. All rights reserved.
+//-----------------------------------------------------------------------------
+#endregion
+
+#region Using Statements
+using System.Reflection;
+using System.Runtime.CompilerServices;
+using System.Runtime.InteropServices;
+#endregion
+
+// General Information about an assembly is controlled through the following 
+// set of attributes. Change these attribute values to modify the information
+// associated with an assembly.
+[assembly: AssemblyTitle("Audio 3D")]
+[assembly: AssemblyProduct("Audio 3D")]
+[assembly: AssemblyDescription("This sample shows how to position sounds in 3D space, implementing panning, Doppler, and distance attenuation effects.")]
+[assembly: AssemblyCompany("Microsoft")]
+
+[assembly: AssemblyCopyright("Copyright © Microsoft 2007")]
+[assembly: AssemblyTrademark("")]
+[assembly: AssemblyCulture("")]
+
+
+// Version information for an assembly consists of the following four values:
+//
+//      Major Version
+//      Minor Version 
+//      Build Number
+//      Revision
+//
+[assembly: AssemblyVersion("1.0.0.0")]

Audio3D/QuadDrawer.cs

@@ -0,0 +1,79 @@
+#region File Description
+//-----------------------------------------------------------------------------
+// QuadDrawer.cs
+//
+// Microsoft XNA Community Game Platform
+// Copyright (C) Microsoft Corporation. All rights reserved.
+//-----------------------------------------------------------------------------
+#endregion
+
+#region Using Statements
+using Microsoft.Xna.Framework;
+using Microsoft.Xna.Framework.Graphics;
+#endregion
+
+namespace Audio3D
+{
+    /// <summary>
+    /// Helper for drawing 3D quadrilaterals. This is used to draw the cat
+    /// and dog billboard sprites, and also the checkered ground polygon.
+    /// </summary>
+    class QuadDrawer
+    {
+        #region Fields
+
+        GraphicsDevice graphicsDevice;
+        AlphaTestEffect effect;
+        VertexPositionTexture[] vertices;
+
+        #endregion
+
+
+        /// <summary>
+        /// Constructs a new quadrilateral drawing worker.
+        /// </summary>
+        public QuadDrawer(GraphicsDevice device)
+        {
+            graphicsDevice = device;
+
+            effect = new AlphaTestEffect(device);
+
+            effect.AlphaFunction = CompareFunction.Greater;
+            effect.ReferenceAlpha = 128;
+
+            // Preallocate an array of four vertices.
+            vertices = new VertexPositionTexture[4];
+
+            vertices[0].Position = new Vector3(1, 1, 0);
+            vertices[1].Position = new Vector3(-1, 1, 0);
+            vertices[2].Position = new Vector3(1, -1, 0);
+            vertices[3].Position = new Vector3(-1, -1, 0);
+        }
+
+
+        /// <summary>
+        /// Draws a quadrilateral as part of the 3D world.
+        /// </summary>
+        public void DrawQuad(Texture2D texture, float textureRepeats,
+                             Matrix world, Matrix view, Matrix projection)
+        {
+            // Set our effect to use the specified texture and camera matrices.
+            effect.Texture = texture;
+
+            effect.World = world;
+            effect.View = view;
+            effect.Projection = projection;
+
+            // Update our vertex array to use the specified number of texture repeats.
+            vertices[0].TextureCoordinate = new Vector2(0, 0);
+            vertices[1].TextureCoordinate = new Vector2(textureRepeats, 0);
+            vertices[2].TextureCoordinate = new Vector2(0, textureRepeats);
+            vertices[3].TextureCoordinate = new Vector2(textureRepeats, textureRepeats);
+
+            // Draw the quad.
+            effect.CurrentTechnique.Passes[0].Apply();
+
+            graphicsDevice.DrawUserPrimitives(PrimitiveType.TriangleStrip, vertices, 0, 2);
+        }
+    }
+}

Audio3D/SpriteEntity.cs

@@ -0,0 +1,109 @@
+#region File Description
+//-----------------------------------------------------------------------------
+// SpriteEntity.cs
+//
+// Microsoft XNA Community Game Platform
+// Copyright (C) Microsoft Corporation. All rights reserved.
+//-----------------------------------------------------------------------------
+#endregion
+
+#region Using Statements
+using Microsoft.Xna.Framework;
+using Microsoft.Xna.Framework.Graphics;
+#endregion
+
+namespace Audio3D
+{
+    /// <summary>
+    /// Base class for game entities that are displayed as billboard sprites,
+    /// and which can emit 3D sounds. The Cat and Dog classes both derive from this.
+    /// </summary>
+    abstract class SpriteEntity : IAudioEmitter
+    {
+        #region Properties
+
+
+        /// <summary>
+        /// Gets or sets the 3D position of the entity.
+        /// </summary>
+        public Vector3 Position
+        {
+            get { return position; }
+            set { position = value; }
+        }
+
+        Vector3 position;
+
+
+        /// <summary>
+        /// Gets or sets which way the entity is facing.
+        /// </summary>
+        public Vector3 Forward
+        {
+            get { return forward; }
+            set { forward = value; }
+        }
+
+        Vector3 forward;
+
+
+        /// <summary>
+        /// Gets or sets the orientation of this entity.
+        /// </summary>
+        public Vector3 Up
+        {
+            get { return up; }
+            set { up = value; }
+        }
+
+        Vector3 up;
+
+        
+        /// <summary>
+        /// Gets or sets how fast this entity is moving.
+        /// </summary>
+        public Vector3 Velocity
+        {
+            get { return velocity; }
+            protected set { velocity = value; }
+        }
+
+        Vector3 velocity;
+
+
+        /// <summary>
+        /// Gets or sets the texture used to display this entity.
+        /// </summary>
+        public Texture2D Texture
+        {
+            get { return texture; }
+            set { texture = value; }
+        }
+
+        Texture2D texture;
+
+
+        #endregion
+
+
+        /// <summary>
+        /// Updates the position of the entity, and allows it to play sounds.
+        /// </summary>
+        public abstract void Update(GameTime gameTime, AudioManager audioManager);
+
+
+        /// <summary>
+        /// Draws the entity as a billboard sprite.
+        /// </summary>
+        public void Draw(QuadDrawer quadDrawer, Vector3 cameraPosition,
+                         Matrix view, Matrix projection)
+        {
+            Matrix world = Matrix.CreateTranslation(0, 1, 0) *
+                           Matrix.CreateScale(800) *
+                           Matrix.CreateConstrainedBillboard(Position, cameraPosition,
+                                                             Up, null, null);
+
+            quadDrawer.DrawQuad(Texture, 1, world, view, projection);
+        }
+    }
+}

BloomSample/BloomComponent.cs

@@ -0,0 +1,317 @@
+#region File Description
+//-----------------------------------------------------------------------------
+// BloomComponent.cs
+//
+// Microsoft XNA Community Game Platform
+// Copyright (C) Microsoft Corporation. All rights reserved.
+//-----------------------------------------------------------------------------
+#endregion
+
+#region Using Statements
+using System;
+using Microsoft.Xna.Framework;
+using Microsoft.Xna.Framework.Content;
+using Microsoft.Xna.Framework.Graphics;
+#endregion
+
+namespace BloomPostprocess
+{
+    public class BloomComponent : DrawableGameComponent
+    {
+        #region Fields
+
+        SpriteBatch spriteBatch;
+
+        Effect bloomExtractEffect;
+        Effect bloomCombineEffect;
+        Effect gaussianBlurEffect;
+
+        RenderTarget2D sceneRenderTarget;
+        RenderTarget2D renderTarget1;
+        RenderTarget2D renderTarget2;
+
+
+        // Choose what display settings the bloom should use.
+        public BloomSettings Settings
+        {
+            get { return settings; }
+            set { settings = value; }
+        }
+
+        BloomSettings settings = BloomSettings.PresetSettings[0];
+
+
+        // Optionally displays one of the intermediate buffers used
+        // by the bloom postprocess, so you can see exactly what is
+        // being drawn into each rendertarget.
+        public enum IntermediateBuffer
+        {
+            PreBloom,
+            BlurredHorizontally,
+            BlurredBothWays,
+            FinalResult,
+        }
+
+        public IntermediateBuffer ShowBuffer
+        {
+            get { return showBuffer; }
+            set { showBuffer = value; }
+        }
+
+        IntermediateBuffer showBuffer = IntermediateBuffer.FinalResult;
+
+
+        #endregion
+
+        #region Initialization
+
+
+        public BloomComponent(Game game)
+            : base(game)
+        {
+            if (game == null)
+                throw new ArgumentNullException("game");
+        }
+
+
+        /// <summary>
+        /// Load your graphics content.
+        /// </summary>
+        protected override void LoadContent()
+        {
+            spriteBatch = new SpriteBatch(GraphicsDevice);
+
+            bloomExtractEffect = Game.Content.Load<Effect>("BloomExtract");
+            bloomCombineEffect = Game.Content.Load<Effect>("BloomCombine");
+            gaussianBlurEffect = Game.Content.Load<Effect>("GaussianBlur");
+
+            // Look up the resolution and format of our main backbuffer.
+            PresentationParameters pp = GraphicsDevice.PresentationParameters;
+
+            int width = pp.BackBufferWidth;
+            int height = pp.BackBufferHeight;
+
+            SurfaceFormat format = pp.BackBufferFormat;
+
+            // Create a texture for rendering the main scene, prior to applying bloom.
+            sceneRenderTarget = new RenderTarget2D(GraphicsDevice, width, height, false,
+                                                   format, pp.DepthStencilFormat, pp.MultiSampleCount,
+                                                   RenderTargetUsage.DiscardContents);
+
+            // Create two rendertargets for the bloom processing. These are half the
+            // size of the backbuffer, in order to minimize fillrate costs. Reducing
+            // the resolution in this way doesn't hurt quality, because we are going
+            // to be blurring the bloom images in any case.
+            width /= 2;
+            height /= 2;
+
+            renderTarget1 = new RenderTarget2D(GraphicsDevice, width, height, false, format, DepthFormat.None);
+            renderTarget2 = new RenderTarget2D(GraphicsDevice, width, height, false, format, DepthFormat.None);
+        }
+
+
+        /// <summary>
+        /// Unload your graphics content.
+        /// </summary>
+        protected override void UnloadContent()
+        {
+            sceneRenderTarget.Dispose();
+            renderTarget1.Dispose();
+            renderTarget2.Dispose();
+        }
+
+
+        #endregion
+
+        #region Draw
+
+
+        /// <summary>
+        /// This should be called at the very start of the scene rendering. The bloom
+        /// component uses it to redirect drawing into its custom rendertarget, so it
+        /// can capture the scene image in preparation for applying the bloom filter.
+        /// </summary>
+        public void BeginDraw()
+        {
+            if (Visible)
+            {
+                GraphicsDevice.SetRenderTarget(sceneRenderTarget);
+            }
+        }
+
+
+        /// <summary>
+        /// This is where it all happens. Grabs a scene that has already been rendered,
+        /// and uses postprocess magic to add a glowing bloom effect over the top of it.
+        /// </summary>
+        public override void Draw(GameTime gameTime)
+        {
+            GraphicsDevice.SamplerStates[1] = SamplerState.LinearClamp;
+
+            // Pass 1: draw the scene into rendertarget 1, using a
+            // shader that extracts only the brightest parts of the image.
+            bloomExtractEffect.Parameters["BloomThreshold"].SetValue(
+                Settings.BloomThreshold);
+
+            DrawFullscreenQuad(sceneRenderTarget, renderTarget1,
+                               bloomExtractEffect,
+                               IntermediateBuffer.PreBloom);
+
+            // Pass 2: draw from rendertarget 1 into rendertarget 2,
+            // using a shader to apply a horizontal gaussian blur filter.
+            SetBlurEffectParameters(1.0f / (float)renderTarget1.Width, 0);
+
+            DrawFullscreenQuad(renderTarget1, renderTarget2,
+                               gaussianBlurEffect,
+                               IntermediateBuffer.BlurredHorizontally);
+
+            // Pass 3: draw from rendertarget 2 back into rendertarget 1,
+            // using a shader to apply a vertical gaussian blur filter.
+            SetBlurEffectParameters(0, 1.0f / (float)renderTarget1.Height);
+
+            DrawFullscreenQuad(renderTarget2, renderTarget1,
+                               gaussianBlurEffect,
+                               IntermediateBuffer.BlurredBothWays);
+
+            // Pass 4: draw both rendertarget 1 and the original scene
+            // image back into the main backbuffer, using a shader that
+            // combines them to produce the final bloomed result.
+            GraphicsDevice.SetRenderTarget(null);
+
+            EffectParameterCollection parameters = bloomCombineEffect.Parameters;
+
+            parameters["BloomIntensity"].SetValue(Settings.BloomIntensity);
+            parameters["BaseIntensity"].SetValue(Settings.BaseIntensity);
+            parameters["BloomSaturation"].SetValue(Settings.BloomSaturation);
+            parameters["BaseSaturation"].SetValue(Settings.BaseSaturation);
+
+            GraphicsDevice.Textures[1] = sceneRenderTarget;
+
+            Viewport viewport = GraphicsDevice.Viewport;
+
+            DrawFullscreenQuad(renderTarget1,
+                               viewport.Width, viewport.Height,
+                               bloomCombineEffect,
+                               IntermediateBuffer.FinalResult);
+        }
+
+
+        /// <summary>
+        /// Helper for drawing a texture into a rendertarget, using
+        /// a custom shader to apply postprocessing effects.
+        /// </summary>
+        void DrawFullscreenQuad(Texture2D texture, RenderTarget2D renderTarget,
+                                Effect effect, IntermediateBuffer currentBuffer)
+        {
+            GraphicsDevice.SetRenderTarget(renderTarget);
+
+            DrawFullscreenQuad(texture,
+                               renderTarget.Width, renderTarget.Height,
+                               effect, currentBuffer);
+        }
+
+
+        /// <summary>
+        /// Helper for drawing a texture into the current rendertarget,
+        /// using a custom shader to apply postprocessing effects.
+        /// </summary>
+        void DrawFullscreenQuad(Texture2D texture, int width, int height,
+                                Effect effect, IntermediateBuffer currentBuffer)
+        {
+            // If the user has selected one of the show intermediate buffer options,
+            // we still draw the quad to make sure the image will end up on the screen,
+            // but might need to skip applying the custom pixel shader.
+            if (showBuffer < currentBuffer)
+            {
+                effect = null;
+            }
+
+            spriteBatch.Begin(0, BlendState.Opaque, null, null, null, effect);
+            spriteBatch.Draw(texture, new Rectangle(0, 0, width, height), Color.White);
+            spriteBatch.End();
+        }
+
+
+        /// <summary>
+        /// Computes sample weightings and texture coordinate offsets
+        /// for one pass of a separable gaussian blur filter.
+        /// </summary>
+        void SetBlurEffectParameters(float dx, float dy)
+        {
+            // Look up the sample weight and offset effect parameters.
+            EffectParameter weightsParameter, offsetsParameter;
+
+            weightsParameter = gaussianBlurEffect.Parameters["SampleWeights"];
+            offsetsParameter = gaussianBlurEffect.Parameters["SampleOffsets"];
+
+            // Look up how many samples our gaussian blur effect supports.
+            int sampleCount = weightsParameter.Elements.Count;
+
+            // Create temporary arrays for computing our filter settings.
+            float[] sampleWeights = new float[sampleCount];
+            Vector2[] sampleOffsets = new Vector2[sampleCount];
+
+            // The first sample always has a zero offset.
+            sampleWeights[0] = ComputeGaussian(0);
+            sampleOffsets[0] = new Vector2(0);
+
+            // Maintain a sum of all the weighting values.
+            float totalWeights = sampleWeights[0];
+
+            // Add pairs of additional sample taps, positioned
+            // along a line in both directions from the center.
+            for (int i = 0; i < sampleCount / 2; i++)
+            {
+                // Store weights for the positive and negative taps.
+                float weight = ComputeGaussian(i + 1);
+
+                sampleWeights[i * 2 + 1] = weight;
+                sampleWeights[i * 2 + 2] = weight;
+
+                totalWeights += weight * 2;
+
+                // To get the maximum amount of blurring from a limited number of
+                // pixel shader samples, we take advantage of the bilinear filtering
+                // hardware inside the texture fetch unit. If we position our texture
+                // coordinates exactly halfway between two texels, the filtering unit
+                // will average them for us, giving two samples for the price of one.
+                // This allows us to step in units of two texels per sample, rather
+                // than just one at a time. The 1.5 offset kicks things off by
+                // positioning us nicely in between two texels.
+                float sampleOffset = i * 2 + 1.5f;
+
+                Vector2 delta = new Vector2(dx, dy) * sampleOffset;
+
+                // Store texture coordinate offsets for the positive and negative taps.
+                sampleOffsets[i * 2 + 1] = delta;
+                sampleOffsets[i * 2 + 2] = -delta;
+            }
+
+            // Normalize the list of sample weightings, so they will always sum to one.
+            for (int i = 0; i < sampleWeights.Length; i++)
+            {
+                sampleWeights[i] /= totalWeights;
+            }
+
+            // Tell the effect about our new filter settings.
+            weightsParameter.SetValue(sampleWeights);
+            offsetsParameter.SetValue(sampleOffsets);
+        }
+
+
+        /// <summary>
+        /// Evaluates a single point on the gaussian falloff curve.
+        /// Used for setting up the blur filter weightings.
+        /// </summary>
+        float ComputeGaussian(float n)
+        {
+            float theta = Settings.BlurAmount;
+
+            return (float)((1.0 / Math.Sqrt(2 * Math.PI * theta)) *
+                           Math.Exp(-(n * n) / (2 * theta * theta)));
+        }
+
+
+        #endregion
+    }
+}

BloomSample/BloomSample.MacOS.csproj

@@ -0,0 +1,101 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project DefaultTargets="Build" ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <PropertyGroup>
+    <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
+    <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
+    <ProductVersion>10.0.0</ProductVersion>
+    <SchemaVersion>2.0</SchemaVersion>
+    <ProjectGuid>{A93553D9-11E5-4B0B-9809-4E4765D5FD8F}</ProjectGuid>
+    <ProjectTypeGuids>{948B3504-5B70-4649-8FE4-BDE1FB46EC69};{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}</ProjectTypeGuids>
+    <OutputType>Exe</OutputType>
+    <RootNamespace>BloomSample</RootNamespace>
+    <AssemblyName>BloomSample</AssemblyName>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
+    <DebugSymbols>true</DebugSymbols>
+    <DebugType>full</DebugType>
+    <Optimize>false</Optimize>
+    <OutputPath>bin\Debug</OutputPath>
+    <DefineConstants>DEBUG;</DefineConstants>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+    <EnablePackageSigning>false</EnablePackageSigning>
+    <IncludeMonoRuntime>false</IncludeMonoRuntime>
+    <ConsolePause>false</ConsolePause>
+    <EnableCodeSigning>false</EnableCodeSigning>
+    <CreatePackage>false</CreatePackage>
+    <CodeSigningKey>Mac Developer</CodeSigningKey>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
+    <DebugType>none</DebugType>
+    <Optimize>true</Optimize>
+    <OutputPath>bin\Release</OutputPath>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+    <EnablePackageSigning>false</EnablePackageSigning>
+    <IncludeMonoRuntime>false</IncludeMonoRuntime>
+    <LinkMode>Full</LinkMode>
+    <ConsolePause>false</ConsolePause>
+    <EnableCodeSigning>false</EnableCodeSigning>
+    <CreatePackage>false</CreatePackage>
+    <CodeSigningKey>Mac Developer</CodeSigningKey>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'AppStore|AnyCPU' ">
+    <DebugType>none</DebugType>
+    <Optimize>true</Optimize>
+    <OutputPath>bin\AppStore</OutputPath>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+    <PackageSigningKey>3rd Party Mac Developer Installer</PackageSigningKey>
+    <IncludeMonoRuntime>true</IncludeMonoRuntime>
+    <LinkMode>Full</LinkMode>
+    <EnablePackageSigning>true</EnablePackageSigning>
+    <ConsolePause>false</ConsolePause>
+    <EnableCodeSigning>true</EnableCodeSigning>
+    <CreatePackage>true</CreatePackage>
+    <CodeSigningKey>3rd Party Mac Developer Application</CodeSigningKey>
+  </PropertyGroup>
+  <ItemGroup>
+    <Reference Include="System" />
+    <Reference Include="System.Xml" />
+    <Reference Include="System.Core" />
+    <Reference Include="System.Xml.Linq" />
+    <Reference Include="System.Drawing" />
+    <Reference Include="MonoMac" />
+  </ItemGroup>
+  <ItemGroup>
+    <ProjectReference Include="..\..\MonoGame.Framework\MonoGame.Framework.MacOS.csproj">
+      <Project>{36C538E6-C32A-4A8D-A39C-566173D7118E}</Project>
+      <Name>MonoGame.Framework.MacOS</Name>
+    </ProjectReference>
+    <ProjectReference Include="..\..\ThirdParty\Lidgren.Network\Lidgren.Network.MacOS.csproj">
+      <Project>{AE483C29-042E-4226-BA52-D247CE7676DA}</Project>
+      <Name>Lidgren.Network.MacOS</Name>
+    </ProjectReference>
+  </ItemGroup>
+  <ItemGroup>
+    <Compile Include="Main.cs" />
+    <Compile Include="Properties\AssemblyInfo.cs" />
+    <Compile Include="BloomComponent.cs" />
+    <Compile Include="BloomSettings.cs" />
+    <Compile Include="Game.cs" />
+  </ItemGroup>
+  <ItemGroup>
+    <None Include="Info.plist" />
+  </ItemGroup>
+  <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" />
+  <Import Project="$(MSBuildExtensionsPath)\Mono\MonoMac\v0.0\Mono.MonoMac.targets" />
+  <ItemGroup>
+    <Folder Include="Content\" />
+  </ItemGroup>
+  <ItemGroup>
+    <Content Include="Content\BloomCombine.xnb" />
+    <Content Include="Content\BloomExtract.xnb" />
+    <Content Include="Content\GaussianBlur.xnb" />
+    <Content Include="Content\engine_diff_tex_0.xnb" />
+    <Content Include="Content\hudFont.xnb" />
+    <Content Include="Content\sunset.xnb" />
+    <Content Include="Content\tank.xnb" />
+    <Content Include="Content\turret_alt_diff_tex_0.xnb" />
+  </ItemGroup>
+</Project>

BloomSample/BloomSettings.cs

@@ -0,0 +1,82 @@
+#region File Description
+//-----------------------------------------------------------------------------
+// BloomSettings.cs
+//
+// Microsoft XNA Community Game Platform
+// Copyright (C) Microsoft Corporation. All rights reserved.
+//-----------------------------------------------------------------------------
+#endregion
+
+namespace BloomPostprocess
+{
+    /// <summary>
+    /// Class holds all the settings used to tweak the bloom effect.
+    /// </summary>
+    public class BloomSettings
+    {
+        #region Fields
+
+
+        // Name of a preset bloom setting, for display to the user.
+        public readonly string Name;
+
+
+        // Controls how bright a pixel needs to be before it will bloom.
+        // Zero makes everything bloom equally, while higher values select
+        // only brighter colors. Somewhere between 0.25 and 0.5 is good.
+        public readonly float BloomThreshold;
+
+
+        // Controls how much blurring is applied to the bloom image.
+        // The typical range is from 1 up to 10 or so.
+        public readonly float BlurAmount;
+
+
+        // Controls the amount of the bloom and base images that
+        // will be mixed into the final scene. Range 0 to 1.
+        public readonly float BloomIntensity;
+        public readonly float BaseIntensity;
+
+
+        // Independently control the color saturation of the bloom and
+        // base images. Zero is totally desaturated, 1.0 leaves saturation
+        // unchanged, while higher values increase the saturation level.
+        public readonly float BloomSaturation;
+        public readonly float BaseSaturation;
+
+
+        #endregion
+
+
+        /// <summary>
+        /// Constructs a new bloom settings descriptor.
+        /// </summary>
+        public BloomSettings(string name, float bloomThreshold, float blurAmount,
+                             float bloomIntensity, float baseIntensity,
+                             float bloomSaturation, float baseSaturation)
+        {
+            Name = name;
+            BloomThreshold = bloomThreshold;
+            BlurAmount = blurAmount;
+            BloomIntensity = bloomIntensity;
+            BaseIntensity = baseIntensity;
+            BloomSaturation = bloomSaturation;
+            BaseSaturation = baseSaturation;
+        }
+        
+
+        /// <summary>
+        /// Table of preset bloom settings, used by the sample program.
+        /// </summary>
+        public static BloomSettings[] PresetSettings =
+        {
+            //                Name           Thresh  Blur Bloom  Base  BloomSat BaseSat
+            new BloomSettings("Default",     0.25f,  4,   1.25f, 1,    1,       1),
+            new BloomSettings("Soft",        0,      3,   1,     1,    1,       1),
+            new BloomSettings("Desaturated", 0.5f,   8,   2,     1,    0,       1),
+            new BloomSettings("Saturated",   0.25f,  4,   2,     1,    2,       0),
+            new BloomSettings("Blurry",      0,      2,   1,     0.1f, 1,       1),
+            new BloomSettings("Subtle",      0.5f,   2,   1,     1,    1,       1),
+        };
+    }
+}

BloomSample/Game.cs

@@ -0,0 +1,254 @@
+#region File Description
+//-----------------------------------------------------------------------------
+// Game.cs
+//
+// Microsoft XNA Community Game Platform
+// Copyright (C) Microsoft Corporation. All rights reserved.
+//-----------------------------------------------------------------------------
+#endregion
+
+#region Using Statements
+using Microsoft.Xna.Framework;
+using Microsoft.Xna.Framework.Content;
+using Microsoft.Xna.Framework.Graphics;
+using Microsoft.Xna.Framework.Input;
+#endregion
+
+namespace BloomPostprocess
+{
+    /// <summary>
+    /// Sample showing how to implement a bloom postprocess,
+    /// adding a glowing effect over the top of an existing scene.
+    /// </summary>
+    public class BloomPostprocessGame : Microsoft.Xna.Framework.Game
+    {
+        #region Fields
+
+        GraphicsDeviceManager graphics;
+
+        BloomComponent bloom;
+
+        int bloomSettingsIndex = 0;
+
+        SpriteBatch spriteBatch;
+        SpriteFont spriteFont;
+        Texture2D background;
+        Model model;
+
+        KeyboardState lastKeyboardState = new KeyboardState();
+        GamePadState lastGamePadState = new GamePadState();
+        KeyboardState currentKeyboardState = new KeyboardState();
+        GamePadState currentGamePadState = new GamePadState();
+
+        #endregion
+
+        #region Initialization
+
+
+        public BloomPostprocessGame()
+        {
+            Content.RootDirectory = "Content";
+
+            graphics = new GraphicsDeviceManager(this);
+
+            bloom = new BloomComponent(this);
+
+            Components.Add(bloom);
+        }
+
+
+        /// <summary>
+        /// Load your graphics content.
+        /// </summary>
+        protected override void LoadContent()
+        {
+            spriteBatch = new SpriteBatch(graphics.GraphicsDevice);
+            spriteFont = Content.Load<SpriteFont>("hudFont");
+            background = Content.Load<Texture2D>("sunset");
+            model = Content.Load<Model>("tank");
+        }
+
+
+        #endregion
+
+        #region Update and Draw
+
+
+        /// <summary>
+        /// Allows the game to run logic.
+        /// </summary>
+        protected override void Update(GameTime gameTime)
+        {
+            HandleInput();
+
+            base.Update(gameTime);
+        }
+
+
+        /// <summary>
+        /// This is called when the game should draw itself.
+        /// </summary>
+        protected override void Draw(GameTime gameTime)
+        {
+            GraphicsDevice device = graphics.GraphicsDevice;
+            Viewport viewport = device.Viewport;
+
+            bloom.BeginDraw();
+
+            device.Clear(Color.Black);
+
+            // Draw the background image.
+            spriteBatch.Begin(0, BlendState.Opaque);
+            
+            spriteBatch.Draw(background,
+                             new Rectangle(0, 0, viewport.Width, viewport.Height),
+                             Color.White);
+            
+            spriteBatch.End();
+
+            // Draw the spinning model.
+            device.DepthStencilState = DepthStencilState.Default;
+
+            DrawModel(gameTime);
+
+            // Draw other components (which includes the bloom).
+            base.Draw(gameTime);
+
+            // Display some text over the top. Note how we draw this after the bloom,
+            // because we don't want the text to be affected by the postprocessing.
+            DrawOverlayText();
+        }
+
+
+        /// <summary>
+        /// Helper for drawing the spinning 3D model.
+        /// </summary>
+        void DrawModel(GameTime gameTime)
+        {
+            float time = (float)gameTime.TotalGameTime.TotalSeconds;
+
+            Viewport viewport = graphics.GraphicsDevice.Viewport;
+            float aspectRatio = (float)viewport.Width / (float)viewport.Height;
+
+            // Create camera matrices.
+            Matrix world = Matrix.CreateRotationY(time * 0.42f);
+            
+            Matrix view = Matrix.CreateLookAt(new Vector3(750, 100, 0),
+                                              new Vector3(0, 300, 0),
+                                              Vector3.Up);
+
+            Matrix projection = Matrix.CreatePerspectiveFieldOfView(1, aspectRatio,
+                                                                    1, 10000);
+
+            // Look up the bone transform matrices.
+            Matrix[] transforms = new Matrix[model.Bones.Count];
+
+            model.CopyAbsoluteBoneTransformsTo(transforms);
+
+            // Draw the model.
+            foreach (ModelMesh mesh in model.Meshes)
+            {
+                foreach (BasicEffect effect in mesh.Effects)
+                {
+                    effect.World = transforms[mesh.ParentBone.Index] * world;
+                    effect.View = view;
+                    effect.Projection = projection;
+
+                    effect.EnableDefaultLighting();
+
+                    // Override the default specular color to make it nice and bright,
+                    // so we'll get some decent glints that the bloom can key off.
+                    effect.SpecularColor = Vector3.One;
+                }
+
+                mesh.Draw();
+            }
+        }
+
+
+        /// <summary>
+        /// Displays an overlay showing what the controls are,
+        /// and which settings are currently selected.
+        /// </summary>
+        void DrawOverlayText()
+        {
+            string text = "A = settings (" + bloom.Settings.Name + ")\n" +
+                          "B = toggle bloom (" + (bloom.Visible ? "on" : "off") + ")\n" +
+                          "X = show buffer (" + bloom.ShowBuffer.ToString() + ")";
+
+            spriteBatch.Begin();
+
+            // Draw the string twice to create a drop shadow, first colored black
+            // and offset one pixel to the bottom right, then again in white at the
+            // intended position. This makes text easier to read over the background.
+            spriteBatch.DrawString(spriteFont, text, new Vector2(65, 65), Color.Black);
+            spriteBatch.DrawString(spriteFont, text, new Vector2(64, 64), Color.White);
+
+            spriteBatch.End();
+        }
+
+
+        #endregion
+
+        #region Handle Input
+
+
+        /// <summary>
+        /// Handles input for quitting or changing the bloom settings.
+        /// </summary>
+        private void HandleInput()
+        {
+            lastKeyboardState = currentKeyboardState;
+            lastGamePadState = currentGamePadState;
+
+            currentKeyboardState = Keyboard.GetState();
+            currentGamePadState = GamePad.GetState(PlayerIndex.One);
+
+            // Check for exit.
+            if (currentKeyboardState.IsKeyDown(Keys.Escape) ||
+                currentGamePadState.Buttons.Back == ButtonState.Pressed)
+            {
+                Exit();
+            }
+
+            // Switch to the next bloom settings preset?
+            if ((currentGamePadState.Buttons.A == ButtonState.Pressed &&
+                 lastGamePadState.Buttons.A != ButtonState.Pressed) ||
+                (currentKeyboardState.IsKeyDown(Keys.A) &&
+                 lastKeyboardState.IsKeyUp(Keys.A)))
+            {
+                bloomSettingsIndex = (bloomSettingsIndex + 1) %
+                                     BloomSettings.PresetSettings.Length;
+             
+                bloom.Settings = BloomSettings.PresetSettings[bloomSettingsIndex];
+                bloom.Visible = true;
+            }
+
+            // Toggle bloom on or off?
+            if ((currentGamePadState.Buttons.B == ButtonState.Pressed &&
+                 lastGamePadState.Buttons.B != ButtonState.Pressed) ||
+                (currentKeyboardState.IsKeyDown(Keys.B) &&
+                 lastKeyboardState.IsKeyUp(Keys.B)))
+            {
+                bloom.Visible = !bloom.Visible;
+            }
+
+            // Cycle through the intermediate buffer debug display modes?
+            if ((currentGamePadState.Buttons.X == ButtonState.Pressed &&
+                 lastGamePadState.Buttons.X != ButtonState.Pressed) ||
+                (currentKeyboardState.IsKeyDown(Keys.X) &&
+                 lastKeyboardState.IsKeyUp(Keys.X)))
+            {
+                bloom.Visible = true;
+                bloom.ShowBuffer++;
+
+                if (bloom.ShowBuffer > BloomComponent.IntermediateBuffer.FinalResult)
+                    bloom.ShowBuffer= 0;
+            }
+        }
+
+
+        #endregion
+    }
+
+}

BloomSample/Info.plist

@@ -0,0 +1,16 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+<plist version="1.0">
+<dict>
+	<key>CFBundleIdentifier</key>
+	<string>com.yourcompany.BloomSample</string>
+	<key>CFBundleName</key>
+	<string>BloomSample</string>
+	<key>CFBundleVersion</key>
+	<string>1</string>
+	<key>LSMinimumSystemVersion</key>
+	<string>10.6</string>
+	<key>NSPrincipalClass</key>
+	<string>NSApplication</string>
+</dict>
+</plist>

BloomSample/Main.cs

@@ -0,0 +1,45 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+
+using MonoMac.AppKit;
+using MonoMac.Foundation;
+
+namespace BloomPostprocess
+{
+	static class Program
+	{
+		/// <summary>
+		/// The main entry point for the application.
+		/// </summary>
+		static void Main (string[] args)
+		{
+			NSApplication.Init ();
+			
+			using (var p = new NSAutoreleasePool ()) {
+				NSApplication.SharedApplication.Delegate = new AppDelegate ();
+				NSApplication.Main (args);
+			}
+
+
+		}
+	}
+
+	class AppDelegate : NSApplicationDelegate
+	{
+		BloomPostprocessGame game;
+
+		public override void FinishedLaunching (MonoMac.Foundation.NSObject notification)
+		{
+			game = new BloomPostprocessGame ();
+			game.Run ();
+		}
+		
+		public override bool ApplicationShouldTerminateAfterLastWindowClosed (NSApplication sender)
+		{
+			return true;
+		}
+	}  
+}
+
+

BloomSample/Properties/AssemblyInfo.cs

@@ -0,0 +1,30 @@
+#region File Description
+//-----------------------------------------------------------------------------
+// AssemblyInfo.cs
+//
+// Microsoft XNA Community Game Platform
+// Copyright (C) Microsoft Corporation. All rights reserved.
+//-----------------------------------------------------------------------------
+#endregion
+
+#region Using Statements
+using System.Reflection;
+using System.Runtime.CompilerServices;
+using System.Runtime.InteropServices;
+#endregion
+
+//Descriptive Assembly attributes
+[assembly: AssemblyTitle("Bloom Postprocess")]
+[assembly: AssemblyProduct("Bloom Postprocess")]
+[assembly: AssemblyDescription("This sample shows how to use bloom post-processing filters to add a glow effect over the top of an existing scene.")]
+[assembly: AssemblyCompany("Microsoft")]
+
+[assembly: AssemblyCopyright("Copyright © Microsoft 2007")]
+[assembly: AssemblyTrademark("")]
+[assembly: AssemblyCulture("")]
+[assembly: AssemblyVersion("1.0.0.0")]
+
+//ComVisible is false for this component.
+[assembly: ComVisible(false)]
+[assembly: Guid("01a21a09-e2a4-473d-bc16-d7f157cc1c78")]
+

BookSourceCode/XNAGameDevelopmentbyExampleCode/AsteroidBeltAssault/AsteroidBeltAssault.MacOS.csproj

@@ -16,7 +16,7 @@
     <DebugType>full</DebugType>
     <Optimize>false</Optimize>
     <OutputPath>bin\Debug</OutputPath>
-    <DefineConstants>DEBUG</DefineConstants>
+    <DefineConstants>DEBUG;MONOMAC</DefineConstants>
     <ErrorReport>prompt</ErrorReport>
     <WarningLevel>4</WarningLevel>
     <ConsolePause>false</ConsolePause>
@@ -36,6 +36,7 @@
     <CreatePackage>false</CreatePackage>
     <EnablePackageSigning>false</EnablePackageSigning>
     <IncludeMonoRuntime>false</IncludeMonoRuntime>
+    <DefineConstants>MONOMAC</DefineConstants>
   </PropertyGroup>
   <ItemGroup>
     <Reference Include="System" />

BookSourceCode/XNAGameDevelopmentbyExampleCode/AsteroidBeltAssault/MonoGame.Samples.AsteroidBeltAssault.Linux.csproj

@@ -11,15 +11,15 @@
     <AssemblyName>FloodControl</AssemblyName>
   </PropertyGroup>
   <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|x86' ">
-    <DebugSymbols>true</DebugSymbols>
+    <DebugSymbols>True</DebugSymbols>
     <DebugType>full</DebugType>
-    <Optimize>false</Optimize>
+    <Optimize>False</Optimize>
     <OutputPath>bin\Debug</OutputPath>
     <DefineConstants>DEBUG,LINUX</DefineConstants>
     <ErrorReport>prompt</ErrorReport>
     <WarningLevel>4</WarningLevel>
     <PlatformTarget>x86</PlatformTarget>
-    <ConsolePause>false</ConsolePause>
+    <ConsolePause>False</ConsolePause>
     <EnvironmentVariables>
       <EnvironmentVariables>
         <Variable name="MONO_IOMAP" value="all" />
@@ -28,12 +28,12 @@
   </PropertyGroup>
   <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|x86' ">
     <DebugType>none</DebugType>
-    <Optimize>false</Optimize>
+    <Optimize>False</Optimize>
     <OutputPath>bin\Release</OutputPath>
     <ErrorReport>prompt</ErrorReport>
     <WarningLevel>4</WarningLevel>
     <PlatformTarget>x86</PlatformTarget>
-    <ConsolePause>false</ConsolePause>
+    <ConsolePause>False</ConsolePause>
   </PropertyGroup>
   <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" />
   <ItemGroup>

BookSourceCode/XNAGameDevelopmentbyExampleCode/FloodControl/FloodControl.MacOS.csproj

@@ -16,7 +16,7 @@
     <DebugType>full</DebugType>
     <Optimize>false</Optimize>
     <OutputPath>bin\Debug</OutputPath>
-    <DefineConstants>DEBUG</DefineConstants>
+    <DefineConstants>DEBUG;MONOMAC</DefineConstants>
     <ErrorReport>prompt</ErrorReport>
     <WarningLevel>4</WarningLevel>
     <ConsolePause>false</ConsolePause>
@@ -36,6 +36,7 @@
     <CreatePackage>false</CreatePackage>
     <EnablePackageSigning>false</EnablePackageSigning>
     <IncludeMonoRuntime>false</IncludeMonoRuntime>
+    <DefineConstants>MONOMAC</DefineConstants>
   </PropertyGroup>
   <ItemGroup>
     <Reference Include="System" />

BookSourceCode/XNAGameDevelopmentbyExampleCode/FloodControl/MonoGame.Samples.FloodControl.Linux.csproj

@@ -11,24 +11,24 @@
     <AssemblyName>FloodControl</AssemblyName>
   </PropertyGroup>
   <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|x86' ">
-    <DebugSymbols>true</DebugSymbols>
+    <DebugSymbols>True</DebugSymbols>
     <DebugType>full</DebugType>
-    <Optimize>false</Optimize>
+    <Optimize>False</Optimize>
     <OutputPath>bin\Debug</OutputPath>
     <DefineConstants>DEBUG,LINUX</DefineConstants>
     <ErrorReport>prompt</ErrorReport>
     <WarningLevel>4</WarningLevel>
     <PlatformTarget>x86</PlatformTarget>
-    <ConsolePause>false</ConsolePause>
+    <ConsolePause>False</ConsolePause>
   </PropertyGroup>
   <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|x86' ">
     <DebugType>none</DebugType>
-    <Optimize>false</Optimize>
+    <Optimize>False</Optimize>
     <OutputPath>bin\Release</OutputPath>
     <ErrorReport>prompt</ErrorReport>
     <WarningLevel>4</WarningLevel>
     <PlatformTarget>x86</PlatformTarget>
-    <ConsolePause>false</ConsolePause>
+    <ConsolePause>False</ConsolePause>
   </PropertyGroup>
   <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" />
   <ItemGroup>

BookSourceCode/XNAGameDevelopmentbyExampleCode/GemstoneHunter/GemstoneHunter.Linux.csproj

@@ -11,24 +11,24 @@
     <AssemblyName>FloodControl</AssemblyName>
   </PropertyGroup>
   <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|x86' ">
-    <DebugSymbols>true</DebugSymbols>
+    <DebugSymbols>True</DebugSymbols>
     <DebugType>full</DebugType>
-    <Optimize>false</Optimize>
+    <Optimize>False</Optimize>
     <OutputPath>bin\Debug</OutputPath>
     <DefineConstants>DEBUG,LINUX</DefineConstants>
     <ErrorReport>prompt</ErrorReport>
     <WarningLevel>4</WarningLevel>
     <PlatformTarget>x86</PlatformTarget>
-    <ConsolePause>false</ConsolePause>
+    <ConsolePause>False</ConsolePause>
   </PropertyGroup>
   <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|x86' ">
     <DebugType>none</DebugType>
-    <Optimize>false</Optimize>
+    <Optimize>False</Optimize>
     <OutputPath>bin\Release</OutputPath>
     <ErrorReport>prompt</ErrorReport>
     <WarningLevel>4</WarningLevel>
     <PlatformTarget>x86</PlatformTarget>
-    <ConsolePause>false</ConsolePause>
+    <ConsolePause>False</ConsolePause>
   </PropertyGroup>
   <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" />
   <ItemGroup>

BookSourceCode/XNAGameDevelopmentbyExampleCode/GemstoneHunter/GemstoneHunter.MacOS.csproj

@@ -16,7 +16,7 @@
     <DebugType>full</DebugType>
     <Optimize>false</Optimize>
     <OutputPath>bin\Debug</OutputPath>
-    <DefineConstants>DEBUG</DefineConstants>
+    <DefineConstants>DEBUG;MONOMAC</DefineConstants>
     <ErrorReport>prompt</ErrorReport>
     <WarningLevel>4</WarningLevel>
     <ConsolePause>false</ConsolePause>
@@ -36,6 +36,7 @@
     <CreatePackage>false</CreatePackage>
     <EnablePackageSigning>false</EnablePackageSigning>
     <IncludeMonoRuntime>false</IncludeMonoRuntime>
+    <DefineConstants>MONOMAC</DefineConstants>
   </PropertyGroup>
   <ItemGroup>
     <Reference Include="System" />

BookSourceCode/XNAGameDevelopmentbyExampleCode/GemstoneHunter/TileEngine/Tile Engine.Linux.csproj

@@ -11,22 +11,22 @@
     <AssemblyName>Tile Engine</AssemblyName>
   </PropertyGroup>
   <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
-    <DebugSymbols>true</DebugSymbols>
+    <DebugSymbols>True</DebugSymbols>
     <DebugType>full</DebugType>
-    <Optimize>false</Optimize>
+    <Optimize>False</Optimize>
     <OutputPath>bin\Debug</OutputPath>
     <DefineConstants>DEBUG</DefineConstants>
     <ErrorReport>prompt</ErrorReport>
     <WarningLevel>4</WarningLevel>
-    <ConsolePause>false</ConsolePause>
+    <ConsolePause>False</ConsolePause>
   </PropertyGroup>
   <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
     <DebugType>none</DebugType>
-    <Optimize>false</Optimize>
+    <Optimize>False</Optimize>
     <OutputPath>bin\Release</OutputPath>
     <ErrorReport>prompt</ErrorReport>
     <WarningLevel>4</WarningLevel>
-    <ConsolePause>false</ConsolePause>
+    <ConsolePause>False</ConsolePause>
   </PropertyGroup>
   <ItemGroup>
     <Reference Include="System" />

BookSourceCode/XNAGameDevelopmentbyExampleCode/RobotRampage/MonoGame.Samples.RobotRampage.Linux.csproj

@@ -11,24 +11,24 @@
     <AssemblyName>FloodControl</AssemblyName>
   </PropertyGroup>
   <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|x86' ">
-    <DebugSymbols>true</DebugSymbols>
+    <DebugSymbols>True</DebugSymbols>
     <DebugType>full</DebugType>
-    <Optimize>false</Optimize>
+    <Optimize>False</Optimize>
     <OutputPath>bin\Debug</OutputPath>
     <DefineConstants>DEBUG,LINUX</DefineConstants>
     <ErrorReport>prompt</ErrorReport>
     <WarningLevel>4</WarningLevel>
     <PlatformTarget>x86</PlatformTarget>
-    <ConsolePause>false</ConsolePause>
+    <ConsolePause>False</ConsolePause>
   </PropertyGroup>
   <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|x86' ">
     <DebugType>none</DebugType>
-    <Optimize>false</Optimize>
+    <Optimize>False</Optimize>
     <OutputPath>bin\Release</OutputPath>
     <ErrorReport>prompt</ErrorReport>
     <WarningLevel>4</WarningLevel>
     <PlatformTarget>x86</PlatformTarget>
-    <ConsolePause>false</ConsolePause>
+    <ConsolePause>False</ConsolePause>
   </PropertyGroup>
   <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" />
   <ItemGroup>

BookSourceCode/XNAGameDevelopmentbyExampleCode/RobotRampage/Program.cs

@@ -44,9 +44,8 @@ namespace Robot_Rampage
 		
 		public override void FinishedLaunching (MonoMac.Foundation.NSObject notification)
 		{
-			using (Game1 game = new Game1 ()) {
-				game.Run ();
-			}
+			Game1 game = new Game1 ();
+			game.Run ();
 		}
 		
 		public override bool ApplicationShouldTerminateAfterLastWindowClosed (NSApplication sender)

BookSourceCode/XNAGameDevelopmentbyExampleCode/RobotRampage/RobotRampage.MacOS.csproj

@@ -16,7 +16,7 @@
     <DebugType>full</DebugType>
     <Optimize>false</Optimize>
     <OutputPath>bin\Debug</OutputPath>
-    <DefineConstants>DEBUG</DefineConstants>
+    <DefineConstants>DEBUG;MONOMAC</DefineConstants>
     <ErrorReport>prompt</ErrorReport>
     <WarningLevel>4</WarningLevel>
     <ConsolePause>false</ConsolePause>
@@ -36,6 +36,7 @@
     <CreatePackage>false</CreatePackage>
     <EnablePackageSigning>false</EnablePackageSigning>
     <IncludeMonoRuntime>false</IncludeMonoRuntime>
+    <DefineConstants>MONOMAC</DefineConstants>
   </PropertyGroup>
   <ItemGroup>
     <Reference Include="System" />

CatapultNetWars/CatapultNetWars.Linux.csproj

@@ -14,9 +14,9 @@
   </PropertyGroup>
   <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|x86' ">
     <PlatformTarget>x86</PlatformTarget>
-    <DebugSymbols>true</DebugSymbols>
+    <DebugSymbols>True</DebugSymbols>
     <DebugType>full</DebugType>
-    <Optimize>false</Optimize>
+    <Optimize>False</Optimize>
     <OutputPath>bin\Debug\</OutputPath>
     <DefineConstants>TRACE;DEBUG;LINUX</DefineConstants>
     <ErrorReport>prompt</ErrorReport>
@@ -25,7 +25,7 @@
   <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|x86' ">
     <PlatformTarget>x86</PlatformTarget>
     <DebugType>pdbonly</DebugType>
-    <Optimize>true</Optimize>
+    <Optimize>True</Optimize>
     <OutputPath>bin\Release\</OutputPath>
     <DefineConstants>TRACE;LINUX</DefineConstants>
     <ErrorReport>prompt</ErrorReport>
@@ -286,12 +286,6 @@
       <CopyToOutputDirectory>PreserveNewest</CopyToOutputDirectory>
     </Content>
   </ItemGroup>
-  <ItemGroup>
-    <Content Include="Content\Textures\Catapults\AnimationsDef.xml">
-      <Link>Content\Textures\Catapults\AnimationsDef.xml</Link>
-      <CopyToOutputDirectory>PreserveNewest</CopyToOutputDirectory>
-    </Content>
-  </ItemGroup>
   <ItemGroup>
     <EmbeddedResource Include="Properties\Resources.resx">
       <Generator>ResXFileCodeGenerator</Generator>

ChaseAndEvade/MonoGame.Samples.ChaseAndEvade.Linux.csproj

@@ -11,24 +11,24 @@
     <AssemblyName>ChaseAndEvade</AssemblyName>
   </PropertyGroup>
   <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|x86' ">
-    <DebugSymbols>true</DebugSymbols>
+    <DebugSymbols>True</DebugSymbols>
     <DebugType>full</DebugType>
-    <Optimize>false</Optimize>
+    <Optimize>False</Optimize>
     <OutputPath>bin\Debug</OutputPath>
     <DefineConstants>LINUX;DEBUG</DefineConstants>
     <ErrorReport>prompt</ErrorReport>
     <WarningLevel>4</WarningLevel>
     <PlatformTarget>x86</PlatformTarget>
-    <ConsolePause>false</ConsolePause>
+    <ConsolePause>False</ConsolePause>
   </PropertyGroup>
   <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|x86' ">
     <DebugType>none</DebugType>
-    <Optimize>false</Optimize>
+    <Optimize>False</Optimize>
     <OutputPath>bin\Release</OutputPath>
     <ErrorReport>prompt</ErrorReport>
     <WarningLevel>4</WarningLevel>
     <PlatformTarget>x86</PlatformTarget>
-    <ConsolePause>false</ConsolePause>
+    <ConsolePause>False</ConsolePause>
     <DefineConstants>LINUX</DefineConstants>
   </PropertyGroup>
   <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" />

Draw2D/MonoGame.Samples.Draw2D.Linux.csproj

@@ -14,9 +14,9 @@
   </PropertyGroup>
   <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|x86' ">
     <PlatformTarget>x86</PlatformTarget>
-    <DebugSymbols>true</DebugSymbols>
+    <DebugSymbols>True</DebugSymbols>
     <DebugType>full</DebugType>
-    <Optimize>false</Optimize>
+    <Optimize>False</Optimize>
     <OutputPath>bin\Debug\</OutputPath>
     <DefineConstants>DEBUG;TRACE;WINDOWS</DefineConstants>
     <ErrorReport>prompt</ErrorReport>
@@ -25,7 +25,7 @@
   <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|x86' ">
     <PlatformTarget>x86</PlatformTarget>
     <DebugType>pdbonly</DebugType>
-    <Optimize>true</Optimize>
+    <Optimize>True</Optimize>
     <OutputPath>bin\Release\</OutputPath>
     <DefineConstants>TRACE;WINDOWS</DefineConstants>
     <ErrorReport>prompt</ErrorReport>

FarseerPhysicsEngine/Collision/Collision.cs

@@ -0,0 +1,1945 @@
+/*
+* Farseer Physics Engine based on Box2D.XNA port:
+* Copyright (c) 2010 Ian Qvist
+* 
+* Box2D.XNA port of Box2D:
+* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
+*
+* Original source Box2D:
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
+* 
+* This software is provided 'as-is', without any express or implied 
+* warranty.  In no event will the authors be held liable for any damages 
+* arising from the use of this software. 
+* Permission is granted to anyone to use this software for any purpose, 
+* including commercial applications, and to alter it and redistribute it 
+* freely, subject to the following restrictions: 
+* 1. The origin of this software must not be misrepresented; you must not 
+* claim that you wrote the original software. If you use this software 
+* in a product, an acknowledgment in the product documentation would be 
+* appreciated but is not required. 
+* 2. Altered source versions must be plainly marked as such, and must not be 
+* misrepresented as being the original software. 
+* 3. This notice may not be removed or altered from any source distribution. 
+*/
+
+using System;
+using System.Diagnostics;
+using System.Runtime.InteropServices;
+using FarseerPhysics.Collision.Shapes;
+using FarseerPhysics.Common;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Collision
+{
+    internal enum ContactFeatureType : byte
+    {
+        Vertex = 0,
+        Face = 1,
+    }
+
+    /// <summary>
+    /// The features that intersect to form the contact point
+    /// This must be 4 bytes or less.
+    /// </summary>
+    public struct ContactFeature
+    {
+        /// <summary>
+        /// Feature index on ShapeA
+        /// </summary>
+        public byte IndexA;
+
+        /// <summary>
+        /// Feature index on ShapeB
+        /// </summary>
+        public byte IndexB;
+
+        /// <summary>
+        /// The feature type on ShapeA
+        /// </summary>
+        public byte TypeA;
+
+        /// <summary>
+        /// The feature type on ShapeB
+        /// </summary>
+        public byte TypeB;
+    }
+
+    /// <summary>
+    /// Contact ids to facilitate warm starting.
+    /// </summary>
+    [StructLayout(LayoutKind.Explicit)]
+    public struct ContactID
+    {
+        /// <summary>
+        /// The features that intersect to form the contact point
+        /// </summary>
+        [FieldOffset(0)]
+        public ContactFeature Features;
+
+        /// <summary>
+        /// Used to quickly compare contact ids.
+        /// </summary>
+        [FieldOffset(0)]
+        public uint Key;
+    }
+
+    /// <summary>
+    /// A manifold point is a contact point belonging to a contact
+    /// manifold. It holds details related to the geometry and dynamics
+    /// of the contact points.
+    /// The local point usage depends on the manifold type:
+    /// -ShapeType.Circles: the local center of circleB
+    /// -SeparationFunction.FaceA: the local center of cirlceB or the clip point of polygonB
+    /// -SeparationFunction.FaceB: the clip point of polygonA
+    /// This structure is stored across time steps, so we keep it small.
+    /// Note: the impulses are used for internal caching and may not
+    /// provide reliable contact forces, especially for high speed collisions.
+    /// </summary>
+    public struct ManifoldPoint
+    {
+        /// <summary>
+        /// Uniquely identifies a contact point between two Shapes
+        /// </summary>
+        public ContactID Id;
+
+        public Vector2 LocalPoint;
+
+        public float NormalImpulse;
+
+        public float TangentImpulse;
+    }
+
+    public enum ManifoldType
+    {
+        Circles,
+        FaceA,
+        FaceB
+    }
+
+    /// <summary>
+    /// A manifold for two touching convex Shapes.
+    /// Box2D supports multiple types of contact:
+    /// - clip point versus plane with radius
+    /// - point versus point with radius (circles)
+    /// The local point usage depends on the manifold type:
+    /// -ShapeType.Circles: the local center of circleA
+    /// -SeparationFunction.FaceA: the center of faceA
+    /// -SeparationFunction.FaceB: the center of faceB
+    /// Similarly the local normal usage:
+    /// -ShapeType.Circles: not used
+    /// -SeparationFunction.FaceA: the normal on polygonA
+    /// -SeparationFunction.FaceB: the normal on polygonB
+    /// We store contacts in this way so that position correction can
+    /// account for movement, which is critical for continuous physics.
+    /// All contact scenarios must be expressed in one of these types.
+    /// This structure is stored across time steps, so we keep it small.
+    /// </summary>
+    public struct Manifold
+    {
+        /// <summary>
+        /// Not use for Type.SeparationFunction.Points
+        /// </summary>
+        public Vector2 LocalNormal;
+
+        /// <summary>
+        /// Usage depends on manifold type
+        /// </summary>
+        public Vector2 LocalPoint;
+
+        /// <summary>
+        /// The number of manifold points
+        /// </summary>
+        public int PointCount;
+
+        /// <summary>
+        /// The points of contact
+        /// </summary>
+        public FixedArray2<ManifoldPoint> Points;
+
+        public ManifoldType Type;
+    }
+
+    /// <summary>
+    /// This is used for determining the state of contact points.
+    /// </summary>
+    public enum PointState
+    {
+        /// <summary>
+        /// Point does not exist
+        /// </summary>
+        Null,
+
+        /// <summary>
+        /// Point was added in the update
+        /// </summary>
+        Add,
+
+        /// <summary>
+        /// Point persisted across the update
+        /// </summary>
+        Persist,
+
+        /// <summary>
+        /// Point was removed in the update
+        /// </summary>
+        Remove,
+    }
+
+    /// <summary>
+    /// Used for computing contact manifolds.
+    /// </summary>
+    public struct ClipVertex
+    {
+        public ContactID ID;
+        public Vector2 V;
+    }
+
+    /// <summary>
+    /// Ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
+    /// </summary>
+    public struct RayCastInput
+    {
+        public float MaxFraction;
+        public Vector2 Point1, Point2;
+    }
+
+    /// <summary>
+    /// Ray-cast output data.  The ray hits at p1 + fraction * (p2 - p1), where p1 and p2
+    /// come from RayCastInput. 
+    /// </summary>
+    public struct RayCastOutput
+    {
+        public float Fraction;
+        public Vector2 Normal;
+    }
+
+    /// <summary>
+    /// An axis aligned bounding box.
+    /// </summary>
+    public struct AABB
+    {
+        private static DistanceInput _input = new DistanceInput();
+
+        /// <summary>
+        /// The lower vertex
+        /// </summary>
+        public Vector2 LowerBound;
+
+        /// <summary>
+        /// The upper vertex
+        /// </summary>
+        public Vector2 UpperBound;
+
+        public AABB(Vector2 min, Vector2 max)
+            : this(ref min, ref max)
+        {
+        }
+
+        public AABB(ref Vector2 min, ref Vector2 max)
+        {
+            LowerBound = min;
+            UpperBound = max;
+        }
+
+        public AABB(Vector2 center, float width, float height)
+        {
+            LowerBound = center - new Vector2(width / 2, height / 2);
+            UpperBound = center + new Vector2(width / 2, height / 2);
+        }
+
+        /// <summary>
+        /// Get the center of the AABB.
+        /// </summary>
+        /// <value></value>
+        public Vector2 Center
+        {
+            get { return 0.5f * (LowerBound + UpperBound); }
+        }
+
+        /// <summary>
+        /// Get the extents of the AABB (half-widths).
+        /// </summary>
+        /// <value></value>
+        public Vector2 Extents
+        {
+            get { return 0.5f * (UpperBound - LowerBound); }
+        }
+
+        /// <summary>
+        /// Get the perimeter length
+        /// </summary>
+        /// <value></value>
+        public float Perimeter
+        {
+            get
+            {
+                float wx = UpperBound.X - LowerBound.X;
+                float wy = UpperBound.Y - LowerBound.Y;
+                return 2.0f * (wx + wy);
+            }
+        }
+
+        /// <summary>
+        /// Gets the vertices of the AABB.
+        /// </summary>
+        /// <value>The corners of the AABB</value>
+        public Vertices Vertices
+        {
+            get
+            {
+                Vertices vertices = new Vertices();
+                vertices.Add(LowerBound);
+                vertices.Add(new Vector2(LowerBound.X, UpperBound.Y));
+                vertices.Add(UpperBound);
+                vertices.Add(new Vector2(UpperBound.X, LowerBound.Y));
+                return vertices;
+            }
+        }
+
+        /// <summary>
+        /// first quadrant
+        /// </summary>
+        public AABB Q1
+        {
+            get { return new AABB(Center, UpperBound); }
+        }
+
+        public AABB Q2
+        {
+            get
+            {
+                return new AABB(new Vector2(LowerBound.X, Center.Y), new Vector2(Center.X, UpperBound.Y));
+                ;
+            }
+        }
+
+        public AABB Q3
+        {
+            get { return new AABB(LowerBound, Center); }
+        }
+
+        public AABB Q4
+        {
+            get { return new AABB(new Vector2(Center.X, LowerBound.Y), new Vector2(UpperBound.X, Center.Y)); }
+        }
+
+        public Vector2[] GetVertices()
+        {
+            Vector2 p1 = UpperBound;
+            Vector2 p2 = new Vector2(UpperBound.X, LowerBound.Y);
+            Vector2 p3 = LowerBound;
+            Vector2 p4 = new Vector2(LowerBound.X, UpperBound.Y);
+            return new[] { p1, p2, p3, p4 };
+        }
+
+        /// <summary>
+        /// Verify that the bounds are sorted.
+        /// </summary>
+        /// <returns>
+        /// 	<c>true</c> if this instance is valid; otherwise, <c>false</c>.
+        /// </returns>
+        public bool IsValid()
+        {
+            Vector2 d = UpperBound - LowerBound;
+            bool valid = d.X >= 0.0f && d.Y >= 0.0f;
+            valid = valid && LowerBound.IsValid() && UpperBound.IsValid();
+            return valid;
+        }
+
+        /// <summary>
+        /// Combine an AABB into this one.
+        /// </summary>
+        /// <param name="aabb">The aabb.</param>
+        public void Combine(ref AABB aabb)
+        {
+            LowerBound = Vector2.Min(LowerBound, aabb.LowerBound);
+            UpperBound = Vector2.Max(UpperBound, aabb.UpperBound);
+        }
+
+        /// <summary>
+        /// Combine two AABBs into this one.
+        /// </summary>
+        /// <param name="aabb1">The aabb1.</param>
+        /// <param name="aabb2">The aabb2.</param>
+        public void Combine(ref AABB aabb1, ref AABB aabb2)
+        {
+            LowerBound = Vector2.Min(aabb1.LowerBound, aabb2.LowerBound);
+            UpperBound = Vector2.Max(aabb1.UpperBound, aabb2.UpperBound);
+        }
+
+        /// <summary>
+        /// Does this aabb contain the provided AABB.
+        /// </summary>
+        /// <param name="aabb">The aabb.</param>
+        /// <returns>
+        /// 	<c>true</c> if it contains the specified aabb; otherwise, <c>false</c>.
+        /// </returns>
+        public bool Contains(ref AABB aabb)
+        {
+            bool result = true;
+            result = result && LowerBound.X <= aabb.LowerBound.X;
+            result = result && LowerBound.Y <= aabb.LowerBound.Y;
+            result = result && aabb.UpperBound.X <= UpperBound.X;
+            result = result && aabb.UpperBound.Y <= UpperBound.Y;
+            return result;
+        }
+
+        /// <summary>
+        /// Determines whether the AAABB contains the specified point.
+        /// </summary>
+        /// <param name="point">The point.</param>
+        /// <returns>
+        /// 	<c>true</c> if it contains the specified point; otherwise, <c>false</c>.
+        /// </returns>
+        public bool Contains(ref Vector2 point)
+        {
+            //using epsilon to try and gaurd against float rounding errors.
+            if ((point.X > (LowerBound.X + Settings.Epsilon) && point.X < (UpperBound.X - Settings.Epsilon) &&
+                 (point.Y > (LowerBound.Y + Settings.Epsilon) && point.Y < (UpperBound.Y - Settings.Epsilon))))
+            {
+                return true;
+            }
+            return false;
+        }
+
+        public static bool TestOverlap(AABB a, AABB b)
+        {
+            return TestOverlap(ref a, ref b);
+        }
+
+        public static bool TestOverlap(ref AABB a, ref AABB b)
+        {
+            Vector2 d1 = b.LowerBound - a.UpperBound;
+            Vector2 d2 = a.LowerBound - b.UpperBound;
+
+            if (d1.X > 0.0f || d1.Y > 0.0f)
+                return false;
+
+            if (d2.X > 0.0f || d2.Y > 0.0f)
+                return false;
+
+            return true;
+        }
+
+        public static bool TestOverlap(Shape shapeA, int indexA,
+                                       Shape shapeB, int indexB,
+                                       ref Transform xfA, ref Transform xfB)
+        {
+            _input.ProxyA.Set(shapeA, indexA);
+            _input.ProxyB.Set(shapeB, indexB);
+            _input.TransformA = xfA;
+            _input.TransformB = xfB;
+            _input.UseRadii = true;
+
+            SimplexCache cache;
+            DistanceOutput output;
+            Distance.ComputeDistance(out output, out cache, _input);
+
+            return output.Distance < 10.0f * Settings.Epsilon;
+        }
+
+
+        // From Real-time Collision Detection, p179.
+        public bool RayCast(out RayCastOutput output, ref RayCastInput input)
+        {
+            output = new RayCastOutput();
+
+            float tmin = -Settings.MaxFloat;
+            float tmax = Settings.MaxFloat;
+
+            Vector2 p = input.Point1;
+            Vector2 d = input.Point2 - input.Point1;
+            Vector2 absD = MathUtils.Abs(d);
+
+            Vector2 normal = Vector2.Zero;
+
+            for (int i = 0; i < 2; ++i)
+            {
+                float absD_i = i == 0 ? absD.X : absD.Y;
+                float lowerBound_i = i == 0 ? LowerBound.X : LowerBound.Y;
+                float upperBound_i = i == 0 ? UpperBound.X : UpperBound.Y;
+                float p_i = i == 0 ? p.X : p.Y;
+
+                if (absD_i < Settings.Epsilon)
+                {
+                    // Parallel.
+                    if (p_i < lowerBound_i || upperBound_i < p_i)
+                    {
+                        return false;
+                    }
+                }
+                else
+                {
+                    float d_i = i == 0 ? d.X : d.Y;
+
+                    float inv_d = 1.0f / d_i;
+                    float t1 = (lowerBound_i - p_i) * inv_d;
+                    float t2 = (upperBound_i - p_i) * inv_d;
+
+                    // Sign of the normal vector.
+                    float s = -1.0f;
+
+                    if (t1 > t2)
+                    {
+                        MathUtils.Swap(ref t1, ref t2);
+                        s = 1.0f;
+                    }
+
+                    // Push the min up
+                    if (t1 > tmin)
+                    {
+                        if (i == 0)
+                        {
+                            normal.X = s;
+                        }
+                        else
+                        {
+                            normal.Y = s;
+                        }
+
+                        tmin = t1;
+                    }
+
+                    // Pull the max down
+                    tmax = Math.Min(tmax, t2);
+
+                    if (tmin > tmax)
+                    {
+                        return false;
+                    }
+                }
+            }
+
+            // Does the ray start inside the box?
+            // Does the ray intersect beyond the max fraction?
+            if (tmin < 0.0f || input.MaxFraction < tmin)
+            {
+                return false;
+            }
+
+            // Intersection.
+            output.Fraction = tmin;
+            output.Normal = normal;
+            return true;
+        }
+    }
+
+    /// <summary>
+    /// Edge shape plus more stuff.
+    /// </summary>
+    public struct FatEdge
+    {
+        public bool HasVertex0, HasVertex3;
+        public Vector2 Normal;
+        public Vector2 V0, V1, V2, V3;
+    }
+
+    /// <summary>
+    /// This lets us treate and edge shape and a polygon in the same
+    /// way in the SAT collider.
+    /// </summary>
+    public class EPProxy
+    {
+        public Vector2 Centroid;
+        public int Count;
+        public Vector2[] Normals = new Vector2[Settings.MaxPolygonVertices];
+        public Vector2[] Vertices = new Vector2[Settings.MaxPolygonVertices];
+    }
+
+    public struct EPAxis
+    {
+        public int Index;
+        public float Separation;
+        public EPAxisType Type;
+    }
+
+    public enum EPAxisType
+    {
+        Unknown,
+        EdgeA,
+        EdgeB,
+    }
+
+    public static class Collision
+    {
+        private static FatEdge _edgeA;
+
+        private static EPProxy _proxyA = new EPProxy();
+        private static EPProxy _proxyB = new EPProxy();
+
+        private static Transform _xf;
+        private static Vector2 _limit11, _limit12;
+        private static Vector2 _limit21, _limit22;
+        private static float _radius;
+        private static Vector2[] _tmpNormals = new Vector2[2];
+
+        /// <summary>
+        /// Evaluate the manifold with supplied transforms. This assumes
+        /// modest motion from the original state. This does not change the
+        /// point count, impulses, etc. The radii must come from the Shapes
+        /// that generated the manifold.
+        /// </summary>
+        /// <param name="manifold">The manifold.</param>
+        /// <param name="transformA">The transform for A.</param>
+        /// <param name="radiusA">The radius for A.</param>
+        /// <param name="transformB">The transform for B.</param>
+        /// <param name="radiusB">The radius for B.</param>
+        /// <param name="normal">World vector pointing from A to B</param>
+        /// <param name="points">Torld contact point (point of intersection).</param>
+        public static void GetWorldManifold(ref Manifold manifold,
+                                            ref Transform transformA, float radiusA,
+                                            ref Transform transformB, float radiusB, out Vector2 normal,
+                                            out FixedArray2<Vector2> points)
+        {
+            points = new FixedArray2<Vector2>();
+            normal = Vector2.Zero;
+
+            if (manifold.PointCount == 0)
+            {
+                normal = Vector2.UnitY;
+                return;
+            }
+
+            switch (manifold.Type)
+            {
+                case ManifoldType.Circles:
+                    {
+                        Vector2 tmp = manifold.Points[0].LocalPoint;
+                        float pointAx = transformA.Position.X + transformA.R.Col1.X * manifold.LocalPoint.X +
+                                        transformA.R.Col2.X * manifold.LocalPoint.Y;
+
+                        float pointAy = transformA.Position.Y + transformA.R.Col1.Y * manifold.LocalPoint.X +
+                                        transformA.R.Col2.Y * manifold.LocalPoint.Y;
+
+                        float pointBx = transformB.Position.X + transformB.R.Col1.X * tmp.X +
+                                        transformB.R.Col2.X * tmp.Y;
+
+                        float pointBy = transformB.Position.Y + transformB.R.Col1.Y * tmp.X +
+                                        transformB.R.Col2.Y * tmp.Y;
+
+                        normal.X = 1;
+                        normal.Y = 0;
+
+                        float result = (pointAx - pointBx) * (pointAx - pointBx) +
+                                       (pointAy - pointBy) * (pointAy - pointBy);
+                        if (result > Settings.Epsilon * Settings.Epsilon)
+                        {
+                            float tmpNormalx = pointBx - pointAx;
+                            float tmpNormaly = pointBy - pointAy;
+                            float factor = 1f / (float)Math.Sqrt(tmpNormalx * tmpNormalx + tmpNormaly * tmpNormaly);
+                            normal.X = tmpNormalx * factor;
+                            normal.Y = tmpNormaly * factor;
+                        }
+
+                        Vector2 c = Vector2.Zero;
+                        c.X = (pointAx + radiusA * normal.X) + (pointBx - radiusB * normal.X);
+                        c.Y = (pointAy + radiusA * normal.Y) + (pointBy - radiusB * normal.Y);
+
+                        points[0] = 0.5f * c;
+                    }
+                    break;
+
+                case ManifoldType.FaceA:
+                    {
+                        normal.X = transformA.R.Col1.X * manifold.LocalNormal.X +
+                                   transformA.R.Col2.X * manifold.LocalNormal.Y;
+                        normal.Y = transformA.R.Col1.Y * manifold.LocalNormal.X +
+                                   transformA.R.Col2.Y * manifold.LocalNormal.Y;
+
+                        float planePointx = transformA.Position.X + transformA.R.Col1.X * manifold.LocalPoint.X +
+                                            transformA.R.Col2.X * manifold.LocalPoint.Y;
+
+                        float planePointy = transformA.Position.Y + transformA.R.Col1.Y * manifold.LocalPoint.X +
+                                            transformA.R.Col2.Y * manifold.LocalPoint.Y;
+
+                        for (int i = 0; i < manifold.PointCount; ++i)
+                        {
+                            Vector2 tmp = manifold.Points[i].LocalPoint;
+
+                            float clipPointx = transformB.Position.X + transformB.R.Col1.X * tmp.X +
+                                               transformB.R.Col2.X * tmp.Y;
+
+                            float clipPointy = transformB.Position.Y + transformB.R.Col1.Y * tmp.X +
+                                               transformB.R.Col2.Y * tmp.Y;
+
+                            float value = (clipPointx - planePointx) * normal.X + (clipPointy - planePointy) * normal.Y;
+
+                            Vector2 c = Vector2.Zero;
+                            c.X = (clipPointx + (radiusA - value) * normal.X) + (clipPointx - radiusB * normal.X);
+                            c.Y = (clipPointy + (radiusA - value) * normal.Y) + (clipPointy - radiusB * normal.Y);
+
+                            points[i] = 0.5f * c;
+                        }
+                    }
+                    break;
+
+                case ManifoldType.FaceB:
+                    {
+                        normal.X = transformB.R.Col1.X * manifold.LocalNormal.X +
+                                   transformB.R.Col2.X * manifold.LocalNormal.Y;
+                        normal.Y = transformB.R.Col1.Y * manifold.LocalNormal.X +
+                                   transformB.R.Col2.Y * manifold.LocalNormal.Y;
+
+                        float planePointx = transformB.Position.X + transformB.R.Col1.X * manifold.LocalPoint.X +
+                                            transformB.R.Col2.X * manifold.LocalPoint.Y;
+
+                        float planePointy = transformB.Position.Y + transformB.R.Col1.Y * manifold.LocalPoint.X +
+                                            transformB.R.Col2.Y * manifold.LocalPoint.Y;
+
+                        for (int i = 0; i < manifold.PointCount; ++i)
+                        {
+                            Vector2 tmp = manifold.Points[i].LocalPoint;
+
+                            float clipPointx = transformA.Position.X + transformA.R.Col1.X * tmp.X +
+                                               transformA.R.Col2.X * tmp.Y;
+
+                            float clipPointy = transformA.Position.Y + transformA.R.Col1.Y * tmp.X +
+                                               transformA.R.Col2.Y * tmp.Y;
+
+                            float value = (clipPointx - planePointx) * normal.X + (clipPointy - planePointy) * normal.Y;
+
+                            Vector2 c = Vector2.Zero;
+                            c.X = (clipPointx - radiusA * normal.X) + (clipPointx + (radiusB - value) * normal.X);
+                            c.Y = (clipPointy - radiusA * normal.Y) + (clipPointy + (radiusB - value) * normal.Y);
+
+                            points[i] = 0.5f * c;
+                        }
+                        // Ensure normal points from A to B.
+                        normal *= -1;
+                    }
+                    break;
+                default:
+                    normal = Vector2.UnitY;
+                    break;
+            }
+        }
+
+        public static void GetPointStates(out FixedArray2<PointState> state1, out FixedArray2<PointState> state2,
+                                          ref Manifold manifold1, ref Manifold manifold2)
+        {
+            state1 = new FixedArray2<PointState>();
+            state2 = new FixedArray2<PointState>();
+
+            // Detect persists and removes.
+            for (int i = 0; i < manifold1.PointCount; ++i)
+            {
+                ContactID id = manifold1.Points[i].Id;
+
+                state1[i] = PointState.Remove;
+
+                for (int j = 0; j < manifold2.PointCount; ++j)
+                {
+                    if (manifold2.Points[j].Id.Key == id.Key)
+                    {
+                        state1[i] = PointState.Persist;
+                        break;
+                    }
+                }
+            }
+
+            // Detect persists and adds.
+            for (int i = 0; i < manifold2.PointCount; ++i)
+            {
+                ContactID id = manifold2.Points[i].Id;
+
+                state2[i] = PointState.Add;
+
+                for (int j = 0; j < manifold1.PointCount; ++j)
+                {
+                    if (manifold1.Points[j].Id.Key == id.Key)
+                    {
+                        state2[i] = PointState.Persist;
+                        break;
+                    }
+                }
+            }
+        }
+
+
+        /// Compute the collision manifold between two circles.
+        public static void CollideCircles(ref Manifold manifold,
+                                          CircleShape circleA, ref Transform xfA,
+                                          CircleShape circleB, ref Transform xfB)
+        {
+            manifold.PointCount = 0;
+
+            float pAx = xfA.Position.X + xfA.R.Col1.X * circleA.Position.X + xfA.R.Col2.X * circleA.Position.Y;
+            float pAy = xfA.Position.Y + xfA.R.Col1.Y * circleA.Position.X + xfA.R.Col2.Y * circleA.Position.Y;
+            float pBx = xfB.Position.X + xfB.R.Col1.X * circleB.Position.X + xfB.R.Col2.X * circleB.Position.Y;
+            float pBy = xfB.Position.Y + xfB.R.Col1.Y * circleB.Position.X + xfB.R.Col2.Y * circleB.Position.Y;
+
+            float distSqr = (pBx - pAx) * (pBx - pAx) + (pBy - pAy) * (pBy - pAy);
+            float radius = circleA.Radius + circleB.Radius;
+            if (distSqr > radius * radius)
+            {
+                return;
+            }
+
+            manifold.Type = ManifoldType.Circles;
+            manifold.LocalPoint = circleA.Position;
+            manifold.LocalNormal = Vector2.Zero;
+            manifold.PointCount = 1;
+
+            ManifoldPoint p0 = manifold.Points[0];
+
+            p0.LocalPoint = circleB.Position;
+            p0.Id.Key = 0;
+
+            manifold.Points[0] = p0;
+        }
+
+        /// <summary>
+        /// Compute the collision manifold between a polygon and a circle.
+        /// </summary>
+        /// <param name="manifold">The manifold.</param>
+        /// <param name="polygonA">The polygon A.</param>
+        /// <param name="transformA">The transform of A.</param>
+        /// <param name="circleB">The circle B.</param>
+        /// <param name="transformB">The transform of B.</param>
+        public static void CollidePolygonAndCircle(ref Manifold manifold,
+                                                   PolygonShape polygonA, ref Transform transformA,
+                                                   CircleShape circleB, ref Transform transformB)
+        {
+            manifold.PointCount = 0;
+
+            // Compute circle position in the frame of the polygon.
+            Vector2 c =
+                new Vector2(
+                    transformB.Position.X + transformB.R.Col1.X * circleB.Position.X +
+                    transformB.R.Col2.X * circleB.Position.Y,
+                    transformB.Position.Y + transformB.R.Col1.Y * circleB.Position.X +
+                    transformB.R.Col2.Y * circleB.Position.Y);
+            Vector2 cLocal =
+                new Vector2(
+                    (c.X - transformA.Position.X) * transformA.R.Col1.X +
+                    (c.Y - transformA.Position.Y) * transformA.R.Col1.Y,
+                    (c.X - transformA.Position.X) * transformA.R.Col2.X +
+                    (c.Y - transformA.Position.Y) * transformA.R.Col2.Y);
+
+            // Find the min separating edge.
+            int normalIndex = 0;
+            float separation = -Settings.MaxFloat;
+            float radius = polygonA.Radius + circleB.Radius;
+            int vertexCount = polygonA.Vertices.Count;
+
+            for (int i = 0; i < vertexCount; ++i)
+            {
+                Vector2 value1 = polygonA.Normals[i];
+                Vector2 value2 = cLocal - polygonA.Vertices[i];
+                float s = value1.X * value2.X + value1.Y * value2.Y;
+
+                if (s > radius)
+                {
+                    // Early out.
+                    return;
+                }
+
+                if (s > separation)
+                {
+                    separation = s;
+                    normalIndex = i;
+                }
+            }
+
+            // Vertices that subtend the incident face.
+            int vertIndex1 = normalIndex;
+            int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
+            Vector2 v1 = polygonA.Vertices[vertIndex1];
+            Vector2 v2 = polygonA.Vertices[vertIndex2];
+
+            // If the center is inside the polygon ...
+            if (separation < Settings.Epsilon)
+            {
+                manifold.PointCount = 1;
+                manifold.Type = ManifoldType.FaceA;
+                manifold.LocalNormal = polygonA.Normals[normalIndex];
+                manifold.LocalPoint = 0.5f * (v1 + v2);
+
+                ManifoldPoint p0 = manifold.Points[0];
+
+                p0.LocalPoint = circleB.Position;
+                p0.Id.Key = 0;
+
+                manifold.Points[0] = p0;
+
+                return;
+            }
+
+            // Compute barycentric coordinates
+            float u1 = (cLocal.X - v1.X) * (v2.X - v1.X) + (cLocal.Y - v1.Y) * (v2.Y - v1.Y);
+            float u2 = (cLocal.X - v2.X) * (v1.X - v2.X) + (cLocal.Y - v2.Y) * (v1.Y - v2.Y);
+
+            if (u1 <= 0.0f)
+            {
+                float r = (cLocal.X - v1.X) * (cLocal.X - v1.X) + (cLocal.Y - v1.Y) * (cLocal.Y - v1.Y);
+                if (r > radius * radius)
+                {
+                    return;
+                }
+
+                manifold.PointCount = 1;
+                manifold.Type = ManifoldType.FaceA;
+                manifold.LocalNormal = cLocal - v1;
+                float factor = 1f /
+                               (float)
+                               Math.Sqrt(manifold.LocalNormal.X * manifold.LocalNormal.X +
+                                         manifold.LocalNormal.Y * manifold.LocalNormal.Y);
+                manifold.LocalNormal.X = manifold.LocalNormal.X * factor;
+                manifold.LocalNormal.Y = manifold.LocalNormal.Y * factor;
+                manifold.LocalPoint = v1;
+
+                ManifoldPoint p0b = manifold.Points[0];
+
+                p0b.LocalPoint = circleB.Position;
+                p0b.Id.Key = 0;
+
+                manifold.Points[0] = p0b;
+            }
+            else if (u2 <= 0.0f)
+            {
+                float r = (cLocal.X - v2.X) * (cLocal.X - v2.X) + (cLocal.Y - v2.Y) * (cLocal.Y - v2.Y);
+                if (r > radius * radius)
+                {
+                    return;
+                }
+
+                manifold.PointCount = 1;
+                manifold.Type = ManifoldType.FaceA;
+                manifold.LocalNormal = cLocal - v2;
+                float factor = 1f /
+                               (float)
+                               Math.Sqrt(manifold.LocalNormal.X * manifold.LocalNormal.X +
+                                         manifold.LocalNormal.Y * manifold.LocalNormal.Y);
+                manifold.LocalNormal.X = manifold.LocalNormal.X * factor;
+                manifold.LocalNormal.Y = manifold.LocalNormal.Y * factor;
+                manifold.LocalPoint = v2;
+
+                ManifoldPoint p0c = manifold.Points[0];
+
+                p0c.LocalPoint = circleB.Position;
+                p0c.Id.Key = 0;
+
+                manifold.Points[0] = p0c;
+            }
+            else
+            {
+                Vector2 faceCenter = 0.5f * (v1 + v2);
+                Vector2 value1 = cLocal - faceCenter;
+                Vector2 value2 = polygonA.Normals[vertIndex1];
+                float separation2 = value1.X * value2.X + value1.Y * value2.Y;
+                if (separation2 > radius)
+                {
+                    return;
+                }
+
+                manifold.PointCount = 1;
+                manifold.Type = ManifoldType.FaceA;
+                manifold.LocalNormal = polygonA.Normals[vertIndex1];
+                manifold.LocalPoint = faceCenter;
+
+                ManifoldPoint p0d = manifold.Points[0];
+
+                p0d.LocalPoint = circleB.Position;
+                p0d.Id.Key = 0;
+
+                manifold.Points[0] = p0d;
+            }
+        }
+
+        /// <summary>
+        /// Compute the collision manifold between two polygons.
+        /// </summary>
+        /// <param name="manifold">The manifold.</param>
+        /// <param name="polyA">The poly A.</param>
+        /// <param name="transformA">The transform A.</param>
+        /// <param name="polyB">The poly B.</param>
+        /// <param name="transformB">The transform B.</param>
+        public static void CollidePolygons(ref Manifold manifold,
+                                           PolygonShape polyA, ref Transform transformA,
+                                           PolygonShape polyB, ref Transform transformB)
+        {
+            manifold.PointCount = 0;
+            float totalRadius = polyA.Radius + polyB.Radius;
+
+            int edgeA = 0;
+            float separationA = FindMaxSeparation(out edgeA, polyA, ref transformA, polyB, ref transformB);
+            if (separationA > totalRadius)
+                return;
+
+            int edgeB = 0;
+            float separationB = FindMaxSeparation(out edgeB, polyB, ref transformB, polyA, ref transformA);
+            if (separationB > totalRadius)
+                return;
+
+            PolygonShape poly1; // reference polygon
+            PolygonShape poly2; // incident polygon
+            Transform xf1, xf2;
+            int edge1; // reference edge
+            bool flip;
+            const float k_relativeTol = 0.98f;
+            const float k_absoluteTol = 0.001f;
+
+            if (separationB > k_relativeTol * separationA + k_absoluteTol)
+            {
+                poly1 = polyB;
+                poly2 = polyA;
+                xf1 = transformB;
+                xf2 = transformA;
+                edge1 = edgeB;
+                manifold.Type = ManifoldType.FaceB;
+                flip = true;
+            }
+            else
+            {
+                poly1 = polyA;
+                poly2 = polyB;
+                xf1 = transformA;
+                xf2 = transformB;
+                edge1 = edgeA;
+                manifold.Type = ManifoldType.FaceA;
+                flip = false;
+            }
+
+            FixedArray2<ClipVertex> incidentEdge;
+            FindIncidentEdge(out incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2);
+
+            int count1 = poly1.Vertices.Count;
+
+            int iv1 = edge1;
+            int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
+
+            Vector2 v11 = poly1.Vertices[iv1];
+            Vector2 v12 = poly1.Vertices[iv2];
+
+            float localTangentX = v12.X - v11.X;
+            float localTangentY = v12.Y - v11.Y;
+
+            float factor = 1f / (float)Math.Sqrt(localTangentX * localTangentX + localTangentY * localTangentY);
+            localTangentX = localTangentX * factor;
+            localTangentY = localTangentY * factor;
+
+            Vector2 localNormal = new Vector2(localTangentY, -localTangentX);
+            Vector2 planePoint = 0.5f * (v11 + v12);
+
+            Vector2 tangent = new Vector2(xf1.R.Col1.X * localTangentX + xf1.R.Col2.X * localTangentY,
+                                          xf1.R.Col1.Y * localTangentX + xf1.R.Col2.Y * localTangentY);
+            float normalx = tangent.Y;
+            float normaly = -tangent.X;
+
+            v11 = new Vector2(xf1.Position.X + xf1.R.Col1.X * v11.X + xf1.R.Col2.X * v11.Y,
+                              xf1.Position.Y + xf1.R.Col1.Y * v11.X + xf1.R.Col2.Y * v11.Y);
+            v12 = new Vector2(xf1.Position.X + xf1.R.Col1.X * v12.X + xf1.R.Col2.X * v12.Y,
+                              xf1.Position.Y + xf1.R.Col1.Y * v12.X + xf1.R.Col2.Y * v12.Y);
+
+            // Face offset.
+            float frontOffset = normalx * v11.X + normaly * v11.Y;
+
+            // Side offsets, extended by polytope skin thickness.
+            float sideOffset1 = -(tangent.X * v11.X + tangent.Y * v11.Y) + totalRadius;
+            float sideOffset2 = tangent.X * v12.X + tangent.Y * v12.Y + totalRadius;
+
+            // Clip incident edge against extruded edge1 side edges.
+            FixedArray2<ClipVertex> clipPoints1;
+            FixedArray2<ClipVertex> clipPoints2;
+
+            // Clip to box side 1
+            int np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1, iv1);
+
+            if (np < 2)
+                return;
+
+            // Clip to negative box side 1
+            np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2, iv2);
+
+            if (np < 2)
+            {
+                return;
+            }
+
+            // Now clipPoints2 contains the clipped points.
+            manifold.LocalNormal = localNormal;
+            manifold.LocalPoint = planePoint;
+
+            int pointCount = 0;
+            for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
+            {
+                Vector2 value = clipPoints2[i].V;
+                float separation = normalx * value.X + normaly * value.Y - frontOffset;
+
+                if (separation <= totalRadius)
+                {
+                    ManifoldPoint cp = manifold.Points[pointCount];
+                    Vector2 tmp = clipPoints2[i].V;
+                    float tmp1X = tmp.X - xf2.Position.X;
+                    float tmp1Y = tmp.Y - xf2.Position.Y;
+                    cp.LocalPoint.X = tmp1X * xf2.R.Col1.X + tmp1Y * xf2.R.Col1.Y;
+                    cp.LocalPoint.Y = tmp1X * xf2.R.Col2.X + tmp1Y * xf2.R.Col2.Y;
+                    cp.Id = clipPoints2[i].ID;
+
+                    if (flip)
+                    {
+                        // Swap features
+                        ContactFeature cf = cp.Id.Features;
+                        cp.Id.Features.IndexA = cf.IndexB;
+                        cp.Id.Features.IndexB = cf.IndexA;
+                        cp.Id.Features.TypeA = cf.TypeB;
+                        cp.Id.Features.TypeB = cf.TypeA;
+                    }
+
+                    manifold.Points[pointCount] = cp;
+
+                    ++pointCount;
+                }
+            }
+
+            manifold.PointCount = pointCount;
+        }
+
+        /// <summary>
+        /// Compute contact points for edge versus circle.
+        /// This accounts for edge connectivity.
+        /// </summary>
+        /// <param name="manifold">The manifold.</param>
+        /// <param name="edgeA">The edge A.</param>
+        /// <param name="transformA">The transform A.</param>
+        /// <param name="circleB">The circle B.</param>
+        /// <param name="transformB">The transform B.</param>
+        public static void CollideEdgeAndCircle(ref Manifold manifold,
+                                                EdgeShape edgeA, ref Transform transformA,
+                                                CircleShape circleB, ref Transform transformB)
+        {
+            manifold.PointCount = 0;
+
+            // Compute circle in frame of edge
+            Vector2 Q = MathUtils.MultiplyT(ref transformA, MathUtils.Multiply(ref transformB, ref circleB._position));
+
+            Vector2 A = edgeA.Vertex1, B = edgeA.Vertex2;
+            Vector2 e = B - A;
+
+            // Barycentric coordinates
+            float u = Vector2.Dot(e, B - Q);
+            float v = Vector2.Dot(e, Q - A);
+
+            float radius = edgeA.Radius + circleB.Radius;
+
+            ContactFeature cf;
+            cf.IndexB = 0;
+            cf.TypeB = (byte)ContactFeatureType.Vertex;
+
+            Vector2 P, d;
+
+            // Region A
+            if (v <= 0.0f)
+            {
+                P = A;
+                d = Q - P;
+                float dd;
+                Vector2.Dot(ref d, ref d, out dd);
+                if (dd > radius * radius)
+                {
+                    return;
+                }
+
+                // Is there an edge connected to A?
+                if (edgeA.HasVertex0)
+                {
+                    Vector2 A1 = edgeA.Vertex0;
+                    Vector2 B1 = A;
+                    Vector2 e1 = B1 - A1;
+                    float u1 = Vector2.Dot(e1, B1 - Q);
+
+                    // Is the circle in Region AB of the previous edge?
+                    if (u1 > 0.0f)
+                    {
+                        return;
+                    }
+                }
+
+                cf.IndexA = 0;
+                cf.TypeA = (byte)ContactFeatureType.Vertex;
+                manifold.PointCount = 1;
+                manifold.Type = ManifoldType.Circles;
+                manifold.LocalNormal = Vector2.Zero;
+                manifold.LocalPoint = P;
+                ManifoldPoint mp = new ManifoldPoint();
+                mp.Id.Key = 0;
+                mp.Id.Features = cf;
+                mp.LocalPoint = circleB.Position;
+                manifold.Points[0] = mp;
+                return;
+            }
+
+            // Region B
+            if (u <= 0.0f)
+            {
+                P = B;
+                d = Q - P;
+                float dd;
+                Vector2.Dot(ref d, ref d, out dd);
+                if (dd > radius * radius)
+                {
+                    return;
+                }
+
+                // Is there an edge connected to B?
+                if (edgeA.HasVertex3)
+                {
+                    Vector2 B2 = edgeA.Vertex3;
+                    Vector2 A2 = B;
+                    Vector2 e2 = B2 - A2;
+                    float v2 = Vector2.Dot(e2, Q - A2);
+
+                    // Is the circle in Region AB of the next edge?
+                    if (v2 > 0.0f)
+                    {
+                        return;
+                    }
+                }
+
+                cf.IndexA = 1;
+                cf.TypeA = (byte)ContactFeatureType.Vertex;
+                manifold.PointCount = 1;
+                manifold.Type = ManifoldType.Circles;
+                manifold.LocalNormal = Vector2.Zero;
+                manifold.LocalPoint = P;
+                ManifoldPoint mp = new ManifoldPoint();
+                mp.Id.Key = 0;
+                mp.Id.Features = cf;
+                mp.LocalPoint = circleB.Position;
+                manifold.Points[0] = mp;
+                return;
+            }
+
+            // Region AB
+            float den;
+            Vector2.Dot(ref e, ref e, out den);
+            Debug.Assert(den > 0.0f);
+            P = (1.0f / den) * (u * A + v * B);
+            d = Q - P;
+            float dd2;
+            Vector2.Dot(ref d, ref d, out dd2);
+            if (dd2 > radius * radius)
+            {
+                return;
+            }
+
+            Vector2 n = new Vector2(-e.Y, e.X);
+            if (Vector2.Dot(n, Q - A) < 0.0f)
+            {
+                n = new Vector2(-n.X, -n.Y);
+            }
+            n.Normalize();
+
+            cf.IndexA = 0;
+            cf.TypeA = (byte)ContactFeatureType.Face;
+            manifold.PointCount = 1;
+            manifold.Type = ManifoldType.FaceA;
+            manifold.LocalNormal = n;
+            manifold.LocalPoint = A;
+            ManifoldPoint mp2 = new ManifoldPoint();
+            mp2.Id.Key = 0;
+            mp2.Id.Features = cf;
+            mp2.LocalPoint = circleB.Position;
+            manifold.Points[0] = mp2;
+        }
+
+        /// <summary>
+        /// Collides and edge and a polygon, taking into account edge adjacency.
+        /// </summary>
+        /// <param name="manifold">The manifold.</param>
+        /// <param name="edgeA">The edge A.</param>
+        /// <param name="xfA">The xf A.</param>
+        /// <param name="polygonB">The polygon B.</param>
+        /// <param name="xfB">The xf B.</param>
+        public static void CollideEdgeAndPolygon(ref Manifold manifold,
+                                                 EdgeShape edgeA, ref Transform xfA,
+                                                 PolygonShape polygonB, ref Transform xfB)
+        {
+            MathUtils.MultiplyT(ref xfA, ref xfB, out _xf);
+
+            // Edge geometry
+            _edgeA.V0 = edgeA.Vertex0;
+            _edgeA.V1 = edgeA.Vertex1;
+            _edgeA.V2 = edgeA.Vertex2;
+            _edgeA.V3 = edgeA.Vertex3;
+            Vector2 e = _edgeA.V2 - _edgeA.V1;
+
+            // Normal points outwards in CCW order.
+            _edgeA.Normal = new Vector2(e.Y, -e.X);
+            _edgeA.Normal.Normalize();
+            _edgeA.HasVertex0 = edgeA.HasVertex0;
+            _edgeA.HasVertex3 = edgeA.HasVertex3;
+
+            // Proxy for edge
+            _proxyA.Vertices[0] = _edgeA.V1;
+            _proxyA.Vertices[1] = _edgeA.V2;
+            _proxyA.Normals[0] = _edgeA.Normal;
+            _proxyA.Normals[1] = -_edgeA.Normal;
+            _proxyA.Centroid = 0.5f * (_edgeA.V1 + _edgeA.V2);
+            _proxyA.Count = 2;
+
+            // Proxy for polygon
+            _proxyB.Count = polygonB.Vertices.Count;
+            _proxyB.Centroid = MathUtils.Multiply(ref _xf, ref polygonB.MassData.Centroid);
+            for (int i = 0; i < polygonB.Vertices.Count; ++i)
+            {
+                _proxyB.Vertices[i] = MathUtils.Multiply(ref _xf, polygonB.Vertices[i]);
+                _proxyB.Normals[i] = MathUtils.Multiply(ref _xf.R, polygonB.Normals[i]);
+            }
+
+            _radius = 2.0f * Settings.PolygonRadius;
+
+            _limit11 = Vector2.Zero;
+            _limit12 = Vector2.Zero;
+            _limit21 = Vector2.Zero;
+            _limit22 = Vector2.Zero;
+
+            //Collide(ref manifold); inline start
+            manifold.PointCount = 0;
+
+            //ComputeAdjacency(); inline start
+            Vector2 v0 = _edgeA.V0;
+            Vector2 v1 = _edgeA.V1;
+            Vector2 v2 = _edgeA.V2;
+            Vector2 v3 = _edgeA.V3;
+
+            // Determine allowable the normal regions based on adjacency.
+            // Note: it may be possible that no normal is admissable.
+            Vector2 centerB = _proxyB.Centroid;
+            if (_edgeA.HasVertex0)
+            {
+                Vector2 e0 = v1 - v0;
+                Vector2 e1 = v2 - v1;
+                Vector2 n0 = new Vector2(e0.Y, -e0.X);
+                Vector2 n1 = new Vector2(e1.Y, -e1.X);
+                n0.Normalize();
+                n1.Normalize();
+
+                bool convex = MathUtils.Cross(n0, n1) >= 0.0f;
+                bool front0 = Vector2.Dot(n0, centerB - v0) >= 0.0f;
+                bool front1 = Vector2.Dot(n1, centerB - v1) >= 0.0f;
+
+                if (convex)
+                {
+                    if (front0 || front1)
+                    {
+                        _limit11 = n1;
+                        _limit12 = n0;
+                    }
+                    else
+                    {
+                        _limit11 = -n1;
+                        _limit12 = -n0;
+                    }
+                }
+                else
+                {
+                    if (front0 && front1)
+                    {
+                        _limit11 = n0;
+                        _limit12 = n1;
+                    }
+                    else
+                    {
+                        _limit11 = -n0;
+                        _limit12 = -n1;
+                    }
+                }
+            }
+            else
+            {
+                _limit11 = Vector2.Zero;
+                _limit12 = Vector2.Zero;
+            }
+
+            if (_edgeA.HasVertex3)
+            {
+                Vector2 e1 = v2 - v1;
+                Vector2 e2 = v3 - v2;
+                Vector2 n1 = new Vector2(e1.Y, -e1.X);
+                Vector2 n2 = new Vector2(e2.Y, -e2.X);
+                n1.Normalize();
+                n2.Normalize();
+
+                bool convex = MathUtils.Cross(n1, n2) >= 0.0f;
+                bool front1 = Vector2.Dot(n1, centerB - v1) >= 0.0f;
+                bool front2 = Vector2.Dot(n2, centerB - v2) >= 0.0f;
+
+                if (convex)
+                {
+                    if (front1 || front2)
+                    {
+                        _limit21 = n2;
+                        _limit22 = n1;
+                    }
+                    else
+                    {
+                        _limit21 = -n2;
+                        _limit22 = -n1;
+                    }
+                }
+                else
+                {
+                    if (front1 && front2)
+                    {
+                        _limit21 = n1;
+                        _limit22 = n2;
+                    }
+                    else
+                    {
+                        _limit21 = -n1;
+                        _limit22 = -n2;
+                    }
+                }
+            }
+            else
+            {
+                _limit21 = Vector2.Zero;
+                _limit22 = Vector2.Zero;
+            }
+
+            //ComputeAdjacency(); inline end
+
+            //EPAxis edgeAxis = ComputeEdgeSeparation(); inline start
+            EPAxis edgeAxis = ComputeEdgeSeparation();
+
+            // If no valid normal can be found than this edge should not collide.
+            // This can happen on the middle edge of a 3-edge zig-zag chain.
+            if (edgeAxis.Type == EPAxisType.Unknown)
+            {
+                return;
+            }
+
+            if (edgeAxis.Separation > _radius)
+            {
+                return;
+            }
+
+            EPAxis polygonAxis = ComputePolygonSeparation();
+            if (polygonAxis.Type != EPAxisType.Unknown && polygonAxis.Separation > _radius)
+            {
+                return;
+            }
+
+            // Use hysteresis for jitter reduction.
+            const float k_relativeTol = 0.98f;
+            const float k_absoluteTol = 0.001f;
+
+            EPAxis primaryAxis;
+            if (polygonAxis.Type == EPAxisType.Unknown)
+            {
+                primaryAxis = edgeAxis;
+            }
+            else if (polygonAxis.Separation > k_relativeTol * edgeAxis.Separation + k_absoluteTol)
+            {
+                primaryAxis = polygonAxis;
+            }
+            else
+            {
+                primaryAxis = edgeAxis;
+            }
+
+            EPProxy proxy1;
+            EPProxy proxy2;
+            FixedArray2<ClipVertex> incidentEdge = new FixedArray2<ClipVertex>();
+            if (primaryAxis.Type == EPAxisType.EdgeA)
+            {
+                proxy1 = _proxyA;
+                proxy2 = _proxyB;
+                manifold.Type = ManifoldType.FaceA;
+            }
+            else
+            {
+                proxy1 = _proxyB;
+                proxy2 = _proxyA;
+                manifold.Type = ManifoldType.FaceB;
+            }
+
+            int edge1 = primaryAxis.Index;
+
+            FindIncidentEdge(ref incidentEdge, proxy1, primaryAxis.Index, proxy2);
+            int count1 = proxy1.Count;
+
+            int iv1 = edge1;
+            int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
+
+            Vector2 v11 = proxy1.Vertices[iv1];
+            Vector2 v12 = proxy1.Vertices[iv2];
+
+            Vector2 tangent = v12 - v11;
+            tangent.Normalize();
+
+            Vector2 normal = MathUtils.Cross(tangent, 1.0f);
+            Vector2 planePoint = 0.5f * (v11 + v12);
+
+            // Face offset.
+            float frontOffset = Vector2.Dot(normal, v11);
+
+            // Side offsets, extended by polytope skin thickness.
+            float sideOffset1 = -Vector2.Dot(tangent, v11) + _radius;
+            float sideOffset2 = Vector2.Dot(tangent, v12) + _radius;
+
+            // Clip incident edge against extruded edge1 side edges.
+            FixedArray2<ClipVertex> clipPoints1;
+            FixedArray2<ClipVertex> clipPoints2;
+            int np;
+
+            // Clip to box side 1
+            np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1, iv1);
+
+            if (np < Settings.MaxManifoldPoints)
+            {
+                return;
+            }
+
+            // Clip to negative box side 1
+            np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2, iv2);
+
+            if (np < Settings.MaxManifoldPoints)
+            {
+                return;
+            }
+
+            // Now clipPoints2 contains the clipped points.
+            if (primaryAxis.Type == EPAxisType.EdgeA)
+            {
+                manifold.LocalNormal = normal;
+                manifold.LocalPoint = planePoint;
+            }
+            else
+            {
+                manifold.LocalNormal = MathUtils.MultiplyT(ref _xf.R, ref normal);
+                manifold.LocalPoint = MathUtils.MultiplyT(ref _xf, ref planePoint);
+            }
+
+            int pointCount = 0;
+            for (int i1 = 0; i1 < Settings.MaxManifoldPoints; ++i1)
+            {
+                float separation = Vector2.Dot(normal, clipPoints2[i1].V) - frontOffset;
+
+                if (separation <= _radius)
+                {
+                    ManifoldPoint cp = manifold.Points[pointCount];
+
+                    if (primaryAxis.Type == EPAxisType.EdgeA)
+                    {
+                        cp.LocalPoint = MathUtils.MultiplyT(ref _xf, clipPoints2[i1].V);
+                        cp.Id = clipPoints2[i1].ID;
+                    }
+                    else
+                    {
+                        cp.LocalPoint = clipPoints2[i1].V;
+                        cp.Id.Features.TypeA = clipPoints2[i1].ID.Features.TypeB;
+                        cp.Id.Features.TypeB = clipPoints2[i1].ID.Features.TypeA;
+                        cp.Id.Features.IndexA = clipPoints2[i1].ID.Features.IndexB;
+                        cp.Id.Features.IndexB = clipPoints2[i1].ID.Features.IndexA;
+                    }
+
+                    manifold.Points[pointCount] = cp;
+
+                    ++pointCount;
+                }
+            }
+
+            manifold.PointCount = pointCount;
+
+            //Collide(ref manifold); inline end
+        }
+
+        private static EPAxis ComputeEdgeSeparation()
+        {
+            // EdgeA separation
+            EPAxis bestAxis;
+            bestAxis.Type = EPAxisType.Unknown;
+            bestAxis.Index = -1;
+            bestAxis.Separation = -Settings.MaxFloat;
+            _tmpNormals[0] = _edgeA.Normal;
+            _tmpNormals[1] = -_edgeA.Normal;
+
+            for (int i = 0; i < 2; ++i)
+            {
+                Vector2 n = _tmpNormals[i];
+
+                // Adjacency
+                bool valid1 = MathUtils.Cross(n, _limit11) >= -Settings.AngularSlop &&
+                              MathUtils.Cross(_limit12, n) >= -Settings.AngularSlop;
+                bool valid2 = MathUtils.Cross(n, _limit21) >= -Settings.AngularSlop &&
+                              MathUtils.Cross(_limit22, n) >= -Settings.AngularSlop;
+
+                if (valid1 == false || valid2 == false)
+                {
+                    continue;
+                }
+
+                EPAxis axis;
+                axis.Type = EPAxisType.EdgeA;
+                axis.Index = i;
+                axis.Separation = Settings.MaxFloat;
+
+                for (int j = 0; j < _proxyB.Count; ++j)
+                {
+                    float s = Vector2.Dot(n, _proxyB.Vertices[j] - _edgeA.V1);
+                    if (s < axis.Separation)
+                    {
+                        axis.Separation = s;
+                    }
+                }
+
+                if (axis.Separation > _radius)
+                {
+                    return axis;
+                }
+
+                if (axis.Separation > bestAxis.Separation)
+                {
+                    bestAxis = axis;
+                }
+            }
+
+            return bestAxis;
+        }
+
+        private static EPAxis ComputePolygonSeparation()
+        {
+            EPAxis axis;
+            axis.Type = EPAxisType.Unknown;
+            axis.Index = -1;
+            axis.Separation = -Settings.MaxFloat;
+            for (int i = 0; i < _proxyB.Count; ++i)
+            {
+                Vector2 n = -_proxyB.Normals[i];
+
+                // Adjacency
+                bool valid1 = MathUtils.Cross(n, _limit11) >= -Settings.AngularSlop &&
+                              MathUtils.Cross(_limit12, n) >= -Settings.AngularSlop;
+                bool valid2 = MathUtils.Cross(n, _limit21) >= -Settings.AngularSlop &&
+                              MathUtils.Cross(_limit22, n) >= -Settings.AngularSlop;
+
+                if (valid1 == false && valid2 == false)
+                {
+                    continue;
+                }
+
+                float s1 = Vector2.Dot(n, _proxyB.Vertices[i] - _edgeA.V1);
+                float s2 = Vector2.Dot(n, _proxyB.Vertices[i] - _edgeA.V2);
+                float s = Math.Min(s1, s2);
+
+                if (s > _radius)
+                {
+                    axis.Type = EPAxisType.EdgeB;
+                    axis.Index = i;
+                    axis.Separation = s;
+                }
+
+                if (s > axis.Separation)
+                {
+                    axis.Type = EPAxisType.EdgeB;
+                    axis.Index = i;
+                    axis.Separation = s;
+                }
+            }
+
+            return axis;
+        }
+
+        private static void FindIncidentEdge(ref FixedArray2<ClipVertex> c, EPProxy proxy1, int edge1, EPProxy proxy2)
+        {
+            int count2 = proxy2.Count;
+
+            Debug.Assert(0 <= edge1 && edge1 < proxy1.Count);
+
+            // Get the normal of the reference edge in proxy2's frame.
+            Vector2 normal1 = proxy1.Normals[edge1];
+
+            // Find the incident edge on proxy2.
+            int index = 0;
+            float minDot = float.MaxValue;
+            for (int i = 0; i < count2; ++i)
+            {
+                float dot = Vector2.Dot(normal1, proxy2.Normals[i]);
+                if (dot < minDot)
+                {
+                    minDot = dot;
+                    index = i;
+                }
+            }
+
+            // Build the clip vertices for the incident edge.
+            int i1 = index;
+            int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
+
+            ClipVertex cTemp = new ClipVertex();
+            cTemp.V = proxy2.Vertices[i1];
+            cTemp.ID.Features.IndexA = (byte)edge1;
+            cTemp.ID.Features.IndexB = (byte)i1;
+            cTemp.ID.Features.TypeA = (byte)ContactFeatureType.Face;
+            cTemp.ID.Features.TypeB = (byte)ContactFeatureType.Vertex;
+            c[0] = cTemp;
+
+            cTemp.V = proxy2.Vertices[i2];
+            cTemp.ID.Features.IndexA = (byte)edge1;
+            cTemp.ID.Features.IndexB = (byte)i2;
+            cTemp.ID.Features.TypeA = (byte)ContactFeatureType.Face;
+            cTemp.ID.Features.TypeB = (byte)ContactFeatureType.Vertex;
+            c[1] = cTemp;
+        }
+
+        /// <summary>
+        /// Clipping for contact manifolds.
+        /// </summary>
+        /// <param name="vOut">The v out.</param>
+        /// <param name="vIn">The v in.</param>
+        /// <param name="normal">The normal.</param>
+        /// <param name="offset">The offset.</param>
+        /// <param name="vertexIndexA">The vertex index A.</param>
+        /// <returns></returns>
+        private static int ClipSegmentToLine(out FixedArray2<ClipVertex> vOut, ref FixedArray2<ClipVertex> vIn,
+                                             Vector2 normal, float offset, int vertexIndexA)
+        {
+            vOut = new FixedArray2<ClipVertex>();
+
+            ClipVertex v0 = vIn[0];
+            ClipVertex v1 = vIn[1];
+
+            // Start with no output points
+            int numOut = 0;
+
+            // Calculate the distance of end points to the line
+            float distance0 = normal.X * v0.V.X + normal.Y * v0.V.Y - offset;
+            float distance1 = normal.X * v1.V.X + normal.Y * v1.V.Y - offset;
+
+            // If the points are behind the plane
+            if (distance0 <= 0.0f) vOut[numOut++] = v0;
+            if (distance1 <= 0.0f) vOut[numOut++] = v1;
+
+            // If the points are on different sides of the plane
+            if (distance0 * distance1 < 0.0f)
+            {
+                // Find intersection point of edge and plane
+                float interp = distance0 / (distance0 - distance1);
+
+                ClipVertex cv = vOut[numOut];
+
+                cv.V.X = v0.V.X + interp * (v1.V.X - v0.V.X);
+                cv.V.Y = v0.V.Y + interp * (v1.V.Y - v0.V.Y);
+
+                // VertexA is hitting edgeB.
+                cv.ID.Features.IndexA = (byte)vertexIndexA;
+                cv.ID.Features.IndexB = v0.ID.Features.IndexB;
+                cv.ID.Features.TypeA = (byte)ContactFeatureType.Vertex;
+                cv.ID.Features.TypeB = (byte)ContactFeatureType.Face;
+
+                vOut[numOut] = cv;
+
+                ++numOut;
+            }
+
+            return numOut;
+        }
+
+        /// <summary>
+        /// Find the separation between poly1 and poly2 for a give edge normal on poly1.
+        /// </summary>
+        /// <param name="poly1">The poly1.</param>
+        /// <param name="xf1">The XF1.</param>
+        /// <param name="edge1">The edge1.</param>
+        /// <param name="poly2">The poly2.</param>
+        /// <param name="xf2">The XF2.</param>
+        /// <returns></returns>
+        private static float EdgeSeparation(PolygonShape poly1, ref Transform xf1, int edge1,
+                                            PolygonShape poly2, ref Transform xf2)
+        {
+            int count2 = poly2.Vertices.Count;
+
+            Debug.Assert(0 <= edge1 && edge1 < poly1.Vertices.Count);
+
+            // Convert normal from poly1's frame into poly2's frame.
+            Vector2 p1n = poly1.Normals[edge1];
+
+            float normalWorldx = xf1.R.Col1.X * p1n.X + xf1.R.Col2.X * p1n.Y;
+            float normalWorldy = xf1.R.Col1.Y * p1n.X + xf1.R.Col2.Y * p1n.Y;
+
+            Vector2 normal = new Vector2(normalWorldx * xf2.R.Col1.X + normalWorldy * xf2.R.Col1.Y,
+                                         normalWorldx * xf2.R.Col2.X + normalWorldy * xf2.R.Col2.Y);
+
+            // Find support vertex on poly2 for -normal.
+            int index = 0;
+            float minDot = Settings.MaxFloat;
+
+            for (int i = 0; i < count2; ++i)
+            {
+                float dot = Vector2.Dot(poly2.Vertices[i], normal);
+
+                if (dot < minDot)
+                {
+                    minDot = dot;
+                    index = i;
+                }
+            }
+
+            Vector2 p1ve = poly1.Vertices[edge1];
+            Vector2 p2vi = poly2.Vertices[index];
+
+            return ((xf2.Position.X + xf2.R.Col1.X * p2vi.X + xf2.R.Col2.X * p2vi.Y) -
+                    (xf1.Position.X + xf1.R.Col1.X * p1ve.X + xf1.R.Col2.X * p1ve.Y)) * normalWorldx +
+                   ((xf2.Position.Y + xf2.R.Col1.Y * p2vi.X + xf2.R.Col2.Y * p2vi.Y) -
+                    (xf1.Position.Y + xf1.R.Col1.Y * p1ve.X + xf1.R.Col2.Y * p1ve.Y)) * normalWorldy;
+        }
+
+        /// <summary>
+        /// Find the max separation between poly1 and poly2 using edge normals from poly1.
+        /// </summary>
+        /// <param name="edgeIndex">Index of the edge.</param>
+        /// <param name="poly1">The poly1.</param>
+        /// <param name="xf1">The XF1.</param>
+        /// <param name="poly2">The poly2.</param>
+        /// <param name="xf2">The XF2.</param>
+        /// <returns></returns>
+        private static float FindMaxSeparation(out int edgeIndex,
+                                               PolygonShape poly1, ref Transform xf1,
+                                               PolygonShape poly2, ref Transform xf2)
+        {
+            int count1 = poly1.Vertices.Count;
+
+            // Vector pointing from the centroid of poly1 to the centroid of poly2.
+            float dx = (xf2.Position.X + xf2.R.Col1.X * poly2.MassData.Centroid.X +
+                        xf2.R.Col2.X * poly2.MassData.Centroid.Y) -
+                       (xf1.Position.X + xf1.R.Col1.X * poly1.MassData.Centroid.X +
+                        xf1.R.Col2.X * poly1.MassData.Centroid.Y);
+            float dy = (xf2.Position.Y + xf2.R.Col1.Y * poly2.MassData.Centroid.X +
+                        xf2.R.Col2.Y * poly2.MassData.Centroid.Y) -
+                       (xf1.Position.Y + xf1.R.Col1.Y * poly1.MassData.Centroid.X +
+                        xf1.R.Col2.Y * poly1.MassData.Centroid.Y);
+            Vector2 dLocal1 = new Vector2(dx * xf1.R.Col1.X + dy * xf1.R.Col1.Y, dx * xf1.R.Col2.X + dy * xf1.R.Col2.Y);
+
+            // Find edge normal on poly1 that has the largest projection onto d.
+            int edge = 0;
+            float maxDot = -Settings.MaxFloat;
+            for (int i = 0; i < count1; ++i)
+            {
+                float dot = Vector2.Dot(poly1.Normals[i], dLocal1);
+                if (dot > maxDot)
+                {
+                    maxDot = dot;
+                    edge = i;
+                }
+            }
+
+            // Get the separation for the edge normal.
+            float s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2);
+
+            // Check the separation for the previous edge normal.
+            int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
+            float sPrev = EdgeSeparation(poly1, ref xf1, prevEdge, poly2, ref xf2);
+
+            // Check the separation for the next edge normal.
+            int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
+            float sNext = EdgeSeparation(poly1, ref xf1, nextEdge, poly2, ref xf2);
+
+            // Find the best edge and the search direction.
+            int bestEdge;
+            float bestSeparation;
+            int increment;
+            if (sPrev > s && sPrev > sNext)
+            {
+                increment = -1;
+                bestEdge = prevEdge;
+                bestSeparation = sPrev;
+            }
+            else if (sNext > s)
+            {
+                increment = 1;
+                bestEdge = nextEdge;
+                bestSeparation = sNext;
+            }
+            else
+            {
+                edgeIndex = edge;
+                return s;
+            }
+
+            // Perform a local search for the best edge normal.
+            for (; ; )
+            {
+                if (increment == -1)
+                    edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
+                else
+                    edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
+
+                s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2);
+
+                if (s > bestSeparation)
+                {
+                    bestEdge = edge;
+                    bestSeparation = s;
+                }
+                else
+                {
+                    break;
+                }
+            }
+
+            edgeIndex = bestEdge;
+            return bestSeparation;
+        }
+
+        private static void FindIncidentEdge(out FixedArray2<ClipVertex> c,
+                                             PolygonShape poly1, ref Transform xf1, int edge1,
+                                             PolygonShape poly2, ref Transform xf2)
+        {
+            c = new FixedArray2<ClipVertex>();
+
+            int count2 = poly2.Vertices.Count;
+
+            Debug.Assert(0 <= edge1 && edge1 < poly1.Vertices.Count);
+
+            // Get the normal of the reference edge in poly2's frame.
+            Vector2 v = poly1.Normals[edge1];
+            float tmpx = xf1.R.Col1.X * v.X + xf1.R.Col2.X * v.Y;
+            float tmpy = xf1.R.Col1.Y * v.X + xf1.R.Col2.Y * v.Y;
+            Vector2 normal1 = new Vector2(tmpx * xf2.R.Col1.X + tmpy * xf2.R.Col1.Y,
+                                          tmpx * xf2.R.Col2.X + tmpy * xf2.R.Col2.Y);
+
+            // Find the incident edge on poly2.
+            int index = 0;
+            float minDot = Settings.MaxFloat;
+            for (int i = 0; i < count2; ++i)
+            {
+                float dot = Vector2.Dot(normal1, poly2.Normals[i]);
+                if (dot < minDot)
+                {
+                    minDot = dot;
+                    index = i;
+                }
+            }
+
+            // Build the clip vertices for the incident edge.
+            int i1 = index;
+            int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
+
+            ClipVertex cv0 = c[0];
+
+            Vector2 v1 = poly2.Vertices[i1];
+            cv0.V.X = xf2.Position.X + xf2.R.Col1.X * v1.X + xf2.R.Col2.X * v1.Y;
+            cv0.V.Y = xf2.Position.Y + xf2.R.Col1.Y * v1.X + xf2.R.Col2.Y * v1.Y;
+            cv0.ID.Features.IndexA = (byte)edge1;
+            cv0.ID.Features.IndexB = (byte)i1;
+            cv0.ID.Features.TypeA = (byte)ContactFeatureType.Face;
+            cv0.ID.Features.TypeB = (byte)ContactFeatureType.Vertex;
+
+            c[0] = cv0;
+
+            ClipVertex cv1 = c[1];
+            Vector2 v2 = poly2.Vertices[i2];
+            cv1.V.X = xf2.Position.X + xf2.R.Col1.X * v2.X + xf2.R.Col2.X * v2.Y;
+            cv1.V.Y = xf2.Position.Y + xf2.R.Col1.Y * v2.X + xf2.R.Col2.Y * v2.Y;
+            cv1.ID.Features.IndexA = (byte)edge1;
+            cv1.ID.Features.IndexB = (byte)i2;
+            cv1.ID.Features.TypeA = (byte)ContactFeatureType.Face;
+            cv1.ID.Features.TypeB = (byte)ContactFeatureType.Vertex;
+
+            c[1] = cv1;
+        }
+    }
+}

FarseerPhysicsEngine/Collision/Distance.cs

@@ -0,0 +1,780 @@
+/*
+* Farseer Physics Engine based on Box2D.XNA port:
+* Copyright (c) 2010 Ian Qvist
+* 
+* Box2D.XNA port of Box2D:
+* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
+*
+* Original source Box2D:
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
+* 
+* This software is provided 'as-is', without any express or implied 
+* warranty.  In no event will the authors be held liable for any damages 
+* arising from the use of this software. 
+* Permission is granted to anyone to use this software for any purpose, 
+* including commercial applications, and to alter it and redistribute it 
+* freely, subject to the following restrictions: 
+* 1. The origin of this software must not be misrepresented; you must not 
+* claim that you wrote the original software. If you use this software 
+* in a product, an acknowledgment in the product documentation would be 
+* appreciated but is not required. 
+* 2. Altered source versions must be plainly marked as such, and must not be 
+* misrepresented as being the original software. 
+* 3. This notice may not be removed or altered from any source distribution. 
+*/
+
+using System;
+using System.Diagnostics;
+using FarseerPhysics.Collision.Shapes;
+using FarseerPhysics.Common;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Collision
+{
+    /// <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;
+                }
+            }
+        }
+    }
+}

FarseerPhysicsEngine/Collision/DynamicTree.cs

@@ -0,0 +1,654 @@
+/*
+* Farseer Physics Engine based on Box2D.XNA port:
+* Copyright (c) 2010 Ian Qvist
+* 
+* Box2D.XNA port of Box2D:
+* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
+*
+* Original source Box2D:
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
+* 
+* This software is provided 'as-is', without any express or implied 
+* warranty.  In no event will the authors be held liable for any damages 
+* arising from the use of this software. 
+* Permission is granted to anyone to use this software for any purpose, 
+* including commercial applications, and to alter it and redistribute it 
+* freely, subject to the following restrictions: 
+* 1. The origin of this software must not be misrepresented; you must not 
+* claim that you wrote the original software. If you use this software 
+* in a product, an acknowledgment in the product documentation would be 
+* appreciated but is not required. 
+* 2. Altered source versions must be plainly marked as such, and must not be 
+* misrepresented as being the original software. 
+* 3. This notice may not be removed or altered from any source distribution. 
+*/
+
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using FarseerPhysics.Common;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Collision
+{
+    /// <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);
+        }
+    }
+}

FarseerPhysicsEngine/Collision/DynamicTreeBroadPhase.cs

@@ -0,0 +1,324 @@
+/*
+* Farseer Physics Engine based on Box2D.XNA port:
+* Copyright (c) 2010 Ian Qvist
+* 
+* Box2D.XNA port of Box2D:
+* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
+*
+* Original source Box2D:
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
+* 
+* This software is provided 'as-is', without any express or implied 
+* warranty.  In no event will the authors be held liable for any damages 
+* arising from the use of this software. 
+* Permission is granted to anyone to use this software for any purpose, 
+* including commercial applications, and to alter it and redistribute it 
+* freely, subject to the following restrictions: 
+* 1. The origin of this software must not be misrepresented; you must not 
+* claim that you wrote the original software. If you use this software 
+* in a product, an acknowledgment in the product documentation would be 
+* appreciated but is not required. 
+* 2. Altered source versions must be plainly marked as such, and must not be 
+* misrepresented as being the original software. 
+* 3. This notice may not be removed or altered from any source distribution. 
+*/
+
+using System;
+using FarseerPhysics.Dynamics;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Collision
+{
+    internal struct Pair : IComparable<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;
+        }
+    }
+}

FarseerPhysicsEngine/Collision/IBroadPhase.cs

@@ -0,0 +1,30 @@
+using System;
+using FarseerPhysics.Dynamics;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Collision
+{
+    public interface IBroadPhase
+    {
+        int ProxyCount { get; }
+        void UpdatePairs(BroadphaseDelegate callback);
+
+        bool TestOverlap(int proxyIdA, int proxyIdB);
+
+        int AddProxy(ref FixtureProxy proxy);
+
+        void RemoveProxy(int proxyId);
+
+        void MoveProxy(int proxyId, ref AABB aabb, Vector2 displacement);
+
+        FixtureProxy GetProxy(int proxyId);
+
+        void TouchProxy(int proxyId);
+
+        void GetFatAABB(int proxyId, out AABB aabb);
+
+        void Query(Func<int, bool> callback, ref AABB aabb);
+
+        void RayCast(Func<RayCastInput, int, float> callback, ref RayCastInput input);
+    }
+}

FarseerPhysicsEngine/Collision/QuadTree.cs

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

FarseerPhysicsEngine/Collision/QuadTreeBroadPhase.cs

@@ -0,0 +1,249 @@
+using System;
+using System.Collections.Generic;
+using FarseerPhysics;
+using FarseerPhysics.Collision;
+using FarseerPhysics.Dynamics;
+using Microsoft.Xna.Framework;
+
+public class QuadTreeBroadPhase : IBroadPhase
+{
+    private const int TreeUpdateThresh = 10000;
+    private int _currID;
+    private Dictionary<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);
+    }
+}

FarseerPhysicsEngine/Collision/Shapes/CircleShape.cs

@@ -0,0 +1,207 @@
+/*
+* Farseer Physics Engine based on Box2D.XNA port:
+* Copyright (c) 2010 Ian Qvist
+* 
+* Box2D.XNA port of Box2D:
+* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
+*
+* Original source Box2D:
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
+* 
+* This software is provided 'as-is', without any express or implied 
+* warranty.  In no event will the authors be held liable for any damages 
+* arising from the use of this software. 
+* Permission is granted to anyone to use this software for any purpose, 
+* including commercial applications, and to alter it and redistribute it 
+* freely, subject to the following restrictions: 
+* 1. The origin of this software must not be misrepresented; you must not 
+* claim that you wrote the original software. If you use this software 
+* in a product, an acknowledgment in the product documentation would be 
+* appreciated but is not required. 
+* 2. Altered source versions must be plainly marked as such, and must not be 
+* misrepresented as being the original software. 
+* 3. This notice may not be removed or altered from any source distribution. 
+*/
+
+using System;
+using FarseerPhysics.Common;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Collision.Shapes
+{
+    public class CircleShape : Shape
+    {
+        internal Vector2 _position;
+
+        public CircleShape(float radius, float density)
+            : base(density)
+        {
+            ShapeType = ShapeType.Circle;
+            _radius = radius;
+            _position = Vector2.Zero;
+            ComputeProperties();
+        }
+
+        internal CircleShape()
+            : base(0)
+        {
+            ShapeType = ShapeType.Circle;
+            _radius = 0.0f;
+            _position = Vector2.Zero;
+        }
+
+        public override int ChildCount
+        {
+            get { return 1; }
+        }
+
+        public Vector2 Position
+        {
+            get { return _position; }
+            set
+            {
+                _position = value;
+                ComputeProperties();
+            }
+        }
+
+        public override Shape Clone()
+        {
+            CircleShape shape = new CircleShape();
+            shape._radius = Radius;
+            shape._density = _density;
+            shape._position = _position;
+            shape.ShapeType = ShapeType;
+            shape.MassData = MassData;
+            return shape;
+        }
+
+        /// <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;
+        }
+    }
+}

FarseerPhysicsEngine/Collision/Shapes/EdgeShape.cs

@@ -0,0 +1,266 @@
+/*
+* Farseer Physics Engine based on Box2D.XNA port:
+* Copyright (c) 2010 Ian Qvist
+* 
+* Box2D.XNA port of Box2D:
+* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
+*
+* Original source Box2D:
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
+* 
+* This software is provided 'as-is', without any express or implied 
+* warranty.  In no event will the authors be held liable for any damages 
+* arising from the use of this software. 
+* Permission is granted to anyone to use this software for any purpose, 
+* including commercial applications, and to alter it and redistribute it 
+* freely, subject to the following restrictions: 
+* 1. The origin of this software must not be misrepresented; you must not 
+* claim that you wrote the original software. If you use this software 
+* in a product, an acknowledgment in the product documentation would be 
+* appreciated but is not required. 
+* 2. Altered source versions must be plainly marked as such, and must not be 
+* misrepresented as being the original software. 
+* 3. This notice may not be removed or altered from any source distribution. 
+*/
+
+using FarseerPhysics.Common;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Collision.Shapes
+{
+    /// <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);
+        }
+    }
+}

FarseerPhysicsEngine/Collision/Shapes/LoopShape.cs

@@ -0,0 +1,186 @@
+/*
+* 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;
+
+            if (Settings.ConserveMemory)
+                Vertices = vertices;
+            else
+                // Copy vertices.
+                Vertices = new Vertices(vertices);
+        }
+
+        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;
+        }
+    }
+}

FarseerPhysicsEngine/Collision/Shapes/PolygonShape.cs

@@ -0,0 +1,550 @@
+/*
+* 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)
+            {
+                clone.Vertices = Vertices;
+                clone.Normals = Normals;
+            }
+            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);
+
+            if (Settings.ConserveMemory)
+                Vertices = vertices;
+            else
+                // Copy vertices.
+                Vertices = new Vertices(vertices);
+
+            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;
+                    }
+                    break;
+                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;
+        }
+    }
+}

FarseerPhysicsEngine/Collision/Shapes/Shape.cs

@@ -0,0 +1,222 @@
+/*
+* Farseer Physics Engine based on Box2D.XNA port:
+* Copyright (c) 2010 Ian Qvist
+* 
+* Box2D.XNA port of Box2D:
+* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
+*
+* Original source Box2D:
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
+* 
+* This software is provided 'as-is', without any express or implied 
+* warranty.  In no event will the authors be held liable for any damages 
+* arising from the use of this software. 
+* Permission is granted to anyone to use this software for any purpose, 
+* including commercial applications, and to alter it and redistribute it 
+* freely, subject to the following restrictions: 
+* 1. The origin of this software must not be misrepresented; you must not 
+* claim that you wrote the original software. If you use this software 
+* in a product, an acknowledgment in the product documentation would be 
+* appreciated but is not required. 
+* 2. Altered source versions must be plainly marked as such, and must not be 
+* misrepresented as being the original software. 
+* 3. This notice may not be removed or altered from any source distribution. 
+*/
+
+using System;
+using FarseerPhysics.Common;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Collision.Shapes
+{
+    /// <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);
+    }
+}

FarseerPhysicsEngine/Collision/TimeOfImpact.cs

@@ -0,0 +1,500 @@
+/*
+* Farseer Physics Engine based on Box2D.XNA port:
+* Copyright (c) 2010 Ian Qvist
+* 
+* Box2D.XNA port of Box2D:
+* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
+*
+* Original source Box2D:
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
+* 
+* This software is provided 'as-is', without any express or implied 
+* warranty.  In no event will the authors be held liable for any damages 
+* arising from the use of this software. 
+* Permission is granted to anyone to use this software for any purpose, 
+* including commercial applications, and to alter it and redistribute it 
+* freely, subject to the following restrictions: 
+* 1. The origin of this software must not be misrepresented; you must not 
+* claim that you wrote the original software. If you use this software 
+* in a product, an acknowledgment in the product documentation would be 
+* appreciated but is not required. 
+* 2. Altered source versions must be plainly marked as such, and must not be 
+* misrepresented as being the original software. 
+* 3. This notice may not be removed or altered from any source distribution. 
+*/
+
+using System;
+using System.Diagnostics;
+using FarseerPhysics.Common;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Collision
+{
+    /// <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);
+        }
+    }
+}

FarseerPhysicsEngine/Common/ConvexHull/ChainHull.cs

@@ -0,0 +1,126 @@
+using System.Collections.Generic;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common.ConvexHull
+{
+    public static class ChainHull
+    {
+        //Andrew's monotone chain 2D convex hull algorithm.
+        //Copyright 2001, softSurfer (www.softsurfer.com)
+
+        /// <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
+    }
+}

FarseerPhysicsEngine/Common/ConvexHull/GiftWrap.cs

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

FarseerPhysicsEngine/Common/ConvexHull/Melkman.cs

@@ -0,0 +1,122 @@
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common.ConvexHull
+{
+    public static class Melkman
+    {
+        //Melkman based convex hull algorithm contributed by Cowdozer
+
+        /// <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;
+        }
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/BayazitDecomposer.cs

@@ -0,0 +1,253 @@
+using System.Collections.Generic;
+using FarseerPhysics.Common.PolygonManipulation;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common.Decomposition
+{
+    //From phed rev 36
+
+    /// <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;
+        }
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/CDT/Delaunay/DelaunayTriangle.cs

@@ -0,0 +1,420 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ * * Neither the name of Poly2Tri nor the names of its contributors may be
+ *   used to endorse or promote products derived from this software without specific
+ *   prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+// Changes from the Java version
+//   attributification
+// Future possibilities
+//   Flattening out the number of indirections
+//     Replacing arrays of 3 with fixed-length arrays?
+//     Replacing bool[3] with a bit array of some sort?
+//     Bundling everything into an AoS mess?
+//     Hardcode them all as ABC ?
+
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using Poly2Tri.Triangulation.Delaunay.Sweep;
+using Poly2Tri.Triangulation.Util;
+
+namespace Poly2Tri.Triangulation.Delaunay
+{
+    public class DelaunayTriangle
+    {
+        /** Neighbor pointers */
+
+        /** Flags to determine if an edge is a Delauney edge */
+        public FixedBitArray3 EdgeIsConstrained;
+
+        /** Flags to determine if an edge is a Constrained edge */
+        public FixedBitArray3 EdgeIsDelaunay;
+        public FixedArray3<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;
+        }
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFront.cs

@@ -0,0 +1,180 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ * * Neither the name of Poly2Tri nor the names of its contributors may be
+ *   used to endorse or promote products derived from this software without specific
+ *   prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+// Changes from the Java version
+//   Removed BST code, but not all artifacts of it
+// Future possibilities
+//   Eliminate Add/RemoveNode ?
+//   Comments comments and more comments!
+
+using System;
+using System.Text;
+
+namespace Poly2Tri.Triangulation.Delaunay.Sweep
+{
+    /**
+     * @author Thomas Åhlen ([email protected])
+     */
+
+    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;
+        }
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFrontNode.cs

@@ -0,0 +1,64 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ * * Neither the name of Poly2Tri nor the names of its contributors may be
+ *   used to endorse or promote products derived from this software without specific
+ *   prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+// Changes from the Java version
+//   Removed getters
+//   Has* turned into attributes
+// Future possibilities
+//   Comments!
+
+namespace Poly2Tri.Triangulation.Delaunay.Sweep
+{
+    public class AdvancingFrontNode
+    {
+        public AdvancingFrontNode Next;
+        public TriangulationPoint Point;
+        public AdvancingFrontNode Prev;
+        public DelaunayTriangle Triangle;
+        public double Value;
+
+        public AdvancingFrontNode(TriangulationPoint point)
+        {
+            Point = point;
+            Value = point.X;
+        }
+
+        public bool HasNext
+        {
+            get { return Next != null; }
+        }
+
+        public bool HasPrev
+        {
+            get { return Prev != null; }
+        }
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/CDT/Delaunay/Sweep/DTSweep.cs

@@ -0,0 +1,1132 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ * * Neither the name of Poly2Tri nor the names of its contributors may be
+ *   used to endorse or promote products derived from this software without specific
+ *   prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * Sweep-line, Constrained Delauney Triangulation (CDT) See: Domiter, V. and
+ * Zalik, B.(2008)'Sweep-line algorithm for constrained Delaunay triangulation',
+ * International Journal of Geographical Information Science
+ * 
+ * "FlipScan" Constrained Edge Algorithm invented by author of this code.
+ * 
+ * Author: Thomas Åhlén, [email protected] 
+ */
+
+// Changes from the Java version
+//   Turned DTSweep into a static class
+//   Lots of deindentation via early bailout
+// Future possibilities
+//   Comments!
+
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using FarseerPhysics.Common.Decomposition.CDT;
+
+namespace Poly2Tri.Triangulation.Delaunay.Sweep
+{
+    public static class DTSweep
+    {
+        private const double PI_div2 = Math.PI/2;
+        private const double PI_3div4 = 3*Math.PI/4;
+
+        /// <summary>
+        /// Triangulate simple polygon with holes
+        /// </summary>
+        public static void Triangulate(DTSweepContext tcx)
+        {
+            tcx.CreateAdvancingFront();
+
+            Sweep(tcx);
+
+            // Finalize triangulation
+            if (tcx.TriangulationMode == TriangulationMode.Polygon)
+            {
+                FinalizationPolygon(tcx);
+            }
+            else
+            {
+                FinalizationConvexHull(tcx);
+            }
+
+            tcx.Done();
+        }
+
+        /// <summary>
+        /// Start sweeping the Y-sorted point set from bottom to top
+        /// </summary>
+        private static void Sweep(DTSweepContext tcx)
+        {
+            List<TriangulationPoint> points = tcx.Points;
+            TriangulationPoint point;
+            AdvancingFrontNode node;
+
+            for (int i = 1; i < points.Count; i++)
+            {
+                point = points[i];
+
+                node = PointEvent(tcx, point);
+
+                if (point.HasEdges)
+                {
+                    foreach (DTSweepConstraint e in point.Edges)
+                    {
+                        EdgeEvent(tcx, e, node);
+                    }
+                }
+                tcx.Update(null);
+            }
+        }
+
+        /// <summary>
+        /// If this is a Delaunay Triangulation of a pointset we need to fill so the triangle mesh gets a ConvexHull 
+        /// </summary>
+        private static void FinalizationConvexHull(DTSweepContext tcx)
+        {
+            AdvancingFrontNode n1, n2;
+            DelaunayTriangle t1, t2;
+            TriangulationPoint first, p1;
+
+            n1 = tcx.aFront.Head.Next;
+            n2 = n1.Next;
+            first = n1.Point;
+
+            TurnAdvancingFrontConvex(tcx, n1, n2);
+
+            // TODO: implement ConvexHull for lower right and left boundary
+
+            // Lets remove triangles connected to the two "algorithm" points
+
+            // XXX: When the first the nodes are points in a triangle we need to do a flip before 
+            //      removing triangles or we will lose a valid triangle.
+            //      Same for last three nodes!
+            // !!! If I implement ConvexHull for lower right and left boundary this fix should not be 
+            //     needed and the removed triangles will be added again by default
+            n1 = tcx.aFront.Tail.Prev;
+            if (n1.Triangle.Contains(n1.Next.Point) && n1.Triangle.Contains(n1.Prev.Point))
+            {
+                t1 = n1.Triangle.NeighborAcross(n1.Point);
+                RotateTrianglePair(n1.Triangle, n1.Point, t1, t1.OppositePoint(n1.Triangle, n1.Point));
+                tcx.MapTriangleToNodes(n1.Triangle);
+                tcx.MapTriangleToNodes(t1);
+            }
+            n1 = tcx.aFront.Head.Next;
+            if (n1.Triangle.Contains(n1.Prev.Point) && n1.Triangle.Contains(n1.Next.Point))
+            {
+                t1 = n1.Triangle.NeighborAcross(n1.Point);
+                RotateTrianglePair(n1.Triangle, n1.Point, t1, t1.OppositePoint(n1.Triangle, n1.Point));
+                tcx.MapTriangleToNodes(n1.Triangle);
+                tcx.MapTriangleToNodes(t1);
+            }
+
+            // Lower right boundary 
+            first = tcx.aFront.Head.Point;
+            n2 = tcx.aFront.Tail.Prev;
+            t1 = n2.Triangle;
+            p1 = n2.Point;
+            n2.Triangle = null;
+            do
+            {
+                tcx.RemoveFromList(t1);
+                p1 = t1.PointCCW(p1);
+                if (p1 == first) break;
+                t2 = t1.NeighborCCW(p1);
+                t1.Clear();
+                t1 = t2;
+            } while (true);
+
+            // Lower left boundary
+            first = tcx.aFront.Head.Next.Point;
+            p1 = t1.PointCW(tcx.aFront.Head.Point);
+            t2 = t1.NeighborCW(tcx.aFront.Head.Point);
+            t1.Clear();
+            t1 = t2;
+            while (p1 != first) //TODO: Port note. This was do while before.
+            {
+                tcx.RemoveFromList(t1);
+                p1 = t1.PointCCW(p1);
+                t2 = t1.NeighborCCW(p1);
+                t1.Clear();
+                t1 = t2;
+            }
+
+            // Remove current head and tail node now that we have removed all triangles attached
+            // to them. Then set new head and tail node points
+            tcx.aFront.Head = tcx.aFront.Head.Next;
+            tcx.aFront.Head.Prev = null;
+            tcx.aFront.Tail = tcx.aFront.Tail.Prev;
+            tcx.aFront.Tail.Next = null;
+
+            tcx.FinalizeTriangulation();
+        }
+
+        /// <summary>
+        /// We will traverse the entire advancing front and fill it to form a convex hull.
+        /// </summary>
+        private static void TurnAdvancingFrontConvex(DTSweepContext tcx, AdvancingFrontNode b, AdvancingFrontNode c)
+        {
+            AdvancingFrontNode first = b;
+            while (c != tcx.aFront.Tail)
+            {
+                if (TriangulationUtil.Orient2d(b.Point, c.Point, c.Next.Point) == Orientation.CCW)
+                {
+                    // [b,c,d] Concave - fill around c
+                    Fill(tcx, c);
+                    c = c.Next;
+                }
+                else
+                {
+                    // [b,c,d] Convex
+                    if (b != first && TriangulationUtil.Orient2d(b.Prev.Point, b.Point, c.Point) == Orientation.CCW)
+                    {
+                        // [a,b,c] Concave - fill around b
+                        Fill(tcx, b);
+                        b = b.Prev;
+                    }
+                    else
+                    {
+                        // [a,b,c] Convex - nothing to fill
+                        b = c;
+                        c = c.Next;
+                    }
+                }
+            }
+        }
+
+        private static void FinalizationPolygon(DTSweepContext tcx)
+        {
+            // Get an Internal triangle to start with
+            DelaunayTriangle t = tcx.aFront.Head.Next.Triangle;
+            TriangulationPoint p = tcx.aFront.Head.Next.Point;
+            while (!t.GetConstrainedEdgeCW(p))
+            {
+                t = t.NeighborCCW(p);
+            }
+
+            // Collect interior triangles constrained by edges
+            tcx.MeshClean(t);
+        }
+
+        /// <summary>
+        /// Find closes node to the left of the new point and
+        /// create a new triangle. If needed new holes and basins
+        /// will be filled to.
+        /// </summary>
+        private static AdvancingFrontNode PointEvent(DTSweepContext tcx, TriangulationPoint point)
+        {
+            AdvancingFrontNode node, newNode;
+
+            node = tcx.LocateNode(point);
+            newNode = NewFrontTriangle(tcx, point, node);
+
+            // Only need to check +epsilon since point never have smaller 
+            // x value than node due to how we fetch nodes from the front
+            if (point.X <= node.Point.X + TriangulationUtil.EPSILON)
+            {
+                Fill(tcx, node);
+            }
+
+            tcx.AddNode(newNode);
+
+            FillAdvancingFront(tcx, newNode);
+            return newNode;
+        }
+
+        /// <summary>
+        /// Creates a new front triangle and legalize it
+        /// </summary>
+        private static AdvancingFrontNode NewFrontTriangle(DTSweepContext tcx, TriangulationPoint point,
+                                                           AdvancingFrontNode node)
+        {
+            AdvancingFrontNode newNode;
+            DelaunayTriangle triangle;
+
+            triangle = new DelaunayTriangle(point, node.Point, node.Next.Point);
+            triangle.MarkNeighbor(node.Triangle);
+            tcx.Triangles.Add(triangle);
+
+            newNode = new AdvancingFrontNode(point);
+            newNode.Next = node.Next;
+            newNode.Prev = node;
+            node.Next.Prev = newNode;
+            node.Next = newNode;
+
+            tcx.AddNode(newNode); // XXX: BST
+
+            if (!Legalize(tcx, triangle))
+            {
+                tcx.MapTriangleToNodes(triangle);
+            }
+
+            return newNode;
+        }
+
+        private static void EdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node)
+        {
+            try
+            {
+                tcx.EdgeEvent.ConstrainedEdge = edge;
+                tcx.EdgeEvent.Right = edge.P.X > edge.Q.X;
+
+                if (IsEdgeSideOfTriangle(node.Triangle, edge.P, edge.Q))
+                {
+                    return;
+                }
+
+                // For now we will do all needed filling
+                // TODO: integrate with flip process might give some better performance 
+                //       but for now this avoid the issue with cases that needs both flips and fills
+                FillEdgeEvent(tcx, edge, node);
+
+                EdgeEvent(tcx, edge.P, edge.Q, node.Triangle, edge.Q);
+            }
+            catch (PointOnEdgeException e)
+            {
+                Debug.WriteLine(String.Format("Skipping Edge: {0}", e.Message));
+            }
+        }
+
+        private static void FillEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node)
+        {
+            if (tcx.EdgeEvent.Right)
+            {
+                FillRightAboveEdgeEvent(tcx, edge, node);
+            }
+            else
+            {
+                FillLeftAboveEdgeEvent(tcx, edge, node);
+            }
+        }
+
+        private static void FillRightConcaveEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge,
+                                                      AdvancingFrontNode node)
+        {
+            Fill(tcx, node.Next);
+            if (node.Next.Point != edge.P)
+            {
+                // Next above or below edge?
+                if (TriangulationUtil.Orient2d(edge.Q, node.Next.Point, edge.P) == Orientation.CCW)
+                {
+                    // Below
+                    if (TriangulationUtil.Orient2d(node.Point, node.Next.Point, node.Next.Next.Point) == Orientation.CCW)
+                    {
+                        // Next is concave
+                        FillRightConcaveEdgeEvent(tcx, edge, node);
+                    }
+                    else
+                    {
+                        // Next is convex
+                    }
+                }
+            }
+        }
+
+        private static void FillRightConvexEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node)
+        {
+            // Next concave or convex?
+            if (TriangulationUtil.Orient2d(node.Next.Point, node.Next.Next.Point, node.Next.Next.Next.Point) ==
+                Orientation.CCW)
+            {
+                // Concave
+                FillRightConcaveEdgeEvent(tcx, edge, node.Next);
+            }
+            else
+            {
+                // Convex
+                // Next above or below edge?
+                if (TriangulationUtil.Orient2d(edge.Q, node.Next.Next.Point, edge.P) == Orientation.CCW)
+                {
+                    // Below
+                    FillRightConvexEdgeEvent(tcx, edge, node.Next);
+                }
+                else
+                {
+                    // Above
+                }
+            }
+        }
+
+        private static void FillRightBelowEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node)
+        {
+            if (node.Point.X < edge.P.X) // needed?
+            {
+                if (TriangulationUtil.Orient2d(node.Point, node.Next.Point, node.Next.Next.Point) == Orientation.CCW)
+                {
+                    // Concave 
+                    FillRightConcaveEdgeEvent(tcx, edge, node);
+                }
+                else
+                {
+                    // Convex
+                    FillRightConvexEdgeEvent(tcx, edge, node);
+                    // Retry this one
+                    FillRightBelowEdgeEvent(tcx, edge, node);
+                }
+            }
+        }
+
+        private static void FillRightAboveEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node)
+        {
+            while (node.Next.Point.X < edge.P.X)
+            {
+                // Check if next node is below the edge
+                Orientation o1 = TriangulationUtil.Orient2d(edge.Q, node.Next.Point, edge.P);
+                if (o1 == Orientation.CCW)
+                {
+                    FillRightBelowEdgeEvent(tcx, edge, node);
+                }
+                else
+                {
+                    node = node.Next;
+                }
+            }
+        }
+
+        private static void FillLeftConvexEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node)
+        {
+            // Next concave or convex?
+            if (TriangulationUtil.Orient2d(node.Prev.Point, node.Prev.Prev.Point, node.Prev.Prev.Prev.Point) ==
+                Orientation.CW)
+            {
+                // Concave
+                FillLeftConcaveEdgeEvent(tcx, edge, node.Prev);
+            }
+            else
+            {
+                // Convex
+                // Next above or below edge?
+                if (TriangulationUtil.Orient2d(edge.Q, node.Prev.Prev.Point, edge.P) == Orientation.CW)
+                {
+                    // Below
+                    FillLeftConvexEdgeEvent(tcx, edge, node.Prev);
+                }
+                else
+                {
+                    // Above
+                }
+            }
+        }
+
+        private static void FillLeftConcaveEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node)
+        {
+            Fill(tcx, node.Prev);
+            if (node.Prev.Point != edge.P)
+            {
+                // Next above or below edge?
+                if (TriangulationUtil.Orient2d(edge.Q, node.Prev.Point, edge.P) == Orientation.CW)
+                {
+                    // Below
+                    if (TriangulationUtil.Orient2d(node.Point, node.Prev.Point, node.Prev.Prev.Point) == Orientation.CW)
+                    {
+                        // Next is concave
+                        FillLeftConcaveEdgeEvent(tcx, edge, node);
+                    }
+                    else
+                    {
+                        // Next is convex
+                    }
+                }
+            }
+        }
+
+        private static void FillLeftBelowEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node)
+        {
+            if (node.Point.X > edge.P.X)
+            {
+                if (TriangulationUtil.Orient2d(node.Point, node.Prev.Point, node.Prev.Prev.Point) == Orientation.CW)
+                {
+                    // Concave 
+                    FillLeftConcaveEdgeEvent(tcx, edge, node);
+                }
+                else
+                {
+                    // Convex
+                    FillLeftConvexEdgeEvent(tcx, edge, node);
+                    // Retry this one
+                    FillLeftBelowEdgeEvent(tcx, edge, node);
+                }
+            }
+        }
+
+        private static void FillLeftAboveEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node)
+        {
+            while (node.Prev.Point.X > edge.P.X)
+            {
+                // Check if next node is below the edge
+                Orientation o1 = TriangulationUtil.Orient2d(edge.Q, node.Prev.Point, edge.P);
+                if (o1 == Orientation.CW)
+                {
+                    FillLeftBelowEdgeEvent(tcx, edge, node);
+                }
+                else
+                {
+                    node = node.Prev;
+                }
+            }
+        }
+
+        //TODO: Port note: There were some structural differences here.
+        private static bool IsEdgeSideOfTriangle(DelaunayTriangle triangle, TriangulationPoint ep, TriangulationPoint eq)
+        {
+            int index;
+            index = triangle.EdgeIndex(ep, eq);
+            if (index != -1)
+            {
+                triangle.MarkConstrainedEdge(index);
+                triangle = triangle.Neighbors[index];
+                if (triangle != null)
+                {
+                    triangle.MarkConstrainedEdge(ep, eq);
+                }
+                return true;
+            }
+            return false;
+        }
+
+        private static void EdgeEvent(DTSweepContext tcx, TriangulationPoint ep, TriangulationPoint eq,
+                                      DelaunayTriangle triangle, TriangulationPoint point)
+        {
+            TriangulationPoint p1, p2;
+
+            if (IsEdgeSideOfTriangle(triangle, ep, eq))
+            {
+                return;
+            }
+
+            p1 = triangle.PointCCW(point);
+            Orientation o1 = TriangulationUtil.Orient2d(eq, p1, ep);
+            if (o1 == Orientation.Collinear)
+            {
+                if (triangle.Contains(eq, p1))
+                {
+                    triangle.MarkConstrainedEdge(eq, p1);
+                    // We are modifying the constraint maybe it would be better to 
+                    // not change the given constraint and just keep a variable for the new constraint
+                    tcx.EdgeEvent.ConstrainedEdge.Q = p1;
+                    triangle = triangle.NeighborAcross(point);
+                    EdgeEvent(tcx, ep, p1, triangle, p1);
+                }
+                else
+                {
+                    throw new PointOnEdgeException("EdgeEvent - Point on constrained edge not supported yet");
+                }
+                if (tcx.IsDebugEnabled)
+                {
+                    Debug.WriteLine("EdgeEvent - Point on constrained edge");
+                }
+                return;
+            }
+
+            p2 = triangle.PointCW(point);
+            Orientation o2 = TriangulationUtil.Orient2d(eq, p2, ep);
+            if (o2 == Orientation.Collinear)
+            {
+                if (triangle.Contains(eq, p2))
+                {
+                    triangle.MarkConstrainedEdge(eq, p2);
+                    // We are modifying the constraint maybe it would be better to 
+                    // not change the given constraint and just keep a variable for the new constraint
+                    tcx.EdgeEvent.ConstrainedEdge.Q = p2;
+                    triangle = triangle.NeighborAcross(point);
+                    EdgeEvent(tcx, ep, p2, triangle, p2);
+                }
+                else
+                {
+                    throw new PointOnEdgeException("EdgeEvent - Point on constrained edge not supported yet");
+                }
+                if (tcx.IsDebugEnabled)
+                {
+                    Debug.WriteLine("EdgeEvent - Point on constrained edge");
+                }
+                return;
+            }
+
+            if (o1 == o2)
+            {
+                // Need to decide if we are rotating CW or CCW to get to a triangle
+                // that will cross edge
+                if (o1 == Orientation.CW)
+                {
+                    triangle = triangle.NeighborCCW(point);
+                }
+                else
+                {
+                    triangle = triangle.NeighborCW(point);
+                }
+                EdgeEvent(tcx, ep, eq, triangle, point);
+            }
+            else
+            {
+                // This triangle crosses constraint so lets flippin start!
+                FlipEdgeEvent(tcx, ep, eq, triangle, point);
+            }
+        }
+
+        private static void FlipEdgeEvent(DTSweepContext tcx, TriangulationPoint ep, TriangulationPoint eq,
+                                          DelaunayTriangle t, TriangulationPoint p)
+        {
+            TriangulationPoint op, newP;
+            DelaunayTriangle ot;
+            bool inScanArea;
+
+            ot = t.NeighborAcross(p);
+            op = ot.OppositePoint(t, p);
+
+            if (ot == null)
+            {
+                // If we want to integrate the fillEdgeEvent do it here
+                // With current implementation we should never get here
+                throw new InvalidOperationException("[BUG:FIXME] FLIP failed due to missing triangle");
+            }
+
+            inScanArea = TriangulationUtil.InScanArea(p, t.PointCCW(p), t.PointCW(p), op);
+            if (inScanArea)
+            {
+                // Lets rotate shared edge one vertex CW
+                RotateTrianglePair(t, p, ot, op);
+                tcx.MapTriangleToNodes(t);
+                tcx.MapTriangleToNodes(ot);
+
+                if (p == eq && op == ep)
+                {
+                    if (eq == tcx.EdgeEvent.ConstrainedEdge.Q
+                        && ep == tcx.EdgeEvent.ConstrainedEdge.P)
+                    {
+                        if (tcx.IsDebugEnabled) Console.WriteLine("[FLIP] - constrained edge done"); // TODO: remove
+                        t.MarkConstrainedEdge(ep, eq);
+                        ot.MarkConstrainedEdge(ep, eq);
+                        Legalize(tcx, t);
+                        Legalize(tcx, ot);
+                    }
+                    else
+                    {
+                        if (tcx.IsDebugEnabled) Console.WriteLine("[FLIP] - subedge done"); // TODO: remove
+                        // XXX: I think one of the triangles should be legalized here?
+                    }
+                }
+                else
+                {
+                    if (tcx.IsDebugEnabled)
+                        Console.WriteLine("[FLIP] - flipping and continuing with triangle still crossing edge");
+                            // TODO: remove
+                    Orientation o = TriangulationUtil.Orient2d(eq, op, ep);
+                    t = NextFlipTriangle(tcx, o, t, ot, p, op);
+                    FlipEdgeEvent(tcx, ep, eq, t, p);
+                }
+            }
+            else
+            {
+                newP = NextFlipPoint(ep, eq, ot, op);
+                FlipScanEdgeEvent(tcx, ep, eq, t, ot, newP);
+                EdgeEvent(tcx, ep, eq, t, p);
+            }
+        }
+
+        /// <summary>
+        /// When we need to traverse from one triangle to the next we need 
+        /// the point in current triangle that is the opposite point to the next
+        /// triangle. 
+        /// </summary>
+        private static TriangulationPoint NextFlipPoint(TriangulationPoint ep, TriangulationPoint eq,
+                                                        DelaunayTriangle ot, TriangulationPoint op)
+        {
+            Orientation o2d = TriangulationUtil.Orient2d(eq, op, ep);
+            if (o2d == Orientation.CW)
+            {
+                // Right
+                return ot.PointCCW(op);
+            }
+            else if (o2d == Orientation.CCW)
+            {
+                // Left
+                return ot.PointCW(op);
+            }
+            else
+            {
+                // TODO: implement support for point on constraint edge
+                throw new PointOnEdgeException("Point on constrained edge not supported yet");
+            }
+        }
+
+        /// <summary>
+        /// After a flip we have two triangles and know that only one will still be
+        /// intersecting the edge. So decide which to contiune with and legalize the other
+        /// </summary>
+        /// <param name="tcx"></param>
+        /// <param name="o">should be the result of an TriangulationUtil.orient2d( eq, op, ep )</param>
+        /// <param name="t">triangle 1</param>
+        /// <param name="ot">triangle 2</param>
+        /// <param name="p">a point shared by both triangles</param>
+        /// <param name="op">another point shared by both triangles</param>
+        /// <returns>returns the triangle still intersecting the edge</returns>
+        private static DelaunayTriangle NextFlipTriangle(DTSweepContext tcx, Orientation o, DelaunayTriangle t,
+                                                         DelaunayTriangle ot, TriangulationPoint p,
+                                                         TriangulationPoint op)
+        {
+            int edgeIndex;
+            if (o == Orientation.CCW)
+            {
+                // ot is not crossing edge after flip
+                edgeIndex = ot.EdgeIndex(p, op);
+                ot.EdgeIsDelaunay[edgeIndex] = true;
+                Legalize(tcx, ot);
+                ot.EdgeIsDelaunay.Clear();
+                return t;
+            }
+            // t is not crossing edge after flip
+            edgeIndex = t.EdgeIndex(p, op);
+            t.EdgeIsDelaunay[edgeIndex] = true;
+            Legalize(tcx, t);
+            t.EdgeIsDelaunay.Clear();
+            return ot;
+        }
+
+        /// <summary>
+        /// Scan part of the FlipScan algorithm<br>
+        /// When a triangle pair isn't flippable we will scan for the next 
+        /// point that is inside the flip triangle scan area. When found 
+        /// we generate a new flipEdgeEvent
+        /// </summary>
+        /// <param name="tcx"></param>
+        /// <param name="ep">last point on the edge we are traversing</param>
+        /// <param name="eq">first point on the edge we are traversing</param>
+        /// <param name="flipTriangle">the current triangle sharing the point eq with edge</param>
+        /// <param name="t"></param>
+        /// <param name="p"></param>
+        private static void FlipScanEdgeEvent(DTSweepContext tcx, TriangulationPoint ep, TriangulationPoint eq,
+                                              DelaunayTriangle flipTriangle, DelaunayTriangle t, TriangulationPoint p)
+        {
+            DelaunayTriangle ot;
+            TriangulationPoint op, newP;
+            bool inScanArea;
+
+            ot = t.NeighborAcross(p);
+            op = ot.OppositePoint(t, p);
+
+            if (ot == null)
+            {
+                // If we want to integrate the fillEdgeEvent do it here
+                // With current implementation we should never get here
+                throw new Exception("[BUG:FIXME] FLIP failed due to missing triangle");
+            }
+
+            inScanArea = TriangulationUtil.InScanArea(eq, flipTriangle.PointCCW(eq), flipTriangle.PointCW(eq), op);
+            if (inScanArea)
+            {
+                // flip with new edge op->eq
+                FlipEdgeEvent(tcx, eq, op, ot, op);
+                // TODO: Actually I just figured out that it should be possible to 
+                //       improve this by getting the next ot and op before the the above 
+                //       flip and continue the flipScanEdgeEvent here
+                // set new ot and op here and loop back to inScanArea test
+                // also need to set a new flipTriangle first
+                // Turns out at first glance that this is somewhat complicated
+                // so it will have to wait.
+            }
+            else
+            {
+                newP = NextFlipPoint(ep, eq, ot, op);
+                FlipScanEdgeEvent(tcx, ep, eq, flipTriangle, ot, newP);
+            }
+        }
+
+        /// <summary>
+        /// Fills holes in the Advancing Front
+        /// </summary>
+        private static void FillAdvancingFront(DTSweepContext tcx, AdvancingFrontNode n)
+        {
+            AdvancingFrontNode node;
+            double angle;
+
+            // Fill right holes
+            node = n.Next;
+            while (node.HasNext)
+            {
+                angle = HoleAngle(node);
+                if (angle > PI_div2 || angle < -PI_div2)
+                {
+                    break;
+                }
+                Fill(tcx, node);
+                node = node.Next;
+            }
+
+            // Fill left holes
+            node = n.Prev;
+            while (node.HasPrev)
+            {
+                angle = HoleAngle(node);
+                if (angle > PI_div2 || angle < -PI_div2)
+                {
+                    break;
+                }
+                Fill(tcx, node);
+                node = node.Prev;
+            }
+
+            // Fill right basins
+            if (n.HasNext && n.Next.HasNext)
+            {
+                angle = BasinAngle(n);
+                if (angle < PI_3div4)
+                {
+                    FillBasin(tcx, n);
+                }
+            }
+        }
+
+        /// <summary>
+        /// Fills a basin that has formed on the Advancing Front to the right
+        /// of given node.<br>
+        /// First we decide a left,bottom and right node that forms the 
+        /// boundaries of the basin. Then we do a reqursive fill.
+        /// </summary>
+        /// <param name="tcx"></param>
+        /// <param name="node">starting node, this or next node will be left node</param>
+        private static void FillBasin(DTSweepContext tcx, AdvancingFrontNode node)
+        {
+            if (TriangulationUtil.Orient2d(node.Point, node.Next.Point, node.Next.Next.Point) == Orientation.CCW)
+            {
+                // tcx.basin.leftNode = node.next.next;
+                tcx.Basin.leftNode = node;
+            }
+            else
+            {
+                tcx.Basin.leftNode = node.Next;
+            }
+
+            // Find the bottom and right node
+            tcx.Basin.bottomNode = tcx.Basin.leftNode;
+            while (tcx.Basin.bottomNode.HasNext && tcx.Basin.bottomNode.Point.Y >= tcx.Basin.bottomNode.Next.Point.Y)
+            {
+                tcx.Basin.bottomNode = tcx.Basin.bottomNode.Next;
+            }
+
+            if (tcx.Basin.bottomNode == tcx.Basin.leftNode)
+            {
+                // No valid basins
+                return;
+            }
+
+            tcx.Basin.rightNode = tcx.Basin.bottomNode;
+            while (tcx.Basin.rightNode.HasNext && tcx.Basin.rightNode.Point.Y < tcx.Basin.rightNode.Next.Point.Y)
+            {
+                tcx.Basin.rightNode = tcx.Basin.rightNode.Next;
+            }
+
+            if (tcx.Basin.rightNode == tcx.Basin.bottomNode)
+            {
+                // No valid basins
+                return;
+            }
+
+            tcx.Basin.width = tcx.Basin.rightNode.Point.X - tcx.Basin.leftNode.Point.X;
+            tcx.Basin.leftHighest = tcx.Basin.leftNode.Point.Y > tcx.Basin.rightNode.Point.Y;
+
+            FillBasinReq(tcx, tcx.Basin.bottomNode);
+        }
+
+        /// <summary>
+        /// Recursive algorithm to fill a Basin with triangles
+        /// </summary>
+        private static void FillBasinReq(DTSweepContext tcx, AdvancingFrontNode node)
+        {
+            // if shallow stop filling
+            if (IsShallow(tcx, node))
+            {
+                return;
+            }
+
+            Fill(tcx, node);
+            if (node.Prev == tcx.Basin.leftNode && node.Next == tcx.Basin.rightNode)
+            {
+                return;
+            }
+            else if (node.Prev == tcx.Basin.leftNode)
+            {
+                Orientation o = TriangulationUtil.Orient2d(node.Point, node.Next.Point, node.Next.Next.Point);
+                if (o == Orientation.CW)
+                {
+                    return;
+                }
+                node = node.Next;
+            }
+            else if (node.Next == tcx.Basin.rightNode)
+            {
+                Orientation o = TriangulationUtil.Orient2d(node.Point, node.Prev.Point, node.Prev.Prev.Point);
+                if (o == Orientation.CCW)
+                {
+                    return;
+                }
+                node = node.Prev;
+            }
+            else
+            {
+                // Continue with the neighbor node with lowest Y value
+                if (node.Prev.Point.Y < node.Next.Point.Y)
+                {
+                    node = node.Prev;
+                }
+                else
+                {
+                    node = node.Next;
+                }
+            }
+            FillBasinReq(tcx, node);
+        }
+
+        private static bool IsShallow(DTSweepContext tcx, AdvancingFrontNode node)
+        {
+            double height;
+
+            if (tcx.Basin.leftHighest)
+            {
+                height = tcx.Basin.leftNode.Point.Y - node.Point.Y;
+            }
+            else
+            {
+                height = tcx.Basin.rightNode.Point.Y - node.Point.Y;
+            }
+            if (tcx.Basin.width > height)
+            {
+                return true;
+            }
+            return false;
+        }
+
+        /// <summary>
+        /// ???
+        /// </summary>
+        /// <param name="node">middle node</param>
+        /// <returns>the angle between 3 front nodes</returns>
+        private static double HoleAngle(AdvancingFrontNode node)
+        {
+            // XXX: do we really need a signed angle for holeAngle?
+            //      could possible save some cycles here
+            /* Complex plane
+             * ab = cosA +i*sinA
+             * ab = (ax + ay*i)(bx + by*i) = (ax*bx + ay*by) + i(ax*by-ay*bx)
+             * atan2(y,x) computes the principal value of the argument function
+             * applied to the complex number x+iy
+             * Where x = ax*bx + ay*by
+             *       y = ax*by - ay*bx
+             */
+            double px = node.Point.X;
+            double py = node.Point.Y;
+            double ax = node.Next.Point.X - px;
+            double ay = node.Next.Point.Y - py;
+            double bx = node.Prev.Point.X - px;
+            double by = node.Prev.Point.Y - py;
+            return Math.Atan2(ax*by - ay*bx, ax*bx + ay*by);
+        }
+
+        /// <summary>
+        /// The basin angle is decided against the horizontal line [1,0]
+        /// </summary>
+        private static double BasinAngle(AdvancingFrontNode node)
+        {
+            double ax = node.Point.X - node.Next.Next.Point.X;
+            double ay = node.Point.Y - node.Next.Next.Point.Y;
+            return Math.Atan2(ay, ax);
+        }
+
+        /// <summary>
+        /// Adds a triangle to the advancing front to fill a hole.
+        /// </summary>
+        /// <param name="tcx"></param>
+        /// <param name="node">middle node, that is the bottom of the hole</param>
+        private static void Fill(DTSweepContext tcx, AdvancingFrontNode node)
+        {
+            DelaunayTriangle triangle = new DelaunayTriangle(node.Prev.Point, node.Point, node.Next.Point);
+            // TODO: should copy the cEdge value from neighbor triangles
+            //       for now cEdge values are copied during the legalize 
+            triangle.MarkNeighbor(node.Prev.Triangle);
+            triangle.MarkNeighbor(node.Triangle);
+            tcx.Triangles.Add(triangle);
+
+            // Update the advancing front
+            node.Prev.Next = node.Next;
+            node.Next.Prev = node.Prev;
+            tcx.RemoveNode(node);
+
+            // If it was legalized the triangle has already been mapped
+            if (!Legalize(tcx, triangle))
+            {
+                tcx.MapTriangleToNodes(triangle);
+            }
+        }
+
+        /// <summary>
+        /// Returns true if triangle was legalized
+        /// </summary>
+        private static bool Legalize(DTSweepContext tcx, DelaunayTriangle t)
+        {
+            int oi;
+            bool inside;
+            TriangulationPoint p, op;
+            DelaunayTriangle ot;
+
+            // To legalize a triangle we start by finding if any of the three edges
+            // violate the Delaunay condition
+            for (int i = 0; i < 3; i++)
+            {
+                // TODO: fix so that cEdge is always valid when creating new triangles then we can check it here
+                //       instead of below with ot
+                if (t.EdgeIsDelaunay[i])
+                {
+                    continue;
+                }
+
+                ot = t.Neighbors[i];
+                if (ot != null)
+                {
+                    p = t.Points[i];
+                    op = ot.OppositePoint(t, p);
+                    oi = ot.IndexOf(op);
+                    // If this is a Constrained Edge or a Delaunay Edge(only during recursive legalization)
+                    // then we should not try to legalize
+                    if (ot.EdgeIsConstrained[oi] || ot.EdgeIsDelaunay[oi])
+                    {
+                        t.EdgeIsConstrained[i] = ot.EdgeIsConstrained[oi];
+                            // XXX: have no good way of setting this property when creating new triangles so lets set it here
+                        continue;
+                    }
+
+                    inside = TriangulationUtil.SmartIncircle(p,
+                                                             t.PointCCW(p),
+                                                             t.PointCW(p),
+                                                             op);
+
+                    if (inside)
+                    {
+                        bool notLegalized;
+
+                        // Lets mark this shared edge as Delaunay 
+                        t.EdgeIsDelaunay[i] = true;
+                        ot.EdgeIsDelaunay[oi] = true;
+
+                        // Lets rotate shared edge one vertex CW to legalize it
+                        RotateTrianglePair(t, p, ot, op);
+
+                        // We now got one valid Delaunay Edge shared by two triangles
+                        // This gives us 4 new edges to check for Delaunay
+
+                        // Make sure that triangle to node mapping is done only one time for a specific triangle
+                        notLegalized = !Legalize(tcx, t);
+
+                        if (notLegalized)
+                        {
+                            tcx.MapTriangleToNodes(t);
+                        }
+                        notLegalized = !Legalize(tcx, ot);
+                        if (notLegalized)
+                        {
+                            tcx.MapTriangleToNodes(ot);
+                        }
+
+                        // Reset the Delaunay edges, since they only are valid Delaunay edges
+                        // until we add a new triangle or point.
+                        // XXX: need to think about this. Can these edges be tried after we 
+                        //      return to previous recursive level?
+                        t.EdgeIsDelaunay[i] = false;
+                        ot.EdgeIsDelaunay[oi] = false;
+
+                        // If triangle have been legalized no need to check the other edges since
+                        // the recursive legalization will handles those so we can end here.
+                        return true;
+                    }
+                }
+            }
+            return false;
+        }
+
+        /// <summary>
+        /// Rotates a triangle pair one vertex CW
+        ///       n2                    n2
+        ///  P +-----+             P +-----+
+        ///    | t  /|               |\  t |  
+        ///    |   / |               | \   |
+        ///  n1|  /  |n3           n1|  \  |n3
+        ///    | /   |    after CW   |   \ |
+        ///    |/ oT |               | oT \|
+        ///    +-----+ oP            +-----+
+        ///       n4                    n4
+        /// </summary>
+        private static void RotateTrianglePair(DelaunayTriangle t, TriangulationPoint p, DelaunayTriangle ot,
+                                               TriangulationPoint op)
+        {
+            DelaunayTriangle n1, n2, n3, n4;
+            n1 = t.NeighborCCW(p);
+            n2 = t.NeighborCW(p);
+            n3 = ot.NeighborCCW(op);
+            n4 = ot.NeighborCW(op);
+
+            bool ce1, ce2, ce3, ce4;
+            ce1 = t.GetConstrainedEdgeCCW(p);
+            ce2 = t.GetConstrainedEdgeCW(p);
+            ce3 = ot.GetConstrainedEdgeCCW(op);
+            ce4 = ot.GetConstrainedEdgeCW(op);
+
+            bool de1, de2, de3, de4;
+            de1 = t.GetDelaunayEdgeCCW(p);
+            de2 = t.GetDelaunayEdgeCW(p);
+            de3 = ot.GetDelaunayEdgeCCW(op);
+            de4 = ot.GetDelaunayEdgeCW(op);
+
+            t.Legalize(p, op);
+            ot.Legalize(op, p);
+
+            // Remap dEdge
+            ot.SetDelaunayEdgeCCW(p, de1);
+            t.SetDelaunayEdgeCW(p, de2);
+            t.SetDelaunayEdgeCCW(op, de3);
+            ot.SetDelaunayEdgeCW(op, de4);
+
+            // Remap cEdge
+            ot.SetConstrainedEdgeCCW(p, ce1);
+            t.SetConstrainedEdgeCW(p, ce2);
+            t.SetConstrainedEdgeCCW(op, ce3);
+            ot.SetConstrainedEdgeCW(op, ce4);
+
+            // Remap neighbors
+            // XXX: might optimize the markNeighbor by keeping track of
+            //      what side should be assigned to what neighbor after the 
+            //      rotation. Now mark neighbor does lots of testing to find 
+            //      the right side.
+            t.Neighbors.Clear();
+            ot.Neighbors.Clear();
+            if (n1 != null) ot.MarkNeighbor(n1);
+            if (n2 != null) t.MarkNeighbor(n2);
+            if (n3 != null) t.MarkNeighbor(n3);
+            if (n4 != null) ot.MarkNeighbor(n4);
+            t.MarkNeighbor(ot);
+        }
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepConstraint.cs

@@ -0,0 +1,66 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ * * Neither the name of Poly2Tri nor the names of its contributors may be
+ *   used to endorse or promote products derived from this software without specific
+ *   prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+namespace Poly2Tri.Triangulation.Delaunay.Sweep
+{
+    public class DTSweepConstraint : TriangulationConstraint
+    {
+        /// <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);
+        }
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepContext.cs

@@ -0,0 +1,236 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ * * Neither the name of Poly2Tri nor the names of its contributors may be
+ *   used to endorse or promote products derived from this software without specific
+ *   prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+namespace Poly2Tri.Triangulation.Delaunay.Sweep
+{
+    /**
+     * 
+     * @author Thomas Åhlén, [email protected]
+     *
+     */
+
+    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
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepPointComparator.cs

@@ -0,0 +1,69 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ * * Neither the name of Poly2Tri nor the names of its contributors may be
+ *   used to endorse or promote products derived from this software without specific
+ *   prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System.Collections.Generic;
+
+namespace Poly2Tri.Triangulation.Delaunay.Sweep
+{
+    public class DTSweepPointComparator : IComparer<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
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/CDT/Delaunay/Sweep/PointOnEdgeException.cs

@@ -0,0 +1,43 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ * * Neither the name of Poly2Tri nor the names of its contributors may be
+ *   used to endorse or promote products derived from this software without specific
+ *   prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+
+namespace Poly2Tri.Triangulation.Delaunay.Sweep
+{
+    public class PointOnEdgeException : NotImplementedException
+    {
+        public PointOnEdgeException(string message)
+            : base(message)
+        {
+        }
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/CDT/ITriangulatable.cs

@@ -0,0 +1,48 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ * * Neither the name of Poly2Tri nor the names of its contributors may be
+ *   used to endorse or promote products derived from this software without specific
+ *   prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System.Collections.Generic;
+using Poly2Tri.Triangulation.Delaunay;
+
+namespace Poly2Tri.Triangulation
+{
+    public interface Triangulatable
+    {
+        IList<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();
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/CDT/Orientation.cs

@@ -0,0 +1,40 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ * * Neither the name of Poly2Tri nor the names of its contributors may be
+ *   used to endorse or promote products derived from this software without specific
+ *   prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+namespace FarseerPhysics.Common.Decomposition.CDT
+{
+    public enum Orientation
+    {
+        CW,
+        CCW,
+        Collinear
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/CDT/Polygon/Polygon.cs

@@ -0,0 +1,272 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ * * Neither the name of Poly2Tri nor the names of its contributors may be
+ *   used to endorse or promote products derived from this software without specific
+ *   prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+// Changes from the Java version
+//   Polygon constructors sprused up, checks for 3+ polys
+//   Naming of everything
+//   getTriangulationMode() -> TriangulationMode { get; }
+//   Exceptions replaced
+// Future possibilities
+//   We have a lot of Add/Clear methods -- we may prefer to just expose the container
+//   Some self-explanitory methods may deserve commenting anyways
+
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using Poly2Tri.Triangulation.Delaunay;
+
+namespace Poly2Tri.Triangulation.Polygon
+{
+    public class Polygon : Triangulatable
+    {
+        protected List<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);
+        }
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/CDT/Polygon/PolygonPoint.cs

@@ -0,0 +1,48 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ * * Neither the name of Poly2Tri nor the names of its contributors may be
+ *   used to endorse or promote products derived from this software without specific
+ *   prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+// Changes from the Java version
+//   Replaced get/set Next/Previous with attributes
+// Future possibilities
+//   Documentation!
+
+namespace Poly2Tri.Triangulation.Polygon
+{
+    public class PolygonPoint : TriangulationPoint
+    {
+        public PolygonPoint(double x, double y) : base(x, y)
+        {
+        }
+
+        public PolygonPoint Next { get; set; }
+        public PolygonPoint Previous { get; set; }
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/CDT/Polygon/PolygonSet.cs

@@ -0,0 +1,65 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ * * Neither the name of Poly2Tri nor the names of its contributors may be
+ *   used to endorse or promote products derived from this software without specific
+ *   prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+// Changes from the Java version
+//   Replaced getPolygons with attribute
+// Future possibilities
+//   Replace Add(Polygon) with exposed container?
+//   Replace entire class with HashSet<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);
+        }
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/CDT/Sets/ConstrainedPointSet.cs

@@ -0,0 +1,114 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ * * Neither the name of Poly2Tri nor the names of its contributors may be
+ *   used to endorse or promote products derived from this software without specific
+ *   prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System.Collections.Generic;
+
+namespace Poly2Tri.Triangulation.Sets
+{
+    /*
+     * Extends the PointSet by adding some Constraints on how it will be triangulated<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, [email protected]
+     */
+
+    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;
+        }
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/CDT/Sets/PointSet.cs

@@ -0,0 +1,84 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ * * Neither the name of Poly2Tri nor the names of its contributors may be
+ *   used to endorse or promote products derived from this software without specific
+ *   prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System.Collections.Generic;
+using Poly2Tri.Triangulation.Delaunay;
+
+namespace Poly2Tri.Triangulation.Sets
+{
+    public class PointSet : Triangulatable
+    {
+        public PointSet(List<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
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/CDT/TriangulationConstraint.cs

@@ -0,0 +1,46 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ * * Neither the name of Poly2Tri nor the names of its contributors may be
+ *   used to endorse or promote products derived from this software without specific
+ *   prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/**
+ * Forces a triangle edge between two points p and q
+ * when triangulating. For example used to enforce
+ * Polygon Edges during a polygon triangulation.
+ * 
+ * @author Thomas Åhlén, [email protected]
+ */
+namespace Poly2Tri.Triangulation
+{
+    public class TriangulationConstraint
+    {
+        public TriangulationPoint P;
+        public TriangulationPoint Q;
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/CDT/TriangulationContext.cs

@@ -0,0 +1,84 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ * * Neither the name of Poly2Tri nor the names of its contributors may be
+ *   used to endorse or promote products derived from this software without specific
+ *   prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System.Collections.Generic;
+using System.Runtime.CompilerServices;
+using 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>();
+        private int _stepTime = -1;
+
+        public TriangulationContext()
+        {
+            Terminated = false;
+        }
+
+        public TriangulationMode TriangulationMode { get; protected set; }
+        public Triangulatable Triangulatable { get; private set; }
+
+        public bool WaitUntilNotified { get; private set; }
+        public bool Terminated { get; set; }
+
+        public int StepCount { get; private set; }
+        public virtual bool IsDebugEnabled { get; protected set; }
+
+        public void Done()
+        {
+            StepCount++;
+        }
+
+        public virtual void PrepareTriangulation(Triangulatable t)
+        {
+            Triangulatable = t;
+            TriangulationMode = t.TriangulationMode;
+            t.PrepareTriangulation(this);
+        }
+
+        public abstract TriangulationConstraint NewConstraint(TriangulationPoint a, TriangulationPoint b);
+
+        [MethodImpl(MethodImplOptions.Synchronized)]
+        public void Update(string message)
+        {
+        }
+
+        public virtual void Clear()
+        {
+            Points.Clear();
+            Terminated = false;
+            StepCount = 0;
+        }
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/CDT/TriangulationMode.cs

@@ -0,0 +1,40 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ * * Neither the name of Poly2Tri nor the names of its contributors may be
+ *   used to endorse or promote products derived from this software without specific
+ *   prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+namespace Poly2Tri.Triangulation
+{
+    public enum TriangulationMode
+    {
+        Unconstrained,
+        Constrained,
+        Polygon
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/CDT/TriangulationPoint.cs

@@ -0,0 +1,82 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ * * Neither the name of Poly2Tri nor the names of its contributors may be
+ *   used to endorse or promote products derived from this software without specific
+ *   prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System.Collections.Generic;
+using Poly2Tri.Triangulation.Delaunay.Sweep;
+
+namespace Poly2Tri.Triangulation
+{
+    public class TriangulationPoint
+    {
+        // List of edges this point constitutes an upper ending point (CDT)
+
+        public double X, Y;
+
+        public TriangulationPoint(double x, double y)
+        {
+            X = x;
+            Y = y;
+        }
+
+        public List<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);
+        }
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/CDT/TriangulationUtil.cs

@@ -0,0 +1,160 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ * * Neither the name of Poly2Tri nor the names of its contributors may be
+ *   used to endorse or promote products derived from this software without specific
+ *   prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using FarseerPhysics.Common.Decomposition.CDT;
+
+namespace Poly2Tri.Triangulation
+{
+    /**
+     * @author Thomas Åhlén, [email protected]
+     */
+
+    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;
+        }
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/CDT/Util/FixedArray3.cs

@@ -0,0 +1,118 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ * * Neither the name of Poly2Tri nor the names of its contributors may be
+ *   used to endorse or promote products derived from this software without specific
+ *   prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using System.Collections;
+using System.Collections.Generic;
+
+namespace Poly2Tri.Triangulation.Util
+{
+    public struct FixedArray3<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];
+        }
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/CDT/Util/FixedBitArray3.cs

@@ -0,0 +1,118 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ * * Neither the name of Poly2Tri nor the names of its contributors may be
+ *   used to endorse or promote products derived from this software without specific
+ *   prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using System.Collections;
+using System.Collections.Generic;
+
+namespace Poly2Tri.Triangulation.Util
+{
+    public struct FixedBitArray3 : IEnumerable<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];
+        }
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/CDT/Util/PointGenerator.cs

@@ -0,0 +1,38 @@
+using System;
+using System.Collections.Generic;
+
+namespace Poly2Tri.Triangulation.Util
+{
+    public class PointGenerator
+    {
+        private static readonly Random RNG = new Random();
+
+        public static List<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;
+        }
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/CDT/Util/PolygonGenerator.cs

@@ -0,0 +1,98 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ * * Neither the name of Poly2Tri nor the names of its contributors may be
+ *   used to endorse or promote products derived from this software without specific
+ *   prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using Poly2Tri.Triangulation.Polygon;
+
+namespace Poly2Tri.Triangulation.Util
+{
+    public class PolygonGenerator
+    {
+        private static readonly Random RNG = new Random();
+
+        private static double PI_2 = 2.0*Math.PI;
+
+        public static Polygon.Polygon RandomCircleSweep(double scale, int vertexCount)
+        {
+            PolygonPoint point;
+            PolygonPoint[] points;
+            double radius = scale/4;
+
+            points = new PolygonPoint[vertexCount];
+            for (int i = 0; i < vertexCount; i++)
+            {
+                do
+                {
+                    if (i%250 == 0)
+                    {
+                        radius += scale/2*(0.5 - RNG.NextDouble());
+                    }
+                    else if (i%50 == 0)
+                    {
+                        radius += scale/5*(0.5 - RNG.NextDouble());
+                    }
+                    else
+                    {
+                        radius += 25*scale/vertexCount*(0.5 - RNG.NextDouble());
+                    }
+                    radius = radius > scale/2 ? scale/2 : radius;
+                    radius = radius < scale/10 ? scale/10 : radius;
+                } while (radius < scale/10 || radius > scale/2);
+                point = new PolygonPoint(radius*Math.Cos((PI_2*i)/vertexCount),
+                                         radius*Math.Sin((PI_2*i)/vertexCount));
+                points[i] = point;
+            }
+            return new Polygon.Polygon(points);
+        }
+
+        public static Polygon.Polygon RandomCircleSweep2(double scale, int vertexCount)
+        {
+            PolygonPoint point;
+            PolygonPoint[] points;
+            double radius = scale/4;
+
+            points = new PolygonPoint[vertexCount];
+            for (int i = 0; i < vertexCount; i++)
+            {
+                do
+                {
+                    radius += scale/5*(0.5 - RNG.NextDouble());
+                    radius = radius > scale/2 ? scale/2 : radius;
+                    radius = radius < scale/10 ? scale/10 : radius;
+                } while (radius < scale/10 || radius > scale/2);
+                point = new PolygonPoint(radius*Math.Cos((PI_2*i)/vertexCount),
+                                         radius*Math.Sin((PI_2*i)/vertexCount));
+                points[i] = point;
+            }
+            return new Polygon.Polygon(points);
+        }
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/CDTDecomposer.cs

@@ -0,0 +1,110 @@
+/* Poly2Tri
+ * Copyright (c) 2009-2010, Poly2Tri Contributors
+ * http://code.google.com/p/poly2tri/
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ *   this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ * * Neither the name of Poly2Tri nor the names of its contributors may be
+ *   used to endorse or promote products derived from this software without specific
+ *   prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System.Collections.Generic;
+using Microsoft.Xna.Framework;
+using Poly2Tri.Triangulation;
+using Poly2Tri.Triangulation.Delaunay;
+using Poly2Tri.Triangulation.Delaunay.Sweep;
+using Poly2Tri.Triangulation.Polygon;
+
+using System.Linq;
+
+namespace FarseerPhysics.Common.Decomposition
+{
+    public static class CDTDecomposer
+    {
+        public static List<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;
+        }
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/EarclipDecomposer.cs

@@ -0,0 +1,691 @@
+/*
+ * C# Version Ported by Matt Bettcher and Ian Qvist 2009-2010
+ * 
+ * Original C++ Version Copyright (c) 2007 Eric Jordan
+ *
+ * This software is provided 'as-is', without any express or implied
+ * warranty.  In no event will the authors be held liable for any damages
+ * arising from the use of this software.
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 1. The origin of this software must not be misrepresented; you must not
+ * claim that you wrote the original software. If you use this software
+ * in a product, an acknowledgment in the product documentation would be
+ * appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ * misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+using System;
+using System.Collections.Generic;
+using FarseerPhysics.Common.PolygonManipulation;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common.Decomposition
+{
+    /// <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));
+        }
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/FlipcodeDecomposer.cs

@@ -0,0 +1,160 @@
+using System.Collections.Generic;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common.Decomposition
+{
+    // Original code can be found here: http://www.flipcode.com/archives/Efficient_Polygon_Triangulation.shtml
+
+    /// <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;
+        }
+    }
+}

FarseerPhysicsEngine/Common/Decomposition/SeidelDecomposer.cs

@@ -0,0 +1,1057 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common.Decomposition
+{
+    //From the Poly2Tri project by Mason Green: http://code.google.com/p/poly2tri/source/browse?repo=archive#hg/scala/src/org/poly2tri/seidel
+
+    /// <summary>
+    /// Convex decomposition algorithm based on Raimund Seidel's paper "A simple and fast incremental randomized
+    /// algorithm for computing trapezoidal decompositions and for triangulating polygons"
+    /// See also: "Computational Geometry", 3rd edition, by Mark de Berg et al, Chapter 6.2
+    ///           "Computational Geometry in C", 2nd edition, by Joseph O'Rourke
+    /// </summary>
+    public static class SeidelDecomposer
+    {
+        /// <summary>
+        /// Decompose the polygon into several smaller non-concave polygon.
+        /// </summary>
+        /// <param name="vertices">The polygon to decompose.</param>
+        /// <param name="sheer">The sheer to use. If you get bad results, try using a higher value. The default value is 0.001</param>
+        /// <returns>A list of triangles</returns>
+        public static List<Vertices> ConvexPartition(Vertices vertices, float sheer)
+        {
+            List<Point> compatList = new List<Point>(vertices.Count);
+
+            foreach (Vector2 vertex in vertices)
+            {
+                compatList.Add(new Point(vertex.X, vertex.Y));
+            }
+
+            Triangulator t = new Triangulator(compatList, sheer);
+
+            List<Vertices> list = new List<Vertices>();
+
+            foreach (List<Point> triangle in t.Triangles)
+            {
+                Vertices verts = new Vertices(triangle.Count);
+
+                foreach (Point point in triangle)
+                {
+                    verts.Add(new Vector2(point.X, point.Y));
+                }
+
+                list.Add(verts);
+            }
+
+            return list;
+        }
+
+        /// <summary>
+        /// Decompose the polygon into several smaller non-concave polygon.
+        /// </summary>
+        /// <param name="vertices">The polygon to decompose.</param>
+        /// <param name="sheer">The sheer to use. If you get bad results, try using a higher value. The default value is 0.001</param>
+        /// <returns>A list of trapezoids</returns>
+        public static List<Vertices> ConvexPartitionTrapezoid(Vertices vertices, float sheer)
+        {
+            List<Point> compatList = new List<Point>(vertices.Count);
+
+            foreach (Vector2 vertex in vertices)
+            {
+                compatList.Add(new Point(vertex.X, vertex.Y));
+            }
+
+            Triangulator t = new Triangulator(compatList, sheer);
+
+            List<Vertices> list = new List<Vertices>();
+
+            foreach (Trapezoid trapezoid in t.Trapezoids)
+            {
+                Vertices verts = new Vertices();
+
+                List<Point> points = trapezoid.Vertices();
+                foreach (Point point in points)
+                {
+                    verts.Add(new Vector2(point.X, point.Y));
+                }
+
+                list.Add(verts);
+            }
+
+            return list;
+        }
+    }
+
+    internal class MonotoneMountain
+    {
+        private const float PiSlop = 3.1f;
+        public List<List<Point>> Triangles;
+        private HashSet<Point> _convexPoints;
+        private Point _head;
+
+        // Monotone mountain points
+        private List<Point> _monoPoly;
+
+        // Triangles that constitute the mountain
+
+        // Used to track which side of the line we are on
+        private bool _positive;
+        private int _size;
+        private Point _tail;
+
+        // Almost Pi!
+
+        public MonotoneMountain()
+        {
+            _size = 0;
+            _tail = null;
+            _head = null;
+            _positive = false;
+            _convexPoints = new HashSet<Point>();
+            _monoPoly = new List<Point>();
+            Triangles = new List<List<Point>>();
+        }
+
+        // Append a point to the list
+        public void Add(Point point)
+        {
+            if (_size == 0)
+            {
+                _head = point;
+                _size = 1;
+            }
+            else if (_size == 1)
+            {
+                // Keep repeat points out of the list
+                _tail = point;
+                _tail.Prev = _head;
+                _head.Next = _tail;
+                _size = 2;
+            }
+            else
+            {
+                // Keep repeat points out of the list
+                _tail.Next = point;
+                point.Prev = _tail;
+                _tail = point;
+                _size += 1;
+            }
+        }
+
+        // Remove a point from the list
+        public void Remove(Point point)
+        {
+            Point next = point.Next;
+            Point prev = point.Prev;
+            point.Prev.Next = next;
+            point.Next.Prev = prev;
+            _size -= 1;
+        }
+
+        // Partition a x-monotone mountain into triangles O(n)
+        // See "Computational Geometry in C", 2nd edition, by Joseph O'Rourke, page 52
+        public void Process()
+        {
+            // Establish the proper sign
+            _positive = AngleSign();
+            // create monotone polygon - for dubug purposes
+            GenMonoPoly();
+
+            // Initialize internal angles at each nonbase vertex
+            // Link strictly convex vertices into a list, ignore reflex vertices
+            Point p = _head.Next;
+            while (p.Neq(_tail))
+            {
+                float a = Angle(p);
+                // If the point is almost colinear with it's neighbor, remove it!
+                if (a >= PiSlop || a <= -PiSlop || a == 0.0)
+                    Remove(p);
+                else if (IsConvex(p))
+                    _convexPoints.Add(p);
+                p = p.Next;
+            }
+
+            Triangulate();
+        }
+
+        private void Triangulate()
+        {
+            while (_convexPoints.Count != 0)
+            {
+                IEnumerator<Point> e = _convexPoints.GetEnumerator();
+                e.MoveNext();
+                Point ear = e.Current;
+
+                _convexPoints.Remove(ear);
+                Point a = ear.Prev;
+                Point b = ear;
+                Point c = ear.Next;
+                List<Point> triangle = new List<Point>(3);
+                triangle.Add(a);
+                triangle.Add(b);
+                triangle.Add(c);
+
+                Triangles.Add(triangle);
+
+                // Remove ear, update angles and convex list
+                Remove(ear);
+                if (Valid(a))
+                    _convexPoints.Add(a);
+                if (Valid(c))
+                    _convexPoints.Add(c);
+            }
+
+            Debug.Assert(_size <= 3, "Triangulation bug, please report");
+        }
+
+        private bool Valid(Point p)
+        {
+            return p.Neq(_head) && p.Neq(_tail) && IsConvex(p);
+        }
+
+        // Create the monotone polygon
+        private void GenMonoPoly()
+        {
+            Point p = _head;
+            while (p != null)
+            {
+                _monoPoly.Add(p);
+                p = p.Next;
+            }
+        }
+
+        private float Angle(Point p)
+        {
+            Point a = (p.Next - p);
+            Point b = (p.Prev - p);
+            return (float)Math.Atan2(a.Cross(b), a.Dot(b));
+        }
+
+        private bool AngleSign()
+        {
+            Point a = (_head.Next - _head);
+            Point b = (_tail - _head);
+            return Math.Atan2(a.Cross(b), a.Dot(b)) >= 0;
+        }
+
+        // Determines if the inslide angle is convex or reflex
+        private bool IsConvex(Point p)
+        {
+            if (_positive != (Angle(p) >= 0))
+                return false;
+            return true;
+        }
+    }
+
+    // Node for a Directed Acyclic graph (DAG)
+    internal abstract class Node
+    {
+        protected Node LeftChild;
+        public List<Node> ParentList;
+        protected Node RightChild;
+
+        protected Node(Node left, Node right)
+        {
+            ParentList = new List<Node>();
+            LeftChild = left;
+            RightChild = right;
+
+            if (left != null)
+                left.ParentList.Add(this);
+            if (right != null)
+                right.ParentList.Add(this);
+        }
+
+        public abstract Sink Locate(Edge s);
+
+        // Replace a node in the graph with this node
+        // Make sure parent pointers are updated
+        public void Replace(Node node)
+        {
+            foreach (Node parent in node.ParentList)
+            {
+                // Select the correct node to replace (left or right child)
+                if (parent.LeftChild == node)
+                    parent.LeftChild = this;
+                else
+                    parent.RightChild = this;
+            }
+            ParentList.AddRange(node.ParentList);
+        }
+    }
+
+    // Directed Acyclic graph (DAG)
+    // See "Computational Geometry", 3rd edition, by Mark de Berg et al, Chapter 6.2
+    internal class QueryGraph
+    {
+        private Node _head;
+
+        public QueryGraph(Node head)
+        {
+            _head = head;
+        }
+
+        private Trapezoid Locate(Edge edge)
+        {
+            return _head.Locate(edge).Trapezoid;
+        }
+
+        public List<Trapezoid> FollowEdge(Edge edge)
+        {
+            List<Trapezoid> trapezoids = new List<Trapezoid>();
+            trapezoids.Add(Locate(edge));
+            int j = 0;
+
+            while (edge.Q.X > trapezoids[j].RightPoint.X)
+            {
+                if (edge.IsAbove(trapezoids[j].RightPoint))
+                {
+                    trapezoids.Add(trapezoids[j].UpperRight);
+                }
+                else
+                {
+                    trapezoids.Add(trapezoids[j].LowerRight);
+                }
+                j += 1;
+            }
+            return trapezoids;
+        }
+
+        private void Replace(Sink sink, Node node)
+        {
+            if (sink.ParentList.Count == 0)
+                _head = node;
+            else
+                node.Replace(sink);
+        }
+
+        public void Case1(Sink sink, Edge edge, Trapezoid[] tList)
+        {
+            YNode yNode = new YNode(edge, Sink.Isink(tList[1]), Sink.Isink(tList[2]));
+            XNode qNode = new XNode(edge.Q, yNode, Sink.Isink(tList[3]));
+            XNode pNode = new XNode(edge.P, Sink.Isink(tList[0]), qNode);
+            Replace(sink, pNode);
+        }
+
+        public void Case2(Sink sink, Edge edge, Trapezoid[] tList)
+        {
+            YNode yNode = new YNode(edge, Sink.Isink(tList[1]), Sink.Isink(tList[2]));
+            XNode pNode = new XNode(edge.P, Sink.Isink(tList[0]), yNode);
+            Replace(sink, pNode);
+        }
+
+        public void Case3(Sink sink, Edge edge, Trapezoid[] tList)
+        {
+            YNode yNode = new YNode(edge, Sink.Isink(tList[0]), Sink.Isink(tList[1]));
+            Replace(sink, yNode);
+        }
+
+        public void Case4(Sink sink, Edge edge, Trapezoid[] tList)
+        {
+            YNode yNode = new YNode(edge, Sink.Isink(tList[0]), Sink.Isink(tList[1]));
+            XNode qNode = new XNode(edge.Q, yNode, Sink.Isink(tList[2]));
+            Replace(sink, qNode);
+        }
+    }
+
+    internal class Sink : Node
+    {
+        public Trapezoid Trapezoid;
+
+        private Sink(Trapezoid trapezoid)
+            : base(null, null)
+        {
+            Trapezoid = trapezoid;
+            trapezoid.Sink = this;
+        }
+
+        public static Sink Isink(Trapezoid trapezoid)
+        {
+            if (trapezoid.Sink == null)
+                return new Sink(trapezoid);
+            return trapezoid.Sink;
+        }
+
+        public override Sink Locate(Edge edge)
+        {
+            return this;
+        }
+    }
+
+    // See "Computational Geometry", 3rd edition, by Mark de Berg et al, Chapter 6.2
+    internal class TrapezoidalMap
+    {
+        // Trapezoid container
+        public HashSet<Trapezoid> Map;
+
+        // AABB margin
+
+        // Bottom segment that spans multiple trapezoids
+        private Edge _bCross;
+
+        // Top segment that spans multiple trapezoids
+        private Edge _cross;
+        private float _margin;
+
+        public TrapezoidalMap()
+        {
+            Map = new HashSet<Trapezoid>();
+            _margin = 50.0f;
+            _bCross = null;
+            _cross = null;
+        }
+
+        public void Clear()
+        {
+            _bCross = null;
+            _cross = null;
+        }
+
+        // Case 1: segment completely enclosed by trapezoid
+        //         break trapezoid into 4 smaller trapezoids
+        public Trapezoid[] Case1(Trapezoid t, Edge e)
+        {
+            Trapezoid[] trapezoids = new Trapezoid[4];
+            trapezoids[0] = new Trapezoid(t.LeftPoint, e.P, t.Top, t.Bottom);
+            trapezoids[1] = new Trapezoid(e.P, e.Q, t.Top, e);
+            trapezoids[2] = new Trapezoid(e.P, e.Q, e, t.Bottom);
+            trapezoids[3] = new Trapezoid(e.Q, t.RightPoint, t.Top, t.Bottom);
+
+            trapezoids[0].UpdateLeft(t.UpperLeft, t.LowerLeft);
+            trapezoids[1].UpdateLeftRight(trapezoids[0], null, trapezoids[3], null);
+            trapezoids[2].UpdateLeftRight(null, trapezoids[0], null, trapezoids[3]);
+            trapezoids[3].UpdateRight(t.UpperRight, t.LowerRight);
+
+            return trapezoids;
+        }
+
+        // Case 2: Trapezoid contains point p, q lies outside
+        //         break trapezoid into 3 smaller trapezoids
+        public Trapezoid[] Case2(Trapezoid t, Edge e)
+        {
+            Point rp;
+            if (e.Q.X == t.RightPoint.X)
+                rp = e.Q;
+            else
+                rp = t.RightPoint;
+
+            Trapezoid[] trapezoids = new Trapezoid[3];
+            trapezoids[0] = new Trapezoid(t.LeftPoint, e.P, t.Top, t.Bottom);
+            trapezoids[1] = new Trapezoid(e.P, rp, t.Top, e);
+            trapezoids[2] = new Trapezoid(e.P, rp, e, t.Bottom);
+
+            trapezoids[0].UpdateLeft(t.UpperLeft, t.LowerLeft);
+            trapezoids[1].UpdateLeftRight(trapezoids[0], null, t.UpperRight, null);
+            trapezoids[2].UpdateLeftRight(null, trapezoids[0], null, t.LowerRight);
+
+            _bCross = t.Bottom;
+            _cross = t.Top;
+
+            e.Above = trapezoids[1];
+            e.Below = trapezoids[2];
+
+            return trapezoids;
+        }
+
+        // Case 3: Trapezoid is bisected
+        public Trapezoid[] Case3(Trapezoid t, Edge e)
+        {
+            Point lp;
+            if (e.P.X == t.LeftPoint.X)
+                lp = e.P;
+            else
+                lp = t.LeftPoint;
+
+            Point rp;
+            if (e.Q.X == t.RightPoint.X)
+                rp = e.Q;
+            else
+                rp = t.RightPoint;
+
+            Trapezoid[] trapezoids = new Trapezoid[2];
+
+            if (_cross == t.Top)
+            {
+                trapezoids[0] = t.UpperLeft;
+                trapezoids[0].UpdateRight(t.UpperRight, null);
+                trapezoids[0].RightPoint = rp;
+            }
+            else
+            {
+                trapezoids[0] = new Trapezoid(lp, rp, t.Top, e);
+                trapezoids[0].UpdateLeftRight(t.UpperLeft, e.Above, t.UpperRight, null);
+            }
+
+            if (_bCross == t.Bottom)
+            {
+                trapezoids[1] = t.LowerLeft;
+                trapezoids[1].UpdateRight(null, t.LowerRight);
+                trapezoids[1].RightPoint = rp;
+            }
+            else
+            {
+                trapezoids[1] = new Trapezoid(lp, rp, e, t.Bottom);
+                trapezoids[1].UpdateLeftRight(e.Below, t.LowerLeft, null, t.LowerRight);
+            }
+
+            _bCross = t.Bottom;
+            _cross = t.Top;
+
+            e.Above = trapezoids[0];
+            e.Below = trapezoids[1];
+
+            return trapezoids;
+        }
+
+        // Case 4: Trapezoid contains point q, p lies outside
+        //         break trapezoid into 3 smaller trapezoids
+        public Trapezoid[] Case4(Trapezoid t, Edge e)
+        {
+            Point lp;
+            if (e.P.X == t.LeftPoint.X)
+                lp = e.P;
+            else
+                lp = t.LeftPoint;
+
+            Trapezoid[] trapezoids = new Trapezoid[3];
+
+            if (_cross == t.Top)
+            {
+                trapezoids[0] = t.UpperLeft;
+                trapezoids[0].RightPoint = e.Q;
+            }
+            else
+            {
+                trapezoids[0] = new Trapezoid(lp, e.Q, t.Top, e);
+                trapezoids[0].UpdateLeft(t.UpperLeft, e.Above);
+            }
+
+            if (_bCross == t.Bottom)
+            {
+                trapezoids[1] = t.LowerLeft;
+                trapezoids[1].RightPoint = e.Q;
+            }
+            else
+            {
+                trapezoids[1] = new Trapezoid(lp, e.Q, e, t.Bottom);
+                trapezoids[1].UpdateLeft(e.Below, t.LowerLeft);
+            }
+
+            trapezoids[2] = new Trapezoid(e.Q, t.RightPoint, t.Top, t.Bottom);
+            trapezoids[2].UpdateLeftRight(trapezoids[0], trapezoids[1], t.UpperRight, t.LowerRight);
+
+            return trapezoids;
+        }
+
+        // Create an AABB around segments
+        public Trapezoid BoundingBox(List<Edge> edges)
+        {
+            Point max = edges[0].P + _margin;
+            Point min = edges[0].Q - _margin;
+
+            foreach (Edge e in edges)
+            {
+                if (e.P.X > max.X) max = new Point(e.P.X + _margin, max.Y);
+                if (e.P.Y > max.Y) max = new Point(max.X, e.P.Y + _margin);
+                if (e.Q.X > max.X) max = new Point(e.Q.X + _margin, max.Y);
+                if (e.Q.Y > max.Y) max = new Point(max.X, e.Q.Y + _margin);
+                if (e.P.X < min.X) min = new Point(e.P.X - _margin, min.Y);
+                if (e.P.Y < min.Y) min = new Point(min.X, e.P.Y - _margin);
+                if (e.Q.X < min.X) min = new Point(e.Q.X - _margin, min.Y);
+                if (e.Q.Y < min.Y) min = new Point(min.X, e.Q.Y - _margin);
+            }
+
+            Edge top = new Edge(new Point(min.X, max.Y), new Point(max.X, max.Y));
+            Edge bottom = new Edge(new Point(min.X, min.Y), new Point(max.X, min.Y));
+            Point left = bottom.P;
+            Point right = top.Q;
+
+            return new Trapezoid(left, right, top, bottom);
+        }
+    }
+
+    internal class Point
+    {
+        // Pointers to next and previous points in Monontone Mountain
+        public Point Next, Prev;
+        public float X, Y;
+
+        public Point(float x, float y)
+        {
+            X = x;
+            Y = y;
+            Next = null;
+            Prev = null;
+        }
+
+        public static Point operator -(Point p1, Point p2)
+        {
+            return new Point(p1.X - p2.X, p1.Y - p2.Y);
+        }
+
+        public static Point operator +(Point p1, Point p2)
+        {
+            return new Point(p1.X + p2.X, p1.Y + p2.Y);
+        }
+
+        public static Point operator -(Point p1, float f)
+        {
+            return new Point(p1.X - f, p1.Y - f);
+        }
+
+        public static Point operator +(Point p1, float f)
+        {
+            return new Point(p1.X + f, p1.Y + f);
+        }
+
+        public float Cross(Point p)
+        {
+            return X * p.Y - Y * p.X;
+        }
+
+        public float Dot(Point p)
+        {
+            return X * p.X + Y * p.Y;
+        }
+
+        public bool Neq(Point p)
+        {
+            return p.X != X || p.Y != Y;
+        }
+
+        public float Orient2D(Point pb, Point pc)
+        {
+            float acx = X - pc.X;
+            float bcx = pb.X - pc.X;
+            float acy = Y - pc.Y;
+            float bcy = pb.Y - pc.Y;
+            return acx * bcy - acy * bcx;
+        }
+    }
+
+    internal class Edge
+    {
+        // Pointers used for building trapezoidal map
+        public Trapezoid Above;
+        public float B;
+        public Trapezoid Below;
+
+        // Equation of a line: y = m*x + b
+        // Slope of the line (m)
+
+        // Montone mountain points
+        public HashSet<Point> MPoints;
+        public Point P;
+        public Point Q;
+        public float Slope;
+
+        // Y intercept
+
+        public Edge(Point p, Point q)
+        {
+            P = p;
+            Q = q;
+
+            if (q.X - p.X != 0)
+                Slope = (q.Y - p.Y) / (q.X - p.X);
+            else
+                Slope = 0;
+
+            B = p.Y - (p.X * Slope);
+            Above = null;
+            Below = null;
+            MPoints = new HashSet<Point>();
+            MPoints.Add(p);
+            MPoints.Add(q);
+        }
+
+        public bool IsAbove(Point point)
+        {
+            return P.Orient2D(Q, point) < 0;
+        }
+
+        public bool IsBelow(Point point)
+        {
+            return P.Orient2D(Q, point) > 0;
+        }
+
+        public void AddMpoint(Point point)
+        {
+            foreach (Point mp in MPoints)
+                if (!mp.Neq(point))
+                    return;
+
+            MPoints.Add(point);
+        }
+    }
+
+    internal class Trapezoid
+    {
+        public Edge Bottom;
+        public bool Inside;
+        public Point LeftPoint;
+
+        // Neighbor pointers
+        public Trapezoid LowerLeft;
+        public Trapezoid LowerRight;
+
+        public Point RightPoint;
+        public Sink Sink;
+
+        public Edge Top;
+        public Trapezoid UpperLeft;
+        public Trapezoid UpperRight;
+
+        public Trapezoid(Point leftPoint, Point rightPoint, Edge top, Edge bottom)
+        {
+            LeftPoint = leftPoint;
+            RightPoint = rightPoint;
+            Top = top;
+            Bottom = bottom;
+            UpperLeft = null;
+            UpperRight = null;
+            LowerLeft = null;
+            LowerRight = null;
+            Inside = true;
+            Sink = null;
+        }
+
+        // Update neighbors to the left
+        public void UpdateLeft(Trapezoid ul, Trapezoid ll)
+        {
+            UpperLeft = ul;
+            if (ul != null) ul.UpperRight = this;
+            LowerLeft = ll;
+            if (ll != null) ll.LowerRight = this;
+        }
+
+        // Update neighbors to the right
+        public void UpdateRight(Trapezoid ur, Trapezoid lr)
+        {
+            UpperRight = ur;
+            if (ur != null) ur.UpperLeft = this;
+            LowerRight = lr;
+            if (lr != null) lr.LowerLeft = this;
+        }
+
+        // Update neighbors on both sides
+        public void UpdateLeftRight(Trapezoid ul, Trapezoid ll, Trapezoid ur, Trapezoid lr)
+        {
+            UpperLeft = ul;
+            if (ul != null) ul.UpperRight = this;
+            LowerLeft = ll;
+            if (ll != null) ll.LowerRight = this;
+            UpperRight = ur;
+            if (ur != null) ur.UpperLeft = this;
+            LowerRight = lr;
+            if (lr != null) lr.LowerLeft = this;
+        }
+
+        // Recursively trim outside neighbors
+        public void TrimNeighbors()
+        {
+            if (Inside)
+            {
+                Inside = false;
+                if (UpperLeft != null) UpperLeft.TrimNeighbors();
+                if (LowerLeft != null) LowerLeft.TrimNeighbors();
+                if (UpperRight != null) UpperRight.TrimNeighbors();
+                if (LowerRight != null) LowerRight.TrimNeighbors();
+            }
+        }
+
+        // Determines if this point lies inside the trapezoid
+        public bool Contains(Point point)
+        {
+            return (point.X > LeftPoint.X && point.X < RightPoint.X && Top.IsAbove(point) && Bottom.IsBelow(point));
+        }
+
+        public List<Point> Vertices()
+        {
+            List<Point> verts = new List<Point>(4);
+            verts.Add(LineIntersect(Top, LeftPoint.X));
+            verts.Add(LineIntersect(Bottom, LeftPoint.X));
+            verts.Add(LineIntersect(Bottom, RightPoint.X));
+            verts.Add(LineIntersect(Top, RightPoint.X));
+            return verts;
+        }
+
+        private Point LineIntersect(Edge edge, float x)
+        {
+            float y = edge.Slope * x + edge.B;
+            return new Point(x, y);
+        }
+
+        // Add points to monotone mountain
+        public void AddPoints()
+        {
+            if (LeftPoint != Bottom.P)
+            {
+                Bottom.AddMpoint(LeftPoint);
+            }
+            if (RightPoint != Bottom.Q)
+            {
+                Bottom.AddMpoint(RightPoint);
+            }
+            if (LeftPoint != Top.P)
+            {
+                Top.AddMpoint(LeftPoint);
+            }
+            if (RightPoint != Top.Q)
+            {
+                Top.AddMpoint(RightPoint);
+            }
+        }
+    }
+
+    internal class XNode : Node
+    {
+        private Point _point;
+
+        public XNode(Point point, Node lChild, Node rChild)
+            : base(lChild, rChild)
+        {
+            _point = point;
+        }
+
+        public override Sink Locate(Edge edge)
+        {
+            if (edge.P.X >= _point.X)
+                // Move to the right in the graph
+                return RightChild.Locate(edge);
+            // Move to the left in the graph
+            return LeftChild.Locate(edge);
+        }
+    }
+
+    internal class YNode : Node
+    {
+        private Edge _edge;
+
+        public YNode(Edge edge, Node lChild, Node rChild)
+            : base(lChild, rChild)
+        {
+            _edge = edge;
+        }
+
+        public override Sink Locate(Edge edge)
+        {
+            if (_edge.IsAbove(edge.P))
+                // Move down the graph
+                return RightChild.Locate(edge);
+
+            if (_edge.IsBelow(edge.P))
+                // Move up the graph
+                return LeftChild.Locate(edge);
+
+            // s and segment share the same endpoint, p
+            if (edge.Slope < _edge.Slope)
+                // Move down the graph
+                return RightChild.Locate(edge);
+
+            // Move up the graph
+            return LeftChild.Locate(edge);
+        }
+    }
+
+    internal class Triangulator
+    {
+        // Trapezoid decomposition list
+        public List<Trapezoid> Trapezoids;
+        public List<List<Point>> Triangles;
+
+        // Initialize trapezoidal map and query structure
+        private Trapezoid _boundingBox;
+        private List<Edge> _edgeList;
+        private QueryGraph _queryGraph;
+        private float _sheer = 0.001f;
+        private TrapezoidalMap _trapezoidalMap;
+        private List<MonotoneMountain> _xMonoPoly;
+
+        public Triangulator(List<Point> polyLine, float sheer)
+        {
+            _sheer = sheer;
+            Triangles = new List<List<Point>>();
+            Trapezoids = new List<Trapezoid>();
+            _xMonoPoly = new List<MonotoneMountain>();
+            _edgeList = InitEdges(polyLine);
+            _trapezoidalMap = new TrapezoidalMap();
+            _boundingBox = _trapezoidalMap.BoundingBox(_edgeList);
+            _queryGraph = new QueryGraph(Sink.Isink(_boundingBox));
+
+            Process();
+        }
+
+        // Build the trapezoidal map and query graph
+        private void Process()
+        {
+            foreach (Edge edge in _edgeList)
+            {
+                List<Trapezoid> traps = _queryGraph.FollowEdge(edge);
+
+                // Remove trapezoids from trapezoidal Map
+                foreach (Trapezoid t in traps)
+                {
+                    _trapezoidalMap.Map.Remove(t);
+
+                    bool cp = t.Contains(edge.P);
+                    bool cq = t.Contains(edge.Q);
+                    Trapezoid[] tList;
+
+                    if (cp && cq)
+                    {
+                        tList = _trapezoidalMap.Case1(t, edge);
+                        _queryGraph.Case1(t.Sink, edge, tList);
+                    }
+                    else if (cp && !cq)
+                    {
+                        tList = _trapezoidalMap.Case2(t, edge);
+                        _queryGraph.Case2(t.Sink, edge, tList);
+                    }
+                    else if (!cp && !cq)
+                    {
+                        tList = _trapezoidalMap.Case3(t, edge);
+                        _queryGraph.Case3(t.Sink, edge, tList);
+                    }
+                    else
+                    {
+                        tList = _trapezoidalMap.Case4(t, edge);
+                        _queryGraph.Case4(t.Sink, edge, tList);
+                    }
+                    // Add new trapezoids to map
+                    foreach (Trapezoid y in tList)
+                    {
+                        _trapezoidalMap.Map.Add(y);
+                    }
+                }
+                _trapezoidalMap.Clear();
+            }
+
+            // Mark outside trapezoids
+            foreach (Trapezoid t in _trapezoidalMap.Map)
+            {
+                MarkOutside(t);
+            }
+
+            // Collect interior trapezoids
+            foreach (Trapezoid t in _trapezoidalMap.Map)
+            {
+                if (t.Inside)
+                {
+                    Trapezoids.Add(t);
+                    t.AddPoints();
+                }
+            }
+
+            // Generate the triangles
+            CreateMountains();
+        }
+
+        // Build a list of x-monotone mountains
+        private void CreateMountains()
+        {
+            foreach (Edge edge in _edgeList)
+            {
+                if (edge.MPoints.Count > 2)
+                {
+                    MonotoneMountain mountain = new MonotoneMountain();
+
+                    // Sorting is a perfromance hit. Literature says this can be accomplised in
+                    // linear time, although I don't see a way around using traditional methods
+                    // when using a randomized incremental algorithm
+
+                    // Insertion sort is one of the fastest algorithms for sorting arrays containing 
+                    // fewer than ten elements, or for lists that are already mostly sorted.
+
+                    List<Point> points = new List<Point>(edge.MPoints);
+                    points.Sort((p1, p2) => p1.X.CompareTo(p2.X));
+
+                    foreach (Point p in points)
+                        mountain.Add(p);
+
+                    // Triangulate monotone mountain
+                    mountain.Process();
+
+                    // Extract the triangles into a single list
+                    foreach (List<Point> t in mountain.Triangles)
+                    {
+                        Triangles.Add(t);
+                    }
+
+                    _xMonoPoly.Add(mountain);
+                }
+            }
+        }
+
+        // Mark the outside trapezoids surrounding the polygon
+        private void MarkOutside(Trapezoid t)
+        {
+            if (t.Top == _boundingBox.Top || t.Bottom == _boundingBox.Bottom)
+                t.TrimNeighbors();
+        }
+
+        // Create segments and connect end points; update edge event pointer
+        private List<Edge> InitEdges(List<Point> points)
+        {
+            List<Edge> edges = new List<Edge>();
+
+            for (int i = 0; i < points.Count - 1; i++)
+            {
+                edges.Add(new Edge(points[i], points[i + 1]));
+            }
+            edges.Add(new Edge(points[0], points[points.Count - 1]));
+            return OrderSegments(edges);
+        }
+
+        private List<Edge> OrderSegments(List<Edge> edgeInput)
+        {
+            // Ignore vertical segments!
+            List<Edge> edges = new List<Edge>();
+
+            foreach (Edge e in edgeInput)
+            {
+                Point p = ShearTransform(e.P);
+                Point q = ShearTransform(e.Q);
+
+                // Point p must be to the left of point q
+                if (p.X > q.X)
+                {
+                    edges.Add(new Edge(q, p));
+                }
+                else if (p.X < q.X)
+                {
+                    edges.Add(new Edge(p, q));
+                }
+            }
+
+            // Randomized triangulation improves performance
+            // See Seidel's paper, or O'Rourke's book, p. 57 
+            Shuffle(edges);
+            return edges;
+        }
+
+        private static void Shuffle<T>(IList<T> list)
+        {
+            Random rng = new Random();
+            int n = list.Count;
+            while (n > 1)
+            {
+                n--;
+                int k = rng.Next(n + 1);
+                T value = list[k];
+                list[k] = list[n];
+                list[n] = value;
+            }
+        }
+
+        // Prevents any two distinct endpoints from lying on a common vertical line, and avoiding
+        // the degenerate case. See Mark de Berg et al, Chapter 6.3
+        private Point ShearTransform(Point point)
+        {
+            return new Point(point.X + _sheer * point.Y, point.Y);
+        }
+    }
+}

FarseerPhysicsEngine/Common/FixedArray.cs

@@ -0,0 +1,227 @@
+/*
+* Farseer Physics Engine based on Box2D.XNA port:
+* Copyright (c) 2010 Ian Qvist
+* 
+* Box2D.XNA port of Box2D:
+* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
+*
+* Original source Box2D:
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
+* 
+* This software is provided 'as-is', without any express or implied 
+* warranty.  In no event will the authors be held liable for any damages 
+* arising from the use of this software. 
+* Permission is granted to anyone to use this software for any purpose, 
+* including commercial applications, and to alter it and redistribute it 
+* freely, subject to the following restrictions: 
+* 1. The origin of this software must not be misrepresented; you must not 
+* claim that you wrote the original software. If you use this software 
+* in a product, an acknowledgment in the product documentation would be 
+* appreciated but is not required. 
+* 2. Altered source versions must be plainly marked as such, and must not be 
+* misrepresented as being the original software. 
+* 3. This notice may not be removed or altered from any source distribution. 
+*/
+
+using System;
+
+namespace FarseerPhysics.Common
+{
+    public struct FixedArray2<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();
+                }
+            }
+        }
+    }
+}

FarseerPhysicsEngine/Common/HashSet.cs

@@ -0,0 +1,81 @@
+
+#if WINDOWS_PHONE || XBOX
+
+//TODO: FIX
+
+using System;
+using System.Collections;
+using System.Collections.Generic;
+
+namespace FarseerPhysics.Common
+{
+    
+    public class HashSet<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

FarseerPhysicsEngine/Common/LineTools.cs

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

FarseerPhysicsEngine/Common/Math.cs

@@ -0,0 +1,638 @@
+/*
+* Farseer Physics Engine based on Box2D.XNA port:
+* Copyright (c) 2010 Ian Qvist
+* 
+* Box2D.XNA port of Box2D:
+* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
+*
+* Original source Box2D:
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
+* 
+* This software is provided 'as-is', without any express or implied 
+* warranty.  In no event will the authors be held liable for any damages 
+* arising from the use of this software. 
+* Permission is granted to anyone to use this software for any purpose, 
+* including commercial applications, and to alter it and redistribute it 
+* freely, subject to the following restrictions: 
+* 1. The origin of this software must not be misrepresented; you must not 
+* claim that you wrote the original software. If you use this software 
+* in a product, an acknowledgment in the product documentation would be 
+* appreciated but is not required. 
+* 2. Altered source versions must be plainly marked as such, and must not be 
+* misrepresented as being the original software. 
+* 3. This notice may not be removed or altered from any source distribution. 
+*/
+
+using System;
+using System.Diagnostics;
+using System.Runtime.InteropServices;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common
+{
+    public static class MathUtils
+    {
+        public static float Cross(Vector2 a, Vector2 b)
+        {
+            return a.X * b.Y - a.Y * b.X;
+        }
+
+        public static Vector2 Cross(Vector2 a, float s)
+        {
+            return new Vector2(s * a.Y, -s * a.X);
+        }
+
+        public static Vector2 Cross(float s, Vector2 a)
+        {
+            return new Vector2(-s * a.Y, s * a.X);
+        }
+
+        public static Vector2 Abs(Vector2 v)
+        {
+            return new Vector2(Math.Abs(v.X), Math.Abs(v.Y));
+        }
+
+        public static Vector2 Multiply(ref Mat22 A, Vector2 v)
+        {
+            return Multiply(ref A, ref v);
+        }
+
+        public static Vector2 Multiply(ref Mat22 A, ref Vector2 v)
+        {
+            return new Vector2(A.Col1.X * v.X + A.Col2.X * v.Y, A.Col1.Y * v.X + A.Col2.Y * v.Y);
+        }
+
+        public static Vector2 MultiplyT(ref Mat22 A, Vector2 v)
+        {
+            return MultiplyT(ref A, ref v);
+        }
+
+        public static Vector2 MultiplyT(ref Mat22 A, ref Vector2 v)
+        {
+            return new Vector2(v.X * A.Col1.X + v.Y * A.Col1.Y, v.X * A.Col2.X + v.Y * A.Col2.Y);
+        }
+
+        public static Vector2 Multiply(ref Transform T, Vector2 v)
+        {
+            return Multiply(ref T, ref v);
+        }
+
+        public static Vector2 Multiply(ref Transform T, ref Vector2 v)
+        {
+            return new Vector2(T.Position.X + T.R.Col1.X * v.X + T.R.Col2.X * v.Y,
+                               T.Position.Y + T.R.Col1.Y * v.X + T.R.Col2.Y * v.Y);
+        }
+
+        public static Vector2 MultiplyT(ref Transform T, Vector2 v)
+        {
+            return MultiplyT(ref T, ref v);
+        }
+
+        public static Vector2 MultiplyT(ref Transform T, ref Vector2 v)
+        {
+            Vector2 tmp = Vector2.Zero;
+            tmp.X = v.X - T.Position.X;
+            tmp.Y = v.Y - T.Position.Y;
+            return MultiplyT(ref T.R, ref tmp);
+        }
+
+        // A^T * B
+        public static void MultiplyT(ref Mat22 A, ref Mat22 B, out Mat22 C)
+        {
+            C = new Mat22();
+            C.Col1.X = A.Col1.X * B.Col1.X + A.Col1.Y * B.Col1.Y;
+            C.Col1.Y = A.Col2.X * B.Col1.X + A.Col2.Y * B.Col1.Y;
+            C.Col2.X = A.Col1.X * B.Col2.X + A.Col1.Y * B.Col2.Y;
+            C.Col2.Y = A.Col2.X * B.Col2.X + A.Col2.Y * B.Col2.Y;
+        }
+
+        // v2 = A.R' * (B.R * v1 + B.p - A.p) = (A.R' * B.R) * v1 + (B.p - A.p)
+        public static void MultiplyT(ref Transform A, ref Transform B, out Transform C)
+        {
+            C = new Transform();
+            MultiplyT(ref A.R, ref B.R, out C.R);
+            C.Position.X = B.Position.X - A.Position.X;
+            C.Position.Y = B.Position.Y - A.Position.Y;
+        }
+
+        public static void Swap<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;
+        }
+    }
+}

FarseerPhysicsEngine/Common/Path.cs

@@ -0,0 +1,341 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+using System.Xml.Serialization;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common
+{
+    //Contributed by Matthew Bettcher
+
+    /// <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;
+        }
+    }
+}

FarseerPhysicsEngine/Common/PathManager.cs

@@ -0,0 +1,240 @@
+using System;
+using System.Collections.Generic;
+using FarseerPhysics.Collision.Shapes;
+using FarseerPhysics.Common;
+using FarseerPhysics.Common.Decomposition;
+using FarseerPhysics.Dynamics;
+using FarseerPhysics.Dynamics.Joints;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Factories
+{
+    /// <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;
+        }
+    }
+}

FarseerPhysicsEngine/Common/PhysicsLogic/Explosion.cs

@@ -0,0 +1,464 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using FarseerPhysics.Collision;
+using FarseerPhysics.Collision.Shapes;
+using FarseerPhysics.Dynamics;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common.PhysicsLogic
+{
+    internal struct ShapeData
+    {
+        public Body Body;
+        public float Max;
+        public float Min; // absolute angles
+    }
+
+    /// <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;
+        }
+    }
+}

FarseerPhysicsEngine/Common/PhysicsLogic/PhysicsLogic.cs

@@ -0,0 +1,66 @@
+using System;
+using FarseerPhysics.Dynamics;
+
+namespace FarseerPhysics.Common.PhysicsLogic
+{
+    [Flags]
+    public enum PhysicsLogicType
+    {
+        Explosion = (1 << 0)
+    }
+
+    public struct PhysicsLogicFilter
+    {
+        public PhysicsLogicType ControllerIgnores;
+
+        /// <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;
+        }
+    }
+}

FarseerPhysicsEngine/Common/PolygonManipulation/CuttingTools.cs

@@ -0,0 +1,246 @@
+using System.Collections.Generic;
+using System.Diagnostics;
+using FarseerPhysics.Collision.Shapes;
+using FarseerPhysics.Dynamics;
+using FarseerPhysics.Factories;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Common.PolygonManipulation
+{
+    public static class CuttingTools
+    {
+        //Cutting a shape into two is based on the work of Daid and his prototype BoxCutter: http://www.box2d.org/forum/viewtopic.php?f=3&t=1473
+
+        /// <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;
+        }
+    }
+}