(61d00a474) v0.9.7.1

Libraries/Farseer Physics Engine 3.5/Dynamics/Body.Factory.cs

@@ -0,0 +1,154 @@
+/* Original source Farseer Physics Engine:
+ * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
+ * Microsoft Permissive License (Ms-PL) v1.1
+ */
+
+using System;
+using System.Collections.Generic;
+using FarseerPhysics.Collision.Shapes;
+using FarseerPhysics.Common;
+using FarseerPhysics.Common.Decomposition;
+using FarseerPhysics.Dynamics;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Dynamics
+{
+    /// <summary>
+    /// An easy to use factory for creating bodies
+    /// </summary>
+    public partial class Body
+    {
+        /// <summary>
+        /// Creates a fixture and attach it to this body.
+        /// If the density is non-zero, this function automatically updates the mass of the body.
+        /// Contacts are not created until the next time step.
+        /// Warning: This method is locked during callbacks.
+        /// </summary>
+        /// <param name="shape">The shape.</param>
+        /// <param name="userData">Application specific data</param>
+        /// <returns></returns>
+        public virtual Fixture CreateFixture(Shape shape)
+        {
+            Fixture fixture = new Fixture(shape);
+            Add(fixture);
+            return fixture;
+        }
+
+        public Fixture CreateEdge(Vector2 start, Vector2 end)
+        {
+            EdgeShape edgeShape = new EdgeShape(start, end);
+            return CreateFixture(edgeShape);
+        }
+
+        public Fixture CreateChainShape(Vertices vertices)
+        {
+            ChainShape shape = new ChainShape(vertices);
+            return CreateFixture(shape);
+        }
+
+        public Fixture CreateLoopShape(Vertices vertices)
+        {
+            ChainShape shape = new ChainShape(vertices, true);
+            return CreateFixture(shape);
+        }
+
+        public Fixture CreateRectangle(float width, float height, float density, Vector2 offset)
+        {
+            Vertices rectangleVertices = PolygonTools.CreateRectangle(width / 2, height / 2);
+            rectangleVertices.Translate(ref offset);
+            PolygonShape rectangleShape = new PolygonShape(rectangleVertices, density);
+            return CreateFixture(rectangleShape);
+        }
+
+        public Fixture CreateRectangle(float width, float height, float density, float rotation, Vector2 offset)
+        {
+            Vertices rectangleVertices = PolygonTools.CreateRectangle(width / 2, height / 2, Vector2.Zero, rotation);
+            rectangleVertices.Translate(ref offset);
+            PolygonShape rectangleShape = new PolygonShape(rectangleVertices, density);
+            return CreateFixture(rectangleShape);
+        }
+
+        public Fixture CreateCircle(float radius, float density)
+        {
+            if (radius <= 0)
+                throw new ArgumentOutOfRangeException("radius", "Radius must be more than 0 meters");
+
+            CircleShape circleShape = new CircleShape(radius, density);
+            return CreateFixture(circleShape);
+        }
+
+        public Fixture CreateCircle(float radius, float density, Vector2 offset)
+        {
+            if (radius <= 0)
+                throw new ArgumentOutOfRangeException("radius", "Radius must be more than 0 meters");
+
+            CircleShape circleShape = new CircleShape(radius, density);
+            circleShape.Position = offset;
+            return CreateFixture(circleShape);
+        }
+
+        public Fixture CreatePolygon(Vertices vertices, float density)
+        {
+            if (vertices.Count <= 1)
+                throw new ArgumentOutOfRangeException("vertices", "Too few points to be a polygon");
+
+            PolygonShape polygon = new PolygonShape(vertices, density);
+            return CreateFixture(polygon);
+        }
+
+        public Fixture CreateEllipse(float xRadius, float yRadius, int edges, float density)
+        {
+            if (xRadius <= 0)
+                throw new ArgumentOutOfRangeException("xRadius", "X-radius must be more than 0");
+
+            if (yRadius <= 0)
+                throw new ArgumentOutOfRangeException("yRadius", "Y-radius must be more than 0");
+
+            Vertices ellipseVertices = PolygonTools.CreateEllipse(xRadius, yRadius, edges);
+            PolygonShape polygonShape = new PolygonShape(ellipseVertices, density);
+            return CreateFixture(polygonShape);
+        }
+
+        public List<Fixture> CreateCompoundPolygon(List<Vertices> list, float density)
+        {
+            List<Fixture> res = new List<Fixture>(list.Count);
+
+            //Then we create several fixtures using the body
+            foreach (Vertices vertices in list)
+            {
+                if (vertices.Count == 2)
+                {
+                    EdgeShape shape = new EdgeShape(vertices[0], vertices[1]);
+                    res.Add(CreateFixture(shape));
+                }
+                else
+                {
+                    PolygonShape shape = new PolygonShape(vertices, density);
+                    res.Add(CreateFixture(shape));
+                }
+            }
+
+            return res;
+        }
+
+        public Fixture CreateLineArc(float radians, int sides, float radius, bool closed)
+        {
+            Vertices arc = PolygonTools.CreateArc(radians, sides, radius);
+            arc.Rotate((MathHelper.Pi - radians) / 2);
+            return closed ? CreateLoopShape(arc) : CreateChainShape(arc);
+        }
+
+        public List<Fixture> CreateSolidArc(float density, float radians, int sides, float radius)
+        {
+            Vertices arc = PolygonTools.CreateArc(radians, sides, radius);
+            arc.Rotate((MathHelper.Pi - radians) / 2);
+
+            //Close the arc
+            arc.Add(arc[0]);
+
+            List<Vertices> triangles = Triangulate.ConvexPartition(arc, TriangulationAlgorithm.Earclip);
+
+            return CreateCompoundPolygon(triangles, density);
+        }
+    }
+}

Libraries/Farseer Physics Engine 3.5/Dynamics/BodyType.cs

@@ -0,0 +1,49 @@
+/* Original source Farseer Physics Engine:
+ * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
+ * Microsoft Permissive License (Ms-PL) v1.1
+ */
+
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+* 
+* Original source Box2D:
+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org 
+* 
+* This software is provided 'as-is', without any express or implied 
+* warranty.  In no event will the authors be held liable for any damages 
+* arising from the use of this software. 
+* Permission is granted to anyone to use this software for any purpose, 
+* including commercial applications, and to alter it and redistribute it 
+* freely, subject to the following restrictions: 
+* 1. The origin of this software must not be misrepresented; you must not 
+* claim that you wrote the original software. If you use this software 
+* in a product, an acknowledgment in the product documentation would be 
+* appreciated but is not required. 
+* 2. Altered source versions must be plainly marked as such, and must not be 
+* misrepresented as being the original software. 
+* 3. This notice may not be removed or altered from any source distribution. 
+*/
+
+
+namespace FarseerPhysics
+{
+    /// <summary>
+    /// The body type.
+    /// </summary>
+    public enum BodyType
+    {
+        /// <summary>
+        /// Zero velocity, may be manually moved. Note: even static bodies have mass.
+        /// </summary>
+        Static,
+        /// <summary>
+        /// Zero mass, non-zero velocity set by user, moved by solver
+        /// </summary>
+        Kinematic,
+        /// <summary>
+        /// Positive mass, non-zero velocity determined by forces, moved by solver
+        /// </summary>
+        Dynamic,
+    }
+}

Libraries/Farseer Physics Engine 3.5/Dynamics/BreakableBody.cs

@@ -1,148 +0,0 @@
-using System;
-using System.Collections.Generic;
-using FarseerPhysics.Collision.Shapes;
-using FarseerPhysics.Common;
-using FarseerPhysics.Dynamics.Contacts;
-using FarseerPhysics.Factories;
-using Microsoft.Xna.Framework;
-
-namespace FarseerPhysics.Dynamics
-{
-    /// <summary>
-    /// A type of body that supports multiple fixtures that can break apart.
-    /// </summary>
-    public class BreakableBody
-    {
-        private float[] _angularVelocitiesCache = new float[8];
-        private bool _break;
-        private Vector2[] _velocitiesCache = new Vector2[8];
-        private World _world;
-
-        public BreakableBody(IEnumerable<Vertices> vertices, World world, float density)
-        {
-            _world = world;
-            _world.ContactManager.PostSolve += PostSolve;
-            MainBody = new Body(_world);
-            MainBody.BodyType = BodyType.Dynamic;
-
-            foreach (Vertices part in vertices)
-            {
-                PolygonShape polygonShape = new PolygonShape(part, density);
-                Fixture fixture = MainBody.CreateFixture(polygonShape);
-                Parts.Add(fixture);
-            }
-        }
-
-        public BreakableBody(IEnumerable<Shape> shapes, World world)
-        {
-            _world = world;
-            _world.ContactManager.PostSolve += PostSolve;
-            MainBody = new Body(_world);
-            MainBody.BodyType = BodyType.Dynamic;
-
-            foreach (Shape part in shapes)
-            {
-                Fixture fixture = MainBody.CreateFixture(part);
-                Parts.Add(fixture);
-            }
-        }
-
-        public bool Broken;
-        public Body MainBody;
-        public List<Fixture> Parts = new List<Fixture>(8);
-
-        /// <summary>
-        /// The force needed to break the body apart.
-        /// Default: 500
-        /// </summary>
-        public float Strength = 500.0f;
-
-        private void PostSolve(Contact contact, ContactVelocityConstraint impulse)
-        {
-            if (!Broken)
-            {
-                if (Parts.Contains(contact.FixtureA) || Parts.Contains(contact.FixtureB))
-                {
-                    float maxImpulse = 0.0f;
-                    int count = contact.Manifold.PointCount;
-
-                    for (int i = 0; i < count; ++i)
-                    {
-                        maxImpulse = Math.Max(maxImpulse, impulse.points[i].normalImpulse);
-                    }
-
-                    if (maxImpulse > Strength)
-                    {
-                        // Flag the body for breaking.
-                        _break = true;
-                    }
-                }
-            }
-        }
-
-        public void Update()
-        {
-            if (_break)
-            {
-                Decompose();
-                Broken = true;
-                _break = false;
-            }
-
-            // Cache velocities to improve movement on breakage.
-            if (Broken == false)
-            {
-                //Enlarge the cache if needed
-                if (Parts.Count > _angularVelocitiesCache.Length)
-                {
-                    _velocitiesCache = new Vector2[Parts.Count];
-                    _angularVelocitiesCache = new float[Parts.Count];
-                }
-
-                //Cache the linear and angular velocities.
-                for (int i = 0; i < Parts.Count; i++)
-                {
-                    _velocitiesCache[i] = Parts[i].Body.LinearVelocity;
-                    _angularVelocitiesCache[i] = Parts[i].Body.AngularVelocity;
-                }
-            }
-        }
-
-        private void Decompose()
-        {
-            //Unsubsribe from the PostSolve delegate
-            _world.ContactManager.PostSolve -= PostSolve;
-
-            for (int i = 0; i < Parts.Count; i++)
-            {
-                Fixture oldFixture = Parts[i];
-
-                Shape shape = oldFixture.Shape.Clone();
-                object userData = oldFixture.UserData;
-
-                MainBody.DestroyFixture(oldFixture);
-
-                Body body = BodyFactory.CreateBody(_world);
-                body.BodyType = BodyType.Dynamic;
-                body.Position = MainBody.Position;
-                body.Rotation = MainBody.Rotation;
-                body.UserData = MainBody.UserData;
-
-                Fixture newFixture = body.CreateFixture(shape);
-                newFixture.UserData = userData;
-                Parts[i] = newFixture;
-
-                body.AngularVelocity = _angularVelocitiesCache[i];
-                body.LinearVelocity = _velocitiesCache[i];
-            }
-
-            _world.RemoveBody(MainBody);
-            _world.RemoveBreakableBody(this);
-        }
-
-        public void Break()
-        {
-            _break = true;
-        }
-    }
-}

Libraries/Farseer Physics Engine 3.5/Dynamics/Category.cs

@@ -0,0 +1,76 @@
+/* Original source Farseer Physics Engine:
+ * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
+ * Microsoft Permissive License (Ms-PL) v1.1
+ */
+
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+* 
+* Original source Box2D:
+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org 
+* 
+* This software is provided 'as-is', without any express or implied 
+* warranty.  In no event will the authors be held liable for any damages 
+* arising from the use of this software. 
+* Permission is granted to anyone to use this software for any purpose, 
+* including commercial applications, and to alter it and redistribute it 
+* freely, subject to the following restrictions: 
+* 1. The origin of this software must not be misrepresented; you must not 
+* claim that you wrote the original software. If you use this software 
+* in a product, an acknowledgment in the product documentation would be 
+* appreciated but is not required. 
+* 2. Altered source versions must be plainly marked as such, and must not be 
+* misrepresented as being the original software. 
+* 3. This notice may not be removed or altered from any source distribution. 
+*/
+
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using FarseerPhysics.Collision;
+using FarseerPhysics.Collision.Shapes;
+using FarseerPhysics.Common;
+using FarseerPhysics.Dynamics.Contacts;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Dynamics
+{
+    [Flags]
+    public enum Category
+    {
+        None  = 0x00000000,        
+        Cat1  = 0x00000001,
+        Cat2  = 0x00000002,
+        Cat3  = 0x00000004,
+        Cat4  = 0x00000008,
+        Cat5  = 0x00000010,
+        Cat6  = 0x00000020,
+        Cat7  = 0x00000040,
+        Cat8  = 0x00000080,
+        Cat9  = 0x00000100,
+        Cat10 = 0x00000200,
+        Cat11 = 0x00000400,
+        Cat12 = 0x00000800,
+        Cat13 = 0x00001000,
+        Cat14 = 0x00002000,
+        Cat15 = 0x00004000,
+        Cat16 = 0x00008000,
+        Cat17 = 0x00010000,
+        Cat18 = 0x00020000,
+        Cat19 = 0x00040000,
+        Cat20 = 0x00080000,
+        Cat21 = 0x00100000,
+        Cat22 = 0x00200000,
+        Cat23 = 0x00400000,
+        Cat24 = 0x00800000,
+        Cat25 = 0x01000000,
+        Cat26 = 0x02000000,
+        Cat27 = 0x04000000,
+        Cat28 = 0x08000000,
+        Cat29 = 0x10000000,
+        Cat30 = 0x20000000,
+        Cat31 = 0x40000000,
+        All = int.MaxValue,
+    }
+}

Libraries/Farseer Physics Engine 3.5/Dynamics/ContactManager.cs

@@ -1,4 +1,11 @@
-/*
+// Copyright (c) 2017 Kastellanos Nikolaos
+
+/* Original source Farseer Physics Engine:
+ * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
+ * Microsoft Permissive License (Ms-PL) v1.1
+ */
+
+/*
 * Farseer Physics Engine:
 * Copyright (c) 2012 Ian Qvist
 * 
@@ -19,7 +26,6 @@
 * misrepresented as being the original software. 
 * 3. This notice may not be removed or altered from any source distribution. 
 */
-//#define USE_ACTIVE_CONTACT_SET
 
 using System.Collections.Generic;
 using FarseerPhysics.Collision;
