diff --git a/.gitignore b/.gitignore index 37a1e68..b51caed 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ /.vs/ae-physics/v14/.suo /ae-physics.csproj.user +.vs \ No newline at end of file diff --git a/.gitmodules b/.gitmodules index 8ff6341..0d65332 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,6 @@ [submodule "lib/FNA"] path = lib/FNA url = ssh://git@srchub.org/fna-workbench.git +[submodule "lib/ae-gamestatemanagement"] + path = lib/ae-gamestatemanagement + url = git://srchub.org/ae-gamestatemanagement.git diff --git a/DebugView.cs b/DebugView.cs new file mode 100644 index 0000000..86da4ce --- /dev/null +++ b/DebugView.cs @@ -0,0 +1,185 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using FarseerPhysics.Common; +using FarseerPhysics.Dynamics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics +{ + [Flags] + public enum DebugViewFlags + { + /// + /// Draw shapes. + /// + Shape = (1 << 0), + + /// + /// Draw joint connections. + /// + Joint = (1 << 1), + + /// + /// Draw axis aligned bounding boxes. + /// + AABB = (1 << 2), + + /// + /// Draw broad-phase pairs. + /// + Pair = (1 << 3), + + /// + /// Draw center of mass frame. + /// + CenterOfMass = (1 << 4), + + /// + /// Draw useful debug data such as timings and number of bodies, joints, contacts and more. + /// + DebugPanel = (1 << 5), + + /// + /// Draw contact points between colliding bodies. + /// + ContactPoints = (1 << 6), + + /// + /// Draw contact normals. Need ContactPoints to be enabled first. + /// + ContactNormals = (1 << 7), + + /// + /// Draws the vertices of polygons. + /// + PolygonPoints = (1 << 8), + + /// + /// Draws the performance graph. + /// + PerformanceGraph = (1 << 9), + + /// + /// Draws controllers. + /// + Controllers = (1 << 10) + } + + /// Implement and register this class with a World to provide debug drawing of physics + /// entities in your game. + public abstract class DebugView + { + protected DebugView(World world) + { + World = world; + } + + protected World World { get; private set; } + + /// + /// Gets or sets the debug view flags. + /// + /// The flags. + public DebugViewFlags Flags { get; set; } + + /// + /// Append flags to the current flags. + /// + /// The flags. + public void AppendFlags(DebugViewFlags flags) + { + Flags |= flags; + } + + /// + /// Remove flags from the current flags. + /// + /// The flags. + public void RemoveFlags(DebugViewFlags flags) + { + Flags &= ~flags; + } + + /// + /// Draw a closed polygon provided in CCW order. + /// + /// The vertices. + /// The vertex count. + /// The red value. + /// The blue value. + /// The green value. + public abstract void DrawPolygon(Vector2[] vertices, int count, float red, float blue, float green); + + /// + /// Draw a solid closed polygon provided in CCW order. + /// + /// The vertices. + /// The vertex count. + /// The red value. + /// The blue value. + /// The green value. + public abstract void DrawSolidPolygon(Vector2[] vertices, int count, float red, float blue, float green); + + /// + /// Draw a circle. + /// + /// The center. + /// The radius. + /// The red value. + /// The blue value. + /// The green value. + public abstract void DrawCircle(Vector2 center, float radius, float red, float blue, float green); + + /// + /// Draw a solid circle. + /// + /// The center. + /// The radius. + /// The axis. + /// The red value. + /// The blue value. + /// The green value. + public abstract void DrawSolidCircle(Vector2 center, float radius, Vector2 axis, float red, float blue, + float green); + + /// + /// Draw a line segment. + /// + /// The start. + /// The end. + /// The red value. + /// The blue value. + /// The green value. + public abstract void DrawSegment(Vector2 start, Vector2 end, float red, float blue, float green); + + /// + /// Draw a transform. Choose your own length scale. + /// + /// The transform. + public abstract void DrawTransform(ref Transform transform); + } +} \ No newline at end of file diff --git a/DebugViewXNA.cs b/DebugViewXNA.cs new file mode 100644 index 0000000..81761b5 --- /dev/null +++ b/DebugViewXNA.cs @@ -0,0 +1,875 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Linq; +using FarseerPhysics.Collision; +using FarseerPhysics.Collision.Shapes; +using FarseerPhysics.Common; +using FarseerPhysics.Controllers; +using FarseerPhysics.Dynamics; +using FarseerPhysics.Dynamics.Contacts; +using FarseerPhysics.Dynamics.Joints; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Content; +using Microsoft.Xna.Framework.Graphics; + +namespace FarseerPhysics.DebugViews +{ + /// + /// A debug view that works in XNA. + /// A debug view shows you what happens inside the physics engine. You can view + /// bodies, joints, fixtures and more. + /// + public class DebugViewXNA : DebugView, IDisposable + { + //Drawing + private PrimitiveBatch _primitiveBatch; + private SpriteBatch _batch; + private SpriteFont _font; + private GraphicsDevice _device; + private Vector2[] _tempVertices = new Vector2[Settings.MaxPolygonVertices]; + private List _stringData; + + private Matrix _localProjection; + private Matrix _localView; + + //Shapes + public Color DefaultShapeColor = new Color(0.9f, 0.7f, 0.7f); + public Color InactiveShapeColor = new Color(0.5f, 0.5f, 0.3f); + public Color KinematicShapeColor = new Color(0.5f, 0.5f, 0.9f); + public Color SleepingShapeColor = new Color(0.6f, 0.6f, 0.6f); + public Color StaticShapeColor = new Color(0.5f, 0.9f, 0.5f); + public Color TextColor = Color.White; + + //Contacts + private int _pointCount; + private const int MaxContactPoints = 2048; + private ContactPoint[] _points = new ContactPoint[MaxContactPoints]; + + //Debug panel +#if XBOX + public Vector2 DebugPanelPosition = new Vector2(55, 100); +#else + public Vector2 DebugPanelPosition = new Vector2(40, 100); +#endif + private int _max; + private int _avg; + private int _min; + + //Performance graph + public bool AdaptiveLimits = true; + public int ValuesToGraph = 500; + public int MinimumValue; + public int MaximumValue = 1000; + private List _graphValues = new List(); + +#if XBOX + public Rectangle PerformancePanelBounds = new Rectangle(265, 100, 200, 100); +#else + public Rectangle PerformancePanelBounds = new Rectangle(250, 100, 200, 100); +#endif + private Vector2[] _background = new Vector2[4]; + public bool Enabled = true; + +#if XBOX || WINDOWS_PHONE + public const int CircleSegments = 16; +#else + public const int CircleSegments = 32; +#endif + + public DebugViewXNA(World world) + : base(world) + { + world.ContactManager.PreSolve += PreSolve; + + //Default flags + AppendFlags(DebugViewFlags.Shape); + AppendFlags(DebugViewFlags.Controllers); + AppendFlags(DebugViewFlags.Joint); + } + + public void BeginCustomDraw(ref Matrix projection, ref Matrix view) + { + _primitiveBatch.Begin(ref projection, ref view); + } + + public void EndCustomDraw() + { + _primitiveBatch.End(); + } + + #region IDisposable Members + + public void Dispose() + { + World.ContactManager.PreSolve -= PreSolve; + } + + #endregion + + private void PreSolve(Contact contact, ref Manifold oldManifold) + { + if ((Flags & DebugViewFlags.ContactPoints) == DebugViewFlags.ContactPoints) + { + Manifold manifold = contact.Manifold; + + if (manifold.PointCount == 0) + { + return; + } + + Fixture fixtureA = contact.FixtureA; + + FixedArray2 state1, state2; + Collision.Collision.GetPointStates(out state1, out state2, ref oldManifold, ref manifold); + + FixedArray2 points; + Vector2 normal; + contact.GetWorldManifold(out normal, out points); + + for (int i = 0; i < manifold.PointCount && _pointCount < MaxContactPoints; ++i) + { + if (fixtureA == null) + { + _points[i] = new ContactPoint(); + } + ContactPoint cp = _points[_pointCount]; + cp.Position = points[i]; + cp.Normal = normal; + cp.State = state2[i]; + _points[_pointCount] = cp; + ++_pointCount; + } + } + } + + /// + /// Call this to draw shapes and other debug draw data. + /// + private void DrawDebugData() + { + if ((Flags & DebugViewFlags.Shape) == DebugViewFlags.Shape) + { + foreach (Body b in World.BodyList) + { + Transform xf; + b.GetTransform(out xf); + foreach (Fixture f in b.FixtureList) + { + if (b.Enabled == false) + { + DrawShape(f, xf, InactiveShapeColor); + } + else if (b.BodyType == BodyType.Static) + { + DrawShape(f, xf, StaticShapeColor); + } + else if (b.BodyType == BodyType.Kinematic) + { + DrawShape(f, xf, KinematicShapeColor); + } + else if (b.Awake == false) + { + DrawShape(f, xf, SleepingShapeColor); + } + else + { + DrawShape(f, xf, DefaultShapeColor); + } + } + } + } + if ((Flags & DebugViewFlags.ContactPoints) == DebugViewFlags.ContactPoints) + { + const float axisScale = 0.3f; + + for (int i = 0; i < _pointCount; ++i) + { + ContactPoint point = _points[i]; + + if (point.State == PointState.Add) + { + // Add + DrawPoint(point.Position, 0.1f, new Color(0.3f, 0.95f, 0.3f)); + } + else if (point.State == PointState.Persist) + { + // Persist + DrawPoint(point.Position, 0.1f, new Color(0.3f, 0.3f, 0.95f)); + } + + if ((Flags & DebugViewFlags.ContactNormals) == DebugViewFlags.ContactNormals) + { + Vector2 p1 = point.Position; + Vector2 p2 = p1 + axisScale * point.Normal; + DrawSegment(p1, p2, new Color(0.4f, 0.9f, 0.4f)); + } + } + _pointCount = 0; + } + if ((Flags & DebugViewFlags.PolygonPoints) == DebugViewFlags.PolygonPoints) + { + foreach (Body body in World.BodyList) + { + foreach (Fixture f in body.FixtureList) + { + PolygonShape polygon = f.Shape as PolygonShape; + if (polygon != null) + { + Transform xf; + body.GetTransform(out xf); + + for (int i = 0; i < polygon.Vertices.Count; i++) + { + Vector2 tmp = MathUtils.Multiply(ref xf, polygon.Vertices[i]); + DrawPoint(tmp, 0.1f, Color.Red); + } + } + } + } + } + if ((Flags & DebugViewFlags.Joint) == DebugViewFlags.Joint) + { + foreach (Joint j in World.JointList) + { + DrawJoint(j); + } + } + if ((Flags & DebugViewFlags.Pair) == DebugViewFlags.Pair) + { + Color color = new Color(0.3f, 0.9f, 0.9f); + for (int i = 0; i < World.ContactManager.ContactList.Count; i++) + { + Contact c = World.ContactManager.ContactList[i]; + Fixture fixtureA = c.FixtureA; + Fixture fixtureB = c.FixtureB; + + AABB aabbA; + fixtureA.GetAABB(out aabbA, 0); + AABB aabbB; + fixtureB.GetAABB(out aabbB, 0); + + Vector2 cA = aabbA.Center; + Vector2 cB = aabbB.Center; + + DrawSegment(cA, cB, color); + } + } + if ((Flags & DebugViewFlags.AABB) == DebugViewFlags.AABB) + { + Color color = new Color(0.9f, 0.3f, 0.9f); + IBroadPhase bp = World.ContactManager.BroadPhase; + + foreach (Body b in World.BodyList) + { + if (b.Enabled == false) + { + continue; + } + + foreach (Fixture f in b.FixtureList) + { + for (int t = 0; t < f.ProxyCount; ++t) + { + FixtureProxy proxy = f.Proxies[t]; + AABB aabb; + bp.GetFatAABB(proxy.ProxyId, out aabb); + + DrawAABB(ref aabb, color); + } + } + } + } + if ((Flags & DebugViewFlags.CenterOfMass) == DebugViewFlags.CenterOfMass) + { + foreach (Body b in World.BodyList) + { + Transform xf; + b.GetTransform(out xf); + xf.Position = b.WorldCenter; + DrawTransform(ref xf); + } + } + if ((Flags & DebugViewFlags.Controllers) == DebugViewFlags.Controllers) + { + for (int i = 0; i < World.ControllerList.Count; i++) + { + Controller controller = World.ControllerList[i]; + + BuoyancyController buoyancy = controller as BuoyancyController; + if (buoyancy != null) + { + AABB container = buoyancy.Container; + DrawAABB(ref container, Color.LightBlue); + } + } + } + if ((Flags & DebugViewFlags.DebugPanel) == DebugViewFlags.DebugPanel) + { + DrawDebugPanel(); + } + } + + private void DrawPerformanceGraph() + { + _graphValues.Add(World.UpdateTime); + + if (_graphValues.Count > ValuesToGraph + 1) + _graphValues.RemoveAt(0); + + float x = PerformancePanelBounds.X; + float deltaX = PerformancePanelBounds.Width / (float)ValuesToGraph; + float yScale = PerformancePanelBounds.Bottom - (float)PerformancePanelBounds.Top; + + // we must have at least 2 values to start rendering + if (_graphValues.Count > 2) + { + _max = (int)_graphValues.Max(); + _avg = (int)_graphValues.Average(); + _min = (int)_graphValues.Min(); + + if (AdaptiveLimits) + { + MaximumValue = _max; + MinimumValue = 0; + } + + // start at last value (newest value added) + // continue until no values are left + for (int i = _graphValues.Count - 1; i > 0; i--) + { + float y1 = PerformancePanelBounds.Bottom - + ((_graphValues[i] / (MaximumValue - MinimumValue)) * yScale); + float y2 = PerformancePanelBounds.Bottom - + ((_graphValues[i - 1] / (MaximumValue - MinimumValue)) * yScale); + + Vector2 x1 = + new Vector2(MathHelper.Clamp(x, PerformancePanelBounds.Left, PerformancePanelBounds.Right), + MathHelper.Clamp(y1, PerformancePanelBounds.Top, PerformancePanelBounds.Bottom)); + + Vector2 x2 = + new Vector2( + MathHelper.Clamp(x + deltaX, PerformancePanelBounds.Left, PerformancePanelBounds.Right), + MathHelper.Clamp(y2, PerformancePanelBounds.Top, PerformancePanelBounds.Bottom)); + + DrawSegment(x1, x2, Color.LightGreen); + + x += deltaX; + } + } + + DrawString(PerformancePanelBounds.Right + 10, PerformancePanelBounds.Top, "Max: " + _max); + DrawString(PerformancePanelBounds.Right + 10, PerformancePanelBounds.Center.Y - 7, "Avg: " + _avg); + DrawString(PerformancePanelBounds.Right + 10, PerformancePanelBounds.Bottom - 15, "Min: " + _min); + + //Draw background. + _background[0] = new Vector2(PerformancePanelBounds.X, PerformancePanelBounds.Y); + _background[1] = new Vector2(PerformancePanelBounds.X, + PerformancePanelBounds.Y + PerformancePanelBounds.Height); + _background[2] = new Vector2(PerformancePanelBounds.X + PerformancePanelBounds.Width, + PerformancePanelBounds.Y + PerformancePanelBounds.Height); + _background[3] = new Vector2(PerformancePanelBounds.X + PerformancePanelBounds.Width, + PerformancePanelBounds.Y); + + DrawSolidPolygon(_background, 4, Color.DarkGray, true); + } + + private void DrawDebugPanel() + { + int fixtures = 0; + for (int i = 0; i < World.BodyList.Count; i++) + { + fixtures += World.BodyList[i].FixtureList.Count; + } + + int x = (int)DebugPanelPosition.X; + int y = (int)DebugPanelPosition.Y; + + DrawString(x, y, "Objects:" + + "\n- Bodies: " + World.BodyList.Count + + "\n- Fixtures: " + fixtures + + "\n- Contacts: " + World.ContactList.Count + + "\n- Joints: " + World.JointList.Count + + "\n- Controllers: " + World.ControllerList.Count + + "\n- Proxies: " + World.ProxyCount); + + DrawString(x + 110, y, "Update time:" + + "\n- Body: " + World.SolveUpdateTime + + "\n- Contact: " + World.ContactsUpdateTime + + "\n- CCD: " + World.ContinuousPhysicsTime + + "\n- Joint: " + World.Island.JointUpdateTime + + "\n- Controller: " + World.ControllersUpdateTime + + "\n- Total: " + World.UpdateTime); + } + + public void DrawAABB(ref AABB aabb, Color color) + { + Vector2[] verts = new Vector2[4]; + verts[0] = new Vector2(aabb.LowerBound.X, aabb.LowerBound.Y); + verts[1] = new Vector2(aabb.UpperBound.X, aabb.LowerBound.Y); + verts[2] = new Vector2(aabb.UpperBound.X, aabb.UpperBound.Y); + verts[3] = new Vector2(aabb.LowerBound.X, aabb.UpperBound.Y); + + DrawPolygon(verts, 4, color); + } + + private void DrawJoint(Joint joint) + { + if (!joint.Enabled) + return; + + Body b1 = joint.BodyA; + Body b2 = joint.BodyB; + Transform xf1, xf2; + b1.GetTransform(out xf1); + + Vector2 x2 = Vector2.Zero; + + // WIP David + if (!joint.IsFixedType()) + { + b2.GetTransform(out xf2); + x2 = xf2.Position; + } + + Vector2 p1 = joint.WorldAnchorA; + Vector2 p2 = joint.WorldAnchorB; + Vector2 x1 = xf1.Position; + + Color color = new Color(0.5f, 0.8f, 0.8f); + + switch (joint.JointType) + { + case JointType.Distance: + DrawSegment(p1, p2, color); + break; + case JointType.Pulley: + PulleyJoint pulley = (PulleyJoint)joint; + Vector2 s1 = pulley.GroundAnchorA; + Vector2 s2 = pulley.GroundAnchorB; + DrawSegment(s1, p1, color); + DrawSegment(s2, p2, color); + DrawSegment(s1, s2, color); + break; + case JointType.FixedMouse: + DrawPoint(p1, 0.5f, new Color(0.0f, 1.0f, 0.0f)); + DrawSegment(p1, p2, new Color(0.8f, 0.8f, 0.8f)); + break; + case JointType.Revolute: + //DrawSegment(x2, p1, color); + DrawSegment(p2, p1, color); + DrawSolidCircle(p2, 0.1f, Vector2.Zero, Color.Red); + DrawSolidCircle(p1, 0.1f, Vector2.Zero, Color.Blue); + break; + case JointType.FixedAngle: + //Should not draw anything. + break; + case JointType.FixedRevolute: + DrawSegment(x1, p1, color); + DrawSolidCircle(p1, 0.1f, Vector2.Zero, Color.Pink); + break; + case JointType.FixedLine: + DrawSegment(x1, p1, color); + DrawSegment(p1, p2, color); + break; + case JointType.FixedDistance: + DrawSegment(x1, p1, color); + DrawSegment(p1, p2, color); + break; + case JointType.FixedPrismatic: + DrawSegment(x1, p1, color); + DrawSegment(p1, p2, color); + break; + case JointType.Gear: + DrawSegment(x1, x2, color); + break; + //case JointType.Weld: + // break; + default: + DrawSegment(x1, p1, color); + DrawSegment(p1, p2, color); + DrawSegment(x2, p2, color); + break; + } + } + + public void DrawShape(Fixture fixture, Transform xf, Color color) + { + switch (fixture.ShapeType) + { + case ShapeType.Circle: + { + CircleShape circle = (CircleShape)fixture.Shape; + + Vector2 center = MathUtils.Multiply(ref xf, circle.Position); + float radius = circle.Radius; + Vector2 axis = xf.R.Col1; + + DrawSolidCircle(center, radius, axis, color); + } + break; + + case ShapeType.Polygon: + { + PolygonShape poly = (PolygonShape)fixture.Shape; + int vertexCount = poly.Vertices.Count; + Debug.Assert(vertexCount <= Settings.MaxPolygonVertices); + + for (int i = 0; i < vertexCount; ++i) + { + _tempVertices[i] = MathUtils.Multiply(ref xf, poly.Vertices[i]); + } + + DrawSolidPolygon(_tempVertices, vertexCount, color); + } + break; + + + case ShapeType.Edge: + { + EdgeShape edge = (EdgeShape)fixture.Shape; + Vector2 v1 = MathUtils.Multiply(ref xf, edge.Vertex1); + Vector2 v2 = MathUtils.Multiply(ref xf, edge.Vertex2); + DrawSegment(v1, v2, color); + } + break; + + case ShapeType.Loop: + { + LoopShape loop = (LoopShape)fixture.Shape; + int count = loop.Vertices.Count; + + Vector2 v1 = MathUtils.Multiply(ref xf, loop.Vertices[count - 1]); + DrawCircle(v1, 0.05f, color); + for (int i = 0; i < count; ++i) + { + Vector2 v2 = MathUtils.Multiply(ref xf, loop.Vertices[i]); + DrawSegment(v1, v2, color); + v1 = v2; + } + } + break; + } + } + + public override void DrawPolygon(Vector2[] vertices, int count, float red, float green, float blue) + { + DrawPolygon(vertices, count, new Color(red, green, blue)); + } + + public void DrawPolygon(Vector2[] vertices, int count, Color color) + { + if (!_primitiveBatch.IsReady()) + { + throw new InvalidOperationException("BeginCustomDraw must be called before drawing anything."); + } + for (int i = 0; i < count - 1; i++) + { + _primitiveBatch.AddVertex(vertices[i], color, PrimitiveType.LineList); + _primitiveBatch.AddVertex(vertices[i + 1], color, PrimitiveType.LineList); + } + + _primitiveBatch.AddVertex(vertices[count - 1], color, PrimitiveType.LineList); + _primitiveBatch.AddVertex(vertices[0], color, PrimitiveType.LineList); + } + + public override void DrawSolidPolygon(Vector2[] vertices, int count, float red, float green, float blue) + { + DrawSolidPolygon(vertices, count, new Color(red, green, blue), true); + } + + public void DrawSolidPolygon(Vector2[] vertices, int count, Color color) + { + DrawSolidPolygon(vertices, count, color, true); + } + + public void DrawSolidPolygon(Vector2[] vertices, int count, Color color, bool outline) + { + if (!_primitiveBatch.IsReady()) + { + throw new InvalidOperationException("BeginCustomDraw must be called before drawing anything."); + } + if (count == 2) + { + DrawPolygon(vertices, count, color); + return; + } + + Color colorFill = color * (outline ? 0.5f : 1.0f); + + for (int i = 1; i < count - 1; i++) + { + _primitiveBatch.AddVertex(vertices[0], colorFill, PrimitiveType.TriangleList); + _primitiveBatch.AddVertex(vertices[i], colorFill, PrimitiveType.TriangleList); + _primitiveBatch.AddVertex(vertices[i + 1], colorFill, PrimitiveType.TriangleList); + } + + if (outline) + { + DrawPolygon(vertices, count, color); + } + } + + public override void DrawCircle(Vector2 center, float radius, float red, float green, float blue) + { + DrawCircle(center, radius, new Color(red, green, blue)); + } + + public void DrawCircle(Vector2 center, float radius, Color color) + { + if (!_primitiveBatch.IsReady()) + { + throw new InvalidOperationException("BeginCustomDraw must be called before drawing anything."); + } + const double increment = Math.PI * 2.0 / CircleSegments; + double theta = 0.0; + + for (int i = 0; i < CircleSegments; i++) + { + Vector2 v1 = center + radius * new Vector2((float)Math.Cos(theta), (float)Math.Sin(theta)); + Vector2 v2 = center + + radius * + new Vector2((float)Math.Cos(theta + increment), (float)Math.Sin(theta + increment)); + + _primitiveBatch.AddVertex(v1, color, PrimitiveType.LineList); + _primitiveBatch.AddVertex(v2, color, PrimitiveType.LineList); + + theta += increment; + } + } + + public override void DrawSolidCircle(Vector2 center, float radius, Vector2 axis, float red, float green, + float blue) + { + DrawSolidCircle(center, radius, axis, new Color(red, green, blue)); + } + + public void DrawSolidCircle(Vector2 center, float radius, Vector2 axis, Color color) + { + if (!_primitiveBatch.IsReady()) + { + throw new InvalidOperationException("BeginCustomDraw must be called before drawing anything."); + } + const double increment = Math.PI * 2.0 / CircleSegments; + double theta = 0.0; + + Color colorFill = color * 0.5f; + + Vector2 v0 = center + radius * new Vector2((float)Math.Cos(theta), (float)Math.Sin(theta)); + theta += increment; + + for (int i = 1; i < CircleSegments - 1; i++) + { + Vector2 v1 = center + radius * new Vector2((float)Math.Cos(theta), (float)Math.Sin(theta)); + Vector2 v2 = center + + radius * + new Vector2((float)Math.Cos(theta + increment), (float)Math.Sin(theta + increment)); + + _primitiveBatch.AddVertex(v0, colorFill, PrimitiveType.TriangleList); + _primitiveBatch.AddVertex(v1, colorFill, PrimitiveType.TriangleList); + _primitiveBatch.AddVertex(v2, colorFill, PrimitiveType.TriangleList); + + theta += increment; + } + DrawCircle(center, radius, color); + + DrawSegment(center, center + axis * radius, color); + } + + public override void DrawSegment(Vector2 start, Vector2 end, float red, float green, float blue) + { + DrawSegment(start, end, new Color(red, green, blue)); + } + + public void DrawSegment(Vector2 start, Vector2 end, Color color) + { + if (!_primitiveBatch.IsReady()) + { + throw new InvalidOperationException("BeginCustomDraw must be called before drawing anything."); + } + _primitiveBatch.AddVertex(start, color, PrimitiveType.LineList); + _primitiveBatch.AddVertex(end, color, PrimitiveType.LineList); + } + + public override void DrawTransform(ref Transform transform) + { + const float axisScale = 0.4f; + Vector2 p1 = transform.Position; + + Vector2 p2 = p1 + axisScale * transform.R.Col1; + DrawSegment(p1, p2, Color.Red); + + p2 = p1 + axisScale * transform.R.Col2; + DrawSegment(p1, p2, Color.Green); + } + + public void DrawPoint(Vector2 p, float size, Color color) + { + Vector2[] verts = new Vector2[4]; + float hs = size / 2.0f; + verts[0] = p + new Vector2(-hs, -hs); + verts[1] = p + new Vector2(hs, -hs); + verts[2] = p + new Vector2(hs, hs); + verts[3] = p + new Vector2(-hs, hs); + + DrawSolidPolygon(verts, 4, color, true); + } + + public void DrawString(int x, int y, string s, params object[] args) + { + _stringData.Add(new StringData(x, y, s, args, TextColor)); + } + + public void DrawArrow(Vector2 start, Vector2 end, float length, float width, bool drawStartIndicator, + Color color) + { + // Draw connection segment between start- and end-point + DrawSegment(start, end, color); + + // Precalculate halfwidth + float halfWidth = width / 2; + + // Create directional reference + Vector2 rotation = (start - end); + rotation.Normalize(); + + // Calculate angle of directional vector + float angle = (float)Math.Atan2(rotation.X, -rotation.Y); + // Create matrix for rotation + Matrix rotMatrix = Matrix.CreateRotationZ(angle); + // Create translation matrix for end-point + Matrix endMatrix = Matrix.CreateTranslation(end.X, end.Y, 0); + + // Setup arrow end shape + Vector2[] verts = new Vector2[3]; + verts[0] = new Vector2(0, 0); + verts[1] = new Vector2(-halfWidth, -length); + verts[2] = new Vector2(halfWidth, -length); + + // Rotate end shape + Vector2.Transform(verts, ref rotMatrix, verts); + // Translate end shape + Vector2.Transform(verts, ref endMatrix, verts); + + // Draw arrow end shape + DrawSolidPolygon(verts, 3, color, false); + + if (drawStartIndicator) + { + // Create translation matrix for start + Matrix startMatrix = Matrix.CreateTranslation(start.X, start.Y, 0); + // Setup arrow start shape + Vector2[] baseVerts = new Vector2[4]; + baseVerts[0] = new Vector2(-halfWidth, length / 4); + baseVerts[1] = new Vector2(halfWidth, length / 4); + baseVerts[2] = new Vector2(halfWidth, 0); + baseVerts[3] = new Vector2(-halfWidth, 0); + + // Rotate start shape + Vector2.Transform(baseVerts, ref rotMatrix, baseVerts); + // Translate start shape + Vector2.Transform(baseVerts, ref startMatrix, baseVerts); + // Draw start shape + DrawSolidPolygon(baseVerts, 4, color, false); + } + } + + public void RenderDebugData(ref Matrix projection, ref Matrix view) + { + if (!Enabled) + { + return; + } + + //Nothing is enabled - don't draw the debug view. + if (Flags == 0) + return; + + _device.RasterizerState = RasterizerState.CullNone; + _device.DepthStencilState = DepthStencilState.Default; + + _primitiveBatch.Begin(ref projection, ref view); + DrawDebugData(); + _primitiveBatch.End(); + + if ((Flags & DebugViewFlags.PerformanceGraph) == DebugViewFlags.PerformanceGraph) + { + _primitiveBatch.Begin(ref _localProjection, ref _localView); + DrawPerformanceGraph(); + _primitiveBatch.End(); + } + + // begin the sprite batch effect + _batch.Begin(SpriteSortMode.Deferred, BlendState.AlphaBlend); + + // draw any strings we have + for (int i = 0; i < _stringData.Count; i++) + { + _batch.DrawString(_font, string.Format(_stringData[i].S, _stringData[i].Args), + new Vector2(_stringData[i].X + 1f, _stringData[i].Y + 1f), Color.Black); + _batch.DrawString(_font, string.Format(_stringData[i].S, _stringData[i].Args), + new Vector2(_stringData[i].X, _stringData[i].Y), _stringData[i].Color); + } + // end the sprite batch effect + _batch.End(); + + _stringData.Clear(); + } + + public void RenderDebugData(ref Matrix projection) + { + if (!Enabled) + { + return; + } + Matrix view = Matrix.Identity; + RenderDebugData(ref projection, ref view); + } + + public void LoadContent(GraphicsDevice device, ContentManager content) + { + // Create a new SpriteBatch, which can be used to draw textures. + _device = device; + _batch = new SpriteBatch(_device); + _primitiveBatch = new PrimitiveBatch(_device, 1000); + _font = content.Load("font"); + _stringData = new List(); + + _localProjection = Matrix.CreateOrthographicOffCenter(0f, _device.Viewport.Width, _device.Viewport.Height, + 0f, 0f, 1f); + _localView = Matrix.Identity; + } + + #region Nested type: ContactPoint + + private struct ContactPoint + { + public Vector2 Normal; + public Vector2 Position; + public PointState State; + } + + #endregion + + #region Nested type: StringData + + private struct StringData + { + public object[] Args; + public Color Color; + public string S; + public int X, Y; + + public StringData(int x, int y, string s, object[] args, Color color) + { + X = x; + Y = y; + S = s; + Args = args; + Color = color; + } + } + + #endregion + } +} \ No newline at end of file diff --git a/Dynamics/Body.cs b/Dynamics/Body.cs new file mode 100644 index 0000000..b1c4dc4 --- /dev/null +++ b/Dynamics/Body.cs @@ -0,0 +1,1393 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Diagnostics; +using FarseerPhysics.Collision; +using FarseerPhysics.Collision.Shapes; +using FarseerPhysics.Common; +using FarseerPhysics.Common.PhysicsLogic; +using FarseerPhysics.Controllers; +using FarseerPhysics.Dynamics.Contacts; +using FarseerPhysics.Dynamics.Joints; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics +{ + /// + /// The body type. + /// + public enum BodyType + { + /// + /// Zero velocity, may be manually moved. Note: even static bodies have mass. + /// + Static, + /// + /// Zero mass, non-zero velocity set by user, moved by solver + /// + Kinematic, + /// + /// Positive mass, non-zero velocity determined by forces, moved by solver + /// + Dynamic, + } + + [Flags] + public enum BodyFlags + { + None = 0, + Island = (1 << 0), + Awake = (1 << 1), + AutoSleep = (1 << 2), + Bullet = (1 << 3), + FixedRotation = (1 << 4), + Enabled = (1 << 5), + IgnoreGravity = (1 << 6), + IgnoreCCD = (1 << 7), + } + + public class Body : IDisposable + { + private static int _bodyIdCounter; + internal float AngularVelocityInternal; + public int BodyId; + public ControllerFilter ControllerFilter; + internal BodyFlags Flags; + internal Vector2 Force; + internal float InvI; + internal float InvMass; + internal Vector2 LinearVelocityInternal; + public PhysicsLogicFilter PhysicsLogicFilter; + internal float SleepTime; + internal Sweep Sweep; // the swept motion for CCD + internal float Torque; + internal World World; + internal Transform Xf; // the body origin transform + private float _angularDamping; + private BodyType _bodyType; + private float _inertia; + private float _linearDamping; + private float _mass; + + internal Body() + { + FixtureList = new List(32); + } + + public Body(World world) + : this(world, null) + { + } + + public Body(World world, object userData) + { + FixtureList = new List(32); + BodyId = _bodyIdCounter++; + + World = world; + UserData = userData; + + FixedRotation = false; + IsBullet = false; + SleepingAllowed = true; + Awake = true; + BodyType = BodyType.Static; + Enabled = true; + + Xf.R.Set(0); + + world.AddBody(this); + } + + /// + /// Gets the total number revolutions the body has made. + /// + /// The revolutions. + public float Revolutions + { + get { return Rotation / (float)Math.PI; } + } + + /// + /// Gets or sets the body type. + /// + /// The type of body. + public BodyType BodyType + { + get { return _bodyType; } + set + { + if (_bodyType == value) + { + return; + } + + _bodyType = value; + + ResetMassData(); + + if (_bodyType == BodyType.Static) + { + LinearVelocityInternal = Vector2.Zero; + AngularVelocityInternal = 0.0f; + } + + Awake = true; + + Force = Vector2.Zero; + Torque = 0.0f; + + // Since the body type changed, we need to flag contacts for filtering. + for (int i = 0; i < FixtureList.Count; i++) + { + Fixture f = FixtureList[i]; + f.Refilter(); + } + } + } + + /// + /// Get or sets the linear velocity of the center of mass. + /// + /// The linear velocity. + public Vector2 LinearVelocity + { + set + { + Debug.Assert(!float.IsNaN(value.X) && !float.IsNaN(value.Y)); + + if (_bodyType == BodyType.Static) + return; + + if (Vector2.Dot(value, value) > 0.0f) + Awake = true; + + LinearVelocityInternal = value; + } + get { return LinearVelocityInternal; } + } + + /// + /// Gets or sets the angular velocity. Radians/second. + /// + /// The angular velocity. + public float AngularVelocity + { + set + { + Debug.Assert(!float.IsNaN(value)); + + if (_bodyType == BodyType.Static) + return; + + if (value * value > 0.0f) + Awake = true; + + AngularVelocityInternal = value; + } + get { return AngularVelocityInternal; } + } + + /// + /// Gets or sets the linear damping. + /// + /// The linear damping. + public float LinearDamping + { + get { return _linearDamping; } + set + { + Debug.Assert(!float.IsNaN(value)); + + _linearDamping = value; + } + } + + /// + /// Gets or sets the angular damping. + /// + /// The angular damping. + public float AngularDamping + { + get { return _angularDamping; } + set + { + Debug.Assert(!float.IsNaN(value)); + + _angularDamping = value; + } + } + + /// + /// Gets or sets a value indicating whether this body should be included in the CCD solver. + /// + /// true if this instance is included in CCD; otherwise, false. + public bool IsBullet + { + set + { + if (value) + { + Flags |= BodyFlags.Bullet; + } + else + { + Flags &= ~BodyFlags.Bullet; + } + } + get { return (Flags & BodyFlags.Bullet) == BodyFlags.Bullet; } + } + + /// + /// You can disable sleeping on this body. If you disable sleeping, the + /// body will be woken. + /// + /// true if sleeping is allowed; otherwise, false. + public bool SleepingAllowed + { + set + { + if (value) + { + Flags |= BodyFlags.AutoSleep; + } + else + { + Flags &= ~BodyFlags.AutoSleep; + Awake = true; + } + } + get { return (Flags & BodyFlags.AutoSleep) == BodyFlags.AutoSleep; } + } + + /// + /// Set the sleep state of the body. A sleeping body has very + /// low CPU cost. + /// + /// true if awake; otherwise, false. + public bool Awake + { + set + { + if (value) + { + if ((Flags & BodyFlags.Awake) == 0) + { + Flags |= BodyFlags.Awake; + SleepTime = 0.0f; + } + } + else + { + Flags &= ~BodyFlags.Awake; + SleepTime = 0.0f; + LinearVelocityInternal = Vector2.Zero; + AngularVelocityInternal = 0.0f; + Force = Vector2.Zero; + Torque = 0.0f; + } + } + get { return (Flags & BodyFlags.Awake) == BodyFlags.Awake; } + } + + /// + /// Set the active state of the body. An inactive body is not + /// simulated and cannot be collided with or woken up. + /// If you pass a flag of true, all fixtures will be added to the + /// broad-phase. + /// If you pass a flag of false, all fixtures will be removed from + /// the broad-phase and all contacts will be destroyed. + /// Fixtures and joints are otherwise unaffected. You may continue + /// to create/destroy fixtures and joints on inactive bodies. + /// Fixtures on an inactive body are implicitly inactive and will + /// not participate in collisions, ray-casts, or queries. + /// Joints connected to an inactive body are implicitly inactive. + /// An inactive body is still owned by a b2World object and remains + /// in the body list. + /// + /// true if active; otherwise, false. + public bool Enabled + { + set + { + if (value == Enabled) + { + return; + } + + if (value) + { + Flags |= BodyFlags.Enabled; + + // Create all proxies. + IBroadPhase broadPhase = World.ContactManager.BroadPhase; + for (int i = 0; i < FixtureList.Count; i++) + { + FixtureList[i].CreateProxies(broadPhase, ref Xf); + } + + // Contacts are created the next time step. + } + else + { + Flags &= ~BodyFlags.Enabled; + + // Destroy all proxies. + IBroadPhase broadPhase = World.ContactManager.BroadPhase; + + for (int i = 0; i < FixtureList.Count; i++) + { + FixtureList[i].DestroyProxies(broadPhase); + } + + // Destroy the attached contacts. + ContactEdge ce = ContactList; + while (ce != null) + { + ContactEdge ce0 = ce; + ce = ce.Next; + World.ContactManager.Destroy(ce0.Contact); + } + ContactList = null; + } + } + get { return (Flags & BodyFlags.Enabled) == BodyFlags.Enabled; } + } + + /// + /// Set this body to have fixed rotation. This causes the mass + /// to be reset. + /// + /// true if it has fixed rotation; otherwise, false. + public bool FixedRotation + { + set + { + if (value) + { + Flags |= BodyFlags.FixedRotation; + } + else + { + Flags &= ~BodyFlags.FixedRotation; + } + + ResetMassData(); + } + get { return (Flags & BodyFlags.FixedRotation) == BodyFlags.FixedRotation; } + } + + /// + /// Gets all the fixtures attached to this body. + /// + /// The fixture list. + public List FixtureList { get; internal set; } + + /// + /// Get the list of all joints attached to this body. + /// + /// The joint list. + public JointEdge JointList { get; internal set; } + + /// + /// Get the list of all contacts attached to this body. + /// Warning: this list changes during the time step and you may + /// miss some collisions if you don't use ContactListener. + /// + /// The contact list. + public ContactEdge ContactList { get; internal set; } + + /// + /// Set the user data. Use this to store your application specific data. + /// + /// The user data. + private object _userdata; + public object UserData + { + get + { + return this._userdata; + } + set + { + this._userdata = value; + foreach (Fixture f in this.FixtureList) + f.UserData = value; + } + } + + /// + /// Get the world body origin position. + /// + /// Return the world position of the body's origin. + public Vector2 Position + { + get { return Xf.Position; } + set + { + Debug.Assert(!float.IsNaN(value.X) && !float.IsNaN(value.Y)); + + SetTransform(ref value, Rotation); + } + } + + /// + /// Get the angle in radians. + /// + /// Return the current world rotation angle in radians. + public float Rotation + { + get { return Sweep.A; } + set + { + Debug.Assert(!float.IsNaN(value)); + + SetTransform(ref Xf.Position, value); + } + } + + /// + /// Gets or sets a value indicating whether this body is static. + /// + /// true if this instance is static; otherwise, false. + public bool IsStatic + { + get { return _bodyType == BodyType.Static; } + set + { + if (value) + BodyType = BodyType.Static; + else + BodyType = BodyType.Dynamic; + } + } + + /// + /// Gets or sets a value indicating whether this body ignores gravity. + /// + /// true if it ignores gravity; otherwise, false. + public bool IgnoreGravity + { + get { return (Flags & BodyFlags.IgnoreGravity) == BodyFlags.IgnoreGravity; } + set + { + if (value) + Flags |= BodyFlags.IgnoreGravity; + else + Flags &= ~BodyFlags.IgnoreGravity; + } + } + + /// + /// Get the world position of the center of mass. + /// + /// The world position. + public Vector2 WorldCenter + { + get { return Sweep.C; } + } + + /// + /// Get the local position of the center of mass. + /// + /// The local position. + public Vector2 LocalCenter + { + get { return Sweep.LocalCenter; } + set + { + if (_bodyType != BodyType.Dynamic) + return; + + // Move center of mass. + Vector2 oldCenter = Sweep.C; + Sweep.LocalCenter = value; + Sweep.C0 = Sweep.C = MathUtils.Multiply(ref Xf, ref Sweep.LocalCenter); + + // Update center of mass velocity. + Vector2 a = Sweep.C - oldCenter; + LinearVelocityInternal += new Vector2(-AngularVelocityInternal * a.Y, AngularVelocityInternal * a.X); + } + } + + /// + /// Gets or sets the mass. Usually in kilograms (kg). + /// + /// The mass. + public float Mass + { + get { return _mass; } + set + { + Debug.Assert(!float.IsNaN(value)); + + if (_bodyType != BodyType.Dynamic) + return; + + _mass = value; + + if (_mass <= 0.0f) + _mass = 1.0f; + + InvMass = 1.0f / _mass; + } + } + + /// + /// Get or set the rotational inertia of the body about the local origin. usually in kg-m^2. + /// + /// The inertia. + public float Inertia + { + get { return _inertia + Mass * Vector2.Dot(Sweep.LocalCenter, Sweep.LocalCenter); } + set + { + Debug.Assert(!float.IsNaN(value)); + + if (_bodyType != BodyType.Dynamic) + return; + + if (value > 0.0f && (Flags & BodyFlags.FixedRotation) == 0) + { + _inertia = value - Mass * Vector2.Dot(LocalCenter, LocalCenter); + Debug.Assert(_inertia > 0.0f); + InvI = 1.0f / _inertia; + } + } + } + + public float Restitution + { + get + { + float res = 0; + + for (int i = 0; i < FixtureList.Count; i++) + { + Fixture f = FixtureList[i]; + res += f.Restitution; + } + + return res / FixtureList.Count; + } + set + { + for (int i = 0; i < FixtureList.Count; i++) + { + Fixture f = FixtureList[i]; + f.Restitution = value; + } + } + } + + public float Friction + { + get + { + float res = 0; + + for (int i = 0; i < FixtureList.Count; i++) + { + Fixture f = FixtureList[i]; + res += f.Friction; + } + + return res / FixtureList.Count; + } + set + { + for (int i = 0; i < FixtureList.Count; i++) + { + Fixture f = FixtureList[i]; + f.Friction = value; + } + } + } + + public Category CollisionCategories + { + set + { + for (int i = 0; i < FixtureList.Count; i++) + { + Fixture f = FixtureList[i]; + f.CollisionCategories = value; + } + } + } + + public Category CollidesWith + { + set + { + for (int i = 0; i < FixtureList.Count; i++) + { + Fixture f = FixtureList[i]; + f.CollidesWith = value; + } + } + } + + public short CollisionGroup + { + set + { + for (int i = 0; i < FixtureList.Count; i++) + { + Fixture f = FixtureList[i]; + f.CollisionGroup = value; + } + } + } + + public bool IsSensor + { + set + { + for (int i = 0; i < FixtureList.Count; i++) + { + Fixture f = FixtureList[i]; + f.IsSensor = value; + } + } + } + + public bool IgnoreCCD + { + get { return (Flags & BodyFlags.IgnoreCCD) == BodyFlags.IgnoreCCD; } + set + { + if (value) + Flags |= BodyFlags.IgnoreCCD; + else + Flags &= ~BodyFlags.IgnoreCCD; + } + } + + #region IDisposable Members + + public bool IsDisposed { get; set; } + + public void Dispose() + { + if (!IsDisposed) + { + + World.RemoveBody(this); + IsDisposed = true; + GC.SuppressFinalize(this); + } + } + + #endregion + + /// + /// Resets the dynamics of this body. + /// Sets torque, force and linear/angular velocity to 0 + /// + public void ResetDynamics() + { + Torque = 0; + AngularVelocityInternal = 0; + Force = Vector2.Zero; + LinearVelocityInternal = Vector2.Zero; + } + + /// + /// 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 function is locked during callbacks. + /// + /// The shape. + /// + public Fixture CreateFixture(Shape shape) + { + return new Fixture(this, shape); + } + + /// + /// 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 function is locked during callbacks. + /// + /// The shape. + /// Application specific data + /// + public Fixture CreateFixture(Shape shape, object userData) + { + return new Fixture(this, shape, userData); + } + + /// + /// Destroy a fixture. This removes the fixture from the broad-phase and + /// destroys all contacts associated with this fixture. This will + /// automatically adjust the mass of the body if the body is dynamic and the + /// fixture has positive density. + /// All fixtures attached to a body are implicitly destroyed when the body is destroyed. + /// Warning: This function is locked during callbacks. + /// + /// The fixture to be removed. + public void DestroyFixture(Fixture fixture) + { + Debug.Assert(fixture.Body == this); + + // Remove the fixture from this body's singly linked list. + Debug.Assert(FixtureList.Count > 0); + + // You tried to remove a fixture that not present in the fixturelist. + Debug.Assert(FixtureList.Contains(fixture)); + + // Destroy any contacts associated with the fixture. + ContactEdge edge = ContactList; + while (edge != null) + { + Contact c = edge.Contact; + edge = edge.Next; + + Fixture fixtureA = c.FixtureA; + Fixture fixtureB = c.FixtureB; + + if (fixture == fixtureA || fixture == fixtureB) + { + // This destroys the contact and removes it from + // this body's contact list. + World.ContactManager.Destroy(c); + } + } + + if ((Flags & BodyFlags.Enabled) == BodyFlags.Enabled) + { + IBroadPhase broadPhase = World.ContactManager.BroadPhase; + fixture.DestroyProxies(broadPhase); + } + + FixtureList.Remove(fixture); + fixture.Destroy(); + fixture.Body = null; + + ResetMassData(); + } + + /// + /// Set the position of the body's origin and rotation. + /// This breaks any contacts and wakes the other bodies. + /// Manipulating a body's transform may cause non-physical behavior. + /// + /// The world position of the body's local origin. + /// The world rotation in radians. + public void SetTransform(ref Vector2 position, float rotation) + { + SetTransformIgnoreContacts(ref position, rotation); + + World.ContactManager.FindNewContacts(); + } + + /// + /// Set the position of the body's origin and rotation. + /// This breaks any contacts and wakes the other bodies. + /// Manipulating a body's transform may cause non-physical behavior. + /// + /// The world position of the body's local origin. + /// The world rotation in radians. + public void SetTransform(Vector2 position, float rotation) + { + SetTransform(ref position, rotation); + } + + /// + /// For teleporting a body without considering new contacts immediately. + /// + /// The position. + /// The angle. + public void SetTransformIgnoreContacts(ref Vector2 position, float angle) + { + // Sometimes this is called with an empty Fixture list + // -- Nathan Adams [adamsna@datanethost.net] - 6/2/2012 + if (FixtureList == null || FixtureList.Count == 0) + return; + + Xf.R.Set(angle); + Xf.Position = position; + + Sweep.C0 = + Sweep.C = + new Vector2(Xf.Position.X + Xf.R.Col1.X * Sweep.LocalCenter.X + Xf.R.Col2.X * Sweep.LocalCenter.Y, + Xf.Position.Y + Xf.R.Col1.Y * Sweep.LocalCenter.X + Xf.R.Col2.Y * Sweep.LocalCenter.Y); + Sweep.A0 = Sweep.A = angle; + + IBroadPhase broadPhase = World.ContactManager.BroadPhase; + for (int i = 0; i < FixtureList.Count; i++) + { + FixtureList[i].Synchronize(broadPhase, ref Xf, ref Xf); + } + } + + /// + /// Get the body transform for the body's origin. + /// + /// The transform of the body's origin. + public void GetTransform(out Transform transform) + { + transform = Xf; + } + + /// + /// Apply a force at a world point. If the force is not + /// applied at the center of mass, it will generate a torque and + /// affect the angular velocity. This wakes up the body. + /// + /// The world force vector, usually in Newtons (N). + /// The world position of the point of application. + public void ApplyForce(Vector2 force, Vector2 point) + { + ApplyForce(ref force, ref point); + } + + /// + /// Applies a force at the center of mass. + /// + /// The force. + public void ApplyForce(ref Vector2 force) + { + ApplyForce(ref force, ref Xf.Position); + } + + /// + /// Applies a force at the center of mass. + /// + /// The force. + public void ApplyForce(Vector2 force) + { + ApplyForce(ref force, ref Xf.Position); + } + + /// + /// Apply a force at a world point. If the force is not + /// applied at the center of mass, it will generate a torque and + /// affect the angular velocity. This wakes up the body. + /// + /// The world force vector, usually in Newtons (N). + /// The world position of the point of application. + public void ApplyForce(ref Vector2 force, ref Vector2 point) + { + Debug.Assert(!float.IsNaN(force.X)); + Debug.Assert(!float.IsNaN(force.Y)); + Debug.Assert(!float.IsNaN(point.X)); + Debug.Assert(!float.IsNaN(point.Y)); + + if (_bodyType == BodyType.Dynamic) + { + if (Awake == false) + { + Awake = true; + } + + Force += force; + Torque += (point.X - Sweep.C.X) * force.Y - (point.Y - Sweep.C.Y) * force.X; + } + } + + /// + /// Apply a torque. This affects the angular velocity + /// without affecting the linear velocity of the center of mass. + /// This wakes up the body. + /// + /// The torque about the z-axis (out of the screen), usually in N-m. + public void ApplyTorque(float torque) + { + Debug.Assert(!float.IsNaN(torque)); + + if (_bodyType == BodyType.Dynamic) + { + if (Awake == false) + { + Awake = true; + } + + Torque += torque; + } + } + + /// + /// Apply an impulse at a point. This immediately modifies the velocity. + /// This wakes up the body. + /// + /// The world impulse vector, usually in N-seconds or kg-m/s. + public void ApplyLinearImpulse(Vector2 impulse) + { + ApplyLinearImpulse(ref impulse); + } + + /// + /// Apply an impulse at a point. This immediately modifies the velocity. + /// It also modifies the angular velocity if the point of application + /// is not at the center of mass. + /// This wakes up the body. + /// + /// The world impulse vector, usually in N-seconds or kg-m/s. + /// The world position of the point of application. + public void ApplyLinearImpulse(Vector2 impulse, Vector2 point) + { + ApplyLinearImpulse(ref impulse, ref point); + } + + /// + /// Apply an impulse at a point. This immediately modifies the velocity. + /// This wakes up the body. + /// + /// The world impulse vector, usually in N-seconds or kg-m/s. + public void ApplyLinearImpulse(ref Vector2 impulse) + { + if (_bodyType != BodyType.Dynamic) + { + return; + } + if (Awake == false) + { + Awake = true; + } + LinearVelocityInternal += InvMass * impulse; + } + + /// + /// Apply an impulse at a point. This immediately modifies the velocity. + /// It also modifies the angular velocity if the point of application + /// is not at the center of mass. + /// This wakes up the body. + /// + /// The world impulse vector, usually in N-seconds or kg-m/s. + /// The world position of the point of application. + public void ApplyLinearImpulse(ref Vector2 impulse, ref Vector2 point) + { + if (_bodyType != BodyType.Dynamic) + return; + + if (Awake == false) + Awake = true; + + LinearVelocityInternal += InvMass * impulse; + AngularVelocityInternal += InvI * ((point.X - Sweep.C.X) * impulse.Y - (point.Y - Sweep.C.Y) * impulse.X); + } + + /// + /// Apply an angular impulse. + /// + /// The angular impulse in units of kg*m*m/s. + public void ApplyAngularImpulse(float impulse) + { + if (_bodyType != BodyType.Dynamic) + { + return; + } + + if (Awake == false) + { + Awake = true; + } + + AngularVelocityInternal += InvI * impulse; + } + + /// + /// This resets the mass properties to the sum of the mass properties of the fixtures. + /// This normally does not need to be called unless you called SetMassData to override + /// the mass and you later want to reset the mass. + /// + public void ResetMassData() + { + // Compute mass data from shapes. Each shape has its own density. + _mass = 0.0f; + InvMass = 0.0f; + _inertia = 0.0f; + InvI = 0.0f; + Sweep.LocalCenter = Vector2.Zero; + + // Kinematic bodies have zero mass. + if (BodyType == BodyType.Kinematic) + { + Sweep.C0 = Sweep.C = Xf.Position; + return; + } + + Debug.Assert(BodyType == BodyType.Dynamic || BodyType == BodyType.Static); + + // Accumulate mass over all fixtures. + Vector2 center = Vector2.Zero; + foreach (Fixture f in FixtureList) + { + if (f.Shape._density == 0) + { + continue; + } + + MassData massData = f.Shape.MassData; + _mass += massData.Mass; + center += massData.Mass * massData.Centroid; + _inertia += massData.Inertia; + } + + //Static bodies only have mass, they don't have other properties. A little hacky tho... + if (BodyType == BodyType.Static) + { + Sweep.C0 = Sweep.C = Xf.Position; + return; + } + + // Compute center of mass. + if (_mass > 0.0f) + { + InvMass = 1.0f / _mass; + center *= InvMass; + } + else + { + // Force all dynamic bodies to have a positive mass. + _mass = 1.0f; + InvMass = 1.0f; + } + + if (_inertia > 0.0f && (Flags & BodyFlags.FixedRotation) == 0) + { + // Center the inertia about the center of mass. + _inertia -= _mass * Vector2.Dot(center, center); + + Debug.Assert(_inertia > 0.0f); + InvI = 1.0f / _inertia; + } + else + { + _inertia = 0.0f; + InvI = 0.0f; + } + + // Move center of mass. + Vector2 oldCenter = Sweep.C; + Sweep.LocalCenter = center; + Sweep.C0 = Sweep.C = MathUtils.Multiply(ref Xf, ref Sweep.LocalCenter); + + // Update center of mass velocity. + Vector2 a = Sweep.C - oldCenter; + LinearVelocityInternal += new Vector2(-AngularVelocityInternal * a.Y, AngularVelocityInternal * a.X); + } + + /// + /// Get the world coordinates of a point given the local coordinates. + /// + /// A point on the body measured relative the the body's origin. + /// The same point expressed in world coordinates. + public Vector2 GetWorldPoint(ref Vector2 localPoint) + { + return new Vector2(Xf.Position.X + Xf.R.Col1.X * localPoint.X + Xf.R.Col2.X * localPoint.Y, + Xf.Position.Y + Xf.R.Col1.Y * localPoint.X + Xf.R.Col2.Y * localPoint.Y); + } + + /// + /// Get the world coordinates of a point given the local coordinates. + /// + /// A point on the body measured relative the the body's origin. + /// The same point expressed in world coordinates. + public Vector2 GetWorldPoint(Vector2 localPoint) + { + return GetWorldPoint(ref localPoint); + } + + /// + /// Get the world coordinates of a vector given the local coordinates. + /// Note that the vector only takes the rotation into account, not the position. + /// + /// A vector fixed in the body. + /// The same vector expressed in world coordinates. + public Vector2 GetWorldVector(ref Vector2 localVector) + { + return new Vector2(Xf.R.Col1.X * localVector.X + Xf.R.Col2.X * localVector.Y, + Xf.R.Col1.Y * localVector.X + Xf.R.Col2.Y * localVector.Y); + } + + /// + /// Get the world coordinates of a vector given the local coordinates. + /// + /// A vector fixed in the body. + /// The same vector expressed in world coordinates. + public Vector2 GetWorldVector(Vector2 localVector) + { + return GetWorldVector(ref localVector); + } + + /// + /// Gets a local point relative to the body's origin given a world point. + /// Note that the vector only takes the rotation into account, not the position. + /// + /// A point in world coordinates. + /// The corresponding local point relative to the body's origin. + public Vector2 GetLocalPoint(ref Vector2 worldPoint) + { + return + new Vector2((worldPoint.X - Xf.Position.X) * Xf.R.Col1.X + (worldPoint.Y - Xf.Position.Y) * Xf.R.Col1.Y, + (worldPoint.X - Xf.Position.X) * Xf.R.Col2.X + (worldPoint.Y - Xf.Position.Y) * Xf.R.Col2.Y); + } + + /// + /// Gets a local point relative to the body's origin given a world point. + /// + /// A point in world coordinates. + /// The corresponding local point relative to the body's origin. + public Vector2 GetLocalPoint(Vector2 worldPoint) + { + return GetLocalPoint(ref worldPoint); + } + + /// + /// Gets a local vector given a world vector. + /// Note that the vector only takes the rotation into account, not the position. + /// + /// A vector in world coordinates. + /// The corresponding local vector. + public Vector2 GetLocalVector(ref Vector2 worldVector) + { + return new Vector2(worldVector.X * Xf.R.Col1.X + worldVector.Y * Xf.R.Col1.Y, + worldVector.X * Xf.R.Col2.X + worldVector.Y * Xf.R.Col2.Y); + } + + /// + /// Gets a local vector given a world vector. + /// Note that the vector only takes the rotation into account, not the position. + /// + /// A vector in world coordinates. + /// The corresponding local vector. + public Vector2 GetLocalVector(Vector2 worldVector) + { + return GetLocalVector(ref worldVector); + } + + /// + /// Get the world linear velocity of a world point attached to this body. + /// + /// A point in world coordinates. + /// The world velocity of a point. + public Vector2 GetLinearVelocityFromWorldPoint(Vector2 worldPoint) + { + return GetLinearVelocityFromWorldPoint(ref worldPoint); + } + + /// + /// Get the world linear velocity of a world point attached to this body. + /// + /// A point in world coordinates. + /// The world velocity of a point. + public Vector2 GetLinearVelocityFromWorldPoint(ref Vector2 worldPoint) + { + return LinearVelocityInternal + + new Vector2(-AngularVelocityInternal * (worldPoint.Y - Sweep.C.Y), + AngularVelocityInternal * (worldPoint.X - Sweep.C.X)); + } + + /// + /// Get the world velocity of a local point. + /// + /// A point in local coordinates. + /// The world velocity of a point. + public Vector2 GetLinearVelocityFromLocalPoint(Vector2 localPoint) + { + return GetLinearVelocityFromLocalPoint(ref localPoint); + } + + /// + /// Get the world velocity of a local point. + /// + /// A point in local coordinates. + /// The world velocity of a point. + public Vector2 GetLinearVelocityFromLocalPoint(ref Vector2 localPoint) + { + return GetLinearVelocityFromWorldPoint(GetWorldPoint(ref localPoint)); + } + + public Body DeepClone() + { + Body body = Clone(); + + for (int i = 0; i < FixtureList.Count; i++) + { + FixtureList[i].Clone(body); + } + + return body; + } + + public Body Clone() + { + Body body = new Body(); + body.World = World; + body.UserData = UserData; + body.LinearDamping = LinearDamping; + body.LinearVelocityInternal = LinearVelocityInternal; + body.AngularDamping = AngularDamping; + body.AngularVelocityInternal = AngularVelocityInternal; + body.Position = Position; + body.Rotation = Rotation; + body._bodyType = _bodyType; + body.Flags = Flags; + + World.AddBody(body); + + return body; + } + + internal void SynchronizeFixtures() + { + Transform xf1 = new Transform(); + float c = (float)Math.Cos(Sweep.A0), s = (float)Math.Sin(Sweep.A0); + xf1.R.Col1.X = c; + xf1.R.Col2.X = -s; + xf1.R.Col1.Y = s; + xf1.R.Col2.Y = c; + + xf1.Position.X = Sweep.C0.X - (xf1.R.Col1.X * Sweep.LocalCenter.X + xf1.R.Col2.X * Sweep.LocalCenter.Y); + xf1.Position.Y = Sweep.C0.Y - (xf1.R.Col1.Y * Sweep.LocalCenter.X + xf1.R.Col2.Y * Sweep.LocalCenter.Y); + + IBroadPhase broadPhase = World.ContactManager.BroadPhase; + for (int i = 0; i < FixtureList.Count; i++) + { + FixtureList[i].Synchronize(broadPhase, ref xf1, ref Xf); + } + } + + internal void SynchronizeTransform() + { + Xf.R.Set(Sweep.A); + + float vx = Xf.R.Col1.X * Sweep.LocalCenter.X + Xf.R.Col2.X * Sweep.LocalCenter.Y; + float vy = Xf.R.Col1.Y * Sweep.LocalCenter.X + Xf.R.Col2.Y * Sweep.LocalCenter.Y; + + Xf.Position.X = Sweep.C.X - vx; + Xf.Position.Y = Sweep.C.Y - vy; + } + + /// + /// This is used to prevent connected bodies from colliding. + /// It may lie, depending on the collideConnected flag. + /// + /// The other body. + /// + internal bool ShouldCollide(Body other) + { + // At least one body should be dynamic. + if (_bodyType != BodyType.Dynamic && other._bodyType != BodyType.Dynamic) + { + return false; + } + + // Does a joint prevent collision? + for (JointEdge jn = JointList; jn != null; jn = jn.Next) + { + if (jn.Other == other) + { + if (jn.Joint.CollideConnected == false) + { + return false; + } + } + } + + return true; + } + + internal void Advance(float alpha) + { + // Advance to the new safe time. + Sweep.Advance(alpha); + Sweep.C = Sweep.C0; + Sweep.A = Sweep.A0; + SynchronizeTransform(); + } + + public event OnCollisionEventHandler OnCollision + { + add + { + for (int i = 0; i < FixtureList.Count; i++) + { + FixtureList[i].OnCollision += value; + } + } + remove + { + for (int i = 0; i < FixtureList.Count; i++) + { + FixtureList[i].OnCollision -= value; + } + } + } + + public event OnSeparationEventHandler OnSeparation + { + add + { + for (int i = 0; i < FixtureList.Count; i++) + { + FixtureList[i].OnSeparation += value; + } + } + remove + { + for (int i = 0; i < FixtureList.Count; i++) + { + FixtureList[i].OnSeparation -= value; + } + } + } + + public void IgnoreCollisionWith(Body other) + { + for (int i = 0; i < FixtureList.Count; i++) + { + Fixture f = FixtureList[i]; + for (int j = 0; j < other.FixtureList.Count; j++) + { + Fixture f2 = other.FixtureList[j]; + + f.IgnoreCollisionWith(f2); + } + } + } + + public void RestoreCollisionWith(Body other) + { + for (int i = 0; i < FixtureList.Count; i++) + { + Fixture f = FixtureList[i]; + for (int j = 0; j < other.FixtureList.Count; j++) + { + Fixture f2 = other.FixtureList[j]; + + f.RestoreCollisionWith(f2); + } + } + } + } +} \ No newline at end of file diff --git a/Dynamics/BreakableBody.cs b/Dynamics/BreakableBody.cs new file mode 100644 index 0000000..4f2066c --- /dev/null +++ b/Dynamics/BreakableBody.cs @@ -0,0 +1,142 @@ +using System; +using System.Collections.Generic; +using FarseerPhysics.Collision.Shapes; +using FarseerPhysics.Common; +using FarseerPhysics.Dynamics.Contacts; +using FarseerPhysics.Factories; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics +{ + /// + /// A type of body that supports multiple fixtures that can break apart. + /// + public class BreakableBody + { + public bool Broken; + public Body MainBody; + public List Parts = new List(8); + + /// + /// The force needed to break the body apart. + /// Default: 500 + /// + public float Strength = 500.0f; + + private float[] _angularVelocitiesCache = new float[8]; + private bool _break; + private Vector2[] _velocitiesCache = new Vector2[8]; + private World _world; + + public BreakableBody(IEnumerable vertices, World world, float density) + : this(vertices, world, density, null) + { + } + + public BreakableBody() + { + + } + + public BreakableBody(IEnumerable vertices, World world, float density, object userData) + { + _world = world; + _world.ContactManager.PostSolve += PostSolve; + MainBody = new Body(_world); + MainBody.BodyType = BodyType.Dynamic; + + foreach (Vertices part in vertices) + { + PolygonShape polygonShape = new PolygonShape(part, density); + Fixture fixture = MainBody.CreateFixture(polygonShape, userData); + Parts.Add(fixture); + } + } + + private void PostSolve(Contact contact, ContactConstraint impulse) + { + if (!Broken) + { + if (Parts.Contains(contact.FixtureA) || Parts.Contains(contact.FixtureB)) + { + float maxImpulse = 0.0f; + int count = contact.Manifold.PointCount; + + for (int i = 0; i < count; ++i) + { + maxImpulse = Math.Max(maxImpulse, impulse.Points[i].NormalImpulse); + } + + if (maxImpulse > Strength) + { + // Flag the body for breaking. + _break = true; + } + } + } + } + + public void Update() + { + if (_break) + { + Decompose(); + Broken = true; + _break = false; + } + + // Cache velocities to improve movement on breakage. + if (Broken == false) + { + //Enlarge the cache if needed + if (Parts.Count > _angularVelocitiesCache.Length) + { + _velocitiesCache = new Vector2[Parts.Count]; + _angularVelocitiesCache = new float[Parts.Count]; + } + + //Cache the linear and angular velocities. + for (int i = 0; i < Parts.Count; i++) + { + _velocitiesCache[i] = Parts[i].Body.LinearVelocity; + _angularVelocitiesCache[i] = Parts[i].Body.AngularVelocity; + } + } + } + + private void Decompose() + { + //Unsubsribe from the PostSolve delegate + _world.ContactManager.PostSolve -= PostSolve; + + for (int i = 0; i < Parts.Count; i++) + { + Fixture fixture = Parts[i]; + + Shape shape = fixture.Shape.Clone(); + + object userdata = fixture.UserData; + MainBody.DestroyFixture(fixture); + + Body body = BodyFactory.CreateBody(_world); + body.BodyType = BodyType.Dynamic; + body.Position = MainBody.Position; + body.Rotation = MainBody.Rotation; + body.UserData = MainBody.UserData; + + body.CreateFixture(shape, userdata); + + body.AngularVelocity = _angularVelocitiesCache[i]; + body.LinearVelocity = _velocitiesCache[i]; + } + + _world.RemoveBody(MainBody); + _world.RemoveBreakableBody(this); + } + + public void Break() + { + _break = true; + } + } +} \ No newline at end of file diff --git a/Dynamics/ContactManager.cs b/Dynamics/ContactManager.cs new file mode 100644 index 0000000..f38fc7c --- /dev/null +++ b/Dynamics/ContactManager.cs @@ -0,0 +1,340 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System.Collections.Generic; +using FarseerPhysics.Collision; +using FarseerPhysics.Dynamics.Contacts; + +namespace FarseerPhysics.Dynamics +{ + public class ContactManager + { + /// + /// Fires when a contact is created + /// + public BeginContactDelegate BeginContact; + + public IBroadPhase BroadPhase; + + /// + /// The filter used by the contact manager. + /// + public CollisionFilterDelegate ContactFilter; + + public List ContactList = new List(128); + + /// + /// Fires when a contact is deleted + /// + public EndContactDelegate EndContact; + + /// + /// Fires when the broadphase detects that two Fixtures are close to each other. + /// + public BroadphaseDelegate OnBroadphaseCollision; + + /// + /// Fires after the solver has run + /// + public PostSolveDelegate PostSolve; + + /// + /// Fires before the solver runs + /// + public PreSolveDelegate PreSolve; + + internal ContactManager(IBroadPhase broadPhase) + { + BroadPhase = broadPhase; + OnBroadphaseCollision = AddPair; + } + + // Broad-phase callback. + private void AddPair(ref FixtureProxy proxyA, ref FixtureProxy proxyB) + { + Fixture fixtureA = proxyA.Fixture; + Fixture fixtureB = proxyB.Fixture; + + int indexA = proxyA.ChildIndex; + int indexB = proxyB.ChildIndex; + + Body bodyA = fixtureA.Body; + Body bodyB = fixtureB.Body; + + // Are the fixtures on the same body? + if (bodyA == bodyB) + { + return; + } + + // Does a contact already exist? + ContactEdge edge = bodyB.ContactList; + while (edge != null) + { + if (edge.Other == bodyA) + { + Fixture fA = edge.Contact.FixtureA; + Fixture fB = edge.Contact.FixtureB; + int iA = edge.Contact.ChildIndexA; + int iB = edge.Contact.ChildIndexB; + + if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB) + { + // A contact already exists. + return; + } + + if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA) + { + // A contact already exists. + return; + } + } + + edge = edge.Next; + } + + // Does a joint override collision? Is at least one body dynamic? + if (bodyB.ShouldCollide(bodyA) == false) + return; + + //Check default filter + if (ShouldCollide(fixtureA, fixtureB) == false) + return; + + // Check user filtering. + if (ContactFilter != null && ContactFilter(fixtureA, fixtureB) == false) + return; + + if (fixtureA.BeforeCollision != null && fixtureA.BeforeCollision(fixtureA, fixtureB) == false) + return; + + if (fixtureB.BeforeCollision != null && fixtureB.BeforeCollision(fixtureB, fixtureA) == false) + return; + + // Call the factory. + Contact c = Contact.Create(fixtureA, indexA, fixtureB, indexB); + + // Contact creation may swap fixtures. + fixtureA = c.FixtureA; + fixtureB = c.FixtureB; + bodyA = fixtureA.Body; + bodyB = fixtureB.Body; + + // Insert into the world. + ContactList.Add(c); + + // Connect to island graph. + + // Connect to body A + c.NodeA.Contact = c; + c.NodeA.Other = bodyB; + + c.NodeA.Prev = null; + c.NodeA.Next = bodyA.ContactList; + if (bodyA.ContactList != null) + { + bodyA.ContactList.Prev = c.NodeA; + } + bodyA.ContactList = c.NodeA; + + // Connect to body B + c.NodeB.Contact = c; + c.NodeB.Other = bodyA; + + c.NodeB.Prev = null; + c.NodeB.Next = bodyB.ContactList; + if (bodyB.ContactList != null) + { + bodyB.ContactList.Prev = c.NodeB; + } + bodyB.ContactList = c.NodeB; + } + + internal void FindNewContacts() + { + BroadPhase.UpdatePairs(OnBroadphaseCollision); + } + + internal void Destroy(Contact contact) + { + Fixture fixtureA = contact.FixtureA; + Fixture fixtureB = contact.FixtureB; + Body bodyA = fixtureA.Body; + Body bodyB = fixtureB.Body; + + if (EndContact != null && contact.IsTouching()) + { + EndContact(contact); + } + + // Remove from the world. + ContactList.Remove(contact); + + // Remove from body 1 + if (contact.NodeA.Prev != null) + { + contact.NodeA.Prev.Next = contact.NodeA.Next; + } + + if (contact.NodeA.Next != null) + { + contact.NodeA.Next.Prev = contact.NodeA.Prev; + } + + if (contact.NodeA == bodyA.ContactList) + { + bodyA.ContactList = contact.NodeA.Next; + } + + // Remove from body 2 + if (contact.NodeB.Prev != null) + { + contact.NodeB.Prev.Next = contact.NodeB.Next; + } + + if (contact.NodeB.Next != null) + { + contact.NodeB.Next.Prev = contact.NodeB.Prev; + } + + if (contact.NodeB == bodyB.ContactList) + { + bodyB.ContactList = contact.NodeB.Next; + } + + contact.Destroy(); + } + + internal void Collide() + { + // Update awake contacts. + for (int i = 0; i < ContactList.Count; i++) + { + Contact c = ContactList[i]; + Fixture fixtureA = c.FixtureA; + Fixture fixtureB = c.FixtureB; + int indexA = c.ChildIndexA; + int indexB = c.ChildIndexB; + Body bodyA = fixtureA.Body; + Body bodyB = fixtureB.Body; + + if (bodyA.Awake == false && bodyB.Awake == false) + { + continue; + } + + // Is this contact flagged for filtering? + if ((c.Flags & ContactFlags.Filter) == ContactFlags.Filter) + { + // Should these bodies collide? + if (bodyB.ShouldCollide(bodyA) == false) + { + Contact cNuke = c; + Destroy(cNuke); + continue; + } + + // Check default filtering + if (ShouldCollide(fixtureA, fixtureB) == false) + { + Contact cNuke = c; + Destroy(cNuke); + continue; + } + + // Check user filtering. + if (ContactFilter != null && ContactFilter(fixtureA, fixtureB) == false) + { + Contact cNuke = c; + Destroy(cNuke); + continue; + } + + // Clear the filtering flag. + c.Flags &= ~ContactFlags.Filter; + } + + int proxyIdA = fixtureA.Proxies[indexA].ProxyId; + int proxyIdB = fixtureB.Proxies[indexB].ProxyId; + + bool overlap = BroadPhase.TestOverlap(proxyIdA, proxyIdB); + + // Here we destroy contacts that cease to overlap in the broad-phase. + if (overlap == false) + { + Contact cNuke = c; + Destroy(cNuke); + continue; + } + + // The contact persists. + c.Update(this); + } + } + + private static bool ShouldCollide(Fixture fixtureA, Fixture fixtureB) + { + if (Settings.UseFPECollisionCategories) + { + if ((fixtureA.CollisionGroup == fixtureB.CollisionGroup) && + fixtureA.CollisionGroup != 0 && fixtureB.CollisionGroup != 0) + return false; + + if (((fixtureA.CollisionCategories & fixtureB.CollidesWith) == + Category.None) & + ((fixtureB.CollisionCategories & fixtureA.CollidesWith) == + Category.None)) + return false; + + if (fixtureA.IsFixtureIgnored(fixtureB) || + fixtureB.IsFixtureIgnored(fixtureA)) + return false; + + return true; + } + + if (fixtureA.CollisionGroup == fixtureB.CollisionGroup && + fixtureA.CollisionGroup != 0) + { + return fixtureA.CollisionGroup > 0; + } + + bool collide = (fixtureA.CollidesWith & fixtureB.CollisionCategories) != 0 && + (fixtureA.CollisionCategories & fixtureB.CollidesWith) != 0; + + if (collide) + { + if (fixtureA.IsFixtureIgnored(fixtureB) || + fixtureB.IsFixtureIgnored(fixtureA)) + { + return false; + } + } + + return collide; + } + } +} \ No newline at end of file diff --git a/Dynamics/Contacts/Contact.cs b/Dynamics/Contacts/Contact.cs new file mode 100644 index 0000000..fe615f3 --- /dev/null +++ b/Dynamics/Contacts/Contact.cs @@ -0,0 +1,502 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Diagnostics; +using FarseerPhysics.Collision; +using FarseerPhysics.Collision.Shapes; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Contacts +{ + /// + /// A contact edge is used to connect bodies and contacts together + /// in a contact graph where each body is a node and each contact + /// is an edge. A contact edge belongs to a doubly linked list + /// maintained in each attached body. Each contact has two contact + /// nodes, one for each attached body. + /// + public sealed class ContactEdge + { + /// + /// The contact + /// + public Contact Contact; + + /// + /// The next contact edge in the body's contact list + /// + public ContactEdge Next; + + /// + /// Provides quick access to the other body attached. + /// + public Body Other; + + /// + /// The previous contact edge in the body's contact list + /// + public ContactEdge Prev; + } + + [Flags] + public enum ContactFlags + { + None = 0, + + /// + /// Used when crawling contact graph when forming islands. + /// + Island = 0x0001, + + /// + /// Set when the shapes are touching. + /// + Touching = 0x0002, + + /// + /// This contact can be disabled (by user) + /// + Enabled = 0x0004, + + /// + /// This contact needs filtering because a fixture filter was changed. + /// + Filter = 0x0008, + + /// + /// This bullet contact had a TOI event + /// + BulletHit = 0x0010, + + /// + /// This contact has a valid TOI i the field TOI + /// + TOI = 0x0020 + } + + /// + /// The class manages contact between two shapes. A contact exists for each overlapping + /// AABB in the broad-phase (except if filtered). Therefore a contact object may exist + /// that has no contact points. + /// + public class Contact + { + private static EdgeShape _edge = new EdgeShape(); + + private static ContactType[,] _registers = new[,] + { + { + ContactType.Circle, + ContactType.EdgeAndCircle, + ContactType.PolygonAndCircle, + ContactType.LoopAndCircle, + }, + { + ContactType.EdgeAndCircle, + ContactType.NotSupported, + // 1,1 is invalid (no ContactType.Edge) + ContactType.EdgeAndPolygon, + ContactType.NotSupported, + // 1,3 is invalid (no ContactType.EdgeAndLoop) + }, + { + ContactType.PolygonAndCircle, + ContactType.EdgeAndPolygon, + ContactType.Polygon, + ContactType.LoopAndPolygon, + }, + { + ContactType.LoopAndCircle, + ContactType.NotSupported, + // 3,1 is invalid (no ContactType.EdgeAndLoop) + ContactType.LoopAndPolygon, + ContactType.NotSupported, + // 3,3 is invalid (no ContactType.Loop) + }, + }; + + public Fixture FixtureA; + public Fixture FixtureB; + internal ContactFlags Flags; + + public Manifold Manifold; + + // Nodes for connecting bodies. + internal ContactEdge NodeA = new ContactEdge(); + internal ContactEdge NodeB = new ContactEdge(); + public float TOI; + internal int TOICount; + private ContactType _type; + + private Contact(Fixture fA, int indexA, Fixture fB, int indexB) + { + Reset(fA, indexA, fB, indexB); + } + + /// Enable/disable this contact. This can be used inside the pre-solve + /// contact listener. The contact is only disabled for the current + /// time step (or sub-step in continuous collisions). + public bool Enabled + { + set + { + if (value) + { + Flags |= ContactFlags.Enabled; + } + else + { + Flags &= ~ContactFlags.Enabled; + } + } + + get { return (Flags & ContactFlags.Enabled) == ContactFlags.Enabled; } + } + + /// + /// Get the child primitive index for fixture A. + /// + /// The child index A. + public int ChildIndexA { get; internal set; } + + /// + /// Get the child primitive index for fixture B. + /// + /// The child index B. + public int ChildIndexB { get; internal set; } + + /// + /// Get the contact manifold. Do not modify the manifold unless you understand the + /// internals of Box2D. + /// + /// The manifold. + public void GetManifold(out Manifold manifold) + { + manifold = Manifold; + } + + /// + /// Gets the world manifold. + /// + public void GetWorldManifold(out Vector2 normal, out FixedArray2 points) + { + Body bodyA = FixtureA.Body; + Body bodyB = FixtureB.Body; + Shape shapeA = FixtureA.Shape; + Shape shapeB = FixtureB.Shape; + + Collision.Collision.GetWorldManifold(ref Manifold, ref bodyA.Xf, shapeA.Radius, ref bodyB.Xf, shapeB.Radius, + out normal, out points); + } + + /// + /// Determines whether this contact is touching. + /// + /// + /// true if this instance is touching; otherwise, false. + /// + public bool IsTouching() + { + return (Flags & ContactFlags.Touching) == ContactFlags.Touching; + } + + /// + /// Flag this contact for filtering. Filtering will occur the next time step. + /// + public void FlagForFiltering() + { + Flags |= ContactFlags.Filter; + } + + private void Reset(Fixture fA, int indexA, Fixture fB, int indexB) + { + Flags = ContactFlags.Enabled; + + FixtureA = fA; + FixtureB = fB; + + ChildIndexA = indexA; + ChildIndexB = indexB; + + Manifold.PointCount = 0; + + NodeA.Contact = null; + NodeA.Prev = null; + NodeA.Next = null; + NodeA.Other = null; + + NodeB.Contact = null; + NodeB.Prev = null; + NodeB.Next = null; + NodeB.Other = null; + + TOICount = 0; + } + + /// + /// Update the contact manifold and touching status. + /// Note: do not assume the fixture AABBs are overlapping or are valid. + /// + /// The contact manager. + internal void Update(ContactManager contactManager) + { + Manifold oldManifold = Manifold; + + // Re-enable this contact. + Flags |= ContactFlags.Enabled; + + bool touching; + bool wasTouching = (Flags & ContactFlags.Touching) == ContactFlags.Touching; + + bool sensor = FixtureA.IsSensor || FixtureB.IsSensor; + + Body bodyA = FixtureA.Body; + Body bodyB = FixtureB.Body; + + // Is this contact a sensor? + if (sensor) + { + Shape shapeA = FixtureA.Shape; + Shape shapeB = FixtureB.Shape; + touching = AABB.TestOverlap(shapeA, ChildIndexA, shapeB, ChildIndexB, ref bodyA.Xf, ref bodyB.Xf); + + // Sensors don't generate manifolds. + Manifold.PointCount = 0; + } + else + { + Evaluate(ref Manifold, ref bodyA.Xf, ref bodyB.Xf); + touching = Manifold.PointCount > 0; + + // Match old contact ids to new contact ids and copy the + // stored impulses to warm start the solver. + for (int i = 0; i < Manifold.PointCount; ++i) + { + ManifoldPoint mp2 = Manifold.Points[i]; + mp2.NormalImpulse = 0.0f; + mp2.TangentImpulse = 0.0f; + ContactID id2 = mp2.Id; + bool found = false; + + for (int j = 0; j < oldManifold.PointCount; ++j) + { + ManifoldPoint mp1 = oldManifold.Points[j]; + + if (mp1.Id.Key == id2.Key) + { + mp2.NormalImpulse = mp1.NormalImpulse; + mp2.TangentImpulse = mp1.TangentImpulse; + found = true; + break; + } + } + if (found == false) + { + mp2.NormalImpulse = 0.0f; + mp2.TangentImpulse = 0.0f; + } + + Manifold.Points[i] = mp2; + } + + if (touching != wasTouching) + { + bodyA.Awake = true; + bodyB.Awake = true; + } + } + + if (touching) + { + Flags |= ContactFlags.Touching; + } + else + { + Flags &= ~ContactFlags.Touching; + } + + if (wasTouching == false && touching) + { + //Report the collision to both participants: + if (FixtureA.OnCollision != null) + Enabled = FixtureA.OnCollision(FixtureA, FixtureB, this); + + //Reverse the order of the reported fixtures. The first fixture is always the one that the + //user subscribed to. + if (FixtureB.OnCollision != null) + Enabled = FixtureB.OnCollision(FixtureB, FixtureA, this); + + //BeginContact can also return false and disable the contact + if (contactManager.BeginContact != null) + Enabled = contactManager.BeginContact(this); + + //if the user disabled the contact (needed to exclude it in TOI solver), we also need to mark + //it as not touching. + if (Enabled == false) + Flags &= ~ContactFlags.Touching; + } + + if (wasTouching && touching == false) + { + //Report the separation to both participants: + if (FixtureA != null && FixtureA.OnSeparation != null) + FixtureA.OnSeparation(FixtureA, FixtureB); + + //Reverse the order of the reported fixtures. The first fixture is always the one that the + //user subscribed to. + if (FixtureB != null && FixtureB.OnSeparation != null) + FixtureB.OnSeparation(FixtureB, FixtureA); + + if (contactManager.EndContact != null) + contactManager.EndContact(this); + } + + if (sensor) + return; + + if (contactManager.PreSolve != null) + contactManager.PreSolve(this, ref oldManifold); + } + + /// + /// Evaluate this contact with your own manifold and transforms. + /// + /// The manifold. + /// The first transform. + /// The second transform. + private void Evaluate(ref Manifold manifold, ref Transform transformA, ref Transform transformB) + { + switch (_type) + { + case ContactType.Polygon: + Collision.Collision.CollidePolygons(ref manifold, + (PolygonShape)FixtureA.Shape, ref transformA, + (PolygonShape)FixtureB.Shape, ref transformB); + break; + case ContactType.PolygonAndCircle: + Collision.Collision.CollidePolygonAndCircle(ref manifold, + (PolygonShape)FixtureA.Shape, ref transformA, + (CircleShape)FixtureB.Shape, ref transformB); + break; + case ContactType.EdgeAndCircle: + Collision.Collision.CollideEdgeAndCircle(ref manifold, + (EdgeShape)FixtureA.Shape, ref transformA, + (CircleShape)FixtureB.Shape, ref transformB); + break; + case ContactType.EdgeAndPolygon: + Collision.Collision.CollideEdgeAndPolygon(ref manifold, + (EdgeShape)FixtureA.Shape, ref transformA, + (PolygonShape)FixtureB.Shape, ref transformB); + break; + case ContactType.LoopAndCircle: + LoopShape loop = (LoopShape)FixtureA.Shape; + loop.GetChildEdge(ref _edge, ChildIndexA); + Collision.Collision.CollideEdgeAndCircle(ref manifold, _edge, ref transformA, + (CircleShape)FixtureB.Shape, ref transformB); + break; + case ContactType.LoopAndPolygon: + LoopShape loop2 = (LoopShape)FixtureA.Shape; + loop2.GetChildEdge(ref _edge, ChildIndexA); + Collision.Collision.CollideEdgeAndPolygon(ref manifold, _edge, ref transformA, + (PolygonShape)FixtureB.Shape, ref transformB); + break; + case ContactType.Circle: + Collision.Collision.CollideCircles(ref manifold, + (CircleShape)FixtureA.Shape, ref transformA, + (CircleShape)FixtureB.Shape, ref transformB); + break; + } + } + + internal static Contact Create(Fixture fixtureA, int indexA, Fixture fixtureB, int indexB) + { + ShapeType type1 = fixtureA.ShapeType; + ShapeType type2 = fixtureB.ShapeType; + + Debug.Assert(ShapeType.Unknown < type1 && type1 < ShapeType.TypeCount); + Debug.Assert(ShapeType.Unknown < type2 && type2 < ShapeType.TypeCount); + + Contact c; + Queue pool = fixtureA.Body.World.ContactPool; + if (pool.Count > 0) + { + c = pool.Dequeue(); + if ((type1 >= type2 || (type1 == ShapeType.Edge && type2 == ShapeType.Polygon)) + && + !(type2 == ShapeType.Edge && type1 == ShapeType.Polygon)) + { + c.Reset(fixtureA, indexA, fixtureB, indexB); + } + else + { + c.Reset(fixtureB, indexB, fixtureA, indexA); + } + } + else + { + // Edge+Polygon is non-symetrical due to the way Erin handles collision type registration. + if ((type1 >= type2 || (type1 == ShapeType.Edge && type2 == ShapeType.Polygon)) + && + !(type2 == ShapeType.Edge && type1 == ShapeType.Polygon)) + { + c = new Contact(fixtureA, indexA, fixtureB, indexB); + } + else + { + c = new Contact(fixtureB, indexB, fixtureA, indexA); + } + } + + c._type = _registers[(int)type1, (int)type2]; + + return c; + } + + internal void Destroy() + { + FixtureA.Body.World.ContactPool.Enqueue(this); + Reset(null, 0, null, 0); + } + + #region Nested type: ContactType + + private enum ContactType + { + NotSupported, + Polygon, + PolygonAndCircle, + Circle, + EdgeAndPolygon, + EdgeAndCircle, + LoopAndPolygon, + LoopAndCircle, + } + + #endregion + } +} \ No newline at end of file diff --git a/Dynamics/Contacts/ContactSolver.cs b/Dynamics/Contacts/ContactSolver.cs new file mode 100644 index 0000000..86b6a03 --- /dev/null +++ b/Dynamics/Contacts/ContactSolver.cs @@ -0,0 +1,794 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using FarseerPhysics.Collision; +using FarseerPhysics.Collision.Shapes; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Contacts +{ + public sealed class ContactConstraintPoint + { + public Vector2 LocalPoint; + public float NormalImpulse; + public float NormalMass; + public float TangentImpulse; + public float TangentMass; + public float VelocityBias; + public Vector2 rA; + public Vector2 rB; + } + + public sealed class ContactConstraint + { + public Body BodyA; + public Body BodyB; + public float Friction; + public Mat22 K; + public Vector2 LocalNormal; + public Vector2 LocalPoint; + public Manifold Manifold; + public Vector2 Normal; + public Mat22 NormalMass; + public int PointCount; + public ContactConstraintPoint[] Points = new ContactConstraintPoint[Settings.MaxPolygonVertices]; + public float RadiusA; + public float RadiusB; + public float Restitution; + public ManifoldType Type; + + public ContactConstraint() + { + for (int i = 0; i < Settings.MaxManifoldPoints; i++) + { + Points[i] = new ContactConstraintPoint(); + } + } + } + + public class ContactSolver + { + public ContactConstraint[] Constraints; + private int _constraintCount; // collection can be bigger. + private Contact[] _contacts; + + public void Reset(Contact[] contacts, int contactCount, float impulseRatio, bool warmstarting) + { + _contacts = contacts; + + _constraintCount = contactCount; + + // grow the array + if (Constraints == null || Constraints.Length < _constraintCount) + { + Constraints = new ContactConstraint[_constraintCount * 2]; + + for (int i = 0; i < Constraints.Length; i++) + { + Constraints[i] = new ContactConstraint(); + } + } + + // Initialize position independent portions of the constraints. + for (int i = 0; i < _constraintCount; ++i) + { + Contact contact = contacts[i]; + + Fixture fixtureA = contact.FixtureA; + Fixture fixtureB = contact.FixtureB; + Shape shapeA = fixtureA.Shape; + Shape shapeB = fixtureB.Shape; + float radiusA = shapeA.Radius; + float radiusB = shapeB.Radius; + Body bodyA = fixtureA.Body; + Body bodyB = fixtureB.Body; + Manifold manifold = contact.Manifold; + + Debug.Assert(manifold.PointCount > 0); + + ContactConstraint cc = Constraints[i]; + cc.Friction = Settings.MixFriction(fixtureA.Friction, fixtureB.Friction); + cc.Restitution = Settings.MixRestitution(fixtureA.Restitution, fixtureB.Restitution); + cc.BodyA = bodyA; + cc.BodyB = bodyB; + cc.Manifold = manifold; + cc.Normal = Vector2.Zero; + cc.PointCount = manifold.PointCount; + + cc.LocalNormal = manifold.LocalNormal; + cc.LocalPoint = manifold.LocalPoint; + cc.RadiusA = radiusA; + cc.RadiusB = radiusB; + cc.Type = manifold.Type; + + for (int j = 0; j < cc.PointCount; ++j) + { + ManifoldPoint cp = manifold.Points[j]; + ContactConstraintPoint ccp = cc.Points[j]; + + if (warmstarting) + { + ccp.NormalImpulse = impulseRatio * cp.NormalImpulse; + ccp.TangentImpulse = impulseRatio * cp.TangentImpulse; + } + else + { + ccp.NormalImpulse = 0.0f; + ccp.TangentImpulse = 0.0f; + } + + ccp.LocalPoint = cp.LocalPoint; + ccp.rA = Vector2.Zero; + ccp.rB = Vector2.Zero; + ccp.NormalMass = 0.0f; + ccp.TangentMass = 0.0f; + ccp.VelocityBias = 0.0f; + } + + cc.K.SetZero(); + cc.NormalMass.SetZero(); + } + } + + public void InitializeVelocityConstraints() + { + for (int i = 0; i < _constraintCount; ++i) + { + ContactConstraint cc = Constraints[i]; + + float radiusA = cc.RadiusA; + float radiusB = cc.RadiusB; + Body bodyA = cc.BodyA; + Body bodyB = cc.BodyB; + Manifold manifold = cc.Manifold; + + Vector2 vA = bodyA.LinearVelocity; + Vector2 vB = bodyB.LinearVelocity; + float wA = bodyA.AngularVelocity; + float wB = bodyB.AngularVelocity; + + Debug.Assert(manifold.PointCount > 0); + FixedArray2 points; + + Collision.Collision.GetWorldManifold(ref manifold, ref bodyA.Xf, radiusA, ref bodyB.Xf, radiusB, + out cc.Normal, out points); + Vector2 tangent = new Vector2(cc.Normal.Y, -cc.Normal.X); + + for (int j = 0; j < cc.PointCount; ++j) + { + ContactConstraintPoint ccp = cc.Points[j]; + + ccp.rA = points[j] - bodyA.Sweep.C; + ccp.rB = points[j] - bodyB.Sweep.C; + + float rnA = ccp.rA.X * cc.Normal.Y - ccp.rA.Y * cc.Normal.X; + float rnB = ccp.rB.X * cc.Normal.Y - ccp.rB.Y * cc.Normal.X; + rnA *= rnA; + rnB *= rnB; + + float kNormal = bodyA.InvMass + bodyB.InvMass + bodyA.InvI * rnA + bodyB.InvI * rnB; + + Debug.Assert(kNormal > Settings.Epsilon); + ccp.NormalMass = 1.0f / kNormal; + + float rtA = ccp.rA.X * tangent.Y - ccp.rA.Y * tangent.X; + float rtB = ccp.rB.X * tangent.Y - ccp.rB.Y * tangent.X; + + rtA *= rtA; + rtB *= rtB; + float kTangent = bodyA.InvMass + bodyB.InvMass + bodyA.InvI * rtA + bodyB.InvI * rtB; + + Debug.Assert(kTangent > Settings.Epsilon); + ccp.TangentMass = 1.0f / kTangent; + + // Setup a velocity bias for restitution. + ccp.VelocityBias = 0.0f; + float vRel = cc.Normal.X * (vB.X + -wB * ccp.rB.Y - vA.X - -wA * ccp.rA.Y) + + cc.Normal.Y * (vB.Y + wB * ccp.rB.X - vA.Y - wA * ccp.rA.X); + if (vRel < -Settings.VelocityThreshold) + { + ccp.VelocityBias = -cc.Restitution * vRel; + } + } + + // If we have two points, then prepare the block solver. + if (cc.PointCount == 2) + { + ContactConstraintPoint ccp1 = cc.Points[0]; + ContactConstraintPoint ccp2 = cc.Points[1]; + + float invMassA = bodyA.InvMass; + float invIA = bodyA.InvI; + float invMassB = bodyB.InvMass; + float invIB = bodyB.InvI; + + float rn1A = ccp1.rA.X * cc.Normal.Y - ccp1.rA.Y * cc.Normal.X; + float rn1B = ccp1.rB.X * cc.Normal.Y - ccp1.rB.Y * cc.Normal.X; + float rn2A = ccp2.rA.X * cc.Normal.Y - ccp2.rA.Y * cc.Normal.X; + float rn2B = ccp2.rB.X * cc.Normal.Y - ccp2.rB.Y * cc.Normal.X; + + float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B; + float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B; + float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B; + + // Ensure a reasonable condition number. + const float k_maxConditionNumber = 100.0f; + if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) + { + // K is safe to invert. + cc.K.Col1.X = k11; + cc.K.Col1.Y = k12; + cc.K.Col2.X = k12; + cc.K.Col2.Y = k22; + + float a = cc.K.Col1.X, b = cc.K.Col2.X, c = cc.K.Col1.Y, d = cc.K.Col2.Y; + float det = a * d - b * c; + if (det != 0.0f) + { + det = 1.0f / det; + } + + cc.NormalMass.Col1.X = det * d; + cc.NormalMass.Col1.Y = -det * c; + cc.NormalMass.Col2.X = -det * b; + cc.NormalMass.Col2.Y = det * a; + } + else + { + // The constraints are redundant, just use one. + // TODO_ERIN use deepest? + cc.PointCount = 1; + } + } + } + } + + public void WarmStart() + { + // Warm start. + for (int i = 0; i < _constraintCount; ++i) + { + ContactConstraint c = Constraints[i]; + + float tangentx = c.Normal.Y; + float tangenty = -c.Normal.X; + + for (int j = 0; j < c.PointCount; ++j) + { + ContactConstraintPoint ccp = c.Points[j]; + float px = ccp.NormalImpulse * c.Normal.X + ccp.TangentImpulse * tangentx; + float py = ccp.NormalImpulse * c.Normal.Y + ccp.TangentImpulse * tangenty; + c.BodyA.AngularVelocityInternal -= c.BodyA.InvI * (ccp.rA.X * py - ccp.rA.Y * px); + c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * px; + c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * py; + c.BodyB.AngularVelocityInternal += c.BodyB.InvI * (ccp.rB.X * py - ccp.rB.Y * px); + c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * px; + c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * py; + } + } + } + + public void SolveVelocityConstraints() + { + for (int i = 0; i < _constraintCount; ++i) + { + ContactConstraint c = Constraints[i]; + float wA = c.BodyA.AngularVelocityInternal; + float wB = c.BodyB.AngularVelocityInternal; + + float tangentx = c.Normal.Y; + float tangenty = -c.Normal.X; + + float friction = c.Friction; + + Debug.Assert(c.PointCount == 1 || c.PointCount == 2); + + // Solve tangent constraints + for (int j = 0; j < c.PointCount; ++j) + { + ContactConstraintPoint ccp = c.Points[j]; + float lambda = ccp.TangentMass * + -((c.BodyB.LinearVelocityInternal.X + (-wB * ccp.rB.Y) - + c.BodyA.LinearVelocityInternal.X - (-wA * ccp.rA.Y)) * tangentx + + (c.BodyB.LinearVelocityInternal.Y + (wB * ccp.rB.X) - + c.BodyA.LinearVelocityInternal.Y - (wA * ccp.rA.X)) * tangenty); + + // MathUtils.Clamp the accumulated force + float maxFriction = friction * ccp.NormalImpulse; + float newImpulse = Math.Max(-maxFriction, Math.Min(ccp.TangentImpulse + lambda, maxFriction)); + lambda = newImpulse - ccp.TangentImpulse; + + // Apply contact impulse + float px = lambda * tangentx; + float py = lambda * tangenty; + + c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * px; + c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * py; + wA -= c.BodyA.InvI * (ccp.rA.X * py - ccp.rA.Y * px); + + c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * px; + c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * py; + wB += c.BodyB.InvI * (ccp.rB.X * py - ccp.rB.Y * px); + + ccp.TangentImpulse = newImpulse; + } + + // Solve normal constraints + if (c.PointCount == 1) + { + ContactConstraintPoint ccp = c.Points[0]; + + // Relative velocity at contact + // Compute normal impulse + float lambda = -ccp.NormalMass * + ((c.BodyB.LinearVelocityInternal.X + (-wB * ccp.rB.Y) - + c.BodyA.LinearVelocityInternal.X - (-wA * ccp.rA.Y)) * c.Normal.X + + (c.BodyB.LinearVelocityInternal.Y + (wB * ccp.rB.X) - + c.BodyA.LinearVelocityInternal.Y - + (wA * ccp.rA.X)) * c.Normal.Y - ccp.VelocityBias); + + // Clamp the accumulated impulse + float newImpulse = Math.Max(ccp.NormalImpulse + lambda, 0.0f); + lambda = newImpulse - ccp.NormalImpulse; + + // Apply contact impulse + float px = lambda * c.Normal.X; + float py = lambda * c.Normal.Y; + + c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * px; + c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * py; + wA -= c.BodyA.InvI * (ccp.rA.X * py - ccp.rA.Y * px); + + c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * px; + c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * py; + wB += c.BodyB.InvI * (ccp.rB.X * py - ccp.rB.Y * px); + + ccp.NormalImpulse = newImpulse; + } + else + { + // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite). + // Build the mini LCP for this contact patch + // + // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2 + // + // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n ) + // b = vn_0 - velocityBias + // + // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i + // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases + // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid + // solution that satisfies the problem is chosen. + // + // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires + // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i). + // + // Substitute: + // + // x = x' - a + // + // Plug into above equation: + // + // vn = A * x + b + // = A * (x' - a) + b + // = A * x' + b - A * a + // = A * x' + b' + // b' = b - A * a; + + ContactConstraintPoint cp1 = c.Points[0]; + ContactConstraintPoint cp2 = c.Points[1]; + + float ax = cp1.NormalImpulse; + float ay = cp2.NormalImpulse; + Debug.Assert(ax >= 0.0f && ay >= 0.0f); + + // Relative velocity at contact + // Compute normal velocity + float vn1 = (c.BodyB.LinearVelocityInternal.X + (-wB * cp1.rB.Y) - c.BodyA.LinearVelocityInternal.X - + (-wA * cp1.rA.Y)) * c.Normal.X + + (c.BodyB.LinearVelocityInternal.Y + (wB * cp1.rB.X) - c.BodyA.LinearVelocityInternal.Y - + (wA * cp1.rA.X)) * c.Normal.Y; + float vn2 = (c.BodyB.LinearVelocityInternal.X + (-wB * cp2.rB.Y) - c.BodyA.LinearVelocityInternal.X - + (-wA * cp2.rA.Y)) * c.Normal.X + + (c.BodyB.LinearVelocityInternal.Y + (wB * cp2.rB.X) - c.BodyA.LinearVelocityInternal.Y - + (wA * cp2.rA.X)) * c.Normal.Y; + + float bx = vn1 - cp1.VelocityBias - (c.K.Col1.X * ax + c.K.Col2.X * ay); + float by = vn2 - cp2.VelocityBias - (c.K.Col1.Y * ax + c.K.Col2.Y * ay); + + float xx = -(c.NormalMass.Col1.X * bx + c.NormalMass.Col2.X * by); + float xy = -(c.NormalMass.Col1.Y * bx + c.NormalMass.Col2.Y * by); + + while (true) + { + // + // Case 1: vn = 0 + // + // 0 = A * x' + b' + // + // Solve for x': + // + // x' = - inv(A) * b' + // + if (xx >= 0.0f && xy >= 0.0f) + { + // Resubstitute for the incremental impulse + float dx = xx - ax; + float dy = xy - ay; + + // Apply incremental impulse + float p1x = dx * c.Normal.X; + float p1y = dx * c.Normal.Y; + + float p2x = dy * c.Normal.X; + float p2y = dy * c.Normal.Y; + + float p12x = p1x + p2x; + float p12y = p1y + p2y; + + c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x; + c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y; + wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x)); + + c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x; + c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y; + wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x)); + + // Accumulate + cp1.NormalImpulse = xx; + cp2.NormalImpulse = xy; + +#if B2_DEBUG_SOLVER + + float k_errorTol = 1e-3f; + + // Postconditions + dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA); + dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA); + + // Compute normal velocity + vn1 = Vector2.Dot(dv1, normal); + vn2 = Vector2.Dot(dv2, normal); + + Debug.Assert(MathUtils.Abs(vn1 - cp1.velocityBias) < k_errorTol); + Debug.Assert(MathUtils.Abs(vn2 - cp2.velocityBias) < k_errorTol); +#endif + break; + } + + // + // Case 2: vn1 = 0 and x2 = 0 + // + // 0 = a11 * x1' + a12 * 0 + b1' + // vn2 = a21 * x1' + a22 * 0 + b2' + // + xx = -cp1.NormalMass * bx; + xy = 0.0f; + vn1 = 0.0f; + vn2 = c.K.Col1.Y * xx + by; + + if (xx >= 0.0f && vn2 >= 0.0f) + { + // Resubstitute for the incremental impulse + float dx = xx - ax; + float dy = xy - ay; + + // Apply incremental impulse + float p1x = dx * c.Normal.X; + float p1y = dx * c.Normal.Y; + + float p2x = dy * c.Normal.X; + float p2y = dy * c.Normal.Y; + + float p12x = p1x + p2x; + float p12y = p1y + p2y; + + c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x; + c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y; + wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x)); + + c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x; + c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y; + wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x)); + + // Accumulate + cp1.NormalImpulse = xx; + cp2.NormalImpulse = xy; + +#if B2_DEBUG_SOLVER + // Postconditions + dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA); + + // Compute normal velocity + vn1 = Vector2.Dot(dv1, normal); + + Debug.Assert(MathUtils.Abs(vn1 - cp1.velocityBias) < k_errorTol); +#endif + break; + } + + + // + // Case 3: vn2 = 0 and x1 = 0 + // + // vn1 = a11 * 0 + a12 * x2' + b1' + // 0 = a21 * 0 + a22 * x2' + b2' + // + xx = 0.0f; + xy = -cp2.NormalMass * by; + vn1 = c.K.Col2.X * xy + bx; + vn2 = 0.0f; + + if (xy >= 0.0f && vn1 >= 0.0f) + { + // Resubstitute for the incremental impulse + float dx = xx - ax; + float dy = xy - ay; + + // Apply incremental impulse + float p1x = dx * c.Normal.X; + float p1y = dx * c.Normal.Y; + + float p2x = dy * c.Normal.X; + float p2y = dy * c.Normal.Y; + + float p12x = p1x + p2x; + float p12y = p1y + p2y; + + c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x; + c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y; + wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x)); + + c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x; + c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y; + wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x)); + + // Accumulate + cp1.NormalImpulse = xx; + cp2.NormalImpulse = xy; + +#if B2_DEBUG_SOLVER + // Postconditions + dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA); + + // Compute normal velocity + vn2 = Vector2.Dot(dv2, normal); + + Debug.Assert(MathUtils.Abs(vn2 - cp2.velocityBias) < k_errorTol); +#endif + break; + } + + // + // Case 4: x1 = 0 and x2 = 0 + // + // vn1 = b1 + // vn2 = b2; + xx = 0.0f; + xy = 0.0f; + vn1 = bx; + vn2 = by; + + if (vn1 >= 0.0f && vn2 >= 0.0f) + { + // Resubstitute for the incremental impulse + float dx = xx - ax; + float dy = xy - ay; + + // Apply incremental impulse + float p1x = dx * c.Normal.X; + float p1y = dx * c.Normal.Y; + + float p2x = dy * c.Normal.X; + float p2y = dy * c.Normal.Y; + + float p12x = p1x + p2x; + float p12y = p1y + p2y; + + c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x; + c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y; + wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x)); + + c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x; + c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y; + wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x)); + + // Accumulate + cp1.NormalImpulse = xx; + cp2.NormalImpulse = xy; + + break; + } + + // No solution, give up. This is hit sometimes, but it doesn't seem to matter. + break; + } + } + + c.BodyA.AngularVelocityInternal = wA; + c.BodyB.AngularVelocityInternal = wB; + } + } + + public void StoreImpulses() + { + for (int i = 0; i < _constraintCount; ++i) + { + ContactConstraint c = Constraints[i]; + Manifold m = c.Manifold; + + for (int j = 0; j < c.PointCount; ++j) + { + ManifoldPoint pj = m.Points[j]; + ContactConstraintPoint cp = c.Points[j]; + + pj.NormalImpulse = cp.NormalImpulse; + pj.TangentImpulse = cp.TangentImpulse; + + m.Points[j] = pj; + } + + c.Manifold = m; + _contacts[i].Manifold = m; + } + } + + public bool SolvePositionConstraints(float baumgarte) + { + float minSeparation = 0.0f; + + for (int i = 0; i < _constraintCount; ++i) + { + ContactConstraint c = Constraints[i]; + + Body bodyA = c.BodyA; + Body bodyB = c.BodyB; + + float invMassA = bodyA.Mass * bodyA.InvMass; + float invIA = bodyA.Mass * bodyA.InvI; + float invMassB = bodyB.Mass * bodyB.InvMass; + float invIB = bodyB.Mass * bodyB.InvI; + + // Solve normal constraints + for (int j = 0; j < c.PointCount; ++j) + { + Vector2 normal; + Vector2 point; + float separation; + + Solve(c, j, out normal, out point, out separation); + + float rax = point.X - bodyA.Sweep.C.X; + float ray = point.Y - bodyA.Sweep.C.Y; + + float rbx = point.X - bodyB.Sweep.C.X; + float rby = point.Y - bodyB.Sweep.C.Y; + + // Track max constraint error. + minSeparation = Math.Min(minSeparation, separation); + + // Prevent large corrections and allow slop. + float C = Math.Max(-Settings.MaxLinearCorrection, + Math.Min(baumgarte * (separation + Settings.LinearSlop), 0.0f)); + + // Compute the effective mass. + float rnA = rax * normal.Y - ray * normal.X; + float rnB = rbx * normal.Y - rby * normal.X; + float K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB; + + // Compute normal impulse + float impulse = K > 0.0f ? -C / K : 0.0f; + + float px = impulse * normal.X; + float py = impulse * normal.Y; + + bodyA.Sweep.C.X -= invMassA * px; + bodyA.Sweep.C.Y -= invMassA * py; + bodyA.Sweep.A -= invIA * (rax * py - ray * px); + + bodyB.Sweep.C.X += invMassB * px; + bodyB.Sweep.C.Y += invMassB * py; + bodyB.Sweep.A += invIB * (rbx * py - rby * px); + + bodyA.SynchronizeTransform(); + bodyB.SynchronizeTransform(); + } + } + + // We can't expect minSpeparation >= -Settings.b2_linearSlop because we don't + // push the separation above -Settings.b2_linearSlop. + return minSeparation >= -1.5f * Settings.LinearSlop; + } + + private static void Solve(ContactConstraint cc, int index, out Vector2 normal, out Vector2 point, + out float separation) + { + Debug.Assert(cc.PointCount > 0); + + normal = Vector2.Zero; + + switch (cc.Type) + { + case ManifoldType.Circles: + { + Vector2 pointA = cc.BodyA.GetWorldPoint(ref cc.LocalPoint); + Vector2 pointB = cc.BodyB.GetWorldPoint(ref cc.Points[0].LocalPoint); + float a = (pointA.X - pointB.X) * (pointA.X - pointB.X) + + (pointA.Y - pointB.Y) * (pointA.Y - pointB.Y); + if (a > Settings.Epsilon * Settings.Epsilon) + { + Vector2 normalTmp = pointB - pointA; + float factor = 1f / (float)Math.Sqrt(normalTmp.X * normalTmp.X + normalTmp.Y * normalTmp.Y); + normal.X = normalTmp.X * factor; + normal.Y = normalTmp.Y * factor; + } + else + { + normal.X = 1; + normal.Y = 0; + } + + point = 0.5f * (pointA + pointB); + separation = (pointB.X - pointA.X) * normal.X + (pointB.Y - pointA.Y) * normal.Y - cc.RadiusA - + cc.RadiusB; + } + break; + + case ManifoldType.FaceA: + { + normal = cc.BodyA.GetWorldVector(ref cc.LocalNormal); + Vector2 planePoint = cc.BodyA.GetWorldPoint(ref cc.LocalPoint); + Vector2 clipPoint = cc.BodyB.GetWorldPoint(ref cc.Points[index].LocalPoint); + separation = (clipPoint.X - planePoint.X) * normal.X + (clipPoint.Y - planePoint.Y) * normal.Y - + cc.RadiusA - cc.RadiusB; + point = clipPoint; + } + break; + + case ManifoldType.FaceB: + { + normal = cc.BodyB.GetWorldVector(ref cc.LocalNormal); + Vector2 planePoint = cc.BodyB.GetWorldPoint(ref cc.LocalPoint); + + Vector2 clipPoint = cc.BodyA.GetWorldPoint(ref cc.Points[index].LocalPoint); + separation = (clipPoint.X - planePoint.X) * normal.X + (clipPoint.Y - planePoint.Y) * normal.Y - + cc.RadiusA - cc.RadiusB; + point = clipPoint; + + // Ensure normal points from A to B + normal = -normal; + } + break; + default: + point = Vector2.Zero; + separation = 0.0f; + break; + } + } + } +} \ No newline at end of file diff --git a/Dynamics/Fixture.cs b/Dynamics/Fixture.cs new file mode 100644 index 0000000..21112ae --- /dev/null +++ b/Dynamics/Fixture.cs @@ -0,0 +1,611 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Diagnostics; +using FarseerPhysics.Collision; +using FarseerPhysics.Collision.Shapes; +using FarseerPhysics.Common; +using FarseerPhysics.Dynamics.Contacts; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics +{ + [Flags] + public enum Category + { + None = 0, + All = int.MaxValue, + Cat1 = 1, + Cat2 = 2, + Cat3 = 4, + Cat4 = 8, + Cat5 = 16, + Cat6 = 32, + Cat7 = 64, + Cat8 = 128, + Cat9 = 256, + Cat10 = 512, + Cat11 = 1024, + Cat12 = 2048, + Cat13 = 4096, + Cat14 = 8192, + Cat15 = 16384, + Cat16 = 32768, + Cat17 = 65536, + Cat18 = 131072, + Cat19 = 262144, + Cat20 = 524288, + Cat21 = 1048576, + Cat22 = 2097152, + Cat23 = 4194304, + Cat24 = 8388608, + Cat25 = 16777216, + Cat26 = 33554432, + Cat27 = 67108864, + Cat28 = 134217728, + Cat29 = 268435456, + Cat30 = 536870912, + Cat31 = 1073741824 + } + + /// + /// This proxy is used internally to connect fixtures to the broad-phase. + /// + public struct FixtureProxy + { + public AABB AABB; + public int ChildIndex; + public Fixture Fixture; + public int ProxyId; + } + + /// + /// A fixture is used to attach a Shape to a body for collision detection. A fixture + /// inherits its transform from its parent. Fixtures hold additional non-geometric data + /// such as friction, collision filters, etc. + /// Fixtures are created via Body.CreateFixture. + /// Warning: You cannot reuse fixtures. + /// + public class Fixture : IDisposable + { + private static int _fixtureIdCounter; + + /// + /// Fires after two shapes has collided and are solved. This gives you a chance to get the impact force. + /// + public AfterCollisionEventHandler AfterCollision; + + /// + /// Fires when two fixtures are close to each other. + /// Due to how the broadphase works, this can be quite inaccurate as shapes are approximated using AABBs. + /// + public BeforeCollisionEventHandler BeforeCollision; + + /// + /// Fires when two shapes collide and a contact is created between them. + /// Note that the first fixture argument is always the fixture that the delegate is subscribed to. + /// + public OnCollisionEventHandler OnCollision; + + /// + /// Fires when two shapes separate and a contact is removed between them. + /// Note that the first fixture argument is always the fixture that the delegate is subscribed to. + /// + public OnSeparationEventHandler OnSeparation; + + public FixtureProxy[] Proxies; + public int ProxyCount; + internal Category _collidesWith; + internal Category _collisionCategories; + internal short _collisionGroup; + internal Dictionary _collisionIgnores; + private float _friction; + private float _restitution; + + internal Fixture() + { + } + + public Fixture(Body body, Shape shape) + : this(body, shape, null) + { + } + + public Fixture(Body body, Shape shape, object userData) + { + if (Settings.UseFPECollisionCategories) + _collisionCategories = Category.All; + else + _collisionCategories = Category.Cat1; + + _collidesWith = Category.All; + _collisionGroup = 0; + + //Fixture defaults + Friction = 0.2f; + Restitution = 0; + + IsSensor = false; + + Body = body; + UserData = userData; + +#pragma warning disable 162 + if (Settings.ConserveMemory) + Shape = shape; + else + Shape = shape.Clone(); +#pragma warning restore 162 + + RegisterFixture(); + } + + /// + /// 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. + /// + public short CollisionGroup + { + set + { + if (_collisionGroup == value) + return; + + _collisionGroup = value; + Refilter(); + } + get { return _collisionGroup; } + } + + /// + /// Defaults to Category.All + /// + /// The collision mask bits. This states the categories that this + /// fixture would accept for collision. + /// Use Settings.UseFPECollisionCategories to change the behavior. + /// + public Category CollidesWith + { + get { return _collidesWith; } + + set + { + if (_collidesWith == value) + return; + + _collidesWith = value; + Refilter(); + } + } + + /// + /// 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 + /// + public Category CollisionCategories + { + get { return _collisionCategories; } + + set + { + if (_collisionCategories == value) + return; + + _collisionCategories = value; + Refilter(); + } + } + + /// + /// Get the type of the child Shape. You can use this to down cast to the concrete Shape. + /// + /// The type of the shape. + public ShapeType ShapeType + { + get { return Shape.ShapeType; } + } + + /// + /// 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. + /// + /// The shape. + public Shape Shape { get; internal set; } + + /// + /// Gets or sets a value indicating whether this fixture is a sensor. + /// + /// true if this instance is a sensor; otherwise, false. + public bool IsSensor { get; set; } + + /// + /// Get the parent body of this fixture. This is null if the fixture is not attached. + /// + /// The body. + public Body Body { get; internal set; } + + /// + /// Set the user data. Use this to store your application specific data. + /// + /// The user data. + public object UserData { get; set; } + + /// + /// Get or set the coefficient of friction. + /// + /// The friction. + public float Friction + { + get { return _friction; } + set + { + Debug.Assert(!float.IsNaN(value)); + + _friction = value; + } + } + + /// + /// Get or set the coefficient of restitution. + /// + /// The restitution. + public float Restitution + { + get { return _restitution; } + set + { + Debug.Assert(!float.IsNaN(value)); + + _restitution = value; + } + } + + /// + /// Gets a unique ID for this fixture. + /// + /// The fixture id. + public int FixtureId { get; private set; } + + #region IDisposable Members + + public bool IsDisposed { get; set; } + + public void Dispose() + { + if (!IsDisposed) + { + Body.DestroyFixture(this); + IsDisposed = true; + GC.SuppressFinalize(this); + } + } + + #endregion + + /// + /// Restores collisions between this fixture and the provided fixture. + /// + /// The fixture. + public void RestoreCollisionWith(Fixture fixture) + { + if (_collisionIgnores == null) + return; + + if (_collisionIgnores.ContainsKey(fixture.FixtureId)) + { + _collisionIgnores[fixture.FixtureId] = false; + Refilter(); + } + } + + /// + /// Ignores collisions between this fixture and the provided fixture. + /// + /// The fixture. + public void IgnoreCollisionWith(Fixture fixture) + { + if (_collisionIgnores == null) + _collisionIgnores = new Dictionary(); + + if (_collisionIgnores.ContainsKey(fixture.FixtureId)) + _collisionIgnores[fixture.FixtureId] = true; + else + _collisionIgnores.Add(fixture.FixtureId, true); + + Refilter(); + } + + /// + /// Determines whether collisions are ignored between this fixture and the provided fixture. + /// + /// The fixture. + /// + /// true if the fixture is ignored; otherwise, false. + /// + public bool IsFixtureIgnored(Fixture fixture) + { + if (_collisionIgnores == null) + return false; + + if (_collisionIgnores.ContainsKey(fixture.FixtureId)) + return _collisionIgnores[fixture.FixtureId]; + + return false; + } + + /// + /// Contacts are persistant and will keep being persistant unless they are + /// flagged for filtering. + /// This methods flags all contacts associated with the body for filtering. + /// + internal void Refilter() + { + // Flag associated contacts for filtering. + ContactEdge edge = Body.ContactList; + while (edge != null) + { + Contact contact = edge.Contact; + Fixture fixtureA = contact.FixtureA; + Fixture fixtureB = contact.FixtureB; + if (fixtureA == this || fixtureB == this) + { + contact.FlagForFiltering(); + } + + edge = edge.Next; + } + + World world = Body.World; + + if (world == null) + { + return; + } + + // Touch each proxy so that new pairs may be created + IBroadPhase broadPhase = world.ContactManager.BroadPhase; + for (int i = 0; i < ProxyCount; ++i) + { + broadPhase.TouchProxy(Proxies[i].ProxyId); + } + } + + private void RegisterFixture() + { + // Reserve proxy space + Proxies = new FixtureProxy[Shape.ChildCount]; + ProxyCount = 0; + + FixtureId = _fixtureIdCounter++; + + if ((Body.Flags & BodyFlags.Enabled) == BodyFlags.Enabled) + { + IBroadPhase broadPhase = Body.World.ContactManager.BroadPhase; + CreateProxies(broadPhase, ref Body.Xf); + } + + Body.FixtureList.Add(this); + + // Adjust mass properties if needed. + if (Shape._density > 0.0f) + { + Body.ResetMassData(); + } + + // Let the world know we have a new fixture. This will cause new contacts + // to be created at the beginning of the next time step. + Body.World.Flags |= WorldFlags.NewFixture; + + if (Body.World.FixtureAdded != null) + { + Body.World.FixtureAdded(this); + } + } + + /// + /// Test a point for containment in this fixture. + /// + /// A point in world coordinates. + /// + public bool TestPoint(ref Vector2 point) + { + return Shape.TestPoint(ref Body.Xf, ref point); + } + + /// + /// Cast a ray against this Shape. + /// + /// The ray-cast results. + /// The ray-cast input parameters. + /// Index of the child. + /// + public bool RayCast(out RayCastOutput output, ref RayCastInput input, int childIndex) + { + return Shape.RayCast(out output, ref input, ref Body.Xf, childIndex); + } + + /// + /// Get the fixture's AABB. This AABB may be enlarge and/or stale. + /// If you need a more accurate AABB, compute it using the Shape and + /// the body transform. + /// + /// The aabb. + /// Index of the child. + public void GetAABB(out AABB aabb, int childIndex) + { + Debug.Assert(0 <= childIndex && childIndex < ProxyCount); + aabb = Proxies[childIndex].AABB; + } + + public Fixture Clone(Body body) + { + Fixture fixture = new Fixture(); + fixture.Body = body; + +#pragma warning disable 162 + if (Settings.ConserveMemory) + fixture.Shape = Shape; + else + fixture.Shape = Shape.Clone(); +#pragma warning restore 162 + + fixture.UserData = UserData; + fixture.Restitution = Restitution; + fixture.Friction = Friction; + fixture.IsSensor = IsSensor; + fixture._collisionGroup = CollisionGroup; + fixture._collisionCategories = CollisionCategories; + fixture._collidesWith = CollidesWith; + + if (_collisionIgnores != null) + { + fixture._collisionIgnores = new Dictionary(); + + foreach (KeyValuePair pair in _collisionIgnores) + { + fixture._collisionIgnores.Add(pair.Key, pair.Value); + } + } + + fixture.RegisterFixture(); + return fixture; + } + + public Fixture DeepClone() + { + Fixture fix = Clone(Body.Clone()); + return fix; + } + + internal void Destroy() + { + // The proxies must be destroyed before calling this. + Debug.Assert(ProxyCount == 0); + + // Free the proxy array. + Proxies = null; + Shape = null; + + BeforeCollision = null; + OnCollision = null; + OnSeparation = null; + AfterCollision = null; + + if (Body.World.FixtureRemoved != null) + { + Body.World.FixtureRemoved(this); + } + + Body.World.FixtureAdded = null; + Body.World.FixtureRemoved = null; + OnSeparation = null; + OnCollision = null; + } + + // These support body activation/deactivation. + internal void CreateProxies(IBroadPhase broadPhase, ref Transform xf) + { + Debug.Assert(ProxyCount == 0); + + // Create proxies in the broad-phase. + ProxyCount = Shape.ChildCount; + + for (int i = 0; i < ProxyCount; ++i) + { + FixtureProxy proxy = new FixtureProxy(); + Shape.ComputeAABB(out proxy.AABB, ref xf, i); + + proxy.Fixture = this; + proxy.ChildIndex = i; + proxy.ProxyId = broadPhase.AddProxy(ref proxy); + + Proxies[i] = proxy; + } + } + + internal void DestroyProxies(IBroadPhase broadPhase) + { + // Destroy proxies in the broad-phase. + for (int i = 0; i < ProxyCount; ++i) + { + broadPhase.RemoveProxy(Proxies[i].ProxyId); + Proxies[i].ProxyId = -1; + } + + ProxyCount = 0; + } + + internal void Synchronize(IBroadPhase broadPhase, ref Transform transform1, ref Transform transform2) + { + if (ProxyCount == 0) + { + return; + } + + for (int i = 0; i < ProxyCount; ++i) + { + FixtureProxy proxy = Proxies[i]; + + // Compute an AABB that covers the swept Shape (may miss some rotation effect). + AABB aabb1, aabb2; + Shape.ComputeAABB(out aabb1, ref transform1, proxy.ChildIndex); + Shape.ComputeAABB(out aabb2, ref transform2, proxy.ChildIndex); + + proxy.AABB.Combine(ref aabb1, ref aabb2); + + Vector2 displacement = transform2.Position - transform1.Position; + + broadPhase.MoveProxy(proxy.ProxyId, ref proxy.AABB, displacement); + } + } + + internal bool CompareTo(Fixture fixture) + { + return ( + CollidesWith == fixture.CollidesWith && + CollisionCategories == fixture.CollisionCategories && + CollisionGroup == fixture.CollisionGroup && + Friction == fixture.Friction && + IsSensor == fixture.IsSensor && + Restitution == fixture.Restitution && + Shape.CompareTo(fixture.Shape) && + UserData == fixture.UserData); + } + } +} \ No newline at end of file diff --git a/Dynamics/Island.cs b/Dynamics/Island.cs new file mode 100644 index 0000000..b42b76a --- /dev/null +++ b/Dynamics/Island.cs @@ -0,0 +1,484 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using FarseerPhysics.Common; +using FarseerPhysics.Dynamics.Contacts; +using FarseerPhysics.Dynamics.Joints; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics +{ + /// + /// This is an internal class. + /// + public class Island + { + public Body[] Bodies; + public int BodyCount; + public int ContactCount; + public int JointCount; + private int _bodyCapacity; + private int _contactCapacity; + private ContactManager _contactManager; + private ContactSolver _contactSolver = new ContactSolver(); + private Contact[] _contacts; + private int _jointCapacity; + private Joint[] _joints; + public float JointUpdateTime; + + private const float LinTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance; + private const float AngTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance; + +#if (!SILVERLIGHT) + private Stopwatch _watch = new Stopwatch(); +#endif + + public void Reset(int bodyCapacity, int contactCapacity, int jointCapacity, ContactManager contactManager) + { + _bodyCapacity = bodyCapacity; + _contactCapacity = contactCapacity; + _jointCapacity = jointCapacity; + + BodyCount = 0; + ContactCount = 0; + JointCount = 0; + + _contactManager = contactManager; + + if (Bodies == null || Bodies.Length < bodyCapacity) + { + Bodies = new Body[bodyCapacity]; + } + + if (_contacts == null || _contacts.Length < contactCapacity) + { + _contacts = new Contact[contactCapacity * 2]; + } + + if (_joints == null || _joints.Length < jointCapacity) + { + _joints = new Joint[jointCapacity * 2]; + } + } + + public void Clear() + { + BodyCount = 0; + ContactCount = 0; + JointCount = 0; + } + + private float _tmpTime; + + public void Solve(ref TimeStep step, ref Vector2 gravity) + { + // Integrate velocities and apply damping. + for (int i = 0; i < BodyCount; ++i) + { + Body b = Bodies[i]; + + if (b.BodyType != BodyType.Dynamic) + { + continue; + } + + // Integrate velocities. + // FPE 3 only - Only apply gravity if the body wants it. + if (b.IgnoreGravity) + { + b.LinearVelocityInternal.X += step.dt * (b.InvMass * b.Force.X); + b.LinearVelocityInternal.Y += step.dt * (b.InvMass * b.Force.Y); + b.AngularVelocityInternal += step.dt * b.InvI * b.Torque; + } + else + { + b.LinearVelocityInternal.X += step.dt * (gravity.X + b.InvMass * b.Force.X); + b.LinearVelocityInternal.Y += step.dt * (gravity.Y + b.InvMass * b.Force.Y); + b.AngularVelocityInternal += step.dt * b.InvI * b.Torque; + } + + // Apply damping. + // ODE: dv/dt + c * v = 0 + // Solution: v(t) = v0 * exp(-c * t) + // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) + // v2 = exp(-c * dt) * v1 + // Taylor expansion: + // v2 = (1.0f - c * dt) * v1 + b.LinearVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.LinearDamping, 0.0f, 1.0f); + b.AngularVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.AngularDamping, 0.0f, 1.0f); + } + + // Partition contacts so that contacts with static bodies are solved last. + int i1 = -1; + for (int i2 = 0; i2 < ContactCount; ++i2) + { + Fixture fixtureA = _contacts[i2].FixtureA; + Fixture fixtureB = _contacts[i2].FixtureB; + Body bodyA = fixtureA.Body; + Body bodyB = fixtureB.Body; + bool nonStatic = bodyA.BodyType != BodyType.Static && bodyB.BodyType != BodyType.Static; + if (nonStatic) + { + ++i1; + + //TODO: Only swap if they are not the same? see http://code.google.com/p/box2d/issues/detail?id=162 + Contact tmp = _contacts[i1]; + _contacts[i1] = _contacts[i2]; + _contacts[i2] = tmp; + } + } + + // Initialize velocity constraints. + _contactSolver.Reset(_contacts, ContactCount, step.dtRatio, Settings.EnableWarmstarting); + _contactSolver.InitializeVelocityConstraints(); + + if (Settings.EnableWarmstarting) + { + _contactSolver.WarmStart(); + } + +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + { + _watch.Start(); + _tmpTime = 0; + } +#endif + + for (int i = 0; i < JointCount; ++i) + { + if (_joints[i].Enabled) + _joints[i].InitVelocityConstraints(ref step); + } + +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + { + _tmpTime += _watch.ElapsedTicks; + } +#endif + + // Solve velocity constraints. + for (int i = 0; i < Settings.VelocityIterations; ++i) + { +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + _watch.Start(); +#endif + for (int j = 0; j < JointCount; ++j) + { + Joint joint = _joints[j]; + + if (!joint.Enabled) + continue; + + joint.SolveVelocityConstraints(ref step); + joint.Validate(step.inv_dt); + } + +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + { + _watch.Stop(); + _tmpTime += _watch.ElapsedTicks; + _watch.Reset(); + } +#endif + + _contactSolver.SolveVelocityConstraints(); + } + + // Post-solve (store impulses for warm starting). + _contactSolver.StoreImpulses(); + + // Integrate positions. + for (int i = 0; i < BodyCount; ++i) + { + Body b = Bodies[i]; + + if (b.BodyType == BodyType.Static) + { + continue; + } + + // Check for large velocities. + float translationX = step.dt * b.LinearVelocityInternal.X; + float translationY = step.dt * b.LinearVelocityInternal.Y; + float result = translationX * translationX + translationY * translationY; + + if (result > Settings.MaxTranslationSquared) + { + float sq = (float)Math.Sqrt(result); + + float ratio = Settings.MaxTranslation / sq; + b.LinearVelocityInternal.X *= ratio; + b.LinearVelocityInternal.Y *= ratio; + } + + float rotation = step.dt * b.AngularVelocityInternal; + if (rotation * rotation > Settings.MaxRotationSquared) + { + float ratio = Settings.MaxRotation / Math.Abs(rotation); + b.AngularVelocityInternal *= ratio; + } + + // Store positions for continuous collision. + b.Sweep.C0.X = b.Sweep.C.X; + b.Sweep.C0.Y = b.Sweep.C.Y; + b.Sweep.A0 = b.Sweep.A; + + // Integrate + b.Sweep.C.X += step.dt * b.LinearVelocityInternal.X; + b.Sweep.C.Y += step.dt * b.LinearVelocityInternal.Y; + b.Sweep.A += step.dt * b.AngularVelocityInternal; + + // Compute new transform + b.SynchronizeTransform(); + + // Note: shapes are synchronized later. + } + + // Iterate over constraints. + for (int i = 0; i < Settings.PositionIterations; ++i) + { + bool contactsOkay = _contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte); + bool jointsOkay = true; + +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + _watch.Start(); +#endif + for (int j = 0; j < JointCount; ++j) + { + Joint joint = _joints[j]; + if (!joint.Enabled) + continue; + + bool jointOkay = joint.SolvePositionConstraints(); + jointsOkay = jointsOkay && jointOkay; + } + +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + { + _watch.Stop(); + _tmpTime += _watch.ElapsedTicks; + _watch.Reset(); + } +#endif + if (contactsOkay && jointsOkay) + { + // Exit early if the position errors are small. + break; + } + } + +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + { + JointUpdateTime = _tmpTime; + } +#endif + + Report(_contactSolver.Constraints); + + if (Settings.AllowSleep) + { + float minSleepTime = Settings.MaxFloat; + + for (int i = 0; i < BodyCount; ++i) + { + Body b = Bodies[i]; + if (b.BodyType == BodyType.Static) + { + continue; + } + + if ((b.Flags & BodyFlags.AutoSleep) == 0) + { + b.SleepTime = 0.0f; + minSleepTime = 0.0f; + } + + if ((b.Flags & BodyFlags.AutoSleep) == 0 || + b.AngularVelocityInternal * b.AngularVelocityInternal > AngTolSqr || + Vector2.Dot(b.LinearVelocityInternal, b.LinearVelocityInternal) > LinTolSqr) + { + b.SleepTime = 0.0f; + minSleepTime = 0.0f; + } + else + { + b.SleepTime += step.dt; + minSleepTime = Math.Min(minSleepTime, b.SleepTime); + } + } + + if (minSleepTime >= Settings.TimeToSleep) + { + for (int i = 0; i < BodyCount; ++i) + { + Body b = Bodies[i]; + b.Awake = false; + } + } + } + } + + internal void SolveTOI(ref TimeStep subStep) + { + _contactSolver.Reset(_contacts, ContactCount, subStep.dtRatio, false); + + // Solve position constraints. + const float kTOIBaumgarte = 0.75f; + for (int i = 0; i < Settings.TOIPositionIterations; ++i) + { + bool contactsOkay = _contactSolver.SolvePositionConstraints(kTOIBaumgarte); + if (contactsOkay) + { + break; + } + + if (i == Settings.TOIPositionIterations - 1) + { + i += 0; + } + } + + // Leap of faith to new safe state. + for (int i = 0; i < BodyCount; ++i) + { + Body body = Bodies[i]; + body.Sweep.A0 = body.Sweep.A; + body.Sweep.C0 = body.Sweep.C; + } + + // No warm starting is needed for TOI events because warm + // starting impulses were applied in the discrete solver. + _contactSolver.InitializeVelocityConstraints(); + + // Solve velocity constraints. + for (int i = 0; i < Settings.TOIVelocityIterations; ++i) + { + _contactSolver.SolveVelocityConstraints(); + } + + // Don't store the TOI contact forces for warm starting + // because they can be quite large. + + // Integrate positions. + for (int i = 0; i < BodyCount; ++i) + { + Body b = Bodies[i]; + + if (b.BodyType == BodyType.Static) + { + continue; + } + + // Check for large velocities. + float translationx = subStep.dt * b.LinearVelocityInternal.X; + float translationy = subStep.dt * b.LinearVelocityInternal.Y; + float dot = translationx * translationx + translationy * translationy; + if (dot > Settings.MaxTranslationSquared) + { + float norm = 1f / (float)Math.Sqrt(dot); + float value = Settings.MaxTranslation * subStep.inv_dt; + b.LinearVelocityInternal.X = value * (translationx * norm); + b.LinearVelocityInternal.Y = value * (translationy * norm); + } + + float rotation = subStep.dt * b.AngularVelocity; + if (rotation * rotation > Settings.MaxRotationSquared) + { + if (rotation < 0.0) + { + b.AngularVelocityInternal = -subStep.inv_dt * Settings.MaxRotation; + } + else + { + b.AngularVelocityInternal = subStep.inv_dt * Settings.MaxRotation; + } + } + + // Integrate + b.Sweep.C.X += subStep.dt * b.LinearVelocityInternal.X; + b.Sweep.C.Y += subStep.dt * b.LinearVelocityInternal.Y; + b.Sweep.A += subStep.dt * b.AngularVelocityInternal; + + // Compute new transform + b.SynchronizeTransform(); + + // Note: shapes are synchronized later. + } + + Report(_contactSolver.Constraints); + } + + public void Add(Body body) + { + Debug.Assert(BodyCount < _bodyCapacity); + Bodies[BodyCount++] = body; + } + + public void Add(Contact contact) + { + Debug.Assert(ContactCount < _contactCapacity); + _contacts[ContactCount++] = contact; + } + + public void Add(Joint joint) + { + Debug.Assert(JointCount < _jointCapacity); + _joints[JointCount++] = joint; + } + + private void Report(ContactConstraint[] constraints) + { + if (_contactManager == null) + return; + + for (int i = 0; i < ContactCount; ++i) + { + Contact c = _contacts[i]; + + if (c.FixtureA.AfterCollision != null) + c.FixtureA.AfterCollision(c.FixtureA, c.FixtureB, c); + + if (c.FixtureB.AfterCollision != null) + c.FixtureB.AfterCollision(c.FixtureB, c.FixtureA, c); + + if (_contactManager.PostSolve != null) + { + ContactConstraint cc = constraints[i]; + + _contactManager.PostSolve(c, cc); + } + } + } + } +} \ No newline at end of file diff --git a/Dynamics/Joints/AngleJoint.cs b/Dynamics/Joints/AngleJoint.cs new file mode 100644 index 0000000..db82c96 --- /dev/null +++ b/Dynamics/Joints/AngleJoint.cs @@ -0,0 +1,93 @@ +using System; +using System.Diagnostics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + /// + /// Maintains a fixed angle between two bodies + /// + public class AngleJoint : Joint + { + public float BiasFactor; + public float MaxImpulse; + public float Softness; + private float _bias; + private float _jointError; + private float _massFactor; + private float _targetAngle; + + internal AngleJoint() + { + JointType = JointType.Angle; + } + + public AngleJoint(Body bodyA, Body bodyB) + : base(bodyA, bodyB) + { + JointType = JointType.Angle; + TargetAngle = 0; + BiasFactor = .2f; + Softness = 0f; + MaxImpulse = float.MaxValue; + } + + public float TargetAngle + { + get { return _targetAngle; } + set + { + if (value != _targetAngle) + { + _targetAngle = value; + WakeBodies(); + } + } + } + + public override Vector2 WorldAnchorA + { + get { return BodyA.Position; } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.Position; } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + public override Vector2 GetReactionForce(float inv_dt) + { + //TODO + //return _inv_dt * _impulse; + return Vector2.Zero; + } + + public override float GetReactionTorque(float inv_dt) + { + return 0; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + _jointError = (BodyB.Sweep.A - BodyA.Sweep.A - TargetAngle); + + _bias = -BiasFactor * step.inv_dt * _jointError; + + _massFactor = (1 - Softness) / (BodyA.InvI + BodyB.InvI); + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + float p = (_bias - BodyB.AngularVelocity + BodyA.AngularVelocity) * _massFactor; + BodyA.AngularVelocity -= BodyA.InvI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse); + BodyB.AngularVelocity += BodyB.InvI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse); + } + + internal override bool SolvePositionConstraints() + { + //no position solving for this joint + return true; + } + } +} \ No newline at end of file diff --git a/Dynamics/Joints/DistanceJoint.cs b/Dynamics/Joints/DistanceJoint.cs new file mode 100644 index 0000000..bd0086b --- /dev/null +++ b/Dynamics/Joints/DistanceJoint.cs @@ -0,0 +1,286 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + // 1-D rained system + // m (v2 - v1) = lambda + // v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass. + // x2 = x1 + h * v2 + + // 1-D mass-damper-spring system + // m (v2 - v1) + h * d * v2 + h * k * + + // C = norm(p2 - p1) - L + // u = (p2 - p1) / norm(p2 - p1) + // Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1)) + // J = [-u -cross(r1, u) u cross(r2, u)] + // K = J * invM * JT + // = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2 + + /// + /// A distance joint rains two points on two bodies + /// to remain at a fixed distance from each other. You can view + /// this as a massless, rigid rod. + /// + public class DistanceJoint : Joint + { + /// + /// The local anchor point relative to bodyA's origin. + /// + public Vector2 LocalAnchorA; + + /// + /// The local anchor point relative to bodyB's origin. + /// + public Vector2 LocalAnchorB; + + private float _bias; + private float _gamma; + private float _impulse; + private float _mass; + private float _tmpFloat1; + private Vector2 _tmpVector1; + private Vector2 _u; + + internal DistanceJoint() + { + JointType = JointType.Distance; + } + + /// + /// This requires defining an + /// anchor point on both bodies and the non-zero length of the + /// distance joint. If you don't supply a length, the local anchor points + /// is used so that the initial configuration can violate the constraint + /// slightly. This helps when saving and loading a game. + /// @warning Do not use a zero or short length. + /// + /// The first body + /// The second body + /// The first body anchor + /// The second body anchor + public DistanceJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB) + : base(bodyA, bodyB) + { + JointType = JointType.Distance; + + LocalAnchorA = localAnchorA; + LocalAnchorB = localAnchorB; + + Vector2 d = WorldAnchorB - WorldAnchorA; + Length = d.Length(); + } + + /// + /// The natural length between the anchor points. + /// Manipulating the length can lead to non-physical behavior when the frequency is zero. + /// + public float Length { get; set; } + + /// + /// The mass-spring-damper frequency in Hertz. + /// + public float Frequency { get; set; } + + /// + /// The damping ratio. 0 = no damping, 1 = critical damping. + /// + public float DampingRatio { get; set; } + + public override sealed Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + } + + public override sealed Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + public override Vector2 GetReactionForce(float inv_dt) + { + Vector2 F = (inv_dt * _impulse) * _u; + return F; + } + + public override float GetReactionTorque(float inv_dt) + { + return 0.0f; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + Body b2 = BodyB; + + // Compute the effective mass matrix. + Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter); + _u = b2.Sweep.C + r2 - b1.Sweep.C - r1; + + // Handle singularity. + float length = _u.Length(); + if (length > Settings.LinearSlop) + { + _u *= 1.0f / length; + } + else + { + _u = Vector2.Zero; + } + + float cr1u, cr2u; + MathUtils.Cross(ref r1, ref _u, out cr1u); + MathUtils.Cross(ref r2, ref _u, out cr2u); + float invMass = b1.InvMass + b1.InvI * cr1u * cr1u + b2.InvMass + b2.InvI * cr2u * cr2u; + Debug.Assert(invMass > Settings.Epsilon); + _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; + + if (Frequency > 0.0f) + { + float C = length - Length; + + // Frequency + float omega = 2.0f * Settings.Pi * Frequency; + + // Damping coefficient + float d = 2.0f * _mass * DampingRatio * omega; + + // Spring stiffness + float k = _mass * omega * omega; + + // magic formulas + _gamma = step.dt * (d + step.dt * k); + _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f; + _bias = C * step.dt * k * _gamma; + + _mass = invMass + _gamma; + _mass = _mass != 0.0f ? 1.0f / _mass : 0.0f; + } + + if (Settings.EnableWarmstarting) + { + // Scale the impulse to support a variable time step. + _impulse *= step.dtRatio; + + Vector2 P = _impulse * _u; + b1.LinearVelocityInternal -= b1.InvMass * P; + MathUtils.Cross(ref r1, ref P, out _tmpFloat1); + b1.AngularVelocityInternal -= b1.InvI * /* r1 x P */ _tmpFloat1; + b2.LinearVelocityInternal += b2.InvMass * P; + MathUtils.Cross(ref r2, ref P, out _tmpFloat1); + b2.AngularVelocityInternal += b2.InvI * /* r2 x P */ _tmpFloat1; + } + else + { + _impulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + Body b2 = BodyB; + + Transform xf1, xf2; + b1.GetTransform(out xf1); + b2.GetTransform(out xf2); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter); + + // Cdot = dot(u, v + cross(w, r)) + MathUtils.Cross(b1.AngularVelocityInternal, ref r1, out _tmpVector1); + Vector2 v1 = b1.LinearVelocityInternal + _tmpVector1; + MathUtils.Cross(b2.AngularVelocityInternal, ref r2, out _tmpVector1); + Vector2 v2 = b2.LinearVelocityInternal + _tmpVector1; + float Cdot = Vector2.Dot(_u, v2 - v1); + + float impulse = -_mass * (Cdot + _bias + _gamma * _impulse); + _impulse += impulse; + + Vector2 P = impulse * _u; + b1.LinearVelocityInternal -= b1.InvMass * P; + MathUtils.Cross(ref r1, ref P, out _tmpFloat1); + b1.AngularVelocityInternal -= b1.InvI * _tmpFloat1; + b2.LinearVelocityInternal += b2.InvMass * P; + MathUtils.Cross(ref r2, ref P, out _tmpFloat1); + b2.AngularVelocityInternal += b2.InvI * _tmpFloat1; + } + + internal override bool SolvePositionConstraints() + { + if (Frequency > 0.0f) + { + // There is no position correction for soft distance constraints. + return true; + } + + Body b1 = BodyA; + Body b2 = BodyB; + + Transform xf1, xf2; + b1.GetTransform(out xf1); + b2.GetTransform(out xf2); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter); + + Vector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1; + + float length = d.Length(); + + if (length == 0.0f) + return true; + + d /= length; + float C = length - Length; + C = MathUtils.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); + + float impulse = -_mass * C; + _u = d; + Vector2 P = impulse * _u; + + b1.Sweep.C -= b1.InvMass * P; + MathUtils.Cross(ref r1, ref P, out _tmpFloat1); + b1.Sweep.A -= b1.InvI * _tmpFloat1; + b2.Sweep.C += b2.InvMass * P; + MathUtils.Cross(ref r2, ref P, out _tmpFloat1); + b2.Sweep.A += b2.InvI * _tmpFloat1; + + b1.SynchronizeTransform(); + b2.SynchronizeTransform(); + + return Math.Abs(C) < Settings.LinearSlop; + } + } +} \ No newline at end of file diff --git a/Dynamics/Joints/FixedAngleJoint.cs b/Dynamics/Joints/FixedAngleJoint.cs new file mode 100644 index 0000000..130881c --- /dev/null +++ b/Dynamics/Joints/FixedAngleJoint.cs @@ -0,0 +1,84 @@ +using System; +using System.Diagnostics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + public class FixedAngleJoint : Joint + { + public float BiasFactor; + public float MaxImpulse; + public float Softness; + private float _bias; + private float _jointError; + private float _massFactor; + private float _targetAngle; + + public FixedAngleJoint(Body bodyA) + : base(bodyA) + { + JointType = JointType.FixedAngle; + TargetAngle = 0; + BiasFactor = .2f; + Softness = 0f; + MaxImpulse = float.MaxValue; + } + + public float TargetAngle + { + get { return _targetAngle; } + set + { + if (value != _targetAngle) + { + _targetAngle = value; + WakeBodies(); + } + } + } + + public override Vector2 WorldAnchorA + { + get { return BodyA.Position; } + } + + public override Vector2 WorldAnchorB + { + get { return BodyA.Position; } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + public override Vector2 GetReactionForce(float inv_dt) + { + //TODO + //return _inv_dt * _impulse; + return Vector2.Zero; + } + + public override float GetReactionTorque(float inv_dt) + { + return 0; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + _jointError = BodyA.Sweep.A - TargetAngle; + + _bias = -BiasFactor * step.inv_dt * _jointError; + + _massFactor = (1 - Softness) / (BodyA.InvI); + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + float p = (_bias - BodyA.AngularVelocity) * _massFactor; + BodyA.AngularVelocity += BodyA.InvI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse); + } + + internal override bool SolvePositionConstraints() + { + //no position solving for this joint + return true; + } + } +} \ No newline at end of file diff --git a/Dynamics/Joints/FixedDistanceJoint.cs b/Dynamics/Joints/FixedDistanceJoint.cs new file mode 100644 index 0000000..cb08a5f --- /dev/null +++ b/Dynamics/Joints/FixedDistanceJoint.cs @@ -0,0 +1,255 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + // 1-D rained system + // m (v2 - v1) = lambda + // v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass. + // x2 = x1 + h * v2 + + // 1-D mass-damper-spring system + // m (v2 - v1) + h * d * v2 + h * k * + + // C = norm(p2 - p1) - L + // u = (p2 - p1) / norm(p2 - p1) + // Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1)) + // J = [-u -cross(r1, u) u cross(r2, u)] + // K = J * invM * JT + // = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2 + + /// + /// A distance joint rains two points on two bodies + /// to remain at a fixed distance from each other. You can view + /// this as a massless, rigid rod. + /// + public class FixedDistanceJoint : Joint + { + /// + /// The local anchor point relative to bodyA's origin. + /// + public Vector2 LocalAnchorA; + + private float _bias; + private float _gamma; + private float _impulse; + private float _mass; + private Vector2 _u; + private Vector2 _worldAnchorB; + + /// + /// This requires defining an + /// anchor point on both bodies and the non-zero length of the + /// distance joint. If you don't supply a length, the local anchor points + /// is used so that the initial configuration can violate the constraint + /// slightly. This helps when saving and loading a game. + /// @warning Do not use a zero or short length. + /// + /// The body. + /// The body anchor. + /// The world anchor. + public FixedDistanceJoint(Body body, Vector2 bodyAnchor, Vector2 worldAnchor) + : base(body) + { + JointType = JointType.FixedDistance; + + LocalAnchorA = bodyAnchor; + _worldAnchorB = worldAnchor; + + //Calculate the length + Vector2 d = WorldAnchorB - WorldAnchorA; + Length = d.Length(); + } + + /// + /// The natural length between the anchor points. + /// Manipulating the length can lead to non-physical behavior when the frequency is zero. + /// + public float Length { get; set; } + + /// + /// The mass-spring-damper frequency in Hertz. + /// + public float Frequency { get; set; } + + /// + /// The damping ratio. 0 = no damping, 1 = critical damping. + /// + public float DampingRatio { get; set; } + + public override sealed Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + } + + public override sealed Vector2 WorldAnchorB + { + get { return _worldAnchorB; } + set { _worldAnchorB = value; } + } + + public override Vector2 GetReactionForce(float invDt) + { + return (invDt * _impulse) * _u; + } + + public override float GetReactionTorque(float invDt) + { + return 0.0f; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + + Transform xf1; + b1.GetTransform(out xf1); + + // Compute the effective mass matrix. + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = _worldAnchorB; + _u = r2 - b1.Sweep.C - r1; + + // Handle singularity. + float length = _u.Length(); + if (length > Settings.LinearSlop) + { + _u *= 1.0f / length; + } + else + { + _u = Vector2.Zero; + } + + float cr1u = MathUtils.Cross(r1, _u); + float cr2u = MathUtils.Cross(r2, _u); + float invMass = b1.InvMass + b1.InvI * cr1u * cr1u + 0 * cr2u * cr2u; + Debug.Assert(invMass > Settings.Epsilon); + _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; + + if (Frequency > 0.0f) + { + float C = length - Length; + + // Frequency + float omega = 2.0f * Settings.Pi * Frequency; + + // Damping coefficient + float d = 2.0f * _mass * DampingRatio * omega; + + // Spring stiffness + float k = _mass * omega * omega; + + // magic formulas + _gamma = step.dt * (d + step.dt * k); + _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f; + _bias = C * step.dt * k * _gamma; + + _mass = invMass + _gamma; + _mass = _mass != 0.0f ? 1.0f / _mass : 0.0f; + } + + if (Settings.EnableWarmstarting) + { + // Scale the impulse to support a variable time step. + _impulse *= step.dtRatio; + + Vector2 P = _impulse * _u; + b1.LinearVelocityInternal -= b1.InvMass * P; + b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P); + } + else + { + _impulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + + Transform xf1; + b1.GetTransform(out xf1); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + + // Cdot = dot(u, v + cross(w, r)) + Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1); + Vector2 v2 = Vector2.Zero; + float Cdot = Vector2.Dot(_u, v2 - v1); + + float impulse = -_mass * (Cdot + _bias + _gamma * _impulse); + _impulse += impulse; + + Vector2 P = impulse * _u; + b1.LinearVelocityInternal -= b1.InvMass * P; + b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P); + } + + internal override bool SolvePositionConstraints() + { + if (Frequency > 0.0f) + { + // There is no position correction for soft distance constraints. + return true; + } + + Body b1 = BodyA; + + Transform xf1; + b1.GetTransform(out xf1); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = _worldAnchorB; + + Vector2 d = r2 - b1.Sweep.C - r1; + + float length = d.Length(); + + if (length == 0.0f) + return true; + + d /= length; + float C = length - Length; + C = MathUtils.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); + + float impulse = -_mass * C; + _u = d; + Vector2 P = impulse * _u; + + b1.Sweep.C -= b1.InvMass * P; + b1.Sweep.A -= b1.InvI * MathUtils.Cross(r1, P); + + b1.SynchronizeTransform(); + + return Math.Abs(C) < Settings.LinearSlop; + } + } +} \ No newline at end of file diff --git a/Dynamics/Joints/FixedFrictionJoint.cs b/Dynamics/Joints/FixedFrictionJoint.cs new file mode 100644 index 0000000..2ec3f86 --- /dev/null +++ b/Dynamics/Joints/FixedFrictionJoint.cs @@ -0,0 +1,227 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + // Point-to-point constraint + // Cdot = v2 - v1 + // = v2 + cross(w2, r2) - v1 - cross(w1, r1) + // J = [-I -r1_skew I r2_skew ] + // Identity used: + // w k % (rx i + ry j) = w * (-ry i + rx j) + + // Angle constraint + // Cdot = w2 - w1 + // J = [0 0 -1 0 0 1] + // K = invI1 + invI2 + + /// + /// Friction joint. This is used for top-down friction. + /// It provides 2D translational friction and angular friction. + /// + public class FixedFrictionJoint : Joint + { + public Vector2 LocalAnchorA; + + /// + /// The maximum friction force in N. + /// + public float MaxForce; + + /// + /// The maximum friction torque in N-m. + /// + public float MaxTorque; + + private float _angularImpulse; + private float _angularMass; + private Vector2 _linearImpulse; + private Mat22 _linearMass; + + public FixedFrictionJoint(Body body, Vector2 localAnchorA) + : base(body) + { + JointType = JointType.FixedFriction; + LocalAnchorA = localAnchorA; + + //Setting default max force and max torque + const float gravity = 10.0f; + + // For a circle: I = 0.5 * m * r * r ==> r = sqrt(2 * I / m) + float radius = (float)Math.Sqrt(2.0 * (body.Inertia / body.Mass)); + + MaxForce = body.Mass * gravity; + MaxTorque = body.Mass * radius * gravity; + } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + } + + public override Vector2 WorldAnchorB + { + get { return Vector2.Zero; } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + public override Vector2 GetReactionForce(float invDT) + { + return invDT * _linearImpulse; + } + + public override float GetReactionTorque(float invDT) + { + return invDT * _angularImpulse; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body bA = BodyA; + + Transform xfA; + bA.GetTransform(out xfA); + + // Compute the effective mass matrix. + Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter); + + // J = [-I -r1_skew I r2_skew] + // [ 0 -1 0 1] + // r_skew = [-ry; rx] + + // Matlab + // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] + // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] + // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] + + float mA = bA.InvMass; + float iA = bA.InvI; + + Mat22 K1 = new Mat22(); + K1.Col1.X = mA; + K1.Col2.X = 0.0f; + K1.Col1.Y = 0.0f; + K1.Col2.Y = mA; + + Mat22 K2 = new Mat22(); + K2.Col1.X = iA * rA.Y * rA.Y; + K2.Col2.X = -iA * rA.X * rA.Y; + K2.Col1.Y = -iA * rA.X * rA.Y; + K2.Col2.Y = iA * rA.X * rA.X; + + Mat22 K12; + Mat22.Add(ref K1, ref K2, out K12); + + _linearMass = K12.Inverse; + + _angularMass = iA; + if (_angularMass > 0.0f) + { + _angularMass = 1.0f / _angularMass; + } + + if (Settings.EnableWarmstarting) + { + // Scale impulses to support a variable time step. + _linearImpulse *= step.dtRatio; + _angularImpulse *= step.dtRatio; + + Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y); + + bA.LinearVelocityInternal -= mA * P; + bA.AngularVelocityInternal -= iA * (MathUtils.Cross(rA, P) + _angularImpulse); + } + else + { + _linearImpulse = Vector2.Zero; + _angularImpulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body bA = BodyA; + + Vector2 vA = bA.LinearVelocityInternal; + float wA = bA.AngularVelocityInternal; + + float mA = bA.InvMass; + float iA = bA.InvI; + + Transform xfA; + bA.GetTransform(out xfA); + + Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter); + + // Solve angular friction + { + float Cdot = -wA; + float impulse = -_angularMass * Cdot; + + float oldImpulse = _angularImpulse; + float maxImpulse = step.dt * MaxTorque; + _angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse); + impulse = _angularImpulse - oldImpulse; + + wA -= iA * impulse; + } + + // Solve linear friction + { + Vector2 Cdot = -vA - MathUtils.Cross(wA, rA); + + Vector2 impulse = -MathUtils.Multiply(ref _linearMass, Cdot); + Vector2 oldImpulse = _linearImpulse; + _linearImpulse += impulse; + + float maxImpulse = step.dt * MaxForce; + + if (_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) + { + _linearImpulse.Normalize(); + _linearImpulse *= maxImpulse; + } + + impulse = _linearImpulse - oldImpulse; + + vA -= mA * impulse; + wA -= iA * MathUtils.Cross(rA, impulse); + } + + bA.LinearVelocityInternal = vA; + bA.AngularVelocityInternal = wA; + } + + internal override bool SolvePositionConstraints() + { + return true; + } + } +} \ No newline at end of file diff --git a/Dynamics/Joints/FixedLineJoint.cs b/Dynamics/Joints/FixedLineJoint.cs new file mode 100644 index 0000000..60f15a5 --- /dev/null +++ b/Dynamics/Joints/FixedLineJoint.cs @@ -0,0 +1,413 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + public class FixedLineJoint : Joint + { + private Vector2 _ax, _ay; + private float _bias; + private bool _enableMotor; + private float _gamma; + private float _impulse; + private Vector2 _localXAxis; + private Vector2 _localYAxisA; + private float _mass; + private float _maxMotorTorque; + private float _motorImpulse; + private float _motorMass; + private float _motorSpeed; + + private float _sAx; + private float _sAy; + private float _sBx; + private float _sBy; + + private float _springImpulse; + private float _springMass; + + // Linear constraint (point-to-line) + // d = pB - pA = xB + rB - xA - rA + // C = dot(ay, d) + // Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA)) + // = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB) + // J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)] + + // Spring linear constraint + // C = dot(ax, d) + // Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB) + // J = [-ax -cross(d+rA, ax) ax cross(rB, ax)] + + // Motor rotational constraint + // Cdot = wB - wA + // J = [0 0 -1 0 0 1] + + internal FixedLineJoint() { JointType = JointType.FixedLine; } + + public FixedLineJoint(Body body, Vector2 worldAnchor, Vector2 axis) + : base(body) + { + JointType = JointType.FixedLine; + + BodyB = BodyA; + + LocalAnchorA = worldAnchor; + LocalAnchorB = BodyB.GetLocalPoint(worldAnchor); + LocalXAxis = axis; + } + + public Vector2 LocalAnchorA { get; set; } + + public Vector2 LocalAnchorB { get; set; } + + public override Vector2 WorldAnchorA + { + get { return LocalAnchorA; } + } + + public override Vector2 WorldAnchorB + { + get { return BodyA.GetWorldPoint(LocalAnchorB); } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + public float JointTranslation + { + get + { + Body bA = BodyA; + Body bB = BodyB; + + Vector2 pA = bA.GetWorldPoint(LocalAnchorA); + Vector2 pB = bB.GetWorldPoint(LocalAnchorB); + Vector2 d = pB - pA; + Vector2 axis = bA.GetWorldVector(LocalXAxis); + + float translation = Vector2.Dot(d, axis); + return translation; + } + } + + public float JointSpeed + { + get + { + float wA = BodyA.AngularVelocityInternal; + float wB = BodyB.AngularVelocityInternal; + return wB - wA; + } + } + + public bool MotorEnabled + { + get { return _enableMotor; } + set + { + BodyA.Awake = true; + BodyB.Awake = true; + _enableMotor = value; + } + } + + public float MotorSpeed + { + set + { + BodyA.Awake = true; + BodyB.Awake = true; + _motorSpeed = value; + } + get { return _motorSpeed; } + } + + public float MaxMotorTorque + { + set + { + BodyA.Awake = true; + BodyB.Awake = true; + _maxMotorTorque = value; + } + get { return _maxMotorTorque; } + } + + public float Frequency { get; set; } + + public float DampingRatio { get; set; } + + public Vector2 LocalXAxis + { + get { return _localXAxis; } + set + { + _localXAxis = value; + _localYAxisA = MathUtils.Cross(1.0f, _localXAxis); + } + } + + public override Vector2 GetReactionForce(float invDt) + { + return invDt * (_impulse * _ay + _springImpulse * _ax); + } + + public override float GetReactionTorque(float invDt) + { + return invDt * _motorImpulse; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body bB = BodyB; + + LocalCenterA = Vector2.Zero; + LocalCenterB = bB.LocalCenter; + + Transform xfB; + bB.GetTransform(out xfB); + + // Compute the effective masses. + Vector2 rA = LocalAnchorA; + Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - LocalCenterB); + Vector2 d = bB.Sweep.C + rB - rA; + + InvMassA = 0.0f; + InvIA = 0.0f; + InvMassB = bB.InvMass; + InvIB = bB.InvI; + + // Point to line constraint + { + _ay = _localYAxisA; + _sAy = MathUtils.Cross(d + rA, _ay); + _sBy = MathUtils.Cross(rB, _ay); + + _mass = InvMassA + InvMassB + InvIA * _sAy * _sAy + InvIB * _sBy * _sBy; + + if (_mass > 0.0f) + { + _mass = 1.0f / _mass; + } + } + + // Spring constraint + _springMass = 0.0f; + if (Frequency > 0.0f) + { + _ax = LocalXAxis; + _sAx = MathUtils.Cross(d + rA, _ax); + _sBx = MathUtils.Cross(rB, _ax); + + float invMass = InvMassA + InvMassB + InvIA * _sAx * _sAx + InvIB * _sBx * _sBx; + + if (invMass > 0.0f) + { + _springMass = 1.0f / invMass; + + float C = Vector2.Dot(d, _ax); + + // Frequency + float omega = 2.0f * Settings.Pi * Frequency; + + // Damping coefficient + float da = 2.0f * _springMass * DampingRatio * omega; + + // Spring stiffness + float k = _springMass * omega * omega; + + // magic formulas + _gamma = step.dt * (da + step.dt * k); + if (_gamma > 0.0f) + { + _gamma = 1.0f / _gamma; + } + + _bias = C * step.dt * k * _gamma; + + _springMass = invMass + _gamma; + if (_springMass > 0.0f) + { + _springMass = 1.0f / _springMass; + } + } + } + else + { + _springImpulse = 0.0f; + _springMass = 0.0f; + } + + // Rotational motor + if (_enableMotor) + { + _motorMass = InvIA + InvIB; + if (_motorMass > 0.0f) + { + _motorMass = 1.0f / _motorMass; + } + } + else + { + _motorMass = 0.0f; + _motorImpulse = 0.0f; + } + + if (Settings.EnableWarmstarting) + { + // Account for variable time step. + _impulse *= step.dtRatio; + _springImpulse *= step.dtRatio; + _motorImpulse *= step.dtRatio; + + Vector2 P = _impulse * _ay + _springImpulse * _ax; + float LB = _impulse * _sBy + _springImpulse * _sBx + _motorImpulse; + + bB.LinearVelocityInternal += InvMassB * P; + bB.AngularVelocityInternal += InvIB * LB; + } + else + { + _impulse = 0.0f; + _springImpulse = 0.0f; + _motorImpulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body bB = BodyB; + + Vector2 vA = Vector2.Zero; + float wA = 0.0f; + Vector2 vB = bB.LinearVelocityInternal; + float wB = bB.AngularVelocityInternal; + + // Solve spring constraint + { + float Cdot = Vector2.Dot(_ax, vB - vA) + _sBx * wB - _sAx * wA; + float impulse = -_springMass * (Cdot + _bias + _gamma * _springImpulse); + _springImpulse += impulse; + + Vector2 P = impulse * _ax; + float LA = impulse * _sAx; + float LB = impulse * _sBx; + + vA -= InvMassA * P; + wA -= InvIA * LA; + + vB += InvMassB * P; + wB += InvIB * LB; + } + + // Solve rotational motor constraint + { + float Cdot = wB - wA - _motorSpeed; + float impulse = -_motorMass * Cdot; + + float oldImpulse = _motorImpulse; + float maxImpulse = step.dt * _maxMotorTorque; + _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = _motorImpulse - oldImpulse; + + wA -= InvIA * impulse; + wB += InvIB * impulse; + } + + // Solve point to line constraint + { + float Cdot = Vector2.Dot(_ay, vB - vA) + _sBy * wB - _sAy * wA; + float impulse = _mass * (-Cdot); + _impulse += impulse; + + Vector2 P = impulse * _ay; + float LB = impulse * _sBy; + + vB += InvMassB * P; + wB += InvIB * LB; + } + + bB.LinearVelocityInternal = vB; + bB.AngularVelocityInternal = wB; + } + + internal override bool SolvePositionConstraints() + { + Body bB = BodyB; + + Vector2 xA = Vector2.Zero; + const float angleA = 0.0f; + + Vector2 xB = bB.Sweep.C; + float angleB = bB.Sweep.A; + + Mat22 RA = new Mat22(angleA); + Mat22 RB = new Mat22(angleB); + + Vector2 rA = MathUtils.Multiply(ref RA, LocalAnchorA - LocalCenterA); + Vector2 rB = MathUtils.Multiply(ref RB, LocalAnchorB - LocalCenterB); + Vector2 d = xB + rB - xA - rA; + + Vector2 ay = MathUtils.Multiply(ref RA, _localYAxisA); + + float sBy = MathUtils.Cross(rB, ay); + + float C = Vector2.Dot(d, ay); + + float k = InvMassA + InvMassB + InvIA * _sAy * _sAy + InvIB * _sBy * _sBy; + + float impulse; + if (k != 0.0f) + { + impulse = -C / k; + } + else + { + impulse = 0.0f; + } + + Vector2 P = impulse * ay; + float LB = impulse * sBy; + + xB += InvMassB * P; + angleB += InvIB * LB; + + // TODO_ERIN remove need for this. + bB.Sweep.C = xB; + bB.Sweep.A = angleB; + bB.SynchronizeTransform(); + + return Math.Abs(C) <= Settings.LinearSlop; + } + + public float GetMotorTorque(float invDt) + { + return invDt * _motorImpulse; + } + } +} \ No newline at end of file diff --git a/Dynamics/Joints/FixedMouseJoint.cs b/Dynamics/Joints/FixedMouseJoint.cs new file mode 100644 index 0000000..8273c55 --- /dev/null +++ b/Dynamics/Joints/FixedMouseJoint.cs @@ -0,0 +1,209 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + /// + /// A mouse joint is used to make a point on a body track a + /// specified world point. This a soft constraint with a maximum + /// force. This allows the constraint to stretch and without + /// applying huge forces. + /// NOTE: this joint is not documented in the manual because it was + /// developed to be used in the testbed. If you want to learn how to + /// use the mouse joint, look at the testbed. + /// + public class FixedMouseJoint : Joint + { + public Vector2 LocalAnchorA; + private Vector2 _C; // position error + private float _beta; + private float _gamma; + private Vector2 _impulse; + private Mat22 _mass; // effective mass for point-to-point constraint. + + private Vector2 _worldAnchor; + + /// + /// This requires a world target point, + /// tuning parameters, and the time step. + /// + /// The body. + /// The target. + public FixedMouseJoint(Body body, Vector2 worldAnchor) + : base(body) + { + JointType = JointType.FixedMouse; + Frequency = 5.0f; + DampingRatio = 0.7f; + + Debug.Assert(worldAnchor.IsValid()); + + Transform xf1; + BodyA.GetTransform(out xf1); + + _worldAnchor = worldAnchor; + LocalAnchorA = BodyA.GetLocalPoint(worldAnchor); + } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + } + + public override Vector2 WorldAnchorB + { + get { return _worldAnchor; } + set + { + BodyA.Awake = true; + _worldAnchor = value; + } + } + + /// + /// The maximum constraint force that can be exerted + /// to move the candidate body. Usually you will express + /// as some multiple of the weight (multiplier * mass * gravity). + /// + public float MaxForce { get; set; } + + /// + /// The response speed. + /// + public float Frequency { get; set; } + + /// + /// The damping ratio. 0 = no damping, 1 = critical damping. + /// + public float DampingRatio { get; set; } + + public override Vector2 GetReactionForce(float inv_dt) + { + return inv_dt * _impulse; + } + + public override float GetReactionTorque(float inv_dt) + { + return inv_dt * 0.0f; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body b = BodyA; + + float mass = b.Mass; + + // Frequency + float omega = 2.0f * Settings.Pi * Frequency; + + // Damping coefficient + float d = 2.0f * mass * DampingRatio * omega; + + // Spring stiffness + float k = mass * (omega * omega); + + // magic formulas + // gamma has units of inverse mass. + // beta has units of inverse time. + Debug.Assert(d + step.dt * k > Settings.Epsilon); + + _gamma = step.dt * (d + step.dt * k); + if (_gamma != 0.0f) + { + _gamma = 1.0f / _gamma; + } + + _beta = step.dt * k * _gamma; + + // Compute the effective mass matrix. + Transform xf1; + b.GetTransform(out xf1); + Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b.LocalCenter); + + // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] + // = [1/m1+1/m2 0 ] + invI1 * [r1.Y*r1.Y -r1.X*r1.Y] + invI2 * [r1.Y*r1.Y -r1.X*r1.Y] + // [ 0 1/m1+1/m2] [-r1.X*r1.Y r1.X*r1.X] [-r1.X*r1.Y r1.X*r1.X] + float invMass = b.InvMass; + float invI = b.InvI; + + Mat22 K1 = new Mat22(new Vector2(invMass, 0.0f), new Vector2(0.0f, invMass)); + Mat22 K2 = new Mat22(new Vector2(invI * r.Y * r.Y, -invI * r.X * r.Y), + new Vector2(-invI * r.X * r.Y, invI * r.X * r.X)); + + Mat22 K; + Mat22.Add(ref K1, ref K2, out K); + + K.Col1.X += _gamma; + K.Col2.Y += _gamma; + + _mass = K.Inverse; + + _C = b.Sweep.C + r - _worldAnchor; + + // Cheat with some damping + b.AngularVelocityInternal *= 0.98f; + + // Warm starting. + _impulse *= step.dtRatio; + b.LinearVelocityInternal += invMass * _impulse; + b.AngularVelocityInternal += invI * MathUtils.Cross(r, _impulse); + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body b = BodyA; + + Transform xf1; + b.GetTransform(out xf1); + + Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b.LocalCenter); + + // Cdot = v + cross(w, r) + Vector2 Cdot = b.LinearVelocityInternal + MathUtils.Cross(b.AngularVelocityInternal, r); + Vector2 impulse = MathUtils.Multiply(ref _mass, -(Cdot + _beta * _C + _gamma * _impulse)); + + Vector2 oldImpulse = _impulse; + _impulse += impulse; + float maxImpulse = step.dt * MaxForce; + if (_impulse.LengthSquared() > maxImpulse * maxImpulse) + { + _impulse *= maxImpulse / _impulse.Length(); + } + impulse = _impulse - oldImpulse; + + b.LinearVelocityInternal += b.InvMass * impulse; + b.AngularVelocityInternal += b.InvI * MathUtils.Cross(r, impulse); + } + + internal override bool SolvePositionConstraints() + { + return true; + } + } +} \ No newline at end of file diff --git a/Dynamics/Joints/FixedPrismaticJoint.cs b/Dynamics/Joints/FixedPrismaticJoint.cs new file mode 100644 index 0000000..d4e7816 --- /dev/null +++ b/Dynamics/Joints/FixedPrismaticJoint.cs @@ -0,0 +1,636 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + // Linear constraint (point-to-line) + // d = p2 - p1 = x2 + r2 - x1 - r1 + // C = dot(perp, d) + // Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1)) + // = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2) + // J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)] + // + // Angular constraint + // C = a2 - a1 + a_initial + // Cdot = w2 - w1 + // J = [0 0 -1 0 0 1] + // + // K = J * invM * JT + // + // J = [-a -s1 a s2] + // [0 -1 0 1] + // a = perp + // s1 = cross(d + r1, a) = cross(p2 - x1, a) + // s2 = cross(r2, a) = cross(p2 - x2, a) + // Motor/Limit linear constraint + // C = dot(ax1, d) + // Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2) + // J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)] + // Block Solver + // We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even + // when the mass has poor distribution (leading to large torques about the joint anchor points). + // + // The Jacobian has 3 rows: + // J = [-uT -s1 uT s2] // linear + // [0 -1 0 1] // angular + // [-vT -a1 vT a2] // limit + // + // u = perp + // v = axis + // s1 = cross(d + r1, u), s2 = cross(r2, u) + // a1 = cross(d + r1, v), a2 = cross(r2, v) + // M * (v2 - v1) = JT * df + // J * v2 = bias + // + // v2 = v1 + invM * JT * df + // J * (v1 + invM * JT * df) = bias + // K * df = bias - J * v1 = -Cdot + // K = J * invM * JT + // Cdot = J * v1 - bias + // + // Now solve for f2. + // df = f2 - f1 + // K * (f2 - f1) = -Cdot + // f2 = invK * (-Cdot) + f1 + // + // Clamp accumulated limit impulse. + // lower: f2(3) = max(f2(3), 0) + // upper: f2(3) = min(f2(3), 0) + // + // Solve for correct f2(1:2) + // K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1 + // = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3) + // K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2) + // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) + // + // Now compute impulse to be applied: + // df = f2 - f1 + + /// + /// A prismatic joint. This joint provides one degree of freedom: translation + /// along an axis fixed in body1. Relative rotation is prevented. You can + /// use a joint limit to restrict the range of motion and a joint motor to + /// drive the motion or to model joint friction. + /// + public class FixedPrismaticJoint : Joint + { + private Mat33 _K; + private float _a1, _a2; + private Vector2 _axis; + private bool _enableLimit; + private bool _enableMotor; + private Vector3 _impulse; + private LimitState _limitState; + private Vector2 _localXAxis1; + private Vector2 _localYAxis1; + private float _lowerTranslation; + private float _maxMotorForce; + private float _motorMass; // effective mass for motor/limit translational constraint. + private float _motorSpeed; + private Vector2 _perp; + private float _refAngle; + private float _s1, _s2; + private float _upperTranslation; + + /// + /// This requires defining a line of + /// motion using an axis and an anchor point. The definition uses local + /// anchor points and a local axis so that the initial configuration + /// can violate the constraint slightly. The joint translation is zero + /// when the local anchor points coincide in world space. Using local + /// anchors and a local axis helps when saving and loading a game. + /// + /// The body. + /// The anchor. + /// The axis. + public FixedPrismaticJoint(Body body, Vector2 worldAnchor, Vector2 axis) + : base(body) + { + JointType = JointType.FixedPrismatic; + + BodyB = BodyA; + + LocalAnchorA = worldAnchor; + LocalAnchorB = BodyB.GetLocalPoint(worldAnchor); + + _localXAxis1 = axis; + _localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1); + _refAngle = BodyB.Rotation; + + _limitState = LimitState.Inactive; + } + + public Vector2 LocalAnchorA { get; set; } + + public Vector2 LocalAnchorB { get; set; } + + public override Vector2 WorldAnchorA + { + get { return LocalAnchorA; } + } + + public override Vector2 WorldAnchorB + { + get { return BodyA.GetWorldPoint(LocalAnchorB); } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + /// + /// Get the current joint translation, usually in meters. + /// + /// + public float JointTranslation + { + get + { + Vector2 d = BodyB.GetWorldPoint(LocalAnchorB) - LocalAnchorA; + Vector2 axis = _localXAxis1; + + return Vector2.Dot(d, axis); + } + } + + /// + /// Get the current joint translation speed, usually in meters per second. + /// + /// + public float JointSpeed + { + get + { + Transform xf2; + BodyB.GetTransform(out xf2); + + Vector2 r1 = LocalAnchorA; + Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - BodyB.LocalCenter); + Vector2 p1 = r1; + Vector2 p2 = BodyB.Sweep.C + r2; + Vector2 d = p2 - p1; + Vector2 axis = _localXAxis1; + + Vector2 v1 = Vector2.Zero; + Vector2 v2 = BodyB.LinearVelocityInternal; + const float w1 = 0.0f; + float w2 = BodyB.AngularVelocityInternal; + + float speed = Vector2.Dot(d, MathUtils.Cross(w1, axis)) + + Vector2.Dot(axis, v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1)); + return speed; + } + } + + /// + /// Is the joint limit enabled? + /// + /// true if [limit enabled]; otherwise, false. + public bool LimitEnabled + { + get { return _enableLimit; } + set + { + Debug.Assert(BodyA.FixedRotation == false, "Warning: limits does currently not work with fixed rotation"); + + WakeBodies(); + _enableLimit = value; + } + } + + /// + /// Get the lower joint limit, usually in meters. + /// + /// + public float LowerLimit + { + get { return _lowerTranslation; } + set + { + WakeBodies(); + _lowerTranslation = value; + } + } + + /// + /// Get the upper joint limit, usually in meters. + /// + /// + public float UpperLimit + { + get { return _upperTranslation; } + set + { + WakeBodies(); + _upperTranslation = value; + } + } + + /// + /// Is the joint motor enabled? + /// + /// true if [motor enabled]; otherwise, false. + public bool MotorEnabled + { + get { return _enableMotor; } + set + { + WakeBodies(); + _enableMotor = value; + } + } + + /// + /// Set the motor speed, usually in meters per second. + /// + /// The speed. + public float MotorSpeed + { + set + { + WakeBodies(); + _motorSpeed = value; + } + get { return _motorSpeed; } + } + + /// + /// Set the maximum motor force, usually in N. + /// + /// The force. + public float MaxMotorForce + { + set + { + WakeBodies(); + _maxMotorForce = value; + } + } + + /// + /// Get the current motor force, usually in N. + /// + /// + public float MotorForce { get; set; } + + public Vector2 LocalXAxis1 + { + get { return _localXAxis1; } + set + { + _localXAxis1 = value; + _localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1); + } + } + + public override Vector2 GetReactionForce(float inv_dt) + { + return inv_dt * (_impulse.X * _perp + (MotorForce + _impulse.Z) * _axis); + } + + public override float GetReactionTorque(float inv_dt) + { + return inv_dt * _impulse.Y; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body bB = BodyB; + + LocalCenterA = Vector2.Zero; + LocalCenterB = bB.LocalCenter; + + Transform xf2; + bB.GetTransform(out xf2); + + // Compute the effective masses. + Vector2 r1 = LocalAnchorA; + Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - LocalCenterB); + Vector2 d = bB.Sweep.C + r2 - /* b1._sweep.Center - */ r1; + + InvMassA = 0.0f; + InvIA = 0.0f; + InvMassB = bB.InvMass; + InvIB = bB.InvI; + + // Compute motor Jacobian and effective mass. + { + _axis = _localXAxis1; + _a1 = MathUtils.Cross(d + r1, _axis); + _a2 = MathUtils.Cross(r2, _axis); + + _motorMass = InvMassA + InvMassB + InvIA * _a1 * _a1 + InvIB * _a2 * _a2; + + if (_motorMass > Settings.Epsilon) + { + _motorMass = 1.0f / _motorMass; + } + } + + // Prismatic constraint. + { + _perp = _localYAxis1; + + _s1 = MathUtils.Cross(d + r1, _perp); + _s2 = MathUtils.Cross(r2, _perp); + + float m1 = InvMassA, m2 = InvMassB; + float i1 = InvIA, i2 = InvIB; + + float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; + float k12 = i1 * _s1 + i2 * _s2; + float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2; + float k22 = i1 + i2; + float k23 = i1 * _a1 + i2 * _a2; + float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2; + + _K.Col1 = new Vector3(k11, k12, k13); + _K.Col2 = new Vector3(k12, k22, k23); + _K.Col3 = new Vector3(k13, k23, k33); + } + + // Compute motor and limit terms. + if (_enableLimit) + { + float jointTranslation = Vector2.Dot(_axis, d); + if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) + { + _limitState = LimitState.Equal; + } + else if (jointTranslation <= _lowerTranslation) + { + if (_limitState != LimitState.AtLower) + { + _limitState = LimitState.AtLower; + _impulse.Z = 0.0f; + } + } + else if (jointTranslation >= _upperTranslation) + { + if (_limitState != LimitState.AtUpper) + { + _limitState = LimitState.AtUpper; + _impulse.Z = 0.0f; + } + } + else + { + _limitState = LimitState.Inactive; + _impulse.Z = 0.0f; + } + } + else + { + _limitState = LimitState.Inactive; + } + + if (_enableMotor == false) + { + MotorForce = 0.0f; + } + + if (Settings.EnableWarmstarting) + { + // Account for variable time step. + _impulse *= step.dtRatio; + MotorForce *= step.dtRatio; + + Vector2 P = _impulse.X * _perp + (MotorForce + _impulse.Z) * _axis; + float L2 = _impulse.X * _s2 + _impulse.Y + (MotorForce + _impulse.Z) * _a2; + + bB.LinearVelocityInternal += InvMassB * P; + bB.AngularVelocityInternal += InvIB * L2; + } + else + { + _impulse = Vector3.Zero; + MotorForce = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body bB = BodyB; + + Vector2 v1 = Vector2.Zero; + float w1 = 0.0f; + Vector2 v2 = bB.LinearVelocityInternal; + float w2 = bB.AngularVelocityInternal; + + // Solve linear motor constraint. + if (_enableMotor && _limitState != LimitState.Equal) + { + float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; + float impulse = _motorMass * (_motorSpeed - Cdot); + float oldImpulse = MotorForce; + float maxImpulse = step.dt * _maxMotorForce; + MotorForce = MathUtils.Clamp(MotorForce + impulse, -maxImpulse, maxImpulse); + impulse = MotorForce - oldImpulse; + + Vector2 P = impulse * _axis; + float L1 = impulse * _a1; + float L2 = impulse * _a2; + + v1 -= InvMassA * P; + w1 -= InvIA * L1; + + v2 += InvMassB * P; + w2 += InvIB * L2; + } + + Vector2 Cdot1 = new Vector2(Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1, w2 - w1); + + if (_enableLimit && _limitState != LimitState.Inactive) + { + // Solve prismatic and limit constraint in block form. + float Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; + Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); + + Vector3 f1 = _impulse; + Vector3 df = _K.Solve33(-Cdot); + _impulse += df; + + if (_limitState == LimitState.AtLower) + { + _impulse.Z = Math.Max(_impulse.Z, 0.0f); + } + else if (_limitState == LimitState.AtUpper) + { + _impulse.Z = Math.Min(_impulse.Z, 0.0f); + } + + // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) + Vector2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vector2(_K.Col3.X, _K.Col3.Y); + Vector2 f2r = _K.Solve22(b) + new Vector2(f1.X, f1.Y); + _impulse.X = f2r.X; + _impulse.Y = f2r.Y; + + df = _impulse - f1; + + Vector2 P = df.X * _perp + df.Z * _axis; + float L2 = df.X * _s2 + df.Y + df.Z * _a2; + + v2 += InvMassB * P; + w2 += InvIB * L2; + } + else + { + // Limit is inactive, just solve the prismatic constraint in block form. + Vector2 df = _K.Solve22(-Cdot1); + _impulse.X += df.X; + _impulse.Y += df.Y; + + Vector2 P = df.X * _perp; + float L2 = df.X * _s2 + df.Y; + + v2 += InvMassB * P; + w2 += InvIB * L2; + } + + bB.LinearVelocityInternal = v2; + bB.AngularVelocityInternal = w2; + } + + internal override bool SolvePositionConstraints() + { + //Body b1 = BodyA; + Body b2 = BodyB; + + Vector2 c1 = Vector2.Zero; // b1._sweep.Center; + float a1 = 0.0f; // b1._sweep.Angle; + + Vector2 c2 = b2.Sweep.C; + float a2 = b2.Sweep.A; + + // Solve linear limit constraint. + float linearError = 0.0f; + bool active = false; + float C2 = 0.0f; + + Mat22 R1 = new Mat22(a1); + Mat22 R2 = new Mat22(a2); + + Vector2 r1 = MathUtils.Multiply(ref R1, LocalAnchorA - LocalCenterA); + Vector2 r2 = MathUtils.Multiply(ref R2, LocalAnchorB - LocalCenterB); + Vector2 d = c2 + r2 - c1 - r1; + + if (_enableLimit) + { + _axis = MathUtils.Multiply(ref R1, _localXAxis1); + + _a1 = MathUtils.Cross(d + r1, _axis); + _a2 = MathUtils.Cross(r2, _axis); + + float translation = Vector2.Dot(_axis, d); + if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) + { + // Prevent large angular corrections + C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); + linearError = Math.Abs(translation); + active = true; + } + else if (translation <= _lowerTranslation) + { + // Prevent large linear corrections and allow some slop. + C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop, + -Settings.MaxLinearCorrection, 0.0f); + linearError = _lowerTranslation - translation; + active = true; + } + else if (translation >= _upperTranslation) + { + // Prevent large linear corrections and allow some slop. + C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, + Settings.MaxLinearCorrection); + linearError = translation - _upperTranslation; + active = true; + } + } + + _perp = MathUtils.Multiply(ref R1, _localYAxis1); + + _s1 = MathUtils.Cross(d + r1, _perp); + _s2 = MathUtils.Cross(r2, _perp); + + Vector3 impulse; + Vector2 C1 = new Vector2(Vector2.Dot(_perp, d), a2 - a1 - _refAngle); + + linearError = Math.Max(linearError, Math.Abs(C1.X)); + float angularError = Math.Abs(C1.Y); + + if (active) + { + float m1 = InvMassA, m2 = InvMassB; + float i1 = InvIA, i2 = InvIB; + + float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; + float k12 = i1 * _s1 + i2 * _s2; + float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2; + float k22 = i1 + i2; + float k23 = i1 * _a1 + i2 * _a2; + float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2; + + _K.Col1 = new Vector3(k11, k12, k13); + _K.Col2 = new Vector3(k12, k22, k23); + _K.Col3 = new Vector3(k13, k23, k33); + + Vector3 C = new Vector3(-C1.X, -C1.Y, -C2); + impulse = _K.Solve33(C); // negated above + } + else + { + float m1 = InvMassA, m2 = InvMassB; + float i1 = InvIA, i2 = InvIB; + + float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; + float k12 = i1 * _s1 + i2 * _s2; + float k22 = i1 + i2; + + _K.Col1 = new Vector3(k11, k12, 0.0f); + _K.Col2 = new Vector3(k12, k22, 0.0f); + + Vector2 impulse1 = _K.Solve22(-C1); + impulse.X = impulse1.X; + impulse.Y = impulse1.Y; + impulse.Z = 0.0f; + } + + Vector2 P = impulse.X * _perp + impulse.Z * _axis; + float L2 = impulse.X * _s2 + impulse.Y + impulse.Z * _a2; + + c2 += InvMassB * P; + a2 += InvIB * L2; + + // TODO_ERIN remove need for this. + b2.Sweep.C = c2; + b2.Sweep.A = a2; + b2.SynchronizeTransform(); + + return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; + } + } +} \ No newline at end of file diff --git a/Dynamics/Joints/FixedRevoluteJoint.cs b/Dynamics/Joints/FixedRevoluteJoint.cs new file mode 100644 index 0000000..afc4e4f --- /dev/null +++ b/Dynamics/Joints/FixedRevoluteJoint.cs @@ -0,0 +1,541 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + /// + /// A revolute joint rains to bodies to share a common point while they + /// are free to rotate about the point. The relative rotation about the shared + /// point is the joint angle. You can limit the relative rotation with + /// a joint limit that specifies a lower and upper angle. You can use a motor + /// to drive the relative rotation about the shared point. A maximum motor torque + /// is provided so that infinite forces are not generated. + /// + public class FixedRevoluteJoint : Joint + { + private bool _enableLimit; + private bool _enableMotor; + private Vector3 _impulse; + private LimitState _limitState; + private float _lowerAngle; + private Mat33 _mass; // effective mass for point-to-point constraint. + private float _maxMotorTorque; + private float _motorImpulse; + private float _motorMass; // effective mass for motor/limit angular constraint. + private float _motorSpeed; + private float _upperAngle; + private Vector2 _worldAnchor; + + /// + /// Initialize the bodies, anchors, and reference angle using the world + /// anchor. + /// This requires defining an + /// anchor point where the bodies are joined. The definition + /// uses local anchor points so that the initial configuration + /// can violate the constraint slightly. You also need to + /// specify the initial relative angle for joint limits. This + /// helps when saving and loading a game. + /// The local anchor points are measured from the body's origin + /// rather than the center of mass because: + /// 1. you might not know where the center of mass will be. + /// 2. if you add/remove shapes from a body and recompute the mass, + /// the joints will be broken. + /// + /// The body. + /// The body anchor. + /// The world anchor. + public FixedRevoluteJoint(Body body, Vector2 bodyAnchor, Vector2 worldAnchor) + : base(body) + { + JointType = JointType.FixedRevolute; + + // Changed to local coordinates. + LocalAnchorA = bodyAnchor; + _worldAnchor = worldAnchor; + + ReferenceAngle = -BodyA.Rotation; + + _impulse = Vector3.Zero; + + _limitState = LimitState.Inactive; + } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + } + + public override Vector2 WorldAnchorB + { + get { return _worldAnchor; } + set { _worldAnchor = value; } + } + + public Vector2 LocalAnchorA { get; set; } + + public float ReferenceAngle { get; set; } + + /// + /// Get the current joint angle in radians. + /// + /// + public float JointAngle + { + get { return BodyA.Sweep.A - ReferenceAngle; } + } + + /// + /// Get the current joint angle speed in radians per second. + /// + /// + public float JointSpeed + { + get { return BodyA.AngularVelocityInternal; } + } + + /// + /// Is the joint limit enabled? + /// + /// true if [limit enabled]; otherwise, false. + public bool LimitEnabled + { + get { return _enableLimit; } + set + { + WakeBodies(); + _enableLimit = value; + } + } + + /// + /// Get the lower joint limit in radians. + /// + /// + public float LowerLimit + { + get { return _lowerAngle; } + set + { + WakeBodies(); + _lowerAngle = value; + } + } + + /// + /// Get the upper joint limit in radians. + /// + /// + public float UpperLimit + { + get { return _upperAngle; } + set + { + WakeBodies(); + _upperAngle = value; + } + } + + /// + /// Is the joint motor enabled? + /// + /// true if [motor enabled]; otherwise, false. + public bool MotorEnabled + { + get { return _enableMotor; } + set + { + WakeBodies(); + _enableMotor = value; + } + } + + /// + /// Set the motor speed in radians per second. + /// + /// The speed. + public float MotorSpeed + { + set + { + WakeBodies(); + _motorSpeed = value; + } + get { return _motorSpeed; } + } + + /// + /// Set the maximum motor torque, usually in N-m. + /// + /// The torque. + public float MaxMotorTorque + { + set + { + WakeBodies(); + _maxMotorTorque = value; + } + get { return _maxMotorTorque; } + } + + /// + /// Get the current motor torque, usually in N-m. + /// + /// + public float MotorTorque + { + get { return _motorImpulse; } + set + { + WakeBodies(); + _motorImpulse = value; + } + } + + public override Vector2 GetReactionForce(float inv_dt) + { + return inv_dt * new Vector2(_impulse.X, _impulse.Y); + } + + public override float GetReactionTorque(float inv_dt) + { + return inv_dt * _impulse.Z; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + + if (_enableMotor || _enableLimit) + { + // You cannot create a rotation limit between bodies that + // both have fixed rotation. + Debug.Assert(b1.InvI > 0.0f /* || b2._invI > 0.0f*/); + } + + // Compute the effective mass matrix. + Transform xf1; + b1.GetTransform(out xf1); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = _worldAnchor; // MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter); + + // J = [-I -r1_skew I r2_skew] + // [ 0 -1 0 1] + // r_skew = [-ry; rx] + + // Matlab + // K = [ m1+r1y^2*i1+m2+r2y^2*i2, -r1y*i1*r1x-r2y*i2*r2x, -r1y*i1-r2y*i2] + // [ -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2, r1x*i1+r2x*i2] + // [ -r1y*i1-r2y*i2, r1x*i1+r2x*i2, i1+i2] + + float m1 = b1.InvMass; + const float m2 = 0; + float i1 = b1.InvI; + const float i2 = 0; + + _mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2; + _mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2; + _mass.Col3.X = -r1.Y * i1 - r2.Y * i2; + _mass.Col1.Y = _mass.Col2.X; + _mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2; + _mass.Col3.Y = r1.X * i1 + r2.X * i2; + _mass.Col1.Z = _mass.Col3.X; + _mass.Col2.Z = _mass.Col3.Y; + _mass.Col3.Z = i1 + i2; + + _motorMass = i1 + i2; + if (_motorMass > 0.0f) + { + _motorMass = 1.0f / _motorMass; + } + + if (_enableMotor == false) + { + _motorImpulse = 0.0f; + } + + if (_enableLimit) + { + float jointAngle = 0 - b1.Sweep.A - ReferenceAngle; + if (Math.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop) + { + _limitState = LimitState.Equal; + } + else if (jointAngle <= _lowerAngle) + { + if (_limitState != LimitState.AtLower) + { + _impulse.Z = 0.0f; + } + _limitState = LimitState.AtLower; + } + else if (jointAngle >= _upperAngle) + { + if (_limitState != LimitState.AtUpper) + { + _impulse.Z = 0.0f; + } + _limitState = LimitState.AtUpper; + } + else + { + _limitState = LimitState.Inactive; + _impulse.Z = 0.0f; + } + } + else + { + _limitState = LimitState.Inactive; + } + + if (Settings.EnableWarmstarting) + { + // Scale impulses to support a variable time step. + _impulse *= step.dtRatio; + _motorImpulse *= step.dtRatio; + + Vector2 P = new Vector2(_impulse.X, _impulse.Y); + + b1.LinearVelocityInternal -= m1 * P; + b1.AngularVelocityInternal -= i1 * (MathUtils.Cross(r1, P) + _motorImpulse + _impulse.Z); + } + else + { + _impulse = Vector3.Zero; + _motorImpulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + + Vector2 v1 = b1.LinearVelocityInternal; + float w1 = b1.AngularVelocityInternal; + Vector2 v2 = Vector2.Zero; + const float w2 = 0; + + float m1 = b1.InvMass; + float i1 = b1.InvI; + + // Solve motor constraint. + if (_enableMotor && _limitState != LimitState.Equal) + { + float Cdot = w2 - w1 - _motorSpeed; + float impulse = _motorMass * (-Cdot); + float oldImpulse = _motorImpulse; + float maxImpulse = step.dt * _maxMotorTorque; + _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = _motorImpulse - oldImpulse; + + w1 -= i1 * impulse; + } + + // Solve limit constraint. + if (_enableLimit && _limitState != LimitState.Inactive) + { + Transform xf1; + b1.GetTransform(out xf1); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = _worldAnchor; + + // Solve point-to-point constraint + Vector2 Cdot1 = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1); + float Cdot2 = w2 - w1; + Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); + + Vector3 impulse = _mass.Solve33(-Cdot); + + if (_limitState == LimitState.Equal) + { + _impulse += impulse; + } + else if (_limitState == LimitState.AtLower) + { + float newImpulse = _impulse.Z + impulse.Z; + if (newImpulse < 0.0f) + { + Vector2 reduced = _mass.Solve22(-Cdot1); + impulse.X = reduced.X; + impulse.Y = reduced.Y; + impulse.Z = -_impulse.Z; + _impulse.X += reduced.X; + _impulse.Y += reduced.Y; + _impulse.Z = 0.0f; + } + } + else if (_limitState == LimitState.AtUpper) + { + float newImpulse = _impulse.Z + impulse.Z; + if (newImpulse > 0.0f) + { + Vector2 reduced = _mass.Solve22(-Cdot1); + impulse.X = reduced.X; + impulse.Y = reduced.Y; + impulse.Z = -_impulse.Z; + _impulse.X += reduced.X; + _impulse.Y += reduced.Y; + _impulse.Z = 0.0f; + } + } + + Vector2 P = new Vector2(impulse.X, impulse.Y); + + v1 -= m1 * P; + w1 -= i1 * (MathUtils.Cross(r1, P) + impulse.Z); + } + else + { + Transform xf1; + b1.GetTransform(out xf1); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = _worldAnchor; + + // Solve point-to-point constraint + Vector2 Cdot = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1); + Vector2 impulse = _mass.Solve22(-Cdot); + + _impulse.X += impulse.X; + _impulse.Y += impulse.Y; + + v1 -= m1 * impulse; + w1 -= i1 * MathUtils.Cross(r1, impulse); + } + + b1.LinearVelocityInternal = v1; + b1.AngularVelocityInternal = w1; + } + + internal override bool SolvePositionConstraints() + { + // TODO_ERIN block solve with limit. COME ON ERIN + + Body b1 = BodyA; + + float angularError = 0.0f; + float positionError; + + // Solve angular limit constraint. + if (_enableLimit && _limitState != LimitState.Inactive) + { + float angle = 0 - b1.Sweep.A - ReferenceAngle; + float limitImpulse = 0.0f; + + if (_limitState == LimitState.Equal) + { + // Prevent large angular corrections + float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection, + Settings.MaxAngularCorrection); + limitImpulse = -_motorMass * C; + angularError = Math.Abs(C); + } + else if (_limitState == LimitState.AtLower) + { + float C = angle - _lowerAngle; + angularError = -C; + + // Prevent large angular corrections and allow some slop. + C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f); + limitImpulse = -_motorMass * C; + } + else if (_limitState == LimitState.AtUpper) + { + float C = angle - _upperAngle; + angularError = C; + + // Prevent large angular corrections and allow some slop. + C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection); + limitImpulse = -_motorMass * C; + } + + b1.Sweep.A -= b1.InvI * limitImpulse; + + b1.SynchronizeTransform(); + } + + // Solve point-to-point constraint. + { + Transform xf1; + b1.GetTransform(out xf1); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = _worldAnchor; + + Vector2 C = Vector2.Zero + r2 - b1.Sweep.C - r1; + positionError = C.Length(); + + float invMass1 = b1.InvMass; + const float invMass2 = 0; + float invI1 = b1.InvI; + const float invI2 = 0; + + // Handle large detachment. + const float k_allowedStretch = 10.0f * Settings.LinearSlop; + if (C.LengthSquared() > k_allowedStretch * k_allowedStretch) + { + // Use a particle solution (no rotation). + Vector2 u = C; + u.Normalize(); + float k = invMass1 + invMass2; + Debug.Assert(k > Settings.Epsilon); + float m = 1.0f / k; + Vector2 impulse2 = m * (-C); + const float k_beta = 0.5f; + b1.Sweep.C -= k_beta * invMass1 * impulse2; + + C = Vector2.Zero + r2 - b1.Sweep.C - r1; + } + + Mat22 K1 = new Mat22(new Vector2(invMass1 + invMass2, 0.0f), new Vector2(0.0f, invMass1 + invMass2)); + Mat22 K2 = new Mat22(new Vector2(invI1 * r1.Y * r1.Y, -invI1 * r1.X * r1.Y), + new Vector2(-invI1 * r1.X * r1.Y, invI1 * r1.X * r1.X)); + Mat22 K3 = new Mat22(new Vector2(invI2 * r2.Y * r2.Y, -invI2 * r2.X * r2.Y), + new Vector2(-invI2 * r2.X * r2.Y, invI2 * r2.X * r2.X)); + + Mat22 Ka; + Mat22.Add(ref K1, ref K2, out Ka); + + Mat22 K; + Mat22.Add(ref Ka, ref K3, out K); + + Vector2 impulse = K.Solve(-C); + + b1.Sweep.C -= b1.InvMass * impulse; + b1.Sweep.A -= b1.InvI * MathUtils.Cross(r1, impulse); + + b1.SynchronizeTransform(); + } + + return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; + } + } +} \ No newline at end of file diff --git a/Dynamics/Joints/FrictionJoint.cs b/Dynamics/Joints/FrictionJoint.cs new file mode 100644 index 0000000..75f475f --- /dev/null +++ b/Dynamics/Joints/FrictionJoint.cs @@ -0,0 +1,249 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + // Point-to-point constraint + // Cdot = v2 - v1 + // = v2 + cross(w2, r2) - v1 - cross(w1, r1) + // J = [-I -r1_skew I r2_skew ] + // Identity used: + // w k % (rx i + ry j) = w * (-ry i + rx j) + + // Angle constraint + // Cdot = w2 - w1 + // J = [0 0 -1 0 0 1] + // K = invI1 + invI2 + + /// + /// Friction joint. This is used for top-down friction. + /// It provides 2D translational friction and angular friction. + /// + public class FrictionJoint : Joint + { + public Vector2 LocalAnchorA; + public Vector2 LocalAnchorB; + private float _angularImpulse; + private float _angularMass; + private Vector2 _linearImpulse; + private Mat22 _linearMass; + + internal FrictionJoint() + { + JointType = JointType.Friction; + } + + public FrictionJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB) + : base(bodyA, bodyB) + { + JointType = JointType.Friction; + LocalAnchorA = localAnchorA; + LocalAnchorB = localAnchorB; + } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + /// + /// The maximum friction force in N. + /// + public float MaxForce { get; set; } + + /// + /// The maximum friction torque in N-m. + /// + public float MaxTorque { get; set; } + + public override Vector2 GetReactionForce(float inv_dt) + { + return inv_dt * _linearImpulse; + } + + public override float GetReactionTorque(float inv_dt) + { + return inv_dt * _angularImpulse; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body bA = BodyA; + Body bB = BodyB; + + Transform xfA, xfB; + bA.GetTransform(out xfA); + bB.GetTransform(out xfB); + + // Compute the effective mass matrix. + Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter); + Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter); + + // J = [-I -r1_skew I r2_skew] + // [ 0 -1 0 1] + // r_skew = [-ry; rx] + + // Matlab + // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] + // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] + // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] + + float mA = bA.InvMass, mB = bB.InvMass; + float iA = bA.InvI, iB = bB.InvI; + + Mat22 K1 = new Mat22(); + K1.Col1.X = mA + mB; + K1.Col2.X = 0.0f; + K1.Col1.Y = 0.0f; + K1.Col2.Y = mA + mB; + + Mat22 K2 = new Mat22(); + K2.Col1.X = iA * rA.Y * rA.Y; + K2.Col2.X = -iA * rA.X * rA.Y; + K2.Col1.Y = -iA * rA.X * rA.Y; + K2.Col2.Y = iA * rA.X * rA.X; + + Mat22 K3 = new Mat22(); + K3.Col1.X = iB * rB.Y * rB.Y; + K3.Col2.X = -iB * rB.X * rB.Y; + K3.Col1.Y = -iB * rB.X * rB.Y; + K3.Col2.Y = iB * rB.X * rB.X; + + Mat22 K12; + Mat22.Add(ref K1, ref K2, out K12); + + Mat22 K; + Mat22.Add(ref K12, ref K3, out K); + + _linearMass = K.Inverse; + + _angularMass = iA + iB; + if (_angularMass > 0.0f) + { + _angularMass = 1.0f / _angularMass; + } + + if (Settings.EnableWarmstarting) + { + // Scale impulses to support a variable time step. + _linearImpulse *= step.dtRatio; + _angularImpulse *= step.dtRatio; + + Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y); + + bA.LinearVelocityInternal -= mA * P; + bA.AngularVelocityInternal -= iA * (MathUtils.Cross(rA, P) + _angularImpulse); + + bB.LinearVelocityInternal += mB * P; + bB.AngularVelocityInternal += iB * (MathUtils.Cross(rB, P) + _angularImpulse); + } + else + { + _linearImpulse = Vector2.Zero; + _angularImpulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body bA = BodyA; + Body bB = BodyB; + + Vector2 vA = bA.LinearVelocityInternal; + float wA = bA.AngularVelocityInternal; + Vector2 vB = bB.LinearVelocityInternal; + float wB = bB.AngularVelocityInternal; + + float mA = bA.InvMass, mB = bB.InvMass; + float iA = bA.InvI, iB = bB.InvI; + + Transform xfA, xfB; + bA.GetTransform(out xfA); + bB.GetTransform(out xfB); + + Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter); + Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter); + + // Solve angular friction + { + float Cdot = wB - wA; + float impulse = -_angularMass * Cdot; + + float oldImpulse = _angularImpulse; + float maxImpulse = step.dt * MaxTorque; + _angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse); + impulse = _angularImpulse - oldImpulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + // Solve linear friction + { + Vector2 Cdot = vB + MathUtils.Cross(wB, rB) - vA - MathUtils.Cross(wA, rA); + + Vector2 impulse = -MathUtils.Multiply(ref _linearMass, Cdot); + Vector2 oldImpulse = _linearImpulse; + _linearImpulse += impulse; + + float maxImpulse = step.dt * MaxForce; + + if (_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) + { + _linearImpulse.Normalize(); + _linearImpulse *= maxImpulse; + } + + impulse = _linearImpulse - oldImpulse; + + vA -= mA * impulse; + wA -= iA * MathUtils.Cross(rA, impulse); + + vB += mB * impulse; + wB += iB * MathUtils.Cross(rB, impulse); + } + + bA.LinearVelocityInternal = vA; + bA.AngularVelocityInternal = wA; + bB.LinearVelocityInternal = vB; + bB.AngularVelocityInternal = wB; + } + + internal override bool SolvePositionConstraints() + { + return true; + } + } +} \ No newline at end of file diff --git a/Dynamics/Joints/GearJoint.cs b/Dynamics/Joints/GearJoint.cs new file mode 100644 index 0000000..17eba31 --- /dev/null +++ b/Dynamics/Joints/GearJoint.cs @@ -0,0 +1,350 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + /// + /// A gear joint is used to connect two joints together. Either joint + /// can be a revolute or prismatic joint. You specify a gear ratio + /// to bind the motions together: + /// coordinate1 + ratio * coordinate2 = ant + /// The ratio can be negative or positive. If one joint is a revolute joint + /// and the other joint is a prismatic joint, then the ratio will have units + /// of length or units of 1/length. + /// @warning The revolute and prismatic joints must be attached to + /// fixed bodies (which must be body1 on those joints). + /// + public class GearJoint : Joint + { + private Jacobian _J; + + private float _ant; + private FixedPrismaticJoint _fixedPrismatic1; + private FixedPrismaticJoint _fixedPrismatic2; + private FixedRevoluteJoint _fixedRevolute1; + private FixedRevoluteJoint _fixedRevolute2; + private float _impulse; + private float _mass; + private PrismaticJoint _prismatic1; + private PrismaticJoint _prismatic2; + private RevoluteJoint _revolute1; + private RevoluteJoint _revolute2; + + /// + /// Requires two existing revolute or prismatic joints (any combination will work). + /// The provided joints must attach a dynamic body to a static body. + /// + /// The first joint. + /// The second joint. + /// The ratio. + public GearJoint(Joint jointA, Joint jointB, float ratio) + : base(jointA.BodyA, jointA.BodyB) + { + JointType = JointType.Gear; + JointA = jointA; + JointB = jointB; + Ratio = ratio; + + JointType type1 = jointA.JointType; + JointType type2 = jointB.JointType; + + // Make sure its the right kind of joint + Debug.Assert(type1 == JointType.Revolute || + type1 == JointType.Prismatic || + type1 == JointType.FixedRevolute || + type1 == JointType.FixedPrismatic); + Debug.Assert(type2 == JointType.Revolute || + type2 == JointType.Prismatic || + type2 == JointType.FixedRevolute || + type2 == JointType.FixedPrismatic); + + // In the case of a prismatic and revolute joint, the first body must be static. + if (type1 == JointType.Revolute || type1 == JointType.Prismatic) + Debug.Assert(jointA.BodyA.BodyType == BodyType.Static); + if (type2 == JointType.Revolute || type2 == JointType.Prismatic) + Debug.Assert(jointB.BodyA.BodyType == BodyType.Static); + + float coordinate1 = 0.0f, coordinate2 = 0.0f; + + switch (type1) + { + case JointType.Revolute: + BodyA = jointA.BodyB; + _revolute1 = (RevoluteJoint)jointA; + LocalAnchor1 = _revolute1.LocalAnchorB; + coordinate1 = _revolute1.JointAngle; + break; + case JointType.Prismatic: + BodyA = jointA.BodyB; + _prismatic1 = (PrismaticJoint)jointA; + LocalAnchor1 = _prismatic1.LocalAnchorB; + coordinate1 = _prismatic1.JointTranslation; + break; + case JointType.FixedRevolute: + BodyA = jointA.BodyA; + _fixedRevolute1 = (FixedRevoluteJoint)jointA; + LocalAnchor1 = _fixedRevolute1.LocalAnchorA; + coordinate1 = _fixedRevolute1.JointAngle; + break; + case JointType.FixedPrismatic: + BodyA = jointA.BodyA; + _fixedPrismatic1 = (FixedPrismaticJoint)jointA; + LocalAnchor1 = _fixedPrismatic1.LocalAnchorA; + coordinate1 = _fixedPrismatic1.JointTranslation; + break; + } + + switch (type2) + { + case JointType.Revolute: + BodyB = jointB.BodyB; + _revolute2 = (RevoluteJoint)jointB; + LocalAnchor2 = _revolute2.LocalAnchorB; + coordinate2 = _revolute2.JointAngle; + break; + case JointType.Prismatic: + BodyB = jointB.BodyB; + _prismatic2 = (PrismaticJoint)jointB; + LocalAnchor2 = _prismatic2.LocalAnchorB; + coordinate2 = _prismatic2.JointTranslation; + break; + case JointType.FixedRevolute: + BodyB = jointB.BodyA; + _fixedRevolute2 = (FixedRevoluteJoint)jointB; + LocalAnchor2 = _fixedRevolute2.LocalAnchorA; + coordinate2 = _fixedRevolute2.JointAngle; + break; + case JointType.FixedPrismatic: + BodyB = jointB.BodyA; + _fixedPrismatic2 = (FixedPrismaticJoint)jointB; + LocalAnchor2 = _fixedPrismatic2.LocalAnchorA; + coordinate2 = _fixedPrismatic2.JointTranslation; + break; + } + + _ant = coordinate1 + Ratio * coordinate2; + } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchor1); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchor2); } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + /// + /// The gear ratio. + /// + public float Ratio { get; set; } + + /// + /// The first revolute/prismatic joint attached to the gear joint. + /// + public Joint JointA { get; set; } + + /// + /// The second revolute/prismatic joint attached to the gear joint. + /// + public Joint JointB { get; set; } + + public Vector2 LocalAnchor1 { get; private set; } + public Vector2 LocalAnchor2 { get; private set; } + + public override Vector2 GetReactionForce(float inv_dt) + { + Vector2 P = _impulse * _J.LinearB; + return inv_dt * P; + } + + public override float GetReactionTorque(float inv_dt) + { + Transform xf1; + BodyB.GetTransform(out xf1); + + Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchor2 - BodyB.LocalCenter); + Vector2 P = _impulse * _J.LinearB; + float L = _impulse * _J.AngularB - MathUtils.Cross(r, P); + return inv_dt * L; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + Body b2 = BodyB; + + float K = 0.0f; + _J.SetZero(); + + if (_revolute1 != null || _fixedRevolute1 != null) + { + _J.AngularA = -1.0f; + K += b1.InvI; + } + else + { + Vector2 ug; + if (_prismatic1 != null) + ug = _prismatic1.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1); + else + ug = _fixedPrismatic1.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1); + + Transform xf1 /*, xfg1*/; + b1.GetTransform(out xf1); + //g1.GetTransform(out xfg1); + + + Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchor1 - b1.LocalCenter); + float crug = MathUtils.Cross(r, ug); + _J.LinearA = -ug; + _J.AngularA = -crug; + K += b1.InvMass + b1.InvI * crug * crug; + } + + if (_revolute2 != null || _fixedRevolute2 != null) + { + _J.AngularB = -Ratio; + K += Ratio * Ratio * b2.InvI; + } + else + { + Vector2 ug; + if (_prismatic2 != null) + ug = _prismatic2.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1); + else + ug = _fixedPrismatic2.LocalXAxis1; // MathUtils.Multiply(ref xfg1.R, _prismatic1.LocalXAxis1); + + Transform /*xfg1,*/ xf2; + //g1.GetTransform(out xfg1); + b2.GetTransform(out xf2); + + Vector2 r = MathUtils.Multiply(ref xf2.R, LocalAnchor2 - b2.LocalCenter); + float crug = MathUtils.Cross(r, ug); + _J.LinearB = -Ratio * ug; + _J.AngularB = -Ratio * crug; + K += Ratio * Ratio * (b2.InvMass + b2.InvI * crug * crug); + } + + // Compute effective mass. + Debug.Assert(K > 0.0f); + _mass = K > 0.0f ? 1.0f / K : 0.0f; + + if (Settings.EnableWarmstarting) + { + // Warm starting. + b1.LinearVelocityInternal += b1.InvMass * _impulse * _J.LinearA; + b1.AngularVelocityInternal += b1.InvI * _impulse * _J.AngularA; + b2.LinearVelocityInternal += b2.InvMass * _impulse * _J.LinearB; + b2.AngularVelocityInternal += b2.InvI * _impulse * _J.AngularB; + } + else + { + _impulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + Body b2 = BodyB; + + float Cdot = _J.Compute(b1.LinearVelocityInternal, b1.AngularVelocityInternal, + b2.LinearVelocityInternal, b2.AngularVelocityInternal); + + float impulse = _mass * (-Cdot); + _impulse += impulse; + + b1.LinearVelocityInternal += b1.InvMass * impulse * _J.LinearA; + b1.AngularVelocityInternal += b1.InvI * impulse * _J.AngularA; + b2.LinearVelocityInternal += b2.InvMass * impulse * _J.LinearB; + b2.AngularVelocityInternal += b2.InvI * impulse * _J.AngularB; + } + + internal override bool SolvePositionConstraints() + { + const float linearError = 0.0f; + + Body b1 = BodyA; + Body b2 = BodyB; + + float coordinate1 = 0.0f, coordinate2 = 0.0f; + if (_revolute1 != null) + { + coordinate1 = _revolute1.JointAngle; + } + else if (_fixedRevolute1 != null) + { + coordinate1 = _fixedRevolute1.JointAngle; + } + else if (_prismatic1 != null) + { + coordinate1 = _prismatic1.JointTranslation; + } + else if (_fixedPrismatic1 != null) + { + coordinate1 = _fixedPrismatic1.JointTranslation; + } + + if (_revolute2 != null) + { + coordinate2 = _revolute2.JointAngle; + } + else if (_fixedRevolute2 != null) + { + coordinate2 = _fixedRevolute2.JointAngle; + } + else if (_prismatic2 != null) + { + coordinate2 = _prismatic2.JointTranslation; + } + else if (_fixedPrismatic2 != null) + { + coordinate2 = _fixedPrismatic2.JointTranslation; + } + + float C = _ant - (coordinate1 + Ratio * coordinate2); + + float impulse = _mass * (-C); + + b1.Sweep.C += b1.InvMass * impulse * _J.LinearA; + b1.Sweep.A += b1.InvI * impulse * _J.AngularA; + b2.Sweep.C += b2.InvMass * impulse * _J.LinearB; + b2.Sweep.A += b2.InvI * impulse * _J.AngularB; + + b1.SynchronizeTransform(); + b2.SynchronizeTransform(); + + // TODO_ERIN not implemented + return linearError < Settings.LinearSlop; + } + } +} \ No newline at end of file diff --git a/Dynamics/Joints/Joint.cs b/Dynamics/Joints/Joint.cs new file mode 100644 index 0000000..fa07561 --- /dev/null +++ b/Dynamics/Joints/Joint.cs @@ -0,0 +1,282 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + public enum JointType + { + Revolute, + Prismatic, + Distance, + Pulley, + Gear, + Line, + Weld, + Friction, + Slider, + Angle, + Rope, + FixedMouse, + FixedRevolute, + FixedDistance, + FixedLine, + FixedPrismatic, + FixedAngle, + FixedFriction, + } + + public enum LimitState + { + Inactive, + AtLower, + AtUpper, + Equal, + } + + internal struct Jacobian + { + public float AngularA; + public float AngularB; + public Vector2 LinearA; + public Vector2 LinearB; + + public void SetZero() + { + LinearA = Vector2.Zero; + AngularA = 0.0f; + LinearB = Vector2.Zero; + AngularB = 0.0f; + } + + public void Set(Vector2 x1, float a1, Vector2 x2, float a2) + { + LinearA = x1; + AngularA = a1; + LinearB = x2; + AngularB = a2; + } + + public float Compute(Vector2 x1, float a1, Vector2 x2, float a2) + { + return Vector2.Dot(LinearA, x1) + AngularA * a1 + Vector2.Dot(LinearB, x2) + AngularB * a2; + } + } + + /// + /// A joint edge is used to connect bodies and joints together + /// in a joint graph where each body is a node and each joint + /// is an edge. A joint edge belongs to a doubly linked list + /// maintained in each attached body. Each joint has two joint + /// nodes, one for each attached body. + /// + public sealed class JointEdge + { + /// + /// The joint. + /// + public Joint Joint; + + /// + /// The next joint edge in the body's joint list. + /// + public JointEdge Next; + + /// + /// Provides quick access to the other body attached. + /// + public Body Other; + + /// + /// The previous joint edge in the body's joint list. + /// + public JointEdge Prev; + } + + public abstract class Joint + { + /// + /// The Breakpoint simply indicates the maximum Value the JointError can be before it breaks. + /// The default value is float.MaxValue + /// + public float Breakpoint = float.MaxValue; + + internal JointEdge EdgeA = new JointEdge(); + internal JointEdge EdgeB = new JointEdge(); + public bool Enabled = true; + protected float InvIA; + protected float InvIB; + protected float InvMassA; + protected float InvMassB; + internal bool IslandFlag; + protected Vector2 LocalCenterA, LocalCenterB; + + protected Joint() + { + } + + protected Joint(Body body, Body bodyB) + { + Debug.Assert(body != bodyB); + + BodyA = body; + BodyB = bodyB; + + //Connected bodies should not collide by default + CollideConnected = false; + } + + /// + /// Constructor for fixed joint + /// + protected Joint(Body body) + { + BodyA = body; + + //Connected bodies should not collide by default + CollideConnected = false; + } + + /// + /// Gets or sets the type of the joint. + /// + /// The type of the joint. + public JointType JointType { get; protected set; } + + /// + /// Get the first body attached to this joint. + /// + /// + public Body BodyA { get; set; } + + /// + /// Get the second body attached to this joint. + /// + /// + public Body BodyB { get; set; } + + /// + /// Get the anchor point on body1 in world coordinates. + /// + /// + public abstract Vector2 WorldAnchorA { get; } + + /// + /// Get the anchor point on body2 in world coordinates. + /// + /// + public abstract Vector2 WorldAnchorB { get; set; } + + /// + /// Set the user data pointer. + /// + /// The data. + public object UserData { get; set; } + + /// + /// Short-cut function to determine if either body is inactive. + /// + /// true if active; otherwise, false. + public bool Active + { + get { return BodyA.Enabled && BodyB.Enabled; } + } + + /// + /// Set this flag to true if the attached bodies should collide. + /// + public bool CollideConnected { get; set; } + + /// + /// Fires when the joint is broken. + /// + public event Action Broke; + + /// + /// Get the reaction force on body2 at the joint anchor in Newtons. + /// + /// The inv_dt. + /// + public abstract Vector2 GetReactionForce(float inv_dt); + + /// + /// Get the reaction torque on body2 in N*m. + /// + /// The inv_dt. + /// + public abstract float GetReactionTorque(float inv_dt); + + protected void WakeBodies() + { + BodyA.Awake = true; + if (BodyB != null) + { + BodyB.Awake = true; + } + } + + /// + /// Return true if the joint is a fixed type. + /// + public bool IsFixedType() + { + return JointType == JointType.FixedRevolute || + JointType == JointType.FixedDistance || + JointType == JointType.FixedPrismatic || + JointType == JointType.FixedLine || + JointType == JointType.FixedMouse || + JointType == JointType.FixedAngle || + JointType == JointType.FixedFriction; + } + + internal abstract void InitVelocityConstraints(ref TimeStep step); + + internal void Validate(float invDT) + { + if (!Enabled) + return; + + float jointError = GetReactionForce(invDT).Length(); + if (Math.Abs(jointError) <= Breakpoint) + return; + + Enabled = false; + + if (Broke != null) + Broke(this, jointError); + } + + internal abstract void SolveVelocityConstraints(ref TimeStep step); + + /// + /// Solves the position constraints. + /// + /// returns true if the position errors are within tolerance. + internal abstract bool SolvePositionConstraints(); + } +} \ No newline at end of file diff --git a/Dynamics/Joints/LineJoint.cs b/Dynamics/Joints/LineJoint.cs new file mode 100644 index 0000000..0567e30 --- /dev/null +++ b/Dynamics/Joints/LineJoint.cs @@ -0,0 +1,436 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + public class LineJoint : Joint + { + private Vector2 _ax, _ay; + private float _bias; + private bool _enableMotor; + private float _gamma; + private float _impulse; + private Vector2 _localXAxis; + private Vector2 _localYAxisA; + private float _mass; + private float _maxMotorTorque; + private float _motorImpulse; + private float _motorMass; + private float _motorSpeed; + + private float _sAx; + private float _sAy; + private float _sBx; + private float _sBy; + + private float _springImpulse; + private float _springMass; + + // Linear constraint (point-to-line) + // d = pB - pA = xB + rB - xA - rA + // C = dot(ay, d) + // Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA)) + // = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB) + // J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)] + + // Spring linear constraint + // C = dot(ax, d) + // Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB) + // J = [-ax -cross(d+rA, ax) ax cross(rB, ax)] + + // Motor rotational constraint + // Cdot = wB - wA + // J = [0 0 -1 0 0 1] + + internal LineJoint() + { + JointType = JointType.Line; + } + + public LineJoint(Body bA, Body bB, Vector2 anchor, Vector2 axis) + : base(bA, bB) + { + JointType = JointType.Line; + + LocalAnchorA = bA.GetLocalPoint(anchor); + LocalAnchorB = bB.GetLocalPoint(anchor); + LocalXAxis = bA.GetLocalVector(axis); + } + + public Vector2 LocalAnchorA { get; set; } + + public Vector2 LocalAnchorB { get; set; } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + public float JointTranslation + { + get + { + Body bA = BodyA; + Body bB = BodyB; + + Vector2 pA = bA.GetWorldPoint(LocalAnchorA); + Vector2 pB = bB.GetWorldPoint(LocalAnchorB); + Vector2 d = pB - pA; + Vector2 axis = bA.GetWorldVector(LocalXAxis); + + float translation = Vector2.Dot(d, axis); + return translation; + } + } + + public float JointSpeed + { + get + { + float wA = BodyA.AngularVelocityInternal; + float wB = BodyB.AngularVelocityInternal; + return wB - wA; + } + } + + public bool MotorEnabled + { + get { return _enableMotor; } + set + { + BodyA.Awake = true; + BodyB.Awake = true; + _enableMotor = value; + } + } + + public float MotorSpeed + { + set + { + BodyA.Awake = true; + BodyB.Awake = true; + _motorSpeed = value; + } + get { return _motorSpeed; } + } + + public float MaxMotorTorque + { + set + { + BodyA.Awake = true; + BodyB.Awake = true; + _maxMotorTorque = value; + } + get { return _maxMotorTorque; } + } + + public float Frequency { get; set; } + + public float DampingRatio { get; set; } + + public Vector2 LocalXAxis + { + get { return _localXAxis; } + set + { + _localXAxis = value; + _localYAxisA = MathUtils.Cross(1.0f, _localXAxis); + } + } + + public override Vector2 GetReactionForce(float invDt) + { + return invDt * (_impulse * _ay + _springImpulse * _ax); + } + + public override float GetReactionTorque(float invDt) + { + return invDt * _motorImpulse; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body bA = BodyA; + Body bB = BodyB; + + LocalCenterA = bA.LocalCenter; + LocalCenterB = bB.LocalCenter; + + Transform xfA; + bA.GetTransform(out xfA); + Transform xfB; + bB.GetTransform(out xfB); + + // Compute the effective masses. + Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - LocalCenterA); + Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - LocalCenterB); + Vector2 d = bB.Sweep.C + rB - bA.Sweep.C - rA; + + InvMassA = bA.InvMass; + InvIA = bA.InvI; + InvMassB = bB.InvMass; + InvIB = bB.InvI; + + // Point to line constraint + { + _ay = MathUtils.Multiply(ref xfA.R, _localYAxisA); + _sAy = MathUtils.Cross(d + rA, _ay); + _sBy = MathUtils.Cross(rB, _ay); + + _mass = InvMassA + InvMassB + InvIA * _sAy * _sAy + InvIB * _sBy * _sBy; + + if (_mass > 0.0f) + { + _mass = 1.0f / _mass; + } + } + + // Spring constraint + _springMass = 0.0f; + if (Frequency > 0.0f) + { + _ax = MathUtils.Multiply(ref xfA.R, LocalXAxis); + _sAx = MathUtils.Cross(d + rA, _ax); + _sBx = MathUtils.Cross(rB, _ax); + + float invMass = InvMassA + InvMassB + InvIA * _sAx * _sAx + InvIB * _sBx * _sBx; + + if (invMass > 0.0f) + { + _springMass = 1.0f / invMass; + + float C = Vector2.Dot(d, _ax); + + // Frequency + float omega = 2.0f * Settings.Pi * Frequency; + + // Damping coefficient + float da = 2.0f * _springMass * DampingRatio * omega; + + // Spring stiffness + float k = _springMass * omega * omega; + + // magic formulas + _gamma = step.dt * (da + step.dt * k); + if (_gamma > 0.0f) + { + _gamma = 1.0f / _gamma; + } + + _bias = C * step.dt * k * _gamma; + + _springMass = invMass + _gamma; + if (_springMass > 0.0f) + { + _springMass = 1.0f / _springMass; + } + } + } + else + { + _springImpulse = 0.0f; + _springMass = 0.0f; + } + + // Rotational motor + if (_enableMotor) + { + _motorMass = InvIA + InvIB; + if (_motorMass > 0.0f) + { + _motorMass = 1.0f / _motorMass; + } + } + else + { + _motorMass = 0.0f; + _motorImpulse = 0.0f; + } + + if (Settings.EnableWarmstarting) + { + // Account for variable time step. + _impulse *= step.dtRatio; + _springImpulse *= step.dtRatio; + _motorImpulse *= step.dtRatio; + + Vector2 P = _impulse * _ay + _springImpulse * _ax; + float LA = _impulse * _sAy + _springImpulse * _sAx + _motorImpulse; + float LB = _impulse * _sBy + _springImpulse * _sBx + _motorImpulse; + + bA.LinearVelocityInternal -= InvMassA * P; + bA.AngularVelocityInternal -= InvIA * LA; + + bB.LinearVelocityInternal += InvMassB * P; + bB.AngularVelocityInternal += InvIB * LB; + } + else + { + _impulse = 0.0f; + _springImpulse = 0.0f; + _motorImpulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body bA = BodyA; + Body bB = BodyB; + + Vector2 vA = bA.LinearVelocity; + float wA = bA.AngularVelocityInternal; + Vector2 vB = bB.LinearVelocityInternal; + float wB = bB.AngularVelocityInternal; + + // Solve spring constraint + { + float Cdot = Vector2.Dot(_ax, vB - vA) + _sBx * wB - _sAx * wA; + float impulse = -_springMass * (Cdot + _bias + _gamma * _springImpulse); + _springImpulse += impulse; + + Vector2 P = impulse * _ax; + float LA = impulse * _sAx; + float LB = impulse * _sBx; + + vA -= InvMassA * P; + wA -= InvIA * LA; + + vB += InvMassB * P; + wB += InvIB * LB; + } + + // Solve rotational motor constraint + { + float Cdot = wB - wA - _motorSpeed; + float impulse = -_motorMass * Cdot; + + float oldImpulse = _motorImpulse; + float maxImpulse = step.dt * _maxMotorTorque; + _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = _motorImpulse - oldImpulse; + + wA -= InvIA * impulse; + wB += InvIB * impulse; + } + + // Solve point to line constraint + { + float Cdot = Vector2.Dot(_ay, vB - vA) + _sBy * wB - _sAy * wA; + float impulse = _mass * (-Cdot); + _impulse += impulse; + + Vector2 P = impulse * _ay; + float LA = impulse * _sAy; + float LB = impulse * _sBy; + + vA -= InvMassA * P; + wA -= InvIA * LA; + + vB += InvMassB * P; + wB += InvIB * LB; + } + + bA.LinearVelocityInternal = vA; + bA.AngularVelocityInternal = wA; + bB.LinearVelocityInternal = vB; + bB.AngularVelocityInternal = wB; + } + + internal override bool SolvePositionConstraints() + { + Body bA = BodyA; + Body bB = BodyB; + + Vector2 xA = bA.Sweep.C; + float angleA = bA.Sweep.A; + + Vector2 xB = bB.Sweep.C; + float angleB = bB.Sweep.A; + + Mat22 RA = new Mat22(angleA); + Mat22 RB = new Mat22(angleB); + + Vector2 rA = MathUtils.Multiply(ref RA, LocalAnchorA - LocalCenterA); + Vector2 rB = MathUtils.Multiply(ref RB, LocalAnchorB - LocalCenterB); + Vector2 d = xB + rB - xA - rA; + + Vector2 ay = MathUtils.Multiply(ref RA, _localYAxisA); + + float sAy = MathUtils.Cross(d + rA, ay); + float sBy = MathUtils.Cross(rB, ay); + + float C = Vector2.Dot(d, ay); + + float k = InvMassA + InvMassB + InvIA * _sAy * _sAy + InvIB * _sBy * _sBy; + + float impulse; + if (k != 0.0f) + { + impulse = -C / k; + } + else + { + impulse = 0.0f; + } + + Vector2 P = impulse * ay; + float LA = impulse * sAy; + float LB = impulse * sBy; + + xA -= InvMassA * P; + angleA -= InvIA * LA; + xB += InvMassB * P; + angleB += InvIB * LB; + + // TODO_ERIN remove need for this. + bA.Sweep.C = xA; + bA.Sweep.A = angleA; + bB.Sweep.C = xB; + bB.Sweep.A = angleB; + bA.SynchronizeTransform(); + bB.SynchronizeTransform(); + + return Math.Abs(C) <= Settings.LinearSlop; + } + + public float GetMotorTorque(float invDt) + { + return invDt * _motorImpulse; + } + } +} \ No newline at end of file diff --git a/Dynamics/Joints/PrismaticJoint.cs b/Dynamics/Joints/PrismaticJoint.cs new file mode 100644 index 0000000..0e229a6 --- /dev/null +++ b/Dynamics/Joints/PrismaticJoint.cs @@ -0,0 +1,677 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + // Linear constraint (point-to-line) + // d = p2 - p1 = x2 + r2 - x1 - r1 + // C = dot(perp, d) + // Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1)) + // = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2) + // J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)] + // + // Angular constraint + // C = a2 - a1 + a_initial + // Cdot = w2 - w1 + // J = [0 0 -1 0 0 1] + // + // K = J * invM * JT + // + // J = [-a -s1 a s2] + // [0 -1 0 1] + // a = perp + // s1 = cross(d + r1, a) = cross(p2 - x1, a) + // s2 = cross(r2, a) = cross(p2 - x2, a) + // Motor/Limit linear constraint + // C = dot(ax1, d) + // Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2) + // J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)] + // Block Solver + // We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even + // when the mass has poor distribution (leading to large torques about the joint anchor points). + // + // The Jacobian has 3 rows: + // J = [-uT -s1 uT s2] // linear + // [0 -1 0 1] // angular + // [-vT -a1 vT a2] // limit + // + // u = perp + // v = axis + // s1 = cross(d + r1, u), s2 = cross(r2, u) + // a1 = cross(d + r1, v), a2 = cross(r2, v) + // M * (v2 - v1) = JT * df + // J * v2 = bias + // + // v2 = v1 + invM * JT * df + // J * (v1 + invM * JT * df) = bias + // K * df = bias - J * v1 = -Cdot + // K = J * invM * JT + // Cdot = J * v1 - bias + // + // Now solve for f2. + // df = f2 - f1 + // K * (f2 - f1) = -Cdot + // f2 = invK * (-Cdot) + f1 + // + // Clamp accumulated limit impulse. + // lower: f2(3) = max(f2(3), 0) + // upper: f2(3) = min(f2(3), 0) + // + // Solve for correct f2(1:2) + // K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1 + // = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3) + // K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2) + // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) + // + // Now compute impulse to be applied: + // df = f2 - f1 + + /// + /// A prismatic joint. This joint provides one degree of freedom: translation + /// along an axis fixed in body1. Relative rotation is prevented. You can + /// use a joint limit to restrict the range of motion and a joint motor to + /// drive the motion or to model joint friction. + /// + public class PrismaticJoint : Joint + { + public Vector2 LocalAnchorA; + + public Vector2 LocalAnchorB; + private Mat33 _K; + private float _a1, _a2; + private Vector2 _axis; + private bool _enableLimit; + private bool _enableMotor; + private Vector3 _impulse; + private LimitState _limitState; + private Vector2 _localXAxis1; + private Vector2 _localYAxis1; + private float _lowerTranslation; + private float _maxMotorForce; + private float _motorImpulse; + private float _motorMass; // effective mass for motor/limit translational constraint. + private float _motorSpeed; + private Vector2 _perp; + private float _refAngle; + private float _s1, _s2; + private float _upperTranslation; + + internal PrismaticJoint() + { + JointType = JointType.Prismatic; + } + + /// + /// This requires defining a line of + /// motion using an axis and an anchor point. The definition uses local + /// anchor points and a local axis so that the initial configuration + /// can violate the constraint slightly. The joint translation is zero + /// when the local anchor points coincide in world space. Using local + /// anchors and a local axis helps when saving and loading a game. + /// + /// The first body. + /// The second body. + /// The first body anchor. + /// The second body anchor. + /// The axis. + public PrismaticJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB, Vector2 axis) + : base(bodyA, bodyB) + { + JointType = JointType.Prismatic; + + LocalAnchorA = localAnchorA; + LocalAnchorB = localAnchorB; + + _localXAxis1 = BodyA.GetLocalVector(axis); + _localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1); + _refAngle = BodyB.Rotation - BodyA.Rotation; + + _limitState = LimitState.Inactive; + } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + /// + /// Get the current joint translation, usually in meters. + /// + /// + public float JointTranslation + { + get + { + Vector2 d = BodyB.GetWorldPoint(LocalAnchorB) - BodyA.GetWorldPoint(LocalAnchorA); + Vector2 axis = BodyA.GetWorldVector(ref _localXAxis1); + + return Vector2.Dot(d, axis); + } + } + + /// + /// Get the current joint translation speed, usually in meters per second. + /// + /// + public float JointSpeed + { + get + { + Transform xf1, xf2; + BodyA.GetTransform(out xf1); + BodyB.GetTransform(out xf2); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - BodyA.LocalCenter); + Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - BodyB.LocalCenter); + Vector2 p1 = BodyA.Sweep.C + r1; + Vector2 p2 = BodyB.Sweep.C + r2; + Vector2 d = p2 - p1; + Vector2 axis = BodyA.GetWorldVector(ref _localXAxis1); + + Vector2 v1 = BodyA.LinearVelocityInternal; + Vector2 v2 = BodyB.LinearVelocityInternal; + float w1 = BodyA.AngularVelocityInternal; + float w2 = BodyB.AngularVelocityInternal; + + float speed = Vector2.Dot(d, MathUtils.Cross(w1, axis)) + + Vector2.Dot(axis, v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1)); + return speed; + } + } + + /// + /// Is the joint limit enabled? + /// + /// true if [limit enabled]; otherwise, false. + public bool LimitEnabled + { + get { return _enableLimit; } + set + { + Debug.Assert(BodyA.FixedRotation == false || BodyB.FixedRotation == false, + "Warning: limits does currently not work with fixed rotation"); + + WakeBodies(); + _enableLimit = value; + } + } + + /// + /// Get the lower joint limit, usually in meters. + /// + /// + public float LowerLimit + { + get { return _lowerTranslation; } + set + { + WakeBodies(); + _lowerTranslation = value; + } + } + + /// + /// Get the upper joint limit, usually in meters. + /// + /// + public float UpperLimit + { + get { return _upperTranslation; } + set + { + WakeBodies(); + _upperTranslation = value; + } + } + + /// + /// Is the joint motor enabled? + /// + /// true if [motor enabled]; otherwise, false. + public bool MotorEnabled + { + get { return _enableMotor; } + set + { + WakeBodies(); + _enableMotor = value; + } + } + + /// + /// Set the motor speed, usually in meters per second. + /// + /// The speed. + public float MotorSpeed + { + set + { + WakeBodies(); + _motorSpeed = value; + } + get { return _motorSpeed; } + } + + /// + /// Set the maximum motor force, usually in N. + /// + /// The force. + public float MaxMotorForce + { + get { return _maxMotorForce; } + set + { + WakeBodies(); + _maxMotorForce = value; + } + } + + /// + /// Get the current motor force, usually in N. + /// + /// + public float MotorForce + { + get { return _motorImpulse; } + set { _motorImpulse = value; } + } + + public Vector2 LocalXAxis1 + { + get { return _localXAxis1; } + set + { + _localXAxis1 = BodyA.GetLocalVector(value); + _localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1); + } + } + + public float ReferenceAngle + { + get { return _refAngle; } + set { _refAngle = value; } + } + + public override Vector2 GetReactionForce(float inv_dt) + { + return inv_dt * (_impulse.X * _perp + (_motorImpulse + _impulse.Z) * _axis); + } + + public override float GetReactionTorque(float inv_dt) + { + return inv_dt * _impulse.Y; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + Body b2 = BodyB; + + LocalCenterA = b1.LocalCenter; + LocalCenterB = b2.LocalCenter; + + Transform xf1, xf2; + b1.GetTransform(out xf1); + b2.GetTransform(out xf2); + + // Compute the effective masses. + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - LocalCenterA); + Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - LocalCenterB); + Vector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1; + + InvMassA = b1.InvMass; + InvIA = b1.InvI; + InvMassB = b2.InvMass; + InvIB = b2.InvI; + + // Compute motor Jacobian and effective mass. + { + _axis = MathUtils.Multiply(ref xf1.R, _localXAxis1); + _a1 = MathUtils.Cross(d + r1, _axis); + _a2 = MathUtils.Cross(r2, _axis); + + _motorMass = InvMassA + InvMassB + InvIA * _a1 * _a1 + InvIB * _a2 * _a2; + + if (_motorMass > Settings.Epsilon) + { + _motorMass = 1.0f / _motorMass; + } + } + + // Prismatic constraint. + { + _perp = MathUtils.Multiply(ref xf1.R, _localYAxis1); + + _s1 = MathUtils.Cross(d + r1, _perp); + _s2 = MathUtils.Cross(r2, _perp); + + float m1 = InvMassA, m2 = InvMassB; + float i1 = InvIA, i2 = InvIB; + + float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; + float k12 = i1 * _s1 + i2 * _s2; + float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2; + float k22 = i1 + i2; + float k23 = i1 * _a1 + i2 * _a2; + float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2; + + _K.Col1 = new Vector3(k11, k12, k13); + _K.Col2 = new Vector3(k12, k22, k23); + _K.Col3 = new Vector3(k13, k23, k33); + } + + // Compute motor and limit terms. + if (_enableLimit) + { + float jointTranslation = Vector2.Dot(_axis, d); + if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) + { + _limitState = LimitState.Equal; + } + else if (jointTranslation <= _lowerTranslation) + { + if (_limitState != LimitState.AtLower) + { + _limitState = LimitState.AtLower; + _impulse.Z = 0.0f; + } + } + else if (jointTranslation >= _upperTranslation) + { + if (_limitState != LimitState.AtUpper) + { + _limitState = LimitState.AtUpper; + _impulse.Z = 0.0f; + } + } + else + { + _limitState = LimitState.Inactive; + _impulse.Z = 0.0f; + } + } + else + { + _limitState = LimitState.Inactive; + } + + if (_enableMotor == false) + { + _motorImpulse = 0.0f; + } + + if (Settings.EnableWarmstarting) + { + // Account for variable time step. + _impulse *= step.dtRatio; + _motorImpulse *= step.dtRatio; + + Vector2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Z) * _axis; + float L1 = _impulse.X * _s1 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a1; + float L2 = _impulse.X * _s2 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a2; + + b1.LinearVelocityInternal -= InvMassA * P; + b1.AngularVelocityInternal -= InvIA * L1; + + b2.LinearVelocityInternal += InvMassB * P; + b2.AngularVelocityInternal += InvIB * L2; + } + else + { + _impulse = Vector3.Zero; + _motorImpulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + Body b2 = BodyB; + + Vector2 v1 = b1.LinearVelocityInternal; + float w1 = b1.AngularVelocityInternal; + Vector2 v2 = b2.LinearVelocityInternal; + float w2 = b2.AngularVelocityInternal; + + // Solve linear motor constraint. + if (_enableMotor && _limitState != LimitState.Equal) + { + float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; + float impulse = _motorMass * (_motorSpeed - Cdot); + float oldImpulse = _motorImpulse; + float maxImpulse = step.dt * _maxMotorForce; + _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = _motorImpulse - oldImpulse; + + Vector2 P = impulse * _axis; + float L1 = impulse * _a1; + float L2 = impulse * _a2; + + v1 -= InvMassA * P; + w1 -= InvIA * L1; + + v2 += InvMassB * P; + w2 += InvIB * L2; + } + + Vector2 Cdot1 = new Vector2(Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1, w2 - w1); + + if (_enableLimit && _limitState != LimitState.Inactive) + { + // Solve prismatic and limit constraint in block form. + float Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; + Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); + + Vector3 f1 = _impulse; + Vector3 df = _K.Solve33(-Cdot); + _impulse += df; + + if (_limitState == LimitState.AtLower) + { + _impulse.Z = Math.Max(_impulse.Z, 0.0f); + } + else if (_limitState == LimitState.AtUpper) + { + _impulse.Z = Math.Min(_impulse.Z, 0.0f); + } + + // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) + Vector2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vector2(_K.Col3.X, _K.Col3.Y); + Vector2 f2r = _K.Solve22(b) + new Vector2(f1.X, f1.Y); + _impulse.X = f2r.X; + _impulse.Y = f2r.Y; + + df = _impulse - f1; + + Vector2 P = df.X * _perp + df.Z * _axis; + float L1 = df.X * _s1 + df.Y + df.Z * _a1; + float L2 = df.X * _s2 + df.Y + df.Z * _a2; + + v1 -= InvMassA * P; + w1 -= InvIA * L1; + + v2 += InvMassB * P; + w2 += InvIB * L2; + } + else + { + // Limit is inactive, just solve the prismatic constraint in block form. + Vector2 df = _K.Solve22(-Cdot1); + _impulse.X += df.X; + _impulse.Y += df.Y; + + Vector2 P = df.X * _perp; + float L1 = df.X * _s1 + df.Y; + float L2 = df.X * _s2 + df.Y; + + v1 -= InvMassA * P; + w1 -= InvIA * L1; + + v2 += InvMassB * P; + w2 += InvIB * L2; + } + + b1.LinearVelocityInternal = v1; + b1.AngularVelocityInternal = w1; + b2.LinearVelocityInternal = v2; + b2.AngularVelocityInternal = w2; + } + + internal override bool SolvePositionConstraints() + { + Body b1 = BodyA; + Body b2 = BodyB; + + Vector2 c1 = b1.Sweep.C; + float a1 = b1.Sweep.A; + + Vector2 c2 = b2.Sweep.C; + float a2 = b2.Sweep.A; + + // Solve linear limit constraint. + float linearError = 0.0f; + bool active = false; + float C2 = 0.0f; + + Mat22 R1 = new Mat22(a1); + Mat22 R2 = new Mat22(a2); + + Vector2 r1 = MathUtils.Multiply(ref R1, LocalAnchorA - LocalCenterA); + Vector2 r2 = MathUtils.Multiply(ref R2, LocalAnchorB - LocalCenterB); + Vector2 d = c2 + r2 - c1 - r1; + + if (_enableLimit) + { + _axis = MathUtils.Multiply(ref R1, _localXAxis1); + + _a1 = MathUtils.Cross(d + r1, _axis); + _a2 = MathUtils.Cross(r2, _axis); + + float translation = Vector2.Dot(_axis, d); + if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) + { + // Prevent large angular corrections + C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); + linearError = Math.Abs(translation); + active = true; + } + else if (translation <= _lowerTranslation) + { + // Prevent large linear corrections and allow some slop. + C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop, + -Settings.MaxLinearCorrection, 0.0f); + linearError = _lowerTranslation - translation; + active = true; + } + else if (translation >= _upperTranslation) + { + // Prevent large linear corrections and allow some slop. + C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, + Settings.MaxLinearCorrection); + linearError = translation - _upperTranslation; + active = true; + } + } + + _perp = MathUtils.Multiply(ref R1, _localYAxis1); + + _s1 = MathUtils.Cross(d + r1, _perp); + _s2 = MathUtils.Cross(r2, _perp); + + Vector3 impulse; + Vector2 C1 = new Vector2(Vector2.Dot(_perp, d), a2 - a1 - ReferenceAngle); + + linearError = Math.Max(linearError, Math.Abs(C1.X)); + float angularError = Math.Abs(C1.Y); + + if (active) + { + float m1 = InvMassA, m2 = InvMassB; + float i1 = InvIA, i2 = InvIB; + + float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; + float k12 = i1 * _s1 + i2 * _s2; + float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2; + float k22 = i1 + i2; + float k23 = i1 * _a1 + i2 * _a2; + float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2; + + _K.Col1 = new Vector3(k11, k12, k13); + _K.Col2 = new Vector3(k12, k22, k23); + _K.Col3 = new Vector3(k13, k23, k33); + + Vector3 C = new Vector3(-C1.X, -C1.Y, -C2); + impulse = _K.Solve33(C); // negated above + } + else + { + float m1 = InvMassA, m2 = InvMassB; + float i1 = InvIA, i2 = InvIB; + + float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; + float k12 = i1 * _s1 + i2 * _s2; + float k22 = i1 + i2; + + _K.Col1 = new Vector3(k11, k12, 0.0f); + _K.Col2 = new Vector3(k12, k22, 0.0f); + + Vector2 impulse1 = _K.Solve22(-C1); + impulse.X = impulse1.X; + impulse.Y = impulse1.Y; + impulse.Z = 0.0f; + } + + Vector2 P = impulse.X * _perp + impulse.Z * _axis; + float L1 = impulse.X * _s1 + impulse.Y + impulse.Z * _a1; + float L2 = impulse.X * _s2 + impulse.Y + impulse.Z * _a2; + + c1 -= InvMassA * P; + a1 -= InvIA * L1; + c2 += InvMassB * P; + a2 += InvIB * L2; + + // TODO_ERIN remove need for this. + b1.Sweep.C = c1; + b1.Sweep.A = a1; + b2.Sweep.C = c2; + b2.Sweep.A = a2; + b1.SynchronizeTransform(); + b2.SynchronizeTransform(); + + return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; + } + } +} \ No newline at end of file diff --git a/Dynamics/Joints/PulleyJoint.cs b/Dynamics/Joints/PulleyJoint.cs new file mode 100644 index 0000000..4936e48 --- /dev/null +++ b/Dynamics/Joints/PulleyJoint.cs @@ -0,0 +1,507 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + /// + /// The pulley joint is connected to two bodies and two fixed ground points. + /// The pulley supports a ratio such that: + /// length1 + ratio * length2 = ant + /// Yes, the force transmitted is scaled by the ratio. + /// The pulley also enforces a maximum length limit on both sides. This is + /// useful to prevent one side of the pulley hitting the top. + /// + public class PulleyJoint : Joint + { + /// + /// Get the first ground anchor. + /// + /// + public Vector2 GroundAnchorA; + + /// + /// Get the second ground anchor. + /// + /// + public Vector2 GroundAnchorB; + + public Vector2 LocalAnchorA; + public Vector2 LocalAnchorB; + + public float MinPulleyLength = 2.0f; + private float _ant; + private float _impulse; + private float _lengthA; + private float _lengthB; + private float _limitImpulse1; + private float _limitImpulse2; + private float _limitMass1; + private float _limitMass2; + private LimitState _limitState1; + private LimitState _limitState2; + private float _maxLengthA; + private float _maxLengthB; + + // Effective masses + private float _pulleyMass; + private LimitState _state; + private Vector2 _u1; + private Vector2 _u2; + + internal PulleyJoint() + { + JointType = JointType.Pulley; + } + + /// + /// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors. + /// This requires two ground anchors, + /// two dynamic body anchor points, max lengths for each side, + /// and a pulley ratio. + /// + /// The first body. + /// The second body. + /// The ground anchor for the first body. + /// The ground anchor for the second body. + /// The first body anchor. + /// The second body anchor. + /// The ratio. + public PulleyJoint(Body bodyA, Body bodyB, + Vector2 groundAnchorA, Vector2 groundAnchorB, + Vector2 localAnchorA, Vector2 localAnchorB, + float ratio) + : base(bodyA, bodyB) + { + JointType = JointType.Pulley; + + GroundAnchorA = groundAnchorA; + GroundAnchorB = groundAnchorB; + LocalAnchorA = localAnchorA; + LocalAnchorB = localAnchorB; + + Vector2 d1 = BodyA.GetWorldPoint(localAnchorA) - groundAnchorA; + _lengthA = d1.Length(); + + Vector2 d2 = BodyB.GetWorldPoint(localAnchorB) - groundAnchorB; + _lengthB = d2.Length(); + + Debug.Assert(ratio != 0.0f); + Debug.Assert(ratio > Settings.Epsilon); + Ratio = ratio; + + float C = _lengthA + Ratio * _lengthB; + + MaxLengthA = C - Ratio * MinPulleyLength; + MaxLengthB = (C - MinPulleyLength) / Ratio; + + _ant = _lengthA + Ratio * _lengthB; + + MaxLengthA = Math.Min(MaxLengthA, _ant - Ratio * MinPulleyLength); + MaxLengthB = Math.Min(MaxLengthB, (_ant - MinPulleyLength) / Ratio); + + _impulse = 0.0f; + _limitImpulse1 = 0.0f; + _limitImpulse2 = 0.0f; + } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + /// + /// Get the current length of the segment attached to body1. + /// + /// + public float LengthA + { + get + { + Vector2 d = BodyA.GetWorldPoint(LocalAnchorA) - GroundAnchorA; + return d.Length(); + } + set { _lengthA = value; } + } + + /// + /// Get the current length of the segment attached to body2. + /// + /// + public float LengthB + { + get + { + Vector2 d = BodyB.GetWorldPoint(LocalAnchorB) - GroundAnchorB; + return d.Length(); + } + set { _lengthB = value; } + } + + /// + /// Get the pulley ratio. + /// + /// + public float Ratio { get; set; } + + public float MaxLengthA + { + get { return _maxLengthA; } + set { _maxLengthA = value; } + } + + public float MaxLengthB + { + get { return _maxLengthB; } + set { _maxLengthB = value; } + } + + public override Vector2 GetReactionForce(float inv_dt) + { + Vector2 P = _impulse * _u2; + return inv_dt * P; + } + + public override float GetReactionTorque(float inv_dt) + { + return 0.0f; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + Body b2 = BodyB; + + Transform xf1, xf2; + b1.GetTransform(out xf1); + b2.GetTransform(out xf2); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter); + + Vector2 p1 = b1.Sweep.C + r1; + Vector2 p2 = b2.Sweep.C + r2; + + Vector2 s1 = GroundAnchorA; + Vector2 s2 = GroundAnchorB; + + // Get the pulley axes. + _u1 = p1 - s1; + _u2 = p2 - s2; + + float length1 = _u1.Length(); + float length2 = _u2.Length(); + + if (length1 > Settings.LinearSlop) + { + _u1 *= 1.0f / length1; + } + else + { + _u1 = Vector2.Zero; + } + + if (length2 > Settings.LinearSlop) + { + _u2 *= 1.0f / length2; + } + else + { + _u2 = Vector2.Zero; + } + + float C = _ant - length1 - Ratio * length2; + if (C > 0.0f) + { + _state = LimitState.Inactive; + _impulse = 0.0f; + } + else + { + _state = LimitState.AtUpper; + } + + if (length1 < MaxLengthA) + { + _limitState1 = LimitState.Inactive; + _limitImpulse1 = 0.0f; + } + else + { + _limitState1 = LimitState.AtUpper; + } + + if (length2 < MaxLengthB) + { + _limitState2 = LimitState.Inactive; + _limitImpulse2 = 0.0f; + } + else + { + _limitState2 = LimitState.AtUpper; + } + + // Compute effective mass. + float cr1u1 = MathUtils.Cross(r1, _u1); + float cr2u2 = MathUtils.Cross(r2, _u2); + + _limitMass1 = b1.InvMass + b1.InvI * cr1u1 * cr1u1; + _limitMass2 = b2.InvMass + b2.InvI * cr2u2 * cr2u2; + _pulleyMass = _limitMass1 + Ratio * Ratio * _limitMass2; + Debug.Assert(_limitMass1 > Settings.Epsilon); + Debug.Assert(_limitMass2 > Settings.Epsilon); + Debug.Assert(_pulleyMass > Settings.Epsilon); + _limitMass1 = 1.0f / _limitMass1; + _limitMass2 = 1.0f / _limitMass2; + _pulleyMass = 1.0f / _pulleyMass; + + if (Settings.EnableWarmstarting) + { + // Scale impulses to support variable time steps. + _impulse *= step.dtRatio; + _limitImpulse1 *= step.dtRatio; + _limitImpulse2 *= step.dtRatio; + + // Warm starting. + Vector2 P1 = -(_impulse + _limitImpulse1) * _u1; + Vector2 P2 = (-Ratio * _impulse - _limitImpulse2) * _u2; + b1.LinearVelocityInternal += b1.InvMass * P1; + b1.AngularVelocityInternal += b1.InvI * MathUtils.Cross(r1, P1); + b2.LinearVelocityInternal += b2.InvMass * P2; + b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P2); + } + else + { + _impulse = 0.0f; + _limitImpulse1 = 0.0f; + _limitImpulse2 = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + Body b2 = BodyB; + + Transform xf1, xf2; + b1.GetTransform(out xf1); + b2.GetTransform(out xf2); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter); + + if (_state == LimitState.AtUpper) + { + Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1); + Vector2 v2 = b2.LinearVelocityInternal + MathUtils.Cross(b2.AngularVelocityInternal, r2); + + float Cdot = -Vector2.Dot(_u1, v1) - Ratio * Vector2.Dot(_u2, v2); + float impulse = _pulleyMass * (-Cdot); + float oldImpulse = _impulse; + _impulse = Math.Max(0.0f, _impulse + impulse); + impulse = _impulse - oldImpulse; + + Vector2 P1 = -impulse * _u1; + Vector2 P2 = -Ratio * impulse * _u2; + b1.LinearVelocityInternal += b1.InvMass * P1; + b1.AngularVelocityInternal += b1.InvI * MathUtils.Cross(r1, P1); + b2.LinearVelocityInternal += b2.InvMass * P2; + b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P2); + } + + if (_limitState1 == LimitState.AtUpper) + { + Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1); + + float Cdot = -Vector2.Dot(_u1, v1); + float impulse = -_limitMass1 * Cdot; + float oldImpulse = _limitImpulse1; + _limitImpulse1 = Math.Max(0.0f, _limitImpulse1 + impulse); + impulse = _limitImpulse1 - oldImpulse; + + Vector2 P1 = -impulse * _u1; + b1.LinearVelocityInternal += b1.InvMass * P1; + b1.AngularVelocityInternal += b1.InvI * MathUtils.Cross(r1, P1); + } + + if (_limitState2 == LimitState.AtUpper) + { + Vector2 v2 = b2.LinearVelocityInternal + MathUtils.Cross(b2.AngularVelocityInternal, r2); + + float Cdot = -Vector2.Dot(_u2, v2); + float impulse = -_limitMass2 * Cdot; + float oldImpulse = _limitImpulse2; + _limitImpulse2 = Math.Max(0.0f, _limitImpulse2 + impulse); + impulse = _limitImpulse2 - oldImpulse; + + Vector2 P2 = -impulse * _u2; + b2.LinearVelocityInternal += b2.InvMass * P2; + b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P2); + } + } + + internal override bool SolvePositionConstraints() + { + Body b1 = BodyA; + Body b2 = BodyB; + + Vector2 s1 = GroundAnchorA; + Vector2 s2 = GroundAnchorB; + + float linearError = 0.0f; + + if (_state == LimitState.AtUpper) + { + Transform xf1, xf2; + b1.GetTransform(out xf1); + b2.GetTransform(out xf2); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter); + + Vector2 p1 = b1.Sweep.C + r1; + Vector2 p2 = b2.Sweep.C + r2; + + // Get the pulley axes. + _u1 = p1 - s1; + _u2 = p2 - s2; + + float length1 = _u1.Length(); + float length2 = _u2.Length(); + + if (length1 > Settings.LinearSlop) + { + _u1 *= 1.0f / length1; + } + else + { + _u1 = Vector2.Zero; + } + + if (length2 > Settings.LinearSlop) + { + _u2 *= 1.0f / length2; + } + else + { + _u2 = Vector2.Zero; + } + + float C = _ant - length1 - Ratio * length2; + linearError = Math.Max(linearError, -C); + + C = MathUtils.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); + float impulse = -_pulleyMass * C; + + Vector2 P1 = -impulse * _u1; + Vector2 P2 = -Ratio * impulse * _u2; + + b1.Sweep.C += b1.InvMass * P1; + b1.Sweep.A += b1.InvI * MathUtils.Cross(r1, P1); + b2.Sweep.C += b2.InvMass * P2; + b2.Sweep.A += b2.InvI * MathUtils.Cross(r2, P2); + + b1.SynchronizeTransform(); + b2.SynchronizeTransform(); + } + + if (_limitState1 == LimitState.AtUpper) + { + Transform xf1; + b1.GetTransform(out xf1); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 p1 = b1.Sweep.C + r1; + + _u1 = p1 - s1; + float length1 = _u1.Length(); + + if (length1 > Settings.LinearSlop) + { + _u1 *= 1.0f / length1; + } + else + { + _u1 = Vector2.Zero; + } + + float C = MaxLengthA - length1; + linearError = Math.Max(linearError, -C); + C = MathUtils.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); + float impulse = -_limitMass1 * C; + + Vector2 P1 = -impulse * _u1; + b1.Sweep.C += b1.InvMass * P1; + b1.Sweep.A += b1.InvI * MathUtils.Cross(r1, P1); + + b1.SynchronizeTransform(); + } + + if (_limitState2 == LimitState.AtUpper) + { + Transform xf2; + b2.GetTransform(out xf2); + + Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter); + Vector2 p2 = b2.Sweep.C + r2; + + _u2 = p2 - s2; + float length2 = _u2.Length(); + + if (length2 > Settings.LinearSlop) + { + _u2 *= 1.0f / length2; + } + else + { + _u2 = Vector2.Zero; + } + + float C = MaxLengthB - length2; + linearError = Math.Max(linearError, -C); + C = MathUtils.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); + float impulse = -_limitMass2 * C; + + Vector2 P2 = -impulse * _u2; + b2.Sweep.C += b2.InvMass * P2; + b2.Sweep.A += b2.InvI * MathUtils.Cross(r2, P2); + + b2.SynchronizeTransform(); + } + + return linearError < Settings.LinearSlop; + } + } +} \ No newline at end of file diff --git a/Dynamics/Joints/RevoluteJoint.cs b/Dynamics/Joints/RevoluteJoint.cs new file mode 100644 index 0000000..5e0062b --- /dev/null +++ b/Dynamics/Joints/RevoluteJoint.cs @@ -0,0 +1,595 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + /// + /// A revolute joint rains to bodies to share a common point while they + /// are free to rotate about the point. The relative rotation about the shared + /// point is the joint angle. You can limit the relative rotation with + /// a joint limit that specifies a lower and upper angle. You can use a motor + /// to drive the relative rotation about the shared point. A maximum motor torque + /// is provided so that infinite forces are not generated. + /// + public class RevoluteJoint : Joint + { + public Vector2 LocalAnchorA; + + public Vector2 LocalAnchorB; + private bool _enableLimit; + private bool _enableMotor; + private Vector3 _impulse; + private LimitState _limitState; + private float _lowerAngle; + private Mat33 _mass; // effective mass for point-to-point constraint. + private float _maxMotorTorque; + private float _motorImpulse; + private float _motorMass; // effective mass for motor/limit angular constraint. + private float _motorSpeed; + private float _referenceAngle; + private float _tmpFloat1; + private Vector2 _tmpVector1, _tmpVector2; + private float _upperAngle; + + internal RevoluteJoint() + { + JointType = JointType.Revolute; + } + + /// + /// Initialize the bodies and local anchor. + /// This requires defining an + /// anchor point where the bodies are joined. The definition + /// uses local anchor points so that the initial configuration + /// can violate the constraint slightly. You also need to + /// specify the initial relative angle for joint limits. This + /// helps when saving and loading a game. + /// The local anchor points are measured from the body's origin + /// rather than the center of mass because: + /// 1. you might not know where the center of mass will be. + /// 2. if you add/remove shapes from a body and recompute the mass, + /// the joints will be broken. + /// + /// The first body. + /// The second body. + /// The first body anchor. + /// The second anchor. + public RevoluteJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB) + : base(bodyA, bodyB) + { + JointType = JointType.Revolute; + + // Changed to local coordinates. + LocalAnchorA = localAnchorA; + LocalAnchorB = localAnchorB; + + ReferenceAngle = BodyB.Rotation - BodyA.Rotation; + + _impulse = Vector3.Zero; + + _limitState = LimitState.Inactive; + } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + public float ReferenceAngle + { + get { return _referenceAngle; } + set + { + WakeBodies(); + _referenceAngle = value; + } + } + + /// + /// Get the current joint angle in radians. + /// + /// + public float JointAngle + { + get { return BodyB.Sweep.A - BodyA.Sweep.A - ReferenceAngle; } + } + + /// + /// Get the current joint angle speed in radians per second. + /// + /// + public float JointSpeed + { + get { return BodyB.AngularVelocityInternal - BodyA.AngularVelocityInternal; } + } + + /// + /// Is the joint limit enabled? + /// + /// true if [limit enabled]; otherwise, false. + public bool LimitEnabled + { + get { return _enableLimit; } + set + { + WakeBodies(); + _enableLimit = value; + } + } + + /// + /// Get the lower joint limit in radians. + /// + /// + public float LowerLimit + { + get { return _lowerAngle; } + set + { + WakeBodies(); + _lowerAngle = value; + } + } + + /// + /// Get the upper joint limit in radians. + /// + /// + public float UpperLimit + { + get { return _upperAngle; } + set + { + WakeBodies(); + _upperAngle = value; + } + } + + /// + /// Is the joint motor enabled? + /// + /// true if [motor enabled]; otherwise, false. + public bool MotorEnabled + { + get { return _enableMotor; } + set + { + WakeBodies(); + _enableMotor = value; + } + } + + /// + /// Set the motor speed in radians per second. + /// + /// The speed. + public float MotorSpeed + { + set + { + WakeBodies(); + _motorSpeed = value; + } + get { return _motorSpeed; } + } + + /// + /// Set the maximum motor torque, usually in N-m. + /// + /// The torque. + public float MaxMotorTorque + { + set + { + WakeBodies(); + _maxMotorTorque = value; + } + get { return _maxMotorTorque; } + } + + /// + /// Get the current motor torque, usually in N-m. + /// + /// + public float MotorTorque + { + get { return _motorImpulse; } + set + { + WakeBodies(); + _motorImpulse = value; + } + } + + public override Vector2 GetReactionForce(float inv_dt) + { + Vector2 P = new Vector2(_impulse.X, _impulse.Y); + return inv_dt * P; + } + + public override float GetReactionTorque(float inv_dt) + { + return inv_dt * _impulse.Z; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + Body b2 = BodyB; + + if (_enableMotor || _enableLimit) + { + // You cannot create a rotation limit between bodies that + // both have fixed rotation. + Debug.Assert(b1.InvI > 0.0f || b2.InvI > 0.0f); + } + + // Compute the effective mass matrix. + /*Transform xf1, xf2; + b1.GetTransform(out xf1); + b2.GetTransform(out xf2);*/ + + Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter); + + // J = [-I -r1_skew I r2_skew] + // [ 0 -1 0 1] + // r_skew = [-ry; rx] + + // Matlab + // K = [ m1+r1y^2*i1+m2+r2y^2*i2, -r1y*i1*r1x-r2y*i2*r2x, -r1y*i1-r2y*i2] + // [ -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2, r1x*i1+r2x*i2] + // [ -r1y*i1-r2y*i2, r1x*i1+r2x*i2, i1+i2] + + float m1 = b1.InvMass, m2 = b2.InvMass; + float i1 = b1.InvI, i2 = b2.InvI; + + _mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2; + _mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2; + _mass.Col3.X = -r1.Y * i1 - r2.Y * i2; + _mass.Col1.Y = _mass.Col2.X; + _mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2; + _mass.Col3.Y = r1.X * i1 + r2.X * i2; + _mass.Col1.Z = _mass.Col3.X; + _mass.Col2.Z = _mass.Col3.Y; + _mass.Col3.Z = i1 + i2; + + _motorMass = i1 + i2; + if (_motorMass > 0.0f) + { + _motorMass = 1.0f / _motorMass; + } + + if (_enableMotor == false) + { + _motorImpulse = 0.0f; + } + + if (_enableLimit) + { + float jointAngle = b2.Sweep.A - b1.Sweep.A - ReferenceAngle; + if (Math.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop) + { + _limitState = LimitState.Equal; + } + else if (jointAngle <= _lowerAngle) + { + if (_limitState != LimitState.AtLower) + { + _impulse.Z = 0.0f; + } + _limitState = LimitState.AtLower; + } + else if (jointAngle >= _upperAngle) + { + if (_limitState != LimitState.AtUpper) + { + _impulse.Z = 0.0f; + } + _limitState = LimitState.AtUpper; + } + else + { + _limitState = LimitState.Inactive; + _impulse.Z = 0.0f; + } + } + else + { + _limitState = LimitState.Inactive; + } + + if (Settings.EnableWarmstarting) + { + // Scale impulses to support a variable time step. + _impulse *= step.dtRatio; + _motorImpulse *= step.dtRatio; + + Vector2 P = new Vector2(_impulse.X, _impulse.Y); + + b1.LinearVelocityInternal -= m1 * P; + MathUtils.Cross(ref r1, ref P, out _tmpFloat1); + b1.AngularVelocityInternal -= i1 * ( /* r1 x P */_tmpFloat1 + _motorImpulse + _impulse.Z); + + b2.LinearVelocityInternal += m2 * P; + MathUtils.Cross(ref r2, ref P, out _tmpFloat1); + b2.AngularVelocityInternal += i2 * ( /* r2 x P */_tmpFloat1 + _motorImpulse + _impulse.Z); + } + else + { + _impulse = Vector3.Zero; + _motorImpulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + Body b2 = BodyB; + + Vector2 v1 = b1.LinearVelocityInternal; + float w1 = b1.AngularVelocityInternal; + Vector2 v2 = b2.LinearVelocityInternal; + float w2 = b2.AngularVelocityInternal; + + float m1 = b1.InvMass, m2 = b2.InvMass; + float i1 = b1.InvI, i2 = b2.InvI; + + // Solve motor constraint. + if (_enableMotor && _limitState != LimitState.Equal) + { + float Cdot = w2 - w1 - _motorSpeed; + float impulse = _motorMass * (-Cdot); + float oldImpulse = _motorImpulse; + float maxImpulse = step.dt * _maxMotorTorque; + _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = _motorImpulse - oldImpulse; + + w1 -= i1 * impulse; + w2 += i2 * impulse; + } + + // Solve limit constraint. + if (_enableLimit && _limitState != LimitState.Inactive) + { + /*Transform xf1, xf2; + b1.GetTransform(out xf1); + b2.GetTransform(out xf2);*/ + + Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter); + + // Solve point-to-point constraint + MathUtils.Cross(w2, ref r2, out _tmpVector2); + MathUtils.Cross(w1, ref r1, out _tmpVector1); + Vector2 Cdot1 = v2 + /* w2 x r2 */ _tmpVector2 - v1 - /* w1 x r1 */ _tmpVector1; + float Cdot2 = w2 - w1; + Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); + + Vector3 impulse = _mass.Solve33(-Cdot); + + if (_limitState == LimitState.Equal) + { + _impulse += impulse; + } + else if (_limitState == LimitState.AtLower) + { + float newImpulse = _impulse.Z + impulse.Z; + if (newImpulse < 0.0f) + { + Vector2 reduced = _mass.Solve22(-Cdot1); + impulse.X = reduced.X; + impulse.Y = reduced.Y; + impulse.Z = -_impulse.Z; + _impulse.X += reduced.X; + _impulse.Y += reduced.Y; + _impulse.Z = 0.0f; + } + } + else if (_limitState == LimitState.AtUpper) + { + float newImpulse = _impulse.Z + impulse.Z; + if (newImpulse > 0.0f) + { + Vector2 reduced = _mass.Solve22(-Cdot1); + impulse.X = reduced.X; + impulse.Y = reduced.Y; + impulse.Z = -_impulse.Z; + _impulse.X += reduced.X; + _impulse.Y += reduced.Y; + _impulse.Z = 0.0f; + } + } + + Vector2 P = new Vector2(impulse.X, impulse.Y); + + v1 -= m1 * P; + MathUtils.Cross(ref r1, ref P, out _tmpFloat1); + w1 -= i1 * ( /* r1 x P */_tmpFloat1 + impulse.Z); + + v2 += m2 * P; + MathUtils.Cross(ref r2, ref P, out _tmpFloat1); + w2 += i2 * ( /* r2 x P */_tmpFloat1 + impulse.Z); + } + else + { + /*Transform xf1, xf2; + b1.GetTransform(out xf1); + b2.GetTransform(out xf2);*/ + + _tmpVector1 = LocalAnchorA - b1.LocalCenter; + _tmpVector2 = LocalAnchorB - b2.LocalCenter; + Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, ref _tmpVector1); + Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, ref _tmpVector2); + + // Solve point-to-point constraint + MathUtils.Cross(w2, ref r2, out _tmpVector2); + MathUtils.Cross(w1, ref r1, out _tmpVector1); + Vector2 Cdot = v2 + /* w2 x r2 */ _tmpVector2 - v1 - /* w1 x r1 */ _tmpVector1; + Vector2 impulse = _mass.Solve22(-Cdot); + + _impulse.X += impulse.X; + _impulse.Y += impulse.Y; + + v1 -= m1 * impulse; + MathUtils.Cross(ref r1, ref impulse, out _tmpFloat1); + w1 -= i1 * /* r1 x impulse */ _tmpFloat1; + + v2 += m2 * impulse; + MathUtils.Cross(ref r2, ref impulse, out _tmpFloat1); + w2 += i2 * /* r2 x impulse */ _tmpFloat1; + } + + b1.LinearVelocityInternal = v1; + b1.AngularVelocityInternal = w1; + b2.LinearVelocityInternal = v2; + b2.AngularVelocityInternal = w2; + } + + internal override bool SolvePositionConstraints() + { + // TODO_ERIN block solve with limit. COME ON ERIN + + Body b1 = BodyA; + Body b2 = BodyB; + + float angularError = 0.0f; + float positionError; + + // Solve angular limit constraint. + if (_enableLimit && _limitState != LimitState.Inactive) + { + float angle = b2.Sweep.A - b1.Sweep.A - ReferenceAngle; + float limitImpulse = 0.0f; + + if (_limitState == LimitState.Equal) + { + // Prevent large angular corrections + float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection, + Settings.MaxAngularCorrection); + limitImpulse = -_motorMass * C; + angularError = Math.Abs(C); + } + else if (_limitState == LimitState.AtLower) + { + float C = angle - _lowerAngle; + angularError = -C; + + // Prevent large angular corrections and allow some slop. + C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f); + limitImpulse = -_motorMass * C; + } + else if (_limitState == LimitState.AtUpper) + { + float C = angle - _upperAngle; + angularError = C; + + // Prevent large angular corrections and allow some slop. + C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection); + limitImpulse = -_motorMass * C; + } + + b1.Sweep.A -= b1.InvI * limitImpulse; + b2.Sweep.A += b2.InvI * limitImpulse; + + b1.SynchronizeTransform(); + b2.SynchronizeTransform(); + } + + // Solve point-to-point constraint. + { + /*Transform xf1, xf2; + b1.GetTransform(out xf1); + b2.GetTransform(out xf2);*/ + + Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter); + + Vector2 C = b2.Sweep.C + r2 - b1.Sweep.C - r1; + positionError = C.Length(); + + float invMass1 = b1.InvMass, invMass2 = b2.InvMass; + float invI1 = b1.InvI, invI2 = b2.InvI; + + // Handle large detachment. + const float k_allowedStretch = 10.0f * Settings.LinearSlop; + if (C.LengthSquared() > k_allowedStretch * k_allowedStretch) + { + // Use a particle solution (no rotation). + Vector2 u = C; + u.Normalize(); + float k = invMass1 + invMass2; + Debug.Assert(k > Settings.Epsilon); + float m = 1.0f / k; + Vector2 impulse2 = m * (-C); + const float k_beta = 0.5f; + b1.Sweep.C -= k_beta * invMass1 * impulse2; + b2.Sweep.C += k_beta * invMass2 * impulse2; + + C = b2.Sweep.C + r2 - b1.Sweep.C - r1; + } + + Mat22 K1 = new Mat22(new Vector2(invMass1 + invMass2, 0.0f), new Vector2(0.0f, invMass1 + invMass2)); + Mat22 K2 = new Mat22(new Vector2(invI1 * r1.Y * r1.Y, -invI1 * r1.X * r1.Y), + new Vector2(-invI1 * r1.X * r1.Y, invI1 * r1.X * r1.X)); + Mat22 K3 = new Mat22(new Vector2(invI2 * r2.Y * r2.Y, -invI2 * r2.X * r2.Y), + new Vector2(-invI2 * r2.X * r2.Y, invI2 * r2.X * r2.X)); + + Mat22 Ka; + Mat22.Add(ref K1, ref K2, out Ka); + + Mat22 K; + Mat22.Add(ref Ka, ref K3, out K); + + + Vector2 impulse = K.Solve(-C); + + b1.Sweep.C -= b1.InvMass * impulse; + MathUtils.Cross(ref r1, ref impulse, out _tmpFloat1); + b1.Sweep.A -= b1.InvI * /* r1 x impulse */ _tmpFloat1; + + b2.Sweep.C += b2.InvMass * impulse; + MathUtils.Cross(ref r2, ref impulse, out _tmpFloat1); + b2.Sweep.A += b2.InvI * /* r2 x impulse */ _tmpFloat1; + + b1.SynchronizeTransform(); + b2.SynchronizeTransform(); + } + + return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; + } + } +} \ No newline at end of file diff --git a/Dynamics/Joints/RopeJoint.cs b/Dynamics/Joints/RopeJoint.cs new file mode 100644 index 0000000..8ea1d07 --- /dev/null +++ b/Dynamics/Joints/RopeJoint.cs @@ -0,0 +1,239 @@ +/* +* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + // Limit: + // C = norm(pB - pA) - L + // u = (pB - pA) / norm(pB - pA) + // Cdot = dot(u, vB + cross(wB, rB) - vA - cross(wA, rA)) + // J = [-u -cross(rA, u) u cross(rB, u)] + // K = J * invM * JT + // = invMassA + invIA * cross(rA, u)^2 + invMassB + invIB * cross(rB, u)^2 + + /// + /// A rope joint enforces a maximum distance between two points + /// on two bodies. It has no other effect. + /// Warning: if you attempt to change the maximum length during + /// the simulation you will get some non-physical behavior. + /// A model that would allow you to dynamically modify the length + /// would have some sponginess, so I chose not to implement it + /// that way. See b2DistanceJoint if you want to dynamically + /// control length. + /// + public class RopeJoint : Joint + { + public Vector2 LocalAnchorA; + public Vector2 LocalAnchorB; + + private float _impulse; + private float _length; + + private float _mass; + private Vector2 _rA, _rB; + private LimitState _state; + private Vector2 _u; + + internal RopeJoint() + { + JointType = JointType.Rope; + } + + public RopeJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB) + : base(bodyA, bodyB) + { + JointType = JointType.Rope; + LocalAnchorA = localAnchorA; + LocalAnchorB = localAnchorB; + + Vector2 d = WorldAnchorB - WorldAnchorA; + MaxLength = d.Length(); + + _mass = 0.0f; + _impulse = 0.0f; + _state = LimitState.Inactive; + _length = 0.0f; + } + + /// Get the maximum length of the rope. + public float MaxLength { get; set; } + + public LimitState State + { + get { return _state; } + } + + public override sealed Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + } + + public override sealed Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + public override Vector2 GetReactionForce(float invDt) + { + return (invDt * _impulse) * _u; + } + + public override float GetReactionTorque(float invDt) + { + return 0; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body bA = BodyA; + Body bB = BodyB; + + Transform xf1; + bA.GetTransform(out xf1); + + Transform xf2; + bB.GetTransform(out xf2); + + _rA = MathUtils.Multiply(ref xf1.R, LocalAnchorA - bA.LocalCenter); + _rB = MathUtils.Multiply(ref xf2.R, LocalAnchorB - bB.LocalCenter); + + // Rope axis + _u = bB.Sweep.C + _rB - bA.Sweep.C - _rA; + + _length = _u.Length(); + + float C = _length - MaxLength; + if (C > 0.0f) + { + _state = LimitState.AtUpper; + } + else + { + _state = LimitState.Inactive; + } + + if (_length > Settings.LinearSlop) + { + _u *= 1.0f / _length; + } + else + { + _u = Vector2.Zero; + _mass = 0.0f; + _impulse = 0.0f; + return; + } + + // Compute effective mass. + float crA = MathUtils.Cross(_rA, _u); + float crB = MathUtils.Cross(_rB, _u); + float invMass = bA.InvMass + bA.InvI * crA * crA + bB.InvMass + bB.InvI * crB * crB; + + _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; + + if (Settings.EnableWarmstarting) + { + // Scale the impulse to support a variable time step. + _impulse *= step.dtRatio; + + Vector2 P = _impulse * _u; + bA.LinearVelocity -= bA.InvMass * P; + bA.AngularVelocity -= bA.InvI * MathUtils.Cross(_rA, P); + bB.LinearVelocity += bB.InvMass * P; + bB.AngularVelocity += bB.InvI * MathUtils.Cross(_rB, P); + } + else + { + _impulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body bA = BodyA; + Body bB = BodyB; + + // Cdot = dot(u, v + cross(w, r)) + Vector2 vA = bA.LinearVelocity + MathUtils.Cross(bA.AngularVelocity, _rA); + Vector2 vB = bB.LinearVelocity + MathUtils.Cross(bB.AngularVelocity, _rB); + float C = _length - MaxLength; + float Cdot = Vector2.Dot(_u, vB - vA); + + // Predictive constraint. + if (C < 0.0f) + { + Cdot += step.inv_dt * C; + } + + float impulse = -_mass * Cdot; + float oldImpulse = _impulse; + _impulse = Math.Min(0.0f, _impulse + impulse); + impulse = _impulse - oldImpulse; + + Vector2 P = impulse * _u; + bA.LinearVelocity -= bA.InvMass * P; + bA.AngularVelocity -= bA.InvI * MathUtils.Cross(_rA, P); + bB.LinearVelocity += bB.InvMass * P; + bB.AngularVelocity += bB.InvI * MathUtils.Cross(_rB, P); + } + + internal override bool SolvePositionConstraints() + { + Body bA = BodyA; + Body bB = BodyB; + + Transform xf1; + bA.GetTransform(out xf1); + + Transform xf2; + bB.GetTransform(out xf2); + + Vector2 rA = MathUtils.Multiply(ref xf1.R, LocalAnchorA - bA.LocalCenter); + Vector2 rB = MathUtils.Multiply(ref xf2.R, LocalAnchorB - bB.LocalCenter); + + Vector2 u = bB.Sweep.C + rB - bA.Sweep.C - rA; + + + float length = u.Length(); + u.Normalize(); + + float C = length - MaxLength; + + C = MathUtils.Clamp(C, 0.0f, Settings.MaxLinearCorrection); + + float impulse = -_mass * C; + Vector2 P = impulse * u; + + bA.Sweep.C -= bA.InvMass * P; + bA.Sweep.A -= bA.InvI * MathUtils.Cross(rA, P); + bB.Sweep.C += bB.InvMass * P; + bB.Sweep.A += bB.InvI * MathUtils.Cross(rB, P); + + bA.SynchronizeTransform(); + bB.SynchronizeTransform(); + + return length - MaxLength < Settings.LinearSlop; + } + } +} \ No newline at end of file diff --git a/Dynamics/Joints/SliderJoint.cs b/Dynamics/Joints/SliderJoint.cs new file mode 100644 index 0000000..a0c1f2c --- /dev/null +++ b/Dynamics/Joints/SliderJoint.cs @@ -0,0 +1,298 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + /// + /// A distance joint contrains two points on two bodies + /// to remain at a fixed distance from each other. You can view + /// this as a massless, rigid rod. + /// + public class SliderJoint : Joint + { + // 1-D constrained system + // m (v2 - v1) = lambda + // v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass. + // x2 = x1 + h * v2 + + // 1-D mass-damper-spring system + // m (v2 - v1) + h * d * v2 + h * k * + + // C = norm(p2 - p1) - L + // u = (p2 - p1) / norm(p2 - p1) + // Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1)) + // J = [-u -cross(r1, u) u cross(r2, u)] + // K = J * invM * JT + // = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2 + + public Vector2 LocalAnchorA; + + public Vector2 LocalAnchorB; + private float _bias; + private float _gamma; + private float _impulse; + private float _mass; + private Vector2 _u; + + internal SliderJoint() + { + JointType = JointType.Slider; + } + + /// + /// Initializes a new instance of the class. + /// Warning: Do not use a zero or short length. + /// + /// The first body. + /// The second body. + /// The first body anchor. + /// The second body anchor. + /// The minimum length between anchorpoints + /// The maximum length between anchorpoints. + public SliderJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB, float minLength, + float maxlength) + : base(bodyA, bodyB) + { + JointType = JointType.Slider; + + LocalAnchorA = localAnchorA; + LocalAnchorB = localAnchorB; + MaxLength = maxlength; + MinLength = minLength; + } + + /// + /// The maximum length between the anchor points. + /// + /// The length. + public float MaxLength { get; set; } + + /// + /// The minimal length between the anchor points. + /// + /// The length. + public float MinLength { get; set; } + + /// + /// The mass-spring-damper frequency in Hertz. + /// + /// The frequency. + public float Frequency { get; set; } + + /// + /// The damping ratio. 0 = no damping, 1 = critical damping. + /// + /// The damping ratio. + public float DampingRatio { get; set; } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + public override Vector2 GetReactionForce(float inv_dt) + { + Vector2 F = (inv_dt * _impulse) * _u; + return F; + } + + public override float GetReactionTorque(float inv_dt) + { + return 0.0f; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + Body b2 = BodyB; + + Transform xf1, xf2; + b1.GetTransform(out xf1); + b2.GetTransform(out xf2); + + // Compute the effective mass matrix. + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter); + _u = b2.Sweep.C + r2 - b1.Sweep.C - r1; + + // Handle singularity. + float length = _u.Length(); + + if (length < MaxLength && length > MinLength) + { + return; + } + + if (length > Settings.LinearSlop) + { + _u *= 1.0f / length; + } + else + { + _u = Vector2.Zero; + } + + float cr1u = MathUtils.Cross(r1, _u); + float cr2u = MathUtils.Cross(r2, _u); + float invMass = b1.InvMass + b1.InvI * cr1u * cr1u + b2.InvMass + b2.InvI * cr2u * cr2u; + Debug.Assert(invMass > Settings.Epsilon); + _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; + + if (Frequency > 0.0f) + { + float C = length - MaxLength; + + // Frequency + float omega = 2.0f * Settings.Pi * Frequency; + + // Damping coefficient + float d = 2.0f * _mass * DampingRatio * omega; + + // Spring stiffness + float k = _mass * omega * omega; + + // magic formulas + _gamma = step.dt * (d + step.dt * k); + _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f; + _bias = C * step.dt * k * _gamma; + + _mass = invMass + _gamma; + _mass = _mass != 0.0f ? 1.0f / _mass : 0.0f; + } + + if (Settings.EnableWarmstarting) + { + // Scale the impulse to support a variable time step. + _impulse *= step.dtRatio; + + Vector2 P = _impulse * _u; + b1.LinearVelocityInternal -= b1.InvMass * P; + b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P); + b2.LinearVelocityInternal += b2.InvMass * P; + b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P); + } + else + { + _impulse = 0.0f; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body b1 = BodyA; + Body b2 = BodyB; + + Transform xf1, xf2; + b1.GetTransform(out xf1); + b2.GetTransform(out xf2); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter); + + Vector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1; + + float length = d.Length(); + + if (length < MaxLength && length > MinLength) + { + return; + } + + // Cdot = dot(u, v + cross(w, r)) + Vector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1); + Vector2 v2 = b2.LinearVelocityInternal + MathUtils.Cross(b2.AngularVelocityInternal, r2); + float Cdot = Vector2.Dot(_u, v2 - v1); + + float impulse = -_mass * (Cdot + _bias + _gamma * _impulse); + _impulse += impulse; + + Vector2 P = impulse * _u; + b1.LinearVelocityInternal -= b1.InvMass * P; + b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P); + b2.LinearVelocityInternal += b2.InvMass * P; + b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P); + } + + internal override bool SolvePositionConstraints() + { + if (Frequency > 0.0f) + { + // There is no position correction for soft distance constraints. + return true; + } + + Body b1 = BodyA; + Body b2 = BodyB; + + Transform xf1, xf2; + b1.GetTransform(out xf1); + b2.GetTransform(out xf2); + + Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); + Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter); + + Vector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1; + + float length = d.Length(); + + if (length < MaxLength && length > MinLength) + { + return true; + } + + if (length == 0.0f) + return true; + + d /= length; + float C = length - MaxLength; + C = MathUtils.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); + + float impulse = -_mass * C; + _u = d; + Vector2 P = impulse * _u; + + b1.Sweep.C -= b1.InvMass * P; + b1.Sweep.A -= b1.InvI * MathUtils.Cross(r1, P); + b2.Sweep.C += b2.InvMass * P; + b2.Sweep.A += b2.InvI * MathUtils.Cross(r2, P); + + b1.SynchronizeTransform(); + b2.SynchronizeTransform(); + + return Math.Abs(C) < Settings.LinearSlop; + } + } +} \ No newline at end of file diff --git a/Dynamics/Joints/WeldJoint.cs b/Dynamics/Joints/WeldJoint.cs new file mode 100644 index 0000000..8c72db2 --- /dev/null +++ b/Dynamics/Joints/WeldJoint.cs @@ -0,0 +1,263 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using FarseerPhysics.Common; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics.Joints +{ + // Point-to-point constraint + // C = p2 - p1 + // Cdot = v2 - v1 + // = v2 + cross(w2, r2) - v1 - cross(w1, r1) + // J = [-I -r1_skew I r2_skew ] + // Identity used: + // w k % (rx i + ry j) = w * (-ry i + rx j) + + // Angle constraint + // C = angle2 - angle1 - referenceAngle + // Cdot = w2 - w1 + // J = [0 0 -1 0 0 1] + // K = invI1 + invI2 + + /// + /// A weld joint essentially glues two bodies together. A weld joint may + /// distort somewhat because the island constraint solver is approximate. + /// + public class WeldJoint : Joint + { + public Vector2 LocalAnchorA; + public Vector2 LocalAnchorB; + private Vector3 _impulse; + private Mat33 _mass; + + internal WeldJoint() + { + JointType = JointType.Weld; + } + + /// + /// You need to specify a local anchor point + /// where they are attached and the relative body angle. The position + /// of the anchor point is important for computing the reaction torque. + /// You can change the anchor points relative to bodyA or bodyB by changing LocalAnchorA + /// and/or LocalAnchorB. + /// + /// The first body + /// The second body + /// The first body anchor. + /// The second body anchor. + public WeldJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB) + : base(bodyA, bodyB) + { + JointType = JointType.Weld; + + LocalAnchorA = localAnchorA; + LocalAnchorB = localAnchorB; + ReferenceAngle = BodyB.Rotation - BodyA.Rotation; + } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + /// + /// The body2 angle minus body1 angle in the reference state (radians). + /// + public float ReferenceAngle { get; private set; } + + public override Vector2 GetReactionForce(float inv_dt) + { + return inv_dt * new Vector2(_impulse.X, _impulse.Y); + } + + public override float GetReactionTorque(float inv_dt) + { + return inv_dt * _impulse.Z; + } + + internal override void InitVelocityConstraints(ref TimeStep step) + { + Body bA = BodyA; + Body bB = BodyB; + + Transform xfA, xfB; + bA.GetTransform(out xfA); + bB.GetTransform(out xfB); + + // Compute the effective mass matrix. + Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter); + Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter); + + // J = [-I -r1_skew I r2_skew] + // [ 0 -1 0 1] + // r_skew = [-ry; rx] + + // Matlab + // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] + // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] + // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] + + float mA = bA.InvMass, mB = bB.InvMass; + float iA = bA.InvI, iB = bB.InvI; + + _mass.Col1.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB; + _mass.Col2.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB; + _mass.Col3.X = -rA.Y * iA - rB.Y * iB; + _mass.Col1.Y = _mass.Col2.X; + _mass.Col2.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB; + _mass.Col3.Y = rA.X * iA + rB.X * iB; + _mass.Col1.Z = _mass.Col3.X; + _mass.Col2.Z = _mass.Col3.Y; + _mass.Col3.Z = iA + iB; + + if (Settings.EnableWarmstarting) + { + // Scale impulses to support a variable time step. + _impulse *= step.dtRatio; + + Vector2 P = new Vector2(_impulse.X, _impulse.Y); + + bA.LinearVelocityInternal -= mA * P; + bA.AngularVelocityInternal -= iA * (MathUtils.Cross(rA, P) + _impulse.Z); + + bB.LinearVelocityInternal += mB * P; + bB.AngularVelocityInternal += iB * (MathUtils.Cross(rB, P) + _impulse.Z); + } + else + { + _impulse = Vector3.Zero; + } + } + + internal override void SolveVelocityConstraints(ref TimeStep step) + { + Body bA = BodyA; + Body bB = BodyB; + + Vector2 vA = bA.LinearVelocityInternal; + float wA = bA.AngularVelocityInternal; + Vector2 vB = bB.LinearVelocityInternal; + float wB = bB.AngularVelocityInternal; + + float mA = bA.InvMass, mB = bB.InvMass; + float iA = bA.InvI, iB = bB.InvI; + + Transform xfA, xfB; + bA.GetTransform(out xfA); + bB.GetTransform(out xfB); + + Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter); + Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter); + + // Solve point-to-point constraint + Vector2 Cdot1 = vB + MathUtils.Cross(wB, rB) - vA - MathUtils.Cross(wA, rA); + float Cdot2 = wB - wA; + Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); + + Vector3 impulse = _mass.Solve33(-Cdot); + _impulse += impulse; + + Vector2 P = new Vector2(impulse.X, impulse.Y); + + vA -= mA * P; + wA -= iA * (MathUtils.Cross(rA, P) + impulse.Z); + + vB += mB * P; + wB += iB * (MathUtils.Cross(rB, P) + impulse.Z); + + bA.LinearVelocityInternal = vA; + bA.AngularVelocityInternal = wA; + bB.LinearVelocityInternal = vB; + bB.AngularVelocityInternal = wB; + } + + internal override bool SolvePositionConstraints() + { + Body bA = BodyA; + Body bB = BodyB; + + float mA = bA.InvMass, mB = bB.InvMass; + float iA = bA.InvI, iB = bB.InvI; + + Transform xfA; + Transform xfB; + bA.GetTransform(out xfA); + bB.GetTransform(out xfB); + + Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter); + Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter); + + Vector2 C1 = bB.Sweep.C + rB - bA.Sweep.C - rA; + float C2 = bB.Sweep.A - bA.Sweep.A - ReferenceAngle; + + // Handle large detachment. + const float k_allowedStretch = 10.0f * Settings.LinearSlop; + float positionError = C1.Length(); + float angularError = Math.Abs(C2); + if (positionError > k_allowedStretch) + { + iA *= 1.0f; + iB *= 1.0f; + } + + _mass.Col1.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB; + _mass.Col2.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB; + _mass.Col3.X = -rA.Y * iA - rB.Y * iB; + _mass.Col1.Y = _mass.Col2.X; + _mass.Col2.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB; + _mass.Col3.Y = rA.X * iA + rB.X * iB; + _mass.Col1.Z = _mass.Col3.X; + _mass.Col2.Z = _mass.Col3.Y; + _mass.Col3.Z = iA + iB; + + Vector3 C = new Vector3(C1.X, C1.Y, C2); + + Vector3 impulse = _mass.Solve33(-C); + + Vector2 P = new Vector2(impulse.X, impulse.Y); + + bA.Sweep.C -= mA * P; + bA.Sweep.A -= iA * (MathUtils.Cross(rA, P) + impulse.Z); + + bB.Sweep.C += mB * P; + bB.Sweep.A += iB * (MathUtils.Cross(rB, P) + impulse.Z); + + bA.SynchronizeTransform(); + bB.SynchronizeTransform(); + + return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; + } + } +} \ No newline at end of file diff --git a/Dynamics/TimeStep.cs b/Dynamics/TimeStep.cs new file mode 100644 index 0000000..5c9a647 --- /dev/null +++ b/Dynamics/TimeStep.cs @@ -0,0 +1,45 @@ +/* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +namespace FarseerPhysics.Dynamics +{ + /// + /// This is an internal structure. + /// + public struct TimeStep + { + /// + /// Time step (Delta time) + /// + public float dt; + + /// + /// dt * inv_dt0 + /// + public float dtRatio; + + /// + /// Inverse time step (0 if dt == 0). + /// + public float inv_dt; + } +} \ No newline at end of file diff --git a/Dynamics/World.cs b/Dynamics/World.cs new file mode 100644 index 0000000..f0d79e9 --- /dev/null +++ b/Dynamics/World.cs @@ -0,0 +1,1456 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Diagnostics; +using FarseerPhysics.Collision; +using FarseerPhysics.Common; +using FarseerPhysics.Controllers; +using FarseerPhysics.Dynamics.Contacts; +using FarseerPhysics.Dynamics.Joints; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics +{ + /// + /// Contains filter data that can determine whether an object should be processed or not. + /// + public abstract class FilterData + { + public Category DisabledOnCategories = Category.None; + + public int DisabledOnGroup; + public Category EnabledOnCategories = Category.All; + public int EnabledOnGroup; + + public virtual bool IsActiveOn(Body body) + { + if (body == null || !body.Enabled || body.IsStatic) + return false; + + if (body.FixtureList == null) + return false; + + foreach (Fixture fixture in body.FixtureList) + { + //Disable + if ((fixture.CollisionGroup == DisabledOnGroup) && + fixture.CollisionGroup != 0 && DisabledOnGroup != 0) + return false; + + if ((fixture.CollisionCategories & DisabledOnCategories) != Category.None) + return false; + + if (EnabledOnGroup != 0 || EnabledOnCategories != Category.All) + { + //Enable + if ((fixture.CollisionGroup == EnabledOnGroup) && + fixture.CollisionGroup != 0 && EnabledOnGroup != 0) + return true; + + if ((fixture.CollisionCategories & EnabledOnCategories) != Category.None && + EnabledOnCategories != Category.All) + return true; + } + else + { + return true; + } + } + + return false; + } + + /// + /// Adds the category. + /// + /// The category. + public void AddDisabledCategory(Category category) + { + DisabledOnCategories |= category; + } + + /// + /// Removes the category. + /// + /// The category. + public void RemoveDisabledCategory(Category category) + { + DisabledOnCategories &= ~category; + } + + /// + /// Determines whether this body ignores the the specified controller. + /// + /// The category. + /// + /// true if the object has the specified category; otherwise, false. + /// + public bool IsInDisabledCategory(Category category) + { + return (DisabledOnCategories & category) == category; + } + + /// + /// Adds the category. + /// + /// The category. + public void AddEnabledCategory(Category category) + { + EnabledOnCategories |= category; + } + + /// + /// Removes the category. + /// + /// The category. + public void RemoveEnabledCategory(Category category) + { + EnabledOnCategories &= ~category; + } + + /// + /// Determines whether this body ignores the the specified controller. + /// + /// The category. + /// + /// true if the object has the specified category; otherwise, false. + /// + public bool IsInEnabledCategory(Category category) + { + return (EnabledOnCategories & category) == category; + } + } + + [Flags] + public enum WorldFlags + { + /// + /// Flag that indicates a new fixture has been added to the world. + /// + NewFixture = (1 << 0), + + /// + /// Flag that clear the forces after each time step. + /// + ClearForces = (1 << 2), + + SubStepping = (1 << 4), + } + + /// + /// The world class manages all physics entities, dynamic simulation, + /// and asynchronous queries. + /// + public class World + { + /// + /// Fires whenever a body has been added + /// + public BodyDelegate BodyAdded; + + /// + /// Fires whenever a body has been removed + /// + public BodyDelegate BodyRemoved; + + internal Queue ContactPool = new Queue(256); + + /// + /// Fires whenever a fixture has been added + /// + public FixtureDelegate FixtureAdded; + + /// + /// Fires whenever a fixture has been removed + /// + public FixtureDelegate FixtureRemoved; + + internal WorldFlags Flags; + + /// + /// Fires whenever a joint has been added + /// + public JointDelegate JointAdded; + + /// + /// Fires whenever a joint has been removed + /// + public JointDelegate JointRemoved; + + public ControllerDelegate ControllerAdded; + + public ControllerDelegate ControllerRemoved; + + private float _invDt0; + public Island Island = new Island(); + private Body[] _stack = new Body[64]; + private bool _stepComplete; + private HashSet _bodyAddList = new HashSet(); + private HashSet _bodyRemoveList = new HashSet(); + private HashSet _jointAddList = new HashSet(); + private HashSet _jointRemoveList = new HashSet(); + private TOIInput _input = new TOIInput(); + + /// + /// If false, the whole simulation stops. It still processes added and removed geometries. + /// + public bool Enabled = true; + +#if (!SILVERLIGHT) + private Stopwatch _watch = new Stopwatch(); +#endif + + /// + /// Initializes a new instance of the class. + /// + private World() + { + Flags = WorldFlags.ClearForces; + + ControllerList = new List(); + BreakableBodyList = new List(); + BodyList = new List(32); + JointList = new List(32); + } + + public World(Vector2 gravity, AABB span) + : this() + { + Gravity = gravity; + ContactManager = new ContactManager(new QuadTreeBroadPhase(span)); + } + + /// + /// Initializes a new instance of the class. + /// + /// The gravity. + public World(Vector2 gravity) + : this() + { + ContactManager = new ContactManager(new DynamicTreeBroadPhase()); + Gravity = gravity; + } + + public List ControllerList { get; private set; } + + public List BreakableBodyList { get; private set; } + + public float UpdateTime { get; private set; } + + public float ContinuousPhysicsTime { get; private set; } + + public float ControllersUpdateTime { get; private set; } + + public float AddRemoveTime { get; private set; } + + public float ContactsUpdateTime { get; private set; } + + public float SolveUpdateTime { get; private set; } + + /// + /// Get the number of broad-phase proxies. + /// + /// The proxy count. + public int ProxyCount + { + get { return ContactManager.BroadPhase.ProxyCount; } + } + + /// + /// Change the global gravity vector. + /// + /// The gravity. + public Vector2 Gravity; + + /// + /// Set flag to control automatic clearing of forces after each time step. + /// + /// true if it should auto clear forces; otherwise, false. + public bool AutoClearForces + { + set + { + if (value) + { + Flags |= WorldFlags.ClearForces; + } + else + { + Flags &= ~WorldFlags.ClearForces; + } + } + get { return (Flags & WorldFlags.ClearForces) == WorldFlags.ClearForces; } + } + + /// + /// Get the contact manager for testing. + /// + /// The contact manager. + public ContactManager ContactManager { get; private set; } + + /// + /// Get the world body list. + /// + /// Thehead of the world body list. + public List BodyList { get; private set; } + + /// + /// Get the world joint list. + /// + /// The joint list. + public List JointList { get; private set; } + + /// + /// Get the world contact list. With the returned contact, use Contact.GetNext to get + /// the next contact in the world list. A null contact indicates the end of the list. + /// + /// The head of the world contact list. + public List ContactList + { + get { return ContactManager.ContactList; } + } + + /// + /// Enable/disable single stepped continuous physics. For testing. + /// + public bool EnableSubStepping + { + set + { + if (value) + { + Flags |= WorldFlags.SubStepping; + } + else + { + Flags &= ~WorldFlags.SubStepping; + } + } + get { return (Flags & WorldFlags.SubStepping) == WorldFlags.SubStepping; } + } + + /// + /// Add a rigid body. + /// + /// + internal void AddBody(Body body) + { + Debug.Assert(!_bodyAddList.Contains(body), "You are adding the same body more than once."); + + if (!_bodyAddList.Contains(body)) + _bodyAddList.Add(body); + } + + /// + /// Destroy a rigid body. + /// Warning: This automatically deletes all associated shapes and joints. + /// + /// The body. + public void RemoveBody(Body body) + { + Debug.Assert(!_bodyRemoveList.Contains(body), + "The body is already marked for removal. You are removing the body more than once."); + + if (!_bodyRemoveList.Contains(body)) + _bodyRemoveList.Add(body); + } + + /// + /// Create a joint to constrain bodies together. This may cause the connected bodies to cease colliding. + /// + /// The joint. + public void AddJoint(Joint joint) + { + Debug.Assert(!_jointAddList.Contains(joint), "You are adding the same joint more than once."); + + if (!_jointAddList.Contains(joint)) + _jointAddList.Add(joint); + } + + private void RemoveJoint(Joint joint, bool doCheck) + { + if (doCheck) + { + Debug.Assert(!_jointRemoveList.Contains(joint), + "The joint is already marked for removal. You are removing the joint more than once."); + } + + if (!_jointRemoveList.Contains(joint)) + _jointRemoveList.Add(joint); + } + + /// + /// Destroy a joint. This may cause the connected bodies to begin colliding. + /// + /// The joint. + public void RemoveJoint(Joint joint) + { + RemoveJoint(joint, true); + } + + /// + /// All adds and removes are cached by the World duing a World step. + /// To process the changes before the world updates again, call this method. + /// + public void ProcessChanges() + { + ProcessAddedBodies(); + ProcessAddedJoints(); + + ProcessRemovedBodies(); + ProcessRemovedJoints(); + } + + private void ProcessRemovedJoints() + { + if (_jointRemoveList.Count > 0) + { + foreach (Joint joint in _jointRemoveList) + { + bool collideConnected = joint.CollideConnected; + + // Remove from the world list. + JointList.Remove(joint); + + // Disconnect from island graph. + Body bodyA = joint.BodyA; + Body bodyB = joint.BodyB; + + // Wake up connected bodies. + bodyA.Awake = true; + + // WIP David + if (!joint.IsFixedType()) + { + bodyB.Awake = true; + } + + // Remove from body 1. + if (joint.EdgeA.Prev != null) + { + joint.EdgeA.Prev.Next = joint.EdgeA.Next; + } + + if (joint.EdgeA.Next != null) + { + joint.EdgeA.Next.Prev = joint.EdgeA.Prev; + } + + if (joint.EdgeA == bodyA.JointList) + { + bodyA.JointList = joint.EdgeA.Next; + } + + joint.EdgeA.Prev = null; + joint.EdgeA.Next = null; + + // WIP David + if (!joint.IsFixedType()) + { + // Remove from body 2 + if (joint.EdgeB.Prev != null) + { + joint.EdgeB.Prev.Next = joint.EdgeB.Next; + } + + if (joint.EdgeB.Next != null) + { + joint.EdgeB.Next.Prev = joint.EdgeB.Prev; + } + + if (joint.EdgeB == bodyB.JointList) + { + bodyB.JointList = joint.EdgeB.Next; + } + + joint.EdgeB.Prev = null; + joint.EdgeB.Next = null; + } + + // WIP David + if (!joint.IsFixedType()) + { + // If the joint prevents collisions, then flag any contacts for filtering. + if (collideConnected == false) + { + ContactEdge edge = bodyB.ContactList; + while (edge != null) + { + if (edge.Other == bodyA) + { + // Flag the contact for filtering at the next time step (where either + // body is awake). + edge.Contact.FlagForFiltering(); + } + + edge = edge.Next; + } + } + } + + if (JointRemoved != null) + { + JointRemoved(joint); + } + } + + _jointRemoveList.Clear(); + } + } + + private void ProcessAddedJoints() + { + if (_jointAddList.Count > 0) + { + foreach (Joint joint in _jointAddList) + { + // Connect to the world list. + JointList.Add(joint); + + // Connect to the bodies' doubly linked lists. + joint.EdgeA.Joint = joint; + joint.EdgeA.Other = joint.BodyB; + joint.EdgeA.Prev = null; + joint.EdgeA.Next = joint.BodyA.JointList; + + if (joint.BodyA.JointList != null) + joint.BodyA.JointList.Prev = joint.EdgeA; + + joint.BodyA.JointList = joint.EdgeA; + + // WIP David + if (!joint.IsFixedType()) + { + joint.EdgeB.Joint = joint; + joint.EdgeB.Other = joint.BodyA; + joint.EdgeB.Prev = null; + joint.EdgeB.Next = joint.BodyB.JointList; + + if (joint.BodyB.JointList != null) + joint.BodyB.JointList.Prev = joint.EdgeB; + + joint.BodyB.JointList = joint.EdgeB; + + Body bodyA = joint.BodyA; + Body bodyB = joint.BodyB; + + // If the joint prevents collisions, then flag any contacts for filtering. + if (joint.CollideConnected == false) + { + ContactEdge edge = bodyB.ContactList; + while (edge != null) + { + if (edge.Other == bodyA) + { + // Flag the contact for filtering at the next time step (where either + // body is awake). + edge.Contact.FlagForFiltering(); + } + + edge = edge.Next; + } + } + } + + if (JointAdded != null) + JointAdded(joint); + + // Note: creating a joint doesn't wake the bodies. + } + + _jointAddList.Clear(); + } + } + + private void ProcessAddedBodies() + { + if (_bodyAddList.Count > 0) + { + foreach (Body body in _bodyAddList) + { + // Add to world list. + BodyList.Add(body); + + if (BodyAdded != null) + BodyAdded(body); + } + + _bodyAddList.Clear(); + } + } + + private void ProcessRemovedBodies() + { + if (_bodyRemoveList.Count > 0) + { + foreach (Body body in _bodyRemoveList) + { + Debug.Assert(BodyList.Count > 0); + + // You tried to remove a body that is not contained in the BodyList. + // Are you removing the body more than once? + Debug.Assert(BodyList.Contains(body)); + + // Delete the attached joints. + JointEdge je = body.JointList; + while (je != null) + { + JointEdge je0 = je; + je = je.Next; + + RemoveJoint(je0.Joint, false); + } + body.JointList = null; + + // Delete the attached contacts. + ContactEdge ce = body.ContactList; + while (ce != null) + { + ContactEdge ce0 = ce; + ce = ce.Next; + ContactManager.Destroy(ce0.Contact); + } + body.ContactList = null; + + // Delete the attached fixtures. This destroys broad-phase proxies. + for (int i = 0; i < body.FixtureList.Count; i++) + { + body.FixtureList[i].DestroyProxies(ContactManager.BroadPhase); + body.FixtureList[i].Destroy(); + } + + body.FixtureList = null; + + // Remove world body list. + BodyList.Remove(body); + + if (BodyRemoved != null) + BodyRemoved(body); + } + + _bodyRemoveList.Clear(); + } + } + + /// + /// Take a time step. This performs collision detection, integration, + /// and consraint solution. + /// + /// The amount of time to simulate, this should not vary. + public void Step(float dt) + { +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + _watch.Start(); +#endif + + ProcessChanges(); + +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + AddRemoveTime = _watch.ElapsedTicks; +#endif + //If there is no change in time, no need to calculate anything. + if (dt == 0 || !Enabled) + { +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + { + _watch.Stop(); + _watch.Reset(); + } +#endif + return; + } + + // If new fixtures were added, we need to find the new contacts. + if ((Flags & WorldFlags.NewFixture) == WorldFlags.NewFixture) + { + ContactManager.FindNewContacts(); + Flags &= ~WorldFlags.NewFixture; + } + + TimeStep step; + step.inv_dt = 1.0f / dt; + step.dt = dt; + step.dtRatio = _invDt0 * dt; + + //Update controllers + for (int i = 0; i < ControllerList.Count; i++) + { + ControllerList[i].Update(dt); + } + +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + ControllersUpdateTime = _watch.ElapsedTicks - AddRemoveTime; +#endif + + // Update contacts. This is where some contacts are destroyed. + ContactManager.Collide(); + +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + ContactsUpdateTime = _watch.ElapsedTicks - (AddRemoveTime + ControllersUpdateTime); +#endif + // Integrate velocities, solve velocity raints, and integrate positions. + Solve(ref step); + +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + SolveUpdateTime = _watch.ElapsedTicks - (AddRemoveTime + ControllersUpdateTime + ContactsUpdateTime); +#endif + + // Handle TOI events. + if (Settings.ContinuousPhysics) + { + SolveTOI(ref step); + } + +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + ContinuousPhysicsTime = _watch.ElapsedTicks - + (AddRemoveTime + ControllersUpdateTime + ContactsUpdateTime + SolveUpdateTime); +#endif + _invDt0 = step.inv_dt; + + if ((Flags & WorldFlags.ClearForces) != 0) + { + ClearForces(); + } + + for (int i = 0; i < BreakableBodyList.Count; i++) + { + BreakableBodyList[i].Update(); + } + +#if (!SILVERLIGHT) + if (Settings.EnableDiagnostics) + { + _watch.Stop(); + //AddRemoveTime = 1000 * AddRemoveTime / Stopwatch.Frequency; + + UpdateTime = _watch.ElapsedTicks; + _watch.Reset(); + } +#endif + } + + /// + /// Call this after you are done with time steps to clear the forces. You normally + /// call this after each call to Step, unless you are performing sub-steps. By default, + /// forces will be automatically cleared, so you don't need to call this function. + /// + public void ClearForces() + { + for (int i = 0; i < BodyList.Count; i++) + { + Body body = BodyList[i]; + body.Force = Vector2.Zero; + body.Torque = 0.0f; + } + } + + /// + /// Query the world for all fixtures that potentially overlap the + /// provided AABB. + /// + /// Inside the callback: + /// Return true: Continues the query + /// Return false: Terminate the query + /// + /// A user implemented callback class. + /// The aabb query box. + public void QueryAABB(Func callback, ref AABB aabb) + { + ContactManager.BroadPhase.Query(proxyId => + { + FixtureProxy proxy = ContactManager.BroadPhase.GetProxy(proxyId); + return callback(proxy.Fixture); + }, ref aabb); + } + + /// + /// Ray-cast the world for all fixtures in the path of the ray. Your callback + /// controls whether you get the closest point, any point, or n-points. + /// The ray-cast ignores shapes that contain the starting point. + /// + /// Inside the callback: + /// return -1: ignore this fixture and continue + /// return 0: terminate the ray cast + /// return fraction: clip the ray to this point + /// return 1: don't clip the ray and continue + /// + /// A user implemented callback class. + /// The ray starting point. + /// The ray ending point. + public void RayCast(RayCastCallback callback, Vector2 point1, Vector2 point2) + { + RayCastInput input = new RayCastInput(); + input.MaxFraction = 1.0f; + input.Point1 = point1; + input.Point2 = point2; + + ContactManager.BroadPhase.RayCast((rayCastInput, proxyId) => + { + FixtureProxy proxy = ContactManager.BroadPhase.GetProxy(proxyId); + Fixture fixture = proxy.Fixture; + int index = proxy.ChildIndex; + RayCastOutput output; + bool hit = fixture.RayCast(out output, ref rayCastInput, index); + + if (hit) + { + float fraction = output.Fraction; + Vector2 point = (1.0f - fraction) * input.Point1 + + fraction * input.Point2; + return callback(fixture, point, output.Normal, fraction); + } + + return input.MaxFraction; + }, ref input); + } + + private void Solve(ref TimeStep step) + { + // Size the island for the worst case. + Island.Reset(BodyList.Count, + ContactManager.ContactList.Count, + JointList.Count, + ContactManager); + + // Clear all the island flags. + foreach (Body b in BodyList) + { + b.Flags &= ~BodyFlags.Island; + } + + for (int i = 0; i < ContactManager.ContactList.Count; i++) + { + Contact c = ContactManager.ContactList[i]; + c.Flags &= ~ContactFlags.Island; + } + foreach (Joint j in JointList) + { + j.IslandFlag = false; + } + + // Build and simulate all awake islands. + int stackSize = BodyList.Count; + if (stackSize > _stack.Length) + _stack = new Body[Math.Max(_stack.Length * 2, stackSize)]; + + for (int index = BodyList.Count - 1; index >= 0; index--) + { + Body seed = BodyList[index]; + if ((seed.Flags & (BodyFlags.Island)) != BodyFlags.None) + { + continue; + } + + if (seed.Awake == false || seed.Enabled == false) + { + continue; + } + + // The seed can be dynamic or kinematic. + if (seed.BodyType == BodyType.Static) + { + continue; + } + + // Reset island and stack. + Island.Clear(); + int stackCount = 0; + _stack[stackCount++] = seed; + seed.Flags |= BodyFlags.Island; + + // Perform a depth first search (DFS) on the constraint graph. + while (stackCount > 0) + { + // Grab the next body off the stack and add it to the island. + Body b = _stack[--stackCount]; + Debug.Assert(b.Enabled); + Island.Add(b); + + // Make sure the body is awake. + b.Awake = true; + + // To keep islands as small as possible, we don't + // propagate islands across static bodies. + if (b.BodyType == BodyType.Static) + { + continue; + } + + // Search all contacts connected to this body. + for (ContactEdge ce = b.ContactList; ce != null; ce = ce.Next) + { + Contact contact = ce.Contact; + + // Has this contact already been added to an island? + if ((contact.Flags & ContactFlags.Island) != ContactFlags.None) + { + continue; + } + + // Is this contact solid and touching? + if (!ce.Contact.Enabled || !ce.Contact.IsTouching()) + { + continue; + } + + // Skip sensors. + bool sensorA = contact.FixtureA.IsSensor; + bool sensorB = contact.FixtureB.IsSensor; + if (sensorA || sensorB) + { + continue; + } + + Island.Add(contact); + contact.Flags |= ContactFlags.Island; + + Body other = ce.Other; + + // Was the other body already added to this island? + if ((other.Flags & BodyFlags.Island) != BodyFlags.None) + { + continue; + } + + Debug.Assert(stackCount < stackSize); + _stack[stackCount++] = other; + other.Flags |= BodyFlags.Island; + } + + // Search all joints connect to this body. + for (JointEdge je = b.JointList; je != null; je = je.Next) + { + if (je.Joint.IslandFlag) + { + continue; + } + + Body other = je.Other; + + // WIP David + //Enter here when it's a non-fixed joint. Non-fixed joints have a other body. + if (other != null) + { + // Don't simulate joints connected to inactive bodies. + if (other.Enabled == false) + { + continue; + } + + Island.Add(je.Joint); + je.Joint.IslandFlag = true; + + if ((other.Flags & BodyFlags.Island) != BodyFlags.None) + { + continue; + } + + Debug.Assert(stackCount < stackSize); + _stack[stackCount++] = other; + other.Flags |= BodyFlags.Island; + } + else + { + Island.Add(je.Joint); + je.Joint.IslandFlag = true; + } + } + } + + Island.Solve(ref step, ref Gravity); + + // Post solve cleanup. + for (int i = 0; i < Island.BodyCount; ++i) + { + // Allow static bodies to participate in other islands. + Body b = Island.Bodies[i]; + if (b.BodyType == BodyType.Static) + { + b.Flags &= ~BodyFlags.Island; + } + } + } + + // Synchronize fixtures, check for out of range bodies. + foreach (Body b in BodyList) + { + // If a body was not in an island then it did not move. + if ((b.Flags & BodyFlags.Island) != BodyFlags.Island) + { + continue; + } + + if (b.BodyType == BodyType.Static) + { + continue; + } + + // Update fixtures (for broad-phase). + b.SynchronizeFixtures(); + } + + // Look for new contacts. + ContactManager.FindNewContacts(); + } + + /// + /// Find TOI contacts and solve them. + /// + /// The step. + private void SolveTOI(ref TimeStep step) + { + Island.Reset(2 * Settings.MaxTOIContacts, Settings.MaxTOIContacts, 0, ContactManager); + + if (_stepComplete) + { + for (int i = 0; i < BodyList.Count; i++) + { + BodyList[i].Flags &= ~BodyFlags.Island; + BodyList[i].Sweep.Alpha0 = 0.0f; + } + + for (int i = 0; i < ContactManager.ContactList.Count; i++) + { + Contact c = ContactManager.ContactList[i]; + + // Invalidate TOI + c.Flags &= ~(ContactFlags.TOI | ContactFlags.Island); + c.TOICount = 0; + c.TOI = 1.0f; + } + } + + // Find TOI events and solve them. + for (; ; ) + { + // Find the first TOI. + Contact minContact = null; + float minAlpha = 1.0f; + + for (int i = 0; i < ContactManager.ContactList.Count; i++) + { + Contact c = ContactManager.ContactList[i]; + + // Is this contact disabled? + if (c.Enabled == false) + { + continue; + } + + // Prevent excessive sub-stepping. + if (c.TOICount > Settings.MaxSubSteps) + { + continue; + } + + float alpha; + if ((c.Flags & ContactFlags.TOI) == ContactFlags.TOI) + { + // This contact has a valid cached TOI. + alpha = c.TOI; + } + else + { + Fixture fA = c.FixtureA; + Fixture fB = c.FixtureB; + + // Is there a sensor? + if (fA.IsSensor || fB.IsSensor) + { + continue; + } + + Body bA = fA.Body; + Body bB = fB.Body; + + BodyType typeA = bA.BodyType; + BodyType typeB = bB.BodyType; + //Debug.Assert(typeA == BodyType.Dynamic || typeB == BodyType.Dynamic); + + bool awakeA = bA.Awake && typeA != BodyType.Static; + bool awakeB = bB.Awake && typeB != BodyType.Static; + + // Is at least one body awake? + if (awakeA == false && awakeB == false) + { + continue; + } + + bool collideA = (bA.IsBullet || typeA != BodyType.Dynamic) && !bA.IgnoreCCD; + bool collideB = (bB.IsBullet || typeB != BodyType.Dynamic) && !bB.IgnoreCCD; + + // Are these two non-bullet dynamic bodies? + if (collideA == false && collideB == false) + { + continue; + } + + // Compute the TOI for this contact. + // Put the sweeps onto the same time interval. + float alpha0 = bA.Sweep.Alpha0; + + if (bA.Sweep.Alpha0 < bB.Sweep.Alpha0) + { + alpha0 = bB.Sweep.Alpha0; + bA.Sweep.Advance(alpha0); + } + else if (bB.Sweep.Alpha0 < bA.Sweep.Alpha0) + { + alpha0 = bA.Sweep.Alpha0; + bB.Sweep.Advance(alpha0); + } + + Debug.Assert(alpha0 < 1.0f); + + // Compute the time of impact in interval [0, minTOI] + _input.ProxyA.Set(fA.Shape, c.ChildIndexA); + _input.ProxyB.Set(fB.Shape, c.ChildIndexB); + _input.SweepA = bA.Sweep; + _input.SweepB = bB.Sweep; + _input.TMax = 1.0f; + + TOIOutput output; + TimeOfImpact.CalculateTimeOfImpact(out output, _input); + + // Beta is the fraction of the remaining portion of the . + float beta = output.T; + if (output.State == TOIOutputState.Touching) + { + alpha = Math.Min(alpha0 + (1.0f - alpha0) * beta, 1.0f); + } + else + { + alpha = 1.0f; + } + + c.TOI = alpha; + c.Flags |= ContactFlags.TOI; + } + + if (alpha < minAlpha) + { + // This is the minimum TOI found so far. + minContact = c; + minAlpha = alpha; + } + } + + if (minContact == null || 1.0f - 10.0f * Settings.Epsilon < minAlpha) + { + // No more TOI events. Done! + _stepComplete = true; + break; + } + + // Advance the bodies to the TOI. + Fixture fA1 = minContact.FixtureA; + Fixture fB1 = minContact.FixtureB; + Body bA1 = fA1.Body; + Body bB1 = fB1.Body; + + Sweep backup1 = bA1.Sweep; + Sweep backup2 = bB1.Sweep; + + bA1.Advance(minAlpha); + bB1.Advance(minAlpha); + + // The TOI contact likely has some new contact points. + minContact.Update(ContactManager); + minContact.Flags &= ~ContactFlags.TOI; + ++minContact.TOICount; + + // Is the contact solid? + if (minContact.Enabled == false || minContact.IsTouching() == false) + { + // Restore the sweeps. + minContact.Enabled = false; + bA1.Sweep = backup1; + bB1.Sweep = backup2; + bA1.SynchronizeTransform(); + bB1.SynchronizeTransform(); + continue; + } + + bA1.Awake = true; + bB1.Awake = true; + + // Build the island + Island.Clear(); + Island.Add(bA1); + Island.Add(bB1); + Island.Add(minContact); + + bA1.Flags |= BodyFlags.Island; + bB1.Flags |= BodyFlags.Island; + minContact.Flags |= ContactFlags.Island; + + // Get contacts on bodyA and bodyB. + Body[] bodies = { bA1, bB1 }; + for (int i = 0; i < 2; ++i) + { + Body body = bodies[i]; + if (body.BodyType == BodyType.Dynamic) + { + // for (ContactEdge ce = body.ContactList; ce && Island.BodyCount < Settings.MaxTOIContacts; ce = ce.Next) + for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next) + { + Contact contact = ce.Contact; + + // Has this contact already been added to the island? + if ((contact.Flags & ContactFlags.Island) == ContactFlags.Island) + { + continue; + } + + // Only add static, kinematic, or bullet bodies. + Body other = ce.Other; + if (other.BodyType == BodyType.Dynamic && + body.IsBullet == false && other.IsBullet == false) + { + continue; + } + + // Skip sensors. + if (contact.FixtureA.IsSensor || contact.FixtureB.IsSensor) + { + continue; + } + + // Tentatively advance the body to the TOI. + Sweep backup = other.Sweep; + if ((other.Flags & BodyFlags.Island) == 0) + { + other.Advance(minAlpha); + } + + // Update the contact points + contact.Update(ContactManager); + + // Was the contact disabled by the user? + if (contact.Enabled == false) + { + other.Sweep = backup; + other.SynchronizeTransform(); + continue; + } + + // Are there contact points? + if (contact.IsTouching() == false) + { + other.Sweep = backup; + other.SynchronizeTransform(); + continue; + } + + // Add the contact to the island + contact.Flags |= ContactFlags.Island; + Island.Add(contact); + + // Has the other body already been added to the island? + if ((other.Flags & BodyFlags.Island) == BodyFlags.Island) + { + continue; + } + + // Add the other body to the island. + other.Flags |= BodyFlags.Island; + + if (other.BodyType != BodyType.Static) + { + other.Awake = true; + } + + Island.Add(other); + } + } + } + + TimeStep subStep; + subStep.dt = (1.0f - minAlpha) * step.dt; + subStep.inv_dt = 1.0f / subStep.dt; + subStep.dtRatio = 1.0f; + //subStep.positionIterations = 20; + //subStep.velocityIterations = step.velocityIterations; + //subStep.warmStarting = false; + Island.SolveTOI(ref subStep); + + // Reset island flags and synchronize broad-phase proxies. + for (int i = 0; i < Island.BodyCount; ++i) + { + Body body = Island.Bodies[i]; + body.Flags &= ~BodyFlags.Island; + + if (body.BodyType != BodyType.Dynamic) + { + continue; + } + + body.SynchronizeFixtures(); + + // Invalidate all contact TOIs on this displaced body. + for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next) + { + ce.Contact.Flags &= ~(ContactFlags.TOI | ContactFlags.Island); + } + } + + // Commit fixture proxy movements to the broad-phase so that new contacts are created. + // Also, some contacts can be destroyed. + ContactManager.FindNewContacts(); + + if (EnableSubStepping) + { + _stepComplete = false; + break; + } + } + } + + public void AddController(Controller controller) + { + Debug.Assert(!ControllerList.Contains(controller), "You are adding the same controller more than once."); + + controller.World = this; + ControllerList.Add(controller); + + if (ControllerAdded != null) + ControllerAdded(controller); + } + + public void RemoveController(Controller controller) + { + Debug.Assert(ControllerList.Contains(controller), + "You are removing a controller that is not in the simulation."); + + if (ControllerList.Contains(controller)) + { + ControllerList.Remove(controller); + + if (ControllerRemoved != null) + ControllerRemoved(controller); + } + } + + public void AddBreakableBody(BreakableBody breakableBody) + { + BreakableBodyList.Add(breakableBody); + } + + public void RemoveBreakableBody(BreakableBody breakableBody) + { + //The breakable body list does not contain the body you tried to remove. + Debug.Assert(BreakableBodyList.Contains(breakableBody)); + + BreakableBodyList.Remove(breakableBody); + } + + public Fixture TestPoint(Vector2 point) + { + AABB aabb; + Vector2 d = new Vector2(Settings.Epsilon, Settings.Epsilon); + aabb.LowerBound = point - d; + aabb.UpperBound = point + d; + + Fixture myFixture = null; + + // Query the world for overlapping shapes. + QueryAABB( + fixture => + { + bool inside = fixture.TestPoint(ref point); + if (inside) + { + myFixture = fixture; + return false; + } + + // Continue the query. + return true; + }, ref aabb); + + return myFixture; + } + + /// + /// Returns a list of fixtures that are at the specified point. + /// + /// The point. + /// + public List TestPointAll(Vector2 point) + { + AABB aabb; + Vector2 d = new Vector2(Settings.Epsilon, Settings.Epsilon); + aabb.LowerBound = point - d; + aabb.UpperBound = point + d; + + List fixtures = new List(); + + // Query the world for overlapping shapes. + QueryAABB( + fixture => + { + bool inside = fixture.TestPoint(ref point); + if (inside) + fixtures.Add(fixture); + + // Continue the query. + return true; + }, ref aabb); + + return fixtures; + } + + public void Clear() + { + ProcessChanges(); + + for (int i = BodyList.Count - 1; i >= 0; i--) + { + RemoveBody(BodyList[i]); + } + + for (int i = ControllerList.Count - 1; i >= 0; i--) + { + RemoveController(ControllerList[i]); + } + + for (int i = BreakableBodyList.Count - 1; i >= 0; i--) + { + RemoveBreakableBody(BreakableBodyList[i]); + } + + ProcessChanges(); + } + } +} \ No newline at end of file diff --git a/Dynamics/WorldCallbacks.cs b/Dynamics/WorldCallbacks.cs new file mode 100644 index 0000000..095030e --- /dev/null +++ b/Dynamics/WorldCallbacks.cs @@ -0,0 +1,74 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using FarseerPhysics.Collision; +using FarseerPhysics.Controllers; +using FarseerPhysics.Dynamics.Contacts; +using FarseerPhysics.Dynamics.Joints; +using Microsoft.Xna.Framework; + +namespace FarseerPhysics.Dynamics +{ + /// + /// Called for each fixture found in the query. You control how the ray cast + /// proceeds by returning a float: + /// -1 to filter, 0 to terminate, fraction to clip the ray for closest hit, 1 to continue + /// + public delegate float RayCastCallback(Fixture fixture, Vector2 point, Vector2 normal, float fraction); + + /// + /// This delegate is called when a contact is deleted + /// + public delegate void EndContactDelegate(Contact contact); + + /// + /// This delegate is called when a contact is created + /// + public delegate bool BeginContactDelegate(Contact contact); + + public delegate void PreSolveDelegate(Contact contact, ref Manifold oldManifold); + + public delegate void PostSolveDelegate(Contact contact, ContactConstraint impulse); + + public delegate void FixtureDelegate(Fixture fixture); + + public delegate void JointDelegate(Joint joint); + + public delegate void BodyDelegate(Body body); + + public delegate void ControllerDelegate(Controller controller); + + public delegate bool CollisionFilterDelegate(Fixture fixtureA, Fixture fixtureB); + + public delegate void BroadphaseDelegate(ref FixtureProxy proxyA, ref FixtureProxy proxyB); + + public delegate bool BeforeCollisionEventHandler(Fixture fixtureA, Fixture fixtureB); + + public delegate bool OnCollisionEventHandler(Fixture fixtureA, Fixture fixtureB, Contact contact); + + public delegate void AfterCollisionEventHandler(Fixture fixtureA, Fixture fixtureB, Contact contact); + + public delegate void OnSeparationEventHandler(Fixture fixtureA, Fixture fixtureB); +} \ No newline at end of file diff --git a/PrimitiveBatch.cs b/PrimitiveBatch.cs new file mode 100644 index 0000000..ff05a68 --- /dev/null +++ b/PrimitiveBatch.cs @@ -0,0 +1,196 @@ +using System; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Graphics; + +namespace FarseerPhysics.DebugViews +{ + public class PrimitiveBatch : IDisposable + { + private const int DefaultBufferSize = 500; + + // a basic effect, which contains the shaders that we will use to draw our + // primitives. + private BasicEffect _basicEffect; + + // the device that we will issue draw calls to. + private GraphicsDevice _device; + + // hasBegun is flipped to true once Begin is called, and is used to make + // sure users don't call End before Begin is called. + private bool _hasBegun; + + private bool _isDisposed; + private VertexPositionColor[] _lineVertices; + private int _lineVertsCount; + private VertexPositionColor[] _triangleVertices; + private int _triangleVertsCount; + + + /// + /// the constructor creates a new PrimitiveBatch and sets up all of the internals + /// that PrimitiveBatch will need. + /// + /// The graphics device. + public PrimitiveBatch(GraphicsDevice graphicsDevice) + : this(graphicsDevice, DefaultBufferSize) + { + } + + public PrimitiveBatch(GraphicsDevice graphicsDevice, int bufferSize) + { + if (graphicsDevice == null) + { + throw new ArgumentNullException("graphicsDevice"); + } + _device = graphicsDevice; + + _triangleVertices = new VertexPositionColor[bufferSize - bufferSize % 3]; + _lineVertices = new VertexPositionColor[bufferSize - bufferSize % 2]; + + // set up a new basic effect, and enable vertex colors. + _basicEffect = new BasicEffect(graphicsDevice); + _basicEffect.VertexColorEnabled = true; + } + + #region IDisposable Members + + public void Dispose() + { + Dispose(true); + GC.SuppressFinalize(this); + } + + #endregion + + public void SetProjection(ref Matrix projection) + { + _basicEffect.Projection = projection; + } + + protected virtual void Dispose(bool disposing) + { + if (disposing && !_isDisposed) + { + if (_basicEffect != null) + _basicEffect.Dispose(); + + _isDisposed = true; + } + } + + + /// + /// Begin is called to tell the PrimitiveBatch what kind of primitives will be + /// drawn, and to prepare the graphics card to render those primitives. + /// + /// The projection. + /// The view. + public void Begin(ref Matrix projection, ref Matrix view) + { + if (_hasBegun) + { + throw new InvalidOperationException("End must be called before Begin can be called again."); + } + + //tell our basic effect to begin. + _basicEffect.Projection = projection; + _basicEffect.View = view; + _basicEffect.CurrentTechnique.Passes[0].Apply(); + + // flip the error checking boolean. It's now ok to call AddVertex, Flush, + // and End. + _hasBegun = true; + } + + public bool IsReady() + { + return _hasBegun; + } + + public void AddVertex(Vector2 vertex, Color color, PrimitiveType primitiveType) + { + if (!_hasBegun) + { + throw new InvalidOperationException("Begin must be called before AddVertex can be called."); + } + if (primitiveType == PrimitiveType.LineStrip || + primitiveType == PrimitiveType.TriangleStrip) + { + throw new NotSupportedException("The specified primitiveType is not supported by PrimitiveBatch."); + } + + if (primitiveType == PrimitiveType.TriangleList) + { + if (_triangleVertsCount >= _triangleVertices.Length) + { + FlushTriangles(); + } + _triangleVertices[_triangleVertsCount].Position = new Vector3(vertex, -0.1f); + _triangleVertices[_triangleVertsCount].Color = color; + _triangleVertsCount++; + } + if (primitiveType == PrimitiveType.LineList) + { + if (_lineVertsCount >= _lineVertices.Length) + { + FlushLines(); + } + _lineVertices[_lineVertsCount].Position = new Vector3(vertex, 0f); + _lineVertices[_lineVertsCount].Color = color; + _lineVertsCount++; + } + } + + + /// + /// End is called once all the primitives have been drawn using AddVertex. + /// it will call Flush to actually submit the draw call to the graphics card, and + /// then tell the basic effect to end. + /// + public void End() + { + if (!_hasBegun) + { + throw new InvalidOperationException("Begin must be called before End can be called."); + } + + // Draw whatever the user wanted us to draw + FlushTriangles(); + FlushLines(); + + _hasBegun = false; + } + + private void FlushTriangles() + { + if (!_hasBegun) + { + throw new InvalidOperationException("Begin must be called before Flush can be called."); + } + if (_triangleVertsCount >= 3) + { + int primitiveCount = _triangleVertsCount / 3; + // submit the draw call to the graphics card + _device.SamplerStates[0] = SamplerState.AnisotropicClamp; + _device.DrawUserPrimitives(PrimitiveType.TriangleList, _triangleVertices, 0, primitiveCount); + _triangleVertsCount -= primitiveCount * 3; + } + } + + private void FlushLines() + { + if (!_hasBegun) + { + throw new InvalidOperationException("Begin must be called before Flush can be called."); + } + if (_lineVertsCount >= 2) + { + int primitiveCount = _lineVertsCount / 2; + // submit the draw call to the graphics card + _device.SamplerStates[0] = SamplerState.AnisotropicClamp; + _device.DrawUserPrimitives(PrimitiveType.LineList, _lineVertices, 0, primitiveCount); + _lineVertsCount -= primitiveCount * 2; + } + } + } +} \ No newline at end of file diff --git a/ScreenSystem/FramerateCounterComponent.cs b/ScreenSystem/FramerateCounterComponent.cs index a936daa..57faf26 100644 --- a/ScreenSystem/FramerateCounterComponent.cs +++ b/ScreenSystem/FramerateCounterComponent.cs @@ -48,10 +48,10 @@ namespace FarseerPhysics.SamplesFramework string fps = string.Format(_format, "{0} fps", _frameRate); _screenManager.SpriteBatch.Begin(); - _screenManager.SpriteBatch.DrawString(_screenManager.Fonts.FrameRateCounterFont, fps, + /*_screenManager.SpriteBatch.DrawString(_screenManager.Fonts.FrameRateCounterFont, fps, _position + Vector2.One, Color.Black); _screenManager.SpriteBatch.DrawString(_screenManager.Fonts.FrameRateCounterFont, fps, - _position, Color.White); + _position, Color.White);*/ _screenManager.SpriteBatch.End(); } } diff --git a/ScreenSystem/InputHelper.cs b/ScreenSystem/InputHelper.cs index eb62565..6195395 100644 --- a/ScreenSystem/InputHelper.cs +++ b/ScreenSystem/InputHelper.cs @@ -3,7 +3,6 @@ using Microsoft.Xna.Framework; using Microsoft.Xna.Framework.Content; using Microsoft.Xna.Framework.Graphics; using Microsoft.Xna.Framework.Input; -using Microsoft.Xna.Framework.Input.Touch; using FarseerPhysics.SamplesFramework; using GameStateManagement; @@ -23,7 +22,6 @@ namespace FarseerPhysics.SamplesFramework2 public class InputHelper { - private readonly List _gestures = new List(); private GamePadState _currentGamePadState; private KeyboardState _currentKeyboardState; private MouseState _currentMouseState; @@ -197,11 +195,11 @@ namespace FarseerPhysics.SamplesFramework2 #endif } - _gestures.Clear(); + /*_gestures.Clear(); while (TouchPanel.IsGestureAvailable) { _gestures.Add(TouchPanel.ReadGesture()); - } + }*/ // Update cursor Vector2 oldCursor = _cursor; diff --git a/ScreenSystem/InputState.cs b/ScreenSystem/InputState.cs deleted file mode 100644 index 3f1705d..0000000 --- a/ScreenSystem/InputState.cs +++ /dev/null @@ -1,671 +0,0 @@ -#region File Description -//----------------------------------------------------------------------------- -// InputState.cs -// -// Microsoft XNA Community Game Platform -// Copyright (C) Microsoft Corporation. All rights reserved. -//----------------------------------------------------------------------------- -#endregion - -using System.Collections.Generic; -using Microsoft.Xna.Framework; -using Microsoft.Xna.Framework.Content; -using Microsoft.Xna.Framework.Input; -using Microsoft.Xna.Framework.Input.Touch; -using FarseerPhysics.SamplesFramework; -using Microsoft.Xna.Framework.Graphics; -using System; -#if WINDOWS -using XNACC.BaseTypes; -#endif -using System.Linq; - - -namespace GameStateManagement -{ - - /// - /// an enum of all available mouse buttons. - /// - public enum MouseButtons - { - LeftButton, - MiddleButton, - RightButton, - ExtraButton1, - ExtraButton2 - } - - /// - /// Helper for reading input from keyboard, gamepad, and touch input. This class - /// tracks both the current and previous state of the input devices, and implements - /// query methods for high level input actions such as "move up through the menu" - /// or "pause the game". - /// -#if WINDOWS - public class InputState : IConsoleKeyboard -#else - public class InputState -#endif - { - -#if WINDOWS - #region XNACC - /* - * These are needed for XNACC - * -- Nathan Adams [adamsna@datanethost.net] - 5/26/2012 - */ - - private KeyboardState KeyState; - private List newlyPressedKeys = new List(); - private List heldKeys = new List(); - private Keys[] oldPressedKeys; - private Keys[] newPressedKeys; - - public KeyboardState CurrentKeyboardState - { - get { return KeyState; } - } - - public IList NewlyPressedKeys - { - get { return newlyPressedKeys; } - } - - public IList HeldKeys - { - get { return heldKeys; } - } - - /* - * End XNACC variables - */ - #endregion -#endif - public const int MaxInputs = 4; - - public readonly KeyboardState[] CurrentKeyboardStates; - public readonly GamePadState[] CurrentGamePadStates; - - public readonly KeyboardState[] LastKeyboardStates; - public readonly GamePadState[] LastGamePadStates; - - public readonly bool[] GamePadWasConnected; - - /* - * Needed for virtual stick on WP7 - * -- Nathan Adams [adamsna@datanethost.net] - 4/12/2012 - */ - private GamePadState _currentVirtualState; - private GamePadState _lastVirtualState; - private bool _handleVirtualStick; - /* - * I didn't create an array for the virtual stick because there will only be one - * -- Nathan Adams [adamsna@datanethost.net] - 4/12/2012 - */ - - public bool MoveCursorWithController = false; - - /* - * Adding variables for the cursor - * -- Nathan Adams [adamsna@datanethost.net] - 4/15/2012 - * - */ - private MouseState _currentMouseState; - private MouseState _lastMouseState; - - private Vector2 _cursor; - private bool _cursorIsValid; - private bool _cursorIsVisible; - private bool _cursorMoved; - private Sprite _cursorSprite; - -#if WINDOWS_PHONE - private VirtualStick _phoneStick; - private VirtualButton _phoneA; - private VirtualButton _phoneB; -#endif - - public TouchCollection TouchState; - - public readonly List Gestures = new List(); - - private ScreenManager _manager; - private Viewport _viewport; - - - /// - /// Constructs a new input state. - /// - public InputState(ScreenManager manager) - { - _manager = manager; - CurrentKeyboardStates = new KeyboardState[MaxInputs]; - CurrentGamePadStates = new GamePadState[MaxInputs]; - - LastKeyboardStates = new KeyboardState[MaxInputs]; - LastGamePadStates = new GamePadState[MaxInputs]; - - GamePadWasConnected = new bool[MaxInputs]; - _currentVirtualState = new GamePadState(); - _lastVirtualState = new GamePadState(); - - _cursorIsVisible = false; - _cursorMoved = false; -#if WINDOWS_PHONE - _cursorIsValid = false; -#else - _cursorIsValid = true; -#endif - _cursor = Vector2.Zero; - - _handleVirtualStick = false; - } - - public MouseState MouseState - { - get { return _currentMouseState; } - } - - public GamePadState VirtualState - { - get { return _currentVirtualState; } - } - - public MouseState PreviousMouseState - { - get { return _lastMouseState; } - } - - public GamePadState PreviousVirtualState - { - get { return _lastVirtualState; } - } - - public bool ShowCursor - { - get { return _cursorIsVisible && _cursorIsValid; } - set { _cursorIsVisible = value; } - } - - public bool EnableVirtualStick - { - get { return _handleVirtualStick; } - set { _handleVirtualStick = value; } - } - - public Vector2 Cursor - { - get { return _cursor; } - } - - public bool IsCursorMoved - { - get { return _cursorMoved; } - } - - public bool IsCursorValid - { - get { return _cursorIsValid; } - } - - public void LoadContent() - { - ContentManager man = new ContentManager(_manager.Game.Services, "Content"); - _cursorSprite = new Sprite(man.Load("Common/cursor")); -#if WINDOWS_PHONE - // virtual stick content - _phoneStick = new VirtualStick(man.Load("Common/socket"), - man.Load("Common/stick"), new Vector2(80f, 400f)); - - Texture2D temp = man.Load("Common/buttons"); - _phoneA = new VirtualButton(temp, new Vector2(695f, 380f), new Rectangle(0, 0, 40, 40), new Rectangle(0, 40, 40, 40)); - _phoneB = new VirtualButton(temp, new Vector2(745f, 360f), new Rectangle(40, 0, 40, 40), new Rectangle(40, 40, 40, 40)); -#endif - _viewport = _manager.GraphicsDevice.Viewport; - } - - private GamePadState HandleVirtualStickWin() - { - Vector2 _leftStick = Vector2.Zero; - List _buttons = new List(); - PlayerIndex pout; - if (IsNewKeyPress(Keys.A, PlayerIndex.One, out pout)) - { - _leftStick.X -= 1f; - } - if (IsNewKeyPress(Keys.S, PlayerIndex.One, out pout)) - { - _leftStick.Y -= 1f; - } - if (IsNewKeyPress(Keys.D, PlayerIndex.One, out pout)) - { - _leftStick.X += 1f; - } - if (IsNewKeyPress(Keys.W, PlayerIndex.One, out pout)) - { - _leftStick.Y += 1f; - } - if (IsNewKeyPress(Keys.Space, PlayerIndex.One, out pout)) - { - _buttons.Add(Buttons.A); - } - if (IsNewKeyPress(Keys.LeftControl, PlayerIndex.One, out pout)) - { - _buttons.Add(Buttons.B); - } - if (_leftStick != Vector2.Zero) - { - _leftStick.Normalize(); - } - - return new GamePadState(_leftStick, Vector2.Zero, 0f, 0f, _buttons.ToArray()); - } - - private GamePadState HandleVirtualStickWP7() - { - List _buttons = new List(); - Vector2 _stick = Vector2.Zero; -#if WINDOWS_PHONE - _phoneA.Pressed = false; - _phoneB.Pressed = false; - TouchCollection touchLocations = TouchPanel.GetState(); - foreach (TouchLocation touchLocation in touchLocations) - { - _phoneA.Update(touchLocation); - _phoneB.Update(touchLocation); - _phoneStick.Update(touchLocation); - } - if (_phoneA.Pressed) - { - _buttons.Add(Buttons.A); - } - if (_phoneB.Pressed) - { - _buttons.Add(Buttons.B); - } - _stick = _phoneStick.StickPosition; -#endif - return new GamePadState(_stick, Vector2.Zero, 0f, 0f, _buttons.ToArray()); - } - - public void Draw() - { - if (_cursorIsVisible && _cursorIsValid) - { - _manager.SpriteBatch.Begin(); - _manager.SpriteBatch.Draw(_cursorSprite.Texture, _cursor, null, Color.White, 0f, _cursorSprite.Origin, 1f, SpriteEffects.None, 0f); - _manager.SpriteBatch.End(); - } -#if WINDOWS_PHONE - if (_handleVirtualStick) - { - _manager.SpriteBatch.Begin(); - _phoneA.Draw(_manager.SpriteBatch); - _phoneB.Draw(_manager.SpriteBatch); - _phoneStick.Draw(_manager.SpriteBatch); - _manager.SpriteBatch.End(); - } -#endif - } - - /// - /// Reads the latest state user input. - /// - public void Update(GameTime gameTime) - { - -#if WINDOWS - #region XNACC - KeyState = Keyboard.GetState(); - - oldPressedKeys = newPressedKeys; - newPressedKeys = KeyState.GetPressedKeys(); - - newlyPressedKeys.Clear(); - heldKeys.Clear(); - - foreach (Keys key in newPressedKeys) - { - if (oldPressedKeys.Contains(key)) - heldKeys.Add(key); - else - newlyPressedKeys.Add(key); - } - #endregion -#endif - - //PlayerIndex p; - _lastMouseState = _currentMouseState; - if (_handleVirtualStick) - { - _lastVirtualState = _currentVirtualState; - } - - _currentMouseState = Mouse.GetState(); - - if (_handleVirtualStick) - { -#if XBOX - _currentVirtualState= GamePad.GetState(PlayerIndex.One); -#elif WINDOWS - if (GamePad.GetState(PlayerIndex.One).IsConnected) - { - _currentVirtualState = GamePad.GetState(PlayerIndex.One); - } - else - { - _currentVirtualState = HandleVirtualStickWin(); - } -#elif WINDOWS_PHONE - _currentVirtualState = HandleVirtualStickWP7(); -#endif - } - for (int i = 0; i < MaxInputs; i++) - { - LastKeyboardStates[i] = CurrentKeyboardStates[i]; - LastGamePadStates[i] = CurrentGamePadStates[i]; - - CurrentKeyboardStates[i] = Keyboard.GetState((PlayerIndex)i); - CurrentGamePadStates[i] = GamePad.GetState((PlayerIndex)i); - - // Keep track of whether a gamepad has ever been - // connected, so we can detect if it is unplugged. - if (CurrentGamePadStates[i].IsConnected) - { - GamePadWasConnected[i] = true; - } - } - - // Get the raw touch state from the TouchPanel - TouchState = TouchPanel.GetState(); - - // Read in any detected gestures into our list for the screens to later process - Gestures.Clear(); - while (TouchPanel.IsGestureAvailable) - { - //System.Diagnostics.Debugger.Break(); - Gestures.Add(TouchPanel.ReadGesture()); - } - //System.Diagnostics.Debugger.Break(); - - // Update cursor - Vector2 oldCursor = _cursor; - - if (CurrentGamePadStates[0].IsConnected && CurrentGamePadStates[0].ThumbSticks.Left != Vector2.Zero && MoveCursorWithController) - { - Vector2 temp = CurrentGamePadStates[0].ThumbSticks.Left; - _cursor += temp * new Vector2(300f, -300f) * (float)gameTime.ElapsedGameTime.TotalSeconds; - Mouse.SetPosition((int)_cursor.X, (int)_cursor.Y); - } - else - { - _cursor.X = _currentMouseState.X; - _cursor.Y = _currentMouseState.Y; - } - - //if (this.IsNewKeyPress(Keys.P, PlayerIndex.One, out p)) - // Console.WriteLine(_cursor.ToString()); - - _cursor.X = MathHelper.Clamp(_cursor.X, 0f, _viewport.Width); - _cursor.Y = MathHelper.Clamp(_cursor.Y, 0f, _viewport.Height); - - //if (this.IsNewKeyPress(Keys.P, PlayerIndex.One, out p)) - // Console.WriteLine(_cursor.ToString()); - - if (_cursorIsValid && oldCursor != _cursor) - { - _cursorMoved = true; - } - else - { - _cursorMoved = false; - } - -#if WINDOWS - if (_viewport.Bounds.Contains(_currentMouseState.X, _currentMouseState.Y)) - { - _cursorIsValid = true; - } - else - { - _cursorIsValid = false; - } -#elif WINDOWS_PHONE - if (_currentMouseState.LeftButton == ButtonState.Pressed) - { - _cursorIsValid = true; - } - else - { - _cursorIsValid = false; - } -#endif - - //if (this.IsNewKeyPress(Keys.P, PlayerIndex.One, out p)) - // Console.WriteLine(_viewport.ToString()); - } - - - /// - /// Helper for checking if a key was pressed during this update. The - /// controllingPlayer parameter specifies which player to read input for. - /// If this is null, it will accept input from any player. When a keypress - /// is detected, the output playerIndex reports which player pressed it. - /// - public bool IsKeyPressed(Keys key, PlayerIndex? controllingPlayer, out PlayerIndex playerIndex) - { - if (controllingPlayer.HasValue) - { - // Read input from the specified player. - playerIndex = controllingPlayer.Value; - - int i = (int)playerIndex; - - return CurrentKeyboardStates[i].IsKeyDown(key); - } - else - { - // Accept input from any player. - return (IsKeyPressed(key, PlayerIndex.One, out playerIndex) || - IsKeyPressed(key, PlayerIndex.Two, out playerIndex) || - IsKeyPressed(key, PlayerIndex.Three, out playerIndex) || - IsKeyPressed(key, PlayerIndex.Four, out playerIndex)); - } - } - - /// - /// Helper for checking if a button was pressed during this update. - /// The controllingPlayer parameter specifies which player to read input for. - /// If this is null, it will accept input from any player. When a button press - /// is detected, the output playerIndex reports which player pressed it. - /// - public bool IsButtonPressed(Buttons button, PlayerIndex? controllingPlayer, out PlayerIndex playerIndex) - { - if (controllingPlayer.HasValue) - { - // Read input from the specified player. - playerIndex = controllingPlayer.Value; - - int i = (int)playerIndex; - - return CurrentGamePadStates[i].IsButtonDown(button); - } - else - { - // Accept input from any player. - return (IsButtonPressed(button, PlayerIndex.One, out playerIndex) || - IsButtonPressed(button, PlayerIndex.Two, out playerIndex) || - IsButtonPressed(button, PlayerIndex.Three, out playerIndex) || - IsButtonPressed(button, PlayerIndex.Four, out playerIndex)); - } - } - - /// - /// Helper for checking if a key was newly pressed during this update. The - /// controllingPlayer parameter specifies which player to read input for. - /// If this is null, it will accept input from any player. When a keypress - /// is detected, the output playerIndex reports which player pressed it. - /// - public bool IsNewKeyPress(Keys key, PlayerIndex? controllingPlayer, out PlayerIndex playerIndex) - { - if (controllingPlayer.HasValue) - { - // Read input from the specified player. - playerIndex = controllingPlayer.Value; - - int i = (int)playerIndex; - - return (CurrentKeyboardStates[i].IsKeyDown(key) && - LastKeyboardStates[i].IsKeyUp(key)); - } - else - { - // Accept input from any player. - return (IsNewKeyPress(key, PlayerIndex.One, out playerIndex) || - IsNewKeyPress(key, PlayerIndex.Two, out playerIndex) || - IsNewKeyPress(key, PlayerIndex.Three, out playerIndex) || - IsNewKeyPress(key, PlayerIndex.Four, out playerIndex)); - } - } - - public bool IsNewKeyRelease(Keys key, PlayerIndex? controllingPlayer, out PlayerIndex playerIndex) - { - if (controllingPlayer.HasValue) - { - // Read input from the specified player. - playerIndex = controllingPlayer.Value; - - int i = (int)playerIndex; - - return (CurrentKeyboardStates[i].IsKeyUp(key) && - LastKeyboardStates[i].IsKeyDown(key)); - } - else - { - // Accept input from any player. - return (IsNewKeyRelease(key, PlayerIndex.One, out playerIndex) || - IsNewKeyRelease(key, PlayerIndex.Two, out playerIndex) || - IsNewKeyRelease(key, PlayerIndex.Three, out playerIndex) || - IsNewKeyRelease(key, PlayerIndex.Four, out playerIndex)); - } - } - - /// - /// Helper for checking if a button was newly pressed during this update. - /// The controllingPlayer parameter specifies which player to read input for. - /// If this is null, it will accept input from any player. When a button press - /// is detected, the output playerIndex reports which player pressed it. - /// - public bool IsNewButtonPress(Buttons button, PlayerIndex? controllingPlayer, out PlayerIndex playerIndex) - { - if (controllingPlayer.HasValue) - { - // Read input from the specified player. - playerIndex = controllingPlayer.Value; - - int i = (int)playerIndex; - - return (CurrentGamePadStates[i].IsButtonDown(button) && - LastGamePadStates[i].IsButtonUp(button)); - } - else - { - // Accept input from any player. - return (IsNewButtonPress(button, PlayerIndex.One, out playerIndex) || - IsNewButtonPress(button, PlayerIndex.Two, out playerIndex) || - IsNewButtonPress(button, PlayerIndex.Three, out playerIndex) || - IsNewButtonPress(button, PlayerIndex.Four, out playerIndex)); - } - } - - public bool IsNewButtonRelease(Buttons button, PlayerIndex? controllingPlayer, out PlayerIndex playerIndex) - { - if (controllingPlayer.HasValue) - { - // Read input from the specified player. - playerIndex = controllingPlayer.Value; - - int i = (int)playerIndex; - - return (CurrentGamePadStates[i].IsButtonUp(button) && - LastGamePadStates[i].IsButtonDown(button)); - } - else - { - // Accept input from any player. - return (IsNewButtonRelease(button, PlayerIndex.One, out playerIndex) || - IsNewButtonRelease(button, PlayerIndex.Two, out playerIndex) || - IsNewButtonRelease(button, PlayerIndex.Three, out playerIndex) || - IsNewButtonRelease(button, PlayerIndex.Four, out playerIndex)); - } - } - - public bool IsNewVirtualButtonPress(Buttons button) - { - return (_lastVirtualState.IsButtonUp(button) && - _currentVirtualState.IsButtonDown(button)); - } - - public bool IsNewVirtualButtonRelease(Buttons button) - { - return (_lastVirtualState.IsButtonDown(button) && - _currentVirtualState.IsButtonUp(button)); - } - - /// - /// Helper for checking if a mouse button was newly pressed during this update. - /// - public bool IsNewMouseButtonPress(MouseButtons button) - { - switch (button) - { - case MouseButtons.LeftButton: - return (_currentMouseState.LeftButton == ButtonState.Pressed && - _lastMouseState.LeftButton == ButtonState.Released); - case MouseButtons.RightButton: - return (_currentMouseState.RightButton == ButtonState.Pressed && - _lastMouseState.RightButton == ButtonState.Released); - case MouseButtons.MiddleButton: - return (_currentMouseState.MiddleButton == ButtonState.Pressed && - _lastMouseState.MiddleButton == ButtonState.Released); - case MouseButtons.ExtraButton1: - return (_currentMouseState.XButton1 == ButtonState.Pressed && - _lastMouseState.XButton1 == ButtonState.Released); - case MouseButtons.ExtraButton2: - return (_currentMouseState.XButton2 == ButtonState.Pressed && - _lastMouseState.XButton2 == ButtonState.Released); - default: - return false; - } - } - - /// - /// Checks if the requested mouse button is released. - /// - /// The button. - public bool IsNewMouseButtonRelease(MouseButtons button) - { - switch (button) - { - case MouseButtons.LeftButton: - return (_lastMouseState.LeftButton == ButtonState.Pressed && - _currentMouseState.LeftButton == ButtonState.Released); - case MouseButtons.RightButton: - return (_lastMouseState.RightButton == ButtonState.Pressed && - _currentMouseState.RightButton == ButtonState.Released); - case MouseButtons.MiddleButton: - return (_lastMouseState.MiddleButton == ButtonState.Pressed && - _currentMouseState.MiddleButton == ButtonState.Released); - case MouseButtons.ExtraButton1: - return (_lastMouseState.XButton1 == ButtonState.Pressed && - _currentMouseState.XButton1 == ButtonState.Released); - case MouseButtons.ExtraButton2: - return (_lastMouseState.XButton2 == ButtonState.Pressed && - _currentMouseState.XButton2 == ButtonState.Released); - default: - return false; - } - } - } -} diff --git a/ScreenSystem/LogoScreen.cs b/ScreenSystem/LogoScreen.cs index 7b545ac..c383ee5 100644 --- a/ScreenSystem/LogoScreen.cs +++ b/ScreenSystem/LogoScreen.cs @@ -59,7 +59,7 @@ namespace GameStateManagement _content.Unload(); } - public override void HandleInput(GameTime gameTime, InputState input) + /*public override void HandleInput(GameTime gameTime, InputState input) { //input. if (input.CurrentKeyboardStates[0].GetPressedKeys().Length > 0 || @@ -68,6 +68,11 @@ namespace GameStateManagement { _duration = TimeSpan.Zero; } + }*/ + + public override void HandleInput(GameTime gameTime, InputState input) + { + base.HandleInput(gameTime, input); } public override void Update(GameTime gameTime, bool otherScreenHasFocus, diff --git a/ScreenSystem/MenuScreen.cs b/ScreenSystem/MenuScreen.cs deleted file mode 100644 index f8442ff..0000000 --- a/ScreenSystem/MenuScreen.cs +++ /dev/null @@ -1,329 +0,0 @@ -#region File Description -//----------------------------------------------------------------------------- -// MenuScreen.cs -// -// XNA Community Game Platform -// Copyright (C) Microsoft Corporation. All rights reserved. -//----------------------------------------------------------------------------- -#endregion - -#region Using Statements -using System; -using System.Collections.Generic; -using Microsoft.Xna.Framework; -using Microsoft.Xna.Framework.Graphics; -using Microsoft.Xna.Framework.Input.Touch; -using Microsoft.Xna.Framework.Input; -using FarseerPhysics.SamplesFramework; -#endregion - -namespace GameStateManagement -{ - /// - /// Base class for screens that contain a menu of options. The user can - /// move up and down to select an entry, or cancel to back out of the screen. - /// - public class MenuScreen : GameScreen - { - #region Fields - - // the number of pixels to pad above and below menu entries for touch input - const int menuEntryPadding = 10; - - private List menuEntries = new List(); - int selectedEntry = 0; - string menuTitle; - - InputAction menuUp; - InputAction menuDown; - InputAction menuSelect; - InputAction menuCancel; - - #endregion - - #region Properties - - - /// - /// Gets the list of menu entries, so derived classes can add - /// or change the menu contents. - /// - protected IList MenuEntries - { - get { return menuEntries; } - } - - - #endregion - - #region Initialization - - - /// - /// Constructor. - /// - public MenuScreen(string menuTitle) - { - this.menuTitle = menuTitle; - // menus generally only need Tap for menu selection - EnabledGestures = GestureType.Tap; - - TransitionOnTime = TimeSpan.FromSeconds(0.5); - TransitionOffTime = TimeSpan.FromSeconds(0.5); - - menuUp = new InputAction( - new Buttons[] { Buttons.DPadUp, Buttons.LeftThumbstickUp }, - new Keys[] { Keys.Up }, - true); - menuDown = new InputAction( - new Buttons[] { Buttons.DPadDown, Buttons.LeftThumbstickDown }, - new Keys[] { Keys.Down }, - true); - menuSelect = new InputAction( - new Buttons[] { Buttons.A, Buttons.Start }, - new Keys[] { Keys.Enter, Keys.Space }, - true); - menuCancel = new InputAction( - new Buttons[] { Buttons.B, Buttons.Back }, - new Keys[] { Keys.Escape }, - true); - } - - - #endregion - - public void AddMenuItem(string name) - { - - menuEntries.Add(new MenuEntry(name)); - } - - #region Handle Input - - /// - /// Allows the screen to create the hit bounds for a particular menu entry. - /// - protected virtual Rectangle GetMenuEntryHitBounds(MenuEntry entry) - { - // the hit bounds are the entire width of the screen, and the height of the entry - // with some additional padding above and below. - return new Rectangle( - 0, - (int)entry.Position.Y - menuEntryPadding, - ScreenManager.GraphicsDevice.Viewport.Width, - entry.GetHeight(this) + (menuEntryPadding * 2)); - } - - /// - /// Responds to user input, changing the selected entry and accepting - /// or cancelling the menu. - /// - public override void HandleInput(GameTime gameTime, InputState input) - { - // For input tests we pass in our ControllingPlayer, which may - // either be null (to accept input from any player) or a specific index. - // If we pass a null controlling player, the InputState helper returns to - // us which player actually provided the input. We pass that through to - // OnSelectEntry and OnCancel, so they can tell which player triggered them. - - -#if WINDOWS || XBOX360 - PlayerIndex playerIndex; - // Move to the previous menu entry? - if (menuUp.Evaluate(input, ControllingPlayer, out playerIndex)) - { - selectedEntry--; - - if (selectedEntry < 0) - selectedEntry = menuEntries.Count - 1; - } - - // Move to the next menu entry? - if (menuDown.Evaluate(input, ControllingPlayer, out playerIndex)) - { - selectedEntry++; - - if (selectedEntry >= menuEntries.Count) - selectedEntry = 0; - } - - if (menuSelect.Evaluate(input, ControllingPlayer, out playerIndex)) - { - OnSelectEntry(selectedEntry, playerIndex); - } - else if (menuCancel.Evaluate(input, ControllingPlayer, out playerIndex)) - { - OnCancel(playerIndex); - } -#endif - -#if WINDOWS_PHONE - //selectedEntry = 1; - - PlayerIndex player; - if (input.IsNewButtonPress(Buttons.Back, ControllingPlayer, out player)) - { - OnCancel(player); - } - - // look for any taps that occurred and select any entries that were tapped - foreach (GestureSample gesture in input.Gestures) - { - //System.Diagnostics.Debugger.Break(); - if (gesture.GestureType == GestureType.Tap) - { - // convert the position to a Point that we can test against a Rectangle - Point tapLocation = new Point((int)gesture.Position.X, (int)gesture.Position.Y); - - // iterate the entries to see if any were tapped - for (int i = 0; i < menuEntries.Count; i++) - { - MenuEntry menuEntry = menuEntries[i]; - - if (GetMenuEntryHitBounds(menuEntry).Contains(tapLocation)) - { - // select the entry. since gestures are only available on Windows Phone, - // we can safely pass PlayerIndex.One to all entries since there is only - // one player on Windows Phone. - OnSelectEntry(i, PlayerIndex.One); - } - } - } - } -#endif - } - - - /// - /// Handler for when the user has chosen a menu entry. - /// - protected virtual void OnSelectEntry(int entryIndex, PlayerIndex playerIndex) - { - menuEntries[entryIndex].OnSelectEntry(playerIndex); - } - - - /// - /// Handler for when the user has cancelled the menu. - /// - protected virtual void OnCancel(PlayerIndex playerIndex) - { - ExitScreen(); - } - - - /// - /// Helper overload makes it easy to use OnCancel as a MenuEntry event handler. - /// - protected void OnCancel(object sender, PlayerIndexEventArgs e) - { - OnCancel(e.PlayerIndex); - } - - - #endregion - - #region Update and Draw - - - /// - /// Allows the screen the chance to position the menu entries. By default - /// all menu entries are lined up in a vertical list, centered on the screen. - /// - protected virtual void UpdateMenuEntryLocations() - { - // Make the menu slide into place during transitions, using a - // power curve to make things look more interesting (this makes - // the movement slow down as it nears the end). - float transitionOffset = (float)Math.Pow(TransitionPosition, 2); - - // start at Y = 175; each X value is generated per entry - Vector2 position = new Vector2(0f, 175f); - - // update each menu entry's location in turn - for (int i = 0; i < menuEntries.Count; i++) - { - MenuEntry menuEntry = menuEntries[i]; - - // each entry is to be centered horizontally - position.X = ScreenManager.GraphicsDevice.Viewport.Width / 2 - menuEntry.GetWidth(this) / 2; - - if (ScreenState == ScreenState.TransitionOn) - position.X -= transitionOffset * 256; - else - position.X += transitionOffset * 512; - - // set the entry's position - menuEntry.Position = position; - - // move down for the next entry the size of this entry - position.Y += menuEntry.GetHeight(this); - } - } - - - /// - /// Updates the menu. - /// - public override void Update(GameTime gameTime, bool otherScreenHasFocus, - bool coveredByOtherScreen) - { - base.Update(gameTime, otherScreenHasFocus, coveredByOtherScreen); - - // Update each nested MenuEntry object. - for (int i = 0; i < menuEntries.Count; i++) - { - bool isSelected = IsActive && (i == selectedEntry); - - menuEntries[i].Update(this, isSelected, gameTime); - } - } - - - /// - /// Draws the menu. - /// - public override void Draw(GameTime gameTime) - { - // make sure our entries are in the right place before we draw them - UpdateMenuEntryLocations(); - - GraphicsDevice graphics = ScreenManager.GraphicsDevice; - SpriteBatch spriteBatch = ScreenManager.SpriteBatch; - SpriteFont font = ScreenManager.Font; - - spriteBatch.Begin(); - - // Draw each menu entry in turn. - for (int i = 0; i < menuEntries.Count; i++) - { - MenuEntry menuEntry = menuEntries[i]; - - bool isSelected = IsActive && (i == selectedEntry); - - menuEntry.Draw(this, isSelected, gameTime); - } - - // Make the menu slide into place during transitions, using a - // power curve to make things look more interesting (this makes - // the movement slow down as it nears the end). - float transitionOffset = (float)Math.Pow(TransitionPosition, 2); - - // Draw the menu title centered on the screen - Vector2 titlePosition = new Vector2(graphics.Viewport.Width / 2, 80); - Vector2 titleOrigin = font.MeasureString(menuTitle) / 2; - Color titleColor = new Color(192, 192, 192) * TransitionAlpha; - float titleScale = 1.25f; - - titlePosition.Y -= transitionOffset * 100; - - spriteBatch.DrawString(font, menuTitle, titlePosition, titleColor, 0, - titleOrigin, titleScale, SpriteEffects.None, 0); - - spriteBatch.End(); - } - - - #endregion - } -} diff --git a/ScreenSystem/PhysicsGameScreen.cs b/ScreenSystem/PhysicsGameScreen.cs index 7dcb9a3..ec961d9 100644 --- a/ScreenSystem/PhysicsGameScreen.cs +++ b/ScreenSystem/PhysicsGameScreen.cs @@ -7,7 +7,6 @@ using Microsoft.Xna.Framework; using Microsoft.Xna.Framework.Content; using Microsoft.Xna.Framework.Input; using FarseerPhysics.SamplesFramework; -using Axios.Engine; namespace GameStateManagement { @@ -68,20 +67,6 @@ namespace GameStateManagement World.Clear(); } - if (DebugView == null) - { - if (!Axios.Settings.ScreenSaver) - { - ContentManager man = new ContentManager(this.ScreenManager.Game.Services, "Content"); - DebugView = new DebugViewXNA(World); - DebugView.RemoveFlags(DebugViewFlags.Shape); - DebugView.RemoveFlags(DebugViewFlags.Joint); - DebugView.DefaultShapeColor = Color.White; - DebugView.SleepingShapeColor = Color.LightGray; - DebugView.LoadContent(ScreenManager.GraphicsDevice, man); - } - } - if (Camera == null) { Camera = new Camera2D(ScreenManager.GraphicsDevice); @@ -211,7 +196,7 @@ namespace GameStateManagement _fixedMouseJoint == null) { Fixture savedFixture = World.TestPoint(position); - if (savedFixture != null && savedFixture.UserData is SimpleAxiosGameObject && ((SimpleAxiosGameObject)(savedFixture.UserData)).AllowAutomaticMouseJoint) + if (savedFixture != null) { Body body = savedFixture.Body; _fixedMouseJoint = new FixedMouseJoint(body, position); @@ -340,8 +325,6 @@ namespace GameStateManagement Matrix projection = Camera.SimProjection; Matrix view = Camera.SimView; - if (!Axios.Settings.ScreenSaver) - DebugView.RenderDebugData(ref projection, ref view); base.Draw(gameTime); } } diff --git a/ScreenSystem/ScreenManager.cs b/ScreenSystem/ScreenManager.cs deleted file mode 100644 index 85927ea..0000000 --- a/ScreenSystem/ScreenManager.cs +++ /dev/null @@ -1,504 +0,0 @@ -#region File Description -//----------------------------------------------------------------------------- -// ScreenManager.cs -// -// Microsoft XNA Community Game Platform -// Copyright (C) Microsoft Corporation. All rights reserved. -//----------------------------------------------------------------------------- -#endregion - -#region Using Statements -using System; -using System.Diagnostics; -using System.Collections.Generic; -using Microsoft.Xna.Framework; -using Microsoft.Xna.Framework.Content; -using Microsoft.Xna.Framework.Graphics; -using Microsoft.Xna.Framework.Input.Touch; -using System.IO; -using System.IO.IsolatedStorage; -using System.Xml.Linq; -using FarseerPhysics.SamplesFramework; -using Axios.Engine; -#endregion - -namespace GameStateManagement -{ - /// - /// The screen manager is a component which manages one or more GameScreen - /// instances. It maintains a stack of screens, calls their Update and Draw - /// methods at the appropriate times, and automatically routes input to the - /// topmost active screen. - /// - public class ScreenManager : DrawableGameComponent - { - #region Fields - - private const string StateFilename = "ScreenManagerState.xml"; - - List screens = new List(); - List tempScreensList = new List(); - - InputState input; - - SpriteBatch spriteBatch; - SpriteFont font; - Texture2D blankTexture; - - bool isInitialized; - - bool traceEnabled; - - /// - /// Contains all the fonts avaliable for use. - /// - private SpriteFonts _spriteFonts; - - #endregion - - #region Properties - - public InputState InputState - { - get { return input; } - private set { input = value; } - } - - public SpriteFonts Fonts - { - get { return _spriteFonts; } - } - - /// - /// A default SpriteBatch shared by all the screens. This saves - /// each screen having to bother creating their own local instance. - /// - public SpriteBatch SpriteBatch - { - get { return spriteBatch; } - } - - - /// - /// A default font shared by all the screens. This saves - /// each screen having to bother loading their own local copy. - /// - public SpriteFont Font - { - get { return font; } - } - - - /// - /// If true, the manager prints out a list of all the screens - /// each time it is updated. This can be useful for making sure - /// everything is being added and removed at the right times. - /// - public bool TraceEnabled - { - get { return traceEnabled; } - set { traceEnabled = value; } - } - - - /// - /// Gets a blank texture that can be used by the screens. - /// - public Texture2D BlankTexture - { - get { return blankTexture; } - } - - - #endregion - - #region Initialization - - - /// - /// Constructs a new screen manager component. - /// - public ScreenManager(Game game) - : base(game) - { - // we must set EnabledGestures before we can query for them, but - // we don't assume the game wants to read them. - TouchPanel.EnabledGestures = GestureType.None; - this.input = new InputState(this); - } - - - /// - /// Initializes the screen manager component. - /// - public override void Initialize() - { - base.Initialize(); - - isInitialized = true; - } - - - /// - /// Load your graphics content. - /// - protected override void LoadContent() - { - // Load content belonging to the screen manager. - ContentManager content = new ContentManager(this.Game.Services, "Content/Fonts"); - _spriteFonts = new SpriteFonts(content); - spriteBatch = new SpriteBatch(GraphicsDevice); - font = content.Load("menufont"); - blankTexture = Game.Content.Load("Materials/blank"); - GameServices.AddService(this.Game.GraphicsDevice); - GameServices.AddService(this.Game.Content); - - // It is advised to use one instance of Random - // because Random is seeded with the current time - // initilizing random objects too quickly can cause - // the impression of generating the same value - // http://stackoverflow.com/questions/2727538/random-encounter-not-so-random - GameServices.AddService(new Random()); - AxiosRandom.init(); - - - input.LoadContent(); - // Tell each of the screens to load their content. - foreach (GameScreen screen in screens) - { - screen.Activate(false); - } - } - - - /// - /// Unload your graphics content. - /// - protected override void UnloadContent() - { - // Tell each of the screens to unload their content. - foreach (GameScreen screen in screens) - { - screen.Unload(); - } - } - - - #endregion - - #region Update and Draw - - - /// - /// Allows each screen to run logic. - /// - public override void Update(GameTime gameTime) - { - // Read the keyboard and gamepad. - input.Update(gameTime); - - // Make a copy of the master screen list, to avoid confusion if - // the process of updating one screen adds or removes others. - tempScreensList.Clear(); - - foreach (GameScreen screen in screens) - tempScreensList.Add(screen); - - bool otherScreenHasFocus = !Game.IsActive; - bool coveredByOtherScreen = false; - - // Loop as long as there are screens waiting to be updated. - while (tempScreensList.Count > 0) - { - // Pop the topmost screen off the waiting list. - GameScreen screen = tempScreensList[tempScreensList.Count - 1]; - - tempScreensList.RemoveAt(tempScreensList.Count - 1); - - // Update the screen. - screen.Update(gameTime, otherScreenHasFocus, coveredByOtherScreen); - - if (screen.ScreenState == ScreenState.TransitionOn || - screen.ScreenState == ScreenState.Active) - { - // If this is the first active screen we came across, - // give it a chance to handle input. - if (!otherScreenHasFocus) - { - // The default implementation of screens aren't aware that it's - // being woke up - input.ShowCursor = screen.HasCursor; - input.EnableVirtualStick = screen.HasVirtualStick; - screen.HandleInput(gameTime, input); - - otherScreenHasFocus = true; - } - - // If this is an active non-popup, inform any subsequent - // screens that they are covered by it. - if (!screen.IsPopup) - coveredByOtherScreen = true; - } - } - - // Print debug trace? - if (traceEnabled) - TraceScreens(); - } - - - /// - /// Prints a list of all the screens, for debugging. - /// - void TraceScreens() - { - List screenNames = new List(); - - foreach (GameScreen screen in screens) - screenNames.Add(screen.GetType().Name); - - Debug.WriteLine(string.Join(", ", screenNames.ToArray())); - } - - - /// - /// Tells each screen to draw itself. - /// - public override void Draw(GameTime gameTime) - { - - foreach (GameScreen screen in screens) - { - if (screen.ScreenState == ScreenState.Hidden) - continue; - - screen.Draw(gameTime); - } - input.Draw(); - } - - - #endregion - - #region Public Methods - - - /// - /// Adds a new screen to the screen manager. - /// - public void AddScreen(GameScreen screen, PlayerIndex? controllingPlayer) - { - screen.ControllingPlayer = controllingPlayer; - screen.ScreenManager = this; - screen.IsExiting = false; - - // If we have a graphics device, tell the screen to load content. - if (isInitialized) - { - screen.Activate(false); - } - - screens.Add(screen); - - // update the TouchPanel to respond to gestures this screen is interested in - TouchPanel.EnabledGestures = screen.EnabledGestures; - } - - /// - /// Adds a new screen to the screen manager with a default PlayerIndex of one - /// - public void AddScreen(GameScreen screen) - { - screen.ControllingPlayer = PlayerIndex.One; - screen.ScreenManager = this; - screen.IsExiting = false; - - // If we have a graphics device, tell the screen to load content. - if (isInitialized) - { - screen.Activate(false); - } - - screens.Add(screen); - - // update the TouchPanel to respond to gestures this screen is interested in - TouchPanel.EnabledGestures = screen.EnabledGestures; - } - - - /// - /// Removes a screen from the screen manager. You should normally - /// use GameScreen.ExitScreen instead of calling this directly, so - /// the screen can gradually transition off rather than just being - /// instantly removed. - /// - public void RemoveScreen(GameScreen screen) - { - // If we have a graphics device, tell the screen to unload content. - if (isInitialized) - { - screen.Unload(); - } - - screens.Remove(screen); - tempScreensList.Remove(screen); - - // if there is a screen still in the manager, update TouchPanel - // to respond to gestures that screen is interested in. - if (screens.Count > 0) - { - TouchPanel.EnabledGestures = screens[screens.Count - 1].EnabledGestures; - } - } - - - /// - /// Expose an array holding all the screens. We return a copy rather - /// than the real master list, because screens should only ever be added - /// or removed using the AddScreen and RemoveScreen methods. - /// - public GameScreen[] GetScreens() - { - return screens.ToArray(); - } - - - /// - /// Helper draws a translucent black fullscreen sprite, used for fading - /// screens in and out, and for darkening the background behind popups. - /// - public void FadeBackBufferToBlack(float alpha) - { - spriteBatch.Begin(); - spriteBatch.Draw(blankTexture, GraphicsDevice.Viewport.Bounds, Color.Black * alpha); - spriteBatch.End(); - } - - /// - /// Informs the screen manager to serialize its state to disk. - /// - public void Deactivate() - { -#if !WINDOWS_PHONE - return; -#else - // Open up isolated storage - using (IsolatedStorageFile storage = IsolatedStorageFile.GetUserStoreForApplication()) - { - // Create an XML document to hold the list of screen types currently in the stack - XDocument doc = new XDocument(); - XElement root = new XElement("ScreenManager"); - doc.Add(root); - - // Make a copy of the master screen list, to avoid confusion if - // the process of deactivating one screen adds or removes others. - tempScreensList.Clear(); - foreach (GameScreen screen in screens) - tempScreensList.Add(screen); - - // Iterate the screens to store in our XML file and deactivate them - foreach (GameScreen screen in tempScreensList) - { - // Only add the screen to our XML if it is serializable - if (screen.IsSerializable) - { - // We store the screen's controlling player so we can rehydrate that value - string playerValue = screen.ControllingPlayer.HasValue - ? screen.ControllingPlayer.Value.ToString() - : ""; - - root.Add(new XElement( - "GameScreen", - new XAttribute("Type", screen.GetType().AssemblyQualifiedName), - new XAttribute("ControllingPlayer", playerValue))); - } - - // Deactivate the screen regardless of whether we serialized it - screen.Deactivate(); - } - - // Save the document - using (IsolatedStorageFileStream stream = storage.CreateFile(StateFilename)) - { - doc.Save(stream); - } - } -#endif - } - - public bool Activate(bool instancePreserved) - { -#if !WINDOWS_PHONE - return false; -#else - // If the game instance was preserved, the game wasn't dehydrated so our screens still exist. - // We just need to activate them and we're ready to go. - if (instancePreserved) - { - // Make a copy of the master screen list, to avoid confusion if - // the process of activating one screen adds or removes others. - tempScreensList.Clear(); - - foreach (GameScreen screen in screens) - tempScreensList.Add(screen); - - foreach (GameScreen screen in tempScreensList) - screen.Activate(true); - } - - // Otherwise we need to refer to our saved file and reconstruct the screens that were present - // when the game was deactivated. - else - { - // Try to get the screen factory from the services, which is required to recreate the screens - IScreenFactory screenFactory = Game.Services.GetService(typeof(IScreenFactory)) as IScreenFactory; - if (screenFactory == null) - { - throw new InvalidOperationException( - "Game.Services must contain an IScreenFactory in order to activate the ScreenManager."); - } - - // Open up isolated storage - using (IsolatedStorageFile storage = IsolatedStorageFile.GetUserStoreForApplication()) - { - // Check for the file; if it doesn't exist we can't restore state - if (!storage.FileExists(StateFilename)) - return false; - - // Read the state file so we can build up our screens - using (IsolatedStorageFileStream stream = storage.OpenFile(StateFilename, FileMode.Open)) - { - XDocument doc = XDocument.Load(stream); - - // Iterate the document to recreate the screen stack - foreach (XElement screenElem in doc.Root.Elements("GameScreen")) - { - // Use the factory to create the screen - Type screenType = Type.GetType(screenElem.Attribute("Type").Value); - GameScreen screen = screenFactory.CreateScreen(screenType); - - // Rehydrate the controlling player for the screen - PlayerIndex? controllingPlayer = screenElem.Attribute("ControllingPlayer").Value != "" - ? (PlayerIndex)Enum.Parse(typeof(PlayerIndex), screenElem.Attribute("ControllingPlayer").Value, true) - : (PlayerIndex?)null; - screen.ControllingPlayer = controllingPlayer; - - // Add the screen to the screens list and activate the screen - screen.ScreenManager = this; - screens.Add(screen); - screen.Activate(false); - - // update the TouchPanel to respond to gestures this screen is interested in - TouchPanel.EnabledGestures = screen.EnabledGestures; - } - } - } - } - - return true; -#endif - } - - #endregion - } -} diff --git a/ScreenSystem/ScreenManagerComponent.cs b/ScreenSystem/ScreenManagerComponent.cs deleted file mode 100644 index 15b1a86..0000000 --- a/ScreenSystem/ScreenManagerComponent.cs +++ /dev/null @@ -1,305 +0,0 @@ -using System.Collections.Generic; -using Microsoft.Xna.Framework; -using Microsoft.Xna.Framework.Content; -using Microsoft.Xna.Framework.Graphics; -using Microsoft.Xna.Framework.Input.Touch; -using FarseerPhysics.SamplesFramework; - -namespace GameStateManagement -{ - /* - /// - /// The screen manager is a component which manages one or more GameScreen - /// instances. It maintains a stack of screens, calls their Update and Draw - /// methods at the appropriate times, and automatically routes input to the - /// topmost active screen. - /// - public class ScreenManager : DrawableGameComponent - { - private AssetCreator _assetCreator; - private ContentManager _contentManager; - - private InputHelper _input; - private bool _isInitialized; - private LineBatch _lineBatch; - - private List _screens; - private List _screensToUpdate; - - private SpriteBatch _spriteBatch; - - /// - /// Contains all the fonts avaliable for use. - /// - private SpriteFonts _spriteFonts; - - private List _transitions; - - /// - /// Constructs a new screen manager component. - /// - public ScreenManager(Game game) - : base(game) - { - // we must set EnabledGestures before we can query for them, but - // we don't assume the game wants to read them. - //game.Components. - TouchPanel.EnabledGestures = GestureType.None; - _contentManager = game.Content; - _contentManager.RootDirectory = "Content"; - _input = new InputHelper(this); - - _screens = new List(); - _screensToUpdate = new List(); - _transitions = new List(); - } - - /// - /// A default SpriteBatch shared by all the screens. This saves - /// each screen having to bother creating their own local instance. - /// - public SpriteBatch SpriteBatch - { - get { return _spriteBatch; } - } - - public LineBatch LineBatch - { - get { return _lineBatch; } - } - - public ContentManager Content - { - get { return _contentManager; } - } - - public SpriteFonts Fonts - { - get { return _spriteFonts; } - } - - public AssetCreator Assets - { - get { return _assetCreator; } - } - - /// - /// Initializes the screen manager component. - /// - public override void Initialize() - { - if (!Axios.Settings.ScreenSaver) - _spriteFonts = new SpriteFonts(_contentManager); - base.Initialize(); - - _isInitialized = true; - } - - /// - /// Load your graphics content. - /// - protected override void LoadContent() - { - _spriteBatch = new SpriteBatch(GraphicsDevice); - _lineBatch = new LineBatch(GraphicsDevice); - - if (!Axios.Settings.ScreenSaver) - { - _assetCreator = new AssetCreator(GraphicsDevice); - _assetCreator.LoadContent(_contentManager); - - _input.LoadContent(); - } - // Tell each of the screens to load their content. - foreach (GameScreen screen in _screens) - { - screen.LoadContent(); - } - } - - /// - /// Unload your graphics content. - /// - protected override void UnloadContent() - { - // Tell each of the screens to unload their content. - foreach (GameScreen screen in _screens) - { - screen.UnloadContent(); - } - } - - /// - /// Allows each screen to run logic. - /// - public override void Update(GameTime gameTime) - { - // Read the keyboard and gamepad. - _input.Update(gameTime); - - // Make a copy of the master screen list, to avoid confusion if - // the process of updating one screen adds or removes others. - _screensToUpdate.Clear(); - - if (_screens.Count == 0 || (_screens.Count == 1 && _screens[0] is BackgroundScreen)) - //I'm done, exit - this.Game.Exit(); - - foreach (GameScreen screen in _screens) - { - _screensToUpdate.Add(screen); - } - - bool otherScreenHasFocus = !Game.IsActive; - bool coveredByOtherScreen = false; - - // Loop as long as there are screens waiting to be updated. - while (_screensToUpdate.Count > 0) - { - // Pop the topmost screen off the waiting list. - GameScreen screen = _screensToUpdate[_screensToUpdate.Count - 1]; - - _screensToUpdate.RemoveAt(_screensToUpdate.Count - 1); - - // Update the screen. - screen.Update(gameTime, otherScreenHasFocus, coveredByOtherScreen); - - if (screen.ScreenState == ScreenState.TransitionOn || - screen.ScreenState == ScreenState.Active) - { - // If this is the first active screen we came across, - // give it a chance to handle input. - if (!otherScreenHasFocus || screen.AlwaysHasFocus) - { - - if (!otherScreenHasFocus) - { - _input.ShowCursor = screen.HasCursor; - _input.EnableVirtualStick = screen.HasVirtualStick; - - otherScreenHasFocus = true; - } - screen.HandleInput(_input, gameTime); - } - - // If this is an active non-popup, inform any subsequent - // screens that they are covered by it. - if (!screen.IsPopup) - { - coveredByOtherScreen = true; - } - } - } - } - - /// - /// Tells each screen to draw itself. - /// - public override void Draw(GameTime gameTime) - { - int transitionCount = 0; - foreach (GameScreen screen in _screens) - { - if (screen.ScreenState == ScreenState.TransitionOn || - screen.ScreenState == ScreenState.TransitionOff) - { - ++transitionCount; - if (_transitions.Count < transitionCount) - { - PresentationParameters _pp = GraphicsDevice.PresentationParameters; - _transitions.Add(new RenderTarget2D(GraphicsDevice, _pp.BackBufferWidth, _pp.BackBufferHeight, - false, - SurfaceFormat.Color, _pp.DepthStencilFormat, - _pp.MultiSampleCount, - RenderTargetUsage.DiscardContents)); - } - GraphicsDevice.SetRenderTarget(_transitions[transitionCount - 1]); - GraphicsDevice.Clear(Color.Transparent); - screen.Draw(gameTime); - GraphicsDevice.SetRenderTarget(null); - } - } - - //GraphicsDevice.Clear(Color.Black); - - transitionCount = 0; - foreach (GameScreen screen in _screens) - { - if (screen.ScreenState == ScreenState.Hidden) - { - continue; - } - - if (screen.ScreenState == ScreenState.TransitionOn || - screen.ScreenState == ScreenState.TransitionOff) - { - _spriteBatch.Begin(0, BlendState.AlphaBlend); - _spriteBatch.Draw(_transitions[transitionCount], Vector2.Zero, Color.White * screen.TransitionAlpha); - _spriteBatch.End(); - - ++transitionCount; - } - else - { - screen.Draw(gameTime); - } - } - _input.Draw(); - } - - /// - /// Adds a new screen to the screen manager. - /// - public void AddScreen(GameScreen screen) - { - screen.ScreenManager = this; - screen.IsExiting = false; - - // If we have a graphics device, tell the screen to load content. - if (_isInitialized) - { - screen.LoadContent(); - } - - _screens.Add(screen); - - // update the TouchPanel to respond to gestures this screen is interested in - TouchPanel.EnabledGestures = screen.EnabledGestures; - } - - /// - /// Removes a screen from the screen manager. You should normally - /// use GameScreen.ExitScreen instead of calling this directly, so - /// the screen can gradually transition off rather than just being - /// instantly removed. - /// - public void RemoveScreen(GameScreen screen) - { - // If we have a graphics device, tell the screen to unload content. - if (_isInitialized) - { - screen.UnloadContent(); - } - - _screens.Remove(screen); - _screensToUpdate.Remove(screen); - - // if there is a screen still in the manager, update TouchPanel - // to respond to gestures that screen is interested in. - if (_screens.Count > 0) - { - TouchPanel.EnabledGestures = _screens[_screens.Count - 1].EnabledGestures; - } - } - - /// - /// Expose an array holding all the screens. We return a copy rather - /// than the real master list, because screens should only ever be added - /// or removed using the AddScreen and RemoveScreen methods. - /// - public GameScreen[] GetScreens() - { - return _screens.ToArray(); - } - }*/ -} \ No newline at end of file diff --git a/ScreenSystem/VirtualButton.cs b/ScreenSystem/VirtualButton.cs index c8365c4..5d51d30 100644 --- a/ScreenSystem/VirtualButton.cs +++ b/ScreenSystem/VirtualButton.cs @@ -1,7 +1,6 @@ using System; using Microsoft.Xna.Framework; using Microsoft.Xna.Framework.Graphics; -using Microsoft.Xna.Framework.Input.Touch; namespace FarseerPhysics.SamplesFramework { @@ -25,7 +24,7 @@ namespace FarseerPhysics.SamplesFramework _position = position; } - public void Update(TouchLocation touchLocation) + /*public void Update(TouchLocation touchLocation) { if (touchLocation.State == TouchLocationState.Pressed || touchLocation.State == TouchLocationState.Moved) @@ -36,7 +35,7 @@ namespace FarseerPhysics.SamplesFramework Pressed = true; } } - } + }*/ public void Draw(SpriteBatch batch) { diff --git a/ScreenSystem/VirtualStick.cs b/ScreenSystem/VirtualStick.cs index b50a359..c5165d6 100644 --- a/ScreenSystem/VirtualStick.cs +++ b/ScreenSystem/VirtualStick.cs @@ -1,7 +1,6 @@ using System; using Microsoft.Xna.Framework; using Microsoft.Xna.Framework.Graphics; -using Microsoft.Xna.Framework.Input.Touch; namespace FarseerPhysics.SamplesFramework { @@ -25,6 +24,8 @@ namespace FarseerPhysics.SamplesFramework StickPosition = Vector2.Zero; } + // FIXME + /* public void Update(TouchLocation touchLocation) { if (touchLocation.State == TouchLocationState.Pressed && _picked < 0) @@ -57,7 +58,7 @@ namespace FarseerPhysics.SamplesFramework _position = _center; StickPosition = Vector2.Zero; } - } + }*/ public void Draw(SpriteBatch batch) { diff --git a/Settings.cs b/Settings.cs new file mode 100644 index 0000000..6a31a96 --- /dev/null +++ b/Settings.cs @@ -0,0 +1,236 @@ +/* +* Farseer Physics Engine based on Box2D.XNA port: +* Copyright (c) 2010 Ian Qvist +* +* Box2D.XNA port of Box2D: +* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler +* +* Original source Box2D: +* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; + +namespace FarseerPhysics +{ + public static class Settings + { + + public static string Version = "Farseer Engine Version 3.3.1 (Patched)"; + + public const float MaxFloat = 3.402823466e+38f; + public const float Epsilon = 1.192092896e-07f; + public const float Pi = 3.14159265359f; + + // Common + + /// + /// Enabling diagnistics causes the engine to gather timing information. + /// You can see how much time it took to solve the contacts, solve CCD + /// and update the controllers. + /// NOTE: If you are using a debug view that shows performance counters, + /// you might want to enable this. + /// + public static bool EnableDiagnostics = true; + + /// + /// The number of velocity iterations used in the solver. + /// + public static int VelocityIterations = 8; + + /// + /// The number of position iterations used in the solver. + /// + public static int PositionIterations = 3; + + /// + /// Enable/Disable Continuous Collision Detection (CCD) + /// + public static bool ContinuousPhysics = true; + + /// + /// The number of velocity iterations in the TOI solver + /// + public static int TOIVelocityIterations = 8; + + /// + /// The number of position iterations in the TOI solver + /// + public static int TOIPositionIterations = 20; + + /// + /// Maximum number of sub-steps per contact in continuous physics simulation. + /// + public const int MaxSubSteps = 8; + + /// + /// Enable/Disable warmstarting + /// + public static bool EnableWarmstarting = true; + + /// + /// Enable/Disable sleeping + /// + public static bool AllowSleep = true; + + /// + /// The maximum number of vertices on a convex polygon. + /// + public static int MaxPolygonVertices = 8; + + /// + /// Farseer Physics Engine has a different way of filtering fixtures than Box2d. + /// We have both FPE and Box2D filtering in the engine. If you are upgrading + /// from earlier versions of FPE, set this to true. + /// + public static bool UseFPECollisionCategories; + + /// + /// Conserve memory makes sure that objects are used by reference instead of cloned. + /// When you give a vertices collection to a PolygonShape, it will by default copy the vertices + /// instead of using the original reference. This is to ensure that objects modified outside the engine + /// does not affect the engine itself, however, this uses extra memory. This behavior + /// can be turned off by setting ConserveMemory to true. + /// + public const bool ConserveMemory = false; + + /// + /// The maximum number of contact points between two convex shapes. + /// + public const int MaxManifoldPoints = 2; + + /// + /// This is used to fatten AABBs in the dynamic tree. This allows proxies + /// to move by a small amount without triggering a tree adjustment. + /// This is in meters. + /// + public const float AABBExtension = 0.1f; + + /// + /// This is used to fatten AABBs in the dynamic tree. This is used to predict + /// the future position based on the current displacement. + /// This is a dimensionless multiplier. + /// + public const float AABBMultiplier = 2.0f; + + /// + /// A small length used as a collision and constraint tolerance. Usually it is + /// chosen to be numerically significant, but visually insignificant. + /// + public const float LinearSlop = 0.005f; + + /// + /// A small angle used as a collision and constraint tolerance. Usually it is + /// chosen to be numerically significant, but visually insignificant. + /// + public const float AngularSlop = (2.0f / 180.0f * Pi); + + /// + /// The radius of the polygon/edge shape skin. This should not be modified. Making + /// this smaller means polygons will have an insufficient buffer for continuous collision. + /// Making it larger may create artifacts for vertex collision. + /// + public const float PolygonRadius = (2.0f * LinearSlop); + + // Dynamics + + /// + /// Maximum number of contacts to be handled to solve a TOI impact. + /// + public const int MaxTOIContacts = 32; + + /// + /// A velocity threshold for elastic collisions. Any collision with a relative linear + /// velocity below this threshold will be treated as inelastic. + /// + public const float VelocityThreshold = 1.0f; + + /// + /// The maximum linear position correction used when solving constraints. This helps to + /// prevent overshoot. + /// + public const float MaxLinearCorrection = 0.2f; + + /// + /// The maximum angular position correction used when solving constraints. This helps to + /// prevent overshoot. + /// + public const float MaxAngularCorrection = (8.0f / 180.0f * Pi); + + /// + /// This scale factor controls how fast overlap is resolved. Ideally this would be 1 so + /// that overlap is removed in one time step. However using values close to 1 often lead + /// to overshoot. + /// + public const float ContactBaumgarte = 0.2f; + + // Sleep + + /// + /// The time that a body must be still before it will go to sleep. + /// + public const float TimeToSleep = 0.5f; + + /// + /// A body cannot sleep if its linear velocity is above this tolerance. + /// + public const float LinearSleepTolerance = 0.01f; + + /// + /// A body cannot sleep if its angular velocity is above this tolerance. + /// + public const float AngularSleepTolerance = (2.0f / 180.0f * Pi); + + /// + /// The maximum linear velocity of a body. This limit is very large and is used + /// to prevent numerical problems. You shouldn't need to adjust this. + /// + public const float MaxTranslation = 2.0f; + + public const float MaxTranslationSquared = (MaxTranslation * MaxTranslation); + + /// + /// The maximum angular velocity of a body. This limit is very large and is used + /// to prevent numerical problems. You shouldn't need to adjust this. + /// + public const float MaxRotation = (0.5f * Pi); + + public const float MaxRotationSquared = (MaxRotation * MaxRotation); + + /// + /// Friction mixing law. Feel free to customize this. + /// + /// The friction1. + /// The friction2. + /// + public static float MixFriction(float friction1, float friction2) + { + return (float) Math.Sqrt(friction1 * friction2); + } + + /// + /// Restitution mixing law. Feel free to customize this. + /// + /// The restitution1. + /// The restitution2. + /// + public static float MixRestitution(float restitution1, float restitution2) + { + return restitution1 > restitution2 ? restitution1 : restitution2; + } + } +} \ No newline at end of file diff --git a/ae-physics.csproj b/ae-physics.csproj index fca03da..2073970 100644 --- a/ae-physics.csproj +++ b/ae-physics.csproj @@ -20,6 +20,7 @@ DEBUG;TRACE prompt 4 + x86 pdbonly @@ -108,29 +109,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - + + + + + {a1a96363-c163-4a2a-8f31-d84d80c4c0d7} + GameStateManagement + + + {35253ce1-c864-4cd3-8249-4d1319748e8f} + FNA +