Adding initial files

axios/Controllers/AbstractForceController.cs

@@ -0,0 +1,323 @@
+using System;
+using FarseerPhysics.Dynamics;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Controllers
+{
+    public abstract class AbstractForceController : Controller
+    {
+        #region DecayModes enum
+
+        /// <summary>
+        /// Modes for Decay. Actual Decay must be implemented in inheriting 
+        /// classes
+        /// </summary>
+        public enum DecayModes
+        {
+            None,
+            Step,
+            Linear,
+            InverseSquare,
+            Curve
+        }
+
+        #endregion
+
+        #region ForceTypes enum
+
+        /// <summary>
+        /// Forcetypes are used in the decay math to properly get the distance.
+        /// They are also used to draw a representation in DebugView
+        /// </summary>
+        public enum ForceTypes
+        {
+            Point,
+            Line,
+            Area
+        }
+
+        #endregion
+
+        #region TimingModes enum
+
+        /// <summary>
+        /// Timing Modes
+        /// Switched: Standard on/off mode using the baseclass enabled property
+        /// Triggered: When the Trigger() method is called the force is active 
+        /// for a specified Impulse Length
+        /// Curve: Still to be defined. The basic idea is having a Trigger 
+        /// combined with a curve for the strength
+        /// </summary>
+        public enum TimingModes
+        {
+            Switched,
+            Triggered,
+            Curve
+        }
+
+        #endregion
+
+        /// <summary>
+        /// Curve to be used for Decay in Curve mode
+        /// </summary>
+        public Curve DecayCurve;
+
+        /// <summary>
+        /// The Forcetype of the instance
+        /// </summary>
+        public ForceTypes ForceType;
+
+        /// <summary>
+        /// Provided for reuse to provide Variation functionality in 
+        /// inheriting classes
+        /// </summary>
+        protected Random Randomize;
+
+        /// <summary>
+        /// Curve used by Curve Mode as an animated multiplier for the force 
+        /// strength.
+        /// Only positions between 0 and 1 are considered as that range is 
+        /// stretched to have ImpulseLength.
+        /// </summary>
+        public Curve StrengthCurve;
+
+        /// <summary>
+        /// Constructor
+        /// </summary>
+        public AbstractForceController()
+            : base(ControllerType.AbstractForceController)
+        {
+            Enabled = true;
+
+            Strength = 1.0f;
+            Position = new Vector2(0, 0);
+            MaximumSpeed = 100.0f;
+            TimingMode = TimingModes.Switched;
+            ImpulseTime = 0.0f;
+            ImpulseLength = 1.0f;
+            Triggered = false;
+            StrengthCurve = new Curve();
+            Variation = 0.0f;
+            Randomize = new Random(1234);
+            DecayMode = DecayModes.None;
+            DecayCurve = new Curve();
+            DecayStart = 0.0f;
+            DecayEnd = 0.0f;
+
+            StrengthCurve.Keys.Add(new CurveKey(0, 5));
+            StrengthCurve.Keys.Add(new CurveKey(0.1f, 5));
+            StrengthCurve.Keys.Add(new CurveKey(0.2f, -4));
+            StrengthCurve.Keys.Add(new CurveKey(1f, 0));
+        }
+
+        /// <summary>
+        /// Overloaded Contstructor with supplying Timing Mode
+        /// </summary>
+        /// <param name="mode"></param>
+        public AbstractForceController(TimingModes mode)
+            : base(ControllerType.AbstractForceController)
+        {
+            TimingMode = mode;
+            switch (mode)
+            {
+                case TimingModes.Switched:
+                    Enabled = true;
+                    break;
+                case TimingModes.Triggered:
+                    Enabled = false;
+                    break;
+                case TimingModes.Curve:
+                    Enabled = false;
+                    break;
+            }
+        }
+
+        /// <summary>
+        /// Global Strength of the force to be applied
+        /// </summary>
+        public float Strength { get; set; }
+
+        /// <summary>
+        /// Position of the Force. Can be ignored (left at (0,0) for forces
+        /// that are not position-dependent
+        /// </summary>
+        public Vector2 Position { get; set; }
+
+        /// <summary>
+        /// Maximum speed of the bodies. Bodies that are travelling faster are
+        /// supposed to be ignored
+        /// </summary>
+        public float MaximumSpeed { get; set; }
+
+        /// <summary>
+        /// Maximum Force to be applied. As opposed to Maximum Speed this is 
+        /// independent of the velocity of
+        /// the affected body
+        /// </summary>
+        public float MaximumForce { get; set; }
+
+        /// <summary>
+        /// Timing Mode of the force instance
+        /// </summary>
+        public TimingModes TimingMode { get; set; }
+
+        /// <summary>
+        /// Time of the current impulse. Incremented in update till 
+        /// ImpulseLength is reached
+        /// </summary>
+        public float ImpulseTime { get; private set; }
+
+        /// <summary>
+        /// Length of a triggered impulse. Used in both Triggered and Curve Mode
+        /// </summary>
+        public float ImpulseLength { get; set; }
+
+        /// <summary>
+        /// Indicating if we are currently during an Impulse 
+        /// (Triggered and Curve Mode)
+        /// </summary>
+        public bool Triggered { get; private set; }
+
+        /// <summary>
+        /// Variation of the force applied to each body affected
+        /// !! Must be used in inheriting classes properly !!
+        /// </summary>
+        public float Variation { get; set; }
+
+        /// <summary>
+        /// See DecayModes
+        /// </summary>
+        public DecayModes DecayMode { get; set; }
+
+        /// <summary>
+        /// Start of the distance based Decay. To set a non decaying area
+        /// </summary>
+        public float DecayStart { get; set; }
+
+        /// <summary>
+        /// Maximum distance a force should be applied
+        /// </summary>
+        public float DecayEnd { get; set; }
+
+        /// <summary>
+        /// Calculate the Decay for a given body. Meant to ease force 
+        /// development and stick to the DRY principle and provide unified and 
+        /// predictable decay math.
+        /// </summary>
+        /// <param name="body">The body to calculate decay for</param>
+        /// <returns>A multiplier to multiply the force with to add decay 
+        /// support in inheriting classes</returns>
+        protected float GetDecayMultiplier(Body body)
+        {
+            //TODO: Consider ForceType in distance calculation!
+            float distance = (body.Position - Position).Length();
+            switch (DecayMode)
+            {
+                case DecayModes.None:
+                    {
+                        return 1.0f;
+                    }
+                case DecayModes.Step:
+                    {
+                        if (distance < DecayEnd)
+                            return 1.0f;
+                        else
+                            return 0.0f;
+                    }
+                case DecayModes.Linear:
+                    {
+                        if (distance < DecayStart)
+                            return 1.0f;
+                        if (distance > DecayEnd)
+                            return 0.0f;
+                        return (DecayEnd - DecayStart / distance - DecayStart);
+                    }
+                case DecayModes.InverseSquare:
+                    {
+                        if (distance < DecayStart)
+                            return 1.0f;
+                        else
+                            return 1.0f / ((distance - DecayStart) * (distance - DecayStart));
+                    }
+                case DecayModes.Curve:
+                    {
+                        if (distance < DecayStart)
+                            return 1.0f;
+                        else
+                            return DecayCurve.Evaluate(distance - DecayStart);
+                    }
+                default:
+                    return 1.0f;
+            }
+        }
+
+        /// <summary>
+        /// Triggers the trigger modes (Trigger and Curve)
+        /// </summary>
+        public void Trigger()
+        {
+            Triggered = true;
+            ImpulseTime = 0;
+        }
+
+        /// <summary>
+        /// Inherited from Controller
+        /// Depending on the TimingMode perform timing logic and call ApplyForce()
+        /// </summary>
+        /// <param name="dt"></param>
+        public override void Update(float dt)
+        {
+            switch (TimingMode)
+            {
+                case TimingModes.Switched:
+                    {
+                        if (Enabled)
+                        {
+                            ApplyForce(dt, Strength);
+                        }
+                        break;
+                    }
+                case TimingModes.Triggered:
+                    {
+                        if (Enabled && Triggered)
+                        {
+                            if (ImpulseTime < ImpulseLength)
+                            {
+                                ApplyForce(dt, Strength);
+                                ImpulseTime += dt;
+                            }
+                            else
+                            {
+                                Triggered = false;
+                            }
+                        }
+                        break;
+                    }
+                case TimingModes.Curve:
+                    {
+                        if (Enabled && Triggered)
+                        {
+                            if (ImpulseTime < ImpulseLength)
+                            {
+                                ApplyForce(dt, Strength * StrengthCurve.Evaluate(ImpulseTime));
+                                ImpulseTime += dt;
+                            }
+                            else
+                            {
+                                Triggered = false;
+                            }
+                        }
+                        break;
+                    }
+            }
+        }
+
+        /// <summary>
+        /// Apply the force supplying strength (wich is modified in Update() 
+        /// according to the TimingMode
+        /// </summary>
+        /// <param name="dt"></param>
+        /// <param name="strength">The strength</param>
+        public abstract void ApplyForce(float dt, float strength);
+    }
+}