@@ -29,6 +35,33 @@ namespace FarseerPhysics.Dynamics
 {
     public class ContactManager
     {
+        #region Settings
+        /// <summary>
+        /// A threshold for activating multiple cores to solve VelocityConstraints.
+        /// An Island with a contact count above this threshold will use multiple threads to solve VelocityConstraints.
+        /// A value of 0 will always use multithreading. A value of (int.MaxValue) will never use multithreading.
+        /// Typical values are {128 or 256}.
+        /// </summary>
+        public int VelocityConstraintsMultithreadThreshold = 128;
+
+        /// <summary>
+        /// A threshold for activating multiple cores to solve PositionConstraints.
+        /// An Island with a contact count above this threshold will use multiple threads to solve PositionConstraints.
+        /// A value of 0 will always use multithreading. A value of (int.MaxValue) will never use multithreading.
+        /// Typical values are {128 or 256}.
+        /// </summary>
+        public int PositionConstraintsMultithreadThreshold = 128;
+        
+        /// <summary>
+        /// A threshold for activating multiple cores to solve Collide.
+        /// An World with a contact count above this threshold will use multiple threads to solve Collide.
+        /// A value of 0 will always use multithreading. A value of (int.MaxValue) will never use multithreading.
+        /// Typical values are {128 or 256}.
+        /// </summary>
+        public int CollideMultithreadThreshold = 64;
+        #endregion
+
+
         /// <summary>
         /// Fires when a contact is created
         /// </summary>
@@ -36,12 +69,15 @@ namespace FarseerPhysics.Dynamics
 
         public IBroadPhase BroadPhase;
 
+        public readonly ContactListHead ContactList;
+        public int ContactCount { get; private set; }
+        internal readonly ContactListHead _contactPoolList;
+
         /// <summary>
         /// The filter used by the contact manager.
         /// </summary>
         public CollisionFilterDelegate ContactFilter;
 
-        public List<Contact> ContactList = new List<Contact>(256);
 
 #if USE_ACTIVE_CONTACT_SET
         /// <summary>
@@ -79,21 +115,22 @@ namespace FarseerPhysics.Dynamics
 
         internal ContactManager(IBroadPhase broadPhase)
         {
+            ContactList = new ContactListHead();
+            ContactCount = 0;
+            _contactPoolList = new ContactListHead();
+
             BroadPhase = broadPhase;
             OnBroadphaseCollision = AddPair;
         }
 
         // Broad-phase callback.
-        private void AddPair(ref FixtureProxy proxyA, ref FixtureProxy proxyB)
+        private void AddPair(int proxyIdA, int proxyIdB)
         {
-            Fixture fixtureA = proxyA.Fixture;
-            Fixture fixtureB = proxyB.Fixture;
+            FixtureProxy proxyA = BroadPhase.GetProxy(proxyIdA);
+            FixtureProxy proxyB = BroadPhase.GetProxy(proxyIdB);
 
-            int indexA = proxyA.ChildIndex;
-            int indexB = proxyB.ChildIndex;
-
-            Body bodyA = fixtureA.Body;
-            Body bodyB = fixtureB.Body;
+            Body bodyA = proxyA.Body;
+            Body bodyB = proxyB.Body;
 
             // Are the fixtures on the same body?
             if (bodyA == bodyB)
@@ -101,16 +138,21 @@ namespace FarseerPhysics.Dynamics
                 return;
             }
 
+            Fixture fixtureA = proxyA.Fixture;
+            Fixture fixtureB = proxyB.Fixture;
+
+            int indexA = proxyA.ChildIndex;
+            int indexB = proxyB.ChildIndex;
+
             // Does a contact already exist?
-            ContactEdge edge = bodyB.ContactList;
-            while (edge != null)
+            for (ContactEdge ceB = bodyB.ContactList; ceB != null; ceB = ceB.Next)
             {
-                if (edge.Other == bodyA)
+                if (ceB.Other == bodyA)
                 {
-                    Fixture fA = edge.Contact.FixtureA;
-                    Fixture fB = edge.Contact.FixtureB;
-                    int iA = edge.Contact.ChildIndexA;
-                    int iB = edge.Contact.ChildIndexB;
+                    Fixture fA = ceB.Contact.FixtureA;
+                    Fixture fB = ceB.Contact.FixtureB;
+                    int iA = ceB.Contact.ChildIndexA;
+                    int iB = ceB.Contact.ChildIndexB;
 
                     if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB)
                     {
@@ -124,8 +166,6 @@ namespace FarseerPhysics.Dynamics
                         return;
                     }
                 }
-
-                edge = edge.Next;
             }
 
             // Does a joint override collision? Is at least one body dynamic?
@@ -148,7 +188,7 @@ namespace FarseerPhysics.Dynamics
                 return;
 
             // Call the factory.
-            Contact c = Contact.Create(fixtureA, indexA, fixtureB, indexB);
+            Contact c = Contact.Create(this, fixtureA, indexA, fixtureB, indexB);
 
             if (c == null)
                 return;
@@ -160,7 +200,11 @@ namespace FarseerPhysics.Dynamics
             bodyB = fixtureB.Body;
 
             // Insert into the world.
-            ContactList.Add(c);
+            c.Prev = ContactList;
+            c.Next = c.Prev.Next;
+            c.Prev.Next = c;
+            c.Next.Prev = c;
+            ContactCount++;
 
 #if USE_ACTIVE_CONTACT_SET
 			ActiveContacts.Add(c);
@@ -215,73 +259,79 @@ namespace FarseerPhysics.Dynamics
             {
                 //Report the separation to both participants:
                 if (fixtureA != null && fixtureA.OnSeparation != null)
-                    fixtureA.OnSeparation(fixtureA, fixtureB);
+                    fixtureA.OnSeparation(fixtureA, fixtureB, contact);
 
                 //Reverse the order of the reported fixtures. The first fixture is always the one that the
                 //user subscribed to.
                 if (fixtureB != null && fixtureB.OnSeparation != null)
-                    fixtureB.OnSeparation(fixtureB, fixtureA);
+                    fixtureB.OnSeparation(fixtureB, fixtureA, contact);
+
+                //Report the separation to both bodies:
+                if (fixtureA != null && fixtureA.Body != null && fixtureA.Body.onSeparationEventHandler != null)
+                    fixtureA.Body.onSeparationEventHandler(fixtureA, fixtureB, contact);
+
+                //Reverse the order of the reported fixtures. The first fixture is always the one that the
+                //user subscribed to.
+                if (fixtureB != null && fixtureB.Body != null && fixtureB.Body.onSeparationEventHandler != null)
+                    fixtureB.Body.onSeparationEventHandler(fixtureB, fixtureA, contact);
 
                 if (EndContact != null)
                     EndContact(contact);
             }
 
             // Remove from the world.
-            ContactList.Remove(contact);
+            contact.Prev.Next = contact.Next;
+            contact.Next.Prev = contact.Prev;
+            contact.Next = null;
+            contact.Prev = null;
+            ContactCount--;
 
             // Remove from body 1
-            if (contact._nodeA.Prev != null)
-            {
-                contact._nodeA.Prev.Next = contact._nodeA.Next;
-            }
-
-            if (contact._nodeA.Next != null)
-            {
-                contact._nodeA.Next.Prev = contact._nodeA.Prev;
-            }
-
             if (contact._nodeA == bodyA.ContactList)
-            {
                 bodyA.ContactList = contact._nodeA.Next;
-            }
+            if (contact._nodeA.Prev != null)
+                contact._nodeA.Prev.Next = contact._nodeA.Next;
+            if (contact._nodeA.Next != null)
+                contact._nodeA.Next.Prev = contact._nodeA.Prev;
 
             // Remove from body 2
-            if (contact._nodeB.Prev != null)
-            {
-                contact._nodeB.Prev.Next = contact._nodeB.Next;
-            }
-
-            if (contact._nodeB.Next != null)
-            {
-                contact._nodeB.Next.Prev = contact._nodeB.Prev;
-            }
-
             if (contact._nodeB == bodyB.ContactList)
-            {
                 bodyB.ContactList = contact._nodeB.Next;
-            }
+            if (contact._nodeB.Prev != null)
+                contact._nodeB.Prev.Next = contact._nodeB.Next;
+            if (contact._nodeB.Next != null)
+                contact._nodeB.Next.Prev = contact._nodeB.Prev;
 
 #if USE_ACTIVE_CONTACT_SET
 			if (ActiveContacts.Contains(contact))
-			{
 				ActiveContacts.Remove(contact);
-			}
 #endif
             contact.Destroy();
+            
+            // Insert into the pool.
+            contact.Next = _contactPoolList.Next;
+            _contactPoolList.Next = contact;
         }
 
         internal void Collide()
         {
+#if NET40 || NET45 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1
+            if (this.ContactCount > CollideMultithreadThreshold && System.Environment.ProcessorCount > 1)
+            {
+                CollideMultiCore();
+                return;
+            }
+#endif
+
             // Update awake contacts.
 #if USE_ACTIVE_CONTACT_SET
-			ActiveList.AddRange(ActiveContacts);
-
-			foreach (var c in ActiveList)
-			{
-#else
-            for (int i = 0; i < ContactList.Count; i++)
+            ActiveList.AddRange(ActiveContacts);
+            foreach (var tmpc in ActiveList)
+            {
+                Contact c = tmpc;
+#else
+            for (Contact c = ContactList.Next; c != ContactList;)
             {
-                Contact c = ContactList[i];
 #endif
                 Fixture fixtureA = c.FixtureA;
                 Fixture fixtureB = c.FixtureB;
@@ -292,7 +342,10 @@ namespace FarseerPhysics.Dynamics
 
                 //Do no try to collide disabled bodies
                 if (!bodyA.Enabled || !bodyB.Enabled)
+                {
+                    c = c.Next;
                     continue;
+                }
 
                 // Is this contact flagged for filtering?
                 if (c.FilterFlag)
@@ -301,6 +354,7 @@ namespace FarseerPhysics.Dynamics
                     if (bodyB.ShouldCollide(bodyA) == false)
                     {
                         Contact cNuke = c;
+                        c = c.Next;
                         Destroy(cNuke);
                         continue;
                     }
@@ -309,6 +363,7 @@ namespace FarseerPhysics.Dynamics
                     if (ShouldCollide(fixtureA, fixtureB) == false)
                     {
                         Contact cNuke = c;
+                        c = c.Next;
                         Destroy(cNuke);
                         continue;
                     }
@@ -317,6 +372,7 @@ namespace FarseerPhysics.Dynamics
                     if (ContactFilter != null && ContactFilter(fixtureA, fixtureB) == false)
                     {
                         Contact cNuke = c;
+                        c = c.Next;
                         Destroy(cNuke);
                         continue;
                     }
@@ -334,6 +390,7 @@ namespace FarseerPhysics.Dynamics
 #if USE_ACTIVE_CONTACT_SET
 					ActiveContacts.Remove(c);
 #endif
+                    c = c.Next;
                     continue;
                 }
 
@@ -346,12 +403,15 @@ namespace FarseerPhysics.Dynamics
                 if (overlap == false)
                 {
                     Contact cNuke = c;
+                    c = c.Next;
                     Destroy(cNuke);
                     continue;
                 }
 
                 // The contact persists.
                 c.Update(this);
+
+                c = c.Next;
             }
 
 #if USE_ACTIVE_CONTACT_SET
@@ -359,87 +419,196 @@ namespace FarseerPhysics.Dynamics
 #endif
         }
 
+        /// <summary>
+        /// A temporary list of contacts to be updated during Collide().
+        /// </summary>
+        List<Contact> updateList = new List<Contact>();
+
+#if NET40 || NET45 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1
+        internal void CollideMultiCore()
+        {
+            int lockOrder = 0;
+ 
+            // Update awake contacts.
+#if USE_ACTIVE_CONTACT_SET
+            ActiveList.AddRange(ActiveContacts);
+            foreach (var tmpc in ActiveList)
+            {
+                Contact c = tmpc;
+#else
+            for (Contact c = ContactList.Next; c != ContactList; )
+            {
+#endif
+                Fixture fixtureA = c.FixtureA;
+                Fixture fixtureB = c.FixtureB;
+                int indexA = c.ChildIndexA;
+                int indexB = c.ChildIndexB;
+                Body bodyA = fixtureA.Body;
+                Body bodyB = fixtureB.Body;
+
+                //Do no try to collide disabled bodies
+                if (!bodyA.Enabled || !bodyB.Enabled)
+                {
+                    c = c.Next;
+                    continue;
+                }
+
+                // Is this contact flagged for filtering?
+                if (c.FilterFlag)
+                {
+                    // Should these bodies collide?
+                    if (bodyB.ShouldCollide(bodyA) == false)
+                    {
+                        Contact cNuke = c;
+                        c = c.Next;
+                        Destroy(cNuke);
+                        continue;
+                    }
+
+                    // Check default filtering
+                    if (ShouldCollide(fixtureA, fixtureB) == false)
+                    {
+                        Contact cNuke = c;
+                        c = c.Next;
+                        Destroy(cNuke);
+                        continue;
+                    }
+
+                    // Check user filtering.
+                    if (ContactFilter != null && ContactFilter(fixtureA, fixtureB) == false)
+                    {
+                        Contact cNuke = c;
+                        c = c.Next;
+                        Destroy(cNuke);
+                        continue;
+                    }
+
+                    // Clear the filtering flag.
+                    c.FilterFlag = false;
+                }
+
+                bool activeA = bodyA.Awake && bodyA.BodyType != BodyType.Static;
+                bool activeB = bodyB.Awake && bodyB.BodyType != BodyType.Static;
+
+                // At least one body must be awake and it must be dynamic or kinematic.
+                if (activeA == false && activeB == false)
+                {
+#if USE_ACTIVE_CONTACT_SET
+					ActiveContacts.Remove(c);
+#endif
+                    c = c.Next;
+                    continue;
+                }
+
+                int proxyIdA = fixtureA.Proxies[indexA].ProxyId;
+                int proxyIdB = fixtureB.Proxies[indexB].ProxyId;
+
+                bool overlap = BroadPhase.TestOverlap(proxyIdA, proxyIdB);
+
+                // Here we destroy contacts that cease to overlap in the broad-phase.
+                if (overlap == false)
+                {
+                    Contact cNuke = c;
+                    c = c.Next;
+                    Destroy(cNuke);
+                    continue;
+                }
+
+                // The contact persists.
+                updateList.Add(c);
+                // Assign a unique id for lock order
+                bodyA._lockOrder = lockOrder++;
+                bodyB._lockOrder = lockOrder++;
+
+
+                c = c.Next;
+            }
+
+#if USE_ACTIVE_CONTACT_SET
+			ActiveList.Clear();
+#endif
+
+            // update contacts
+            System.Threading.Tasks.Parallel.ForEach<Contact>(updateList, (c) =>
+            {
+                // find lower order item
+                Fixture fixtureA = c.FixtureA;
+                Fixture fixtureB = c.FixtureB;
+
+                // find lower order item
+                Body orderedBodyA = fixtureA.Body;
+                Body orderedBodyB = fixtureB.Body;
+                int idA = orderedBodyA._lockOrder;
+                int idB = orderedBodyB._lockOrder;
+                if (idA == idB)
+                    throw new System.Exception();
+
+                if (idA > idB)
+                {
+                    orderedBodyA = fixtureB.Body;
+                    orderedBodyB = fixtureA.Body;
+                }
+
+                // obtain lock
+                for (; ; )
+                {
+                    if (System.Threading.Interlocked.CompareExchange(ref orderedBodyA._lock, 1, 0) == 0)
+                    {
+                        if (System.Threading.Interlocked.CompareExchange(ref orderedBodyB._lock, 1, 0) == 0)
+                            break;
+                        System.Threading.Interlocked.Exchange(ref orderedBodyA._lock, 0);
+                    }
+#if NET40 || NET45
+                    System.Threading.Thread.Sleep(0);
+#endif
+                }
+
+                c.Update(this);
+
+                System.Threading.Interlocked.Exchange(ref orderedBodyB._lock, 0);
+                System.Threading.Interlocked.Exchange(ref orderedBodyA._lock, 0);
+            });
+
+            updateList.Clear();
+        }
+#endif
+
         private static bool ShouldCollide(Fixture fixtureA, Fixture fixtureB)
         {
-            if (Settings.UseFPECollisionCategories)
+            if (fixtureA.CollisionGroup != 0 && fixtureA.CollisionGroup == fixtureB.CollisionGroup)
             {
-                if ((fixtureA.CollisionGroup == fixtureB.CollisionGroup) &&
-                    fixtureA.CollisionGroup != 0 && fixtureB.CollisionGroup != 0)
-                    return false;
-
-                if (((fixtureA.CollisionCategories & fixtureB.CollidesWith) ==
-                     Category.None) &
-                    ((fixtureB.CollisionCategories & fixtureA.CollidesWith) ==
-                     Category.None))
-                    return false;
-
-                if (fixtureA.IsFixtureIgnored(fixtureB) ||
-                    fixtureB.IsFixtureIgnored(fixtureA))
-                    return false;
-
-                return true;
+                return (fixtureA.CollisionGroup > 0);
             }
 
-            if (fixtureA.CollisionGroup == fixtureB.CollisionGroup &&
-                fixtureA.CollisionGroup != 0)
-            {
-                return fixtureA.CollisionGroup > 0;
-            }
-
-            bool collide = (fixtureA.CollidesWith & fixtureB.CollisionCategories) != 0 &&
-                           (fixtureA.CollisionCategories & fixtureB.CollidesWith) != 0;
-
-            if (collide)
-            {
-                if (fixtureA.IsFixtureIgnored(fixtureB) ||
-                    fixtureB.IsFixtureIgnored(fixtureA))
-                {
-                    return false;
-                }
-            }
+            bool collide = ((fixtureA.CollidesWith & fixtureB.CollisionCategories) != 0) &&
+                            ((fixtureB.CollidesWith & fixtureA.CollisionCategories) != 0);
 
             return collide;
         }
 
-        internal void UpdateContacts(ContactEdge contactEdge, bool value)
+#if USE_ACTIVE_CONTACT_SET
+        internal void UpdateActiveContacts(ContactEdge ContactList, bool value)
         {
-#if USE_ACTIVE_CONTACT_SET
-			if(value)
-			{
-				while(contactEdge != null)
-				{
-					var c = contactEdge.Contact;
-					if (!ActiveContacts.Contains(c))
-					{
-						ActiveContacts.Add(c);
-					}
-					contactEdge = contactEdge.Next;
-				}
-			}
-			else
-			{
-				while (contactEdge != null)
-				{
-					var c = contactEdge.Contact;
-					if (!contactEdge.Other.Awake)
-					{
-						if (ActiveContacts.Contains(c))
-						{
-							ActiveContacts.Remove(c);
-						}
-					}
-					contactEdge = contactEdge.Next;
-				}
-			}
-#endif
+            if(value)
+            {
+                for (var contactEdge = ContactList; contactEdge != null; contactEdge = contactEdge.Next)
+                {
+                    if (!ActiveContacts.Contains(contactEdge.Contact))
+                        ActiveContacts.Add(contactEdge.Contact);
+                }
+            }
+            else
+            {
+                for (var contactEdge = ContactList; contactEdge != null; contactEdge = contactEdge.Next)
+                {
+                    if (!contactEdge.Other.Awake)
+                    {
+                        if (ActiveContacts.Contains(contactEdge.Contact))
+                            ActiveContacts.Remove(contactEdge.Contact);
+                    }
+                }
+            }
         }
-
-#if USE_ACTIVE_CONTACT_SET
-		internal void RemoveActiveContact(Contact contact)
-		{
-			if (ActiveContacts.Contains(contact))
-				ActiveContacts.Remove(contact);
-		}
 #endif
     }
 }

Libraries/Farseer Physics Engine 3.5/Dynamics/Contacts/Contact.cs

@@ -1,3 +1,10 @@
+// Copyright (c) 2017 Kastellanos Nikolaos
+
+/* Original source Farseer Physics Engine:
+ * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
+ * Microsoft Permissive License (Ms-PL) v1.1
+ */
+
 /*
 * Farseer Physics Engine:
 * Copyright (c) 2012 Ian Qvist
@@ -19,7 +26,6 @@
 * misrepresented as being the original software. 
 * 3. This notice may not be removed or altered from any source distribution. 
 */
-//#define USE_ACTIVE_CONTACT_SET
 
 using System.Collections.Generic;
 using System.Diagnostics;
@@ -42,22 +48,22 @@ namespace FarseerPhysics.Dynamics.Contacts
         /// <summary>
         /// The contact
         /// </summary>
-        public Contact Contact;
-
-        /// <summary>
-        /// The next contact edge in the body's contact list
-        /// </summary>
-        public ContactEdge Next;
+        public Contact Contact { get; internal set; }
 
         /// <summary>
         /// Provides quick access to the other body attached.
         /// </summary>
-        public Body Other;
+        public Body Other { get; internal set; }
 
+        /// <summary>
+        /// The next contact edge in the body's contact list
+        /// </summary>
+        public ContactEdge Next { get; internal set; }
+        
         /// <summary>
         /// The previous contact edge in the body's contact list
         /// </summary>
-        public ContactEdge Prev;
+        public ContactEdge Prev { get; internal set; }
     }
 
     /// <summary>
@@ -108,8 +114,9 @@ namespace FarseerPhysics.Dynamics.Contacts
         internal int _toiCount;
         internal float _toi;
 
-        public Fixture FixtureA;
-        public Fixture FixtureB;
+        public Fixture FixtureA { get; internal set; }
+        public Fixture FixtureB { get; internal set; }
+
         public float Friction { get; set; }
         public float Restitution { get; set; }
 
@@ -142,6 +149,18 @@ namespace FarseerPhysics.Dynamics.Contacts
         /// <value>The child index B.</value>
         public int ChildIndexB { get; internal set; }
 
+        /// <summary>
+        /// Get the next contact in the world's contact list.
+        /// </summary>
+        /// <value>The next.</value>
+        public Contact Next { get; internal set; }
+
+        /// <summary>
+        /// Get the previous contact in the world's contact list.
+        /// </summary>
+        /// <value>The prev.</value>
+        public Contact Prev { get; internal set; }
+
         /// <summary>
         /// Determines whether this contact is touching.
         /// </summary>
@@ -164,7 +183,7 @@ namespace FarseerPhysics.Dynamics.Contacts
             Friction = Settings.MixFriction(FixtureA.Friction, FixtureB.Friction);
         }
 
-        private Contact(Fixture fA, int indexA, Fixture fB, int indexB)
+        protected Contact(Fixture fA, int indexA, Fixture fB, int indexB)
         {
             Reset(fA, indexA, fB, indexB);
         }
@@ -198,15 +217,18 @@ namespace FarseerPhysics.Dynamics.Contacts
 
             Manifold.PointCount = 0;
 
+            Next = null;
+            Prev = null;
+
             _nodeA.Contact = null;
-            _nodeA.Prev = null;
-            _nodeA.Next = null;
             _nodeA.Other = null;
+            _nodeA.Next = null;
+            _nodeA.Prev = null;
 
             _nodeB.Contact = null;
-            _nodeB.Prev = null;
-            _nodeB.Next = null;
             _nodeB.Other = null;
+            _nodeB.Next = null;
+            _nodeB.Prev = null;
 
             _toiCount = 0;
 