axios/Controllers/BuoyancyController.cs

@@ -0,0 +1,135 @@
+using System.Collections.Generic;
+using FarseerPhysics.Collision;
+using FarseerPhysics.Collision.Shapes;
+using FarseerPhysics.Dynamics;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Controllers
+{
+    public sealed class BuoyancyController : Controller
+    {
+        /// <summary>
+        /// Controls the rotational drag that the fluid exerts on the bodies within it. Use higher values will simulate thick fluid, like honey, lower values to
+        /// simulate water-like fluids. 
+        /// </summary>
+        public float AngularDragCoefficient;
+
+        /// <summary>
+        /// Density of the fluid. Higher values will make things more buoyant, lower values will cause things to sink.
+        /// </summary>
+        public float Density;
+
+        /// <summary>
+        /// Controls the linear drag that the fluid exerts on the bodies within it.  Use higher values will simulate thick fluid, like honey, lower values to
+        /// simulate water-like fluids.
+        /// </summary>
+        public float LinearDragCoefficient;
+
+        /// <summary>
+        /// Acts like waterflow. Defaults to 0,0.
+        /// </summary>
+        public Vector2 Velocity;
+
+        private AABB _container;
+
+        private Vector2 _gravity;
+        private Vector2 _normal;
+        private float _offset;
+        private Dictionary<int, Body> _uniqueBodies = new Dictionary<int, Body>();
+
+        /// <summary>
+        /// Initializes a new instance of the <see cref="BuoyancyController"/> class.
+        /// </summary>
+        /// <param name="container">Only bodies inside this AABB will be influenced by the controller</param>
+        /// <param name="density">Density of the fluid</param>
+        /// <param name="linearDragCoefficient">Linear drag coefficient of the fluid</param>
+        /// <param name="rotationalDragCoefficient">Rotational drag coefficient of the fluid</param>
+        /// <param name="gravity">The direction gravity acts. Buoyancy force will act in opposite direction of gravity.</param>
+        public BuoyancyController(AABB container, float density, float linearDragCoefficient,
+                                  float rotationalDragCoefficient, Vector2 gravity)
+            : base(ControllerType.BuoyancyController)
+        {
+            Container = container;
+            _normal = new Vector2(0, 1);
+            Density = density;
+            LinearDragCoefficient = linearDragCoefficient;
+            AngularDragCoefficient = rotationalDragCoefficient;
+            _gravity = gravity;
+        }
+
+        public AABB Container
+        {
+            get { return _container; }
+            set
+            {
+                _container = value;
+                _offset = _container.UpperBound.Y;
+            }
+        }
+
+        public override void Update(float dt)
+        {
+            _uniqueBodies.Clear();
+            World.QueryAABB(fixture =>
+                                {
+                                    if (fixture.Body.IsStatic || !fixture.Body.Awake)
+                                        return true;
+
+                                    if (!_uniqueBodies.ContainsKey(fixture.Body.BodyId))
+                                        _uniqueBodies.Add(fixture.Body.BodyId, fixture.Body);
+
+                                    return true;
+                                }, ref _container);
+
+            foreach (KeyValuePair<int, Body> kv in _uniqueBodies)
+            {
+                Body body = kv.Value;
+
+                Vector2 areac = Vector2.Zero;
+                Vector2 massc = Vector2.Zero;
+                float area = 0;
+                float mass = 0;
+
+                for (int j = 0; j < body.FixtureList.Count; j++)
+                {
+                    Fixture fixture = body.FixtureList[j];
+
+                    if (fixture.Shape.ShapeType != ShapeType.Polygon && fixture.Shape.ShapeType != ShapeType.Circle)
+                        continue;
+
+                    Shape shape = fixture.Shape;
+
+                    Vector2 sc;
+                    float sarea = shape.ComputeSubmergedArea(_normal, _offset, body.Xf, out sc);
+                    area += sarea;
+                    areac.X += sarea * sc.X;
+                    areac.Y += sarea * sc.Y;
+
+                    mass += sarea * shape.Density;
+                    massc.X += sarea * sc.X * shape.Density;
+                    massc.Y += sarea * sc.Y * shape.Density;
+                }
+
+                areac.X /= area;
+                areac.Y /= area;
+                massc.X /= mass;
+                massc.Y /= mass;
+
+                if (area < Settings.Epsilon)
+                    continue;
+
+                //Buoyancy
+                Vector2 buoyancyForce = -Density * area * _gravity;
+                body.ApplyForce(buoyancyForce, massc);
+
+                //Linear drag
+                Vector2 dragForce = body.GetLinearVelocityFromWorldPoint(areac) - Velocity;
+                dragForce *= -LinearDragCoefficient * area;
+                body.ApplyForce(dragForce, areac);
+
+                //Angular drag
+                body.ApplyTorque(-body.Inertia / body.Mass * area * body.AngularVelocity * AngularDragCoefficient);
+            }
+        }
+    }
+}