@@ -295,45 +317,37 @@ namespace FarseerPhysics.Dynamics.Contacts
             {
                 if (touching)
                 {
-                    if (Settings.AllCollisionCallbacksAgree)
-                    {
-                        bool enabledA = true, enabledB = true;
+                    bool enabledA = true, enabledB = true;
 
-                        // Report the collision to both participants. Track which ones returned true so we can
-                        // later call OnSeparation if the contact is disabled for a different reason.
-                        if (FixtureA.OnCollision != null)
-                            foreach (OnCollisionEventHandler handler in FixtureA.OnCollision.GetInvocationList())
-                                enabledA = handler(FixtureA, FixtureB, this) && enabledA;
+                    // Report the collision to both participants. Track which ones returned true so we can
+                    // later call OnSeparation if the contact is disabled for a different reason.
+                    if (FixtureA.OnCollision != null)
+                        foreach (OnCollisionEventHandler handler in FixtureA.OnCollision.GetInvocationList())
+                            enabledA = handler(FixtureA, FixtureB, this) && enabledA;
 
-                        // Reverse the order of the reported fixtures. The first fixture is always the one that the
-                        // user subscribed to.
-                        if (FixtureB.OnCollision != null)
-                            foreach (OnCollisionEventHandler handler in FixtureB.OnCollision.GetInvocationList())
-                                enabledB = handler(FixtureB, FixtureA, this) && enabledB;
+                    // Reverse the order of the reported fixtures. The first fixture is always the one that the
+                    // user subscribed to.
+                    if (FixtureB.OnCollision != null)
+                        foreach (OnCollisionEventHandler handler in FixtureB.OnCollision.GetInvocationList())
+                            enabledB = handler(FixtureB, FixtureA, this) && enabledB;
 
-                        Enabled = enabledA && enabledB;
+                    // Report the collision to both bodies:
+                    if (FixtureA.Body != null && FixtureA.Body.onCollisionEventHandler != null)
+                        foreach (OnCollisionEventHandler handler in FixtureA.Body.onCollisionEventHandler.GetInvocationList())
+                            enabledA = handler(FixtureA, FixtureB, this) && enabledA;
 
-                        // BeginContact can also return false and disable the contact
-                        if (enabledA && enabledB && contactManager.BeginContact != null)
-                            Enabled = contactManager.BeginContact(this);
-                    }
-                    else
-                    {
-                        //Report the collision to both participants:
-                        if (FixtureA.OnCollision != null)
-                            foreach (OnCollisionEventHandler handler in FixtureA.OnCollision.GetInvocationList())
-                                Enabled = handler(FixtureA, FixtureB, this);
+                    // Reverse the order of the reported fixtures. The first fixture is always the one that the
+                    // user subscribed to.
+                    if (FixtureB.Body != null && FixtureB.Body.onCollisionEventHandler != null)
+                        foreach (OnCollisionEventHandler handler in FixtureB.Body.onCollisionEventHandler.GetInvocationList())
+                            enabledB = handler(FixtureB, FixtureA, this) && enabledB;
 
-                        //Reverse the order of the reported fixtures. The first fixture is always the one that the
-                        //user subscribed to.
-                        if (FixtureB.OnCollision != null)
-                            foreach (OnCollisionEventHandler handler in FixtureB.OnCollision.GetInvocationList())
-                                Enabled = handler(FixtureB, FixtureA, this);
 
-                        //BeginContact can also return false and disable the contact
-                        if (contactManager.BeginContact != null)
-                            Enabled = contactManager.BeginContact(this);
-                    }
+                    Enabled = enabledA && enabledB;
+
+                    // BeginContact can also return false and disable the contact
+                    if (enabledA && enabledB && contactManager.BeginContact != null)
+                        Enabled = contactManager.BeginContact(this);
 
                     // If the user disabled the contact (needed to exclude it in TOI solver) at any point by
                     // any of the callbacks, we need to mark it as not touching and call any separation
@@ -348,12 +362,22 @@ namespace FarseerPhysics.Dynamics.Contacts
                 {
                     //Report the separation to both participants:
                     if (FixtureA != null && FixtureA.OnSeparation != null)
-                        FixtureA.OnSeparation(FixtureA, FixtureB);
+                        FixtureA.OnSeparation(FixtureA, FixtureB, this);
 
                     //Reverse the order of the reported fixtures. The first fixture is always the one that the
                     //user subscribed to.
                     if (FixtureB != null && FixtureB.OnSeparation != null)
-                        FixtureB.OnSeparation(FixtureB, FixtureA);
+                        FixtureB.OnSeparation(FixtureB, FixtureA, this);
+                    
+                    //Report the separation to both bodies:
+                    if (FixtureA != null && FixtureA.Body != null && FixtureA.Body.onSeparationEventHandler != null)
+                        FixtureA.Body.onSeparationEventHandler(FixtureA, FixtureB, this);
+
+                    //Reverse the order of the reported fixtures. The first fixture is always the one that the
+                    //user subscribed to.
+                    if (FixtureB != null && FixtureB.Body != null && FixtureB.Body.onSeparationEventHandler != null)
+                        FixtureB.Body.onSeparationEventHandler(FixtureB, FixtureA, this);
+
 
                     if (contactManager.EndContact != null)
                         contactManager.EndContact(this);
@@ -405,7 +429,7 @@ namespace FarseerPhysics.Dynamics.Contacts
             }
         }
 
-        internal static Contact Create(Fixture fixtureA, int indexA, Fixture fixtureB, int indexB)
+        internal static Contact Create(ContactManager contactManager, Fixture fixtureA, int indexA, Fixture fixtureB, int indexB)
         {
             ShapeType type1 = fixtureA.Shape.ShapeType;
             ShapeType type2 = fixtureB.Shape.ShapeType;
@@ -413,32 +437,32 @@ namespace FarseerPhysics.Dynamics.Contacts
             Debug.Assert(ShapeType.Unknown < type1 && type1 < ShapeType.TypeCount);
             Debug.Assert(ShapeType.Unknown < type2 && type2 < ShapeType.TypeCount);
 
-            Contact c;
-            Queue<Contact> pool = fixtureA.Body._world._contactPool;
-            if (pool.Count > 0)
+            Contact c = null;
+            var contactPoolList = contactManager._contactPoolList;
+            if (contactPoolList.Next != contactPoolList)
+            {                
+                // get first item in the pool.
+                c = contactPoolList.Next;
+                // Remove from the pool.
+                contactPoolList.Next = c.Next;
+                c.Next = null;
+            }
+            // Edge+Polygon is non-symetrical due to the way Erin handles collision type registration.
+            if ((type1 >= type2 || (type1 == ShapeType.Edge && type2 == ShapeType.Polygon)) && !(type2 == ShapeType.Edge && type1 == ShapeType.Polygon))
             {
-                c = pool.Dequeue();
-                if ((type1 >= type2 || (type1 == ShapeType.Edge && type2 == ShapeType.Polygon)) && !(type2 == ShapeType.Edge && type1 == ShapeType.Polygon))
-                {
-                    c.Reset(fixtureA, indexA, fixtureB, indexB);
-                }
+                if (c == null)
+                    c = new Contact(fixtureA, indexA, fixtureB, indexB);
                 else
-                {
-                    c.Reset(fixtureB, indexB, fixtureA, indexA);
-                }
+                    c.Reset(fixtureA, indexA, fixtureB, indexB);
             }
             else
             {
-                // Edge+Polygon is non-symetrical due to the way Erin handles collision type registration.
-                if ((type1 >= type2 || (type1 == ShapeType.Edge && type2 == ShapeType.Polygon)) && !(type2 == ShapeType.Edge && type1 == ShapeType.Polygon))
-                {
-                    c = new Contact(fixtureA, indexA, fixtureB, indexB);
-                }
-                else
-                {
+                if (c == null)
                     c = new Contact(fixtureB, indexB, fixtureA, indexA);
-                }
+                else
+                    c.Reset(fixtureB, indexB, fixtureA, indexA);
             }
+        
 
             c._type = _registers[(int)type1, (int)type2];
 
@@ -447,11 +471,6 @@ namespace FarseerPhysics.Dynamics.Contacts
 
         internal void Destroy()
         {
-#if USE_ACTIVE_CONTACT_SET
-            FixtureA.Body.World.ContactManager.RemoveActiveContact(this);
-#endif
-            FixtureA.Body._world._contactPool.Enqueue(this);
-
             if (Manifold.PointCount > 0 && FixtureA.IsSensor == false && FixtureB.IsSensor == false)
             {
                 FixtureA.Body.Awake = true;

Libraries/Farseer Physics Engine 3.5/Dynamics/Contacts/ContactListHead.cs

@@ -0,0 +1,69 @@
+// Copyright (c) 2017 Kastellanos Nikolaos
+
+using System;
+using System.Collections;
+using System.Collections.Generic;
+
+namespace FarseerPhysics.Dynamics.Contacts
+{
+    /// <summary>
+    /// Head of a circular doubly linked list.
+    /// </summary>
+    public class ContactListHead : Contact , IEnumerable<Contact>
+    {
+        internal ContactListHead(): base(null, 0, null, 0)
+        {
+            this.Prev = this;
+            this.Next = this;
+        }
+        
+        IEnumerator<Contact> IEnumerable<Contact>.GetEnumerator()
+        {
+            return new ContactEnumerator(this);
+        }
+
+        IEnumerator IEnumerable.GetEnumerator()
+        {
+            return new ContactEnumerator(this);
+        }
+
+        
+        #region Nested type: ContactEnumerator
+
+        private struct ContactEnumerator : IEnumerator<Contact>
+        {
+            private ContactListHead _head;
+            private Contact _current;
+
+            public Contact Current { get { return _current; } }
+            object IEnumerator.Current { get { return _current; } }
+
+
+            public ContactEnumerator(ContactListHead contact)
+            {
+                _head = contact;
+                 _current = _head;
+            }
+
+            public void Reset()
+            {
+                _current = _head;
+            }
+
+            public bool MoveNext()
+            {
+                _current = _current.Next;
+                return (_current != _head);
+            }
+
+            public void Dispose()
+            {
+                _head = null;
+                _current = null;
+            }
+        }
+
+        #endregion
+
+    }
+}

Libraries/Farseer Physics Engine 3.5/Dynamics/Contacts/ContactSolver.cs

@@ -1,3 +1,10 @@
+// Copyright (c) 2017 Kastellanos Nikolaos
+
+/* Original source Farseer Physics Engine:
+ * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
+ * Microsoft Permissive License (Ms-PL) v1.1
+ */
+
 /*
 * Farseer Physics Engine:
 * Copyright (c) 2012 Ian Qvist
@@ -25,7 +32,12 @@ using System.Diagnostics;
 using FarseerPhysics.Collision;
 using FarseerPhysics.Collision.Shapes;
 using FarseerPhysics.Common;
+using FarseerPhysics.Common.Maths;
 using Microsoft.Xna.Framework;
+#if NET40 || NET45 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1
+using System.Threading;
+using System.Threading.Tasks;
+#endif
 
 namespace FarseerPhysics.Dynamics.Contacts
 {
@@ -82,21 +94,24 @@ namespace FarseerPhysics.Dynamics.Contacts
 
     public class ContactSolver
     {
-        public TimeStep _step;
         public Position[] _positions;
         public Velocity[] _velocities;
         public ContactPositionConstraint[] _positionConstraints;
         public ContactVelocityConstraint[] _velocityConstraints;
         public Contact[] _contacts;
         public int _count;
+        int _velocityConstraintsMultithreadThreshold;
+        int _positionConstraintsMultithreadThreshold;
 
-        public void Reset(TimeStep step, int count, Contact[] contacts, Position[] positions, Velocity[] velocities, bool warmstarting = Settings.EnableWarmstarting)
+        public void Reset(ref TimeStep step, int count, Contact[] contacts, Position[] positions, Velocity[] velocities,
+            int velocityConstraintsMultithreadThreshold, int positionConstraintsMultithreadThreshold)
         {
-            _step = step;
             _count = count;
             _positions = positions;
             _velocities = velocities;
             _contacts = contacts;
+            _velocityConstraintsMultithreadThreshold = velocityConstraintsMultithreadThreshold;
+            _positionConstraintsMultithreadThreshold = positionConstraintsMultithreadThreshold;
 
             // grow the array
             if (_velocityConstraints == null || _velocityConstraints.Length < count)
@@ -169,10 +184,10 @@ namespace FarseerPhysics.Dynamics.Contacts
                     ManifoldPoint cp = manifold.Points[j];
                     VelocityConstraintPoint vcp = vc.points[j];
 
-                    if (Settings.EnableWarmstarting)
+                    if (step.warmStarting)
                     {
-                        vcp.normalImpulse = _step.dtRatio * cp.NormalImpulse;
-                        vcp.tangentImpulse = _step.dtRatio * cp.TangentImpulse;
+                        vcp.normalImpulse = step.dtRatio * cp.NormalImpulse;
+                        vcp.tangentImpulse = step.dtRatio * cp.TangentImpulse;
                     }
                     else
                     {
@@ -224,18 +239,17 @@ namespace FarseerPhysics.Dynamics.Contacts
 
                 Debug.Assert(manifold.PointCount > 0);
 
-                Transform xfA = new Transform();
-                Transform xfB = new Transform();
-                xfA.q.Set(aA);
-                xfB.q.Set(aB);
-                xfA.p = cA - MathUtils.Mul(xfA.q, localCenterA);
-                xfB.p = cB - MathUtils.Mul(xfB.q, localCenterB);
+                Transform xfA = new Transform(Vector2.Zero, aA);
+                Transform xfB = new Transform(Vector2.Zero, aB);
+                xfA.p = cA - Complex.Multiply(ref localCenterA, ref xfA.q);
+                xfB.p = cB - Complex.Multiply(ref localCenterB, ref xfB.q);
 
                 Vector2 normal;
                 FixedArray2<Vector2> points;
                 WorldManifold.Initialize(ref manifold, ref xfA, radiusA, ref xfB, radiusB, out normal, out points);
 
                 vc.normal = normal;
+                Vector2 tangent = MathUtils.Rot270(ref vc.normal);
 
                 int pointCount = vc.pointCount;
                 for (int j = 0; j < pointCount; ++j)
@@ -245,17 +259,16 @@ namespace FarseerPhysics.Dynamics.Contacts
                     vcp.rA = points[j] - cA;
                     vcp.rB = points[j] - cB;
 
-                    float rnA = MathUtils.Cross(vcp.rA, vc.normal);
-                    float rnB = MathUtils.Cross(vcp.rB, vc.normal);
+                    float rnA = MathUtils.Cross(ref vcp.rA, ref vc.normal);
+                    float rnB = MathUtils.Cross(ref vcp.rB, ref vc.normal);
 
                     float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
 
                     vcp.normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
 
-                    Vector2 tangent = MathUtils.Cross(vc.normal, 1.0f);
 
-                    float rtA = MathUtils.Cross(vcp.rA, tangent);
-                    float rtB = MathUtils.Cross(vcp.rB, tangent);
+                    float rtA = MathUtils.Cross(ref vcp.rA, ref tangent);
+                    float rtB = MathUtils.Cross(ref vcp.rB, ref tangent);
 
                     float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
 
@@ -263,7 +276,7 @@ namespace FarseerPhysics.Dynamics.Contacts
 
                     // Setup a velocity bias for restitution.
                     vcp.velocityBias = 0.0f;
-                    float vRel = Vector2.Dot(vc.normal, vB + MathUtils.Cross(wB, vcp.rB) - vA - MathUtils.Cross(wA, vcp.rA));
+                    float vRel = Vector2.Dot(vc.normal, vB + MathUtils.Cross(wB, ref vcp.rB) - vA - MathUtils.Cross(wA, ref vcp.rA));
                     if (vRel < -Settings.VelocityThreshold)
                     {
                         vcp.velocityBias = -vc.restitution * vRel;
@@ -276,10 +289,10 @@ namespace FarseerPhysics.Dynamics.Contacts
                     VelocityConstraintPoint vcp1 = vc.points[0];
                     VelocityConstraintPoint vcp2 = vc.points[1];
 
-                    float rn1A = MathUtils.Cross(vcp1.rA, vc.normal);
-                    float rn1B = MathUtils.Cross(vcp1.rB, vc.normal);
-                    float rn2A = MathUtils.Cross(vcp2.rA, vc.normal);
-                    float rn2B = MathUtils.Cross(vcp2.rB, vc.normal);
+                    float rn1A = MathUtils.Cross(ref vcp1.rA, ref vc.normal);
+                    float rn1B = MathUtils.Cross(ref vcp1.rB, ref vc.normal);
+                    float rn2A = MathUtils.Cross(ref vcp2.rA, ref vc.normal);
+                    float rn2B = MathUtils.Cross(ref vcp2.rB, ref vc.normal);
 
                     float k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
                     float k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
@@ -325,15 +338,15 @@ namespace FarseerPhysics.Dynamics.Contacts
                 float wB = _velocities[indexB].w;
 
                 Vector2 normal = vc.normal;
-                Vector2 tangent = MathUtils.Cross(normal, 1.0f);
+                Vector2 tangent = MathUtils.Rot270(ref normal);
 
                 for (int j = 0; j < pointCount; ++j)
                 {
                     VelocityConstraintPoint vcp = vc.points[j];
                     Vector2 P = vcp.normalImpulse * normal + vcp.tangentImpulse * tangent;
-                    wA -= iA * MathUtils.Cross(vcp.rA, P);
+                    wA -= iA * MathUtils.Cross(ref vcp.rA, ref P);
                     vA -= mA * P;
-                    wB += iB * MathUtils.Cross(vcp.rB, P);
+                    wB += iB * MathUtils.Cross(ref vcp.rB, ref P);
                     vB += mB * P;
                 }
 
@@ -346,10 +359,115 @@ namespace FarseerPhysics.Dynamics.Contacts
 
         public void SolveVelocityConstraints()
         {
-            for (int i = 0; i < _count; ++i)
+            if (_count >= _velocityConstraintsMultithreadThreshold && System.Environment.ProcessorCount > 1)
+            {
+                if (_count == 0) return;
+                var batchSize = (int)Math.Ceiling((float)_count / System.Environment.ProcessorCount);
+                var batches = (int)Math.Ceiling((float)_count / batchSize);
+
+#if NET40 || NET45
+                SolveVelocityConstraintsWaitLock.Reset(batches);
+                for (int i = 0; i < batches; i++)
+                {
+                    var start = i * batchSize;
+                    var end = Math.Min(start + batchSize, _count);
+                    ThreadPool.QueueUserWorkItem( SolveVelocityConstraintsCallback, SolveVelocityConstraintsState.Get(this, start,end));                    
+                }
+                // We avoid SolveVelocityConstraintsWaitLock.Wait(); because it spins a few milliseconds before going into sleep. Going into sleep(0) directly in a while loop is faster.
+                while (SolveVelocityConstraintsWaitLock.CurrentCount > 0)
+                    Thread.Sleep(0);
+#elif PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1
+                Parallel.For(0, batches, (i) =>
+                {
+                    var start = i * batchSize;
+                    var end = Math.Min(start + batchSize, _count);
+                    SolveVelocityConstraints(start, end);
+                });
+#else
+                SolveVelocityConstraints(0, _count);
+#endif
+            }
+            else
+            {                
+                SolveVelocityConstraints(0, _count);
+            }
+
+            return;
+        }
+
+#if NET40 || NET45
+        CountdownEvent SolveVelocityConstraintsWaitLock = new CountdownEvent(0);
+        static void SolveVelocityConstraintsCallback(object state)
+        {
+            var svcState = (SolveVelocityConstraintsState)state;
+
+            svcState.ContactSolver.SolveVelocityConstraints(svcState.Start, svcState.End);
+            SolveVelocityConstraintsState.Return(svcState);
+            svcState.ContactSolver.SolveVelocityConstraintsWaitLock.Signal();
+        }
+
+        private class SolveVelocityConstraintsState
+        {
+            private static System.Collections.Concurrent.ConcurrentQueue<SolveVelocityConstraintsState> _queue = new System.Collections.Concurrent.ConcurrentQueue<SolveVelocityConstraintsState>(); // pool
+
+            public ContactSolver ContactSolver;
+            public int Start { get; private set; }
+            public int End { get; private set; }
+
+            private SolveVelocityConstraintsState()
+            {
+            }
+
+            internal static object Get(ContactSolver contactSolver, int start, int end)
+            {
+                SolveVelocityConstraintsState result;
+                if (!_queue.TryDequeue(out result))
+                    result = new SolveVelocityConstraintsState();
+
+                result.ContactSolver = contactSolver;
+                result.Start = start;
+                result.End = end;
+
+                return result;
+            }
+
+            internal static void Return(object state)
+            {
+                _queue.Enqueue((SolveVelocityConstraintsState)state);
+            }
+        }
+#endif
+
+        private void SolveVelocityConstraints(int start, int end)
+        {
+            for (int i = start; i < end; ++i)
             {
                 ContactVelocityConstraint vc = _velocityConstraints[i];
 
+#if NET40 || NET45 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1
+                // find lower order item
+                int orderedIndexA = vc.indexA;
+                int orderedIndexB = vc.indexB;
+                if (orderedIndexB < orderedIndexA)
+                {
+                    orderedIndexA = vc.indexB;
+                    orderedIndexB = vc.indexA;
+                }
+                
+                for (; ; )
+                {
+                    if (Interlocked.CompareExchange(ref _velocities[orderedIndexA].Lock, 1, 0) == 0)
+                    {
+                        if (Interlocked.CompareExchange(ref _velocities[orderedIndexB].Lock, 1, 0) == 0)
+                            break;
+                        System.Threading.Interlocked.Exchange(ref _velocities[orderedIndexA].Lock, 0);
+                    }
+#if NET40 || NET45
+                    Thread.Sleep(0);
+#endif
+                }
+#endif
+
                 int indexA = vc.indexA;
                 int indexB = vc.indexB;
                 float mA = vc.invMassA;
@@ -364,7 +482,7 @@ namespace FarseerPhysics.Dynamics.Contacts
                 float wB = _velocities[indexB].w;
 
                 Vector2 normal = vc.normal;
-                Vector2 tangent = MathUtils.Cross(normal, 1.0f);
+                Vector2 tangent = MathUtils.Rot270(ref normal);
                 float friction = vc.friction;
 
                 Debug.Assert(pointCount == 1 || pointCount == 2);
@@ -376,7 +494,7 @@ namespace FarseerPhysics.Dynamics.Contacts
                     VelocityConstraintPoint vcp = vc.points[j];
 
                     // Relative velocity at contact
-                    Vector2 dv = vB + MathUtils.Cross(wB, vcp.rB) - vA - MathUtils.Cross(wA, vcp.rA);
+                    Vector2 dv = vB + MathUtils.Cross(wB, ref vcp.rB) - vA - MathUtils.Cross(wA, ref vcp.rA);
 
                     // Compute tangent force
                     float vt = Vector2.Dot(dv, tangent) - vc.tangentSpeed;
@@ -392,10 +510,10 @@ namespace FarseerPhysics.Dynamics.Contacts
                     Vector2 P = lambda * tangent;
 
                     vA -= mA * P;
-                    wA -= iA * MathUtils.Cross(vcp.rA, P);
+                    wA -= iA * MathUtils.Cross(ref vcp.rA, ref P);
 
                     vB += mB * P;
-                    wB += iB * MathUtils.Cross(vcp.rB, P);
+                    wB += iB * MathUtils.Cross(ref vcp.rB, ref P);
                 }
 
                 // Solve normal constraints
@@ -404,7 +522,7 @@ namespace FarseerPhysics.Dynamics.Contacts
                     VelocityConstraintPoint vcp = vc.points[0];
 
                     // Relative velocity at contact
-                    Vector2 dv = vB + MathUtils.Cross(wB, vcp.rB) - vA - MathUtils.Cross(wA, vcp.rA);
+                    Vector2 dv = vB + MathUtils.Cross(wB, ref vcp.rB) - vA - MathUtils.Cross(wA, ref vcp.rA);
 
                     // Compute normal impulse
                     float vn = Vector2.Dot(dv, normal);
@@ -418,10 +536,10 @@ namespace FarseerPhysics.Dynamics.Contacts
                     // Apply contact impulse
                     Vector2 P = lambda * normal;
                     vA -= mA * P;
-                    wA -= iA * MathUtils.Cross(vcp.rA, P);
+                    wA -= iA * MathUtils.Cross(ref vcp.rA, ref P);
 
                     vB += mB * P;
-                    wB += iB * MathUtils.Cross(vcp.rB, P);
+                    wB += iB * MathUtils.Cross(ref vcp.rB, ref P);
                 }
                 else
                 {
@@ -465,8 +583,8 @@ namespace FarseerPhysics.Dynamics.Contacts
                     Debug.Assert(a.X >= 0.0f && a.Y >= 0.0f);
 
                     // Relative velocity at contact
-                    Vector2 dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA);
-                    Vector2 dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA);
+                    Vector2 dv1 = vB + MathUtils.Cross(wB, ref cp1.rB) - vA - MathUtils.Cross(wA, ref cp1.rA);
+                    Vector2 dv2 = vB + MathUtils.Cross(wB, ref cp2.rB) - vA - MathUtils.Cross(wA, ref cp2.rA);
 
                     // Compute normal velocity
                     float vn1 = Vector2.Dot(dv1, normal);
@@ -477,7 +595,7 @@ namespace FarseerPhysics.Dynamics.Contacts
                     b.Y = vn2 - cp2.velocityBias;
 
                     // Compute b'
-                    b -= MathUtils.Mul(ref vc.K, a);
+                    b -= MathUtils.Mul(ref vc.K, ref a);
 
                     const float k_errorTol = 1e-3f;
                     //B2_NOT_USED(k_errorTol);
@@ -493,7 +611,7 @@ namespace FarseerPhysics.Dynamics.Contacts
                         //
                         // x = - inv(A) * b'
                         //
-                        Vector2 x = -MathUtils.Mul(ref vc.normalMass, b);
+                        Vector2 x = -MathUtils.Mul(ref vc.normalMass, ref b);
 
                         if (x.X >= 0.0f && x.Y >= 0.0f)
                         {
@@ -504,10 +622,10 @@ namespace FarseerPhysics.Dynamics.Contacts
                             Vector2 P1 = d.X * normal;
                             Vector2 P2 = d.Y * normal;
                             vA -= mA * (P1 + P2);
-                            wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));
+                            wA -= iA * (MathUtils.Cross(ref cp1.rA, ref P1) + MathUtils.Cross(ref cp2.rA, ref P2));
 
                             vB += mB * (P1 + P2);
-                            wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
+                            wB += iB * (MathUtils.Cross(ref cp1.rB, ref P1) + MathUtils.Cross(ref cp2.rB, ref P2));
 
                             // Accumulate
                             cp1.normalImpulse = x.X;
@@ -548,10 +666,10 @@ namespace FarseerPhysics.Dynamics.Contacts
                             Vector2 P1 = d.X * normal;
                             Vector2 P2 = d.Y * normal;
                             vA -= mA * (P1 + P2);
-                            wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));
+                            wA -= iA * (MathUtils.Cross(ref cp1.rA, ref P1) + MathUtils.Cross(ref cp2.rA, ref P2));
 
                             vB += mB * (P1 + P2);
-                            wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
+                            wB += iB * (MathUtils.Cross(ref cp1.rB, ref P1) + MathUtils.Cross(ref cp2.rB, ref P2));
 
                             // Accumulate
                             cp1.normalImpulse = x.X;
@@ -590,10 +708,10 @@ namespace FarseerPhysics.Dynamics.Contacts
                             Vector2 P1 = d.X * normal;
                             Vector2 P2 = d.Y * normal;
                             vA -= mA * (P1 + P2);
-                            wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));
+                            wA -= iA * (MathUtils.Cross(ref cp1.rA, ref P1) + MathUtils.Cross(ref cp2.rA, ref P2));
 
                             vB += mB * (P1 + P2);
-                            wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
+                            wB += iB * (MathUtils.Cross(ref cp1.rB, ref P1) + MathUtils.Cross(ref cp2.rB, ref P2));
 
                             // Accumulate
                             cp1.normalImpulse = x.X;
@@ -630,10 +748,10 @@ namespace FarseerPhysics.Dynamics.Contacts
                             Vector2 P1 = d.X * normal;
                             Vector2 P2 = d.Y * normal;
                             vA -= mA * (P1 + P2);
-                            wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));
+                            wA -= iA * (MathUtils.Cross(ref cp1.rA, ref P1) + MathUtils.Cross(ref cp2.rA, ref P2));
 
                             vB += mB * (P1 + P2);
-                            wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
+                            wB += iB * (MathUtils.Cross(ref cp1.rB, ref P1) + MathUtils.Cross(ref cp2.rB, ref P2));
 
                             // Accumulate
                             cp1.normalImpulse = x.X;
@@ -651,6 +769,11 @@ namespace FarseerPhysics.Dynamics.Contacts
                 _velocities[indexA].w = wA;
                 _velocities[indexB].v = vB;
                 _velocities[indexB].w = wB;
+
+#if NET40 || NET45 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1
+                System.Threading.Interlocked.Exchange(ref _velocities[orderedIndexB].Lock, 0);
+                System.Threading.Interlocked.Exchange(ref _velocities[orderedIndexA].Lock, 0);
+#endif
             }
         }
 
@@ -674,12 +797,71 @@ namespace FarseerPhysics.Dynamics.Contacts
         }
 
         public bool SolvePositionConstraints()
+        {
+            bool contactsOkay = false;
+
+            if (_count >= _positionConstraintsMultithreadThreshold && System.Environment.ProcessorCount > 1)
+            {
+                if (_count == 0) return true;
+                var batchSize = (int)Math.Ceiling((float)_count / System.Environment.ProcessorCount);
+                var batches = (int)Math.Ceiling((float)_count / batchSize);
+                
+#if NET40 || NET45 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1
+                Parallel.For(0, batches, (i) =>
+                {
+                    var start = i * batchSize;
+                    var end = Math.Min(start + batchSize, _count);
+                    var res = SolvePositionConstraints(start, end);
+                    lock (this)
+                    {
+                        contactsOkay = contactsOkay || res;
+                    }
+                });
+#else            
+                contactsOkay = SolvePositionConstraints(0, _count);
+#endif
+            }
+            else
+            {
+                contactsOkay = SolvePositionConstraints(0, _count);
+            }
+            
+            return contactsOkay;
+        }
+
+        private bool SolvePositionConstraints(int start, int end)
         {
             float minSeparation = 0.0f;
 
-            for (int i = 0; i < _count; ++i)
+            for (int i = start; i < end; ++i)
             {
                 ContactPositionConstraint pc = _positionConstraints[i];
+                
+#if NET40 || NET45 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1
+                // Find lower order item.
+                int orderedIndexA = pc.indexA;
+                int orderedIndexB = pc.indexB;
+                if (orderedIndexB < orderedIndexA)
+                {
+                    orderedIndexA = pc.indexB;
+                    orderedIndexB = pc.indexA;
+                }
+                
+                // Lock bodies.
+                for (; ; )
+                {
+                    if (Interlocked.CompareExchange(ref _positions[orderedIndexA].Lock, 1, 0) == 0)
+                    {
+                        if (Interlocked.CompareExchange(ref _positions[orderedIndexB].Lock, 1, 0) == 0)
+                            break;
+                        System.Threading.Interlocked.Exchange(ref _positions[orderedIndexA].Lock, 0);
+                    }
+#if NET40 || NET45
+                    Thread.Sleep(0);
+#endif
+                }
+#endif
+
 
                 int indexA = pc.indexA;
                 int indexB = pc.indexB;
@@ -693,25 +875,22 @@ namespace FarseerPhysics.Dynamics.Contacts
 
                 Vector2 cA = _positions[indexA].c;
                 float aA = _positions[indexA].a;
-
                 Vector2 cB = _positions[indexB].c;
                 float aB = _positions[indexB].a;
 
                 // Solve normal constraints
                 for (int j = 0; j < pointCount; ++j)
                 {
-                    Transform xfA = new Transform();
-                    Transform xfB = new Transform();
-                    xfA.q.Set(aA);
-                    xfB.q.Set(aB);
-                    xfA.p = cA - MathUtils.Mul(xfA.q, localCenterA);
-                    xfB.p = cB - MathUtils.Mul(xfB.q, localCenterB);
+                    Transform xfA = new Transform(Vector2.Zero, aA);
+                    Transform xfB = new Transform(Vector2.Zero, aB);
+                    xfA.p = cA - Complex.Multiply(ref localCenterA, ref xfA.q);
+                    xfB.p = cB - Complex.Multiply(ref localCenterB, ref xfB.q);
 
                     Vector2 normal;
                     Vector2 point;
                     float separation;
 
-                    PositionSolverManifold.Initialize(pc, xfA, xfB, j, out normal, out point, out separation);
+                    PositionSolverManifold.Initialize(pc, ref xfA, ref xfB, j, out normal, out point, out separation);
 
                     Vector2 rA = point - cA;
                     Vector2 rB = point - cB;
@@ -723,8 +902,8 @@ namespace FarseerPhysics.Dynamics.Contacts
                     float C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f);
 
                     // Compute the effective mass.
-                    float rnA = MathUtils.Cross(rA, normal);
-                    float rnB = MathUtils.Cross(rB, normal);
+                    float rnA = MathUtils.Cross(ref rA, ref normal);
+                    float rnB = MathUtils.Cross(ref rB, ref normal);
                     float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
 
                     // Compute normal impulse
@@ -733,17 +912,22 @@ namespace FarseerPhysics.Dynamics.Contacts
                     Vector2 P = impulse * normal;
 
                     cA -= mA * P;
-                    aA -= iA * MathUtils.Cross(rA, P);
+                    aA -= iA * MathUtils.Cross(ref rA, ref P);
 
                     cB += mB * P;
-                    aB += iB * MathUtils.Cross(rB, P);
+                    aB += iB * MathUtils.Cross(ref rB, ref P);
                 }
 
                 _positions[indexA].c = cA;
                 _positions[indexA].a = aA;
-
                 _positions[indexB].c = cB;
                 _positions[indexB].a = aB;
+
+#if NET40 || NET45 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1
+                // Unlock bodies.
+                System.Threading.Interlocked.Exchange(ref _positions[orderedIndexB].Lock, 0);
+                System.Threading.Interlocked.Exchange(ref _positions[orderedIndexA].Lock, 0);
+#endif
             }
 
             // We can't expect minSpeparation >= -b2_linearSlop because we don't
@@ -791,18 +975,16 @@ namespace FarseerPhysics.Dynamics.Contacts
                 // Solve normal constraints
                 for (int j = 0; j < pointCount; ++j)
                 {
-                    Transform xfA = new Transform();
-                    Transform xfB = new Transform();
-                    xfA.q.Set(aA);
-                    xfB.q.Set(aB);
-                    xfA.p = cA - MathUtils.Mul(xfA.q, localCenterA);
-                    xfB.p = cB - MathUtils.Mul(xfB.q, localCenterB);
+                    Transform xfA = new Transform(Vector2.Zero, aA);
+                    Transform xfB = new Transform(Vector2.Zero, aB);
+                    xfA.p = cA - Complex.Multiply(ref localCenterA, ref xfA.q);
+                    xfB.p = cB - Complex.Multiply(ref localCenterB, ref xfB.q);
 
                     Vector2 normal;
                     Vector2 point;
                     float separation;
 
-                    PositionSolverManifold.Initialize(pc, xfA, xfB, j, out normal, out point, out separation);
+                    PositionSolverManifold.Initialize(pc, ref xfA, ref xfB, j, out normal, out point, out separation);
 
                     Vector2 rA = point - cA;
                     Vector2 rB = point - cB;
@@ -814,8 +996,8 @@ namespace FarseerPhysics.Dynamics.Contacts
                     float C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f);
 
                     // Compute the effective mass.
-                    float rnA = MathUtils.Cross(rA, normal);
-                    float rnB = MathUtils.Cross(rB, normal);
+                    float rnA = MathUtils.Cross(ref rA, ref normal);
+                    float rnB = MathUtils.Cross(ref rB, ref normal);
                     float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
 
                     // Compute normal impulse
@@ -824,10 +1006,10 @@ namespace FarseerPhysics.Dynamics.Contacts
                     Vector2 P = impulse * normal;
 
                     cA -= mA * P;
-                    aA -= iA * MathUtils.Cross(rA, P);
+                    aA -= iA * MathUtils.Cross(ref rA, ref P);
 
                     cB += mB * P;
-                    aB += iB * MathUtils.Cross(rB, P);
+                    aB += iB * MathUtils.Cross(ref rB, ref P);
                 }
 
                 _positions[indexA].c = cA;
@@ -872,8 +1054,8 @@ namespace FarseerPhysics.Dynamics.Contacts
                     case ManifoldType.Circles:
                         {
                             normal = new Vector2(1.0f, 0.0f);
-                            Vector2 pointA = MathUtils.Mul(ref xfA, manifold.LocalPoint);
-                            Vector2 pointB = MathUtils.Mul(ref xfB, manifold.Points[0].LocalPoint);
+                            Vector2 pointA = Transform.Multiply(ref manifold.LocalPoint, ref xfA);
+                            Vector2 pointB = Transform.Multiply(manifold.Points[0].LocalPoint, ref xfB);
                             if (Vector2.DistanceSquared(pointA, pointB) > Settings.Epsilon * Settings.Epsilon)
                             {
                                 normal = pointB - pointA;
@@ -888,12 +1070,12 @@ namespace FarseerPhysics.Dynamics.Contacts
 
                     case ManifoldType.FaceA:
                         {
-                            normal = MathUtils.Mul(xfA.q, manifold.LocalNormal);
-                            Vector2 planePoint = MathUtils.Mul(ref xfA, manifold.LocalPoint);
+                            normal = Complex.Multiply(ref manifold.LocalNormal, ref xfA.q);
+                            Vector2 planePoint = Transform.Multiply(ref manifold.LocalPoint, ref xfA);
 
                             for (int i = 0; i < manifold.PointCount; ++i)
                             {
-                                Vector2 clipPoint = MathUtils.Mul(ref xfB, manifold.Points[i].LocalPoint);
+                                Vector2 clipPoint = Transform.Multiply(manifold.Points[i].LocalPoint, ref xfB);
                                 Vector2 cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
                                 Vector2 cB = clipPoint - radiusB * normal;
                                 points[i] = 0.5f * (cA + cB);
@@ -903,12 +1085,12 @@ namespace FarseerPhysics.Dynamics.Contacts
 
                     case ManifoldType.FaceB:
                         {
-                            normal = MathUtils.Mul(xfB.q, manifold.LocalNormal);
-                            Vector2 planePoint = MathUtils.Mul(ref xfB, manifold.LocalPoint);
+                            normal = Complex.Multiply(ref manifold.LocalNormal, ref xfB.q);
+                            Vector2 planePoint = Transform.Multiply(ref manifold.LocalPoint, ref xfB);
 
                             for (int i = 0; i < manifold.PointCount; ++i)
                             {
-                                Vector2 clipPoint = MathUtils.Mul(ref xfA, manifold.Points[i].LocalPoint);
+                                Vector2 clipPoint = Transform.Multiply(manifold.Points[i].LocalPoint, ref xfA);
                                 Vector2 cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
                                 Vector2 cA = clipPoint - radiusA * normal;
                                 points[i] = 0.5f * (cA + cB);
@@ -924,19 +1106,22 @@ namespace FarseerPhysics.Dynamics.Contacts
 
         private static class PositionSolverManifold
         {
-            public static void Initialize(ContactPositionConstraint pc, Transform xfA, Transform xfB, int index, out Vector2 normal, out Vector2 point, out float separation)
+            public static void Initialize(ContactPositionConstraint pc, ref Transform xfA, ref Transform xfB, int index, out Vector2 normal, out Vector2 point, out float separation)
             {
                 Debug.Assert(pc.pointCount > 0);
 
-
                 switch (pc.type)
                 {
                     case ManifoldType.Circles:
                         {
-                            Vector2 pointA = MathUtils.Mul(ref xfA, pc.localPoint);
-                            Vector2 pointB = MathUtils.Mul(ref xfB, pc.localPoints[0]);
+                            Vector2 pointA = Transform.Multiply(ref pc.localPoint, ref xfA);
+                            Vector2 pointB = Transform.Multiply(pc.localPoints[0], ref xfB);
                             normal = pointB - pointA;
-                            normal.Normalize();
+
+                            // Handle zero normalization
+                            if (normal != Vector2.Zero)
+                                normal.Normalize();
+
                             point = 0.5f * (pointA + pointB);
                             separation = Vector2.Dot(pointB - pointA, normal) - pc.radiusA - pc.radiusB;
                         }
@@ -944,10 +1129,10 @@ namespace FarseerPhysics.Dynamics.Contacts
 
                     case ManifoldType.FaceA:
                         {
-                            normal = MathUtils.Mul(xfA.q, pc.localNormal);
-                            Vector2 planePoint = MathUtils.Mul(ref xfA, pc.localPoint);
+                            Complex.Multiply(ref pc.localNormal, ref xfA.q, out normal);
+                            Vector2 planePoint = Transform.Multiply(ref pc.localPoint, ref xfA);
 
-                            Vector2 clipPoint = MathUtils.Mul(ref xfB, pc.localPoints[index]);
+                            Vector2 clipPoint = Transform.Multiply(pc.localPoints[index], ref xfB);
                             separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB;
                             point = clipPoint;
                         }
@@ -955,10 +1140,10 @@ namespace FarseerPhysics.Dynamics.Contacts
 
                     case ManifoldType.FaceB:
                         {
-                            normal = MathUtils.Mul(xfB.q, pc.localNormal);
-                            Vector2 planePoint = MathUtils.Mul(ref xfB, pc.localPoint);
+                            Complex.Multiply(ref pc.localNormal, ref xfB.q, out normal);
+                            Vector2 planePoint = Transform.Multiply(ref pc.localPoint, ref xfB);
 
-                            Vector2 clipPoint = MathUtils.Mul(ref xfA, pc.localPoints[index]);
+                            Vector2 clipPoint = Transform.Multiply(pc.localPoints[index], ref xfA);
                             separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB;
                             point = clipPoint;
 
@@ -976,4 +1161,6 @@ namespace FarseerPhysics.Dynamics.Contacts
             }
         }
     }
+
+
 }

Libraries/Farseer Physics Engine 3.5/Dynamics/Fixture.cs

@@ -1,3 +1,10 @@
+// Copyright (c) 2017 Kastellanos Nikolaos
+
+/* Original source Farseer Physics Engine:
+ * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
+ * Microsoft Permissive License (Ms-PL) v1.1
+ */
+
 /*
 * Farseer Physics Engine:
 * Copyright (c) 2012 Ian Qvist
@@ -19,7 +26,6 @@
 * misrepresented as being the original software. 
 * 3. This notice may not be removed or altered from any source distribution. 
 */
-//#define USE_IGNORE_CCD_CATEGORIES
 
 using System;
 using System.Collections.Generic;
@@ -32,55 +38,6 @@ using Microsoft.Xna.Framework;
 
 namespace FarseerPhysics.Dynamics
 {
-    [Flags]
-    public enum Category
-    {
-        None = 0,
-        All = int.MaxValue,
-        Cat1 = 1,
-        Cat2 = 2,
-        Cat3 = 4,
-        Cat4 = 8,
-        Cat5 = 16,
-        Cat6 = 32,
-        Cat7 = 64,
-        Cat8 = 128,
-        Cat9 = 256,
-        Cat10 = 512,
-        Cat11 = 1024,
-        Cat12 = 2048,
-        Cat13 = 4096,
-        Cat14 = 8192,
-        Cat15 = 16384,
-        Cat16 = 32768,
-        Cat17 = 65536,
-        Cat18 = 131072,
-        Cat19 = 262144,
-        Cat20 = 524288,
-        Cat21 = 1048576,
-        Cat22 = 2097152,
-        Cat23 = 4194304,
-        Cat24 = 8388608,
-        Cat25 = 16777216,
-        Cat26 = 33554432,
-        Cat27 = 67108864,
-        Cat28 = 134217728,
-        Cat29 = 268435456,
-        Cat30 = 536870912,
-        Cat31 = 1073741824
-    }
-
-    /// <summary>
-    /// This proxy is used internally to connect fixtures to the broad-phase.
-    /// </summary>
-    public struct FixtureProxy
-    {
-        public AABB AABB;
-        public int ChildIndex;
-        public Fixture Fixture;
-        public int ProxyId;
-    }
-
     /// <summary>
     /// A fixture is used to attach a Shape to a body for collision detection. A fixture
     /// inherits its transform from its parent. Fixtures hold additional non-geometric data
@@ -88,10 +45,8 @@ namespace FarseerPhysics.Dynamics
     /// Fixtures are created via Body.CreateFixture.
     /// Warning: You cannot reuse fixtures.
     /// </summary>
-    public class Fixture : IDisposable
+    public class Fixture
     {
-        [ThreadStatic]
-        private static int _fixtureIdCounter;
         private bool _isSensor;
         private float _friction;
         private float _restitution;
@@ -99,11 +54,9 @@ namespace FarseerPhysics.Dynamics
         internal Category _collidesWith;
         internal Category _collisionCategories;
         internal short _collisionGroup;
-        internal HashSet<int> _collisionIgnores;
 
-        public FixtureProxy[] Proxies;
-        public int ProxyCount;
-        public Category IgnoreCCDWith;
+        public FixtureProxy[] Proxies { get; private set; }
+        public int ProxyCount { get; private set; }
 
         /// <summary>
         /// Fires after two shapes has collided and are solved. This gives you a chance to get the impact force.
@@ -129,47 +82,32 @@ namespace FarseerPhysics.Dynamics
         /// </summary>
         public OnSeparationEventHandler OnSeparation;
 
-        internal Fixture()
-        {
-            FixtureId = _fixtureIdCounter++;
-
-            _collisionCategories = Settings.DefaultFixtureCollisionCategories;
-            _collidesWith = Settings.DefaultFixtureCollidesWith;
+        internal Fixture() // Note: This is internal because it's used by Deserialization.
+        {   
+            _collisionCategories = Category.Cat1;
+            _collidesWith = Category.All;
             _collisionGroup = 0;
-            _collisionIgnores = new HashSet<int>();
-
-            IgnoreCCDWith = Settings.DefaultFixtureIgnoreCCDWith;
 
             //Fixture defaults
             Friction = 0.2f;
-            Restitution = 0;
+            Restitution = 0f;
         }
 
-        internal Fixture(Body body, Shape shape, object userData = null)
-            : this()
+        public Fixture(Shape shape) : this()
         {
-#if DEBUG
-            if (shape.ShapeType == ShapeType.Polygon)
-                ((PolygonShape)shape).Vertices.AttachedToBody = true;
-#endif
-
-            Body = body;
-            UserData = userData;
             Shape = shape.Clone();
-
-            RegisterFixture();
+            
+            // Reserve proxy space
+            Proxies = new FixtureProxy[Shape.ChildCount];
+            ProxyCount = 0;
         }
-
+        
         /// <summary>
         /// Defaults to 0
         /// 
-        /// If Settings.UseFPECollisionCategories is set to false:
         /// Collision groups allow a certain group of objects to never collide (negative)
         /// or always collide (positive). Zero means no collision group. Non-zero group
         /// filtering always wins against the mask bits.
-        /// 
-        /// If Settings.UseFPECollisionCategories is set to true:
-        /// If 2 fixtures are in the same collision group, they will not collide.
         /// </summary>
         public short CollisionGroup
         {
@@ -189,7 +127,6 @@ namespace FarseerPhysics.Dynamics
         /// 
         /// The collision mask bits. This states the categories that this
         /// fixture would accept for collision.
-        /// Use Settings.UseFPECollisionCategories to change the behavior.
         /// </summary>
         public Category CollidesWith
         {
@@ -208,11 +145,7 @@ namespace FarseerPhysics.Dynamics
         /// <summary>
         /// The collision categories this fixture is a part of.
         /// 
-        /// If Settings.UseFPECollisionCategories is set to false:
         /// Defaults to Category.Cat1
-        /// 
-        /// If Settings.UseFPECollisionCategories is set to true:
-        /// Defaults to Category.All
         /// </summary>
         public Category CollisionCategories
         {
@@ -229,11 +162,10 @@ namespace FarseerPhysics.Dynamics
         }
 
         /// <summary>
-        /// Get the child Shape. You can modify the child Shape, however you should not change the
-        /// number of vertices because this will crash some collision caching mechanisms.
+        /// Get the child Shape.
         /// </summary>
         /// <value>The shape.</value>
-        public Shape Shape { get; internal set; }
+        public Shape Shape { get; private set; }
 
         /// <summary>
         /// Gets or sets a value indicating whether this fixture is a sensor.
@@ -261,7 +193,7 @@ namespace FarseerPhysics.Dynamics
         /// Set the user data. Use this to store your application specific data.
         /// </summary>
         /// <value>The user data.</value>
-        public object UserData { get; set; }
+        public object UserData;
 
         /// <summary>
         /// Set the coefficient of friction. This will _not_ change the friction of
@@ -274,7 +206,6 @@ namespace FarseerPhysics.Dynamics
             set
             {
                 Debug.Assert(!float.IsNaN(value));
-
                 _friction = value;
             }
         }
@@ -290,71 +221,11 @@ namespace FarseerPhysics.Dynamics
             set
             {
                 Debug.Assert(!float.IsNaN(value));
-
                 _restitution = value;
             }
         }
-
-        /// <summary>
-        /// Gets a unique ID for this fixture.
-        /// </summary>
-        /// <value>The fixture id.</value>
-        public int FixtureId { get; internal set; }
-
-        #region IDisposable Members
-
-        public bool IsDisposed { get; set; }
-
-        public void Dispose()
-        {
-            if (!IsDisposed)
-            {
-                Body.DestroyFixture(this);
-                IsDisposed = true;
-                GC.SuppressFinalize(this);
-            }
-        }
-
-        #endregion
-
-        /// <summary>
-        /// Restores collisions between this fixture and the provided fixture.
-        /// </summary>
-        /// <param name="fixture">The fixture.</param>
-        public void RestoreCollisionWith(Fixture fixture)
-        {
-            if (_collisionIgnores.Contains(fixture.FixtureId))
-            {
-                _collisionIgnores.Remove(fixture.FixtureId);
-                Refilter();
-            }
-        }
-
-        /// <summary>
-        /// Ignores collisions between this fixture and the provided fixture.
-        /// </summary>
-        /// <param name="fixture">The fixture.</param>
-        public void IgnoreCollisionWith(Fixture fixture)
-        {
-            if (!_collisionIgnores.Contains(fixture.FixtureId))
-            {
-                _collisionIgnores.Add(fixture.FixtureId);
-                Refilter();
-            }
-        }
-
-        /// <summary>
-        /// Determines whether collisions are ignored between this fixture and the provided fixture.
-        /// </summary>
-        /// <param name="fixture">The fixture.</param>
-        /// <returns>
-        /// 	<c>true</c> if the fixture is ignored; otherwise, <c>false</c>.
-        /// </returns>
-        public bool IsFixtureIgnored(Fixture fixture)
-        {
-            return _collisionIgnores.Contains(fixture.FixtureId);
-        }
-
+        
+        
         /// <summary>
         /// Contacts are persistant and will keep being persistant unless they are
         /// flagged for filtering.
@@ -377,49 +248,24 @@ namespace FarseerPhysics.Dynamics
                 edge = edge.Next;
             }
 
-            World world = Body._world;
+            World world = Body.World;
 
             if (world == null)
-            {
                 return;
-            }
 
             // Touch each proxy so that new pairs may be created
             IBroadPhase broadPhase = world.ContactManager.BroadPhase;
-            for (int i = 0; i < ProxyCount; ++i)
-            {
-                broadPhase.TouchProxy(Proxies[i].ProxyId);
-            }
+            TouchProxies(broadPhase);
         }
 
-        private void RegisterFixture()
+        /// <summary>
+        /// Touch each proxy so that new pairs may be created
+        /// </summary>
+        /// <param name="broadPhase"></param>
+        internal void TouchProxies(IBroadPhase broadPhase)
         {
-            // Reserve proxy space
-            Proxies = new FixtureProxy[Shape.ChildCount];
-            ProxyCount = 0;
-
-            if (Body.Enabled)
-            {
-                IBroadPhase broadPhase = Body._world.ContactManager.BroadPhase;
-                CreateProxies(broadPhase, ref Body._xf);
-            }
-
-            Body.FixtureList.Add(this);
-
-            // Adjust mass properties if needed.
-            if (Shape._density > 0.0f)
-            {
-                Body.ResetMassData();
-            }
-
-            // Let the world know we have a new fixture. This will cause new contacts
-            // to be created at the beginning of the next time step.
-            Body._world._worldHasNewFixture = true;
-
-            if (Body._world.FixtureAdded != null)
-            {
-                Body._world.FixtureAdded(this);
-            }
+            for (int i = 0; i < ProxyCount; ++i)
+                broadPhase.TouchProxy(Proxies[i].ProxyId);
         }
 
         /// <summary>
@@ -457,43 +303,11 @@ namespace FarseerPhysics.Dynamics
             aabb = Proxies[childIndex].AABB;
         }
 
-        internal void Destroy()
-        {
-#if DEBUG
-            if (Shape.ShapeType == ShapeType.Polygon)
-                ((PolygonShape)Shape).Vertices.AttachedToBody = false;
-#endif
-
-            // The proxies must be destroyed before calling this.
-            Debug.Assert(ProxyCount == 0);
-
-            // Free the proxy array.
-            Proxies = null;
-            Shape = null;
-
-            //FPE: We set the userdata to null here to help prevent bugs related to stale references in GC
-            UserData = null;
-
-            BeforeCollision = null;
-            OnCollision = null;
-            OnSeparation = null;
-            AfterCollision = null;
-
-            if (Body._world.FixtureRemoved != null)
-            {
-                Body._world.FixtureRemoved(this);
-            }
-
-            Body._world.FixtureAdded = null;
-            Body._world.FixtureRemoved = null;
-            OnSeparation = null;
-            OnCollision = null;
-        }
-
         // These support body activation/deactivation.
         internal void CreateProxies(IBroadPhase broadPhase, ref Transform xf)
         {
-            Debug.Assert(ProxyCount == 0);
+            if (ProxyCount != 0)
+                throw new InvalidOperationException("Proxies already created for this Fixture.");
 
             // Create proxies in the broad-phase.
             ProxyCount = Shape.ChildCount;
@@ -501,12 +315,12 @@ namespace FarseerPhysics.Dynamics
             for (int i = 0; i < ProxyCount; ++i)
             {
                 FixtureProxy proxy = new FixtureProxy();
-                Shape.ComputeAABB(out proxy.AABB, ref xf, i);
                 proxy.Fixture = this;
                 proxy.ChildIndex = i;
-
-                //FPE note: This line needs to be after the previous two because FixtureProxy is a struct
-                proxy.ProxyId = broadPhase.AddProxy(ref proxy);
+                proxy.Body = this.Body;
+                Shape.ComputeAABB(out proxy.AABB, ref xf, i);
+                proxy.ProxyId = broadPhase.AddProxy(ref proxy.AABB);
+                broadPhase.SetProxy(proxy.ProxyId, ref proxy);
 
                 Proxies[i] = proxy;
             }
@@ -526,11 +340,6 @@ namespace FarseerPhysics.Dynamics
 
         internal void Synchronize(IBroadPhase broadPhase, ref Transform transform1, ref Transform transform2)
         {
-            if (ProxyCount == 0)
-            {
-                return;
-            }
-
             for (int i = 0; i < ProxyCount; ++i)
             {
                 FixtureProxy proxy = Proxies[i];
@@ -549,55 +358,24 @@ namespace FarseerPhysics.Dynamics
         }
 
         /// <summary>
-        /// Only compares the values of this fixture, and not the attached shape or body.
-        /// This is used for deduplication in serialization only.
-        /// </summary>
-        internal bool CompareTo(Fixture fixture)
-        {
-            return (_collidesWith == fixture._collidesWith &&
-                    _collisionCategories == fixture._collisionCategories &&
-                    _collisionGroup == fixture._collisionGroup &&
-                    Friction == fixture.Friction &&
-                    IsSensor == fixture.IsSensor &&
-                    Restitution == fixture.Restitution &&
-                    UserData == fixture.UserData &&
-                    IgnoreCCDWith == fixture.IgnoreCCDWith &&
-                    SequenceEqual(_collisionIgnores, fixture._collisionIgnores));
-        }
-
-        private bool SequenceEqual<T>(HashSet<T> first, HashSet<T> second)
-        {
-            if (first.Count != second.Count)
-                return false;
-
-            using (IEnumerator<T> enumerator1 = first.GetEnumerator())
-            {
-                using (IEnumerator<T> enumerator2 = second.GetEnumerator())
-                {
-                    while (enumerator1.MoveNext())
-                    {
-                        if (!enumerator2.MoveNext() || !Equals(enumerator1.Current, enumerator2.Current))
-                            return false;
-                    }
-
-                    if (enumerator2.MoveNext())
-                        return false;
-                }
-            }
-
-            return true;
-        }
-
-        /// <summary>
-        /// Clones the fixture and attached shape onto the specified body.
+        /// Clones the fixture onto the specified body.
         /// </summary>
         /// <param name="body">The body you wish to clone the fixture onto.</param>
         /// <returns>The cloned fixture.</returns>
         public Fixture CloneOnto(Body body)
         {
-            Fixture fixture = new Fixture();
-            fixture.Body = body;
-            fixture.Shape = Shape.Clone();
+            return CloneOnto(body, this.Shape);
+        }
+        
+        /// <summary>
+        /// Clones the fixture and attached shape onto the specified body.
+        /// Note: This is used only by Deserialization.
+        /// </summary>
+        /// <param name="body">The body you wish to clone the fixture onto.</param>
+        /// <returns>The cloned fixture.</returns>
+        internal Fixture CloneOnto(Body body, Shape shape)
+        {
+            Fixture fixture = new Fixture(shape.Clone());
             fixture.UserData = UserData;
             fixture.Restitution = Restitution;
             fixture.Friction = Friction;
@@ -605,15 +383,9 @@ namespace FarseerPhysics.Dynamics
             fixture._collisionGroup = _collisionGroup;
             fixture._collisionCategories = _collisionCategories;
             fixture._collidesWith = _collidesWith;
-            fixture.IgnoreCCDWith = IgnoreCCDWith;
-
-            foreach (int ignore in _collisionIgnores)
-            {
-                fixture._collisionIgnores.Add(ignore);
-            }
-
-            fixture.RegisterFixture();
+            
+            body.Add(fixture);
             return fixture;
         }
     }
-}
+}

Libraries/Farseer Physics Engine 3.5/Dynamics/FixtureProxy.cs

@@ -0,0 +1,50 @@
+/* Original source Farseer Physics Engine:
+ * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
+ * Microsoft Permissive License (Ms-PL) v1.1
+ */
+
+/*
+* Farseer Physics Engine:
+* Copyright (c) 2012 Ian Qvist
+* 
+* Original source Box2D:
+* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org 
+* 
+* This software is provided 'as-is', without any express or implied 
+* warranty.  In no event will the authors be held liable for any damages 
+* arising from the use of this software. 
+* Permission is granted to anyone to use this software for any purpose, 
+* including commercial applications, and to alter it and redistribute it 
+* freely, subject to the following restrictions: 
+* 1. The origin of this software must not be misrepresented; you must not 
+* claim that you wrote the original software. If you use this software 
+* in a product, an acknowledgment in the product documentation would be 
+* appreciated but is not required. 
+* 2. Altered source versions must be plainly marked as such, and must not be 
+* misrepresented as being the original software. 
+* 3. This notice may not be removed or altered from any source distribution. 
+*/
+
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using FarseerPhysics.Collision;
+using FarseerPhysics.Collision.Shapes;
+using FarseerPhysics.Common;
+using FarseerPhysics.Dynamics.Contacts;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Dynamics
+{
+    /// <summary>
+    /// This proxy is used internally to connect fixtures to the broad-phase.
+    /// </summary>
+    public struct FixtureProxy
+    {
+        public AABB AABB;
+        public int ChildIndex;
+        public Fixture Fixture;
+        public int ProxyId;
+        public Body Body;
+    }
+}

Libraries/Farseer Physics Engine 3.5/Dynamics/Island.cs

@@ -1,4 +1,11 @@
-/*
+// Copyright (c) 2017 Kastellanos Nikolaos
+
+/* Original source Farseer Physics Engine:
+ * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
+ * Microsoft Permissive License (Ms-PL) v1.1
+ */
+
+/*
 * Farseer Physics Engine:
 * Copyright (c) 2012 Ian Qvist
 * 
@@ -22,10 +29,11 @@
 
 using System;
 using System.Diagnostics;
+using Microsoft.Xna.Framework;
 using FarseerPhysics.Common;
+using FarseerPhysics.Diagnostics;
 using FarseerPhysics.Dynamics.Contacts;
 using FarseerPhysics.Dynamics.Joints;
-using Microsoft.Xna.Framework;
 
 namespace FarseerPhysics.Dynamics
 {
@@ -54,7 +62,7 @@ namespace FarseerPhysics.Dynamics
         public int BodyCapacity;
         public int ContactCapacity;
         public int JointCapacity;
-        public float JointUpdateTime;
+        public TimeSpan JointUpdateTime;
 
         public void Reset(int bodyCapacity, int contactCapacity, int jointCapacity, ContactManager contactManager)
         {
@@ -118,7 +126,7 @@ namespace FarseerPhysics.Dynamics
                     if (b.IgnoreGravity)
                         v += h * (b._invMass * b._force);
                     else
-                        v += h * (b.GravityScale * gravity + b._invMass * b._force);
+                        v += h * (gravity + b._invMass * b._force);
 
                     w += h * b._invI * b._torque;
 
@@ -145,10 +153,11 @@ namespace FarseerPhysics.Dynamics
             solverData.positions = _positions;
             solverData.velocities = _velocities;
 
-            _contactSolver.Reset(step, ContactCount, _contacts, _positions, _velocities);
+            _contactSolver.Reset(ref step, ContactCount, _contacts, _positions, _velocities,
+                _contactManager.VelocityConstraintsMultithreadThreshold, _contactManager.PositionConstraintsMultithreadThreshold);
             _contactSolver.InitializeVelocityConstraints();
 
-            if (Settings.EnableWarmstarting)
+            if (step.warmStarting)
             {
                 _contactSolver.WarmStart();
             }
@@ -166,7 +175,7 @@ namespace FarseerPhysics.Dynamics
                 _watch.Stop();
 
             // Solve velocity constraints.
-            for (int i = 0; i < Settings.VelocityIterations; ++i)
+            for (int i = 0; i < step.velocityIterations; ++i)
             {
                 for (int j = 0; j < JointCount; ++j)
                 {
@@ -227,7 +236,7 @@ namespace FarseerPhysics.Dynamics
 
             // Solve position constraints
             bool positionSolved = false;
-            for (int i = 0; i < Settings.PositionIterations; ++i)
+            for (int i = 0; i < step.positionIterations; ++i)
             {
                 bool contactsOkay = _contactSolver.SolvePositionConstraints();
 
@@ -260,7 +269,7 @@ namespace FarseerPhysics.Dynamics
 
             if (Settings.EnableDiagnostics)
             {
-                JointUpdateTime = _watch.ElapsedTicks;
+                JointUpdateTime = TimeSpan.FromTicks(_watch.ElapsedTicks);
                 _watch.Reset();
             }
 
@@ -311,7 +320,7 @@ namespace FarseerPhysics.Dynamics
             }
         }
 
-        internal void SolveTOI(ref TimeStep subStep, int toiIndexA, int toiIndexB, bool warmstarting)
+        internal void SolveTOI(ref TimeStep subStep, int toiIndexA, int toiIndexB)
         {
             Debug.Assert(toiIndexA < BodyCount);
             Debug.Assert(toiIndexB < BodyCount);
@@ -326,10 +335,11 @@ namespace FarseerPhysics.Dynamics
                 _velocities[i].w = b._angularVelocity;
             }
 
-            _contactSolver.Reset(subStep, ContactCount, _contacts, _positions, _velocities, warmstarting);
+            _contactSolver.Reset(ref subStep, ContactCount, _contacts, _positions, _velocities,
+                _contactManager.VelocityConstraintsMultithreadThreshold, _contactManager.PositionConstraintsMultithreadThreshold);
 
             // Solve position constraints.
-            for (int i = 0; i < Settings.TOIPositionIterations; ++i)
+            for (int i = 0; i < subStep.positionIterations; ++i)
             {
                 bool contactsOkay = _contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
                 if (contactsOkay)
@@ -349,7 +359,7 @@ namespace FarseerPhysics.Dynamics
             _contactSolver.InitializeVelocityConstraints();
 
             // Solve velocity constraints.
-            for (int i = 0; i < Settings.TOIVelocityIterations; ++i)
+            for (int i = 0; i < subStep.velocityIterations; ++i)
             {
                 _contactSolver.SolveVelocityConstraints();
             }

Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/AngleJoint.cs

@@ -1,3 +1,8 @@
+/* Original source Farseer Physics Engine:
+ * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
+ * Microsoft Permissive License (Ms-PL) v1.1
+ */
+
 /*
 * Farseer Physics Engine:
 * Copyright (c) 2012 Ian Qvist

Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/DistanceJoint.cs

@@ -1,3 +1,8 @@
+/* Original source Farseer Physics Engine:
+ * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
+ * Microsoft Permissive License (Ms-PL) v1.1
+ */
+
 /*
 * Farseer Physics Engine:
 * Copyright (c) 2012 Ian Qvist
@@ -23,6 +28,7 @@
 using System;
 using System.Diagnostics;
 using FarseerPhysics.Common;
+using FarseerPhysics.Common.Maths;
 using Microsoft.Xna.Framework;
 
 namespace FarseerPhysics.Dynamics.Joints
@@ -187,10 +193,11 @@ namespace FarseerPhysics.Dynamics.Joints
             Vector2 vB = data.velocities[_indexB].v;
             float wB = data.velocities[_indexB].w;
 
-            Rot qA = new Rot(aA), qB = new Rot(aB);
+            Complex qA = Complex.FromAngle(aA);
+            Complex qB = Complex.FromAngle(aB);
 
-            _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
-            _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
+            _rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
+            _rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB);
             _u = cB + _rB - cA - _rA;
 
             // Handle singularity.
@@ -204,8 +211,8 @@ namespace FarseerPhysics.Dynamics.Joints
                 _u = Vector2.Zero;
             }
 
-            float crAu = MathUtils.Cross(_rA, _u);
-            float crBu = MathUtils.Cross(_rB, _u);
+            float crAu = MathUtils.Cross(ref _rA, ref _u);
+            float crBu = MathUtils.Cross(ref _rB, ref _u);
             float invMass = _invMassA + _invIA * crAu * crAu + _invMassB + _invIB * crBu * crBu;
 
             // Compute the effective mass matrix.
@@ -216,7 +223,7 @@ namespace FarseerPhysics.Dynamics.Joints
                 float C = length - Length;
 
                 // Frequency
-                float omega = 2.0f * Settings.Pi * Frequency;
+                float omega = 2.0f * MathHelper.Pi * Frequency;
 
                 // Damping coefficient
                 float d = 2.0f * _mass * DampingRatio * omega;
@@ -239,16 +246,16 @@ namespace FarseerPhysics.Dynamics.Joints
                 _bias = 0.0f;
             }
 
-            if (Settings.EnableWarmstarting)
+            if (data.step.warmStarting)
             {
                 // Scale the impulse to support a variable time step.
                 _impulse *= data.step.dtRatio;
 
                 Vector2 P = _impulse * _u;
                 vA -= _invMassA * P;
-                wA -= _invIA * MathUtils.Cross(_rA, P);
+                wA -= _invIA * MathUtils.Cross(ref _rA, ref P);
                 vB += _invMassB * P;
-                wB += _invIB * MathUtils.Cross(_rB, P);
+                wB += _invIB * MathUtils.Cross(ref _rB, ref P);
             }
             else
             {
@@ -269,8 +276,8 @@ namespace FarseerPhysics.Dynamics.Joints
             float wB = data.velocities[_indexB].w;
 
             // Cdot = dot(u, v + cross(w, r))
-            Vector2 vpA = vA + MathUtils.Cross(wA, _rA);
-            Vector2 vpB = vB + MathUtils.Cross(wB, _rB);
+            Vector2 vpA = vA + MathUtils.Cross(wA, ref _rA);
+            Vector2 vpB = vB + MathUtils.Cross(wB, ref _rB);
             float Cdot = Vector2.Dot(_u, vpB - vpA);
 
             float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
@@ -278,9 +285,9 @@ namespace FarseerPhysics.Dynamics.Joints
 
             Vector2 P = impulse * _u;
             vA -= _invMassA * P;
-            wA -= _invIA * MathUtils.Cross(_rA, P);
+            wA -= _invIA * MathUtils.Cross(ref _rA, ref P);
             vB += _invMassB * P;
-            wB += _invIB * MathUtils.Cross(_rB, P);
+            wB += _invIB * MathUtils.Cross(ref _rB, ref P);
 
             data.velocities[_indexA].v = vA;
             data.velocities[_indexA].w = wA;
@@ -302,10 +309,11 @@ namespace FarseerPhysics.Dynamics.Joints
             Vector2 cB = data.positions[_indexB].c;
             float aB = data.positions[_indexB].a;
 
-            Rot qA = new Rot(aA), qB = new Rot(aB);
+            Complex qA = Complex.FromAngle(aA);
+            Complex qB = Complex.FromAngle(aB);
 
-            Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
-            Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
+            Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
+            Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB);
             Vector2 u = cB + rB - cA - rA;
 
             float length = u.Length(); u.Normalize();
@@ -316,9 +324,9 @@ namespace FarseerPhysics.Dynamics.Joints
             Vector2 P = impulse * u;
 
             cA -= _invMassA * P;
-            aA -= _invIA * MathUtils.Cross(rA, P);
+            aA -= _invIA * MathUtils.Cross(ref rA, ref P);
             cB += _invMassB * P;
-            aB += _invIB * MathUtils.Cross(rB, P);
+            aB += _invIB * MathUtils.Cross(ref rB, ref P);
 
             data.positions[_indexA].c = cA;
             data.positions[_indexA].a = aA;

Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/FixedMouseJoint.cs

@@ -1,3 +1,8 @@
+/* Original source Farseer Physics Engine:
+ * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
+ * Microsoft Permissive License (Ms-PL) v1.1
+ */
+
 /*
 * Farseer Physics Engine:
 * Copyright (c) 2012 Ian Qvist
@@ -22,6 +27,7 @@
 
 using System.Diagnostics;
 using FarseerPhysics.Common;
+using FarseerPhysics.Common.Maths;
 using Microsoft.Xna.Framework;
 
 namespace FarseerPhysics.Dynamics.Joints
@@ -81,7 +87,7 @@ namespace FarseerPhysics.Dynamics.Joints
             Debug.Assert(worldAnchor.IsValid());
 
             _worldAnchor = worldAnchor;
-            LocalAnchorA = MathUtils.MulT(BodyA._xf, worldAnchor);
+            LocalAnchorA = Transform.Divide(ref worldAnchor, ref BodyA._xf);
         }
 
         /// <summary>
@@ -168,12 +174,12 @@ namespace FarseerPhysics.Dynamics.Joints
             Vector2 vA = data.velocities[_indexA].v;
             float wA = data.velocities[_indexA].w;
 
-            Rot qA = new Rot(aA);
+            Complex qA = Complex.FromAngle(aA);
 
             float mass = BodyA.Mass;
 
             // Frequency
-            float omega = 2.0f * Settings.Pi * Frequency;
+            float omega = 2.0f * MathHelper.Pi * Frequency;
 
             // Damping coefficient
             float d = 2.0f * mass * DampingRatio * omega;
@@ -195,7 +201,7 @@ namespace FarseerPhysics.Dynamics.Joints
             _beta = h * k * _gamma;
 
             // Compute the effective mass matrix.
-            _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
+            _rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
             // K    = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
             //      = [1/m1+1/m2     0    ] + invI1 * [r1.Y*r1.Y -r1.X*r1.Y] + invI2 * [r1.Y*r1.Y -r1.X*r1.Y]
             //        [    0     1/m1+1/m2]           [-r1.X*r1.Y r1.X*r1.X]           [-r1.X*r1.Y r1.X*r1.X]
@@ -213,11 +219,11 @@ namespace FarseerPhysics.Dynamics.Joints
             // Cheat with some damping
             wA *= 0.98f;
 
-            if (Settings.EnableWarmstarting)
+            if (data.step.warmStarting)
             {
                 _impulse *= data.step.dtRatio;
                 vA += _invMassA * _impulse;
-                wA += _invIA * MathUtils.Cross(_rA, _impulse);
+                wA += _invIA * MathUtils.Cross(ref _rA, ref _impulse);
             }
             else
             {
@@ -234,7 +240,7 @@ namespace FarseerPhysics.Dynamics.Joints
             float wA = data.velocities[_indexA].w;
 
             // Cdot = v + cross(w, r)
-            Vector2 Cdot = vA + MathUtils.Cross(wA, _rA);
+            Vector2 Cdot = vA + MathUtils.Cross(wA, ref _rA);
             Vector2 impulse = MathUtils.Mul(ref _mass, -(Cdot + _C + _gamma * _impulse));
 
             Vector2 oldImpulse = _impulse;
@@ -247,7 +253,7 @@ namespace FarseerPhysics.Dynamics.Joints
             impulse = _impulse - oldImpulse;
 
             vA += _invMassA * impulse;
-            wA += _invIA * MathUtils.Cross(_rA, impulse);
+            wA += _invIA * MathUtils.Cross(ref _rA, ref impulse);
 
             data.velocities[_indexA].v = vA;
             data.velocities[_indexA].w = wA;

Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/FrictionJoint.cs

@@ -1,3 +1,8 @@
+/* Original source Farseer Physics Engine:
+ * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
+ * Microsoft Permissive License (Ms-PL) v1.1
+ */
+
 /*
 * Farseer Physics Engine:
 * Copyright (c) 2012 Ian Qvist
@@ -21,6 +26,7 @@
 */
 
 using FarseerPhysics.Common;
+using FarseerPhysics.Common.Maths;
 using Microsoft.Xna.Framework;
 
 namespace FarseerPhysics.Dynamics.Joints
@@ -151,11 +157,12 @@ namespace FarseerPhysics.Dynamics.Joints
             Vector2 vB = data.velocities[_indexB].v;
             float wB = data.velocities[_indexB].w;
 
-            Rot qA = new Rot(aA), qB = new Rot(aB);
+            Complex qA = Complex.FromAngle(aA);
+            Complex qB = Complex.FromAngle(aB);
 
             // Compute the effective mass matrix.
-            _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
-            _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
+            _rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
+            _rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB);
 
             // J = [-I -r1_skew I r2_skew]
             //     [ 0       -1 0       1]
@@ -183,7 +190,7 @@ namespace FarseerPhysics.Dynamics.Joints
                 _angularMass = 1.0f / _angularMass;
             }
 
-            if (Settings.EnableWarmstarting)
+            if (data.step.warmStarting)
             {
                 // Scale impulses to support a variable time step.
                 _linearImpulse *= data.step.dtRatio;
@@ -191,9 +198,9 @@ namespace FarseerPhysics.Dynamics.Joints
 
                 Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y);
                 vA -= mA * P;
-                wA -= iA * (MathUtils.Cross(_rA, P) + _angularImpulse);
+                wA -= iA * (MathUtils.Cross(ref _rA, ref P) + _angularImpulse);
                 vB += mB * P;
-                wB += iB * (MathUtils.Cross(_rB, P) + _angularImpulse);
+                wB += iB * (MathUtils.Cross(ref _rB, ref P) + _angularImpulse);
             }
             else
             {
@@ -235,9 +242,9 @@ namespace FarseerPhysics.Dynamics.Joints
 
             // Solve linear friction
             {
-                Vector2 Cdot = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
+                Vector2 Cdot = vB + MathUtils.Cross(wB, ref _rB) - vA - MathUtils.Cross(wA, ref _rA);
 
-                Vector2 impulse = -MathUtils.Mul(ref _linearMass, Cdot);
+                Vector2 impulse = -MathUtils.Mul(ref _linearMass, ref Cdot);
                 Vector2 oldImpulse = _linearImpulse;
                 _linearImpulse += impulse;
 
@@ -252,10 +259,10 @@ namespace FarseerPhysics.Dynamics.Joints
                 impulse = _linearImpulse - oldImpulse;
 
                 vA -= mA * impulse;
-                wA -= iA * MathUtils.Cross(_rA, impulse);
+                wA -= iA * MathUtils.Cross(ref _rA, ref impulse);
 
                 vB += mB * impulse;
-                wB += iB * MathUtils.Cross(_rB, impulse);
+                wB += iB * MathUtils.Cross(ref _rB, ref impulse);
             }
 
             data.velocities[_indexA].v = vA;

Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/GearJoint.cs

@@ -1,3 +1,8 @@
+/* Original source Farseer Physics Engine:
+ * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
+ * Microsoft Permissive License (Ms-PL) v1.1
+ */
+
 /*
 * Farseer Physics Engine:
 * Copyright (c) 2012 Ian Qvist
@@ -22,6 +27,7 @@
 
 using System.Diagnostics;
 using FarseerPhysics.Common;
+using FarseerPhysics.Common.Maths;
 using Microsoft.Xna.Framework;
 
 namespace FarseerPhysics.Dynamics.Joints
@@ -148,7 +154,7 @@ namespace FarseerPhysics.Dynamics.Joints
                 _localAxisC = prismatic.LocalXAxis;
 
                 Vector2 pC = _localAnchorC;
-                Vector2 pA = MathUtils.MulT(xfC.q, MathUtils.Mul(xfA.q, _localAnchorA) + (xfA.p - xfC.p));
+                Vector2 pA = Complex.Divide(Complex.Multiply(ref _localAnchorA, ref xfA.q) + (xfA.p - xfC.p), ref xfC.q);
                 coordinateA = Vector2.Dot(pA - pC, _localAxisC);
             }
 
@@ -180,7 +186,7 @@ namespace FarseerPhysics.Dynamics.Joints
                 _localAxisD = prismatic.LocalXAxis;
 
                 Vector2 pD = _localAnchorD;
-                Vector2 pB = MathUtils.MulT(xfD.q, MathUtils.Mul(xfB.q, _localAnchorB) + (xfB.p - xfD.p));
+                Vector2 pB = Complex.Divide(Complex.Multiply(ref _localAnchorB, ref xfB.q) + (xfB.p - xfD.p), ref xfD.q);
                 coordinateB = Vector2.Dot(pB - pD, _localAxisD);
             }
 
@@ -271,7 +277,10 @@ namespace FarseerPhysics.Dynamics.Joints
             Vector2 vD = data.velocities[_indexD].v;
             float wD = data.velocities[_indexD].w;
 
-            Rot qA = new Rot(aA), qB = new Rot(aB), qC = new Rot(aC), qD = new Rot(aD);
+            Complex qA = Complex.FromAngle(aA);
+            Complex qB = Complex.FromAngle(aB);
+            Complex qC = Complex.FromAngle(aC);
+            Complex qD = Complex.FromAngle(aD);
 
             _mass = 0.0f;
 
@@ -284,12 +293,12 @@ namespace FarseerPhysics.Dynamics.Joints
             }
             else
             {
-                Vector2 u = MathUtils.Mul(qC, _localAxisC);
-                Vector2 rC = MathUtils.Mul(qC, _localAnchorC - _lcC);
-                Vector2 rA = MathUtils.Mul(qA, _localAnchorA - _lcA);
+                Vector2 u = Complex.Multiply(ref _localAxisC, ref qC);
+                Vector2 rC = Complex.Multiply(_localAnchorC - _lcC, ref qC);
+                Vector2 rA = Complex.Multiply(_localAnchorA - _lcA, ref qA);
                 _JvAC = u;
-                _JwC = MathUtils.Cross(rC, u);
-                _JwA = MathUtils.Cross(rA, u);
+                _JwC = MathUtils.Cross(ref rC, ref u);
+                _JwA = MathUtils.Cross(ref rA, ref u);
                 _mass += _mC + _mA + _iC * _JwC * _JwC + _iA * _JwA * _JwA;
             }
 
@@ -302,19 +311,19 @@ namespace FarseerPhysics.Dynamics.Joints
             }
             else
             {
-                Vector2 u = MathUtils.Mul(qD, _localAxisD);
-                Vector2 rD = MathUtils.Mul(qD, _localAnchorD - _lcD);
-                Vector2 rB = MathUtils.Mul(qB, _localAnchorB - _lcB);
+                Vector2 u = Complex.Multiply(ref _localAxisD, ref qD);
+                Vector2 rD = Complex.Multiply(_localAnchorD - _lcD, ref qD);
+                Vector2 rB = Complex.Multiply(_localAnchorB - _lcB, ref qB);
                 _JvBD = _ratio * u;
-                _JwD = _ratio * MathUtils.Cross(rD, u);
-                _JwB = _ratio * MathUtils.Cross(rB, u);
+                _JwD = _ratio * MathUtils.Cross(ref rD, ref u);
+                _JwB = _ratio * MathUtils.Cross(ref rB, ref u);
                 _mass += _ratio * _ratio * (_mD + _mB) + _iD * _JwD * _JwD + _iB * _JwB * _JwB;
             }
 
             // Compute effective mass.
             _mass = _mass > 0.0f ? 1.0f / _mass : 0.0f;
 
-            if (Settings.EnableWarmstarting)
+            if (data.step.warmStarting)
             {
                 vA += (_mA * _impulse) * _JvAC;
                 wA += _iA * _impulse * _JwA;
@@ -387,7 +396,10 @@ namespace FarseerPhysics.Dynamics.Joints
             Vector2 cD = data.positions[_indexD].c;
             float aD = data.positions[_indexD].a;
 
-            Rot qA = new Rot(aA), qB = new Rot(aB), qC = new Rot(aC), qD = new Rot(aD);
+            Complex qA = Complex.FromAngle(aA);
+            Complex qB = Complex.FromAngle(aB);
+            Complex qC = Complex.FromAngle(aC);
+            Complex qD = Complex.FromAngle(aD);
 
             const float linearError = 0.0f;
 
@@ -408,16 +420,16 @@ namespace FarseerPhysics.Dynamics.Joints
             }
             else
             {
-                Vector2 u = MathUtils.Mul(qC, _localAxisC);
-                Vector2 rC = MathUtils.Mul(qC, _localAnchorC - _lcC);
-                Vector2 rA = MathUtils.Mul(qA, _localAnchorA - _lcA);
+                Vector2 u = Complex.Multiply(ref _localAxisC, ref qC);
+                Vector2 rC = Complex.Multiply(_localAnchorC - _lcC, ref qC);
+                Vector2 rA = Complex.Multiply(_localAnchorA - _lcA, ref qA);
                 JvAC = u;
-                JwC = MathUtils.Cross(rC, u);
-                JwA = MathUtils.Cross(rA, u);
+                JwC = MathUtils.Cross(ref rC, ref u);
+                JwA = MathUtils.Cross(ref rA, ref u);
                 mass += _mC + _mA + _iC * JwC * JwC + _iA * JwA * JwA;
 
                 Vector2 pC = _localAnchorC - _lcC;
-                Vector2 pA = MathUtils.MulT(qC, rA + (cA - cC));
+                Vector2 pA = Complex.Divide(rA + (cA - cC), ref qC);
                 coordinateA = Vector2.Dot(pA - pC, _localAxisC);
             }
 
@@ -432,16 +444,16 @@ namespace FarseerPhysics.Dynamics.Joints
             }
             else
             {
-                Vector2 u = MathUtils.Mul(qD, _localAxisD);
-                Vector2 rD = MathUtils.Mul(qD, _localAnchorD - _lcD);
-                Vector2 rB = MathUtils.Mul(qB, _localAnchorB - _lcB);
+                Vector2 u = Complex.Multiply(ref _localAxisD, ref qD);
+                Vector2 rD = Complex.Multiply(_localAnchorD - _lcD, ref qD);
+                Vector2 rB = Complex.Multiply(_localAnchorB - _lcB, ref qB);
                 JvBD = _ratio * u;
-                JwD = _ratio * MathUtils.Cross(rD, u);
-                JwB = _ratio * MathUtils.Cross(rB, u);
+                JwD = _ratio * MathUtils.Cross(ref rD, ref u);
+                JwB = _ratio * MathUtils.Cross(ref rB, ref u);
                 mass += _ratio * _ratio * (_mD + _mB) + _iD * JwD * JwD + _iB * JwB * JwB;
 
                 Vector2 pD = _localAnchorD - _lcD;
-                Vector2 pB = MathUtils.MulT(qD, rB + (cB - cD));
+                Vector2 pB = Complex.Divide(rB + (cB - cD), ref qD);
                 coordinateB = Vector2.Dot(pB - pD, _localAxisD);
             }
 

Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/Joint.cs

@@ -1,3 +1,8 @@
+/* Original source Farseer Physics Engine:
+ * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
+ * Microsoft Permissive License (Ms-PL) v1.1
+ */
+
 /*
 * Farseer Physics Engine:
 * Copyright (c) 2012 Ian Qvist
@@ -162,7 +167,7 @@ namespace FarseerPhysics.Dynamics.Joints
         /// Set the user data pointer.
         /// </summary>
         /// <value>The data.</value>
-        public object UserData { get; set; }
+        public object Tag;
 
         /// <summary>
         /// Set this flag to true if the attached bodies should collide.

Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/JointFactory.cs

@@ -0,0 +1,124 @@
+/* Original source Farseer Physics Engine:
+ * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
+ * Microsoft Permissive License (Ms-PL) v1.1
+ */
+
+using FarseerPhysics.Dynamics;
+using FarseerPhysics.Dynamics.Joints;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Dynamics.Joints
+{
+    /// <summary>
+    /// An easy to use factory for using joints.
+    /// </summary>
+    public static class JointFactory
+    {
+        public static MotorJoint CreateMotorJoint(World world, Body bodyA, Body bodyB, bool useWorldCoordinates = false)
+        {
+            MotorJoint joint = new MotorJoint(bodyA, bodyB, useWorldCoordinates);
+            world.Add(joint);
+            return joint;
+        }
+
+        public static RevoluteJoint CreateRevoluteJoint(World world, Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false)
+        {
+            RevoluteJoint joint = new RevoluteJoint(bodyA, bodyB, anchorA, anchorB, useWorldCoordinates);
+            world.Add(joint);
+            return joint;
+        }
+
+        public static RevoluteJoint CreateRevoluteJoint(World world, Body bodyA, Body bodyB, Vector2 anchor)
+        {
+            Vector2 localanchorA = bodyA.GetLocalPoint(bodyB.GetWorldPoint(anchor));
+            RevoluteJoint joint = new RevoluteJoint(bodyA, bodyB, localanchorA, anchor);
+            world.Add(joint);
+            return joint;
+        }
+
+        public static RopeJoint CreateRopeJoint(World world, Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false)
+        {
+            RopeJoint ropeJoint = new RopeJoint(bodyA, bodyB, anchorA, anchorB, useWorldCoordinates);
+            world.Add(ropeJoint);
+            return ropeJoint;
+        }
+
+        public static WeldJoint CreateWeldJoint(World world, Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false)
+        {
+            WeldJoint weldJoint = new WeldJoint(bodyA, bodyB, anchorA, anchorB, useWorldCoordinates);
+            world.Add(weldJoint);
+            return weldJoint;
+        }
+
+        public static PrismaticJoint CreatePrismaticJoint(World world, Body bodyA, Body bodyB, Vector2 anchor, Vector2 axis, bool useWorldCoordinates = false)
+        {
+            PrismaticJoint joint = new PrismaticJoint(bodyA, bodyB, anchor, axis, useWorldCoordinates);
+            world.Add(joint);
+            return joint;
+        }
+
+        public static WheelJoint CreateWheelJoint(World world, Body bodyA, Body bodyB, Vector2 anchor, Vector2 axis, bool useWorldCoordinates = false)
+        {
+            WheelJoint joint = new WheelJoint(bodyA, bodyB, anchor, axis, useWorldCoordinates);
+            world.Add(joint);
+            return joint;
+        }
+
+        public static WheelJoint CreateWheelJoint(World world, Body bodyA, Body bodyB, Vector2 axis)
+        {
+            return CreateWheelJoint(world, bodyA, bodyB, Vector2.Zero, axis);
+        }
+
+        public static AngleJoint CreateAngleJoint(World world, Body bodyA, Body bodyB)
+        {
+            AngleJoint angleJoint = new AngleJoint(bodyA, bodyB);
+            world.Add(angleJoint);
+            return angleJoint;
+        }
+
+        public static DistanceJoint CreateDistanceJoint(World world, Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false)
+        {
+            DistanceJoint distanceJoint = new DistanceJoint(bodyA, bodyB, anchorA, anchorB, useWorldCoordinates);
+            world.Add(distanceJoint);
+            return distanceJoint;
+        }
+
+        public static DistanceJoint CreateDistanceJoint(World world, Body bodyA, Body bodyB)
+        {
+            return CreateDistanceJoint(world, bodyA, bodyB, Vector2.Zero, Vector2.Zero);
+        }
+
+        public static FrictionJoint CreateFrictionJoint(World world, Body bodyA, Body bodyB, Vector2 anchor, bool useWorldCoordinates = false)
+        {
+            FrictionJoint frictionJoint = new FrictionJoint(bodyA, bodyB, anchor, useWorldCoordinates);
+            world.Add(frictionJoint);
+            return frictionJoint;
+        }
+
+        public static FrictionJoint CreateFrictionJoint(World world, Body bodyA, Body bodyB)
+        {
+            return CreateFrictionJoint(world, bodyA, bodyB, Vector2.Zero);
+        }
+
+        public static GearJoint CreateGearJoint(World world, Body bodyA, Body bodyB, Joint jointA, Joint jointB, float ratio)
+        {
+            GearJoint gearJoint = new GearJoint(bodyA, bodyB, jointA, jointB, ratio);
+            world.Add(gearJoint);
+            return gearJoint;
+        }
+
+        public static PulleyJoint CreatePulleyJoint(World world, Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, Vector2 worldAnchorA, Vector2 worldAnchorB, float ratio, bool useWorldCoordinates = false)
+        {
+            PulleyJoint pulleyJoint = new PulleyJoint(bodyA, bodyB, anchorA, anchorB, worldAnchorA, worldAnchorB, ratio, useWorldCoordinates);
+            world.Add(pulleyJoint);
+            return pulleyJoint;
+        }
+
+        public static FixedMouseJoint CreateFixedMouseJoint(World world, Body body, Vector2 worldAnchor)
+        {
+            FixedMouseJoint joint = new FixedMouseJoint(body, worldAnchor);
+            world.Add(joint);
+            return joint;
+        }
+    }
+}

Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/MotorJoint.cs

@@ -1,3 +1,8 @@
+/* Original source Farseer Physics Engine:
+ * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
+ * Microsoft Permissive License (Ms-PL) v1.1
+ */
+
 /*
 * Farseer Physics Engine:
 * Copyright (c) 2012 Ian Qvist
@@ -22,6 +27,7 @@
 
 using System.Diagnostics;
 using FarseerPhysics.Common;
+using FarseerPhysics.Common.Maths;
 using Microsoft.Xna.Framework;
 
 namespace FarseerPhysics.Dynamics.Joints
@@ -193,12 +199,12 @@ namespace FarseerPhysics.Dynamics.Joints
             Vector2 vB = data.velocities[_indexB].v;
             float wB = data.velocities[_indexB].w;
 
-            Rot qA = new Rot(aA);
-            Rot qB = new Rot(aB);
+            Complex qA = Complex.FromAngle(aA);
+            Complex qB = Complex.FromAngle(aB);
 
             // Compute the effective mass matrix.
-            _rA = MathUtils.Mul(qA, -_localCenterA);
-            _rB = MathUtils.Mul(qB, -_localCenterB);
+            _rA = -Complex.Multiply(ref _localCenterA, ref qA);
+            _rB = -Complex.Multiply(ref _localCenterB, ref qB);
 
             // J = [-I -r1_skew I r2_skew]
             //     [ 0       -1 0       1]
@@ -226,10 +232,10 @@ namespace FarseerPhysics.Dynamics.Joints
                 _angularMass = 1.0f / _angularMass;
             }
 
-            _linearError = cB + _rB - cA - _rA - MathUtils.Mul(qA, _linearOffset);
+            _linearError = cB + _rB - cA - _rA - Complex.Multiply(ref _linearOffset, ref qA);
             _angularError = aB - aA - _angularOffset;
 
-            if (Settings.EnableWarmstarting)
+            if (data.step.warmStarting)
             {
                 // Scale impulses to support a variable time step.
                 _linearImpulse *= data.step.dtRatio;
@@ -238,9 +244,9 @@ namespace FarseerPhysics.Dynamics.Joints
                 Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y);
 
                 vA -= mA * P;
-                wA -= iA * (MathUtils.Cross(_rA, P) + _angularImpulse);
+                wA -= iA * (MathUtils.Cross(ref _rA, ref P) + _angularImpulse);
                 vB += mB * P;
-                wB += iB * (MathUtils.Cross(_rB, P) + _angularImpulse);
+                wB += iB * (MathUtils.Cross(ref _rB, ref P) + _angularImpulse);
             }
             else
             {
@@ -283,7 +289,7 @@ namespace FarseerPhysics.Dynamics.Joints
 
             // Solve linear friction
             {
-                Vector2 Cdot = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA) + inv_h * CorrectionFactor * _linearError;
+                Vector2 Cdot = vB + MathUtils.Cross(wB, ref _rB) - vA - MathUtils.Cross(wA, ref _rA) + inv_h * CorrectionFactor * _linearError;
 
                 Vector2 impulse = -MathUtils.Mul(ref _linearMass, ref Cdot);
                 Vector2 oldImpulse = _linearImpulse;
@@ -300,10 +306,10 @@ namespace FarseerPhysics.Dynamics.Joints
                 impulse = _linearImpulse - oldImpulse;
 
                 vA -= mA * impulse;
-                wA -= iA * MathUtils.Cross(_rA, impulse);
+                wA -= iA * MathUtils.Cross(ref _rA, ref impulse);
 
                 vB += mB * impulse;
-                wB += iB * MathUtils.Cross(_rB, impulse);
+                wB += iB * MathUtils.Cross(ref _rB, ref impulse);
             }
 
             data.velocities[_indexA].v = vA;

Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/PrismaticJoint.cs

@@ -1,3 +1,8 @@
+/* Original source Farseer Physics Engine:
+ * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
+ * Microsoft Permissive License (Ms-PL) v1.1
+ */
+
 /*
 * Farseer Physics Engine:
 * Copyright (c) 2012 Ian Qvist
@@ -23,6 +28,7 @@
 using System;
 using System.Diagnostics;
 using FarseerPhysics.Common;
+using FarseerPhysics.Common.Maths;
 using Microsoft.Xna.Framework;
 
 namespace FarseerPhysics.Dynamics.Joints
@@ -98,6 +104,7 @@ namespace FarseerPhysics.Dynamics.Joints
     /// </summary>
     public class PrismaticJoint : Joint
     {
+        private Vector2 _localXAxis;
         private Vector2 _localYAxisA;
         private Vector3 _impulse;
         private float _lowerTranslation;
@@ -207,7 +214,7 @@ namespace FarseerPhysics.Dynamics.Joints
             get
             {
                 Vector2 d = BodyB.GetWorldPoint(LocalAnchorB) - BodyA.GetWorldPoint(LocalAnchorA);
-                Vector2 axis = BodyA.GetWorldVector(LocalXAxis);
+                Vector2 axis = BodyA.GetWorldVector(ref _localXAxis);
 
                 return Vector2.Dot(d, axis);
             }
@@ -221,23 +228,22 @@ namespace FarseerPhysics.Dynamics.Joints
         {
             get
             {
-                Transform xf1, xf2;
-                BodyA.GetTransform(out xf1);
-                BodyB.GetTransform(out xf2);
+                Transform xf1 = BodyA.GetTransform();
+                Transform xf2 = BodyB.GetTransform();
 
-                Vector2 r1 = MathUtils.Mul(ref xf1.q, LocalAnchorA - BodyA.LocalCenter);
-                Vector2 r2 = MathUtils.Mul(ref xf2.q, LocalAnchorB - BodyB.LocalCenter);
+                Vector2 r1 = Complex.Multiply(LocalAnchorA - BodyA.LocalCenter, ref xf1.q);
+                Vector2 r2 = Complex.Multiply(LocalAnchorB - BodyB.LocalCenter, ref xf2.q);
                 Vector2 p1 = BodyA._sweep.C + r1;
                 Vector2 p2 = BodyB._sweep.C + r2;
                 Vector2 d = p2 - p1;
-                Vector2 axis = BodyA.GetWorldVector(LocalXAxis);
+                Vector2 axis = BodyA.GetWorldVector(ref _localXAxis);
 
                 Vector2 v1 = BodyA._linearVelocity;
                 Vector2 v2 = BodyB._linearVelocity;
                 float w1 = BodyA._angularVelocity;
                 float w2 = BodyB._angularVelocity;
 
-                float speed = Vector2.Dot(d, MathUtils.Cross(w1, axis)) + Vector2.Dot(axis, v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1));
+                float speed = Vector2.Dot(d, MathUtils.Cross(w1, ref axis)) + Vector2.Dot(axis, v2 + MathUtils.Cross(w2, ref r2) - v1 - MathUtils.Cross(w1, ref r1));
                 return speed;
             }
         }
@@ -380,16 +386,16 @@ namespace FarseerPhysics.Dynamics.Joints
             set
             {
                 _axis1 = value;
-                LocalXAxis = BodyA.GetLocalVector(_axis1);
-                LocalXAxis.Normalize();
-                _localYAxisA = MathUtils.Cross(1.0f, LocalXAxis);
+                _localXAxis = BodyA.GetLocalVector(_axis1);
+                _localXAxis.Normalize();
+                _localYAxisA = MathUtils.Cross(1.0f, ref _localXAxis);
             }
         }
 
         /// <summary>
         /// The axis in local coordinates relative to BodyA
         /// </summary>
-        public Vector2 LocalXAxis { get; private set; }
+        public Vector2 LocalXAxis { get { return _localXAxis; } }
 
         /// <summary>
         /// The reference angle.
@@ -427,11 +433,12 @@ namespace FarseerPhysics.Dynamics.Joints
             Vector2 vB = data.velocities[_indexB].v;
             float wB = data.velocities[_indexB].w;
 
-            Rot qA = new Rot(aA), qB = new Rot(aB);
+            Complex qA = Complex.FromAngle(aA);
+            Complex qB = Complex.FromAngle(aB);
 
             // Compute the effective masses.
-            Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
-            Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
+            Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
+            Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB);
             Vector2 d = (cB - cA) + rB - rA;
 
             float mA = _invMassA, mB = _invMassB;
@@ -439,9 +446,9 @@ namespace FarseerPhysics.Dynamics.Joints
 
             // Compute motor Jacobian and effective mass.
             {
-                _axis = MathUtils.Mul(qA, LocalXAxis);
+                _axis = Complex.Multiply(ref _localXAxis, ref qA);
                 _a1 = MathUtils.Cross(d + rA, _axis);
-                _a2 = MathUtils.Cross(rB, _axis);
+                _a2 = MathUtils.Cross(ref rB, ref _axis);
 
                 _motorMass = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2;
                 if (_motorMass > 0.0f)
@@ -452,10 +459,10 @@ namespace FarseerPhysics.Dynamics.Joints
 
             // Prismatic constraint.
             {
-                _perp = MathUtils.Mul(qA, _localYAxisA);
+                _perp = Complex.Multiply(ref _localYAxisA, ref qA);
 
                 _s1 = MathUtils.Cross(d + rA, _perp);
-                _s2 = MathUtils.Cross(rB, _perp);
+                _s2 = MathUtils.Cross(ref rB, ref _perp);
 
                 float k11 = mA + mB + iA * _s1 * _s1 + iB * _s2 * _s2;
                 float k12 = iA * _s1 + iB * _s2;
@@ -515,7 +522,7 @@ namespace FarseerPhysics.Dynamics.Joints
                 MotorImpulse = 0.0f;
             }
 
-            if (Settings.EnableWarmstarting)
+            if (data.step.warmStarting)
             {
                 // Account for variable time step.
                 _impulse *= data.step.dtRatio;
@@ -647,23 +654,24 @@ namespace FarseerPhysics.Dynamics.Joints
             Vector2 cB = data.positions[_indexB].c;
             float aB = data.positions[_indexB].a;
 
-            Rot qA = new Rot(aA), qB = new Rot(aB);
+            Complex qA = Complex.FromAngle(aA);
+            Complex qB = Complex.FromAngle(aB);
 
             float mA = _invMassA, mB = _invMassB;
             float iA = _invIA, iB = _invIB;
 
             // Compute fresh Jacobians
-            Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
-            Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
+            Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
+            Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB);
             Vector2 d = cB + rB - cA - rA;
 
-            Vector2 axis = MathUtils.Mul(qA, LocalXAxis);
+            Vector2 axis = Complex.Multiply(ref _localXAxis, ref qA);
             float a1 = MathUtils.Cross(d + rA, axis);
-            float a2 = MathUtils.Cross(rB, axis);
-            Vector2 perp = MathUtils.Mul(qA, _localYAxisA);
+            float a2 = MathUtils.Cross(ref rB, ref axis);
+            Vector2 perp = Complex.Multiply(ref _localYAxisA, ref qA);
 
             float s1 = MathUtils.Cross(d + rA, perp);
-            float s2 = MathUtils.Cross(rB, perp);
+            float s2 = MathUtils.Cross(ref rB, ref perp);
 
             Vector3 impulse;
             Vector2 C1 = new Vector2();

Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/PulleyJoint.cs

@@ -1,3 +1,8 @@
+/* Original source Farseer Physics Engine:
+ * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
+ * Microsoft Permissive License (Ms-PL) v1.1
+ */
+
 /*
 * Farseer Physics Engine:
 * Copyright (c) 2012 Ian Qvist
@@ -23,6 +28,7 @@
 using System;
 using System.Diagnostics;
 using FarseerPhysics.Common;
+using FarseerPhysics.Common.Maths;
 using Microsoft.Xna.Framework;
 
 namespace FarseerPhysics.Dynamics.Joints
@@ -225,10 +231,11 @@ namespace FarseerPhysics.Dynamics.Joints
             Vector2 vB = data.velocities[_indexB].v;
             float wB = data.velocities[_indexB].w;
 
-            Rot qA = new Rot(aA), qB = new Rot(aB);
+            Complex qA = Complex.FromAngle(aA);
+            Complex qB = Complex.FromAngle(aB);
 
-            _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
-            _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
+            _rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
+            _rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB);
 
             // Get the pulley axes.
             _uA = cA + _rA - WorldAnchorA;
@@ -256,8 +263,8 @@ namespace FarseerPhysics.Dynamics.Joints
             }
 
             // Compute effective mass.
-            float ruA = MathUtils.Cross(_rA, _uA);
-            float ruB = MathUtils.Cross(_rB, _uB);
+            float ruA = MathUtils.Cross(ref _rA, ref _uA);
+            float ruB = MathUtils.Cross(ref _rB, ref _uB);
 
             float mA = _invMassA + _invIA * ruA * ruA;
             float mB = _invMassB + _invIB * ruB * ruB;
@@ -269,7 +276,7 @@ namespace FarseerPhysics.Dynamics.Joints
                 _mass = 1.0f / _mass;
             }
 
-            if (Settings.EnableWarmstarting)
+            if (data.step.warmStarting)
             {
                 // Scale impulses to support variable time steps.
                 _impulse *= data.step.dtRatio;
@@ -279,9 +286,9 @@ namespace FarseerPhysics.Dynamics.Joints
                 Vector2 PB = (-Ratio * _impulse) * _uB;
 
                 vA += _invMassA * PA;
-                wA += _invIA * MathUtils.Cross(_rA, PA);
+                wA += _invIA * MathUtils.Cross(ref _rA, ref PA);
                 vB += _invMassB * PB;
-                wB += _invIB * MathUtils.Cross(_rB, PB);
+                wB += _invIB * MathUtils.Cross(ref _rB, ref PB);
             }
             else
             {
@@ -301,8 +308,8 @@ namespace FarseerPhysics.Dynamics.Joints
             Vector2 vB = data.velocities[_indexB].v;
             float wB = data.velocities[_indexB].w;
 
-            Vector2 vpA = vA + MathUtils.Cross(wA, _rA);
-            Vector2 vpB = vB + MathUtils.Cross(wB, _rB);
+            Vector2 vpA = vA + MathUtils.Cross(wA, ref _rA);
+            Vector2 vpB = vB + MathUtils.Cross(wB, ref _rB);
 
             float Cdot = -Vector2.Dot(_uA, vpA) - Ratio * Vector2.Dot(_uB, vpB);
             float impulse = -_mass * Cdot;
@@ -311,9 +318,9 @@ namespace FarseerPhysics.Dynamics.Joints
             Vector2 PA = -impulse * _uA;
             Vector2 PB = -Ratio * impulse * _uB;
             vA += _invMassA * PA;
-            wA += _invIA * MathUtils.Cross(_rA, PA);
+            wA += _invIA * MathUtils.Cross(ref _rA, ref PA);
             vB += _invMassB * PB;
-            wB += _invIB * MathUtils.Cross(_rB, PB);
+            wB += _invIB * MathUtils.Cross(ref _rB, ref PB);
 
             data.velocities[_indexA].v = vA;
             data.velocities[_indexA].w = wA;
@@ -328,10 +335,11 @@ namespace FarseerPhysics.Dynamics.Joints
             Vector2 cB = data.positions[_indexB].c;
             float aB = data.positions[_indexB].a;
 
-            Rot qA = new Rot(aA), qB = new Rot(aB);
+            Complex qA = Complex.FromAngle(aA);
+            Complex qB = Complex.FromAngle(aB);
 
-            Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
-            Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
+            Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
+            Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB);
 
             // Get the pulley axes.
             Vector2 uA = cA + rA - WorldAnchorA;
@@ -359,8 +367,8 @@ namespace FarseerPhysics.Dynamics.Joints
             }
 
             // Compute effective mass.
-            float ruA = MathUtils.Cross(rA, uA);
-            float ruB = MathUtils.Cross(rB, uB);
+            float ruA = MathUtils.Cross(ref rA, ref uA);
+            float ruB = MathUtils.Cross(ref rB, ref uB);
 
             float mA = _invMassA + _invIA * ruA * ruA;
             float mB = _invMassB + _invIB * ruB * ruB;
@@ -381,9 +389,9 @@ namespace FarseerPhysics.Dynamics.Joints
             Vector2 PB = -Ratio * impulse * uB;
 
             cA += _invMassA * PA;
-            aA += _invIA * MathUtils.Cross(rA, PA);
+            aA += _invIA * MathUtils.Cross(ref rA, ref PA);
             cB += _invMassB * PB;
-            aB += _invIB * MathUtils.Cross(rB, PB);
+            aB += _invIB * MathUtils.Cross(ref rB, ref PB);
 
             data.positions[_indexA].c = cA;
             data.positions[_indexA].a = aA;

Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/RevoluteJoint.cs

@@ -1,3 +1,8 @@
+/* Original source Farseer Physics Engine:
+ * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
+ * Microsoft Permissive License (Ms-PL) v1.1
+ */
+
 /*
 * Farseer Physics Engine:
 * Copyright (c) 2012 Ian Qvist
@@ -22,6 +27,7 @@
 
 using System;
 using FarseerPhysics.Common;
+using FarseerPhysics.Common.Maths;
 using Microsoft.Xna.Framework;
 
 namespace FarseerPhysics.Dynamics.Joints
@@ -322,10 +328,11 @@ namespace FarseerPhysics.Dynamics.Joints
             Vector2 vB = data.velocities[_indexB].v;
             float wB = data.velocities[_indexB].w;
 
-            Rot qA = new Rot(aA), qB = new Rot(aB);
+            Complex qA = Complex.FromAngle(aA);
+            Complex qB = Complex.FromAngle(aB);
 
-            _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
-            _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
+            _rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
+            _rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB);
 
             // J = [-I -r1_skew I r2_skew]
             //     [ 0       -1 0       1]
@@ -396,7 +403,7 @@ namespace FarseerPhysics.Dynamics.Joints
                 _limitState = LimitState.Inactive;
             }
 
-            if (Settings.EnableWarmstarting)
+            if (data.step.warmStarting)
             {
                 // Scale impulses to support a variable time step.
                 _impulse *= data.step.dtRatio;
@@ -405,10 +412,10 @@ namespace FarseerPhysics.Dynamics.Joints
                 Vector2 P = new Vector2(_impulse.X, _impulse.Y);
 
                 vA -= mA * P;
-                wA -= iA * (MathUtils.Cross(_rA, P) + MotorImpulse + _impulse.Z);
+                wA -= iA * (MathUtils.Cross(ref _rA, ref P) + MotorImpulse + _impulse.Z);
 
                 vB += mB * P;
-                wB += iB * (MathUtils.Cross(_rB, P) + MotorImpulse + _impulse.Z);
+                wB += iB * (MathUtils.Cross(ref _rB, ref P) + MotorImpulse + _impulse.Z);
             }
             else
             {
@@ -451,7 +458,7 @@ namespace FarseerPhysics.Dynamics.Joints
             // Solve limit constraint.
             if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false)
             {
-                Vector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
+                Vector2 Cdot1 = vB + MathUtils.Cross(wB, ref _rB) - vA - MathUtils.Cross(wA, ref _rA);
                 float Cdot2 = wB - wA;
                 Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
 
@@ -503,25 +510,25 @@ namespace FarseerPhysics.Dynamics.Joints
                 Vector2 P = new Vector2(impulse.X, impulse.Y);
 
                 vA -= mA * P;
-                wA -= iA * (MathUtils.Cross(_rA, P) + impulse.Z);
+                wA -= iA * (MathUtils.Cross(ref _rA, ref P) + impulse.Z);
 
                 vB += mB * P;
-                wB += iB * (MathUtils.Cross(_rB, P) + impulse.Z);
+                wB += iB * (MathUtils.Cross(ref _rB, ref P) + impulse.Z);
             }
             else
             {
                 // Solve point-to-point constraint
-                Vector2 Cdot = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
+                Vector2 Cdot = vB + MathUtils.Cross(wB, ref _rB) - vA - MathUtils.Cross(wA, ref _rA);
                 Vector2 impulse = _mass.Solve22(-Cdot);
 
                 _impulse.X += impulse.X;
                 _impulse.Y += impulse.Y;
 
                 vA -= mA * impulse;
-                wA -= iA * MathUtils.Cross(_rA, impulse);
+                wA -= iA * MathUtils.Cross(ref _rA, ref impulse);
 
                 vB += mB * impulse;
-                wB += iB * MathUtils.Cross(_rB, impulse);
+                wB += iB * MathUtils.Cross(ref _rB, ref impulse);
             }
 
             data.velocities[_indexA].v = vA;
@@ -537,7 +544,6 @@ namespace FarseerPhysics.Dynamics.Joints
             Vector2 cB = data.positions[_indexB].c;
             float aB = data.positions[_indexB].a;
 
-            Rot qA = new Rot(aA), qB = new Rot(aB);
 
             float angularError = 0.0f;
             float positionError;
@@ -582,10 +588,10 @@ namespace FarseerPhysics.Dynamics.Joints
 
             // Solve point-to-point constraint.
             {
-                qA.Set(aA);
-                qB.Set(aB);
-                Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
-                Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
+                Complex qA = Complex.FromAngle(aA);
+                Complex qB = Complex.FromAngle(aB);
+                Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
+                Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB);
 
                 Vector2 C = cB + rB - cA - rA;
                 positionError = C.Length();
@@ -602,10 +608,10 @@ namespace FarseerPhysics.Dynamics.Joints
                 Vector2 impulse = -K.Solve(C);
 
                 cA -= mA * impulse;
-                aA -= iA * MathUtils.Cross(rA, impulse);
+                aA -= iA * MathUtils.Cross(ref rA, ref impulse);
 
                 cB += mB * impulse;
-                aB += iB * MathUtils.Cross(rB, impulse);
+                aB += iB * MathUtils.Cross(ref rB, ref impulse);
             }
 
             data.positions[_indexA].c = cA;

Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/RopeJoint.cs

@@ -1,4 +1,9 @@
-/*
+/* Original source Farseer Physics Engine:
+ * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
+ * Microsoft Permissive License (Ms-PL) v1.1
+ */
+
+/*
 * Farseer Physics Engine:
 * Copyright (c) 2012 Ian Qvist
 * 
@@ -22,6 +27,7 @@
 
 using System;
 using FarseerPhysics.Common;
+using FarseerPhysics.Common.Maths;
 using Microsoft.Xna.Framework;
 
 namespace FarseerPhysics.Dynamics.Joints
@@ -159,10 +165,11 @@ namespace FarseerPhysics.Dynamics.Joints
             Vector2 vB = data.velocities[_indexB].v;
             float wB = data.velocities[_indexB].w;
 
-            Rot qA = new Rot(aA), qB = new Rot(aB);
+            Complex qA = Complex.FromAngle(aA);
+            Complex qB = Complex.FromAngle(aB);
 
-            _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
-            _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
+            _rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
+            _rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB);
             _u = cB + _rB - cA - _rA;
 
             _length = _u.Length();
@@ -190,22 +197,22 @@ namespace FarseerPhysics.Dynamics.Joints
             }
 
             // Compute effective mass.
-            float crA = MathUtils.Cross(_rA, _u);
-            float crB = MathUtils.Cross(_rB, _u);
+            float crA = MathUtils.Cross(ref _rA, ref _u);
+            float crB = MathUtils.Cross(ref _rB, ref _u);
             float invMass = _invMassA + _invIA * crA * crA + _invMassB + _invIB * crB * crB;
 
             _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
 
-            if (Settings.EnableWarmstarting)
+            if (data.step.warmStarting)
             {
                 // Scale the impulse to support a variable time step.
                 _impulse *= data.step.dtRatio;
 
                 Vector2 P = _impulse * _u;
                 vA -= _invMassA * P;
-                wA -= _invIA * MathUtils.Cross(_rA, P);
+                wA -= _invIA * MathUtils.Cross(ref _rA, ref P);
                 vB += _invMassB * P;
-                wB += _invIB * MathUtils.Cross(_rB, P);
+                wB += _invIB * MathUtils.Cross(ref _rB, ref P);
             }
             else
             {
@@ -226,8 +233,8 @@ namespace FarseerPhysics.Dynamics.Joints
             float wB = data.velocities[_indexB].w;
 
             // Cdot = dot(u, v + cross(w, r))
-            Vector2 vpA = vA + MathUtils.Cross(wA, _rA);
-            Vector2 vpB = vB + MathUtils.Cross(wB, _rB);
+            Vector2 vpA = vA + MathUtils.Cross(wA, ref _rA);
+            Vector2 vpB = vB + MathUtils.Cross(wB, ref _rB);
             float C = _length - MaxLength;
             float Cdot = Vector2.Dot(_u, vpB - vpA);
 
@@ -244,9 +251,9 @@ namespace FarseerPhysics.Dynamics.Joints
 
             Vector2 P = impulse * _u;
             vA -= _invMassA * P;
-            wA -= _invIA * MathUtils.Cross(_rA, P);
+            wA -= _invIA * MathUtils.Cross(ref _rA, ref P);
             vB += _invMassB * P;
-            wB += _invIB * MathUtils.Cross(_rB, P);
+            wB += _invIB * MathUtils.Cross(ref _rB, ref P);
 
             data.velocities[_indexA].v = vA;
             data.velocities[_indexA].w = wA;
@@ -261,10 +268,11 @@ namespace FarseerPhysics.Dynamics.Joints
             Vector2 cB = data.positions[_indexB].c;
             float aB = data.positions[_indexB].a;
 
-            Rot qA = new Rot(aA), qB = new Rot(aB);
+            Complex qA = Complex.FromAngle(aA);
+            Complex qB = Complex.FromAngle(aB);
 
-            Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
-            Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
+            Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
+            Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB);
             Vector2 u = cB + rB - cA - rA;
 
             float length = u.Length(); u.Normalize();
@@ -276,9 +284,9 @@ namespace FarseerPhysics.Dynamics.Joints
             Vector2 P = impulse * u;
 
             cA -= _invMassA * P;
-            aA -= _invIA * MathUtils.Cross(rA, P);
+            aA -= _invIA * MathUtils.Cross(ref rA, ref P);
             cB += _invMassB * P;
-            aB += _invIB * MathUtils.Cross(rB, P);
+            aB += _invIB * MathUtils.Cross(ref rB, ref P);
 
             data.positions[_indexA].c = cA;
             data.positions[_indexA].a = aA;

Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/WeldJoint.cs

@@ -1,3 +1,8 @@
+/* Original source Farseer Physics Engine:
+ * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
+ * Microsoft Permissive License (Ms-PL) v1.1
+ */
+
 /*
 * Farseer Physics Engine:
 * Copyright (c) 2012 Ian Qvist
@@ -22,6 +27,7 @@
 
 using System;
 using FarseerPhysics.Common;
+using FarseerPhysics.Common.Maths;
 using Microsoft.Xna.Framework;
 
 namespace FarseerPhysics.Dynamics.Joints
@@ -68,11 +74,6 @@ namespace FarseerPhysics.Dynamics.Joints
         private float _invIB;
         private Mat33 _mass;
 
-        /// <summary>
-        /// If true, body B is treated as if it was kinematic (i.e. as if it had infinite mass)
-        /// </summary>
-        public bool KinematicBodyB;
-
         internal WeldJoint()
         {
             JointType = JointType.Weld;
@@ -116,6 +117,11 @@ namespace FarseerPhysics.Dynamics.Joints
         /// </summary>
         public Vector2 LocalAnchorB { get; set; }
 
+        /// <summary>
+        /// If true, body B is treated as if it was kinematic (i.e. as if it had infinite mass)
+        /// </summary>
+        public bool KinematicBodyB;
+
         public override Vector2 WorldAnchorA
         {
             get { return BodyA.GetWorldPoint(LocalAnchorA); }
@@ -165,6 +171,7 @@ namespace FarseerPhysics.Dynamics.Joints
             _invMassA = BodyA._invMass;
             _invMassB = KinematicBodyB ? 0.0f : BodyB._invMass;
             _invIA = BodyA._invI;
+            _invIB = BodyB._invI;
             _invIB = KinematicBodyB ? 0.0f : BodyB._invI;
 
             float aA = data.positions[_indexA].a;
@@ -175,10 +182,11 @@ namespace FarseerPhysics.Dynamics.Joints
             Vector2 vB = data.velocities[_indexB].v;
             float wB = data.velocities[_indexB].w;
 
-            Rot qA = new Rot(aA), qB = new Rot(aB);
+            Complex qA = Complex.FromAngle(aA);
+            Complex qB = Complex.FromAngle(aB);
 
-            _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
-            _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
+            _rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
+            _rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB);
 
             // J = [-I -r1_skew I r2_skew]
             //     [ 0       -1 0       1]
@@ -213,7 +221,7 @@ namespace FarseerPhysics.Dynamics.Joints
                 float C = aB - aA - ReferenceAngle;
 
                 // Frequency
-                float omega = 2.0f * Settings.Pi * FrequencyHz;
+                float omega = 2.0f * MathHelper.Pi * FrequencyHz;
 
                 // Damping coefficient
                 float d = 2.0f * m * DampingRatio * omega;
@@ -230,6 +238,12 @@ namespace FarseerPhysics.Dynamics.Joints
                 invM += _gamma;
                 _mass.ez.Z = invM != 0.0f ? 1.0f / invM : 0.0f;
             }
+            else if (K.ez.Z == 0.0f)
+            {
+                K.GetInverse22(ref _mass);
+                _gamma = 0.0f;
+                _bias = 0.0f;
+            }
             else
             {
                 K.GetSymInverse33(ref _mass);
@@ -237,7 +251,7 @@ namespace FarseerPhysics.Dynamics.Joints
                 _bias = 0.0f;
             }
 
-            if (Settings.EnableWarmstarting)
+            if (data.step.warmStarting)
             {
                 // Scale impulses to support a variable time step.
                 _impulse *= data.step.dtRatio;
@@ -245,10 +259,10 @@ namespace FarseerPhysics.Dynamics.Joints
                 Vector2 P = new Vector2(_impulse.X, _impulse.Y);
 
                 vA -= mA * P;
-                wA -= iA * (MathUtils.Cross(_rA, P) + _impulse.Z);
+                wA -= iA * (MathUtils.Cross(ref _rA, ref P) + _impulse.Z);
 
                 vB += mB * P;
-                wB += iB * (MathUtils.Cross(_rB, P) + _impulse.Z);
+                wB += iB * (MathUtils.Cross(ref _rB, ref P) + _impulse.Z);
             }
             else
             {
@@ -281,7 +295,7 @@ namespace FarseerPhysics.Dynamics.Joints
                 wA -= iA * impulse2;
                 wB += iB * impulse2;
 
-                Vector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
+                Vector2 Cdot1 = vB + MathUtils.Cross(wB, ref _rB) - vA - MathUtils.Cross(wA, ref _rA);
 
                 Vector2 impulse1 = -MathUtils.Mul22(_mass, Cdot1);
                 _impulse.X += impulse1.X;
@@ -290,14 +304,14 @@ namespace FarseerPhysics.Dynamics.Joints
                 Vector2 P = impulse1;
 
                 vA -= mA * P;
-                wA -= iA * MathUtils.Cross(_rA, P);
+                wA -= iA * MathUtils.Cross(ref _rA, ref P);
 
                 vB += mB * P;
-                wB += iB * MathUtils.Cross(_rB, P);
+                wB += iB * MathUtils.Cross(ref _rB, ref P);
             }
             else
             {
-                Vector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
+                Vector2 Cdot1 = vB + MathUtils.Cross(wB, ref _rB) - vA - MathUtils.Cross(wA, ref _rA);
                 float Cdot2 = wB - wA;
                 Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
 
@@ -307,10 +321,10 @@ namespace FarseerPhysics.Dynamics.Joints
                 Vector2 P = new Vector2(impulse.X, impulse.Y);
 
                 vA -= mA * P;
-                wA -= iA * (MathUtils.Cross(_rA, P) + impulse.Z);
+                wA -= iA * (MathUtils.Cross(ref _rA, ref P) + impulse.Z);
 
                 vB += mB * P;
-                wB += iB * (MathUtils.Cross(_rB, P) + impulse.Z);
+                wB += iB * (MathUtils.Cross(ref _rB, ref P) + impulse.Z);
             }
 
             data.velocities[_indexA].v = vA;
@@ -326,13 +340,14 @@ namespace FarseerPhysics.Dynamics.Joints
             Vector2 cB = data.positions[_indexB].c;
             float aB = data.positions[_indexB].a;
 
-            Rot qA = new Rot(aA), qB = new Rot(aB);
+            Complex qA = Complex.FromAngle(aA);
+            Complex qB = Complex.FromAngle(aB);
 
             float mA = _invMassA, mB = _invMassB;
             float iA = _invIA, iB = _invIB;
 
-            Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
-            Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
+            Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
+            Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB);
 
             float positionError, angularError;
 
@@ -357,10 +372,10 @@ namespace FarseerPhysics.Dynamics.Joints
                 Vector2 P = -K.Solve22(C1);
 
                 cA -= mA * P;
-                aA -= iA * MathUtils.Cross(rA, P);
+                aA -= iA * MathUtils.Cross(ref rA, ref P);
 
                 cB += mB * P;
-                aB += iB * MathUtils.Cross(rB, P);
+                aB += iB * MathUtils.Cross(ref rB, ref P);
             }
             else
             {
@@ -372,14 +387,23 @@ namespace FarseerPhysics.Dynamics.Joints
 
                 Vector3 C = new Vector3(C1.X, C1.Y, C2);
 
-                Vector3 impulse = -K.Solve33(C);
+                Vector3 impulse;
+                if (K.ez.Z <= 0.0f)
+                {
+                    Vector2 impulse2 = -K.Solve22(C1);
+                    impulse = new Vector3(impulse2.X, impulse2.Y, 0.0f);
+                }
+                else 
+                {
+                    impulse = -K.Solve33(C);
+                }
                 Vector2 P = new Vector2(impulse.X, impulse.Y);
 
                 cA -= mA * P;
-                aA -= iA * (MathUtils.Cross(rA, P) + impulse.Z);
+                aA -= iA * (MathUtils.Cross(ref rA, ref P) + impulse.Z);
 
                 cB += mB * P;
-                aB += iB * (MathUtils.Cross(rB, P) + impulse.Z);
+                aB += iB * (MathUtils.Cross(ref rB, ref P) + impulse.Z);
             }
 
             data.positions[_indexA].c = cA;

Libraries/Farseer Physics Engine 3.5/Dynamics/Joints/WheelJoint.cs

@@ -1,3 +1,8 @@
+/* Original source Farseer Physics Engine:
+ * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
+ * Microsoft Permissive License (Ms-PL) v1.1
+ */
+
 /*
 * Farseer Physics Engine:
 * Copyright (c) 2012 Ian Qvist
@@ -22,6 +27,7 @@
 
 using System;
 using FarseerPhysics.Common;
+using FarseerPhysics.Common.Maths;
 using Microsoft.Xna.Framework;
 
 namespace FarseerPhysics.Dynamics.Joints
@@ -52,6 +58,7 @@ namespace FarseerPhysics.Dynamics.Joints
     public class WheelJoint : Joint
     {
         // Solver shared
+        private Vector2 _localXAxis;
         private Vector2 _localYAxis;
 
         private float _impulse;
@@ -147,15 +154,15 @@ namespace FarseerPhysics.Dynamics.Joints
             set
             {
                 _axis = value;
-                LocalXAxis = BodyA.GetLocalVector(_axis);
-                _localYAxis = MathUtils.Cross(1.0f, LocalXAxis);
+                _localXAxis = BodyA.GetLocalVector(_axis);
+                _localYAxis = MathUtils.Rot90(ref _localXAxis);
             }
         }
 
         /// <summary>
         /// The axis in local coordinates relative to BodyA
         /// </summary>
-        public Vector2 LocalXAxis { get; private set; }
+        public Vector2 LocalXAxis { get { return _localXAxis; } }
 
         /// <summary>
         /// The desired motor speed in radians per second.
@@ -206,7 +213,7 @@ namespace FarseerPhysics.Dynamics.Joints
                 Vector2 pA = bA.GetWorldPoint(LocalAnchorA);
                 Vector2 pB = bB.GetWorldPoint(LocalAnchorB);
                 Vector2 d = pB - pA;
-                Vector2 axis = bA.GetWorldVector(LocalXAxis);
+                Vector2 axis = bA.GetWorldVector(ref _localXAxis);
 
                 float translation = Vector2.Dot(d, axis);
                 return translation;
@@ -282,18 +289,19 @@ namespace FarseerPhysics.Dynamics.Joints
             Vector2 vB = data.velocities[_indexB].v;
             float wB = data.velocities[_indexB].w;
 
-            Rot qA = new Rot(aA), qB = new Rot(aB);
+            Complex qA = Complex.FromAngle(aA);
+            Complex qB = Complex.FromAngle(aB);
 
             // Compute the effective masses.
-            Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
-            Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
+            Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
+            Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB);
             Vector2 d1 = cB + rB - cA - rA;
 
             // Point to line constraint
             {
-                _ay = MathUtils.Mul(qA, _localYAxis);
+                _ay = Complex.Multiply(ref _localYAxis, ref qA);
                 _sAy = MathUtils.Cross(d1 + rA, _ay);
-                _sBy = MathUtils.Cross(rB, _ay);
+                _sBy = MathUtils.Cross(ref rB, ref _ay);
 
                 _mass = mA + mB + iA * _sAy * _sAy + iB * _sBy * _sBy;
 
@@ -309,9 +317,9 @@ namespace FarseerPhysics.Dynamics.Joints
             _gamma = 0.0f;
             if (Frequency > 0.0f)
             {
-                _ax = MathUtils.Mul(qA, LocalXAxis);
+                _ax = Complex.Multiply(ref _localXAxis, ref qA);
                 _sAx = MathUtils.Cross(d1 + rA, _ax);
-                _sBx = MathUtils.Cross(rB, _ax);
+                _sBx = MathUtils.Cross(ref rB, ref _ax);
 
                 float invMass = mA + mB + iA * _sAx * _sAx + iB * _sBx * _sBx;
 
@@ -322,7 +330,7 @@ namespace FarseerPhysics.Dynamics.Joints
                     float C = Vector2.Dot(d1, _ax);
 
                     // Frequency
-                    float omega = 2.0f * Settings.Pi * Frequency;
+                    float omega = 2.0f * MathHelper.Pi * Frequency;
 
                     // Damping coefficient
                     float d = 2.0f * _springMass * DampingRatio * omega;
@@ -367,7 +375,7 @@ namespace FarseerPhysics.Dynamics.Joints
                 _motorImpulse = 0.0f;
             }
 
-            if (Settings.EnableWarmstarting)
+            if (data.step.warmStarting)
             {
                 // Account for variable time step.
                 _impulse *= data.step.dtRatio;
@@ -468,16 +476,17 @@ namespace FarseerPhysics.Dynamics.Joints
             Vector2 cB = data.positions[_indexB].c;
             float aB = data.positions[_indexB].a;
 
-            Rot qA = new Rot(aA), qB = new Rot(aB);
+            Complex qA = Complex.FromAngle(aA);
+            Complex qB = Complex.FromAngle(aB);
 
-            Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
-            Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
+            Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
+            Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB);
             Vector2 d = (cB - cA) + rB - rA;
 
-            Vector2 ay = MathUtils.Mul(qA, _localYAxis);
+            Vector2 ay = Complex.Multiply(ref _localYAxis, ref qA);
 
             float sAy = MathUtils.Cross(d + rA, ay);
-            float sBy = MathUtils.Cross(rB, ay);
+            float sBy = MathUtils.Cross(ref rB, ref ay);
 
             float C = Vector2.Dot(d, ay);
 

Libraries/Farseer Physics Engine 3.5/Dynamics/SolverIterations.cs

@@ -0,0 +1,19 @@
+// Copyright (c) 2018 Kastellanos Nikolaos
+
+namespace FarseerPhysics.Dynamics
+{
+    public struct SolverIterations
+    {
+        /// <summary>The number of velocity iterations used in the solver.</summary>
+        public int VelocityIterations;
+
+        /// <summary>The number of position iterations used in the solver.</summary>
+        public int PositionIterations;
+
+        /// <summary>The number of velocity iterations in the TOI solver</summary>
+        public int TOIVelocityIterations;
+
+        /// <summary>The number of position iterations in the TOI solver</summary>
+        public int TOIPositionIterations;
+    }
+}

Libraries/Farseer Physics Engine 3.5/Dynamics/TimeStep.cs

@@ -1,4 +1,9 @@
-/*
+/* Original source Farseer Physics Engine:
+ * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
+ * Microsoft Permissive License (Ms-PL) v1.1
+ */
+
+/*
 * Original source Box2D:
 * Copyright (c) 2006-2011 Erin Catto http://www.box2d.org 
 * 
@@ -40,6 +45,11 @@ namespace FarseerPhysics.Dynamics
         /// Inverse time step (0 if dt == 0).
         /// </summary>
         public float inv_dt;
+
+        public int positionIterations;
+        public int velocityIterations;
+
+        public bool warmStarting;
     }
 
     /// This is an internal structure.
@@ -47,6 +57,7 @@ namespace FarseerPhysics.Dynamics
     {
         public Vector2 c;
         public float a;
+        internal int Lock;
     }
 
     /// This is an internal structure.
@@ -54,6 +65,7 @@ namespace FarseerPhysics.Dynamics
     {
         public Vector2 v;
         public float w;
+        internal int Lock;
     }
 
     /// Solver Data

Libraries/Farseer Physics Engine 3.5/Dynamics/World.Factory.cs

@@ -0,0 +1,260 @@
+// Copyright (c) 2017 Kastellanos Nikolaos
+
+/* Original source Farseer Physics Engine:
+ * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
+ * Microsoft Permissive License (Ms-PL) v1.1
+ */
+
+using System;
+using System.Collections.Generic;
+using Microsoft.Xna.Framework;
+using FarseerPhysics.Collision.Shapes;
+using FarseerPhysics.Common;
+using FarseerPhysics.Common.Decomposition;
+using FarseerPhysics.Dynamics.Joints;
+
+namespace FarseerPhysics.Dynamics
+{
+    public partial class World
+    {
+        public virtual Body CreateBody(Vector2 position = new Vector2(), float rotation = 0, BodyType bodyType = BodyType.Static)
+        {
+            Body body = new Body();
+            body.Position = position;
+            body.Rotation = rotation;            
+            body.BodyType = bodyType;
+            
+            AddAsync(body);
+
+            return body;
+        }
+
+        public Body CreateEdge(Vector2 start, Vector2 end)
+        {
+            Body body = CreateBody();
+
+            body.CreateEdge(start, end);
+            return body;
+        }
+
+        public Body CreateChainShape(Vertices vertices, Vector2 position = new Vector2())
+        {
+            Body body = CreateBody(position);
+
+            body.CreateChainShape(vertices);
+            return body;
+        }
+
+        public Body CreateLoopShape(Vertices vertices, Vector2 position = new Vector2())
+        {
+            Body body = CreateBody(position);
+
+            body.CreateLoopShape(vertices);
+            return body;
+        }
+
+        public Body CreateRectangle(float width, float height, float density, Vector2 position = new Vector2(), float rotation = 0, BodyType bodyType = BodyType.Static)
+        {
+            if (width <= 0)
+                throw new ArgumentOutOfRangeException("width", "Width must be more than 0 meters");
+
+            if (height <= 0)
+                throw new ArgumentOutOfRangeException("height", "Height must be more than 0 meters");
+
+            Body body = CreateBody(position, rotation, bodyType);
+
+            Vertices rectangleVertices = PolygonTools.CreateRectangle(width / 2, height / 2);
+            body.CreatePolygon(rectangleVertices, density);
+
+            return body;
+        }
+
+        public Body CreateCircle(float radius, float density, Vector2 position = new Vector2(), BodyType bodyType = BodyType.Static)
+        {
+            Body body = CreateBody(position, 0, bodyType);
+            body.CreateCircle(radius, density);
+            return body;
+        }
+
+        public Body CreateEllipse(float xRadius, float yRadius, int edges, float density, Vector2 position = new Vector2(), float rotation = 0, BodyType bodyType = BodyType.Static)
+        {
+            Body body = CreateBody(position, rotation, bodyType);
+            body.CreateEllipse(xRadius, yRadius, edges, density);
+            return body;
+        }
+
+        public Body CreatePolygon(Vertices vertices, float density, Vector2 position = new Vector2(), float rotation = 0, BodyType bodyType = BodyType.Static)
+        {
+            Body body = CreateBody(position, rotation, bodyType);
+            body.CreatePolygon(vertices, density);
+            return body;
+        }
+
+        public Body CreateCompoundPolygon(List<Vertices> list, float density, Vector2 position = new Vector2(), float rotation = 0, BodyType bodyType = BodyType.Static)
+        {
+            //We create a single body
+            Body body = CreateBody(position, rotation, bodyType);
+            body.CreateCompoundPolygon(list, density);
+            return body;
+        }
+
+        public Body CreateGear(float radius, int numberOfTeeth, float tipPercentage, float toothHeight, float density, Vector2 position = new Vector2(), float rotation = 0, BodyType bodyType = BodyType.Static)
+        {
+            Vertices gearPolygon = PolygonTools.CreateGear(radius, numberOfTeeth, tipPercentage, toothHeight);
+
+            //Gears can in some cases be convex
+            if (!gearPolygon.IsConvex())
+            {
+                //Decompose the gear:
+                List<Vertices> list = Triangulate.ConvexPartition(gearPolygon, TriangulationAlgorithm.Earclip);
+
+                return CreateCompoundPolygon(list, density, position, rotation, bodyType);
+            }
+
+            return CreatePolygon(gearPolygon, density, position, rotation, bodyType);
+        }
+
+        public Body CreateCapsule(float height, float topRadius, int topEdges, float bottomRadius, int bottomEdges, float density, Vector2 position = new Vector2(), float rotation = 0, BodyType bodyType = BodyType.Static)
+        {
+            Vertices verts = PolygonTools.CreateCapsule(height, topRadius, topEdges, bottomRadius, bottomEdges);
+
+            //There are too many vertices in the capsule. We decompose it.
+            if (verts.Count >= Settings.MaxPolygonVertices)
+            {
+                List<Vertices> vertList = Triangulate.ConvexPartition(verts, TriangulationAlgorithm.Earclip);
+                return CreateCompoundPolygon(vertList, density, position, rotation, bodyType);
+            }
+
+            return CreatePolygon(verts, density, position, rotation, bodyType);
+        }
+
+        public Body CreateCapsuleHorizontal(float width, float endRadius, float density, Vector2 position = new Vector2(), float rotation = 0, BodyType bodyType = BodyType.Static)
+        {
+            //Create the middle rectangle
+            Vertices rectangle = PolygonTools.CreateRectangle(width / 2, endRadius);
+
+            List<Vertices> list = new List<Vertices>();
+            list.Add(rectangle);
+
+            Body body = CreateCompoundPolygon(list, density, position, rotation, bodyType);
+            body.CreateCircle(endRadius, density, new Vector2(width / 2, 0));
+            body.CreateCircle(endRadius, density, new Vector2(-width / 2, 0));
+
+            //Create the two circles
+            //CircleShape topCircle = new CircleShape(endRadius, density);
+            //topCircle.Position = new Vector2(0, height / 2);
+            //body.CreateFixture(topCircle);
+
+            //CircleShape bottomCircle = new CircleShape(endRadius, density);
+            //bottomCircle.Position = new Vector2(0, -(height / 2));
+            //body.CreateFixture(bottomCircle);
+            return body;
+        }
+        public Body CreateCapsule(float height, float endRadius, float density, Vector2 position = new Vector2(), float rotation = 0, BodyType bodyType = BodyType.Static)
+        {
+            //Create the middle rectangle
+            Vertices rectangle = PolygonTools.CreateRectangle(endRadius, height / 2);
+
+            List<Vertices> list = new List<Vertices>();
+            list.Add(rectangle);
+
+            Body body = CreateCompoundPolygon(list, density, position, rotation, bodyType);
+            body.CreateCircle(endRadius, density, new Vector2(0, height / 2));
+            body.CreateCircle(endRadius, density, new Vector2(0, -(height / 2)));
+
+            //Create the two circles
+            //CircleShape topCircle = new CircleShape(endRadius, density);
+            //topCircle.Position = new Vector2(0, height / 2);
+            //body.CreateFixture(topCircle);
+
+            //CircleShape bottomCircle = new CircleShape(endRadius, density);
+            //bottomCircle.Position = new Vector2(0, -(height / 2));
+            //body.CreateFixture(bottomCircle);
+            return body;
+        }
+
+        public Body CreateRoundedRectangle(float width, float height, float xRadius, float yRadius, int segments, float density, Vector2 position = new Vector2(), float rotation = 0, BodyType bodyType = BodyType.Static)
+        {
+            Vertices verts = PolygonTools.CreateRoundedRectangle(width, height, xRadius, yRadius, segments);
+
+            //There are too many vertices in the capsule. We decompose it.
+            if (verts.Count >= Settings.MaxPolygonVertices)
+            {
+                List<Vertices> vertList = Triangulate.ConvexPartition(verts, TriangulationAlgorithm.Earclip);
+                return CreateCompoundPolygon(vertList, density, position, rotation, bodyType);
+            }
+
+            return CreatePolygon(verts, density, position, rotation, bodyType);
+        }
+
+        public Body CreateLineArc(float radians, int sides, float radius, bool closed = false, Vector2 position = new Vector2(), float rotation = 0, BodyType bodyType = BodyType.Static)
+        {
+            Body body = CreateBody(position, rotation, bodyType);
+            body.CreateLineArc(radians, sides, radius, closed);
+            return body;
+        }
+
+        public Body CreateSolidArc(float density, float radians, int sides, float radius, Vector2 position = new Vector2(), float rotation = 0, BodyType bodyType = BodyType.Static)
+        {
+            Body body = CreateBody(position, rotation, bodyType);
+            body.CreateSolidArc(density, radians, sides, radius);
+
+            return body;
+        }
+
+        /// <summary>
+        /// Creates a chain.
+        /// </summary>
+        /// <param name="world">The world.</param>
+        /// <param name="start">The start.</param>
+        /// <param name="end">The end.</param>
+        /// <param name="linkWidth">The width.</param>
+        /// <param name="linkHeight">The height.</param>
+        /// <param name="numberOfLinks">The number of links.</param>
+        /// <param name="linkDensity">The link density.</param>
+        /// <param name="attachRopeJoint">Creates a rope joint between start and end. This enforces the length of the rope. Said in another way: it makes the rope less bouncy.</param>
+        /// <returns></returns>
+        public Path CreateChain(Vector2 start, Vector2 end, float linkWidth, float linkHeight, int numberOfLinks, float linkDensity, bool attachRopeJoint)
+        {
+            System.Diagnostics.Debug.Assert(numberOfLinks >= 2);
+
+            //Chain start / end
+            Path path = new Path();
+            path.Add(start);
+            path.Add(end);
+
+            //A single chainlink
+            PolygonShape shape = new PolygonShape(PolygonTools.CreateRectangle(linkWidth, linkHeight), linkDensity);
+
+            //Use PathManager to create all the chainlinks based on the chainlink created before.
+            List<Body> chainLinks = PathManager.EvenlyDistributeShapesAlongPath(this, path, shape, BodyType.Dynamic, numberOfLinks);
+
+            //TODO
+            //if (fixStart)
+            //{
+            //    //Fix the first chainlink to the world
+            //    JointFactory.CreateFixedRevoluteJoint(this, chainLinks[0], new Vector2(0, -(linkHeight / 2)),
+            //                                          chainLinks[0].Position);
+            //}
+
+            //if (fixEnd)
+            //{
+            //    //Fix the last chainlink to the world
+            //    JointFactory.CreateFixedRevoluteJoint(this, chainLinks[chainLinks.Count - 1],
+            //                                          new Vector2(0, (linkHeight / 2)),
+            //                                          chainLinks[chainLinks.Count - 1].Position);
+            //}
+
+            //Attach all the chainlinks together with a revolute joint
+            PathManager.AttachBodiesWithRevoluteJoint(this, chainLinks, new Vector2(0, -linkHeight), new Vector2(0, linkHeight), false, false);
+
+            if (attachRopeJoint)
+                JointFactory.CreateRopeJoint(this, chainLinks[0], chainLinks[chainLinks.Count - 1], Vector2.Zero, Vector2.Zero);
+
+            return path;
+        }
+
+
+
+    }
+}

Libraries/Farseer Physics Engine 3.5/Dynamics/WorldCallbacks.cs

@@ -1,4 +1,11 @@
-/*
+// Copyright (c) 2017 Kastellanos Nikolaos
+
+/* Original source Farseer Physics Engine:
+ * Copyright (c) 2014 Ian Qvist, http://farseerphysics.codeplex.com
+ * Microsoft Permissive License (Ms-PL) v1.1
+ */
+
+/*
 * Farseer Physics Engine:
 * Copyright (c) 2012 Ian Qvist
 * 
@@ -41,23 +48,23 @@ namespace FarseerPhysics.Dynamics
 
     public delegate void PostSolveDelegate(Contact contact, ContactVelocityConstraint impulse);
 
-    public delegate void FixtureDelegate(Fixture fixture);
+    public delegate void FixtureDelegate(World sender, Body body, Fixture fixture);
 
-    public delegate void JointDelegate(Joint joint);
+    public delegate void JointDelegate(World sender, Joint joint);
 
-    public delegate void BodyDelegate(Body body);
+    public delegate void BodyDelegate(World sender, Body body);
 
-    public delegate void ControllerDelegate(Controller controller);
+    public delegate void ControllerDelegate(World sender, Controller controller);
 
     public delegate bool CollisionFilterDelegate(Fixture fixtureA, Fixture fixtureB);
 
-    public delegate void BroadphaseDelegate(ref FixtureProxy proxyA, ref FixtureProxy proxyB);
+    public delegate void BroadphaseDelegate(int proxyIdA, int proxyIdB);
 
-    public delegate bool BeforeCollisionEventHandler(Fixture fixtureA, Fixture fixtureB);
+    public delegate bool BeforeCollisionEventHandler(Fixture sender, Fixture other);
 
-    public delegate bool OnCollisionEventHandler(Fixture fixtureA, Fixture fixtureB, Contact contact);
+    public delegate bool OnCollisionEventHandler(Fixture sender, Fixture other, Contact contact);
 
-    public delegate void AfterCollisionEventHandler(Fixture fixtureA, Fixture fixtureB, Contact contact, ContactVelocityConstraint impulse);
+    public delegate void AfterCollisionEventHandler(Fixture sender, Fixture other, Contact contact, ContactVelocityConstraint impulse);
 
-    public delegate void OnSeparationEventHandler(Fixture fixtureA, Fixture fixtureB);
+    public delegate void OnSeparationEventHandler(Fixture sender, Fixture other, Contact contact);
 }