axios/Controllers/Controller.cs

@@ -0,0 +1,71 @@
+using System;
+using FarseerPhysics.Dynamics;
+
+namespace FarseerPhysics.Controllers
+{
+    [Flags]
+    public enum ControllerType
+    {
+        GravityController = (1 << 0),
+        VelocityLimitController = (1 << 1),
+        AbstractForceController = (1 << 2),
+        BuoyancyController = (1 << 3),
+    }
+
+    public struct ControllerFilter
+    {
+        public ControllerType ControllerFlags;
+
+        /// <summary>
+        /// Ignores the controller. The controller has no effect on this body.
+        /// </summary>
+        /// <param name="controller">The controller type.</param>
+        public void IgnoreController(ControllerType controller)
+        {
+            ControllerFlags |= controller;
+        }
+
+        /// <summary>
+        /// Restore the controller. The controller affects this body.
+        /// </summary>
+        /// <param name="controller">The controller type.</param>
+        public void RestoreController(ControllerType controller)
+        {
+            ControllerFlags &= ~controller;
+        }
+
+        /// <summary>
+        /// Determines whether this body ignores the the specified controller.
+        /// </summary>
+        /// <param name="controller">The controller type.</param>
+        /// <returns>
+        /// 	<c>true</c> if the body has the specified flag; otherwise, <c>false</c>.
+        /// </returns>
+        public bool IsControllerIgnored(ControllerType controller)
+        {
+            return (ControllerFlags & controller) == controller;
+        }
+    }
+
+    public abstract class Controller : FilterData
+    {
+        public bool Enabled;
+        public World World;
+        private ControllerType _type;
+
+        public Controller(ControllerType controllerType)
+        {
+            _type = controllerType;
+        }
+
+        public override bool IsActiveOn(Body body)
+        {
+            if (body.ControllerFilter.IsControllerIgnored(_type))
+                return false;
+
+            return base.IsActiveOn(body);
+        }
+
+        public abstract void Update(float dt);
+    }
+}

axios/Controllers/GravityController.cs

@@ -0,0 +1,117 @@
+using System;
+using System.Collections.Generic;
+using FarseerPhysics.Dynamics;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Controllers
+{
+    public enum GravityType
+    {
+        Linear,
+        DistanceSquared
+    }
+
+    public class GravityController : Controller
+    {
+        public List<Body> Bodies = new List<Body>();
+        public List<Vector2> Points = new List<Vector2>();
+
+        public GravityController(float strength)
+            : base(ControllerType.GravityController)
+        {
+            Strength = strength;
+            MaxRadius = float.MaxValue;
+        }
+
+        public GravityController(float strength, float maxRadius, float minRadius)
+            : base(ControllerType.GravityController)
+        {
+            MinRadius = minRadius;
+            MaxRadius = maxRadius;
+            Strength = strength;
+        }
+
+        public float MinRadius { get; set; }
+        public float MaxRadius { get; set; }
+        public float Strength { get; set; }
+        public GravityType GravityType { get; set; }
+
+        public override void Update(float dt)
+        {
+            Vector2 f = Vector2.Zero;
+
+            foreach (Body body1 in World.BodyList)
+            {
+                if (!IsActiveOn(body1))
+                    continue;
+
+                foreach (Body body2 in Bodies)
+                {
+                    if (body1 == body2 || (body1.IsStatic && body2.IsStatic) || !body2.Enabled)
+                        continue;
+
+                    Vector2 d = body2.WorldCenter - body1.WorldCenter;
+                    float r2 = d.LengthSquared();
+
+                    if (r2 < Settings.Epsilon)
+                        continue;
+
+                    float r = d.Length();
+
+                    if (r >= MaxRadius || r <= MinRadius)
+                        continue;
+
+                    switch (GravityType)
+                    {
+                        case GravityType.DistanceSquared:
+                            f = Strength / r2 / (float)Math.Sqrt(r2) * body1.Mass * body2.Mass * d;
+                            break;
+                        case GravityType.Linear:
+                            f = Strength / r2 * body1.Mass * body2.Mass * d;
+                            break;
+                    }
+
+                    body1.ApplyForce(ref f);
+                    Vector2.Negate(ref f, out f);
+                    body2.ApplyForce(ref f);
+                }
+
+                foreach (Vector2 point in Points)
+                {
+                    Vector2 d = point - body1.Position;
+                    float r2 = d.LengthSquared();
+
+                    if (r2 < Settings.Epsilon)
+                        continue;
+
+                    float r = d.Length();
+
+                    if (r >= MaxRadius || r <= MinRadius)
+                        continue;
+
+                    switch (GravityType)
+                    {
+                        case GravityType.DistanceSquared:
+                            f = Strength / r2 / (float)Math.Sqrt(r2) * body1.Mass * d;
+                            break;
+                        case GravityType.Linear:
+                            f = Strength / r2 * body1.Mass * d;
+                            break;
+                    }
+
+                    body1.ApplyForce(ref f);
+                }
+            }
+        }
+
+        public void AddBody(Body body)
+        {
+            Bodies.Add(body);
+        }
+
+        public void AddPoint(Vector2 point)
+        {
+            Points.Add(point);
+        }
+    }
+}

axios/Controllers/SimpleWindForce.cs

@@ -0,0 +1,75 @@
+using FarseerPhysics.Dynamics;
+using Microsoft.Xna.Framework;
+
+namespace FarseerPhysics.Controllers
+{
+    /// <summary>
+    /// Reference implementation for forces based on AbstractForceController
+    /// It supports all features provided by the base class and illustrates proper
+    /// usage as an easy to understand example.
+    /// As a side-effect it is a nice and easy to use wind force for your projects
+    /// </summary>
+    public class SimpleWindForce : AbstractForceController
+    {
+        /// <summary>
+        /// Direction of the windforce
+        /// </summary>
+        public Vector2 Direction { get; set; }
+
+        /// <summary>
+        /// The amount of Direction randomization. Allowed range is 0-1.
+        /// </summary>
+        public float Divergence { get; set; }
+
+        /// <summary>
+        /// Ignore the position and apply the force. If off only in the "front" (relative to position and direction)
+        /// will be affected
+        /// </summary>
+        public bool IgnorePosition { get; set; }
+
+
+        public override void ApplyForce(float dt, float strength)
+        {
+            foreach (Body body in World.BodyList)
+            {
+                //TODO: Consider Force Type
+                float decayMultiplier = GetDecayMultiplier(body);
+
+                if (decayMultiplier != 0)
+                {
+                    Vector2 forceVector;
+
+                    if (ForceType == ForceTypes.Point)
+                    {
+                        forceVector = body.Position - Position;
+                    }
+                    else
+                    {
+                        Direction.Normalize();
+
+                        forceVector = Direction;
+
+                        if (forceVector.Length() == 0)
+                            forceVector = new Vector2(0, 1);
+                    }
+
+                    //TODO: Consider Divergence:
+                    //forceVector = Vector2.Transform(forceVector, Matrix.CreateRotationZ((MathHelper.Pi - MathHelper.Pi/2) * (float)Randomize.NextDouble()));
+
+                    // Calculate random Variation
+                    if (Variation != 0)
+                    {
+                        float strengthVariation = (float)Randomize.NextDouble() * MathHelper.Clamp(Variation, 0, 1);
+                        forceVector.Normalize();
+                        body.ApplyForce(forceVector * strength * decayMultiplier * strengthVariation);
+                    }
+                    else
+                    {
+                        forceVector.Normalize();
+                        body.ApplyForce(forceVector * strength * decayMultiplier);
+                    }
+                }
+            }
+        }
+    }
+}

axios/Controllers/VelocityLimitController.cs

@@ -0,0 +1,129 @@
+using System;
+using System.Collections.Generic;
+using FarseerPhysics.Dynamics;
+
+namespace FarseerPhysics.Controllers
+{
+    /// <summary>
+    /// Put a limit on the linear (translation - the movespeed) and angular (rotation) velocity
+    /// of bodies added to this controller.
+    /// </summary>
+    public class VelocityLimitController : Controller
+    {
+        public bool LimitAngularVelocity = true;
+        public bool LimitLinearVelocity = true;
+        private List<Body> _bodies = new List<Body>();
+        private float _maxAngularSqared;
+        private float _maxAngularVelocity;
+        private float _maxLinearSqared;
+        private float _maxLinearVelocity;
+
+        /// <summary>
+        /// Initializes a new instance of the <see cref="VelocityLimitController"/> class.
+        /// Sets the max linear velocity to Settings.MaxTranslation
+        /// Sets the max angular velocity to Settings.MaxRotation
+        /// </summary>
+        public VelocityLimitController()
+            : base(ControllerType.VelocityLimitController)
+        {
+            MaxLinearVelocity = Settings.MaxTranslation;
+            MaxAngularVelocity = Settings.MaxRotation;
+        }
+
+        /// <summary>
+        /// Initializes a new instance of the <see cref="VelocityLimitController"/> class.
+        /// Pass in 0 or float.MaxValue to disable the limit.
+        /// maxAngularVelocity = 0 will disable the angular velocity limit.
+        /// </summary>
+        /// <param name="maxLinearVelocity">The max linear velocity.</param>
+        /// <param name="maxAngularVelocity">The max angular velocity.</param>
+        public VelocityLimitController(float maxLinearVelocity, float maxAngularVelocity)
+            : base(ControllerType.VelocityLimitController)
+        {
+            if (maxLinearVelocity == 0 || maxLinearVelocity == float.MaxValue)
+                LimitLinearVelocity = false;
+
+            if (maxAngularVelocity == 0 || maxAngularVelocity == float.MaxValue)
+                LimitAngularVelocity = false;
+
+            MaxLinearVelocity = maxLinearVelocity;
+            MaxAngularVelocity = maxAngularVelocity;
+        }
+
+        /// <summary>
+        /// Gets or sets the max angular velocity.
+        /// </summary>
+        /// <value>The max angular velocity.</value>
+        public float MaxAngularVelocity
+        {
+            get { return _maxAngularVelocity; }
+            set
+            {
+                _maxAngularVelocity = value;
+                _maxAngularSqared = _maxAngularVelocity * _maxAngularVelocity;
+            }
+        }
+
+        /// <summary>
+        /// Gets or sets the max linear velocity.
+        /// </summary>
+        /// <value>The max linear velocity.</value>
+        public float MaxLinearVelocity
+        {
+            get { return _maxLinearVelocity; }
+            set
+            {
+                _maxLinearVelocity = value;
+                _maxLinearSqared = _maxLinearVelocity * _maxLinearVelocity;
+            }
+        }
+
+        public override void Update(float dt)
+        {
+            foreach (Body body in _bodies)
+            {
+                if (!IsActiveOn(body))
+                    continue;
+
+                if (LimitLinearVelocity)
+                {
+                    //Translation
+                    // Check for large velocities.
+                    float translationX = dt * body.LinearVelocityInternal.X;
+                    float translationY = dt * body.LinearVelocityInternal.Y;
+                    float result = translationX * translationX + translationY * translationY;
+
+                    if (result > dt * _maxLinearSqared)
+                    {
+                        float sq = (float)Math.Sqrt(result);
+
+                        float ratio = _maxLinearVelocity / sq;
+                        body.LinearVelocityInternal.X *= ratio;
+                        body.LinearVelocityInternal.Y *= ratio;
+                    }
+                }
+
+                if (LimitAngularVelocity)
+                {
+                    //Rotation
+                    float rotation = dt * body.AngularVelocityInternal;
+                    if (rotation * rotation > _maxAngularSqared)
+                    {
+                        float ratio = _maxAngularVelocity / Math.Abs(rotation);
+                        body.AngularVelocityInternal *= ratio;
+                    }
+                }
+            }
+        }
+
+        public void AddBody(Body body)
+        {
+            _bodies.Add(body);
+        }
+
+        public void RemoveBody(Body body)
+        {
+            _bodies.Remove(body);
+        }
+    }
+}