diff --git a/.DS_Store b/.DS_Store index cbd99ad..693a132 100644 Binary files a/.DS_Store and b/.DS_Store differ diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..3bb1e00 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "python.pythonPath": "/Users/zhuyu/anaconda3/bin/python" +} \ No newline at end of file diff --git a/examples/.DS_Store b/examples/.DS_Store index ea508f3..d7e1452 100644 Binary files a/examples/.DS_Store and b/examples/.DS_Store differ diff --git a/examples/HelloWorld/.DS_Store b/examples/HelloWorld/.DS_Store index 5008ddf..cedc6f9 100644 Binary files a/examples/HelloWorld/.DS_Store and b/examples/HelloWorld/.DS_Store differ diff --git a/examples/HelloWorld/Background.png b/examples/HelloWorld/Background.png new file mode 100644 index 0000000..4af7a5f Binary files /dev/null and b/examples/HelloWorld/Background.png differ diff --git a/examples/HelloWorld/Content/CircleSprite.png b/examples/HelloWorld/Content/CircleSprite.png new file mode 100644 index 0000000..b10a876 Binary files /dev/null and b/examples/HelloWorld/Content/CircleSprite.png differ diff --git a/examples/HelloWorld/Content/Font.spritefont b/examples/HelloWorld/Content/Font.spritefont new file mode 100644 index 0000000..20e1297 --- /dev/null +++ b/examples/HelloWorld/Content/Font.spritefont @@ -0,0 +1,60 @@ + + + + + + + Segoe UI Mono + + + 12 + + + 0 + + + true + + + + + + + + + + + + ~ + + + + diff --git a/examples/HelloWorld/Content/GroundSprite.png b/examples/HelloWorld/Content/GroundSprite.png new file mode 100644 index 0000000..423344e Binary files /dev/null and b/examples/HelloWorld/Content/GroundSprite.png differ diff --git a/examples/HelloWorld/Game.ico b/examples/HelloWorld/Game.ico new file mode 100644 index 0000000..8cff41e Binary files /dev/null and b/examples/HelloWorld/Game.ico differ diff --git a/examples/HelloWorld/Game1.cs b/examples/HelloWorld/Game1.cs new file mode 100644 index 0000000..bf0ce98 --- /dev/null +++ b/examples/HelloWorld/Game1.cs @@ -0,0 +1,196 @@ +using Box2DNet; +using Box2DNet.Dynamics; +using Box2DNet.Factories; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Graphics; +using Microsoft.Xna.Framework.Input; + +namespace HelloWorld +{ + public class Game1 : Game + { + private GraphicsDeviceManager _graphics; + private SpriteBatch _batch; + private KeyboardState _oldKeyState; + private GamePadState _oldPadState; + private SpriteFont _font; + + private World _world; + + private Body _circleBody; + private Body _groundBody; + + private Texture2D _circleSprite; + private Texture2D _groundSprite; + + // Simple camera controls + private Matrix _view; + private Vector2 _cameraPosition; + private Vector2 _screenCenter; + private Vector2 _groundOrigin; + private Vector2 _circleOrigin; + + +#if !XBOX360 + const string Text = "Press A or D to rotate the ball\n" + + "Press Space to jump\n" + + "Use arrow keys to move the camera"; +#else + const string Text = "Use left stick to move\n" + + "Use right stick to move camera\n" + + "Press A to jump\n"; +#endif + + public Game1() + { + _graphics = new GraphicsDeviceManager(this) + { + PreferredBackBufferWidth = 800, PreferredBackBufferHeight = 480 + }; + + Content.RootDirectory = "Content"; + + //Create a world with gravity. + _world = new World(new Vector2(0, 9.82f)); + } + + protected override void LoadContent() + { + // Initialize camera controls + _view = Matrix.Identity; + _cameraPosition = Vector2.Zero; + _screenCenter = new Vector2(_graphics.GraphicsDevice.Viewport.Width / 2f, _graphics.GraphicsDevice.Viewport.Height / 2f); + _batch = new SpriteBatch(_graphics.GraphicsDevice); + + _font = Content.Load("font"); + + // Load sprites + _circleSprite = Content.Load("CircleSprite"); // 96px x 96px => 1.5m x 1.5m + _groundSprite = Content.Load("GroundSprite"); // 512px x 64px => 8m x 1m + + /* We need XNA to draw the ground and circle at the center of the shapes */ + _groundOrigin = new Vector2(_groundSprite.Width / 2f, _groundSprite.Height / 2f); + _circleOrigin = new Vector2(_circleSprite.Width / 2f, _circleSprite.Height / 2f); + + // Farseer expects objects to be scaled to MKS (meters, kilos, seconds) + // 1 meters equals 64 pixels here + ConvertUnits.SetDisplayUnitToSimUnitRatio(64f); + + /* Circle */ + // Convert screen center from pixels to meters + Vector2 circlePosition = ConvertUnits.ToSimUnits(_screenCenter) + new Vector2(0, -1.5f); + + // Create the circle fixture + _circleBody = BodyFactory.CreateCircle(_world, ConvertUnits.ToSimUnits(96 / 2f), 1f, circlePosition,BodyType.Dynamic); + + // Give it some bounce and friction + _circleBody.Restitution = 0.3f; + _circleBody.Friction = 0.5f; + + /* Ground */ + Vector2 groundPosition = ConvertUnits.ToSimUnits(_screenCenter) + new Vector2(0, 1.25f); + + // Create the ground fixture + _groundBody = BodyFactory.CreateRectangle(_world, ConvertUnits.ToSimUnits(512f), ConvertUnits.ToSimUnits(64f), 1f, groundPosition); + _groundBody.IsStatic = true; + _groundBody.Restitution = 0.3f; + _groundBody.Friction = 0.5f; + } + + /// + /// Allows the game to run logic such as updating the world, + /// checking for collisions, gathering input, and playing audio. + /// + /// Provides a snapshot of timing values. + protected override void Update(GameTime gameTime) + { + HandleGamePad(); + HandleKeyboard(); + + //We update the world + _world.Step((float)gameTime.ElapsedGameTime.TotalMilliseconds * 0.001f); + + base.Update(gameTime); + } + + private void HandleGamePad() + { + GamePadState padState = GamePad.GetState(0); + + if (padState.IsConnected) + { + if (padState.Buttons.Back == ButtonState.Pressed) + Exit(); + + if (padState.Buttons.A == ButtonState.Pressed && _oldPadState.Buttons.A == ButtonState.Released) + _circleBody.ApplyLinearImpulse(new Vector2(0, -10)); + + _circleBody.ApplyForce(padState.ThumbSticks.Left); + _cameraPosition.X -= padState.ThumbSticks.Right.X; + _cameraPosition.Y += padState.ThumbSticks.Right.Y; + + _view = Matrix.CreateTranslation(new Vector3(_cameraPosition - _screenCenter, 0f)) * Matrix.CreateTranslation(new Vector3(_screenCenter, 0f)); + + _oldPadState = padState; + } + } + + private void HandleKeyboard() + { + KeyboardState state = Keyboard.GetState(); + + // Move camera + if (state.IsKeyDown(Keys.Left)) + _cameraPosition.X += 1.5f; + + if (state.IsKeyDown(Keys.Right)) + _cameraPosition.X -= 1.5f; + + if (state.IsKeyDown(Keys.Up)) + _cameraPosition.Y += 1.5f; + + if (state.IsKeyDown(Keys.Down)) + _cameraPosition.Y -= 1.5f; + + _view = Matrix.CreateTranslation(new Vector3(_cameraPosition - _screenCenter, 0f)) * Matrix.CreateTranslation(new Vector3(_screenCenter, 0f)); + + // We make it possible to rotate the circle body + if (state.IsKeyDown(Keys.A)) + _circleBody.ApplyTorque(-10); + + if (state.IsKeyDown(Keys.D)) + _circleBody.ApplyTorque(10); + + if (state.IsKeyDown(Keys.Space) && _oldKeyState.IsKeyUp(Keys.Space)) + _circleBody.ApplyLinearImpulse(new Vector2(0, -10)); + + if (state.IsKeyDown(Keys.Escape)) + Exit(); + + _oldKeyState = state; + } + + /// + /// This is called when the game should draw itself. + /// + /// Provides a snapshot of timing values. + protected override void Draw(GameTime gameTime) + { + GraphicsDevice.Clear(Color.CornflowerBlue); + + //Draw circle and ground + _batch.Begin(SpriteSortMode.Deferred, null, null, null, null, null, _view); + _batch.Draw(_circleSprite, ConvertUnits.ToDisplayUnits(_circleBody.Position), null, Color.White, _circleBody.Rotation, _circleOrigin, 1f, SpriteEffects.None, 0f); + _batch.Draw(_groundSprite, ConvertUnits.ToDisplayUnits(_groundBody.Position), null, Color.White, 0f, _groundOrigin, 1f, SpriteEffects.None, 0f); + _batch.End(); + + // Display instructions + _batch.Begin(); + _batch.DrawString(_font, Text, new Vector2(14f, 14f), Color.Black); + _batch.DrawString(_font, Text, new Vector2(12f, 12f), Color.White); + _batch.End(); + + base.Draw(gameTime); + } + } +} \ No newline at end of file diff --git a/examples/HelloWorld/GameThumbnail.png b/examples/HelloWorld/GameThumbnail.png new file mode 100644 index 0000000..462311a Binary files /dev/null and b/examples/HelloWorld/GameThumbnail.png differ diff --git a/examples/HelloWorld/PhoneGameThumb.png b/examples/HelloWorld/PhoneGameThumb.png new file mode 100644 index 0000000..a45f4db Binary files /dev/null and b/examples/HelloWorld/PhoneGameThumb.png differ diff --git a/examples/HelloWorld/Program.cs b/examples/HelloWorld/Program.cs index f0bef31..61cd501 100644 --- a/examples/HelloWorld/Program.cs +++ b/examples/HelloWorld/Program.cs @@ -14,86 +14,10 @@ class Program // box. static void Main(string[] args) { - // Define the size of the world. Simulation will still work - // if bodies reach the end of the world, but it will be slower. - AABB worldAABB = new AABB + using (var game = new Game1()) { - UpperBound = new Vector2(100, 100), - LowerBound = new Vector2(-100, -100) - }; - - // Define the gravity vector. - Vector2 gravity = new Vector2(0.0f, -10.0f); - - // Do we want to let bodies sleep? - bool doSleep = true; - - // Construct a world object, which will hold and simulate the rigid bodies. - World world = new World(worldAABB, gravity, doSleep); - - // Define the ground body. - BodyDef groundBodyDef = new BodyDef(); - groundBodyDef.Position = new Vector2(0.0f, -10.0f); - - // Call the body factory which creates the ground box shape. - // The body is also added to the world. - Body groundBody = world.CreateBody(groundBodyDef); - - // Define the ground box shape. - PolygonDef groundShapeDef = new PolygonDef(); - - // The extents are the half-widths of the box. - groundShapeDef.SetAsBox(50.0f, 10.0f); - - // Add the ground shape to the ground body. - groundBody.CreateFixture(groundShapeDef); - - // Define the dynamic body. We set its position and call the body factory. - BodyDef bodyDef = new BodyDef(); - bodyDef.Position = new Vector2(0.0f, 4.0f); - Body body = world.CreateBody(bodyDef); - - // Define another box shape for our dynamic body. - PolygonDef shapeDef = new PolygonDef(); - shapeDef.SetAsBox(1.0f, 1.0f); - - // Set the box density to be non-zero, so it will be dynamic. - shapeDef.Density = 1.0f; - - // Override the default friction. - shapeDef.Friction = 0.3f; - - // Add the shape to the body. - body.CreateFixture(shapeDef); - - // Now tell the dynamic body to compute it's mass properties base - // on its shape. - body.SetMassFromShapes(); - - // Prepare for simulation. Typically we use a time step of 1/60 of a - // second (60Hz) and 10 iterations. This provides a high quality simulation - // in most game scenarios. - float timeStep = 1.0f / 60.0f; - int velocityIterations = 8; - int positionIterations = 1; - - // This is our little game loop. - for (int i = 0; i < 100; ++i) - { - // Instruct the world to perform a single step of simulation. It is - // generally best to keep the time step and iterations fixed. - world.Step(timeStep, velocityIterations, positionIterations); - - // Now print the position and angle of the body. - Vector2 position = body.GetPosition(); - float angle = body.GetAngle(); - - Console.WriteLine("Step: {3} - X: {0}, Y: {1}, Angle: {2}", new object[] { position.X.ToString(), position.Y.ToString(), angle.ToString(), i.ToString() }); + game.Run(); } - - // When the world destructor is called, all bodies and joints are freed. This can - // create orphaned pointers, so be careful about your world management. - Console.ReadLine(); } } } \ No newline at end of file diff --git a/examples/Testbed/Framework/DebugViewBase.cs b/examples/Testbed/Framework/DebugViewBase.cs index bf446df..c94ee11 100644 --- a/examples/Testbed/Framework/DebugViewBase.cs +++ b/examples/Testbed/Framework/DebugViewBase.cs @@ -1,8 +1,7 @@ using System; -using System.Numerics; using Box2DNet.Common; using Box2DNet.Dynamics; - +using Microsoft.Xna.Framework; namespace Testbed.Framework { [Flags] diff --git a/examples/Testbed/Framework/DebugViewXNA.cs b/examples/Testbed/Framework/DebugViewXNA.cs index 591e027..78fd682 100644 --- a/examples/Testbed/Framework/DebugViewXNA.cs +++ b/examples/Testbed/Framework/DebugViewXNA.cs @@ -4,9 +4,12 @@ using System.Linq; using System.Text; using Box2DNet.Collision; +using Box2DNet.Collision.Shapes; using Box2DNet.Common; +using Box2DNet.Controllers; using Box2DNet.Dynamics; -using Box2DNet.Dynamics.Controllers; +using Box2DNet.Dynamics.Contacts; +using Box2DNet.Dynamics.Joints; using Microsoft.Xna.Framework; using Microsoft.Xna.Framework.Content; using Microsoft.Xna.Framework.Graphics; @@ -100,7 +103,7 @@ private void PreSolve(Contact contact, ref Manifold oldManifold) Fixture fixtureA = contact.FixtureA; FixedArray2 state1, state2; - Collision.Collision.GetPointStates(out state1, out state2, ref oldManifold, ref manifold); + Collision.GetPointStates(out state1, out state2, ref oldManifold, ref manifold); FixedArray2 points; Vector2 normal; @@ -136,9 +139,9 @@ private void DrawDebugData() { if (b.Enabled == false) DrawShape(f, xf, InactiveShapeColor); - else if (b.BodyType == Body.BodyType.Static) + else if (b.BodyType == BodyType.Static) DrawShape(f, xf, StaticShapeColor); - else if (b.BodyType == Body.BodyType.Kinematic) + else if (b.BodyType == BodyType.Kinematic) DrawShape(f, xf, KinematicShapeColor); else if (b.Awake == false) DrawShape(f, xf, SleepingShapeColor); diff --git a/examples/Testbed/Framework/Test.cs b/examples/Testbed/Framework/Test.cs index fd7140b..5df5e4b 100644 --- a/examples/Testbed/Framework/Test.cs +++ b/examples/Testbed/Framework/Test.cs @@ -1,6 +1,9 @@ using System; using Box2DNet.Collision; +using Box2DNet.Common; using Box2DNet.Dynamics; +using Box2DNet.Dynamics.Contacts; +using Box2DNet.Dynamics.Joints; using Microsoft.Xna.Framework; using Microsoft.Xna.Framework.Input; diff --git a/examples/Testbed/Game1.cs b/examples/Testbed/Game1.cs index 2630887..ef96154 100644 --- a/examples/Testbed/Game1.cs +++ b/examples/Testbed/Game1.cs @@ -1,26 +1,30 @@ using System; +using Box2DNet.Common; using Microsoft.Xna.Framework; using Microsoft.Xna.Framework.Input; using Testbed.Framework; namespace Testbed { + /// + /// This is the main type for your game + /// public class Game1 : Game { private TestEntry _entry; - private readonly GraphicsDeviceManager _graphics; - private readonly KeyboardManager _keyboardManager = new KeyboardManager(); + private GraphicsDeviceManager _graphics; + private KeyboardManager _keyboardManager = new KeyboardManager(); private Vector2 _lower; private GamePadState _oldGamePad; private MouseState _oldMouseState; - private Matrix _projection; - private readonly GameSettings _settings = new GameSettings(); + public Matrix Projection; + private GameSettings _settings = new GameSettings(); private Test _test; private int _testCount; private int _testIndex; private int _testSelection; private Vector2 _upper; - private Matrix _view; + public Matrix View; private Vector2 _viewCenter; private float _viewZoom; @@ -30,10 +34,10 @@ public Game1() //Default view ResetView(); - _graphics = new GraphicsDeviceManager(this) - { - PreferMultiSampling = true, PreferredBackBufferWidth = 1024, PreferredBackBufferHeight = 768 - }; + _graphics = new GraphicsDeviceManager(this); + _graphics.PreferMultiSampling = true; + _graphics.PreferredBackBufferWidth = 1024; + _graphics.PreferredBackBufferHeight = 768; Content.RootDirectory = "Content"; IsMouseVisible = true; @@ -43,9 +47,9 @@ public Game1() _graphics.SynchronizeWithVerticalRetrace = false; } - private float ViewZoom + public float ViewZoom { - get => _viewZoom; + get { return _viewZoom; } set { _viewZoom = value; @@ -53,9 +57,9 @@ private float ViewZoom } } - private Vector2 ViewCenter + public Vector2 ViewCenter { - get => _viewCenter; + get { return _viewCenter; } set { _viewCenter = value; @@ -85,7 +89,7 @@ protected override void Initialize() ++_testCount; } - _testIndex = Math.Clamp(_testIndex, 0, _testCount - 1); + _testIndex = MathUtils.Clamp(_testIndex, 0, _testCount - 1); _testSelection = _testIndex; StartTest(_testIndex); } @@ -96,7 +100,7 @@ private void CreateProjection() _upper = new Vector2(25.0f * GraphicsDevice.Viewport.AspectRatio, 25.0f); // L/R/B/T - _projection = Matrix.CreateOrthographicOffCenter(_lower.X, _upper.X, _lower.Y, _upper.Y, -1, 1); + Projection = Matrix.CreateOrthographicOffCenter(_lower.X, _upper.X, _lower.Y, _upper.Y, -1, 1); } private void StartTest(int index) @@ -133,8 +137,8 @@ protected override void Update(GameTime gameTime) Exit(); _keyboardManager._newKeyboardState = Keyboard.GetState(); - var newGamePad = GamePad.GetState(PlayerIndex.One); - var newMouseState = Mouse.GetState(); + GamePadState newGamePad = GamePad.GetState(PlayerIndex.One); + MouseState newMouseState = Mouse.GetState(); if (_keyboardManager.IsKeyDown(Keys.Z)) // Press 'z' to zoom out. @@ -190,10 +194,12 @@ protected override void Update(GameTime gameTime) EnableOrDisableFlag(DebugViewFlags.PolygonPoints); else { - _test?.Keyboard(_keyboardManager); + if (_test != null) + _test.Keyboard(_keyboardManager); } - _test?.Mouse(newMouseState, _oldMouseState); + if (_test != null) + _test.Mouse(newMouseState, _oldMouseState); if (_test != null && newGamePad.IsConnected) _test.Gamepad(newGamePad, _oldGamePad); @@ -204,9 +210,11 @@ protected override void Update(GameTime gameTime) _oldMouseState = newMouseState; _oldGamePad = newGamePad; - if (_test == null) return; - _test.TextLine = 30; - _test.Update(_settings, gameTime); + if (_test != null) + { + _test.TextLine = 30; + _test.Update(_settings, gameTime); + } } private void EnableOrDisableFlag(DebugViewFlags flag) @@ -232,7 +240,7 @@ protected override void Draw(GameTime gameTime) ResetView(); } - _test.DebugView.RenderDebugData(ref _projection, ref _view); + _test.DebugView.RenderDebugData(ref Projection, ref View); base.Draw(gameTime); } @@ -246,18 +254,18 @@ private void ResetView() private void Resize() { - _view = Matrix.CreateTranslation(new Vector3(-ViewCenter.X, -ViewCenter.Y, 0)) * Matrix.CreateScale(ViewZoom); + View = Matrix.CreateTranslation(new Vector3(-ViewCenter.X, -ViewCenter.Y, 0)) * Matrix.CreateScale(ViewZoom); } public Vector2 ConvertWorldToScreen(Vector2 position) { - var temp = GraphicsDevice.Viewport.Project(new Vector3(position, 0), _projection, _view, Matrix.Identity); + Vector3 temp = GraphicsDevice.Viewport.Project(new Vector3(position, 0), Projection, View, Matrix.Identity); return new Vector2(temp.X, temp.Y); } public Vector2 ConvertScreenToWorld(int x, int y) { - var temp = GraphicsDevice.Viewport.Unproject(new Vector3(x, y, 0), _projection, _view, Matrix.Identity); + Vector3 temp = GraphicsDevice.Viewport.Unproject(new Vector3(x, y, 0), Projection, View, Matrix.Identity); return new Vector2(temp.X, temp.Y); } diff --git a/examples/Testbed/Tests/Bridge.cs b/examples/Testbed/Tests/Bridge.cs index 061843d..83dcb14 100644 --- a/examples/Testbed/Tests/Bridge.cs +++ b/examples/Testbed/Tests/Bridge.cs @@ -1,14 +1,18 @@ using Box2DNet.Collision; +using Box2DNet.Collision.Shapes; +using Box2DNet.Common; using Box2DNet.Dynamics; +using Box2DNet.Dynamics.Joints; +using Box2DNet.Factories; using Testbed.Framework; - +using Microsoft.Xna.Framework; namespace Testbed.Tests { public class Bridge : Test { private const int Count = 30; - private BridgeTest() + private Bridge() { Body ground; { @@ -25,7 +29,7 @@ private BridgeTest() for (int i = 0; i < Count; ++i) { Body body = BodyFactory.CreateBody(World); - body.BodyType = Body.BodyType.Dynamic; + body.BodyType = BodyType.Dynamic; body.Position = new Vector2(-14.5f + 1.0f * i, 5.0f); Fixture fixture = body.CreateFixture(shape); @@ -53,7 +57,7 @@ private BridgeTest() PolygonShape shape = new PolygonShape(vertices, 1); Body body = BodyFactory.CreateBody(World); - body.BodyType = Body.BodyType.Dynamic; + body.BodyType = BodyType.Dynamic; body.Position = new Vector2(-8.0f + 8.0f * i, 12.0f); body.CreateFixture(shape); @@ -64,7 +68,7 @@ private BridgeTest() CircleShape shape = new CircleShape(0.5f, 1); Body body = BodyFactory.CreateBody(World); - body.BodyType = Body.BodyType.Dynamic; + body.BodyType = BodyType.Dynamic; body.Position = new Vector2(-6.0f + 6.0f * i, 10.0f); body.CreateFixture(shape); @@ -73,7 +77,7 @@ private BridgeTest() internal static Test Create() { - return new BridgeTest(); + return new Bridge(); } } } \ No newline at end of file diff --git a/examples/Testbed/Tests/Car.cs b/examples/Testbed/Tests/Car.cs index 7e4c44a..313e9f4 100644 --- a/examples/Testbed/Tests/Car.cs +++ b/examples/Testbed/Tests/Car.cs @@ -1,5 +1,11 @@ +using System; +using Box2DNet.Collision.Shapes; using Box2DNet.Common; using Box2DNet.Dynamics; +using Box2DNet.Dynamics.Joints; +using Box2DNet.Factories; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Input; using Testbed.Framework; using Settings = Testbed.Framework.Settings; @@ -7,192 +13,228 @@ namespace Testbed.Tests { public class Car : Test { - Body _leftWheel; - Body _rightWheel; - Body _vehicle; - RevoluteJoint _leftJoint; - RevoluteJoint _rightJoint; - - public Car() - { - { // car body - PolygonDef poly1 = new PolygonDef(), poly2 = new PolygonDef(); - - // bottom half - poly1.VertexCount = 5; - poly1.Vertices[4].Set(-2.2f, -0.74f); - poly1.Vertices[3].Set(-2.2f, 0); - poly1.Vertices[2].Set(1.0f, 0); - poly1.Vertices[1].Set(2.2f, -0.2f); - poly1.Vertices[0].Set(2.2f, -0.74f); - poly1.Filter.GroupIndex = -1; - - poly1.Density = 20.0f; - poly1.Friction = 0.68f; - poly1.Filter.GroupIndex = -1; - - // top half - poly2.VertexCount = 4; - poly2.Vertices[3].Set(-1.7f, 0); - poly2.Vertices[2].Set(-1.3f, 0.7f); - poly2.Vertices[1].Set(0.5f, 0.74f); - poly2.Vertices[0].Set(1.0f, 0); - poly2.Filter.GroupIndex = -1; - - poly2.Density = 5.0f; - poly2.Friction = 0.68f; - poly2.Filter.GroupIndex = -1; - - BodyDef bd = new BodyDef(); - bd.Position.Set(-35.0f, 2.8f); - - _vehicle = _world.CreateBody(bd); - _vehicle.CreateFixture(poly1); - _vehicle.CreateFixture(poly2); - _vehicle.SetMassFromShapes(); - } - - { // vehicle wheels - CircleDef circ = new CircleDef(); - circ.Density = 40.0f; - circ.Radius = 0.38608f; - circ.Friction = 0.8f; - circ.Filter.GroupIndex = -1; - - BodyDef bd = new BodyDef(); - bd.AllowSleep = false; - bd.Position.Set(-33.8f, 2.0f); - - _rightWheel = _world.CreateBody(bd); - _rightWheel.CreateFixture(circ); - _rightWheel.SetMassFromShapes(); - - bd.Position.Set(-36.2f, 2.0f); - _leftWheel = _world.CreateBody(bd); - _leftWheel.CreateFixture(circ); - _leftWheel.SetMassFromShapes(); - } - - { // join wheels to chassis - Vec2 anchor = new Vec2(); - RevoluteJointDef jd = new RevoluteJointDef(); - jd.Initialize(_vehicle, _leftWheel, _leftWheel.GetWorldCenter()); - jd.CollideConnected = false; - jd.EnableMotor = true; - jd.MaxMotorTorque = 10.0f; - jd.MotorSpeed = 0.0f; - _leftJoint = (RevoluteJoint)_world.CreateJoint(jd); - - jd.Initialize(_vehicle, _rightWheel, _rightWheel.GetWorldCenter()); - jd.CollideConnected = false; - _rightJoint = (RevoluteJoint)_world.CreateJoint(jd); - } - - { // ground - PolygonDef box = new PolygonDef(); - box.SetAsBox(19.5f, 0.5f); - box.Friction = 0.62f; - - BodyDef bd = new BodyDef(); - bd.Position.Set(-25.0f, 1.0f); - - Body ground = _world.CreateBody(bd); - ground.CreateFixture(box); - } - - { // more ground - PolygonDef box = new PolygonDef(); - BodyDef bd = new BodyDef(); - - box.SetAsBox(9.5f, 0.5f, Vec2.Zero, 0.1f * Box2DNet.Common.Settings.Pi); - box.Friction = 0.62f; - bd.Position.Set(27.0f - 30.0f, 3.1f); - - Body ground = _world.CreateBody(bd); - ground.CreateFixture(box); - } - - { // more ground - PolygonDef box = new PolygonDef(); - BodyDef bd = new BodyDef(); - - box.SetAsBox(9.5f, 0.5f, Vec2.Zero, -0.1f * Box2DNet.Common.Settings.Pi); - box.Friction = 0.62f; - bd.Position.Set(55.0f - 30.0f, 3.1f); - - Body ground = _world.CreateBody(bd); - ground.CreateFixture(box); - } - - { // more ground - PolygonDef box = new PolygonDef(); - BodyDef bd = new BodyDef(); - - box.SetAsBox(9.5f, 0.5f, Vec2.Zero, 0.03f * Box2DNet.Common.Settings.Pi); - box.Friction = 0.62f; - bd.Position.Set(41.0f, 2.0f); - - Body ground = _world.CreateBody(bd); - ground.CreateFixture(box); - } - - { // more ground - PolygonDef box = new PolygonDef(); - BodyDef bd = new BodyDef(); - - box.SetAsBox(5.0f, 0.5f, Vec2.Zero, 0.15f * Box2DNet.Common.Settings.Pi); - box.Friction = 0.62f; - bd.Position.Set(50.0f, 4.0f); - - Body ground = _world.CreateBody(bd); - ground.CreateFixture(box); - } - - { // more ground - PolygonDef box = new PolygonDef(); - BodyDef bd = new BodyDef(); - - box.SetAsBox(20.0f, 0.5f); - box.Friction = 0.62f; - bd.Position.Set(85.0f, 2.0f); - - Body ground = _world.CreateBody(bd); - ground.CreateFixture(box); - } - } - - public static Test Create() - { - return new Car(); - } - - public override void Keyboard(System.Windows.Forms.Keys key) - { - switch (key) - { - case System.Windows.Forms.Keys.A: - _leftJoint.SetMaxMotorTorque(800.0f); - _leftJoint.MotorSpeed = 12.0f; - break; - - case System.Windows.Forms.Keys.S: - _leftJoint.SetMaxMotorTorque(100.0f); - _leftJoint.MotorSpeed = 0.0f; - break; - - case System.Windows.Forms.Keys.D: - _leftJoint.SetMaxMotorTorque(1200.0f); - _leftJoint.MotorSpeed = -36.0f; - break; - } - } - - public override void Step(Settings settings) - { - OpenGLDebugDraw.DrawString(5, _textLine, "Keys: left = a, brake = s, right = d"); - _textLine += 15; - - base.Step(settings); - } + private Body _car; + private float _hz; + private float _speed; + private WheelJoint _spring1; + private WheelJoint _spring2; + private Body _wheel1; + private Body _wheel2; + private float _zeta; + + private Car() + { + _hz = 4.0f; + _zeta = 0.7f; + _speed = 50.0f; + + Body ground = BodyFactory.CreateEdge(World, new Vector2(-20.0f, 0.0f), new Vector2(20.0f, 0.0f)); + { + float[] hs = new[] { 0.25f, 1.0f, 4.0f, 0.0f, 0.0f, -1.0f, -2.0f, -2.0f, -1.25f, 0.0f }; + + float x = 20.0f, y1 = 0.0f; + const float dx = 5.0f; + + for (int i = 0; i < 10; ++i) + { + float y2 = hs[i]; + FixtureFactory.AttachEdge(new Vector2(x, y1), new Vector2(x + dx, y2), ground); + y1 = y2; + x += dx; + } + + for (int i = 0; i < 10; ++i) + { + float y2 = hs[i]; + FixtureFactory.AttachEdge(new Vector2(x, y1), new Vector2(x + dx, y2), ground); + y1 = y2; + x += dx; + } + + FixtureFactory.AttachEdge(new Vector2(x, 0.0f), new Vector2(x + 40.0f, 0.0f), ground); + x += 80.0f; + FixtureFactory.AttachEdge(new Vector2(x, 0.0f), new Vector2(x + 40.0f, 0.0f), ground); + x += 40.0f; + FixtureFactory.AttachEdge(new Vector2(x, 0.0f), new Vector2(x + 10.0f, 5.0f), ground); + x += 20.0f; + FixtureFactory.AttachEdge(new Vector2(x, 0.0f), new Vector2(x + 40.0f, 0.0f), ground); + x += 40.0f; + FixtureFactory.AttachEdge(new Vector2(x, 0.0f), new Vector2(x, 20.0f), ground); + + ground.Friction = 0.6f; + } + + // Teeter + { + Body body = new Body(World); + body.BodyType = BodyType.Dynamic; + body.Position = new Vector2(140.0f, 1.0f); + + PolygonShape box = new PolygonShape(1); + box.Vertices = PolygonTools.CreateRectangle(10.0f, 0.25f); + body.CreateFixture(box); + + RevoluteJoint jd = JointFactory.CreateRevoluteJoint(World, ground, body, Vector2.Zero); + jd.LowerLimit = -8.0f * Settings.Pi / 180.0f; + jd.UpperLimit = 8.0f * Settings.Pi / 180.0f; + jd.LimitEnabled = true; + + body.ApplyAngularImpulse(100.0f); + } + + //Bridge + { + const int N = 20; + PolygonShape shape = new PolygonShape(1); + shape.Vertices = PolygonTools.CreateRectangle(1.0f, 0.125f); + + Body prevBody = ground; + for (int i = 0; i < N; ++i) + { + Body body = new Body(World); + body.BodyType = BodyType.Dynamic; + body.Position = new Vector2(161.0f + 2.0f * i, -0.125f); + Fixture fix = body.CreateFixture(shape); + fix.Friction = 0.6f; + + Vector2 anchor = new Vector2(-1, 0); + JointFactory.CreateRevoluteJoint(World, prevBody, body, anchor); + + prevBody = body; + } + + Vector2 anchor2 = new Vector2(1.0f, 0); + JointFactory.CreateRevoluteJoint(World, ground, prevBody, anchor2); + } + + // Boxes + { + PolygonShape box = new PolygonShape(0.5f); + box.Vertices = PolygonTools.CreateRectangle(0.5f, 0.5f); + + Body body = new Body(World); + body.BodyType = BodyType.Dynamic; + body.Position = new Vector2(230.0f, 0.5f); + body.CreateFixture(box); + + body = new Body(World); + body.BodyType = BodyType.Dynamic; + body.Position = new Vector2(230.0f, 1.5f); + body.CreateFixture(box); + + body = new Body(World); + body.BodyType = BodyType.Dynamic; + body.Position = new Vector2(230.0f, 2.5f); + body.CreateFixture(box); + + body = new Body(World); + body.BodyType = BodyType.Dynamic; + body.Position = new Vector2(230.0f, 3.5f); + body.CreateFixture(box); + + body = new Body(World); + body.BodyType = BodyType.Dynamic; + body.Position = new Vector2(230.0f, 4.5f); + body.CreateFixture(box); + } + + // Car + { + Vertices vertices = new Vertices(8); + vertices.Add(new Vector2(-1.5f, -0.5f)); + vertices.Add(new Vector2(1.5f, -0.5f)); + vertices.Add(new Vector2(1.5f, 0.0f)); + vertices.Add(new Vector2(0.0f, 0.9f)); + vertices.Add(new Vector2(-1.15f, 0.9f)); + vertices.Add(new Vector2(-1.5f, 0.2f)); + + PolygonShape chassis = new PolygonShape(vertices, 1); + + CircleShape circle = new CircleShape(0.4f, 1); + + _car = new Body(World); + _car.BodyType = BodyType.Dynamic; + _car.Position = new Vector2(0.0f, 1.0f); + _car.CreateFixture(chassis); + + _wheel1 = new Body(World); + _wheel1.BodyType = BodyType.Dynamic; + _wheel1.Position = new Vector2(-1.0f, 0.35f); + _wheel1.CreateFixture(circle); + _wheel1.Friction = 0.9f; + + _wheel2 = new Body(World); + _wheel2.BodyType = BodyType.Dynamic; + _wheel2.Position = new Vector2(1.0f, 0.4f); + _wheel2.CreateFixture(circle); + _wheel2.Friction = 0.9f; + + Vector2 axis = new Vector2(0.0f, 1.0f); + _spring1 = new WheelJoint(_car, _wheel1, _wheel1.Position, axis, true); + _spring1.MotorSpeed = 0.0f; + _spring1.MaxMotorTorque = 20.0f; + _spring1.MotorEnabled = true; + _spring1.Frequency = _hz; + _spring1.DampingRatio = _zeta; + World.AddJoint(_spring1); + + _spring2 = new WheelJoint(_car, _wheel2, _wheel2.Position, axis, true); + _spring2.MotorSpeed = 0.0f; + _spring2.MaxMotorTorque = 10.0f; + _spring2.MotorEnabled = false; + _spring2.Frequency = _hz; + _spring2.DampingRatio = _zeta; + World.AddJoint(_spring2); + } + } + + public override void Keyboard(KeyboardManager keyboardManager) + { + if (keyboardManager.IsNewKeyPress(Keys.A)) + { + _spring1.MotorSpeed = _speed; + } + else if (keyboardManager.IsNewKeyPress(Keys.S)) + { + _spring1.MotorSpeed = 0.0f; + } + else if (keyboardManager.IsNewKeyPress(Keys.D)) + { + _spring1.MotorSpeed = -_speed; + } + else if (keyboardManager.IsNewKeyPress(Keys.Q)) + { + _hz = Math.Max(0.0f, _hz - 1.0f); + _spring1.Frequency = _hz; + _spring2.Frequency = _hz; + } + else if (keyboardManager.IsNewKeyPress(Keys.E)) + { + _hz += 1.0f; + _spring1.Frequency = _hz; + _spring2.Frequency = _hz; + } + + base.Keyboard(keyboardManager); + } + + public override void Update(GameSettings settings, GameTime gameTime) + { + DrawString("Keys: left = a, brake = s, right = d, hz down = q, hz up = e"); + + DrawString(string.Format("frequency = {0} hz, damping ratio = {1}", _hz, _zeta)); + + DrawString(string.Format("actual speed = {0} rad/sec", _spring1.JointSpeed)); + + + GameInstance.ViewCenter = _car.Position; + + base.Update(settings, gameTime); + } + + internal static Test Create() + { + return new Car(); + } } } \ No newline at end of file diff --git a/examples/Testbed/Tests/SimpleTest.cs b/examples/Testbed/Tests/SimpleTest.cs index 3c9eeca..0d2f18c 100644 --- a/examples/Testbed/Tests/SimpleTest.cs +++ b/examples/Testbed/Tests/SimpleTest.cs @@ -1,59 +1,157 @@ +using System.Diagnostics; +using Box2DNet.Common; +using Box2DNet.Common.PolygonManipulation; using Box2DNet.Dynamics; +using Microsoft.Xna.Framework; using Testbed.Framework; namespace Testbed.Tests { public class SimpleTest : Test { - public SimpleTest() - { - // Define the ground body. - BodyDef groundBodyDef = new BodyDef(); - groundBodyDef.Position.Set(0.0f, -10.0f); - - // Call the body factory which creates the ground box shape. - // The body is also added to the world. - Body groundBody = _world.CreateBody(groundBodyDef); - - // Define the ground box shape. - PolygonDef groundShapeDef = new PolygonDef(); - - // The extents are the half-widths of the box. - groundShapeDef.SetAsBox(50.0f, 10.0f); - - // Add the ground shape to the ground body. - groundBody.CreateFixture(groundShapeDef); - - for (int i = 0; i < 1; i++) - { - // Define the dynamic body. We set its position and call the body factory. - BodyDef bodyDef = new BodyDef(); - bodyDef.Position.Set(0.0f, 4.0f * (i + 1)); - Body body = _world.CreateBody(bodyDef); - - // Define another box shape for our dynamic body. - PolygonDef shapeDef = new PolygonDef(); - shapeDef.SetAsBox(1.0f, 1.0f); - - // Set the box density to be non-zero, so it will be dynamic. - shapeDef.Density = 1.0f; - - // Override the default friction. - shapeDef.Friction = 0.3f; - - // Add the shape to the body. - body.CreateFixture(shapeDef); - - // Now tell the dynamic body to compute it's mass properties base - // on its shape. - body.SetMassFromShapes(); - } - - } - - public static Test Create() - { - return new SimpleTest(); - } + private Vertices _upperLeft; + private Vertices _upperRight; + private Vertices _lowerLeft; + private Vertices _lowerRight; + private Vertices _twoShape; + + public override void Initialize() + { + base.Initialize(); + + GameInstance.ViewCenter = Vector2.Zero; + + _twoShape = new Vertices + { + new Vector2(5.510646f,-6.312136f), + new Vector2(5.510646f,-9.534955f), + new Vector2(-6.016356f,-9.534955f), + new Vector2(-6.016356f,-6.837597f), + new Vector2(-1.933609f,-0.7320573f), + new Vector2(-0.6714239f,1.242431f), + new Vector2(0.07130214f,2.498066f), + new Vector2(0.4939168f,3.344996f), + new Vector2(0.7957863f,4.093408f), + new Vector2(0.9769094f,4.743301f), + new Vector2(1.037288f,5.294677f), + new Vector2(0.9643505f,5.967545f), + new Vector2(0.7455474f,6.44485f), + new Vector2(0.5806286f,6.610869f), + new Vector2(0.3776243f,6.729462f), + new Vector2(0.1365345f,6.80062f), + new Vector2(-0.1426414f,6.824349f), + new Vector2(-0.4218241f,6.798073f), + new Vector2(-0.6629166f,6.719252f), + new Vector2(-0.8659183f,6.587883f), + new Vector2(-1.030829f,6.403981f), + new Vector2(-1.158469f,6.141973f), + new Vector2(-1.249639f,5.776335f), + new Vector2(-1.32257f,4.734189f), + new Vector2(-1.32257f,2.935948f), + new Vector2(-6.016356f,2.935948f), + new Vector2(-6.016356f,3.624884f), + new Vector2(-5.970973f,5.045072f), + new Vector2(-5.834826f,6.129576f), + new Vector2(-5.710837f,6.586056f), + new Vector2(-5.520398f,7.0389f), + new Vector2(-5.263501f,7.488094f), + new Vector2(-4.940154f,7.933653f), + new Vector2(-4.556844f,8.350358f), + new Vector2(-4.120041f,8.71307f), + new Vector2(-3.629755f,9.02178f), + new Vector2(-3.085981f,9.276493f), + new Vector2(-2.487104f,9.475718f), + new Vector2(-1.8315f,9.618026f), + new Vector2(-1.119165f,9.703418f), + new Vector2(-0.3501012f,9.731889f), + new Vector2(1.117107f,9.644661f), + new Vector2(1.779295f,9.535644f), + new Vector2(2.393876f,9.383026f), + new Vector2(2.960846f,9.186799f), + new Vector2(3.480206f,8.946972f), + new Vector2(3.951957f,8.663539f), + new Vector2(4.376098f,8.336502f), + new Vector2(5.076675f,7.592458f), + new Vector2(5.577088f,6.755733f), + new Vector2(5.877342f,5.82633f), + new Vector2(5.977431f,4.804249f), + new Vector2(5.921109f,3.981021f), + new Vector2(5.752138f,3.134446f), + new Vector2(5.470524f,2.264521f), + new Vector2(5.076274f,1.371247f), + new Vector2(4.406482f,0.2123121f), + new Vector2(3.298271f,-1.454563f), + new Vector2(1.751642f,-3.629379f), + new Vector2(-0.233405f,-6.312136f), + }; + + int beforeCount = _twoShape.Count; + _twoShape.AddRange(_twoShape); //Duplicate the points + _twoShape = SimplifyTools.MergeIdenticalPoints(_twoShape); + + Debug.Assert(beforeCount == _twoShape.Count); //The merge should have removed all duplicate points. + + const int xOffset = 18; + const int yOffset = 18; + + _upperLeft = new Vertices(_twoShape); + _upperLeft.Translate(new Vector2(-xOffset, yOffset)); + _upperLeft = SimplifyTools.ReduceByArea(_upperLeft, 0.1f); + + _upperRight = new Vertices(_twoShape); + _upperRight.Translate(new Vector2(xOffset, yOffset)); + _upperRight = SimplifyTools.ReduceByNth(_upperRight, 3); + + _lowerLeft = new Vertices(_twoShape); + _lowerLeft.Translate(new Vector2(-xOffset, -yOffset)); + _lowerLeft = SimplifyTools.ReduceByDistance(_lowerLeft, 0.5f); + + _lowerRight = new Vertices(_twoShape); + _lowerRight.Translate(new Vector2(xOffset, -yOffset)); + _lowerRight = SimplifyTools.DouglasPeuckerSimplify(_lowerRight, 0.5f); + } + + public override void Update(GameSettings settings, GameTime gameTime) + { + DrawString(string.Format("Center ({0}): Original polygon", _twoShape.Count)); + + DrawString(string.Format("Upper left ({0}): Simplified by removing points with an area of below 0.1", _upperLeft.Count)); + + DrawString(string.Format("Upper right ({0}): Simplified by removing every 3 point", _upperRight.Count)); + + DrawString(string.Format("Lower left ({0}): Simplified by removing points with a distance of less than 1", _lowerLeft.Count)); + + DrawString(string.Format("Lower right ({0}): Simplified with Douglas Peucker", _lowerRight.Count)); + + DebugView.BeginCustomDraw(ref GameInstance.Projection, ref GameInstance.View); + + DrawVertices(_twoShape); + DrawVertices(_upperLeft); + DrawVertices(_upperRight); + DrawVertices(_lowerLeft); + DrawVertices(_lowerRight); + + DebugView.EndCustomDraw(); + + base.Update(settings, gameTime); + } + + private void DrawVertices(Vertices vertices) + { + if (vertices.Count >= 1) + { + DebugView.DrawPolygon(vertices.ToArray(), vertices.Count, Color.Red); + + foreach (Vector2 vector2 in vertices) + { + DebugView.DrawPoint(vector2, 0.1f, Color.Yellow); + } + } + } + + public static Test Create() + { + return new SimpleTest(); + } } } \ No newline at end of file diff --git a/src/Box2DNet/.DS_Store b/src/Box2DNet/.DS_Store index 25ec8a4..5008ddf 100644 Binary files a/src/Box2DNet/.DS_Store and b/src/Box2DNet/.DS_Store differ diff --git a/src/Box2DNet/Box2DNet.csproj b/src/Box2DNet/Box2DNet.csproj index dcbbb66..864cdb8 100644 --- a/src/Box2DNet/Box2DNet.csproj +++ b/src/Box2DNet/Box2DNet.csproj @@ -5,7 +5,7 @@ - + diff --git a/src/Box2DNet/Box2DXDebug.cs b/src/Box2DNet/Box2DXDebug.cs deleted file mode 100644 index 70f0176..0000000 --- a/src/Box2DNet/Box2DXDebug.cs +++ /dev/null @@ -1,64 +0,0 @@ -/* - Box2DNet Copyright (c) 2018 codeyu https://github.com/codeyu/Box2DNet - Box2D original C++ version Copyright (c) 2006-2007 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.Numerics; -using System.Text; -using System.Diagnostics; -using System.Collections.Generic; - -namespace Box2DNet -{ - public static class Box2DNetDebug - { - [Conditional("DEBUG")] - public static void Assert(bool condition) - { - if (!condition) - { - condition = condition; - } - // Debug.Assert(condition); - } - - [Conditional("DEBUG")] - public static void Assert(bool condition, string message) - { - if (!condition) - { - condition = condition; - } - Debug.Assert(condition, message); - } - - [Conditional("DEBUG")] - public static void Assert(bool condition, string message, string detailMessage) - { - if (!condition) - { - condition = condition; - } - Debug.Assert(condition, message, detailMessage); - } - - public static void ThrowBox2DNetException(String message) - { - string msg = $"Error: {message}"; - throw new Exception(msg); - } - } -} \ No newline at end of file diff --git a/src/Box2DNet/Collision/BroadPhase.cs b/src/Box2DNet/Collision/BroadPhase.cs deleted file mode 100644 index d6eb1b6..0000000 --- a/src/Box2DNet/Collision/BroadPhase.cs +++ /dev/null @@ -1,1183 +0,0 @@ -/* - Box2DNet Copyright (c) 2018 codeyu https://github.com/codeyu/Box2DNet - Box2D original C++ version Copyright (c) 2006-2007 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. -*/ - -/* -This broad phase uses the Sweep and Prune algorithm as described in: -Collision Detection in Interactive 3D Environments by Gino van den Bergen -Also, some ideas, such as using integral values for fast compares comes from -Bullet (http:/www.bulletphysics.com). -*/ - -// Notes: -// - we use bound arrays instead of linked lists for cache coherence. -// - we use quantized integral values for fast compares. -// - we use short indices rather than pointers to save memory. -// - we use a stabbing count for fast overlap queries (less than order N). -// - we also use a time stamp on each proxy to speed up the registration of -// overlap query results. -// - where possible, we compare bound indices instead of values to reduce -// cache misses (TODO_ERIN). -// - no broadphase is perfect and neither is this one: it is not great for huge -// worlds (use a multi-SAP instead), it is not great for large objects. - -//#define TARGET_FLOAT32_IS_FIXED - -using System; using System.Numerics; -using System.Collections.Generic; -using System.Text; - -using Box2DNet.Common; - - -namespace Box2DNet.Collision -{ - public delegate float SortKeyFunc(object shape); - -#warning "CAS" - public class BoundValues - { - public ushort[/*2*/] LowerValues = new ushort[2]; - public ushort[/*2*/] UpperValues = new ushort[2]; - } -#warning "CAS" - public class Bound - { - public bool IsLower { get { return (Value & (ushort)1) == (ushort)0; } } - public bool IsUpper { get { return (Value & (ushort)1) == (ushort)1; } } - - public ushort Value; - public ushort ProxyId; - public ushort StabbingCount; - - public Bound Clone() - { - Bound newBound = new Bound(); - newBound.Value = this.Value; - newBound.ProxyId = this.ProxyId; - newBound.StabbingCount = this.StabbingCount; - return newBound; - } - } -#warning "CAS" - public class Proxy - { - public ushort[/*2*/] LowerBounds = new ushort[2], UpperBounds = new ushort[2]; - public ushort OverlapCount; - public ushort TimeStamp; - public object UserData; - - public ushort Next - { - get { return LowerBounds[0]; } - set { LowerBounds[0] = value; } - } - - public bool IsValid { get { return OverlapCount != BroadPhase.Invalid; } } - } - - public class BroadPhase - { -#if TARGET_FLOAT32_IS_FIXED - public static readonly ushort BROADPHASE_MAX = (Common.Math.USHRT_MAX/2); -#else - public static readonly ushort BROADPHASE_MAX = Common.Math.USHRT_MAX; -#endif - - public static readonly ushort Invalid = BROADPHASE_MAX; - public static readonly ushort NullEdge = BROADPHASE_MAX; - - public PairManager _pairManager; - - public Proxy[] _proxyPool = new Proxy[Settings.MaxProxies]; - public ushort _freeProxy; - - public Bound[][] _bounds = new Bound[2][/*(2 * Settings.MaxProxies)*/]; - - public ushort[] _queryResults = new ushort[Settings.MaxProxies]; - public float[] _querySortKeys = new float[Settings.MaxProxies]; - public int _queryResultCount; - - public AABB _worldAABB; - public Vector2 _quantizationFactor; - public int _proxyCount; - public ushort _timeStamp; - - public static bool IsValidate = false; - - public BroadPhase(AABB worldAABB, PairCallback callback) - { - _pairManager = new PairManager(); - _pairManager.Initialize(this, callback); - - Box2DNetDebug.Assert(worldAABB.IsValid); - _worldAABB = worldAABB; - _proxyCount = 0; - - Vector2 d = worldAABB.UpperBound - worldAABB.LowerBound; - _quantizationFactor.X = (float)BROADPHASE_MAX / d.X; - _quantizationFactor.Y = (float)BROADPHASE_MAX / d.Y; - - for (ushort i = 0; i < Settings.MaxProxies - 1; ++i) - { - _proxyPool[i] = new Proxy(); - _proxyPool[i].Next = (ushort)(i + 1); - _proxyPool[i].TimeStamp = 0; - _proxyPool[i].OverlapCount = BroadPhase.Invalid; - _proxyPool[i].UserData = null; - } - _proxyPool[Settings.MaxProxies - 1] = new Proxy(); - _proxyPool[Settings.MaxProxies - 1].Next = PairManager.NullProxy; - _proxyPool[Settings.MaxProxies - 1].TimeStamp = 0; - _proxyPool[Settings.MaxProxies - 1].OverlapCount = BroadPhase.Invalid; - _proxyPool[Settings.MaxProxies - 1].UserData = null; - _freeProxy = 0; - - _timeStamp = 1; - _queryResultCount = 0; - - for (int i = 0; i < 2; i++) - { - _bounds[i] = new Bound[(2 * Settings.MaxProxies)]; - } - - int bCount = 2 * Settings.MaxProxies; - for (int j = 0; j < 2; j++) - for (int k = 0; k < bCount; k++) - _bounds[j][k] = new Bound(); - } - - // Use this to see if your proxy is in range. If it is not in range, - // it should be destroyed. Otherwise you may get O(m^2) pairs, where m - // is the number of proxies that are out of range. - public bool InRange(AABB aabb) - { - Vector2 d = Vector2.Max(aabb.LowerBound - _worldAABB.UpperBound, _worldAABB.LowerBound - aabb.UpperBound); - return System.Math.Max(d.X, d.Y) < 0.0f; - } - - // Create and destroy proxies. These call Flush first. - public ushort CreateProxy(AABB aabb, object userData) - { - Box2DNetDebug.Assert(_proxyCount < Settings.MaxProxies); - Box2DNetDebug.Assert(_freeProxy != PairManager.NullProxy); - - ushort proxyId = _freeProxy; - Proxy proxy = _proxyPool[proxyId]; - _freeProxy = proxy.Next; - - proxy.OverlapCount = 0; - proxy.UserData = userData; - - int boundCount = 2 * _proxyCount; - - ushort[] lowerValues = new ushort[2], upperValues = new ushort[2]; - ComputeBounds(out lowerValues, out upperValues, aabb); - - for (int axis = 0; axis < 2; ++axis) - { - Bound[] bounds = _bounds[axis]; - int lowerIndex, upperIndex; - Query(out lowerIndex, out upperIndex, lowerValues[axis], upperValues[axis], bounds, boundCount, axis); - -#warning "Check this" - //memmove(bounds + upperIndex + 2, bounds + upperIndex, (boundCount - upperIndex) * sizeof(b2Bound)); - Bound[] tmp = new Bound[boundCount - upperIndex]; - for (int i = 0; i < (boundCount - upperIndex); i++) - { - tmp[i] = bounds[upperIndex + i].Clone(); - } - for (int i = 0; i < (boundCount - upperIndex); i++) - { - bounds[upperIndex + 2 + i] = tmp[i]; - } - - //memmove(bounds + lowerIndex + 1, bounds + lowerIndex, (upperIndex - lowerIndex) * sizeof(b2Bound)); - tmp = new Bound[upperIndex - lowerIndex]; - for (int i = 0; i < (upperIndex - lowerIndex); i++) - { - tmp[i] = bounds[lowerIndex + i].Clone(); - } - for (int i = 0; i < (upperIndex - lowerIndex); i++) - { - bounds[lowerIndex + 1 + i] = tmp[i]; - } - - // The upper index has increased because of the lower bound insertion. - ++upperIndex; - - // Copy in the new bounds. - bounds[lowerIndex].Value = lowerValues[axis]; - bounds[lowerIndex].ProxyId = proxyId; - bounds[upperIndex].Value = upperValues[axis]; - bounds[upperIndex].ProxyId = proxyId; - - bounds[lowerIndex].StabbingCount = lowerIndex == 0 ? (ushort)0 : bounds[lowerIndex - 1].StabbingCount; - bounds[upperIndex].StabbingCount = bounds[upperIndex - 1].StabbingCount; - - // Adjust the stabbing count between the new bounds. - for (int index = lowerIndex; index < upperIndex; ++index) - { - ++bounds[index].StabbingCount; - } - - // Adjust the all the affected bound indices. - for (int index = lowerIndex; index < boundCount + 2; ++index) - { - Proxy proxy_ = _proxyPool[bounds[index].ProxyId]; - if (bounds[index].IsLower) - { - proxy_.LowerBounds[axis] = (ushort)index; - } - else - { - proxy_.UpperBounds[axis] = (ushort)index; - } - } - } - - ++_proxyCount; - - Box2DNetDebug.Assert(_queryResultCount < Settings.MaxProxies); - - // Create pairs if the AABB is in range. - for (int i = 0; i < _queryResultCount; ++i) - { - Box2DNetDebug.Assert(_queryResults[i] < Settings.MaxProxies); - Box2DNetDebug.Assert(_proxyPool[_queryResults[i]].IsValid); - - _pairManager.AddBufferedPair(proxyId, _queryResults[i]); - } - - _pairManager.Commit(); - - if (IsValidate) - { - Validate(); - } - - // Prepare for next query. - _queryResultCount = 0; - IncrementTimeStamp(); - - return proxyId; - } - - public void DestroyProxy(int proxyId) - { - Box2DNetDebug.Assert(0 < _proxyCount && _proxyCount <= Settings.MaxProxies); - Proxy proxy = _proxyPool[proxyId]; - Box2DNetDebug.Assert(proxy.IsValid); - - int boundCount = 2 * _proxyCount; - - for (int axis = 0; axis < 2; ++axis) - { - Bound[] bounds = _bounds[axis]; - - int lowerIndex = proxy.LowerBounds[axis]; - int upperIndex = proxy.UpperBounds[axis]; - ushort lowerValue = bounds[lowerIndex].Value; - ushort upperValue = bounds[upperIndex].Value; - -#warning "Check this" - //memmove(bounds + lowerIndex, bounds + lowerIndex + 1, (upperIndex - lowerIndex - 1) * sizeof(b2Bound)); - Bound[] tmp = new Bound[upperIndex - lowerIndex - 1]; - for (int i = 0; i < (upperIndex - lowerIndex - 1); i++) - { - tmp[i] = bounds[lowerIndex + 1 + i].Clone(); - } - for (int i = 0; i < (upperIndex - lowerIndex - 1); i++) - { - bounds[lowerIndex + i] = tmp[i]; - } - - //memmove(bounds + upperIndex - 1, bounds + upperIndex + 1, (boundCount - upperIndex - 1) * sizeof(b2Bound)); - tmp = new Bound[boundCount - upperIndex - 1]; - for (int i = 0; i < (boundCount - upperIndex - 1); i++) - { - tmp[i] = bounds[upperIndex + 1 + i].Clone(); - } - for (int i = 0; i < (boundCount - upperIndex - 1); i++) - { - bounds[upperIndex - 1 + i] = tmp[i]; - } - - // Fix bound indices. - for (int index = lowerIndex; index < boundCount - 2; ++index) - { - Proxy proxy_ = _proxyPool[bounds[index].ProxyId]; - if (bounds[index].IsLower) - { - proxy_.LowerBounds[axis] = (ushort)index; - } - else - { - proxy_.UpperBounds[axis] = (ushort)index; - } - } - - // Fix stabbing count. - for (int index = lowerIndex; index < upperIndex - 1; ++index) - { - --bounds[index].StabbingCount; - } - - // Query for pairs to be removed. lowerIndex and upperIndex are not needed. - Query(out lowerIndex, out upperIndex, lowerValue, upperValue, bounds, boundCount - 2, axis); - } - - Box2DNetDebug.Assert(_queryResultCount < Settings.MaxProxies); - - for (int i = 0; i < _queryResultCount; ++i) - { - Box2DNetDebug.Assert(_proxyPool[_queryResults[i]].IsValid); - _pairManager.RemoveBufferedPair(proxyId, _queryResults[i]); - } - - _pairManager.Commit(); - - // Prepare for next query. - _queryResultCount = 0; - IncrementTimeStamp(); - - // Return the proxy to the pool. - proxy.UserData = null; - proxy.OverlapCount = BroadPhase.Invalid; - proxy.LowerBounds[0] = BroadPhase.Invalid; - proxy.LowerBounds[1] = BroadPhase.Invalid; - proxy.UpperBounds[0] = BroadPhase.Invalid; - proxy.UpperBounds[1] = BroadPhase.Invalid; - - proxy.Next = _freeProxy; - _freeProxy = (ushort)proxyId; - --_proxyCount; - - if (IsValidate) - { - Validate(); - } - } - - // Call MoveProxy as many times as you like, then when you are done - // call Commit to finalized the proxy pairs (for your time step). - public void MoveProxy(int proxyId, AABB aabb) - { - if (proxyId == PairManager.NullProxy || Settings.MaxProxies <= proxyId) - { - Box2DNetDebug.Assert(false); - return; - } - - if (aabb.IsValid == false) - { - Box2DNetDebug.Assert(false); - return; - } - - int boundCount = 2 * _proxyCount; - - Proxy proxy = _proxyPool[proxyId]; - - // Get new bound values - BoundValues newValues = new BoundValues(); ; - ComputeBounds(out newValues.LowerValues, out newValues.UpperValues, aabb); - - // Get old bound values - BoundValues oldValues = new BoundValues(); - for (int axis = 0; axis < 2; ++axis) - { - oldValues.LowerValues[axis] = _bounds[axis][proxy.LowerBounds[axis]].Value; - oldValues.UpperValues[axis] = _bounds[axis][proxy.UpperBounds[axis]].Value; - } - - for (int axis = 0; axis < 2; ++axis) - { - Bound[] bounds = _bounds[axis]; - - int lowerIndex = proxy.LowerBounds[axis]; - int upperIndex = proxy.UpperBounds[axis]; - - ushort lowerValue = newValues.LowerValues[axis]; - ushort upperValue = newValues.UpperValues[axis]; - - int deltaLower = lowerValue - bounds[lowerIndex].Value; - int deltaUpper = upperValue - bounds[upperIndex].Value; - - bounds[lowerIndex].Value = lowerValue; - bounds[upperIndex].Value = upperValue; - - // - // Expanding adds overlaps - // - - // Should we move the lower bound down? - if (deltaLower < 0) - { - int index = lowerIndex; - while (index > 0 && lowerValue < bounds[index - 1].Value) - { - Bound bound = bounds[index]; - Bound prevBound = bounds[index - 1]; - - int prevProxyId = prevBound.ProxyId; - Proxy prevProxy = _proxyPool[prevBound.ProxyId]; - - ++prevBound.StabbingCount; - - if (prevBound.IsUpper == true) - { - if (TestOverlap(newValues, prevProxy)) - { - _pairManager.AddBufferedPair(proxyId, prevProxyId); - } - - ++prevProxy.UpperBounds[axis]; - ++bound.StabbingCount; - } - else - { - ++prevProxy.LowerBounds[axis]; - --bound.StabbingCount; - } - - --proxy.LowerBounds[axis]; - Common.Math.Swap(ref bounds[index], ref bounds[index - 1]); - --index; - } - } - - // Should we move the upper bound up? - if (deltaUpper > 0) - { - int index = upperIndex; - while (index < boundCount - 1 && bounds[index + 1].Value <= upperValue) - { - Bound bound = bounds[index]; - Bound nextBound = bounds[index + 1]; - int nextProxyId = nextBound.ProxyId; - Proxy nextProxy = _proxyPool[nextProxyId]; - - ++nextBound.StabbingCount; - - if (nextBound.IsLower == true) - { - if (TestOverlap(newValues, nextProxy)) - { - _pairManager.AddBufferedPair(proxyId, nextProxyId); - } - - --nextProxy.LowerBounds[axis]; - ++bound.StabbingCount; - } - else - { - --nextProxy.UpperBounds[axis]; - --bound.StabbingCount; - } - - ++proxy.UpperBounds[axis]; - Common.Math.Swap(ref bounds[index], ref bounds[index + 1]); - ++index; - } - } - - // - // Shrinking removes overlaps - // - - // Should we move the lower bound up? - if (deltaLower > 0) - { - int index = lowerIndex; - while (index < boundCount - 1 && bounds[index + 1].Value <= lowerValue) - { - Bound bound = bounds[index]; - Bound nextBound = bounds[index + 1]; - - int nextProxyId = nextBound.ProxyId; - Proxy nextProxy = _proxyPool[nextProxyId]; - - --nextBound.StabbingCount; - - if (nextBound.IsUpper) - { - if (TestOverlap(oldValues, nextProxy)) - { - _pairManager.RemoveBufferedPair(proxyId, nextProxyId); - } - - --nextProxy.UpperBounds[axis]; - --bound.StabbingCount; - } - else - { - --nextProxy.LowerBounds[axis]; - ++bound.StabbingCount; - } - - ++proxy.LowerBounds[axis]; - Common.Math.Swap(ref bounds[index], ref bounds[index + 1]); - ++index; - } - } - - // Should we move the upper bound down? - if (deltaUpper < 0) - { - int index = upperIndex; - while (index > 0 && upperValue < bounds[index - 1].Value) - { - Bound bound = bounds[index]; - Bound prevBound = bounds[index - 1]; - - int prevProxyId = prevBound.ProxyId; - Proxy prevProxy = _proxyPool[prevProxyId]; - - --prevBound.StabbingCount; - - if (prevBound.IsLower == true) - { - if (TestOverlap(oldValues, prevProxy)) - { - _pairManager.RemoveBufferedPair(proxyId, prevProxyId); - } - - ++prevProxy.LowerBounds[axis]; - --bound.StabbingCount; - } - else - { - ++prevProxy.UpperBounds[axis]; - ++bound.StabbingCount; - } - - --proxy.UpperBounds[axis]; - Common.Math.Swap(ref bounds[index], ref bounds[index - 1]); - --index; - } - } - } - - if (IsValidate) - { - Validate(); - } - } - - public void Commit() - { - _pairManager.Commit(); - } - - // Get a single proxy. Returns NULL if the id is invalid. - public Proxy GetProxy(int proxyId) - { - if (proxyId == PairManager.NullProxy || _proxyPool[proxyId].IsValid == false) - { - return null; - } - - return _proxyPool[proxyId]; - } - - // Query an AABB for overlapping proxies, returns the user data and - // the count, up to the supplied maximum count. - public int Query(AABB aabb, object[] userData, int maxCount) - { - ushort[] lowerValues; - ushort[] upperValues; - ComputeBounds(out lowerValues, out upperValues, aabb); - - int lowerIndex, upperIndex; - - Query(out lowerIndex, out upperIndex, lowerValues[0], upperValues[0], _bounds[0], 2 * _proxyCount, 0); - Query(out lowerIndex, out upperIndex, lowerValues[1], upperValues[1], _bounds[1], 2 * _proxyCount, 1); - - Box2DNetDebug.Assert(_queryResultCount < Settings.MaxProxies); - - int count = 0; - for (int i = 0; i < _queryResultCount && count < maxCount; ++i, ++count) - { - Box2DNetDebug.Assert(_queryResults[i] < Settings.MaxProxies); - Proxy proxy = _proxyPool[_queryResults[i]]; - Box2DNetDebug.Assert(proxy.IsValid); - userData[i] = proxy.UserData; - } - - // Prepare for next query. - _queryResultCount = 0; - IncrementTimeStamp(); - - return count; - } - - /// - /// Query a segment for overlapping proxies, returns the user data and - /// the count, up to the supplied maximum count. - /// If sortKey is provided, then it is a function mapping from proxy userDatas to distances along the segment (between 0 & 1) - /// Then the returned proxies are sorted on that, before being truncated to maxCount - /// The sortKey of a proxy is assumed to be larger than the closest point inside the proxy along the segment, this allows for early exits - /// Proxies with a negative sortKey are discarded - /// - public -#if ALLOWUNSAFE - unsafe -#endif - int QuerySegment(Segment segment, object[] userData, int maxCount, SortKeyFunc sortKey) - { - float maxLambda = 1; - - float dx = (segment.P2.X - segment.P1.X) * _quantizationFactor.X; - float dy = (segment.P2.Y - segment.P1.Y) * _quantizationFactor.Y; - - int sx = dx < -Settings.FLT_EPSILON ? -1 : (dx > Settings.FLT_EPSILON ? 1 : 0); - int sy = dy < -Settings.FLT_EPSILON ? -1 : (dy > Settings.FLT_EPSILON ? 1 : 0); - - Box2DNetDebug.Assert(sx != 0 || sy != 0); - - float p1x = (segment.P1.X - _worldAABB.LowerBound.X) * _quantizationFactor.X; - float p1y = (segment.P1.Y - _worldAABB.LowerBound.Y) * _quantizationFactor.Y; -#if ALLOWUNSAFE - ushort* startValues = stackalloc ushort[2]; - ushort* startValues2 = stackalloc ushort[2]; -#else - ushort[] startValues = new ushort[2]; - ushort[] startValues2 = new ushort[2]; -#endif - - int xIndex; - int yIndex; - - ushort proxyId; - Proxy proxy; - - // TODO_ERIN implement fast float to ushort conversion. - startValues[0] = (ushort)((ushort)(p1x) & (BROADPHASE_MAX - 1)); - startValues2[0] = (ushort)((ushort)(p1x) | 1); - - startValues[1] = (ushort)((ushort)(p1y) & (BROADPHASE_MAX - 1)); - startValues2[1] = (ushort)((ushort)(p1y) | 1); - - //First deal with all the proxies that contain segment.p1 - int lowerIndex; - int upperIndex; - Query(out lowerIndex, out upperIndex, startValues[0], startValues2[0], _bounds[0], 2 * _proxyCount, 0); - if (sx >= 0) xIndex = upperIndex - 1; - else xIndex = lowerIndex; - Query(out lowerIndex, out upperIndex, startValues[1], startValues2[1], _bounds[1], 2 * _proxyCount, 1); - if (sy >= 0) yIndex = upperIndex - 1; - else yIndex = lowerIndex; - - //If we are using sortKey, then sort what we have so far, filtering negative keys - if (sortKey != null) - { - //Fill keys - for (int j = 0; j < _queryResultCount; j++) - { - _querySortKeys[j] = sortKey(_proxyPool[_queryResults[j]].UserData); - } - //Bubble sort keys - //Sorting negative values to the top, so we can easily remove them - int i = 0; - while (i < _queryResultCount - 1) - { - float a = _querySortKeys[i]; - float b = _querySortKeys[i + 1]; - if ((a < 0) ? (b >= 0) : (a > b && b >= 0)) - { - _querySortKeys[i + 1] = a; - _querySortKeys[i] = b; - ushort tempValue = _queryResults[i + 1]; - _queryResults[i + 1] = _queryResults[i]; - _queryResults[i] = tempValue; - i--; - if (i == -1) i = 1; - } - else - { - i++; - } - } - //Skim off negative values - while (_queryResultCount > 0 && _querySortKeys[_queryResultCount - 1] < 0) - _queryResultCount--; - } - - //Now work through the rest of the segment - for (; ; ) - { - float xProgress = 0; - float yProgress = 0; - if (xIndex < 0 || xIndex >= _proxyCount * 2) - break; - if (yIndex < 0 || yIndex >= _proxyCount * 2) - break; - if (sx != 0) - { - //Move on to the next bound - if (sx > 0) - { - xIndex++; - if (xIndex == _proxyCount * 2) - break; - } - else - { - xIndex--; - if (xIndex < 0) - break; - } - xProgress = (_bounds[0][xIndex].Value - p1x) / dx; - } - if (sy != 0) - { - //Move on to the next bound - if (sy > 0) - { - yIndex++; - if (yIndex == _proxyCount * 2) - break; - } - else - { - yIndex--; - if (yIndex < 0) - break; - } - yProgress = (_bounds[1][yIndex].Value - p1y) / dy; - } - for (; ; ) - { - if (sy == 0 || (sx != 0 && xProgress < yProgress)) - { - if (xProgress > maxLambda) - break; - - //Check that we are entering a proxy, not leaving - if (sx > 0 ? _bounds[0][xIndex].IsLower : _bounds[0][xIndex].IsUpper) - { - //Check the other axis of the proxy - proxyId = _bounds[0][xIndex].ProxyId; - proxy = _proxyPool[proxyId]; - if (sy >= 0) - { - if (proxy.LowerBounds[1] <= yIndex - 1 && proxy.UpperBounds[1] >= yIndex) - { - //Add the proxy - if (sortKey != null) - { - AddProxyResult(proxyId, proxy, maxCount, sortKey); - } - else - { - _queryResults[_queryResultCount] = proxyId; - ++_queryResultCount; - } - } - } - else - { - if (proxy.LowerBounds[1] <= yIndex && proxy.UpperBounds[1] >= yIndex + 1) - { - //Add the proxy - if (sortKey != null) - { - AddProxyResult(proxyId, proxy, maxCount, sortKey); - } - else - { - _queryResults[_queryResultCount] = proxyId; - ++_queryResultCount; - } - } - } - } - - //Early out - if (sortKey != null && _queryResultCount == maxCount && _queryResultCount > 0 && xProgress > _querySortKeys[_queryResultCount - 1]) - break; - - //Move on to the next bound - if (sx > 0) - { - xIndex++; - if (xIndex == _proxyCount * 2) - break; - } - else - { - xIndex--; - if (xIndex < 0) - break; - } - xProgress = (_bounds[0][xIndex].Value - p1x) / dx; - } - else - { - if (yProgress > maxLambda) - break; - - //Check that we are entering a proxy, not leaving - if (sy > 0 ? _bounds[1][yIndex].IsLower : _bounds[1][yIndex].IsUpper) - { - //Check the other axis of the proxy - proxyId = _bounds[1][yIndex].ProxyId; - proxy = _proxyPool[proxyId]; - if (sx >= 0) - { - if (proxy.LowerBounds[0] <= xIndex - 1 && proxy.UpperBounds[0] >= xIndex) - { - //Add the proxy - if (sortKey != null) - { - AddProxyResult(proxyId, proxy, maxCount, sortKey); - } - else - { - _queryResults[_queryResultCount] = proxyId; - ++_queryResultCount; - } - } - } - else - { - if (proxy.LowerBounds[0] <= xIndex && proxy.UpperBounds[0] >= xIndex + 1) - { - //Add the proxy - if (sortKey != null) - { - AddProxyResult(proxyId, proxy, maxCount, sortKey); - } - else - { - _queryResults[_queryResultCount] = proxyId; - ++_queryResultCount; - } - } - } - } - - //Early out - if (sortKey != null && _queryResultCount == maxCount && _queryResultCount > 0 && yProgress > _querySortKeys[_queryResultCount - 1]) - break; - - //Move on to the next bound - if (sy > 0) - { - yIndex++; - if (yIndex == _proxyCount * 2) - break; - } - else - { - yIndex--; - if (yIndex < 0) - break; - } - yProgress = (_bounds[1][yIndex].Value - p1y) / dy; - } - } - - break; - } - - int count = 0; - for (int i = 0; i < _queryResultCount && count < maxCount; ++i, ++count) - { - Box2DNetDebug.Assert(_queryResults[i] < Settings.MaxProxies); - Proxy proxy_ = _proxyPool[_queryResults[i]]; - Box2DNetDebug.Assert(proxy_.IsValid); - userData[i] = proxy_.UserData; - } - - // Prepare for next query. - _queryResultCount = 0; - IncrementTimeStamp(); - - return count; - } - - public void Validate() - { - for (int axis = 0; axis < 2; ++axis) - { - Bound[] bounds = _bounds[axis]; - - int boundCount = 2 * _proxyCount; - ushort stabbingCount = 0; - - for (int i = 0; i < boundCount; ++i) - { - Bound bound = bounds[i]; - Box2DNetDebug.Assert(i == 0 || bounds[i - 1].Value <= bound.Value); - Box2DNetDebug.Assert(bound.ProxyId != PairManager.NullProxy); - Box2DNetDebug.Assert(_proxyPool[bound.ProxyId].IsValid); - - if (bound.IsLower == true) - { - Box2DNetDebug.Assert(_proxyPool[bound.ProxyId].LowerBounds[axis] == i); - ++stabbingCount; - } - else - { - Box2DNetDebug.Assert(_proxyPool[bound.ProxyId].UpperBounds[axis] == i); - --stabbingCount; - } - - Box2DNetDebug.Assert(bound.StabbingCount == stabbingCount); - } - } - } - - private void ComputeBounds(out ushort[] lowerValues, out ushort[] upperValues, AABB aabb) - { - lowerValues = new ushort[2]; - upperValues = new ushort[2]; - - Box2DNetDebug.Assert(aabb.UpperBound.X >= aabb.LowerBound.X); - Box2DNetDebug.Assert(aabb.UpperBound.Y >= aabb.LowerBound.Y); - - Vector2 minVertex = Common.Math.Clamp(aabb.LowerBound, _worldAABB.LowerBound, _worldAABB.UpperBound); - Vector2 maxVertex = Common.Math.Clamp(aabb.UpperBound, _worldAABB.LowerBound, _worldAABB.UpperBound); - - // Bump lower bounds downs and upper bounds up. This ensures correct sorting of - // lower/upper bounds that would have equal values. - // TODO_ERIN implement fast float to uint16 conversion. - lowerValues[0] = (ushort)((ushort)(_quantizationFactor.X * (minVertex.X - _worldAABB.LowerBound.X)) & (BROADPHASE_MAX - 1)); - upperValues[0] = (ushort)((ushort)(_quantizationFactor.X * (maxVertex.X - _worldAABB.LowerBound.X)) | 1); - - lowerValues[1] = (ushort)((ushort)(_quantizationFactor.Y * (minVertex.Y - _worldAABB.LowerBound.Y)) & (BROADPHASE_MAX - 1)); - upperValues[1] = (ushort)((ushort)(_quantizationFactor.Y * (maxVertex.Y - _worldAABB.LowerBound.Y)) | 1); - } - - // This one is only used for validation. - internal bool TestOverlap(Proxy p1, Proxy p2) - { - for (int axis = 0; axis < 2; ++axis) - { - Bound[] bounds = _bounds[axis]; - - Box2DNetDebug.Assert(p1.LowerBounds[axis] < 2 * _proxyCount); - Box2DNetDebug.Assert(p1.UpperBounds[axis] < 2 * _proxyCount); - Box2DNetDebug.Assert(p2.LowerBounds[axis] < 2 * _proxyCount); - Box2DNetDebug.Assert(p2.UpperBounds[axis] < 2 * _proxyCount); - - if (bounds[p1.LowerBounds[axis]].Value > bounds[p2.UpperBounds[axis]].Value) - return false; - - if (bounds[p1.UpperBounds[axis]].Value < bounds[p2.LowerBounds[axis]].Value) - return false; - } - - return true; - } - - internal bool TestOverlap(BoundValues b, Proxy p) - { - for (int axis = 0; axis < 2; ++axis) - { - Bound[] bounds = _bounds[axis]; - - Box2DNetDebug.Assert(p.LowerBounds[axis] < 2 * _proxyCount); - Box2DNetDebug.Assert(p.UpperBounds[axis] < 2 * _proxyCount); - - if (b.LowerValues[axis] > bounds[p.UpperBounds[axis]].Value) - return false; - - if (b.UpperValues[axis] < bounds[p.LowerBounds[axis]].Value) - return false; - } - - return true; - } - - private void Query(out int lowerQueryOut, out int upperQueryOut, - ushort lowerValue, ushort upperValue, - Bound[] bounds, int boundCount, int axis) - { - int lowerQuery = BinarySearch(bounds, boundCount, lowerValue); - int upperQuery = BinarySearch(bounds, boundCount, upperValue); - - // Easy case: lowerQuery <= lowerIndex(i) < upperQuery - // Solution: search query range for min bounds. - for (int i = lowerQuery; i < upperQuery; ++i) - { - if (bounds[i].IsLower) - { - IncrementOverlapCount(bounds[i].ProxyId); - } - } - - // Hard case: lowerIndex(i) < lowerQuery < upperIndex(i) - // Solution: use the stabbing count to search down the bound array. - if (lowerQuery > 0) - { - int i = lowerQuery - 1; - int s = bounds[i].StabbingCount; - - // Find the s overlaps. - while (s != 0) - { - Box2DNetDebug.Assert(i >= 0); - - if (bounds[i].IsLower) - { - Proxy proxy = _proxyPool[bounds[i].ProxyId]; - if (lowerQuery <= proxy.UpperBounds[axis]) - { - IncrementOverlapCount(bounds[i].ProxyId); - --s; - } - } - --i; - } - } - - lowerQueryOut = lowerQuery; - upperQueryOut = upperQuery; - } - int qi1 = 0; - int qi2 = 0; - private void IncrementOverlapCount(int proxyId) - { - Proxy proxy = _proxyPool[proxyId]; - if (proxy.TimeStamp < _timeStamp) - { - proxy.TimeStamp = _timeStamp; - proxy.OverlapCount = 1; - qi1++; - } - else - { - proxy.OverlapCount = 2; - Box2DNetDebug.Assert(_queryResultCount < Settings.MaxProxies); - _queryResults[_queryResultCount] = (ushort)proxyId; - ++_queryResultCount; - qi2++; - } - } - - private void IncrementTimeStamp() - { - if (_timeStamp == BROADPHASE_MAX) - { - for (ushort i = 0; i < Settings.MaxProxies; ++i) - { - _proxyPool[i].TimeStamp = 0; - } - _timeStamp = 1; - } - else - { - ++_timeStamp; - } - } - -#if ALLOWUNSAFE - public unsafe void AddProxyResult(ushort proxyId, Proxy proxy, int maxCount, SortKeyFunc sortKey) - { - float key = sortKey(proxy.UserData); - //Filter proxies on positive keys - if (key < 0) - return; - //Merge the new key into the sorted list. - //float32* p = std::lower_bound(m_querySortKeys,m_querySortKeys+m_queryResultCount,key); - fixed (float* querySortKeysPtr = _querySortKeys) - { - float* p = querySortKeysPtr; - while (*p < key && p < &querySortKeysPtr[_queryResultCount]) - p++; - int i = (int)(p - &querySortKeysPtr[0]); - if (maxCount == _queryResultCount && i == _queryResultCount) - return; - if (maxCount == _queryResultCount) - _queryResultCount--; - //std::copy_backward - for (int j = _queryResultCount + 1; j > i; --j) - { - _querySortKeys[j] = _querySortKeys[j - 1]; - _queryResults[j] = _queryResults[j - 1]; - } - _querySortKeys[i] = key; - _queryResults[i] = proxyId; - _queryResultCount++; - } - } -#else - public void AddProxyResult(ushort proxyId, Proxy proxy, int maxCount, SortKeyFunc sortKey) - { - float key = sortKey(proxy.UserData); - //Filter proxies on positive keys - if (key < 0) - return; - //Merge the new key into the sorted list. - //float32* p = std::lower_bound(m_querySortKeys,m_querySortKeys+m_queryResultCount,key); - float[] querySortKeysPtr = _querySortKeys; - - int ip = 0; - float p = querySortKeysPtr[ip]; - while (p < key && ip < _queryResultCount) - { - p = querySortKeysPtr[ip]; - ip++; - } - int i = ip; - if (maxCount == _queryResultCount && i == _queryResultCount) - return; - if (maxCount == _queryResultCount) - _queryResultCount--; - //std::copy_backward - for (int j = _queryResultCount + 1; j > i; --j) - { - _querySortKeys[j] = _querySortKeys[j - 1]; - _queryResults[j] = _queryResults[j - 1]; - } - _querySortKeys[i] = key; - _queryResults[i] = proxyId; - _queryResultCount++; - } -#endif - - private static int BinarySearch(Bound[] bounds, int count, ushort value) - { - int low = 0; - int high = count - 1; - while (low <= high) - { - int mid = (low + high) >> 1; - if (bounds[mid].Value > value) - { - high = mid - 1; - } - else if (bounds[mid].Value < value) - { - low = mid + 1; - } - else - { - return (ushort)mid; - } - } - - return low; - } - } -} diff --git a/src/Box2DNet/Collision/Collision.CollideCircle.cs b/src/Box2DNet/Collision/Collision.CollideCircle.cs deleted file mode 100644 index f14a865..0000000 --- a/src/Box2DNet/Collision/Collision.CollideCircle.cs +++ /dev/null @@ -1,156 +0,0 @@ -/* - Box2DNet Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version 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 Box2DNet.Common; -using System.Numerics; - -using Transform = Box2DNet.Common.Transform; -using System.Numerics; -namespace Box2DNet.Collision -{ - public partial class Collision - { - public static void CollideCircles(ref Manifold manifold, CircleShape circle1, Transform xf1, CircleShape circle2, Transform xf2) - { - manifold.PointCount = 0; - - Vector2 p1 = xf1.TransformPoint(circle1._position); - Vector2 p2 = xf2.TransformPoint(circle2._position); - - Vector2 d = p2 - p1; - float distSqr = Vector2.Dot(d, d); - float radius = circle1._radius + circle2._radius; - if (distSqr > radius * radius) - { - return; - } - - manifold.Type = ManifoldType.Circles; - manifold.LocalPoint = circle1._position; - manifold.LocalPlaneNormal = Vector2.Zero; - manifold.PointCount = 1; - - manifold.Points[0].LocalPoint = circle2._position; - manifold.Points[0].ID.Key = 0; - } - - public static void CollidePolygonAndCircle(ref Manifold manifold, PolygonShape polygon, Transform xf1, CircleShape circle, Transform xf2) - { - manifold.PointCount = 0; - - // Compute circle position in the frame of the polygon. - Vector2 c = xf2.TransformPoint(circle._position); - Vector2 cLocal = xf1.InverseTransformPoint(c); - - // Find the min separating edge. - int normalIndex = 0; - float separation = -Settings.FLT_MAX; - float radius = polygon._radius + circle._radius; - int vertexCount = polygon._vertexCount; - Vector2[] vertices = polygon._vertices; - Vector2[] normals = polygon._normals; - - for (int i = 0; i < vertexCount; ++i) - { - float s = Vector2.Dot(normals[i], cLocal - vertices[i]); - if (s > radius) - { - // Early out. - return; - } - - if (s > separation) - { - separation = s; - normalIndex = i; - } - } - - // Vertices that subtend the incident face. - int vertIndex1 = normalIndex; - int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0; - Vector2 v1 = vertices[vertIndex1]; - Vector2 v2 = vertices[vertIndex2]; - - // If the center is inside the polygon ... - if (separation < Common.Settings.FLT_EPSILON) - { - manifold.PointCount = 1; - manifold.Type = ManifoldType.FaceA; - manifold.LocalPlaneNormal = normals[normalIndex]; - manifold.LocalPoint = 0.5f * (v1 + v2); - manifold.Points[0].LocalPoint = circle._position; - manifold.Points[0].ID.Key = 0; - return; - } - - // Compute barycentric coordinates - float u1 = Vector2.Dot(cLocal - v1, v2 - v1); - float u2 = Vector2.Dot(cLocal - v2, v1 - v2); - if (u1 <= 0.0f) - { - if ((cLocal - v1).LengthSquared() > radius * radius) - { - return; - } - - manifold.PointCount = 1; - manifold.Type = ManifoldType.FaceA; - manifold.LocalPlaneNormal = cLocal - v1; - manifold.LocalPlaneNormal.Normalize(); - manifold.LocalPoint = v1; - manifold.Points[0].LocalPoint = circle._position; - manifold.Points[0].ID.Key = 0; - } - else if (u2 <= 0.0f) - { - if ((cLocal - v2).LengthSquared() > radius * radius) - { - return; - } - - manifold.PointCount = 1; - manifold.Type = ManifoldType.FaceA; - manifold.LocalPlaneNormal = cLocal - v2; - manifold.LocalPlaneNormal.Normalize(); - manifold.LocalPoint = v2; - manifold.Points[0].LocalPoint = circle._position; - manifold.Points[0].ID.Key = 0; - } - else - { - Vector2 faceCenter = 0.5f * (v1 + v2); - float separation_ = Vector2.Dot(cLocal - faceCenter, normals[vertIndex1]); - if (separation_ > radius) - { - return; - } - - manifold.PointCount = 1; - manifold.Type = ManifoldType.FaceA; - manifold.LocalPlaneNormal = normals[vertIndex1]; - manifold.LocalPoint = faceCenter; - manifold.Points[0].LocalPoint = circle._position; - manifold.Points[0].ID.Key = 0; - } - } - } -} \ No newline at end of file diff --git a/src/Box2DNet/Collision/Collision.CollideEdge.cs b/src/Box2DNet/Collision/Collision.CollideEdge.cs deleted file mode 100644 index 2560a3e..0000000 --- a/src/Box2DNet/Collision/Collision.CollideEdge.cs +++ /dev/null @@ -1,103 +0,0 @@ -/* - Box2DNet Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version 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 Box2DNet.Common; - - -using Transform = Box2DNet.Common.Transform; -using System.Numerics; -namespace Box2DNet.Collision -{ - public partial class Collision - { - // This implements 2-sided edge vs circle collision. - public static void CollideEdgeAndCircle(ref Manifold manifold, EdgeShape edge, Transform transformA, CircleShape circle, Transform transformB) - { - manifold.PointCount = 0; - Vector2 cLocal = Common.Math.MulT(transformA, Common.Math.Mul(transformB, circle._position)); - Vector2 normal = edge._normal; - Vector2 v1 = edge._v1; - Vector2 v2 = edge._v2; - float radius = edge._radius + circle._radius; - - // Barycentric coordinates - float u1 = Vector2.Dot(cLocal - v1, v2 - v1); - float u2 = Vector2.Dot(cLocal - v2, v1 - v2); - - if (u1 <= 0.0f) - { - // Behind v1 - if ((cLocal- v1).LengthSquared() > radius * radius) - { - return; - } - - manifold.PointCount = 1; - manifold.Type = ManifoldType.FaceA; - manifold.LocalPlaneNormal = cLocal - v1; - manifold.LocalPlaneNormal.Normalize(); - manifold.LocalPoint = v1; - manifold.Points[0].LocalPoint = circle._position; - manifold.Points[0].ID.Key = 0; - } - else if (u2 <= 0.0f) - { - // Ahead of v2 - if ((cLocal- v2).LengthSquared() > radius * radius) - { - return; - } - - manifold.PointCount = 1; - manifold.Type = ManifoldType.FaceA; - manifold.LocalPlaneNormal = cLocal - v2; - manifold.LocalPlaneNormal.Normalize(); - manifold.LocalPoint = v2; - manifold.Points[0].LocalPoint = circle._position; - manifold.Points[0].ID.Key = 0; - } - else - { - float separation = Vector2.Dot(cLocal - v1, normal); - if (separation < -radius || radius < separation) - { - return; - } - - manifold.PointCount = 1; - manifold.Type = ManifoldType.FaceA; - manifold.LocalPlaneNormal = separation < 0.0f ? -normal : normal; - manifold.LocalPoint = 0.5f * (v1 + v2); - manifold.Points[0].LocalPoint = circle._position; - manifold.Points[0].ID.Key = 0; - } - } - - // Polygon versus 2-sided edge. - public static void CollidePolyAndEdge(ref Manifold manifold, PolygonShape polygon, Transform TransformA, EdgeShape edge, Transform TransformB) - { - PolygonShape polygonB = new PolygonShape(); - polygonB.SetAsEdge(edge._v1, edge._v2); - - CollidePolygons(ref manifold, polygon, TransformA, polygonB, TransformB); - } - } -} diff --git a/src/Box2DNet/Collision/Collision.CollidePoly.cs b/src/Box2DNet/Collision/Collision.CollidePoly.cs deleted file mode 100644 index b93c78a..0000000 --- a/src/Box2DNet/Collision/Collision.CollidePoly.cs +++ /dev/null @@ -1,311 +0,0 @@ -/* - Box2DNet Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version 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 Box2DNet.Common; - -using System.Numerics; -using Transform = Box2DNet.Common.Transform; -using System.Numerics; -namespace Box2DNet.Collision -{ - public partial class Collision - { - /// - /// Find the separation between poly1 and poly2 for a give edge normal on poly1. - /// - public static float EdgeSeparation(PolygonShape poly1, Transform xf1, int edge1, PolygonShape poly2, Transform xf2) - { - int count1 = poly1._vertexCount; - Vector2[] vertices1 = poly1._vertices; - Vector2[] normals1 = poly1._normals; - - int count2 = poly2._vertexCount; - Vector2[] vertices2 = poly2._vertices; - - Box2DNetDebug.Assert(0 <= edge1 && edge1 < count1); - - // Convert normal from poly1's frame into poly2's frame. - Vector2 normal1World = xf1.TransformDirection(normals1[edge1]); - Vector2 normal1 = xf2.InverseTransformDirection(normal1World); - - // Find support vertex on poly2 for -normal. - int index = 0; - float minDot = Common.Settings.FLT_MAX; - for (int i = 0; i < count2; ++i) - { - float dot = Vector2.Dot(vertices2[i], normal1); - if (dot < minDot) - { - minDot = dot; - index = i; - } - } - - Vector2 v1 = xf1.TransformPoint(vertices1[edge1]); - Vector2 v2 = xf2.TransformPoint(vertices2[index]); - float separation = Vector2.Dot(v2 - v1, normal1World); - return separation; - } - - /// - /// Find the max separation between poly1 and poly2 using edge normals from poly1. - /// - public static float FindMaxSeparation(ref int edgeIndex, PolygonShape poly1, Transform xf1, PolygonShape poly2, Transform xf2) - { - int count1 = poly1._vertexCount; - Vector2[] normals1 = poly1._normals; - - // Vector pointing from the centroid of poly1 to the centroid of poly2. - Vector2 d = xf2.TransformPoint(poly2._centroid) - xf1.TransformPoint(poly2._centroid); - Vector2 dLocal1 = xf1.InverseTransformDirection(d); - - // Find edge normal on poly1 that has the largest projection onto d. - int edge = 0; - float maxDot = -Common.Settings.FLT_MAX; - for (int i = 0; i < count1; ++i) - { - float dot = Vector2.Dot(normals1[i], dLocal1); - if (dot > maxDot) - { - maxDot = dot; - edge = i; - } - } - - // Get the separation for the edge normal. - float s = Collision.EdgeSeparation(poly1, xf1, edge, poly2, xf2); - - // Check the separation for the previous edge normal. - int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1; - float sPrev = Collision.EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2); - - // Check the separation for the next edge normal. - int nextEdge = edge + 1 < count1 ? edge + 1 : 0; - float sNext = Collision.EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2); - - // Find the best edge and the search direction. - int bestEdge; - float bestSeparation; - int increment; - if (sPrev > s && sPrev > sNext) - { - increment = -1; - bestEdge = prevEdge; - bestSeparation = sPrev; - } - else if (sNext > s) - { - increment = 1; - bestEdge = nextEdge; - bestSeparation = sNext; - } - else - { - edgeIndex = edge; - return s; - } - - // Perform a local search for the best edge normal. - for (; ; ) - { - if (increment == -1) - edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1; - else - edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0; - - s = Collision.EdgeSeparation(poly1, xf1, edge, poly2, xf2); - - if (s > bestSeparation) - { - bestEdge = edge; - bestSeparation = s; - } - else - { - break; - } - } - - edgeIndex = bestEdge; - return bestSeparation; - } - - public static void FindIncidentEdge(out ClipVertex[] c, PolygonShape poly1, Transform xf1, int edge1, PolygonShape poly2, Transform xf2) - { - int count1 = poly1._vertexCount; - Vector2[] normals1 = poly1._normals; - - int count2 = poly2._vertexCount; - Vector2[] vertices2 = poly2._vertices; - Vector2[] normals2 = poly2._normals; - - Box2DNetDebug.Assert(0 <= edge1 && edge1 < count1); - - // Get the normal of the reference edge in poly2's frame. - Vector2 normal1 = xf2.InverseTransformDirection( xf1.TransformDirection(normals1[edge1]) ); - - // Find the incident edge on poly2. - int index = 0; - float minDot = Settings.FLT_MAX; - for (int i = 0; i < count2; ++i) - { - float dot = Vector2.Dot(normal1, normals2[i]); - if (dot < minDot) - { - minDot = dot; - index = i; - } - } - - // Build the clip vertices for the incident edge. - int i1 = index; - int i2 = i1 + 1 < count2 ? i1 + 1 : 0; - - c = new ClipVertex[2]; - - c[0].V = Common.Math.Mul(xf2, vertices2[i1]); - c[0].ID.Features.ReferenceEdge = (byte)edge1; - c[0].ID.Features.IncidentEdge = (byte)i1; - c[0].ID.Features.IncidentVertex = 0; - - c[1].V = Common.Math.Mul(xf2, vertices2[i2]); - c[1].ID.Features.ReferenceEdge = (byte)edge1; - c[1].ID.Features.IncidentEdge = (byte)i2; - c[1].ID.Features.IncidentVertex = 1; - } - - // Find edge normal of max separation on A - return if separating axis is found - // Find edge normal of max separation on B - return if separation axis is found - // Choose reference edge as min(minA, minB) - // Find incident edge - // Clip - // The normal points from 1 to 2 - public static void CollidePolygons(ref Manifold manifold, - PolygonShape polyA, Transform xfA, PolygonShape polyB, Transform xfB) - { - manifold.PointCount = 0; - float totalRadius = polyA._radius + polyB._radius; - - int edgeA = 0; - float separationA = Collision.FindMaxSeparation(ref edgeA, polyA, xfA, polyB, xfB); - if (separationA > totalRadius) - return; - - int edgeB = 0; - float separationB = Collision.FindMaxSeparation(ref edgeB, polyB, xfB, polyA, xfA); - if (separationB > totalRadius) - return; - - PolygonShape poly1; // reference poly - PolygonShape poly2; // incident poly - Transform xf1, xf2; - int edge1; // reference edge - byte flip; - const float k_relativeTol = 0.98f; - const float k_absoluteTol = 0.001f; - - if (separationB > k_relativeTol * separationA + k_absoluteTol) - { - poly1 = polyB; - poly2 = polyA; - xf1 = xfB; - xf2 = xfA; - edge1 = edgeB; - manifold.Type = ManifoldType.FaceB; - flip = 1; - } - else - { - poly1 = polyA; - poly2 = polyB; - xf1 = xfA; - xf2 = xfB; - edge1 = edgeA; - manifold.Type = ManifoldType.FaceA; - flip = 0; - } - - ClipVertex[] incidentEdge; - Collision.FindIncidentEdge(out incidentEdge, poly1, xf1, edge1, poly2, xf2); - - int count1 = poly1._vertexCount; - Vector2[] vertices1 = poly1._vertices; - - Vector2 v11 = vertices1[edge1]; - Vector2 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0]; - - Vector2 dv = v12 - v11; - - Vector2 localNormal = dv.CrossScalarPostMultiply(1.0f); - localNormal.Normalize(); - Vector2 planePoint = 0.5f * (v11 + v12); - - Vector2 sideNormal = xf1.TransformDirection(v12 - v11); - sideNormal.Normalize(); - Vector2 frontNormal = sideNormal.CrossScalarPostMultiply(1.0f); - - v11 = Common.Math.Mul(xf1, v11); - v12 = Common.Math.Mul(xf1, v12); - - float frontOffset = Vector2.Dot(frontNormal, v11); - float sideOffset1 = -Vector2.Dot(sideNormal, v11); - float sideOffset2 = Vector2.Dot(sideNormal, v12); - - // Clip incident edge against extruded edge1 side edges. - ClipVertex[] clipPoints1; - ClipVertex[] clipPoints2; - int np; - - // Clip to box side 1 - np = Collision.ClipSegmentToLine(out clipPoints1, incidentEdge, -sideNormal, sideOffset1); - - if (np < 2) - return; - - // Clip to negative box side 1 - np = ClipSegmentToLine(out clipPoints2, clipPoints1, sideNormal, sideOffset2); - - if (np < 2) - return; - - // Now clipPoints2 contains the clipped points. - manifold.LocalPlaneNormal = localNormal; - manifold.LocalPoint = planePoint; - - int pointCount = 0; - for (int i = 0; i < Settings.MaxManifoldPoints; ++i) - { - float separation = Vector2.Dot(frontNormal, clipPoints2[i].V) - frontOffset; - - if (separation <= totalRadius) - { - ManifoldPoint cp = manifold.Points[pointCount]; - cp.LocalPoint = xf2.InverseTransformPoint(clipPoints2[i].V); - cp.ID = clipPoints2[i].ID; - cp.ID.Features.Flip = flip; - ++pointCount; - } - } - - manifold.PointCount = pointCount; - } - } -} diff --git a/src/Box2DNet/Collision/Collision.Distance.cs b/src/Box2DNet/Collision/Collision.Distance.cs deleted file mode 100644 index bfa903c..0000000 --- a/src/Box2DNet/Collision/Collision.Distance.cs +++ /dev/null @@ -1,774 +0,0 @@ -/* - Box2DNet Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version 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. -*/ - -#define DEBUG -#undef ALLOWSAFE - -using System; using System.Numerics; -using Box2DNet.Common; - - -using Transform = Box2DNet.Common.Transform; - -namespace Box2DNet.Collision -{ - /// - /// Used to warm start Distance. - /// Set count to zero on first call. - /// - public struct SimplexCache - { - /// - /// Length or area. - /// - public Single Metric; - public UInt16 Count; - /// - /// Vertices on shape A. - /// - //public Byte[/*3*/] IndexA; - public IndexArray IndexA; - /// - /// Vertices on shape B. - /// - //public Byte[/*3*/] IndexB; - public IndexArray IndexB; - - //public SimplexCache(byte init) - //{ - // Metric = 0; - // Count = 0; - // IndexA = new Byte[3]; - // IndexB = new Byte[3]; - //} - } - - public struct IndexArray - { - private Byte I0, I1, I2; - - public Byte this[int index] - { - get - { -#if DEBUG - Box2DNetDebug.Assert(index >= 0 && index < 3); -#endif - if (index == 0) return I0; - else if (index == 1) return I1; - else return I2; - } - set - { -#if DEBUG - Box2DNetDebug.Assert(index >= 0 && index < 3); -#endif - if (index == 0) I0 = value; - else if (index == 1) I1 = value; - else I2 = value; - } - } - } - - /// - /// Input for Distance. - /// You have to option to use the shape radii - /// in the computation. - /// - public struct DistanceInput - { - public Transform TransformA; - public Transform TransformB; - public bool UseRadii; - } - - /// - /// Output for Distance. - /// - public struct DistanceOutput - { - /// - /// Closest point on shapeA. - /// - public Vector2 PointA; - /// - /// Closest point on shapeB. - /// - public Vector2 PointB; - public float Distance; - /// - /// Number of GJK iterations used. - /// - public int Iterations; - } - - // GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates. - - internal struct SimplexVertex - { - internal Vector2 wA; // support point in shapeA - internal Vector2 wB; // support point in shapeB - internal Vector2 w; // wB - wA - internal float a; // barycentric coordinate for closest point - internal int indexA; // wA index - internal int indexB; // wB index - } - - internal struct Simplex - { - internal SimplexVertex _v1, _v2, _v3; - internal int _count; - -#if ALLOWUNSAFE - internal unsafe void ReadCache(SimplexCache* cache, Shape shapeA, Transform TransformA, Shape shapeB, Transform TransformB) - { - Box2DNetDebug.Assert(0 <= cache->Count && cache->Count <= 3); - - // Copy data from cache. - _count = cache->Count; - SimplexVertex** vertices = stackalloc SimplexVertex*[3]; - fixed (SimplexVertex* v1Ptr = &_v1, v2Ptr = &_v2, v3Ptr = &_v3) - { - vertices[0] = v1Ptr; - vertices[1] = v2Ptr; - vertices[2] = v3Ptr; - for (int i = 0; i < _count; ++i) - { - SimplexVertex* v = vertices[i]; - v->indexA = cache->IndexA[i]; - v->indexB = cache->IndexB[i]; - Vector2 wALocal = shapeA.GetVertex(v->indexA); - Vector2 wBLocal = shapeB.GetVertex(v->indexB); - v->wA = TransformA.TransformPoint(wALocal); - v->wB = TransformB.TransformPoint(wBLocal); - v->w = v->wB - v->wA; - v->a = 0.0f; - } - - // Compute the new simplex metric, if it is substantially different than - // old metric then flush the simplex. - if (_count > 1) - { - float metric1 = cache->Metric; - float metric2 = GetMetric(); - if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Common.Settings.FLT_EPSILON) - { - // Reset the simplex. - _count = 0; - } - } - - // If the cache is empty or invalid ... - if (_count == 0) - { - SimplexVertex* v = vertices[0]; - v->indexA = 0; - v->indexB = 0; - Vector2 wALocal = shapeA.GetVertex(0); - Vector2 wBLocal = shapeB.GetVertex(0); - v->wA = TransformA.TransformPoint(wALocal); - v->wB = TransformB.TransformPoint(wBLocal); - v->w = v->wB - v->wA; - _count = 1; - } - } - } - - internal unsafe void WriteCache(SimplexCache* cache) - { - cache->Metric = GetMetric(); - cache->Count = (UInt16)_count; - SimplexVertex** vertices = stackalloc SimplexVertex*[3]; - fixed (SimplexVertex* v1Ptr = &_v1, v2Ptr = &_v2, v3Ptr = &_v3) - { - vertices[0] = v1Ptr; - vertices[1] = v2Ptr; - vertices[2] = v3Ptr; - for (int i = 0; i < _count; ++i) - { - cache->IndexA[i] = (Byte)(vertices[i]->indexA); - cache->IndexB[i] = (Byte)(vertices[i]->indexB); - } - } - } -#else // ALLOWUNSAFE - - internal void ReadCache(SimplexCache cache, Shape shapeA, Transform transformA, Shape shapeB, Transform transformB) - { - Box2DNetDebug.Assert(0 <= cache.Count && cache.Count <= 3); - - // Copy data from cache. - _count = cache.Count; - SimplexVertex[] vertices = new SimplexVertex[] { _v1, _v2, _v3 }; - for (int i = 0; i < _count; ++i) - { - SimplexVertex v = vertices[i]; - v.indexA = cache.IndexA[i]; - v.indexB = cache.IndexB[i]; - Vector2 wALocal = shapeA.GetVertex(v.indexA); - Vector2 wBLocal = shapeB.GetVertex(v.indexB); - v.wA = transformA.TransformPoint(wALocal); - v.wB = transformB.TransformPoint(wBLocal); - v.w = v.wB - v.wA; - v.a = 0.0f; - } - - // Compute the new simplex metric, if it is substantially different than - // old metric then flush the simplex. - if (_count > 1) - { - float metric1 = cache.Metric; - float metric2 = GetMetric(); - if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Common.Settings.FLT_EPSILON) - { - // Reset the simplex. - _count = 0; - } - } - - // If the cache is empty or invalid ... - if (_count == 0) - { - SimplexVertex v = vertices[0]; - v.indexA = 0; - v.indexB = 0; - Vector2 wALocal = shapeA.GetVertex(0); - Vector2 wBLocal = shapeB.GetVertex(0); - v.wA = transformA.TransformPoint(wALocal); - v.wB = transformB.TransformPoint(wBLocal); - v.w = v.wB - v.wA; - _count = 1; - } - } - - internal void WriteCache(SimplexCache cache) - { - cache.Metric = GetMetric(); - cache.Count = (UInt16)_count; - SimplexVertex[] vertices = new SimplexVertex[] { _v1, _v2, _v3 }; - for (int i = 0; i < _count; ++i) - { - cache.IndexA[i] = (Byte)(vertices[i].indexA); - cache.IndexB[i] = (Byte)(vertices[i].indexB); - } - } -#endif // ALLOWUNSAFE - - internal Vector2 GetClosestPoint() - { - switch (_count) - { - case 0: -#if DEBUG - Box2DNetDebug.Assert(false); -#endif - return Vector2.Zero; - case 1: - return _v1.w; - case 2: - return _v1.a * _v1.w + _v2.a * _v2.w; - case 3: - return Vector2.Zero; - default: -#if DEBUG - Box2DNetDebug.Assert(false); -#endif - return Vector2.Zero; - } - } - -#if ALLOWUNSAFE - internal unsafe void GetWitnessPoints(Vector2* pA, Vector2* pB) - { - switch (_count) - { - case 0: - Box2DNetDebug.Assert(false); - break; - - case 1: - *pA = _v1.wA; - *pB = _v1.wB; - break; - - case 2: - *pA = _v1.a * _v1.wA + _v2.a * _v2.wA; - *pB = _v1.a * _v1.wB + _v2.a * _v2.wB; - break; - - case 3: - *pA = _v1.a * _v1.wA + _v2.a * _v2.wA + _v3.a * _v3.wA; - *pB = *pA; - break; - - default: - Box2DNetDebug.Assert(false); - break; - } - } -#else - internal void GetWitnessPoints(ref Vector2 pA, ref Vector2 pB) - { - switch (_count) - { - case 0: - Box2DNetDebug.Assert(false); - break; - - case 1: - pA = _v1.wA; - pB = _v1.wB; - break; - - case 2: - pA = _v1.a * _v1.wA + _v2.a * _v2.wA; - pB = _v1.a * _v1.wB + _v2.a * _v2.wB; - break; - - case 3: - pA = _v1.a * _v1.wA + _v2.a * _v2.wA + _v3.a * _v3.wA; - pB = pA; - break; - - default: - Box2DNetDebug.Assert(false); - break; - } - } - -#endif // ALLOWUNSAFE - - internal float GetMetric() - { - switch (_count) - { - case 0: -#if DEBUG - Box2DNetDebug.Assert(false); -#endif - return 0.0f; - - case 1: - return 0.0f; - - case 2: - return (_v1.w - _v2.w).Length(); - - case 3: - return (_v2.w - _v1.w).Cross(_v3.w - _v1.w); - - default: -#if DEBUG - Box2DNetDebug.Assert(false); -#endif - return 0.0f; - } - } - - // Solve a line segment using barycentric coordinates. - // - // p = a1 * w1 + a2 * w2 - // a1 + a2 = 1 - // - // The vector from the origin to the closest point on the line is - // perpendicular to the line. - // e12 = w2 - w1 - // dot(p, e) = 0 - // a1 * dot(w1, e) + a2 * dot(w2, e) = 0 - // - // 2-by-2 linear system - // [1 1 ][a1] = [1] - // [w1.e12 w2.e12][a2] = [0] - // - // Define - // d12_1 = dot(w2, e12) - // d12_2 = -dot(w1, e12) - // d12 = d12_1 + d12_2 - // - // Solution - // a1 = d12_1 / d12 - // a2 = d12_2 / d12 - internal void Solve2() - { - Vector2 w1 = _v1.w; - Vector2 w2 = _v2.w; - Vector2 e12 = w2 - w1; - - // w1 region - float d12_2 = -Vector2.Dot(w1, e12); - if (d12_2 <= 0.0f) - { - // a2 <= 0, so we clamp it to 0 - _v1.a = 1.0f; - _count = 1; - return; - } - - // w2 region - float d12_1 = Vector2.Dot(w2, e12); - if (d12_1 <= 0.0f) - { - // a1 <= 0, so we clamp it to 0 - _v2.a = 1.0f; - _count = 1; - _v1 = _v2; - return; - } - - // Must be in e12 region. - float inv_d12 = 1.0f / (d12_1 + d12_2); - _v1.a = d12_1 * inv_d12; - _v2.a = d12_2 * inv_d12; - _count = 2; - } - - // Possible regions: - // - points[2] - // - edge points[0]-points[2] - // - edge points[1]-points[2] - // - inside the triangle - internal void Solve3() - { - Vector2 w1 = _v1.w; - Vector2 w2 = _v2.w; - Vector2 w3 = _v3.w; - - // Edge12 - // [1 1 ][a1] = [1] - // [w1.e12 w2.e12][a2] = [0] - // a3 = 0 - Vector2 e12 = w2 - w1; - float w1e12 = Vector2.Dot(w1, e12); - float w2e12 = Vector2.Dot(w2, e12); - float d12_1 = w2e12; - float d12_2 = -w1e12; - - // Edge13 - // [1 1 ][a1] = [1] - // [w1.e13 w3.e13][a3] = [0] - // a2 = 0 - Vector2 e13 = w3 - w1; - float w1e13 = Vector2.Dot(w1, e13); - float w3e13 = Vector2.Dot(w3, e13); - float d13_1 = w3e13; - float d13_2 = -w1e13; - - // Edge23 - // [1 1 ][a2] = [1] - // [w2.e23 w3.e23][a3] = [0] - // a1 = 0 - Vector2 e23 = w3 - w2; - float w2e23 = Vector2.Dot(w2, e23); - float w3e23 = Vector2.Dot(w3, e23); - float d23_1 = w3e23; - float d23_2 = -w2e23; - - // Triangle123 - float n123 = e12.Cross(e13); - - float d123_1 = n123 * w2.Cross(w3); - float d123_2 = n123 * w3.Cross(w1); - float d123_3 = n123 * w1.Cross(w2); - - // w1 region - if (d12_2 <= 0.0f && d13_2 <= 0.0f) - { - _v1.a = 1.0f; - _count = 1; - return; - } - - // e12 - if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f) - { - float inv_d12 = 1.0f / (d12_1 + d12_2); - _v1.a = d12_1 * inv_d12; - _v2.a = d12_1 * inv_d12; - _count = 2; - return; - } - - // e13 - if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f) - { - float inv_d13 = 1.0f / (d13_1 + d13_2); - _v1.a = d13_1 * inv_d13; - _v3.a = d13_2 * inv_d13; - _count = 2; - _v2 = _v3; - return; - } - - // w2 region - if (d12_1 <= 0.0f && d23_2 <= 0.0f) - { - _v2.a = 1.0f; - _count = 1; - _v1 = _v2; - return; - } - - // w3 region - if (d13_1 <= 0.0f && d23_1 <= 0.0f) - { - _v3.a = 1.0f; - _count = 1; - _v1 = _v3; - return; - } - - // e23 - if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f) - { - float inv_d23 = 1.0f / (d23_1 + d23_2); - _v2.a = d23_1 * inv_d23; - _v3.a = d23_2 * inv_d23; - _count = 2; - _v1 = _v3; - return; - } - - // Must be in triangle123 - float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3); - _v1.a = d123_1 * inv_d123; - _v2.a = d123_2 * inv_d123; - _v3.a = d123_3 * inv_d123; - _count = 3; - } - } - - public partial class Collision - { - /// - /// Compute the closest points between two shapes. Supports any combination of: - /// CircleShape, PolygonShape, EdgeShape. The simplex cache is input/output. - /// On the first call set SimplexCache.Count to zero. - /// - public -#if ALLOWUNSAFE - unsafe -#endif // ALLOWUNSAFE - static void Distance(out DistanceOutput output, ref SimplexCache cache, ref DistanceInput input, Shape shapeA, Shape shapeB) - { - output = new DistanceOutput(); - - Transform transformA = input.TransformA; - Transform transformB = input.TransformB; - - // Initialize the simplex. - Simplex simplex = new Simplex(); -#if ALLOWUNSAFE - fixed (SimplexCache* sPtr = &cache) - { - simplex.ReadCache(sPtr, shapeA, transformA, shapeB, transformB); - } -#else - simplex.ReadCache(cache, shapeA, transformA, shapeB, transformB); -#endif - - // Get simplex vertices as an array. -#if ALLOWUNSAFE - SimplexVertex* vertices = &simplex._v1; -#else - SimplexVertex[] vertices = new SimplexVertex[] { simplex._v1, simplex._v2, simplex._v3 }; -#endif - - // These store the vertices of the last simplex so that we - // can check for duplicates and prevent cycling. -#if ALLOWUNSAFE - int* lastA = stackalloc int[4], lastB = stackalloc int[4]; -#else - int[] lastA = new int[4]; - int[] lastB = new int[4]; -#endif // ALLOWUNSAFE - int lastCount; - - // Main iteration loop. - int iter = 0; - const int k_maxIterationCount = 20; - while (iter < k_maxIterationCount) - { - // Copy simplex so we can identify duplicates. - lastCount = simplex._count; - int i; - for (i = 0; i < lastCount; ++i) - { - lastA[i] = vertices[i].indexA; - lastB[i] = vertices[i].indexB; - } - - switch (simplex._count) - { - case 1: - break; - - case 2: - simplex.Solve2(); - break; - - case 3: - simplex.Solve3(); - break; - - default: -#if DEBUG - Box2DNetDebug.Assert(false); -#endif - break; - } - - // If we have 3 points, then the origin is in the corresponding triangle. - if (simplex._count == 3) - { - break; - } - - // Compute closest point. - Vector2 p = simplex.GetClosestPoint(); - float distanceSqr = p.LengthSquared(); - - // Ensure the search direction is numerically fit. - if (distanceSqr < Common.Settings.FLT_EPSILON_SQUARED) - { - // The origin is probably contained by a line segment - // or triangle. Thus the shapes are overlapped. - - // We can't return zero here even though there may be overlap. - // In case the simplex is a point, segment, or triangle it is difficult - // to determine if the origin is contained in the CSO or very close to it. - break; - } - - // Compute a tentative new simplex vertex using support points. -#if ALLOWUNSAFE - SimplexVertex* vertex = vertices + simplex._count; - vertex->indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p)); - vertex->wA = transformA.TransformPoint(shapeA.GetVertex(vertex->indexA)); - //Vec2 wBLocal; - vertex->indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p)); - vertex->wB = transformB.TransformPoint(shapeB.GetVertex(vertex->indexB)); - vertex->w = vertex->wB - vertex->wA; -#else - SimplexVertex vertex = vertices[simplex._count - 1]; - vertex.indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p)); - vertex.wA = transformA.TransformPoint(shapeA.GetVertex(vertex.indexA)); - //Vec2 wBLocal; - vertex.indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p)); - vertex.wB = transformB.TransformPoint(shapeB.GetVertex(vertex.indexB)); - vertex.w = vertex.wB - vertex.wA; -#endif // ALLOWUNSAFE - - // Iteration count is equated to the number of support point calls. - ++iter; - - // Check for convergence. -#if ALLOWUNSAFE - float lowerBound = Vector2.Dot(p, vertex->w); -#else - float lowerBound = Vector2.Dot(p, vertex.w); -#endif - float upperBound = distanceSqr; - const float k_relativeTolSqr = 0.01f * 0.01f; // 1:100 - if (upperBound - lowerBound <= k_relativeTolSqr * upperBound) - { - // Converged! - break; - } - - // Check for duplicate support points. - bool duplicate = false; - for (i = 0; i < lastCount; ++i) - { -#if ALLOWUNSAFE - if (vertex->indexA == lastA[i] && vertex->indexB == lastB[i]) -#else - if (vertex.indexA == lastA[i] && vertex.indexB == lastB[i]) -#endif - { - duplicate = true; - break; - } - } - - // If we found a duplicate support point we must exit to avoid cycling. - if (duplicate) - { - break; - } - - // New vertex is ok and needed. - ++simplex._count; - } - - -#if ALLOWUNSAFE - fixed (DistanceOutput* doPtr = &output) - { - // Prepare output. - simplex.GetWitnessPoints(&doPtr->PointA, &doPtr->PointB); - doPtr->Distance = Vector2.Distance(doPtr->PointA, doPtr->PointB); - doPtr->Iterations = iter; - } - - fixed (SimplexCache* sPtr = &cache) - { - // Cache the simplex. - simplex.WriteCache(sPtr); - } -#else - // Prepare output. - simplex.GetWitnessPoints(ref output.PointA, ref output.PointB); - output.Distance = Box2DNet.Common.Math.Distance(output.PointA, output.PointB); - output.Iterations = iter; - - // Cache the simplex. - simplex.WriteCache(cache); -#endif - - // Apply radii if requested. - if (input.UseRadii) - { - float rA = shapeA._radius; - float rB = shapeB._radius; - - if (output.Distance > rA + rB && output.Distance > Common.Settings.FLT_EPSILON) - { - // Shapes are still no overlapped. - // Move the witness points to the outer surface. - output.Distance -= rA + rB; - Vector2 normal = output.PointB - output.PointA; - normal.Normalize(); - output.PointA += rA * normal; - output.PointB -= rB * normal; - } - else - { - // Shapes are overlapped when radii are considered. - // Move the witness points to the middle. - Vector2 p = 0.5f * (output.PointA + output.PointB); - output.PointA = p; - output.PointB = p; - output.Distance = 0.0f; - } - } - } - } -} \ No newline at end of file diff --git a/src/Box2DNet/Collision/Collision.TimeOfImpact.cs b/src/Box2DNet/Collision/Collision.TimeOfImpact.cs deleted file mode 100644 index 4c59f33..0000000 --- a/src/Box2DNet/Collision/Collision.TimeOfImpact.cs +++ /dev/null @@ -1,450 +0,0 @@ -/* - Box2DNet Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version 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.Numerics; -using Box2DNet.Common; - - -using Transform = Box2DNet.Common.Transform; - -namespace Box2DNet.Collision -{ - /// - /// Inpute parameters for TimeOfImpact - /// - public struct TOIInput - { - public Sweep SweepA; - public Sweep SweepB; - public float SweepRadiusA; - public float SweepRadiusB; - public float Tolerance; - } - - internal struct SeparationFunction - { - internal enum Type - { - Points, - FaceA, - FaceB - }; - -#if ALLOWUNSAFE - internal unsafe void Initialize(SimplexCache* cache, - Shape shapeA, Transform TransformA, - Shape shapeB, Transform TransformB) - { - ShapeA = shapeA; - ShapeB = shapeB; - int count = cache->Count; - Box2DNetDebug.Assert(0 < count && count < 3); - - if (count == 1) - { - FaceType = Type.Points; - Vector2 localPointA = ShapeA.GetVertex(cache->IndexA[0]); - Vector2 localPointB = ShapeB.GetVertex(cache->IndexB[0]); - Vector2 pointA = TransformA.TransformPoint(localPointA); - Vector2 pointB = TransformB.TransformPoint(localPointB); - Axis = pointB - pointA; - Axis.Normalize(); - } - else if (cache->IndexB[0] == cache->IndexB[1]) - { - // Two points on A and one on B - FaceType = Type.FaceA; - Vector2 localPointA1 = ShapeA.GetVertex(cache->IndexA[0]); - Vector2 localPointA2 = ShapeA.GetVertex(cache->IndexA[1]); - Vector2 localPointB = ShapeB.GetVertex(cache->IndexB[0]); - LocalPoint = 0.5f * (localPointA1 + localPointA2); - Axis = (localPointA2 - localPointA1).CrossScalarPostMultiply(1.0f); - Axis.Normalize(); - - Vector2 normal = TransformA.TransformDirection(Axis); - Vector2 pointA = TransformA.TransformPoint(LocalPoint); - Vector2 pointB = TransformB.TransformPoint(localPointB); - - float s = Vector2.Dot(pointB - pointA, normal); - if (s < 0.0f) - { - Axis = -Axis; - } - } - else - { - // Two points on B and one or two points on A. - // We ignore the second point on A. - FaceType = Type.FaceB; - Vector2 localPointA = shapeA.GetVertex(cache->IndexA[0]); - Vector2 localPointB1 = shapeB.GetVertex(cache->IndexB[0]); - Vector2 localPointB2 = shapeB.GetVertex(cache->IndexB[1]); - LocalPoint = 0.5f * (localPointB1 + localPointB2); - Axis = (localPointB2 - localPointB1).CrossScalarPostMultiply(1.0f); - Axis.Normalize(); - - Vector2 normal = TransformB.TransformDirection(Axis); - Vector2 pointB = TransformB.TransformPoint(LocalPoint); - Vector2 pointA = TransformA.TransformPoint(localPointA); - - float s = Vector2.Dot(pointA - pointB, normal); - if (s < 0.0f) - { - Axis = -Axis; - } - } - } -#else - internal void Initialize(SimplexCache cache, Shape shapeA, Transform transformA, Shape shapeB, Transform transformB) - { - ShapeA = shapeA; - ShapeB = shapeB; - int count = cache.Count; - Box2DNetDebug.Assert(0 < count && count < 3); - - if (count == 1) - { - FaceType = Type.Points; - Vector2 localPointA = ShapeA.GetVertex(cache.IndexA[0]); - Vector2 localPointB = ShapeB.GetVertex(cache.IndexB[0]); - Vector2 pointA = transformA.TransformPoint(localPointA); - Vector2 pointB = transformB.TransformPoint(localPointB); - Axis = pointB - pointA; - Axis.Normalize(); - } - else if (cache.IndexB[0] == cache.IndexB[1]) - { - // Two points on A and one on B - FaceType = Type.FaceA; - Vector2 localPointA1 = ShapeA.GetVertex(cache.IndexA[0]); - Vector2 localPointA2 = ShapeA.GetVertex(cache.IndexA[1]); - Vector2 localPointB = ShapeB.GetVertex(cache.IndexB[0]); - LocalPoint = 0.5f * (localPointA1 + localPointA2); - Axis = (localPointA2 - localPointA1).CrossScalarPostMultiply(1.0f); - Axis.Normalize(); - - Vector2 normal = transformA.TransformDirection(Axis); - Vector2 pointA = transformA.TransformPoint(LocalPoint); - Vector2 pointB = transformB.TransformPoint(localPointB); - - float s = Vector2.Dot(pointB - pointA, normal); - if (s < 0.0f) - { - Axis = -Axis; - } - } - else - { - // Two points on B and one or two points on A. - // We ignore the second point on A. - FaceType = Type.FaceB; - Vector2 localPointA = shapeA.GetVertex(cache.IndexA[0]); - Vector2 localPointB1 = shapeB.GetVertex(cache.IndexB[0]); - Vector2 localPointB2 = shapeB.GetVertex(cache.IndexB[1]); - LocalPoint = 0.5f * (localPointB1 + localPointB2); - Axis = (localPointB2 - localPointB1).CrossScalarPostMultiply(1.0f); - Axis.Normalize(); - - Vector2 normal = transformB.TransformDirection(Axis); - Vector2 pointB = transformB.TransformPoint(LocalPoint); - Vector2 pointA = transformA.TransformPoint(localPointA); - - float s = Vector2.Dot(pointA - pointB, normal); - if (s < 0.0f) - { - Axis = -Axis; - } - } - } -#endif - - internal float Evaluate(Transform TransformA, Transform TransformB) - { - switch (FaceType) - { - case Type.Points: - { - Vector2 axisA = TransformA.InverseTransformDirection(Axis); - Vector2 axisB = TransformB.InverseTransformDirection(-Axis); - Vector2 localPointA = ShapeA.GetSupportVertex(axisA); - Vector2 localPointB = ShapeB.GetSupportVertex(axisB); - Vector2 pointA = TransformA.TransformPoint(localPointA); - Vector2 pointB = TransformB.TransformPoint(localPointB); - float separation = Vector2.Dot(pointB - pointA, Axis); - return separation; - } - - case Type.FaceA: - { - Vector2 normal = TransformA.TransformDirection(Axis); - Vector2 pointA = TransformA.TransformPoint(LocalPoint); - - Vector2 axisB = TransformB.InverseTransformDirection(-normal); - - Vector2 localPointB = ShapeB.GetSupportVertex(axisB); - Vector2 pointB = TransformB.TransformPoint(localPointB); - - float separation = Vector2.Dot(pointB - pointA, normal); - return separation; - } - - case Type.FaceB: - { - Vector2 normal = TransformB.TransformDirection(Axis); - Vector2 pointB = TransformB.TransformPoint(LocalPoint); - - Vector2 axisA = TransformA.InverseTransformDirection(-normal); - - Vector2 localPointA = ShapeA.GetSupportVertex(axisA); - Vector2 pointA = TransformA.TransformPoint(localPointA); - - float separation = Vector2.Dot(pointA - pointB, normal); - return separation; - } - - default: - Box2DNetDebug.Assert(false); - return 0.0f; - } - } - - internal Shape ShapeA; - internal Shape ShapeB; - internal Type FaceType; - internal Vector2 LocalPoint; - internal Vector2 Axis; - } - - public partial class Collision - { - public static int MaxToiIters; - public static int MaxToiRootIters; - - // CCD via the secant method. - /// - /// Compute the time when two shapes begin to touch or touch at a closer distance. - /// TOI considers the shape radii. It attempts to have the radii overlap by the tolerance. - /// Iterations terminate with the overlap is within 0.5 * tolerance. The tolerance should be - /// smaller than sum of the shape radii. - /// Warning the sweeps must have the same time interval. - /// - /// - /// The fraction between [0,1] in which the shapes first touch. - /// fraction=0 means the shapes begin touching/overlapped, and fraction=1 means the shapes don't touch. - /// - public static float TimeOfImpact(TOIInput input, Shape shapeA, Shape shapeB) - { - Sweep sweepA = input.SweepA; - Sweep sweepB = input.SweepB; - - Box2DNetDebug.Assert(sweepA.T0 == sweepB.T0); - Box2DNetDebug.Assert(1.0f - sweepA.T0 > Common.Settings.FLT_EPSILON); - - float radius = shapeA._radius + shapeB._radius; - float tolerance = input.Tolerance; - - float alpha = 0.0f; - - const int k_maxIterations = 1000; // TODO_ERIN b2Settings - int iter = 0; - float target = 0.0f; - - // Prepare input for distance query. - SimplexCache cache = new SimplexCache(); - cache.Count = 0; - DistanceInput distanceInput; - distanceInput.UseRadii = false; - - for (; ; ) - { - Transform xfA, xfB; - sweepA.GetTransform(out xfA, alpha); - sweepB.GetTransform(out xfB, alpha); - - // Get the distance between shapes. - distanceInput.TransformA = xfA; - distanceInput.TransformB = xfB; - DistanceOutput distanceOutput; - Distance(out distanceOutput, ref cache, ref distanceInput, shapeA, shapeB); - - if (distanceOutput.Distance <= 0.0f) - { - alpha = 1.0f; - break; - } - - SeparationFunction fcn = new SeparationFunction(); -#if ALLOWUNSAFE - unsafe - { - fcn.Initialize(&cache, shapeA, xfA, shapeB, xfB); - } -#else - fcn.Initialize(cache, shapeA, xfA, shapeB, xfB); -#endif - - float separation = fcn.Evaluate(xfA, xfB); - if (separation <= 0.0f) - { - alpha = 1.0f; - break; - } - - if (iter == 0) - { - // Compute a reasonable target distance to give some breathing room - // for conservative advancement. We take advantage of the shape radii - // to create additional clearance. - if (separation > radius) - { - target = Common.Math.Max(radius - tolerance, 0.75f * radius); - } - else - { - target = Common.Math.Max(separation - tolerance, 0.02f * radius); - } - } - - if (separation - target < 0.5f * tolerance) - { - if (iter == 0) - { - alpha = 1.0f; - break; - } - - break; - } - -#if _FALSE - // Dump the curve seen by the root finder - { - const int32 N = 100; - float32 dx = 1.0f / N; - float32 xs[N+1]; - float32 fs[N+1]; - - float32 x = 0.0f; - - for (int32 i = 0; i <= N; ++i) - { - sweepA.GetTransform(&xfA, x); - sweepB.GetTransform(&xfB, x); - float32 f = fcn.Evaluate(xfA, xfB) - target; - - printf("%g %g\n", x, f); - - xs[i] = x; - fs[i] = f; - - x += dx; - } - } -#endif - - // Compute 1D root of: f(x) - target = 0 - float newAlpha = alpha; - { - float x1 = alpha, x2 = 1.0f; - - float f1 = separation; - - sweepA.GetTransform(out xfA, x2); - sweepB.GetTransform(out xfB, x2); - float f2 = fcn.Evaluate(xfA, xfB); - - // If intervals don't overlap at t2, then we are done. - if (f2 >= target) - { - alpha = 1.0f; - break; - } - - // Determine when intervals intersect. - int rootIterCount = 0; - for (; ; ) - { - // Use a mix of the secant rule and bisection. - float x; - if ((rootIterCount & 1) != 0) - { - // Secant rule to improve convergence. - x = x1 + (target - f1) * (x2 - x1) / (f2 - f1); - } - else - { - // Bisection to guarantee progress. - x = 0.5f * (x1 + x2); - } - - sweepA.GetTransform(out xfA, x); - sweepB.GetTransform(out xfB, x); - - float f = fcn.Evaluate(xfA, xfB); - - if (Common.Math.Abs(f - target) < 0.025f * tolerance) - { - newAlpha = x; - break; - } - - // Ensure we continue to bracket the root. - if (f > target) - { - x1 = x; - f1 = f; - } - else - { - x2 = x; - f2 = f; - } - - ++rootIterCount; - - Box2DNetDebug.Assert(rootIterCount < 50); - } - - MaxToiRootIters = Common.Math.Max(MaxToiRootIters, rootIterCount); - } - - // Ensure significant advancement. - if (newAlpha < (1.0f + 100.0f * Common.Settings.FLT_EPSILON) * alpha) - { - break; - } - - alpha = newAlpha; - - ++iter; - - if (iter == k_maxIterations) - { - break; - } - } - - MaxToiIters = Common.Math.Max(MaxToiIters, iter); - - return alpha; - } - } -} \ No newline at end of file diff --git a/src/Box2DNet/Collision/Collision.cs b/src/Box2DNet/Collision/Collision.cs index a656e64..137dde9 100644 --- a/src/Box2DNet/Collision/Collision.cs +++ b/src/Box2DNet/Collision/Collision.cs @@ -1,625 +1,1863 @@ -/* - Box2DNet Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version 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.Numerics; -using Box2DNet.Common; - - -using Transform = Box2DNet.Common.Transform; - -namespace Box2DNet.Collision -{ - // Structures and functions used for computing contact points, distance - // queries, and TOI queries. - - public partial class Collision - { - public static readonly byte NullFeature = Common.Math.UCHAR_MAX; - - public static bool TestOverlap(AABB a, AABB b) - { - Vector2 d1, d2; - d1 = b.LowerBound - a.UpperBound; - d2 = a.LowerBound - b.UpperBound; - - if (d1.X > 0.0f || d1.Y > 0.0f) - return false; - - if (d2.X > 0.0f || d2.Y > 0.0f) - return false; - - return true; - } - - /// - /// Compute the point states given two manifolds. The states pertain to the transition from manifold1 - /// to manifold2. So state1 is either persist or remove while state2 is either add or persist. - /// - public static void GetPointStates(PointState[/*b2_maxManifoldPoints*/] state1, PointState[/*b2_maxManifoldPoints*/] state2, Manifold manifold1, Manifold manifold2) - { - for (int i = 0; i < Common.Settings.MaxManifoldPoints; ++i) - { - state1[i] = PointState.NullState; - state2[i] = PointState.NullState; - } - - // Detect persists and removes. - for (int i = 0; i < manifold1.PointCount; ++i) - { - ContactID id = manifold1.Points[i].ID; - - state1[i] = PointState.RemoveState; - - for (int j = 0; j < manifold2.PointCount; ++j) - { - if (manifold2.Points[j].ID.Key == id.Key) - { - state1[i] = PointState.PersistState; - break; - } - } - } - - // Detect persists and adds. - for (int i = 0; i < manifold2.PointCount; ++i) - { - ContactID id = manifold2.Points[i].ID; - - state2[i] = PointState.AddState; - - for (int j = 0; j < manifold1.PointCount; ++j) - { - if (manifold1.Points[j].ID.Key == id.Key) - { - state2[i] = PointState.PersistState; - break; - } - } - } - } - - // Sutherland-Hodgman clipping. - public static int ClipSegmentToLine(out ClipVertex[/*2*/] vOut, ClipVertex[/*2*/] vIn, Vector2 normal, float offset) - { - vOut = new ClipVertex[2]; - - // Start with no output points - int numOut = 0; - - // Calculate the distance of end points to the line - float distance0 = Vector2.Dot(normal, vIn[0].V) - offset; - float distance1 = Vector2.Dot(normal, vIn[1].V) - offset; - - // If the points are behind the plane - if (distance0 <= 0.0f) vOut[numOut++] = vIn[0]; - if (distance1 <= 0.0f) vOut[numOut++] = vIn[1]; - - // If the points are on different sides of the plane - if (distance0 * distance1 < 0.0f) - { - // Find intersection point of edge and plane - float interp = distance0 / (distance0 - distance1); - vOut[numOut].V = vIn[0].V + interp * (vIn[1].V - vIn[0].V); - if (distance0 > 0.0f) - { - vOut[numOut].ID = vIn[0].ID; - } - else - { - vOut[numOut].ID = vIn[1].ID; - } - ++numOut; - } - - return numOut; - } - } - - /// - /// The features that intersect to form the contact point. - /// - public struct Features - { - /// - /// The edge that defines the outward contact normal. - /// - public Byte ReferenceEdge; - - /// - /// The edge most anti-parallel to the reference edge. - /// - public Byte IncidentEdge; - - /// - /// The vertex (0 or 1) on the incident edge that was clipped. - /// - public Byte IncidentVertex; - - /// - /// A value of 1 indicates that the reference edge is on shape2. - /// - public Byte Flip; - } - - /// - /// Contact ids to facilitate warm starting. - /// - [System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Explicit)] - public struct ContactID - { - [System.Runtime.InteropServices.FieldOffset(0)] - public Features Features; - - /// - /// Used to quickly compare contact ids. - /// - [System.Runtime.InteropServices.FieldOffset(0)] - public UInt32 Key; - } - - /// - /// A manifold point is a contact point belonging to a contact - /// manifold. It holds details related to the geometry and dynamics - /// of the contact points. - /// The local point usage depends on the manifold type: - /// -Circles: the local center of circleB - /// -FaceA: the local center of cirlceB or the clip point of polygonB - /// -FaceB: the clip point of polygonA - /// This structure is stored across time steps, so we keep it small. - /// Note: the impulses are used for internal caching and may not - /// provide reliable contact forces, especially for high speed collisions. - /// - public class ManifoldPoint - { - /// - /// Usage depends on manifold type. - /// - public Vector2 LocalPoint; - - /// - /// The non-penetration impulse. - /// - public float NormalImpulse; - - /// - /// The friction impulse. - /// - public float TangentImpulse; - - /// - /// Uniquely identifies a contact point between two shapes. - /// - public ContactID ID; - - public ManifoldPoint Clone() - { - ManifoldPoint newPoint = new ManifoldPoint(); - newPoint.LocalPoint = this.LocalPoint; - newPoint.NormalImpulse = this.NormalImpulse; - newPoint.TangentImpulse = this.TangentImpulse; - newPoint.ID = this.ID; - return newPoint; - } - } - - public enum ManifoldType - { - Circles, - FaceA, - FaceB - } - - /// - /// A manifold for two touching convex shapes. - /// - public class Manifold - { - /// - /// The points of contact. - /// - public ManifoldPoint[/*Settings.MaxManifoldPoints*/] Points = new ManifoldPoint[Settings.MaxManifoldPoints]; - - public Vector2 LocalPlaneNormal; - - /// - /// Usage depends on manifold type. - /// - public Vector2 LocalPoint; - - public ManifoldType Type; - - /// - /// The number of manifold points. - /// - public int PointCount; - - public Manifold() - { - for (int i = 0; i < Settings.MaxManifoldPoints; i++) - Points[i] = new ManifoldPoint(); - } - - public Manifold Clone() - { - Manifold newManifold = new Manifold(); - newManifold.LocalPlaneNormal = this.LocalPlaneNormal; - newManifold.LocalPoint = this.LocalPoint; - newManifold.Type = this.Type; - newManifold.PointCount = this.PointCount; - int pointCount = this.Points.Length; - ManifoldPoint[] tmp = new ManifoldPoint[pointCount]; - for (int i = 0; i < pointCount; i++) - { - tmp[i] = this.Points[i].Clone(); - } - newManifold.Points = tmp; - return newManifold; - } - } - - /// - /// A line segment. - /// - public struct Segment - { - // Collision Detection in Interactive 3D Environments by Gino van den Bergen - // From Section 3.4.1 - // x = mu1 * p1 + mu2 * p2 - // mu1 + mu2 = 1 && mu1 >= 0 && mu2 >= 0 - // mu1 = 1 - mu2; - // x = (1 - mu2) * p1 + mu2 * p2 - // = p1 + mu2 * (p2 - p1) - // x = s + a * r (s := start, r := end - start) - // s + a * r = p1 + mu2 * d (d := p2 - p1) - // -a * r + mu2 * d = b (b := s - p1) - // [-r d] * [a; mu2] = b - // Cramer's rule: - // denom = det[-r d] - // a = det[b d] / denom - // mu2 = det[-r b] / denom - /// - /// Ray cast against this segment with another segment. - /// - public bool TestSegment(out float lambda, out Vector2 normal, Segment segment, float maxLambda) - { - lambda = 0f; - normal = new Vector2(); - - Vector2 s = segment.P1; - Vector2 r = segment.P2 - s; - Vector2 d = P2 - P1; - Vector2 n = d.CrossScalarPostMultiply(1.0f); - - float k_slop = 100.0f * Common.Settings.FLT_EPSILON; - float denom = -Vector2.Dot(r, n); - - // Cull back facing collision and ignore parallel segments. - if (denom > k_slop) - { - // Does the segment intersect the infinite line associated with this segment? - Vector2 b = s - P1; - float a = Vector2.Dot(b, n); - - if (0.0f <= a && a <= maxLambda * denom) - { - float mu2 = -r.X * b.Y + r.Y * b.X; - - // Does the segment intersect this segment? - if (-k_slop * denom <= mu2 && mu2 <= denom * (1.0f + k_slop)) - { - a /= denom; - n.Normalize(); - lambda = a; - normal = n; - return true; - } - } - } - - return false; - } - - /// - /// The starting point. - /// - public Vector2 P1; - - /// - /// The ending point. - /// - public Vector2 P2; - } - - /// - /// An axis aligned bounding box. - /// - public struct AABB - { - /// - /// The lower vertex. - /// - public Vector2 LowerBound; - - /// - /// The upper vertex. - /// - public Vector2 UpperBound; - - /// Verify that the bounds are sorted. - public bool IsValid - { - get - { - Vector2 d = UpperBound - LowerBound; - bool valid = d.X >= 0.0f && d.Y >= 0.0f; - valid = valid && LowerBound.IsValid() && UpperBound.IsValid(); - return valid; - } - } - - /// Get the center of the AABB. - public Vector2 Center - { - get { return 0.5f * (LowerBound + UpperBound); } - } - - /// Get the extents of the AABB (half-widths). - public Vector2 Extents - { - get { return 0.5f * (UpperBound - LowerBound); } - } - - /// Combine two AABBs into this one. - public void Combine(AABB aabb1, AABB aabb2) - { - LowerBound = Vector2.Min(aabb1.LowerBound, aabb2.LowerBound); - UpperBound = Vector2.Max(aabb1.UpperBound, aabb2.UpperBound); - } - - /// Does this aabb contain the provided AABB. - public bool Contains(AABB aabb) - { - bool result = LowerBound.X <= aabb.LowerBound.X; - result = result && LowerBound.Y <= aabb.LowerBound.Y; - result = result && aabb.UpperBound.X <= UpperBound.X; - result = result && aabb.UpperBound.Y <= UpperBound.Y; - return result; - } - - /// - // From Real-time Collision Detection, p179. - /// - public void RayCast(out RayCastOutput output, RayCastInput input) - { - float tmin = -Common.Settings.FLT_MAX; - float tmax = Common.Settings.FLT_MAX; - - output = new RayCastOutput(); - - output.Hit = false; - - Vector2 p = input.P1; - Vector2 d = input.P2 - input.P1; - Vector2 absD = d.Abs(); - - Vector2 normal = Vector2.Zero; - - for (int i = 0; i < 2; ++i) - { - if (absD.GetByIndex(i) < Common.Settings.FLT_EPSILON) - { - // Parallel. - if (p.GetByIndex(i) < LowerBound.GetByIndex(i) || UpperBound.GetByIndex(i) < p.GetByIndex(i)) - { - return; - } - } - else - { - float inv_d = 1.0f / d.GetByIndex(i); - float t1 = (LowerBound.GetByIndex(i) - p.GetByIndex(i)) * inv_d; - float t2 = (UpperBound.GetByIndex(i) - p.GetByIndex(i)) * inv_d; - - // Sign of the normal vector. - float s = -1.0f; - - if (t1 > t2) - { - Common.Math.Swap(ref t1, ref t2); - s = 1.0f; - } - - // Push the min up - if (t1 > tmin) - { - normal = Vector2.Zero; - normal.SetByIndex(i, s); - tmin = t1; - } - - // Pull the max down - tmax = Common.Math.Min(tmax, t2); - - if (tmin > tmax) - { - return; - } - } - } - - // Does the ray start inside the box? - // Does the ray intersect beyond the max fraction? - if (tmin < 0.0f || input.MaxFraction < tmin) - { - return; - } - - // Intersection. - output.Fraction = tmin; - output.Normal = normal; - output.Hit = true; - } - } - - /// - /// This is used for determining the state of contact points. - /// - public enum PointState - { - /// - /// Point does not exist. - /// - NullState, - /// - /// Point was added in the update. - /// - AddState, - /// - /// Point persisted across the update. - /// - PersistState, - /// - ///Point was removed in the update. - /// - RemoveState - } - - /// - /// This is used to compute the current state of a contact manifold. - /// - public class WorldManifold - { - /// - /// World vector pointing from A to B. - /// - public Vector2 Normal; - /// - /// World contact point (point of intersection). - /// - public Vector2[] Points = new Vector2[Common.Settings.MaxManifoldPoints]; - - public WorldManifold Clone() - { - WorldManifold newManifold = new WorldManifold(); - newManifold.Normal = this.Normal; - this.Points.CopyTo(newManifold.Points, 0); - return newManifold; - } - - /// Evaluate the manifold with supplied Transforms. This assumes - /// modest motion from the original state. This does not change the - /// point count, impulses, etc. The radii must come from the shapes - /// that generated the manifold. - public void Initialize(Manifold manifold, Transform xfA, float radiusA, Transform xfB, float radiusB) - { - if (manifold.PointCount == 0) - { - return; - } - - switch (manifold.Type) - { - case ManifoldType.Circles: - { - Vector2 pointA = xfA.TransformPoint(manifold.LocalPoint); - Vector2 pointB = xfB.TransformPoint(manifold.Points[0].LocalPoint); - Vector2 normal = new Vector2(1.0f, 0.0f); - if ((pointA - pointB).LengthSquared() > (Box2DNet.Common.Math.Epsilon * Box2DNet.Common.Math.Epsilon)) - { - normal = pointB - pointA; - normal.Normalize(); - } - - Normal = normal; - - Vector2 cA = pointA + radiusA * normal; - Vector2 cB = pointB - radiusB * normal; - Points[0] = 0.5f * (cA + cB); - } - break; - - case ManifoldType.FaceA: - { - Vector2 normal = xfA.TransformDirection(manifold.LocalPlaneNormal); - Vector2 planePoint = xfA.TransformPoint(manifold.LocalPoint); - - // Ensure normal points from A to B. - Normal = normal; - - for (int i = 0; i < manifold.PointCount; ++i) - { - Vector2 clipPoint = xfB.TransformPoint(manifold.Points[i].LocalPoint); - Vector2 cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, normal)) * normal; - Vector2 cB = clipPoint - radiusB * normal; - Points[i] = 0.5f * (cA + cB); - } - } - break; - - case ManifoldType.FaceB: - { - Vector2 normal = xfB.TransformDirection(manifold.LocalPlaneNormal); - Vector2 planePoint = xfB.TransformPoint(manifold.LocalPoint); - - // Ensure normal points from A to B. - Normal = -normal; - - for (int i = 0; i < manifold.PointCount; ++i) - { - Vector2 clipPoint = xfA.TransformPoint(manifold.Points[i].LocalPoint); - Vector2 cA = clipPoint - radiusA * normal; - Vector2 cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, normal)) * normal; - Points[i] = 0.5f * (cA + cB); - } - } - break; - } - } - } - - /// - /// Used for computing contact manifolds. - /// - public struct ClipVertex - { - public Vector2 V; - public ContactID ID; - } - - /// - /// Ray-cast input data. - /// - public struct RayCastInput - { - public Vector2 P1, P2; - public float MaxFraction; - } - - /// - /// Ray-cast output data. - /// - public struct RayCastOutput - { - public Vector2 Normal; - public float Fraction; - public bool Hit; - } +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Runtime.InteropServices; +using Box2DNet.Collision.Shapes; +using Box2DNet.Common; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Collision +{ + internal enum ContactFeatureType : byte + { + Vertex = 0, + Face = 1, + } + + /// + /// The features that intersect to form the contact point + /// This must be 4 bytes or less. + /// + public struct ContactFeature + { + /// + /// Feature index on ShapeA + /// + public byte IndexA; + + /// + /// Feature index on ShapeB + /// + public byte IndexB; + + /// + /// The feature type on ShapeA + /// + public byte TypeA; + + /// + /// The feature type on ShapeB + /// + public byte TypeB; + } + + /// + /// Contact ids to facilitate warm starting. + /// + [StructLayout(LayoutKind.Explicit)] + public struct ContactID + { + /// + /// The features that intersect to form the contact point + /// + [FieldOffset(0)] + public ContactFeature Features; + + /// + /// Used to quickly compare contact ids. + /// + [FieldOffset(0)] + public uint Key; + } + + /// + /// A manifold point is a contact point belonging to a contact + /// manifold. It holds details related to the geometry and dynamics + /// of the contact points. + /// The local point usage depends on the manifold type: + /// -ShapeType.Circles: the local center of circleB + /// -SeparationFunction.FaceA: the local center of cirlceB or the clip point of polygonB + /// -SeparationFunction.FaceB: the clip point of polygonA + /// This structure is stored across time steps, so we keep it small. + /// Note: the impulses are used for internal caching and may not + /// provide reliable contact forces, especially for high speed collisions. + /// + public struct ManifoldPoint + { + /// + /// Uniquely identifies a contact point between two Shapes + /// + public ContactID Id; + + /// + /// Usage depends on manifold type + /// + public Vector2 LocalPoint; + + /// + /// The non-penetration impulse + /// + public float NormalImpulse; + + /// + /// The friction impulse + /// + public float TangentImpulse; + } + + public enum ManifoldType + { + Circles, + FaceA, + FaceB + } + + /// + /// A manifold for two touching convex Shapes. + /// Box2D supports multiple types of contact: + /// - Clip point versus plane with radius + /// - Point versus point with radius (circles) + /// The local point usage depends on the manifold type: + /// - ShapeType.Circles: the local center of circleA + /// - SeparationFunction.FaceA: the center of faceA + /// - SeparationFunction.FaceB: the center of faceB + /// Similarly the local normal usage: + /// - ShapeType.Circles: not used + /// - SeparationFunction.FaceA: the normal on polygonA + /// - SeparationFunction.FaceB: the normal on polygonB + /// We store contacts in this way so that position correction can + /// account for movement, which is critical for continuous physics. + /// All contact scenarios must be expressed in one of these types. + /// This structure is stored across time steps, so we keep it small. + /// + public struct Manifold + { + /// + /// Not use for Type.SeparationFunction.Points + /// + public Vector2 LocalNormal; + + /// + /// Usage depends on manifold type + /// + public Vector2 LocalPoint; + + /// + /// The number of manifold points + /// + public int PointCount; + + /// + /// The points of contact + /// + public FixedArray2 Points; + + public ManifoldType Type; + } + + /// + /// This is used for determining the state of contact points. + /// + public enum PointState + { + /// + /// Point does not exist + /// + Null, + + /// + /// Point was added in the update + /// + Add, + + /// + /// Point persisted across the update + /// + Persist, + + /// + /// Point was removed in the update + /// + Remove, + } + + /// + /// Used for computing contact manifolds. + /// + public struct ClipVertex + { + public ContactID ID; + public Vector2 V; + } + + /// + /// Ray-cast input data. + /// + public struct RayCastInput + { + /// + /// The ray extends from p1 to p1 + maxFraction * (p2 - p1). + /// If you supply a max fraction of 1, the ray extends from p1 to p2. + /// A max fraction of 0.5 makes the ray go from p1 and half way to p2. + /// + public float MaxFraction; + + /// + /// The starting point of the ray. + /// + public Vector2 Point1; + + /// + /// The ending point of the ray. + /// + public Vector2 Point2; + } + + /// + /// Ray-cast output data. + /// + public struct RayCastOutput + { + /// + /// The ray hits at p1 + fraction * (p2 - p1), where p1 and p2 come from RayCastInput. + /// Contains the actual fraction of the ray where it has the intersection point. + /// + public float Fraction; + + /// + /// The normal of the face of the shape the ray has hit. + /// + public Vector2 Normal; + } + + /// + /// An axis aligned bounding box. + /// + public struct AABB + { + /// + /// The lower vertex + /// + public Vector2 LowerBound; + + /// + /// The upper vertex + /// + public Vector2 UpperBound; + + public AABB(Vector2 min, Vector2 max) + : this(ref min, ref max) + { + } + + public AABB(ref Vector2 min, ref Vector2 max) + { + LowerBound = min; + UpperBound = max; + } + + public AABB(Vector2 center, float width, float height) + { + LowerBound = center - new Vector2(width / 2, height / 2); + UpperBound = center + new Vector2(width / 2, height / 2); + } + + public float Width + { + get { return UpperBound.X - LowerBound.X; } + } + + public float Height + { + get { return UpperBound.Y - LowerBound.Y; } + } + + /// + /// Get the center of the AABB. + /// + public Vector2 Center + { + get { return 0.5f * (LowerBound + UpperBound); } + } + + /// + /// Get the extents of the AABB (half-widths). + /// + public Vector2 Extents + { + get { return 0.5f * (UpperBound - LowerBound); } + } + + /// + /// Get the perimeter length + /// + public float Perimeter + { + get + { + float wx = UpperBound.X - LowerBound.X; + float wy = UpperBound.Y - LowerBound.Y; + return 2.0f * (wx + wy); + } + } + + /// + /// Gets the vertices of the AABB. + /// + /// The corners of the AABB + public Vertices Vertices + { + get + { + Vertices vertices = new Vertices(4); + vertices.Add(UpperBound); + vertices.Add(new Vector2(UpperBound.X, LowerBound.Y)); + vertices.Add(LowerBound); + vertices.Add(new Vector2(LowerBound.X, UpperBound.Y)); + return vertices; + } + } + + /// + /// First quadrant + /// + public AABB Q1 + { + get { return new AABB(Center, UpperBound); } + } + + /// + /// Second quadrant + /// + public AABB Q2 + { + get { return new AABB(new Vector2(LowerBound.X, Center.Y), new Vector2(Center.X, UpperBound.Y)); } + } + + /// + /// Third quadrant + /// + public AABB Q3 + { + get { return new AABB(LowerBound, Center); } + } + + /// + /// Forth quadrant + /// + public AABB Q4 + { + get { return new AABB(new Vector2(Center.X, LowerBound.Y), new Vector2(UpperBound.X, Center.Y)); } + } + + /// + /// Verify that the bounds are sorted. And the bounds are valid numbers (not NaN). + /// + /// + /// true if this instance is valid; otherwise, false. + /// + public bool IsValid() + { + Vector2 d = UpperBound - LowerBound; + bool valid = d.X >= 0.0f && d.Y >= 0.0f; + valid = valid && LowerBound.IsValid() && UpperBound.IsValid(); + return valid; + } + + /// + /// Combine an AABB into this one. + /// + /// The aabb. + public void Combine(ref AABB aabb) + { + LowerBound = Vector2.Min(LowerBound, aabb.LowerBound); + UpperBound = Vector2.Max(UpperBound, aabb.UpperBound); + } + + /// + /// Combine two AABBs into this one. + /// + /// The aabb1. + /// The aabb2. + public void Combine(ref AABB aabb1, ref AABB aabb2) + { + LowerBound = Vector2.Min(aabb1.LowerBound, aabb2.LowerBound); + UpperBound = Vector2.Max(aabb1.UpperBound, aabb2.UpperBound); + } + + /// + /// Does this aabb contain the provided AABB. + /// + /// The aabb. + /// + /// true if it contains the specified aabb; otherwise, false. + /// + public bool Contains(ref AABB aabb) + { + bool result = true; + result = result && LowerBound.X <= aabb.LowerBound.X; + result = result && LowerBound.Y <= aabb.LowerBound.Y; + result = result && aabb.UpperBound.X <= UpperBound.X; + result = result && aabb.UpperBound.Y <= UpperBound.Y; + return result; + } + + /// + /// Determines whether the AAABB contains the specified point. + /// + /// The point. + /// + /// true if it contains the specified point; otherwise, false. + /// + public bool Contains(ref Vector2 point) + { + //using epsilon to try and gaurd against float rounding errors. + return (point.X > (LowerBound.X + Settings.Epsilon) && point.X < (UpperBound.X - Settings.Epsilon) && + (point.Y > (LowerBound.Y + Settings.Epsilon) && point.Y < (UpperBound.Y - Settings.Epsilon))); + } + + /// + /// Test if the two AABBs overlap. + /// + /// The first AABB. + /// The second AABB. + /// True if they are overlapping. + public static bool TestOverlap(ref AABB a, ref AABB b) + { + Vector2 d1 = b.LowerBound - a.UpperBound; + Vector2 d2 = a.LowerBound - b.UpperBound; + + if (d1.X > 0.0f || d1.Y > 0.0f) + return false; + + if (d2.X > 0.0f || d2.Y > 0.0f) + return false; + + return true; + } + + /// + /// Raycast against this AABB using the specificed points and maxfraction (found in input) + /// + /// The results of the raycast. + /// The parameters for the raycast. + /// True if the ray intersects the AABB + public bool RayCast(out RayCastOutput output, ref RayCastInput input, bool doInteriorCheck = true) + { + // From Real-time Collision Detection, p179. + + output = new RayCastOutput(); + + float tmin = -Settings.MaxFloat; + float tmax = Settings.MaxFloat; + + Vector2 p = input.Point1; + Vector2 d = input.Point2 - input.Point1; + Vector2 absD = MathUtils.Abs(d); + + Vector2 normal = Vector2.Zero; + + for (int i = 0; i < 2; ++i) + { + float absD_i = i == 0 ? absD.X : absD.Y; + float lowerBound_i = i == 0 ? LowerBound.X : LowerBound.Y; + float upperBound_i = i == 0 ? UpperBound.X : UpperBound.Y; + float p_i = i == 0 ? p.X : p.Y; + + if (absD_i < Settings.Epsilon) + { + // Parallel. + if (p_i < lowerBound_i || upperBound_i < p_i) + { + return false; + } + } + else + { + float d_i = i == 0 ? d.X : d.Y; + + float inv_d = 1.0f / d_i; + float t1 = (lowerBound_i - p_i) * inv_d; + float t2 = (upperBound_i - p_i) * inv_d; + + // Sign of the normal vector. + float s = -1.0f; + + if (t1 > t2) + { + MathUtils.Swap(ref t1, ref t2); + s = 1.0f; + } + + // Push the min up + if (t1 > tmin) + { + if (i == 0) + { + normal.X = s; + } + else + { + normal.Y = s; + } + + tmin = t1; + } + + // Pull the max down + tmax = Math.Min(tmax, t2); + + if (tmin > tmax) + { + return false; + } + } + } + + // Does the ray start inside the box? + // Does the ray intersect beyond the max fraction? + if (doInteriorCheck && (tmin < 0.0f || input.MaxFraction < tmin)) + { + return false; + } + + // Intersection. + output.Fraction = tmin; + output.Normal = normal; + return true; + } + } + + /// + /// This holds polygon B expressed in frame A. + /// + public class TempPolygon + { + public Vector2[] Vertices = new Vector2[Settings.MaxPolygonVertices]; + public Vector2[] Normals = new Vector2[Settings.MaxPolygonVertices]; + public int Count; + } + + /// + /// This structure is used to keep track of the best separating axis. + /// + public struct EPAxis + { + public int Index; + public float Separation; + public EPAxisType Type; + } + + /// + /// Reference face used for clipping + /// + public struct ReferenceFace + { + public int i1, i2; + + public Vector2 v1, v2; + + public Vector2 normal; + + public Vector2 sideNormal1; + public float sideOffset1; + + public Vector2 sideNormal2; + public float sideOffset2; + } + + public enum EPAxisType + { + Unknown, + EdgeA, + EdgeB, + } + + /// + /// Collision methods + /// + public static class Collision + { + [ThreadStatic] + private static DistanceInput _input; + + /// + /// Test overlap between the two shapes. + /// + /// The first shape. + /// The index for the first shape. + /// The second shape. + /// The index for the second shape. + /// The transform for the first shape. + /// The transform for the seconds shape. + /// + public static bool TestOverlap(Shape shapeA, int indexA, Shape shapeB, int indexB, ref Transform xfA, ref Transform xfB) + { + _input = _input ?? new DistanceInput(); + _input.ProxyA.Set(shapeA, indexA); + _input.ProxyB.Set(shapeB, indexB); + _input.TransformA = xfA; + _input.TransformB = xfB; + _input.UseRadii = true; + + SimplexCache cache; + DistanceOutput output; + Distance.ComputeDistance(out output, out cache, _input); + + return output.Distance < 10.0f * Settings.Epsilon; + } + + public static void GetPointStates(out FixedArray2 state1, out FixedArray2 state2, ref Manifold manifold1, ref Manifold manifold2) + { + state1 = new FixedArray2(); + state2 = new FixedArray2(); + + // Detect persists and removes. + for (int i = 0; i < manifold1.PointCount; ++i) + { + ContactID id = manifold1.Points[i].Id; + + state1[i] = PointState.Remove; + + for (int j = 0; j < manifold2.PointCount; ++j) + { + if (manifold2.Points[j].Id.Key == id.Key) + { + state1[i] = PointState.Persist; + break; + } + } + } + + // Detect persists and adds. + for (int i = 0; i < manifold2.PointCount; ++i) + { + ContactID id = manifold2.Points[i].Id; + + state2[i] = PointState.Add; + + for (int j = 0; j < manifold1.PointCount; ++j) + { + if (manifold1.Points[j].Id.Key == id.Key) + { + state2[i] = PointState.Persist; + break; + } + } + } + } + + /// + /// Compute the collision manifold between two circles. + /// + public static void CollideCircles(ref Manifold manifold, CircleShape circleA, ref Transform xfA, CircleShape circleB, ref Transform xfB) + { + manifold.PointCount = 0; + + Vector2 pA = MathUtils.Mul(ref xfA, circleA.Position); + Vector2 pB = MathUtils.Mul(ref xfB, circleB.Position); + + Vector2 d = pB - pA; + float distSqr = Vector2.Dot(d, d); + float radius = circleA.Radius + circleB.Radius; + if (distSqr > radius * radius) + { + return; + } + + manifold.Type = ManifoldType.Circles; + manifold.LocalPoint = circleA.Position; + manifold.LocalNormal = Vector2.Zero; + manifold.PointCount = 1; + + ManifoldPoint p0 = manifold.Points[0]; + + p0.LocalPoint = circleB.Position; + p0.Id.Key = 0; + + manifold.Points[0] = p0; + } + + /// + /// Compute the collision manifold between a polygon and a circle. + /// + /// The manifold. + /// The polygon A. + /// The transform of A. + /// The circle B. + /// The transform of B. + public static void CollidePolygonAndCircle(ref Manifold manifold, PolygonShape polygonA, ref Transform xfA, CircleShape circleB, ref Transform xfB) + { + manifold.PointCount = 0; + + // Compute circle position in the frame of the polygon. + Vector2 c = MathUtils.Mul(ref xfB, circleB.Position); + Vector2 cLocal = MathUtils.MulT(ref xfA, c); + + // Find the min separating edge. + int normalIndex = 0; + float separation = -Settings.MaxFloat; + float radius = polygonA.Radius + circleB.Radius; + int vertexCount = polygonA.Vertices.Count; + + for (int i = 0; i < vertexCount; ++i) + { + Vector2 value1 = polygonA.Normals[i]; + Vector2 value2 = cLocal - polygonA.Vertices[i]; + float s = value1.X * value2.X + value1.Y * value2.Y; + + if (s > radius) + { + // Early out. + return; + } + + if (s > separation) + { + separation = s; + normalIndex = i; + } + } + + // Vertices that subtend the incident face. + int vertIndex1 = normalIndex; + int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0; + Vector2 v1 = polygonA.Vertices[vertIndex1]; + Vector2 v2 = polygonA.Vertices[vertIndex2]; + + // If the center is inside the polygon ... + if (separation < Settings.Epsilon) + { + manifold.PointCount = 1; + manifold.Type = ManifoldType.FaceA; + manifold.LocalNormal = polygonA.Normals[normalIndex]; + manifold.LocalPoint = 0.5f * (v1 + v2); + + ManifoldPoint p0 = manifold.Points[0]; + + p0.LocalPoint = circleB.Position; + p0.Id.Key = 0; + + manifold.Points[0] = p0; + + return; + } + + // Compute barycentric coordinates + float u1 = (cLocal.X - v1.X) * (v2.X - v1.X) + (cLocal.Y - v1.Y) * (v2.Y - v1.Y); + float u2 = (cLocal.X - v2.X) * (v1.X - v2.X) + (cLocal.Y - v2.Y) * (v1.Y - v2.Y); + + if (u1 <= 0.0f) + { + float r = (cLocal.X - v1.X) * (cLocal.X - v1.X) + (cLocal.Y - v1.Y) * (cLocal.Y - v1.Y); + if (r > radius * radius) + { + return; + } + + manifold.PointCount = 1; + manifold.Type = ManifoldType.FaceA; + manifold.LocalNormal = cLocal - v1; + float factor = 1f / + (float) + Math.Sqrt(manifold.LocalNormal.X * manifold.LocalNormal.X + + manifold.LocalNormal.Y * manifold.LocalNormal.Y); + manifold.LocalNormal.X = manifold.LocalNormal.X * factor; + manifold.LocalNormal.Y = manifold.LocalNormal.Y * factor; + manifold.LocalPoint = v1; + + ManifoldPoint p0b = manifold.Points[0]; + + p0b.LocalPoint = circleB.Position; + p0b.Id.Key = 0; + + manifold.Points[0] = p0b; + } + else if (u2 <= 0.0f) + { + float r = (cLocal.X - v2.X) * (cLocal.X - v2.X) + (cLocal.Y - v2.Y) * (cLocal.Y - v2.Y); + if (r > radius * radius) + { + return; + } + + manifold.PointCount = 1; + manifold.Type = ManifoldType.FaceA; + manifold.LocalNormal = cLocal - v2; + float factor = 1f / + (float) + Math.Sqrt(manifold.LocalNormal.X * manifold.LocalNormal.X + + manifold.LocalNormal.Y * manifold.LocalNormal.Y); + manifold.LocalNormal.X = manifold.LocalNormal.X * factor; + manifold.LocalNormal.Y = manifold.LocalNormal.Y * factor; + manifold.LocalPoint = v2; + + ManifoldPoint p0c = manifold.Points[0]; + + p0c.LocalPoint = circleB.Position; + p0c.Id.Key = 0; + + manifold.Points[0] = p0c; + } + else + { + Vector2 faceCenter = 0.5f * (v1 + v2); + Vector2 value1 = cLocal - faceCenter; + Vector2 value2 = polygonA.Normals[vertIndex1]; + float separation2 = value1.X * value2.X + value1.Y * value2.Y; + if (separation2 > radius) + { + return; + } + + manifold.PointCount = 1; + manifold.Type = ManifoldType.FaceA; + manifold.LocalNormal = polygonA.Normals[vertIndex1]; + manifold.LocalPoint = faceCenter; + + ManifoldPoint p0d = manifold.Points[0]; + + p0d.LocalPoint = circleB.Position; + p0d.Id.Key = 0; + + manifold.Points[0] = p0d; + } + } + + /// + /// Compute the collision manifold between two polygons. + /// + /// The manifold. + /// The poly A. + /// The transform A. + /// The poly B. + /// The transform B. + public static void CollidePolygons(ref Manifold manifold, PolygonShape polyA, ref Transform transformA, PolygonShape polyB, ref Transform transformB) + { + manifold.PointCount = 0; + float totalRadius = polyA.Radius + polyB.Radius; + + int edgeA = 0; + float separationA = FindMaxSeparation(out edgeA, polyA, ref transformA, polyB, ref transformB); + if (separationA > totalRadius) + return; + + int edgeB = 0; + float separationB = FindMaxSeparation(out edgeB, polyB, ref transformB, polyA, ref transformA); + if (separationB > totalRadius) + return; + + PolygonShape poly1; // reference polygon + PolygonShape poly2; // incident polygon + Transform xf1, xf2; + int edge1; // reference edge + bool flip; + const float k_relativeTol = 0.98f; + const float k_absoluteTol = 0.001f; + + if (separationB > k_relativeTol * separationA + k_absoluteTol) + { + poly1 = polyB; + poly2 = polyA; + xf1 = transformB; + xf2 = transformA; + edge1 = edgeB; + manifold.Type = ManifoldType.FaceB; + flip = true; + } + else + { + poly1 = polyA; + poly2 = polyB; + xf1 = transformA; + xf2 = transformB; + edge1 = edgeA; + manifold.Type = ManifoldType.FaceA; + flip = false; + } + + FixedArray2 incidentEdge; + FindIncidentEdge(out incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2); + + int count1 = poly1.Vertices.Count; + + int iv1 = edge1; + int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; + + Vector2 v11 = poly1.Vertices[iv1]; + Vector2 v12 = poly1.Vertices[iv2]; + + Vector2 localTangent = v12 - v11; + localTangent.Normalize(); + + Vector2 localNormal = new Vector2(localTangent.Y, -localTangent.X); + Vector2 planePoint = 0.5f * (v11 + v12); + + Vector2 tangent = MathUtils.Mul(xf1.q, localTangent); + + float normalx = tangent.Y; + float normaly = -tangent.X; + + v11 = MathUtils.Mul(ref xf1, v11); + v12 = MathUtils.Mul(ref xf1, v12); + + // Face offset. + float frontOffset = normalx * v11.X + normaly * v11.Y; + + // Side offsets, extended by polytope skin thickness. + float sideOffset1 = -(tangent.X * v11.X + tangent.Y * v11.Y) + totalRadius; + float sideOffset2 = tangent.X * v12.X + tangent.Y * v12.Y + totalRadius; + + // Clip incident edge against extruded edge1 side edges. + FixedArray2 clipPoints1; + FixedArray2 clipPoints2; + + // Clip to box side 1 + int np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1, iv1); + + if (np < 2) + return; + + // Clip to negative box side 1 + np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2, iv2); + + if (np < 2) + { + return; + } + + // Now clipPoints2 contains the clipped points. + manifold.LocalNormal = localNormal; + manifold.LocalPoint = planePoint; + + int pointCount = 0; + for (int i = 0; i < Settings.MaxManifoldPoints; ++i) + { + Vector2 value = clipPoints2[i].V; + float separation = normalx * value.X + normaly * value.Y - frontOffset; + + if (separation <= totalRadius) + { + ManifoldPoint cp = manifold.Points[pointCount]; + cp.LocalPoint = MathUtils.MulT(ref xf2, clipPoints2[i].V); + cp.Id = clipPoints2[i].ID; + + if (flip) + { + // Swap features + ContactFeature cf = cp.Id.Features; + cp.Id.Features.IndexA = cf.IndexB; + cp.Id.Features.IndexB = cf.IndexA; + cp.Id.Features.TypeA = cf.TypeB; + cp.Id.Features.TypeB = cf.TypeA; + } + + manifold.Points[pointCount] = cp; + + ++pointCount; + } + } + + manifold.PointCount = pointCount; + } + + /// + /// Compute contact points for edge versus circle. + /// This accounts for edge connectivity. + /// + /// The manifold. + /// The edge A. + /// The transform A. + /// The circle B. + /// The transform B. + public static void CollideEdgeAndCircle(ref Manifold manifold, EdgeShape edgeA, ref Transform transformA, CircleShape circleB, ref Transform transformB) + { + manifold.PointCount = 0; + + // Compute circle in frame of edge + Vector2 Q = MathUtils.MulT(ref transformA, MathUtils.Mul(ref transformB, ref circleB._position)); + + Vector2 A = edgeA.Vertex1, B = edgeA.Vertex2; + Vector2 e = B - A; + + // Barycentric coordinates + float u = Vector2.Dot(e, B - Q); + float v = Vector2.Dot(e, Q - A); + + float radius = edgeA.Radius + circleB.Radius; + + ContactFeature cf; + cf.IndexB = 0; + cf.TypeB = (byte)ContactFeatureType.Vertex; + + Vector2 P, d; + + // Region A + if (v <= 0.0f) + { + P = A; + d = Q - P; + float dd; + Vector2.Dot(ref d, ref d, out dd); + if (dd > radius * radius) + { + return; + } + + // Is there an edge connected to A? + if (edgeA.HasVertex0) + { + Vector2 A1 = edgeA.Vertex0; + Vector2 B1 = A; + Vector2 e1 = B1 - A1; + float u1 = Vector2.Dot(e1, B1 - Q); + + // Is the circle in Region AB of the previous edge? + if (u1 > 0.0f) + { + return; + } + } + + cf.IndexA = 0; + cf.TypeA = (byte)ContactFeatureType.Vertex; + manifold.PointCount = 1; + manifold.Type = ManifoldType.Circles; + manifold.LocalNormal = Vector2.Zero; + manifold.LocalPoint = P; + ManifoldPoint mp = new ManifoldPoint(); + mp.Id.Key = 0; + mp.Id.Features = cf; + mp.LocalPoint = circleB.Position; + manifold.Points[0] = mp; + return; + } + + // Region B + if (u <= 0.0f) + { + P = B; + d = Q - P; + float dd; + Vector2.Dot(ref d, ref d, out dd); + if (dd > radius * radius) + { + return; + } + + // Is there an edge connected to B? + if (edgeA.HasVertex3) + { + Vector2 B2 = edgeA.Vertex3; + Vector2 A2 = B; + Vector2 e2 = B2 - A2; + float v2 = Vector2.Dot(e2, Q - A2); + + // Is the circle in Region AB of the next edge? + if (v2 > 0.0f) + { + return; + } + } + + cf.IndexA = 1; + cf.TypeA = (byte)ContactFeatureType.Vertex; + manifold.PointCount = 1; + manifold.Type = ManifoldType.Circles; + manifold.LocalNormal = Vector2.Zero; + manifold.LocalPoint = P; + ManifoldPoint mp = new ManifoldPoint(); + mp.Id.Key = 0; + mp.Id.Features = cf; + mp.LocalPoint = circleB.Position; + manifold.Points[0] = mp; + return; + } + + // Region AB + float den; + Vector2.Dot(ref e, ref e, out den); + Debug.Assert(den > 0.0f); + P = (1.0f / den) * (u * A + v * B); + d = Q - P; + float dd2; + Vector2.Dot(ref d, ref d, out dd2); + if (dd2 > radius * radius) + { + return; + } + + Vector2 n = new Vector2(-e.Y, e.X); + if (Vector2.Dot(n, Q - A) < 0.0f) + { + n = new Vector2(-n.X, -n.Y); + } + n.Normalize(); + + cf.IndexA = 0; + cf.TypeA = (byte)ContactFeatureType.Face; + manifold.PointCount = 1; + manifold.Type = ManifoldType.FaceA; + manifold.LocalNormal = n; + manifold.LocalPoint = A; + ManifoldPoint mp2 = new ManifoldPoint(); + mp2.Id.Key = 0; + mp2.Id.Features = cf; + mp2.LocalPoint = circleB.Position; + manifold.Points[0] = mp2; + } + + /// + /// Collides and edge and a polygon, taking into account edge adjacency. + /// + /// The manifold. + /// The edge A. + /// The xf A. + /// The polygon B. + /// The xf B. + public static void CollideEdgeAndPolygon(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB) + { + EPCollider collider = new EPCollider(); + collider.Collide(ref manifold, edgeA, ref xfA, polygonB, ref xfB); + } + + private class EPCollider + { + private TempPolygon _polygonB = new TempPolygon(); + + Transform _xf; + Vector2 _centroidB; + Vector2 _v0, _v1, _v2, _v3; + Vector2 _normal0, _normal1, _normal2; + Vector2 _normal; + Vector2 _lowerLimit, _upperLimit; + float _radius; + bool _front; + + public void Collide(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB) + { + // Algorithm: + // 1. Classify v1 and v2 + // 2. Classify polygon centroid as front or back + // 3. Flip normal if necessary + // 4. Initialize normal range to [-pi, pi] about face normal + // 5. Adjust normal range according to adjacent edges + // 6. Visit each separating axes, only accept axes within the range + // 7. Return if _any_ axis indicates separation + // 8. Clip + + _xf = MathUtils.MulT(xfA, xfB); + + _centroidB = MathUtils.Mul(ref _xf, polygonB.MassData.Centroid); + + _v0 = edgeA.Vertex0; + _v1 = edgeA._vertex1; + _v2 = edgeA._vertex2; + _v3 = edgeA.Vertex3; + + bool hasVertex0 = edgeA.HasVertex0; + bool hasVertex3 = edgeA.HasVertex3; + + Vector2 edge1 = _v2 - _v1; + edge1.Normalize(); + _normal1 = new Vector2(edge1.Y, -edge1.X); + float offset1 = Vector2.Dot(_normal1, _centroidB - _v1); + float offset0 = 0.0f, offset2 = 0.0f; + bool convex1 = false, convex2 = false; + + // Is there a preceding edge? + if (hasVertex0) + { + Vector2 edge0 = _v1 - _v0; + edge0.Normalize(); + _normal0 = new Vector2(edge0.Y, -edge0.X); + convex1 = MathUtils.Cross(edge0, edge1) >= 0.0f; + offset0 = Vector2.Dot(_normal0, _centroidB - _v0); + } + + // Is there a following edge? + if (hasVertex3) + { + Vector2 edge2 = _v3 - _v2; + edge2.Normalize(); + _normal2 = new Vector2(edge2.Y, -edge2.X); + convex2 = MathUtils.Cross(edge1, edge2) > 0.0f; + offset2 = Vector2.Dot(_normal2, _centroidB - _v2); + } + + // Determine front or back collision. Determine collision normal limits. + if (hasVertex0 && hasVertex3) + { + if (convex1 && convex2) + { + _front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f; + if (_front) + { + _normal = _normal1; + _lowerLimit = _normal0; + _upperLimit = _normal2; + } + else + { + _normal = -_normal1; + _lowerLimit = -_normal1; + _upperLimit = -_normal1; + } + } + else if (convex1) + { + _front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f); + if (_front) + { + _normal = _normal1; + _lowerLimit = _normal0; + _upperLimit = _normal1; + } + else + { + _normal = -_normal1; + _lowerLimit = -_normal2; + _upperLimit = -_normal1; + } + } + else if (convex2) + { + _front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f); + if (_front) + { + _normal = _normal1; + _lowerLimit = _normal1; + _upperLimit = _normal2; + } + else + { + _normal = -_normal1; + _lowerLimit = -_normal1; + _upperLimit = -_normal0; + } + } + else + { + _front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f; + if (_front) + { + _normal = _normal1; + _lowerLimit = _normal1; + _upperLimit = _normal1; + } + else + { + _normal = -_normal1; + _lowerLimit = -_normal2; + _upperLimit = -_normal0; + } + } + } + else if (hasVertex0) + { + if (convex1) + { + _front = offset0 >= 0.0f || offset1 >= 0.0f; + if (_front) + { + _normal = _normal1; + _lowerLimit = _normal0; + _upperLimit = -_normal1; + } + else + { + _normal = -_normal1; + _lowerLimit = _normal1; + _upperLimit = -_normal1; + } + } + else + { + _front = offset0 >= 0.0f && offset1 >= 0.0f; + if (_front) + { + _normal = _normal1; + _lowerLimit = _normal1; + _upperLimit = -_normal1; + } + else + { + _normal = -_normal1; + _lowerLimit = _normal1; + _upperLimit = -_normal0; + } + } + } + else if (hasVertex3) + { + if (convex2) + { + _front = offset1 >= 0.0f || offset2 >= 0.0f; + if (_front) + { + _normal = _normal1; + _lowerLimit = -_normal1; + _upperLimit = _normal2; + } + else + { + _normal = -_normal1; + _lowerLimit = -_normal1; + _upperLimit = _normal1; + } + } + else + { + _front = offset1 >= 0.0f && offset2 >= 0.0f; + if (_front) + { + _normal = _normal1; + _lowerLimit = -_normal1; + _upperLimit = _normal1; + } + else + { + _normal = -_normal1; + _lowerLimit = -_normal2; + _upperLimit = _normal1; + } + } + } + else + { + _front = offset1 >= 0.0f; + if (_front) + { + _normal = _normal1; + _lowerLimit = -_normal1; + _upperLimit = -_normal1; + } + else + { + _normal = -_normal1; + _lowerLimit = _normal1; + _upperLimit = _normal1; + } + } + + // Get polygonB in frameA + _polygonB.Count = polygonB.Vertices.Count; + for (int i = 0; i < polygonB.Vertices.Count; ++i) + { + _polygonB.Vertices[i] = MathUtils.Mul(ref _xf, polygonB.Vertices[i]); + _polygonB.Normals[i] = MathUtils.Mul(_xf.q, polygonB.Normals[i]); + } + + _radius = 2.0f * Settings.PolygonRadius; + + manifold.PointCount = 0; + + EPAxis edgeAxis = ComputeEdgeSeparation(); + + // If no valid normal can be found than this edge should not collide. + if (edgeAxis.Type == EPAxisType.Unknown) + { + return; + } + + if (edgeAxis.Separation > _radius) + { + return; + } + + EPAxis polygonAxis = ComputePolygonSeparation(); + if (polygonAxis.Type != EPAxisType.Unknown && polygonAxis.Separation > _radius) + { + return; + } + + // Use hysteresis for jitter reduction. + const float k_relativeTol = 0.98f; + const float k_absoluteTol = 0.001f; + + EPAxis primaryAxis; + if (polygonAxis.Type == EPAxisType.Unknown) + { + primaryAxis = edgeAxis; + } + else if (polygonAxis.Separation > k_relativeTol * edgeAxis.Separation + k_absoluteTol) + { + primaryAxis = polygonAxis; + } + else + { + primaryAxis = edgeAxis; + } + + FixedArray2 ie = new FixedArray2(); + ReferenceFace rf; + if (primaryAxis.Type == EPAxisType.EdgeA) + { + manifold.Type = ManifoldType.FaceA; + + // Search for the polygon normal that is most anti-parallel to the edge normal. + int bestIndex = 0; + float bestValue = Vector2.Dot(_normal, _polygonB.Normals[0]); + for (int i = 1; i < _polygonB.Count; ++i) + { + float value = Vector2.Dot(_normal, _polygonB.Normals[i]); + if (value < bestValue) + { + bestValue = value; + bestIndex = i; + } + } + + int i1 = bestIndex; + int i2 = i1 + 1 < _polygonB.Count ? i1 + 1 : 0; + + ClipVertex c0 = ie[0]; + c0.V = _polygonB.Vertices[i1]; + c0.ID.Features.IndexA = 0; + c0.ID.Features.IndexB = (byte)i1; + c0.ID.Features.TypeA = (byte)ContactFeatureType.Face; + c0.ID.Features.TypeB = (byte)ContactFeatureType.Vertex; + ie[0] = c0; + + ClipVertex c1 = ie[1]; + c1.V = _polygonB.Vertices[i2]; + c1.ID.Features.IndexA = 0; + c1.ID.Features.IndexB = (byte)i2; + c1.ID.Features.TypeA = (byte)ContactFeatureType.Face; + c1.ID.Features.TypeB = (byte)ContactFeatureType.Vertex; + ie[1] = c1; + + if (_front) + { + rf.i1 = 0; + rf.i2 = 1; + rf.v1 = _v1; + rf.v2 = _v2; + rf.normal = _normal1; + } + else + { + rf.i1 = 1; + rf.i2 = 0; + rf.v1 = _v2; + rf.v2 = _v1; + rf.normal = -_normal1; + } + } + else + { + manifold.Type = ManifoldType.FaceB; + ClipVertex c0 = ie[0]; + c0.V = _v1; + c0.ID.Features.IndexA = 0; + c0.ID.Features.IndexB = (byte)primaryAxis.Index; + c0.ID.Features.TypeA = (byte)ContactFeatureType.Vertex; + c0.ID.Features.TypeB = (byte)ContactFeatureType.Face; + ie[0] = c0; + + ClipVertex c1 = ie[1]; + c1.V = _v2; + c1.ID.Features.IndexA = 0; + c1.ID.Features.IndexB = (byte)primaryAxis.Index; + c1.ID.Features.TypeA = (byte)ContactFeatureType.Vertex; + c1.ID.Features.TypeB = (byte)ContactFeatureType.Face; + ie[1] = c1; + + rf.i1 = primaryAxis.Index; + rf.i2 = rf.i1 + 1 < _polygonB.Count ? rf.i1 + 1 : 0; + rf.v1 = _polygonB.Vertices[rf.i1]; + rf.v2 = _polygonB.Vertices[rf.i2]; + rf.normal = _polygonB.Normals[rf.i1]; + } + + rf.sideNormal1 = new Vector2(rf.normal.Y, -rf.normal.X); + rf.sideNormal2 = -rf.sideNormal1; + rf.sideOffset1 = Vector2.Dot(rf.sideNormal1, rf.v1); + rf.sideOffset2 = Vector2.Dot(rf.sideNormal2, rf.v2); + + // Clip incident edge against extruded edge1 side edges. + FixedArray2 clipPoints1; + FixedArray2 clipPoints2; + int np; + + // Clip to box side 1 + np = ClipSegmentToLine(out clipPoints1, ref ie, rf.sideNormal1, rf.sideOffset1, rf.i1); + + if (np < Settings.MaxManifoldPoints) + { + return; + } + + // Clip to negative box side 1 + np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2); + + if (np < Settings.MaxManifoldPoints) + { + return; + } + + // Now clipPoints2 contains the clipped points. + if (primaryAxis.Type == EPAxisType.EdgeA) + { + manifold.LocalNormal = rf.normal; + manifold.LocalPoint = rf.v1; + } + else + { + manifold.LocalNormal = polygonB.Normals[rf.i1]; + manifold.LocalPoint = polygonB.Vertices[rf.i1]; + } + + int pointCount = 0; + for (int i = 0; i < Settings.MaxManifoldPoints; ++i) + { + float separation = Vector2.Dot(rf.normal, clipPoints2[i].V - rf.v1); + + if (separation <= _radius) + { + ManifoldPoint cp = manifold.Points[pointCount]; + + if (primaryAxis.Type == EPAxisType.EdgeA) + { + cp.LocalPoint = MathUtils.MulT(ref _xf, clipPoints2[i].V); + cp.Id = clipPoints2[i].ID; + } + else + { + cp.LocalPoint = clipPoints2[i].V; + cp.Id.Features.TypeA = clipPoints2[i].ID.Features.TypeB; + cp.Id.Features.TypeB = clipPoints2[i].ID.Features.TypeA; + cp.Id.Features.IndexA = clipPoints2[i].ID.Features.IndexB; + cp.Id.Features.IndexB = clipPoints2[i].ID.Features.IndexA; + } + + manifold.Points[pointCount] = cp; + ++pointCount; + } + } + + manifold.PointCount = pointCount; + } + + private EPAxis ComputeEdgeSeparation() + { + EPAxis axis; + axis.Type = EPAxisType.EdgeA; + axis.Index = _front ? 0 : 1; + axis.Separation = Settings.MaxFloat; + + for (int i = 0; i < _polygonB.Count; ++i) + { + float s = Vector2.Dot(_normal, _polygonB.Vertices[i] - _v1); + if (s < axis.Separation) + { + axis.Separation = s; + } + } + + return axis; + } + + private EPAxis ComputePolygonSeparation() + { + EPAxis axis; + axis.Type = EPAxisType.Unknown; + axis.Index = -1; + axis.Separation = -Settings.MaxFloat; + + Vector2 perp = new Vector2(-_normal.Y, _normal.X); + + for (int i = 0; i < _polygonB.Count; ++i) + { + Vector2 n = -_polygonB.Normals[i]; + + float s1 = Vector2.Dot(n, _polygonB.Vertices[i] - _v1); + float s2 = Vector2.Dot(n, _polygonB.Vertices[i] - _v2); + float s = Math.Min(s1, s2); + + if (s > _radius) + { + // No collision + axis.Type = EPAxisType.EdgeB; + axis.Index = i; + axis.Separation = s; + return axis; + } + + // Adjacency + if (Vector2.Dot(n, perp) >= 0.0f) + { + if (Vector2.Dot(n - _upperLimit, _normal) < -Settings.AngularSlop) + { + continue; + } + } + else + { + if (Vector2.Dot(n - _lowerLimit, _normal) < -Settings.AngularSlop) + { + continue; + } + } + + if (s > axis.Separation) + { + axis.Type = EPAxisType.EdgeB; + axis.Index = i; + axis.Separation = s; + } + } + + return axis; + } + } + + /// + /// Clipping for contact manifolds. + /// + /// The v out. + /// The v in. + /// The normal. + /// The offset. + /// The vertex index A. + /// + private static int ClipSegmentToLine(out FixedArray2 vOut, ref FixedArray2 vIn, Vector2 normal, float offset, int vertexIndexA) + { + vOut = new FixedArray2(); + + ClipVertex v0 = vIn[0]; + ClipVertex v1 = vIn[1]; + + // Start with no output points + int numOut = 0; + + // Calculate the distance of end points to the line + float distance0 = normal.X * v0.V.X + normal.Y * v0.V.Y - offset; + float distance1 = normal.X * v1.V.X + normal.Y * v1.V.Y - offset; + + // If the points are behind the plane + if (distance0 <= 0.0f) vOut[numOut++] = v0; + if (distance1 <= 0.0f) vOut[numOut++] = v1; + + // If the points are on different sides of the plane + if (distance0 * distance1 < 0.0f) + { + // Find intersection point of edge and plane + float interp = distance0 / (distance0 - distance1); + + ClipVertex cv = vOut[numOut]; + + cv.V.X = v0.V.X + interp * (v1.V.X - v0.V.X); + cv.V.Y = v0.V.Y + interp * (v1.V.Y - v0.V.Y); + + // VertexA is hitting edgeB. + cv.ID.Features.IndexA = (byte)vertexIndexA; + cv.ID.Features.IndexB = v0.ID.Features.IndexB; + cv.ID.Features.TypeA = (byte)ContactFeatureType.Vertex; + cv.ID.Features.TypeB = (byte)ContactFeatureType.Face; + + vOut[numOut] = cv; + + ++numOut; + } + + return numOut; + } + + /// + /// Find the separation between poly1 and poly2 for a give edge normal on poly1. + /// + /// The poly1. + /// The XF1. + /// The edge1. + /// The poly2. + /// The XF2. + /// + private static float EdgeSeparation(PolygonShape poly1, ref Transform xf1, int edge1, PolygonShape poly2, ref Transform xf2) + { + List vertices1 = poly1.Vertices; + List normals1 = poly1.Normals; + + int count2 = poly2.Vertices.Count; + List vertices2 = poly2.Vertices; + + Debug.Assert(0 <= edge1 && edge1 < poly1.Vertices.Count); + + // Convert normal from poly1's frame into poly2's frame. + Vector2 normal1World = MathUtils.Mul(xf1.q, normals1[edge1]); + Vector2 normal1 = MathUtils.MulT(xf2.q, normal1World); + + // Find support vertex on poly2 for -normal. + int index = 0; + float minDot = Settings.MaxFloat; + + for (int i = 0; i < count2; ++i) + { + float dot = Vector2.Dot(vertices2[i], normal1); + if (dot < minDot) + { + minDot = dot; + index = i; + } + } + + Vector2 v1 = MathUtils.Mul(ref xf1, vertices1[edge1]); + Vector2 v2 = MathUtils.Mul(ref xf2, vertices2[index]); + float separation = Vector2.Dot(v2 - v1, normal1World); + return separation; + } + + /// + /// Find the max separation between poly1 and poly2 using edge normals from poly1. + /// + /// Index of the edge. + /// The poly1. + /// The XF1. + /// The poly2. + /// The XF2. + /// + private static float FindMaxSeparation(out int edgeIndex, PolygonShape poly1, ref Transform xf1, PolygonShape poly2, ref Transform xf2) + { + int count1 = poly1.Vertices.Count; + List normals1 = poly1.Normals; + + // Vector pointing from the centroid of poly1 to the centroid of poly2. + Vector2 d = MathUtils.Mul(ref xf2, poly2.MassData.Centroid) - MathUtils.Mul(ref xf1, poly1.MassData.Centroid); + Vector2 dLocal1 = MathUtils.MulT(xf1.q, d); + + // Find edge normal on poly1 that has the largest projection onto d. + int edge = 0; + float maxDot = -Settings.MaxFloat; + for (int i = 0; i < count1; ++i) + { + float dot = Vector2.Dot(normals1[i], dLocal1); + if (dot > maxDot) + { + maxDot = dot; + edge = i; + } + } + + // Get the separation for the edge normal. + float s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2); + + // Check the separation for the previous edge normal. + int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1; + float sPrev = EdgeSeparation(poly1, ref xf1, prevEdge, poly2, ref xf2); + + // Check the separation for the next edge normal. + int nextEdge = edge + 1 < count1 ? edge + 1 : 0; + float sNext = EdgeSeparation(poly1, ref xf1, nextEdge, poly2, ref xf2); + + // Find the best edge and the search direction. + int bestEdge; + float bestSeparation; + int increment; + if (sPrev > s && sPrev > sNext) + { + increment = -1; + bestEdge = prevEdge; + bestSeparation = sPrev; + } + else if (sNext > s) + { + increment = 1; + bestEdge = nextEdge; + bestSeparation = sNext; + } + else + { + edgeIndex = edge; + return s; + } + + // Perform a local search for the best edge normal. + for (; ; ) + { + if (increment == -1) + edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1; + else + edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0; + + s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2); + + if (s > bestSeparation) + { + bestEdge = edge; + bestSeparation = s; + } + else + { + break; + } + } + + edgeIndex = bestEdge; + return bestSeparation; + } + + private static void FindIncidentEdge(out FixedArray2 c, PolygonShape poly1, ref Transform xf1, int edge1, PolygonShape poly2, ref Transform xf2) + { + c = new FixedArray2(); + Vertices normals1 = poly1.Normals; + + int count2 = poly2.Vertices.Count; + Vertices vertices2 = poly2.Vertices; + Vertices normals2 = poly2.Normals; + + Debug.Assert(0 <= edge1 && edge1 < poly1.Vertices.Count); + + // Get the normal of the reference edge in poly2's frame. + Vector2 normal1 = MathUtils.MulT(xf2.q, MathUtils.Mul(xf1.q, normals1[edge1])); + + + // Find the incident edge on poly2. + int index = 0; + float minDot = Settings.MaxFloat; + for (int i = 0; i < count2; ++i) + { + float dot = Vector2.Dot(normal1, normals2[i]); + if (dot < minDot) + { + minDot = dot; + index = i; + } + } + + // Build the clip vertices for the incident edge. + int i1 = index; + int i2 = i1 + 1 < count2 ? i1 + 1 : 0; + + ClipVertex cv0 = c[0]; + + cv0.V = MathUtils.Mul(ref xf2, vertices2[i1]); + cv0.ID.Features.IndexA = (byte)edge1; + cv0.ID.Features.IndexB = (byte)i1; + cv0.ID.Features.TypeA = (byte)ContactFeatureType.Face; + cv0.ID.Features.TypeB = (byte)ContactFeatureType.Vertex; + + c[0] = cv0; + + ClipVertex cv1 = c[1]; + cv1.V = MathUtils.Mul(ref xf2, vertices2[i2]); + cv1.ID.Features.IndexA = (byte)edge1; + cv1.ID.Features.IndexB = (byte)i2; + cv1.ID.Features.TypeA = (byte)ContactFeatureType.Face; + cv1.ID.Features.TypeB = (byte)ContactFeatureType.Vertex; + + c[1] = cv1; + } + } } \ No newline at end of file diff --git a/src/Box2DNet/Collision/Distance.cs b/src/Box2DNet/Collision/Distance.cs new file mode 100644 index 0000000..4f92fe7 --- /dev/null +++ b/src/Box2DNet/Collision/Distance.cs @@ -0,0 +1,794 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using Box2DNet.Collision.Shapes; +using Box2DNet.Common; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Collision +{ + /// + /// A distance proxy is used by the GJK algorithm. + /// It encapsulates any shape. + /// + public class DistanceProxy + { + internal float Radius; + internal Vertices Vertices = new Vertices(); + + // GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates. + + /// + /// Initialize the proxy using the given shape. The shape + /// must remain in scope while the proxy is in use. + /// + /// The shape. + /// The index. + public void Set(Shape shape, int index) + { + switch (shape.ShapeType) + { + case ShapeType.Circle: + { + CircleShape circle = (CircleShape)shape; + Vertices.Clear(); + Vertices.Add(circle.Position); + Radius = circle.Radius; + } + break; + + case ShapeType.Polygon: + { + PolygonShape polygon = (PolygonShape)shape; + Vertices.Clear(); + for (int i = 0; i < polygon.Vertices.Count; i++) + { + Vertices.Add(polygon.Vertices[i]); + } + Radius = polygon.Radius; + } + break; + + case ShapeType.Chain: + { + ChainShape chain = (ChainShape)shape; + Debug.Assert(0 <= index && index < chain.Vertices.Count); + Vertices.Clear(); + Vertices.Add(chain.Vertices[index]); + Vertices.Add(index + 1 < chain.Vertices.Count ? chain.Vertices[index + 1] : chain.Vertices[0]); + + Radius = chain.Radius; + } + break; + + case ShapeType.Edge: + { + EdgeShape edge = (EdgeShape)shape; + Vertices.Clear(); + Vertices.Add(edge.Vertex1); + Vertices.Add(edge.Vertex2); + Radius = edge.Radius; + } + break; + + default: + Debug.Assert(false); + break; + } + } + + /// + /// Get the supporting vertex index in the given direction. + /// + /// The direction. + /// + public int GetSupport(Vector2 direction) + { + int bestIndex = 0; + float bestValue = Vector2.Dot(Vertices[0], direction); + for (int i = 1; i < Vertices.Count; ++i) + { + float value = Vector2.Dot(Vertices[i], direction); + if (value > bestValue) + { + bestIndex = i; + bestValue = value; + } + } + + return bestIndex; + } + + /// + /// Get the supporting vertex in the given direction. + /// + /// The direction. + /// + public Vector2 GetSupportVertex(Vector2 direction) + { + int bestIndex = 0; + float bestValue = Vector2.Dot(Vertices[0], direction); + for (int i = 1; i < Vertices.Count; ++i) + { + float value = Vector2.Dot(Vertices[i], direction); + if (value > bestValue) + { + bestIndex = i; + bestValue = value; + } + } + + return Vertices[bestIndex]; + } + } + + /// + /// Used to warm start ComputeDistance. + /// Set count to zero on first call. + /// + public struct SimplexCache + { + /// + /// Length or area + /// + public ushort Count; + + /// + /// Vertices on shape A + /// + public FixedArray3 IndexA; + + /// + /// Vertices on shape B + /// + public FixedArray3 IndexB; + + public float Metric; + } + + /// + /// Input for Distance.ComputeDistance(). + /// You have to option to use the shape radii in the computation. + /// + public class DistanceInput + { + public DistanceProxy ProxyA = new DistanceProxy(); + public DistanceProxy ProxyB = new DistanceProxy(); + public Transform TransformA; + public Transform TransformB; + public bool UseRadii; + } + + /// + /// Output for Distance.ComputeDistance(). + /// + public struct DistanceOutput + { + public float Distance; + + /// + /// Number of GJK iterations used + /// + public int Iterations; + + /// + /// Closest point on shapeA + /// + public Vector2 PointA; + + /// + /// Closest point on shapeB + /// + public Vector2 PointB; + } + + internal struct SimplexVertex + { + /// + /// Barycentric coordinate for closest point + /// + public float A; + + /// + /// wA index + /// + public int IndexA; + + /// + /// wB index + /// + public int IndexB; + + /// + /// wB - wA + /// + public Vector2 W; + + /// + /// Support point in proxyA + /// + public Vector2 WA; + + /// + /// Support point in proxyB + /// + public Vector2 WB; + } + + internal struct Simplex + { + internal int Count; + internal FixedArray3 V; + + internal void ReadCache(ref SimplexCache cache, DistanceProxy proxyA, ref Transform transformA, DistanceProxy proxyB, ref Transform transformB) + { + Debug.Assert(cache.Count <= 3); + + // Copy data from cache. + Count = cache.Count; + for (int i = 0; i < Count; ++i) + { + SimplexVertex v = V[i]; + v.IndexA = cache.IndexA[i]; + v.IndexB = cache.IndexB[i]; + Vector2 wALocal = proxyA.Vertices[v.IndexA]; + Vector2 wBLocal = proxyB.Vertices[v.IndexB]; + v.WA = MathUtils.Mul(ref transformA, wALocal); + v.WB = MathUtils.Mul(ref transformB, wBLocal); + v.W = v.WB - v.WA; + v.A = 0.0f; + V[i] = v; + } + + // Compute the new simplex metric, if it is substantially different than + // old metric then flush the simplex. + if (Count > 1) + { + float metric1 = cache.Metric; + float metric2 = GetMetric(); + if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.Epsilon) + { + // Reset the simplex. + Count = 0; + } + } + + // If the cache is empty or invalid ... + if (Count == 0) + { + SimplexVertex v = V[0]; + v.IndexA = 0; + v.IndexB = 0; + Vector2 wALocal = proxyA.Vertices[0]; + Vector2 wBLocal = proxyB.Vertices[0]; + v.WA = MathUtils.Mul(ref transformA, wALocal); + v.WB = MathUtils.Mul(ref transformB, wBLocal); + v.W = v.WB - v.WA; + v.A = 1.0f; + V[0] = v; + Count = 1; + } + } + + internal void WriteCache(ref SimplexCache cache) + { + cache.Metric = GetMetric(); + cache.Count = (UInt16)Count; + for (int i = 0; i < Count; ++i) + { + cache.IndexA[i] = (byte)(V[i].IndexA); + cache.IndexB[i] = (byte)(V[i].IndexB); + } + } + + internal Vector2 GetSearchDirection() + { + switch (Count) + { + case 1: + return -V[0].W; + + case 2: + { + Vector2 e12 = V[1].W - V[0].W; + float sgn = MathUtils.Cross(e12, -V[0].W); + if (sgn > 0.0f) + { + // Origin is left of e12. + return new Vector2(-e12.Y, e12.X); + } + else + { + // Origin is right of e12. + return new Vector2(e12.Y, -e12.X); + } + } + + default: + Debug.Assert(false); + return Vector2.Zero; + } + } + + internal Vector2 GetClosestPoint() + { + switch (Count) + { + case 0: + Debug.Assert(false); + return Vector2.Zero; + + case 1: + return V[0].W; + + case 2: + return V[0].A * V[0].W + V[1].A * V[1].W; + + case 3: + return Vector2.Zero; + + default: + Debug.Assert(false); + return Vector2.Zero; + } + } + + internal void GetWitnessPoints(out Vector2 pA, out Vector2 pB) + { + switch (Count) + { + case 0: + pA = Vector2.Zero; + pB = Vector2.Zero; + Debug.Assert(false); + break; + + case 1: + pA = V[0].WA; + pB = V[0].WB; + break; + + case 2: + pA = V[0].A * V[0].WA + V[1].A * V[1].WA; + pB = V[0].A * V[0].WB + V[1].A * V[1].WB; + break; + + case 3: + pA = V[0].A * V[0].WA + V[1].A * V[1].WA + V[2].A * V[2].WA; + pB = pA; + break; + + default: + throw new Exception(); + } + } + + internal float GetMetric() + { + switch (Count) + { + case 0: + Debug.Assert(false); + return 0.0f; + case 1: + return 0.0f; + + case 2: + return (V[0].W - V[1].W).Length(); + + case 3: + return MathUtils.Cross(V[1].W - V[0].W, V[2].W - V[0].W); + + default: + Debug.Assert(false); + return 0.0f; + } + } + + // Solve a line segment using barycentric coordinates. + // + // p = a1 * w1 + a2 * w2 + // a1 + a2 = 1 + // + // The vector from the origin to the closest point on the line is + // perpendicular to the line. + // e12 = w2 - w1 + // dot(p, e) = 0 + // a1 * dot(w1, e) + a2 * dot(w2, e) = 0 + // + // 2-by-2 linear system + // [1 1 ][a1] = [1] + // [w1.e12 w2.e12][a2] = [0] + // + // Define + // d12_1 = dot(w2, e12) + // d12_2 = -dot(w1, e12) + // d12 = d12_1 + d12_2 + // + // Solution + // a1 = d12_1 / d12 + // a2 = d12_2 / d12 + + internal void Solve2() + { + Vector2 w1 = V[0].W; + Vector2 w2 = V[1].W; + Vector2 e12 = w2 - w1; + + // w1 region + float d12_2 = -Vector2.Dot(w1, e12); + if (d12_2 <= 0.0f) + { + // a2 <= 0, so we clamp it to 0 + SimplexVertex v0 = V[0]; + v0.A = 1.0f; + V[0] = v0; + Count = 1; + return; + } + + // w2 region + float d12_1 = Vector2.Dot(w2, e12); + if (d12_1 <= 0.0f) + { + // a1 <= 0, so we clamp it to 0 + SimplexVertex v1 = V[1]; + v1.A = 1.0f; + V[1] = v1; + Count = 1; + V[0] = V[1]; + return; + } + + // Must be in e12 region. + float inv_d12 = 1.0f / (d12_1 + d12_2); + SimplexVertex v0_2 = V[0]; + SimplexVertex v1_2 = V[1]; + v0_2.A = d12_1 * inv_d12; + v1_2.A = d12_2 * inv_d12; + V[0] = v0_2; + V[1] = v1_2; + Count = 2; + } + + // Possible regions: + // - points[2] + // - edge points[0]-points[2] + // - edge points[1]-points[2] + // - inside the triangle + internal void Solve3() + { + Vector2 w1 = V[0].W; + Vector2 w2 = V[1].W; + Vector2 w3 = V[2].W; + + // Edge12 + // [1 1 ][a1] = [1] + // [w1.e12 w2.e12][a2] = [0] + // a3 = 0 + Vector2 e12 = w2 - w1; + float w1e12 = Vector2.Dot(w1, e12); + float w2e12 = Vector2.Dot(w2, e12); + float d12_1 = w2e12; + float d12_2 = -w1e12; + + // Edge13 + // [1 1 ][a1] = [1] + // [w1.e13 w3.e13][a3] = [0] + // a2 = 0 + Vector2 e13 = w3 - w1; + float w1e13 = Vector2.Dot(w1, e13); + float w3e13 = Vector2.Dot(w3, e13); + float d13_1 = w3e13; + float d13_2 = -w1e13; + + // Edge23 + // [1 1 ][a2] = [1] + // [w2.e23 w3.e23][a3] = [0] + // a1 = 0 + Vector2 e23 = w3 - w2; + float w2e23 = Vector2.Dot(w2, e23); + float w3e23 = Vector2.Dot(w3, e23); + float d23_1 = w3e23; + float d23_2 = -w2e23; + + // Triangle123 + float n123 = MathUtils.Cross(e12, e13); + + float d123_1 = n123 * MathUtils.Cross(w2, w3); + float d123_2 = n123 * MathUtils.Cross(w3, w1); + float d123_3 = n123 * MathUtils.Cross(w1, w2); + + // w1 region + if (d12_2 <= 0.0f && d13_2 <= 0.0f) + { + SimplexVertex v0_1 = V[0]; + v0_1.A = 1.0f; + V[0] = v0_1; + Count = 1; + return; + } + + // e12 + if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f) + { + float inv_d12 = 1.0f / (d12_1 + d12_2); + SimplexVertex v0_2 = V[0]; + SimplexVertex v1_2 = V[1]; + v0_2.A = d12_1 * inv_d12; + v1_2.A = d12_2 * inv_d12; + V[0] = v0_2; + V[1] = v1_2; + Count = 2; + return; + } + + // e13 + if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f) + { + float inv_d13 = 1.0f / (d13_1 + d13_2); + SimplexVertex v0_3 = V[0]; + SimplexVertex v2_3 = V[2]; + v0_3.A = d13_1 * inv_d13; + v2_3.A = d13_2 * inv_d13; + V[0] = v0_3; + V[2] = v2_3; + Count = 2; + V[1] = V[2]; + return; + } + + // w2 region + if (d12_1 <= 0.0f && d23_2 <= 0.0f) + { + SimplexVertex v1_4 = V[1]; + v1_4.A = 1.0f; + V[1] = v1_4; + Count = 1; + V[0] = V[1]; + return; + } + + // w3 region + if (d13_1 <= 0.0f && d23_1 <= 0.0f) + { + SimplexVertex v2_5 = V[2]; + v2_5.A = 1.0f; + V[2] = v2_5; + Count = 1; + V[0] = V[2]; + return; + } + + // e23 + if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f) + { + float inv_d23 = 1.0f / (d23_1 + d23_2); + SimplexVertex v1_6 = V[1]; + SimplexVertex v2_6 = V[2]; + v1_6.A = d23_1 * inv_d23; + v2_6.A = d23_2 * inv_d23; + V[1] = v1_6; + V[2] = v2_6; + Count = 2; + V[0] = V[2]; + return; + } + + // Must be in triangle123 + float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3); + SimplexVertex v0_7 = V[0]; + SimplexVertex v1_7 = V[1]; + SimplexVertex v2_7 = V[2]; + v0_7.A = d123_1 * inv_d123; + v1_7.A = d123_2 * inv_d123; + v2_7.A = d123_3 * inv_d123; + V[0] = v0_7; + V[1] = v1_7; + V[2] = v2_7; + Count = 3; + } + } + + /// + /// The Gilbert–Johnson–Keerthi distance algorithm that provides the distance between shapes. + /// + public static class Distance + { + /// + /// The number of calls made to the ComputeDistance() function. + /// Note: This is only activated when Settings.EnableDiagnostics = true + /// + [ThreadStatic] + public static int GJKCalls; + + /// + /// The number of iterations that was made on the last call to ComputeDistance(). + /// Note: This is only activated when Settings.EnableDiagnostics = true + /// + [ThreadStatic] + public static int GJKIters; + + /// + /// The maximum numer of iterations ever mae with calls to the CompteDistance() funtion. + /// Note: This is only activated when Settings.EnableDiagnostics = true + /// + [ThreadStatic] + public static int GJKMaxIters; + + public static void ComputeDistance(out DistanceOutput output, out SimplexCache cache, DistanceInput input) + { + cache = new SimplexCache(); + + if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled + ++GJKCalls; + + // Initialize the simplex. + Simplex simplex = new Simplex(); + simplex.ReadCache(ref cache, input.ProxyA, ref input.TransformA, input.ProxyB, ref input.TransformB); + + // These store the vertices of the last simplex so that we + // can check for duplicates and prevent cycling. + FixedArray3 saveA = new FixedArray3(); + FixedArray3 saveB = new FixedArray3(); + + //float distanceSqr1 = Settings.MaxFloat; + + // Main iteration loop. + int iter = 0; + while (iter < Settings.MaxGJKIterations) + { + // Copy simplex so we can identify duplicates. + int saveCount = simplex.Count; + for (int i = 0; i < saveCount; ++i) + { + saveA[i] = simplex.V[i].IndexA; + saveB[i] = simplex.V[i].IndexB; + } + + switch (simplex.Count) + { + case 1: + break; + case 2: + simplex.Solve2(); + break; + case 3: + simplex.Solve3(); + break; + default: + Debug.Assert(false); + break; + } + + // If we have 3 points, then the origin is in the corresponding triangle. + if (simplex.Count == 3) + { + break; + } + + //FPE: This code was not used anyway. + // Compute closest point. + //Vector2 p = simplex.GetClosestPoint(); + //float distanceSqr2 = p.LengthSquared(); + + // Ensure progress + //if (distanceSqr2 >= distanceSqr1) + //{ + //break; + //} + //distanceSqr1 = distanceSqr2; + + // Get search direction. + Vector2 d = simplex.GetSearchDirection(); + + // Ensure the search direction is numerically fit. + if (d.LengthSquared() < Settings.Epsilon * Settings.Epsilon) + { + // The origin is probably contained by a line segment + // or triangle. Thus the shapes are overlapped. + + // We can't return zero here even though there may be overlap. + // In case the simplex is a point, segment, or triangle it is difficult + // to determine if the origin is contained in the CSO or very close to it. + break; + } + + // Compute a tentative new simplex vertex using support points. + SimplexVertex vertex = simplex.V[simplex.Count]; + vertex.IndexA = input.ProxyA.GetSupport(MathUtils.MulT(input.TransformA.q, -d)); + vertex.WA = MathUtils.Mul(ref input.TransformA, input.ProxyA.Vertices[vertex.IndexA]); + + vertex.IndexB = input.ProxyB.GetSupport(MathUtils.MulT(input.TransformB.q, d)); + vertex.WB = MathUtils.Mul(ref input.TransformB, input.ProxyB.Vertices[vertex.IndexB]); + vertex.W = vertex.WB - vertex.WA; + simplex.V[simplex.Count] = vertex; + + // Iteration count is equated to the number of support point calls. + ++iter; + + if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled + ++GJKIters; + + // Check for duplicate support points. This is the main termination criteria. + bool duplicate = false; + for (int i = 0; i < saveCount; ++i) + { + if (vertex.IndexA == saveA[i] && vertex.IndexB == saveB[i]) + { + duplicate = true; + break; + } + } + + // If we found a duplicate support point we must exit to avoid cycling. + if (duplicate) + { + break; + } + + // New vertex is ok and needed. + ++simplex.Count; + } + + if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled + GJKMaxIters = Math.Max(GJKMaxIters, iter); + + // Prepare output. + simplex.GetWitnessPoints(out output.PointA, out output.PointB); + output.Distance = (output.PointA - output.PointB).Length(); + output.Iterations = iter; + + // Cache the simplex. + simplex.WriteCache(ref cache); + + // Apply radii if requested. + if (input.UseRadii) + { + float rA = input.ProxyA.Radius; + float rB = input.ProxyB.Radius; + + if (output.Distance > rA + rB && output.Distance > Settings.Epsilon) + { + // Shapes are still no overlapped. + // Move the witness points to the outer surface. + output.Distance -= rA + rB; + Vector2 normal = output.PointB - output.PointA; + normal.Normalize(); + output.PointA += rA * normal; + output.PointB -= rB * normal; + } + else + { + // Shapes are overlapped when radii are considered. + // Move the witness points to the middle. + Vector2 p = 0.5f * (output.PointA + output.PointB); + output.PointA = p; + output.PointB = p; + output.Distance = 0.0f; + } + } + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Collision/DynamicTree.cs b/src/Box2DNet/Collision/DynamicTree.cs new file mode 100644 index 0000000..7870639 --- /dev/null +++ b/src/Box2DNet/Collision/DynamicTree.cs @@ -0,0 +1,1034 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Diagnostics; +using Box2DNet.Common; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Collision +{ + /// + /// A node in the dynamic tree. The client does not interact with this directly. + /// + internal class TreeNode + { + /// + /// Enlarged AABB + /// + internal AABB AABB; + + internal int Child1; + internal int Child2; + + internal int Height; + internal int ParentOrNext; + internal T UserData; + + internal bool IsLeaf() + { + return Child1 == DynamicTree.NullNode; + } + } + + /// + /// A dynamic tree arranges data in a binary tree to accelerate + /// queries such as volume queries and ray casts. Leafs are proxies + /// with an AABB. In the tree we expand the proxy AABB by Settings.b2_fatAABBFactor + /// so that the proxy AABB is bigger than the client object. This allows the client + /// object to move by small amounts without triggering a tree update. + /// + /// Nodes are pooled and relocatable, so we use node indices rather than pointers. + /// + public class DynamicTree + { + private Stack _raycastStack = new Stack(256); + private Stack _queryStack = new Stack(256); + private int _freeList; + private int _nodeCapacity; + private int _nodeCount; + private TreeNode[] _nodes; + private int _root; + internal const int NullNode = -1; + + /// + /// Constructing the tree initializes the node pool. + /// + public DynamicTree() + { + _root = NullNode; + + _nodeCapacity = 16; + _nodeCount = 0; + _nodes = new TreeNode[_nodeCapacity]; + + // Build a linked list for the free list. + for (int i = 0; i < _nodeCapacity - 1; ++i) + { + _nodes[i] = new TreeNode(); + _nodes[i].ParentOrNext = i + 1; + _nodes[i].Height = 1; + } + _nodes[_nodeCapacity - 1] = new TreeNode(); + _nodes[_nodeCapacity - 1].ParentOrNext = NullNode; + _nodes[_nodeCapacity - 1].Height = 1; + _freeList = 0; + } + + /// + /// Compute the height of the binary tree in O(N) time. Should not be called often. + /// + public int Height + { + get + { + if (_root == NullNode) + { + return 0; + } + + return _nodes[_root].Height; + } + } + + /// + /// Get the ratio of the sum of the node areas to the root area. + /// + public float AreaRatio + { + get + { + if (_root == NullNode) + { + return 0.0f; + } + + TreeNode root = _nodes[_root]; + float rootArea = root.AABB.Perimeter; + + float totalArea = 0.0f; + for (int i = 0; i < _nodeCapacity; ++i) + { + TreeNode node = _nodes[i]; + if (node.Height < 0) + { + // Free node in pool + continue; + } + + totalArea += node.AABB.Perimeter; + } + + return totalArea / rootArea; + } + } + + /// + /// Get the maximum balance of an node in the tree. The balance is the difference + /// in height of the two children of a node. + /// + public int MaxBalance + { + get + { + int maxBalance = 0; + for (int i = 0; i < _nodeCapacity; ++i) + { + TreeNode node = _nodes[i]; + if (node.Height <= 1) + { + continue; + } + + Debug.Assert(node.IsLeaf() == false); + + int child1 = node.Child1; + int child2 = node.Child2; + int balance = Math.Abs(_nodes[child2].Height - _nodes[child1].Height); + maxBalance = Math.Max(maxBalance, balance); + } + + return maxBalance; + } + } + + /// + /// Create a proxy in the tree as a leaf node. We return the index + /// of the node instead of a pointer so that we can grow + /// the node pool. + /// /// + /// The aabb. + /// The user data. + /// Index of the created proxy + public int AddProxy(ref AABB aabb, T userData) + { + int proxyId = AllocateNode(); + + // Fatten the aabb. + Vector2 r = new Vector2(Settings.AABBExtension, Settings.AABBExtension); + _nodes[proxyId].AABB.LowerBound = aabb.LowerBound - r; + _nodes[proxyId].AABB.UpperBound = aabb.UpperBound + r; + _nodes[proxyId].UserData = userData; + _nodes[proxyId].Height = 0; + + InsertLeaf(proxyId); + + return proxyId; + } + + /// + /// Destroy a proxy. This asserts if the id is invalid. + /// + /// The proxy id. + public void RemoveProxy(int proxyId) + { + Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity); + Debug.Assert(_nodes[proxyId].IsLeaf()); + + RemoveLeaf(proxyId); + FreeNode(proxyId); + } + + /// + /// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB, + /// then the proxy is removed from the tree and re-inserted. Otherwise + /// the function returns immediately. + /// + /// The proxy id. + /// The aabb. + /// The displacement. + /// true if the proxy was re-inserted. + public bool MoveProxy(int proxyId, ref AABB aabb, Vector2 displacement) + { + Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity); + + Debug.Assert(_nodes[proxyId].IsLeaf()); + + if (_nodes[proxyId].AABB.Contains(ref aabb)) + { + return false; + } + + RemoveLeaf(proxyId); + + // Extend AABB. + AABB b = aabb; + Vector2 r = new Vector2(Settings.AABBExtension, Settings.AABBExtension); + b.LowerBound = b.LowerBound - r; + b.UpperBound = b.UpperBound + r; + + // Predict AABB displacement. + Vector2 d = Settings.AABBMultiplier * displacement; + + if (d.X < 0.0f) + { + b.LowerBound.X += d.X; + } + else + { + b.UpperBound.X += d.X; + } + + if (d.Y < 0.0f) + { + b.LowerBound.Y += d.Y; + } + else + { + b.UpperBound.Y += d.Y; + } + + _nodes[proxyId].AABB = b; + + InsertLeaf(proxyId); + return true; + } + + /// + /// Get proxy user data. + /// + /// + /// The proxy id. + /// the proxy user data or 0 if the id is invalid. + public T GetUserData(int proxyId) + { + Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity); + return _nodes[proxyId].UserData; + } + + /// + /// Get the fat AABB for a proxy. + /// + /// The proxy id. + /// The fat AABB. + public void GetFatAABB(int proxyId, out AABB fatAABB) + { + Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity); + fatAABB = _nodes[proxyId].AABB; + } + + /// + /// Query an AABB for overlapping proxies. The callback class + /// is called for each proxy that overlaps the supplied AABB. + /// + /// The callback. + /// The aabb. + public void Query(Func callback, ref AABB aabb) + { + _queryStack.Clear(); + _queryStack.Push(_root); + + while (_queryStack.Count > 0) + { + int nodeId = _queryStack.Pop(); + if (nodeId == NullNode) + { + continue; + } + + TreeNode node = _nodes[nodeId]; + + if (AABB.TestOverlap(ref node.AABB, ref aabb)) + { + if (node.IsLeaf()) + { + bool proceed = callback(nodeId); + if (proceed == false) + { + return; + } + } + else + { + _queryStack.Push(node.Child1); + _queryStack.Push(node.Child2); + } + } + } + } + + /// + /// Ray-cast against the proxies in the tree. This relies on the callback + /// to perform a exact ray-cast in the case were the proxy contains a Shape. + /// The callback also performs the any collision filtering. This has performance + /// roughly equal to k * log(n), where k is the number of collisions and n is the + /// number of proxies in the tree. + /// + /// A callback class that is called for each proxy that is hit by the ray. + /// The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). + public void RayCast(Func callback, ref RayCastInput input) + { + Vector2 p1 = input.Point1; + Vector2 p2 = input.Point2; + Vector2 r = p2 - p1; + Debug.Assert(r.LengthSquared() > 0.0f); + r.Normalize(); + + // v is perpendicular to the segment. + Vector2 absV = MathUtils.Abs(new Vector2(-r.Y, r.X)); //FPE: Inlined the 'v' variable + + // Separating axis for segment (Gino, p80). + // |dot(v, p1 - c)| > dot(|v|, h) + + float maxFraction = input.MaxFraction; + + // Build a bounding box for the segment. + AABB segmentAABB = new AABB(); + { + Vector2 t = p1 + maxFraction * (p2 - p1); + Vector2.Min(ref p1, ref t, out segmentAABB.LowerBound); + Vector2.Max(ref p1, ref t, out segmentAABB.UpperBound); + } + + _raycastStack.Clear(); + _raycastStack.Push(_root); + + while (_raycastStack.Count > 0) + { + int nodeId = _raycastStack.Pop(); + if (nodeId == NullNode) + { + continue; + } + + TreeNode node = _nodes[nodeId]; + + if (AABB.TestOverlap(ref node.AABB, ref segmentAABB) == false) + { + continue; + } + + // Separating axis for segment (Gino, p80). + // |dot(v, p1 - c)| > dot(|v|, h) + Vector2 c = node.AABB.Center; + Vector2 h = node.AABB.Extents; + float separation = Math.Abs(Vector2.Dot(new Vector2(-r.Y, r.X), p1 - c)) - Vector2.Dot(absV, h); + if (separation > 0.0f) + { + continue; + } + + if (node.IsLeaf()) + { + RayCastInput subInput; + subInput.Point1 = input.Point1; + subInput.Point2 = input.Point2; + subInput.MaxFraction = maxFraction; + + float value = callback(subInput, nodeId); + + if (value == 0.0f) + { + // the client has terminated the raycast. + return; + } + + if (value > 0.0f) + { + // Update segment bounding box. + maxFraction = value; + Vector2 t = p1 + maxFraction * (p2 - p1); + segmentAABB.LowerBound = Vector2.Min(p1, t); + segmentAABB.UpperBound = Vector2.Max(p1, t); + } + } + else + { + _raycastStack.Push(node.Child1); + _raycastStack.Push(node.Child2); + } + } + } + + private int AllocateNode() + { + // Expand the node pool as needed. + if (_freeList == NullNode) + { + Debug.Assert(_nodeCount == _nodeCapacity); + + // The free list is empty. Rebuild a bigger pool. + TreeNode[] oldNodes = _nodes; + _nodeCapacity *= 2; + _nodes = new TreeNode[_nodeCapacity]; + Array.Copy(oldNodes, _nodes, _nodeCount); + + // Build a linked list for the free list. The parent + // pointer becomes the "next" pointer. + for (int i = _nodeCount; i < _nodeCapacity - 1; ++i) + { + _nodes[i] = new TreeNode(); + _nodes[i].ParentOrNext = i + 1; + _nodes[i].Height = -1; + } + _nodes[_nodeCapacity - 1] = new TreeNode(); + _nodes[_nodeCapacity - 1].ParentOrNext = NullNode; + _nodes[_nodeCapacity - 1].Height = -1; + _freeList = _nodeCount; + } + + // Peel a node off the free list. + int nodeId = _freeList; + _freeList = _nodes[nodeId].ParentOrNext; + _nodes[nodeId].ParentOrNext = NullNode; + _nodes[nodeId].Child1 = NullNode; + _nodes[nodeId].Child2 = NullNode; + _nodes[nodeId].Height = 0; + _nodes[nodeId].UserData = default(T); + ++_nodeCount; + return nodeId; + } + + private void FreeNode(int nodeId) + { + Debug.Assert(0 <= nodeId && nodeId < _nodeCapacity); + Debug.Assert(0 < _nodeCount); + _nodes[nodeId].ParentOrNext = _freeList; + _nodes[nodeId].Height = -1; + _freeList = nodeId; + --_nodeCount; + } + + private void InsertLeaf(int leaf) + { + if (_root == NullNode) + { + _root = leaf; + _nodes[_root].ParentOrNext = NullNode; + return; + } + + // Find the best sibling for this node + AABB leafAABB = _nodes[leaf].AABB; + int index = _root; + while (_nodes[index].IsLeaf() == false) + { + int child1 = _nodes[index].Child1; + int child2 = _nodes[index].Child2; + + float area = _nodes[index].AABB.Perimeter; + + AABB combinedAABB = new AABB(); + combinedAABB.Combine(ref _nodes[index].AABB, ref leafAABB); + float combinedArea = combinedAABB.Perimeter; + + // Cost of creating a new parent for this node and the new leaf + float cost = 2.0f * combinedArea; + + // Minimum cost of pushing the leaf further down the tree + float inheritanceCost = 2.0f * (combinedArea - area); + + // Cost of descending into child1 + float cost1; + if (_nodes[child1].IsLeaf()) + { + AABB aabb = new AABB(); + aabb.Combine(ref leafAABB, ref _nodes[child1].AABB); + cost1 = aabb.Perimeter + inheritanceCost; + } + else + { + AABB aabb = new AABB(); + aabb.Combine(ref leafAABB, ref _nodes[child1].AABB); + float oldArea = _nodes[child1].AABB.Perimeter; + float newArea = aabb.Perimeter; + cost1 = (newArea - oldArea) + inheritanceCost; + } + + // Cost of descending into child2 + float cost2; + if (_nodes[child2].IsLeaf()) + { + AABB aabb = new AABB(); + aabb.Combine(ref leafAABB, ref _nodes[child2].AABB); + cost2 = aabb.Perimeter + inheritanceCost; + } + else + { + AABB aabb = new AABB(); + aabb.Combine(ref leafAABB, ref _nodes[child2].AABB); + float oldArea = _nodes[child2].AABB.Perimeter; + float newArea = aabb.Perimeter; + cost2 = newArea - oldArea + inheritanceCost; + } + + // Descend according to the minimum cost. + if (cost < cost1 && cost1 < cost2) + { + break; + } + + // Descend + if (cost1 < cost2) + { + index = child1; + } + else + { + index = child2; + } + } + + int sibling = index; + + // Create a new parent. + int oldParent = _nodes[sibling].ParentOrNext; + int newParent = AllocateNode(); + _nodes[newParent].ParentOrNext = oldParent; + _nodes[newParent].UserData = default(T); + _nodes[newParent].AABB.Combine(ref leafAABB, ref _nodes[sibling].AABB); + _nodes[newParent].Height = _nodes[sibling].Height + 1; + + if (oldParent != NullNode) + { + // The sibling was not the root. + if (_nodes[oldParent].Child1 == sibling) + { + _nodes[oldParent].Child1 = newParent; + } + else + { + _nodes[oldParent].Child2 = newParent; + } + + _nodes[newParent].Child1 = sibling; + _nodes[newParent].Child2 = leaf; + _nodes[sibling].ParentOrNext = newParent; + _nodes[leaf].ParentOrNext = newParent; + } + else + { + // The sibling was the root. + _nodes[newParent].Child1 = sibling; + _nodes[newParent].Child2 = leaf; + _nodes[sibling].ParentOrNext = newParent; + _nodes[leaf].ParentOrNext = newParent; + _root = newParent; + } + + // Walk back up the tree fixing heights and AABBs + index = _nodes[leaf].ParentOrNext; + while (index != NullNode) + { + index = Balance(index); + + int child1 = _nodes[index].Child1; + int child2 = _nodes[index].Child2; + + Debug.Assert(child1 != NullNode); + Debug.Assert(child2 != NullNode); + + _nodes[index].Height = 1 + Math.Max(_nodes[child1].Height, _nodes[child2].Height); + _nodes[index].AABB.Combine(ref _nodes[child1].AABB, ref _nodes[child2].AABB); + + index = _nodes[index].ParentOrNext; + } + + //Validate(); + } + + private void RemoveLeaf(int leaf) + { + if (leaf == _root) + { + _root = NullNode; + return; + } + + int parent = _nodes[leaf].ParentOrNext; + int grandParent = _nodes[parent].ParentOrNext; + int sibling; + if (_nodes[parent].Child1 == leaf) + { + sibling = _nodes[parent].Child2; + } + else + { + sibling = _nodes[parent].Child1; + } + + if (grandParent != NullNode) + { + // Destroy parent and connect sibling to grandParent. + if (_nodes[grandParent].Child1 == parent) + { + _nodes[grandParent].Child1 = sibling; + } + else + { + _nodes[grandParent].Child2 = sibling; + } + _nodes[sibling].ParentOrNext = grandParent; + FreeNode(parent); + + // Adjust ancestor bounds. + int index = grandParent; + while (index != NullNode) + { + index = Balance(index); + + int child1 = _nodes[index].Child1; + int child2 = _nodes[index].Child2; + + _nodes[index].AABB.Combine(ref _nodes[child1].AABB, ref _nodes[child2].AABB); + _nodes[index].Height = 1 + Math.Max(_nodes[child1].Height, _nodes[child2].Height); + + index = _nodes[index].ParentOrNext; + } + } + else + { + _root = sibling; + _nodes[sibling].ParentOrNext = NullNode; + FreeNode(parent); + } + + //Validate(); + } + + /// + /// Perform a left or right rotation if node A is imbalanced. + /// + /// + /// the new root index. + private int Balance(int iA) + { + Debug.Assert(iA != NullNode); + + TreeNode A = _nodes[iA]; + if (A.IsLeaf() || A.Height < 2) + { + return iA; + } + + int iB = A.Child1; + int iC = A.Child2; + Debug.Assert(0 <= iB && iB < _nodeCapacity); + Debug.Assert(0 <= iC && iC < _nodeCapacity); + + TreeNode B = _nodes[iB]; + TreeNode C = _nodes[iC]; + + int balance = C.Height - B.Height; + + // Rotate C up + if (balance > 1) + { + int iF = C.Child1; + int iG = C.Child2; + TreeNode F = _nodes[iF]; + TreeNode G = _nodes[iG]; + Debug.Assert(0 <= iF && iF < _nodeCapacity); + Debug.Assert(0 <= iG && iG < _nodeCapacity); + + // Swap A and C + C.Child1 = iA; + C.ParentOrNext = A.ParentOrNext; + A.ParentOrNext = iC; + + // A's old parent should point to C + if (C.ParentOrNext != NullNode) + { + if (_nodes[C.ParentOrNext].Child1 == iA) + { + _nodes[C.ParentOrNext].Child1 = iC; + } + else + { + Debug.Assert(_nodes[C.ParentOrNext].Child2 == iA); + _nodes[C.ParentOrNext].Child2 = iC; + } + } + else + { + _root = iC; + } + + // Rotate + if (F.Height > G.Height) + { + C.Child2 = iF; + A.Child2 = iG; + G.ParentOrNext = iA; + A.AABB.Combine(ref B.AABB, ref G.AABB); + C.AABB.Combine(ref A.AABB, ref F.AABB); + + A.Height = 1 + Math.Max(B.Height, G.Height); + C.Height = 1 + Math.Max(A.Height, F.Height); + } + else + { + C.Child2 = iG; + A.Child2 = iF; + F.ParentOrNext = iA; + A.AABB.Combine(ref B.AABB, ref F.AABB); + C.AABB.Combine(ref A.AABB, ref G.AABB); + + A.Height = 1 + Math.Max(B.Height, F.Height); + C.Height = 1 + Math.Max(A.Height, G.Height); + } + + return iC; + } + + // Rotate B up + if (balance < -1) + { + int iD = B.Child1; + int iE = B.Child2; + TreeNode D = _nodes[iD]; + TreeNode E = _nodes[iE]; + Debug.Assert(0 <= iD && iD < _nodeCapacity); + Debug.Assert(0 <= iE && iE < _nodeCapacity); + + // Swap A and B + B.Child1 = iA; + B.ParentOrNext = A.ParentOrNext; + A.ParentOrNext = iB; + + // A's old parent should point to B + if (B.ParentOrNext != NullNode) + { + if (_nodes[B.ParentOrNext].Child1 == iA) + { + _nodes[B.ParentOrNext].Child1 = iB; + } + else + { + Debug.Assert(_nodes[B.ParentOrNext].Child2 == iA); + _nodes[B.ParentOrNext].Child2 = iB; + } + } + else + { + _root = iB; + } + + // Rotate + if (D.Height > E.Height) + { + B.Child2 = iD; + A.Child1 = iE; + E.ParentOrNext = iA; + A.AABB.Combine(ref C.AABB, ref E.AABB); + B.AABB.Combine(ref A.AABB, ref D.AABB); + + A.Height = 1 + Math.Max(C.Height, E.Height); + B.Height = 1 + Math.Max(A.Height, D.Height); + } + else + { + B.Child2 = iE; + A.Child1 = iD; + D.ParentOrNext = iA; + A.AABB.Combine(ref C.AABB, ref D.AABB); + B.AABB.Combine(ref A.AABB, ref E.AABB); + + A.Height = 1 + Math.Max(C.Height, D.Height); + B.Height = 1 + Math.Max(A.Height, E.Height); + } + + return iB; + } + + return iA; + } + + /// + /// Compute the height of a sub-tree. + /// + /// The node id to use as parent. + /// The height of the tree. + public int ComputeHeight(int nodeId) + { + Debug.Assert(0 <= nodeId && nodeId < _nodeCapacity); + TreeNode node = _nodes[nodeId]; + + if (node.IsLeaf()) + { + return 0; + } + + int height1 = ComputeHeight(node.Child1); + int height2 = ComputeHeight(node.Child2); + return 1 + Math.Max(height1, height2); + } + + /// + /// Compute the height of the entire tree. + /// + /// The height of the tree. + public int ComputeHeight() + { + int height = ComputeHeight(_root); + return height; + } + + public void ValidateStructure(int index) + { + if (index == NullNode) + { + return; + } + + if (index == _root) + { + Debug.Assert(_nodes[index].ParentOrNext == NullNode); + } + + TreeNode node = _nodes[index]; + + int child1 = node.Child1; + int child2 = node.Child2; + + if (node.IsLeaf()) + { + Debug.Assert(child1 == NullNode); + Debug.Assert(child2 == NullNode); + Debug.Assert(node.Height == 0); + return; + } + + Debug.Assert(0 <= child1 && child1 < _nodeCapacity); + Debug.Assert(0 <= child2 && child2 < _nodeCapacity); + + Debug.Assert(_nodes[child1].ParentOrNext == index); + Debug.Assert(_nodes[child2].ParentOrNext == index); + + ValidateStructure(child1); + ValidateStructure(child2); + } + + public void ValidateMetrics(int index) + { + if (index == NullNode) + { + return; + } + + TreeNode node = _nodes[index]; + + int child1 = node.Child1; + int child2 = node.Child2; + + if (node.IsLeaf()) + { + Debug.Assert(child1 == NullNode); + Debug.Assert(child2 == NullNode); + Debug.Assert(node.Height == 0); + return; + } + + Debug.Assert(0 <= child1 && child1 < _nodeCapacity); + Debug.Assert(0 <= child2 && child2 < _nodeCapacity); + + int height1 = _nodes[child1].Height; + int height2 = _nodes[child2].Height; + int height = 1 + Math.Max(height1, height2); + Debug.Assert(node.Height == height); + + AABB AABB = new AABB(); + AABB.Combine(ref _nodes[child1].AABB, ref _nodes[child2].AABB); + + Debug.Assert(AABB.LowerBound == node.AABB.LowerBound); + Debug.Assert(AABB.UpperBound == node.AABB.UpperBound); + + ValidateMetrics(child1); + ValidateMetrics(child2); + } + + /// + /// Validate this tree. For testing. + /// + public void Validate() + { + ValidateStructure(_root); + ValidateMetrics(_root); + + int freeCount = 0; + int freeIndex = _freeList; + while (freeIndex != NullNode) + { + Debug.Assert(0 <= freeIndex && freeIndex < _nodeCapacity); + freeIndex = _nodes[freeIndex].ParentOrNext; + ++freeCount; + } + + Debug.Assert(Height == ComputeHeight()); + + Debug.Assert(_nodeCount + freeCount == _nodeCapacity); + } + + /// + /// Build an optimal tree. Very expensive. For testing. + /// + public void RebuildBottomUp() + { + int[] nodes = new int[_nodeCount]; + int count = 0; + + // Build array of leaves. Free the rest. + for (int i = 0; i < _nodeCapacity; ++i) + { + if (_nodes[i].Height < 0) + { + // free node in pool + continue; + } + + if (_nodes[i].IsLeaf()) + { + _nodes[i].ParentOrNext = NullNode; + nodes[count] = i; + ++count; + } + else + { + FreeNode(i); + } + } + + while (count > 1) + { + float minCost = Settings.MaxFloat; + int iMin = -1, jMin = -1; + for (int i = 0; i < count; ++i) + { + AABB AABBi = _nodes[nodes[i]].AABB; + + for (int j = i + 1; j < count; ++j) + { + AABB AABBj = _nodes[nodes[j]].AABB; + AABB b = new AABB(); + b.Combine(ref AABBi, ref AABBj); + float cost = b.Perimeter; + if (cost < minCost) + { + iMin = i; + jMin = j; + minCost = cost; + } + } + } + + int index1 = nodes[iMin]; + int index2 = nodes[jMin]; + TreeNode child1 = _nodes[index1]; + TreeNode child2 = _nodes[index2]; + + int parentIndex = AllocateNode(); + TreeNode parent = _nodes[parentIndex]; + parent.Child1 = index1; + parent.Child2 = index2; + parent.Height = 1 + Math.Max(child1.Height, child2.Height); + parent.AABB.Combine(ref child1.AABB, ref child2.AABB); + parent.ParentOrNext = NullNode; + + child1.ParentOrNext = parentIndex; + child2.ParentOrNext = parentIndex; + + nodes[jMin] = nodes[count - 1]; + nodes[iMin] = parentIndex; + --count; + } + + _root = nodes[0]; + + Validate(); + } + + /// + /// Shift the origin of the nodes + /// + /// The displacement to use. + public void ShiftOrigin(Vector2 newOrigin) + { + // Build array of leaves. Free the rest. + for (int i = 0; i < _nodeCapacity; ++i) + { + _nodes[i].AABB.LowerBound -= newOrigin; + _nodes[i].AABB.UpperBound -= newOrigin; + } + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Collision/DynamicTreeBroadPhase.cs b/src/Box2DNet/Collision/DynamicTreeBroadPhase.cs new file mode 100644 index 0000000..5631497 --- /dev/null +++ b/src/Box2DNet/Collision/DynamicTreeBroadPhase.cs @@ -0,0 +1,347 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using Box2DNet.Dynamics; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Collision +{ + internal struct Pair : IComparable + { + public int ProxyIdA; + public int ProxyIdB; + + #region IComparable Members + + public int CompareTo(Pair other) + { + if (ProxyIdA < other.ProxyIdA) + { + return -1; + } + if (ProxyIdA == other.ProxyIdA) + { + if (ProxyIdB < other.ProxyIdB) + { + return -1; + } + if (ProxyIdB == other.ProxyIdB) + { + return 0; + } + } + + return 1; + } + + #endregion + } + + /// + /// The broad-phase is used for computing pairs and performing volume queries and ray casts. + /// This broad-phase does not persist pairs. Instead, this reports potentially new pairs. + /// It is up to the client to consume the new pairs and to track subsequent overlap. + /// + public class DynamicTreeBroadPhase : IBroadPhase + { + private const int NullProxy = -1; + private int[] _moveBuffer; + private int _moveCapacity; + private int _moveCount; + + private Pair[] _pairBuffer; + private int _pairCapacity; + private int _pairCount; + private int _proxyCount; + private Func _queryCallback; + private int _queryProxyId; + private DynamicTree _tree = new DynamicTree(); + + /// + /// Constructs a new broad phase based on the dynamic tree implementation + /// + public DynamicTreeBroadPhase() + { + _queryCallback = QueryCallback; + _proxyCount = 0; + + _pairCapacity = 16; + _pairCount = 0; + _pairBuffer = new Pair[_pairCapacity]; + + _moveCapacity = 16; + _moveCount = 0; + _moveBuffer = new int[_moveCapacity]; + } + + /// + /// Get the number of proxies. + /// + /// The proxy count. + public int ProxyCount + { + get { return _proxyCount; } + } + + /// + /// Create a proxy with an initial AABB. Pairs are not reported until + /// UpdatePairs is called. + /// + /// The user data. + /// + public int AddProxy(ref FixtureProxy proxy) + { + int proxyId = _tree.AddProxy(ref proxy.AABB, proxy); + ++_proxyCount; + BufferMove(proxyId); + return proxyId; + } + + /// + /// Destroy a proxy. It is up to the client to remove any pairs. + /// + /// The proxy id. + public void RemoveProxy(int proxyId) + { + UnBufferMove(proxyId); + --_proxyCount; + _tree.RemoveProxy(proxyId); + } + + public void MoveProxy(int proxyId, ref AABB aabb, Vector2 displacement) + { + bool buffer = _tree.MoveProxy(proxyId, ref aabb, displacement); + if (buffer) + { + BufferMove(proxyId); + } + } + + public void TouchProxy(int proxyId) + { + BufferMove(proxyId); + } + + private void BufferMove(int proxyId) + { + if (_moveCount == _moveCapacity) + { + int[] oldBuffer = _moveBuffer; + _moveCapacity *= 2; + _moveBuffer = new int[_moveCapacity]; + Array.Copy(oldBuffer, _moveBuffer, _moveCount); + } + + _moveBuffer[_moveCount] = proxyId; + ++_moveCount; + } + + private void UnBufferMove(int proxyId) + { + for (int i = 0; i < _moveCount; ++i) + { + if (_moveBuffer[i] == proxyId) + { + _moveBuffer[i] = NullProxy; + } + } + } + + /// + /// This is called from DynamicTree.Query when we are gathering pairs. + /// + /// + /// + private bool QueryCallback(int proxyId) + { + // A proxy cannot form a pair with itself. + if (proxyId == _queryProxyId) + { + return true; + } + + // Grow the pair buffer as needed. + if (_pairCount == _pairCapacity) + { + Pair[] oldBuffer = _pairBuffer; + _pairCapacity *= 2; + _pairBuffer = new Pair[_pairCapacity]; + Array.Copy(oldBuffer, _pairBuffer, _pairCount); + } + + _pairBuffer[_pairCount].ProxyIdA = Math.Min(proxyId, _queryProxyId); + _pairBuffer[_pairCount].ProxyIdB = Math.Max(proxyId, _queryProxyId); + ++_pairCount; + + return true; + } + + /// + /// Get the AABB for a proxy. + /// + /// The proxy id. + /// The aabb. + public void GetFatAABB(int proxyId, out AABB aabb) + { + _tree.GetFatAABB(proxyId, out aabb); + } + + /// + /// Get user data from a proxy. Returns null if the id is invalid. + /// + /// The proxy id. + /// + public FixtureProxy GetProxy(int proxyId) + { + return _tree.GetUserData(proxyId); + } + + /// + /// Test overlap of fat AABBs. + /// + /// The proxy id A. + /// The proxy id B. + /// + public bool TestOverlap(int proxyIdA, int proxyIdB) + { + AABB aabbA, aabbB; + _tree.GetFatAABB(proxyIdA, out aabbA); + _tree.GetFatAABB(proxyIdB, out aabbB); + return AABB.TestOverlap(ref aabbA, ref aabbB); + } + + /// + /// Update the pairs. This results in pair callbacks. This can only add pairs. + /// + /// The callback. + public void UpdatePairs(BroadphaseDelegate callback) + { + // Reset pair buffer + _pairCount = 0; + + // Perform tree queries for all moving proxies. + for (int j = 0; j < _moveCount; ++j) + { + _queryProxyId = _moveBuffer[j]; + if (_queryProxyId == NullProxy) + { + continue; + } + + // We have to query the tree with the fat AABB so that + // we don't fail to create a pair that may touch later. + AABB fatAABB; + _tree.GetFatAABB(_queryProxyId, out fatAABB); + + // Query tree, create pairs and add them pair buffer. + _tree.Query(_queryCallback, ref fatAABB); + } + + // Reset move buffer + _moveCount = 0; + + // Sort the pair buffer to expose duplicates. + Array.Sort(_pairBuffer, 0, _pairCount); + + // Send the pairs back to the client. + int i = 0; + while (i < _pairCount) + { + Pair primaryPair = _pairBuffer[i]; + FixtureProxy userDataA = _tree.GetUserData(primaryPair.ProxyIdA); + FixtureProxy userDataB = _tree.GetUserData(primaryPair.ProxyIdB); + + callback(ref userDataA, ref userDataB); + ++i; + + // Skip any duplicate pairs. + while (i < _pairCount) + { + Pair pair = _pairBuffer[i]; + if (pair.ProxyIdA != primaryPair.ProxyIdA || pair.ProxyIdB != primaryPair.ProxyIdB) + { + break; + } + ++i; + } + } + + // Try to keep the tree balanced. + //_tree.Rebalance(4); + } + + /// + /// Query an AABB for overlapping proxies. The callback class + /// is called for each proxy that overlaps the supplied AABB. + /// + /// The callback. + /// The aabb. + public void Query(Func callback, ref AABB aabb) + { + _tree.Query(callback, ref aabb); + } + + /// + /// Ray-cast against the proxies in the tree. This relies on the callback + /// to perform a exact ray-cast in the case were the proxy contains a shape. + /// The callback also performs the any collision filtering. This has performance + /// roughly equal to k * log(n), where k is the number of collisions and n is the + /// number of proxies in the tree. + /// + /// A callback class that is called for each proxy that is hit by the ray. + /// The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). + public void RayCast(Func callback, ref RayCastInput input) + { + _tree.RayCast(callback, ref input); + } + + public void ShiftOrigin(Vector2 newOrigin) + { + _tree.ShiftOrigin(newOrigin); + } + + /// + /// Get the tree quality based on the area of the tree. + /// + public float TreeQuality + { + get { return _tree.AreaRatio; } + } + + /// + /// Gets the balance of the tree. + /// + public int TreeBalance + { + get { return _tree.MaxBalance; } + } + + /// + /// Gets the height of the tree. + /// + public int TreeHeight + { + get { return _tree.Height; } + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Collision/IBroadPhase.cs b/src/Box2DNet/Collision/IBroadPhase.cs new file mode 100644 index 0000000..1352b83 --- /dev/null +++ b/src/Box2DNet/Collision/IBroadPhase.cs @@ -0,0 +1,32 @@ +using System; +using Box2DNet.Dynamics; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Collision +{ + public interface IBroadPhase + { + int ProxyCount { get; } + void UpdatePairs(BroadphaseDelegate callback); + + bool TestOverlap(int proxyIdA, int proxyIdB); + + int AddProxy(ref FixtureProxy proxy); + + void RemoveProxy(int proxyId); + + void MoveProxy(int proxyId, ref AABB aabb, Vector2 displacement); + + FixtureProxy GetProxy(int proxyId); + + void TouchProxy(int proxyId); + + void GetFatAABB(int proxyId, out AABB aabb); + + void Query(Func callback, ref AABB aabb); + + void RayCast(Func callback, ref RayCastInput input); + + void ShiftOrigin(Vector2 newOrigin); + } +} \ No newline at end of file diff --git a/src/Box2DNet/Collision/PairManager.cs b/src/Box2DNet/Collision/PairManager.cs deleted file mode 100644 index f96022b..0000000 --- a/src/Box2DNet/Collision/PairManager.cs +++ /dev/null @@ -1,493 +0,0 @@ -/* - Box2DNet Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version 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. -*/ - -// The pair manager is used by the broad-phase to quickly add/remove/find pairs -// of overlapping proxies. It is based closely on code provided by Pierre Terdiman. -// http://www.codercorner.com/IncrementalSAP.txt - -#define DEBUG - -using System; using System.Numerics; -using Box2DNet.Common; - -namespace Box2DNet.Collision -{ - public class Pair - { - [Flags] - public enum PairStatus - { - PairBuffered = 0x0001, - PairRemoved = 0x0002, - PairFinal = 0x0004 - } - - public void SetBuffered() { Status |= PairStatus.PairBuffered; } - public void ClearBuffered() { Status &= ~PairStatus.PairBuffered; } - public bool IsBuffered() { return (Status & PairStatus.PairBuffered) == PairStatus.PairBuffered; } - - public void SetRemoved() { Status |= PairStatus.PairRemoved; } - public void ClearRemoved() { Status &= ~PairStatus.PairRemoved; } - public bool IsRemoved() { return (Status & PairStatus.PairRemoved) == PairStatus.PairRemoved; } - - public void SetFinal() { Status |= PairStatus.PairFinal; } - public bool IsFinal() { return (Status & PairStatus.PairFinal) == PairStatus.PairFinal; } - - public object UserData; - public ushort ProxyId1; - public ushort ProxyId2; - public ushort Next; - public PairStatus Status; - } - - public struct BufferedPair - { - public ushort ProxyId1; - public ushort ProxyId2; - } - - public abstract class PairCallback - { - // This should return the new pair user data. It is ok if the - // user data is null. - public abstract object PairAdded(object proxyUserData1, object proxyUserData2); - - // This should free the pair's user data. In extreme circumstances, it is possible - // this will be called with null pairUserData because the pair never existed. - public abstract void PairRemoved(object proxyUserData1, object proxyUserData2, object pairUserData); - } - - public class PairManager - { - public static readonly ushort NullPair = Common.Math.USHRT_MAX; - public static readonly ushort NullProxy = Common.Math.USHRT_MAX; - public static readonly int TableCapacity = Settings.MaxPairs; // must be a power of two - public static readonly int TableMask = PairManager.TableCapacity - 1; - - public BroadPhase _broadPhase; - public PairCallback _callback; - public Pair[] _pairs = new Pair[Settings.MaxPairs]; - public ushort _freePair; - public int _pairCount; - - public BufferedPair[] _pairBuffer = new BufferedPair[Settings.MaxPairs]; - public int _pairBufferCount; - - public ushort[] _hashTable = new ushort[PairManager.TableCapacity]; - - public PairManager() - { - Box2DNetDebug.Assert(Common.Math.IsPowerOfTwo((uint)PairManager.TableCapacity) == true); - Box2DNetDebug.Assert(PairManager.TableCapacity >= Settings.MaxPairs); - for (int i = 0; i < PairManager.TableCapacity; ++i) - { - _hashTable[i] = PairManager.NullPair; - } - _freePair = 0; - for (int i = 0; i < Settings.MaxPairs; ++i) - { - _pairs[i] = new Pair();//todo: need some pool here - _pairs[i].ProxyId1 = PairManager.NullProxy; - _pairs[i].ProxyId2 = PairManager.NullProxy; - _pairs[i].UserData = null; - _pairs[i].Status = 0; - _pairs[i].Next = (ushort)(i + 1U); - } - _pairs[Settings.MaxPairs - 1].Next = PairManager.NullPair; - _pairCount = 0; - _pairBufferCount = 0; - } - - public void Initialize(BroadPhase broadPhase, PairCallback callback) - { - _broadPhase = broadPhase; - _callback = callback; - } - - /* - As proxies are created and moved, many pairs are created and destroyed. Even worse, the same - pair may be added and removed multiple times in a single time step of the physics engine. To reduce - traffic in the pair manager, we try to avoid destroying pairs in the pair manager until the - end of the physics step. This is done by buffering all the RemovePair requests. AddPair - requests are processed immediately because we need the hash table entry for quick lookup. - - All user user callbacks are delayed until the buffered pairs are confirmed in Commit. - This is very important because the user callbacks may be very expensive and client logic - may be harmed if pairs are added and removed within the same time step. - - Buffer a pair for addition. - We may add a pair that is not in the pair manager or pair buffer. - We may add a pair that is already in the pair manager and pair buffer. - If the added pair is not a new pair, then it must be in the pair buffer (because RemovePair was called). - */ - public void AddBufferedPair(int id1, int id2) - { - Box2DNetDebug.Assert(id1 != PairManager.NullProxy && id2 != PairManager.NullProxy); - Box2DNetDebug.Assert(_pairBufferCount < Settings.MaxPairs); - - Pair pair = AddPair(id1, id2); - - // If this pair is not in the pair buffer ... - if (pair.IsBuffered() == false) - { - // This must be a newly added pair. - Box2DNetDebug.Assert(pair.IsFinal() == false); - - // Add it to the pair buffer. - pair.SetBuffered(); - _pairBuffer[_pairBufferCount].ProxyId1 = pair.ProxyId1; - _pairBuffer[_pairBufferCount].ProxyId2 = pair.ProxyId2; - ++_pairBufferCount; - - Box2DNetDebug.Assert(_pairBufferCount <= _pairCount); - } - - // Confirm this pair for the subsequent call to Commit. - pair.ClearRemoved(); - - if (BroadPhase.IsValidate) - { - ValidateBuffer(); - } - } - - // Buffer a pair for removal. - public void RemoveBufferedPair(int id1, int id2) - { - Box2DNetDebug.Assert(id1 != PairManager.NullProxy && id2 != PairManager.NullProxy); - Box2DNetDebug.Assert(_pairBufferCount < Settings.MaxPairs); - - Pair pair = Find(id1, id2); - - if (pair == null) - { - // The pair never existed. This is legal (due to collision filtering). - return; - } - - // If this pair is not in the pair buffer ... - if (pair.IsBuffered() == false) - { - // This must be an old pair. - Box2DNetDebug.Assert(pair.IsFinal() == true); - - pair.SetBuffered(); - _pairBuffer[_pairBufferCount].ProxyId1 = pair.ProxyId1; - _pairBuffer[_pairBufferCount].ProxyId2 = pair.ProxyId2; - ++_pairBufferCount; - - Box2DNetDebug.Assert(_pairBufferCount <= _pairCount); - } - - pair.SetRemoved(); - - if (BroadPhase.IsValidate) - { - ValidateBuffer(); - } - } - - public void Commit() - { - int removeCount = 0; - - Proxy[] proxies = _broadPhase._proxyPool; - - for (int i = 0; i < _pairBufferCount; ++i) - { - Pair pair = Find(_pairBuffer[i].ProxyId1, _pairBuffer[i].ProxyId2); - Box2DNetDebug.Assert(pair.IsBuffered()); - pair.ClearBuffered(); - - Box2DNetDebug.Assert(pair.ProxyId1 < Settings.MaxProxies && pair.ProxyId2 < Settings.MaxProxies); - - Proxy proxy1 = proxies[pair.ProxyId1]; - Proxy proxy2 = proxies[pair.ProxyId2]; - - Box2DNetDebug.Assert(proxy1.IsValid); - Box2DNetDebug.Assert(proxy2.IsValid); - - if (pair.IsRemoved()) - { - // It is possible a pair was added then removed before a commit. Therefore, - // we should be careful not to tell the user the pair was removed when the - // the user didn't receive a matching add. - if (pair.IsFinal() == true) - { - _callback.PairRemoved(proxy1.UserData, proxy2.UserData, pair.UserData); - } - - // Store the ids so we can actually remove the pair below. - _pairBuffer[removeCount].ProxyId1 = pair.ProxyId1; - _pairBuffer[removeCount].ProxyId2 = pair.ProxyId2; - ++removeCount; - } - else - { - Box2DNetDebug.Assert(_broadPhase.TestOverlap(proxy1, proxy2) == true); - - if (pair.IsFinal() == false) - { - pair.UserData = _callback.PairAdded(proxy1.UserData, proxy2.UserData); - pair.SetFinal(); - } - } - } - - for (int i = 0; i < removeCount; ++i) - { - RemovePair(_pairBuffer[i].ProxyId1, _pairBuffer[i].ProxyId2); - } - - _pairBufferCount = 0; - - if (BroadPhase.IsValidate) - { - ValidateTable(); - } - } - - private Pair Find(int proxyId1, int proxyId2) - { - if (proxyId1 > proxyId2) - Common.Math.Swap(ref proxyId1, ref proxyId2); - - uint hash = (uint)(Hash((uint)proxyId1, (uint)proxyId2) & PairManager.TableMask); - - return Find(proxyId1, proxyId2, hash); - } - - private Pair Find(int proxyId1, int proxyId2, uint hash) - { - int index = _hashTable[hash]; - - while (index != PairManager.NullPair && Equals(_pairs[index], proxyId1, proxyId2) == false) - { - index = _pairs[index].Next; - } - - if (index == PairManager.NullPair) - { - return null; - } - - Box2DNetDebug.Assert(index < Settings.MaxPairs); - - return _pairs[index]; - } - - // Returns existing pair or creates a new one. - private Pair AddPair(int proxyId1, int proxyId2) - { - if (proxyId1 > proxyId2) - Common.Math.Swap(ref proxyId1, ref proxyId2); - - int hash = (int)(Hash((uint)proxyId1, (uint)proxyId2) & PairManager.TableMask); - - Pair pair = Find(proxyId1, proxyId2, (uint)hash); - if (pair != null) - { - return pair; - } - - Box2DNetDebug.Assert(_pairCount < Settings.MaxPairs && _freePair != PairManager.NullPair); - - ushort pairIndex = _freePair; - pair = _pairs[pairIndex]; - _freePair = pair.Next; - - pair.ProxyId1 = (ushort)proxyId1; - pair.ProxyId2 = (ushort)proxyId2; - pair.Status = 0; - pair.UserData = null; - pair.Next = _hashTable[hash]; - - _hashTable[hash] = pairIndex; - - ++_pairCount; - - return pair; - } - - // Removes a pair. The pair must exist. - private object RemovePair(int proxyId1, int proxyId2) - { - Box2DNetDebug.Assert(_pairCount > 0); - - if (proxyId1 > proxyId2) - Common.Math.Swap(ref proxyId1, ref proxyId2); - - int hash = (int)(Hash((uint)proxyId1, (uint)proxyId2) & PairManager.TableMask); - - //uint16* node = &m_hashTable[hash]; - ushort node = _hashTable[hash]; - bool ion = false; - int ni = 0; - while (node != PairManager.NullPair) - { - if (Equals(_pairs[node], proxyId1, proxyId2)) - { - //uint16 index = *node; - //*node = m_pairs[*node].next; - - ushort index = node; - node = _pairs[node].Next; - if (ion) - _pairs[ni].Next = node; - else - { - _hashTable[hash] = node; - } - - Pair pair = _pairs[index]; - object userData = pair.UserData; - - // Scrub - pair.Next = _freePair; - pair.ProxyId1 = PairManager.NullProxy; - pair.ProxyId2 = PairManager.NullProxy; - pair.UserData = null; - pair.Status = 0; - - _freePair = index; - --_pairCount; - return userData; - } - else - { - //node = &m_pairs[*node].next; - ni = node; - node = _pairs[ni].Next; - ion = true; - } - } - - Box2DNetDebug.Assert(false); - return null; - } - - private void ValidateBuffer() - { -#if DEBUG - Box2DNetDebug.Assert(_pairBufferCount <= _pairCount); - - //std::sort(m_pairBuffer, m_pairBuffer + m_pairBufferCount); - BufferedPair[] tmp = new BufferedPair[_pairBufferCount]; - Array.Copy(_pairBuffer, 0, tmp, 0, _pairBufferCount); - Array.Sort(tmp, BufferedPairSortPredicate); - Array.Copy(tmp, 0, _pairBuffer, 0, _pairBufferCount); - - for (int i = 0; i < _pairBufferCount; ++i) - { - if (i > 0) - { - Box2DNetDebug.Assert(Equals(_pairBuffer[i], _pairBuffer[i - 1]) == false); - } - - Pair pair = Find(_pairBuffer[i].ProxyId1, _pairBuffer[i].ProxyId2); - Box2DNetDebug.Assert(pair.IsBuffered()); - - Box2DNetDebug.Assert(pair.ProxyId1 != pair.ProxyId2); - Box2DNetDebug.Assert(pair.ProxyId1 < Settings.MaxProxies); - Box2DNetDebug.Assert(pair.ProxyId2 < Settings.MaxProxies); - - Proxy proxy1 = _broadPhase._proxyPool[pair.ProxyId1]; - Proxy proxy2 = _broadPhase._proxyPool[pair.ProxyId2]; - - Box2DNetDebug.Assert(proxy1.IsValid == true); - Box2DNetDebug.Assert(proxy2.IsValid == true); - } -#endif - } - - private void ValidateTable() - { -#if DEBUG - for (int i = 0; i < PairManager.TableCapacity; ++i) - { - ushort index = _hashTable[i]; - while (index != PairManager.NullPair) - { - Pair pair = _pairs[index]; - Box2DNetDebug.Assert(pair.IsBuffered() == false); - Box2DNetDebug.Assert(pair.IsFinal() == true); - Box2DNetDebug.Assert(pair.IsRemoved() == false); - - Box2DNetDebug.Assert(pair.ProxyId1 != pair.ProxyId2); - Box2DNetDebug.Assert(pair.ProxyId1 < Settings.MaxProxies); - Box2DNetDebug.Assert(pair.ProxyId2 < Settings.MaxProxies); - - Proxy proxy1 = _broadPhase._proxyPool[pair.ProxyId1]; - Proxy proxy2 = _broadPhase._proxyPool[pair.ProxyId2]; - - Box2DNetDebug.Assert(proxy1.IsValid == true); - Box2DNetDebug.Assert(proxy2.IsValid == true); - - Box2DNetDebug.Assert(_broadPhase.TestOverlap(proxy1, proxy2) == true); - - index = pair.Next; - } - } -#endif - } - - // Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm - // This assumes proxyId1 and proxyId2 are 16-bit. - private uint Hash(uint proxyId1, uint proxyId2) - { - uint key = (proxyId2 << 16) | proxyId1; - key = ~key + (key << 15); - key = key ^ (key >> 12); - key = key + (key << 2); - key = key ^ (key >> 4); - key = key * 2057; - key = key ^ (key >> 16); - return key; - } - - private bool Equals(Pair pair, int proxyId1, int proxyId2) - { - return pair.ProxyId1 == proxyId1 && pair.ProxyId2 == proxyId2; - } - - private bool Equals(ref BufferedPair pair1, ref BufferedPair pair2) - { - return pair1.ProxyId1 == pair2.ProxyId1 && pair1.ProxyId2 == pair2.ProxyId2; - } - - public static int BufferedPairSortPredicate(BufferedPair pair1, BufferedPair pair2) - { - if (pair1.ProxyId1 < pair2.ProxyId1) - return 1; - else if (pair1.ProxyId1 > pair2.ProxyId1) - return -1; - else - { - if (pair1.ProxyId2 < pair2.ProxyId2) - return 1; - else if (pair1.ProxyId2 > pair2.ProxyId2) - return -1; - } - - return 0; - } - } -} \ No newline at end of file diff --git a/src/Box2DNet/Collision/Shapes/ChainShape.cs b/src/Box2DNet/Collision/Shapes/ChainShape.cs new file mode 100644 index 0000000..c0c2c91 --- /dev/null +++ b/src/Box2DNet/Collision/Shapes/ChainShape.cs @@ -0,0 +1,262 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System.Diagnostics; +using Box2DNet.Common; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Collision.Shapes +{ + /// + /// A chain shape is a free form sequence of line segments. + /// The chain has two-sided collision, so you can use inside and outside collision. + /// Therefore, you may use any winding order. + /// Connectivity information is used to create smooth collisions. + /// WARNING: The chain will not collide properly if there are self-intersections. + /// + public class ChainShape : Shape + { + /// + /// The vertices. These are not owned/freed by the chain Shape. + /// + public Vertices Vertices; + private Vector2 _prevVertex, _nextVertex; + private bool _hasPrevVertex, _hasNextVertex; + private static EdgeShape _edgeShape = new EdgeShape(); + + /// + /// Constructor for ChainShape. By default have 0 in density. + /// + public ChainShape() + : base(0) + { + ShapeType = ShapeType.Chain; + _radius = Settings.PolygonRadius; + } + + /// + /// Create a new chainshape from the vertices. + /// + /// The vertices to use. Must contain 2 or more vertices. + /// Set to true to create a closed loop. It connects the first vertice to the last, and automatically adjusts connectivity to create smooth collisions along the chain. + public ChainShape(Vertices vertices, bool createLoop = false) + : base(0) + { + ShapeType = ShapeType.Chain; + _radius = Settings.PolygonRadius; + + Debug.Assert(vertices != null && vertices.Count >= 3); + Debug.Assert(vertices[0] != vertices[vertices.Count - 1]); // FPE. See http://www.box2d.org/forum/viewtopic.php?f=4&t=7973&p=35363 + + for (int i = 1; i < vertices.Count; ++i) + { + Vector2 v1 = vertices[i - 1]; + Vector2 v2 = vertices[i]; + + // If the code crashes here, it means your vertices are too close together. + Debug.Assert(Vector2.DistanceSquared(v1, v2) > Settings.LinearSlop * Settings.LinearSlop); + } + + Vertices = new Vertices(vertices); + + if (createLoop) + { + Vertices.Add(vertices[0]); + PrevVertex = Vertices[Vertices.Count - 2]; //FPE: We use the properties instead of the private fields here. + NextVertex = Vertices[1]; //FPE: We use the properties instead of the private fields here. + } + } + + public override int ChildCount + { + // edge count = vertex count - 1 + get { return Vertices.Count - 1; } + } + + /// + /// Establish connectivity to a vertex that precedes the first vertex. + /// Don't call this for loops. + /// + public Vector2 PrevVertex + { + get { return _prevVertex; } + set + { + Debug.Assert(value != null); + + _prevVertex = value; + _hasPrevVertex = true; + } + } + + /// + /// Establish connectivity to a vertex that follows the last vertex. + /// Don't call this for loops. + /// + public Vector2 NextVertex + { + get { return _nextVertex; } + set + { + Debug.Assert(value != null); + + _nextVertex = value; + _hasNextVertex = true; + } + } + + /// + /// This method has been optimized to reduce garbage. + /// + /// The cached edge to set properties on. + /// The index. + internal void GetChildEdge(EdgeShape edge, int index) + { + Debug.Assert(0 <= index && index < Vertices.Count - 1); + Debug.Assert(edge != null); + + edge.ShapeType = ShapeType.Edge; + edge._radius = _radius; + + edge.Vertex1 = Vertices[index + 0]; + edge.Vertex2 = Vertices[index + 1]; + + if (index > 0) + { + edge.Vertex0 = Vertices[index - 1]; + edge.HasVertex0 = true; + } + else + { + edge.Vertex0 = _prevVertex; + edge.HasVertex0 = _hasPrevVertex; + } + + if (index < Vertices.Count - 2) + { + edge.Vertex3 = Vertices[index + 2]; + edge.HasVertex3 = true; + } + else + { + edge.Vertex3 = _nextVertex; + edge.HasVertex3 = _hasNextVertex; + } + } + + /// + /// Get a child edge. + /// + /// The index. + public EdgeShape GetChildEdge(int index) + { + EdgeShape edgeShape = new EdgeShape(); + GetChildEdge(edgeShape, index); + return edgeShape; + } + + public override bool TestPoint(ref Transform transform, ref Vector2 point) + { + return false; + } + + public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex) + { + Debug.Assert(childIndex < Vertices.Count); + + int i1 = childIndex; + int i2 = childIndex + 1; + if (i2 == Vertices.Count) + { + i2 = 0; + } + + _edgeShape.Vertex1 = Vertices[i1]; + _edgeShape.Vertex2 = Vertices[i2]; + + return _edgeShape.RayCast(out output, ref input, ref transform, 0); + } + + public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex) + { + Debug.Assert(childIndex < Vertices.Count); + + int i1 = childIndex; + int i2 = childIndex + 1; + if (i2 == Vertices.Count) + { + i2 = 0; + } + + Vector2 v1 = MathUtils.Mul(ref transform, Vertices[i1]); + Vector2 v2 = MathUtils.Mul(ref transform, Vertices[i2]); + + aabb.LowerBound = Vector2.Min(v1, v2); + aabb.UpperBound = Vector2.Max(v1, v2); + } + + protected override void ComputeProperties() + { + //Does nothing. Chain shapes don't have properties. + } + + public override float ComputeSubmergedArea(ref Vector2 normal, float offset, ref Transform xf, out Vector2 sc) + { + sc = Vector2.Zero; + return 0; + } + + /// + /// Compare the chain to another chain + /// + /// The other chain + /// True if the two chain shapes are the same + public bool CompareTo(ChainShape shape) + { + if (Vertices.Count != shape.Vertices.Count) + return false; + + for (int i = 0; i < Vertices.Count; i++) + { + if (Vertices[i] != shape.Vertices[i]) + return false; + } + + return PrevVertex == shape.PrevVertex && NextVertex == shape.NextVertex; + } + + public override Shape Clone() + { + ChainShape clone = new ChainShape(); + clone.ShapeType = ShapeType; + clone._density = _density; + clone._radius = _radius; + clone.PrevVertex = _prevVertex; + clone.NextVertex = _nextVertex; + clone._hasNextVertex = _hasNextVertex; + clone._hasPrevVertex = _hasPrevVertex; + clone.Vertices = new Vertices(Vertices); + clone.MassData = MassData; + return clone; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Collision/Shapes/CircleShape.cs b/src/Box2DNet/Collision/Shapes/CircleShape.cs index 235e718..53e5438 100644 --- a/src/Box2DNet/Collision/Shapes/CircleShape.cs +++ b/src/Box2DNet/Collision/Shapes/CircleShape.cs @@ -1,182 +1,198 @@ -/* - Box2DNet Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version 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 Box2DNet.Common; -using System.Numerics; - -using Transform = Box2DNet.Common.Transform; - -namespace Box2DNet.Collision -{ - /// - /// A circle shape. - /// - public class CircleShape : Shape - { - // Position - internal Vector2 _position; - - public CircleShape() - { - _type = ShapeType.CircleShape; - } - - public override bool TestPoint(Transform xf, Vector2 p) - { - Vector2 center = xf.position + xf.TransformDirection(_position); - Vector2 d = p - center; - return Vector2.Dot(d, d) <= _radius * _radius; - } - - // Collision Detection in Interactive 3D Environments by Gino van den Bergen - // From Section 3.1.2 - // x = s + a * r - // norm(x) = radius - public override SegmentCollide TestSegment(Transform xf, out float lambda, out Vector2 normal, Segment segment, float maxLambda) - { - lambda = 0f; - normal = Vector2.Zero; - - Vector2 position = xf.position + xf.TransformDirection(_position); - Vector2 s = segment.P1 - position; - float b = Vector2.Dot(s, s) - _radius * _radius; - - // Does the segment start inside the circle? - if (b < 0.0f) - { - lambda = 0f; - return SegmentCollide.StartInsideCollide; - } - - // Solve quadratic equation. - Vector2 r = segment.P2 - segment.P1; - float c = Vector2.Dot(s, r); - float rr = Vector2.Dot(r, r); - float sigma = c * c - rr * b; - - // Check for negative discriminant and short segment. - if (sigma < 0.0f || rr < Common.Settings.FLT_EPSILON) - { - return SegmentCollide.MissCollide; - } - - // Find the point of intersection of the line with the circle. - float a = -(c + Common.Math.Sqrt(sigma)); - - // Is the intersection point on the segment? - if (0.0f <= a && a <= maxLambda * rr) - { - a /= rr; - lambda = a; - normal = s + a * r; - normal.Normalize(); - return SegmentCollide.HitCollide; - } - - return SegmentCollide.MissCollide; - } - - public override void ComputeAABB(out AABB aabb, Transform xf) - { - aabb = new AABB(); - - Vector2 p = xf.position + xf.TransformDirection(_position); - aabb.LowerBound = new Vector2(p.X - _radius, p.Y - _radius); - aabb.UpperBound = new Vector2(p.X + _radius, p.Y + _radius); - } - - public override void ComputeMass(out MassData massData, float density) - { - massData = new MassData(); - - massData.Mass = density * (float)System.Math.PI * _radius * _radius; - massData.Center = _position; - - // inertia about the local origin - massData.I = massData.Mass * (0.5f * _radius * _radius + Vector2.Dot(_position, _position)); - } - - public override float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 c) - { - Vector2 p = xf.TransformPoint(_position); - float l = -(Vector2.Dot(normal, p) - offset); - if (l < -_radius + Box2DNet.Common.Settings.FLT_EPSILON) - { - //Completely dry - c = new Vector2(); - return 0; - } - if (l > _radius) - { - //Completely wet - c = p; - return Box2DNet.Common.Settings.Pi * _radius * _radius; - } - - //Magic - float r2 = _radius * _radius; - float l2 = l * l; - float area = r2 * ((float)System.Math.Asin(l / _radius) + Box2DNet.Common.Settings.Pi / 2) + - l * Box2DNet.Common.Math.Sqrt(r2 - l2); - float com = -2.0f / 3.0f * (float)System.Math.Pow(r2 - l2, 1.5f) / area; - - c.X = p.X + normal.X * com; - c.Y = p.Y + normal.Y * com; - - return area; - } - - /// - /// Get the supporting vertex index in the given direction. - /// - public override int GetSupport(Vector2 d) - { - return 0; - } - - /// - /// Get the supporting vertex in the given direction. - /// - public override Vector2 GetSupportVertex(Vector2 d) - { - return _position; - } - - /// - /// Get a vertex by index. Used by Distance. - /// - public override Vector2 GetVertex(int index) - { - Box2DNetDebug.Assert(index == 0); - return _position; - } - - public override float ComputeSweepRadius(Vector2 pivot) - { - return Box2DNet.Common.Math.Distance(_position, pivot); - } - - /// - /// Get the vertex count. - /// - public int VertexCount { get { return 1; } } - } +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using Box2DNet.Common; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Collision.Shapes +{ + /// + /// A circle shape. + /// + public class CircleShape : Shape + { + internal Vector2 _position; + + /// + /// Create a new circle with the desired radius and density. + /// + /// The radius of the circle. + /// The density of the circle. + public CircleShape(float radius, float density) + : base(density) + { + Debug.Assert(radius >= 0); + Debug.Assert(density >= 0); + + ShapeType = ShapeType.Circle; + _position = Vector2.Zero; + Radius = radius; // The Radius property cache 2radius and calls ComputeProperties(). So no need to call ComputeProperties() here. + } + + internal CircleShape() + : base(0) + { + ShapeType = ShapeType.Circle; + _radius = 0.0f; + _position = Vector2.Zero; + } + + public override int ChildCount + { + get { return 1; } + } + + /// + /// Get or set the position of the circle + /// + public Vector2 Position + { + get { return _position; } + set + { + _position = value; + ComputeProperties(); //TODO: Optimize here + } + } + + public override bool TestPoint(ref Transform transform, ref Vector2 point) + { + Vector2 center = transform.p + MathUtils.Mul(transform.q, Position); + Vector2 d = point - center; + return Vector2.Dot(d, d) <= _2radius; + } + + public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex) + { + // Collision Detection in Interactive 3D Environments by Gino van den Bergen + // From Section 3.1.2 + // x = s + a * r + // norm(x) = radius + + output = new RayCastOutput(); + + Vector2 position = transform.p + MathUtils.Mul(transform.q, Position); + Vector2 s = input.Point1 - position; + float b = Vector2.Dot(s, s) - _2radius; + + // Solve quadratic equation. + Vector2 r = input.Point2 - input.Point1; + float c = Vector2.Dot(s, r); + float rr = Vector2.Dot(r, r); + float sigma = c * c - rr * b; + + // Check for negative discriminant and short segment. + if (sigma < 0.0f || rr < Settings.Epsilon) + { + return false; + } + + // Find the point of intersection of the line with the circle. + float a = -(c + (float)Math.Sqrt(sigma)); + + // Is the intersection point on the segment? + if (0.0f <= a && a <= input.MaxFraction * rr) + { + a /= rr; + output.Fraction = a; + + //TODO: Check results here + output.Normal = s + a * r; + output.Normal.Normalize(); + return true; + } + + return false; + } + + public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex) + { + Vector2 p = transform.p + MathUtils.Mul(transform.q, Position); + aabb.LowerBound = new Vector2(p.X - Radius, p.Y - Radius); + aabb.UpperBound = new Vector2(p.X + Radius, p.Y + Radius); + } + + protected override sealed void ComputeProperties() + { + float area = Settings.Pi * _2radius; + MassData.Area = area; + MassData.Mass = Density * area; + MassData.Centroid = Position; + + // inertia about the local origin + MassData.Inertia = MassData.Mass * (0.5f * _2radius + Vector2.Dot(Position, Position)); + } + + public override float ComputeSubmergedArea(ref Vector2 normal, float offset, ref Transform xf, out Vector2 sc) + { + sc = Vector2.Zero; + + Vector2 p = MathUtils.Mul(ref xf, Position); + float l = -(Vector2.Dot(normal, p) - offset); + if (l < -Radius + Settings.Epsilon) + { + //Completely dry + return 0; + } + if (l > Radius) + { + //Completely wet + sc = p; + return Settings.Pi * _2radius; + } + + //Magic + float l2 = l * l; + float area = _2radius * (float)((Math.Asin(l / Radius) + Settings.Pi / 2) + l * Math.Sqrt(_2radius - l2)); + float com = -2.0f / 3.0f * (float)Math.Pow(_2radius - l2, 1.5f) / area; + + sc.X = p.X + normal.X * com; + sc.Y = p.Y + normal.Y * com; + + return area; + } + + /// + /// Compare the circle to another circle + /// + /// The other circle + /// True if the two circles are the same size and have the same position + public bool CompareTo(CircleShape shape) + { + return (Radius == shape.Radius && Position == shape.Position); + } + + public override Shape Clone() + { + CircleShape clone = new CircleShape(); + clone.ShapeType = ShapeType; + clone._radius = Radius; + clone._2radius = _2radius; //FPE note: We also copy the cache + clone._density = _density; + clone._position = _position; + clone.MassData = MassData; + return clone; + } + } } \ No newline at end of file diff --git a/src/Box2DNet/Collision/Shapes/EdgeShape.cs b/src/Box2DNet/Collision/Shapes/EdgeShape.cs index b9f264d..c7e61fe 100644 --- a/src/Box2DNet/Collision/Shapes/EdgeShape.cs +++ b/src/Box2DNet/Collision/Shapes/EdgeShape.cs @@ -1,280 +1,252 @@ -/* - Box2DNet Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version 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.Numerics; -using System.Collections.Generic; -using System.Text; -using Box2DNet.Common; - - -using Transform = Box2DNet.Common.Transform; - -namespace Box2DNet.Collision -{ - public class EdgeShape : Shape - { - public Vector2 _v1; - public Vector2 _v2; - - public float _length; - - public Vector2 _normal; - - public Vector2 _direction; - - // Unit vector halfway between m_direction and m_prevEdge.m_direction: - public Vector2 _cornerDir1; - - // Unit vector halfway between m_direction and m_nextEdge.m_direction: - public Vector2 _cornerDir2; - - public bool _cornerConvex1; - public bool _cornerConvex2; - - public EdgeShape _nextEdge; - public EdgeShape _prevEdge; - - public EdgeShape() - { - _type = ShapeType.EdgeShape; - _radius = Settings.PolygonRadius; - } - - public override void Dispose() - { - if (_prevEdge != null) - { - _prevEdge._nextEdge = null; - } - - if (_nextEdge != null) - { - _nextEdge._prevEdge = null; - } - } - - public void Set(Vector2 v1, Vector2 v2) - { - _v1 = v1; - _v2 = v2; - - _direction = _v2 - _v1; - _length = _direction.Length(); - _direction.Normalize(); - _normal = _direction.CrossScalarPostMultiply(1.0f); - - _cornerDir1 = _normal; - _cornerDir2 = -1.0f * _normal; - } - - public override bool TestPoint(Transform xf, Vector2 p) - { - return false; - } - - public override SegmentCollide TestSegment(Transform xf, out float lambda, out Vector2 normal, Segment segment, float maxLambda) - { - Vector2 r = segment.P2 - segment.P1; - Vector2 v1 = xf.TransformPoint(_v1); - Vector2 d = ((Vector2)xf.TransformPoint(_v2)) - v1; - Vector2 n = d.CrossScalarPostMultiply(1.0f); - - float k_slop = 100.0f * Common.Settings.FLT_EPSILON; - float denom = -Vector2.Dot(r, n); - - // Cull back facing collision and ignore parallel segments. - if (denom > k_slop) - { - // Does the segment intersect the infinite line associated with this segment? - Vector2 b = segment.P1 - v1; - float a = Vector2.Dot(b, n); - - if (0.0f <= a && a <= maxLambda * denom) - { - float mu2 = -r.X * b.Y + r.Y * b.X; - - // Does the segment intersect this segment? - if (-k_slop * denom <= mu2 && mu2 <= denom * (1.0f + k_slop)) - { - a /= denom; - n.Normalize(); - lambda = a; - normal = n; - return SegmentCollide.HitCollide; - } - } - } - - lambda = 0; - normal = new Vector2(); - return SegmentCollide.MissCollide; - } - - public override void ComputeAABB(out AABB aabb, Transform xf) - { - Vector2 v1 = xf.TransformPoint(_v1); - Vector2 v2 = xf.TransformPoint(_v2); - - Vector2 r = new Vector2(_radius, _radius); - aabb.LowerBound = Vector2.Min(v1, v2) - r; - aabb.UpperBound = Vector2.Max(v1, v2) + r; - } - - public override void ComputeMass(out MassData massData, float density) - { - massData.Mass = 0.0f; - massData.Center = _v1; - massData.I = 0.0f; - } - - public void SetPrevEdge(EdgeShape edge, Vector2 cornerDir, bool convex) - { - _prevEdge = edge; - _cornerDir1 = cornerDir; - _cornerConvex1 = convex; - } - - public void SetNextEdge(EdgeShape edge, Vector2 cornerDir, bool convex) - { - _nextEdge = edge; - _cornerDir2 = cornerDir; - _cornerConvex2 = convex; - } - - public override float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 c) - { - //Note that v0 is independent of any details of the specific edge - //We are relying on v0 being consistent between multiple edges of the same body - Vector2 v0 = offset * normal; - //b2Vec2 v0 = xf.position + (offset - b2Dot(normal, xf.position)) * normal; - - Vector2 v1 = xf.TransformPoint(_v1); - Vector2 v2 = xf.TransformPoint(_v2); - - float d1 = Vector2.Dot(normal, v1) - offset; - float d2 = Vector2.Dot(normal, v2) - offset; - - if (d1 > 0.0f) - { - if (d2 > 0.0f) - { - c = new Vector2(); - return 0.0f; - } - else - { - v1 = -d2 / (d1 - d2) * v1 + d1 / (d1 - d2) * v2; - } - } - else - { - if (d2 > 0.0f) - { - v2 = -d2 / (d1 - d2) * v1 + d1 / (d1 - d2) * v2; - } - else - { - //Nothing - } - } - - // v0,v1,v2 represents a fully submerged triangle - float k_inv3 = 1.0f / 3.0f; - - // Area weighted centroid - c = k_inv3 * (v0 + v1 + v2); - - Vector2 e1 = v1 - v0; - Vector2 e2 = v2 - v0; - - return 0.5f * e1.Cross(e2); - } - - public float Length - { - get { return _length; } - } - - public Vector2 Vertex1 - { - get { return _v1; } - } - - public Vector2 Vertex2 - { - get { return _v2; } - } - - public Vector2 NormalVector - { - get { return _normal; } - } - - public Vector2 DirectionVector - { - get { return _direction; } - } - - public Vector2 Corner1Vector - { - get { return _cornerDir1; } - } - - public Vector2 Corner2Vector - { - get { return _cornerDir2; } - } - - public override int GetSupport(Vector2 d) - { - return Vector2.Dot(_v1, d) > Vector2.Dot(_v2, d) ? 0 : 1; - } - - public override Vector2 GetSupportVertex(Vector2 d) - { - return Vector2.Dot(_v1, d) > Vector2.Dot(_v2, d) ? _v1 : _v2; - } - - public override Vector2 GetVertex(int index) - { - Box2DNetDebug.Assert(0 <= index && index < 2); - if (index == 0) return _v1; - else return _v2; - } - - public bool Corner1IsConvex - { - get { return _cornerConvex1; } - } - - public bool Corner2IsConvex - { - get { return _cornerConvex2; } - } - - public override float ComputeSweepRadius(Vector2 pivot) - { - float ds1 = (_v1 - pivot).LengthSquared(); - float ds2 = (_v2 - pivot).LengthSquared(); - return (float)System.Math.Sqrt((float)System.Math.Max(ds1, ds2)); - } - } -} +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using Box2DNet.Common; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Collision.Shapes +{ + /// + /// A line segment (edge) shape. These can be connected in chains or loops + /// to other edge shapes. + /// The connectivity information is used to ensure correct contact normals. + /// + public class EdgeShape : Shape + { + /// + /// Edge start vertex + /// + internal Vector2 _vertex1; + + /// + /// Edge end vertex + /// + internal Vector2 _vertex2; + + internal EdgeShape() + : base(0) + { + ShapeType = ShapeType.Edge; + _radius = Settings.PolygonRadius; + } + + /// + /// Create a new EdgeShape with the specified start and end. + /// + /// The start of the edge. + /// The end of the edge. + public EdgeShape(Vector2 start, Vector2 end) + : base(0) + { + ShapeType = ShapeType.Edge; + _radius = Settings.PolygonRadius; + Set(start, end); + } + + public override int ChildCount + { + get { return 1; } + } + + /// + /// Is true if the edge is connected to an adjacent vertex before vertex 1. + /// + public bool HasVertex0 { get; set; } + + /// + /// Is true if the edge is connected to an adjacent vertex after vertex2. + /// + public bool HasVertex3 { get; set; } + + /// + /// Optional adjacent vertices. These are used for smooth collision. + /// + public Vector2 Vertex0 { get; set; } + + /// + /// Optional adjacent vertices. These are used for smooth collision. + /// + public Vector2 Vertex3 { get; set; } + + /// + /// These are the edge vertices + /// + public Vector2 Vertex1 + { + get { return _vertex1; } + set + { + _vertex1 = value; + ComputeProperties(); + } + } + + /// + /// These are the edge vertices + /// + public Vector2 Vertex2 + { + get { return _vertex2; } + set + { + _vertex2 = value; + ComputeProperties(); + } + } + + /// + /// Set this as an isolated edge. + /// + /// The start. + /// The end. + public void Set(Vector2 start, Vector2 end) + { + _vertex1 = start; + _vertex2 = end; + HasVertex0 = false; + HasVertex3 = false; + + ComputeProperties(); + } + + public override bool TestPoint(ref Transform transform, ref Vector2 point) + { + return false; + } + + public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex) + { + // p = p1 + t * d + // v = v1 + s * e + // p1 + t * d = v1 + s * e + // s * e - t * d = p1 - v1 + + output = new RayCastOutput(); + + // Put the ray into the edge's frame of reference. + Vector2 p1 = MathUtils.MulT(transform.q, input.Point1 - transform.p); + Vector2 p2 = MathUtils.MulT(transform.q, input.Point2 - transform.p); + Vector2 d = p2 - p1; + + Vector2 v1 = _vertex1; + Vector2 v2 = _vertex2; + Vector2 e = v2 - v1; + Vector2 normal = new Vector2(e.Y, -e.X); //TODO: Could possibly cache the normal. + normal.Normalize(); + + // q = p1 + t * d + // dot(normal, q - v1) = 0 + // dot(normal, p1 - v1) + t * dot(normal, d) = 0 + float numerator = Vector2.Dot(normal, v1 - p1); + float denominator = Vector2.Dot(normal, d); + + if (denominator == 0.0f) + { + return false; + } + + float t = numerator / denominator; + if (t < 0.0f || input.MaxFraction < t) + { + return false; + } + + Vector2 q = p1 + t * d; + + // q = v1 + s * r + // s = dot(q - v1, r) / dot(r, r) + Vector2 r = v2 - v1; + float rr = Vector2.Dot(r, r); + if (rr == 0.0f) + { + return false; + } + + float s = Vector2.Dot(q - v1, r) / rr; + if (s < 0.0f || 1.0f < s) + { + return false; + } + + output.Fraction = t; + if (numerator > 0.0f) + { + output.Normal = -normal; + } + else + { + output.Normal = normal; + } + return true; + } + + public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex) + { + Vector2 v1 = MathUtils.Mul(ref transform, _vertex1); + Vector2 v2 = MathUtils.Mul(ref transform, _vertex2); + + Vector2 lower = Vector2.Min(v1, v2); + Vector2 upper = Vector2.Max(v1, v2); + + Vector2 r = new Vector2(Radius, Radius); + aabb.LowerBound = lower - r; + aabb.UpperBound = upper + r; + } + + protected override void ComputeProperties() + { + MassData.Centroid = 0.5f * (_vertex1 + _vertex2); + } + + public override float ComputeSubmergedArea(ref Vector2 normal, float offset, ref Transform xf, out Vector2 sc) + { + sc = Vector2.Zero; + return 0; + } + + public bool CompareTo(EdgeShape shape) + { + return (HasVertex0 == shape.HasVertex0 && + HasVertex3 == shape.HasVertex3 && + Vertex0 == shape.Vertex0 && + Vertex1 == shape.Vertex1 && + Vertex2 == shape.Vertex2 && + Vertex3 == shape.Vertex3); + } + + public override Shape Clone() + { + EdgeShape clone = new EdgeShape(); + clone.ShapeType = ShapeType; + clone._radius = _radius; + clone._density = _density; + clone.HasVertex0 = HasVertex0; + clone.HasVertex3 = HasVertex3; + clone.Vertex0 = Vertex0; + clone._vertex1 = _vertex1; + clone._vertex2 = _vertex2; + clone.Vertex3 = Vertex3; + clone.MassData = MassData; + return clone; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Collision/Shapes/PolygonShape.cs b/src/Box2DNet/Collision/Shapes/PolygonShape.cs index 96e0225..efd3d4a 100644 --- a/src/Box2DNet/Collision/Shapes/PolygonShape.cs +++ b/src/Box2DNet/Collision/Shapes/PolygonShape.cs @@ -1,730 +1,471 @@ -/* - Box2DNet Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version 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. -*/ - -#define DEBUG - -using System; using System.Numerics; -using System.Collections.Generic; -using System.Text; - -using Box2DNet.Common; - - -using Transform = Box2DNet.Common.Transform; - -namespace Box2DNet.Collision -{ - /// - /// A convex polygon. It is assumed that the interior of the polygon is to the left of each edge. - /// - public class PolygonShape : Shape - { - internal Vector2 _centroid; - internal Vector2[] _vertices = new Vector2[Settings.MaxPolygonVertices]; - internal Vector2[] _normals = new Vector2[Settings.MaxPolygonVertices]; - - internal int _vertexCount; - - public int VertexCount - { - get { return _vertexCount; } - } - - public Vector2[] Vertices - { - get { return _vertices; } - } - - /// - /// Copy vertices. This assumes the vertices define a convex polygon. - /// It is assumed that the exterior is the the right of each edge. - /// - public void Set(Vector2[] vertices, int count) - { - Box2DNetDebug.Assert(3 <= count && count <= Settings.MaxPolygonVertices); - _vertexCount = count; - - int i; - // Copy vertices. - for (i = 0; i < _vertexCount; ++i) - { - _vertices[i] = vertices[i]; - } - - // Compute normals. Ensure the edges have non-zero length. - for (i = 0; i < _vertexCount; ++i) - { - int i1 = i; - int i2 = i + 1 < count ? i + 1 : 0; - Vector2 edge = _vertices[i2] - _vertices[i1]; - Box2DNetDebug.Assert(edge.LengthSquared() > (Box2DNet.Common.Math.Epsilon * Box2DNet.Common.Math.Epsilon)); - _normals[i] = edge.CrossScalarPostMultiply(1.0f); - _normals[i].Normalize(); - } - -#if DEBUG - // Ensure the polygon is convex and the interior - // is to the left of each edge. - for (i = 0; i < _vertexCount; ++i) - { - int i1 = i; - int i2 = i + 1 < count ? i + 1 : 0; - Vector2 edge = _vertices[i2] - _vertices[i1]; - - for (int j = 0; j < _vertexCount; ++j) - { - // Don't check vertices on the current edge. - if (j == i1 || j == i2) - { - continue; - } - - Vector2 r = _vertices[j] - _vertices[i1]; - - // Your polygon is non-convex (it has an indentation) or - // has colinear edges. - float s = edge.Cross(r); - Box2DNetDebug.Assert(s > 0.0f); - } - } -#endif - - // Compute the polygon centroid. - _centroid = ComputeCentroid(_vertices, _vertexCount); - } - - /// - /// Build vertices to represent an axis-aligned box. - /// - /// The half-width - /// The half-height. - public void SetAsBox(float hx, float hy) - { - _vertexCount = 4; - _vertices[0] = new Vector2(-hx, -hy); - _vertices[1] = new Vector2(hx, -hy); - _vertices[2] = new Vector2(hx, hy); - _vertices[3] = new Vector2(-hx, hy); - _normals[0] = new Vector2(0.0f, -1.0f); - _normals[1] = new Vector2(1.0f, 0.0f); - _normals[2] = new Vector2(0.0f, 1.0f); - _normals[3] = new Vector2(-1.0f, 0.0f); - _centroid = Vector2.Zero; - } - - - /// - /// Build vertices to represent an oriented box. - /// - /// The half-width - /// The half-height. - /// The center of the box in local coordinates. - /// The rotation of the box in local coordinates. - public void SetAsBox(float hx, float hy, Vector2 center, float angle) - { - SetAsBox(hx, hy); - - Transform xf = new Transform(); - xf.position = center; - xf.rotation = Box2DNet.Common.Math.AngleToRotation(angle); - // xf.R = new Mat22(angle); - - //Debug.Log(string.Format("xf.position = ({0},{1}) xf.rotation = ({2},{3},{4},{5})", xf.position.x, xf.position.y, xf.rotation.x, xf.rotation.y, xf.rotation.z, xf.rotation.w)); - - // Transform vertices and normals. - for (int i = 0; i < _vertexCount; ++i) - { - _vertices[i] = xf.TransformPoint(_vertices[i]); - _normals[i] = xf.TransformDirection(_normals[i]); - } - } - - public void SetAsEdge(Vector2 v1, Vector2 v2) - { - _vertexCount = 2; - _vertices[0] = v1; - _vertices[1] = v2; - _centroid = 0.5f * (v1 + v2); - _normals[0] = (v2 - v1).CrossScalarPostMultiply(1.0f); - _normals[0].Normalize(); - _normals[1] = -_normals[0]; - } - - public override bool TestPoint(Transform xf, Vector2 p) - { - Vector2 pLocal = xf.InverseTransformDirection(p - xf.position); - - int vc = _vertexCount; - for (int i = 0; i < vc; ++i) - { - float dot = Vector2.Dot(_normals[i], pLocal - _vertices[i]); - if (dot > 0.0f) - { - return false; - } - } - - return true; - } - - public override SegmentCollide TestSegment(Transform xf, out float lambda, out Vector2 normal, Segment segment, float maxLambda) - { - lambda = 0f; - normal = Vector2.Zero; - - float lower = 0.0f, upper = maxLambda; - - Vector2 p1 = xf.InverseTransformDirection(segment.P1 - xf.position); - Vector2 p2 = xf.InverseTransformDirection(segment.P2 - xf.position); - Vector2 d = p2 - p1; - int index = -1; - - for (int i = 0; i < _vertexCount; ++i) - { - // p = p1 + a * d - // dot(normal, p - v) = 0 - // dot(normal, p1 - v) + a * dot(normal, d) = 0 - float numerator = Vector2.Dot(_normals[i], _vertices[i] - p1); - float denominator = Vector2.Dot(_normals[i], d); - - if (denominator == 0.0f) - { - if (numerator < 0.0f) - { - return SegmentCollide.MissCollide; - } - } - else - { - // Note: we want this predicate without division: - // lower < numerator / denominator, where denominator < 0 - // Since denominator < 0, we have to flip the inequality: - // lower < numerator / denominator <==> denominator * lower > numerator. - if (denominator < 0.0f && numerator < lower * denominator) - { - // Increase lower. - // The segment enters this half-space. - lower = numerator / denominator; - index = i; - } - else if (denominator > 0.0f && numerator < upper * denominator) - { - // Decrease upper. - // The segment exits this half-space. - upper = numerator / denominator; - } - } - - if (upper < lower) - { - return SegmentCollide.MissCollide; - } - } - - Box2DNetDebug.Assert(0.0f <= lower && lower <= maxLambda); - - if (index >= 0) - { - lambda = lower; - normal = xf.TransformDirection(_normals[index]); - return SegmentCollide.HitCollide; - } - - lambda = 0f; - return SegmentCollide.StartInsideCollide; - } - - public override void ComputeAABB(out AABB aabb, Transform xf) - { - Vector2 lower = xf.TransformPoint( _vertices[0]); - Vector2 upper = lower; - - for (int i = 1; i < _vertexCount; ++i) - { - Vector2 v = xf.TransformPoint(_vertices[i]); - lower = Vector2.Min(lower, v); - upper = Vector2.Max(upper, v); - } - - Vector2 r = new Vector2(_radius, _radius); - aabb.LowerBound = lower - r; - aabb.UpperBound = upper + r; - } - - public override void ComputeMass(out MassData massData, float denstity) - { - // Polygon mass, centroid, and inertia. - // Let rho be the polygon density in mass per unit area. - // Then: - // mass = rho * int(dA) - // centroid.x = (1/mass) * rho * int(x * dA) - // centroid.y = (1/mass) * rho * int(y * dA) - // I = rho * int((x*x + y*y) * dA) - // - // We can compute these integrals by summing all the integrals - // for each triangle of the polygon. To evaluate the integral - // for a single triangle, we make a change of variables to - // the (u,v) coordinates of the triangle: - // x = x0 + e1x * u + e2x * v - // y = y0 + e1y * u + e2y * v - // where 0 <= u && 0 <= v && u + v <= 1. - // - // We integrate u from [0,1-v] and then v from [0,1]. - // We also need to use the Jacobian of the Transformation: - // D = cross(e1, e2) - // - // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3) - // - // The rest of the derivation is handled by computer algebra. - - Box2DNetDebug.Assert(_vertexCount >= 3); - - Vector2 center = Vector2.Zero; - float area = 0.0f; - float I = 0.0f; - - // pRef is the reference point for forming triangles. - // It's location doesn't change the result (except for rounding error). - Vector2 pRef = Vector2.Zero; - -#if O - // This code would put the reference point inside the polygon. - for (int i = 0; i < vCount; ++i) - { - pRef += _vertices[i]; - } - pRef *= 1.0f / count; -#endif - - const float k_inv3 = 1.0f / 3.0f; - - for (int i = 0; i < _vertexCount; ++i) - { - // Triangle vertices. - Vector2 p1 = pRef; - Vector2 p2 = _vertices[i]; - Vector2 p3 = i + 1 < _vertexCount ? _vertices[i + 1] : _vertices[0]; - - Vector2 e1 = p2 - p1; - Vector2 e2 = p3 - p1; - - float D = e1.Cross(e2); - - float triangleArea = 0.5f * D; - area += triangleArea; - - // Area weighted centroid - center += triangleArea * k_inv3 * (p1 + p2 + p3); - - float px = p1.X, py = p1.Y; - float ex1 = e1.X, ey1 = e1.Y; - float ex2 = e2.X, ey2 = e2.Y; - - float intx2 = k_inv3 * (0.25f * (ex1 * ex1 + ex2 * ex1 + ex2 * ex2) + (px * ex1 + px * ex2)) + 0.5f * px * px; - float inty2 = k_inv3 * (0.25f * (ey1 * ey1 + ey2 * ey1 + ey2 * ey2) + (py * ey1 + py * ey2)) + 0.5f * py * py; - - I += D * (intx2 + inty2); - } - - // Total mass - massData.Mass = denstity * area; - - // Center of mass - Box2DNetDebug.Assert(area > Common.Settings.FLT_EPSILON); - center *= 1.0f / area; - massData.Center = center; - - // Inertia tensor relative to the local origin. - massData.I = denstity * I; - } - - public override float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 c) - { - //Transform plane into shape co-ordinates - Vector2 normalL = xf.InverseTransformDirection(normal); - float offsetL = offset - Vector2.Dot(normal, xf.position); - - float[] depths = new float[Common.Settings.MaxPolygonVertices]; - int diveCount = 0; - int intoIndex = -1; - int outoIndex = -1; - - bool lastSubmerged = false; - int i; - for (i = 0; i < _vertexCount; i++) - { - depths[i] = Vector2.Dot(normalL, _vertices[i]) - offsetL; - bool isSubmerged = depths[i] < -Common.Settings.FLT_EPSILON; - if (i > 0) - { - if (isSubmerged) - { - if (!lastSubmerged) - { - intoIndex = i - 1; - diveCount++; - } - } - else - { - if (lastSubmerged) - { - outoIndex = i - 1; - diveCount++; - } - } - } - lastSubmerged = isSubmerged; - } - switch (diveCount) - { - case 0: - if (lastSubmerged) - { - //Completely submerged - MassData md; - ComputeMass(out md, 1f); - c = xf.TransformPoint(md.Center); - return md.Mass; - } - else - { - //Completely dry - c = new Vector2(); - return 0; - } - break; - case 1: - if (intoIndex == -1) - { - intoIndex = _vertexCount - 1; - } - else - { - outoIndex = _vertexCount - 1; - } - break; - } - int intoIndex2 = (intoIndex + 1) % _vertexCount; - int outoIndex2 = (outoIndex + 1) % _vertexCount; - - float intoLambda = (0 - depths[intoIndex]) / (depths[intoIndex2] - depths[intoIndex]); - float outoLambda = (0 - depths[outoIndex]) / (depths[outoIndex2] - depths[outoIndex]); - - Vector2 intoVec = new Vector2(_vertices[intoIndex].X * (1 - intoLambda) + _vertices[intoIndex2].X * intoLambda, - _vertices[intoIndex].Y * (1 - intoLambda) + _vertices[intoIndex2].Y * intoLambda); - Vector2 outoVec = new Vector2(_vertices[outoIndex].X * (1 - outoLambda) + _vertices[outoIndex2].X * outoLambda, - _vertices[outoIndex].Y * (1 - outoLambda) + _vertices[outoIndex2].Y * outoLambda); - - //Initialize accumulator - float area = 0; - Vector2 center = Vector2.Zero; - Vector2 p2 = _vertices[intoIndex2]; - Vector2 p3; - - const float k_inv3 = 1.0f / 3.0f; - - //An awkward loop from intoIndex2+1 to outIndex2 - i = intoIndex2; - while (i != outoIndex2) - { - i = (i + 1) % _vertexCount; - if (i == outoIndex2) - p3 = outoVec; - else - p3 = _vertices[i]; - //Add the triangle formed by intoVec,p2,p3 - { - Vector2 e1 = p2 - intoVec; - Vector2 e2 = p3 - intoVec; - - float D = e1.Cross(e2); - - float triangleArea = 0.5f * D; - - area += triangleArea; - - // Area weighted centroid - center += triangleArea * k_inv3 * (intoVec + p2 + p3); - - } - // - p2 = p3; - } - - //Normalize and Transform centroid - center *= 1.0f / area; - - c = xf.TransformPoint(center); - - return area; - } - - public override float ComputeSweepRadius(Vector2 pivot) - { - int vCount = _vertexCount; - Box2DNetDebug.Assert(vCount > 0); - float sr = (_vertices[0] - pivot).LengthSquared(); - for (int i = 1; i < vCount; ++i) - { - sr = Common.Math.Max(sr, (_vertices[i] - pivot).LengthSquared()); - } - - return (float)System.Math.Sqrt(sr); - } - - /// - /// Get the supporting vertex index in the given direction. - /// - public override int GetSupport(Vector2 d) - { - int bestIndex = 0; - float bestValue = Vector2.Dot(_vertices[0], d); - for (int i = 1; i < _vertexCount; ++i) - { - float value = Vector2.Dot(_vertices[i], d); - if (value > bestValue) - { - bestIndex = i; - bestValue = value; - } - } - - return bestIndex; - } - - public override Vector2 GetSupportVertex(Vector2 d) - { - int bestIndex = 0; - float bestValue = Vector2.Dot(_vertices[0], d); - for (int i = 1; i < _vertexCount; ++i) - { - float value = Vector2.Dot(_vertices[i], d); - if (value > bestValue) - { - bestIndex = i; - bestValue = value; - } - } - - return _vertices[bestIndex]; - } - - public override Vector2 GetVertex(int index) - { - Box2DNetDebug.Assert(0 <= index && index < _vertexCount); - return _vertices[index]; - } - - public static Vector2 ComputeCentroid(Vector2[] vs, int count) - { - Box2DNetDebug.Assert(count >= 3); - - Vector2 c = Vector2.Zero; - float area = 0f; - - // pRef is the reference point for forming triangles. - // It's location doesn't change the result (except for rounding error). - Vector2 pRef = Vector2.Zero; -#if O - // This code would put the reference point inside the polygon. - for (int i = 0; i < count; ++i) - { - pRef += vs[i]; - } - pRef *= 1.0f / count; -#endif - - const float inv3 = 1.0f / 3.0f; - - for (int i = 0; i < count; ++i) - { - // Triangle vertices. - Vector2 p1 = pRef; - Vector2 p2 = vs[i]; - Vector2 p3 = i + 1 < count ? vs[i + 1] : vs[0]; - - Vector2 e1 = p2 - p1; - Vector2 e2 = p3 - p1; - - float D = e1.Cross(e2); - - float triangleArea = 0.5f * D; - area += triangleArea; - - // Area weighted centroid - c += triangleArea * inv3 * (p1 + p2 + p3); - } - - // Centroid - Box2DNetDebug.Assert(area > Box2DNet.Common.Math.Epsilon); - c *= 1.0f / area; - return c; - } - - public PolygonShape() - { - _type = ShapeType.PolygonShape; - _radius = Settings.PolygonRadius; - - /*Box2DNetDebug.Assert(def.Type == ShapeType.PolygonShape); - _type = ShapeType.PolygonShape; - PolygonDef poly = (PolygonDef)def; - - // Get the vertices Transformed into the body frame. - _vertexCount = poly.VertexCount; - Box2DNetDebug.Assert(3 <= _vertexCount && _vertexCount <= Settings.MaxPolygonVertices); - - // Copy vertices. - for (int i = 0; i < _vertexCount; ++i) - { - _vertices[i] = poly.Vertices[i]; - } - - // Compute normals. Ensure the edges have non-zero length. - for (int i = 0; i < _vertexCount; ++i) - { - int i1 = i; - int i2 = i + 1 < _vertexCount ? i + 1 : 0; - Vec2 edge = _vertices[i2] - _vertices[i1]; - Box2DNetDebug.Assert(edge.LengthSquared() > Common.Settings.FLT_EPSILON * Common.Settings.FLT_EPSILON); - _normals[i] = Vec2.Cross(edge, 1.0f); - _normals[i].Normalize(); - } - -#if DEBUG - // Ensure the polygon is convex. - for (int i = 0; i < _vertexCount; ++i) - { - for (int j = 0; j < _vertexCount; ++j) - { - // Don't check vertices on the current edge. - if (j == i || j == (i + 1) % _vertexCount) - { - continue; - } - - // Your polygon is non-convex (it has an indentation). - // Or your polygon is too skinny. - float s = Vec2.Dot(_normals[i], _vertices[j] - _vertices[i]); - Box2DNetDebug.Assert(s < -Settings.LinearSlop); - } - } - - // Ensure the polygon is counter-clockwise. - for (int i = 1; i < _vertexCount; ++i) - { - float cross = Vec2.Cross(_normals[i - 1], _normals[i]); - - // Keep asinf happy. - cross = Common.Math.Clamp(cross, -1.0f, 1.0f); - - // You have consecutive edges that are almost parallel on your polygon. - float angle = (float)System.Math.Asin(cross); - Box2DNetDebug.Assert(angle > Settings.AngularSlop); - } -#endif - - // Compute the polygon centroid. - _centroid = ComputeCentroid(poly.Vertices, poly.VertexCount); - - // Compute the oriented bounding box. - ComputeOBB(out _obb, _vertices, _vertexCount); - - // Create core polygon shape by shifting edges inward. - // Also compute the min/max radius for CCD. - for (int i = 0; i < _vertexCount; ++i) - { - int i1 = i - 1 >= 0 ? i - 1 : _vertexCount - 1; - int i2 = i; - - Vec2 n1 = _normals[i1]; - Vec2 n2 = _normals[i2]; - Vec2 v = _vertices[i] - _centroid; ; - - Vec2 d = new Vec2(); - d.X = Vec2.Dot(n1, v) - Settings.ToiSlop; - d.Y = Vec2.Dot(n2, v) - Settings.ToiSlop; - - // Shifting the edge inward by b2_toiSlop should - // not cause the plane to pass the centroid. - - // Your shape has a radius/extent less than b2_toiSlop. - Box2DNetDebug.Assert(d.X >= 0.0f); - Box2DNetDebug.Assert(d.Y >= 0.0f); - Mat22 A = new Mat22(); - A.Col1.X = n1.X; A.Col2.X = n1.Y; - A.Col1.Y = n2.X; A.Col2.Y = n2.Y; - _coreVertices[i] = A.Solve(d) + _centroid; - }*/ - } - - /*// http://www.geometrictools.com/Documentation/MinimumAreaRectangle.pdf - public static void ComputeOBB(out OBB obb, Vec2[] vs, int count) - { - obb = new OBB(); - - Box2DNetDebug.Assert(count <= Settings.MaxPolygonVertices); - Vec2[] p = new Vec2[Settings.MaxPolygonVertices + 1]; - for (int i = 0; i < count; ++i) - { - p[i] = vs[i]; - } - p[count] = p[0]; - - float minArea = Common.Settings.FLT_MAX; - - for (int i = 1; i <= count; ++i) - { - Vec2 root = p[i - 1]; - Vec2 ux = p[i] - root; - float length = ux.Normalize(); - Box2DNetDebug.Assert(length > Common.Settings.FLT_EPSILON); - Vec2 uy = new Vec2(-ux.Y, ux.X); - Vec2 lower = new Vec2(Common.Settings.FLT_MAX, Common.Settings.FLT_MAX); - Vec2 upper = new Vec2(-Common.Settings.FLT_MAX, -Common.Settings.FLT_MAX); - - for (int j = 0; j < count; ++j) - { - Vec2 d = p[j] - root; - Vec2 r = new Vec2(); - r.X = Vec2.Dot(ux, d); - r.Y = Vec2.Dot(uy, d); - lower = Common.Math.Min(lower, r); - upper = Common.Math.Max(upper, r); - } - - float area = (upper.X - lower.X) * (upper.Y - lower.Y); - if (area < 0.95f * minArea) - { - minArea = area; - obb.R.Col1 = ux; - obb.R.Col2 = uy; - Vec2 center = 0.5f * (lower + upper); - obb.Center = root + Common.Math.Mul(obb.R, center); - obb.Extents = 0.5f * (upper - lower); - } - } - - Box2DNetDebug.Assert(minArea < Common.Settings.FLT_MAX); - }*/ - } +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System.Diagnostics; +using Box2DNet.Common; +using Box2DNet.Common.ConvexHull; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Collision.Shapes +{ + /// + /// Represents a simple non-selfintersecting convex polygon. + /// Create a convex hull from the given array of points. + /// + public class PolygonShape : Shape + { + private Vertices _vertices; + private Vertices _normals; + + /// + /// Initializes a new instance of the class. + /// + /// The vertices. + /// The density. + public PolygonShape(Vertices vertices, float density) + : base(density) + { + ShapeType = ShapeType.Polygon; + _radius = Settings.PolygonRadius; + + Vertices = vertices; + } + + /// + /// Create a new PolygonShape with the specified density. + /// + /// The density. + public PolygonShape(float density) + : base(density) + { + Debug.Assert(density >= 0f); + + ShapeType = ShapeType.Polygon; + _radius = Settings.PolygonRadius; + _vertices = new Vertices(Settings.MaxPolygonVertices); + _normals = new Vertices(Settings.MaxPolygonVertices); + } + + internal PolygonShape() + : base(0) + { + ShapeType = ShapeType.Polygon; + _radius = Settings.PolygonRadius; + _vertices = new Vertices(Settings.MaxPolygonVertices); + _normals = new Vertices(Settings.MaxPolygonVertices); + } + + /// + /// Create a convex hull from the given array of local points. + /// The number of vertices must be in the range [3, Settings.MaxPolygonVertices]. + /// Warning: the points may be re-ordered, even if they form a convex polygon + /// Warning: collinear points are handled but not removed. Collinear points may lead to poor stacking behavior. + /// + public Vertices Vertices + { + get { return _vertices; } + set + { + _vertices = new Vertices(value); + + Debug.Assert(_vertices.Count >= 3 && _vertices.Count <= Settings.MaxPolygonVertices); + + if (Settings.UseConvexHullPolygons) + { + //FPE note: This check is required as the GiftWrap algorithm early exits on triangles + //So instead of giftwrapping a triangle, we just force it to be clock wise. + if (_vertices.Count <= 3) + _vertices.ForceCounterClockWise(); + else + _vertices = GiftWrap.GetConvexHull(_vertices); + } + + _normals = new Vertices(_vertices.Count); + + // Compute normals. Ensure the edges have non-zero length. + for (int i = 0; i < _vertices.Count; ++i) + { + int next = i + 1 < _vertices.Count ? i + 1 : 0; + Vector2 edge = _vertices[next] - _vertices[i]; + Debug.Assert(edge.LengthSquared() > Settings.Epsilon * Settings.Epsilon); + + //FPE optimization: Normals.Add(MathHelper.Cross(edge, 1.0f)); + Vector2 temp = new Vector2(edge.Y, -edge.X); + temp.Normalize(); + _normals.Add(temp); + } + + // Compute the polygon mass data + ComputeProperties(); + } + } + + public Vertices Normals { get { return _normals; } } + + public override int ChildCount { get { return 1; } } + + protected override void ComputeProperties() + { + // Polygon mass, centroid, and inertia. + // Let rho be the polygon density in mass per unit area. + // Then: + // mass = rho * int(dA) + // centroid.X = (1/mass) * rho * int(x * dA) + // centroid.Y = (1/mass) * rho * int(y * dA) + // I = rho * int((x*x + y*y) * dA) + // + // We can compute these integrals by summing all the integrals + // for each triangle of the polygon. To evaluate the integral + // for a single triangle, we make a change of variables to + // the (u,v) coordinates of the triangle: + // x = x0 + e1x * u + e2x * v + // y = y0 + e1y * u + e2y * v + // where 0 <= u && 0 <= v && u + v <= 1. + // + // We integrate u from [0,1-v] and then v from [0,1]. + // We also need to use the Jacobian of the transformation: + // D = cross(e1, e2) + // + // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3) + // + // The rest of the derivation is handled by computer algebra. + + Debug.Assert(Vertices.Count >= 3); + + //FPE optimization: Early exit as polygons with 0 density does not have any properties. + if (_density <= 0) + return; + + //FPE optimization: Consolidated the calculate centroid and mass code to a single method. + Vector2 center = Vector2.Zero; + float area = 0.0f; + float I = 0.0f; + + // pRef is the reference point for forming triangles. + // It's location doesn't change the result (except for rounding error). + Vector2 s = Vector2.Zero; + + // This code would put the reference point inside the polygon. + for (int i = 0; i < Vertices.Count; ++i) + { + s += Vertices[i]; + } + s *= 1.0f / Vertices.Count; + + const float k_inv3 = 1.0f / 3.0f; + + for (int i = 0; i < Vertices.Count; ++i) + { + // Triangle vertices. + Vector2 e1 = Vertices[i] - s; + Vector2 e2 = i + 1 < Vertices.Count ? Vertices[i + 1] - s : Vertices[0] - s; + + float D = MathUtils.Cross(e1, e2); + + float triangleArea = 0.5f * D; + area += triangleArea; + + // Area weighted centroid + center += triangleArea * k_inv3 * (e1 + e2); + + float ex1 = e1.X, ey1 = e1.Y; + float ex2 = e2.X, ey2 = e2.Y; + + float intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2; + float inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2; + + I += (0.25f * k_inv3 * D) * (intx2 + inty2); + } + + //The area is too small for the engine to handle. + Debug.Assert(area > Settings.Epsilon); + + // We save the area + MassData.Area = area; + + // Total mass + MassData.Mass = _density * area; + + // Center of mass + center *= 1.0f / area; + MassData.Centroid = center + s; + + // Inertia tensor relative to the local origin (point s). + MassData.Inertia = _density * I; + + // Shift to center of mass then to original body origin. + MassData.Inertia += MassData.Mass * (Vector2.Dot(MassData.Centroid, MassData.Centroid) - Vector2.Dot(center, center)); + } + + public override bool TestPoint(ref Transform transform, ref Vector2 point) + { + Vector2 pLocal = MathUtils.MulT(transform.q, point - transform.p); + + for (int i = 0; i < Vertices.Count; ++i) + { + float dot = Vector2.Dot(Normals[i], pLocal - Vertices[i]); + if (dot > 0.0f) + { + return false; + } + } + + return true; + } + + public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex) + { + output = new RayCastOutput(); + + // Put the ray into the polygon's frame of reference. + Vector2 p1 = MathUtils.MulT(transform.q, input.Point1 - transform.p); + Vector2 p2 = MathUtils.MulT(transform.q, input.Point2 - transform.p); + Vector2 d = p2 - p1; + + float lower = 0.0f, upper = input.MaxFraction; + + int index = -1; + + for (int i = 0; i < Vertices.Count; ++i) + { + // p = p1 + a * d + // dot(normal, p - v) = 0 + // dot(normal, p1 - v) + a * dot(normal, d) = 0 + float numerator = Vector2.Dot(Normals[i], Vertices[i] - p1); + float denominator = Vector2.Dot(Normals[i], d); + + if (denominator == 0.0f) + { + if (numerator < 0.0f) + { + return false; + } + } + else + { + // Note: we want this predicate without division: + // lower < numerator / denominator, where denominator < 0 + // Since denominator < 0, we have to flip the inequality: + // lower < numerator / denominator <==> denominator * lower > numerator. + if (denominator < 0.0f && numerator < lower * denominator) + { + // Increase lower. + // The segment enters this half-space. + lower = numerator / denominator; + index = i; + } + else if (denominator > 0.0f && numerator < upper * denominator) + { + // Decrease upper. + // The segment exits this half-space. + upper = numerator / denominator; + } + } + + // The use of epsilon here causes the assert on lower to trip + // in some cases. Apparently the use of epsilon was to make edge + // shapes work, but now those are handled separately. + //if (upper < lower - b2_epsilon) + if (upper < lower) + { + return false; + } + } + + Debug.Assert(0.0f <= lower && lower <= input.MaxFraction); + + if (index >= 0) + { + output.Fraction = lower; + output.Normal = MathUtils.Mul(transform.q, Normals[index]); + return true; + } + + return false; + } + + /// + /// Given a transform, compute the associated axis aligned bounding box for a child shape. + /// + /// The aabb results. + /// The world transform of the shape. + /// The child shape index. + public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex) + { + Vector2 lower = MathUtils.Mul(ref transform, Vertices[0]); + Vector2 upper = lower; + + for (int i = 1; i < Vertices.Count; ++i) + { + Vector2 v = MathUtils.Mul(ref transform, Vertices[i]); + lower = Vector2.Min(lower, v); + upper = Vector2.Max(upper, v); + } + + Vector2 r = new Vector2(Radius, Radius); + aabb.LowerBound = lower - r; + aabb.UpperBound = upper + r; + } + + public override float ComputeSubmergedArea(ref Vector2 normal, float offset, ref Transform xf, out Vector2 sc) + { + sc = Vector2.Zero; + + //Transform plane into shape co-ordinates + Vector2 normalL = MathUtils.MulT(xf.q, normal); + float offsetL = offset - Vector2.Dot(normal, xf.p); + + float[] depths = new float[Settings.MaxPolygonVertices]; + int diveCount = 0; + int intoIndex = -1; + int outoIndex = -1; + + bool lastSubmerged = false; + int i; + for (i = 0; i < Vertices.Count; i++) + { + depths[i] = Vector2.Dot(normalL, Vertices[i]) - offsetL; + bool isSubmerged = depths[i] < -Settings.Epsilon; + if (i > 0) + { + if (isSubmerged) + { + if (!lastSubmerged) + { + intoIndex = i - 1; + diveCount++; + } + } + else + { + if (lastSubmerged) + { + outoIndex = i - 1; + diveCount++; + } + } + } + lastSubmerged = isSubmerged; + } + switch (diveCount) + { + case 0: + if (lastSubmerged) + { + //Completely submerged + sc = MathUtils.Mul(ref xf, MassData.Centroid); + return MassData.Mass / Density; + } + + //Completely dry + return 0; + case 1: + if (intoIndex == -1) + { + intoIndex = Vertices.Count - 1; + } + else + { + outoIndex = Vertices.Count - 1; + } + break; + } + + int intoIndex2 = (intoIndex + 1) % Vertices.Count; + int outoIndex2 = (outoIndex + 1) % Vertices.Count; + + float intoLambda = (0 - depths[intoIndex]) / (depths[intoIndex2] - depths[intoIndex]); + float outoLambda = (0 - depths[outoIndex]) / (depths[outoIndex2] - depths[outoIndex]); + + Vector2 intoVec = new Vector2(Vertices[intoIndex].X * (1 - intoLambda) + Vertices[intoIndex2].X * intoLambda, Vertices[intoIndex].Y * (1 - intoLambda) + Vertices[intoIndex2].Y * intoLambda); + Vector2 outoVec = new Vector2(Vertices[outoIndex].X * (1 - outoLambda) + Vertices[outoIndex2].X * outoLambda, Vertices[outoIndex].Y * (1 - outoLambda) + Vertices[outoIndex2].Y * outoLambda); + + //Initialize accumulator + float area = 0; + Vector2 center = new Vector2(0, 0); + Vector2 p2 = Vertices[intoIndex2]; + + const float k_inv3 = 1.0f / 3.0f; + + //An awkward loop from intoIndex2+1 to outIndex2 + i = intoIndex2; + while (i != outoIndex2) + { + i = (i + 1) % Vertices.Count; + Vector2 p3; + if (i == outoIndex2) + p3 = outoVec; + else + p3 = Vertices[i]; + //Add the triangle formed by intoVec,p2,p3 + { + Vector2 e1 = p2 - intoVec; + Vector2 e2 = p3 - intoVec; + + float D = MathUtils.Cross(e1, e2); + + float triangleArea = 0.5f * D; + + area += triangleArea; + + // Area weighted centroid + center += triangleArea * k_inv3 * (intoVec + p2 + p3); + } + + p2 = p3; + } + + //Normalize and transform centroid + center *= 1.0f / area; + + sc = MathUtils.Mul(ref xf, center); + + return area; + } + + public bool CompareTo(PolygonShape shape) + { + if (Vertices.Count != shape.Vertices.Count) + return false; + + for (int i = 0; i < Vertices.Count; i++) + { + if (Vertices[i] != shape.Vertices[i]) + return false; + } + + return (Radius == shape.Radius && MassData == shape.MassData); + } + + public override Shape Clone() + { + PolygonShape clone = new PolygonShape(); + clone.ShapeType = ShapeType; + clone._radius = _radius; + clone._density = _density; + clone._vertices = new Vertices(_vertices); + clone._normals = new Vertices(_normals); + clone.MassData = MassData; + return clone; + } + } } \ No newline at end of file diff --git a/src/Box2DNet/Collision/Shapes/Shape.cs b/src/Box2DNet/Collision/Shapes/Shape.cs index e9e1518..dd061b7 100644 --- a/src/Box2DNet/Collision/Shapes/Shape.cs +++ b/src/Box2DNet/Collision/Shapes/Shape.cs @@ -1,147 +1,255 @@ -/* - Box2DNet Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version 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.Numerics; -using Box2DNet.Common; - - -using Transform = Box2DNet.Common.Transform; - -namespace Box2DNet.Collision -{ - /// - /// This holds the mass data computed for a shape. - /// - public struct MassData - { - /// - /// The mass of the shape, usually in kilograms. - /// - public float Mass; - - /// - /// The position of the shape's centroid relative to the shape's origin. - /// - public Vector2 Center; - - /// - /// The rotational inertia of the shape. - /// - public float I; - } - - /// - /// The various collision shape types supported by Box2D. - /// - public enum ShapeType - { - UnknownShape = -1, - CircleShape, - PolygonShape, - EdgeShape, - ShapeTypeCount, - } - - /// - /// Returns code from TestSegment - /// - public enum SegmentCollide - { - StartInsideCollide = -1, - MissCollide = 0, - HitCollide = 1 - } - - /// - /// A shape is used for collision detection. You can create a shape however you like. - /// Shapes used for simulation in World are created automatically when a Fixture is created. - /// - public abstract class Shape : IDisposable - { - #region Fields - - protected ShapeType _type = ShapeType.UnknownShape; - internal float _radius; - - #endregion Fields - - protected Shape() { } - - /// - /// Test a point for containment in this shape. This only works for convex shapes. - /// - /// The shape world Transform. - /// A point in world coordinates. - /// - public abstract bool TestPoint(Transform xf, Vector2 p); - - /// - /// Perform a ray cast against this shape. - /// - /// The shape world Transform. - /// Returns the hit fraction. You can use this to compute the contact point - /// p = (1 - lambda) * segment.P1 + lambda * segment.P2. - /// Returns the normal at the contact point. If there is no intersection, - /// the normal is not set. - /// Defines the begin and end point of the ray cast. - /// A number typically in the range [0,1]. - public abstract SegmentCollide TestSegment(Transform xf, out float lambda, out Vector2 normal, Segment segment, float maxLambda); - - /// - /// Given a Transform, compute the associated axis aligned bounding box for this shape. - /// - /// Returns the axis aligned box. - /// The world Transform of the shape. - public abstract void ComputeAABB(out AABB aabb, Transform xf); - - /// - /// Compute the mass properties of this shape using its dimensions and density. - /// The inertia tensor is computed about the local origin, not the centroid. - /// - /// Returns the mass data for this shape - public abstract void ComputeMass(out MassData massData, float density); - - /// - /// Compute the volume and centroid of this shape intersected with a half plane. - /// - /// Normal the surface normal. - /// Offset the surface offset along normal. - /// The shape Transform. - /// Returns the centroid. - /// The total volume less than offset along normal. - public abstract float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 c); - - /// - /// Compute the sweep radius. This is used for conservative advancement (continuous collision detection). - /// - /// Pivot is the pivot point for rotation. - /// The distance of the furthest point from the pivot. - public abstract float ComputeSweepRadius(Vector2 pivot); - - public abstract Vector2 GetVertex(int index); - - public abstract int GetSupport(Vector2 d); - - public abstract Vector2 GetSupportVertex(Vector2 d); - - public virtual void Dispose(){} - } +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using Box2DNet.Common; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Collision.Shapes +{ + /// + /// This holds the mass data computed for a shape. + /// + public struct MassData : IEquatable + { + /// + /// The area of the shape + /// + public float Area { get; internal set; } + + /// + /// The position of the shape's centroid relative to the shape's origin. + /// + public Vector2 Centroid { get; internal set; } + + /// + /// The rotational inertia of the shape about the local origin. + /// + public float Inertia { get; internal set; } + + /// + /// The mass of the shape, usually in kilograms. + /// + public float Mass { get; internal set; } + + /// + /// The equal operator + /// + /// + /// + /// + public static bool operator ==(MassData left, MassData right) + { + return (left.Area == right.Area && left.Mass == right.Mass && left.Centroid == right.Centroid && left.Inertia == right.Inertia); + } + + /// + /// The not equal operator + /// + /// + /// + /// + public static bool operator !=(MassData left, MassData right) + { + return !(left == right); + } + + public bool Equals(MassData other) + { + return this == other; + } + + public override bool Equals(object obj) + { + if (ReferenceEquals(null, obj)) + return false; + + if (obj.GetType() != typeof(MassData)) + return false; + + return Equals((MassData)obj); + } + + public override int GetHashCode() + { + unchecked + { + int result = Area.GetHashCode(); + result = (result * 397) ^ Centroid.GetHashCode(); + result = (result * 397) ^ Inertia.GetHashCode(); + result = (result * 397) ^ Mass.GetHashCode(); + return result; + } + } + } + + public enum ShapeType + { + Unknown = -1, + Circle = 0, + Edge = 1, + Polygon = 2, + Chain = 3, + TypeCount = 4, + } + + /// + /// A shape is used for collision detection. You can create a shape however you like. + /// Shapes used for simulation in World are created automatically when a Fixture + /// is created. Shapes may encapsulate a one or more child shapes. + /// + public abstract class Shape + { + internal float _density; + internal float _radius; + internal float _2radius; + + protected Shape(float density) + { + _density = density; + ShapeType = ShapeType.Unknown; + } + + /// + /// Contains the properties of the shape such as: + /// - Area of the shape + /// - Centroid + /// - Inertia + /// - Mass + /// + public MassData MassData; + + /// + /// Get the type of this shape. + /// + /// The type of the shape. + public ShapeType ShapeType { get; internal set; } + + /// + /// Get the number of child primitives. + /// + /// + public abstract int ChildCount { get; } + + /// + /// Gets or sets the density. + /// Changing the density causes a recalculation of shape properties. + /// + /// The density. + public float Density + { + get { return _density; } + set + { + Debug.Assert(value >= 0); + + _density = value; + ComputeProperties(); + } + } + + /// + /// Radius of the Shape + /// Changing the radius causes a recalculation of shape properties. + /// + public float Radius + { + get { return _radius; } + set + { + Debug.Assert(value >= 0); + + _radius = value; + _2radius = _radius * _radius; + + ComputeProperties(); + } + } + + /// + /// Clone the concrete shape + /// + /// A clone of the shape + public abstract Shape Clone(); + + /// + /// Test a point for containment in this shape. + /// Note: This only works for convex shapes. + /// + /// The shape world transform. + /// A point in world coordinates. + /// True if the point is inside the shape + public abstract bool TestPoint(ref Transform transform, ref Vector2 point); + + /// + /// Cast a ray against a child shape. + /// + /// The ray-cast results. + /// The ray-cast input parameters. + /// The transform to be applied to the shape. + /// The child shape index. + /// True if the ray-cast hits the shape + public abstract bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex); + + /// + /// Given a transform, compute the associated axis aligned bounding box for a child shape. + /// + /// The aabb results. + /// The world transform of the shape. + /// The child shape index. + public abstract void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex); + + /// + /// Compute the mass properties of this shape using its dimensions and density. + /// The inertia tensor is computed about the local origin, not the centroid. + /// + protected abstract void ComputeProperties(); + + /// + /// Compare this shape to another shape based on type and properties. + /// + /// The other shape + /// True if the two shapes are the same. + public bool CompareTo(Shape shape) + { + if (shape is PolygonShape && this is PolygonShape) + return ((PolygonShape)this).CompareTo((PolygonShape)shape); + + if (shape is CircleShape && this is CircleShape) + return ((CircleShape)this).CompareTo((CircleShape)shape); + + if (shape is EdgeShape && this is EdgeShape) + return ((EdgeShape)this).CompareTo((EdgeShape)shape); + + if (shape is ChainShape && this is ChainShape) + return ((ChainShape)this).CompareTo((ChainShape)shape); + + return false; + } + + /// + /// Used for the buoyancy controller + /// + public abstract float ComputeSubmergedArea(ref Vector2 normal, float offset, ref Transform xf, out Vector2 sc); + } } \ No newline at end of file diff --git a/src/Box2DNet/Collision/TimeOfImpact.cs b/src/Box2DNet/Collision/TimeOfImpact.cs new file mode 100644 index 0000000..7606cad --- /dev/null +++ b/src/Box2DNet/Collision/TimeOfImpact.cs @@ -0,0 +1,498 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using Box2DNet.Common; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Collision +{ + /// + /// Input parameters for CalculateTimeOfImpact + /// + public class TOIInput + { + public DistanceProxy ProxyA = new DistanceProxy(); + public DistanceProxy ProxyB = new DistanceProxy(); + public Sweep SweepA; + public Sweep SweepB; + public float TMax; // defines sweep interval [0, tMax] + } + + public enum TOIOutputState + { + Unknown, + Failed, + Overlapped, + Touching, + Seperated, + } + + public struct TOIOutput + { + public TOIOutputState State; + public float T; + } + + public enum SeparationFunctionType + { + Points, + FaceA, + FaceB + } + + public static class SeparationFunction + { + [ThreadStatic] + private static Vector2 _axis; + [ThreadStatic] + private static Vector2 _localPoint; + [ThreadStatic] + private static DistanceProxy _proxyA; + [ThreadStatic] + private static DistanceProxy _proxyB; + [ThreadStatic] + private static Sweep _sweepA, _sweepB; + [ThreadStatic] + private static SeparationFunctionType _type; + + public static void Set(ref SimplexCache cache, DistanceProxy proxyA, ref Sweep sweepA, DistanceProxy proxyB, ref Sweep sweepB, float t1) + { + _localPoint = Vector2.Zero; + _proxyA = proxyA; + _proxyB = proxyB; + int count = cache.Count; + Debug.Assert(0 < count && count < 3); + + _sweepA = sweepA; + _sweepB = sweepB; + + Transform xfA, xfB; + _sweepA.GetTransform(out xfA, t1); + _sweepB.GetTransform(out xfB, t1); + + if (count == 1) + { + _type = SeparationFunctionType.Points; + Vector2 localPointA = _proxyA.Vertices[cache.IndexA[0]]; + Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]]; + Vector2 pointA = MathUtils.Mul(ref xfA, localPointA); + Vector2 pointB = MathUtils.Mul(ref xfB, localPointB); + _axis = pointB - pointA; + _axis.Normalize(); + } + else if (cache.IndexA[0] == cache.IndexA[1]) + { + // Two points on B and one on A. + _type = SeparationFunctionType.FaceB; + Vector2 localPointB1 = proxyB.Vertices[cache.IndexB[0]]; + Vector2 localPointB2 = proxyB.Vertices[cache.IndexB[1]]; + + Vector2 a = localPointB2 - localPointB1; + _axis = new Vector2(a.Y, -a.X); + _axis.Normalize(); + Vector2 normal = MathUtils.Mul(ref xfB.q, _axis); + + _localPoint = 0.5f * (localPointB1 + localPointB2); + Vector2 pointB = MathUtils.Mul(ref xfB, _localPoint); + + Vector2 localPointA = proxyA.Vertices[cache.IndexA[0]]; + Vector2 pointA = MathUtils.Mul(ref xfA, localPointA); + + float s = Vector2.Dot(pointA - pointB, normal); + if (s < 0.0f) + { + _axis = -_axis; + } + } + else + { + // Two points on A and one or two points on B. + _type = SeparationFunctionType.FaceA; + Vector2 localPointA1 = _proxyA.Vertices[cache.IndexA[0]]; + Vector2 localPointA2 = _proxyA.Vertices[cache.IndexA[1]]; + + Vector2 a = localPointA2 - localPointA1; + _axis = new Vector2(a.Y, -a.X); + _axis.Normalize(); + Vector2 normal = MathUtils.Mul(ref xfA.q, _axis); + + _localPoint = 0.5f * (localPointA1 + localPointA2); + Vector2 pointA = MathUtils.Mul(ref xfA, _localPoint); + + Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]]; + Vector2 pointB = MathUtils.Mul(ref xfB, localPointB); + + float s = Vector2.Dot(pointB - pointA, normal); + if (s < 0.0f) + { + _axis = -_axis; + } + } + + //FPE note: the returned value that used to be here has been removed, as it was not used. + } + + public static float FindMinSeparation(out int indexA, out int indexB, float t) + { + Transform xfA, xfB; + _sweepA.GetTransform(out xfA, t); + _sweepB.GetTransform(out xfB, t); + + switch (_type) + { + case SeparationFunctionType.Points: + { + Vector2 axisA = MathUtils.MulT(ref xfA.q, _axis); + Vector2 axisB = MathUtils.MulT(ref xfB.q, -_axis); + + indexA = _proxyA.GetSupport(axisA); + indexB = _proxyB.GetSupport(axisB); + + Vector2 localPointA = _proxyA.Vertices[indexA]; + Vector2 localPointB = _proxyB.Vertices[indexB]; + + Vector2 pointA = MathUtils.Mul(ref xfA, localPointA); + Vector2 pointB = MathUtils.Mul(ref xfB, localPointB); + + float separation = Vector2.Dot(pointB - pointA, _axis); + return separation; + } + + case SeparationFunctionType.FaceA: + { + Vector2 normal = MathUtils.Mul(ref xfA.q, _axis); + Vector2 pointA = MathUtils.Mul(ref xfA, _localPoint); + + Vector2 axisB = MathUtils.MulT(ref xfB.q, -normal); + + indexA = -1; + indexB = _proxyB.GetSupport(axisB); + + Vector2 localPointB = _proxyB.Vertices[indexB]; + Vector2 pointB = MathUtils.Mul(ref xfB, localPointB); + + float separation = Vector2.Dot(pointB - pointA, normal); + return separation; + } + + case SeparationFunctionType.FaceB: + { + Vector2 normal = MathUtils.Mul(ref xfB.q, _axis); + Vector2 pointB = MathUtils.Mul(ref xfB, _localPoint); + + Vector2 axisA = MathUtils.MulT(ref xfA.q, -normal); + + indexB = -1; + indexA = _proxyA.GetSupport(axisA); + + Vector2 localPointA = _proxyA.Vertices[indexA]; + Vector2 pointA = MathUtils.Mul(ref xfA, localPointA); + + float separation = Vector2.Dot(pointA - pointB, normal); + return separation; + } + + default: + Debug.Assert(false); + indexA = -1; + indexB = -1; + return 0.0f; + } + } + + public static float Evaluate(int indexA, int indexB, float t) + { + Transform xfA, xfB; + _sweepA.GetTransform(out xfA, t); + _sweepB.GetTransform(out xfB, t); + + switch (_type) + { + case SeparationFunctionType.Points: + { + Vector2 localPointA = _proxyA.Vertices[indexA]; + Vector2 localPointB = _proxyB.Vertices[indexB]; + + Vector2 pointA = MathUtils.Mul(ref xfA, localPointA); + Vector2 pointB = MathUtils.Mul(ref xfB, localPointB); + float separation = Vector2.Dot(pointB - pointA, _axis); + + return separation; + } + case SeparationFunctionType.FaceA: + { + Vector2 normal = MathUtils.Mul(ref xfA.q, _axis); + Vector2 pointA = MathUtils.Mul(ref xfA, _localPoint); + + Vector2 localPointB = _proxyB.Vertices[indexB]; + Vector2 pointB = MathUtils.Mul(ref xfB, localPointB); + + float separation = Vector2.Dot(pointB - pointA, normal); + return separation; + } + case SeparationFunctionType.FaceB: + { + Vector2 normal = MathUtils.Mul(ref xfB.q, _axis); + Vector2 pointB = MathUtils.Mul(ref xfB, _localPoint); + + Vector2 localPointA = _proxyA.Vertices[indexA]; + Vector2 pointA = MathUtils.Mul(ref xfA, localPointA); + + float separation = Vector2.Dot(pointA - pointB, normal); + return separation; + } + default: + Debug.Assert(false); + return 0.0f; + } + } + } + + public static class TimeOfImpact + { + // CCD via the local separating axis method. This seeks progression + // by computing the largest time at which separation is maintained. + + [ThreadStatic] + public static int TOICalls, TOIIters, TOIMaxIters; + [ThreadStatic] + public static int TOIRootIters, TOIMaxRootIters; + [ThreadStatic] + private static DistanceInput _distanceInput; + + /// + /// Compute the upper bound on time before two shapes penetrate. Time is represented as + /// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate, + /// non-tunneling collision. If you change the time interval, you should call this function + /// again. + /// Note: use Distance() to compute the contact point and normal at the time of impact. + /// + /// The output. + /// The input. + public static void CalculateTimeOfImpact(out TOIOutput output, TOIInput input) + { + if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled + ++TOICalls; + + output = new TOIOutput(); + output.State = TOIOutputState.Unknown; + output.T = input.TMax; + + Sweep sweepA = input.SweepA; + Sweep sweepB = input.SweepB; + + // Large rotations can make the root finder fail, so we normalize the + // sweep angles. + sweepA.Normalize(); + sweepB.Normalize(); + + float tMax = input.TMax; + + float totalRadius = input.ProxyA.Radius + input.ProxyB.Radius; + float target = Math.Max(Settings.LinearSlop, totalRadius - 3.0f * Settings.LinearSlop); + const float tolerance = 0.25f * Settings.LinearSlop; + Debug.Assert(target > tolerance); + + float t1 = 0.0f; + const int k_maxIterations = 20; + int iter = 0; + + // Prepare input for distance query. + _distanceInput = _distanceInput ?? new DistanceInput(); + _distanceInput.ProxyA = input.ProxyA; + _distanceInput.ProxyB = input.ProxyB; + _distanceInput.UseRadii = false; + + // The outer loop progressively attempts to compute new separating axes. + // This loop terminates when an axis is repeated (no progress is made). + for (; ; ) + { + Transform xfA, xfB; + sweepA.GetTransform(out xfA, t1); + sweepB.GetTransform(out xfB, t1); + + // Get the distance between shapes. We can also use the results + // to get a separating axis. + _distanceInput.TransformA = xfA; + _distanceInput.TransformB = xfB; + DistanceOutput distanceOutput; + SimplexCache cache; + Distance.ComputeDistance(out distanceOutput, out cache, _distanceInput); + + // If the shapes are overlapped, we give up on continuous collision. + if (distanceOutput.Distance <= 0.0f) + { + // Failure! + output.State = TOIOutputState.Overlapped; + output.T = 0.0f; + break; + } + + if (distanceOutput.Distance < target + tolerance) + { + // Victory! + output.State = TOIOutputState.Touching; + output.T = t1; + break; + } + + SeparationFunction.Set(ref cache, input.ProxyA, ref sweepA, input.ProxyB, ref sweepB, t1); + + // Compute the TOI on the separating axis. We do this by successively + // resolving the deepest point. This loop is bounded by the number of vertices. + bool done = false; + float t2 = tMax; + int pushBackIter = 0; + for (; ; ) + { + // Find the deepest point at t2. Store the witness point indices. + int indexA, indexB; + float s2 = SeparationFunction.FindMinSeparation(out indexA, out indexB, t2); + + // Is the final configuration separated? + if (s2 > target + tolerance) + { + // Victory! + output.State = TOIOutputState.Seperated; + output.T = tMax; + done = true; + break; + } + + // Has the separation reached tolerance? + if (s2 > target - tolerance) + { + // Advance the sweeps + t1 = t2; + break; + } + + // Compute the initial separation of the witness points. + float s1 = SeparationFunction.Evaluate(indexA, indexB, t1); + + // Check for initial overlap. This might happen if the root finder + // runs out of iterations. + if (s1 < target - tolerance) + { + output.State = TOIOutputState.Failed; + output.T = t1; + done = true; + break; + } + + // Check for touching + if (s1 <= target + tolerance) + { + // Victory! t1 should hold the TOI (could be 0.0). + output.State = TOIOutputState.Touching; + output.T = t1; + done = true; + break; + } + + // Compute 1D root of: f(x) - target = 0 + int rootIterCount = 0; + float a1 = t1, a2 = t2; + for (; ; ) + { + // Use a mix of the secant rule and bisection. + float t; + if ((rootIterCount & 1) != 0) + { + // Secant rule to improve convergence. + t = a1 + (target - s1) * (a2 - a1) / (s2 - s1); + } + else + { + // Bisection to guarantee progress. + t = 0.5f * (a1 + a2); + } + + ++rootIterCount; + + if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled + ++TOIRootIters; + + float s = SeparationFunction.Evaluate(indexA, indexB, t); + + if (Math.Abs(s - target) < tolerance) + { + // t2 holds a tentative value for t1 + t2 = t; + break; + } + + // Ensure we continue to bracket the root. + if (s > target) + { + a1 = t; + s1 = s; + } + else + { + a2 = t; + s2 = s; + } + + if (rootIterCount == 50) + { + break; + } + } + + if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled + TOIMaxRootIters = Math.Max(TOIMaxRootIters, rootIterCount); + + ++pushBackIter; + + if (pushBackIter == Settings.MaxPolygonVertices) + { + break; + } + } + + ++iter; + + if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled + ++TOIIters; + + if (done) + { + break; + } + + if (iter == k_maxIterations) + { + // Root finder got stuck. Semi-victory. + output.State = TOIOutputState.Failed; + output.T = t1; + break; + } + } + + if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled + TOIMaxIters = Math.Max(TOIMaxIters, iter); + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/ConvexHull/ChainHull.cs b/src/Box2DNet/Common/ConvexHull/ChainHull.cs new file mode 100644 index 0000000..e6507f6 --- /dev/null +++ b/src/Box2DNet/Common/ConvexHull/ChainHull.cs @@ -0,0 +1,143 @@ +using System.Collections.Generic; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Common.ConvexHull +{ + /// + /// Andrew's Monotone Chain Convex Hull algorithm. + /// Used to get the convex hull of a point cloud. + /// + /// Source: http://www.softsurfer.com/Archive/algorithm_0109/algorithm_0109.htm + /// + public static class ChainHull + { + //Copyright 2001, softSurfer (www.softsurfer.com) + + private static PointComparer _pointComparer = new PointComparer(); + + /// + /// Returns the convex hull from the given vertices.. + /// + public static Vertices GetConvexHull(Vertices vertices) + { + if (vertices.Count <= 3) + return vertices; + + Vertices pointSet = new Vertices(vertices); + + //Sort by X-axis + pointSet.Sort(_pointComparer); + + Vector2[] h = new Vector2[pointSet.Count]; + Vertices res; + + int top = -1; // indices for bottom and top of the stack + int i; // array scan index + + // Get the indices of points with min x-coord and min|max y-coord + const int minmin = 0; + float xmin = pointSet[0].X; + for (i = 1; i < pointSet.Count; i++) + { + if (pointSet[i].X != xmin) + break; + } + + // degenerate case: all x-coords == xmin + int minmax = i - 1; + if (minmax == pointSet.Count - 1) + { + h[++top] = pointSet[minmin]; + + if (pointSet[minmax].Y != pointSet[minmin].Y) // a nontrivial segment + h[++top] = pointSet[minmax]; + + h[++top] = pointSet[minmin]; // add polygon endpoint + + res = new Vertices(top + 1); + for (int j = 0; j < top + 1; j++) + { + res.Add(h[j]); + } + + return res; + } + + top = -1; + + // Get the indices of points with max x-coord and min|max y-coord + int maxmax = pointSet.Count - 1; + float xmax = pointSet[pointSet.Count - 1].X; + for (i = pointSet.Count - 2; i >= 0; i--) + { + if (pointSet[i].X != xmax) + break; + } + int maxmin = i + 1; + + // Compute the lower hull on the stack H + h[++top] = pointSet[minmin]; // push minmin point onto stack + i = minmax; + while (++i <= maxmin) + { + // the lower line joins P[minmin] with P[maxmin] + if (MathUtils.Area(pointSet[minmin], pointSet[maxmin], pointSet[i]) >= 0 && i < maxmin) + continue; // ignore P[i] above or on the lower line + + while (top > 0) // there are at least 2 points on the stack + { + // test if P[i] is left of the line at the stack top + if (MathUtils.Area(h[top - 1], h[top], pointSet[i]) > 0) + break; // P[i] is a new hull vertex + + top--; // pop top point off stack + } + h[++top] = pointSet[i]; // push P[i] onto stack + } + + // Next, compute the upper hull on the stack H above the bottom hull + if (maxmax != maxmin) // if distinct xmax points + h[++top] = pointSet[maxmax]; // push maxmax point onto stack + int bot = top; + i = maxmin; + while (--i >= minmax) + { + // the upper line joins P[maxmax] with P[minmax] + if (MathUtils.Area(pointSet[maxmax], pointSet[minmax], pointSet[i]) >= 0 && i > minmax) + continue; // ignore P[i] below or on the upper line + + while (top > bot) // at least 2 points on the upper stack + { + // test if P[i] is left of the line at the stack top + if (MathUtils.Area(h[top - 1], h[top], pointSet[i]) > 0) + break; // P[i] is a new hull vertex + + top--; // pop top point off stack + } + + h[++top] = pointSet[i]; // push P[i] onto stack + } + + if (minmax != minmin) + h[++top] = pointSet[minmin]; // push joining endpoint onto stack + + res = new Vertices(top + 1); + + for (int j = 0; j < top + 1; j++) + { + res.Add(h[j]); + } + + return res; + } + + private class PointComparer : Comparer + { + public override int Compare(Vector2 a, Vector2 b) + { + int f = a.X.CompareTo(b.X); + return f != 0 ? f : a.Y.CompareTo(b.Y); + } + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/ConvexHull/GiftWrap.cs b/src/Box2DNet/Common/ConvexHull/GiftWrap.cs new file mode 100644 index 0000000..29c4bf4 --- /dev/null +++ b/src/Box2DNet/Common/ConvexHull/GiftWrap.cs @@ -0,0 +1,88 @@ +using Microsoft.Xna.Framework; + +namespace Box2DNet.Common.ConvexHull +{ + /// + /// Giftwrap convex hull algorithm. + /// O(nh) time complexity, where n is the number of points and h is the number of points on the convex hull. + /// + /// See http://en.wikipedia.org/wiki/Gift_wrapping_algorithm for more details. + /// + public static class GiftWrap + { + //Extracted from Box2D + + /// + /// Returns the convex hull from the given vertices. + /// + /// The vertices. + public static Vertices GetConvexHull(Vertices vertices) + { + if (vertices.Count <= 3) + return vertices; + + // Find the right most point on the hull + int i0 = 0; + float x0 = vertices[0].X; + for (int i = 1; i < vertices.Count; ++i) + { + float x = vertices[i].X; + if (x > x0 || (x == x0 && vertices[i].Y < vertices[i0].Y)) + { + i0 = i; + x0 = x; + } + } + + int[] hull = new int[vertices.Count]; + int m = 0; + int ih = i0; + + for (; ; ) + { + hull[m] = ih; + + int ie = 0; + for (int j = 1; j < vertices.Count; ++j) + { + if (ie == ih) + { + ie = j; + continue; + } + + Vector2 r = vertices[ie] - vertices[hull[m]]; + Vector2 v = vertices[j] - vertices[hull[m]]; + float c = MathUtils.Cross(ref r, ref v); + if (c < 0.0f) + { + ie = j; + } + + // Collinearity check + if (c == 0.0f && v.LengthSquared() > r.LengthSquared()) + { + ie = j; + } + } + + ++m; + ih = ie; + + if (ie == i0) + { + break; + } + } + + Vertices result = new Vertices(m); + + // Copy vertices. + for (int i = 0; i < m; ++i) + { + result.Add(vertices[hull[i]]); + } + return result; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/ConvexHull/Melkman.cs b/src/Box2DNet/Common/ConvexHull/Melkman.cs new file mode 100644 index 0000000..a1d167b --- /dev/null +++ b/src/Box2DNet/Common/ConvexHull/Melkman.cs @@ -0,0 +1,132 @@ +using Microsoft.Xna.Framework; + +namespace Box2DNet.Common.ConvexHull +{ + /// + /// Creates a convex hull. + /// Note: + /// 1. Vertices must be of a simple polygon, i.e. edges do not overlap. + /// 2. Melkman does not work on point clouds + /// + /// + /// Implemented using Melkman's Convex Hull Algorithm - O(n) time complexity. + /// Reference: http://www.ams.sunysb.edu/~jsbm/courses/345/melkman.pdf + /// + public static class Melkman + { + //Melkman based convex hull algorithm contributed by Cowdozer + + /// + /// Returns a convex hull from the given vertices. + /// + /// A convex hull in counter clockwise winding order. + public static Vertices GetConvexHull(Vertices vertices) + { + if (vertices.Count <= 3) + return vertices; + + //We'll never need a queue larger than the current number of Vertices +1 + //Create double-ended queue + Vector2[] deque = new Vector2[vertices.Count + 1]; + int qf = 3, qb = 0; //Queue front index, queue back index + + //Start by placing first 3 vertices in convex CCW order + int startIndex = 3; + float k = MathUtils.Area(vertices[0], vertices[1], vertices[2]); + if (k == 0) + { + //Vertices are collinear. + deque[0] = vertices[0]; + deque[1] = vertices[2]; //We can skip vertex 1 because it should be between 0 and 2 + deque[2] = vertices[0]; + qf = 2; + + //Go until the end of the collinear sequence of vertices + for (startIndex = 3; startIndex < vertices.Count; startIndex++) + { + Vector2 tmp = vertices[startIndex]; + if (MathUtils.Area(ref deque[0], ref deque[1], ref tmp) == 0) //This point is also collinear + deque[1] = vertices[startIndex]; + else break; + } + } + else + { + deque[0] = deque[3] = vertices[2]; + if (k > 0) + { + //Is Left. Set deque = {2, 0, 1, 2} + deque[1] = vertices[0]; + deque[2] = vertices[1]; + } + else + { + //Is Right. Set deque = {2, 1, 0, 2} + deque[1] = vertices[1]; + deque[2] = vertices[0]; + } + } + + int qfm1 = qf == 0 ? deque.Length - 1 : qf - 1; + int qbm1 = qb == deque.Length - 1 ? 0 : qb + 1; + + //Add vertices one at a time and adjust convex hull as needed + for (int i = startIndex; i < vertices.Count; i++) + { + Vector2 nextPt = vertices[i]; + + //Ignore if it is already within the convex hull we have constructed + if (MathUtils.Area(ref deque[qfm1], ref deque[qf], ref nextPt) > 0 && MathUtils.Area(ref deque[qb], ref deque[qbm1], ref nextPt) > 0) + continue; + + //Pop front until convex + while (!(MathUtils.Area(ref deque[qfm1], ref deque[qf], ref nextPt) > 0)) + { + //Pop the front element from the queue + qf = qfm1; //qf--; + qfm1 = qf == 0 ? deque.Length - 1 : qf - 1; //qfm1 = qf - 1; + } + + //Add vertex to the front of the queue + qf = qf == deque.Length - 1 ? 0 : qf + 1; //qf++; + qfm1 = qf == 0 ? deque.Length - 1 : qf - 1; //qfm1 = qf - 1; + deque[qf] = nextPt; + + //Pop back until convex + while (!(MathUtils.Area(ref deque[qb], ref deque[qbm1], ref nextPt) > 0)) + { + //Pop the back element from the queue + qb = qbm1; //qb++; + qbm1 = qb == deque.Length - 1 ? 0 : qb + 1; //qbm1 = qb + 1; + } + //Add vertex to the back of the queue + qb = qb == 0 ? deque.Length - 1 : qb - 1; //qb--; + qbm1 = qb == deque.Length - 1 ? 0 : qb + 1; //qbm1 = qb + 1; + deque[qb] = nextPt; + } + + //Create the convex hull from what is left in the deque + if (qb < qf) + { + Vertices convexHull = new Vertices(qf); + + for (int i = qb; i < qf; i++) + convexHull.Add(deque[i]); + + return convexHull; + } + else + { + Vertices convexHull = new Vertices(qf + deque.Length); + + for (int i = 0; i < qf; i++) + convexHull.Add(deque[i]); + + for (int i = qb; i < deque.Length; i++) + convexHull.Add(deque[i]); + + return convexHull; + } + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/BayazitDecomposer.cs b/src/Box2DNet/Common/Decomposition/BayazitDecomposer.cs new file mode 100644 index 0000000..2f888bd --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/BayazitDecomposer.cs @@ -0,0 +1,243 @@ +using System.Collections.Generic; +using System.Diagnostics; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Common.Decomposition +{ + //From phed rev 36: http://code.google.com/p/phed/source/browse/trunk/Polygon.cpp + + /// + /// Convex decomposition algorithm created by Mark Bayazit (http://mnbayazit.com/) + /// + /// Properties: + /// - Tries to decompose using polygons instead of triangles. + /// - Tends to produce optimal results with low processing time. + /// - Running time is O(nr), n = number of vertices, r = reflex vertices. + /// - Does not support holes. + /// + /// For more information about this algorithm, see http://mnbayazit.com/406/bayazit + /// + internal static class BayazitDecomposer + { + /// + /// Decompose the polygon into several smaller non-concave polygon. + /// If the polygon is already convex, it will return the original polygon, unless it is over Settings.MaxPolygonVertices. + /// + public static List ConvexPartition(Vertices vertices) + { + Debug.Assert(vertices.Count > 3); + Debug.Assert(vertices.IsCounterClockWise()); + + return TriangulatePolygon(vertices); + } + + private static List TriangulatePolygon(Vertices vertices) + { + List list = new List(); + Vector2 lowerInt = new Vector2(); + Vector2 upperInt = new Vector2(); // intersection points + int lowerIndex = 0, upperIndex = 0; + Vertices lowerPoly, upperPoly; + + for (int i = 0; i < vertices.Count; ++i) + { + if (Reflex(i, vertices)) + { + float upperDist; + float lowerDist = upperDist = float.MaxValue; + for (int j = 0; j < vertices.Count; ++j) + { + // if line intersects with an edge + float d; + Vector2 p; + if (Left(At(i - 1, vertices), At(i, vertices), At(j, vertices)) && RightOn(At(i - 1, vertices), At(i, vertices), At(j - 1, vertices))) + { + // find the point of intersection + p = LineTools.LineIntersect(At(i - 1, vertices), At(i, vertices), At(j, vertices), At(j - 1, vertices)); + + if (Right(At(i + 1, vertices), At(i, vertices), p)) + { + // make sure it's inside the poly + d = SquareDist(At(i, vertices), p); + if (d < lowerDist) + { + // keep only the closest intersection + lowerDist = d; + lowerInt = p; + lowerIndex = j; + } + } + } + + if (Left(At(i + 1, vertices), At(i, vertices), At(j + 1, vertices)) && RightOn(At(i + 1, vertices), At(i, vertices), At(j, vertices))) + { + p = LineTools.LineIntersect(At(i + 1, vertices), At(i, vertices), At(j, vertices), At(j + 1, vertices)); + + if (Left(At(i - 1, vertices), At(i, vertices), p)) + { + d = SquareDist(At(i, vertices), p); + if (d < upperDist) + { + upperDist = d; + upperIndex = j; + upperInt = p; + } + } + } + } + + // if there are no vertices to connect to, choose a point in the middle + if (lowerIndex == (upperIndex + 1) % vertices.Count) + { + Vector2 p = ((lowerInt + upperInt) / 2); + + lowerPoly = Copy(i, upperIndex, vertices); + lowerPoly.Add(p); + upperPoly = Copy(lowerIndex, i, vertices); + upperPoly.Add(p); + } + else + { + double highestScore = 0, bestIndex = lowerIndex; + while (upperIndex < lowerIndex) + upperIndex += vertices.Count; + + for (int j = lowerIndex; j <= upperIndex; ++j) + { + if (CanSee(i, j, vertices)) + { + double score = 1 / (SquareDist(At(i, vertices), At(j, vertices)) + 1); + if (Reflex(j, vertices)) + { + if (RightOn(At(j - 1, vertices), At(j, vertices), At(i, vertices)) && LeftOn(At(j + 1, vertices), At(j, vertices), At(i, vertices))) + score += 3; + else + score += 2; + } + else + { + score += 1; + } + if (score > highestScore) + { + bestIndex = j; + highestScore = score; + } + } + } + lowerPoly = Copy(i, (int)bestIndex, vertices); + upperPoly = Copy((int)bestIndex, i, vertices); + } + list.AddRange(TriangulatePolygon(lowerPoly)); + list.AddRange(TriangulatePolygon(upperPoly)); + return list; + } + } + + // polygon is already convex + if (vertices.Count > Settings.MaxPolygonVertices) + { + lowerPoly = Copy(0, vertices.Count / 2, vertices); + upperPoly = Copy(vertices.Count / 2, 0, vertices); + list.AddRange(TriangulatePolygon(lowerPoly)); + list.AddRange(TriangulatePolygon(upperPoly)); + } + else + list.Add(vertices); + + return list; + } + + private static Vector2 At(int i, Vertices vertices) + { + int s = vertices.Count; + return vertices[i < 0 ? s - 1 - ((-i - 1) % s) : i % s]; + } + + private static Vertices Copy(int i, int j, Vertices vertices) + { + while (j < i) + j += vertices.Count; + + Vertices p = new Vertices(j); + + for (; i <= j; ++i) + { + p.Add(At(i, vertices)); + } + return p; + } + + private static bool CanSee(int i, int j, Vertices vertices) + { + if (Reflex(i, vertices)) + { + if (LeftOn(At(i, vertices), At(i - 1, vertices), At(j, vertices)) && RightOn(At(i, vertices), At(i + 1, vertices), At(j, vertices))) + return false; + } + else + { + if (RightOn(At(i, vertices), At(i + 1, vertices), At(j, vertices)) || LeftOn(At(i, vertices), At(i - 1, vertices), At(j, vertices))) + return false; + } + if (Reflex(j, vertices)) + { + if (LeftOn(At(j, vertices), At(j - 1, vertices), At(i, vertices)) && RightOn(At(j, vertices), At(j + 1, vertices), At(i, vertices))) + return false; + } + else + { + if (RightOn(At(j, vertices), At(j + 1, vertices), At(i, vertices)) || LeftOn(At(j, vertices), At(j - 1, vertices), At(i, vertices))) + return false; + } + for (int k = 0; k < vertices.Count; ++k) + { + if ((k + 1) % vertices.Count == i || k == i || (k + 1) % vertices.Count == j || k == j) + continue; // ignore incident edges + + Vector2 intersectionPoint; + + if (LineTools.LineIntersect(At(i, vertices), At(j, vertices), At(k, vertices), At(k + 1, vertices), out intersectionPoint)) + return false; + } + return true; + } + + private static bool Reflex(int i, Vertices vertices) + { + return Right(i, vertices); + } + + private static bool Right(int i, Vertices vertices) + { + return Right(At(i - 1, vertices), At(i, vertices), At(i + 1, vertices)); + } + + private static bool Left(Vector2 a, Vector2 b, Vector2 c) + { + return MathUtils.Area(ref a, ref b, ref c) > 0; + } + + private static bool LeftOn(Vector2 a, Vector2 b, Vector2 c) + { + return MathUtils.Area(ref a, ref b, ref c) >= 0; + } + + private static bool Right(Vector2 a, Vector2 b, Vector2 c) + { + return MathUtils.Area(ref a, ref b, ref c) < 0; + } + + private static bool RightOn(Vector2 a, Vector2 b, Vector2 c) + { + return MathUtils.Area(ref a, ref b, ref c) <= 0; + } + + private static float SquareDist(Vector2 a, Vector2 b) + { + float dx = b.X - a.X; + float dy = b.Y - a.Y; + return dx * dx + dy * dy; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/CDT/Delaunay/DelaunayTriangle.cs b/src/Box2DNet/Common/Decomposition/CDT/Delaunay/DelaunayTriangle.cs new file mode 100644 index 0000000..8c6abe1 --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/CDT/Delaunay/DelaunayTriangle.cs @@ -0,0 +1,420 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +// Changes from the Java version +// attributification +// Future possibilities +// Flattening out the number of indirections +// Replacing arrays of 3 with fixed-length arrays? +// Replacing bool[3] with a bit array of some sort? +// Bundling everything into an AoS mess? +// Hardcode them all as ABC ? + +using System; +using System.Collections.Generic; +using System.Diagnostics; +using Box2DNet.Common.Decomposition.CDT.Delaunay.Sweep; +using Box2DNet.Common.Decomposition.CDT.Util; + +namespace Box2DNet.Common.Decomposition.CDT.Delaunay +{ + internal class DelaunayTriangle + { + /** Neighbor pointers */ + + /** Flags to determine if an edge is a Delauney edge */ + public FixedBitArray3 EdgeIsConstrained; + + /** Flags to determine if an edge is a Constrained edge */ + public FixedBitArray3 EdgeIsDelaunay; + public Util.FixedArray3 Neighbors; + + /** Has this triangle been marked as an interior triangle? */ + + public Util.FixedArray3 Points; + + public DelaunayTriangle(TriangulationPoint p1, TriangulationPoint p2, TriangulationPoint p3) + { + Points[0] = p1; + Points[1] = p2; + Points[2] = p3; + } + + public bool IsInterior { get; set; } + + public int IndexOf(TriangulationPoint p) + { + int i = Points.IndexOf(p); + if (i == -1) throw new Exception("Calling index with a point that doesn't exist in triangle"); + return i; + } + + //TODO: Port note - different implementation + public int IndexCW(TriangulationPoint p) + { + int index = IndexOf(p); + switch (index) + { + case 0: + return 2; + case 1: + return 0; + default: + return 1; + } + } + + //TODO: Port note - different implementation + public int IndexCCW(TriangulationPoint p) + { + int index = IndexOf(p); + switch (index) + { + case 0: + return 1; + case 1: + return 2; + default: + return 0; + } + } + + public bool Contains(TriangulationPoint p) + { + return (p == Points[0] || p == Points[1] || p == Points[2]); + } + + public bool Contains(DTSweepConstraint e) + { + return (Contains(e.P) && Contains(e.Q)); + } + + public bool Contains(TriangulationPoint p, TriangulationPoint q) + { + return (Contains(p) && Contains(q)); + } + + /// + /// Update neighbor pointers + /// + /// Point 1 of the shared edge + /// Point 2 of the shared edge + /// This triangle's new neighbor + private void MarkNeighbor(TriangulationPoint p1, TriangulationPoint p2, DelaunayTriangle t) + { + if ((p1 == Points[2] && p2 == Points[1]) || (p1 == Points[1] && p2 == Points[2])) + { + Neighbors[0] = t; + } + else if ((p1 == Points[0] && p2 == Points[2]) || (p1 == Points[2] && p2 == Points[0])) + { + Neighbors[1] = t; + } + else if ((p1 == Points[0] && p2 == Points[1]) || (p1 == Points[1] && p2 == Points[0])) + { + Neighbors[2] = t; + } + else + { + Debug.WriteLine("Neighbor error, please report!"); + // throw new Exception("Neighbor error, please report!"); + } + } + + /// + /// Exhaustive search to update neighbor pointers + /// + public void MarkNeighbor(DelaunayTriangle t) + { + if (t.Contains(Points[1], Points[2])) + { + Neighbors[0] = t; + t.MarkNeighbor(Points[1], Points[2], this); + } + else if (t.Contains(Points[0], Points[2])) + { + Neighbors[1] = t; + t.MarkNeighbor(Points[0], Points[2], this); + } + else if (t.Contains(Points[0], Points[1])) + { + Neighbors[2] = t; + t.MarkNeighbor(Points[0], Points[1], this); + } + else + { + Debug.WriteLine("markNeighbor failed"); + } + } + + public void ClearNeighbors() + { + Neighbors[0] = Neighbors[1] = Neighbors[2] = null; + } + + public void ClearNeighbor(DelaunayTriangle triangle) + { + if (Neighbors[0] == triangle) + { + Neighbors[0] = null; + } + else if (Neighbors[1] == triangle) + { + Neighbors[1] = null; + } + else + { + Neighbors[2] = null; + } + } + + /** + * Clears all references to all other triangles and points + */ + + public void Clear() + { + DelaunayTriangle t; + for (int i = 0; i < 3; i++) + { + t = Neighbors[i]; + if (t != null) + { + t.ClearNeighbor(this); + } + } + ClearNeighbors(); + Points[0] = Points[1] = Points[2] = null; + } + + /// Opposite triangle + /// The point in t that isn't shared between the triangles + public TriangulationPoint OppositePoint(DelaunayTriangle t, TriangulationPoint p) + { + Debug.Assert(t != this, "self-pointer error"); + return PointCW(t.PointCW(p)); + } + + public DelaunayTriangle NeighborCW(TriangulationPoint point) + { + return Neighbors[(Points.IndexOf(point) + 1)%3]; + } + + public DelaunayTriangle NeighborCCW(TriangulationPoint point) + { + return Neighbors[(Points.IndexOf(point) + 2)%3]; + } + + public DelaunayTriangle NeighborAcross(TriangulationPoint point) + { + return Neighbors[Points.IndexOf(point)]; + } + + public TriangulationPoint PointCCW(TriangulationPoint point) + { + return Points[(IndexOf(point) + 1)%3]; + } + + public TriangulationPoint PointCW(TriangulationPoint point) + { + return Points[(IndexOf(point) + 2)%3]; + } + + private void RotateCW() + { + var t = Points[2]; + Points[2] = Points[1]; + Points[1] = Points[0]; + Points[0] = t; + } + + /// + /// Legalize triangle by rotating clockwise around oPoint + /// + /// The origin point to rotate around + /// ??? + public void Legalize(TriangulationPoint oPoint, TriangulationPoint nPoint) + { + RotateCW(); + Points[IndexCCW(oPoint)] = nPoint; + } + + public override string ToString() + { + return Points[0] + "," + Points[1] + "," + Points[2]; + } + + /// + /// Finalize edge marking + /// + public void MarkNeighborEdges() + { + for (int i = 0; i < 3; i++) + if (EdgeIsConstrained[i] && Neighbors[i] != null) + { + Neighbors[i].MarkConstrainedEdge(Points[(i + 1)%3], Points[(i + 2)%3]); + } + } + + public void MarkEdge(DelaunayTriangle triangle) + { + for (int i = 0; i < 3; i++) + if (EdgeIsConstrained[i]) + { + triangle.MarkConstrainedEdge(Points[(i + 1)%3], Points[(i + 2)%3]); + } + } + + public void MarkEdge(List tList) + { + foreach (DelaunayTriangle t in tList) + for (int i = 0; i < 3; i++) + if (t.EdgeIsConstrained[i]) + { + MarkConstrainedEdge(t.Points[(i + 1)%3], t.Points[(i + 2)%3]); + } + } + + public void MarkConstrainedEdge(int index) + { + EdgeIsConstrained[index] = true; + } + + public void MarkConstrainedEdge(DTSweepConstraint edge) + { + MarkConstrainedEdge(edge.P, edge.Q); + } + + /// + /// Mark edge as constrained + /// + public void MarkConstrainedEdge(TriangulationPoint p, TriangulationPoint q) + { + int i = EdgeIndex(p, q); + if (i != -1) EdgeIsConstrained[i] = true; + } + + public double Area() + { + double b = Points[0].X - Points[1].X; + double h = Points[2].Y - Points[1].Y; + + return Math.Abs((b*h*0.5f)); + } + + public TriangulationPoint Centroid() + { + double cx = (Points[0].X + Points[1].X + Points[2].X)/3f; + double cy = (Points[0].Y + Points[1].Y + Points[2].Y)/3f; + return new TriangulationPoint(cx, cy); + } + + /// + /// Get the index of the neighbor that shares this edge (or -1 if it isn't shared) + /// + /// index of the shared edge or -1 if edge isn't shared + public int EdgeIndex(TriangulationPoint p1, TriangulationPoint p2) + { + int i1 = Points.IndexOf(p1); + int i2 = Points.IndexOf(p2); + + // Points of this triangle in the edge p1-p2 + bool a = (i1 == 0 || i2 == 0); + bool b = (i1 == 1 || i2 == 1); + bool c = (i1 == 2 || i2 == 2); + + if (b && c) return 0; + if (a && c) return 1; + if (a && b) return 2; + return -1; + } + + public bool GetConstrainedEdgeCCW(TriangulationPoint p) + { + return EdgeIsConstrained[(IndexOf(p) + 2)%3]; + } + + public bool GetConstrainedEdgeCW(TriangulationPoint p) + { + return EdgeIsConstrained[(IndexOf(p) + 1)%3]; + } + + public bool GetConstrainedEdgeAcross(TriangulationPoint p) + { + return EdgeIsConstrained[IndexOf(p)]; + } + + public void SetConstrainedEdgeCCW(TriangulationPoint p, bool ce) + { + EdgeIsConstrained[(IndexOf(p) + 2)%3] = ce; + } + + public void SetConstrainedEdgeCW(TriangulationPoint p, bool ce) + { + EdgeIsConstrained[(IndexOf(p) + 1)%3] = ce; + } + + public void SetConstrainedEdgeAcross(TriangulationPoint p, bool ce) + { + EdgeIsConstrained[IndexOf(p)] = ce; + } + + public bool GetDelaunayEdgeCCW(TriangulationPoint p) + { + return EdgeIsDelaunay[(IndexOf(p) + 2)%3]; + } + + public bool GetDelaunayEdgeCW(TriangulationPoint p) + { + return EdgeIsDelaunay[(IndexOf(p) + 1)%3]; + } + + public bool GetDelaunayEdgeAcross(TriangulationPoint p) + { + return EdgeIsDelaunay[IndexOf(p)]; + } + + public void SetDelaunayEdgeCCW(TriangulationPoint p, bool ce) + { + EdgeIsDelaunay[(IndexOf(p) + 2)%3] = ce; + } + + public void SetDelaunayEdgeCW(TriangulationPoint p, bool ce) + { + EdgeIsDelaunay[(IndexOf(p) + 1)%3] = ce; + } + + public void SetDelaunayEdgeAcross(TriangulationPoint p, bool ce) + { + EdgeIsDelaunay[IndexOf(p)] = ce; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFront.cs b/src/Box2DNet/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFront.cs new file mode 100644 index 0000000..2a0bd47 --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFront.cs @@ -0,0 +1,180 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +// Changes from the Java version +// Removed BST code, but not all artifacts of it +// Future possibilities +// Eliminate Add/RemoveNode ? +// Comments comments and more comments! + +using System; +using System.Text; + +namespace Box2DNet.Common.Decomposition.CDT.Delaunay.Sweep +{ + /** + * @author Thomas Åhlen (thahlen@gmail.com) + */ + + internal class AdvancingFront + { + public AdvancingFrontNode Head; + protected AdvancingFrontNode Search; + public AdvancingFrontNode Tail; + + public AdvancingFront(AdvancingFrontNode head, AdvancingFrontNode tail) + { + Head = head; + Tail = tail; + Search = head; + AddNode(head); + AddNode(tail); + } + + public void AddNode(AdvancingFrontNode node) + { + //_searchTree.put(node.key, node); + } + + public void RemoveNode(AdvancingFrontNode node) + { + //_searchTree.delete( node.key ); + } + + public override string ToString() + { + StringBuilder sb = new StringBuilder(); + AdvancingFrontNode node = Head; + while (node != Tail) + { + sb.Append(node.Point.X).Append("->"); + node = node.Next; + } + sb.Append(Tail.Point.X); + return sb.ToString(); + } + + /// + /// MM: This seems to be used by LocateNode to guess a position in the implicit linked list of AdvancingFrontNodes near x + /// Removed an overload that depended on this being exact + /// + private AdvancingFrontNode FindSearchNode(double x) + { + // TODO: implement BST index + return Search; + } + + /// + /// We use a balancing tree to locate a node smaller or equal to given key value + /// + public AdvancingFrontNode LocateNode(TriangulationPoint point) + { + return LocateNode(point.X); + } + + private AdvancingFrontNode LocateNode(double x) + { + AdvancingFrontNode node = FindSearchNode(x); + if (x < node.Value) + { + while ((node = node.Prev) != null) + if (x >= node.Value) + { + Search = node; + return node; + } + } + else + { + while ((node = node.Next) != null) + if (x < node.Value) + { + Search = node.Prev; + return node.Prev; + } + } + return null; + } + + /// + /// This implementation will use simple node traversal algorithm to find a point on the front + /// + public AdvancingFrontNode LocatePoint(TriangulationPoint point) + { + double px = point.X; + AdvancingFrontNode node = FindSearchNode(px); + double nx = node.Point.X; + + if (px == nx) + { + if (point != node.Point) + { + // We might have two nodes with same x value for a short time + if (point == node.Prev.Point) + { + node = node.Prev; + } + else if (point == node.Next.Point) + { + node = node.Next; + } + else + { + throw new Exception("Failed to find Node for given afront point"); + //node = null; + } + } + } + else if (px < nx) + { + while ((node = node.Prev) != null) + { + if (point == node.Point) + { + break; + } + } + } + else + { + while ((node = node.Next) != null) + { + if (point == node.Point) + { + break; + } + } + } + Search = node; + return node; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFrontNode.cs b/src/Box2DNet/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFrontNode.cs new file mode 100644 index 0000000..43bb4bc --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/CDT/Delaunay/Sweep/AdvancingFrontNode.cs @@ -0,0 +1,64 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +// Changes from the Java version +// Removed getters +// Has* turned into attributes +// Future possibilities +// Comments! + +namespace Box2DNet.Common.Decomposition.CDT.Delaunay.Sweep +{ + internal class AdvancingFrontNode + { + public AdvancingFrontNode Next; + public TriangulationPoint Point; + public AdvancingFrontNode Prev; + public DelaunayTriangle Triangle; + public double Value; + + public AdvancingFrontNode(TriangulationPoint point) + { + Point = point; + Value = point.X; + } + + public bool HasNext + { + get { return Next != null; } + } + + public bool HasPrev + { + get { return Prev != null; } + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/CDT/Delaunay/Sweep/DTSweep.cs b/src/Box2DNet/Common/Decomposition/CDT/Delaunay/Sweep/DTSweep.cs new file mode 100644 index 0000000..ea193ea --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/CDT/Delaunay/Sweep/DTSweep.cs @@ -0,0 +1,1151 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Sweep-line, Constrained Delauney Triangulation (CDT) See: Domiter, V. and + * Zalik, B.(2008)'Sweep-line algorithm for constrained Delaunay triangulation', + * International Journal of Geographical Information Science + * + * "FlipScan" Constrained Edge Algorithm invented by author of this code. + * + * Author: Thomas Åhlén, thahlen@gmail.com + */ + +// Changes from the Java version +// Turned DTSweep into a static class +// Lots of deindentation via early bailout +// Future possibilities +// Comments! + +using System; +using System.Collections.Generic; +using System.Diagnostics; + +namespace Box2DNet.Common.Decomposition.CDT.Delaunay.Sweep +{ + internal static class DTSweep + { + private const double PI_div2 = Math.PI / 2; + private const double PI_3div4 = 3 * Math.PI / 4; + + /// + /// Triangulate simple polygon with holes + /// + public static void Triangulate(DTSweepContext tcx) + { + tcx.CreateAdvancingFront(); + + Sweep(tcx); + + // Finalize triangulation + if (tcx.TriangulationMode == TriangulationMode.Polygon) + { + FinalizationPolygon(tcx); + } + else + { + FinalizationConvexHull(tcx); + } + + tcx.Done(); + } + + /// + /// Start sweeping the Y-sorted point set from bottom to top + /// + private static void Sweep(DTSweepContext tcx) + { + List points = tcx.Points; + + for (int i = 1; i < points.Count; i++) + { + TriangulationPoint point = points[i]; + + AdvancingFrontNode node = PointEvent(tcx, point); + + if (point.HasEdges) + { + foreach (DTSweepConstraint e in point.Edges) + { + EdgeEvent(tcx, e, node); + } + } + tcx.Update(null); + } + } + + /// + /// If this is a Delaunay Triangulation of a pointset we need to fill so the triangle mesh gets a ConvexHull + /// + private static void FinalizationConvexHull(DTSweepContext tcx) + { + DelaunayTriangle t1, t2; + + AdvancingFrontNode n1 = tcx.aFront.Head.Next; + AdvancingFrontNode n2 = n1.Next; + + TurnAdvancingFrontConvex(tcx, n1, n2); + + // TODO: implement ConvexHull for lower right and left boundary + + // Lets remove triangles connected to the two "algorithm" points + + // XXX: When the first the nodes are points in a triangle we need to do a flip before + // removing triangles or we will lose a valid triangle. + // Same for last three nodes! + // !!! If I implement ConvexHull for lower right and left boundary this fix should not be + // needed and the removed triangles will be added again by default + n1 = tcx.aFront.Tail.Prev; + if (n1.Triangle.Contains(n1.Next.Point) && n1.Triangle.Contains(n1.Prev.Point)) + { + t1 = n1.Triangle.NeighborAcross(n1.Point); + RotateTrianglePair(n1.Triangle, n1.Point, t1, t1.OppositePoint(n1.Triangle, n1.Point)); + tcx.MapTriangleToNodes(n1.Triangle); + tcx.MapTriangleToNodes(t1); + } + n1 = tcx.aFront.Head.Next; + if (n1.Triangle.Contains(n1.Prev.Point) && n1.Triangle.Contains(n1.Next.Point)) + { + t1 = n1.Triangle.NeighborAcross(n1.Point); + RotateTrianglePair(n1.Triangle, n1.Point, t1, t1.OppositePoint(n1.Triangle, n1.Point)); + tcx.MapTriangleToNodes(n1.Triangle); + tcx.MapTriangleToNodes(t1); + } + + // Lower right boundary + TriangulationPoint first = tcx.aFront.Head.Point; + n2 = tcx.aFront.Tail.Prev; + t1 = n2.Triangle; + TriangulationPoint p1 = n2.Point; + n2.Triangle = null; + do + { + tcx.RemoveFromList(t1); + p1 = t1.PointCCW(p1); + if (p1 == first) break; + t2 = t1.NeighborCCW(p1); + t1.Clear(); + t1 = t2; + } while (true); + + // Lower left boundary + first = tcx.aFront.Head.Next.Point; + p1 = t1.PointCW(tcx.aFront.Head.Point); + t2 = t1.NeighborCW(tcx.aFront.Head.Point); + t1.Clear(); + t1 = t2; + while (p1 != first) //TODO: Port note. This was do while before. + { + tcx.RemoveFromList(t1); + p1 = t1.PointCCW(p1); + t2 = t1.NeighborCCW(p1); + t1.Clear(); + t1 = t2; + } + + // Remove current head and tail node now that we have removed all triangles attached + // to them. Then set new head and tail node points + tcx.aFront.Head = tcx.aFront.Head.Next; + tcx.aFront.Head.Prev = null; + tcx.aFront.Tail = tcx.aFront.Tail.Prev; + tcx.aFront.Tail.Next = null; + + tcx.FinalizeTriangulation(); + } + + /// + /// We will traverse the entire advancing front and fill it to form a convex hull. + /// + private static void TurnAdvancingFrontConvex(DTSweepContext tcx, AdvancingFrontNode b, AdvancingFrontNode c) + { + AdvancingFrontNode first = b; + while (c != tcx.aFront.Tail) + { + if (TriangulationUtil.Orient2d(b.Point, c.Point, c.Next.Point) == Orientation.CCW) + { + // [b,c,d] Concave - fill around c + Fill(tcx, c); + c = c.Next; + } + else + { + // [b,c,d] Convex + if (b != first && TriangulationUtil.Orient2d(b.Prev.Point, b.Point, c.Point) == Orientation.CCW) + { + // [a,b,c] Concave - fill around b + Fill(tcx, b); + b = b.Prev; + } + else + { + // [a,b,c] Convex - nothing to fill + b = c; + c = c.Next; + } + } + } + } + + private static void FinalizationPolygon(DTSweepContext tcx) + { + // Get an Internal triangle to start with + DelaunayTriangle t = tcx.aFront.Head.Next.Triangle; + TriangulationPoint p = tcx.aFront.Head.Next.Point; + while (!t.GetConstrainedEdgeCW(p)) + { + t = t.NeighborCCW(p); + } + + // Collect interior triangles constrained by edges + tcx.MeshClean(t); + } + + /// + /// Find closes node to the left of the new point and + /// create a new triangle. If needed new holes and basins + /// will be filled to. + /// + private static AdvancingFrontNode PointEvent(DTSweepContext tcx, TriangulationPoint point) + { + AdvancingFrontNode node = tcx.LocateNode(point); + AdvancingFrontNode newNode = NewFrontTriangle(tcx, point, node); + + // Only need to check +epsilon since point never have smaller + // x value than node due to how we fetch nodes from the front + if (point.X <= node.Point.X + TriangulationUtil.EPSILON) + { + Fill(tcx, node); + } + + tcx.AddNode(newNode); + + FillAdvancingFront(tcx, newNode); + return newNode; + } + + /// + /// Creates a new front triangle and legalize it + /// + private static AdvancingFrontNode NewFrontTriangle(DTSweepContext tcx, TriangulationPoint point, AdvancingFrontNode node) + { + DelaunayTriangle triangle = new DelaunayTriangle(point, node.Point, node.Next.Point); + triangle.MarkNeighbor(node.Triangle); + tcx.Triangles.Add(triangle); + + AdvancingFrontNode newNode = new AdvancingFrontNode(point); + newNode.Next = node.Next; + newNode.Prev = node; + node.Next.Prev = newNode; + node.Next = newNode; + + tcx.AddNode(newNode); // XXX: BST + + if (!Legalize(tcx, triangle)) + { + tcx.MapTriangleToNodes(triangle); + } + + return newNode; + } + + private static void EdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node) + { + try + { + tcx.EdgeEvent.ConstrainedEdge = edge; + tcx.EdgeEvent.Right = edge.P.X > edge.Q.X; + + if (IsEdgeSideOfTriangle(node.Triangle, edge.P, edge.Q)) + { + return; + } + + // For now we will do all needed filling + // TODO: integrate with flip process might give some better performance + // but for now this avoid the issue with cases that needs both flips and fills + FillEdgeEvent(tcx, edge, node); + + EdgeEvent(tcx, edge.P, edge.Q, node.Triangle, edge.Q); + } + catch (PointOnEdgeException e) + { + Debug.WriteLine("Skipping Edge: {0}", e.Message); + } + } + + private static void FillEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node) + { + if (tcx.EdgeEvent.Right) + { + FillRightAboveEdgeEvent(tcx, edge, node); + } + else + { + FillLeftAboveEdgeEvent(tcx, edge, node); + } + } + + private static void FillRightConcaveEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, + AdvancingFrontNode node) + { + Fill(tcx, node.Next); + if (node.Next.Point != edge.P) + { + // Next above or below edge? + if (TriangulationUtil.Orient2d(edge.Q, node.Next.Point, edge.P) == Orientation.CCW) + { + // Below + if (TriangulationUtil.Orient2d(node.Point, node.Next.Point, node.Next.Next.Point) == Orientation.CCW) + { + // Next is concave + FillRightConcaveEdgeEvent(tcx, edge, node); + } + else + { + // Next is convex + } + } + } + } + + private static void FillRightConvexEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node) + { + // Next concave or convex? + if (TriangulationUtil.Orient2d(node.Next.Point, node.Next.Next.Point, node.Next.Next.Next.Point) == + Orientation.CCW) + { + // Concave + FillRightConcaveEdgeEvent(tcx, edge, node.Next); + } + else + { + // Convex + // Next above or below edge? + if (TriangulationUtil.Orient2d(edge.Q, node.Next.Next.Point, edge.P) == Orientation.CCW) + { + // Below + FillRightConvexEdgeEvent(tcx, edge, node.Next); + } + else + { + // Above + } + } + } + + private static void FillRightBelowEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node) + { + if (node.Point.X < edge.P.X) // needed? + { + if (TriangulationUtil.Orient2d(node.Point, node.Next.Point, node.Next.Next.Point) == Orientation.CCW) + { + // Concave + FillRightConcaveEdgeEvent(tcx, edge, node); + } + else + { + // Convex + FillRightConvexEdgeEvent(tcx, edge, node); + // Retry this one + FillRightBelowEdgeEvent(tcx, edge, node); + } + } + } + + private static void FillRightAboveEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node) + { + while (node.Next.Point.X < edge.P.X) + { + // Check if next node is below the edge + Orientation o1 = TriangulationUtil.Orient2d(edge.Q, node.Next.Point, edge.P); + if (o1 == Orientation.CCW) + { + FillRightBelowEdgeEvent(tcx, edge, node); + } + else + { + node = node.Next; + } + } + } + + private static void FillLeftConvexEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node) + { + // Next concave or convex? + if (TriangulationUtil.Orient2d(node.Prev.Point, node.Prev.Prev.Point, node.Prev.Prev.Prev.Point) == + Orientation.CW) + { + // Concave + FillLeftConcaveEdgeEvent(tcx, edge, node.Prev); + } + else + { + // Convex + // Next above or below edge? + if (TriangulationUtil.Orient2d(edge.Q, node.Prev.Prev.Point, edge.P) == Orientation.CW) + { + // Below + FillLeftConvexEdgeEvent(tcx, edge, node.Prev); + } + else + { + // Above + } + } + } + + private static void FillLeftConcaveEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node) + { + Fill(tcx, node.Prev); + if (node.Prev.Point != edge.P) + { + // Next above or below edge? + if (TriangulationUtil.Orient2d(edge.Q, node.Prev.Point, edge.P) == Orientation.CW) + { + // Below + if (TriangulationUtil.Orient2d(node.Point, node.Prev.Point, node.Prev.Prev.Point) == Orientation.CW) + { + // Next is concave + FillLeftConcaveEdgeEvent(tcx, edge, node); + } + else + { + // Next is convex + } + } + } + } + + private static void FillLeftBelowEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node) + { + if (node.Point.X > edge.P.X) + { + if (TriangulationUtil.Orient2d(node.Point, node.Prev.Point, node.Prev.Prev.Point) == Orientation.CW) + { + // Concave + FillLeftConcaveEdgeEvent(tcx, edge, node); + } + else + { + // Convex + FillLeftConvexEdgeEvent(tcx, edge, node); + // Retry this one + FillLeftBelowEdgeEvent(tcx, edge, node); + } + } + } + + private static void FillLeftAboveEdgeEvent(DTSweepContext tcx, DTSweepConstraint edge, AdvancingFrontNode node) + { + while (node.Prev.Point.X > edge.P.X) + { + // Check if next node is below the edge + Orientation o1 = TriangulationUtil.Orient2d(edge.Q, node.Prev.Point, edge.P); + if (o1 == Orientation.CW) + { + FillLeftBelowEdgeEvent(tcx, edge, node); + } + else + { + node = node.Prev; + } + } + } + + private static bool IsEdgeSideOfTriangle(DelaunayTriangle triangle, TriangulationPoint ep, TriangulationPoint eq) + { + int index = triangle.EdgeIndex(ep, eq); + if (index != -1) + { + triangle.MarkConstrainedEdge(index); + triangle = triangle.Neighbors[index]; + if (triangle != null) + { + triangle.MarkConstrainedEdge(ep, eq); + } + return true; + } + return false; + } + + private static void EdgeEvent(DTSweepContext tcx, TriangulationPoint ep, TriangulationPoint eq, DelaunayTriangle triangle, TriangulationPoint point) + { + if (IsEdgeSideOfTriangle(triangle, ep, eq)) + return; + + TriangulationPoint p1 = triangle.PointCCW(point); + Orientation o1 = TriangulationUtil.Orient2d(eq, p1, ep); + if (o1 == Orientation.Collinear) + { + if (triangle.Contains(eq, p1)) + { + triangle.MarkConstrainedEdge(eq, p1); + // We are modifying the constraint maybe it would be better to + // not change the given constraint and just keep a variable for the new constraint + tcx.EdgeEvent.ConstrainedEdge.Q = p1; + triangle = triangle.NeighborAcross(point); + EdgeEvent(tcx, ep, p1, triangle, p1); + } + else + { + throw new PointOnEdgeException("EdgeEvent - Point on constrained edge not supported yet"); + } + if (tcx.IsDebugEnabled) + { + Debug.WriteLine("EdgeEvent - Point on constrained edge"); + } + return; + } + + TriangulationPoint p2 = triangle.PointCW(point); + Orientation o2 = TriangulationUtil.Orient2d(eq, p2, ep); + if (o2 == Orientation.Collinear) + { + if (triangle.Contains(eq, p2)) + { + triangle.MarkConstrainedEdge(eq, p2); + // We are modifying the constraint maybe it would be better to + // not change the given constraint and just keep a variable for the new constraint + tcx.EdgeEvent.ConstrainedEdge.Q = p2; + triangle = triangle.NeighborAcross(point); + EdgeEvent(tcx, ep, p2, triangle, p2); + } + else + { + throw new PointOnEdgeException("EdgeEvent - Point on constrained edge not supported yet"); + } + if (tcx.IsDebugEnabled) + { + Debug.WriteLine("EdgeEvent - Point on constrained edge"); + } + return; + } + + if (o1 == o2) + { + // Need to decide if we are rotating CW or CCW to get to a triangle + // that will cross edge + if (o1 == Orientation.CW) + { + triangle = triangle.NeighborCCW(point); + } + else + { + triangle = triangle.NeighborCW(point); + } + EdgeEvent(tcx, ep, eq, triangle, point); + } + else + { + // This triangle crosses constraint so lets flippin start! + FlipEdgeEvent(tcx, ep, eq, triangle, point); + } + } + + private static void FlipEdgeEvent(DTSweepContext tcx, TriangulationPoint ep, TriangulationPoint eq, DelaunayTriangle t, TriangulationPoint p) + { + DelaunayTriangle ot = t.NeighborAcross(p); + TriangulationPoint op = ot.OppositePoint(t, p); + + if (ot == null) + { + // If we want to integrate the fillEdgeEvent do it here + // With current implementation we should never get here + throw new InvalidOperationException("[BUG:FIXME] FLIP failed due to missing triangle"); + } + + if (t.GetConstrainedEdgeAcross(p)) + { + throw new Exception("Intersecting Constraints"); + } + + bool inScanArea = TriangulationUtil.InScanArea(p, t.PointCCW(p), t.PointCW(p), op); + if (inScanArea) + { + // Lets rotate shared edge one vertex CW + RotateTrianglePair(t, p, ot, op); + tcx.MapTriangleToNodes(t); + tcx.MapTriangleToNodes(ot); + + if (p == eq && op == ep) + { + if (eq == tcx.EdgeEvent.ConstrainedEdge.Q + && ep == tcx.EdgeEvent.ConstrainedEdge.P) + { + if (tcx.IsDebugEnabled) Console.WriteLine("[FLIP] - constrained edge done"); // TODO: remove + t.MarkConstrainedEdge(ep, eq); + ot.MarkConstrainedEdge(ep, eq); + Legalize(tcx, t); + Legalize(tcx, ot); + } + else + { + if (tcx.IsDebugEnabled) Console.WriteLine("[FLIP] - subedge done"); // TODO: remove + // XXX: I think one of the triangles should be legalized here? + } + } + else + { + if (tcx.IsDebugEnabled) + Console.WriteLine("[FLIP] - flipping and continuing with triangle still crossing edge"); + // TODO: remove + Orientation o = TriangulationUtil.Orient2d(eq, op, ep); + t = NextFlipTriangle(tcx, o, t, ot, p, op); + FlipEdgeEvent(tcx, ep, eq, t, p); + } + } + else + { + TriangulationPoint newP = NextFlipPoint(ep, eq, ot, op); + FlipScanEdgeEvent(tcx, ep, eq, t, ot, newP); + EdgeEvent(tcx, ep, eq, t, p); + } + } + + /// + /// When we need to traverse from one triangle to the next we need + /// the point in current triangle that is the opposite point to the next + /// triangle. + /// + private static TriangulationPoint NextFlipPoint(TriangulationPoint ep, TriangulationPoint eq, DelaunayTriangle ot, TriangulationPoint op) + { + Orientation o2d = TriangulationUtil.Orient2d(eq, op, ep); + if (o2d == Orientation.CW) + { + // Right + return ot.PointCCW(op); + } + else if (o2d == Orientation.CCW) + { + // Left + return ot.PointCW(op); + } + else + { + // TODO: implement support for point on constraint edge + throw new PointOnEdgeException("Point on constrained edge not supported yet"); + } + } + + /// + /// After a flip we have two triangles and know that only one will still be + /// intersecting the edge. So decide which to contiune with and legalize the other + /// + /// + /// should be the result of an TriangulationUtil.orient2d( eq, op, ep ) + /// triangle 1 + /// triangle 2 + /// a point shared by both triangles + /// another point shared by both triangles + /// returns the triangle still intersecting the edge + private static DelaunayTriangle NextFlipTriangle(DTSweepContext tcx, Orientation o, DelaunayTriangle t, DelaunayTriangle ot, TriangulationPoint p, TriangulationPoint op) + { + int edgeIndex; + if (o == Orientation.CCW) + { + // ot is not crossing edge after flip + edgeIndex = ot.EdgeIndex(p, op); + ot.EdgeIsDelaunay[edgeIndex] = true; + Legalize(tcx, ot); + ot.EdgeIsDelaunay.Clear(); + return t; + } + // t is not crossing edge after flip + edgeIndex = t.EdgeIndex(p, op); + t.EdgeIsDelaunay[edgeIndex] = true; + Legalize(tcx, t); + t.EdgeIsDelaunay.Clear(); + return ot; + } + + /// + /// Scan part of the FlipScan algorithm
+ /// When a triangle pair isn't flippable we will scan for the next + /// point that is inside the flip triangle scan area. When found + /// we generate a new flipEdgeEvent + /// + /// + /// last point on the edge we are traversing + /// first point on the edge we are traversing + /// the current triangle sharing the point eq with edge + /// + /// + private static void FlipScanEdgeEvent(DTSweepContext tcx, TriangulationPoint ep, TriangulationPoint eq, DelaunayTriangle flipTriangle, DelaunayTriangle t, TriangulationPoint p) + { + DelaunayTriangle ot = t.NeighborAcross(p); + TriangulationPoint op = ot.OppositePoint(t, p); + + if (ot == null) + { + // If we want to integrate the fillEdgeEvent do it here + // With current implementation we should never get here + throw new Exception("[BUG:FIXME] FLIP failed due to missing triangle"); + } + + bool inScanArea = TriangulationUtil.InScanArea(eq, flipTriangle.PointCCW(eq), flipTriangle.PointCW(eq), op); + if (inScanArea) + { + // flip with new edge op->eq + FlipEdgeEvent(tcx, eq, op, ot, op); + // TODO: Actually I just figured out that it should be possible to + // improve this by getting the next ot and op before the the above + // flip and continue the flipScanEdgeEvent here + // set new ot and op here and loop back to inScanArea test + // also need to set a new flipTriangle first + // Turns out at first glance that this is somewhat complicated + // so it will have to wait. + } + else + { + TriangulationPoint newP = NextFlipPoint(ep, eq, ot, op); + FlipScanEdgeEvent(tcx, ep, eq, flipTriangle, ot, newP); + } + } + + /// + /// Fills holes in the Advancing Front + /// + private static void FillAdvancingFront(DTSweepContext tcx, AdvancingFrontNode n) + { + double angle; + + // Fill right holes + AdvancingFrontNode node = n.Next; + while (node.HasNext) + { + // if HoleAngle exceeds 90 degrees then break. + if (LargeHole_DontFill(node)) + break; + + Fill(tcx, node); + node = node.Next; + } + + // Fill left holes + node = n.Prev; + while (node.HasPrev) + { + // if HoleAngle exceeds 90 degrees then break. + if (LargeHole_DontFill(node)) + break; + + angle = HoleAngle(node); + if (angle > PI_div2 || angle < -PI_div2) + { + break; + } + Fill(tcx, node); + node = node.Prev; + } + + // Fill right basins + if (n.HasNext && n.Next.HasNext) + { + angle = BasinAngle(n); + if (angle < PI_3div4) + { + FillBasin(tcx, n); + } + } + } + + // True if HoleAngle exceeds 90 degrees. + private static bool LargeHole_DontFill(AdvancingFrontNode node) + { + AdvancingFrontNode nextNode = node.Next; + AdvancingFrontNode prevNode = node.Prev; + if (!AngleExceeds90Degrees(node.Point, nextNode.Point, prevNode.Point)) + return false; + + // Check additional points on front. + AdvancingFrontNode next2Node = nextNode.Next; + // "..Plus.." because only want angles on same side as point being added. + if ((next2Node != null) && !AngleExceedsPlus90DegreesOrIsNegative(node.Point, next2Node.Point, prevNode.Point)) + return false; + + AdvancingFrontNode prev2Node = prevNode.Prev; + // "..Plus.." because only want angles on same side as point being added. + if ((prev2Node != null) && !AngleExceedsPlus90DegreesOrIsNegative(node.Point, nextNode.Point, prev2Node.Point)) + return false; + + return true; + } + + private static bool AngleExceeds90Degrees(TriangulationPoint origin, TriangulationPoint pa, TriangulationPoint pb) + { + double angle = Angle(origin, pa, pb); + bool exceeds90Degrees = ((angle > PI_div2) || (angle < -PI_div2)); + return exceeds90Degrees; + } + + private static bool AngleExceedsPlus90DegreesOrIsNegative(TriangulationPoint origin, TriangulationPoint pa, TriangulationPoint pb) + { + double angle = Angle(origin, pa, pb); + bool exceedsPlus90DegreesOrIsNegative = (angle > PI_div2) || (angle < 0); + return exceedsPlus90DegreesOrIsNegative; + } + + private static double Angle(TriangulationPoint origin, TriangulationPoint pa, TriangulationPoint pb) + { + /* Complex plane + * ab = cosA +i*sinA + * ab = (ax + ay*i)(bx + by*i) = (ax*bx + ay*by) + i(ax*by-ay*bx) + * atan2(y,x) computes the principal value of the argument function + * applied to the complex number x+iy + * Where x = ax*bx + ay*by + * y = ax*by - ay*bx + */ + double px = origin.X; + double py = origin.Y; + double ax = pa.X - px; + double ay = pa.Y - py; + double bx = pb.X - px; + double by = pb.Y - py; + double x = ax * by - ay * bx; + double y = ax * bx + ay * by; + double angle = Math.Atan2(x, y); + return angle; + } + + /// + /// Fills a basin that has formed on the Advancing Front to the right + /// of given node. + /// First we decide a left,bottom and right node that forms the + /// boundaries of the basin. Then we do a reqursive fill. + /// + /// + /// starting node, this or next node will be left node + private static void FillBasin(DTSweepContext tcx, AdvancingFrontNode node) + { + if (TriangulationUtil.Orient2d(node.Point, node.Next.Point, node.Next.Next.Point) == Orientation.CCW) + { + // tcx.basin.leftNode = node.next.next; + tcx.Basin.leftNode = node; + } + else + { + tcx.Basin.leftNode = node.Next; + } + + // Find the bottom and right node + tcx.Basin.bottomNode = tcx.Basin.leftNode; + while (tcx.Basin.bottomNode.HasNext && tcx.Basin.bottomNode.Point.Y >= tcx.Basin.bottomNode.Next.Point.Y) + { + tcx.Basin.bottomNode = tcx.Basin.bottomNode.Next; + } + + if (tcx.Basin.bottomNode == tcx.Basin.leftNode) + { + // No valid basins + return; + } + + tcx.Basin.rightNode = tcx.Basin.bottomNode; + while (tcx.Basin.rightNode.HasNext && tcx.Basin.rightNode.Point.Y < tcx.Basin.rightNode.Next.Point.Y) + { + tcx.Basin.rightNode = tcx.Basin.rightNode.Next; + } + + if (tcx.Basin.rightNode == tcx.Basin.bottomNode) + { + // No valid basins + return; + } + + tcx.Basin.width = tcx.Basin.rightNode.Point.X - tcx.Basin.leftNode.Point.X; + tcx.Basin.leftHighest = tcx.Basin.leftNode.Point.Y > tcx.Basin.rightNode.Point.Y; + + FillBasinReq(tcx, tcx.Basin.bottomNode); + } + + /// + /// Recursive algorithm to fill a Basin with triangles + /// + private static void FillBasinReq(DTSweepContext tcx, AdvancingFrontNode node) + { + // if shallow stop filling + if (IsShallow(tcx, node)) + { + return; + } + + Fill(tcx, node); + if (node.Prev == tcx.Basin.leftNode && node.Next == tcx.Basin.rightNode) + { + return; + } + else if (node.Prev == tcx.Basin.leftNode) + { + Orientation o = TriangulationUtil.Orient2d(node.Point, node.Next.Point, node.Next.Next.Point); + if (o == Orientation.CW) + { + return; + } + node = node.Next; + } + else if (node.Next == tcx.Basin.rightNode) + { + Orientation o = TriangulationUtil.Orient2d(node.Point, node.Prev.Point, node.Prev.Prev.Point); + if (o == Orientation.CCW) + { + return; + } + node = node.Prev; + } + else + { + // Continue with the neighbor node with lowest Y value + if (node.Prev.Point.Y < node.Next.Point.Y) + { + node = node.Prev; + } + else + { + node = node.Next; + } + } + FillBasinReq(tcx, node); + } + + private static bool IsShallow(DTSweepContext tcx, AdvancingFrontNode node) + { + double height; + + if (tcx.Basin.leftHighest) + { + height = tcx.Basin.leftNode.Point.Y - node.Point.Y; + } + else + { + height = tcx.Basin.rightNode.Point.Y - node.Point.Y; + } + if (tcx.Basin.width > height) + { + return true; + } + return false; + } + + /// + /// ??? + /// + /// middle node + /// the angle between 3 front nodes + private static double HoleAngle(AdvancingFrontNode node) + { + // XXX: do we really need a signed angle for holeAngle? + // could possible save some cycles here + /* Complex plane + * ab = cosA +i*sinA + * ab = (ax + ay*i)(bx + by*i) = (ax*bx + ay*by) + i(ax*by-ay*bx) + * atan2(y,x) computes the principal value of the argument function + * applied to the complex number x+iy + * Where x = ax*bx + ay*by + * y = ax*by - ay*bx + */ + double px = node.Point.X; + double py = node.Point.Y; + double ax = node.Next.Point.X - px; + double ay = node.Next.Point.Y - py; + double bx = node.Prev.Point.X - px; + double by = node.Prev.Point.Y - py; + return Math.Atan2(ax * by - ay * bx, ax * bx + ay * by); + } + + /// + /// The basin angle is decided against the horizontal line [1,0] + /// + private static double BasinAngle(AdvancingFrontNode node) + { + double ax = node.Point.X - node.Next.Next.Point.X; + double ay = node.Point.Y - node.Next.Next.Point.Y; + return Math.Atan2(ay, ax); + } + + /// + /// Adds a triangle to the advancing front to fill a hole. + /// + /// + /// middle node, that is the bottom of the hole + private static void Fill(DTSweepContext tcx, AdvancingFrontNode node) + { + DelaunayTriangle triangle = new DelaunayTriangle(node.Prev.Point, node.Point, node.Next.Point); + // TODO: should copy the cEdge value from neighbor triangles + // for now cEdge values are copied during the legalize + triangle.MarkNeighbor(node.Prev.Triangle); + triangle.MarkNeighbor(node.Triangle); + tcx.Triangles.Add(triangle); + + // Update the advancing front + node.Prev.Next = node.Next; + node.Next.Prev = node.Prev; + tcx.RemoveNode(node); + + // If it was legalized the triangle has already been mapped + if (!Legalize(tcx, triangle)) + { + tcx.MapTriangleToNodes(triangle); + } + } + + /// + /// Returns true if triangle was legalized + /// + private static bool Legalize(DTSweepContext tcx, DelaunayTriangle t) + { + // To legalize a triangle we start by finding if any of the three edges + // violate the Delaunay condition + for (int i = 0; i < 3; i++) + { + // TODO: fix so that cEdge is always valid when creating new triangles then we can check it here + // instead of below with ot + if (t.EdgeIsDelaunay[i]) + { + continue; + } + + DelaunayTriangle ot = t.Neighbors[i]; + if (ot != null) + { + TriangulationPoint p = t.Points[i]; + TriangulationPoint op = ot.OppositePoint(t, p); + int oi = ot.IndexOf(op); + // If this is a Constrained Edge or a Delaunay Edge(only during recursive legalization) + // then we should not try to legalize + if (ot.EdgeIsConstrained[oi] || ot.EdgeIsDelaunay[oi]) + { + t.EdgeIsConstrained[i] = ot.EdgeIsConstrained[oi]; + // XXX: have no good way of setting this property when creating new triangles so lets set it here + continue; + } + + bool inside = TriangulationUtil.SmartIncircle(p, t.PointCCW(p), t.PointCW(p), op); + + if (inside) + { + // Lets mark this shared edge as Delaunay + t.EdgeIsDelaunay[i] = true; + ot.EdgeIsDelaunay[oi] = true; + + // Lets rotate shared edge one vertex CW to legalize it + RotateTrianglePair(t, p, ot, op); + + // We now got one valid Delaunay Edge shared by two triangles + // This gives us 4 new edges to check for Delaunay + + // Make sure that triangle to node mapping is done only one time for a specific triangle + bool notLegalized = !Legalize(tcx, t); + + if (notLegalized) + { + tcx.MapTriangleToNodes(t); + } + notLegalized = !Legalize(tcx, ot); + if (notLegalized) + { + tcx.MapTriangleToNodes(ot); + } + + // Reset the Delaunay edges, since they only are valid Delaunay edges + // until we add a new triangle or point. + // XXX: need to think about this. Can these edges be tried after we + // return to previous recursive level? + t.EdgeIsDelaunay[i] = false; + ot.EdgeIsDelaunay[oi] = false; + + // If triangle have been legalized no need to check the other edges since + // the recursive legalization will handles those so we can end here. + return true; + } + } + } + return false; + } + + /// + /// Rotates a triangle pair one vertex CW + /// n2 n2 + /// P +-----+ P +-----+ + /// | t /| |\ t | + /// | / | | \ | + /// n1| / |n3 n1| \ |n3 + /// | / | after CW | \ | + /// |/ oT | | oT \| + /// +-----+ oP +-----+ + /// n4 n4 + /// + private static void RotateTrianglePair(DelaunayTriangle t, TriangulationPoint p, DelaunayTriangle ot, TriangulationPoint op) + { + DelaunayTriangle n1 = t.NeighborCCW(p); + DelaunayTriangle n2 = t.NeighborCW(p); + DelaunayTriangle n3 = ot.NeighborCCW(op); + DelaunayTriangle n4 = ot.NeighborCW(op); + + bool ce1 = t.GetConstrainedEdgeCCW(p); + bool ce2 = t.GetConstrainedEdgeCW(p); + bool ce3 = ot.GetConstrainedEdgeCCW(op); + bool ce4 = ot.GetConstrainedEdgeCW(op); + + bool de1 = t.GetDelaunayEdgeCCW(p); + bool de2 = t.GetDelaunayEdgeCW(p); + bool de3 = ot.GetDelaunayEdgeCCW(op); + bool de4 = ot.GetDelaunayEdgeCW(op); + + t.Legalize(p, op); + ot.Legalize(op, p); + + // Remap dEdge + ot.SetDelaunayEdgeCCW(p, de1); + t.SetDelaunayEdgeCW(p, de2); + t.SetDelaunayEdgeCCW(op, de3); + ot.SetDelaunayEdgeCW(op, de4); + + // Remap cEdge + ot.SetConstrainedEdgeCCW(p, ce1); + t.SetConstrainedEdgeCW(p, ce2); + t.SetConstrainedEdgeCCW(op, ce3); + ot.SetConstrainedEdgeCW(op, ce4); + + // Remap neighbors + // XXX: might optimize the markNeighbor by keeping track of + // what side should be assigned to what neighbor after the + // rotation. Now mark neighbor does lots of testing to find + // the right side. + t.Neighbors.Clear(); + ot.Neighbors.Clear(); + if (n1 != null) ot.MarkNeighbor(n1); + if (n2 != null) t.MarkNeighbor(n2); + if (n3 != null) t.MarkNeighbor(n3); + if (n4 != null) ot.MarkNeighbor(n4); + t.MarkNeighbor(ot); + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepConstraint.cs b/src/Box2DNet/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepConstraint.cs new file mode 100644 index 0000000..b17b520 --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepConstraint.cs @@ -0,0 +1,66 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +namespace Box2DNet.Common.Decomposition.CDT.Delaunay.Sweep +{ + internal class DTSweepConstraint : TriangulationConstraint + { + /// + /// Give two points in any order. Will always be ordered so + /// that q.y > p.y and q.x > p.x if same y value + /// + public DTSweepConstraint(TriangulationPoint p1, TriangulationPoint p2) + { + P = p1; + Q = p2; + if (p1.Y > p2.Y) + { + Q = p1; + P = p2; + } + else if (p1.Y == p2.Y) + { + if (p1.X > p2.X) + { + Q = p1; + P = p2; + } + else if (p1.X == p2.X) + { + // logger.info( "Failed to create constraint {}={}", p1, p2 ); + // throw new DuplicatePointException( p1 + "=" + p2 ); + // return; + } + } + Q.AddEdge(this); + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepContext.cs b/src/Box2DNet/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepContext.cs new file mode 100644 index 0000000..5bb7d63 --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepContext.cs @@ -0,0 +1,236 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +namespace Box2DNet.Common.Decomposition.CDT.Delaunay.Sweep +{ + /** + * + * @author Thomas Åhlén, thahlen@gmail.com + * + */ + + internal class DTSweepContext : TriangulationContext + { + // Inital triangle factor, seed triangle will extend 30% of + // PointSet width to both left and right. + private const float ALPHA = 0.3f; + + public DTSweepBasin Basin = new DTSweepBasin(); + public DTSweepEdgeEvent EdgeEvent = new DTSweepEdgeEvent(); + + private DTSweepPointComparator _comparator = new DTSweepPointComparator(); + public AdvancingFront aFront; + + public DTSweepContext() + { + Clear(); + } + + public TriangulationPoint Head { get; set; } + public TriangulationPoint Tail { get; set; } + + public void RemoveFromList(DelaunayTriangle triangle) + { + Triangles.Remove(triangle); + // TODO: remove all neighbor pointers to this triangle + // for( int i=0; i<3; i++ ) + // { + // if( triangle.neighbors[i] != null ) + // { + // triangle.neighbors[i].clearNeighbor( triangle ); + // } + // } + // triangle.clearNeighbors(); + } + + public void MeshClean(DelaunayTriangle triangle) + { + MeshCleanReq(triangle); + } + + private void MeshCleanReq(DelaunayTriangle triangle) + { + if (triangle != null && !triangle.IsInterior) + { + triangle.IsInterior = true; + Triangulatable.AddTriangle(triangle); + for (int i = 0; i < 3; i++) + { + if (!triangle.EdgeIsConstrained[i]) + { + MeshCleanReq(triangle.Neighbors[i]); + } + } + } + } + + public override void Clear() + { + base.Clear(); + Triangles.Clear(); + } + + public void AddNode(AdvancingFrontNode node) + { + // Console.WriteLine( "add:" + node.key + ":" + System.identityHashCode(node.key)); + // m_nodeTree.put( node.getKey(), node ); + aFront.AddNode(node); + } + + public void RemoveNode(AdvancingFrontNode node) + { + // Console.WriteLine( "remove:" + node.key + ":" + System.identityHashCode(node.key)); + // m_nodeTree.delete( node.getKey() ); + aFront.RemoveNode(node); + } + + public AdvancingFrontNode LocateNode(TriangulationPoint point) + { + return aFront.LocateNode(point); + } + + public void CreateAdvancingFront() + { + AdvancingFrontNode head, tail, middle; + // Initial triangle + DelaunayTriangle iTriangle = new DelaunayTriangle(Points[0], Tail, Head); + Triangles.Add(iTriangle); + + head = new AdvancingFrontNode(iTriangle.Points[1]); + head.Triangle = iTriangle; + middle = new AdvancingFrontNode(iTriangle.Points[0]); + middle.Triangle = iTriangle; + tail = new AdvancingFrontNode(iTriangle.Points[2]); + + aFront = new AdvancingFront(head, tail); + aFront.AddNode(middle); + + // TODO: I think it would be more intuitive if head is middles next and not previous + // so swap head and tail + aFront.Head.Next = middle; + middle.Next = aFront.Tail; + middle.Prev = aFront.Head; + aFront.Tail.Prev = middle; + } + + /// + /// Try to map a node to all sides of this triangle that don't have + /// a neighbor. + /// + public void MapTriangleToNodes(DelaunayTriangle t) + { + AdvancingFrontNode n; + for (int i = 0; i < 3; i++) + { + if (t.Neighbors[i] == null) + { + n = aFront.LocatePoint(t.PointCW(t.Points[i])); + if (n != null) + { + n.Triangle = t; + } + } + } + } + + public override void PrepareTriangulation(Triangulatable t) + { + base.PrepareTriangulation(t); + + double xmax, xmin; + double ymax, ymin; + + xmax = xmin = Points[0].X; + ymax = ymin = Points[0].Y; + + // Calculate bounds. Should be combined with the sorting + foreach (TriangulationPoint p in Points) + { + if (p.X > xmax) + xmax = p.X; + if (p.X < xmin) + xmin = p.X; + if (p.Y > ymax) + ymax = p.Y; + if (p.Y < ymin) + ymin = p.Y; + } + + double deltaX = ALPHA*(xmax - xmin); + double deltaY = ALPHA*(ymax - ymin); + TriangulationPoint p1 = new TriangulationPoint(xmax + deltaX, ymin - deltaY); + TriangulationPoint p2 = new TriangulationPoint(xmin - deltaX, ymin - deltaY); + + Head = p1; + Tail = p2; + + // long time = System.nanoTime(); + // Sort the points along y-axis + Points.Sort(_comparator); + // logger.info( "Triangulation setup [{}ms]", ( System.nanoTime() - time ) / 1e6 ); + } + + + public void FinalizeTriangulation() + { + Triangulatable.AddTriangles(Triangles); + Triangles.Clear(); + } + + public override TriangulationConstraint NewConstraint(TriangulationPoint a, TriangulationPoint b) + { + return new DTSweepConstraint(a, b); + } + + #region Nested type: DTSweepBasin + + public class DTSweepBasin + { + public AdvancingFrontNode bottomNode; + public bool leftHighest; + public AdvancingFrontNode leftNode; + public AdvancingFrontNode rightNode; + public double width; + } + + #endregion + + #region Nested type: DTSweepEdgeEvent + + public class DTSweepEdgeEvent + { + public DTSweepConstraint ConstrainedEdge; + public bool Right; + } + + #endregion + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepPointComparator.cs b/src/Box2DNet/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepPointComparator.cs new file mode 100644 index 0000000..802131e --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/CDT/Delaunay/Sweep/DTSweepPointComparator.cs @@ -0,0 +1,69 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System.Collections.Generic; + +namespace Box2DNet.Common.Decomposition.CDT.Delaunay.Sweep +{ + internal class DTSweepPointComparator : IComparer + { + #region IComparer Members + + public int Compare(TriangulationPoint p1, TriangulationPoint p2) + { + if (p1.Y < p2.Y) + { + return -1; + } + else if (p1.Y > p2.Y) + { + return 1; + } + else + { + if (p1.X < p2.X) + { + return -1; + } + else if (p1.X > p2.X) + { + return 1; + } + else + { + return 0; + } + } + } + + #endregion + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/CDT/Delaunay/Sweep/PointOnEdgeException.cs b/src/Box2DNet/Common/Decomposition/CDT/Delaunay/Sweep/PointOnEdgeException.cs new file mode 100644 index 0000000..fcf0b57 --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/CDT/Delaunay/Sweep/PointOnEdgeException.cs @@ -0,0 +1,43 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System; + +namespace Box2DNet.Common.Decomposition.CDT.Delaunay.Sweep +{ + internal class PointOnEdgeException : NotImplementedException + { + public PointOnEdgeException(string message) + : base(message) + { + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/CDT/ITriangulatable.cs b/src/Box2DNet/Common/Decomposition/CDT/ITriangulatable.cs new file mode 100644 index 0000000..b9516a3 --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/CDT/ITriangulatable.cs @@ -0,0 +1,48 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System.Collections.Generic; +using Box2DNet.Common.Decomposition.CDT.Delaunay; + +namespace Box2DNet.Common.Decomposition.CDT +{ + internal interface Triangulatable + { + IList Points { get; } // MM: Neither of these are used via interface (yet?) + IList Triangles { get; } + TriangulationMode TriangulationMode { get; } + void PrepareTriangulation(TriangulationContext tcx); + + void AddTriangle(DelaunayTriangle t); + void AddTriangles(IEnumerable list); + void ClearTriangles(); + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/CDT/Orientation.cs b/src/Box2DNet/Common/Decomposition/CDT/Orientation.cs new file mode 100644 index 0000000..0098220 --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/CDT/Orientation.cs @@ -0,0 +1,40 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +namespace Box2DNet.Common.Decomposition.CDT +{ + internal enum Orientation + { + CW, + CCW, + Collinear + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/CDT/Polygon/Polygon.cs b/src/Box2DNet/Common/Decomposition/CDT/Polygon/Polygon.cs new file mode 100644 index 0000000..adcea4e --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/CDT/Polygon/Polygon.cs @@ -0,0 +1,272 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +// Changes from the Java version +// Polygon constructors sprused up, checks for 3+ polys +// Naming of everything +// getTriangulationMode() -> TriangulationMode { get; } +// Exceptions replaced +// Future possibilities +// We have a lot of Add/Clear methods -- we may prefer to just expose the container +// Some self-explanitory methods may deserve commenting anyways + +using System; +using System.Collections.Generic; +using System.Linq; +using Box2DNet.Common.Decomposition.CDT.Delaunay; + +namespace Box2DNet.Common.Decomposition.CDT.Polygon +{ + internal class Polygon : Triangulatable + { + protected List _holes; + protected PolygonPoint _last; + protected List _points = new List(); + protected List _steinerPoints; + protected List _triangles; + + /// + /// Create a polygon from a list of at least 3 points with no duplicates. + /// + /// A list of unique points + public Polygon(IList points) + { + if (points.Count < 3) throw new ArgumentException("List has fewer than 3 points", "points"); + + // Lets do one sanity check that first and last point hasn't got same position + // Its something that often happen when importing polygon data from other formats + if (points[0].Equals(points[points.Count - 1])) points.RemoveAt(points.Count - 1); + + _points.AddRange(points.Cast()); + } + + /// + /// Create a polygon from a list of at least 3 points with no duplicates. + /// + /// A list of unique points. + public Polygon(IEnumerable points) : this((points as IList) ?? points.ToArray()) + { + } + + public Polygon() + { + } + + public IList Holes + { + get { return _holes; } + } + + #region Triangulatable Members + + public TriangulationMode TriangulationMode + { + get { return TriangulationMode.Polygon; } + } + + public IList Points + { + get { return _points; } + } + + public IList Triangles + { + get { return _triangles; } + } + + public void AddTriangle(DelaunayTriangle t) + { + _triangles.Add(t); + } + + public void AddTriangles(IEnumerable list) + { + _triangles.AddRange(list); + } + + public void ClearTriangles() + { + if (_triangles != null) _triangles.Clear(); + } + + /// + /// Creates constraints and populates the context with points + /// + /// The context + public void PrepareTriangulation(TriangulationContext tcx) + { + if (_triangles == null) + { + _triangles = new List(_points.Count); + } + else + { + _triangles.Clear(); + } + + // Outer constraints + for (int i = 0; i < _points.Count - 1; i++) + { + tcx.NewConstraint(_points[i], _points[i + 1]); + } + tcx.NewConstraint(_points[0], _points[_points.Count - 1]); + tcx.Points.AddRange(_points); + + // Hole constraints + if (_holes != null) + { + foreach (Polygon p in _holes) + { + for (int i = 0; i < p._points.Count - 1; i++) + { + tcx.NewConstraint(p._points[i], p._points[i + 1]); + } + tcx.NewConstraint(p._points[0], p._points[p._points.Count - 1]); + tcx.Points.AddRange(p._points); + } + } + + if (_steinerPoints != null) + { + tcx.Points.AddRange(_steinerPoints); + } + } + + #endregion + + public void AddSteinerPoint(TriangulationPoint point) + { + if (_steinerPoints == null) + { + _steinerPoints = new List(); + } + _steinerPoints.Add(point); + } + + public void AddSteinerPoints(List points) + { + if (_steinerPoints == null) + { + _steinerPoints = new List(); + } + _steinerPoints.AddRange(points); + } + + public void ClearSteinerPoints() + { + if (_steinerPoints != null) + { + _steinerPoints.Clear(); + } + } + + /// + /// Add a hole to the polygon. + /// + /// A subtraction polygon fully contained inside this polygon. + public void AddHole(Polygon poly) + { + if (_holes == null) _holes = new List(); + _holes.Add(poly); + // XXX: tests could be made here to be sure it is fully inside + // addSubtraction( poly.getPoints() ); + } + + /// + /// Inserts newPoint after point. + /// + /// The point to insert after in the polygon + /// The point to insert into the polygon + public void InsertPointAfter(PolygonPoint point, PolygonPoint newPoint) + { + // Validate that + int index = _points.IndexOf(point); + if (index == -1) + throw new ArgumentException( + "Tried to insert a point into a Polygon after a point not belonging to the Polygon", "point"); + newPoint.Next = point.Next; + newPoint.Previous = point; + point.Next.Previous = newPoint; + point.Next = newPoint; + _points.Insert(index + 1, newPoint); + } + + /// + /// Inserts list (after last point in polygon?) + /// + /// + public void AddPoints(IEnumerable list) + { + PolygonPoint first; + foreach (PolygonPoint p in list) + { + p.Previous = _last; + if (_last != null) + { + p.Next = _last.Next; + _last.Next = p; + } + _last = p; + _points.Add(p); + } + first = (PolygonPoint) _points[0]; + _last.Next = first; + first.Previous = _last; + } + + /// + /// Adds a point after the last in the polygon. + /// + /// The point to add + public void AddPoint(PolygonPoint p) + { + p.Previous = _last; + p.Next = _last.Next; + _last.Next = p; + _points.Add(p); + } + + /// + /// Removes a point from the polygon. + /// + /// + public void RemovePoint(PolygonPoint p) + { + PolygonPoint next, prev; + + next = p.Next; + prev = p.Previous; + prev.Next = next; + next.Previous = prev; + _points.Remove(p); + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/CDT/Polygon/PolygonPoint.cs b/src/Box2DNet/Common/Decomposition/CDT/Polygon/PolygonPoint.cs new file mode 100644 index 0000000..aced2bb --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/CDT/Polygon/PolygonPoint.cs @@ -0,0 +1,48 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +// Changes from the Java version +// Replaced get/set Next/Previous with attributes +// Future possibilities +// Documentation! + +namespace Box2DNet.Common.Decomposition.CDT.Polygon +{ + internal class PolygonPoint : TriangulationPoint + { + public PolygonPoint(double x, double y) : base(x, y) + { + } + + public PolygonPoint Next { get; set; } + public PolygonPoint Previous { get; set; } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/CDT/Polygon/PolygonSet.cs b/src/Box2DNet/Common/Decomposition/CDT/Polygon/PolygonSet.cs new file mode 100644 index 0000000..0b6dc35 --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/CDT/Polygon/PolygonSet.cs @@ -0,0 +1,65 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +// Changes from the Java version +// Replaced getPolygons with attribute +// Future possibilities +// Replace Add(Polygon) with exposed container? +// Replace entire class with HashSet ? + +using System.Collections.Generic; + +namespace Box2DNet.Common.Decomposition.CDT.Polygon +{ + internal class PolygonSet + { + protected List _polygons = new List(); + + public PolygonSet() + { + } + + public PolygonSet(Polygon poly) + { + _polygons.Add(poly); + } + + public IEnumerable Polygons + { + get { return _polygons; } + } + + public void Add(Polygon p) + { + _polygons.Add(p); + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/CDT/Sets/ConstrainedPointSet.cs b/src/Box2DNet/Common/Decomposition/CDT/Sets/ConstrainedPointSet.cs new file mode 100644 index 0000000..8230c0f --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/CDT/Sets/ConstrainedPointSet.cs @@ -0,0 +1,114 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System.Collections.Generic; + +namespace Box2DNet.Common.Decomposition.CDT.Sets +{ + /* + * Extends the PointSet by adding some Constraints on how it will be triangulated
+ * A constraint defines an edge between two points in the set, these edges can not + * be crossed. They will be enforced triangle edges after a triangulation. + *

+ * + * + * @author Thomas Åhlén, thahlen@gmail.com + */ + + internal class ConstrainedPointSet : PointSet + { + private List _constrainedPointList; + + public ConstrainedPointSet(List points, int[] index) + : base(points) + { + EdgeIndex = index; + } + + /** + * + * @param points - A list of all points in PointSet + * @param constraints - Pairs of two points defining a constraint, all points must be part of given PointSet! + */ + + public ConstrainedPointSet(List points, IEnumerable constraints) + : base(points) + { + _constrainedPointList = new List(); + _constrainedPointList.AddRange(constraints); + } + + public int[] EdgeIndex { get; private set; } + + public override TriangulationMode TriangulationMode + { + get { return TriangulationMode.Constrained; } + } + + public override void PrepareTriangulation(TriangulationContext tcx) + { + base.PrepareTriangulation(tcx); + if (_constrainedPointList != null) + { + TriangulationPoint p1, p2; + List.Enumerator iterator = _constrainedPointList.GetEnumerator(); + while (iterator.MoveNext()) + { + p1 = iterator.Current; + iterator.MoveNext(); + p2 = iterator.Current; + tcx.NewConstraint(p1, p2); + } + } + else + { + for (int i = 0; i < EdgeIndex.Length; i += 2) + { + // XXX: must change!! + tcx.NewConstraint(Points[EdgeIndex[i]], Points[EdgeIndex[i + 1]]); + } + } + } + + /** + * TODO: TO BE IMPLEMENTED! + * Peforms a validation on given input
+ * 1. Check's if there any constraint edges are crossing or collinear
+ * 2. + * @return + */ + + public bool isValid() + { + return true; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/CDT/Sets/PointSet.cs b/src/Box2DNet/Common/Decomposition/CDT/Sets/PointSet.cs new file mode 100644 index 0000000..f2ecb6b --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/CDT/Sets/PointSet.cs @@ -0,0 +1,84 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System.Collections.Generic; +using Box2DNet.Common.Decomposition.CDT.Delaunay; + +namespace Box2DNet.Common.Decomposition.CDT.Sets +{ + internal class PointSet : Triangulatable + { + public PointSet(List points) + { + Points = new List(points); + } + + #region Triangulatable Members + + public IList Points { get; private set; } + public IList Triangles { get; private set; } + + public virtual TriangulationMode TriangulationMode + { + get { return TriangulationMode.Unconstrained; } + } + + public void AddTriangle(DelaunayTriangle t) + { + Triangles.Add(t); + } + + public void AddTriangles(IEnumerable list) + { + foreach (DelaunayTriangle tri in list) Triangles.Add(tri); + } + + public void ClearTriangles() + { + Triangles.Clear(); + } + + public virtual void PrepareTriangulation(TriangulationContext tcx) + { + if (Triangles == null) + { + Triangles = new List(Points.Count); + } + else + { + Triangles.Clear(); + } + tcx.Points.AddRange(Points); + } + + #endregion + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/CDT/TriangulationConstraint.cs b/src/Box2DNet/Common/Decomposition/CDT/TriangulationConstraint.cs new file mode 100644 index 0000000..80bb15b --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/CDT/TriangulationConstraint.cs @@ -0,0 +1,46 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +/** + * Forces a triangle edge between two points p and q + * when triangulating. For example used to enforce + * Polygon Edges during a polygon triangulation. + * + * @author Thomas Åhlén, thahlen@gmail.com + */ +namespace Box2DNet.Common.Decomposition.CDT +{ + internal class TriangulationConstraint + { + public TriangulationPoint P; + public TriangulationPoint Q; + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/CDT/TriangulationContext.cs b/src/Box2DNet/Common/Decomposition/CDT/TriangulationContext.cs new file mode 100644 index 0000000..59f20bc --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/CDT/TriangulationContext.cs @@ -0,0 +1,84 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System.Collections.Generic; +using System.Runtime.CompilerServices; +using Box2DNet.Common.Decomposition.CDT.Delaunay; + +namespace Box2DNet.Common.Decomposition.CDT +{ + internal abstract class TriangulationContext + { + public readonly List Points = new List(200); + public readonly List Triangles = new List(); + private int _stepTime = -1; + + public TriangulationContext() + { + Terminated = false; + } + + public TriangulationMode TriangulationMode { get; protected set; } + public Triangulatable Triangulatable { get; private set; } + + public bool WaitUntilNotified { get; private set; } + public bool Terminated { get; set; } + + public int StepCount { get; private set; } + public virtual bool IsDebugEnabled { get; protected set; } + + public void Done() + { + StepCount++; + } + + public virtual void PrepareTriangulation(Triangulatable t) + { + Triangulatable = t; + TriangulationMode = t.TriangulationMode; + t.PrepareTriangulation(this); + } + + public abstract TriangulationConstraint NewConstraint(TriangulationPoint a, TriangulationPoint b); + + [MethodImpl(MethodImplOptions.Synchronized)] + public void Update(string message) + { + } + + public virtual void Clear() + { + Points.Clear(); + Terminated = false; + StepCount = 0; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/CDT/TriangulationMode.cs b/src/Box2DNet/Common/Decomposition/CDT/TriangulationMode.cs new file mode 100644 index 0000000..c91bf16 --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/CDT/TriangulationMode.cs @@ -0,0 +1,40 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +namespace Box2DNet.Common.Decomposition.CDT +{ + internal enum TriangulationMode + { + Unconstrained, + Constrained, + Polygon + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/CDT/TriangulationPoint.cs b/src/Box2DNet/Common/Decomposition/CDT/TriangulationPoint.cs new file mode 100644 index 0000000..c13923f --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/CDT/TriangulationPoint.cs @@ -0,0 +1,82 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System.Collections.Generic; +using Box2DNet.Common.Decomposition.CDT.Delaunay.Sweep; + +namespace Box2DNet.Common.Decomposition.CDT +{ + internal class TriangulationPoint + { + // List of edges this point constitutes an upper ending point (CDT) + + public double X, Y; + + public TriangulationPoint(double x, double y) + { + X = x; + Y = y; + } + + public List Edges { get; private set; } + + public float Xf + { + get { return (float) X; } + set { X = value; } + } + + public float Yf + { + get { return (float) Y; } + set { Y = value; } + } + + public bool HasEdges + { + get { return Edges != null; } + } + + public override string ToString() + { + return "[" + X + "," + Y + "]"; + } + + public void AddEdge(DTSweepConstraint e) + { + if (Edges == null) + { + Edges = new List(); + } + Edges.Add(e); + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/CDT/TriangulationUtil.cs b/src/Box2DNet/Common/Decomposition/CDT/TriangulationUtil.cs new file mode 100644 index 0000000..f737192 --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/CDT/TriangulationUtil.cs @@ -0,0 +1,175 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +namespace Box2DNet.Common.Decomposition.CDT +{ + /** + * @author Thomas Åhlén, thahlen@gmail.com + */ + + internal class TriangulationUtil + { + public static double EPSILON = 1e-12; + + ///

+ /// Requirements: + /// 1. a,b and c form a triangle. + /// 2. a and d is know to be on opposite side of bc + /// + /// a + /// + + /// / \ + /// / \ + /// b/ \c + /// +-------+ + /// / B \ + /// / \ + /// + /// Facts: + /// d has to be in area B to have a chance to be inside the circle formed by a,b and c + /// d is outside B if orient2d(a,b,d) or orient2d(c,a,d) is CW + /// This preknowledge gives us a way to optimize the incircle test + /// + /// triangle point, opposite d + /// triangle point + /// triangle point + /// point opposite a + /// true if d is inside circle, false if on circle edge + public static bool SmartIncircle(TriangulationPoint pa, TriangulationPoint pb, TriangulationPoint pc, + TriangulationPoint pd) + { + double pdx = pd.X; + double pdy = pd.Y; + double adx = pa.X - pdx; + double ady = pa.Y - pdy; + double bdx = pb.X - pdx; + double bdy = pb.Y - pdy; + + double adxbdy = adx * bdy; + double bdxady = bdx * ady; + double oabd = adxbdy - bdxady; + // oabd = orient2d(pa,pb,pd); + if (oabd <= 0) return false; + + double cdx = pc.X - pdx; + double cdy = pc.Y - pdy; + + double cdxady = cdx * ady; + double adxcdy = adx * cdy; + double ocad = cdxady - adxcdy; + // ocad = orient2d(pc,pa,pd); + if (ocad <= 0) return false; + + double bdxcdy = bdx * cdy; + double cdxbdy = cdx * bdy; + + double alift = adx * adx + ady * ady; + double blift = bdx * bdx + bdy * bdy; + double clift = cdx * cdx + cdy * cdy; + + double det = alift * (bdxcdy - cdxbdy) + blift * ocad + clift * oabd; + + return det > 0; + } + /* + public static bool InScanArea(TriangulationPoint pa, TriangulationPoint pb, TriangulationPoint pc, + TriangulationPoint pd) + { + double pdx = pd.X; + double pdy = pd.Y; + double adx = pa.X - pdx; + double ady = pa.Y - pdy; + double bdx = pb.X - pdx; + double bdy = pb.Y - pdy; + + double adxbdy = adx*bdy; + double bdxady = bdx*ady; + double oabd = adxbdy - bdxady; + // oabd = orient2d(pa,pb,pd); + if (oabd <= 0) + { + return false; + } + + double cdx = pc.X - pdx; + double cdy = pc.Y - pdy; + + double cdxady = cdx*ady; + double adxcdy = adx*cdy; + double ocad = cdxady - adxcdy; + // ocad = orient2d(pc,pa,pd); + if (ocad <= 0) + { + return false; + } + return true; + } + */ + + public static bool InScanArea(TriangulationPoint pa, TriangulationPoint pb, TriangulationPoint pc, TriangulationPoint pd) + { + double oadb = (pa.X - pb.X) * (pd.Y - pb.Y) - (pd.X - pb.X) * (pa.Y - pb.Y); + if (oadb >= -EPSILON) + { + return false; + } + + double oadc = (pa.X - pc.X) * (pd.Y - pc.Y) - (pd.X - pc.X) * (pa.Y - pc.Y); + if (oadc <= EPSILON) + { + return false; + } + return true; + } + + /// Forumla to calculate signed area + /// Positive if CCW + /// Negative if CW + /// 0 if collinear + /// A[P1,P2,P3] = (x1*y2 - y1*x2) + (x2*y3 - y2*x3) + (x3*y1 - y3*x1) + /// = (x1-x3)*(y2-y3) - (y1-y3)*(x2-x3) + public static Orientation Orient2d(TriangulationPoint pa, TriangulationPoint pb, TriangulationPoint pc) + { + double detleft = (pa.X - pc.X) * (pb.Y - pc.Y); + double detright = (pa.Y - pc.Y) * (pb.X - pc.X); + double val = detleft - detright; + if (val > -EPSILON && val < EPSILON) + { + return Orientation.Collinear; + } + else if (val > 0) + { + return Orientation.CCW; + } + return Orientation.CW; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/CDT/Util/FixedArray3.cs b/src/Box2DNet/Common/Decomposition/CDT/Util/FixedArray3.cs new file mode 100644 index 0000000..3de628b --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/CDT/Util/FixedArray3.cs @@ -0,0 +1,118 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using System.Collections; +using System.Collections.Generic; + +namespace Box2DNet.Common.Decomposition.CDT.Util +{ + internal struct FixedArray3 : IEnumerable where T : class + { + public T _0, _1, _2; + + public T this[int index] + { + get + { + switch (index) + { + case 0: + return _0; + case 1: + return _1; + case 2: + return _2; + default: + throw new IndexOutOfRangeException(); + } + } + set + { + switch (index) + { + case 0: + _0 = value; + break; + case 1: + _1 = value; + break; + case 2: + _2 = value; + break; + default: + throw new IndexOutOfRangeException(); + } + } + } + + #region IEnumerable Members + + public IEnumerator GetEnumerator() + { + return Enumerate().GetEnumerator(); + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + + #endregion + + public bool Contains(T value) + { + for (int i = 0; i < 3; ++i) if (this[i] == value) return true; + return false; + } + + public int IndexOf(T value) + { + for (int i = 0; i < 3; ++i) if (this[i] == value) return i; + return -1; + } + + public void Clear() + { + _0 = _1 = _2 = null; + } + + public void Clear(T value) + { + for (int i = 0; i < 3; ++i) if (this[i] == value) this[i] = null; + } + + private IEnumerable Enumerate() + { + for (int i = 0; i < 3; ++i) yield return this[i]; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/CDT/Util/FixedBitArray3.cs b/src/Box2DNet/Common/Decomposition/CDT/Util/FixedBitArray3.cs new file mode 100644 index 0000000..99f6d46 --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/CDT/Util/FixedBitArray3.cs @@ -0,0 +1,118 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using System.Collections; +using System.Collections.Generic; + +namespace Box2DNet.Common.Decomposition.CDT.Util +{ + internal struct FixedBitArray3 : IEnumerable + { + public bool _0, _1, _2; + + public bool this[int index] + { + get + { + switch (index) + { + case 0: + return _0; + case 1: + return _1; + case 2: + return _2; + default: + throw new IndexOutOfRangeException(); + } + } + set + { + switch (index) + { + case 0: + _0 = value; + break; + case 1: + _1 = value; + break; + case 2: + _2 = value; + break; + default: + throw new IndexOutOfRangeException(); + } + } + } + + #region IEnumerable Members + + public IEnumerator GetEnumerator() + { + return Enumerate().GetEnumerator(); + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + + #endregion + + public bool Contains(bool value) + { + for (int i = 0; i < 3; ++i) if (this[i] == value) return true; + return false; + } + + public int IndexOf(bool value) + { + for (int i = 0; i < 3; ++i) if (this[i] == value) return i; + return -1; + } + + public void Clear() + { + _0 = _1 = _2 = false; + } + + public void Clear(bool value) + { + for (int i = 0; i < 3; ++i) if (this[i] == value) this[i] = false; + } + + private IEnumerable Enumerate() + { + for (int i = 0; i < 3; ++i) yield return this[i]; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/CDT/Util/PointGenerator.cs b/src/Box2DNet/Common/Decomposition/CDT/Util/PointGenerator.cs new file mode 100644 index 0000000..f988c50 --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/CDT/Util/PointGenerator.cs @@ -0,0 +1,38 @@ +using System; +using System.Collections.Generic; + +namespace Box2DNet.Common.Decomposition.CDT.Util +{ + internal class PointGenerator + { + private static readonly Random RNG = new Random(); + + public static List UniformDistribution(int n, double scale) + { + List points = new List(); + for (int i = 0; i < n; i++) + { + points.Add(new TriangulationPoint(scale*(0.5 - RNG.NextDouble()), scale*(0.5 - RNG.NextDouble()))); + } + return points; + } + + public static List UniformGrid(int n, double scale) + { + double x = 0; + double size = scale/n; + double halfScale = 0.5*scale; + + List points = new List(); + for (int i = 0; i < n + 1; i++) + { + x = halfScale - i*size; + for (int j = 0; j < n + 1; j++) + { + points.Add(new TriangulationPoint(x, halfScale - j*size)); + } + } + return points; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/CDT/Util/PolygonGenerator.cs b/src/Box2DNet/Common/Decomposition/CDT/Util/PolygonGenerator.cs new file mode 100644 index 0000000..20b46b5 --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/CDT/Util/PolygonGenerator.cs @@ -0,0 +1,98 @@ +/* Poly2Tri + * Copyright (c) 2009-2010, Poly2Tri Contributors + * http://code.google.com/p/poly2tri/ + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * * Neither the name of Poly2Tri nor the names of its contributors may be + * used to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using Box2DNet.Common.Decomposition.CDT.Polygon; + +namespace Box2DNet.Common.Decomposition.CDT.Util +{ + internal class PolygonGenerator + { + private static readonly Random RNG = new Random(); + + private static double PI_2 = 2.0*Math.PI; + + public static Polygon.Polygon RandomCircleSweep(double scale, int vertexCount) + { + PolygonPoint point; + PolygonPoint[] points; + double radius = scale/4; + + points = new PolygonPoint[vertexCount]; + for (int i = 0; i < vertexCount; i++) + { + do + { + if (i%250 == 0) + { + radius += scale/2*(0.5 - RNG.NextDouble()); + } + else if (i%50 == 0) + { + radius += scale/5*(0.5 - RNG.NextDouble()); + } + else + { + radius += 25*scale/vertexCount*(0.5 - RNG.NextDouble()); + } + radius = radius > scale/2 ? scale/2 : radius; + radius = radius < scale/10 ? scale/10 : radius; + } while (radius < scale/10 || radius > scale/2); + point = new PolygonPoint(radius*Math.Cos((PI_2*i)/vertexCount), + radius*Math.Sin((PI_2*i)/vertexCount)); + points[i] = point; + } + return new Polygon.Polygon(points); + } + + public static Polygon.Polygon RandomCircleSweep2(double scale, int vertexCount) + { + PolygonPoint point; + PolygonPoint[] points; + double radius = scale/4; + + points = new PolygonPoint[vertexCount]; + for (int i = 0; i < vertexCount; i++) + { + do + { + radius += scale/5*(0.5 - RNG.NextDouble()); + radius = radius > scale/2 ? scale/2 : radius; + radius = radius < scale/10 ? scale/10 : radius; + } while (radius < scale/10 || radius > scale/2); + point = new PolygonPoint(radius*Math.Cos((PI_2*i)/vertexCount), + radius*Math.Sin((PI_2*i)/vertexCount)); + points[i] = point; + } + return new Polygon.Polygon(points); + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/CDTDecomposer.cs b/src/Box2DNet/Common/Decomposition/CDTDecomposer.cs new file mode 100644 index 0000000..ca7fce1 --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/CDTDecomposer.cs @@ -0,0 +1,75 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +*/ + +using System.Collections.Generic; +using System.Diagnostics; +using Box2DNet.Common.Decomposition.CDT; +using Box2DNet.Common.Decomposition.CDT.Delaunay; +using Box2DNet.Common.Decomposition.CDT.Delaunay.Sweep; +using Box2DNet.Common.Decomposition.CDT.Polygon; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Common.Decomposition +{ + /// + /// 2D constrained Delaunay triangulation algorithm. + /// Based on the paper "Sweep-line algorithm for constrained Delaunay triangulation" by V. Domiter and and B. Zalik + /// + /// Properties: + /// - Creates triangles with a large interior angle. + /// - Supports holes + /// - Generate a lot of garbage due to incapsulation of the Poly2Tri library. + /// - Running time is O(n^2), n = number of vertices. + /// - Does not care about winding order. + /// + /// Source: http://code.google.com/p/poly2tri/ + /// + internal static class CDTDecomposer + { + /// + /// Decompose the polygon into several smaller non-concave polygon. + /// + public static List ConvexPartition(Vertices vertices) + { + Debug.Assert(vertices.Count > 3); + + Polygon poly = new Polygon(); + + foreach (Vector2 vertex in vertices) + poly.Points.Add(new TriangulationPoint(vertex.X, vertex.Y)); + + if (vertices.Holes != null) + { + foreach (Vertices holeVertices in vertices.Holes) + { + Polygon hole = new Polygon(); + + foreach (Vector2 vertex in holeVertices) + hole.Points.Add(new TriangulationPoint(vertex.X, vertex.Y)); + + poly.AddHole(hole); + } + } + + DTSweepContext tcx = new DTSweepContext(); + tcx.PrepareTriangulation(poly); + DTSweep.Triangulate(tcx); + + List results = new List(); + + foreach (DelaunayTriangle triangle in poly.Triangles) + { + Vertices v = new Vertices(); + foreach (TriangulationPoint p in triangle.Points) + { + v.Add(new Vector2((float)p.X, (float)p.Y)); + } + results.Add(v); + } + + return results; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/EarclipDecomposer.cs b/src/Box2DNet/Common/Decomposition/EarclipDecomposer.cs new file mode 100644 index 0000000..72fded4 --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/EarclipDecomposer.cs @@ -0,0 +1,403 @@ +/* +* C# Version Ported by Matt Bettcher and Ian Qvist 2009-2010 +* +* Original C++ Version Copyright (c) 2007 Eric Jordan +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Collections.Generic; +using System.Diagnostics; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Common.Decomposition +{ + /// + /// Convex decomposition algorithm using ear clipping + /// + /// Properties: + /// - Only works on simple polygons. + /// - Does not support holes. + /// - Running time is O(n^2), n = number of vertices. + /// + /// Source: http://www.ewjordan.com/earClip/ + /// + internal static class EarclipDecomposer + { + //box2D rev 32 - for details, see http://www.box2d.org/forum/viewtopic.php?f=4&t=83&start=50 + + /// + /// Decompose the polygon into several smaller non-concave polygon. + /// Each resulting polygon will have no more than Settings.MaxPolygonVertices vertices. + /// + /// The vertices. + /// The tolerance. + public static List ConvexPartition(Vertices vertices, float tolerance = 0.001f) + { + Debug.Assert(vertices.Count > 3); + Debug.Assert(!vertices.IsCounterClockWise()); + + return TriangulatePolygon(vertices, tolerance); + } + + /// + /// Triangulates a polygon using simple ear-clipping algorithm. Returns + /// size of Triangle array unless the polygon can't be triangulated. + /// This should only happen if the polygon self-intersects, + /// though it will not _always_ return null for a bad polygon - it is the + /// caller's responsibility to check for self-intersection, and if it + /// doesn't, it should at least check that the return value is non-null + /// before using. You're warned! + /// + /// Triangles may be degenerate, especially if you have identical points + /// in the input to the algorithm. Check this before you use them. + /// + /// This is totally unoptimized, so for large polygons it should not be part + /// of the simulation loop. + /// + /// + /// Only works on simple polygons. + /// + private static List TriangulatePolygon(Vertices vertices, float tolerance) + { + //FPE note: Check is needed as invalid triangles can be returned in recursive calls. + if (vertices.Count < 3) + return new List(); + + List results = new List(); + + //Recurse and split on pinch points + Vertices pA, pB; + Vertices pin = new Vertices(vertices); + if (ResolvePinchPoint(pin, out pA, out pB, tolerance)) + { + List mergeA = TriangulatePolygon(pA, tolerance); + List mergeB = TriangulatePolygon(pB, tolerance); + + if (mergeA.Count == -1 || mergeB.Count == -1) + throw new Exception("Can't triangulate your polygon."); + + for (int i = 0; i < mergeA.Count; ++i) + { + results.Add(new Vertices(mergeA[i])); + } + for (int i = 0; i < mergeB.Count; ++i) + { + results.Add(new Vertices(mergeB[i])); + } + + return results; + } + + Vertices[] buffer = new Vertices[vertices.Count - 2]; + int bufferSize = 0; + float[] xrem = new float[vertices.Count]; + float[] yrem = new float[vertices.Count]; + for (int i = 0; i < vertices.Count; ++i) + { + xrem[i] = vertices[i].X; + yrem[i] = vertices[i].Y; + } + + int vNum = vertices.Count; + + while (vNum > 3) + { + // Find an ear + int earIndex = -1; + float earMaxMinCross = -10.0f; + for (int i = 0; i < vNum; ++i) + { + if (IsEar(i, xrem, yrem, vNum)) + { + int lower = Remainder(i - 1, vNum); + int upper = Remainder(i + 1, vNum); + Vector2 d1 = new Vector2(xrem[upper] - xrem[i], yrem[upper] - yrem[i]); + Vector2 d2 = new Vector2(xrem[i] - xrem[lower], yrem[i] - yrem[lower]); + Vector2 d3 = new Vector2(xrem[lower] - xrem[upper], yrem[lower] - yrem[upper]); + + d1.Normalize(); + d2.Normalize(); + d3.Normalize(); + float cross12; + MathUtils.Cross(ref d1, ref d2, out cross12); + cross12 = Math.Abs(cross12); + + float cross23; + MathUtils.Cross(ref d2, ref d3, out cross23); + cross23 = Math.Abs(cross23); + + float cross31; + MathUtils.Cross(ref d3, ref d1, out cross31); + cross31 = Math.Abs(cross31); + + //Find the maximum minimum angle + float minCross = Math.Min(cross12, Math.Min(cross23, cross31)); + if (minCross > earMaxMinCross) + { + earIndex = i; + earMaxMinCross = minCross; + } + } + } + + // If we still haven't found an ear, we're screwed. + // Note: sometimes this is happening because the + // remaining points are collinear. Really these + // should just be thrown out without halting triangulation. + if (earIndex == -1) + { + for (int i = 0; i < bufferSize; i++) + { + results.Add(buffer[i]); + } + + return results; + } + + // Clip off the ear: + // - remove the ear tip from the list + + --vNum; + float[] newx = new float[vNum]; + float[] newy = new float[vNum]; + int currDest = 0; + for (int i = 0; i < vNum; ++i) + { + if (currDest == earIndex) ++currDest; + newx[i] = xrem[currDest]; + newy[i] = yrem[currDest]; + ++currDest; + } + + // - add the clipped triangle to the triangle list + int under = (earIndex == 0) ? (vNum) : (earIndex - 1); + int over = (earIndex == vNum) ? 0 : (earIndex + 1); + Triangle toAdd = new Triangle(xrem[earIndex], yrem[earIndex], xrem[over], yrem[over], xrem[under], + yrem[under]); + buffer[bufferSize] = toAdd; + ++bufferSize; + + // - replace the old list with the new one + xrem = newx; + yrem = newy; + } + + Triangle tooAdd = new Triangle(xrem[1], yrem[1], xrem[2], yrem[2], xrem[0], yrem[0]); + buffer[bufferSize] = tooAdd; + ++bufferSize; + + for (int i = 0; i < bufferSize; i++) + { + results.Add(new Vertices(buffer[i])); + } + + return results; + } + + /// + /// Finds and fixes "pinch points," points where two polygon + /// vertices are at the same point. + /// + /// If a pinch point is found, pin is broken up into poutA and poutB + /// and true is returned; otherwise, returns false. + /// + /// Mostly for internal use. + /// + /// O(N^2) time, which sucks... + /// + /// The pin. + /// The pout A. + /// The pout B. + /// + private static bool ResolvePinchPoint(Vertices pin, out Vertices poutA, out Vertices poutB, float tolerance) + { + poutA = new Vertices(); + poutB = new Vertices(); + + if (pin.Count < 3) + return false; + + bool hasPinchPoint = false; + int pinchIndexA = -1; + int pinchIndexB = -1; + for (int i = 0; i < pin.Count; ++i) + { + for (int j = i + 1; j < pin.Count; ++j) + { + //Don't worry about pinch points where the points + //are actually just dupe neighbors + if (Math.Abs(pin[i].X - pin[j].X) < tolerance && Math.Abs(pin[i].Y - pin[j].Y) < tolerance && j != i + 1) + { + pinchIndexA = i; + pinchIndexB = j; + hasPinchPoint = true; + break; + } + } + if (hasPinchPoint) break; + } + if (hasPinchPoint) + { + int sizeA = pinchIndexB - pinchIndexA; + if (sizeA == pin.Count) return false; //has dupe points at wraparound, not a problem here + for (int i = 0; i < sizeA; ++i) + { + int ind = Remainder(pinchIndexA + i, pin.Count); // is this right + poutA.Add(pin[ind]); + } + + int sizeB = pin.Count - sizeA; + for (int i = 0; i < sizeB; ++i) + { + int ind = Remainder(pinchIndexB + i, pin.Count); // is this right + poutB.Add(pin[ind]); + } + } + return hasPinchPoint; + } + + /// + /// Fix for obnoxious behavior for the % operator for negative numbers... + /// + /// The x. + /// The modulus. + /// + private static int Remainder(int x, int modulus) + { + int rem = x % modulus; + while (rem < 0) + { + rem += modulus; + } + return rem; + } + + /// + /// Checks if vertex i is the tip of an ear in polygon defined by xv[] and yv[]. + /// + /// The i. + /// The xv. + /// The yv. + /// Length of the xv. + /// + /// Assumes clockwise orientation of polygon. + /// + /// + /// true if the specified i is ear; otherwise, false. + /// + private static bool IsEar(int i, float[] xv, float[] yv, int xvLength) + { + float dx0, dy0, dx1, dy1; + if (i >= xvLength || i < 0 || xvLength < 3) + { + return false; + } + int upper = i + 1; + int lower = i - 1; + if (i == 0) + { + dx0 = xv[0] - xv[xvLength - 1]; + dy0 = yv[0] - yv[xvLength - 1]; + dx1 = xv[1] - xv[0]; + dy1 = yv[1] - yv[0]; + lower = xvLength - 1; + } + else if (i == xvLength - 1) + { + dx0 = xv[i] - xv[i - 1]; + dy0 = yv[i] - yv[i - 1]; + dx1 = xv[0] - xv[i]; + dy1 = yv[0] - yv[i]; + upper = 0; + } + else + { + dx0 = xv[i] - xv[i - 1]; + dy0 = yv[i] - yv[i - 1]; + dx1 = xv[i + 1] - xv[i]; + dy1 = yv[i + 1] - yv[i]; + } + + float cross = dx0 * dy1 - dx1 * dy0; + + if (cross > 0) + return false; + + Triangle myTri = new Triangle(xv[i], yv[i], xv[upper], yv[upper], xv[lower], yv[lower]); + + for (int j = 0; j < xvLength; ++j) + { + if (j == i || j == lower || j == upper) + continue; + if (myTri.IsInside(xv[j], yv[j])) + return false; + } + return true; + } + + private class Triangle : Vertices + { + //Constructor automatically fixes orientation to ccw + public Triangle(float x1, float y1, float x2, float y2, float x3, float y3) + { + float cross = (x2 - x1) * (y3 - y1) - (x3 - x1) * (y2 - y1); + if (cross > 0) + { + Add(new Vector2(x1, y1)); + Add(new Vector2(x2, y2)); + Add(new Vector2(x3, y3)); + } + else + { + Add(new Vector2(x1, y1)); + Add(new Vector2(x3, y3)); + Add(new Vector2(x2, y2)); + } + } + + public bool IsInside(float x, float y) + { + Vector2 a = this[0]; + Vector2 b = this[1]; + Vector2 c = this[2]; + + if (x < a.X && x < b.X && x < c.X) return false; + if (x > a.X && x > b.X && x > c.X) return false; + if (y < a.Y && y < b.Y && y < c.Y) return false; + if (y > a.Y && y > b.Y && y > c.Y) return false; + + float vx2 = x - a.X; + float vy2 = y - a.Y; + float vx1 = b.X - a.X; + float vy1 = b.Y - a.Y; + float vx0 = c.X - a.X; + float vy0 = c.Y - a.Y; + + float dot00 = vx0 * vx0 + vy0 * vy0; + float dot01 = vx0 * vx1 + vy0 * vy1; + float dot02 = vx0 * vx2 + vy0 * vy2; + float dot11 = vx1 * vx1 + vy1 * vy1; + float dot12 = vx1 * vx2 + vy1 * vy2; + float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01); + float u = (dot11 * dot02 - dot01 * dot12) * invDenom; + float v = (dot00 * dot12 - dot01 * dot02) * invDenom; + + return ((u > 0) && (v > 0) && (u + v < 1)); + } + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/FlipcodeDecomposer.cs b/src/Box2DNet/Common/Decomposition/FlipcodeDecomposer.cs new file mode 100644 index 0000000..1d3df2d --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/FlipcodeDecomposer.cs @@ -0,0 +1,152 @@ +using System.Collections.Generic; +using System.Diagnostics; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Common.Decomposition +{ + /// + /// Convex decomposition algorithm created by unknown + /// + /// Properties: + /// - No support for holes + /// - Very fast + /// - Only works on simple polygons + /// - Only works on counter clockwise polygons + /// + /// More information: http://www.flipcode.com/archives/Efficient_Polygon_Triangulation.shtml + /// + internal static class FlipcodeDecomposer + { + private static Vector2 _tmpA; + private static Vector2 _tmpB; + private static Vector2 _tmpC; + + /// + /// Decompose the polygon into triangles. + /// + /// Properties: + /// - Only works on counter clockwise polygons + /// + /// + /// The list of points describing the polygon + public static List ConvexPartition(Vertices vertices) + { + Debug.Assert(vertices.Count > 3); + Debug.Assert(vertices.IsCounterClockWise()); + + int[] polygon = new int[vertices.Count]; + + for (int v = 0; v < vertices.Count; v++) + polygon[v] = v; + + int nv = vertices.Count; + + // Remove nv-2 Vertices, creating 1 triangle every time + int count = 2 * nv; /* error detection */ + + List result = new List(); + + for (int v = nv - 1; nv > 2; ) + { + // If we loop, it is probably a non-simple polygon + if (0 >= (count--)) + { + // Triangulate: ERROR - probable bad polygon! + return new List(); + } + + // Three consecutive vertices in current polygon, + int u = v; + if (nv <= u) + u = 0; // Previous + v = u + 1; + if (nv <= v) + v = 0; // New v + int w = v + 1; + if (nv <= w) + w = 0; // Next + + _tmpA = vertices[polygon[u]]; + _tmpB = vertices[polygon[v]]; + _tmpC = vertices[polygon[w]]; + + if (Snip(vertices, u, v, w, nv, polygon)) + { + int s, t; + + // Output Triangle + Vertices triangle = new Vertices(3); + triangle.Add(_tmpA); + triangle.Add(_tmpB); + triangle.Add(_tmpC); + result.Add(triangle); + + // Remove v from remaining polygon + for (s = v, t = v + 1; t < nv; s++, t++) + { + polygon[s] = polygon[t]; + } + nv--; + + // Reset error detection counter + count = 2 * nv; + } + } + + return result; + } + + /// + /// Check if the point P is inside the triangle defined by + /// the points A, B, C + /// + /// The A point. + /// The B point. + /// The C point. + /// The point to be tested. + /// True if the point is inside the triangle + private static bool InsideTriangle(ref Vector2 a, ref Vector2 b, ref Vector2 c, ref Vector2 p) + { + //A cross bp + float abp = (c.X - b.X) * (p.Y - b.Y) - (c.Y - b.Y) * (p.X - b.X); + + //A cross ap + float aap = (b.X - a.X) * (p.Y - a.Y) - (b.Y - a.Y) * (p.X - a.X); + + //b cross cp + float bcp = (a.X - c.X) * (p.Y - c.Y) - (a.Y - c.Y) * (p.X - c.X); + + return ((abp >= 0.0f) && (bcp >= 0.0f) && (aap >= 0.0f)); + } + + /// + /// Cut a the contour and add a triangle into V to describe the + /// location of the cut + /// + /// The list of points defining the polygon + /// The index of the first point + /// The index of the second point + /// The index of the third point + /// The number of elements in the array. + /// The array to populate with indicies of triangles. + /// True if a triangle was found + private static bool Snip(Vertices contour, int u, int v, int w, int n, int[] V) + { + if (Settings.Epsilon > MathUtils.Area(ref _tmpA, ref _tmpB, ref _tmpC)) + return false; + + for (int p = 0; p < n; p++) + { + if ((p == u) || (p == v) || (p == w)) + continue; + + Vector2 point = contour[V[p]]; + + if (InsideTriangle(ref _tmpA, ref _tmpB, ref _tmpC, ref point)) + return false; + } + + return true; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/Seidel/Edge.cs b/src/Box2DNet/Common/Decomposition/Seidel/Edge.cs new file mode 100644 index 0000000..4bf01b8 --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/Seidel/Edge.cs @@ -0,0 +1,60 @@ +using System.Collections.Generic; + +namespace Box2DNet.Common.Decomposition.Seidel +{ + internal class Edge + { + // Pointers used for building trapezoidal map + public Trapezoid Above; + public float B; + public Trapezoid Below; + + // Montone mountain points + public HashSet MPoints; + public Point P; + public Point Q; + + // Slope of the line (m) + public float Slope; + + + public Edge(Point p, Point q) + { + P = p; + Q = q; + + if (q.X - p.X != 0) + Slope = (q.Y - p.Y) / (q.X - p.X); + else + Slope = 0; + + B = p.Y - (p.X * Slope); + Above = null; + Below = null; + MPoints = new HashSet(); + MPoints.Add(p); + MPoints.Add(q); + } + + public bool IsAbove(Point point) + { + return P.Orient2D(Q, point) < 0; + } + + public bool IsBelow(Point point) + { + return P.Orient2D(Q, point) > 0; + } + + public void AddMpoint(Point point) + { + foreach (Point mp in MPoints) + { + if (!mp.Neq(point)) + return; + } + + MPoints.Add(point); + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/Seidel/MonotoneMountain.cs b/src/Box2DNet/Common/Decomposition/Seidel/MonotoneMountain.cs new file mode 100644 index 0000000..283fce7 --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/Seidel/MonotoneMountain.cs @@ -0,0 +1,166 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; + +namespace Box2DNet.Common.Decomposition.Seidel +{ + internal class MonotoneMountain + { + // Almost Pi! + private const float PiSlop = 3.1f; + + // Triangles that constitute the mountain + public List> Triangles; + private HashSet _convexPoints; + private Point _head; + + // Monotone mountain points + private List _monoPoly; + + // Used to track which side of the line we are on + private bool _positive; + private int _size; + private Point _tail; + + public MonotoneMountain() + { + _size = 0; + _tail = null; + _head = null; + _positive = false; + _convexPoints = new HashSet(); + _monoPoly = new List(); + Triangles = new List>(); + } + + // Append a point to the list + public void Add(Point point) + { + if (_size == 0) + { + _head = point; + _size = 1; + } + else if (_size == 1) + { + // Keep repeat points out of the list + _tail = point; + _tail.Prev = _head; + _head.Next = _tail; + _size = 2; + } + else + { + // Keep repeat points out of the list + _tail.Next = point; + point.Prev = _tail; + _tail = point; + _size += 1; + } + } + + // Remove a point from the list + public void Remove(Point point) + { + Point next = point.Next; + Point prev = point.Prev; + point.Prev.Next = next; + point.Next.Prev = prev; + _size -= 1; + } + + // Partition a x-monotone mountain into triangles O(n) + // See "Computational Geometry in C", 2nd edition, by Joseph O'Rourke, page 52 + public void Process() + { + // Establish the proper sign + _positive = AngleSign(); + // create monotone polygon - for dubug purposes + GenMonoPoly(); + + // Initialize internal angles at each nonbase vertex + // Link strictly convex vertices into a list, ignore reflex vertices + Point p = _head.Next; + while (p.Neq(_tail)) + { + float a = Angle(p); + // If the point is almost colinear with it's neighbor, remove it! + if (a >= PiSlop || a <= -PiSlop || a == 0.0f) + Remove(p); + else if (IsConvex(p)) + _convexPoints.Add(p); + p = p.Next; + } + + Triangulate(); + } + + private void Triangulate() + { + while (_convexPoints.Count != 0) + { + IEnumerator e = _convexPoints.GetEnumerator(); + e.MoveNext(); + Point ear = e.Current; + + _convexPoints.Remove(ear); + Point a = ear.Prev; + Point b = ear; + Point c = ear.Next; + List triangle = new List(3); + triangle.Add(a); + triangle.Add(b); + triangle.Add(c); + + Triangles.Add(triangle); + + // Remove ear, update angles and convex list + Remove(ear); + if (Valid(a)) + _convexPoints.Add(a); + if (Valid(c)) + _convexPoints.Add(c); + } + + Debug.Assert(_size <= 3, "Triangulation bug, please report"); + } + + private bool Valid(Point p) + { + return p.Neq(_head) && p.Neq(_tail) && IsConvex(p); + } + + // Create the monotone polygon + private void GenMonoPoly() + { + Point p = _head; + while (p != null) + { + _monoPoly.Add(p); + p = p.Next; + } + } + + private float Angle(Point p) + { + Point a = (p.Next - p); + Point b = (p.Prev - p); + return (float)Math.Atan2(a.Cross(b), a.Dot(b)); + } + + private bool AngleSign() + { + Point a = (_head.Next - _head); + Point b = (_tail - _head); + return Math.Atan2(a.Cross(b), a.Dot(b)) >= 0; + } + + // Determines if the inslide angle is convex or reflex + private bool IsConvex(Point p) + { + if (_positive != (Angle(p) >= 0)) + return false; + return true; + } + } +} diff --git a/src/Box2DNet/Common/Decomposition/Seidel/Node.cs b/src/Box2DNet/Common/Decomposition/Seidel/Node.cs new file mode 100644 index 0000000..d082353 --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/Seidel/Node.cs @@ -0,0 +1,41 @@ +using System.Collections.Generic; + +namespace Box2DNet.Common.Decomposition.Seidel +{ + // Node for a Directed Acyclic graph (DAG) + internal abstract class Node + { + protected Node LeftChild; + public List ParentList; + protected Node RightChild; + + protected Node(Node left, Node right) + { + ParentList = new List(); + LeftChild = left; + RightChild = right; + + if (left != null) + left.ParentList.Add(this); + if (right != null) + right.ParentList.Add(this); + } + + public abstract Sink Locate(Edge s); + + // Replace a node in the graph with this node + // Make sure parent pointers are updated + public void Replace(Node node) + { + foreach (Node parent in node.ParentList) + { + // Select the correct node to replace (left or right child) + if (parent.LeftChild == node) + parent.LeftChild = this; + else + parent.RightChild = this; + } + ParentList.AddRange(node.ParentList); + } + } +} diff --git a/src/Box2DNet/Common/Decomposition/Seidel/Point.cs b/src/Box2DNet/Common/Decomposition/Seidel/Point.cs new file mode 100644 index 0000000..c82ce8d --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/Seidel/Point.cs @@ -0,0 +1,61 @@ +namespace Box2DNet.Common.Decomposition.Seidel +{ + internal class Point + { + // Pointers to next and previous points in Monontone Mountain + public Point Next, Prev; + public float X, Y; + + public Point(float x, float y) + { + X = x; + Y = y; + Next = null; + Prev = null; + } + + public static Point operator -(Point p1, Point p2) + { + return new Point(p1.X - p2.X, p1.Y - p2.Y); + } + + public static Point operator +(Point p1, Point p2) + { + return new Point(p1.X + p2.X, p1.Y + p2.Y); + } + + public static Point operator -(Point p1, float f) + { + return new Point(p1.X - f, p1.Y - f); + } + + public static Point operator +(Point p1, float f) + { + return new Point(p1.X + f, p1.Y + f); + } + + public float Cross(Point p) + { + return X * p.Y - Y * p.X; + } + + public float Dot(Point p) + { + return X * p.X + Y * p.Y; + } + + public bool Neq(Point p) + { + return p.X != X || p.Y != Y; + } + + public float Orient2D(Point pb, Point pc) + { + float acx = X - pc.X; + float bcx = pb.X - pc.X; + float acy = Y - pc.Y; + float bcy = pb.Y - pc.Y; + return acx * bcy - acy * bcx; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/Seidel/QueryGraph.cs b/src/Box2DNet/Common/Decomposition/Seidel/QueryGraph.cs new file mode 100644 index 0000000..0dbf2ca --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/Seidel/QueryGraph.cs @@ -0,0 +1,78 @@ +using System.Collections.Generic; + +namespace Box2DNet.Common.Decomposition.Seidel +{ + // Directed Acyclic graph (DAG) + // See "Computational Geometry", 3rd edition, by Mark de Berg et al, Chapter 6.2 + internal class QueryGraph + { + private Node _head; + + public QueryGraph(Node head) + { + _head = head; + } + + private Trapezoid Locate(Edge edge) + { + return _head.Locate(edge).Trapezoid; + } + + public List FollowEdge(Edge edge) + { + List trapezoids = new List(); + trapezoids.Add(Locate(edge)); + int j = 0; + + while (edge.Q.X > trapezoids[j].RightPoint.X) + { + if (edge.IsAbove(trapezoids[j].RightPoint)) + { + trapezoids.Add(trapezoids[j].UpperRight); + } + else + { + trapezoids.Add(trapezoids[j].LowerRight); + } + j += 1; + } + return trapezoids; + } + + private void Replace(Sink sink, Node node) + { + if (sink.ParentList.Count == 0) + _head = node; + else + node.Replace(sink); + } + + public void Case1(Sink sink, Edge edge, Trapezoid[] tList) + { + YNode yNode = new YNode(edge, Sink.Isink(tList[1]), Sink.Isink(tList[2])); + XNode qNode = new XNode(edge.Q, yNode, Sink.Isink(tList[3])); + XNode pNode = new XNode(edge.P, Sink.Isink(tList[0]), qNode); + Replace(sink, pNode); + } + + public void Case2(Sink sink, Edge edge, Trapezoid[] tList) + { + YNode yNode = new YNode(edge, Sink.Isink(tList[1]), Sink.Isink(tList[2])); + XNode pNode = new XNode(edge.P, Sink.Isink(tList[0]), yNode); + Replace(sink, pNode); + } + + public void Case3(Sink sink, Edge edge, Trapezoid[] tList) + { + YNode yNode = new YNode(edge, Sink.Isink(tList[0]), Sink.Isink(tList[1])); + Replace(sink, yNode); + } + + public void Case4(Sink sink, Edge edge, Trapezoid[] tList) + { + YNode yNode = new YNode(edge, Sink.Isink(tList[0]), Sink.Isink(tList[1])); + XNode qNode = new XNode(edge.Q, yNode, Sink.Isink(tList[2])); + Replace(sink, qNode); + } + } +} diff --git a/src/Box2DNet/Common/Decomposition/Seidel/Sink.cs b/src/Box2DNet/Common/Decomposition/Seidel/Sink.cs new file mode 100644 index 0000000..c7e7177 --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/Seidel/Sink.cs @@ -0,0 +1,27 @@ +namespace Box2DNet.Common.Decomposition.Seidel +{ + internal class Sink : Node + { + public Trapezoid Trapezoid; + + private Sink(Trapezoid trapezoid) + : base(null, null) + { + Trapezoid = trapezoid; + trapezoid.Sink = this; + } + + public static Sink Isink(Trapezoid trapezoid) + { + if (trapezoid.Sink == null) + return new Sink(trapezoid); + + return trapezoid.Sink; + } + + public override Sink Locate(Edge edge) + { + return this; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/Seidel/Trapezoid.cs b/src/Box2DNet/Common/Decomposition/Seidel/Trapezoid.cs new file mode 100644 index 0000000..2478de7 --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/Seidel/Trapezoid.cs @@ -0,0 +1,123 @@ +using System.Collections.Generic; + +namespace Box2DNet.Common.Decomposition.Seidel +{ + internal class Trapezoid + { + public Edge Bottom; + public bool Inside; + public Point LeftPoint; + + // Neighbor pointers + public Trapezoid LowerLeft; + public Trapezoid LowerRight; + + public Point RightPoint; + public Sink Sink; + + public Edge Top; + public Trapezoid UpperLeft; + public Trapezoid UpperRight; + + public Trapezoid(Point leftPoint, Point rightPoint, Edge top, Edge bottom) + { + LeftPoint = leftPoint; + RightPoint = rightPoint; + Top = top; + Bottom = bottom; + UpperLeft = null; + UpperRight = null; + LowerLeft = null; + LowerRight = null; + Inside = true; + Sink = null; + } + + // Update neighbors to the left + public void UpdateLeft(Trapezoid ul, Trapezoid ll) + { + UpperLeft = ul; + if (ul != null) ul.UpperRight = this; + LowerLeft = ll; + if (ll != null) ll.LowerRight = this; + } + + // Update neighbors to the right + public void UpdateRight(Trapezoid ur, Trapezoid lr) + { + UpperRight = ur; + if (ur != null) ur.UpperLeft = this; + LowerRight = lr; + if (lr != null) lr.LowerLeft = this; + } + + // Update neighbors on both sides + public void UpdateLeftRight(Trapezoid ul, Trapezoid ll, Trapezoid ur, Trapezoid lr) + { + UpperLeft = ul; + if (ul != null) ul.UpperRight = this; + LowerLeft = ll; + if (ll != null) ll.LowerRight = this; + UpperRight = ur; + if (ur != null) ur.UpperLeft = this; + LowerRight = lr; + if (lr != null) lr.LowerLeft = this; + } + + // Recursively trim outside neighbors + public void TrimNeighbors() + { + if (Inside) + { + Inside = false; + if (UpperLeft != null) UpperLeft.TrimNeighbors(); + if (LowerLeft != null) LowerLeft.TrimNeighbors(); + if (UpperRight != null) UpperRight.TrimNeighbors(); + if (LowerRight != null) LowerRight.TrimNeighbors(); + } + } + + // Determines if this point lies inside the trapezoid + public bool Contains(Point point) + { + return (point.X > LeftPoint.X && point.X < RightPoint.X && Top.IsAbove(point) && Bottom.IsBelow(point)); + } + + public List GetVertices() + { + List verts = new List(4); + verts.Add(LineIntersect(Top, LeftPoint.X)); + verts.Add(LineIntersect(Bottom, LeftPoint.X)); + verts.Add(LineIntersect(Bottom, RightPoint.X)); + verts.Add(LineIntersect(Top, RightPoint.X)); + return verts; + } + + private Point LineIntersect(Edge edge, float x) + { + float y = edge.Slope * x + edge.B; + return new Point(x, y); + } + + // Add points to monotone mountain + public void AddPoints() + { + if (LeftPoint != Bottom.P) + { + Bottom.AddMpoint(LeftPoint); + } + if (RightPoint != Bottom.Q) + { + Bottom.AddMpoint(RightPoint); + } + if (LeftPoint != Top.P) + { + Top.AddMpoint(LeftPoint); + } + if (RightPoint != Top.Q) + { + Top.AddMpoint(RightPoint); + } + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/Seidel/TrapezoidalMap.cs b/src/Box2DNet/Common/Decomposition/Seidel/TrapezoidalMap.cs new file mode 100644 index 0000000..d8335b4 --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/Seidel/TrapezoidalMap.cs @@ -0,0 +1,195 @@ +using System.Collections.Generic; + +namespace Box2DNet.Common.Decomposition.Seidel +{ + internal class TrapezoidalMap + { + // Trapezoid container + public HashSet Map; + + // Bottom segment that spans multiple trapezoids + private Edge _bCross; + + // Top segment that spans multiple trapezoids + private Edge _cross; + + // AABB margin + private float _margin; + + public TrapezoidalMap() + { + Map = new HashSet(); + _margin = 50.0f; + _bCross = null; + _cross = null; + } + + public void Clear() + { + _bCross = null; + _cross = null; + } + + // Case 1: segment completely enclosed by trapezoid + // break trapezoid into 4 smaller trapezoids + public Trapezoid[] Case1(Trapezoid t, Edge e) + { + Trapezoid[] trapezoids = new Trapezoid[4]; + trapezoids[0] = new Trapezoid(t.LeftPoint, e.P, t.Top, t.Bottom); + trapezoids[1] = new Trapezoid(e.P, e.Q, t.Top, e); + trapezoids[2] = new Trapezoid(e.P, e.Q, e, t.Bottom); + trapezoids[3] = new Trapezoid(e.Q, t.RightPoint, t.Top, t.Bottom); + + trapezoids[0].UpdateLeft(t.UpperLeft, t.LowerLeft); + trapezoids[1].UpdateLeftRight(trapezoids[0], null, trapezoids[3], null); + trapezoids[2].UpdateLeftRight(null, trapezoids[0], null, trapezoids[3]); + trapezoids[3].UpdateRight(t.UpperRight, t.LowerRight); + + return trapezoids; + } + + // Case 2: Trapezoid contains point p, q lies outside + // break trapezoid into 3 smaller trapezoids + public Trapezoid[] Case2(Trapezoid t, Edge e) + { + Point rp; + if (e.Q.X == t.RightPoint.X) + rp = e.Q; + else + rp = t.RightPoint; + + Trapezoid[] trapezoids = new Trapezoid[3]; + trapezoids[0] = new Trapezoid(t.LeftPoint, e.P, t.Top, t.Bottom); + trapezoids[1] = new Trapezoid(e.P, rp, t.Top, e); + trapezoids[2] = new Trapezoid(e.P, rp, e, t.Bottom); + + trapezoids[0].UpdateLeft(t.UpperLeft, t.LowerLeft); + trapezoids[1].UpdateLeftRight(trapezoids[0], null, t.UpperRight, null); + trapezoids[2].UpdateLeftRight(null, trapezoids[0], null, t.LowerRight); + + _bCross = t.Bottom; + _cross = t.Top; + + e.Above = trapezoids[1]; + e.Below = trapezoids[2]; + + return trapezoids; + } + + // Case 3: Trapezoid is bisected + public Trapezoid[] Case3(Trapezoid t, Edge e) + { + Point lp; + if (e.P.X == t.LeftPoint.X) + lp = e.P; + else + lp = t.LeftPoint; + + Point rp; + if (e.Q.X == t.RightPoint.X) + rp = e.Q; + else + rp = t.RightPoint; + + Trapezoid[] trapezoids = new Trapezoid[2]; + + if (_cross == t.Top) + { + trapezoids[0] = t.UpperLeft; + trapezoids[0].UpdateRight(t.UpperRight, null); + trapezoids[0].RightPoint = rp; + } + else + { + trapezoids[0] = new Trapezoid(lp, rp, t.Top, e); + trapezoids[0].UpdateLeftRight(t.UpperLeft, e.Above, t.UpperRight, null); + } + + if (_bCross == t.Bottom) + { + trapezoids[1] = t.LowerLeft; + trapezoids[1].UpdateRight(null, t.LowerRight); + trapezoids[1].RightPoint = rp; + } + else + { + trapezoids[1] = new Trapezoid(lp, rp, e, t.Bottom); + trapezoids[1].UpdateLeftRight(e.Below, t.LowerLeft, null, t.LowerRight); + } + + _bCross = t.Bottom; + _cross = t.Top; + + e.Above = trapezoids[0]; + e.Below = trapezoids[1]; + + return trapezoids; + } + + // Case 4: Trapezoid contains point q, p lies outside + // break trapezoid into 3 smaller trapezoids + public Trapezoid[] Case4(Trapezoid t, Edge e) + { + Point lp; + if (e.P.X == t.LeftPoint.X) + lp = e.P; + else + lp = t.LeftPoint; + + Trapezoid[] trapezoids = new Trapezoid[3]; + + if (_cross == t.Top) + { + trapezoids[0] = t.UpperLeft; + trapezoids[0].RightPoint = e.Q; + } + else + { + trapezoids[0] = new Trapezoid(lp, e.Q, t.Top, e); + trapezoids[0].UpdateLeft(t.UpperLeft, e.Above); + } + + if (_bCross == t.Bottom) + { + trapezoids[1] = t.LowerLeft; + trapezoids[1].RightPoint = e.Q; + } + else + { + trapezoids[1] = new Trapezoid(lp, e.Q, e, t.Bottom); + trapezoids[1].UpdateLeft(e.Below, t.LowerLeft); + } + + trapezoids[2] = new Trapezoid(e.Q, t.RightPoint, t.Top, t.Bottom); + trapezoids[2].UpdateLeftRight(trapezoids[0], trapezoids[1], t.UpperRight, t.LowerRight); + + return trapezoids; + } + + // Create an AABB around segments + public Trapezoid BoundingBox(List edges) + { + Point max = edges[0].P + _margin; + Point min = edges[0].Q - _margin; + + foreach (Edge e in edges) + { + if (e.P.X > max.X) max = new Point(e.P.X + _margin, max.Y); + if (e.P.Y > max.Y) max = new Point(max.X, e.P.Y + _margin); + if (e.Q.X > max.X) max = new Point(e.Q.X + _margin, max.Y); + if (e.Q.Y > max.Y) max = new Point(max.X, e.Q.Y + _margin); + if (e.P.X < min.X) min = new Point(e.P.X - _margin, min.Y); + if (e.P.Y < min.Y) min = new Point(min.X, e.P.Y - _margin); + if (e.Q.X < min.X) min = new Point(e.Q.X - _margin, min.Y); + if (e.Q.Y < min.Y) min = new Point(min.X, e.Q.Y - _margin); + } + + Edge top = new Edge(new Point(min.X, max.Y), new Point(max.X, max.Y)); + Edge bottom = new Edge(new Point(min.X, min.Y), new Point(max.X, min.Y)); + Point left = bottom.P; + Point right = top.Q; + + return new Trapezoid(left, right, top, bottom); + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/Seidel/Triangulator.cs b/src/Box2DNet/Common/Decomposition/Seidel/Triangulator.cs new file mode 100644 index 0000000..5cad47d --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/Seidel/Triangulator.cs @@ -0,0 +1,203 @@ +using System; +using System.Collections.Generic; + +namespace Box2DNet.Common.Decomposition.Seidel +{ + internal class Triangulator + { + // Trapezoid decomposition list + public List Trapezoids; + public List> Triangles; + + // Initialize trapezoidal map and query structure + private Trapezoid _boundingBox; + private List _edgeList; + private QueryGraph _queryGraph; + private float _sheer = 0.001f; + private TrapezoidalMap _trapezoidalMap; + private List _xMonoPoly; + + public Triangulator(List polyLine, float sheer) + { + _sheer = sheer; + Triangles = new List>(); + Trapezoids = new List(); + _xMonoPoly = new List(); + _edgeList = InitEdges(polyLine); + _trapezoidalMap = new TrapezoidalMap(); + _boundingBox = _trapezoidalMap.BoundingBox(_edgeList); + _queryGraph = new QueryGraph(Sink.Isink(_boundingBox)); + + Process(); + } + + // Build the trapezoidal map and query graph + private void Process() + { + foreach (Edge edge in _edgeList) + { + List traps = _queryGraph.FollowEdge(edge); + + // Remove trapezoids from trapezoidal Map + foreach (Trapezoid t in traps) + { + _trapezoidalMap.Map.Remove(t); + + bool cp = t.Contains(edge.P); + bool cq = t.Contains(edge.Q); + Trapezoid[] tList; + + if (cp && cq) + { + tList = _trapezoidalMap.Case1(t, edge); + _queryGraph.Case1(t.Sink, edge, tList); + } + else if (cp && !cq) + { + tList = _trapezoidalMap.Case2(t, edge); + _queryGraph.Case2(t.Sink, edge, tList); + } + else if (!cp && !cq) + { + tList = _trapezoidalMap.Case3(t, edge); + _queryGraph.Case3(t.Sink, edge, tList); + } + else + { + tList = _trapezoidalMap.Case4(t, edge); + _queryGraph.Case4(t.Sink, edge, tList); + } + // Add new trapezoids to map + foreach (Trapezoid y in tList) + { + _trapezoidalMap.Map.Add(y); + } + } + _trapezoidalMap.Clear(); + } + + // Mark outside trapezoids + foreach (Trapezoid t in _trapezoidalMap.Map) + { + MarkOutside(t); + } + + // Collect interior trapezoids + foreach (Trapezoid t in _trapezoidalMap.Map) + { + if (t.Inside) + { + Trapezoids.Add(t); + t.AddPoints(); + } + } + + // Generate the triangles + CreateMountains(); + } + + // Build a list of x-monotone mountains + private void CreateMountains() + { + foreach (Edge edge in _edgeList) + { + if (edge.MPoints.Count > 2) + { + MonotoneMountain mountain = new MonotoneMountain(); + + // Sorting is a perfromance hit. Literature says this can be accomplised in + // linear time, although I don't see a way around using traditional methods + // when using a randomized incremental algorithm + + // Insertion sort is one of the fastest algorithms for sorting arrays containing + // fewer than ten elements, or for lists that are already mostly sorted. + + List points = new List(edge.MPoints); + points.Sort((p1, p2) => p1.X.CompareTo(p2.X)); + + foreach (Point p in points) + mountain.Add(p); + + // Triangulate monotone mountain + mountain.Process(); + + // Extract the triangles into a single list + foreach (List t in mountain.Triangles) + { + Triangles.Add(t); + } + + _xMonoPoly.Add(mountain); + } + } + } + + // Mark the outside trapezoids surrounding the polygon + private void MarkOutside(Trapezoid t) + { + if (t.Top == _boundingBox.Top || t.Bottom == _boundingBox.Bottom) + t.TrimNeighbors(); + } + + // Create segments and connect end points; update edge event pointer + private List InitEdges(List points) + { + List edges = new List(); + + for (int i = 0; i < points.Count - 1; i++) + { + edges.Add(new Edge(points[i], points[i + 1])); + } + edges.Add(new Edge(points[0], points[points.Count - 1])); + return OrderSegments(edges); + } + + private List OrderSegments(List edgeInput) + { + // Ignore vertical segments! + List edges = new List(); + + foreach (Edge e in edgeInput) + { + Point p = ShearTransform(e.P); + Point q = ShearTransform(e.Q); + + // Point p must be to the left of point q + if (p.X > q.X) + { + edges.Add(new Edge(q, p)); + } + else if (p.X < q.X) + { + edges.Add(new Edge(p, q)); + } + } + + // Randomized triangulation improves performance + // See Seidel's paper, or O'Rourke's book, p. 57 + Shuffle(edges); + return edges; + } + + private static void Shuffle(IList list) + { + Random rng = new Random(); + int n = list.Count; + while (n > 1) + { + n--; + int k = rng.Next(n + 1); + T value = list[k]; + list[k] = list[n]; + list[n] = value; + } + } + + // Prevents any two distinct endpoints from lying on a common vertical line, and avoiding + // the degenerate case. See Mark de Berg et al, Chapter 6.3 + private Point ShearTransform(Point point) + { + return new Point(point.X + _sheer * point.Y, point.Y); + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/Seidel/XNode.cs b/src/Box2DNet/Common/Decomposition/Seidel/XNode.cs new file mode 100644 index 0000000..19a4abe --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/Seidel/XNode.cs @@ -0,0 +1,21 @@ +namespace Box2DNet.Common.Decomposition.Seidel +{ + internal class XNode : Node + { + private Point _point; + + public XNode(Point point, Node lChild, Node rChild) + : base(lChild, rChild) + { + _point = point; + } + + public override Sink Locate(Edge edge) + { + if (edge.P.X >= _point.X) + return RightChild.Locate(edge); // Move to the right in the graph + + return LeftChild.Locate(edge); // Move to the left in the graph + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/Seidel/YNode.cs b/src/Box2DNet/Common/Decomposition/Seidel/YNode.cs new file mode 100644 index 0000000..c1c6f26 --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/Seidel/YNode.cs @@ -0,0 +1,29 @@ +namespace Box2DNet.Common.Decomposition.Seidel +{ + internal class YNode : Node + { + private Edge _edge; + + public YNode(Edge edge, Node lChild, Node rChild) + : base(lChild, rChild) + { + _edge = edge; + } + + public override Sink Locate(Edge edge) + { + if (_edge.IsAbove(edge.P)) + return RightChild.Locate(edge); // Move down the graph + + if (_edge.IsBelow(edge.P)) + return LeftChild.Locate(edge); // Move up the graph + + // s and segment share the same endpoint, p + if (edge.Slope < _edge.Slope) + return RightChild.Locate(edge); // Move down the graph + + // Move up the graph + return LeftChild.Locate(edge); + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/SeidelDecomposer.cs b/src/Box2DNet/Common/Decomposition/SeidelDecomposer.cs new file mode 100644 index 0000000..9678ea8 --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/SeidelDecomposer.cs @@ -0,0 +1,109 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +*/ + +using System.Collections.Generic; +using System.Diagnostics; +using Box2DNet.Common.Decomposition.Seidel; +using Microsoft.Xna.Framework; +using Point = Box2DNet.Common.Decomposition.Seidel.Point; + +namespace Box2DNet.Common.Decomposition +{ + /// + /// Convex decomposition algorithm created by Raimund Seidel + /// + /// Properties: + /// - Decompose the polygon into trapezoids, then triangulate. + /// - To use the trapezoid data, use ConvexPartitionTrapezoid() + /// - Generate a lot of garbage due to incapsulation of the Poly2Tri library. + /// - Running time is O(n log n), n = number of vertices. + /// - Running time is almost linear for most simple polygons. + /// - Does not care about winding order. + /// + /// For more information, see Raimund Seidel's paper "A simple and fast incremental randomized + /// algorithm for computing trapezoidal decompositions and for triangulating polygons" + /// + /// See also: "Computational Geometry", 3rd edition, by Mark de Berg et al, Chapter 6.2 + /// "Computational Geometry in C", 2nd edition, by Joseph O'Rourke + /// + /// Original code from the Poly2Tri project by Mason Green. + /// http://code.google.com/p/poly2tri/source/browse?repo=archive#hg/scala/src/org/poly2tri/seidel + /// + /// This implementation is from Dec 14, 2010 + /// + internal static class SeidelDecomposer + { + /// + /// Decompose the polygon into several smaller non-concave polygons. + /// + /// The polygon to decompose. + /// The sheer to use if you get bad results, try using a higher value. + /// A list of triangles + public static List ConvexPartition(Vertices vertices, float sheer = 0.001f) + { + Debug.Assert(vertices.Count > 3); + + List compatList = new List(vertices.Count); + + foreach (Vector2 vertex in vertices) + { + compatList.Add(new Point(vertex.X, vertex.Y)); + } + + Triangulator t = new Triangulator(compatList, sheer); + + List list = new List(); + + foreach (List triangle in t.Triangles) + { + Vertices outTriangles = new Vertices(triangle.Count); + + foreach (Point outTriangle in triangle) + { + outTriangles.Add(new Vector2(outTriangle.X, outTriangle.Y)); + } + + list.Add(outTriangles); + } + + return list; + } + + /// + /// Decompose the polygon into several smaller non-concave polygons. + /// + /// The polygon to decompose. + /// The sheer to use if you get bad results, try using a higher value. + /// A list of trapezoids + public static List ConvexPartitionTrapezoid(Vertices vertices, float sheer = 0.001f) + { + List compatList = new List(vertices.Count); + + foreach (Vector2 vertex in vertices) + { + compatList.Add(new Point(vertex.X, vertex.Y)); + } + + Triangulator t = new Triangulator(compatList, sheer); + + List list = new List(); + + foreach (Trapezoid trapezoid in t.Trapezoids) + { + Vertices verts = new Vertices(); + + List points = trapezoid.GetVertices(); + foreach (Point point in points) + { + verts.Add(new Vector2(point.X, point.Y)); + } + + list.Add(verts); + } + + return list; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Decomposition/Triangulate.cs b/src/Box2DNet/Common/Decomposition/Triangulate.cs new file mode 100644 index 0000000..fca9c03 --- /dev/null +++ b/src/Box2DNet/Common/Decomposition/Triangulate.cs @@ -0,0 +1,172 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using Box2DNet.Common.ConvexHull; + +namespace Box2DNet.Common.Decomposition +{ + public enum TriangulationAlgorithm + { + /// + /// Convex decomposition algorithm using ear clipping + /// + /// Properties: + /// - Only works on simple polygons. + /// - Does not support holes. + /// - Running time is O(n^2), n = number of vertices. + /// + Earclip, + + /// + /// Convex decomposition algorithm created by Mark Bayazit (http://mnbayazit.com/) + /// + /// Properties: + /// - Tries to decompose using polygons instead of triangles. + /// - Tends to produce optimal results with low processing time. + /// - Running time is O(nr), n = number of vertices, r = reflex vertices. + /// - Does not support holes. + /// + Bayazit, + + /// + /// Convex decomposition algorithm created by unknown + /// + /// Properties: + /// - No support for holes + /// - Very fast + /// - Only works on simple polygons + /// - Only works on counter clockwise polygons + /// + Flipcode, + + /// + /// Convex decomposition algorithm created by Raimund Seidel + /// + /// Properties: + /// - Decompose the polygon into trapezoids, then triangulate. + /// - To use the trapezoid data, use ConvexPartitionTrapezoid() + /// - Generate a lot of garbage due to incapsulation of the Poly2Tri library. + /// - Running time is O(n log n), n = number of vertices. + /// - Running time is almost linear for most simple polygons. + /// - Does not care about winding order. + /// + Seidel, + SeidelTrapezoids, + + /// + /// 2D constrained Delaunay triangulation algorithm. + /// Based on the paper "Sweep-line algorithm for constrained Delaunay triangulation" by V. Domiter and and B. Zalik + /// + /// Properties: + /// - Creates triangles with a large interior angle. + /// - Supports holes + /// - Generate a lot of garbage due to incapsulation of the Poly2Tri library. + /// - Running time is O(n^2), n = number of vertices. + /// - Does not care about winding order. + /// + Delauny + } + + public static class Triangulate + { + public static List ConvexPartition(Vertices vertices, TriangulationAlgorithm algorithm, bool discardAndFixInvalid = true, float tolerance = 0.001f) + { + if (vertices.Count <= 3) + return new List { vertices }; + + List results; + + switch (algorithm) + { + case TriangulationAlgorithm.Earclip: + if (Settings.SkipSanityChecks) + Debug.Assert(!vertices.IsCounterClockWise(), "The Earclip algorithm expects the polygon to be clockwise."); + else + { + if (vertices.IsCounterClockWise()) + { + Vertices temp = new Vertices(vertices); + temp.Reverse(); + results = EarclipDecomposer.ConvexPartition(temp, tolerance); + } + else + results = EarclipDecomposer.ConvexPartition(vertices, tolerance); + } + break; + case TriangulationAlgorithm.Bayazit: + if (Settings.SkipSanityChecks) + Debug.Assert(vertices.IsCounterClockWise(), "The polygon is not counter clockwise. This is needed for Bayazit to work correctly."); + else + { + if (!vertices.IsCounterClockWise()) + { + Vertices temp = new Vertices(vertices); + temp.Reverse(); + results = BayazitDecomposer.ConvexPartition(temp); + } + else + results = BayazitDecomposer.ConvexPartition(vertices); + } + break; + case TriangulationAlgorithm.Flipcode: + if (Settings.SkipSanityChecks) + Debug.Assert(vertices.IsCounterClockWise(), "The polygon is not counter clockwise. This is needed for Bayazit to work correctly."); + else + { + if (!vertices.IsCounterClockWise()) + { + Vertices temp = new Vertices(vertices); + temp.Reverse(); + results = FlipcodeDecomposer.ConvexPartition(temp); + } + else + results = FlipcodeDecomposer.ConvexPartition(vertices); + } + break; + case TriangulationAlgorithm.Seidel: + results = SeidelDecomposer.ConvexPartition(vertices, tolerance); + break; + case TriangulationAlgorithm.SeidelTrapezoids: + results = SeidelDecomposer.ConvexPartitionTrapezoid(vertices, tolerance); + break; + case TriangulationAlgorithm.Delauny: + results = CDTDecomposer.ConvexPartition(vertices); + break; + default: + throw new ArgumentOutOfRangeException("algorithm"); + } + + if (discardAndFixInvalid) + { + for (int i = results.Count - 1; i >= 0; i--) + { + Vertices polygon = results[i]; + + if (!ValidatePolygon(polygon)) + results.RemoveAt(i); + } + } + + return results; + } + + private static bool ValidatePolygon(Vertices polygon) + { + PolygonError errorCode = polygon.CheckPolygon(); + + if (errorCode == PolygonError.InvalidAmountOfVertices || errorCode == PolygonError.AreaTooSmall || errorCode == PolygonError.SideTooSmall || errorCode == PolygonError.NotSimple) + return false; + + if (errorCode == PolygonError.NotCounterClockWise) //NotCounterCloseWise is the last check in CheckPolygon(), thus we don't need to call ValidatePolygon again. + polygon.Reverse(); + + if (errorCode == PolygonError.NotConvex) + { + polygon = GiftWrap.GetConvexHull(polygon); + return ValidatePolygon(polygon); + } + + return true; + } + } +} diff --git a/src/Box2DNet/Common/FixedArray.cs b/src/Box2DNet/Common/FixedArray.cs new file mode 100644 index 0000000..623a5d4 --- /dev/null +++ b/src/Box2DNet/Common/FixedArray.cs @@ -0,0 +1,224 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; + +namespace Box2DNet.Common +{ + public struct FixedArray2 + { + private T _value0; + private T _value1; + + public T this[int index] + { + get + { + switch (index) + { + case 0: + return _value0; + case 1: + return _value1; + default: + throw new IndexOutOfRangeException(); + } + } + set + { + switch (index) + { + case 0: + _value0 = value; + break; + case 1: + _value1 = value; + break; + default: + throw new IndexOutOfRangeException(); + } + } + } + } + + public struct FixedArray3 + { + private T _value0; + private T _value1; + private T _value2; + + public T this[int index] + { + get + { + switch (index) + { + case 0: + return _value0; + case 1: + return _value1; + case 2: + return _value2; + default: + throw new IndexOutOfRangeException(); + } + } + set + { + switch (index) + { + case 0: + _value0 = value; + break; + case 1: + _value1 = value; + break; + case 2: + _value2 = value; + break; + default: + throw new IndexOutOfRangeException(); + } + } + } + } + + public struct FixedArray4 + { + private T _value0; + private T _value1; + private T _value2; + private T _value3; + + public T this[int index] + { + get + { + switch (index) + { + case 0: + return _value0; + case 1: + return _value1; + case 2: + return _value2; + case 3: + return _value3; + default: + throw new IndexOutOfRangeException(); + } + } + set + { + switch (index) + { + case 0: + _value0 = value; + break; + case 1: + _value1 = value; + break; + case 2: + _value2 = value; + break; + case 3: + _value3 = value; + break; + default: + throw new IndexOutOfRangeException(); + } + } + } + } + + public struct FixedArray8 + { + private T _value0; + private T _value1; + private T _value2; + private T _value3; + private T _value4; + private T _value5; + private T _value6; + private T _value7; + + public T this[int index] + { + get + { + switch (index) + { + case 0: + return _value0; + case 1: + return _value1; + case 2: + return _value2; + case 3: + return _value3; + case 4: + return _value4; + case 5: + return _value5; + case 6: + return _value6; + case 7: + return _value7; + default: + throw new IndexOutOfRangeException(); + } + } + set + { + switch (index) + { + case 0: + _value0 = value; + break; + case 1: + _value1 = value; + break; + case 2: + _value2 = value; + break; + case 3: + _value3 = value; + break; + case 4: + _value4 = value; + break; + case 5: + _value5 = value; + break; + case 6: + _value6 = value; + break; + case 7: + _value7 = value; + break; + default: + throw new IndexOutOfRangeException(); + } + } + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/HashSet.cs b/src/Box2DNet/Common/HashSet.cs new file mode 100644 index 0000000..2ce561a --- /dev/null +++ b/src/Box2DNet/Common/HashSet.cs @@ -0,0 +1,78 @@ +#if WINDOWS_PHONE || XBOX + +using System.Collections; +using System.Collections.Generic; + +namespace Box2DNet.Common +{ + public class HashSet : ICollection + { + private Dictionary _dict; + + public HashSet(int capacity) + { + _dict = new Dictionary(capacity); + } + + public HashSet() + { + _dict = new Dictionary(); + } + + #region ICollection Members + + public void Add(T item) + { + // We don't care for the value in dictionary, only keys matter. + if (!_dict.ContainsKey(item)) + _dict.Add(item, 0); + } + + public void Clear() + { + _dict.Clear(); + } + + public bool Contains(T item) + { + return _dict.ContainsKey(item); + } + + public void CopyTo(T[] array, int arrayIndex) + { + foreach (var item in _dict.Keys) + { + array[arrayIndex++] = item; + } + } + + public bool Remove(T item) + { + return _dict.Remove(item); + } + + public IEnumerator GetEnumerator() + { + return _dict.Keys.GetEnumerator(); + } + + IEnumerator IEnumerable.GetEnumerator() + { + return _dict.Keys.GetEnumerator(); + } + + // Properties + public int Count + { + get { return _dict.Keys.Count; } + } + + public bool IsReadOnly + { + get { return false; } + } + + #endregion + } +} +#endif \ No newline at end of file diff --git a/src/Box2DNet/Common/LineTools.cs b/src/Box2DNet/Common/LineTools.cs new file mode 100644 index 0000000..f54e89b --- /dev/null +++ b/src/Box2DNet/Common/LineTools.cs @@ -0,0 +1,287 @@ +using System; +using Box2DNet.Collision; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Common +{ + /// + /// Collection of helper methods for misc collisions. + /// Does float tolerance and line collisions with lines and AABBs. + /// + public static class LineTools + { + public static float DistanceBetweenPointAndLineSegment(ref Vector2 point, ref Vector2 start, ref Vector2 end) + { + if (start == end) + return Vector2.Distance(point, start); + + Vector2 v = Vector2.Subtract(end, start); + Vector2 w = Vector2.Subtract(point, start); + + float c1 = Vector2.Dot(w, v); + if (c1 <= 0) return Vector2.Distance(point, start); + + float c2 = Vector2.Dot(v, v); + if (c2 <= c1) return Vector2.Distance(point, end); + + float b = c1 / c2; + Vector2 pointOnLine = Vector2.Add(start, Vector2.Multiply(v, b)); + return Vector2.Distance(point, pointOnLine); + } + + // From Eric Jordan's convex decomposition library + /// + ///Check if the lines a0->a1 and b0->b1 cross. + ///If they do, intersectionPoint will be filled + ///with the point of crossing. + /// + ///Grazing lines should not return true. + /// + /// + public static bool LineIntersect2(ref Vector2 a0, ref Vector2 a1, ref Vector2 b0, ref Vector2 b1, out Vector2 intersectionPoint) + { + intersectionPoint = Vector2.Zero; + + if (a0 == b0 || a0 == b1 || a1 == b0 || a1 == b1) + return false; + + float x1 = a0.X; + float y1 = a0.Y; + float x2 = a1.X; + float y2 = a1.Y; + float x3 = b0.X; + float y3 = b0.Y; + float x4 = b1.X; + float y4 = b1.Y; + + //AABB early exit + if (Math.Max(x1, x2) < Math.Min(x3, x4) || Math.Max(x3, x4) < Math.Min(x1, x2)) + return false; + + if (Math.Max(y1, y2) < Math.Min(y3, y4) || Math.Max(y3, y4) < Math.Min(y1, y2)) + return false; + + float ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3)); + float ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3)); + float denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1); + if (Math.Abs(denom) < Settings.Epsilon) + { + //Lines are too close to parallel to call + return false; + } + ua /= denom; + ub /= denom; + + if ((0 < ua) && (ua < 1) && (0 < ub) && (ub < 1)) + { + intersectionPoint.X = (x1 + ua * (x2 - x1)); + intersectionPoint.Y = (y1 + ua * (y2 - y1)); + return true; + } + + return false; + } + + //From Mark Bayazit's convex decomposition algorithm + public static Vector2 LineIntersect(Vector2 p1, Vector2 p2, Vector2 q1, Vector2 q2) + { + Vector2 i = Vector2.Zero; + float a1 = p2.Y - p1.Y; + float b1 = p1.X - p2.X; + float c1 = a1 * p1.X + b1 * p1.Y; + float a2 = q2.Y - q1.Y; + float b2 = q1.X - q2.X; + float c2 = a2 * q1.X + b2 * q1.Y; + float det = a1 * b2 - a2 * b1; + + if (!MathUtils.FloatEquals(det, 0)) + { + // lines are not parallel + i.X = (b2 * c1 - b1 * c2) / det; + i.Y = (a1 * c2 - a2 * c1) / det; + } + return i; + } + + /// + /// This method detects if two line segments (or lines) intersect, + /// and, if so, the point of intersection. Use the and + /// parameters to set whether the intersection point + /// must be on the first and second line segments. Setting these + /// both to true means you are doing a line-segment to line-segment + /// intersection. Setting one of them to true means you are doing a + /// line to line-segment intersection test, and so on. + /// Note: If two line segments are coincident, then + /// no intersection is detected (there are actually + /// infinite intersection points). + /// Author: Jeremy Bell + /// + /// The first point of the first line segment. + /// The second point of the first line segment. + /// The first point of the second line segment. + /// The second point of the second line segment. + /// This is set to the intersection + /// point if an intersection is detected. + /// Set this to true to require that the + /// intersection point be on the first line segment. + /// Set this to true to require that the + /// intersection point be on the second line segment. + /// True if an intersection is detected, false otherwise. + public static bool LineIntersect(ref Vector2 point1, ref Vector2 point2, ref Vector2 point3, ref Vector2 point4, bool firstIsSegment, bool secondIsSegment, out Vector2 point) + { + point = new Vector2(); + + // these are reused later. + // each lettered sub-calculation is used twice, except + // for b and d, which are used 3 times + float a = point4.Y - point3.Y; + float b = point2.X - point1.X; + float c = point4.X - point3.X; + float d = point2.Y - point1.Y; + + // denominator to solution of linear system + float denom = (a * b) - (c * d); + + // if denominator is 0, then lines are parallel + if (!(denom >= -Settings.Epsilon && denom <= Settings.Epsilon)) + { + float e = point1.Y - point3.Y; + float f = point1.X - point3.X; + float oneOverDenom = 1.0f / denom; + + // numerator of first equation + float ua = (c * e) - (a * f); + ua *= oneOverDenom; + + // check if intersection point of the two lines is on line segment 1 + if (!firstIsSegment || ua >= 0.0f && ua <= 1.0f) + { + // numerator of second equation + float ub = (b * e) - (d * f); + ub *= oneOverDenom; + + // check if intersection point of the two lines is on line segment 2 + // means the line segments intersect, since we know it is on + // segment 1 as well. + if (!secondIsSegment || ub >= 0.0f && ub <= 1.0f) + { + // check if they are coincident (no collision in this case) + if (ua != 0f || ub != 0f) + { + //There is an intersection + point.X = point1.X + ua * b; + point.Y = point1.Y + ua * d; + return true; + } + } + } + } + + return false; + } + + /// + /// This method detects if two line segments (or lines) intersect, + /// and, if so, the point of intersection. Use the and + /// parameters to set whether the intersection point + /// must be on the first and second line segments. Setting these + /// both to true means you are doing a line-segment to line-segment + /// intersection. Setting one of them to true means you are doing a + /// line to line-segment intersection test, and so on. + /// Note: If two line segments are coincident, then + /// no intersection is detected (there are actually + /// infinite intersection points). + /// Author: Jeremy Bell + /// + /// The first point of the first line segment. + /// The second point of the first line segment. + /// The first point of the second line segment. + /// The second point of the second line segment. + /// This is set to the intersection + /// point if an intersection is detected. + /// Set this to true to require that the + /// intersection point be on the first line segment. + /// Set this to true to require that the + /// intersection point be on the second line segment. + /// True if an intersection is detected, false otherwise. + public static bool LineIntersect(Vector2 point1, Vector2 point2, Vector2 point3, Vector2 point4, bool firstIsSegment, bool secondIsSegment, out Vector2 intersectionPoint) + { + return LineIntersect(ref point1, ref point2, ref point3, ref point4, firstIsSegment, secondIsSegment, out intersectionPoint); + } + + /// + /// This method detects if two line segments intersect, + /// and, if so, the point of intersection. + /// Note: If two line segments are coincident, then + /// no intersection is detected (there are actually + /// infinite intersection points). + /// + /// The first point of the first line segment. + /// The second point of the first line segment. + /// The first point of the second line segment. + /// The second point of the second line segment. + /// This is set to the intersection + /// point if an intersection is detected. + /// True if an intersection is detected, false otherwise. + public static bool LineIntersect(ref Vector2 point1, ref Vector2 point2, ref Vector2 point3, ref Vector2 point4, out Vector2 intersectionPoint) + { + return LineIntersect(ref point1, ref point2, ref point3, ref point4, true, true, out intersectionPoint); + } + + /// + /// This method detects if two line segments intersect, + /// and, if so, the point of intersection. + /// Note: If two line segments are coincident, then + /// no intersection is detected (there are actually + /// infinite intersection points). + /// + /// The first point of the first line segment. + /// The second point of the first line segment. + /// The first point of the second line segment. + /// The second point of the second line segment. + /// This is set to the intersection + /// point if an intersection is detected. + /// True if an intersection is detected, false otherwise. + public static bool LineIntersect(Vector2 point1, Vector2 point2, Vector2 point3, Vector2 point4, out Vector2 intersectionPoint) + { + return LineIntersect(ref point1, ref point2, ref point3, ref point4, true, true, out intersectionPoint); + } + + /// + /// Get all intersections between a line segment and a list of vertices + /// representing a polygon. The vertices reuse adjacent points, so for example + /// edges one and two are between the first and second vertices and between the + /// second and third vertices. The last edge is between vertex vertices.Count - 1 + /// and verts0. (ie, vertices from a Geometry or AABB) + /// + /// The first point of the line segment to test + /// The second point of the line segment to test. + /// The vertices, as described above + public static Vertices LineSegmentVerticesIntersect(ref Vector2 point1, ref Vector2 point2, Vertices vertices) + { + Vertices intersectionPoints = new Vertices(); + + for (int i = 0; i < vertices.Count; i++) + { + Vector2 point; + if (LineIntersect(vertices[i], vertices[vertices.NextIndex(i)], point1, point2, true, true, out point)) + { + intersectionPoints.Add(point); + } + } + + return intersectionPoints; + } + + /// + /// Get all intersections between a line segment and an AABB. + /// + /// The first point of the line segment to test + /// The second point of the line segment to test. + /// The AABB that is used for testing intersection. + public static Vertices LineSegmentAABBIntersect(ref Vector2 point1, ref Vector2 point2, AABB aabb) + { + return LineSegmentVerticesIntersect(ref point1, ref point2, aabb.Vertices); + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Mar33.cs b/src/Box2DNet/Common/Mar33.cs deleted file mode 100644 index eaed306..0000000 --- a/src/Box2DNet/Common/Mar33.cs +++ /dev/null @@ -1,90 +0,0 @@ -/* - Box2DNet Copyright (c) 2018 codeyu https://github.com/codeyu/Box2DNet - Box2D original C++ version Copyright (c) 2006-2007 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. -*/ - -//r175 - -using System; using System.Numerics; -using System.Collections.Generic; -using System.Text; - - -namespace Box2DNet.Common -{ - /// - /// A 3-by-3 matrix. Stored in column-major order. - /// - public struct Mat33 - { - /// - /// Construct this matrix using columns. - /// - public Mat33(Vector3 c1, Vector3 c2, Vector3 c3) - { - Col1 = c1; - Col2 = c2; - Col3 = c3; - } - - /// - /// Set this matrix to all zeros. - /// - public void SetZero() - { - Col1 = Vector3.Zero; - Col2 = Vector3.Zero; - Col3 = Vector3.Zero; - } - - /// - /// Solve A * x = b, where b is a column vector. This is more efficient - /// than computing the inverse in one-shot cases. - /// - public Vector3 Solve33(Vector3 b) - { - float det = Vector3.Dot(Col1, Vector3.Cross(Col2, Col3)); - Box2DNetDebug.Assert(det != 0.0f); - det = 1.0f / det; - Vector3 x = new Vector3(); - x.X = det * Vector3.Dot(b, Vector3.Cross(Col2, Col3)); - x.Y = det * Vector3.Dot(Col1, Vector3.Cross(b, Col3)); - x.Z = det * Vector3.Dot(Col1, Vector3.Cross(Col2, b)); - return x; - } - - /// - /// Solve A * x = b, where b is a column vector. This is more efficient - /// than computing the inverse in one-shot cases. Solve only the upper - /// 2-by-2 matrix equation. - /// - public Vector2 Solve22(Vector2 b) - { - float a11 = Col1.X, a12 = Col2.X, a21 = Col1.Y, a22 = Col2.Y; - float det = a11 * a22 - a12 * a21; - Box2DNetDebug.Assert(det != 0.0f); - det = 1.0f / det; - Vector2 x = new Vector2(); - x.X = det * (a22 * b.X - a12 * b.Y); - x.Y = det * (a11 * b.Y - a21 * b.X); - return x; - } - - public Vector3 Col1; - public Vector3 Col2; - public Vector3 Col3; - } -} \ No newline at end of file diff --git a/src/Box2DNet/Common/Mat22.cs b/src/Box2DNet/Common/Mat22.cs deleted file mode 100644 index 978e75a..0000000 --- a/src/Box2DNet/Common/Mat22.cs +++ /dev/null @@ -1,137 +0,0 @@ -/* - Box2DNet Copyright (c) 2018 codeyu https://github.com/codeyu/Box2DNet - Box2D original C++ version Copyright (c) 2006-2007 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.Numerics; -using System.Collections.Generic; -using System.Text; - - -namespace Box2DNet.Common -{ - /// - /// A 2-by-2 matrix. Stored in column-major order. - /// - public struct Mat22 - { - public Vector2 Col1; - public Vector2 Col2; - - /// - /// Construct this matrix using columns. - /// - public Mat22(Vector2 c1, Vector2 c2) - { - Col1 = c1; - Col2 = c2; - } - - /// - /// Construct this matrix using scalars. - /// - public Mat22(float a11, float a12, float a21, float a22) - { - Col1.X = a11; Col1.Y = a21; - Col2.X = a12; Col2.Y = a22; - } - - /// - /// Construct this matrix using an angle. - /// This matrix becomes an orthonormal rotation matrix. - /// - public Mat22(float angle) - { - float c = (float)System.Math.Cos(angle); - float s = (float)System.Math.Sin(angle); - Col1.X = c; Col2.X = -s; - Col1.Y = s; Col2.Y = c; - } - - /// - /// Initialize this matrix using columns. - /// - public void Set(Vector2 c1, Vector2 c2) - { - Col1 = c1; - Col2 = c2; - } - - /// - /// Initialize this matrix using an angle. - /// This matrix becomes an orthonormal rotation matrix. - /// - public void Set(float angle) - { - float c = (float)System.Math.Cos(angle); - float s = (float)System.Math.Sin(angle); - Col1.X = c; Col2.X = -s; - Col1.Y = s; Col2.Y = c; - } - - /// - /// Extract the angle from this matrix (assumed to be a rotation matrix). - /// - public float GetAngle() - { - return Math.Atan2(Col1.Y, Col1.X); - } - - public Vector2 Multiply(Vector2 vector) { - return new Vector2(Col1.X * vector.Y + Col2.X * vector.Y, Col1.Y * vector.X + Col2.Y * vector.Y); - } - - /// - /// Compute the inverse of this matrix, such that inv(A) * A = identity. - /// - public Mat22 GetInverse() - { - float a = Col1.X, b = Col2.X, c = Col1.Y, d = Col2.Y; - Mat22 B = new Mat22(); - float det = a * d - b * c; - Box2DNetDebug.Assert(det != 0.0f); - det = 1.0f / det; - B.Col1.X = det * d; B.Col2.X = -det * b; - B.Col1.Y = -det * c; B.Col2.Y = det * a; - return B; - } - - /// - /// Solve A * x = b, where b is a column vector. This is more efficient - /// than computing the inverse in one-shot cases. - /// - public Vector2 Solve(Vector2 b) - { - float a11 = Col1.X, a12 = Col2.X, a21 = Col1.Y, a22 = Col2.Y; - float det = a11 * a22 - a12 * a21; - Box2DNetDebug.Assert(det != 0.0f); - det = 1.0f / det; - Vector2 x = new Vector2(); - x.X = det * (a22 * b.X - a12 * b.Y); - x.Y = det * (a11 * b.Y - a21 * b.X); - return x; - } - - public static Mat22 Identity { get { return new Mat22(1, 0, 0, 1); } } - - public static Mat22 operator +(Mat22 A, Mat22 B) - { - Mat22 C = new Mat22(); - C.Set(A.Col1 + B.Col1, A.Col2 + B.Col2); - return C; - } - } -} \ No newline at end of file diff --git a/src/Box2DNet/Common/Math.cs b/src/Box2DNet/Common/Math.cs index 17b5f4d..780db59 100644 --- a/src/Box2DNet/Common/Math.cs +++ b/src/Box2DNet/Common/Math.cs @@ -1,291 +1,815 @@ -/* - Box2DNet Copyright (c) 2018 codeyu https://github.com/codeyu/Box2DNet - Box2D original C++ version Copyright (c) 2006-2007 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.Numerics; -using System.Collections.Generic; -using System.Text; - - -using Random = System.Random; - -namespace Box2DNet.Common -{ - public static class Vector2Extension - { - public static float Rad2Deg = (float)360.0f / (float)(System.Math.PI * 2); - public static Vector3 ToVector3(this Vector2 vector) - { - return new Vector3(vector.X, vector.Y, 0.0f); - } - - public static bool IsValid(this Vector2 vector) - { - return Math.IsValid(vector.X) && Math.IsValid(vector.Y); - } - - public static float Cross(this Vector2 vector, Vector2 other) - { - return vector.X * other.Y - vector.Y * other.X; - } - - public static Vector2 CrossScalarPostMultiply(this Vector2 vector, float s) - { - return new Vector2(s * vector.Y, -s * vector.X); - } - - public static Vector2 CrossScalarPreMultiply(this Vector2 vector, float s) - { - return new Vector2(-s * vector.Y, s * vector.X); - } - - public static Vector2 Abs(this Vector2 vector) { - return new Vector2(Math.Abs(vector.X), Math.Abs(vector.Y)); - } - } - - public static class Vector3Extension - { - public static Vector2 ToVector2(this Vector3 vector) - { - return new Vector2(vector.X, vector.Y); - } - } - - public static class QuaternionExtension - { - public static Quaternion FromAngle2D(float radians) - { - return Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), radians * ((float)360.0f / (float)(System.Math.PI * 2))); - } - } - - public class Math - { - public static readonly ushort USHRT_MAX = 0xffff; - public static readonly byte UCHAR_MAX = 0xff; - public static readonly int RAND_LIMIT = 32767; - - /// - /// This function is used to ensure that a floating point number is - /// not a NaN or infinity. - /// - public static bool IsValid(float x) - { - return !(float.IsNaN(x) || float.IsNegativeInfinity(x) || float.IsPositiveInfinity(x)); - } - - [System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Explicit)] - public struct Convert - { - [System.Runtime.InteropServices.FieldOffset(0)] - public float x; - - [System.Runtime.InteropServices.FieldOffset(0)] - public int i; - } - - -#if USE_MATRIX_FOR_ROTATION - public static Mat22 AngleToRotation(float angle) - { - return new Mat22(angle); - } -#else - public static Quaternion AngleToRotation(float angle) - { - return QuaternionExtension.FromAngle2D(angle); - } -#endif - - /// - /// This is a approximate yet fast inverse square-root. - /// - public static float InvSqrt(float x) - { - Convert convert = new Convert(); - convert.x = x; - float xhalf = 0.5f * x; - convert.i = 0x5f3759df - (convert.i >> 1); - x = convert.x; - x = x * (1.5f - xhalf * x * x); - return x; - } - - public static float Clamp(float f, float min, float max) { - if (f < min) - return min; - else if (f > max) - return max; - else return f; - } - public static float Rad2Deg = 57.29578f; - public static float Epsilon = 1.401298E-45f; - public static float Sqrt(float x) - { - return Math.Sqrt(x); - } - public static float Distance(Vector2 v1, Vector2 v2) { - return (float)System.Math.Sqrt(System.Math.Pow(v2.X - v1.X, 2) + System.Math.Pow(v2.Y - v1.Y, 2)); - } - - /// - /// Random floating point number in range [lo, hi] - /// - - /// - /// "Next Largest Power of 2 - /// Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm - /// that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with - /// the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next - /// largest power of 2. For a 32-bit value:" - /// - public static uint NextPowerOfTwo(uint x) - { - x |= (x >> 1); - x |= (x >> 2); - x |= (x >> 4); - x |= (x >> 8); - x |= (x >> 16); - return x + 1; - } - - public static bool IsPowerOfTwo(uint x) - { - bool result = x > 0 && (x & (x - 1)) == 0; - return result; - } - - public static float Abs(float a) - { - return a > 0.0f ? a : -a; - } - - public static Vector2 Abs(Vector2 a) - { - return new Vector2(Math.Abs(a.X), Math.Abs(a.Y)); - } - - public static Mat22 Abs(Mat22 A) - { - Mat22 B = new Mat22(); - B.Set(Math.Abs(A.Col1), Math.Abs(A.Col2)); - return B; - } - - public static float Min(float a, float b) - { - return a < b ? a : b; - } - - public static int Min(int a, int b) - { - return a < b ? a : b; - } - - public static float Max(float a, float b) - { - return a > b ? a : b; - } - - public static int Max(int a, int b) - { - return a > b ? a : b; - } - - public static Vector2 Clamp(Vector2 a, Vector2 low, Vector2 high) - { - return Vector2.Max(low, Vector2.Min(a, high)); - } - - public static void Swap(ref T a, ref T b) - { - T tmp = a; - a = b; - b = tmp; - } - - /// - /// Multiply a matrix times a vector. If a rotation matrix is provided, - /// then this Transforms the vector from one frame to another. - /// - public static Vector2 Mul(Mat22 A, Vector2 v) - { - return new Vector2(A.Col1.X * v.X + A.Col2.X * v.Y, A.Col1.Y * v.X + A.Col2.Y * v.Y); - } - - /// - /// Multiply a matrix transpose times a vector. If a rotation matrix is provided, - /// then this Transforms the vector from one frame to another (inverse Transform). - /// - public static Vector2 MulT(Mat22 A, Vector2 v) - { - return new Vector2(Vector2.Dot(v, A.Col1), Vector2.Dot(v, A.Col2)); - } - - /// - /// A * B - /// - public static Mat22 Mul(Mat22 A, Mat22 B) - { - Mat22 C = new Mat22(); - C.Set(Math.Mul(A, B.Col1), Math.Mul(A, B.Col2)); - return C; - } - - /// - /// A^T * B - /// - public static Mat22 MulT(Mat22 A, Mat22 B) - { - Vector2 c1 = new Vector2(Vector2.Dot(A.Col1, B.Col1), Vector2.Dot(A.Col2, B.Col1)); - Vector2 c2 = new Vector2(Vector2.Dot(A.Col1, B.Col2), Vector2.Dot(A.Col2, B.Col2)); - return new Mat22(c1, c2); - } - - public static Vector2 Mul(Transform T, Vector2 v) - { -#if USE_MATRIX_FOR_ROTATION - return T.position + Math.Mul(T.R, v); -#else - return T.position + T.TransformDirection(v); -#endif - } - - public static Vector2 MulT(Transform T, Vector2 v) - { -#if USE_MATRIX_FOR_ROTATION - return Math.MulT(T.R, v - T.position); -#else - return T.InverseTransformDirection(v - T.position); -#endif - } - - /// - /// Multiply a matrix times a vector. - /// - public static Vector3 Mul(Mat33 A, Vector3 v) - { - return v.X * A.Col1 + v.Y * A.Col2 + v.Z * A.Col3; - } - - public static float Atan2(float y, float x) - { - return (float)System.Math.Atan2(y, x); - } - } +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using System.Runtime.InteropServices; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Common +{ + public static class MathUtils + { + public static float Cross(ref Vector2 a, ref Vector2 b) + { + return a.X * b.Y - a.Y * b.X; + } + + public static float Cross(Vector2 a, Vector2 b) + { + return Cross(ref a, ref b); + } + + /// Perform the cross product on two vectors. + public static Vector3 Cross(Vector3 a, Vector3 b) + { + return new Vector3(a.Y * b.Z - a.Z * b.Y, a.Z * b.X - a.X * b.Z, a.X * b.Y - a.Y * b.X); + } + + public static Vector2 Cross(Vector2 a, float s) + { + return new Vector2(s * a.Y, -s * a.X); + } + + public static Vector2 Cross(float s, Vector2 a) + { + return new Vector2(-s * a.Y, s * a.X); + } + + public static Vector2 Abs(Vector2 v) + { + return new Vector2(Math.Abs(v.X), Math.Abs(v.Y)); + } + + public static Vector2 Mul(ref Mat22 A, Vector2 v) + { + return Mul(ref A, ref v); + } + + public static Vector2 Mul(ref Mat22 A, ref Vector2 v) + { + return new Vector2(A.ex.X * v.X + A.ey.X * v.Y, A.ex.Y * v.X + A.ey.Y * v.Y); + } + + public static Vector2 Mul(ref Transform T, Vector2 v) + { + return Mul(ref T, ref v); + } + + public static Vector2 Mul(ref Transform T, ref Vector2 v) + { + float x = (T.q.c * v.X - T.q.s * v.Y) + T.p.X; + float y = (T.q.s * v.X + T.q.c * v.Y) + T.p.Y; + + return new Vector2(x, y); + } + + public static Vector2 MulT(ref Mat22 A, Vector2 v) + { + return MulT(ref A, ref v); + } + + public static Vector2 MulT(ref Mat22 A, ref Vector2 v) + { + return new Vector2(v.X * A.ex.X + v.Y * A.ex.Y, v.X * A.ey.X + v.Y * A.ey.Y); + } + + public static Vector2 MulT(ref Transform T, Vector2 v) + { + return MulT(ref T, ref v); + } + + public static Vector2 MulT(ref Transform T, ref Vector2 v) + { + float px = v.X - T.p.X; + float py = v.Y - T.p.Y; + float x = (T.q.c * px + T.q.s * py); + float y = (-T.q.s * px + T.q.c * py); + + return new Vector2(x, y); + } + + // A^T * B + public static void MulT(ref Mat22 A, ref Mat22 B, out Mat22 C) + { + C = new Mat22(); + C.ex.X = A.ex.X * B.ex.X + A.ex.Y * B.ex.Y; + C.ex.Y = A.ey.X * B.ex.X + A.ey.Y * B.ex.Y; + C.ey.X = A.ex.X * B.ey.X + A.ex.Y * B.ey.Y; + C.ey.Y = A.ey.X * B.ey.X + A.ey.Y * B.ey.Y; + } + + /// Multiply a matrix times a vector. + public static Vector3 Mul(Mat33 A, Vector3 v) + { + return v.X * A.ex + v.Y * A.ey + v.Z * A.ez; + } + + // v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p + // = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p + public static Transform Mul(Transform A, Transform B) + { + Transform C = new Transform(); + C.q = Mul(A.q, B.q); + C.p = Mul(A.q, B.p) + A.p; + return C; + } + + // v2 = A.q' * (B.q * v1 + B.p - A.p) + // = A.q' * B.q * v1 + A.q' * (B.p - A.p) + public static void MulT(ref Transform A, ref Transform B, out Transform C) + { + C = new Transform(); + C.q = MulT(A.q, B.q); + C.p = MulT(A.q, B.p - A.p); + } + + public static void Swap(ref T a, ref T b) + { + T tmp = a; + a = b; + b = tmp; + } + + /// Multiply a matrix times a vector. + public static Vector2 Mul22(Mat33 A, Vector2 v) + { + return new Vector2(A.ex.X * v.X + A.ey.X * v.Y, A.ex.Y * v.X + A.ey.Y * v.Y); + } + + /// Multiply two rotations: q * r + public static Rot Mul(Rot q, Rot r) + { + // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc] + // [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc] + // s = qs * rc + qc * rs + // c = qc * rc - qs * rs + Rot qr; + qr.s = q.s * r.c + q.c * r.s; + qr.c = q.c * r.c - q.s * r.s; + return qr; + } + + public static Vector2 MulT(Transform T, Vector2 v) + { + float px = v.X - T.p.X; + float py = v.Y - T.p.Y; + float x = (T.q.c * px + T.q.s * py); + float y = (-T.q.s * px + T.q.c * py); + + return new Vector2(x, y); + } + + /// Transpose multiply two rotations: qT * r + public static Rot MulT(Rot q, Rot r) + { + // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc] + // [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc] + // s = qc * rs - qs * rc + // c = qc * rc + qs * rs + Rot qr; + qr.s = q.c * r.s - q.s * r.c; + qr.c = q.c * r.c + q.s * r.s; + return qr; + } + + // v2 = A.q' * (B.q * v1 + B.p - A.p) + // = A.q' * B.q * v1 + A.q' * (B.p - A.p) + public static Transform MulT(Transform A, Transform B) + { + Transform C = new Transform(); + C.q = MulT(A.q, B.q); + C.p = MulT(A.q, B.p - A.p); + return C; + } + + /// Rotate a vector + public static Vector2 Mul(Rot q, Vector2 v) + { + return new Vector2(q.c * v.X - q.s * v.Y, q.s * v.X + q.c * v.Y); + } + + /// Inverse rotate a vector + public static Vector2 MulT(Rot q, Vector2 v) + { + return new Vector2(q.c * v.X + q.s * v.Y, -q.s * v.X + q.c * v.Y); + } + + /// Get the skew vector such that dot(skew_vec, other) == cross(vec, other) + public static Vector2 Skew(Vector2 input) + { + return new Vector2(-input.Y, input.X); + } + + /// + /// This function is used to ensure that a floating point number is + /// not a NaN or infinity. + /// + /// The x. + /// + /// true if the specified x is valid; otherwise, false. + /// + public static bool IsValid(float x) + { + if (float.IsNaN(x)) + { + // NaN. + return false; + } + + return !float.IsInfinity(x); + } + + public static bool IsValid(this Vector2 x) + { + return IsValid(x.X) && IsValid(x.Y); + } + + /// + /// This is a approximate yet fast inverse square-root. + /// + /// The x. + /// + public static float InvSqrt(float x) + { + FloatConverter convert = new FloatConverter(); + convert.x = x; + float xhalf = 0.5f * x; + convert.i = 0x5f3759df - (convert.i >> 1); + x = convert.x; + x = x * (1.5f - xhalf * x * x); + return x; + } + + public static int Clamp(int a, int low, int high) + { + return Math.Max(low, Math.Min(a, high)); + } + + public static float Clamp(float a, float low, float high) + { + return Math.Max(low, Math.Min(a, high)); + } + + public static Vector2 Clamp(Vector2 a, Vector2 low, Vector2 high) + { + return Vector2.Max(low, Vector2.Min(a, high)); + } + + public static void Cross(ref Vector2 a, ref Vector2 b, out float c) + { + c = a.X * b.Y - a.Y * b.X; + } + + /// + /// Return the angle between two vectors on a plane + /// The angle is from vector 1 to vector 2, positive anticlockwise + /// The result is between -pi -> pi + /// + public static double VectorAngle(ref Vector2 p1, ref Vector2 p2) + { + double theta1 = Math.Atan2(p1.Y, p1.X); + double theta2 = Math.Atan2(p2.Y, p2.X); + double dtheta = theta2 - theta1; + while (dtheta > Math.PI) + dtheta -= (2 * Math.PI); + while (dtheta < -Math.PI) + dtheta += (2 * Math.PI); + + return (dtheta); + } + + /// Perform the dot product on two vectors. + public static float Dot(Vector3 a, Vector3 b) + { + return a.X * b.X + a.Y * b.Y + a.Z * b.Z; + } + + public static double VectorAngle(Vector2 p1, Vector2 p2) + { + return VectorAngle(ref p1, ref p2); + } + + /// + /// Returns a positive number if c is to the left of the line going from a to b. + /// + /// Positive number if point is left, negative if point is right, + /// and 0 if points are collinear. + public static float Area(Vector2 a, Vector2 b, Vector2 c) + { + return Area(ref a, ref b, ref c); + } + + /// + /// Returns a positive number if c is to the left of the line going from a to b. + /// + /// Positive number if point is left, negative if point is right, + /// and 0 if points are collinear. + public static float Area(ref Vector2 a, ref Vector2 b, ref Vector2 c) + { + return a.X * (b.Y - c.Y) + b.X * (c.Y - a.Y) + c.X * (a.Y - b.Y); + } + + /// + /// Determines if three vertices are collinear (ie. on a straight line) + /// + /// First vertex + /// Second vertex + /// Third vertex + /// The tolerance + /// + public static bool IsCollinear(ref Vector2 a, ref Vector2 b, ref Vector2 c, float tolerance = 0) + { + return FloatInRange(Area(ref a, ref b, ref c), -tolerance, tolerance); + } + + public static void Cross(float s, ref Vector2 a, out Vector2 b) + { + b = new Vector2(-s * a.Y, s * a.X); + } + + public static bool FloatEquals(float value1, float value2) + { + return Math.Abs(value1 - value2) <= Settings.Epsilon; + } + + /// + /// Checks if a floating point Value is equal to another, + /// within a certain tolerance. + /// + /// The first floating point Value. + /// The second floating point Value. + /// The floating point tolerance. + /// True if the values are "equal", false otherwise. + public static bool FloatEquals(float value1, float value2, float delta) + { + return FloatInRange(value1, value2 - delta, value2 + delta); + } + + /// + /// Checks if a floating point Value is within a specified + /// range of values (inclusive). + /// + /// The Value to check. + /// The minimum Value. + /// The maximum Value. + /// True if the Value is within the range specified, + /// false otherwise. + public static bool FloatInRange(float value, float min, float max) + { + return (value >= min && value <= max); + } + + #region Nested type: FloatConverter + + [StructLayout(LayoutKind.Explicit)] + private struct FloatConverter + { + [FieldOffset(0)] + public float x; + [FieldOffset(0)] + public int i; + } + + #endregion + + public static Vector2 Mul(ref Rot rot, Vector2 axis) + { + return Mul(rot, axis); + } + + public static Vector2 MulT(ref Rot rot, Vector2 axis) + { + return MulT(rot, axis); + } + } + + /// + /// A 2-by-2 matrix. Stored in column-major order. + /// + public struct Mat22 + { + public Vector2 ex, ey; + + /// + /// Construct this matrix using columns. + /// + /// The c1. + /// The c2. + public Mat22(Vector2 c1, Vector2 c2) + { + ex = c1; + ey = c2; + } + + /// + /// Construct this matrix using scalars. + /// + /// The a11. + /// The a12. + /// The a21. + /// The a22. + public Mat22(float a11, float a12, float a21, float a22) + { + ex = new Vector2(a11, a21); + ey = new Vector2(a12, a22); + } + + public Mat22 Inverse + { + get + { + float a = ex.X, b = ey.X, c = ex.Y, d = ey.Y; + float det = a * d - b * c; + if (det != 0.0f) + { + det = 1.0f / det; + } + + Mat22 result = new Mat22(); + result.ex.X = det * d; + result.ex.Y = -det * c; + + result.ey.X = -det * b; + result.ey.Y = det * a; + + return result; + } + } + + /// + /// Initialize this matrix using columns. + /// + /// The c1. + /// The c2. + public void Set(Vector2 c1, Vector2 c2) + { + ex = c1; + ey = c2; + } + + /// + /// Set this to the identity matrix. + /// + public void SetIdentity() + { + ex.X = 1.0f; + ey.X = 0.0f; + ex.Y = 0.0f; + ey.Y = 1.0f; + } + + /// + /// Set this matrix to all zeros. + /// + public void SetZero() + { + ex.X = 0.0f; + ey.X = 0.0f; + ex.Y = 0.0f; + ey.Y = 0.0f; + } + + /// + /// Solve A * x = b, where b is a column vector. This is more efficient + /// than computing the inverse in one-shot cases. + /// + /// The b. + /// + public Vector2 Solve(Vector2 b) + { + float a11 = ex.X, a12 = ey.X, a21 = ex.Y, a22 = ey.Y; + float det = a11 * a22 - a12 * a21; + if (det != 0.0f) + { + det = 1.0f / det; + } + + return new Vector2(det * (a22 * b.X - a12 * b.Y), det * (a11 * b.Y - a21 * b.X)); + } + + public static void Add(ref Mat22 A, ref Mat22 B, out Mat22 R) + { + R.ex = A.ex + B.ex; + R.ey = A.ey + B.ey; + } + } + + /// + /// A 3-by-3 matrix. Stored in column-major order. + /// + public struct Mat33 + { + public Vector3 ex, ey, ez; + + /// + /// Construct this matrix using columns. + /// + /// The c1. + /// The c2. + /// The c3. + public Mat33(Vector3 c1, Vector3 c2, Vector3 c3) + { + ex = c1; + ey = c2; + ez = c3; + } + + /// + /// Set this matrix to all zeros. + /// + public void SetZero() + { + ex = Vector3.Zero; + ey = Vector3.Zero; + ez = Vector3.Zero; + } + + /// + /// Solve A * x = b, where b is a column vector. This is more efficient + /// than computing the inverse in one-shot cases. + /// + /// The b. + /// + public Vector3 Solve33(Vector3 b) + { + float det = Vector3.Dot(ex, Vector3.Cross(ey, ez)); + if (det != 0.0f) + { + det = 1.0f / det; + } + + return new Vector3(det * Vector3.Dot(b, Vector3.Cross(ey, ez)), det * Vector3.Dot(ex, Vector3.Cross(b, ez)), det * Vector3.Dot(ex, Vector3.Cross(ey, b))); + } + + /// + /// Solve A * x = b, where b is a column vector. This is more efficient + /// than computing the inverse in one-shot cases. Solve only the upper + /// 2-by-2 matrix equation. + /// + /// The b. + /// + public Vector2 Solve22(Vector2 b) + { + float a11 = ex.X, a12 = ey.X, a21 = ex.Y, a22 = ey.Y; + float det = a11 * a22 - a12 * a21; + + if (det != 0.0f) + { + det = 1.0f / det; + } + + return new Vector2(det * (a22 * b.X - a12 * b.Y), det * (a11 * b.Y - a21 * b.X)); + } + + /// Get the inverse of this matrix as a 2-by-2. + /// Returns the zero matrix if singular. + public void GetInverse22(ref Mat33 M) + { + float a = ex.X, b = ey.X, c = ex.Y, d = ey.Y; + float det = a * d - b * c; + if (det != 0.0f) + { + det = 1.0f / det; + } + + M.ex.X = det * d; M.ey.X = -det * b; M.ex.Z = 0.0f; + M.ex.Y = -det * c; M.ey.Y = det * a; M.ey.Z = 0.0f; + M.ez.X = 0.0f; M.ez.Y = 0.0f; M.ez.Z = 0.0f; + } + + /// Get the symmetric inverse of this matrix as a 3-by-3. + /// Returns the zero matrix if singular. + public void GetSymInverse33(ref Mat33 M) + { + float det = MathUtils.Dot(ex, MathUtils.Cross(ey, ez)); + if (det != 0.0f) + { + det = 1.0f / det; + } + + float a11 = ex.X, a12 = ey.X, a13 = ez.X; + float a22 = ey.Y, a23 = ez.Y; + float a33 = ez.Z; + + M.ex.X = det * (a22 * a33 - a23 * a23); + M.ex.Y = det * (a13 * a23 - a12 * a33); + M.ex.Z = det * (a12 * a23 - a13 * a22); + + M.ey.X = M.ex.Y; + M.ey.Y = det * (a11 * a33 - a13 * a13); + M.ey.Z = det * (a13 * a12 - a11 * a23); + + M.ez.X = M.ex.Z; + M.ez.Y = M.ey.Z; + M.ez.Z = det * (a11 * a22 - a12 * a12); + } + } + + /// + /// Rotation + /// + public struct Rot + { + /// Sine and cosine + public float s, c; + + /// + /// Initialize from an angle in radians + /// + /// Angle in radians + public Rot(float angle) + { + // TODO_ERIN optimize + s = (float)Math.Sin(angle); + c = (float)Math.Cos(angle); + } + + /// + /// Set using an angle in radians. + /// + /// + public void Set(float angle) + { + //FPE: Optimization + if (angle == 0) + { + s = 0; + c = 1; + } + else + { + // TODO_ERIN optimize + s = (float)Math.Sin(angle); + c = (float)Math.Cos(angle); + } + } + + /// + /// Set to the identity rotation + /// + public void SetIdentity() + { + s = 0.0f; + c = 1.0f; + } + + /// + /// Get the angle in radians + /// + public float GetAngle() + { + return (float)Math.Atan2(s, c); + } + + /// + /// Get the x-axis + /// + public Vector2 GetXAxis() + { + return new Vector2(c, s); + } + + /// + /// Get the y-axis + /// + public Vector2 GetYAxis() + { + return new Vector2(-s, c); + } + } + + /// + /// A transform contains translation and rotation. It is used to represent + /// the position and orientation of rigid frames. + /// + public struct Transform + { + public Vector2 p; + public Rot q; + + /// + /// Initialize using a position vector and a rotation matrix. + /// + /// The position. + /// The r. + public Transform(ref Vector2 position, ref Rot rotation) + { + p = position; + q = rotation; + } + + /// + /// Set this to the identity transform. + /// + public void SetIdentity() + { + p = Vector2.Zero; + q.SetIdentity(); + } + + /// + /// Set this based on the position and angle. + /// + /// The position. + /// The angle. + public void Set(Vector2 position, float angle) + { + p = position; + q.Set(angle); + } + } + + /// + /// This describes the motion of a body/shape for TOI computation. + /// Shapes are defined with respect to the body origin, which may + /// no coincide with the center of mass. However, to support dynamics + /// we must interpolate the center of mass position. + /// + public struct Sweep + { + /// + /// World angles + /// + public float A; + + public float A0; + + /// + /// Fraction of the current time step in the range [0,1] + /// c0 and a0 are the positions at alpha0. + /// + public float Alpha0; + + /// + /// Center world positions + /// + public Vector2 C; + + public Vector2 C0; + + /// + /// Local center of mass position + /// + public Vector2 LocalCenter; + + /// + /// Get the interpolated transform at a specific time. + /// + /// The transform. + /// beta is a factor in [0,1], where 0 indicates alpha0. + public void GetTransform(out Transform xfb, float beta) + { + xfb = new Transform(); + xfb.p.X = (1.0f - beta) * C0.X + beta * C.X; + xfb.p.Y = (1.0f - beta) * C0.Y + beta * C.Y; + float angle = (1.0f - beta) * A0 + beta * A; + xfb.q.Set(angle); + + // Shift to origin + xfb.p -= MathUtils.Mul(xfb.q, LocalCenter); + } + + /// + /// Advance the sweep forward, yielding a new initial state. + /// + /// new initial time.. + public void Advance(float alpha) + { + Debug.Assert(Alpha0 < 1.0f); + float beta = (alpha - Alpha0) / (1.0f - Alpha0); + C0 += beta * (C - C0); + A0 += beta * (A - A0); + Alpha0 = alpha; + } + + /// + /// Normalize the angles. + /// + public void Normalize() + { + float d = MathHelper.TwoPi * (float)Math.Floor(A0 / MathHelper.TwoPi); + A0 -= d; + A -= d; + } + } } \ No newline at end of file diff --git a/src/Box2DNet/Common/Path.cs b/src/Box2DNet/Common/Path.cs new file mode 100644 index 0000000..b2dff99 --- /dev/null +++ b/src/Box2DNet/Common/Path.cs @@ -0,0 +1,337 @@ +using System; +using System.Collections.Generic; +using System.Text; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Common +{ + //Contributed by Matthew Bettcher + + /// + /// Path: + /// Very similar to Vertices, but this + /// class contains vectors describing + /// control points on a Catmull-Rom + /// curve. + /// + public class Path + { + /// + /// All the points that makes up the curve + /// + public List ControlPoints; + + private float _deltaT; + + /// + /// Initializes a new instance of the class. + /// + public Path() + { + ControlPoints = new List(); + } + + /// + /// Initializes a new instance of the class. + /// + /// The vertices to created the path from. + public Path(Vector2[] vertices) + { + ControlPoints = new List(vertices.Length); + + for (int i = 0; i < vertices.Length; i++) + { + Add(vertices[i]); + } + } + + /// + /// Initializes a new instance of the class. + /// + /// The vertices to created the path from. + public Path(IList vertices) + { + ControlPoints = new List(vertices.Count); + for (int i = 0; i < vertices.Count; i++) + { + Add(vertices[i]); + } + } + + /// + /// True if the curve is closed. + /// + /// true if closed; otherwise, false. + public bool Closed { get; set; } + + /// + /// Gets the next index of a controlpoint + /// + /// The index. + /// + public int NextIndex(int index) + { + if (index == ControlPoints.Count - 1) + { + return 0; + } + return index + 1; + } + + /// + /// Gets the previous index of a controlpoint + /// + /// The index. + /// + public int PreviousIndex(int index) + { + if (index == 0) + { + return ControlPoints.Count - 1; + } + return index - 1; + } + + /// + /// Translates the control points by the specified vector. + /// + /// The vector. + public void Translate(ref Vector2 vector) + { + for (int i = 0; i < ControlPoints.Count; i++) + ControlPoints[i] = Vector2.Add(ControlPoints[i], vector); + } + + /// + /// Scales the control points by the specified vector. + /// + /// The Value. + public void Scale(ref Vector2 value) + { + for (int i = 0; i < ControlPoints.Count; i++) + ControlPoints[i] = Vector2.Multiply(ControlPoints[i], value); + } + + /// + /// Rotate the control points by the defined value in radians. + /// + /// The amount to rotate by in radians. + public void Rotate(float value) + { + Matrix rotationMatrix; + Matrix.CreateRotationZ(value, out rotationMatrix); + + for (int i = 0; i < ControlPoints.Count; i++) + ControlPoints[i] = Vector2.Transform(ControlPoints[i], rotationMatrix); + } + + public override string ToString() + { + StringBuilder builder = new StringBuilder(); + for (int i = 0; i < ControlPoints.Count; i++) + { + builder.Append(ControlPoints[i].ToString()); + if (i < ControlPoints.Count - 1) + { + builder.Append(" "); + } + } + return builder.ToString(); + } + + /// + /// Returns a set of points defining the + /// curve with the specifed number of divisions + /// between each control point. + /// + /// Number of divisions between each control point. + /// + public Vertices GetVertices(int divisions) + { + Vertices verts = new Vertices(); + + float timeStep = 1f / divisions; + + for (float i = 0; i < 1f; i += timeStep) + { + verts.Add(GetPosition(i)); + } + + return verts; + } + + public Vector2 GetPosition(float time) + { + Vector2 temp; + + if (ControlPoints.Count < 2) + throw new Exception("You need at least 2 control points to calculate a position."); + + if (Closed) + { + Add(ControlPoints[0]); + + _deltaT = 1f / (ControlPoints.Count - 1); + + int p = (int)(time / _deltaT); + + // use a circular indexing system + int p0 = p - 1; + if (p0 < 0) p0 = p0 + (ControlPoints.Count - 1); + else if (p0 >= ControlPoints.Count - 1) p0 = p0 - (ControlPoints.Count - 1); + int p1 = p; + if (p1 < 0) p1 = p1 + (ControlPoints.Count - 1); + else if (p1 >= ControlPoints.Count - 1) p1 = p1 - (ControlPoints.Count - 1); + int p2 = p + 1; + if (p2 < 0) p2 = p2 + (ControlPoints.Count - 1); + else if (p2 >= ControlPoints.Count - 1) p2 = p2 - (ControlPoints.Count - 1); + int p3 = p + 2; + if (p3 < 0) p3 = p3 + (ControlPoints.Count - 1); + else if (p3 >= ControlPoints.Count - 1) p3 = p3 - (ControlPoints.Count - 1); + + // relative time + float lt = (time - _deltaT * p) / _deltaT; + + temp = Vector2.CatmullRom(ControlPoints[p0], ControlPoints[p1], ControlPoints[p2], ControlPoints[p3], lt); + + RemoveAt(ControlPoints.Count - 1); + } + else + { + int p = (int)(time / _deltaT); + + // + int p0 = p - 1; + if (p0 < 0) p0 = 0; + else if (p0 >= ControlPoints.Count - 1) p0 = ControlPoints.Count - 1; + int p1 = p; + if (p1 < 0) p1 = 0; + else if (p1 >= ControlPoints.Count - 1) p1 = ControlPoints.Count - 1; + int p2 = p + 1; + if (p2 < 0) p2 = 0; + else if (p2 >= ControlPoints.Count - 1) p2 = ControlPoints.Count - 1; + int p3 = p + 2; + if (p3 < 0) p3 = 0; + else if (p3 >= ControlPoints.Count - 1) p3 = ControlPoints.Count - 1; + + // relative time + float lt = (time - _deltaT * p) / _deltaT; + + temp = Vector2.CatmullRom(ControlPoints[p0], ControlPoints[p1], ControlPoints[p2], ControlPoints[p3], lt); + } + + return temp; + } + + /// + /// Gets the normal for the given time. + /// + /// The time + /// The normal. + public Vector2 GetPositionNormal(float time) + { + float offsetTime = time + 0.0001f; + + Vector2 a = GetPosition(time); + Vector2 b = GetPosition(offsetTime); + + Vector2 output, temp; + + Vector2.Subtract(ref a, ref b, out temp); + +#if (XBOX360 || WINDOWS_PHONE) +output = new Vector2(); +#endif + output.X = -temp.Y; + output.Y = temp.X; + + Vector2.Normalize(ref output, out output); + + return output; + } + + public void Add(Vector2 point) + { + ControlPoints.Add(point); + _deltaT = 1f / (ControlPoints.Count - 1); + } + + public void Remove(Vector2 point) + { + ControlPoints.Remove(point); + _deltaT = 1f / (ControlPoints.Count - 1); + } + + public void RemoveAt(int index) + { + ControlPoints.RemoveAt(index); + _deltaT = 1f / (ControlPoints.Count - 1); + } + + public float GetLength() + { + List verts = GetVertices(ControlPoints.Count * 25); + float length = 0; + + for (int i = 1; i < verts.Count; i++) + { + length += Vector2.Distance(verts[i - 1], verts[i]); + } + + if (Closed) + length += Vector2.Distance(verts[ControlPoints.Count - 1], verts[0]); + + return length; + } + + public List SubdivideEvenly(int divisions) + { + List verts = new List(); + + float length = GetLength(); + + float deltaLength = length / divisions + 0.001f; + float t = 0.000f; + + // we always start at the first control point + Vector2 start = ControlPoints[0]; + Vector2 end = GetPosition(t); + + // increment t until we are at half the distance + while (deltaLength * 0.5f >= Vector2.Distance(start, end)) + { + end = GetPosition(t); + t += 0.0001f; + + if (t >= 1f) + break; + } + + start = end; + + // for each box + for (int i = 1; i < divisions; i++) + { + Vector2 normal = GetPositionNormal(t); + float angle = (float)Math.Atan2(normal.Y, normal.X); + + verts.Add(new Vector3(end, angle)); + + // until we reach the correct distance down the curve + while (deltaLength >= Vector2.Distance(start, end)) + { + end = GetPosition(t); + t += 0.00001f; + + if (t >= 1f) + break; + } + if (t >= 1f) + break; + + start = end; + } + return verts; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/PathManager.cs b/src/Box2DNet/Common/PathManager.cs new file mode 100644 index 0000000..1f9d6d6 --- /dev/null +++ b/src/Box2DNet/Common/PathManager.cs @@ -0,0 +1,186 @@ +using System; +using System.Collections.Generic; +using Box2DNet.Collision.Shapes; +using Box2DNet.Common.Decomposition; +using Box2DNet.Dynamics; +using Box2DNet.Dynamics.Joints; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Common +{ + /// + /// An easy to use manager for creating paths. + /// + public static class PathManager + { + #region LinkType enum + + public enum LinkType + { + Revolute, + Slider + } + + #endregion + + //Contributed by Matthew Bettcher + + /// + /// Convert a path into a set of edges and attaches them to the specified body. + /// Note: use only for static edges. + /// + /// The path. + /// The body. + /// The subdivisions. + public static void ConvertPathToEdges(Path path, Body body, int subdivisions) + { + Vertices verts = path.GetVertices(subdivisions); + + if (path.Closed) + { + ChainShape chain = new ChainShape(verts, true); + body.CreateFixture(chain); + } + else + { + for (int i = 1; i < verts.Count; i++) + { + body.CreateFixture(new EdgeShape(verts[i], verts[i - 1])); + } + } + } + + /// + /// Convert a closed path into a polygon. + /// Convex decomposition is automatically performed. + /// + /// The path. + /// The body. + /// The density. + /// The subdivisions. + public static void ConvertPathToPolygon(Path path, Body body, float density, int subdivisions) + { + if (!path.Closed) + throw new Exception("The path must be closed to convert to a polygon."); + + List verts = path.GetVertices(subdivisions); + + List decomposedVerts = Triangulate.ConvexPartition(new Vertices(verts), TriangulationAlgorithm.Bayazit); + + foreach (Vertices item in decomposedVerts) + { + body.CreateFixture(new PolygonShape(item, density)); + } + } + + /// + /// Duplicates the given Body along the given path for approximatly the given copies. + /// + /// The world. + /// The path. + /// The shapes. + /// The type. + /// The copies. + /// + /// + public static List EvenlyDistributeShapesAlongPath(World world, Path path, IEnumerable shapes, BodyType type, int copies, object userData = null) + { + List centers = path.SubdivideEvenly(copies); + List bodyList = new List(); + + for (int i = 0; i < centers.Count; i++) + { + Body b = new Body(world); + + // copy the type from original body + b.BodyType = type; + b.Position = new Vector2(centers[i].X, centers[i].Y); + b.Rotation = centers[i].Z; + b.UserData = userData; + + foreach (Shape shape in shapes) + { + b.CreateFixture(shape); + } + + bodyList.Add(b); + } + + return bodyList; + } + + + /// + /// Duplicates the given Body along the given path for approximatly the given copies. + /// + /// The world. + /// The path. + /// The shape. + /// The type. + /// The copies. + /// The user data. + /// + public static List EvenlyDistributeShapesAlongPath(World world, Path path, Shape shape, BodyType type, + int copies, object userData) + { + List shapes = new List(1); + shapes.Add(shape); + + return EvenlyDistributeShapesAlongPath(world, path, shapes, type, copies, userData); + } + + public static List EvenlyDistributeShapesAlongPath(World world, Path path, Shape shape, BodyType type, int copies) + { + return EvenlyDistributeShapesAlongPath(world, path, shape, type, copies, null); + } + + /// + /// Moves the given body along the defined path. + /// + /// The path. + /// The body. + /// The time. + /// The strength. + /// The time step. + public static void MoveBodyOnPath(Path path, Body body, float time, float strength, float timeStep) + { + Vector2 destination = path.GetPosition(time); + Vector2 positionDelta = body.Position - destination; + Vector2 velocity = (positionDelta / timeStep) * strength; + + body.LinearVelocity = -velocity; + } + + /// + /// Attaches the bodies with revolute joints. + /// + /// The world. + /// The bodies. + /// The local anchor A. + /// The local anchor B. + /// if set to true [connect first and last]. + /// if set to true [collide connected]. + public static List AttachBodiesWithRevoluteJoint(World world, List bodies, Vector2 localAnchorA, Vector2 localAnchorB, bool connectFirstAndLast, bool collideConnected) + { + List joints = new List(bodies.Count + 1); + + for (int i = 1; i < bodies.Count; i++) + { + RevoluteJoint joint = new RevoluteJoint(bodies[i], bodies[i - 1], localAnchorA, localAnchorB); + joint.CollideConnected = collideConnected; + world.AddJoint(joint); + joints.Add(joint); + } + + if (connectFirstAndLast) + { + RevoluteJoint lastjoint = new RevoluteJoint(bodies[0], bodies[bodies.Count - 1], localAnchorA, localAnchorB); + lastjoint.CollideConnected = collideConnected; + world.AddJoint(lastjoint); + joints.Add(lastjoint); + } + + return joints; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/PhysicsLogic/FilterData.cs b/src/Box2DNet/Common/PhysicsLogic/FilterData.cs new file mode 100644 index 0000000..5038ca3 --- /dev/null +++ b/src/Box2DNet/Common/PhysicsLogic/FilterData.cs @@ -0,0 +1,133 @@ +using Box2DNet.Dynamics; + +namespace Box2DNet.Common.PhysicsLogic +{ + /// + /// Contains filter data that can determine whether an object should be processed or not. + /// + public abstract class FilterData + { + /// + /// Disable the logic on specific categories. + /// Category.None by default. + /// + public Category DisabledOnCategories = Category.None; + + /// + /// Disable the logic on specific groups + /// + public int DisabledOnGroup; + + /// + /// Enable the logic on specific categories + /// Category.All by default. + /// + public Category EnabledOnCategories = Category.All; + + /// + /// Enable the logic on specific groups. + /// + 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 IsInEnabledInCategory(Category category) + { + return (EnabledOnCategories & category) == category; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/PhysicsLogic/PhysicsLogic.cs b/src/Box2DNet/Common/PhysicsLogic/PhysicsLogic.cs new file mode 100644 index 0000000..72a02db --- /dev/null +++ b/src/Box2DNet/Common/PhysicsLogic/PhysicsLogic.cs @@ -0,0 +1,66 @@ +using System; +using Box2DNet.Dynamics; + +namespace Box2DNet.Common.PhysicsLogic +{ + [Flags] + public enum PhysicsLogicType + { + Explosion = (1 << 0) + } + + public struct PhysicsLogicFilter + { + public PhysicsLogicType ControllerIgnores; + + /// + /// Ignores the controller. The controller has no effect on this body. + /// + /// The logic type. + public void IgnorePhysicsLogic(PhysicsLogicType type) + { + ControllerIgnores |= type; + } + + /// + /// Restore the controller. The controller affects this body. + /// + /// The logic type. + public void RestorePhysicsLogic(PhysicsLogicType type) + { + ControllerIgnores &= ~type; + } + + /// + /// Determines whether this body ignores the the specified controller. + /// + /// The logic type. + /// + /// true if the body has the specified flag; otherwise, false. + /// + public bool IsPhysicsLogicIgnored(PhysicsLogicType type) + { + return (ControllerIgnores & type) == type; + } + } + + public abstract class PhysicsLogic : FilterData + { + private PhysicsLogicType _type; + public World World; + + public override bool IsActiveOn(Body body) + { + if (body.PhysicsLogicFilter.IsPhysicsLogicIgnored(_type)) + return false; + + return base.IsActiveOn(body); + } + + public PhysicsLogic(World world, PhysicsLogicType type) + { + _type = type; + World = world; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/PhysicsLogic/RealExplosion.cs b/src/Box2DNet/Common/PhysicsLogic/RealExplosion.cs new file mode 100644 index 0000000..681e10b --- /dev/null +++ b/src/Box2DNet/Common/PhysicsLogic/RealExplosion.cs @@ -0,0 +1,418 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using Box2DNet.Collision; +using Box2DNet.Collision.Shapes; +using Box2DNet.Dynamics; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Common.PhysicsLogic +{ + // Original Code by Steven Lu - see http://www.box2d.org/forum/viewtopic.php?f=3&t=1688 + // Ported to Farseer 3.0 by Nicol�s Hormaz�bal + + internal struct ShapeData + { + public Body Body; + public float Max; + public float Min; // absolute angles + } + + /// + /// This is a comprarer used for + /// detecting angle difference between rays + /// + internal class RayDataComparer : IComparer + { + #region IComparer Members + + int IComparer.Compare(float a, float b) + { + float diff = (a - b); + if (diff > 0) + return 1; + if (diff < 0) + return -1; + return 0; + } + + #endregion + } + + /* Methodology: + * Force applied at a ray is inversely proportional to the square of distance from source + * AABB is used to query for shapes that may be affected + * For each RIGID BODY (not shape -- this is an optimization) that is matched, loop through its vertices to determine + * the extreme points -- if there is structure that contains outlining polygon, use that as an additional optimization + * Evenly cast a number of rays against the shape - number roughly proportional to the arc coverage + * - Something like every 3 degrees should do the trick although this can be altered depending on the distance (if really close don't need such a high density of rays) + * - There should be a minimum number of rays (3-5?) applied to each body so that small bodies far away are still accurately modeled + * - Be sure to have the forces of each ray be proportional to the average arc length covered by each. + * For each ray that actually intersects with the shape (non intersections indicate something blocking the path of explosion): + * - Apply the appropriate force dotted with the negative of the collision normal at the collision point + * - Optionally apply linear interpolation between aforementioned Normal force and the original explosion force in the direction of ray to simulate "surface friction" of sorts + */ + + /// + /// Creates a realistic explosion based on raycasting. Objects in the open will be affected, but objects behind + /// static bodies will not. A body that is half in cover, half in the open will get half the force applied to the end in + /// the open. + /// + public sealed class RealExplosion : PhysicsLogic + { + /// + /// Two degrees: maximum angle from edges to first ray tested + /// + private const float MaxEdgeOffset = MathHelper.Pi / 90; + + /// + /// Ratio of arc length to angle from edges to first ray tested. + /// Defaults to 1/40. + /// + public float EdgeRatio = 1.0f / 40.0f; + + /// + /// Ignore Explosion if it happens inside a shape. + /// Default value is false. + /// + public bool IgnoreWhenInsideShape = false; + + /// + /// Max angle between rays (used when segment is large). + /// Defaults to 15 degrees + /// + public float MaxAngle = MathHelper.Pi / 15; + + /// + /// Maximum number of shapes involved in the explosion. + /// Defaults to 100 + /// + public int MaxShapes = 100; + + /// + /// How many rays per shape/body/segment. + /// Defaults to 5 + /// + public int MinRays = 5; + + private List _data = new List(); + private RayDataComparer _rdc; + + public RealExplosion(World world) + : base(world, PhysicsLogicType.Explosion) + { + _rdc = new RayDataComparer(); + _data = new List(); + } + + /// + /// Activate the explosion at the specified position. + /// + /// The position where the explosion happens + /// The explosion radius + /// The explosion force at the explosion point (then is inversely proportional to the square of the distance) + /// A list of bodies and the amount of force that was applied to them. + public Dictionary Activate(Vector2 pos, float radius, float maxForce) + { + AABB aabb; + aabb.LowerBound = pos + new Vector2(-radius, -radius); + aabb.UpperBound = pos + new Vector2(radius, radius); + Fixture[] shapes = new Fixture[MaxShapes]; + + // More than 5 shapes in an explosion could be possible, but still strange. + Fixture[] containedShapes = new Fixture[5]; + bool exit = false; + + int shapeCount = 0; + int containedShapeCount = 0; + + // Query the world for overlapping shapes. + World.QueryAABB( + fixture => + { + if (fixture.TestPoint(ref pos)) + { + if (IgnoreWhenInsideShape) + { + exit = true; + return false; + } + + containedShapes[containedShapeCount++] = fixture; + } + else + { + shapes[shapeCount++] = fixture; + } + + // Continue the query. + return true; + }, ref aabb); + + if (exit) + return new Dictionary(); + + Dictionary exploded = new Dictionary(shapeCount + containedShapeCount); + + // Per shape max/min angles for now. + float[] vals = new float[shapeCount * 2]; + int valIndex = 0; + for (int i = 0; i < shapeCount; ++i) + { + PolygonShape ps; + CircleShape cs = shapes[i].Shape as CircleShape; + if (cs != null) + { + // We create a "diamond" approximation of the circle + Vertices v = new Vertices(); + Vector2 vec = Vector2.Zero + new Vector2(cs.Radius, 0); + v.Add(vec); + vec = Vector2.Zero + new Vector2(0, cs.Radius); + v.Add(vec); + vec = Vector2.Zero + new Vector2(-cs.Radius, cs.Radius); + v.Add(vec); + vec = Vector2.Zero + new Vector2(0, -cs.Radius); + v.Add(vec); + ps = new PolygonShape(v, 0); + } + else + ps = shapes[i].Shape as PolygonShape; + + if ((shapes[i].Body.BodyType == BodyType.Dynamic) && ps != null) + { + Vector2 toCentroid = shapes[i].Body.GetWorldPoint(ps.MassData.Centroid) - pos; + float angleToCentroid = (float)Math.Atan2(toCentroid.Y, toCentroid.X); + float min = float.MaxValue; + float max = float.MinValue; + float minAbsolute = 0.0f; + float maxAbsolute = 0.0f; + + for (int j = 0; j < ps.Vertices.Count; ++j) + { + Vector2 toVertex = (shapes[i].Body.GetWorldPoint(ps.Vertices[j]) - pos); + float newAngle = (float)Math.Atan2(toVertex.Y, toVertex.X); + float diff = (newAngle - angleToCentroid); + + diff = (diff - MathHelper.Pi) % (2 * MathHelper.Pi); + // the minus pi is important. It means cutoff for going other direction is at 180 deg where it needs to be + + if (diff < 0.0f) + diff += 2 * MathHelper.Pi; // correction for not handling negs + + diff -= MathHelper.Pi; + + if (Math.Abs(diff) > MathHelper.Pi) + continue; // Something's wrong, point not in shape but exists angle diff > 180 + + if (diff > max) + { + max = diff; + maxAbsolute = newAngle; + } + if (diff < min) + { + min = diff; + minAbsolute = newAngle; + } + } + + vals[valIndex] = minAbsolute; + ++valIndex; + vals[valIndex] = maxAbsolute; + ++valIndex; + } + } + + Array.Sort(vals, 0, valIndex, _rdc); + _data.Clear(); + bool rayMissed = true; + + for (int i = 0; i < valIndex; ++i) + { + Fixture fixture = null; + float midpt; + + int iplus = (i == valIndex - 1 ? 0 : i + 1); + if (vals[i] == vals[iplus]) + continue; + + if (i == valIndex - 1) + { + // the single edgecase + midpt = (vals[0] + MathHelper.Pi * 2 + vals[i]); + } + else + { + midpt = (vals[i + 1] + vals[i]); + } + + midpt = midpt / 2; + + Vector2 p1 = pos; + Vector2 p2 = radius * new Vector2((float)Math.Cos(midpt), (float)Math.Sin(midpt)) + pos; + + // RaycastOne + bool hitClosest = false; + World.RayCast((f, p, n, fr) => + { + Body body = f.Body; + + if (!IsActiveOn(body)) + return 0; + + hitClosest = true; + fixture = f; + return fr; + }, p1, p2); + + //draws radius points + if ((hitClosest) && (fixture.Body.BodyType == BodyType.Dynamic)) + { + if ((_data.Any()) && (_data.Last().Body == fixture.Body) && (!rayMissed)) + { + int laPos = _data.Count - 1; + ShapeData la = _data[laPos]; + la.Max = vals[iplus]; + _data[laPos] = la; + } + else + { + // make new + ShapeData d; + d.Body = fixture.Body; + d.Min = vals[i]; + d.Max = vals[iplus]; + _data.Add(d); + } + + if ((_data.Count > 1) + && (i == valIndex - 1) + && (_data.Last().Body == _data.First().Body) + && (_data.Last().Max == _data.First().Min)) + { + ShapeData fi = _data[0]; + fi.Min = _data.Last().Min; + _data.RemoveAt(_data.Count - 1); + _data[0] = fi; + while (_data.First().Min >= _data.First().Max) + { + fi.Min -= MathHelper.Pi * 2; + _data[0] = fi; + } + } + + int lastPos = _data.Count - 1; + ShapeData last = _data[lastPos]; + while ((_data.Count > 0) + && (_data.Last().Min >= _data.Last().Max)) // just making sure min fl = _data[i].Body.FixtureList; + for (int x = 0; x < fl.Count; x++) + { + Fixture f = fl[x]; + RayCastInput ri; + ri.Point1 = p1; + ri.Point2 = p2; + ri.MaxFraction = 50f; + + RayCastOutput ro; + if (f.RayCast(out ro, ref ri, 0)) + { + if (minlambda > ro.Fraction) + { + minlambda = ro.Fraction; + hitpoint = ro.Fraction * p2 + (1 - ro.Fraction) * p1; + } + } + + // the force that is to be applied for this particular ray. + // offset is angular coverage. lambda*length of segment is distance. + float impulse = (arclen / (MinRays + insertedRays)) * maxForce * 180.0f / MathHelper.Pi * (1.0f - Math.Min(1.0f, minlambda)); + + // We Apply the impulse!!! + Vector2 vectImp = Vector2.Dot(impulse * new Vector2((float)Math.Cos(j), (float)Math.Sin(j)), -ro.Normal) * new Vector2((float)Math.Cos(j), (float)Math.Sin(j)); + _data[i].Body.ApplyLinearImpulse(ref vectImp, ref hitpoint); + + // We gather the fixtures for returning them + if (exploded.ContainsKey(f)) + exploded[f] += vectImp; + else + exploded.Add(f, vectImp); + + if (minlambda > 1.0f) + hitpoint = p2; + } + } + } + + // We check contained shapes + for (int i = 0; i < containedShapeCount; ++i) + { + Fixture fix = containedShapes[i]; + + if (!IsActiveOn(fix.Body)) + continue; + + float impulse = MinRays * maxForce * 180.0f / MathHelper.Pi; + Vector2 hitPoint; + + CircleShape circShape = fix.Shape as CircleShape; + if (circShape != null) + { + hitPoint = fix.Body.GetWorldPoint(circShape.Position); + } + else + { + PolygonShape shape = fix.Shape as PolygonShape; + hitPoint = fix.Body.GetWorldPoint(shape.MassData.Centroid); + } + + Vector2 vectImp = impulse * (hitPoint - pos); + + fix.Body.ApplyLinearImpulse(ref vectImp, ref hitPoint); + + if (!exploded.ContainsKey(fix)) + exploded.Add(fix, vectImp); + } + + return exploded; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/PhysicsLogic/SimpleExplosion.cs b/src/Box2DNet/Common/PhysicsLogic/SimpleExplosion.cs new file mode 100644 index 0000000..8dd840d --- /dev/null +++ b/src/Box2DNet/Common/PhysicsLogic/SimpleExplosion.cs @@ -0,0 +1,92 @@ +using System; +using System.Collections.Generic; +using Box2DNet.Collision; +using Box2DNet.Dynamics; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Common.PhysicsLogic +{ + /// + /// Creates a simple explosion that ignores other bodies hiding behind static bodies. + /// + public sealed class SimpleExplosion : PhysicsLogic + { + public SimpleExplosion(World world) + : base(world, PhysicsLogicType.Explosion) + { + Power = 1; //linear + } + + /// + /// This is the power used in the power function. A value of 1 means the force + /// applied to bodies in the explosion is linear. A value of 2 means it is exponential. + /// + public float Power { get; set; } + + /// + /// Activate the explosion at the specified position. + /// + /// The position (center) of the explosion. + /// The radius of the explosion. + /// The force applied + /// A maximum amount of force. When force gets over this value, it will be equal to maxForce + /// A list of bodies and the amount of force that was applied to them. + public Dictionary Activate(Vector2 pos, float radius, float force, float maxForce = float.MaxValue) + { + HashSet affectedBodies = new HashSet(); + + AABB aabb; + aabb.LowerBound = pos - new Vector2(radius); + aabb.UpperBound = pos + new Vector2(radius); + + // Query the world for bodies within the radius. + World.QueryAABB(fixture => + { + if (Vector2.Distance(fixture.Body.Position, pos) <= radius) + { + if (!affectedBodies.Contains(fixture.Body)) + affectedBodies.Add(fixture.Body); + } + + return true; + }, ref aabb); + + return ApplyImpulse(pos, radius, force, maxForce, affectedBodies); + } + + private Dictionary ApplyImpulse(Vector2 pos, float radius, float force, float maxForce, HashSet overlappingBodies) + { + Dictionary forces = new Dictionary(overlappingBodies.Count); + + foreach (Body overlappingBody in overlappingBodies) + { + if (IsActiveOn(overlappingBody)) + { + float distance = Vector2.Distance(pos, overlappingBody.Position); + float forcePercent = GetPercent(distance, radius); + + Vector2 forceVector = pos - overlappingBody.Position; + forceVector *= 1f / (float)Math.Sqrt(forceVector.X * forceVector.X + forceVector.Y * forceVector.Y); + forceVector *= MathHelper.Min(force * forcePercent, maxForce); + forceVector *= -1; + + overlappingBody.ApplyLinearImpulse(forceVector); + forces.Add(overlappingBody, forceVector); + } + } + + return forces; + } + + private float GetPercent(float distance, float radius) + { + //(1-(distance/radius))^power-1 + float percent = (float)Math.Pow(1 - ((distance - radius) / radius), Power) - 1; + + if (float.IsNaN(percent)) + return 0f; + + return MathHelper.Clamp(percent, 0f, 1f); + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/PolygonManipulation/CuttingTools.cs b/src/Box2DNet/Common/PolygonManipulation/CuttingTools.cs new file mode 100644 index 0000000..34e4a8e --- /dev/null +++ b/src/Box2DNet/Common/PolygonManipulation/CuttingTools.cs @@ -0,0 +1,217 @@ +using System.Collections.Generic; +using System.Diagnostics; +using Box2DNet.Collision.Shapes; +using Box2DNet.Dynamics; +using Box2DNet.Factories; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Common.PolygonManipulation +{ + public static class CuttingTools + { + //Cutting a shape into two is based on the work of Daid and his prototype BoxCutter: http://www.box2d.org/forum/viewtopic.php?f=3&t=1473 + + /// + /// Split a fixture into 2 vertice collections using the given entry and exit-point. + /// + /// The Fixture to split + /// The entry point - The start point + /// The exit point - The end point + /// The first collection of vertexes + /// The second collection of vertexes + public static void SplitShape(Fixture fixture, Vector2 entryPoint, Vector2 exitPoint, out Vertices first, out Vertices second) + { + Vector2 localEntryPoint = fixture.Body.GetLocalPoint(ref entryPoint); + Vector2 localExitPoint = fixture.Body.GetLocalPoint(ref exitPoint); + + PolygonShape shape = fixture.Shape as PolygonShape; + + //We can only cut polygons at the moment + if (shape == null) + { + first = new Vertices(); + second = new Vertices(); + return; + } + + //Offset the entry and exit points if they are too close to the vertices + foreach (Vector2 vertex in shape.Vertices) + { + if (vertex.Equals(localEntryPoint)) + localEntryPoint -= new Vector2(0, Settings.Epsilon); + + if (vertex.Equals(localExitPoint)) + localExitPoint += new Vector2(0, Settings.Epsilon); + } + + Vertices vertices = new Vertices(shape.Vertices); + Vertices[] newPolygon = new Vertices[2]; + + for (int i = 0; i < newPolygon.Length; i++) + { + newPolygon[i] = new Vertices(vertices.Count); + } + + int[] cutAdded = { -1, -1 }; + int last = -1; + for (int i = 0; i < vertices.Count; i++) + { + int n; + //Find out if this vertex is on the old or new shape. + if (Vector2.Dot(MathUtils.Cross(localExitPoint - localEntryPoint, 1), vertices[i] - localEntryPoint) > Settings.Epsilon) + n = 0; + else + n = 1; + + if (last != n) + { + //If we switch from one shape to the other add the cut vertices. + if (last == 0) + { + Debug.Assert(cutAdded[0] == -1); + cutAdded[0] = newPolygon[last].Count; + newPolygon[last].Add(localExitPoint); + newPolygon[last].Add(localEntryPoint); + } + if (last == 1) + { + Debug.Assert(cutAdded[last] == -1); + cutAdded[last] = newPolygon[last].Count; + newPolygon[last].Add(localEntryPoint); + newPolygon[last].Add(localExitPoint); + } + } + + newPolygon[n].Add(vertices[i]); + last = n; + } + + //Add the cut in case it has not been added yet. + if (cutAdded[0] == -1) + { + cutAdded[0] = newPolygon[0].Count; + newPolygon[0].Add(localExitPoint); + newPolygon[0].Add(localEntryPoint); + } + if (cutAdded[1] == -1) + { + cutAdded[1] = newPolygon[1].Count; + newPolygon[1].Add(localEntryPoint); + newPolygon[1].Add(localExitPoint); + } + + for (int n = 0; n < 2; n++) + { + Vector2 offset; + if (cutAdded[n] > 0) + { + offset = (newPolygon[n][cutAdded[n] - 1] - newPolygon[n][cutAdded[n]]); + } + else + { + offset = (newPolygon[n][newPolygon[n].Count - 1] - newPolygon[n][0]); + } + offset.Normalize(); + + if (!offset.IsValid()) + offset = Vector2.One; + + newPolygon[n][cutAdded[n]] += Settings.Epsilon * offset; + + if (cutAdded[n] < newPolygon[n].Count - 2) + { + offset = (newPolygon[n][cutAdded[n] + 2] - newPolygon[n][cutAdded[n] + 1]); + } + else + { + offset = (newPolygon[n][0] - newPolygon[n][newPolygon[n].Count - 1]); + } + offset.Normalize(); + + if (!offset.IsValid()) + offset = Vector2.One; + + newPolygon[n][cutAdded[n] + 1] += Settings.Epsilon * offset; + } + + first = newPolygon[0]; + second = newPolygon[1]; + } + + /// + /// This is a high-level function to cuts fixtures inside the given world, using the start and end points. + /// Note: We don't support cutting when the start or end is inside a shape. + /// + /// The world. + /// The startpoint. + /// The endpoint. + /// True if the cut was performed. + public static bool Cut(World world, Vector2 start, Vector2 end) + { + List fixtures = new List(); + List entryPoints = new List(); + List exitPoints = new List(); + + //We don't support cutting when the start or end is inside a shape. + if (world.TestPoint(start) != null || world.TestPoint(end) != null) + return false; + + //Get the entry points + world.RayCast((f, p, n, fr) => + { + fixtures.Add(f); + entryPoints.Add(p); + return 1; + }, start, end); + + //Reverse the ray to get the exitpoints + world.RayCast((f, p, n, fr) => + { + exitPoints.Add(p); + return 1; + }, end, start); + + //We only have a single point. We need at least 2 + if (entryPoints.Count + exitPoints.Count < 2) + return false; + + for (int i = 0; i < fixtures.Count; i++) + { + // can't cut circles or edges yet ! + if (fixtures[i].Shape.ShapeType != ShapeType.Polygon) + continue; + + if (fixtures[i].Body.BodyType != BodyType.Static) + { + //Split the shape up into two shapes + Vertices first; + Vertices second; + SplitShape(fixtures[i], entryPoints[i], exitPoints[i], out first, out second); + + //Delete the original shape and create two new. Retain the properties of the body. + if (first.CheckPolygon() == PolygonError.NoError) + { + Body firstFixture = BodyFactory.CreatePolygon(world, first, fixtures[i].Shape.Density, fixtures[i].Body.Position); + firstFixture.Rotation = fixtures[i].Body.Rotation; + firstFixture.LinearVelocity = fixtures[i].Body.LinearVelocity; + firstFixture.AngularVelocity = fixtures[i].Body.AngularVelocity; + firstFixture.BodyType = BodyType.Dynamic; + } + + if (second.CheckPolygon() == PolygonError.NoError) + { + Body secondFixture = BodyFactory.CreatePolygon(world, second, fixtures[i].Shape.Density, fixtures[i].Body.Position); + secondFixture.Rotation = fixtures[i].Body.Rotation; + secondFixture.LinearVelocity = fixtures[i].Body.LinearVelocity; + secondFixture.AngularVelocity = fixtures[i].Body.AngularVelocity; + secondFixture.BodyType = BodyType.Dynamic; + } + + world.RemoveBody(fixtures[i].Body); + } + } + + return true; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/PolygonManipulation/SimpleCombiner.cs b/src/Box2DNet/Common/PolygonManipulation/SimpleCombiner.cs new file mode 100644 index 0000000..16f8350 --- /dev/null +++ b/src/Box2DNet/Common/PolygonManipulation/SimpleCombiner.cs @@ -0,0 +1,226 @@ +/* +* C# Version Ported by Matt Bettcher and Ian Qvist 2009-2010 +* +* Original C++ Version Copyright (c) 2007 Eric Jordan +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System.Collections.Generic; +using System.Diagnostics; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Common.PolygonManipulation +{ + /// + /// Combines a list of triangles into a list of convex polygons. + /// Starts with a seed triangle, keep adding triangles to it until you can't add any more without making the polygon non-convex. + /// + public static class SimpleCombiner + { + /// + /// Combine a list of triangles into a list of convex polygons. + /// + /// Note: This only works on triangles. + /// + ///The triangles. + ///The maximun number of polygons to return. + ///The tolerance + public static List PolygonizeTriangles(List triangles, int maxPolys = int.MaxValue, float tolerance = 0.001f) + { + if (triangles.Count <= 0) + return triangles; + + List polys = new List(); + + bool[] covered = new bool[triangles.Count]; + for (int i = 0; i < triangles.Count; ++i) + { + covered[i] = false; + + //Check here for degenerate triangles + Vertices triangle = triangles[i]; + Vector2 a = triangle[0]; + Vector2 b = triangle[1]; + Vector2 c = triangle[2]; + + if ((a.X == b.X && a.Y == b.Y) || (b.X == c.X && b.Y == c.Y) || (a.X == c.X && a.Y == c.Y)) + covered[i] = true; + } + + int polyIndex = 0; + + bool notDone = true; + while (notDone) + { + int currTri = -1; + for (int i = 0; i < triangles.Count; ++i) + { + if (covered[i]) + continue; + + currTri = i; + break; + } + + if (currTri == -1) + { + notDone = false; + } + else + { + Vertices poly = new Vertices(3); + + for (int i = 0; i < 3; i++) + { + poly.Add(triangles[currTri][i]); + } + + covered[currTri] = true; + int index = 0; + for (int i = 0; i < 2 * triangles.Count; ++i, ++index) + { + while (index >= triangles.Count) index -= triangles.Count; + if (covered[index]) + { + continue; + } + Vertices newP = AddTriangle(triangles[index], poly); + if (newP == null) + continue; // is this right + + if (newP.Count > Settings.MaxPolygonVertices) + continue; + + if (newP.IsConvex()) + { + //Or should it be IsUsable? Maybe re-write IsConvex to apply the angle threshold from Box2d + poly = new Vertices(newP); + covered[index] = true; + } + } + + //We have a maximum of polygons that we need to keep under. + if (polyIndex < maxPolys) + { + SimplifyTools.MergeParallelEdges(poly, tolerance); + + //If identical points are present, a triangle gets + //borked by the MergeParallelEdges function, hence + //the vertex number check + if (poly.Count >= 3) + polys.Add(new Vertices(poly)); + else + Debug.WriteLine("Skipping corrupt poly."); + } + + if (poly.Count >= 3) + polyIndex++; //Must be outside (polyIndex < polysLength) test + } + } + + //TODO: Add sanity check + //Remove empty vertice collections + for (int i = polys.Count - 1; i >= 0; i--) + { + if (polys[i].Count == 0) + polys.RemoveAt(i); + } + + return polys; + } + + private static Vertices AddTriangle(Vertices t, Vertices vertices) + { + // First, find vertices that connect + int firstP = -1; + int firstT = -1; + int secondP = -1; + int secondT = -1; + for (int i = 0; i < vertices.Count; i++) + { + if (t[0].X == vertices[i].X && t[0].Y == vertices[i].Y) + { + if (firstP == -1) + { + firstP = i; + firstT = 0; + } + else + { + secondP = i; + secondT = 0; + } + } + else if (t[1].X == vertices[i].X && t[1].Y == vertices[i].Y) + { + if (firstP == -1) + { + firstP = i; + firstT = 1; + } + else + { + secondP = i; + secondT = 1; + } + } + else if (t[2].X == vertices[i].X && t[2].Y == vertices[i].Y) + { + if (firstP == -1) + { + firstP = i; + firstT = 2; + } + else + { + secondP = i; + secondT = 2; + } + } + } + // Fix ordering if first should be last vertex of poly + if (firstP == 0 && secondP == vertices.Count - 1) + { + firstP = vertices.Count - 1; + secondP = 0; + } + + // Didn't find it + if (secondP == -1) + { + return null; + } + + // Find tip index on triangle + int tipT = 0; + if (tipT == firstT || tipT == secondT) + tipT = 1; + if (tipT == firstT || tipT == secondT) + tipT = 2; + + Vertices result = new Vertices(vertices.Count + 1); + for (int i = 0; i < vertices.Count; i++) + { + result.Add(vertices[i]); + + if (i == firstP) + result.Add(t[tipT]); + } + + return result; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/PolygonManipulation/SimplifyTools.cs b/src/Box2DNet/Common/PolygonManipulation/SimplifyTools.cs new file mode 100644 index 0000000..d1bd6dc --- /dev/null +++ b/src/Box2DNet/Common/PolygonManipulation/SimplifyTools.cs @@ -0,0 +1,302 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Common.PolygonManipulation +{ + /// + /// Provides a set of tools to simplify polygons in various ways. + /// + public static class SimplifyTools + { + /// + /// Removes all collinear points on the polygon. + /// + /// The polygon that needs simplification. + /// The collinearity tolerance. + /// A simplified polygon. + public static Vertices CollinearSimplify(Vertices vertices, float collinearityTolerance = 0) + { + if (vertices.Count <= 3) + return vertices; + + Vertices simplified = new Vertices(vertices.Count); + + for (int i = 0; i < vertices.Count; i++) + { + Vector2 prev = vertices.PreviousVertex(i); + Vector2 current = vertices[i]; + Vector2 next = vertices.NextVertex(i); + + //If they collinear, continue + if (MathUtils.IsCollinear(ref prev, ref current, ref next, collinearityTolerance)) + continue; + + simplified.Add(current); + } + + return simplified; + } + + /// + /// Ramer-Douglas-Peucker polygon simplification algorithm. This is the general recursive version that does not use the + /// speed-up technique by using the Melkman convex hull. + /// + /// If you pass in 0, it will remove all collinear points. + /// + /// The simplified polygon + public static Vertices DouglasPeuckerSimplify(Vertices vertices, float distanceTolerance) + { + if (vertices.Count <= 3) + return vertices; + + bool[] usePoint = new bool[vertices.Count]; + + for (int i = 0; i < vertices.Count; i++) + usePoint[i] = true; + + SimplifySection(vertices, 0, vertices.Count - 1, usePoint, distanceTolerance); + + Vertices simplified = new Vertices(vertices.Count); + + for (int i = 0; i < vertices.Count; i++) + { + if (usePoint[i]) + simplified.Add(vertices[i]); + } + + return simplified; + } + + private static void SimplifySection(Vertices vertices, int i, int j, bool[] usePoint, float distanceTolerance) + { + if ((i + 1) == j) + return; + + Vector2 a = vertices[i]; + Vector2 b = vertices[j]; + + double maxDistance = -1.0; + int maxIndex = i; + for (int k = i + 1; k < j; k++) + { + Vector2 point = vertices[k]; + + double distance = LineTools.DistanceBetweenPointAndLineSegment(ref point, ref a, ref b); + + if (distance > maxDistance) + { + maxDistance = distance; + maxIndex = k; + } + } + + if (maxDistance <= distanceTolerance) + { + for (int k = i + 1; k < j; k++) + { + usePoint[k] = false; + } + } + else + { + SimplifySection(vertices, i, maxIndex, usePoint, distanceTolerance); + SimplifySection(vertices, maxIndex, j, usePoint, distanceTolerance); + } + } + + /// + /// Merges all parallel edges in the list of vertices + /// + /// The vertices. + /// The tolerance. + public static Vertices MergeParallelEdges(Vertices vertices, float tolerance) + { + //From Eric Jordan's convex decomposition library + + if (vertices.Count <= 3) + return vertices; //Can't do anything useful here to a triangle + + bool[] mergeMe = new bool[vertices.Count]; + int newNVertices = vertices.Count; + + //Gather points to process + for (int i = 0; i < vertices.Count; ++i) + { + int lower = (i == 0) ? (vertices.Count - 1) : (i - 1); + int middle = i; + int upper = (i == vertices.Count - 1) ? (0) : (i + 1); + + float dx0 = vertices[middle].X - vertices[lower].X; + float dy0 = vertices[middle].Y - vertices[lower].Y; + float dx1 = vertices[upper].Y - vertices[middle].X; + float dy1 = vertices[upper].Y - vertices[middle].Y; + float norm0 = (float)Math.Sqrt(dx0 * dx0 + dy0 * dy0); + float norm1 = (float)Math.Sqrt(dx1 * dx1 + dy1 * dy1); + + if (!(norm0 > 0.0f && norm1 > 0.0f) && newNVertices > 3) + { + //Merge identical points + mergeMe[i] = true; + --newNVertices; + } + + dx0 /= norm0; + dy0 /= norm0; + dx1 /= norm1; + dy1 /= norm1; + float cross = dx0 * dy1 - dx1 * dy0; + float dot = dx0 * dx1 + dy0 * dy1; + + if (Math.Abs(cross) < tolerance && dot > 0 && newNVertices > 3) + { + mergeMe[i] = true; + --newNVertices; + } + else + mergeMe[i] = false; + } + + if (newNVertices == vertices.Count || newNVertices == 0) + return vertices; + + int currIndex = 0; + + //Copy the vertices to a new list and clear the old + Vertices newVertices = new Vertices(newNVertices); + + for (int i = 0; i < vertices.Count; ++i) + { + if (mergeMe[i] || newNVertices == 0 || currIndex == newNVertices) + continue; + + Debug.Assert(currIndex < newNVertices); + + newVertices.Add(vertices[i]); + ++currIndex; + } + + return newVertices; + } + + /// + /// Merges the identical points in the polygon. + /// + /// The vertices. + public static Vertices MergeIdenticalPoints(Vertices vertices) + { + HashSet unique = new HashSet(); + + foreach (Vector2 vertex in vertices) + { + unique.Add(vertex); + } + + return new Vertices(unique); + } + + /// + /// Reduces the polygon by distance. + /// + /// The vertices. + /// The distance between points. Points closer than this will be removed. + public static Vertices ReduceByDistance(Vertices vertices, float distance) + { + if (vertices.Count <= 3) + return vertices; + + float distance2 = distance * distance; + + Vertices simplified = new Vertices(vertices.Count); + + for (int i = 0; i < vertices.Count; i++) + { + Vector2 current = vertices[i]; + Vector2 next = vertices.NextVertex(i); + + //If they are closer than the distance, continue + if ((next - current).LengthSquared() <= distance2) + continue; + + simplified.Add(current); + } + + return simplified; + } + + /// + /// Reduces the polygon by removing the Nth vertex in the vertices list. + /// + /// The vertices. + /// The Nth point to remove. Example: 5. + /// + public static Vertices ReduceByNth(Vertices vertices, int nth) + { + if (vertices.Count <= 3) + return vertices; + + if (nth == 0) + return vertices; + + Vertices simplified = new Vertices(vertices.Count); + + for (int i = 0; i < vertices.Count; i++) + { + if (i % nth == 0) + continue; + + simplified.Add(vertices[i]); + } + + return simplified; + } + + /// + /// Simplify the polygon by removing all points that in pairs of 3 have an area less than the tolerance. + /// + /// Pass in 0 as tolerance, and it will only remove collinear points. + /// + /// + /// + /// + public static Vertices ReduceByArea(Vertices vertices, float areaTolerance) + { + //From physics2d.net + + if (vertices.Count <= 3) + return vertices; + + if (areaTolerance < 0) + throw new ArgumentOutOfRangeException("areaTolerance", "must be equal to or greater than zero."); + + Vertices simplified = new Vertices(vertices.Count); + Vector2 v3; + Vector2 v1 = vertices[vertices.Count - 2]; + Vector2 v2 = vertices[vertices.Count - 1]; + areaTolerance *= 2; + + for (int i = 0; i < vertices.Count; ++i, v2 = v3) + { + v3 = i == vertices.Count - 1 ? simplified[0] : vertices[i]; + + float old1; + MathUtils.Cross(ref v1, ref v2, out old1); + + float old2; + MathUtils.Cross(ref v2, ref v3, out old2); + + float new1; + MathUtils.Cross(ref v1, ref v3, out new1); + + if (Math.Abs(new1 - (old1 + old2)) > areaTolerance) + { + simplified.Add(v2); + v1 = v2; + } + } + + return simplified; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/PolygonManipulation/YuPengClipper.cs b/src/Box2DNet/Common/PolygonManipulation/YuPengClipper.cs new file mode 100644 index 0000000..63051db --- /dev/null +++ b/src/Box2DNet/Common/PolygonManipulation/YuPengClipper.cs @@ -0,0 +1,514 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Common.PolygonManipulation +{ + internal enum PolyClipType + { + Intersect, + Union, + Difference + } + + public enum PolyClipError + { + None, + DegeneratedOutput, + NonSimpleInput, + BrokenResult + } + + //Clipper contributed by Helge Backhaus + + public static class YuPengClipper + { + private const float ClipperEpsilonSquared = 1.192092896e-07f; + + public static List Union(Vertices polygon1, Vertices polygon2, out PolyClipError error) + { + return Execute(polygon1, polygon2, PolyClipType.Union, out error); + } + + public static List Difference(Vertices polygon1, Vertices polygon2, out PolyClipError error) + { + return Execute(polygon1, polygon2, PolyClipType.Difference, out error); + } + + public static List Intersect(Vertices polygon1, Vertices polygon2, out PolyClipError error) + { + return Execute(polygon1, polygon2, PolyClipType.Intersect, out error); + } + + /// + /// Implements "A new algorithm for Boolean operations on general polygons" + /// available here: http://liama.ia.ac.cn/wiki/_media/user:dong:dong_cg_05.pdf + /// Merges two polygons, a subject and a clip with the specified operation. Polygons may not be + /// self-intersecting. + /// + /// Warning: May yield incorrect results or even crash if polygons contain collinear points. + /// + /// The subject polygon. + /// The clip polygon, which is added, + /// substracted or intersected with the subject + /// The operation to be performed. Either + /// Union, Difference or Intersection. + /// The error generated (if any) + /// A list of closed polygons, which make up the result of the clipping operation. + /// Outer contours are ordered counter clockwise, holes are ordered clockwise. + private static List Execute(Vertices subject, Vertices clip, PolyClipType clipType, out PolyClipError error) + { + Debug.Assert(subject.IsSimple() && clip.IsSimple(), "Non simple input!", "Input polygons must be simple (cannot intersect themselves)."); + + // Copy polygons + Vertices slicedSubject; + Vertices slicedClip; + // Calculate the intersection and touch points between + // subject and clip and add them to both + CalculateIntersections(subject, clip, out slicedSubject, out slicedClip); + + // Translate polygons into upper right quadrant + // as the algorithm depends on it + Vector2 lbSubject = subject.GetAABB().LowerBound; + Vector2 lbClip = clip.GetAABB().LowerBound; + Vector2 translate; + Vector2.Min(ref lbSubject, ref lbClip, out translate); + translate = Vector2.One - translate; + if (translate != Vector2.Zero) + { + slicedSubject.Translate(ref translate); + slicedClip.Translate(ref translate); + } + + // Enforce counterclockwise contours + slicedSubject.ForceCounterClockWise(); + slicedClip.ForceCounterClockWise(); + + List subjectSimplices; + List subjectCoeff; + List clipSimplices; + List clipCoeff; + // Build simplical chains from the polygons and calculate the + // the corresponding coefficients + CalculateSimplicalChain(slicedSubject, out subjectCoeff, out subjectSimplices); + CalculateSimplicalChain(slicedClip, out clipCoeff, out clipSimplices); + + List resultSimplices; + + // Determine the characteristics function for all non-original edges + // in subject and clip simplical chain and combine the edges contributing + // to the result, depending on the clipType + CalculateResultChain(subjectCoeff, subjectSimplices, clipCoeff, clipSimplices, clipType, + out resultSimplices); + + List result; + // Convert result chain back to polygon(s) + error = BuildPolygonsFromChain(resultSimplices, out result); + + // Reverse the polygon translation from the beginning + // and remove collinear points from output + translate *= -1f; + for (int i = 0; i < result.Count; ++i) + { + result[i].Translate(ref translate); + SimplifyTools.CollinearSimplify(result[i]); + } + return result; + } + + /// + /// Calculates all intersections between two polygons. + /// + /// The first polygon. + /// The second polygon. + /// Returns the first polygon with added intersection points. + /// Returns the second polygon with added intersection points. + private static void CalculateIntersections(Vertices polygon1, Vertices polygon2, + out Vertices slicedPoly1, out Vertices slicedPoly2) + { + slicedPoly1 = new Vertices(polygon1); + slicedPoly2 = new Vertices(polygon2); + + // Iterate through polygon1's edges + for (int i = 0; i < polygon1.Count; i++) + { + // Get edge vertices + Vector2 a = polygon1[i]; + Vector2 b = polygon1[polygon1.NextIndex(i)]; + + // Get intersections between this edge and polygon2 + for (int j = 0; j < polygon2.Count; j++) + { + Vector2 c = polygon2[j]; + Vector2 d = polygon2[polygon2.NextIndex(j)]; + + Vector2 intersectionPoint; + // Check if the edges intersect + if (LineTools.LineIntersect(a, b, c, d, out intersectionPoint)) + { + // calculate alpha values for sorting multiple intersections points on a edge + float alpha; + // Insert intersection point into first polygon + alpha = GetAlpha(a, b, intersectionPoint); + if (alpha > 0f && alpha < 1f) + { + int index = slicedPoly1.IndexOf(a) + 1; + while (index < slicedPoly1.Count && + GetAlpha(a, b, slicedPoly1[index]) <= alpha) + { + ++index; + } + slicedPoly1.Insert(index, intersectionPoint); + } + // Insert intersection point into second polygon + alpha = GetAlpha(c, d, intersectionPoint); + if (alpha > 0f && alpha < 1f) + { + int index = slicedPoly2.IndexOf(c) + 1; + while (index < slicedPoly2.Count && + GetAlpha(c, d, slicedPoly2[index]) <= alpha) + { + ++index; + } + slicedPoly2.Insert(index, intersectionPoint); + } + } + } + } + // Check for very small edges + for (int i = 0; i < slicedPoly1.Count; ++i) + { + int iNext = slicedPoly1.NextIndex(i); + //If they are closer than the distance remove vertex + if ((slicedPoly1[iNext] - slicedPoly1[i]).LengthSquared() <= ClipperEpsilonSquared) + { + slicedPoly1.RemoveAt(i); + --i; + } + } + for (int i = 0; i < slicedPoly2.Count; ++i) + { + int iNext = slicedPoly2.NextIndex(i); + //If they are closer than the distance remove vertex + if ((slicedPoly2[iNext] - slicedPoly2[i]).LengthSquared() <= ClipperEpsilonSquared) + { + slicedPoly2.RemoveAt(i); + --i; + } + } + } + + /// + /// Calculates the simplical chain corresponding to the input polygon. + /// + /// Used by method Execute(). + private static void CalculateSimplicalChain(Vertices poly, out List coeff, + out List simplicies) + { + simplicies = new List(); + coeff = new List(); + for (int i = 0; i < poly.Count; ++i) + { + simplicies.Add(new Edge(poly[i], poly[poly.NextIndex(i)])); + coeff.Add(CalculateSimplexCoefficient(Vector2.Zero, poly[i], poly[poly.NextIndex(i)])); + } + } + + /// + /// Calculates the characteristics function for all edges of + /// the given simplical chains and builds the result chain. + /// + /// Used by method Execute(). + private static void CalculateResultChain(List poly1Coeff, List poly1Simplicies, + List poly2Coeff, List poly2Simplicies, + PolyClipType clipType, out List resultSimplices) + { + resultSimplices = new List(); + + for (int i = 0; i < poly1Simplicies.Count; ++i) + { + float edgeCharacter = 0; + if (poly2Simplicies.Contains(poly1Simplicies[i])) + { + edgeCharacter = 1f; + } + else if (poly2Simplicies.Contains(-poly1Simplicies[i]) && clipType == PolyClipType.Union) + { + edgeCharacter = 1f; + } + else + { + for (int j = 0; j < poly2Simplicies.Count; ++j) + { + if (!poly2Simplicies.Contains(-poly1Simplicies[i])) + { + edgeCharacter += CalculateBeta(poly1Simplicies[i].GetCenter(), + poly2Simplicies[j], poly2Coeff[j]); + } + } + } + if (clipType == PolyClipType.Intersect) + { + if (edgeCharacter == 1f) + { + resultSimplices.Add(poly1Simplicies[i]); + } + } + else + { + if (edgeCharacter == 0f) + { + resultSimplices.Add(poly1Simplicies[i]); + } + } + } + for (int i = 0; i < poly2Simplicies.Count; ++i) + { + float edgeCharacter = 0f; + if (!resultSimplices.Contains(poly2Simplicies[i]) && + !resultSimplices.Contains(-poly2Simplicies[i])) + { + if (poly1Simplicies.Contains(-poly2Simplicies[i]) && clipType == PolyClipType.Union) + { + edgeCharacter = 1f; + } + else + { + edgeCharacter = 0f; + for (int j = 0; j < poly1Simplicies.Count; ++j) + { + if (!poly1Simplicies.Contains(poly2Simplicies[i]) && !poly1Simplicies.Contains(-poly2Simplicies[i])) + { + edgeCharacter += CalculateBeta(poly2Simplicies[i].GetCenter(), + poly1Simplicies[j], poly1Coeff[j]); + } + } + if (clipType == PolyClipType.Intersect || clipType == PolyClipType.Difference) + { + if (edgeCharacter == 1f) + { + resultSimplices.Add(-poly2Simplicies[i]); + } + } + else + { + if (edgeCharacter == 0f) + { + resultSimplices.Add(poly2Simplicies[i]); + } + } + } + } + } + } + + /// + /// Calculates the polygon(s) from the result simplical chain. + /// + /// Used by method Execute(). + private static PolyClipError BuildPolygonsFromChain(List simplicies, out List result) + { + result = new List(); + PolyClipError errVal = PolyClipError.None; + + while (simplicies.Count > 0) + { + Vertices output = new Vertices(); + output.Add(simplicies[0].EdgeStart); + output.Add(simplicies[0].EdgeEnd); + simplicies.RemoveAt(0); + bool closed = false; + int index = 0; + int count = simplicies.Count; // Needed to catch infinite loops + while (!closed && simplicies.Count > 0) + { + if (VectorEqual(output[output.Count - 1], simplicies[index].EdgeStart)) + { + if (VectorEqual(simplicies[index].EdgeEnd, output[0])) + { + closed = true; + } + else + { + output.Add(simplicies[index].EdgeEnd); + } + simplicies.RemoveAt(index); + --index; + } + else if (VectorEqual(output[output.Count - 1], simplicies[index].EdgeEnd)) + { + if (VectorEqual(simplicies[index].EdgeStart, output[0])) + { + closed = true; + } + else + { + output.Add(simplicies[index].EdgeStart); + } + simplicies.RemoveAt(index); + --index; + } + if (!closed) + { + if (++index == simplicies.Count) + { + if (count == simplicies.Count) + { + result = new List(); + Debug.WriteLine("Undefined error while building result polygon(s)."); + return PolyClipError.BrokenResult; + } + index = 0; + count = simplicies.Count; + } + } + } + if (output.Count < 3) + { + errVal = PolyClipError.DegeneratedOutput; + Debug.WriteLine("Degenerated output polygon produced (vertices < 3)."); + } + result.Add(output); + } + return errVal; + } + + /// + /// Needed to calculate the characteristics function of a simplex. + /// + /// Used by method CalculateEdgeCharacter(). + private static float CalculateBeta(Vector2 point, Edge e, float coefficient) + { + float result = 0f; + if (PointInSimplex(point, e)) + { + result = coefficient; + } + if (PointOnLineSegment(Vector2.Zero, e.EdgeStart, point) || + PointOnLineSegment(Vector2.Zero, e.EdgeEnd, point)) + { + result = .5f * coefficient; + } + return result; + } + + /// + /// Needed for sorting multiple intersections points on the same edge. + /// + /// Used by method CalculateIntersections(). + private static float GetAlpha(Vector2 start, Vector2 end, Vector2 point) + { + return (point - start).LengthSquared() / (end - start).LengthSquared(); + } + + /// + /// Returns the coefficient of a simplex. + /// + /// Used by method CalculateSimplicalChain(). + private static float CalculateSimplexCoefficient(Vector2 a, Vector2 b, Vector2 c) + { + float isLeft = MathUtils.Area(ref a, ref b, ref c); + if (isLeft < 0f) + { + return -1f; + } + + if (isLeft > 0f) + { + return 1f; + } + + return 0f; + } + + /// + /// Winding number test for a point in a simplex. + /// + /// The point to be tested. + /// The edge that the point is tested against. + /// False if the winding number is even and the point is outside + /// the simplex and True otherwise. + private static bool PointInSimplex(Vector2 point, Edge edge) + { + Vertices polygon = new Vertices(); + polygon.Add(Vector2.Zero); + polygon.Add(edge.EdgeStart); + polygon.Add(edge.EdgeEnd); + return (polygon.PointInPolygon(ref point) == 1); + } + + /// + /// Tests if a point lies on a line segment. + /// + /// Used by method CalculateBeta(). + private static bool PointOnLineSegment(Vector2 start, Vector2 end, Vector2 point) + { + Vector2 segment = end - start; + return MathUtils.Area(ref start, ref end, ref point) == 0f && + Vector2.Dot(point - start, segment) >= 0f && + Vector2.Dot(point - end, segment) <= 0f; + } + + private static bool VectorEqual(Vector2 vec1, Vector2 vec2) + { + return (vec2 - vec1).LengthSquared() <= ClipperEpsilonSquared; + } + + #region Nested type: Edge + + /// Specifies an Edge. Edges are used to represent simplicies in simplical chains + private sealed class Edge + { + public Edge(Vector2 edgeStart, Vector2 edgeEnd) + { + EdgeStart = edgeStart; + EdgeEnd = edgeEnd; + } + + public Vector2 EdgeStart { get; private set; } + public Vector2 EdgeEnd { get; private set; } + + public Vector2 GetCenter() + { + return (EdgeStart + EdgeEnd) / 2f; + } + + public static Edge operator -(Edge e) + { + return new Edge(e.EdgeEnd, e.EdgeStart); + } + + public override bool Equals(Object obj) + { + // If parameter is null return false. + if (obj == null) + { + return false; + } + + // If parameter cannot be cast to Point return false. + return Equals(obj as Edge); + } + + public bool Equals(Edge e) + { + // If parameter is null return false: + if (e == null) + { + return false; + } + + // Return true if the fields match + return VectorEqual(EdgeStart, e.EdgeStart) && VectorEqual(EdgeEnd, e.EdgeEnd); + } + + public override int GetHashCode() + { + return EdgeStart.GetHashCode() ^ EdgeEnd.GetHashCode(); + } + } + + #endregion + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/PolygonTools.cs b/src/Box2DNet/Common/PolygonTools.cs new file mode 100644 index 0000000..9fc315e --- /dev/null +++ b/src/Box2DNet/Common/PolygonTools.cs @@ -0,0 +1,360 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using Box2DNet.Common.TextureTools; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Common +{ + public static class PolygonTools + { + /// + /// Build vertices to represent an axis-aligned box. + /// + /// the half-width. + /// the half-height. + public static Vertices CreateRectangle(float hx, float hy) + { + Vertices vertices = new Vertices(4); + vertices.Add(new Vector2(-hx, -hy)); + vertices.Add(new Vector2(hx, -hy)); + vertices.Add(new Vector2(hx, hy)); + vertices.Add(new Vector2(-hx, hy)); + + return vertices; + } + + /// + /// Build vertices to represent an oriented box. + /// + /// the half-width. + /// the half-height. + /// the center of the box in local coordinates. + /// the rotation of the box in local coordinates. + public static Vertices CreateRectangle(float hx, float hy, Vector2 center, float angle) + { + Vertices vertices = CreateRectangle(hx, hy); + + Transform xf = new Transform(); + xf.p = center; + xf.q.Set(angle); + + // Transform vertices + for (int i = 0; i < 4; ++i) + { + vertices[i] = MathUtils.Mul(ref xf, vertices[i]); + } + + return vertices; + } + + //Rounded rectangle contributed by Jonathan Smars - jsmars@gmail.com + + /// + /// Creates a rounded rectangle with the specified width and height. + /// + /// The width. + /// The height. + /// The rounding X radius. + /// The rounding Y radius. + /// The number of segments to subdivide the edges. + /// + public static Vertices CreateRoundedRectangle(float width, float height, float xRadius, float yRadius, + int segments) + { + if (yRadius > height / 2 || xRadius > width / 2) + throw new Exception("Rounding amount can't be more than half the height and width respectively."); + if (segments < 0) + throw new Exception("Segments must be zero or more."); + + //We need at least 8 vertices to create a rounded rectangle + Debug.Assert(Settings.MaxPolygonVertices >= 8); + + Vertices vertices = new Vertices(); + if (segments == 0) + { + vertices.Add(new Vector2(width * .5f - xRadius, -height * .5f)); + vertices.Add(new Vector2(width * .5f, -height * .5f + yRadius)); + + vertices.Add(new Vector2(width * .5f, height * .5f - yRadius)); + vertices.Add(new Vector2(width * .5f - xRadius, height * .5f)); + + vertices.Add(new Vector2(-width * .5f + xRadius, height * .5f)); + vertices.Add(new Vector2(-width * .5f, height * .5f - yRadius)); + + vertices.Add(new Vector2(-width * .5f, -height * .5f + yRadius)); + vertices.Add(new Vector2(-width * .5f + xRadius, -height * .5f)); + } + else + { + int numberOfEdges = (segments * 4 + 8); + + float stepSize = MathHelper.TwoPi / (numberOfEdges - 4); + int perPhase = numberOfEdges / 4; + + Vector2 posOffset = new Vector2(width / 2 - xRadius, height / 2 - yRadius); + vertices.Add(posOffset + new Vector2(xRadius, -yRadius + yRadius)); + short phase = 0; + for (int i = 1; i < numberOfEdges; i++) + { + if (i - perPhase == 0 || i - perPhase * 3 == 0) + { + posOffset.X *= -1; + phase--; + } + else if (i - perPhase * 2 == 0) + { + posOffset.Y *= -1; + phase--; + } + + vertices.Add(posOffset + new Vector2(xRadius * (float)Math.Cos(stepSize * -(i + phase)), + -yRadius * (float)Math.Sin(stepSize * -(i + phase)))); + } + } + + return vertices; + } + + /// + /// Set this as a single edge. + /// + /// The first point. + /// The second point. + public static Vertices CreateLine(Vector2 start, Vector2 end) + { + Vertices vertices = new Vertices(2); + vertices.Add(start); + vertices.Add(end); + + return vertices; + } + + /// + /// Creates a circle with the specified radius and number of edges. + /// + /// The radius. + /// The number of edges. The more edges, the more it resembles a circle + /// + public static Vertices CreateCircle(float radius, int numberOfEdges) + { + return CreateEllipse(radius, radius, numberOfEdges); + } + + /// + /// Creates a ellipse with the specified width, height and number of edges. + /// + /// Width of the ellipse. + /// Height of the ellipse. + /// The number of edges. The more edges, the more it resembles an ellipse + /// + public static Vertices CreateEllipse(float xRadius, float yRadius, int numberOfEdges) + { + Vertices vertices = new Vertices(); + + float stepSize = MathHelper.TwoPi / numberOfEdges; + + vertices.Add(new Vector2(xRadius, 0)); + for (int i = numberOfEdges - 1; i > 0; --i) + vertices.Add(new Vector2(xRadius * (float)Math.Cos(stepSize * i), + -yRadius * (float)Math.Sin(stepSize * i))); + + return vertices; + } + + public static Vertices CreateArc(float radians, int sides, float radius) + { + Debug.Assert(radians > 0, "The arc needs to be larger than 0"); + Debug.Assert(sides > 1, "The arc needs to have more than 1 sides"); + Debug.Assert(radius > 0, "The arc needs to have a radius larger than 0"); + + Vertices vertices = new Vertices(); + + float stepSize = radians / sides; + for (int i = sides - 1; i > 0; i--) + { + vertices.Add(new Vector2(radius * (float)Math.Cos(stepSize * i), + radius * (float)Math.Sin(stepSize * i))); + } + + return vertices; + } + + //Capsule contributed by Yobiv + + /// + /// Creates an capsule with the specified height, radius and number of edges. + /// A capsule has the same form as a pill capsule. + /// + /// Height (inner height + 2 * radius) of the capsule. + /// Radius of the capsule ends. + /// The number of edges of the capsule ends. The more edges, the more it resembles an capsule + /// + public static Vertices CreateCapsule(float height, float endRadius, int edges) + { + if (endRadius >= height / 2) + throw new ArgumentException( + "The radius must be lower than height / 2. Higher values of radius would create a circle, and not a half circle.", + "endRadius"); + + return CreateCapsule(height, endRadius, edges, endRadius, edges); + } + + /// + /// Creates an capsule with the specified height, radius and number of edges. + /// A capsule has the same form as a pill capsule. + /// + /// Height (inner height + radii) of the capsule. + /// Radius of the top. + /// The number of edges of the top. The more edges, the more it resembles an capsule + /// Radius of bottom. + /// The number of edges of the bottom. The more edges, the more it resembles an capsule + /// + public static Vertices CreateCapsule(float height, float topRadius, int topEdges, float bottomRadius, + int bottomEdges) + { + if (height <= 0) + throw new ArgumentException("Height must be longer than 0", "height"); + + if (topRadius <= 0) + throw new ArgumentException("The top radius must be more than 0", "topRadius"); + + if (topEdges <= 0) + throw new ArgumentException("Top edges must be more than 0", "topEdges"); + + if (bottomRadius <= 0) + throw new ArgumentException("The bottom radius must be more than 0", "bottomRadius"); + + if (bottomEdges <= 0) + throw new ArgumentException("Bottom edges must be more than 0", "bottomEdges"); + + if (topRadius >= height / 2) + throw new ArgumentException( + "The top radius must be lower than height / 2. Higher values of top radius would create a circle, and not a half circle.", + "topRadius"); + + if (bottomRadius >= height / 2) + throw new ArgumentException( + "The bottom radius must be lower than height / 2. Higher values of bottom radius would create a circle, and not a half circle.", + "bottomRadius"); + + Vertices vertices = new Vertices(); + + float newHeight = (height - topRadius - bottomRadius) * 0.5f; + + // top + vertices.Add(new Vector2(topRadius, newHeight)); + + float stepSize = MathHelper.Pi / topEdges; + for (int i = 1; i < topEdges; i++) + { + vertices.Add(new Vector2(topRadius * (float)Math.Cos(stepSize * i), + topRadius * (float)Math.Sin(stepSize * i) + newHeight)); + } + + vertices.Add(new Vector2(-topRadius, newHeight)); + + // bottom + vertices.Add(new Vector2(-bottomRadius, -newHeight)); + + stepSize = MathHelper.Pi / bottomEdges; + for (int i = 1; i < bottomEdges; i++) + { + vertices.Add(new Vector2(-bottomRadius * (float)Math.Cos(stepSize * i), + -bottomRadius * (float)Math.Sin(stepSize * i) - newHeight)); + } + + vertices.Add(new Vector2(bottomRadius, -newHeight)); + + return vertices; + } + + /// + /// Creates a gear shape with the specified radius and number of teeth. + /// + /// The radius. + /// The number of teeth. + /// The tip percentage. + /// Height of the tooth. + /// + public static Vertices CreateGear(float radius, int numberOfTeeth, float tipPercentage, float toothHeight) + { + Vertices vertices = new Vertices(); + + float stepSize = MathHelper.TwoPi / numberOfTeeth; + tipPercentage /= 100f; + MathHelper.Clamp(tipPercentage, 0f, 1f); + float toothTipStepSize = (stepSize / 2f) * tipPercentage; + + float toothAngleStepSize = (stepSize - (toothTipStepSize * 2f)) / 2f; + + for (int i = numberOfTeeth - 1; i >= 0; --i) + { + if (toothTipStepSize > 0f) + { + vertices.Add( + new Vector2(radius * + (float)Math.Cos(stepSize * i + toothAngleStepSize * 2f + toothTipStepSize), + -radius * + (float)Math.Sin(stepSize * i + toothAngleStepSize * 2f + toothTipStepSize))); + + vertices.Add( + new Vector2((radius + toothHeight) * + (float)Math.Cos(stepSize * i + toothAngleStepSize + toothTipStepSize), + -(radius + toothHeight) * + (float)Math.Sin(stepSize * i + toothAngleStepSize + toothTipStepSize))); + } + + vertices.Add(new Vector2((radius + toothHeight) * + (float)Math.Cos(stepSize * i + toothAngleStepSize), + -(radius + toothHeight) * + (float)Math.Sin(stepSize * i + toothAngleStepSize))); + + vertices.Add(new Vector2(radius * (float)Math.Cos(stepSize * i), + -radius * (float)Math.Sin(stepSize * i))); + } + + return vertices; + } + + /// + /// Detects the vertices by analyzing the texture data. + /// + /// The texture data. + /// The texture width. + /// + public static Vertices CreatePolygon(uint[] data, int width) + { + return TextureConverter.DetectVertices(data, width); + } + + /// + /// Detects the vertices by analyzing the texture data. + /// + /// The texture data. + /// The texture width. + /// if set to true it will perform hole detection. + /// + public static Vertices CreatePolygon(uint[] data, int width, bool holeDetection) + { + return TextureConverter.DetectVertices(data, width, holeDetection); + } + + /// + /// Detects the vertices by analyzing the texture data. + /// + /// The texture data. + /// The texture width. + /// The hull tolerance. + /// The alpha tolerance. + /// if set to true it will perform multi part detection. + /// if set to true it will perform hole detection. + /// + public static List CreatePolygon(uint[] data, int width, float hullTolerance, + byte alphaTolerance, bool multiPartDetection, bool holeDetection) + { + return TextureConverter.DetectVertices(data, width, hullTolerance, alphaTolerance, + multiPartDetection, holeDetection); + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Serialization.cs b/src/Box2DNet/Common/Serialization.cs new file mode 100644 index 0000000..2b0a01c --- /dev/null +++ b/src/Box2DNet/Common/Serialization.cs @@ -0,0 +1,1480 @@ +using System; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Text; +using System.Xml; +using System.Xml.Serialization; +using Box2DNet.Collision.Shapes; +using Box2DNet.Dynamics; +using Box2DNet.Dynamics.Joints; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Common +{ + /// + /// Serialize the world into an XML file + /// + public static class WorldSerializer + { + /// + /// Serialize the world to an XML file + /// + /// + /// + public static void Serialize(World world, string filename) + { + using (FileStream fs = new FileStream(filename, FileMode.Create)) + { + WorldXmlSerializer.Serialize(world, fs); + } + } + + /// + /// Deserialize the world from an XML file + /// + /// + public static World Deserialize(string filename) + { + using (FileStream fs = new FileStream(filename, FileMode.Open)) + { + return WorldXmlDeserializer.Deserialize(fs); + } + } + } + + internal static class WorldXmlSerializer + { + private static XmlWriter _writer; + + private static void SerializeShape(Shape shape) + { + _writer.WriteStartElement("Shape"); + _writer.WriteAttributeString("Type", shape.ShapeType.ToString()); + _writer.WriteAttributeString("Density", shape.Density.ToString()); + + switch (shape.ShapeType) + { + case ShapeType.Circle: + { + CircleShape circle = (CircleShape)shape; + + _writer.WriteElementString("Radius", circle.Radius.ToString()); + + WriteElement("Position", circle.Position); + } + break; + case ShapeType.Polygon: + { + PolygonShape poly = (PolygonShape)shape; + + _writer.WriteStartElement("Vertices"); + foreach (Vector2 v in poly.Vertices) + WriteElement("Vertex", v); + _writer.WriteEndElement(); + + WriteElement("Centroid", poly.MassData.Centroid); + } + break; + case ShapeType.Edge: + { + EdgeShape poly = (EdgeShape)shape; + WriteElement("Vertex1", poly.Vertex1); + WriteElement("Vertex2", poly.Vertex2); + } + break; + case ShapeType.Chain: + { + ChainShape chain = (ChainShape)shape; + + _writer.WriteStartElement("Vertices"); + foreach (Vector2 v in chain.Vertices) + WriteElement("Vertex", v); + _writer.WriteEndElement(); + + WriteElement("NextVertex", chain.NextVertex); + WriteElement("PrevVertex", chain.PrevVertex); + } + break; + default: + throw new Exception(); + } + + _writer.WriteEndElement(); + } + + private static void SerializeFixture(Fixture fixture) + { + _writer.WriteStartElement("Fixture"); + _writer.WriteAttributeString("Id", fixture.FixtureId.ToString()); + + _writer.WriteStartElement("FilterData"); + _writer.WriteElementString("CategoryBits", ((int)fixture.CollisionCategories).ToString()); + _writer.WriteElementString("MaskBits", ((int)fixture.CollidesWith).ToString()); + _writer.WriteElementString("GroupIndex", fixture.CollisionGroup.ToString()); + _writer.WriteElementString("CollisionIgnores", Join("|", fixture._collisionIgnores)); + _writer.WriteEndElement(); + + _writer.WriteElementString("Friction", fixture.Friction.ToString()); + _writer.WriteElementString("IsSensor", fixture.IsSensor.ToString()); + _writer.WriteElementString("Restitution", fixture.Restitution.ToString()); + + if (fixture.UserData != null) + { + _writer.WriteStartElement("UserData"); + WriteDynamicType(fixture.UserData.GetType(), fixture.UserData); + _writer.WriteEndElement(); + } + + _writer.WriteEndElement(); + } + + private static void SerializeBody(List fixtures, List shapes, Body body) + { + _writer.WriteStartElement("Body"); + _writer.WriteAttributeString("Type", body.BodyType.ToString()); + _writer.WriteElementString("Active", body.Enabled.ToString()); + _writer.WriteElementString("AllowSleep", body.SleepingAllowed.ToString()); + _writer.WriteElementString("Angle", body.Rotation.ToString()); + _writer.WriteElementString("AngularDamping", body.AngularDamping.ToString()); + _writer.WriteElementString("AngularVelocity", body.AngularVelocity.ToString()); + _writer.WriteElementString("Awake", body.Awake.ToString()); + _writer.WriteElementString("Bullet", body.IsBullet.ToString()); + _writer.WriteElementString("FixedRotation", body.FixedRotation.ToString()); + _writer.WriteElementString("LinearDamping", body.LinearDamping.ToString()); + WriteElement("LinearVelocity", body.LinearVelocity); + WriteElement("Position", body.Position); + + if (body.UserData != null) + { + _writer.WriteStartElement("UserData"); + WriteDynamicType(body.UserData.GetType(), body.UserData); + _writer.WriteEndElement(); + } + + _writer.WriteStartElement("Bindings"); + for (int i = 0; i < body.FixtureList.Count; i++) + { + _writer.WriteStartElement("Pair"); + _writer.WriteAttributeString("FixtureId", FindIndex(fixtures, body.FixtureList[i]).ToString()); + _writer.WriteAttributeString("ShapeId", FindIndex(shapes, body.FixtureList[i].Shape).ToString()); + _writer.WriteEndElement(); + } + + _writer.WriteEndElement(); + _writer.WriteEndElement(); + } + + private static void SerializeJoint(List bodies, Joint joint) + { + _writer.WriteStartElement("Joint"); + _writer.WriteAttributeString("Type", joint.JointType.ToString()); + + WriteElement("BodyA", FindIndex(bodies, joint.BodyA)); + WriteElement("BodyB", FindIndex(bodies, joint.BodyB)); + + WriteElement("CollideConnected", joint.CollideConnected); + + WriteElement("Breakpoint", joint.Breakpoint); + + if (joint.UserData != null) + { + _writer.WriteStartElement("UserData"); + WriteDynamicType(joint.UserData.GetType(), joint.UserData); + _writer.WriteEndElement(); + } + + switch (joint.JointType) + { + case JointType.Distance: + { + DistanceJoint distanceJoint = (DistanceJoint)joint; + WriteElement("DampingRatio", distanceJoint.DampingRatio); + WriteElement("FrequencyHz", distanceJoint.Frequency); + WriteElement("Length", distanceJoint.Length); + WriteElement("LocalAnchorA", distanceJoint.LocalAnchorA); + WriteElement("LocalAnchorB", distanceJoint.LocalAnchorB); + } + break; + case JointType.Friction: + { + FrictionJoint frictionJoint = (FrictionJoint)joint; + WriteElement("LocalAnchorA", frictionJoint.LocalAnchorA); + WriteElement("LocalAnchorB", frictionJoint.LocalAnchorB); + WriteElement("MaxForce", frictionJoint.MaxForce); + WriteElement("MaxTorque", frictionJoint.MaxTorque); + } + break; + case JointType.Gear: + throw new Exception("Gear joint not supported by serialization"); + case JointType.Wheel: + { + WheelJoint wheelJoint = (WheelJoint)joint; + WriteElement("EnableMotor", wheelJoint.MotorEnabled); + WriteElement("LocalAnchorA", wheelJoint.LocalAnchorA); + WriteElement("LocalAnchorB", wheelJoint.LocalAnchorB); + WriteElement("MotorSpeed", wheelJoint.MotorSpeed); + WriteElement("DampingRatio", wheelJoint.DampingRatio); + WriteElement("MaxMotorTorque", wheelJoint.MaxMotorTorque); + WriteElement("FrequencyHz", wheelJoint.Frequency); + WriteElement("Axis", wheelJoint.Axis); + } + break; + case JointType.Prismatic: + { + //NOTE: Does not conform with Box2DScene + + PrismaticJoint prismaticJoint = (PrismaticJoint)joint; + WriteElement("EnableLimit", prismaticJoint.LimitEnabled); + WriteElement("EnableMotor", prismaticJoint.MotorEnabled); + WriteElement("LocalAnchorA", prismaticJoint.LocalAnchorA); + WriteElement("LocalAnchorB", prismaticJoint.LocalAnchorB); + WriteElement("Axis", prismaticJoint.Axis); + WriteElement("LowerTranslation", prismaticJoint.LowerLimit); + WriteElement("UpperTranslation", prismaticJoint.UpperLimit); + WriteElement("MaxMotorForce", prismaticJoint.MaxMotorForce); + WriteElement("MotorSpeed", prismaticJoint.MotorSpeed); + } + break; + case JointType.Pulley: + { + PulleyJoint pulleyJoint = (PulleyJoint)joint; + WriteElement("WorldAnchorA", pulleyJoint.WorldAnchorA); + WriteElement("WorldAnchorB", pulleyJoint.WorldAnchorB); + WriteElement("LengthA", pulleyJoint.LengthA); + WriteElement("LengthB", pulleyJoint.LengthB); + WriteElement("LocalAnchorA", pulleyJoint.LocalAnchorA); + WriteElement("LocalAnchorB", pulleyJoint.LocalAnchorB); + WriteElement("Ratio", pulleyJoint.Ratio); + WriteElement("Constant", pulleyJoint.Constant); + } + break; + case JointType.Revolute: + { + RevoluteJoint revoluteJoint = (RevoluteJoint)joint; + WriteElement("EnableLimit", revoluteJoint.LimitEnabled); + WriteElement("EnableMotor", revoluteJoint.MotorEnabled); + WriteElement("LocalAnchorA", revoluteJoint.LocalAnchorA); + WriteElement("LocalAnchorB", revoluteJoint.LocalAnchorB); + WriteElement("LowerAngle", revoluteJoint.LowerLimit); + WriteElement("MaxMotorTorque", revoluteJoint.MaxMotorTorque); + WriteElement("MotorSpeed", revoluteJoint.MotorSpeed); + WriteElement("ReferenceAngle", revoluteJoint.ReferenceAngle); + WriteElement("UpperAngle", revoluteJoint.UpperLimit); + } + break; + case JointType.Weld: + { + WeldJoint weldJoint = (WeldJoint)joint; + WriteElement("LocalAnchorA", weldJoint.LocalAnchorA); + WriteElement("LocalAnchorB", weldJoint.LocalAnchorB); + } + break; + // + // Not part of Box2DScene + // + case JointType.Rope: + { + RopeJoint ropeJoint = (RopeJoint)joint; + WriteElement("LocalAnchorA", ropeJoint.LocalAnchorA); + WriteElement("LocalAnchorB", ropeJoint.LocalAnchorB); + WriteElement("MaxLength", ropeJoint.MaxLength); + } + break; + case JointType.Angle: + { + AngleJoint angleJoint = (AngleJoint)joint; + WriteElement("BiasFactor", angleJoint.BiasFactor); + WriteElement("MaxImpulse", angleJoint.MaxImpulse); + WriteElement("Softness", angleJoint.Softness); + WriteElement("TargetAngle", angleJoint.TargetAngle); + } + break; + case JointType.Motor: + { + MotorJoint motorJoint = (MotorJoint)joint; + WriteElement("AngularOffset", motorJoint.AngularOffset); + WriteElement("LinearOffset", motorJoint.LinearOffset); + WriteElement("MaxForce", motorJoint.MaxForce); + WriteElement("MaxTorque", motorJoint.MaxTorque); + WriteElement("CorrectionFactor", motorJoint.CorrectionFactor); + } + break; + default: + throw new Exception("Joint not supported"); + } + + _writer.WriteEndElement(); + } + + private static void WriteDynamicType(Type type, object val) + { + _writer.WriteElementString("Type", type.AssemblyQualifiedName); + + _writer.WriteStartElement("Value"); + XmlSerializer serializer = new XmlSerializer(type); + XmlSerializerNamespaces xmlnsEmpty = new XmlSerializerNamespaces(); + xmlnsEmpty.Add("", ""); + serializer.Serialize(_writer, val, xmlnsEmpty); + _writer.WriteEndElement(); + } + + private static void WriteElement(string name, Vector2 vec) + { + _writer.WriteElementString(name, vec.X + " " + vec.Y); + } + + private static void WriteElement(string name, int val) + { + _writer.WriteElementString(name, val.ToString()); + } + + private static void WriteElement(string name, bool val) + { + _writer.WriteElementString(name, val.ToString()); + } + + private static void WriteElement(string name, float val) + { + _writer.WriteElementString(name, val.ToString()); + } + + private static int FindIndex(List list, Body item) + { + for (int i = 0; i < list.Count; ++i) + if (list[i] == item) + return i; + + return -1; + } + + private static int FindIndex(List list, Fixture item) + { + for (int i = 0; i < list.Count; ++i) + if (list[i].CompareTo(item)) + return i; + + return -1; + } + + private static int FindIndex(List list, Shape item) + { + for (int i = 0; i < list.Count; ++i) + if (list[i].CompareTo(item)) + return i; + + return -1; + } + + private static String Join(String separator, IEnumerable values) + { + using (IEnumerator en = values.GetEnumerator()) + { + if (!en.MoveNext()) + return String.Empty; + + StringBuilder result = new StringBuilder(); + if (en.Current != null) + { + // handle the case that the enumeration has null entries + // and the case where their ToString() override is broken + string value = en.Current.ToString(); + if (value != null) + result.Append(value); + } + + while (en.MoveNext()) + { + result.Append(separator); + if (en.Current != null) + { + // handle the case that the enumeration has null entries + // and the case where their ToString() override is broken + string value = en.Current.ToString(); + if (value != null) + result.Append(value); + } + } + return result.ToString(); + } + } + + internal static void Serialize(World world, Stream stream) + { + List bodies = new List(); + List fixtures = new List(); + List shapes = new List(); + + XmlWriterSettings settings = new XmlWriterSettings(); + settings.Indent = true; + settings.NewLineOnAttributes = false; + settings.OmitXmlDeclaration = true; + + _writer = XmlWriter.Create(stream, settings); + + _writer.WriteStartElement("World"); + _writer.WriteAttributeString("Version", "3"); + WriteElement("Gravity", world.Gravity); + + _writer.WriteStartElement("Shapes"); + + foreach (Body body in world.BodyList) + { + foreach (Fixture fixture in body.FixtureList) + { + if (!shapes.Any(s2 => fixture.Shape.CompareTo(s2))) + { + SerializeShape(fixture.Shape); + shapes.Add(fixture.Shape); + } + } + } + + _writer.WriteEndElement(); + _writer.WriteStartElement("Fixtures"); + + foreach (Body body in world.BodyList) + { + foreach (Fixture fixture in body.FixtureList) + { + if (!fixtures.Any(f2 => fixture.CompareTo(f2))) + { + SerializeFixture(fixture); + fixtures.Add(fixture); + } + } + } + + _writer.WriteEndElement(); + _writer.WriteStartElement("Bodies"); + + foreach (Body body in world.BodyList) + { + bodies.Add(body); + SerializeBody(fixtures, shapes, body); + } + + _writer.WriteEndElement(); + _writer.WriteStartElement("Joints"); + + foreach (Joint joint in world.JointList) + { + SerializeJoint(bodies, joint); + } + + _writer.WriteEndElement(); + _writer.WriteEndElement(); + + _writer.Flush(); + _writer.Close(); + } + } + + internal static class WorldXmlDeserializer + { + internal static World Deserialize(Stream stream) + { + World world = new World(Vector2.Zero); + Deserialize(world, stream); + return world; + } + + private static void Deserialize(World world, Stream stream) + { + List bodies = new List(); + List fixtures = new List(); + List joints = new List(); + List shapes = new List(); + + XMLFragmentElement root = XMLFragmentParser.LoadFromStream(stream); + + if (root.Name.ToLower() != "world") + throw new Exception(); + + //Read gravity + foreach (XMLFragmentElement element in root.Elements) + { + if (element.Name.ToLower() == "gravity") + { + world.Gravity = ReadVector(element); + break; + } + } + + //Read shapes + foreach (XMLFragmentElement shapeElement in root.Elements) + { + if (shapeElement.Name.ToLower() == "shapes") + { + foreach (XMLFragmentElement element in shapeElement.Elements) + { + if (element.Name.ToLower() != "shape") + throw new Exception(); + + ShapeType type = (ShapeType)Enum.Parse(typeof(ShapeType), element.Attributes[0].Value, true); + float density = float.Parse(element.Attributes[1].Value); + + switch (type) + { + case ShapeType.Circle: + { + CircleShape shape = new CircleShape(); + shape._density = density; + + foreach (XMLFragmentElement sn in element.Elements) + { + switch (sn.Name.ToLower()) + { + case "radius": + shape.Radius = float.Parse(sn.Value); + break; + case "position": + shape.Position = ReadVector(sn); + break; + default: + throw new Exception(); + } + } + + shapes.Add(shape); + } + break; + case ShapeType.Polygon: + { + PolygonShape shape = new PolygonShape(); + shape._density = density; + + foreach (XMLFragmentElement sn in element.Elements) + { + switch (sn.Name.ToLower()) + { + case "vertices": + { + List verts = new List(sn.Elements.Count); + + foreach (XMLFragmentElement vert in sn.Elements) + verts.Add(ReadVector(vert)); + + shape.Vertices = new Vertices(verts); + } + break; + case "centroid": + shape.MassData.Centroid = ReadVector(sn); + break; + } + } + + shapes.Add(shape); + } + break; + case ShapeType.Edge: + { + EdgeShape shape = new EdgeShape(); + shape._density = density; + + foreach (XMLFragmentElement sn in element.Elements) + { + switch (sn.Name.ToLower()) + { + case "hasvertex0": + shape.HasVertex0 = bool.Parse(sn.Value); + break; + case "hasvertex3": + shape.HasVertex0 = bool.Parse(sn.Value); + break; + case "vertex0": + shape.Vertex0 = ReadVector(sn); + break; + case "vertex1": + shape.Vertex1 = ReadVector(sn); + break; + case "vertex2": + shape.Vertex2 = ReadVector(sn); + break; + case "vertex3": + shape.Vertex3 = ReadVector(sn); + break; + default: + throw new Exception(); + } + } + shapes.Add(shape); + } + break; + case ShapeType.Chain: + { + ChainShape shape = new ChainShape(); + shape._density = density; + + foreach (XMLFragmentElement sn in element.Elements) + { + switch (sn.Name.ToLower()) + { + case "vertices": + { + List verts = new List(sn.Elements.Count); + + foreach (XMLFragmentElement vert in sn.Elements) + verts.Add(ReadVector(vert)); + + shape.Vertices = new Vertices(verts); + } + break; + case "nextvertex": + shape.NextVertex = ReadVector(sn); + break; + case "prevvertex": + shape.PrevVertex = ReadVector(sn); + break; + + default: + throw new Exception(); + } + } + shapes.Add(shape); + } + break; + } + } + } + } + + //Read fixtures + foreach (XMLFragmentElement fixtureElement in root.Elements) + { + if (fixtureElement.Name.ToLower() == "fixtures") + { + foreach (XMLFragmentElement element in fixtureElement.Elements) + { + Fixture fixture = new Fixture(); + + if (element.Name.ToLower() != "fixture") + throw new Exception(); + + fixture.FixtureId = int.Parse(element.Attributes[0].Value); + + foreach (XMLFragmentElement sn in element.Elements) + { + switch (sn.Name.ToLower()) + { + case "filterdata": + foreach (XMLFragmentElement ssn in sn.Elements) + { + switch (ssn.Name.ToLower()) + { + case "categorybits": + fixture._collisionCategories = (Category)int.Parse(ssn.Value); + break; + case "maskbits": + fixture._collidesWith = (Category)int.Parse(ssn.Value); + break; + case "groupindex": + fixture._collisionGroup = short.Parse(ssn.Value); + break; + case "CollisionIgnores": + string[] split = ssn.Value.Split('|'); + foreach (string s in split) + { + fixture._collisionIgnores.Add(int.Parse(s)); + } + break; + } + } + + break; + case "friction": + fixture.Friction = float.Parse(sn.Value); + break; + case "issensor": + fixture.IsSensor = bool.Parse(sn.Value); + break; + case "restitution": + fixture.Restitution = float.Parse(sn.Value); + break; + case "userdata": + fixture.UserData = ReadSimpleType(sn, null, false); + break; + } + } + + fixtures.Add(fixture); + } + } + } + + //Read bodies + foreach (XMLFragmentElement bodyElement in root.Elements) + { + if (bodyElement.Name.ToLower() == "bodies") + { + foreach (XMLFragmentElement element in bodyElement.Elements) + { + Body body = new Body(world); + + if (element.Name.ToLower() != "body") + throw new Exception(); + + body.BodyType = (BodyType)Enum.Parse(typeof(BodyType), element.Attributes[0].Value, true); + + foreach (XMLFragmentElement sn in element.Elements) + { + switch (sn.Name.ToLower()) + { + case "active": + body._enabled = bool.Parse(sn.Value); + break; + case "allowsleep": + body.SleepingAllowed = bool.Parse(sn.Value); + break; + case "angle": + { + Vector2 position = body.Position; + body.SetTransformIgnoreContacts(ref position, float.Parse(sn.Value)); + } + break; + case "angulardamping": + body.AngularDamping = float.Parse(sn.Value); + break; + case "angularvelocity": + body.AngularVelocity = float.Parse(sn.Value); + break; + case "awake": + body.Awake = bool.Parse(sn.Value); + break; + case "bullet": + body.IsBullet = bool.Parse(sn.Value); + break; + case "fixedrotation": + body.FixedRotation = bool.Parse(sn.Value); + break; + case "lineardamping": + body.LinearDamping = float.Parse(sn.Value); + break; + case "linearvelocity": + body.LinearVelocity = ReadVector(sn); + break; + case "position": + { + float rotation = body.Rotation; + Vector2 position = ReadVector(sn); + body.SetTransformIgnoreContacts(ref position, rotation); + } + break; + case "userdata": + body.UserData = ReadSimpleType(sn, null, false); + break; + case "bindings": + { + foreach (XMLFragmentElement pair in sn.Elements) + { + Fixture fix = fixtures[int.Parse(pair.Attributes[0].Value)]; + fix.Shape = shapes[int.Parse(pair.Attributes[1].Value)].Clone(); + fix.CloneOnto(body); + } + break; + } + } + } + + bodies.Add(body); + } + } + } + + //Read joints + foreach (XMLFragmentElement jointElement in root.Elements) + { + if (jointElement.Name.ToLower() == "joints") + { + foreach (XMLFragmentElement n in jointElement.Elements) + { + Joint joint; + + if (n.Name.ToLower() != "joint") + throw new Exception(); + + JointType type = (JointType)Enum.Parse(typeof(JointType), n.Attributes[0].Value, true); + + int bodyAIndex = -1, bodyBIndex = -1; + bool collideConnected = false; + object userData = null; + + foreach (XMLFragmentElement sn in n.Elements) + { + switch (sn.Name.ToLower()) + { + case "bodya": + bodyAIndex = int.Parse(sn.Value); + break; + case "bodyb": + bodyBIndex = int.Parse(sn.Value); + break; + case "collideconnected": + collideConnected = bool.Parse(sn.Value); + break; + case "userdata": + userData = ReadSimpleType(sn, null, false); + break; + } + } + + Body bodyA = bodies[bodyAIndex]; + Body bodyB = bodies[bodyBIndex]; + + switch (type) + { + //case JointType.FixedMouse: + // joint = new FixedMouseJoint(); + // break; + //case JointType.FixedRevolute: + // break; + //case JointType.FixedDistance: + // break; + //case JointType.FixedLine: + // break; + //case JointType.FixedPrismatic: + // break; + //case JointType.FixedAngle: + // break; + //case JointType.FixedFriction: + // break; + case JointType.Distance: + joint = new DistanceJoint(); + break; + case JointType.Friction: + joint = new FrictionJoint(); + break; + case JointType.Wheel: + joint = new WheelJoint(); + break; + case JointType.Prismatic: + joint = new PrismaticJoint(); + break; + case JointType.Pulley: + joint = new PulleyJoint(); + break; + case JointType.Revolute: + joint = new RevoluteJoint(); + break; + case JointType.Weld: + joint = new WeldJoint(); + break; + case JointType.Rope: + joint = new RopeJoint(); + break; + case JointType.Angle: + joint = new AngleJoint(); + break; + case JointType.Motor: + joint = new MotorJoint(); + break; + case JointType.Gear: + throw new Exception("GearJoint is not supported."); + default: + throw new Exception("Invalid or unsupported joint."); + } + + joint.CollideConnected = collideConnected; + joint.UserData = userData; + joint.BodyA = bodyA; + joint.BodyB = bodyB; + joints.Add(joint); + world.AddJoint(joint); + + foreach (XMLFragmentElement sn in n.Elements) + { + // check for specific nodes + switch (type) + { + case JointType.Distance: + { + switch (sn.Name.ToLower()) + { + case "dampingratio": + ((DistanceJoint)joint).DampingRatio = float.Parse(sn.Value); + break; + case "frequencyhz": + ((DistanceJoint)joint).Frequency = float.Parse(sn.Value); + break; + case "length": + ((DistanceJoint)joint).Length = float.Parse(sn.Value); + break; + case "localanchora": + ((DistanceJoint)joint).LocalAnchorA = ReadVector(sn); + break; + case "localanchorb": + ((DistanceJoint)joint).LocalAnchorB = ReadVector(sn); + break; + } + } + break; + case JointType.Friction: + { + switch (sn.Name.ToLower()) + { + case "localanchora": + ((FrictionJoint)joint).LocalAnchorA = ReadVector(sn); + break; + case "localanchorb": + ((FrictionJoint)joint).LocalAnchorB = ReadVector(sn); + break; + case "maxforce": + ((FrictionJoint)joint).MaxForce = float.Parse(sn.Value); + break; + case "maxtorque": + ((FrictionJoint)joint).MaxTorque = float.Parse(sn.Value); + break; + } + } + break; + case JointType.Wheel: + { + switch (sn.Name.ToLower()) + { + case "enablemotor": + ((WheelJoint)joint).MotorEnabled = bool.Parse(sn.Value); + break; + case "localanchora": + ((WheelJoint)joint).LocalAnchorA = ReadVector(sn); + break; + case "localanchorb": + ((WheelJoint)joint).LocalAnchorB = ReadVector(sn); + break; + case "motorspeed": + ((WheelJoint)joint).MotorSpeed = float.Parse(sn.Value); + break; + case "dampingratio": + ((WheelJoint)joint).DampingRatio = float.Parse(sn.Value); + break; + case "maxmotortorque": + ((WheelJoint)joint).MaxMotorTorque = float.Parse(sn.Value); + break; + case "frequencyhz": + ((WheelJoint)joint).Frequency = float.Parse(sn.Value); + break; + case "axis": + ((WheelJoint)joint).Axis = ReadVector(sn); + break; + } + } + break; + case JointType.Prismatic: + { + switch (sn.Name.ToLower()) + { + case "enablelimit": + ((PrismaticJoint)joint).LimitEnabled = bool.Parse(sn.Value); + break; + case "enablemotor": + ((PrismaticJoint)joint).MotorEnabled = bool.Parse(sn.Value); + break; + case "localanchora": + ((PrismaticJoint)joint).LocalAnchorA = ReadVector(sn); + break; + case "localanchorb": + ((PrismaticJoint)joint).LocalAnchorB = ReadVector(sn); + break; + case "axis": + ((PrismaticJoint)joint).Axis = ReadVector(sn); + break; + case "maxmotorforce": + ((PrismaticJoint)joint).MaxMotorForce = float.Parse(sn.Value); + break; + case "motorspeed": + ((PrismaticJoint)joint).MotorSpeed = float.Parse(sn.Value); + break; + case "lowertranslation": + ((PrismaticJoint)joint).LowerLimit = float.Parse(sn.Value); + break; + case "uppertranslation": + ((PrismaticJoint)joint).UpperLimit = float.Parse(sn.Value); + break; + case "referenceangle": + ((PrismaticJoint)joint).ReferenceAngle = float.Parse(sn.Value); + break; + } + } + break; + case JointType.Pulley: + { + switch (sn.Name.ToLower()) + { + case "worldanchora": + ((PulleyJoint)joint).WorldAnchorA = ReadVector(sn); + break; + case "worldanchorb": + ((PulleyJoint)joint).WorldAnchorB = ReadVector(sn); + break; + case "lengtha": + ((PulleyJoint)joint).LengthA = float.Parse(sn.Value); + break; + case "lengthb": + ((PulleyJoint)joint).LengthB = float.Parse(sn.Value); + break; + case "localanchora": + ((PulleyJoint)joint).LocalAnchorA = ReadVector(sn); + break; + case "localanchorb": + ((PulleyJoint)joint).LocalAnchorB = ReadVector(sn); + break; + case "ratio": + ((PulleyJoint)joint).Ratio = float.Parse(sn.Value); + break; + case "constant": + ((PulleyJoint)joint).Constant = float.Parse(sn.Value); + break; + } + } + break; + case JointType.Revolute: + { + switch (sn.Name.ToLower()) + { + case "enablelimit": + ((RevoluteJoint)joint).LimitEnabled = bool.Parse(sn.Value); + break; + case "enablemotor": + ((RevoluteJoint)joint).MotorEnabled = bool.Parse(sn.Value); + break; + case "localanchora": + ((RevoluteJoint)joint).LocalAnchorA = ReadVector(sn); + break; + case "localanchorb": + ((RevoluteJoint)joint).LocalAnchorB = ReadVector(sn); + break; + case "maxmotortorque": + ((RevoluteJoint)joint).MaxMotorTorque = float.Parse(sn.Value); + break; + case "motorspeed": + ((RevoluteJoint)joint).MotorSpeed = float.Parse(sn.Value); + break; + case "lowerangle": + ((RevoluteJoint)joint).LowerLimit = float.Parse(sn.Value); + break; + case "upperangle": + ((RevoluteJoint)joint).UpperLimit = float.Parse(sn.Value); + break; + case "referenceangle": + ((RevoluteJoint)joint).ReferenceAngle = float.Parse(sn.Value); + break; + } + } + break; + case JointType.Weld: + { + switch (sn.Name.ToLower()) + { + case "localanchora": + ((WeldJoint)joint).LocalAnchorA = ReadVector(sn); + break; + case "localanchorb": + ((WeldJoint)joint).LocalAnchorB = ReadVector(sn); + break; + } + } + break; + case JointType.Rope: + { + switch (sn.Name.ToLower()) + { + case "localanchora": + ((RopeJoint)joint).LocalAnchorA = ReadVector(sn); + break; + case "localanchorb": + ((RopeJoint)joint).LocalAnchorB = ReadVector(sn); + break; + case "maxlength": + ((RopeJoint)joint).MaxLength = float.Parse(sn.Value); + break; + } + } + break; + case JointType.Gear: + throw new Exception("Gear joint is unsupported"); + case JointType.Angle: + { + switch (sn.Name.ToLower()) + { + case "biasfactor": + ((AngleJoint)joint).BiasFactor = float.Parse(sn.Value); + break; + case "maximpulse": + ((AngleJoint)joint).MaxImpulse = float.Parse(sn.Value); + break; + case "softness": + ((AngleJoint)joint).Softness = float.Parse(sn.Value); + break; + case "targetangle": + ((AngleJoint)joint).TargetAngle = float.Parse(sn.Value); + break; + } + } + break; + case JointType.Motor: + switch (sn.Name.ToLower()) + { + case "angularoffset": + ((MotorJoint)joint).AngularOffset = float.Parse(sn.Value); + break; + case "linearoffset": + ((MotorJoint)joint).LinearOffset = ReadVector(sn); + break; + case "maxforce": + ((MotorJoint)joint).MaxForce = float.Parse(sn.Value); + break; + case "maxtorque": + ((MotorJoint)joint).MaxTorque = float.Parse(sn.Value); + break; + case "correctionfactor": + ((MotorJoint)joint).CorrectionFactor = float.Parse(sn.Value); + break; + } + break; + } + } + } + } + } + + world.ProcessChanges(); + } + + private static Vector2 ReadVector(XMLFragmentElement node) + { + string[] values = node.Value.Split(' '); + return new Vector2(float.Parse(values[0]), float.Parse(values[1])); + } + + private static object ReadSimpleType(XMLFragmentElement node, Type type, bool outer) + { + if (type == null) + return ReadSimpleType(node.Elements[1], Type.GetType(node.Elements[0].Value), outer); + + XmlSerializer serializer = new XmlSerializer(type); + XmlSerializerNamespaces xmlnsEmpty = new XmlSerializerNamespaces(); + xmlnsEmpty.Add("", ""); + + using (MemoryStream stream = new MemoryStream()) + { + StreamWriter writer = new StreamWriter(stream); + { + writer.Write((outer) ? node.OuterXml : node.InnerXml); + writer.Flush(); + stream.Position = 0; + } + XmlReaderSettings settings = new XmlReaderSettings(); + settings.ConformanceLevel = ConformanceLevel.Fragment; + + return serializer.Deserialize(XmlReader.Create(stream, settings)); + } + } + } + + #region XMLFragment + + internal class XMLFragmentAttribute + { + public string Name { get; set; } + public string Value { get; set; } + } + + internal class XMLFragmentElement + { + private List _attributes = new List(); + private List _elements = new List(); + + public IList Elements + { + get { return _elements; } + } + + public IList Attributes + { + get { return _attributes; } + } + + public string Name { get; set; } + public string Value { get; set; } + public string OuterXml { get; set; } + public string InnerXml { get; set; } + } + + internal class XMLFragmentException : Exception + { + public XMLFragmentException(string message) + : base(message) + { + } + } + + internal class FileBuffer + { + public FileBuffer(Stream stream) + { + using (StreamReader sr = new StreamReader(stream)) + Buffer = sr.ReadToEnd(); + + Position = 0; + } + + public string Buffer { get; set; } + + public int Position { get; set; } + + private int Length + { + get { return Buffer.Length; } + } + + public char Next + { + get + { + char c = Buffer[Position]; + Position++; + return c; + } + } + + public bool EndOfBuffer + { + get { return Position == Length; } + } + } + + internal class XMLFragmentParser + { + private static List _punctuation = new List { '/', '<', '>', '=' }; + private FileBuffer _buffer; + private XMLFragmentElement _rootNode; + + public XMLFragmentParser(Stream stream) + { + Load(stream); + } + + public XMLFragmentParser(string fileName) + { + using (FileStream fs = new FileStream(fileName, FileMode.Open, FileAccess.Read)) + Load(fs); + } + + public XMLFragmentElement RootNode + { + get { return _rootNode; } + } + + public void Load(Stream stream) + { + _buffer = new FileBuffer(stream); + } + + public static XMLFragmentElement LoadFromStream(Stream stream) + { + XMLFragmentParser x = new XMLFragmentParser(stream); + x.Parse(); + return x.RootNode; + } + + private string NextToken() + { + string str = ""; + bool _done = false; + + while (true) + { + char c = _buffer.Next; + + if (_punctuation.Contains(c)) + { + if (str != "") + { + _buffer.Position--; + break; + } + + _done = true; + } + else if (char.IsWhiteSpace(c)) + { + if (str != "") + break; + else + continue; + } + + str += c; + + if (_done) + break; + } + + str = TrimControl(str); + + // Trim quotes from start and end + if (str[0] == '\"') + str = str.Remove(0, 1); + + if (str[str.Length - 1] == '\"') + str = str.Remove(str.Length - 1, 1); + + return str; + } + + private string PeekToken() + { + int oldPos = _buffer.Position; + string str = NextToken(); + _buffer.Position = oldPos; + return str; + } + + private string ReadUntil(char c) + { + string str = ""; + + while (true) + { + char ch = _buffer.Next; + + if (ch == c) + { + _buffer.Position--; + break; + } + + str += ch; + } + + // Trim quotes from start and end + if (str[0] == '\"') + str = str.Remove(0, 1); + + if (str[str.Length - 1] == '\"') + str = str.Remove(str.Length - 1, 1); + + return str; + } + + private string TrimControl(string str) + { + string newStr = str; + + // Trim control characters + int i = 0; + while (true) + { + if (i == newStr.Length) + break; + + if (char.IsControl(newStr[i])) + newStr = newStr.Remove(i, 1); + else + i++; + } + + return newStr; + } + + private string TrimTags(string outer) + { + int start = outer.IndexOf('>') + 1; + int end = outer.LastIndexOf('<'); + + return TrimControl(outer.Substring(start, end - start)); + } + + public XMLFragmentElement TryParseNode() + { + if (_buffer.EndOfBuffer) + return null; + + int startOuterXml = _buffer.Position; + string token = NextToken(); + + if (token != "<") + throw new XMLFragmentException("Expected \"<\", got " + token); + + XMLFragmentElement element = new XMLFragmentElement(); + element.Name = NextToken(); + + while (true) + { + token = NextToken(); + + if (token == ">") + break; + else if (token == "/") // quick-exit case + { + NextToken(); + + element.OuterXml = + TrimControl(_buffer.Buffer.Substring(startOuterXml, _buffer.Position - startOuterXml)).Trim(); + element.InnerXml = ""; + + return element; + } + else + { + XMLFragmentAttribute attribute = new XMLFragmentAttribute(); + attribute.Name = token; + if ((token = NextToken()) != "=") + throw new XMLFragmentException("Expected \"=\", got " + token); + attribute.Value = NextToken(); + + element.Attributes.Add(attribute); + } + } + + while (true) + { + int oldPos = _buffer.Position; // for restoration below + token = NextToken(); + + if (token == "<") + { + token = PeekToken(); + + if (token == "/") // finish element + { + NextToken(); // skip the / again + token = NextToken(); + NextToken(); // skip > + + element.OuterXml = TrimControl(_buffer.Buffer.Substring(startOuterXml, _buffer.Position - startOuterXml)).Trim(); + element.InnerXml = TrimTags(element.OuterXml); + + if (token != element.Name) + throw new XMLFragmentException("Mismatched element pairs: \"" + element.Name + "\" vs \"" + + token + "\""); + + break; + } + else + { + _buffer.Position = oldPos; + element.Elements.Add(TryParseNode()); + } + } + else + { + // value, probably + _buffer.Position = oldPos; + element.Value = ReadUntil('<'); + } + } + + return element; + } + + private void Parse() + { + _rootNode = TryParseNode(); + + if (_rootNode == null) + throw new XMLFragmentException("Unable to load root node"); + } + } + + #endregion +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Settings.cs b/src/Box2DNet/Common/Settings.cs deleted file mode 100644 index 47590fc..0000000 --- a/src/Box2DNet/Common/Settings.cs +++ /dev/null @@ -1,178 +0,0 @@ -/* - Box2DNet Copyright (c) 2018 codeyu https://github.com/codeyu/Box2DNet - Box2D original C++ version Copyright (c) 2006-2007 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.Numerics; -using System.Collections.Generic; -using System.Text; - -namespace Box2DNet.Common -{ - public class Settings - { -#if TARGET_FLOAT32_IS_FIXED - public static readonly float FLT_EPSILON = FIXED_EPSILON; - public static readonly float FLT_MAX = FIXED_MAX; - public static float FORCE_SCALE2(x){ return x<<7;} - public static float FORCE_INV_SCALE2(x) {return x>>7;} -#else - public static readonly float FLT_EPSILON = 1.192092896e-07F;//smallest such that 1.0f+FLT_EPSILON != 1.0f - public static readonly float FLT_EPSILON_SQUARED = FLT_EPSILON * FLT_EPSILON;//smallest such that 1.0f+FLT_EPSILON != 1.0f - public static readonly float FLT_MAX = 3.402823466e+38F; - public static float FORCE_SCALE(float x) { return x; } - public static float FORCE_INV_SCALE(float x) { return x; } -#endif - - public static readonly float Pi = 3.14159265359f; - - // Global tuning constants based on meters-kilograms-seconds (MKS) units. - - // Collision - public static readonly int MaxManifoldPoints = 2; - public static readonly int MaxPolygonVertices = 8; - public static readonly int MaxProxies = 512; // this must be a power of two - public static readonly int MaxPairs = 8 * MaxProxies; // this must be a power of two - - // Dynamics - - /// - /// A small length used as a collision and constraint tolerance. Usually it is - /// chosen to be numerically significant, but visually insignificant. - /// - public static readonly float LinearSlop = 0.005f; // 0.5 cm - - /// - /// A small angle used as a collision and constraint tolerance. Usually it is - /// chosen to be numerically significant, but visually insignificant. - /// - public static readonly float AngularSlop = 2.0f / 180.0f * Pi; // 2 degrees - - /// - /// The radius of the polygon/edge shape skin. This should not be modified. Making - /// this smaller means polygons will have and insufficient for continuous collision. - /// Making it larger may create artifacts for vertex collision. - /// - public static readonly float PolygonRadius = 2.0f * LinearSlop; - - /// - /// Continuous collision detection (CCD) works with core, shrunken shapes. This is amount - /// by which shapes are automatically shrunk to work with CCD. - /// This must be larger than LinearSlop. - /// - public static readonly float ToiSlop = 8.0f * LinearSlop; - - /// - /// Maximum number of contacts to be handled to solve a TOI island. - /// - public static readonly int MaxTOIContactsPerIsland = 32; - - /// - /// Maximum number of joints to be handled to solve a TOI island. - /// - public static readonly int MaxTOIJointsPerIsland = 32; - - /// - /// A velocity threshold for elastic collisions. Any collision with a relative linear - /// velocity below this threshold will be treated as inelastic. - /// - public static readonly float VelocityThreshold = 1.0f; // 1 m/s - - /// - /// The maximum linear position correction used when solving constraints. - /// This helps to prevent overshoot. - /// - public static readonly float MaxLinearCorrection = 0.2f; // 20 cm - - /// - /// The maximum angular position correction used when solving constraints. - /// This helps to prevent overshoot. - /// - public static readonly float MaxAngularCorrection = 8.0f / 180.0f * Pi; // 8 degrees - - /// - /// 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. - /// -#if TARGET_FLOAT32_IS_FIXED - public static readonly float MaxLinearVelocity = 100.0f; -#else - public static readonly float MaxLinearVelocity = 200.0f; - public static readonly float MaxLinearVelocitySquared = MaxLinearVelocity * MaxLinearVelocity; -#endif - /// - /// 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 static readonly float MaxAngularVelocity = 250.0f; -#if !TARGET_FLOAT32_IS_FIXED - public static readonly float MaxAngularVelocitySquared = MaxAngularVelocity * MaxAngularVelocity; -#endif - - /// - /// 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 static readonly float MaxTranslation = 2.0f; - public static readonly 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 static readonly float MaxRotation = (0.5f * Pi); - public static readonly float MaxRotationSquared = (MaxRotation * MaxRotation); - - /// - /// 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 static readonly float ContactBaumgarte = 0.2f; - - // Sleep - - /// - /// The time that a body must be still before it will go to sleep. - /// - public static readonly float TimeToSleep = 0.5f; // half a second - - /// - /// A body cannot sleep if its linear velocity is above this tolerance. - /// - public static readonly float LinearSleepTolerance = 0.01f; // 1 cm/s - - /// - /// A body cannot sleep if its angular velocity is above this tolerance. - /// - public static readonly float AngularSleepTolerance = 2.0f / 180.0f; // 2 degrees/s - - /// - /// Friction mixing law. Feel free to customize this. - /// - public static float MixFriction(float friction1, float friction2) - { - return (float)System.Math.Sqrt(friction1 * friction2); - } - - /// - /// Restitution mixing law. Feel free to customize this. - /// - public static float MixRestitution(float restitution1, float restitution2) - { - return restitution1 > restitution2 ? restitution1 : restitution2; - } - } -} \ No newline at end of file diff --git a/src/Box2DNet/Common/Stopwatch.cs b/src/Box2DNet/Common/Stopwatch.cs new file mode 100644 index 0000000..9ade030 --- /dev/null +++ b/src/Box2DNet/Common/Stopwatch.cs @@ -0,0 +1,112 @@ +#if SILVERLIGHT +using System; + +namespace Box2DNet.Common +{ + // Source: http://www.wiredprairie.us/blog/index.php/archives/723 + + /// + /// An emulation for the Stopwatch class for Windows Phone and Silverlight. + /// + public sealed class Stopwatch + { + private long _startTick; + private long _elapsed; + private bool _isRunning; + + /// + /// Creates a new instance of the class and starts the watch immediately. + /// + /// An instance of Stopwatch, running. + public static Stopwatch StartNew() + { + Stopwatch sw = new Stopwatch(); + sw.Start(); + return sw; + } + + /// + /// Creates an instance of the Stopwatch class. + /// + public Stopwatch() { } + + /// + /// Completely resets and deactivates the timer. + /// + public void Reset() + { + _elapsed = 0; + _isRunning = false; + _startTick = 0; + } + + /// + /// Begins the timer. + /// + public void Start() + { + if (!_isRunning) + { + _startTick = GetCurrentTicks(); + _isRunning = true; + } + } + + /// + /// Stops the current timer. + /// + public void Stop() + { + if (_isRunning) + { + _elapsed += GetCurrentTicks() - _startTick; + _isRunning = false; + } + } + + /// + /// Gets a value indicating whether the instance is currently recording. + /// + public bool IsRunning + { + get { return _isRunning; } + } + + /// + /// Gets the Elapsed time as a Timespan. + /// + public TimeSpan Elapsed + { + get { return TimeSpan.FromMilliseconds(ElapsedMilliseconds); } + } + + /// + /// Gets the Elapsed time as the total number of milliseconds. + /// + public long ElapsedMilliseconds + { + get { return GetCurrentElapsedTicks() / TimeSpan.TicksPerMillisecond; } + } + + /// + /// Gets the Elapsed time as the total number of ticks (which is faked + /// as Silverlight doesn't have a way to get at the actual "Ticks") + /// + public long ElapsedTicks + { + get { return GetCurrentElapsedTicks(); } + } + + private long GetCurrentElapsedTicks() + { + return _elapsed + (IsRunning ? (GetCurrentTicks() - _startTick) : 0); + } + + private long GetCurrentTicks() + { + // TickCount: Gets the number of milliseconds elapsed since the system started. + return Environment.TickCount * TimeSpan.TicksPerMillisecond; + } + } +} +#endif \ No newline at end of file diff --git a/src/Box2DNet/Common/Sweep.cs b/src/Box2DNet/Common/Sweep.cs deleted file mode 100644 index bf2f7d6..0000000 --- a/src/Box2DNet/Common/Sweep.cs +++ /dev/null @@ -1,67 +0,0 @@ -/* - Box2DNet Copyright (c) 2018 codeyu https://github.com/codeyu/Box2DNet - Box2D original C++ version Copyright (c) 2006-2007 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. -*/ - -//r175 - -using System; using System.Numerics; -using System.Collections.Generic; -using System.Text; - - -namespace Box2DNet.Common -{ - public struct Sweep - { - public Vector2 LocalCenter; //local center of mass position - public Vector2 C0, C; //local center of mass position - public float A0, A; //world angles - public float T0; //time interval = [T0,1], where T0 is in [0,1] - - /// - /// Get the interpolated Transform at a specific time. - /// - /// Alpha is a factor in [0,1], where 0 indicates t0. - public void GetTransform(out Transform xf, float alpha) - { - xf = new Transform(); - xf.position = (1.0f - alpha) * C0 + alpha * C; - float angle = (1.0f - alpha) * A0 + alpha * A; - - xf.rotation = Box2DNet.Common.Math.AngleToRotation(angle); - //xf.R = new Mat22(angle); - - // Shift to origin - xf.position -= xf.TransformDirection(LocalCenter); - } - - /// - /// Advance the sweep forward, yielding a new initial state. - /// - /// The new initial time. - public void Advance(float t) - { - if (T0 < t && 1.0f - T0 > Settings.FLT_EPSILON) - { - float alpha = (t - T0) / (1.0f - T0); - C0 = (1.0f - alpha) * C0 + alpha * C; - A0 = (1.0f - alpha) * A0 + alpha * A; - T0 = t; - } - } - } -} \ No newline at end of file diff --git a/src/Box2DNet/Common/TextureTools/MarchingSquares.cs b/src/Box2DNet/Common/TextureTools/MarchingSquares.cs new file mode 100644 index 0000000..69f4ebc --- /dev/null +++ b/src/Box2DNet/Common/TextureTools/MarchingSquares.cs @@ -0,0 +1,800 @@ +using System.Collections.Generic; +using Box2DNet.Collision; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Common.TextureTools +{ + // Ported by Matthew Bettcher - Feb 2011 + + /* + Copyright (c) 2010, Luca Deltodesco + All rights reserved. + + Redistribution and use in source and binary forms, with or without modification, are permitted + provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this list of conditions + and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, this list of + conditions and the following disclaimer in the documentation and/or other materials provided + with the distribution. + * Neither the name of the nape project nor the names of its contributors may be used to endorse + or promote products derived from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR + IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND + FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER + IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT + OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + + public static class MarchingSquares + { + /// + /// Marching squares over the given domain using the mesh defined via the dimensions + /// (wid,hei) to build a set of polygons such that f(x,y) less than 0, using the given number + /// 'bin' for recursive linear inteprolation along cell boundaries. + /// + /// if 'comb' is true, then the polygons will also be composited into larger possible concave + /// polygons. + /// + /// + /// + /// + /// + /// + /// + /// + public static List DetectSquares(AABB domain, float cellWidth, float cellHeight, sbyte[,] f, + int lerpCount, bool combine) + { + CxFastList ret = new CxFastList(); + + List verticesList = new List(); + + //NOTE: removed assignments as they were not used. + List polyList; + GeomPoly gp; + + int xn = (int)(domain.Extents.X * 2 / cellWidth); + bool xp = xn == (domain.Extents.X * 2 / cellWidth); + int yn = (int)(domain.Extents.Y * 2 / cellHeight); + bool yp = yn == (domain.Extents.Y * 2 / cellHeight); + if (!xp) xn++; + if (!yp) yn++; + + sbyte[,] fs = new sbyte[xn + 1, yn + 1]; + GeomPolyVal[,] ps = new GeomPolyVal[xn + 1, yn + 1]; + + //populate shared function lookups. + for (int x = 0; x < xn + 1; x++) + { + int x0; + if (x == xn) x0 = (int)domain.UpperBound.X; + else x0 = (int)(x * cellWidth + domain.LowerBound.X); + for (int y = 0; y < yn + 1; y++) + { + int y0; + if (y == yn) y0 = (int)domain.UpperBound.Y; + else y0 = (int)(y * cellHeight + domain.LowerBound.Y); + fs[x, y] = f[x0, y0]; + } + } + + //generate sub-polys and combine to scan lines + for (int y = 0; y < yn; y++) + { + float y0 = y * cellHeight + domain.LowerBound.Y; + float y1; + if (y == yn - 1) y1 = domain.UpperBound.Y; + else y1 = y0 + cellHeight; + GeomPoly pre = null; + for (int x = 0; x < xn; x++) + { + float x0 = x * cellWidth + domain.LowerBound.X; + float x1; + if (x == xn - 1) x1 = domain.UpperBound.X; + else x1 = x0 + cellWidth; + + gp = new GeomPoly(); + + int key = MarchSquare(f, fs, ref gp, x, y, x0, y0, x1, y1, lerpCount); + if (gp.Length != 0) + { + if (combine && pre != null && (key & 9) != 0) + { + combLeft(ref pre, ref gp); + gp = pre; + } + else + ret.Add(gp); + ps[x, y] = new GeomPolyVal(gp, key); + } + else + gp = null; + pre = gp; + } + } + if (!combine) + { + polyList = ret.GetListOfElements(); + + foreach (GeomPoly poly in polyList) + { + verticesList.Add(new Vertices(poly.Points.GetListOfElements())); + } + + return verticesList; + } + + //combine scan lines together + for (int y = 1; y < yn; y++) + { + int x = 0; + while (x < xn) + { + GeomPolyVal p = ps[x, y]; + + //skip along scan line if no polygon exists at this point + if (p == null) + { + x++; + continue; + } + + //skip along if current polygon cannot be combined above. + if ((p.Key & 12) == 0) + { + x++; + continue; + } + + //skip along if no polygon exists above. + GeomPolyVal u = ps[x, y - 1]; + if (u == null) + { + x++; + continue; + } + + //skip along if polygon above cannot be combined with. + if ((u.Key & 3) == 0) + { + x++; + continue; + } + + float ax = x * cellWidth + domain.LowerBound.X; + float ay = y * cellHeight + domain.LowerBound.Y; + + CxFastList bp = p.GeomP.Points; + CxFastList ap = u.GeomP.Points; + + //skip if it's already been combined with above polygon + if (u.GeomP == p.GeomP) + { + x++; + continue; + } + + //combine above (but disallow the hole thingies + CxFastListNode bi = bp.Begin(); + while (Square(bi.Elem().Y - ay) > Settings.Epsilon || bi.Elem().X < ax) bi = bi.Next(); + + //NOTE: Unused + //Vector2 b0 = bi.elem(); + Vector2 b1 = bi.Next().Elem(); + if (Square(b1.Y - ay) > Settings.Epsilon) + { + x++; + continue; + } + + bool brk = true; + CxFastListNode ai = ap.Begin(); + while (ai != ap.End()) + { + if (VecDsq(ai.Elem(), b1) < Settings.Epsilon) + { + brk = false; + break; + } + ai = ai.Next(); + } + if (brk) + { + x++; + continue; + } + + CxFastListNode bj = bi.Next().Next(); + if (bj == bp.End()) bj = bp.Begin(); + while (bj != bi) + { + ai = ap.Insert(ai, bj.Elem()); // .clone() + bj = bj.Next(); + if (bj == bp.End()) bj = bp.Begin(); + u.GeomP.Length++; + } + //u.p.simplify(float.Epsilon,float.Epsilon); + // + ax = x + 1; + while (ax < xn) + { + GeomPolyVal p2 = ps[(int)ax, y]; + if (p2 == null || p2.GeomP != p.GeomP) + { + ax++; + continue; + } + p2.GeomP = u.GeomP; + ax++; + } + ax = x - 1; + while (ax >= 0) + { + GeomPolyVal p2 = ps[(int)ax, y]; + if (p2 == null || p2.GeomP != p.GeomP) + { + ax--; + continue; + } + p2.GeomP = u.GeomP; + ax--; + } + ret.Remove(p.GeomP); + p.GeomP = u.GeomP; + + x = (int)((bi.Next().Elem().X - domain.LowerBound.X) / cellWidth) + 1; + //x++; this was already commented out! + } + } + + polyList = ret.GetListOfElements(); + + foreach (GeomPoly poly in polyList) + { + verticesList.Add(new Vertices(poly.Points.GetListOfElements())); + } + + return verticesList; + } + + #region Private Methods + + //- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + /** Linearly interpolate between (x0 to x1) given a value at these coordinates (v0 and v1) + such as to approximate value(return) = 0 + **/ + + private static int[] _lookMarch = { + 0x00, 0xE0, 0x38, 0xD8, 0x0E, 0xEE, 0x36, 0xD6, 0x83, 0x63, 0xBB, 0x5B, 0x8D, + 0x6D, 0xB5, 0x55 + }; + + private static float Lerp(float x0, float x1, float v0, float v1) + { + float dv = v0 - v1; + float t; + if (dv * dv < Settings.Epsilon) + t = 0.5f; + else t = v0 / dv; + return x0 + t * (x1 - x0); + } + + //- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + /** Recursive linear interpolation for use in marching squares **/ + + private static float Xlerp(float x0, float x1, float y, float v0, float v1, sbyte[,] f, int c) + { + float xm = Lerp(x0, x1, v0, v1); + if (c == 0) + return xm; + + sbyte vm = f[(int)xm, (int)y]; + + if (v0 * vm < 0) + return Xlerp(x0, xm, y, v0, vm, f, c - 1); + + return Xlerp(xm, x1, y, vm, v1, f, c - 1); + } + + /** Recursive linear interpolation for use in marching squares **/ + + private static float Ylerp(float y0, float y1, float x, float v0, float v1, sbyte[,] f, int c) + { + float ym = Lerp(y0, y1, v0, v1); + if (c == 0) + return ym; + + sbyte vm = f[(int)x, (int)ym]; + + if (v0 * vm < 0) + return Ylerp(y0, ym, x, v0, vm, f, c - 1); + + return Ylerp(ym, y1, x, vm, v1, f, c - 1); + } + + /** Square value for use in marching squares **/ + + private static float Square(float x) + { + return x * x; + } + + private static float VecDsq(Vector2 a, Vector2 b) + { + Vector2 d = a - b; + return d.X * d.X + d.Y * d.Y; + } + + private static float VecCross(Vector2 a, Vector2 b) + { + return a.X * b.Y - a.Y * b.X; + } + + /** Look-up table to relate polygon key with the vertices that should be used for + the sub polygon in marching squares + **/ + + /** Perform a single celled marching square for for the given cell defined by (x0,y0) (x1,y1) + using the function f for recursive interpolation, given the look-up table 'fs' of + the values of 'f' at cell vertices with the result to be stored in 'poly' given the actual + coordinates of 'ax' 'ay' in the marching squares mesh. + **/ + + private static int MarchSquare(sbyte[,] f, sbyte[,] fs, ref GeomPoly poly, int ax, int ay, float x0, float y0, + float x1, float y1, int bin) + { + //key lookup + int key = 0; + sbyte v0 = fs[ax, ay]; + if (v0 < 0) key |= 8; + sbyte v1 = fs[ax + 1, ay]; + if (v1 < 0) key |= 4; + sbyte v2 = fs[ax + 1, ay + 1]; + if (v2 < 0) key |= 2; + sbyte v3 = fs[ax, ay + 1]; + if (v3 < 0) key |= 1; + + int val = _lookMarch[key]; + if (val != 0) + { + CxFastListNode pi = null; + for (int i = 0; i < 8; i++) + { + Vector2 p; + if ((val & (1 << i)) != 0) + { + if (i == 7 && (val & 1) == 0) + poly.Points.Add(p = new Vector2(x0, Ylerp(y0, y1, x0, v0, v3, f, bin))); + else + { + if (i == 0) p = new Vector2(x0, y0); + else if (i == 2) p = new Vector2(x1, y0); + else if (i == 4) p = new Vector2(x1, y1); + else if (i == 6) p = new Vector2(x0, y1); + + else if (i == 1) p = new Vector2(Xlerp(x0, x1, y0, v0, v1, f, bin), y0); + else if (i == 5) p = new Vector2(Xlerp(x0, x1, y1, v3, v2, f, bin), y1); + + else if (i == 3) p = new Vector2(x1, Ylerp(y0, y1, x1, v1, v2, f, bin)); + else p = new Vector2(x0, Ylerp(y0, y1, x0, v0, v3, f, bin)); + + pi = poly.Points.Insert(pi, p); + } + poly.Length++; + } + } + //poly.simplify(float.Epsilon,float.Epsilon); + } + return key; + } + + /** Used in polygon composition to composit polygons into scan lines + Combining polya and polyb into one super-polygon stored in polya. + **/ + + private static void combLeft(ref GeomPoly polya, ref GeomPoly polyb) + { + CxFastList ap = polya.Points; + CxFastList bp = polyb.Points; + CxFastListNode ai = ap.Begin(); + CxFastListNode bi = bp.Begin(); + + Vector2 b = bi.Elem(); + CxFastListNode prea = null; + while (ai != ap.End()) + { + Vector2 a = ai.Elem(); + if (VecDsq(a, b) < Settings.Epsilon) + { + //ignore shared vertex if parallel + if (prea != null) + { + Vector2 a0 = prea.Elem(); + b = bi.Next().Elem(); + + Vector2 u = a - a0; + //vec_new(u); vec_sub(a.p.p, a0.p.p, u); + Vector2 v = b - a; + //vec_new(v); vec_sub(b.p.p, a.p.p, v); + float dot = VecCross(u, v); + if (dot * dot < Settings.Epsilon) + { + ap.Erase(prea, ai); + polya.Length--; + ai = prea; + } + } + + //insert polyb into polya + bool fst = true; + CxFastListNode preb = null; + while (!bp.Empty()) + { + Vector2 bb = bp.Front(); + bp.Pop(); + if (!fst && !bp.Empty()) + { + ai = ap.Insert(ai, bb); + polya.Length++; + preb = ai; + } + fst = false; + } + + //ignore shared vertex if parallel + ai = ai.Next(); + Vector2 a1 = ai.Elem(); + ai = ai.Next(); + if (ai == ap.End()) ai = ap.Begin(); + Vector2 a2 = ai.Elem(); + Vector2 a00 = preb.Elem(); + Vector2 uu = a1 - a00; + //vec_new(u); vec_sub(a1.p, a0.p, u); + Vector2 vv = a2 - a1; + //vec_new(v); vec_sub(a2.p, a1.p, v); + float dot1 = VecCross(uu, vv); + if (dot1 * dot1 < Settings.Epsilon) + { + ap.Erase(preb, preb.Next()); + polya.Length--; + } + + return; + } + prea = ai; + ai = ai.Next(); + } + } + + #endregion + + #region CxFastList from nape physics + + #region Nested type: CxFastList + + /// + /// Designed as a complete port of CxFastList from CxStd. + /// + internal class CxFastList + { + // first node in the list + private CxFastListNode _head; + private int _count; + + /// + /// Iterator to start of list (O(1)) + /// + public CxFastListNode Begin() + { + return _head; + } + + /// + /// Iterator to end of list (O(1)) + /// + public CxFastListNode End() + { + return null; + } + + /// + /// Returns first element of list (O(1)) + /// + public T Front() + { + return _head.Elem(); + } + + /// + /// add object to list (O(1)) + /// + public CxFastListNode Add(T value) + { + CxFastListNode newNode = new CxFastListNode(value); + if (_head == null) + { + newNode._next = null; + _head = newNode; + _count++; + return newNode; + } + newNode._next = _head; + _head = newNode; + + _count++; + + return newNode; + } + + /// + /// remove object from list, returns true if an element was removed (O(n)) + /// + public bool Remove(T value) + { + CxFastListNode head = _head; + CxFastListNode prev = _head; + + EqualityComparer comparer = EqualityComparer.Default; + + if (head != null) + { + if (value != null) + { + do + { + // if we are on the value to be removed + if (comparer.Equals(head._elt, value)) + { + // then we need to patch the list + // check to see if we are removing the _head + if (head == _head) + { + _head = head._next; + _count--; + return true; + } + else + { + // were not at the head + prev._next = head._next; + _count--; + return true; + } + } + // cache the current as the previous for the next go around + prev = head; + head = head._next; + } while (head != null); + } + } + return false; + } + + /// + /// pop element from head of list (O(1)) Note: this does not return the object popped! + /// There is good reason to this, and it regards the Alloc list variants which guarantee + /// objects are released to the object pool. You do not want to retrieve an element + /// through pop or else that object may suddenly be used by another piece of code which + /// retrieves it from the object pool. + /// + public CxFastListNode Pop() + { + return Erase(null, _head); + } + + /// + /// insert object after 'node' returning an iterator to the inserted object. + /// + public CxFastListNode Insert(CxFastListNode node, T value) + { + if (node == null) + { + return Add(value); + } + CxFastListNode newNode = new CxFastListNode(value); + CxFastListNode nextNode = node._next; + newNode._next = nextNode; + node._next = newNode; + + _count++; + + return newNode; + } + + /// + /// removes the element pointed to by 'node' with 'prev' being the previous iterator, + /// returning an iterator to the element following that of 'node' (O(1)) + /// + public CxFastListNode Erase(CxFastListNode prev, CxFastListNode node) + { + // cache the node after the node to be removed + CxFastListNode nextNode = node._next; + if (prev != null) + prev._next = nextNode; + else if (_head != null) + _head = _head._next; + else + return null; + + _count--; + return nextNode; + } + + /// + /// whether the list is empty (O(1)) + /// + public bool Empty() + { + if (_head == null) + return true; + return false; + } + + /// + /// computes size of list (O(n)) + /// + public int Size() + { + CxFastListNode i = Begin(); + int count = 0; + + do + { + count++; + } while (i.Next() != null); + + return count; + } + + /// + /// empty the list (O(1) if CxMixList, O(n) otherwise) + /// + public void Clear() + { + CxFastListNode head = _head; + while (head != null) + { + CxFastListNode node2 = head; + head = head._next; + node2._next = null; + } + _head = null; + _count = 0; + } + + /// + /// returns true if 'value' is an element of the list (O(n)) + /// + public bool Has(T value) + { + return (Find(value) != null); + } + + // Non CxFastList Methods + public CxFastListNode Find(T value) + { + // start at head + CxFastListNode head = _head; + EqualityComparer comparer = EqualityComparer.Default; + if (head != null) + { + if (value != null) + { + do + { + if (comparer.Equals(head._elt, value)) + { + return head; + } + head = head._next; + } while (head != _head); + } + else + { + do + { + if (head._elt == null) + { + return head; + } + head = head._next; + } while (head != _head); + } + } + return null; + } + + public List GetListOfElements() + { + List list = new List(); + + CxFastListNode iter = Begin(); + + if (iter != null) + { + do + { + list.Add(iter._elt); + iter = iter._next; + } while (iter != null); + } + return list; + } + } + + #endregion + + #region Nested type: CxFastListNode + + internal class CxFastListNode + { + internal T _elt; + internal CxFastListNode _next; + + public CxFastListNode(T obj) + { + _elt = obj; + } + + public T Elem() + { + return _elt; + } + + public CxFastListNode Next() + { + return _next; + } + } + + #endregion + + #endregion + + #region Internal Stuff + + #region Nested type: GeomPoly + + internal class GeomPoly + { + public int Length; + public CxFastList Points; + + public GeomPoly() + { + Points = new CxFastList(); + Length = 0; + } + } + + #endregion + + #region Nested type: GeomPolyVal + + private class GeomPolyVal + { + /** Associated polygon at coordinate **/ + /** Key of original sub-polygon **/ + public int Key; + public GeomPoly GeomP; + + public GeomPolyVal(GeomPoly geomP, int K) + { + GeomP = geomP; + Key = K; + } + } + + #endregion + + #endregion + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/TextureTools/Terrain.cs b/src/Box2DNet/Common/TextureTools/Terrain.cs new file mode 100644 index 0000000..f3982b1 --- /dev/null +++ b/src/Box2DNet/Common/TextureTools/Terrain.cs @@ -0,0 +1,264 @@ +using System.Collections.Generic; +using Box2DNet.Collision; +using Box2DNet.Common.Decomposition; +using Box2DNet.Common.PolygonManipulation; +using Box2DNet.Dynamics; +using Box2DNet.Factories; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Common.TextureTools +{ + /// + /// Simple class to maintain a terrain. It can keep track + /// + public class Terrain + { + /// + /// World to manage terrain in. + /// + public World World; + + /// + /// Center of terrain in world units. + /// + public Vector2 Center; + + /// + /// Width of terrain in world units. + /// + public float Width; + + /// + /// Height of terrain in world units. + /// + public float Height; + + /// + /// Points per each world unit used to define the terrain in the point cloud. + /// + public int PointsPerUnit; + + /// + /// Points per cell. + /// + public int CellSize; + + /// + /// Points per sub cell. + /// + public int SubCellSize; + + /// + /// Number of iterations to perform in the Marching Squares algorithm. + /// Note: More then 3 has almost no effect on quality. + /// + public int Iterations = 2; + + /// + /// Decomposer to use when regenerating terrain. Can be changed on the fly without consequence. + /// Note: Some decomposerers are unstable. + /// + public TriangulationAlgorithm Decomposer; + + /// + /// Point cloud defining the terrain. + /// + private sbyte[,] _terrainMap; + + /// + /// Generated bodies. + /// + private List[,] _bodyMap; + + private float _localWidth; + private float _localHeight; + private int _xnum; + private int _ynum; + private AABB _dirtyArea; + private Vector2 _topLeft; + + /// + /// Creates a new terrain. + /// + /// The World + /// The area of the terrain. + public Terrain(World world, AABB area) + { + World = world; + Width = area.Width; + Height = area.Height; + Center = area.Center; + } + + /// + /// Creates a new terrain + /// + /// The World + /// The position (center) of the terrain. + /// The width of the terrain. + /// The height of the terrain. + public Terrain(World world, Vector2 position, float width, float height) + { + World = world; + Width = width; + Height = height; + Center = position; + } + + /// + /// Initialize the terrain for use. + /// + public void Initialize() + { + // find top left of terrain in world space + _topLeft = new Vector2(Center.X - (Width * 0.5f), Center.Y - (-Height * 0.5f)); + + // convert the terrains size to a point cloud size + _localWidth = Width * PointsPerUnit; + _localHeight = Height * PointsPerUnit; + + _terrainMap = new sbyte[(int)_localWidth + 1, (int)_localHeight + 1]; + + for (int x = 0; x < _localWidth; x++) + { + for (int y = 0; y < _localHeight; y++) + { + _terrainMap[x, y] = 1; + } + } + + _xnum = (int)(_localWidth / CellSize); + _ynum = (int)(_localHeight / CellSize); + _bodyMap = new List[_xnum, _ynum]; + + // make sure to mark the dirty area to an infinitely small box + _dirtyArea = new AABB(new Vector2(float.MaxValue, float.MaxValue), new Vector2(float.MinValue, float.MinValue)); + } + + /// + /// Apply the specified texture data to the terrain. + /// + /// + /// + public void ApplyData(sbyte[,] data, Vector2 offset = default(Vector2)) + { + for (int x = 0; x < data.GetUpperBound(0); x++) + { + for (int y = 0; y < data.GetUpperBound(1); y++) + { + if (x + offset.X >= 0 && x + offset.X < _localWidth && y + offset.Y >= 0 && y + offset.Y < _localHeight) + { + _terrainMap[(int)(x + offset.X), (int)(y + offset.Y)] = data[x, y]; + } + } + } + + RemoveOldData(0, _xnum, 0, _ynum); + } + + /// + /// Modify a single point in the terrain. + /// + /// World location to modify. Automatically clipped. + /// -1 = inside terrain, 1 = outside terrain + public void ModifyTerrain(Vector2 location, sbyte value) + { + // find local position + // make position local to map space + Vector2 p = location - _topLeft; + + // find map position for each axis + p.X = p.X * _localWidth / Width; + p.Y = p.Y * -_localHeight / Height; + + if (p.X >= 0 && p.X < _localWidth && p.Y >= 0 && p.Y < _localHeight) + { + _terrainMap[(int)p.X, (int)p.Y] = value; + + // expand dirty area + if (p.X < _dirtyArea.LowerBound.X) _dirtyArea.LowerBound.X = p.X; + if (p.X > _dirtyArea.UpperBound.X) _dirtyArea.UpperBound.X = p.X; + + if (p.Y < _dirtyArea.LowerBound.Y) _dirtyArea.LowerBound.Y = p.Y; + if (p.Y > _dirtyArea.UpperBound.Y) _dirtyArea.UpperBound.Y = p.Y; + } + } + + /// + /// Regenerate the terrain. + /// + public void RegenerateTerrain() + { + //iterate effected cells + int xStart = (int)(_dirtyArea.LowerBound.X / CellSize); + if (xStart < 0) xStart = 0; + + int xEnd = (int)(_dirtyArea.UpperBound.X / CellSize) + 1; + if (xEnd > _xnum) xEnd = _xnum; + + int yStart = (int)(_dirtyArea.LowerBound.Y / CellSize); + if (yStart < 0) yStart = 0; + + int yEnd = (int)(_dirtyArea.UpperBound.Y / CellSize) + 1; + if (yEnd > _ynum) yEnd = _ynum; + + RemoveOldData(xStart, xEnd, yStart, yEnd); + + _dirtyArea = new AABB(new Vector2(float.MaxValue, float.MaxValue), new Vector2(float.MinValue, float.MinValue)); + } + + private void RemoveOldData(int xStart, int xEnd, int yStart, int yEnd) + { + for (int x = xStart; x < xEnd; x++) + { + for (int y = yStart; y < yEnd; y++) + { + //remove old terrain object at grid cell + if (_bodyMap[x, y] != null) + { + for (int i = 0; i < _bodyMap[x, y].Count; i++) + { + World.RemoveBody(_bodyMap[x, y][i]); + } + } + + _bodyMap[x, y] = null; + + //generate new one + GenerateTerrain(x, y); + } + } + } + + private void GenerateTerrain(int gx, int gy) + { + float ax = gx * CellSize; + float ay = gy * CellSize; + + List polys = MarchingSquares.DetectSquares(new AABB(new Vector2(ax, ay), new Vector2(ax + CellSize, ay + CellSize)), SubCellSize, SubCellSize, _terrainMap, Iterations, true); + if (polys.Count == 0) return; + + _bodyMap[gx, gy] = new List(); + + // create the scale vector + Vector2 scale = new Vector2(1f / PointsPerUnit, 1f / -PointsPerUnit); + + // create physics object for this grid cell + foreach (Vertices item in polys) + { + // does this need to be negative? + item.Scale(ref scale); + item.Translate(ref _topLeft); + Vertices simplified = SimplifyTools.CollinearSimplify(item); + + List decompPolys = Triangulate.ConvexPartition(simplified, Decomposer); + + foreach (Vertices poly in decompPolys) + { + if (poly.Count > 2) + _bodyMap[gx, gy].Add(BodyFactory.CreatePolygon(World, poly, 1)); + } + } + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/TextureTools/TextureConverter.cs b/src/Box2DNet/Common/TextureTools/TextureConverter.cs new file mode 100644 index 0000000..862f2e1 --- /dev/null +++ b/src/Box2DNet/Common/TextureTools/TextureConverter.cs @@ -0,0 +1,1258 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Common.TextureTools +{ + // User contribution from Sickbattery aka David Reschke. + + /// + /// The detection type affects the resulting polygon data. + /// + public enum VerticesDetectionType + { + /// + /// Holes are integrated into the main polygon. + /// + Integrated = 0, + + /// + /// The data of the main polygon and hole polygons is returned separately. + /// + Separated = 1 + } + + public sealed class TextureConverter + { + private const int ClosepixelsLength = 8; + + /// + /// This array is ment to be readonly. + /// It's not because it is accessed very frequently. + /// + private static int[,] _closePixels = new[,] { { -1, -1 }, { 0, -1 }, { 1, -1 }, { 1, 0 }, { 1, 1 }, { 0, 1 }, { -1, 1 }, { -1, 0 } }; + + private uint[] _data; + private int _dataLength; + private int _width; + private int _height; + + private VerticesDetectionType _polygonDetectionType; + + private uint _alphaTolerance; + private float _hullTolerance; + + private bool _holeDetection; + private bool _multipartDetection; + private bool _pixelOffsetOptimization; + + private Matrix _transform = Matrix.Identity; + + #region Properties + /// + /// Get or set the polygon detection type. + /// + public VerticesDetectionType PolygonDetectionType + { + get { return _polygonDetectionType; } + set { _polygonDetectionType = value; } + } + + /// + /// Will detect texture 'holes' if set to true. Slows down the detection. Default is false. + /// + public bool HoleDetection + { + get { return _holeDetection; } + set { _holeDetection = value; } + } + + /// + /// Will detect texture multiple 'solid' isles if set to true. Slows down the detection. Default is false. + /// + public bool MultipartDetection + { + get { return _multipartDetection; } + set { _multipartDetection = value; } + } + + /// + /// Will optimize the vertex positions along the interpolated normal between two edges about a half pixel (post processing). Default is false. + /// + public bool PixelOffsetOptimization + { + get { return _pixelOffsetOptimization; } + set { _pixelOffsetOptimization = value; } + } + + /// + /// Can be used for scaling. + /// + public Matrix Transform + { + get { return _transform; } + set { _transform = value; } + } + + /// + /// Alpha (coverage) tolerance. Default is 20: Every pixel with a coverage value equal or greater to 20 will be counts as solid. + /// + public byte AlphaTolerance + { + get { return (byte)(_alphaTolerance >> 24); } + set { _alphaTolerance = (uint)value << 24; } + } + + /// + /// Default is 1.5f. + /// + public float HullTolerance + { + get { return _hullTolerance; } + set + { + if (value > 4f) + { + _hullTolerance = 4f; + } + else if (value < 0.9f) + { + _hullTolerance = 0.9f; + } + else + { + _hullTolerance = value; + } + } + } + #endregion + + #region Constructors + public TextureConverter() + { + Initialize(null, null, null, null, null, null, null, null); + } + + public TextureConverter(byte? alphaTolerance, float? hullTolerance, + bool? holeDetection, bool? multipartDetection, bool? pixelOffsetOptimization, Matrix? transform) + { + Initialize(null, null, alphaTolerance, hullTolerance, holeDetection, + multipartDetection, pixelOffsetOptimization, transform); + } + + public TextureConverter(uint[] data, int width) + { + Initialize(data, width, null, null, null, null, null, null); + } + + public TextureConverter(uint[] data, int width, byte? alphaTolerance, + float? hullTolerance, bool? holeDetection, bool? multipartDetection, + bool? pixelOffsetOptimization, Matrix? transform) + { + Initialize(data, width, alphaTolerance, hullTolerance, holeDetection, + multipartDetection, pixelOffsetOptimization, transform); + } + #endregion + + #region Initialization + private void Initialize(uint[] data, int? width, byte? alphaTolerance, + float? hullTolerance, bool? holeDetection, bool? multipartDetection, + bool? pixelOffsetOptimization, Matrix? transform) + { + if (data != null && !width.HasValue) + throw new ArgumentNullException("width", "'width' can't be null if 'data' is set."); + + if (data == null && width.HasValue) + throw new ArgumentNullException("data", "'data' can't be null if 'width' is set."); + + if (data != null && width.HasValue) + SetTextureData(data, width.Value); + + if (alphaTolerance.HasValue) + AlphaTolerance = alphaTolerance.Value; + else + AlphaTolerance = 20; + + if (hullTolerance.HasValue) + HullTolerance = hullTolerance.Value; + else + HullTolerance = 1.5f; + + if (holeDetection.HasValue) + HoleDetection = holeDetection.Value; + else + HoleDetection = false; + + if (multipartDetection.HasValue) + MultipartDetection = multipartDetection.Value; + else + MultipartDetection = false; + + if (pixelOffsetOptimization.HasValue) + PixelOffsetOptimization = pixelOffsetOptimization.Value; + else + PixelOffsetOptimization = false; + + if (transform.HasValue) + Transform = transform.Value; + else + Transform = Matrix.Identity; + } + #endregion + + private void SetTextureData(uint[] data, int width) + { + if (data == null) + throw new ArgumentNullException("data", "'data' can't be null."); + + if (data.Length < 4) + throw new ArgumentOutOfRangeException("data", "'data' length can't be less then 4. Your texture must be at least 2 x 2 pixels in size."); + + if (width < 2) + throw new ArgumentOutOfRangeException("width", "'width' can't be less then 2. Your texture must be at least 2 x 2 pixels in size."); + + if (data.Length % width != 0) + throw new ArgumentException("'width' has an invalid value."); + + _data = data; + _dataLength = _data.Length; + _width = width; + _height = _dataLength / width; + } + + /// + /// Detects the vertices of the supplied texture data. (PolygonDetectionType.Integrated) + /// + /// The texture data. + /// The texture width. + /// + public static Vertices DetectVertices(uint[] data, int width) + { + TextureConverter tc = new TextureConverter(data, width); + + List detectedVerticesList = tc.DetectVertices(); + + return detectedVerticesList[0]; + } + + /// + /// Detects the vertices of the supplied texture data. + /// + /// The texture data. + /// The texture width. + /// if set to true it will perform hole detection. + /// + public static Vertices DetectVertices(uint[] data, int width, bool holeDetection) + { + TextureConverter tc = + new TextureConverter(data, width) + { + HoleDetection = holeDetection + }; + + List detectedVerticesList = tc.DetectVertices(); + + return detectedVerticesList[0]; + } + + /// + /// Detects the vertices of the supplied texture data. + /// + /// The texture data. + /// The texture width. + /// if set to true it will perform hole detection. + /// The hull tolerance. + /// The alpha tolerance. + /// if set to true it will perform multi part detection. + /// + public static List DetectVertices(uint[] data, int width, float hullTolerance, byte alphaTolerance, bool multiPartDetection, bool holeDetection) + { + TextureConverter tc = + new TextureConverter(data, width) + { + HullTolerance = hullTolerance, + AlphaTolerance = alphaTolerance, + MultipartDetection = multiPartDetection, + HoleDetection = holeDetection + }; + + List detectedVerticesList = tc.DetectVertices(); + List result = new List(); + + for (int i = 0; i < detectedVerticesList.Count; i++) + { + result.Add(detectedVerticesList[i]); + } + + return result; + } + + public List DetectVertices() + { + #region Check TextureConverter setup. + + if (_data == null) + throw new Exception("'_data' can't be null. You have to use SetTextureData(uint[] data, int width) before calling this method."); + + if (_data.Length < 4) + throw new Exception("'_data' length can't be less then 4. Your texture must be at least 2 x 2 pixels in size. " + + "You have to use SetTextureData(uint[] data, int width) before calling this method."); + + if (_width < 2) + throw new Exception("'_width' can't be less then 2. Your texture must be at least 2 x 2 pixels in size. " + + "You have to use SetTextureData(uint[] data, int width) before calling this method."); + + if (_data.Length % _width != 0) + throw new Exception("'_width' has an invalid value. You have to use SetTextureData(uint[] data, int width) before calling this method."); + + #endregion + + List detectedPolygons = new List(); + + Vector2? holeEntrance = null; + Vector2? polygonEntrance = null; + + List blackList = new List(); + + bool searchOn; + do + { + Vertices polygon; + if (detectedPolygons.Count == 0) + { + // First pass / single polygon + polygon = new Vertices(CreateSimplePolygon(Vector2.Zero, Vector2.Zero)); + + if (polygon.Count > 2) + polygonEntrance = GetTopMostVertex(polygon); + } + else if (polygonEntrance.HasValue) + { + // Multi pass / multiple polygons + polygon = new Vertices(CreateSimplePolygon(polygonEntrance.Value, new Vector2(polygonEntrance.Value.X - 1f, polygonEntrance.Value.Y))); + } + else + break; + + searchOn = false; + + + if (polygon.Count > 2) + { + if (_holeDetection) + { + do + { + holeEntrance = SearchHoleEntrance(polygon, holeEntrance); + + if (holeEntrance.HasValue) + { + if (!blackList.Contains(holeEntrance.Value)) + { + blackList.Add(holeEntrance.Value); + Vertices holePolygon = CreateSimplePolygon(holeEntrance.Value, + new Vector2(holeEntrance.Value.X + 1, holeEntrance.Value.Y)); + + if (holePolygon != null && holePolygon.Count > 2) + { + switch (_polygonDetectionType) + { + case VerticesDetectionType.Integrated: + + // Add first hole polygon vertex to close the hole polygon. + holePolygon.Add(holePolygon[0]); + + int vertex1Index, vertex2Index; + if (SplitPolygonEdge(polygon, holeEntrance.Value, out vertex1Index, out vertex2Index)) + polygon.InsertRange(vertex2Index, holePolygon); + + break; + + case VerticesDetectionType.Separated: + if (polygon.Holes == null) + polygon.Holes = new List(); + + polygon.Holes.Add(holePolygon); + break; + } + } + } + else + break; + } + else + break; + } + while (true); + } + + detectedPolygons.Add(polygon); + } + + if (_multipartDetection || polygon.Count <= 2) + { + if (SearchNextHullEntrance(detectedPolygons, polygonEntrance.Value, out polygonEntrance)) + searchOn = true; + } + } + while (searchOn); + + if (detectedPolygons == null || (detectedPolygons != null && detectedPolygons.Count == 0)) + throw new Exception("Couldn't detect any vertices."); + + // Post processing. + if (PolygonDetectionType == VerticesDetectionType.Separated) // Only when VerticesDetectionType.Separated? -> Recheck. + ApplyTriangulationCompatibleWinding(ref detectedPolygons); + + if (_transform != Matrix.Identity) + ApplyTransform(ref detectedPolygons); + + return detectedPolygons; + } + + private void ApplyTriangulationCompatibleWinding(ref List detectedPolygons) + { + for (int i = 0; i < detectedPolygons.Count; i++) + { + detectedPolygons[i].Reverse(); + + if (detectedPolygons[i].Holes != null && detectedPolygons[i].Holes.Count > 0) + { + for (int j = 0; j < detectedPolygons[i].Holes.Count; j++) + detectedPolygons[i].Holes[j].Reverse(); + } + } + } + + private void ApplyTransform(ref List detectedPolygons) + { + for (int i = 0; i < detectedPolygons.Count; i++) + detectedPolygons[i].Transform(ref _transform); + } + + #region Data[] functions + private int _tempIsSolidX; + private int _tempIsSolidY; + public bool IsSolid(ref Vector2 v) + { + _tempIsSolidX = (int)v.X; + _tempIsSolidY = (int)v.Y; + + if (_tempIsSolidX >= 0 && _tempIsSolidX < _width && _tempIsSolidY >= 0 && _tempIsSolidY < _height) + return (_data[_tempIsSolidX + _tempIsSolidY * _width] >= _alphaTolerance); + //return ((_data[_tempIsSolidX + _tempIsSolidY * _width] & 0xFF000000) >= _alphaTolerance); + + return false; + } + + public bool IsSolid(ref int x, ref int y) + { + if (x >= 0 && x < _width && y >= 0 && y < _height) + return (_data[x + y * _width] >= _alphaTolerance); + //return ((_data[x + y * _width] & 0xFF000000) >= _alphaTolerance); + + return false; + } + + public bool IsSolid(ref int index) + { + if (index >= 0 && index < _dataLength) + return (_data[index] >= _alphaTolerance); + //return ((_data[index] & 0xFF000000) >= _alphaTolerance); + + return false; + } + + public bool InBounds(ref Vector2 coord) + { + return (coord.X >= 0f && coord.X < _width && coord.Y >= 0f && coord.Y < _height); + } + #endregion + + /// + /// Function to search for an entrance point of a hole in a polygon. It searches the polygon from top to bottom between the polygon edges. + /// + /// The polygon to search in. + /// The last entrance point. + /// The next holes entrance point. Null if ther are no holes. + private Vector2? SearchHoleEntrance(Vertices polygon, Vector2? lastHoleEntrance) + { + if (polygon == null) + throw new ArgumentNullException("'polygon' can't be null."); + + if (polygon.Count < 3) + throw new ArgumentException("'polygon.MainPolygon.Count' can't be less then 3."); + + + List xCoords; + Vector2? entrance; + + int startY; + int endY; + + int lastSolid = 0; + bool foundSolid; + bool foundTransparent; + + // Set start y coordinate. + if (lastHoleEntrance.HasValue) + { + // We need the y coordinate only. + startY = (int)lastHoleEntrance.Value.Y; + } + else + { + // Start from the top of the polygon if last entrance == null. + startY = (int)GetTopMostCoord(polygon); + } + + // Set the end y coordinate. + endY = (int)GetBottomMostCoord(polygon); + + if (startY > 0 && startY < _height && endY > 0 && endY < _height) + { + // go from top to bottom of the polygon + for (int y = startY; y <= endY; y++) + { + // get x-coord of every polygon edge which crosses y + xCoords = SearchCrossingEdges(polygon, y); + + // We need an even number of crossing edges. + // It's always a pair of start and end edge: nothing | polygon | hole | polygon | nothing ... + // If it's not then don't bother, it's probably a peak ... + // ...which should be filtered out by SearchCrossingEdges() anyway. + if (xCoords.Count > 1 && xCoords.Count % 2 == 0) + { + // Ok, this is short, but probably a little bit confusing. + // This part searches from left to right between the edges inside the polygon. + // The problem: We are using the polygon data to search in the texture data. + // That's simply not accurate, but necessary because of performance. + for (int i = 0; i < xCoords.Count; i += 2) + { + foundSolid = false; + foundTransparent = false; + + // We search between the edges inside the polygon. + for (int x = (int)xCoords[i]; x <= (int)xCoords[i + 1]; x++) + { + // First pass: IsSolid might return false. + // In that case the polygon edge doesn't lie on the texture's solid pixel, because of the hull tolearance. + // If the edge lies before the first solid pixel then we need to skip our transparent pixel finds. + + // The algorithm starts to search for a relevant transparent pixel (which indicates a possible hole) + // after it has found a solid pixel. + + // After we've found a solid and a transparent pixel (a hole's left edge) + // we search for a solid pixel again (a hole's right edge). + // When found the distance of that coodrinate has to be greater then the hull tolerance. + + if (IsSolid(ref x, ref y)) + { + if (!foundTransparent) + { + foundSolid = true; + lastSolid = x; + } + + if (foundSolid && foundTransparent) + { + entrance = new Vector2(lastSolid, y); + + if (DistanceToHullAcceptable(polygon, entrance.Value, true)) + return entrance; + + entrance = null; + break; + } + } + else + { + if (foundSolid) + foundTransparent = true; + } + } + } + } + else + { + if (xCoords.Count % 2 == 0) + Debug.WriteLine("SearchCrossingEdges() % 2 != 0"); + } + } + } + + return null; + } + + private bool DistanceToHullAcceptableHoles(Vertices polygon, Vector2 point, bool higherDetail) + { + if (polygon == null) + throw new ArgumentNullException("polygon", "'polygon' can't be null."); + + if (polygon.Count < 3) + throw new ArgumentException("'polygon.MainPolygon.Count' can't be less then 3."); + + // Check the distance to main polygon. + if (DistanceToHullAcceptable(polygon, point, higherDetail)) + { + if (polygon.Holes != null) + { + for (int i = 0; i < polygon.Holes.Count; i++) + { + // If there is one distance not acceptable then return false. + if (!DistanceToHullAcceptable(polygon.Holes[i], point, higherDetail)) + return false; + } + } + + // All distances are larger then _hullTolerance. + return true; + } + + // Default to false. + return false; + } + + private bool DistanceToHullAcceptable(Vertices polygon, Vector2 point, bool higherDetail) + { + if (polygon == null) + throw new ArgumentNullException("polygon", "'polygon' can't be null."); + + if (polygon.Count < 3) + throw new ArgumentException("'polygon.Count' can't be less then 3."); + + + Vector2 edgeVertex2 = polygon[polygon.Count - 1]; + Vector2 edgeVertex1; + + if (higherDetail) + { + for (int i = 0; i < polygon.Count; i++) + { + edgeVertex1 = polygon[i]; + + if (LineTools.DistanceBetweenPointAndLineSegment(ref point, ref edgeVertex1, ref edgeVertex2) <= _hullTolerance || Vector2.Distance(point, edgeVertex1) <= _hullTolerance) + return false; + + edgeVertex2 = polygon[i]; + } + + return true; + } + else + { + for (int i = 0; i < polygon.Count; i++) + { + edgeVertex1 = polygon[i]; + + if (LineTools.DistanceBetweenPointAndLineSegment(ref point, ref edgeVertex1, ref edgeVertex2) <= _hullTolerance) + return false; + + edgeVertex2 = polygon[i]; + } + + return true; + } + } + + private bool InPolygon(Vertices polygon, Vector2 point) + { + bool inPolygon = !DistanceToHullAcceptableHoles(polygon, point, true); + + if (!inPolygon) + { + List xCoords = SearchCrossingEdgesHoles(polygon, (int)point.Y); + + if (xCoords.Count > 0 && xCoords.Count % 2 == 0) + { + for (int i = 0; i < xCoords.Count; i += 2) + { + if (xCoords[i] <= point.X && xCoords[i + 1] >= point.X) + return true; + } + } + + return false; + } + + return true; + } + + private Vector2? GetTopMostVertex(Vertices vertices) + { + float topMostValue = float.MaxValue; + Vector2? topMost = null; + + for (int i = 0; i < vertices.Count; i++) + { + if (topMostValue > vertices[i].Y) + { + topMostValue = vertices[i].Y; + topMost = vertices[i]; + } + } + + return topMost; + } + + private float GetTopMostCoord(Vertices vertices) + { + float returnValue = float.MaxValue; + + for (int i = 0; i < vertices.Count; i++) + { + if (returnValue > vertices[i].Y) + { + returnValue = vertices[i].Y; + } + } + + return returnValue; + } + + private float GetBottomMostCoord(Vertices vertices) + { + float returnValue = float.MinValue; + + for (int i = 0; i < vertices.Count; i++) + { + if (returnValue < vertices[i].Y) + { + returnValue = vertices[i].Y; + } + } + + return returnValue; + } + + private List SearchCrossingEdgesHoles(Vertices polygon, int y) + { + if (polygon == null) + throw new ArgumentNullException("polygon", "'polygon' can't be null."); + + if (polygon.Count < 3) + throw new ArgumentException("'polygon.MainPolygon.Count' can't be less then 3."); + + List result = SearchCrossingEdges(polygon, y); + + if (polygon.Holes != null) + { + for (int i = 0; i < polygon.Holes.Count; i++) + { + result.AddRange(SearchCrossingEdges(polygon.Holes[i], y)); + } + } + + result.Sort(); + return result; + } + + /// + /// Searches the polygon for the x coordinates of the edges that cross the specified y coordinate. + /// + /// Polygon to search in. + /// Y coordinate to check for edges. + /// Descending sorted list of x coordinates of edges that cross the specified y coordinate. + private List SearchCrossingEdges(Vertices polygon, int y) + { + // sick-o-note: + // Used to search the x coordinates of edges in the polygon for a specific y coordinate. + // (Usualy comming from the texture data, that's why it's an int and not a float.) + + List edges = new List(); + + // current edge + Vector2 slope; + Vector2 vertex1; // i + Vector2 vertex2; // i - 1 + + // next edge + Vector2 nextSlope; + Vector2 nextVertex; // i + 1 + + bool addFind; + + if (polygon.Count > 2) + { + // There is a gap between the last and the first vertex in the vertex list. + // We will bridge that by setting the last vertex (vertex2) to the last + // vertex in the list. + vertex2 = polygon[polygon.Count - 1]; + + // We are moving along the polygon edges. + for (int i = 0; i < polygon.Count; i++) + { + vertex1 = polygon[i]; + + // Approx. check if the edge crosses our y coord. + if ((vertex1.Y >= y && vertex2.Y <= y) || + (vertex1.Y <= y && vertex2.Y >= y)) + { + // Ignore edges that are parallel to y. + if (vertex1.Y != vertex2.Y) + { + addFind = true; + slope = vertex2 - vertex1; + + // Special threatment for edges that end at the y coord. + if (vertex1.Y == y) + { + // Create preview of the next edge. + nextVertex = polygon[(i + 1) % polygon.Count]; + nextSlope = vertex1 - nextVertex; + + // Ignore peaks. + // If thwo edges are aligned like this: /\ and the y coordinate lies on the top, + // then we get the same x coord twice and we don't need that. + if (slope.Y > 0) + addFind = (nextSlope.Y <= 0); + else + addFind = (nextSlope.Y >= 0); + } + + if (addFind) + edges.Add((y - vertex1.Y) / slope.Y * slope.X + vertex1.X); // Calculate and add the x coord. + } + } + + // vertex1 becomes vertex2 :). + vertex2 = vertex1; + } + } + + edges.Sort(); + return edges; + } + + private bool SplitPolygonEdge(Vertices polygon, Vector2 coordInsideThePolygon, out int vertex1Index, out int vertex2Index) + { + Vector2 slope; + int nearestEdgeVertex1Index = 0; + int nearestEdgeVertex2Index = 0; + bool edgeFound = false; + + float shortestDistance = float.MaxValue; + + bool edgeCoordFound = false; + Vector2 foundEdgeCoord = Vector2.Zero; + + List xCoords = SearchCrossingEdges(polygon, (int)coordInsideThePolygon.Y); + + vertex1Index = 0; + vertex2Index = 0; + + foundEdgeCoord.Y = coordInsideThePolygon.Y; + + if (xCoords != null && xCoords.Count > 1 && xCoords.Count % 2 == 0) + { + float distance; + for (int i = 0; i < xCoords.Count; i++) + { + if (xCoords[i] < coordInsideThePolygon.X) + { + distance = coordInsideThePolygon.X - xCoords[i]; + + if (distance < shortestDistance) + { + shortestDistance = distance; + foundEdgeCoord.X = xCoords[i]; + + edgeCoordFound = true; + } + } + } + + if (edgeCoordFound) + { + shortestDistance = float.MaxValue; + + int edgeVertex2Index = polygon.Count - 1; + + int edgeVertex1Index; + for (edgeVertex1Index = 0; edgeVertex1Index < polygon.Count; edgeVertex1Index++) + { + Vector2 tempVector1 = polygon[edgeVertex1Index]; + Vector2 tempVector2 = polygon[edgeVertex2Index]; + distance = LineTools.DistanceBetweenPointAndLineSegment(ref foundEdgeCoord, + ref tempVector1, ref tempVector2); + if (distance < shortestDistance) + { + shortestDistance = distance; + + nearestEdgeVertex1Index = edgeVertex1Index; + nearestEdgeVertex2Index = edgeVertex2Index; + + edgeFound = true; + } + + edgeVertex2Index = edgeVertex1Index; + } + + if (edgeFound) + { + slope = polygon[nearestEdgeVertex2Index] - polygon[nearestEdgeVertex1Index]; + slope.Normalize(); + + Vector2 tempVector = polygon[nearestEdgeVertex1Index]; + distance = Vector2.Distance(tempVector, foundEdgeCoord); + + vertex1Index = nearestEdgeVertex1Index; + vertex2Index = nearestEdgeVertex1Index + 1; + + polygon.Insert(nearestEdgeVertex1Index, distance * slope + polygon[vertex1Index]); + polygon.Insert(nearestEdgeVertex1Index, distance * slope + polygon[vertex2Index]); + + return true; + } + } + } + + return false; + } + + /// + /// + /// + /// + /// + /// + private Vertices CreateSimplePolygon(Vector2 entrance, Vector2 last) + { + bool entranceFound = false; + bool endOfHull = false; + + Vertices polygon = new Vertices(32); + Vertices hullArea = new Vertices(32); + Vertices endOfHullArea = new Vertices(32); + + Vector2 current = Vector2.Zero; + + #region Entrance check + + // Get the entrance point. //todo: alle möglichkeiten testen + if (entrance == Vector2.Zero || !InBounds(ref entrance)) + { + entranceFound = SearchHullEntrance(out entrance); + + if (entranceFound) + { + current = new Vector2(entrance.X - 1f, entrance.Y); + } + } + else + { + if (IsSolid(ref entrance)) + { + if (IsNearPixel(ref entrance, ref last)) + { + current = last; + entranceFound = true; + } + else + { + Vector2 temp; + if (SearchNearPixels(false, ref entrance, out temp)) + { + current = temp; + entranceFound = true; + } + else + { + entranceFound = false; + } + } + } + } + + #endregion + + if (entranceFound) + { + polygon.Add(entrance); + hullArea.Add(entrance); + + Vector2 next = entrance; + + do + { + // Search in the pre vision list for an outstanding point. + Vector2 outstanding; + if (SearchForOutstandingVertex(hullArea, out outstanding)) + { + if (endOfHull) + { + // We have found the next pixel, but is it on the last bit of the hull? + if (endOfHullArea.Contains(outstanding)) + { + // Indeed. + polygon.Add(outstanding); + } + + // That's enough, quit. + break; + } + + // Add it and remove all vertices that don't matter anymore + // (all the vertices before the outstanding). + polygon.Add(outstanding); + hullArea.RemoveRange(0, hullArea.IndexOf(outstanding)); + } + + // Last point gets current and current gets next. Our little spider is moving forward on the hull ;). + last = current; + current = next; + + // Get the next point on hull. + if (GetNextHullPoint(ref last, ref current, out next)) + { + // Add the vertex to a hull pre vision list. + hullArea.Add(next); + } + else + { + // Quit + break; + } + + if (next == entrance && !endOfHull) + { + // It's the last bit of the hull, search on and exit at next found vertex. + endOfHull = true; + endOfHullArea.AddRange(hullArea); + + // We don't want the last vertex to be the same as the first one, because it causes the triangulation code to crash. + if (endOfHullArea.Contains(entrance)) + endOfHullArea.Remove(entrance); + } + + } while (true); + } + + return polygon; + } + + private bool SearchNearPixels(bool searchingForSolidPixel, ref Vector2 current, out Vector2 foundPixel) + { + for (int i = 0; i < ClosepixelsLength; i++) + { + int x = (int)current.X + _closePixels[i, 0]; + int y = (int)current.Y + _closePixels[i, 1]; + + if (!searchingForSolidPixel ^ IsSolid(ref x, ref y)) + { + foundPixel = new Vector2(x, y); + return true; + } + } + + // Nothing found. + foundPixel = Vector2.Zero; + return false; + } + + private bool IsNearPixel(ref Vector2 current, ref Vector2 near) + { + for (int i = 0; i < ClosepixelsLength; i++) + { + int x = (int)current.X + _closePixels[i, 0]; + int y = (int)current.Y + _closePixels[i, 1]; + + if (x >= 0 && x <= _width && y >= 0 && y <= _height) + { + if (x == (int)near.X && y == (int)near.Y) + { + return true; + } + } + } + + return false; + } + + private bool SearchHullEntrance(out Vector2 entrance) + { + // Search for first solid pixel. + for (int y = 0; y <= _height; y++) + { + for (int x = 0; x <= _width; x++) + { + if (IsSolid(ref x, ref y)) + { + entrance = new Vector2(x, y); + return true; + } + } + } + + // If there are no solid pixels. + entrance = Vector2.Zero; + return false; + } + + /// + /// Searches for the next shape. + /// + /// Already detected polygons. + /// Search start coordinate. + /// Returns the found entrance coordinate. Null if no other shapes found. + /// True if a new shape was found. + private bool SearchNextHullEntrance(List detectedPolygons, Vector2 start, out Vector2? entrance) + { + int x; + + bool foundTransparent = false; + bool inPolygon = false; + + for (int i = (int)start.X + (int)start.Y * _width; i <= _dataLength; i++) + { + if (IsSolid(ref i)) + { + if (foundTransparent) + { + x = i % _width; + entrance = new Vector2(x, (i - x) / (float)_width); + + inPolygon = false; + for (int polygonIdx = 0; polygonIdx < detectedPolygons.Count; polygonIdx++) + { + if (InPolygon(detectedPolygons[polygonIdx], entrance.Value)) + { + inPolygon = true; + break; + } + } + + if (inPolygon) + foundTransparent = false; + else + return true; + } + } + else + foundTransparent = true; + } + + entrance = null; + return false; + } + + private bool GetNextHullPoint(ref Vector2 last, ref Vector2 current, out Vector2 next) + { + int x; + int y; + + int indexOfFirstPixelToCheck = GetIndexOfFirstPixelToCheck(ref last, ref current); + int indexOfPixelToCheck; + + for (int i = 0; i < ClosepixelsLength; i++) + { + indexOfPixelToCheck = (indexOfFirstPixelToCheck + i) % ClosepixelsLength; + + x = (int)current.X + _closePixels[indexOfPixelToCheck, 0]; + y = (int)current.Y + _closePixels[indexOfPixelToCheck, 1]; + + if (x >= 0 && x < _width && y >= 0 && y <= _height) + { + if (IsSolid(ref x, ref y)) + { + next = new Vector2(x, y); + return true; + } + } + } + + next = Vector2.Zero; + return false; + } + + private bool SearchForOutstandingVertex(Vertices hullArea, out Vector2 outstanding) + { + Vector2 outstandingResult = Vector2.Zero; + bool found = false; + + if (hullArea.Count > 2) + { + int hullAreaLastPoint = hullArea.Count - 1; + + Vector2 tempVector1; + Vector2 tempVector2 = hullArea[0]; + Vector2 tempVector3 = hullArea[hullAreaLastPoint]; + + // Search between the first and last hull point. + for (int i = 1; i < hullAreaLastPoint; i++) + { + tempVector1 = hullArea[i]; + + // Check if the distance is over the one that's tolerable. + if (LineTools.DistanceBetweenPointAndLineSegment(ref tempVector1, ref tempVector2, ref tempVector3) >= _hullTolerance) + { + outstandingResult = hullArea[i]; + found = true; + break; + } + } + } + + outstanding = outstandingResult; + return found; + } + + private int GetIndexOfFirstPixelToCheck(ref Vector2 last, ref Vector2 current) + { + // .: pixel + // l: last position + // c: current position + // f: first pixel for next search + + // f . . + // l c . + // . . . + + //Calculate in which direction the last move went and decide over the next pixel to check. + switch ((int)(current.X - last.X)) + { + case 1: + switch ((int)(current.Y - last.Y)) + { + case 1: + return 1; + + case 0: + return 0; + + case -1: + return 7; + } + break; + + case 0: + switch ((int)(current.Y - last.Y)) + { + case 1: + return 2; + + case -1: + return 6; + } + break; + + case -1: + switch ((int)(current.Y - last.Y)) + { + case 1: + return 3; + + case 0: + return 4; + + case -1: + return 5; + } + break; + } + + return 0; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/Transform.cs b/src/Box2DNet/Common/Transform.cs deleted file mode 100644 index 0a11518..0000000 --- a/src/Box2DNet/Common/Transform.cs +++ /dev/null @@ -1,111 +0,0 @@ -/* - Box2DNet Copyright (c) 2018 codeyu https://github.com/codeyu/Box2DNet - Box2D original C++ version Copyright (c) 2006-2007 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.Numerics; -using System.Collections.Generic; -using System.Text; - - - -namespace Box2DNet.Common -{ - /// - /// A Transform contains translation and rotation. - /// It is used to represent the position and orientation of rigid frames. - /// - public struct Transform - { - public Vector2 position; -#if USE_MATRIX_FOR_ROTATION - public Mat22 rotation; -#else - public Quaternion rotation; -#endif - -#if USE_MATRIX_FOR_ROTATION - /// - /// Initialize using a position vector and a rotation matrix. - /// - /// - /// - public Transform(Vector2 position, Mat22 rotation) - { - this.position = position; - this.rotation = rotation; - } -#else - /// - /// Initialize using a position vector and a rotation matrix. - /// - /// - /// - public Transform(Vector2 position, Quaternion rotation) - { - this.position = position; - this.rotation = rotation; - } -#endif - - public Vector2 InverseTransformPoint(Vector2 vector) - { -#if USE_MATRIX_FOR_ROTATION - return Math.MulT(rotation, vector - position); -#else - return Quaternion.Inverse(rotation).Xyz().ToVector2() * (vector - position); -#endif - } - - public Vector2 InverseTransformDirection(Vector2 vector) - { -#if USE_MATRIX_FOR_ROTATION - return Math.MulT(rotation, vector); -#else - return Quaternion.Inverse(rotation).Xyz().ToVector2() * vector; -#endif - } - - public Vector2 TransformPoint(Vector2 vector) - { -#if USE_MATRIX_FOR_ROTATION - return position + Math.Mul(rotation, vector); -#else - return position + (rotation.Xyz() * vector.ToVector3()).ToVector2(); -#endif - - } - - // - // Transforms direction from local space to world space. - // - public Vector2 TransformDirection(Vector2 vector) - { -#if USE_MATRIX_FOR_ROTATION - return Math.Mul(rotation, vector); -#else - return (rotation.Xyz() * vector.ToVector3()).ToVector2(); -#endif - } - -#if USE_MATRIX_FOR_ROTATION - public static readonly Transform identify = new Transform(Vector2.zero, Mat22.Identity); -#else - public static readonly Transform identity = new Transform(Vector2.Zero, Quaternion.Identity); -#endif - } -} \ No newline at end of file diff --git a/src/Box2DNet/Common/Utils.cs b/src/Box2DNet/Common/Utils.cs deleted file mode 100644 index ce36856..0000000 --- a/src/Box2DNet/Common/Utils.cs +++ /dev/null @@ -1,62 +0,0 @@ -using System; -using System.Numerics; - -namespace Box2DNet.Common -{ - public static class Utils - { - public static Vector3 Xyz(this Quaternion q) - { - return new Vector3(q.X,q.Y,q.Z); - } - - public static void Normalize(this Vector2 v2) - { - var scale = 1.0f / v2.Length(); - v2.X *= scale; - v2.Y *= scale; - } - - public static Vector2 Normalized(this Vector2 v2) - { - - var scale = 1.0f / v2.Length(); - v2.X *= scale; - v2.Y *= scale; - return v2; - } - /// - /// Gets or sets the value at the index of the Vector. - /// - public static float GetByIndex(this Vector2 v2, int index) - { - - if (index == 0) - { - return v2.X; - } - else if (index == 1) - { - return v2.Y; - } - throw new IndexOutOfRangeException("You tried to access this vector at index: " + index); - - } - - public static void SetByIndex(this Vector2 v2, int index, float value) - { - switch (index) - { - case 0: - v2.X = value; - break; - case 1: - v2.Y = value; - break; - default: - throw new IndexOutOfRangeException("You tried to set this vector at index: " + index); - } - } - - } -} \ No newline at end of file diff --git a/src/Box2DNet/Common/Vec2.cs b/src/Box2DNet/Common/Vec2.cs deleted file mode 100644 index 60ccb46..0000000 --- a/src/Box2DNet/Common/Vec2.cs +++ /dev/null @@ -1,227 +0,0 @@ -/* - Box2DNet Copyright (c) 2018 codeyu https://github.com/codeyu/Box2DNet - Box2D original C++ version Copyright (c) 2006-2007 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.Numerics; -using System.Collections.Generic; -using System.Text; - -namespace Box2DNet.Common -{ - /// - /// A 2D column vector. - /// - public struct Vec2 - { - public float X, Y; - - public float this[int i] - { - get - { - if (i == 0) return X; - else if (i == 1) return Y; - else - { - Box2DNetDebug.Assert(false, "Incorrect Vec2 element!"); - return 0; - } - } - set - { - if (i == 0) X = value; - else if (i == 1) Y = value; - else - { - Box2DNetDebug.Assert(false, "Incorrect Vec2 element!"); - } - } - } - - /// - /// Construct using coordinates. - /// - public Vec2(float x) - { - X = x; - Y = x; - } - - /// - /// Construct using coordinates. - /// - public Vec2(float x, float y) - { - X = x; - Y = y; - } - - /// - /// Set this vector to all zeros. - /// - public void SetZero() { X = 0.0f; Y = 0.0f; } - - /// - /// Set this vector to some specified coordinates. - /// - public void Set(float x, float y) { X = x; Y = y; } - - public void Set(float xy) { X = xy; Y = xy; } - - /// - /// Get the length of this vector (the norm). - /// - public float Length() - { - return (float)System.Math.Sqrt(X * X + Y * Y); - } - - /// - /// Get the length squared. For performance, use this instead of - /// Length (if possible). - /// - public float LengthSquared() - { - return X * X + Y * Y; - } - - /// - /// Convert this vector into a unit vector. Returns the length. - /// - public float Normalize() - { - float length = Length(); - if (length < Settings.FLT_EPSILON) - { - return 0.0f; - } - float invLength = 1.0f / length; - X *= invLength; - Y *= invLength; - - return length; - } - - /// - /// Does this vector contain finite coordinates? - /// - public bool IsValid - { - get { return Math.IsValid(X) && Math.IsValid(Y); } - } - - /// - /// Negate this vector. - /// - public static Vec2 operator -(Vec2 v1) - { - Vec2 v = new Vec2(); - v.Set(-v1.X, -v1.Y); - return v; - } - - public static Vec2 operator +(Vec2 v1, Vec2 v2) - { - Vec2 v = new Vec2(); - v.Set(v1.X + v2.X, v1.Y + v2.Y); - return v; - } - - public static Vec2 operator -(Vec2 v1, Vec2 v2) - { - Vec2 v = new Vec2(); - v.Set(v1.X - v2.X, v1.Y - v2.Y); - return v; - } - - public static Vec2 operator *(Vec2 v1, float a) - { - Vec2 v = new Vec2(); - v.Set(v1.X * a, v1.Y * a); - return v; - } - - public static Vec2 operator *(float a, Vec2 v1) - { - Vec2 v = new Vec2(); - v.Set(v1.X * a, v1.Y * a); - return v; - } - - public static bool operator ==(Vec2 a, Vec2 b) - { - return a.X == b.X && a.Y == b.Y; - } - - public static bool operator !=(Vec2 a, Vec2 b) - { - return a.X != b.X || a.Y != b.Y; - } - - public static Vec2 Zero { get { return new Vec2(0, 0); } } - - /// - /// Peform the dot product on two vectors. - /// - public static float Dot(Vec2 a, Vec2 b) - { - return a.X * b.X + a.Y * b.Y; - } - - /// - /// Perform the cross product on two vectors. In 2D this produces a scalar. - /// - public static float Cross(Vec2 a, Vec2 b) - { - return a.X * b.Y - a.Y * b.X; - } - - /// - /// Perform the cross product on a vector and a scalar. - /// In 2D this produces a vector. - /// - public static Vec2 Cross(Vec2 a, float s) - { - Vec2 v = new Vec2(); - v.Set(s * a.Y, -s * a.X); - return v; - } - - /// - /// Perform the cross product on a scalar and a vector. - /// In 2D this produces a vector. - /// - public static Vec2 Cross(float s, Vec2 a) - { - Vec2 v = new Vec2(); - v.Set(-s * a.Y, s * a.X); - return v; - } - - public static float Distance(Vec2 a, Vec2 b) - { - Vec2 c = a - b; - return c.Length(); - } - - public static float DistanceSquared(Vec2 a, Vec2 b) - { - Vec2 c = a - b; - return Vec2.Dot(c, c); - } - } -} \ No newline at end of file diff --git a/src/Box2DNet/Common/Vec3.cs b/src/Box2DNet/Common/Vec3.cs deleted file mode 100644 index 768e2be..0000000 --- a/src/Box2DNet/Common/Vec3.cs +++ /dev/null @@ -1,105 +0,0 @@ -/* - Box2DNet Copyright (c) 2018 codeyu https://github.com/codeyu/Box2DNet - Box2D original C++ version Copyright (c) 2006-2007 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. -*/ - -//r175 - -using System; using System.Numerics; -using System.Collections.Generic; -using System.Text; - -namespace Box2DNet.Common -{ - /// - /// A 2D column vector with 3 elements. - /// - public struct Vec3 - { - /// - /// Construct using coordinates. - /// - public Vec3(float x, float y, float z) { X = x; Y = y; Z = z; } - - /// - /// Set this vector to all zeros. - /// - public void SetZero() { X = 0.0f; Y = 0.0f; Z = 0.0f; } - - /// - /// Set this vector to some specified coordinates. - /// - public void Set(float x, float y, float z) { X = x; Y = y; Z = z; } - - /// - /// Perform the dot product on two vectors. - /// - public static float Dot(Vec3 a, Vec3 b) - { - return a.X * b.X + a.Y * b.Y + a.Z * b.Z; - } - - /// - /// Perform the cross product on two vectors. - /// - public static Vec3 Cross(Vec3 a, Vec3 b) - { - return new Vec3(a.Y * b.Z - a.Z * b.Y, a.Z * b.X - a.X * b.Z, a.X * b.Y - a.Y * b.X); - } - - /// - /// Negate this vector. - /// - public static Vec3 operator -(Vec3 v) - { - return new Vec3(-v.X, -v.Y, -v.Z); - } - - /// - /// Add two vectors component-wise. - /// - public static Vec3 operator +(Vec3 v1, Vec3 v2) - { - return new Vec3(v1.X + v2.X, v1.Y + v2.Y, v1.Z + v2.Z); - } - - /// - /// Subtract two vectors component-wise. - /// - public static Vec3 operator -(Vec3 v1, Vec3 v2) - { - return new Vec3(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z); - } - - /// - /// Multiply this vector by a scalar. - /// - public static Vec3 operator *(Vec3 v, float s) - { - return new Vec3(v.X * s, v.Y * s, v.Z * s); - } - - /// - /// Multiply this vector by a scalar. - /// - public static Vec3 operator *(float s, Vec3 v) - { - return new Vec3(v.X * s, v.Y * s, v.Z * s); - } - - public float X, Y, Z; - } -} \ No newline at end of file diff --git a/src/Box2DNet/Common/Vertices.cs b/src/Box2DNet/Common/Vertices.cs new file mode 100644 index 0000000..2c8caed --- /dev/null +++ b/src/Box2DNet/Common/Vertices.cs @@ -0,0 +1,582 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +*/ + +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Text; +using Box2DNet.Collision; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Common +{ + public enum PolygonError + { + /// + /// There were no errors in the polygon + /// + NoError, + + /// + /// Polygon must have between 3 and Settings.MaxPolygonVertices vertices. + /// + InvalidAmountOfVertices, + + /// + /// Polygon must be simple. This means no overlapping edges. + /// + NotSimple, + + /// + /// Polygon must have a counter clockwise winding. + /// + NotCounterClockWise, + + /// + /// The polygon is concave, it needs to be convex. + /// + NotConvex, + + /// + /// Polygon area is too small. + /// + AreaTooSmall, + + /// + /// The polygon has a side that is too short. + /// + SideTooSmall + } + +#if !(XBOX360) + [DebuggerDisplay("Count = {Count} Vertices = {ToString()}")] +#endif + public class Vertices : List + { + public Vertices() { } + + public Vertices(int capacity) : base(capacity) { } + + public Vertices(IEnumerable vertices) + { + AddRange(vertices); + } + + internal bool AttachedToBody { get; set; } + + /// + /// You can add holes to this collection. + /// It will get respected by some of the triangulation algoithms, but otherwise not used. + /// + public List Holes { get; set; } + + /// + /// Gets the next index. Used for iterating all the edges with wrap-around. + /// + /// The current index + public int NextIndex(int index) + { + return (index + 1 > Count - 1) ? 0 : index + 1; + } + + /// + /// Gets the next vertex. Used for iterating all the edges with wrap-around. + /// + /// The current index + public Vector2 NextVertex(int index) + { + return this[NextIndex(index)]; + } + + /// + /// Gets the previous index. Used for iterating all the edges with wrap-around. + /// + /// The current index + public int PreviousIndex(int index) + { + return index - 1 < 0 ? Count - 1 : index - 1; + } + + /// + /// Gets the previous vertex. Used for iterating all the edges with wrap-around. + /// + /// The current index + public Vector2 PreviousVertex(int index) + { + return this[PreviousIndex(index)]; + } + + /// + /// Gets the signed area. + /// If the area is less than 0, it indicates that the polygon is clockwise winded. + /// + /// The signed area + public float GetSignedArea() + { + //The simplest polygon which can exist in the Euclidean plane has 3 sides. + if (Count < 3) + return 0; + + int i; + float area = 0; + + for (i = 0; i < Count; i++) + { + int j = (i + 1) % Count; + + Vector2 vi = this[i]; + Vector2 vj = this[j]; + + area += vi.X * vj.Y; + area -= vi.Y * vj.X; + } + area /= 2.0f; + return area; + } + + /// + /// Gets the area. + /// + /// + public float GetArea() + { + float area = GetSignedArea(); + return (area < 0 ? -area : area); + } + + /// + /// Gets the centroid. + /// + /// + public Vector2 GetCentroid() + { + //The simplest polygon which can exist in the Euclidean plane has 3 sides. + if (Count < 3) + return new Vector2(float.NaN, float.NaN); + + // Same algorithm is used by Box2D + Vector2 c = Vector2.Zero; + float area = 0.0f; + const float inv3 = 1.0f / 3.0f; + + for (int i = 0; i < Count; ++i) + { + // Triangle vertices. + Vector2 current = this[i]; + Vector2 next = (i + 1 < Count ? this[i + 1] : this[0]); + + float triangleArea = 0.5f * (current.X * next.Y - current.Y * next.X); + area += triangleArea; + + // Area weighted centroid + c += triangleArea * inv3 * (current + next); + } + + // Centroid + c *= 1.0f / area; + return c; + } + + /// + /// Returns an AABB that fully contains this polygon. + /// + public AABB GetAABB() + { + AABB aabb; + Vector2 lowerBound = new Vector2(float.MaxValue, float.MaxValue); + Vector2 upperBound = new Vector2(float.MinValue, float.MinValue); + + for (int i = 0; i < Count; ++i) + { + if (this[i].X < lowerBound.X) + { + lowerBound.X = this[i].X; + } + if (this[i].X > upperBound.X) + { + upperBound.X = this[i].X; + } + + if (this[i].Y < lowerBound.Y) + { + lowerBound.Y = this[i].Y; + } + if (this[i].Y > upperBound.Y) + { + upperBound.Y = this[i].Y; + } + } + + aabb.LowerBound = lowerBound; + aabb.UpperBound = upperBound; + + return aabb; + } + + /// + /// Translates the vertices with the specified vector. + /// + /// The value. + public void Translate(Vector2 value) + { + Translate(ref value); + } + + /// + /// Translates the vertices with the specified vector. + /// + /// The vector. + public void Translate(ref Vector2 value) + { + Debug.Assert(!AttachedToBody, "Translating vertices that are used by a Body can result in unstable behavior. Use Body.Position instead."); + + for (int i = 0; i < Count; i++) + this[i] = Vector2.Add(this[i], value); + + if (Holes != null && Holes.Count > 0) + { + foreach (Vertices hole in Holes) + { + hole.Translate(ref value); + } + } + } + + /// + /// Scales the vertices with the specified vector. + /// + /// The Value. + public void Scale(Vector2 value) + { + Scale(ref value); + } + + /// + /// Scales the vertices with the specified vector. + /// + /// The Value. + public void Scale(ref Vector2 value) + { + Debug.Assert(!AttachedToBody, "Scaling vertices that are used by a Body can result in unstable behavior."); + + for (int i = 0; i < Count; i++) + this[i] = Vector2.Multiply(this[i], value); + + if (Holes != null && Holes.Count > 0) + { + foreach (Vertices hole in Holes) + { + hole.Scale(ref value); + } + } + } + + /// + /// Rotate the vertices with the defined value in radians. + /// + /// Warning: Using this method on an active set of vertices of a Body, + /// will cause problems with collisions. Use Body.Rotation instead. + /// + /// The amount to rotate by in radians. + public void Rotate(float value) + { + Debug.Assert(!AttachedToBody, "Rotating vertices that are used by a Body can result in unstable behavior."); + + float num1 = (float)Math.Cos(value); + float num2 = (float)Math.Sin(value); + + for (int i = 0; i < Count; i++) + { + Vector2 position = this[i]; + this[i] = new Vector2((position.X * num1 + position.Y * -num2), (position.X * num2 + position.Y * num1)); + } + + if (Holes != null && Holes.Count > 0) + { + foreach (Vertices hole in Holes) + { + hole.Rotate(value); + } + } + } + + /// + /// Determines whether the polygon is convex. + /// O(n^2) running time. + /// + /// Assumptions: + /// - The polygon is in counter clockwise order + /// - The polygon has no overlapping edges + /// + /// + /// true if it is convex; otherwise, false. + /// + public bool IsConvex() + { + //The simplest polygon which can exist in the Euclidean plane has 3 sides. + if (Count < 3) + return false; + + //Triangles are always convex + if (Count == 3) + return true; + + // Checks the polygon is convex and the interior is to the left of each edge. + for (int i = 0; i < Count; ++i) + { + int next = i + 1 < Count ? i + 1 : 0; + Vector2 edge = this[next] - this[i]; + + for (int j = 0; j < Count; ++j) + { + // Don't check vertices on the current edge. + if (j == i || j == next) + continue; + + Vector2 r = this[j] - this[i]; + + float s = edge.X * r.Y - edge.Y * r.X; + + if (s <= 0.0f) + return false; + } + } + return true; + } + + /// + /// Indicates if the vertices are in counter clockwise order. + /// Warning: If the area of the polygon is 0, it is unable to determine the winding. + /// + public bool IsCounterClockWise() + { + //The simplest polygon which can exist in the Euclidean plane has 3 sides. + if (Count < 3) + return false; + + return (GetSignedArea() > 0.0f); + } + + /// + /// Forces the vertices to be counter clock wise order. + /// + public void ForceCounterClockWise() + { + //The simplest polygon which can exist in the Euclidean plane has 3 sides. + if (Count < 3) + return; + + if (!IsCounterClockWise()) + Reverse(); + } + + /// + /// Checks if the vertices forms an simple polygon by checking for edge crossings. + /// + public bool IsSimple() + { + //The simplest polygon which can exist in the Euclidean plane has 3 sides. + if (Count < 3) + return false; + + for (int i = 0; i < Count; ++i) + { + Vector2 a1 = this[i]; + Vector2 a2 = NextVertex(i); + for (int j = i + 1; j < Count; ++j) + { + Vector2 b1 = this[j]; + Vector2 b2 = NextVertex(j); + + Vector2 temp; + + if (LineTools.LineIntersect2(ref a1, ref a2, ref b1, ref b2, out temp)) + return false; + } + } + return true; + } + + /// + /// Checks if the polygon is valid for use in the engine. + /// + /// Performs a full check, for simplicity, convexity, + /// orientation, minimum angle, and volume. + /// + /// From Eric Jordan's convex decomposition library + /// + /// PolygonError.NoError if there were no error. + public PolygonError CheckPolygon() + { + if (Count < 3 || Count > Settings.MaxPolygonVertices) + return PolygonError.InvalidAmountOfVertices; + + if (!IsSimple()) + return PolygonError.NotSimple; + + if (GetArea() <= Settings.Epsilon) + return PolygonError.AreaTooSmall; + + if (!IsConvex()) + return PolygonError.NotConvex; + + //Check if the sides are of adequate length. + for (int i = 0; i < Count; ++i) + { + int next = i + 1 < Count ? i + 1 : 0; + Vector2 edge = this[next] - this[i]; + if (edge.LengthSquared() <= Settings.Epsilon*Settings.Epsilon) + { + return PolygonError.SideTooSmall; + } + } + + if (!IsCounterClockWise()) + return PolygonError.NotCounterClockWise; + + return PolygonError.NoError; + } + + /// + /// Projects to axis. + /// + /// The axis. + /// The min. + /// The max. + public void ProjectToAxis(ref Vector2 axis, out float min, out float max) + { + // To project a point on an axis use the dot product + float dotProduct = Vector2.Dot(axis, this[0]); + min = dotProduct; + max = dotProduct; + + for (int i = 0; i < Count; i++) + { + dotProduct = Vector2.Dot(this[i], axis); + if (dotProduct < min) + { + min = dotProduct; + } + else + { + if (dotProduct > max) + { + max = dotProduct; + } + } + } + } + + /// + /// Winding number test for a point in a polygon. + /// + /// See more info about the algorithm here: http://softsurfer.com/Archive/algorithm_0103/algorithm_0103.htm + /// The point to be tested. + /// -1 if the winding number is zero and the point is outside + /// the polygon, 1 if the point is inside the polygon, and 0 if the point + /// is on the polygons edge. + public int PointInPolygon(ref Vector2 point) + { + // Winding number + int wn = 0; + + // Iterate through polygon's edges + for (int i = 0; i < Count; i++) + { + // Get points + Vector2 p1 = this[i]; + Vector2 p2 = this[NextIndex(i)]; + + // Test if a point is directly on the edge + Vector2 edge = p2 - p1; + float area = MathUtils.Area(ref p1, ref p2, ref point); + if (area == 0f && Vector2.Dot(point - p1, edge) >= 0f && Vector2.Dot(point - p2, edge) <= 0f) + { + return 0; + } + // Test edge for intersection with ray from point + if (p1.Y <= point.Y) + { + if (p2.Y > point.Y && area > 0f) + { + ++wn; + } + } + else + { + if (p2.Y <= point.Y && area < 0f) + { + --wn; + } + } + } + return (wn == 0 ? -1 : 1); + } + + /// + /// Compute the sum of the angles made between the test point and each pair of points making up the polygon. + /// If this sum is 2pi then the point is an interior point, if 0 then the point is an exterior point. + /// ref: http://ozviz.wasp.uwa.edu.au/~pbourke/geometry/insidepoly/ - Solution 2 + /// + public bool PointInPolygonAngle(ref Vector2 point) + { + double angle = 0; + + // Iterate through polygon's edges + for (int i = 0; i < Count; i++) + { + // Get points + Vector2 p1 = this[i] - point; + Vector2 p2 = this[NextIndex(i)] - point; + + angle += MathUtils.VectorAngle(ref p1, ref p2); + } + + if (Math.Abs(angle) < Math.PI) + { + return false; + } + + return true; + } + + /// + /// Transforms the polygon using the defined matrix. + /// + /// The matrix to use as transformation. + public void Transform(ref Matrix transform) + { + // Transform main polygon + for (int i = 0; i < Count; i++) + this[i] = Vector2.Transform(this[i], transform); + + // Transform holes + if (Holes != null && Holes.Count > 0) + { + for (int i = 0; i < Holes.Count; i++) + { + Vector2[] temp = Holes[i].ToArray(); + Vector2.Transform(temp, ref transform, temp); + + Holes[i] = new Vertices(temp); + } + } + } + + public override string ToString() + { + StringBuilder builder = new StringBuilder(); + for (int i = 0; i < Count; i++) + { + builder.Append(this[i].ToString()); + if (i < Count - 1) + { + builder.Append(" "); + } + } + return builder.ToString(); + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Common/XForm.cs b/src/Box2DNet/Common/XForm.cs deleted file mode 100644 index 13fe7d6..0000000 --- a/src/Box2DNet/Common/XForm.cs +++ /dev/null @@ -1,90 +0,0 @@ -/* - Box2DNet Copyright (c) 2018 codeyu https://github.com/codeyu/Box2DNet - Box2D original C++ version Copyright (c) 2006-2007 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.Numerics; -using System.Collections.Generic; -using System.Text; - - -namespace Box2DNet.Common -{ - /// - /// A transform contains translation and rotation. - /// It is used to represent the position and orientation of rigid frames. - /// - public struct XForm - { - public Vector2 position; - public Mat22 R; - - /// - /// Initialize using a position vector and a rotation matrix. - /// - /// - /// - public XForm(Vector2 p, Mat22 rotation) - { - position = p; - R = rotation; - } - - /// - /// Set this to the identity transform. - /// - public void SetIdentity() - { - position = Vector2.Zero; - R = Mat22.Identity; - } - - /// Set this based on the position and angle. - public void Set(Vector2 p, float angle) - { - position = p; - R = new Mat22(angle); - } - - /// Calculate the angle that the rotation matrix represents. - public float GetAngle() - { - return Math.Atan2(R.Col1.Y, R.Col1.X); - } - - public Vector2 TransformDirection(Vector2 vector) - { - return Math.Mul(R, vector); - } - - public Vector2 InverseTransformDirection(Vector2 vector) - { - return Math.MulT(R, vector); - } - - public Vector2 TransformPoint(Vector2 vector) - { - return position + Math.Mul(R, vector); - } - - public Vector2 InverseTransformPoint(Vector2 vector) - { - return Math.MulT(R, vector - position); - } - - public static XForm Identity { get { return new XForm(Vector2.Zero, Mat22.Identity); } } - } -} \ No newline at end of file diff --git a/src/Box2DNet/Content/BodyContainer.cs b/src/Box2DNet/Content/BodyContainer.cs new file mode 100644 index 0000000..809b604 --- /dev/null +++ b/src/Box2DNet/Content/BodyContainer.cs @@ -0,0 +1,60 @@ +using System.Collections.Generic; +using Box2DNet.Collision.Shapes; +using Box2DNet.Dynamics; + +namespace Box2DNet.Content +{ + public class FixtureTemplate + { + public Shape Shape; + public float Restitution; + public float Friction; + public string Name; + } + + public class BodyTemplate + { + public readonly List Fixtures; + public float Mass; + public BodyType BodyType; + + public BodyTemplate() + { + Fixtures = new List(); + } + + public Body Create(World world) + { + Body body = new Body(world); + body.BodyType = BodyType; + + foreach (FixtureTemplate fixtureTemplate in Fixtures) + { + Fixture fixture = body.CreateFixture(fixtureTemplate.Shape, fixtureTemplate.Name); + fixture.Restitution = fixtureTemplate.Restitution; + fixture.Friction = fixtureTemplate.Friction; + } + + if (Mass > 0f) + body.Mass = Mass; + + return body; + } + + public BreakableBody CreateBreakable(World world) + { + List shapes = new List(); + foreach (FixtureTemplate f in Fixtures) + { + shapes.Add(f.Shape); + } + + BreakableBody body = new BreakableBody(world, shapes); + world.AddBreakableBody(body); + + return body; + } + } + + public class BodyContainer : Dictionary { } +} diff --git a/src/Box2DNet/Content/PolygonContainer.cs b/src/Box2DNet/Content/PolygonContainer.cs new file mode 100644 index 0000000..819e34a --- /dev/null +++ b/src/Box2DNet/Content/PolygonContainer.cs @@ -0,0 +1,49 @@ +using System.Collections.Generic; +using Box2DNet.Common; +using Box2DNet.Common.Decomposition; + +namespace Box2DNet.Content +{ + public struct Polygon + { + public Vertices Vertices; + public bool Closed; + + public Polygon(Vertices v, bool closed) + { + Vertices = v; + Closed = closed; + } + } + + public class PolygonContainer : Dictionary + { + public bool IsDecomposed + { + get { return _decomposed; } + } + + private bool _decomposed; + + public void Decompose() + { + Dictionary containerCopy = new Dictionary(this); + foreach (string key in containerCopy.Keys) + { + if (containerCopy[key].Closed) + { + List partition = Triangulate.ConvexPartition(containerCopy[key].Vertices, TriangulationAlgorithm.Bayazit); + if (partition.Count > 1) + { + Remove(key); + for (int i = 0; i < partition.Count; i++) + { + this[key + "_" + i.ToString()] = new Polygon(partition[i], true); + } + } + _decomposed = true; + } + } + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Controllers/AbstractForceController.cs b/src/Box2DNet/Controllers/AbstractForceController.cs new file mode 100644 index 0000000..3d8b255 --- /dev/null +++ b/src/Box2DNet/Controllers/AbstractForceController.cs @@ -0,0 +1,323 @@ +using System; +using Box2DNet.Dynamics; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Controllers +{ + public abstract class AbstractForceController : Controller + { + #region DecayModes enum + + /// + /// Modes for Decay. Actual Decay must be implemented in inheriting + /// classes + /// + public enum DecayModes + { + None, + Step, + Linear, + InverseSquare, + Curve + } + + #endregion + + #region ForceTypes enum + + /// + /// Forcetypes are used in the decay math to properly get the distance. + /// They are also used to draw a representation in DebugView + /// + public enum ForceTypes + { + Point, + Line, + Area + } + + #endregion + + #region TimingModes enum + + /// + /// Timing Modes + /// Switched: Standard on/off mode using the baseclass enabled property + /// Triggered: When the Trigger() method is called the force is active + /// for a specified Impulse Length + /// Curve: Still to be defined. The basic idea is having a Trigger + /// combined with a curve for the strength + /// + public enum TimingModes + { + Switched, + Triggered, + Curve + } + + #endregion + + /// + /// Curve to be used for Decay in Curve mode + /// + public Curve DecayCurve; + + /// + /// The Forcetype of the instance + /// + public ForceTypes ForceType; + + /// + /// Provided for reuse to provide Variation functionality in + /// inheriting classes + /// + protected Random Randomize; + + /// + /// Curve used by Curve Mode as an animated multiplier for the force + /// strength. + /// Only positions between 0 and 1 are considered as that range is + /// stretched to have ImpulseLength. + /// + public Curve StrengthCurve; + + /// + /// Constructor + /// + public AbstractForceController() + : base(ControllerType.AbstractForceController) + { + Enabled = true; + + Strength = 1.0f; + Position = new Vector2(0, 0); + MaximumSpeed = 100.0f; + TimingMode = TimingModes.Switched; + ImpulseTime = 0.0f; + ImpulseLength = 1.0f; + Triggered = false; + StrengthCurve = new Curve(); + Variation = 0.0f; + Randomize = new Random(1234); + DecayMode = DecayModes.None; + DecayCurve = new Curve(); + DecayStart = 0.0f; + DecayEnd = 0.0f; + + StrengthCurve.Keys.Add(new CurveKey(0, 5)); + StrengthCurve.Keys.Add(new CurveKey(0.1f, 5)); + StrengthCurve.Keys.Add(new CurveKey(0.2f, -4)); + StrengthCurve.Keys.Add(new CurveKey(1f, 0)); + } + + /// + /// Overloaded Contstructor with supplying Timing Mode + /// + /// + public AbstractForceController(TimingModes mode) + : base(ControllerType.AbstractForceController) + { + TimingMode = mode; + switch (mode) + { + case TimingModes.Switched: + Enabled = true; + break; + case TimingModes.Triggered: + Enabled = false; + break; + case TimingModes.Curve: + Enabled = false; + break; + } + } + + /// + /// Global Strength of the force to be applied + /// + public float Strength { get; set; } + + /// + /// Position of the Force. Can be ignored (left at (0,0) for forces + /// that are not position-dependent + /// + public Vector2 Position { get; set; } + + /// + /// Maximum speed of the bodies. Bodies that are travelling faster are + /// supposed to be ignored + /// + public float MaximumSpeed { get; set; } + + /// + /// Maximum Force to be applied. As opposed to Maximum Speed this is + /// independent of the velocity of + /// the affected body + /// + public float MaximumForce { get; set; } + + /// + /// Timing Mode of the force instance + /// + public TimingModes TimingMode { get; set; } + + /// + /// Time of the current impulse. Incremented in update till + /// ImpulseLength is reached + /// + public float ImpulseTime { get; private set; } + + /// + /// Length of a triggered impulse. Used in both Triggered and Curve Mode + /// + public float ImpulseLength { get; set; } + + /// + /// Indicating if we are currently during an Impulse + /// (Triggered and Curve Mode) + /// + public bool Triggered { get; private set; } + + /// + /// Variation of the force applied to each body affected + /// !! Must be used in inheriting classes properly !! + /// + public float Variation { get; set; } + + /// + /// See DecayModes + /// + public DecayModes DecayMode { get; set; } + + /// + /// Start of the distance based Decay. To set a non decaying area + /// + public float DecayStart { get; set; } + + /// + /// Maximum distance a force should be applied + /// + public float DecayEnd { get; set; } + + /// + /// Calculate the Decay for a given body. Meant to ease force + /// development and stick to the DRY principle and provide unified and + /// predictable decay math. + /// + /// The body to calculate decay for + /// A multiplier to multiply the force with to add decay + /// support in inheriting classes + protected float GetDecayMultiplier(Body body) + { + //TODO: Consider ForceType in distance calculation! + float distance = (body.Position - Position).Length(); + switch (DecayMode) + { + case DecayModes.None: + { + return 1.0f; + } + case DecayModes.Step: + { + if (distance < DecayEnd) + return 1.0f; + else + return 0.0f; + } + case DecayModes.Linear: + { + if (distance < DecayStart) + return 1.0f; + if (distance > DecayEnd) + return 0.0f; + return (DecayEnd - DecayStart / distance - DecayStart); + } + case DecayModes.InverseSquare: + { + if (distance < DecayStart) + return 1.0f; + else + return 1.0f / ((distance - DecayStart) * (distance - DecayStart)); + } + case DecayModes.Curve: + { + if (distance < DecayStart) + return 1.0f; + else + return DecayCurve.Evaluate(distance - DecayStart); + } + default: + return 1.0f; + } + } + + /// + /// Triggers the trigger modes (Trigger and Curve) + /// + public void Trigger() + { + Triggered = true; + ImpulseTime = 0; + } + + /// + /// Inherited from Controller + /// Depending on the TimingMode perform timing logic and call ApplyForce() + /// + /// + public override void Update(float dt) + { + switch (TimingMode) + { + case TimingModes.Switched: + { + if (Enabled) + { + ApplyForce(dt, Strength); + } + break; + } + case TimingModes.Triggered: + { + if (Enabled && Triggered) + { + if (ImpulseTime < ImpulseLength) + { + ApplyForce(dt, Strength); + ImpulseTime += dt; + } + else + { + Triggered = false; + } + } + break; + } + case TimingModes.Curve: + { + if (Enabled && Triggered) + { + if (ImpulseTime < ImpulseLength) + { + ApplyForce(dt, Strength * StrengthCurve.Evaluate(ImpulseTime)); + ImpulseTime += dt; + } + else + { + Triggered = false; + } + } + break; + } + } + } + + /// + /// Apply the force supplying strength (wich is modified in Update() + /// according to the TimingMode + /// + /// + /// The strength + public abstract void ApplyForce(float dt, float strength); + } +} \ No newline at end of file diff --git a/src/Box2DNet/Controllers/BuoyancyController.cs b/src/Box2DNet/Controllers/BuoyancyController.cs new file mode 100644 index 0000000..1f9af50 --- /dev/null +++ b/src/Box2DNet/Controllers/BuoyancyController.cs @@ -0,0 +1,134 @@ +using System.Collections.Generic; +using Box2DNet.Collision; +using Box2DNet.Collision.Shapes; +using Box2DNet.Dynamics; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Controllers +{ + public sealed class BuoyancyController : Controller + { + /// + /// Controls the rotational drag that the fluid exerts on the bodies within it. Use higher values will simulate thick fluid, like honey, lower values to + /// simulate water-like fluids. + /// + public float AngularDragCoefficient; + + /// + /// Density of the fluid. Higher values will make things more buoyant, lower values will cause things to sink. + /// + public float Density; + + /// + /// Controls the linear drag that the fluid exerts on the bodies within it. Use higher values will simulate thick fluid, like honey, lower values to + /// simulate water-like fluids. + /// + public float LinearDragCoefficient; + + /// + /// Acts like waterflow. Defaults to 0,0. + /// + public Vector2 Velocity; + + private AABB _container; + + private Vector2 _gravity; + private Vector2 _normal; + private float _offset; + private Dictionary _uniqueBodies = new Dictionary(); + + /// + /// Initializes a new instance of the class. + /// + /// Only bodies inside this AABB will be influenced by the controller + /// Density of the fluid + /// Linear drag coefficient of the fluid + /// Rotational drag coefficient of the fluid + /// The direction gravity acts. Buoyancy force will act in opposite direction of gravity. + public BuoyancyController(AABB container, float density, float linearDragCoefficient, float rotationalDragCoefficient, Vector2 gravity) + : base(ControllerType.BuoyancyController) + { + Container = container; + _normal = new Vector2(0, 1); + Density = density; + LinearDragCoefficient = linearDragCoefficient; + AngularDragCoefficient = rotationalDragCoefficient; + _gravity = gravity; + } + + public AABB Container + { + get { return _container; } + set + { + _container = value; + _offset = _container.UpperBound.Y; + } + } + + public override void Update(float dt) + { + _uniqueBodies.Clear(); + World.QueryAABB(fixture => + { + if (fixture.Body.IsStatic || !fixture.Body.Awake) + return true; + + if (!_uniqueBodies.ContainsKey(fixture.Body.BodyId)) + _uniqueBodies.Add(fixture.Body.BodyId, fixture.Body); + + return true; + }, ref _container); + + foreach (KeyValuePair kv in _uniqueBodies) + { + Body body = kv.Value; + + Vector2 areac = Vector2.Zero; + Vector2 massc = Vector2.Zero; + float area = 0; + float mass = 0; + + for (int j = 0; j < body.FixtureList.Count; j++) + { + Fixture fixture = body.FixtureList[j]; + + if (fixture.Shape.ShapeType != ShapeType.Polygon && fixture.Shape.ShapeType != ShapeType.Circle) + continue; + + Shape shape = fixture.Shape; + + Vector2 sc; + float sarea = shape.ComputeSubmergedArea(ref _normal, _offset, ref body._xf, out sc); + area += sarea; + areac.X += sarea * sc.X; + areac.Y += sarea * sc.Y; + + mass += sarea * shape.Density; + massc.X += sarea * sc.X * shape.Density; + massc.Y += sarea * sc.Y * shape.Density; + } + + areac.X /= area; + areac.Y /= area; + massc.X /= mass; + massc.Y /= mass; + + if (area < Settings.Epsilon) + continue; + + //Buoyancy + Vector2 buoyancyForce = -Density * area * _gravity; + body.ApplyForce(buoyancyForce, massc); + + //Linear drag + Vector2 dragForce = body.GetLinearVelocityFromWorldPoint(areac) - Velocity; + dragForce *= -LinearDragCoefficient * area; + body.ApplyForce(dragForce, areac); + + //Angular drag + body.ApplyTorque(-body.Inertia / body.Mass * area * body.AngularVelocity * AngularDragCoefficient); + } + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Controllers/Controller.cs b/src/Box2DNet/Controllers/Controller.cs new file mode 100644 index 0000000..9862ba3 --- /dev/null +++ b/src/Box2DNet/Controllers/Controller.cs @@ -0,0 +1,72 @@ +using System; +using Box2DNet.Common.PhysicsLogic; +using Box2DNet.Dynamics; + +namespace Box2DNet.Controllers +{ + [Flags] + public enum ControllerType + { + GravityController = (1 << 0), + VelocityLimitController = (1 << 1), + AbstractForceController = (1 << 2), + BuoyancyController = (1 << 3), + } + + public struct ControllerFilter + { + public ControllerType ControllerFlags; + + /// + /// Ignores the controller. The controller has no effect on this body. + /// + /// The controller type. + public void IgnoreController(ControllerType controller) + { + ControllerFlags |= controller; + } + + /// + /// Restore the controller. The controller affects this body. + /// + /// The controller type. + public void RestoreController(ControllerType controller) + { + ControllerFlags &= ~controller; + } + + /// + /// Determines whether this body ignores the the specified controller. + /// + /// The controller type. + /// + /// true if the body has the specified flag; otherwise, false. + /// + public bool IsControllerIgnored(ControllerType controller) + { + return (ControllerFlags & controller) == controller; + } + } + + public abstract class Controller : FilterData + { + public bool Enabled; + public World World; + private ControllerType _type; + + public Controller(ControllerType controllerType) + { + _type = controllerType; + } + + public override bool IsActiveOn(Body body) + { + if (body.ControllerFilter.IsControllerIgnored(_type)) + return false; + + return base.IsActiveOn(body); + } + + public abstract void Update(float dt); + } +} \ No newline at end of file diff --git a/src/Box2DNet/Controllers/GravityController.cs b/src/Box2DNet/Controllers/GravityController.cs new file mode 100644 index 0000000..30cf3b8 --- /dev/null +++ b/src/Box2DNet/Controllers/GravityController.cs @@ -0,0 +1,110 @@ +using System; +using System.Collections.Generic; +using Box2DNet.Dynamics; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Controllers +{ + public enum GravityType + { + Linear, + DistanceSquared + } + + public class GravityController : Controller + { + public GravityController(float strength) + : base(ControllerType.GravityController) + { + Strength = strength; + MaxRadius = float.MaxValue; + GravityType = GravityType.DistanceSquared; + Points = new List(); + Bodies = new List(); + } + + public GravityController(float strength, float maxRadius, float minRadius) + : base(ControllerType.GravityController) + { + MinRadius = minRadius; + MaxRadius = maxRadius; + Strength = strength; + GravityType = GravityType.DistanceSquared; + Points = new List(); + Bodies = new List(); + } + + public float MinRadius { get; set; } + public float MaxRadius { get; set; } + public float Strength { get; set; } + public GravityType GravityType { get; set; } + public List Bodies { get; set; } + public List Points { get; set; } + + public override void Update(float dt) + { + Vector2 f = Vector2.Zero; + + foreach (Body worldBody in World.BodyList) + { + if (!IsActiveOn(worldBody)) + continue; + + foreach (Body controllerBody in Bodies) + { + if (worldBody == controllerBody || (worldBody.IsStatic && controllerBody.IsStatic) || !controllerBody.Enabled) + continue; + + Vector2 d = controllerBody.Position - worldBody.Position; + float r2 = d.LengthSquared(); + + if (r2 <= Settings.Epsilon || r2 > MaxRadius * MaxRadius || r2 < MinRadius * MinRadius) + continue; + + switch (GravityType) + { + case GravityType.DistanceSquared: + f = Strength / r2 * worldBody.Mass * controllerBody.Mass * d; + break; + case GravityType.Linear: + f = Strength / (float)Math.Sqrt(r2) * worldBody.Mass * controllerBody.Mass * d; + break; + } + + worldBody.ApplyForce(ref f); + } + + foreach (Vector2 point in Points) + { + Vector2 d = point - worldBody.Position; + float r2 = d.LengthSquared(); + + if (r2 <= Settings.Epsilon || r2 > MaxRadius * MaxRadius || r2 < MinRadius * MinRadius) + continue; + + switch (GravityType) + { + case GravityType.DistanceSquared: + f = Strength / r2 * worldBody.Mass * d; + break; + case GravityType.Linear: + f = Strength / (float)Math.Sqrt(r2) * worldBody.Mass * d; + break; + } + + worldBody.ApplyForce(ref f); + } + } + } + + public void AddBody(Body body) + { + Bodies.Add(body); + } + + public void AddPoint(Vector2 point) + { + Points.Add(point); + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Controllers/SimpleWindForce.cs b/src/Box2DNet/Controllers/SimpleWindForce.cs new file mode 100644 index 0000000..071b3a9 --- /dev/null +++ b/src/Box2DNet/Controllers/SimpleWindForce.cs @@ -0,0 +1,75 @@ +using Box2DNet.Dynamics; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Controllers +{ + /// + /// Reference implementation for forces based on AbstractForceController + /// It supports all features provided by the base class and illustrates proper + /// usage as an easy to understand example. + /// As a side-effect it is a nice and easy to use wind force for your projects + /// + public class SimpleWindForce : AbstractForceController + { + /// + /// Direction of the windforce + /// + public Vector2 Direction { get; set; } + + /// + /// The amount of Direction randomization. Allowed range is 0-1. + /// + public float Divergence { get; set; } + + /// + /// Ignore the position and apply the force. If off only in the "front" (relative to position and direction) + /// will be affected + /// + public bool IgnorePosition { get; set; } + + + public override void ApplyForce(float dt, float strength) + { + foreach (Body body in World.BodyList) + { + //TODO: Consider Force Type + float decayMultiplier = GetDecayMultiplier(body); + + if (decayMultiplier != 0) + { + Vector2 forceVector; + + if (ForceType == ForceTypes.Point) + { + forceVector = body.Position - Position; + } + else + { + Direction.Normalize(); + + forceVector = Direction; + + if (forceVector.Length() == 0) + forceVector = new Vector2(0, 1); + } + + //TODO: Consider Divergence: + //forceVector = Vector2.Transform(forceVector, Matrix.CreateRotationZ((MathHelper.Pi - MathHelper.Pi/2) * (float)Randomize.NextDouble())); + + // Calculate random Variation + if (Variation != 0) + { + float strengthVariation = (float)Randomize.NextDouble() * MathHelper.Clamp(Variation, 0, 1); + forceVector.Normalize(); + body.ApplyForce(forceVector * strength * decayMultiplier * strengthVariation); + } + else + { + forceVector.Normalize(); + body.ApplyForce(forceVector * strength * decayMultiplier); + } + } + } + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Controllers/VelocityLimitController.cs b/src/Box2DNet/Controllers/VelocityLimitController.cs new file mode 100644 index 0000000..906a44a --- /dev/null +++ b/src/Box2DNet/Controllers/VelocityLimitController.cs @@ -0,0 +1,129 @@ +using System; +using System.Collections.Generic; +using Box2DNet.Dynamics; + +namespace Box2DNet.Controllers +{ + /// + /// Put a limit on the linear (translation - the movespeed) and angular (rotation) velocity + /// of bodies added to this controller. + /// + public class VelocityLimitController : Controller + { + public bool LimitAngularVelocity = true; + public bool LimitLinearVelocity = true; + private List _bodies = new List(); + private float _maxAngularSqared; + private float _maxAngularVelocity; + private float _maxLinearSqared; + private float _maxLinearVelocity; + + /// + /// Initializes a new instance of the class. + /// Sets the max linear velocity to Settings.MaxTranslation + /// Sets the max angular velocity to Settings.MaxRotation + /// + public VelocityLimitController() + : base(ControllerType.VelocityLimitController) + { + MaxLinearVelocity = Settings.MaxTranslation; + MaxAngularVelocity = Settings.MaxRotation; + } + + /// + /// Initializes a new instance of the class. + /// Pass in 0 or float.MaxValue to disable the limit. + /// maxAngularVelocity = 0 will disable the angular velocity limit. + /// + /// The max linear velocity. + /// The max angular velocity. + public VelocityLimitController(float maxLinearVelocity, float maxAngularVelocity) + : base(ControllerType.VelocityLimitController) + { + if (maxLinearVelocity == 0 || maxLinearVelocity == float.MaxValue) + LimitLinearVelocity = false; + + if (maxAngularVelocity == 0 || maxAngularVelocity == float.MaxValue) + LimitAngularVelocity = false; + + MaxLinearVelocity = maxLinearVelocity; + MaxAngularVelocity = maxAngularVelocity; + } + + /// + /// Gets or sets the max angular velocity. + /// + /// The max angular velocity. + public float MaxAngularVelocity + { + get { return _maxAngularVelocity; } + set + { + _maxAngularVelocity = value; + _maxAngularSqared = _maxAngularVelocity * _maxAngularVelocity; + } + } + + /// + /// Gets or sets the max linear velocity. + /// + /// The max linear velocity. + public float MaxLinearVelocity + { + get { return _maxLinearVelocity; } + set + { + _maxLinearVelocity = value; + _maxLinearSqared = _maxLinearVelocity * _maxLinearVelocity; + } + } + + public override void Update(float dt) + { + foreach (Body body in _bodies) + { + if (!IsActiveOn(body)) + continue; + + if (LimitLinearVelocity) + { + //Translation + // Check for large velocities. + float translationX = dt * body._linearVelocity.X; + float translationY = dt * body._linearVelocity.Y; + float result = translationX * translationX + translationY * translationY; + + if (result > dt * _maxLinearSqared) + { + float sq = (float)Math.Sqrt(result); + + float ratio = _maxLinearVelocity / sq; + body._linearVelocity.X *= ratio; + body._linearVelocity.Y *= ratio; + } + } + + if (LimitAngularVelocity) + { + //Rotation + float rotation = dt * body._angularVelocity; + if (rotation * rotation > _maxAngularSqared) + { + float ratio = _maxAngularVelocity / Math.Abs(rotation); + body._angularVelocity *= ratio; + } + } + } + } + + public void AddBody(Body body) + { + _bodies.Add(body); + } + + public void RemoveBody(Body body) + { + _bodies.Remove(body); + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/ConvertUnits.cs b/src/Box2DNet/ConvertUnits.cs new file mode 100644 index 0000000..824abf0 --- /dev/null +++ b/src/Box2DNet/ConvertUnits.cs @@ -0,0 +1,108 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +*/ + +using Microsoft.Xna.Framework; + +namespace Box2DNet +{ + /// + /// Convert units between display and simulation units. + /// + public static class ConvertUnits + { + private static float _displayUnitsToSimUnitsRatio = 100f; + private static float _simUnitsToDisplayUnitsRatio = 1 / _displayUnitsToSimUnitsRatio; + + public static void SetDisplayUnitToSimUnitRatio(float displayUnitsPerSimUnit) + { + _displayUnitsToSimUnitsRatio = displayUnitsPerSimUnit; + _simUnitsToDisplayUnitsRatio = 1 / displayUnitsPerSimUnit; + } + + public static float ToDisplayUnits(float simUnits) + { + return simUnits * _displayUnitsToSimUnitsRatio; + } + + public static float ToDisplayUnits(int simUnits) + { + return simUnits * _displayUnitsToSimUnitsRatio; + } + + public static Vector2 ToDisplayUnits(Vector2 simUnits) + { + return simUnits * _displayUnitsToSimUnitsRatio; + } + + public static void ToDisplayUnits(ref Vector2 simUnits, out Vector2 displayUnits) + { + Vector2.Multiply(ref simUnits, _displayUnitsToSimUnitsRatio, out displayUnits); + } + + public static Vector3 ToDisplayUnits(Vector3 simUnits) + { + return simUnits * _displayUnitsToSimUnitsRatio; + } + + public static Vector2 ToDisplayUnits(float x, float y) + { + return new Vector2(x, y) * _displayUnitsToSimUnitsRatio; + } + + public static void ToDisplayUnits(float x, float y, out Vector2 displayUnits) + { + displayUnits = Vector2.Zero; + displayUnits.X = x * _displayUnitsToSimUnitsRatio; + displayUnits.Y = y * _displayUnitsToSimUnitsRatio; + } + + public static float ToSimUnits(float displayUnits) + { + return displayUnits * _simUnitsToDisplayUnitsRatio; + } + + public static float ToSimUnits(double displayUnits) + { + return (float)displayUnits * _simUnitsToDisplayUnitsRatio; + } + + public static float ToSimUnits(int displayUnits) + { + return displayUnits * _simUnitsToDisplayUnitsRatio; + } + + public static Vector2 ToSimUnits(Vector2 displayUnits) + { + return displayUnits * _simUnitsToDisplayUnitsRatio; + } + + public static Vector3 ToSimUnits(Vector3 displayUnits) + { + return displayUnits * _simUnitsToDisplayUnitsRatio; + } + + public static void ToSimUnits(ref Vector2 displayUnits, out Vector2 simUnits) + { + Vector2.Multiply(ref displayUnits, _simUnitsToDisplayUnitsRatio, out simUnits); + } + + public static Vector2 ToSimUnits(float x, float y) + { + return new Vector2(x, y) * _simUnitsToDisplayUnitsRatio; + } + + public static Vector2 ToSimUnits(double x, double y) + { + return new Vector2((float)x, (float)y) * _simUnitsToDisplayUnitsRatio; + } + + public static void ToSimUnits(float x, float y, out Vector2 simUnits) + { + simUnits = Vector2.Zero; + simUnits.X = x * _simUnitsToDisplayUnitsRatio; + simUnits.Y = y * _simUnitsToDisplayUnitsRatio; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/DebugViewBase.cs b/src/Box2DNet/DebugViewBase.cs new file mode 100644 index 0000000..39236e8 --- /dev/null +++ b/src/Box2DNet/DebugViewBase.cs @@ -0,0 +1,165 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +*/ + +using System; +using Box2DNet.Common; +using Box2DNet.Dynamics; +using Microsoft.Xna.Framework; + +namespace Box2DNet +{ + [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 DebugViewBase + { + protected DebugViewBase(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, bool closed = true); + + /// + /// 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/src/Box2DNet/Dynamics/Body.cs b/src/Box2DNet/Dynamics/Body.cs index 4fc49a4..4a8859c 100644 --- a/src/Box2DNet/Dynamics/Body.cs +++ b/src/Box2DNet/Dynamics/Body.cs @@ -1,1105 +1,1367 @@ -/* - Box2DNet Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version 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.Numerics; - -using Box2DNet.Common; -using Box2DNet.Collision; - - -using Transform = Box2DNet.Common.Transform; - -namespace Box2DNet.Dynamics -{ - /// - /// A body definition holds all the data needed to construct a rigid body. - /// You can safely re-use body definitions. - /// - public struct BodyDef - { - /// - /// This constructor sets the body definition default values. - /// - public BodyDef(byte init) - { - MassData = new MassData(); - MassData.Center = Vector2.Zero; - MassData.Mass = 0.0f; - MassData.I = 0.0f; - UserData = null; - Position = Vector2.Zero; - Angle = 0.0f; - LinearVelocity = Vector2.Zero; - AngularVelocity = 0.0f; - LinearDamping = 0.0f; - AngularDamping = 0.0f; - AllowSleep = true; - IsSleeping = false; - FixedRotation = false; - IsBullet = false; - } - - /// - /// You can use this to initialized the mass properties of the body. - /// If you prefer, you can set the mass properties after the shapes - /// have been added using Body.SetMassFromShapes. - /// - public MassData MassData; - - /// - /// Use this to store application specific body data. - /// - public object UserData; - - /// - /// The world position of the body. Avoid creating bodies at the origin - /// since this can lead to many overlapping shapes. - /// - public Vector2 Position; - - /// - /// The world angle of the body in radians. - /// - public float Angle; - - /// The linear velocity of the body in world co-ordinates. - public Vector2 LinearVelocity; - - // The angular velocity of the body. - public float AngularVelocity; - - /// - /// Linear damping is use to reduce the linear velocity. The damping parameter - /// can be larger than 1.0f but the damping effect becomes sensitive to the - /// time step when the damping parameter is large. - /// - public float LinearDamping; - - /// - /// Angular damping is use to reduce the angular velocity. The damping parameter - /// can be larger than 1.0f but the damping effect becomes sensitive to the - /// time step when the damping parameter is large. - /// - public float AngularDamping; - - /// - /// Set this flag to false if this body should never fall asleep. Note that - /// this increases CPU usage. - /// - public bool AllowSleep; - - /// - /// Is this body initially sleeping? - /// - public bool IsSleeping; - - /// - /// Should this body be prevented from rotating? Useful for characters. - /// - public bool FixedRotation; - - /// - /// Is this a fast moving body that should be prevented from tunneling through - /// other moving bodies? Note that all bodies are prevented from tunneling through - /// static bodies. - /// @warning You should use this flag sparingly since it increases processing time. - /// - public bool IsBullet; - } - - /// - /// A rigid body. These are created via World.CreateBody. - /// - public class Body : IDisposable - { - [Flags] - public enum BodyFlags - { - Frozen = 0x0002, - Island = 0x0004, - Sleep = 0x0008, - AllowSleep = 0x0010, - Bullet = 0x0020, - FixedRotation = 0x0040 - } - - public enum BodyType - { - Static, - Dynamic, - MaxTypes - } - - internal BodyFlags _flags; - private BodyType _type; - - internal int _islandIndex; - - internal Transform _xf; // the body origin Transform - - internal Sweep _sweep; // the swept motion for CCD - - internal Vector2 _linearVelocity; - internal float _angularVelocity; - - internal Vector2 _force; - internal float _torque; - - private World _world; - internal Body _prev; - internal Body _next; - - internal Fixture _fixtureList; - internal int _fixtureCount; - - internal JointEdge _jointList; - internal ContactEdge _contactList; - - internal Controllers.ControllerEdge _controllerList; - - internal float _mass; - internal float _invMass; - internal float _I; - internal float _invI; - - internal float _linearDamping; - internal float _angularDamping; - - internal float _sleepTime; - - private object _userData; - - public int bodyID; - - static int bodyCount = 0; - - internal Body(BodyDef bd, World world) - { - Box2DNetDebug.Assert(world._lock == false); - - _flags = 0; - - if (bd.IsBullet) - { - _flags |= BodyFlags.Bullet; - } - if (bd.FixedRotation) - { - _flags |= BodyFlags.FixedRotation; - } - if (bd.AllowSleep) - { - _flags |= BodyFlags.AllowSleep; - } - if (bd.IsSleeping) - { - _flags |= BodyFlags.Sleep; - } - - _world = world; - - _xf.position = bd.Position; - _xf.rotation = Box2DNet.Common.Math.AngleToRotation(bd.Angle); - //_xf.R = new Mat22(bd.Angle); - - _sweep.LocalCenter = bd.MassData.Center; - _sweep.T0 = 1.0f; - _sweep.A0 = _sweep.A = bd.Angle; - _sweep.C0 = _sweep.C = _xf.TransformPoint(_sweep.LocalCenter); - - //_jointList = null; - //_contactList = null; - //_controllerList = null; - //_prev = null; - //_next = null; - - _linearVelocity = bd.LinearVelocity; - _angularVelocity = bd.AngularVelocity; - - _linearDamping = bd.LinearDamping; - _angularDamping = bd.AngularDamping; - - //_force.Set(0.0f, 0.0f); - //_torque = 0.0f; - - //_linearVelocity.SetZero(); - //_angularVelocity = 0.0f; - - //_sleepTime = 0.0f; - - //_invMass = 0.0f; - //_I = 0.0f; - //_invI = 0.0f; - - _mass = bd.MassData.Mass; - - if (_mass > 0.0f) - { - _invMass = 1.0f / _mass; - } - - _I = bd.MassData.I; - - if (_I > 0.0f && (_flags & BodyFlags.FixedRotation) == 0) - { - _invI = 1.0f / _I; - } - - if (_invMass == 0.0f && _invI == 0.0f) - { - _type = BodyType.Static; - } - else - { - _type = BodyType.Dynamic; - } - - _userData = bd.UserData; - - //_fixtureList = null; - //_fixtureCount = 0; - - bodyID = bodyCount++; - } - - public void Dispose() - { - Box2DNetDebug.Assert(_world._lock == false); - // shapes and joints are destroyed in World.Destroy - } - - internal bool SynchronizeFixtures() - { - Transform xf1 = new Transform(); - xf1.rotation = Box2DNet.Common.Math.AngleToRotation(_sweep.A0); - //xf1.R = new Mat22(_sweep.A0); - xf1.position = _sweep.C0 - xf1.TransformDirection(_sweep.LocalCenter); - - bool inRange = true; - for (Fixture f = _fixtureList; f != null; f = f.Next) - { - inRange = f.Synchronize(_world._broadPhase, xf1, _xf); - if (inRange == false) - { - break; - } - } - - if (inRange == false) - { - _flags |= BodyFlags.Frozen; - _linearVelocity = Vector2.Zero; - _angularVelocity = 0.0f; - - // Failure - return false; - } - - // Success - return true; - } - - // This is used to prevent connected bodies from colliding. - // It may lie, depending on the collideConnected flag. - internal bool IsConnected(Body other) - { - for (JointEdge jn = _jointList; jn != null; jn = jn.Next) - { - if (jn.Other == other) - return jn.Joint._collideConnected == false; - } - - return false; - } - - /// - /// Creates a fixture and attach it to this body. - /// @warning This function is locked during callbacks. - /// - /// The fixture definition. - public Fixture CreateFixture(FixtureDef def) - { - Box2DNetDebug.Assert(_world._lock == false); - if (_world._lock == true) - { - return null; - } - - BroadPhase broadPhase = _world._broadPhase; - - Fixture fixture = new Fixture(); - fixture.Create(broadPhase, this, _xf, def); - - fixture._next = _fixtureList; - _fixtureList = fixture; - ++_fixtureCount; - - fixture._body = this; - - return fixture; - } - - /// - /// Destroy a fixture. This removes the fixture from the broad-phase and - /// therefore destroys any contacts associated with this fixture. 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) - { - Box2DNetDebug.Assert(_world._lock == false); - if (_world._lock == true) - { - return; - } - - Box2DNetDebug.Assert(fixture.Body == this); - - // Remove the fixture from this body's singly linked list. - Box2DNetDebug.Assert(_fixtureCount > 0); - Fixture node = _fixtureList; - bool found = false; - while (node != null) - { - if (node == fixture) - { - //*node = fixture->m_next; - _fixtureList = fixture.Next; - found = true; - break; - } - - node = node.Next; - } - - // You tried to remove a shape that is not attached to this body. - Box2DNetDebug.Assert(found); - - BroadPhase broadPhase = _world._broadPhase; - - fixture.Destroy(broadPhase); - fixture._body = null; - fixture._next = null; - - --_fixtureCount; - } - - // TODO_ERIN adjust linear velocity and torque to account for movement of center. - /// - /// Set the mass properties. Note that this changes the center of mass position. - /// If you are not sure how to compute mass properties, use SetMassFromShapes. - /// The inertia tensor is assumed to be relative to the center of mass. - /// - /// The mass properties. - public void SetMass(MassData massData) - { - Box2DNetDebug.Assert(_world._lock == false); - if (_world._lock == true) - { - return; - } - - _invMass = 0.0f; - _I = 0.0f; - _invI = 0.0f; - - _mass = massData.Mass; - - if (_mass > 0.0f) - { - _invMass = 1.0f / _mass; - } - - _I = massData.I; - - if (_I > 0.0f && (_flags & BodyFlags.FixedRotation) == 0) - { - _invI = 1.0f / _I; - } - - // Move center of mass. - _sweep.LocalCenter = massData.Center; - _sweep.C0 = _sweep.C = _xf.TransformPoint(_sweep.LocalCenter); - - BodyType oldType = _type; - if (_invMass == 0.0f && _invI == 0.0f) - { - _type = BodyType.Static; - } - else - { - _type = BodyType.Dynamic; - } - - // If the body type changed, we need to refilter the broad-phase proxies. - if (oldType != _type) - { - for (Fixture f = _fixtureList; f != null; f = f.Next) - { - f.RefilterProxy(_world._broadPhase, _xf); - } - } - } - - // TODO_ERIN adjust linear velocity and torque to account for movement of center. - /// - /// Compute the mass properties from the attached shapes. You typically call this - /// after adding all the shapes. If you add or remove shapes later, you may want - /// to call this again. Note that this changes the center of mass position. - /// - public void SetMassFromShapes() - { - Box2DNetDebug.Assert(_world._lock == false); - if (_world._lock == true) - { - return; - } - - // Compute mass data from shapes. Each shape has its own density. - _mass = 0.0f; - _invMass = 0.0f; - _I = 0.0f; - _invI = 0.0f; - - Vector2 center = Vector2.Zero; - for (Fixture f = _fixtureList; f != null; f = f.Next) - { - MassData massData; - f.ComputeMass(out massData); - _mass += massData.Mass; - center += massData.Mass * massData.Center; - _I += massData.I; - } - - // Compute center of mass, and shift the origin to the COM. - if (_mass > 0.0f) - { - _invMass = 1.0f / _mass; - center *= _invMass; - } - - if (_I > 0.0f && (_flags & BodyFlags.FixedRotation) == 0) - { - // Center the inertia about the center of mass. - _I -= _mass * Vector2.Dot(center, center); - Box2DNetDebug.Assert(_I > 0.0f); - _invI = 1.0f / _I; - } - else - { - _I = 0.0f; - _invI = 0.0f; - } - - // Move center of mass. - _sweep.LocalCenter = center; - _sweep.C0 = _sweep.C = _xf.TransformPoint(_sweep.LocalCenter); - - BodyType oldType = _type; - if (_invMass == 0.0f && _invI == 0.0f) - { - _type = BodyType.Static; - } - else - { - _type = BodyType.Dynamic; - } - - // If the body type changed, we need to refilter the broad-phase proxies. - if (oldType != _type) - { - for (Fixture f = _fixtureList; f != null; f = f.Next) - { - f.RefilterProxy(_world._broadPhase, _xf); - } - } - } - - public bool SetTransform(Vector2 position, float angle) - { - return SetTransform(position, Box2DNet.Common.Math.AngleToRotation(angle)); - } - -#if USE_MATRIX_FOR_ROTATION - /// - /// Set the position of the body's origin and rotation (radians). - /// This breaks any contacts and wakes the other bodies. - /// - /// The new world position of the body's origin (not necessarily - /// the center of mass). - /// The new world rotation angle of the body in radians. - /// Return false if the movement put a shape outside the world. In this case the - /// body is automatically frozen. - public bool SetTransform(Vector2 position, Mat22 rotation) -#else - public bool SetTransform(Vector2 position, Quaternion rotation) -#endif - { - Box2DNetDebug.Assert(_world._lock == false); - if (_world._lock == true) - { - return true; - } - - if (IsFrozen()) - { - return false; - } - - _xf.rotation = rotation; - //_xf.R = rotation; - _xf.position = position; - - _sweep.C0 = _sweep.C = _xf.TransformPoint(_sweep.LocalCenter); -#if USE_MATRIX_FOR_ROTATION - _sweep.A0 = _sweep.A = rotation.GetAngle(); -#else - _sweep.A0 = _sweep.A = (rotation.Z * 180 / (float)System.Math.PI) * 0.0174532924f; -#endif - - bool freeze = false; - for (Fixture f = _fixtureList; f != null; f = f.Next) - { - bool inRange = f.Synchronize(_world._broadPhase, _xf, _xf); - - if (inRange == false) - { - freeze = true; - break; - } - } - - if (freeze == true) - { - _flags |= BodyFlags.Frozen; - _linearVelocity = Vector2.Zero; - _angularVelocity = 0.0f; - - // Failure - return false; - } - - // Success - _world._broadPhase.Commit(); - return true; - } - - /// - /// Set the position of the body's origin and rotation (radians). - /// This breaks any contacts and wakes the other bodies. - /// Note this is less efficient than the other overload - you should use that - /// if the angle is available. - /// - /// The Transform of position and angle to set the body to. - /// False if the movement put a shape outside the world. In this case the - /// body is automatically frozen. - public bool SetTransform(Transform xf) - { - return SetTransform(xf.position, xf.rotation); - } - - /// - /// Get the body Transform for the body's origin. - /// - /// Return the world Transform of the body's origin. - public Transform GetTransform() - { - return _xf; - } - - /// - /// Set the world body origin position. - /// - /// The new position of the body. - public void SetPosition(Vector2 position) - { -#if USE_MATRIX_FOR_ROTATION - SetTransform(position, new Mat22(GetAngle())); -#else - SetTransform(position, Box2DNet.Common.Math.AngleToRotation(GetAngle())); -#endif - } - - /// - /// Set the world body angle. - /// - /// The new angle of the body in radians - public void SetAngle(float angle) - { -#if USE_MATRIX_FOR_ROTATION - SetTransform(GetPosition(), new Mat22(angle)); -#else - SetTransform(GetPosition(), Box2DNet.Common.Math.AngleToRotation(angle)); -#endif - } - - /// - /// Get the world body origin position. - /// - /// Return the world position of the body's origin. - public Vector2 GetPosition() - { - return _xf.position; - } - - /// - /// Get the angle in radians. - /// - /// Return the current world rotation angle in radians. - public float GetAngle() - { - return _sweep.A; - } - - /// - /// Get the world position of the center of mass. - /// - /// - public Vector2 GetWorldCenter() - { - return _sweep.C; - } - - /// - /// Get the local position of the center of mass. - /// - /// - public Vector2 GetLocalCenter() - { - return _sweep.LocalCenter; - } - - /// - /// Set the linear velocity of the center of mass. - /// - /// The new linear velocity of the center of mass. - public void SetLinearVelocity(Vector2 v) - { - _linearVelocity = v; - } - - /// - /// Get the linear velocity of the center of mass. - /// - /// Return the linear velocity of the center of mass. - public Vector2 GetLinearVelocity() - { - return _linearVelocity; - } - - /// - /// Set the angular velocity. - /// - /// The new angular velocity in radians/second. - public void SetAngularVelocity(float w) - { - _angularVelocity = w; - } - - /// - /// Get the angular velocity. - /// - /// Return the angular velocity in radians/second. - public float GetAngularVelocity() - { - return _angularVelocity; - } - - /// - /// 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) - { - if (IsSleeping()) - { - WakeUp(); - } - - _force += force; - _torque += (point - _sweep.C).Cross(force); - } - - /// - /// Apply a torque. This affects the angular velocity - /// without affecting the linear velocity of the center of mass. - /// This wakes up the body. - /// - /// Torque about the z-axis (out of the screen), usually in N-m. - public void ApplyTorque(float torque) - { - if (IsSleeping()) - { - WakeUp(); - } - _torque += torque; - } - - /// - /// 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 ApplyImpulse(Vector2 impulse, Vector2 point) - { - if (IsSleeping()) - { - WakeUp(); - } - - _linearVelocity += _invMass * impulse; - _angularVelocity += _invI * (point - _sweep.C).Cross(impulse); - } - - /// - /// Get the total mass of the body. - /// - /// Return the mass, usually in kilograms (kg). - public float GetMass() - { - return _mass; - } - - /// - /// Get the central rotational inertia of the body. - /// - /// Return the rotational inertia, usually in kg-m^2. - public float GetInertia() - { - return _I; - } - - /// - /// Get the mass data of the body. - /// - /// A struct containing the mass, inertia and center of the body. - public MassData GetMassData() - { - MassData massData = new MassData(); - massData.Mass = _mass; - massData.I = _I; - massData.Center = GetWorldCenter(); - return massData; - } - - /// - /// Get the world coordinates of a point given the local coordinates. - /// - /// A point on the body measured relative the the body's origin. - /// Return the same point expressed in world coordinates. - public Vector2 GetWorldPoint(Vector2 localPoint) - { - return _xf.TransformPoint(localPoint); - } - - /// - /// Get the world coordinates of a vector given the local coordinates. - /// - /// A vector fixed in the body. - /// Return the same vector expressed in world coordinates. - public Vector2 GetWorldVector(Vector2 localVector) - { - return _xf.TransformDirection(localVector); - } - - /// - /// Gets a local point relative to the body's origin given a world point. - /// - /// A point in world coordinates. - /// Return the corresponding local point relative to the body's origin. - public Vector2 GetLocalPoint(Vector2 worldPoint) - { - return _xf.InverseTransformPoint(worldPoint); - } - - /// - /// Gets a local vector given a world vector. - /// - /// A vector in world coordinates. - /// Return the corresponding local vector. - public Vector2 GetLocalVector(Vector2 worldVector) - { - return _xf.InverseTransformDirection(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 _linearVelocity + (worldPoint - _sweep.C).CrossScalarPreMultiply(_angularVelocity); - } - - /// - /// 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 GetLinearVelocityFromWorldPoint(GetWorldPoint(localPoint)); - } - - public float GetLinearDamping() - { - return _linearDamping; - } - - public void SetLinearDamping(float linearDamping) - { - _linearDamping = linearDamping; - } - - public float GetAngularDamping() - { - return _angularDamping; - } - - public void SetAngularDamping(float angularDamping) - { - _angularDamping = angularDamping; - } - - /// - /// Is this body treated like a bullet for continuous collision detection? - /// - /// - public bool IsBullet() - { - return (_flags & BodyFlags.Bullet) == BodyFlags.Bullet; - } - - /// - /// Should this body be treated like a bullet for continuous collision detection? - /// - /// - public void SetBullet(bool flag) - { - if (flag) - { - _flags |= BodyFlags.Bullet; - } - else - { - _flags &= ~BodyFlags.Bullet; - } - } - - public bool IsFixedRotation() - { - return (_flags & BodyFlags.FixedRotation) == BodyFlags.FixedRotation; - } - - public void SetFixedRotation(bool fixedr) - { - if (fixedr) - { - _angularVelocity = 0.0f; - _invI = 0.0f; - _flags |= BodyFlags.FixedRotation; - } - else - { - if (_I > 0.0f) - { - // Recover _invI from _I. - _invI = 1.0f / _I; - _flags &= BodyFlags.FixedRotation; - } - // TODO: Else what? - } - } - - /// - /// Is this body static (immovable)? - /// - /// - public bool IsStatic() - { - return _type == BodyType.Static; - } - - public void SetStatic() - { - if (_type == BodyType.Static) - return; - _mass = 0.0f; - _invMass = 0.0f; - _I = 0.0f; - _invI = 0.0f; - _type = BodyType.Static; - - for (Fixture f = _fixtureList; f != null; f = f.Next) - { - f.RefilterProxy(_world._broadPhase, _xf); - } - } - - /// - /// Is this body dynamic (movable)? - /// - /// - public bool IsDynamic() - { - return _type == BodyType.Dynamic; - } - - /// - /// Is this body frozen? - /// - /// - public bool IsFrozen() - { - return (_flags & BodyFlags.Frozen) == BodyFlags.Frozen; - } - - /// - /// Is this body sleeping (not simulating). - /// - /// - public bool IsSleeping() - { - return (_flags & BodyFlags.Sleep) == BodyFlags.Sleep; - } - - public bool IsAllowSleeping() - { - return (_flags & BodyFlags.AllowSleep) == BodyFlags.AllowSleep; - } - - /// - /// You can disable sleeping on this body. - /// - /// - public void AllowSleeping(bool flag) - { - if (flag) - { - _flags |= BodyFlags.AllowSleep; - } - else - { - _flags &= ~BodyFlags.AllowSleep; - WakeUp(); - } - } - - /// - /// Wake up this body so it will begin simulating. - /// - public void WakeUp() - { - _flags &= ~BodyFlags.Sleep; - _sleepTime = 0.0f; - } - - /// - /// Put this body to sleep so it will stop simulating. - /// This also sets the velocity to zero. - /// - public void PutToSleep() - { - _flags |= BodyFlags.Sleep; - _sleepTime = 0.0f; - _linearVelocity = Vector2.Zero; - _angularVelocity = 0.0f; - _force = Vector2.Zero; - _torque = 0.0f; - } - - /// - /// Get the list of all fixtures attached to this body. - /// - /// - public Fixture GetFixtureList() - { - return _fixtureList; - } - - /// - /// Get the list of all joints attached to this body. - /// - /// - public JointEdge GetJointList() - { - return _jointList; - } - - public Controllers.ControllerEdge GetControllerList() - { - return _controllerList; - } - - /// - /// Get the next body in the world's body list. - /// - /// - public Body GetNext() - { - return _next; - } - - /// - /// Get the user data pointer that was provided in the body definition. - /// - /// - public object GetUserData() - { - return _userData; - } - - /// - /// Set the user data. Use this to store your application specific data. - /// - /// - public void SetUserData(object data) { _userData = data; } - - /// - /// Get the parent world of this body. - /// - /// - public World GetWorld() { return _world; } - - internal void SynchronizeTransform() - { - //_xf.R = new Mat22(_sweep.A); - _xf.rotation = Box2DNet.Common.Math.AngleToRotation(_sweep.A); - _xf.position = _sweep.C - _xf.TransformDirection(_sweep.LocalCenter);//Common.Math.Mul(_xf.R, _sweep.LocalCenter); - } - - internal void Advance(float t) - { - // Advance to the new safe time. - _sweep.Advance(t); - _sweep.C = _sweep.C0; - _sweep.A = _sweep.A0; - SynchronizeTransform(); - } - } -} +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ +//#define USE_AWAKE_BODY_SET + +using System; +using System.Collections.Generic; +using System.Diagnostics; +using Box2DNet.Collision; +using Box2DNet.Collision.Shapes; +using Box2DNet.Common; +using Box2DNet.Common.PhysicsLogic; +using Box2DNet.Controllers; +using Box2DNet.Dynamics.Contacts; +using Box2DNet.Dynamics.Joints; +using Microsoft.Xna.Framework; + +namespace Box2DNet.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, + } + + public class Body : IDisposable + { + [ThreadStatic] + private static int _bodyIdCounter; + + private float _angularDamping; + private BodyType _bodyType; + private float _inertia; + private float _linearDamping; + private float _mass; + private bool _sleepingAllowed; + private bool _awake; + private bool _fixedRotation; + + internal bool _enabled; + internal float _angularVelocity; + internal Vector2 _linearVelocity; + internal Vector2 _force; + internal float _invI; + internal float _invMass; + internal float _sleepTime; + internal Sweep _sweep; // the swept motion for CCD + internal float _torque; + internal World _world; + internal Transform _xf; // the body origin transform + internal bool _island; + + public PhysicsLogicFilter PhysicsLogicFilter; + public ControllerFilter ControllerFilter; + + public Body(World world, Vector2 position = new Vector2(), float rotation = 0, BodyType bodyType = BodyType.Static, object userdata = null) + { + FixtureList = new List(); + BodyId = _bodyIdCounter++; + + _world = world; + _enabled = true; + _awake = true; + _sleepingAllowed = true; + + UserData = userdata; + GravityScale = 1.0f; + BodyType = bodyType; + + _xf.q.Set(rotation); + + //FPE: optimization + if (position != Vector2.Zero) + { + _xf.p = position; + _sweep.C0 = _xf.p; + _sweep.C = _xf.p; + } + + //FPE: optimization + if (rotation != 0) + { + _sweep.A0 = rotation; + _sweep.A = rotation; + } + + world.AddBody(this); //FPE note: bodies can't live without a World + } + + /// + /// A unique id for this body. + /// + public int BodyId { get; private set; } + + public int IslandIndex { get; set; } + + /// + /// Scale the gravity applied to this body. + /// Defaults to 1. A value of 2 means double the gravity is applied to this body. + /// + public float GravityScale { get; set; } + + /// + /// Set the user data. Use this to store your application specific data. + /// + /// The user data. + public object UserData { get; set; } + + /// + /// 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. + /// Warning: Calling this mid-update might cause a crash. + /// + /// The type of body. + public BodyType BodyType + { + get { return _bodyType; } + set + { + if (_bodyType == value) + return; + + _bodyType = value; + + ResetMassData(); + + if (_bodyType == BodyType.Static) + { + _linearVelocity = Vector2.Zero; + _angularVelocity = 0.0f; + _sweep.A0 = _sweep.A; + _sweep.C0 = _sweep.C; + SynchronizeFixtures(); + } + + Awake = true; + + _force = Vector2.Zero; + _torque = 0.0f; + + // Delete the attached contacts. + ContactEdge ce = ContactList; + while (ce != null) + { + ContactEdge ce0 = ce; + ce = ce.Next; + _world.ContactManager.Destroy(ce0.Contact); + } + + ContactList = null; + + // Touch the proxies so that new contacts will be created (when appropriate) + IBroadPhase broadPhase = _world.ContactManager.BroadPhase; + foreach (Fixture fixture in FixtureList) + { + int proxyCount = fixture.ProxyCount; + for (int j = 0; j < proxyCount; j++) + { + broadPhase.TouchProxy(fixture.Proxies[j].ProxyId); + } + } + } + } + + /// + /// 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; + + _linearVelocity = value; + } + get { return _linearVelocity; } + } + + /// + /// 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; + + _angularVelocity = value; + } + get { return _angularVelocity; } + } + + /// + /// 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 { get; set; } + + /// + /// 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) + Awake = true; + + _sleepingAllowed = value; + } + get { return _sleepingAllowed; } + } + + /// + /// 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 (!_awake) + { + _sleepTime = 0.0f; + _world.ContactManager.UpdateContacts(ContactList, true); +#if USE_AWAKE_BODY_SET + if (InWorld && !World.AwakeBodySet.Contains(this)) + { + World.AwakeBodySet.Add(this); + } +#endif + } + } + else + { +#if USE_AWAKE_BODY_SET + // Check even for BodyType.Static because if this body had just been changed to Static it will have + // set Awake = false in the process. + if (InWorld && World.AwakeBodySet.Contains(this)) + { + World.AwakeBodySet.Remove(this); + } +#endif + ResetDynamics(); + _sleepTime = 0.0f; + _world.ContactManager.UpdateContacts(ContactList, false); + } + + _awake = value; + } + get { return _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) + { + // 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 + { + // 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; + } + + _enabled = value; + } + get { return _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 (_fixedRotation == value) + return; + + _fixedRotation = value; + + _angularVelocity = 0f; + ResetMassData(); + } + get { return _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; } + + /// + /// Get the world body origin position. + /// + /// Return the world position of the body's origin. + public Vector2 Position + { + get { return _xf.p; } + 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.p, 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 { BodyType = value ? BodyType.Static : BodyType.Dynamic; } + } + + /// + /// Gets or sets a value indicating whether this body is kinematic. + /// + /// true if this instance is kinematic; otherwise, false. + public bool IsKinematic + { + get { return _bodyType == BodyType.Kinematic; } + set { BodyType = value ? BodyType.Kinematic : BodyType.Dynamic; } + } + + /// + /// Gets or sets a value indicating whether this body ignores gravity. + /// + /// true if it ignores gravity; otherwise, false. + public bool IgnoreGravity { get; set; } + + /// + /// 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.Mul(ref _xf, ref _sweep.LocalCenter); + + // Update center of mass velocity. + Vector2 a = _sweep.C - oldCenter; + _linearVelocity += new Vector2(-_angularVelocity * a.Y, _angularVelocity * 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) //Make an assert + 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) //Make an assert + return; + + if (value > 0.0f && !_fixedRotation) //Make an assert + { + _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 FixtureList.Count > 0 ? res / FixtureList.Count : 0; + } + 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 FixtureList.Count > 0 ? res / FixtureList.Count : 0; + } + 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; + } + } + } + + /// + /// Body objects can define which categories of bodies they wish to ignore CCD with. + /// This allows certain bodies to be configured to ignore CCD with objects that + /// aren't a penetration problem due to the way content has been prepared. + /// This is compared against the other Body's fixture CollisionCategories within World.SolveTOI(). + /// + public Category IgnoreCCDWith + { + set + { + for (int i = 0; i < FixtureList.Count; i++) + { + Fixture f = FixtureList[i]; + f.IgnoreCCDWith = 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; set; } + + /// + /// Resets the dynamics of this body. + /// Sets torque, force and linear/angular velocity to 0 + /// + public void ResetDynamics() + { + _torque = 0; + _angularVelocity = 0; + _force = Vector2.Zero; + _linearVelocity = 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. + /// Application specific data + /// + public Fixture CreateFixture(Shape shape, object userData = null) + { + 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 (_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) + { + _xf.q.Set(angle); + _xf.p = position; + + _sweep.C = MathUtils.Mul(ref _xf, _sweep.LocalCenter); + _sweep.A = angle; + + _sweep.C0 = _sweep.C; + _sweep.A0 = 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.p); + } + + /// + /// Applies a force at the center of mass. + /// + /// The force. + public void ApplyForce(Vector2 force) + { + ApplyForce(ref force, ref _xf.p); + } + + /// + /// 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; + } + _linearVelocity += _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; + + _linearVelocity += _invMass * impulse; + _angularVelocity += _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; + } + + _angularVelocity += _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 = _xf.p; + _sweep.C = _xf.p; + _sweep.A0 = _sweep.A; + return; + } + + Debug.Assert(BodyType == BodyType.Dynamic || BodyType == BodyType.Static); + + // Accumulate mass over all fixtures. + Vector2 localCenter = Vector2.Zero; + foreach (Fixture f in FixtureList) + { + if (f.Shape._density == 0) + { + continue; + } + + MassData massData = f.Shape.MassData; + _mass += massData.Mass; + localCenter += massData.Mass * massData.Centroid; + _inertia += massData.Inertia; + } + + //FPE: Static bodies only have mass, they don't have other properties. A little hacky tho... + if (BodyType == BodyType.Static) + { + _sweep.C0 = _sweep.C = _xf.p; + return; + } + + // Compute center of mass. + if (_mass > 0.0f) + { + _invMass = 1.0f / _mass; + localCenter *= _invMass; + } + else + { + // Force all dynamic bodies to have a positive mass. + _mass = 1.0f; + _invMass = 1.0f; + } + + if (_inertia > 0.0f && !_fixedRotation) + { + // Center the inertia about the center of mass. + _inertia -= _mass * Vector2.Dot(localCenter, localCenter); + + 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 = localCenter; + _sweep.C0 = _sweep.C = MathUtils.Mul(ref _xf, ref _sweep.LocalCenter); + + // Update center of mass velocity. + Vector2 a = _sweep.C - oldCenter; + _linearVelocity += new Vector2(-_angularVelocity * a.Y, _angularVelocity * 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 MathUtils.Mul(ref _xf, ref localPoint); + } + + /// + /// 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 MathUtils.Mul(_xf.q, localVector); + } + + /// + /// 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 MathUtils.MulT(ref _xf, worldPoint); + } + + /// + /// 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 MathUtils.MulT(_xf.q, worldVector); + } + + /// + /// 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 _linearVelocity + + new Vector2(-_angularVelocity * (worldPoint.Y - _sweep.C.Y), + _angularVelocity * (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)); + } + + internal void SynchronizeFixtures() + { + Transform xf1 = new Transform(); + xf1.q.Set(_sweep.A0); + xf1.p = _sweep.C0 - MathUtils.Mul(xf1.q, _sweep.LocalCenter); + + 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.q.Set(_sweep.A); + _xf.p = _sweep.C - MathUtils.Mul(_xf.q, _sweep.LocalCenter); + } + + /// + /// 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. This doesn't sync the broad-phase. + _sweep.Advance(alpha); + _sweep.C = _sweep.C0; + _sweep.A = _sweep.A0; + _xf.q.Set(_sweep.A); + _xf.p = _sweep.C - MathUtils.Mul(_xf.q, _sweep.LocalCenter); + } + + 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++) + { + for (int j = 0; j < other.FixtureList.Count; j++) + { + FixtureList[i].IgnoreCollisionWith(other.FixtureList[j]); + } + } + } + + public void RestoreCollisionWith(Body other) + { + for (int i = 0; i < FixtureList.Count; i++) + { + for (int j = 0; j < other.FixtureList.Count; j++) + { + FixtureList[i].RestoreCollisionWith(other.FixtureList[j]); + } + } + } + + #region IDisposable Members + + public bool IsDisposed { get; set; } + + public void Dispose() + { + if (!IsDisposed) + { + _world.RemoveBody(this); + IsDisposed = true; + GC.SuppressFinalize(this); + } + } + + #endregion + + /// + /// Makes a clone of the body. Fixtures and therefore shapes are not included. + /// Use DeepClone() to clone the body, as well as fixtures and shapes. + /// + /// + /// + public Body Clone(World world = null) + { + Body body = new Body(world ?? _world, Position, Rotation); + body._bodyType = _bodyType; + body._linearVelocity = _linearVelocity; + body._angularVelocity = _angularVelocity; + body.GravityScale = GravityScale; + body.UserData = UserData; + body._enabled = _enabled; + body._fixedRotation = _fixedRotation; + body._sleepingAllowed = _sleepingAllowed; + body._linearDamping = _linearDamping; + body._angularDamping = _angularDamping; + body._awake = _awake; + body.IsBullet = IsBullet; + body.IgnoreCCD = IgnoreCCD; + body.IgnoreGravity = IgnoreGravity; + body._torque = _torque; + + return body; + } + + /// + /// Clones the body and all attached fixtures and shapes. Simply said, it makes a complete copy of the body. + /// + /// + /// + public Body DeepClone(World world = null) + { + Body body = Clone(world ?? _world); + + int count = FixtureList.Count; //Make a copy of the count. Otherwise it causes an infinite loop. + for (int i = 0; i < count; i++) + { + FixtureList[i].CloneOnto(body); + } + + return body; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/BreakableBody.cs b/src/Box2DNet/Dynamics/BreakableBody.cs new file mode 100644 index 0000000..284f990 --- /dev/null +++ b/src/Box2DNet/Dynamics/BreakableBody.cs @@ -0,0 +1,142 @@ +using System; +using System.Collections.Generic; +using Box2DNet.Collision.Shapes; +using Box2DNet.Common; +using Box2DNet.Dynamics.Contacts; +using Box2DNet.Factories; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Dynamics +{ + /// + /// A type of body that supports multiple fixtures that can break apart. + /// + public class BreakableBody + { + private float[] _angularVelocitiesCache = new float[8]; + private bool _break; + private Vector2[] _velocitiesCache = new Vector2[8]; + private World _world; + + public BreakableBody(World world, IEnumerable vertices, float density, Vector2 position = new Vector2(), float rotation = 0) + { + _world = world; + _world.ContactManager.PostSolve += PostSolve; + MainBody = new Body(_world, position, rotation, BodyType.Dynamic); + + foreach (Vertices part in vertices) + { + PolygonShape polygonShape = new PolygonShape(part, density); + Fixture fixture = MainBody.CreateFixture(polygonShape); + Parts.Add(fixture); + } + } + + public BreakableBody(World world, IEnumerable shapes, Vector2 position = new Vector2(), float rotation = 0) + { + _world = world; + _world.ContactManager.PostSolve += PostSolve; + MainBody = new Body(_world, position, rotation, BodyType.Dynamic); + + foreach (Shape part in shapes) + { + Fixture fixture = MainBody.CreateFixture(part); + Parts.Add(fixture); + } + } + + 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 void PostSolve(Contact contact, ContactVelocityConstraint impulse) + { + if (!Broken) + { + if (Parts.Contains(contact.FixtureA) || Parts.Contains(contact.FixtureB)) + { + float maxImpulse = 0.0f; + int count = contact.Manifold.PointCount; + + for (int i = 0; i < count; ++i) + { + maxImpulse = Math.Max(maxImpulse, impulse.points[i].normalImpulse); + } + + if (maxImpulse > Strength) + { + // Flag the body for breaking. + _break = true; + } + } + } + } + + public void Update() + { + if (_break) + { + Decompose(); + Broken = true; + _break = false; + } + + // Cache velocities to improve movement on breakage. + if (Broken == false) + { + //Enlarge the cache if needed + if (Parts.Count > _angularVelocitiesCache.Length) + { + _velocitiesCache = new Vector2[Parts.Count]; + _angularVelocitiesCache = new float[Parts.Count]; + } + + //Cache the linear and angular velocities. + for (int i = 0; i < Parts.Count; i++) + { + _velocitiesCache[i] = Parts[i].Body.LinearVelocity; + _angularVelocitiesCache[i] = Parts[i].Body.AngularVelocity; + } + } + } + + private void Decompose() + { + //Unsubsribe from the PostSolve delegate + _world.ContactManager.PostSolve -= PostSolve; + + for (int i = 0; i < Parts.Count; i++) + { + Fixture oldFixture = Parts[i]; + + Shape shape = oldFixture.Shape.Clone(); + object userData = oldFixture.UserData; + + MainBody.DestroyFixture(oldFixture); + + Body body = BodyFactory.CreateBody(_world,MainBody.Position,MainBody.Rotation,BodyType.Dynamic,MainBody.UserData); + + Fixture newFixture = body.CreateFixture(shape); + newFixture.UserData = userData; + Parts[i] = newFixture; + + body.AngularVelocity = _angularVelocitiesCache[i]; + body.LinearVelocity = _velocitiesCache[i]; + } + + _world.RemoveBody(MainBody); + _world.RemoveBreakableBody(this); + } + + public void Break() + { + _break = true; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/ContactManager.cs b/src/Box2DNet/Dynamics/ContactManager.cs index b4dfb18..5ccefff 100644 --- a/src/Box2DNet/Dynamics/ContactManager.cs +++ b/src/Box2DNet/Dynamics/ContactManager.cs @@ -1,232 +1,445 @@ /* - Box2DNet Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version 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. +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. */ +//#define USE_ACTIVE_CONTACT_SET +using System.Collections.Generic; using Box2DNet.Collision; +using Box2DNet.Dynamics.Contacts; namespace Box2DNet.Dynamics { - /// - // Delegate of World. - /// - public class ContactManager : PairCallback - { - public World _world; - - // This lets us provide broadphase proxy pair user data for - // contacts that shouldn't exist. - public NullContact _nullContact; - - public bool _destroyImmediate; - - public ContactManager() { } - - // This is a callback from the broadphase when two AABB proxies begin - // to overlap. We create a Contact to manage the narrow phase. - public override object PairAdded(object proxyUserDataA, object proxyUserDataB) - { - Fixture fixtureA = proxyUserDataA as Fixture; - Fixture fixtureB = proxyUserDataB as Fixture; - - Body bodyA = fixtureA.Body; - Body bodyB = fixtureB.Body; - - if (bodyA.IsStatic() && bodyB.IsStatic()) + 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); + +#if USE_ACTIVE_CONTACT_SET + /// + /// The set of active contacts. + /// + public HashSet ActiveContacts = new HashSet(); + + /// + /// A temporary copy of active contacts that is used during updates so + /// the hash set can have members added/removed during the update. + /// This list is cleared after every update. + /// + List ActiveList = new List(); +#endif + + /// + /// 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; + + //FPE feature: BeforeCollision delegate + 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); + + if (c == null) + return; + + // Contact creation may swap fixtures. + fixtureA = c.FixtureA; + fixtureB = c.FixtureB; + bodyA = fixtureA.Body; + bodyB = fixtureB.Body; + + // Insert into the world. + ContactList.Add(c); + +#if USE_ACTIVE_CONTACT_SET + ActiveContacts.Add(c); +#endif + // 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; + + // Wake up the bodies + if (fixtureA.IsSensor == false && fixtureB.IsSensor == false) + { + bodyA.Awake = true; + bodyB.Awake = true; + } + } + + 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 (contact.IsTouching) + { + //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 (EndContact != null) + 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; + } + +#if USE_ACTIVE_CONTACT_SET + if (ActiveContacts.Contains(contact)) { - return _nullContact; + ActiveContacts.Remove(contact); } +#endif + contact.Destroy(); + } - if (fixtureA.Body == fixtureB.Body) - { - return _nullContact; - } + internal void Collide() + { + // Update awake contacts. +#if USE_ACTIVE_CONTACT_SET + ActiveList.AddRange(ActiveContacts); - if (bodyB.IsConnected(bodyA)) + foreach (var c in ActiveList) { - return _nullContact; - } - - if (_world._contactFilter != null && _world._contactFilter.ShouldCollide(fixtureA, fixtureB) == false) +#else + for (int i = 0; i < ContactList.Count; i++) + { + Contact c = ContactList[i]; +#endif + Fixture fixtureA = c.FixtureA; + Fixture fixtureB = c.FixtureB; + int indexA = c.ChildIndexA; + int indexB = c.ChildIndexB; + Body bodyA = fixtureA.Body; + Body bodyB = fixtureB.Body; + + //Do no try to collide disabled bodies + if (!bodyA.Enabled || !bodyB.Enabled) + continue; + + // Is this contact flagged for filtering? + if (c.FilterFlag) + { + // 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.FilterFlag = false; + } + + bool activeA = bodyA.Awake && bodyA.BodyType != BodyType.Static; + bool activeB = bodyB.Awake && bodyB.BodyType != BodyType.Static; + + // At least one body must be awake and it must be dynamic or kinematic. + if (activeA == false && activeB == false) + { +#if USE_ACTIVE_CONTACT_SET + ActiveContacts.Remove(c); +#endif + continue; + } + + int proxyIdA = fixtureA.Proxies[indexA].ProxyId; + int proxyIdB = fixtureB.Proxies[indexB].ProxyId; + + bool overlap = BroadPhase.TestOverlap(proxyIdA, proxyIdB); + + // Here we destroy contacts that cease to overlap in the broad-phase. + if (overlap == false) + { + Contact cNuke = c; + Destroy(cNuke); + continue; + } + + // The contact persists. + c.Update(this); + } + +#if USE_ACTIVE_CONTACT_SET + ActiveList.Clear(); +#endif + } + + 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; + } + + internal void UpdateContacts(ContactEdge contactEdge, bool value) + { +#if USE_ACTIVE_CONTACT_SET + if(value) { - return _nullContact; - } - - // Call the factory. - Contact c = Contact.Create(fixtureA, fixtureB); - - if (c == null) - { - return _nullContact; - } - - // Contact creation may swap shapes. - fixtureA = c.FixtureA; - fixtureB = c.FixtureB; - bodyA = fixtureA.Body; - bodyB = fixtureB.Body; - - // Insert into the world. - c._prev = null; - c._next = _world._contactList; - if (_world._contactList != null) - { - _world._contactList._prev = c; - } - _world._contactList = c; - - // Connect to island graph. - - // Connect to body 1 - 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 2 - 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; - - ++_world._contactCount; - return c; - } - - // This is a callback from the broadphase when two AABB proxies cease - // to overlap. We retire the Contact. - public override void PairRemoved(object proxyUserData1, object proxyUserData2, object pairUserData) - { - //B2_NOT_USED(proxyUserData1); - //B2_NOT_USED(proxyUserData2); - - if (pairUserData == null) - { - return; - } - - Contact c = pairUserData as Contact; - if (c == _nullContact) - { - return; - } - - // An attached body is being destroyed, we must destroy this contact - // immediately to avoid orphaned shape pointers. - Destroy(c); - } - - public void Destroy(Contact c) - { - Fixture fixtureA = c.FixtureA; - Fixture fixtureB = c.FixtureB; - Body bodyA = fixtureA.Body; - Body bodyB = fixtureB.Body; - - if (c.Manifold.PointCount > 0) - { - if(_world._contactListener!=null) - _world._contactListener.EndContact(c); - } - - // Remove from the world. - if (c._prev != null) - { - c._prev._next = c._next; - } - - if (c._next != null) - { - c._next._prev = c._prev; - } - - if (c == _world._contactList) - { - _world._contactList = c._next; - } - - // Remove from body 1 - if (c._nodeA.Prev != null) - { - c._nodeA.Prev.Next = c._nodeA.Next; - } - - if (c._nodeA.Next != null) - { - c._nodeA.Next.Prev = c._nodeA.Prev; - } - - if (c._nodeA == bodyA._contactList) - { - bodyA._contactList = c._nodeA.Next; - } - - // Remove from body 2 - if (c._nodeB.Prev != null) - { - c._nodeB.Prev.Next = c._nodeB.Next; - } - - if (c._nodeB.Next != null) - { - c._nodeB.Next.Prev = c._nodeB.Prev; - } - - if (c._nodeB == bodyB._contactList) - { - bodyB._contactList = c._nodeB.Next; + while(contactEdge != null) + { + var c = contactEdge.Contact; + if (!ActiveContacts.Contains(c)) + { + ActiveContacts.Add(c); + } + contactEdge = contactEdge.Next; + } } - - // Call the factory. - Contact.Destroy(ref c); - --_world._contactCount; - } - - // This is the top level collision call for the time step. Here - // all the narrow phase collision is processed for the world - // contact list. - public void Collide() - { - // Update awake contacts. - for (Contact c = _world._contactList; c != null; c = c.GetNext()) + else { - Body bodyA = c._fixtureA.Body; - Body bodyB = c._fixtureB.Body; - if (bodyA.IsSleeping() && bodyB.IsSleeping()) + while (contactEdge != null) { - continue; + var c = contactEdge.Contact; + if (!contactEdge.Other.Awake) + { + if (ActiveContacts.Contains(c)) + { + ActiveContacts.Remove(c); + } + } + contactEdge = contactEdge.Next; } - - c.Update(_world._contactListener); } +#endif + } + +#if USE_ACTIVE_CONTACT_SET + internal void RemoveActiveContact(Contact contact) + { + if (ActiveContacts.Contains(contact)) + ActiveContacts.Remove(contact); } - } -} +#endif + } +} \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/Contacts/CircleContact.cs b/src/Box2DNet/Dynamics/Contacts/CircleContact.cs deleted file mode 100644 index 44e0b43..0000000 --- a/src/Box2DNet/Dynamics/Contacts/CircleContact.cs +++ /dev/null @@ -1,52 +0,0 @@ -/* - Box2DNet Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version 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 Box2DNet.Collision; -using Box2DNet.Common; - -namespace Box2DNet.Dynamics -{ - public class CircleContact : Contact - { - public CircleContact(Fixture fixtureA, Fixture fixtureB) - : base(fixtureA, fixtureB) - { - Box2DNetDebug.Assert(fixtureA.ShapeType == ShapeType.CircleShape); - Box2DNetDebug.Assert(fixtureB.ShapeType == ShapeType.CircleShape); - CollideShapeFunction = CollideCircles; - } - - private static void CollideCircles(ref Manifold manifold, Shape shape1, Transform xf1, Shape shape2, Transform xf2) - { - Collision.Collision.CollideCircles(ref manifold, (CircleShape)shape1, xf1, (CircleShape)shape2, xf2); - } - - new public static Contact Create(Fixture fixtureA, Fixture fixtureB) - { - return new CircleContact(fixtureA, fixtureB); - } - - new public static void Destroy(ref Contact contact) - { - contact = null; - } - } -} diff --git a/src/Box2DNet/Dynamics/Contacts/Contact.cs b/src/Box2DNet/Dynamics/Contacts/Contact.cs index d01542e..6ff0627 100644 --- a/src/Box2DNet/Dynamics/Contacts/Contact.cs +++ b/src/Box2DNet/Dynamics/Contacts/Contact.cs @@ -1,378 +1,480 @@ -/* - Box2DNet Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version 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.Numerics; -using Box2DNet.Collision; -using Box2DNet.Common; - -using Transform = Box2DNet.Common.Transform; - -namespace Box2DNet.Dynamics -{ - public delegate Contact ContactCreateFcn(Fixture fixtureA, Fixture fixtureB); - public delegate void ContactDestroyFcn(ref Contact contact); - - public struct ContactRegister - { - public ContactCreateFcn CreateFcn; - public ContactDestroyFcn DestroyFcn; - public bool Primary; - } - - /// - /// 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 class ContactEdge - { - /// - /// Provides quick access to the other body attached. - /// - public Body Other; - /// - /// The contact. - /// - public Contact Contact; - /// - /// The previous contact edge in the body's contact list. - /// - public ContactEdge Prev; - /// - /// The next contact edge in the body's contact list. - /// - public ContactEdge Next; - } - - /// - /// 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 abstract class Contact - { - [Flags] - public enum CollisionFlags - { - NonSolid = 0x0001, - Slow = 0x0002, - Island = 0x0004, - Toi = 0x0008, - Touch = 0x0010 - } - - public static ContactRegister[][] s_registers = - new ContactRegister[(int)ShapeType.ShapeTypeCount][/*(int)ShapeType.ShapeTypeCount*/]; - public static bool s_initialized; - - public CollisionFlags _flags; - - // World pool and list pointers. - public Contact _prev; - public Contact _next; - - // Nodes for connecting bodies. - public ContactEdge _nodeA; - public ContactEdge _nodeB; - - public Fixture _fixtureA; - public Fixture _fixtureB; - - public Manifold _manifold = new Manifold(); - - public float _toi; - - internal delegate void CollideShapeDelegate( - ref Manifold manifold, Shape circle1, Transform xf1, Shape circle2, Transform xf2); - internal CollideShapeDelegate CollideShapeFunction; - - public Contact(){} - - public Contact(Fixture fA, Fixture fB) - { - _flags = 0; - - if (fA.IsSensor || fB.IsSensor) - { - _flags |= CollisionFlags.NonSolid; - } - - _fixtureA = fA; - _fixtureB = fB; - - _manifold.PointCount = 0; - - _prev = null; - _next = null; - - _nodeA = new ContactEdge(); - _nodeB = new ContactEdge(); - } - - public static void AddType(ContactCreateFcn createFcn, ContactDestroyFcn destoryFcn, - ShapeType type1, ShapeType type2) - { - Box2DNetDebug.Assert(ShapeType.UnknownShape < type1 && type1 < ShapeType.ShapeTypeCount); - Box2DNetDebug.Assert(ShapeType.UnknownShape < type2 && type2 < ShapeType.ShapeTypeCount); - - if (s_registers[(int)type1] == null) - s_registers[(int)type1] = new ContactRegister[(int)ShapeType.ShapeTypeCount]; - - s_registers[(int)type1][(int)type2].CreateFcn = createFcn; - s_registers[(int)type1][(int)type2].DestroyFcn = destoryFcn; - s_registers[(int)type1][(int)type2].Primary = true; - - if (type1 != type2) - { - s_registers[(int)type2][(int)type1].CreateFcn = createFcn; - s_registers[(int)type2][(int)type1].DestroyFcn = destoryFcn; - s_registers[(int)type2][(int)type1].Primary = false; - } - } - - public static void InitializeRegisters() - { - AddType(CircleContact.Create, CircleContact.Destroy, ShapeType.CircleShape, ShapeType.CircleShape); - AddType(PolyAndCircleContact.Create, PolyAndCircleContact.Destroy, ShapeType.PolygonShape, ShapeType.CircleShape); - AddType(PolygonContact.Create, PolygonContact.Destroy, ShapeType.PolygonShape, ShapeType.PolygonShape); - - AddType(EdgeAndCircleContact.Create, EdgeAndCircleContact.Destroy, ShapeType.EdgeShape, ShapeType.CircleShape); - AddType(PolyAndEdgeContact.Create, PolyAndEdgeContact.Destroy, ShapeType.PolygonShape, ShapeType.EdgeShape); - } - - public static Contact Create(Fixture fixtureA, Fixture fixtureB) - { - if (s_initialized == false) - { - InitializeRegisters(); - s_initialized = true; - } - - ShapeType type1 = fixtureA.ShapeType; - ShapeType type2 = fixtureB.ShapeType; - - Box2DNetDebug.Assert(ShapeType.UnknownShape < type1 && type1 < ShapeType.ShapeTypeCount); - Box2DNetDebug.Assert(ShapeType.UnknownShape < type2 && type2 < ShapeType.ShapeTypeCount); - - ContactCreateFcn createFcn = s_registers[(int)type1][(int)type2].CreateFcn; - if (createFcn != null) - { - if (s_registers[(int)type1][(int)type2].Primary) - { - return createFcn(fixtureA, fixtureB); - } - else - { - return createFcn(fixtureB, fixtureA); - } - } - else - { - return null; - } - } - - public static void Destroy(ref Contact contact) - { - Box2DNetDebug.Assert(s_initialized == true); - - if (contact._manifold.PointCount > 0) - { - contact.FixtureA.Body.WakeUp(); - contact.FixtureB.Body.WakeUp(); - } - - ShapeType typeA = contact.FixtureA.ShapeType; - ShapeType typeB = contact.FixtureB.ShapeType; - - Box2DNetDebug.Assert(ShapeType.UnknownShape < typeA && typeA < ShapeType.ShapeTypeCount); - Box2DNetDebug.Assert(ShapeType.UnknownShape < typeB && typeB < ShapeType.ShapeTypeCount); - - ContactDestroyFcn destroyFcn = s_registers[(int)typeA][(int)typeB].DestroyFcn; - destroyFcn(ref contact); - } - - public void Update(ContactListener listener) - { - Manifold oldManifold = _manifold.Clone(); - - Evaluate(); - - Body bodyA = _fixtureA.Body; - Body bodyB = _fixtureB.Body; - - int oldCount = oldManifold.PointCount; - int newCount = _manifold.PointCount; - - if (newCount == 0 && oldCount > 0) - { - bodyA.WakeUp(); - bodyB.WakeUp(); - } - - // Slow contacts don't generate TOI events. - if (bodyA.IsStatic() || bodyA.IsBullet() || bodyB.IsStatic() || bodyB.IsBullet()) - { - _flags &= ~CollisionFlags.Slow; - } - else - { - _flags |= CollisionFlags.Slow; - } - - // 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; - - 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; - break; - } - } - } - - if (oldCount == 0 && newCount > 0) - { - _flags |= CollisionFlags.Touch; - if(listener!=null) - listener.BeginContact(this); - } - - if (oldCount > 0 && newCount == 0) - { - _flags &= ~CollisionFlags.Touch; - if (listener != null) - listener.EndContact(this); - } - - if ((_flags & CollisionFlags.NonSolid) == 0) - { - if (listener != null) - listener.PreSolve(this, oldManifold); - - // The user may have disabled contact. - if (_manifold.PointCount == 0) - { - _flags &= ~CollisionFlags.Touch; - } - } - } - - public void Evaluate() - { - Body bodyA = _fixtureA.Body; - Body bodyB = _fixtureB.Body; - - Box2DNetDebug.Assert(CollideShapeFunction!=null); - - CollideShapeFunction(ref _manifold, _fixtureA.Shape, bodyA.GetTransform(), _fixtureB.Shape, bodyB.GetTransform()); - } - - public float ComputeTOI(Sweep sweepA, Sweep sweepB) - { - TOIInput input = new TOIInput(); - input.SweepA = sweepA; - input.SweepB = sweepB; - input.SweepRadiusA = _fixtureA.ComputeSweepRadius(sweepA.LocalCenter); - input.SweepRadiusB = _fixtureB.ComputeSweepRadius(sweepB.LocalCenter); - input.Tolerance = Common.Settings.LinearSlop; - - return Collision.Collision.TimeOfImpact(input, _fixtureA.Shape, _fixtureB.Shape); - } - - /// - /// Get the contact manifold. - /// - public Manifold Manifold - { - get { return _manifold; } - } - - /// - /// Get the world manifold. - /// - public void GetWorldManifold(out WorldManifold worldManifold) - { - worldManifold = new WorldManifold(); - - Body bodyA = _fixtureA.Body; - Body bodyB = _fixtureB.Body; - Shape shapeA = _fixtureA.Shape; - Shape shapeB = _fixtureB.Shape; - - worldManifold.Initialize(_manifold, bodyA.GetTransform(), shapeA._radius, bodyB.GetTransform(), shapeB._radius); - } - - /// - /// Is this contact solid? - /// - /// True if this contact should generate a response. - public bool IsSolid - { - get { return (_flags & CollisionFlags.NonSolid) == 0; } - } - - /// - /// Are fixtures touching? - /// - public bool AreTouching - { - get { return (_flags & CollisionFlags.Touch) == CollisionFlags.Touch; } - } - - /// - /// Get the next contact in the world's contact list. - /// - public Contact GetNext() - { - return _next; - } - - /// - /// Get the first fixture in this contact. - /// - public Fixture FixtureA - { - get { return _fixtureA; } - } - - /// - /// Get the second fixture in this contact. - /// - public Fixture FixtureB - { - get { return _fixtureB; } - } - } -} +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ +//#define USE_ACTIVE_CONTACT_SET + +using System.Collections.Generic; +using System.Diagnostics; +using Box2DNet.Collision; +using Box2DNet.Collision.Shapes; +using Box2DNet.Common; +using Microsoft.Xna.Framework; + +namespace Box2DNet.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; + } + + /// + /// 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 ContactType _type; + + private static EdgeShape _edge = new EdgeShape(); + + private static ContactType[,] _registers = new[,] + { + { + ContactType.Circle, + ContactType.EdgeAndCircle, + ContactType.PolygonAndCircle, + ContactType.ChainAndCircle, + }, + { + 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.ChainAndPolygon, + }, + { + ContactType.ChainAndCircle, + ContactType.NotSupported, + // 3,1 is invalid (no ContactType.EdgeAndLoop) + ContactType.ChainAndPolygon, + ContactType.NotSupported, + // 3,3 is invalid (no ContactType.Loop) + }, + }; + // Nodes for connecting bodies. + internal ContactEdge _nodeA = new ContactEdge(); + internal ContactEdge _nodeB = new ContactEdge(); + internal int _toiCount; + internal float _toi; + + public Fixture FixtureA; + public Fixture FixtureB; + public float Friction { get; set; } + public float Restitution { get; set; } + + /// + /// Get the contact manifold. Do not modify the manifold unless you understand the + /// internals of Box2D. + /// + public Manifold Manifold; + + /// Get or set the desired tangent speed for a conveyor belt behavior. In meters per second. + public float TangentSpeed { get; set; } + + /// 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). + /// NOTE: If you are setting Enabled to a constant true or false, + /// use the explicit Enable() or Disable() functions instead to + /// save the CPU from doing a branch operation. + public bool Enabled { get; set; } + + /// + /// 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; } + + /// + /// Determines whether this contact is touching. + /// + /// + /// true if this instance is touching; otherwise, false. + /// + public bool IsTouching { get; set; } + + internal bool IslandFlag { get; set; } + internal bool TOIFlag { get; set; } + internal bool FilterFlag { get; set; } + + public void ResetRestitution() + { + Restitution = Settings.MixRestitution(FixtureA.Restitution, FixtureB.Restitution); + } + + public void ResetFriction() + { + Friction = Settings.MixFriction(FixtureA.Friction, FixtureB.Friction); + } + + private Contact(Fixture fA, int indexA, Fixture fB, int indexB) + { + Reset(fA, indexA, fB, indexB); + } + + /// + /// 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; + + ContactSolver.WorldManifold.Initialize(ref Manifold, ref bodyA._xf, shapeA.Radius, ref bodyB._xf, shapeB.Radius, out normal, out points); + } + + private void Reset(Fixture fA, int indexA, Fixture fB, int indexB) + { + Enabled = true; + IsTouching = false; + IslandFlag = false; + FilterFlag = false; + TOIFlag = false; + + 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; + + //FPE: We only set the friction and restitution if we are not destroying the contact + if (FixtureA != null && FixtureB != null) + { + Friction = Settings.MixFriction(FixtureA.Friction, FixtureB.Friction); + Restitution = Settings.MixRestitution(FixtureA.Restitution, FixtureB.Restitution); + } + + TangentSpeed = 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) + { + Body bodyA = FixtureA.Body; + Body bodyB = FixtureB.Body; + + if (FixtureA == null || FixtureB == null) + return; + + Manifold oldManifold = Manifold; + + // Re-enable this contact. + Enabled = true; + + bool touching; + bool wasTouching = IsTouching; + + bool sensor = FixtureA.IsSensor || FixtureB.IsSensor; + + // Is this contact a sensor? + if (sensor) + { + Shape shapeA = FixtureA.Shape; + Shape shapeB = FixtureB.Shape; + touching = Collision.Collision.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; + + 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; + break; + } + } + + Manifold.Points[i] = mp2; + } + + if (touching != wasTouching) + { + bodyA.Awake = true; + bodyB.Awake = true; + } + } + + IsTouching = touching; + + if (wasTouching == false) + { + if (touching) + { + if (Settings.AllCollisionCallbacksAgree) + { + bool enabledA = true, enabledB = true; + + // Report the collision to both participants. Track which ones returned true so we can + // later call OnSeparation if the contact is disabled for a different reason. + if (FixtureA.OnCollision != null) + foreach (OnCollisionEventHandler handler in FixtureA.OnCollision.GetInvocationList()) + enabledA = handler(FixtureA, FixtureB, this) && enabledA; + + // Reverse the order of the reported fixtures. The first fixture is always the one that the + // user subscribed to. + if (FixtureB.OnCollision != null) + foreach (OnCollisionEventHandler handler in FixtureB.OnCollision.GetInvocationList()) + enabledB = handler(FixtureB, FixtureA, this) && enabledB; + + Enabled = enabledA && enabledB; + + // BeginContact can also return false and disable the contact + if (enabledA && enabledB && contactManager.BeginContact != null) + Enabled = contactManager.BeginContact(this); + } + else + { + //Report the collision to both participants: + if (FixtureA.OnCollision != null) + foreach (OnCollisionEventHandler handler in FixtureA.OnCollision.GetInvocationList()) + Enabled = handler(FixtureA, FixtureB, this); + + //Reverse the order of the reported fixtures. The first fixture is always the one that the + //user subscribed to. + if (FixtureB.OnCollision != null) + foreach (OnCollisionEventHandler handler in FixtureB.OnCollision.GetInvocationList()) + Enabled = handler(FixtureB, FixtureA, this); + + //BeginContact can also return false and disable the contact + if (contactManager.BeginContact != null) + Enabled = contactManager.BeginContact(this); + } + + // If the user disabled the contact (needed to exclude it in TOI solver) at any point by + // any of the callbacks, we need to mark it as not touching and call any separation + // callbacks for fixtures that didn't explicitly disable the collision. + if (!Enabled) + IsTouching = false; + } + } + else + { + if (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.ChainAndCircle: + ChainShape chain = (ChainShape)FixtureA.Shape; + chain.GetChildEdge(_edge, ChildIndexA); + Collision.Collision.CollideEdgeAndCircle(ref manifold, _edge, ref transformA, (CircleShape)FixtureB.Shape, ref transformB); + break; + case ContactType.ChainAndPolygon: + ChainShape loop2 = (ChainShape)FixtureA.Shape; + loop2.GetChildEdge(_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.Shape.ShapeType; + ShapeType type2 = fixtureB.Shape.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() + { +#if USE_ACTIVE_CONTACT_SET + FixtureA.Body.World.ContactManager.RemoveActiveContact(this); +#endif + FixtureA.Body._world._contactPool.Enqueue(this); + + if (Manifold.PointCount > 0 && FixtureA.IsSensor == false && FixtureB.IsSensor == false) + { + FixtureA.Body.Awake = true; + FixtureB.Body.Awake = true; + } + + Reset(null, 0, null, 0); + } + + #region Nested type: ContactType + + private enum ContactType + { + NotSupported, + Polygon, + PolygonAndCircle, + Circle, + EdgeAndPolygon, + EdgeAndCircle, + ChainAndPolygon, + ChainAndCircle, + } + + #endregion + } +} \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/Contacts/ContactSolver.cs b/src/Box2DNet/Dynamics/Contacts/ContactSolver.cs index 68fcc2f..cdcdf0a 100644 --- a/src/Box2DNet/Dynamics/Contacts/ContactSolver.cs +++ b/src/Box2DNet/Dynamics/Contacts/ContactSolver.cs @@ -1,1149 +1,978 @@ -/* - Box2DNet Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version 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. -*/ - -//#define B2_DEBUG_SOLVER - -using System; using System.Numerics; -using Box2DNet.Collision; -using Box2DNet.Common; - - -using Transform = Box2DNet.Common.Transform; - -namespace Box2DNet.Dynamics -{ -#if ALLOWUNSAFE - public struct ContactConstraintPoint -#else - public class ContactConstraintPoint -#endif - { - public Vector2 LocalPoint; - public Vector2 RA; - public Vector2 RB; - public float NormalImpulse; - public float TangentImpulse; - public float NormalMass; - public float TangentMass; - public float EqualizedMass; - public float VelocityBias; - } - - public class ContactConstraint - { - public ContactConstraintPoint[] Points = new ContactConstraintPoint[Settings.MaxManifoldPoints]; - public Vector2 LocalPlaneNormal; - public Vector2 LocalPoint; - public Vector2 Normal; - public Mat22 NormalMass; - public Mat22 K; - public Body BodyA; - public Body BodyB; - public ManifoldType Type; - public float Radius; - public float Friction; - public float Restitution; - public int PointCount; - public Manifold Manifold; - -#if !ALLOWUNSAFE - public ContactConstraint() - { - for (int i = 0; i < Settings.MaxManifoldPoints; i++) - Points[i] = new ContactConstraintPoint(); - } -#endif - } - - public class ContactSolver : IDisposable - { - public TimeStep _step; - public ContactConstraint[] _constraints; - public int _constraintCount; - - public ContactSolver(TimeStep step, Contact[] contacts, int contactCount) - { - _step = step; - _constraintCount = contactCount; - - _constraints = new ContactConstraint[_constraintCount]; - for (int i = 0; i < _constraintCount; i++) - { - _constraints[i] = new ContactConstraint(); - } - - 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; - - float friction = Settings.MixFriction(fixtureA.Friction, fixtureB.Friction); - float restitution = Settings.MixRestitution(fixtureA.Restitution, fixtureB.Restitution); - - Box2DNetDebug.Assert(manifold.PointCount > 0); - - WorldManifold worldManifold = new WorldManifold(); - worldManifold.Initialize(manifold, bodyA._xf, radiusA, bodyB._xf, radiusB); - - ContactConstraint cc = _constraints[i]; - cc.BodyA = bodyA; - cc.BodyB = bodyB; - cc.Manifold = manifold; - cc.Normal = worldManifold.Normal; - cc.PointCount = manifold.PointCount; - cc.Friction = friction; - cc.Restitution = restitution; - - cc.LocalPlaneNormal = manifold.LocalPlaneNormal; - cc.LocalPoint = manifold.LocalPoint; - cc.Radius = radiusA + radiusB; - cc.Type = manifold.Type; - - ContactSolverSetup(manifold, worldManifold, cc); - } - } - -#if ALLOWUNSAFE - internal unsafe void ContactSolverSetup(Manifold manifold, WorldManifold worldManifold, ContactConstraint cc) - { - // this is kind of yucky but we do know these were setup before entry to this method - var bodyA = cc.BodyA; - var bodyB = cc.BodyB; - - Vector2 vA = bodyA._linearVelocity; - Vector2 vB = bodyB._linearVelocity; - float wA = bodyA._angularVelocity; - float wB = bodyB._angularVelocity; - - fixed (ContactConstraintPoint* ccPointsPtr = cc.Points) - { - for (int j = 0; j < cc.PointCount; ++j) - { - ManifoldPoint cp = manifold.Points[j]; - ContactConstraintPoint* ccp = &ccPointsPtr[j]; - - ccp->NormalImpulse = cp.NormalImpulse; - ccp->TangentImpulse = cp.TangentImpulse; - - ccp->LocalPoint = cp.LocalPoint; - - ccp->RA = worldManifold.Points[j] - bodyA._sweep.C; - ccp->RB = worldManifold.Points[j] - bodyB._sweep.C; - - float rnA = ccp->RA.Cross(cc.Normal); - float rnB = ccp->RB.Cross(cc.Normal); - rnA *= rnA; - rnB *= rnB; - - float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB; - - Box2DNetDebug.Assert(kNormal > Common.Settings.FLT_EPSILON); - ccp->NormalMass = 1.0f / kNormal; - - float kEqualized = bodyA._mass * bodyA._invMass + bodyB._mass * bodyB._invMass; - kEqualized += bodyA._mass * bodyA._invI * rnA + bodyB._mass * bodyB._invI * rnB; - - Box2DNetDebug.Assert(kEqualized > Common.Settings.FLT_EPSILON); - ccp->EqualizedMass = 1.0f / kEqualized; - - Vector2 tangent = cc.Normal.CrossScalarPostMultiply(1.0f); - - float rtA = ccp->RA.Cross(tangent); - float rtB = ccp->RB.Cross(tangent); - rtA *= rtA; - rtB *= rtB; - - float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB; - - Box2DNetDebug.Assert(kTangent > Common.Settings.FLT_EPSILON); - ccp->TangentMass = 1.0f / kTangent; - - // Setup a velocity bias for restitution. - ccp->VelocityBias = 0.0f; - float vRel = Vector2.Dot(cc.Normal, vB + ccp->RB.CrossScalarPreMultiply(wB) - vA - ccp->RA.CrossScalarPreMultiply(wA)); - if (vRel < -Common.Settings.VelocityThreshold) - { - ccp->VelocityBias = -cc.Restitution * vRel; - } - } - - // If we have two points, then prepare the block solver. - if (cc.PointCount == 2) - { - ContactConstraintPoint* ccp1 = &ccPointsPtr[0]; - ContactConstraintPoint* ccp2 = &ccPointsPtr[1]; - - float invMassA = bodyA._invMass; - float invIA = bodyA._invI; - float invMassB = bodyB._invMass; - float invIB = bodyB._invI; - - float rn1A = ccp1->RA.Cross(cc.Normal); - float rn1B = ccp1->RB.Cross(cc.Normal); - float rn2A = ccp2->RA.Cross(cc.Normal); - float rn2B = ccp2->RB.Cross(cc.Normal); - - 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 = new Vector2(k11, k12); - cc.K.Col2 = new Vector2(k12, k22); - cc.NormalMass = cc.K.GetInverse(); - } - else - { - // The constraints are redundant, just use one. - // TODO_ERIN use deepest? - cc.PointCount = 1; - } - } - } - } -#else - internal void ContactSolverSetup(Manifold manifold, WorldManifold worldManifold, ContactConstraint cc) - { - // this is kind of yucky but we do know these were setup before entry to this method - var bodyA = cc.BodyA; - var bodyB = cc.BodyB; - - Vector2 vA = bodyA._linearVelocity; - Vector2 vB = bodyB._linearVelocity; - float wA = bodyA._angularVelocity; - float wB = bodyB._angularVelocity; - - ContactConstraintPoint[] ccPointsPtr = cc.Points; - for (int j = 0; j < cc.PointCount; ++j) - { - ManifoldPoint cp = manifold.Points[j]; - ContactConstraintPoint ccp = ccPointsPtr[j]; - - ccp.NormalImpulse = cp.NormalImpulse; - ccp.TangentImpulse = cp.TangentImpulse; - - ccp.LocalPoint = cp.LocalPoint; - - ccp.RA = worldManifold.Points[j] - bodyA._sweep.C; - ccp.RB = worldManifold.Points[j] - bodyB._sweep.C; - - float rnA = ccp.RA.Cross(cc.Normal); - float rnB = ccp.RB.Cross(cc.Normal); - rnA *= rnA; - rnB *= rnB; - - float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB; - - Box2DNetDebug.Assert(kNormal > Common.Settings.FLT_EPSILON); - ccp.NormalMass = 1.0f / kNormal; - - float kEqualized = bodyA._mass * bodyA._invMass + bodyB._mass * bodyB._invMass; - kEqualized += bodyA._mass * bodyA._invI * rnA + bodyB._mass * bodyB._invI * rnB; - - Box2DNetDebug.Assert(kEqualized > Common.Settings.FLT_EPSILON); - ccp.EqualizedMass = 1.0f / kEqualized; - - Vector2 tangent = cc.Normal.CrossScalarPostMultiply(1.0f); - - float rtA = ccp.RA.Cross(tangent); - float rtB = ccp.RB.Cross(tangent); - rtA *= rtA; - rtB *= rtB; - - float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB; - - Box2DNetDebug.Assert(kTangent > Common.Settings.FLT_EPSILON); - ccp.TangentMass = 1.0f / kTangent; - - // Setup a velocity bias for restitution. - ccp.VelocityBias = 0.0f; - float vRel = Vector2.Dot(cc.Normal, vB + ccp.RB.CrossScalarPreMultiply(wB) - vA - ccp.RA.CrossScalarPreMultiply(wA)); - if (vRel < -Common.Settings.VelocityThreshold) - { - ccp.VelocityBias = -cc.Restitution * vRel; - } - } - - // If we have two points, then prepare the block solver. - if (cc.PointCount == 2) - { - ContactConstraintPoint ccp1 = ccPointsPtr[0]; - ContactConstraintPoint ccp2 = ccPointsPtr[1]; - - float invMassA = bodyA._invMass; - float invIA = bodyA._invI; - float invMassB = bodyB._invMass; - float invIB = bodyB._invI; - - float rn1A = ccp1.RA.Cross(cc.Normal); - float rn1B = ccp1.RB.Cross(cc.Normal); - float rn2A = ccp2.RA.Cross(cc.Normal); - float rn2B = ccp2.RB.Cross(cc.Normal); - - 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 = new Vector2(k11, k12); - cc.K.Col2 = new Vector2(k12, k22); - cc.NormalMass = cc.K.GetInverse(); - } - else - { - // The constraints are redundant, just use one. - // TODO_ERIN use deepest? - cc.PointCount = 1; - } - } - } -#endif - - public void Dispose() - { - _constraints = null; - } - - - public void InitVelocityConstraints(TimeStep step) - { -#if ALLOWUNSAFE - unsafe - { - // Warm start. - for (int i = 0; i < _constraintCount; ++i) - { - ContactConstraint c = _constraints[i]; - - Body bodyA = c.BodyA; - Body bodyB = c.BodyB; - float invMassA = bodyA._invMass; - float invIA = bodyA._invI; - float invMassB = bodyB._invMass; - float invIB = bodyB._invI; - Vector2 normal = c.Normal; - Vector2 tangent = normal.CrossScalarPostMultiply(1.0f); - - fixed (ContactConstraintPoint* pointsPtr = c.Points) - { - if (step.WarmStarting) - { - for (int j = 0; j < c.PointCount; ++j) - { - ContactConstraintPoint* ccp = &pointsPtr[j]; - ccp->NormalImpulse *= step.DtRatio; - ccp->TangentImpulse *= step.DtRatio; - Vector2 P = ccp->NormalImpulse * normal + ccp->TangentImpulse * tangent; - bodyA._angularVelocity -= invIA * ccp->RA.Cross(P); - bodyA._linearVelocity -= invMassA * P; - bodyB._angularVelocity += invIB * ccp->RB.Cross(P); - bodyB._linearVelocity += invMassB * P; - } - } - else - { - for (int j = 0; j < c.PointCount; ++j) - { - ContactConstraintPoint* ccp = &pointsPtr[j]; - ccp->NormalImpulse = 0.0f; - ccp->TangentImpulse = 0.0f; - } - } - } - } - } -#else - // Warm start. - for (int i = 0; i < _constraintCount; ++i) - { - ContactConstraint c = _constraints[i]; - - Body bodyA = c.BodyA; - Body bodyB = c.BodyB; - float invMassA = bodyA._invMass; - float invIA = bodyA._invI; - float invMassB = bodyB._invMass; - float invIB = bodyB._invI; - Vector2 normal = c.Normal; - Vector2 tangent = normal.CrossScalarPostMultiply(1.0f); - - ContactConstraintPoint[] points = c.Points; - if (step.WarmStarting) - { - for (int j = 0; j < c.PointCount; ++j) - { - ContactConstraintPoint ccp = points[j]; - ccp.NormalImpulse *= step.DtRatio; - ccp.TangentImpulse *= step.DtRatio; - Vector2 P = ccp.NormalImpulse * normal + ccp.TangentImpulse * tangent; - bodyA._angularVelocity -= invIA * ccp.RA.Cross(P); - bodyA._linearVelocity -= invMassA * P; - bodyB._angularVelocity += invIB * ccp.RB.Cross(P); - bodyB._linearVelocity += invMassB * P; - } - } - else - { - for (int j = 0; j < c.PointCount; ++j) - { - ContactConstraintPoint ccp = points[j]; - ccp.NormalImpulse = 0.0f; - ccp.TangentImpulse = 0.0f; - } - } - } -#endif - } - - public void SolveVelocityConstraints() - { - for (int i = 0; i < _constraintCount; ++i) - { - ContactConstraint c = _constraints[i]; - Body bodyA = c.BodyA; - Body bodyB = c.BodyB; - float wA = bodyA._angularVelocity; - float wB = bodyB._angularVelocity; - Vector2 vA = bodyA._linearVelocity; - Vector2 vB = bodyB._linearVelocity; - float invMassA = bodyA._invMass; - float invIA = bodyA._invI; - float invMassB = bodyB._invMass; - float invIB = bodyB._invI; - Vector2 normal = c.Normal; - Vector2 tangent = normal.CrossScalarPostMultiply(1.0f); - float friction = c.Friction; - - Box2DNetDebug.Assert(c.PointCount == 1 || c.PointCount == 2); - -#if ALLOWUNSAFE - unsafe - { - fixed (ContactConstraintPoint* pointsPtr = c.Points) - { - // Solve tangent constraints - for (int j = 0; j < c.PointCount; ++j) - { - ContactConstraintPoint* ccp = &pointsPtr[j]; - - // Relative velocity at contact - Vector2 dv = vB + ccp->RB.CrossScalarPreMultiply(wB) - vA - ccp->RA.CrossScalarPreMultiply(wA); - - // Compute tangent force - float vt = Vector2.Dot(dv, tangent); - float lambda = ccp->TangentMass * (-vt); - - // b2Clamp the accumulated force - float maxFriction = friction * ccp->NormalImpulse; - float newImpulse = Mathf.Clamp(ccp->TangentImpulse + lambda, -maxFriction, maxFriction); - lambda = newImpulse - ccp->TangentImpulse; - - // Apply contact impulse - Vector2 P = lambda * tangent; - - vA -= invMassA * P; - wA -= invIA * ccp->RA.Cross(P); - - vB += invMassB * P; - wB += invIB * ccp->RB.Cross(P); - - ccp->TangentImpulse = newImpulse; - } - - // Solve normal constraints - if (c.PointCount == 1) - { - ContactConstraintPoint ccp = c.Points[0]; - - // Relative velocity at contact - Vector2 dv = vB + ccp.RB.CrossScalarPreMultiply(wB) - vA - ccp.RA.CrossScalarPreMultiply(wA); - - // Compute normal impulse - float vn = Vector2.Dot(dv, normal); - float lambda = -ccp.NormalMass * (vn - ccp.VelocityBias); - - // Clamp the accumulated impulse - float newImpulse = Common.Math.Max(ccp.NormalImpulse + lambda, 0.0f); - lambda = newImpulse - ccp.NormalImpulse; - - // Apply contact impulse - Vector2 P = lambda * normal; - vA -= invMassA * P; - wA -= invIA * ccp.RA.Cross(P); - - vB += invMassB * P; - wB += invIB * ccp.RB.Cross(P); - 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 = &pointsPtr[0]; - ContactConstraintPoint* cp2 = &pointsPtr[1]; - - Vector2 a = new Vector2(cp1->NormalImpulse, cp2->NormalImpulse); - Box2DNetDebug.Assert(a.x >= 0.0f && a.y >= 0.0f); - - // Relative velocity at contact - Vector2 dv1 = vB + cp1->RB.CrossScalarPreMultiply(wB) - vA - cp1->RA.CrossScalarPreMultiply(wA); - Vector2 dv2 = vB + cp2->RB.CrossScalarPreMultiply(wB) - vA - cp2->RA.CrossScalarPreMultiply(wA); - - // Compute normal velocity - float vn1 = Vector2.Dot(dv1, normal); - float vn2 = Vector2.Dot(dv2, normal); - - Vector2 b = new Vector2(vn1 - cp1->VelocityBias, vn2 - cp2->VelocityBias); - b -= c.K.Multiply(a); - - const float k_errorTol = 1e-3f; - //B2_NOT_USED(k_errorTol); - - for (; ; ) - { - // - // Case 1: vn = 0 - // - // 0 = A * x' + b' - // - // Solve for x': - // - // x' = - inv(A) * b' - // - Vector2 x = -c.NormalMass.Multiply(b); - - if (x.x >= 0.0f && x.y >= 0.0f) - { - // Resubstitute for the incremental impulse - Vector2 d = x - a; - - // Apply incremental impulse - Vector2 P1 = d.x * normal; - Vector2 P2 = d.y * normal; - vA -= invMassA * (P1 + P2); - wA -= invIA * (cp1->RA.Cross(P1) + cp2->RA.Cross(P2)); - - vB += invMassB * (P1 + P2); - wB += invIB * (cp1->RB.Cross(P1) + cp2->RB.Cross(P2)); - - // Accumulate - cp1->NormalImpulse = x.x; - cp2->NormalImpulse = x.y; - -#if DEBUG_SOLVER - // Postconditions - dv1 = vB + Vec2.Cross(wB, cp1->RB) - vA - Vec2.Cross(wA, cp1->RA); - dv2 = vB + Vec2.Cross(wB, cp2->RB) - vA - Vec2.Cross(wA, cp2->RA); - - // Compute normal velocity - vn1 = Vec2.Dot(dv1, normal); - vn2 = Vec2.Dot(dv2, normal); - - Box2DNetDebug.Assert(Common.Math.Abs(vn1 - cp1.VelocityBias) < k_errorTol); - Box2DNetDebug.Assert(Common.Math.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' - // - x.x = -cp1->NormalMass * b.x; - x.y = 0.0f; - vn1 = 0.0f; - vn2 = c.K.Col1.y * x.x + b.y; - - if (x.x >= 0.0f && vn2 >= 0.0f) - { - // Resubstitute for the incremental impulse - Vector2 d = x - a; - - // Apply incremental impulse - Vector2 P1 = d.x * normal; - Vector2 P2 = d.y * normal; - vA -= invMassA * (P1 + P2); - wA -= invIA * (cp1->RA.Cross(P1) + cp2->RA.Cross(P2)); - - vB += invMassB * (P1 + P2); - wB += invIB * (cp1->RB.Cross(P1) + cp2->RB.Cross(P2)); - - // Accumulate - cp1->NormalImpulse = x.x; - cp2->NormalImpulse = x.y; - -#if DEBUG_SOLVER - // Postconditions - dv1 = vB + Vec2.Cross(wB, cp1->RB) - vA - Vec2.Cross(wA, cp1->RA); - - // Compute normal velocity - vn1 = Vec2.Dot(dv1, normal); - - Box2DNetDebug.Assert(Common.Math.Abs(vn1 - cp1.VelocityBias) < k_errorTol); -#endif - break; - } - - - // - // Case 3: w2 = 0 and x1 = 0 - // - // vn1 = a11 * 0 + a12 * x2' + b1' - // 0 = a21 * 0 + a22 * x2' + b2' - // - x.x = 0.0f; - x.y = -cp2->NormalMass * b.y; - vn1 = c.K.Col2.x * x.y + b.x; - vn2 = 0.0f; - - if (x.y >= 0.0f && vn1 >= 0.0f) - { - // Resubstitute for the incremental impulse - Vector2 d = x - a; - - // Apply incremental impulse - Vector2 P1 = d.x * normal; - Vector2 P2 = d.y * normal; - vA -= invMassA * (P1 + P2); - wA -= invIA * (cp1->RA.Cross(P1) + cp2->RA.Cross(P2)); - - vB += invMassB * (P1 + P2); - wB += invIB * (cp1->RB.Cross(P1) + cp2->RB.Cross(P2)); - - // Accumulate - cp1->NormalImpulse = x.x; - cp2->NormalImpulse = x.y; - -#if DEBUG_SOLVER - // Postconditions - dv2 = vB + Vec2.Cross(wB, cp2->RB) - vA - Vec2.Cross(wA, cp2->RA); - - // Compute normal velocity - vn2 = Vec2.Dot(dv2, normal); - - Box2DNetDebug.Assert(Common.Math.Abs(vn2 - cp2.VelocityBias) < k_errorTol); -#endif - break; - } - - // - // Case 4: x1 = 0 and x2 = 0 - // - // vn1 = b1 - // vn2 = b2; - x.x = 0.0f; - x.y = 0.0f; - vn1 = b.x; - vn2 = b.y; - - if (vn1 >= 0.0f && vn2 >= 0.0f) - { - // Resubstitute for the incremental impulse - Vector2 d = x - a; - - // Apply incremental impulse - Vector2 P1 = d.x * normal; - Vector2 P2 = d.y * normal; - vA -= invMassA * (P1 + P2); - wA -= invIA * (cp1->RA.Cross(P1) + cp2->RA.Cross(P2)); - - vB += invMassB * (P1 + P2); - wB += invIB * (cp1->RB.Cross(P1) + cp2->RB.Cross(P2)); - - // Accumulate - cp1->NormalImpulse = x.x; - cp2->NormalImpulse = x.y; - - break; - } - - // No solution, give up. This is hit sometimes, but it doesn't seem to matter. - break; - } - } - - bodyA._linearVelocity = vA; - bodyA._angularVelocity = wA; - bodyB._linearVelocity = vB; - bodyB._angularVelocity = wB; - } - } -#else - ContactConstraintPoint[] pointsPtr = c.Points; - - // Solve tangent constraints - for (int j = 0; j < c.PointCount; ++j) - { - ContactConstraintPoint ccp = pointsPtr[j]; - - // Relative velocity at contact - Vector2 dv = vB + ccp.RB.CrossScalarPreMultiply(wB) - vA - ccp.RA.CrossScalarPreMultiply(wA); - - // Compute tangent force - float vt = Vector2.Dot(dv, tangent); - float lambda = ccp.TangentMass * (-vt); - - // b2Clamp the accumulated force - float maxFriction = friction * ccp.NormalImpulse; - float newImpulse = Box2DNet.Common.Math.Clamp(ccp.TangentImpulse + lambda, -maxFriction, maxFriction); - lambda = newImpulse - ccp.TangentImpulse; - - // Apply contact impulse - Vector2 P = lambda * tangent; - - vA -= invMassA * P; - wA -= invIA * ccp.RA.Cross(P); - - vB += invMassB * P; - wB += invIB * ccp.RB.Cross(P); - - ccp.TangentImpulse = newImpulse; - } - - // Solve normal constraints - if (c.PointCount == 1) - { - ContactConstraintPoint ccp = c.Points[0]; - - // Relative velocity at contact - Vector2 dv = vB + ccp.RB.CrossScalarPreMultiply(wB) - vA - ccp.RA.CrossScalarPreMultiply(wA); - - // Compute normal impulse - float vn = Vector2.Dot(dv, normal); - float lambda = -ccp.NormalMass * (vn - ccp.VelocityBias); - - // Clamp the accumulated impulse - float newImpulse = Common.Math.Max(ccp.NormalImpulse + lambda, 0.0f); - lambda = newImpulse - ccp.NormalImpulse; - - // Apply contact impulse - Vector2 P = lambda * normal; - vA -= invMassA * P; - wA -= invIA * ccp.RA.Cross(P); - - vB += invMassB * P; - wB += invIB * ccp.RB.Cross(P); - 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 = pointsPtr[0]; - ContactConstraintPoint cp2 = pointsPtr[1]; - - Vector2 a = new Vector2(cp1.NormalImpulse, cp2.NormalImpulse); - Box2DNetDebug.Assert(a.X >= 0.0f && a.Y >= 0.0f); - - // Relative velocity at contact - Vector2 dv1 = vB + cp1.RB.CrossScalarPreMultiply(wB) - vA - cp1.RA.CrossScalarPreMultiply(wA); - Vector2 dv2 = vB + cp2.RB.CrossScalarPreMultiply(wB) - vA - cp2.RA.CrossScalarPreMultiply(wA); - - // Compute normal velocity - float vn1 = Vector2.Dot(dv1, normal); - float vn2 = Vector2.Dot(dv2, normal); - - Vector2 b = new Vector2(vn1 - cp1.VelocityBias, vn2 - cp2.VelocityBias); - b -= c.K.Multiply(a); - - const float k_errorTol = 1e-3f; - //B2_NOT_USED(k_errorTol); - - for (; ; ) - { - // - // Case 1: vn = 0 - // - // 0 = A * x' + b' - // - // Solve for x': - // - // x' = - inv(A) * b' - // - Vector2 x = -c.NormalMass.Multiply(b); - - if (x.X >= 0.0f && x.Y >= 0.0f) - { - // Resubstitute for the incremental impulse - Vector2 d = x - a; - - // Apply incremental impulse - Vector2 P1 = d.X * normal; - Vector2 P2 = d.Y * normal; - vA -= invMassA * (P1 + P2); - wA -= invIA * (cp1.RA.Cross(P1) + cp2.RA.Cross(P2)); - - vB += invMassB * (P1 + P2); - wB += invIB * (cp1.RB.Cross(P1) + cp2.RB.Cross(P2)); - - // Accumulate - cp1.NormalImpulse = x.X; - cp2.NormalImpulse = x.Y; - -#if DEBUG_SOLVER - // Postconditions - dv1 = vB + Vec2.Cross(wB, cp1->RB) - vA - Vec2.Cross(wA, cp1->RA); - dv2 = vB + Vec2.Cross(wB, cp2->RB) - vA - Vec2.Cross(wA, cp2->RA); - - // Compute normal velocity - vn1 = Vec2.Dot(dv1, normal); - vn2 = Vec2.Dot(dv2, normal); - - Box2DNetDebug.Assert(Common.Math.Abs(vn1 - cp1.VelocityBias) < k_errorTol); - Box2DNetDebug.Assert(Common.Math.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' - // - x.X = -cp1.NormalMass * b.X; - x.Y = 0.0f; - vn1 = 0.0f; - vn2 = c.K.Col1.Y * x.X + b.Y; - - if (x.X >= 0.0f && vn2 >= 0.0f) - { - // Resubstitute for the incremental impulse - Vector2 d = x - a; - - // Apply incremental impulse - Vector2 P1 = d.X * normal; - Vector2 P2 = d.Y * normal; - vA -= invMassA * (P1 + P2); - wA -= invIA * (cp1.RA.Cross(P1) + cp2.RA.Cross(P2)); - - vB += invMassB * (P1 + P2); - wB += invIB * (cp1.RB.Cross(P1) + cp2.RB.Cross(P2)); - - // Accumulate - cp1.NormalImpulse = x.X; - cp2.NormalImpulse = x.Y; - -#if DEBUG_SOLVER - // Postconditions - dv1 = vB + Vec2.Cross(wB, cp1->RB) - vA - Vec2.Cross(wA, cp1->RA); - - // Compute normal velocity - vn1 = Vec2.Dot(dv1, normal); - - Box2DNetDebug.Assert(Common.Math.Abs(vn1 - cp1.VelocityBias) < k_errorTol); -#endif - break; - } - - - // - // Case 3: w2 = 0 and x1 = 0 - // - // vn1 = a11 * 0 + a12 * x2' + b1' - // 0 = a21 * 0 + a22 * x2' + b2' - // - x.X = 0.0f; - x.Y = -cp2.NormalMass * b.Y; - vn1 = c.K.Col2.X * x.Y + b.X; - vn2 = 0.0f; - - if (x.Y >= 0.0f && vn1 >= 0.0f) - { - // Resubstitute for the incremental impulse - Vector2 d = x - a; - - // Apply incremental impulse - Vector2 P1 = d.X * normal; - Vector2 P2 = d.Y * normal; - vA -= invMassA * (P1 + P2); - wA -= invIA * (cp1.RA.Cross(P1) + cp2.RA.Cross(P2)); - - vB += invMassB * (P1 + P2); - wB += invIB * (cp1.RB.Cross(P1) + cp2.RB.Cross(P2)); - - // Accumulate - cp1.NormalImpulse = x.X; - cp2.NormalImpulse = x.Y; - -#if DEBUG_SOLVER - // Postconditions - dv2 = vB + Vec2.Cross(wB, cp2->RB) - vA - Vec2.Cross(wA, cp2->RA); - - // Compute normal velocity - vn2 = Vec2.Dot(dv2, normal); - - Box2DNetDebug.Assert(Common.Math.Abs(vn2 - cp2.VelocityBias) < k_errorTol); -#endif - break; - } - - // - // Case 4: x1 = 0 and x2 = 0 - // - // vn1 = b1 - // vn2 = b2; - x.X = 0.0f; - x.Y = 0.0f; - vn1 = b.X; - vn2 = b.Y; - - if (vn1 >= 0.0f && vn2 >= 0.0f) - { - // Resubstitute for the incremental impulse - Vector2 d = x - a; - - // Apply incremental impulse - Vector2 P1 = d.X * normal; - Vector2 P2 = d.Y * normal; - vA -= invMassA * (P1 + P2); - wA -= invIA * (cp1.RA.Cross(P1) + cp2.RA.Cross(P2)); - - vB += invMassB * (P1 + P2); - wB += invIB * (cp1.RB.Cross(P1) + cp2.RB.Cross(P2)); - - // Accumulate - cp1.NormalImpulse = x.X; - cp2.NormalImpulse = x.Y; - - break; - } - - // No solution, give up. This is hit sometimes, but it doesn't seem to matter. - break; - } - } - - bodyA._linearVelocity = vA; - bodyA._angularVelocity = wA; - bodyB._linearVelocity = vB; - bodyB._angularVelocity = wB; -#endif // ALLOWUNSAFE - } - } - - public void FinalizeVelocityConstraints() - { - for (int i = 0; i < _constraintCount; ++i) - { - ContactConstraint c = _constraints[i]; - Manifold m = c.Manifold; - - for (int j = 0; j < c.PointCount; ++j) - { - m.Points[j].NormalImpulse = c.Points[j].NormalImpulse; - m.Points[j].TangentImpulse = c.Points[j].TangentImpulse; - } - } - } - - internal class PositionSolverManifold - { - internal Vector2 Normal; - internal Vector2[] Points = new Vector2[Settings.MaxManifoldPoints]; - internal float[] Separations = new float[Settings.MaxManifoldPoints]; - - internal void Initialize(ContactConstraint cc) - { - Box2DNetDebug.Assert(cc.PointCount > 0); - - switch (cc.Type) - { - case ManifoldType.Circles: - { - Vector2 pointA = cc.BodyA.GetWorldPoint(cc.LocalPoint); - Vector2 pointB = cc.BodyB.GetWorldPoint(cc.Points[0].LocalPoint); - if ((pointA - pointB).LengthSquared() > (Box2DNet.Common.Math.Epsilon *Box2DNet.Common.Math.Epsilon)) - { - Normal = pointB - pointA; - Normal.Normalize(); - } - else - { - Normal = new Vector2(1.0f, 0.0f); - } - - Points[0] = 0.5f * (pointA + pointB); - Separations[0] = Vector2.Dot(pointB - pointA, Normal) - cc.Radius; - } - break; - - case ManifoldType.FaceA: - { - Normal = cc.BodyA.GetWorldVector(cc.LocalPlaneNormal); - Vector2 planePoint = cc.BodyA.GetWorldPoint(cc.LocalPoint); - - for (int i = 0; i < cc.PointCount; ++i) - { - Vector2 clipPoint = cc.BodyB.GetWorldPoint(cc.Points[i].LocalPoint); - Separations[i] = Vector2.Dot(clipPoint - planePoint, Normal) - cc.Radius; - Points[i] = clipPoint; - } - } - break; - - case ManifoldType.FaceB: - { - Normal = cc.BodyB.GetWorldVector(cc.LocalPlaneNormal); - Vector2 planePoint = cc.BodyB.GetWorldPoint(cc.LocalPoint); - - for (int i = 0; i < cc.PointCount; ++i) - { - Vector2 clipPoint = cc.BodyA.GetWorldPoint(cc.Points[i].LocalPoint); - Separations[i] = Vector2.Dot(clipPoint - planePoint, Normal) - cc.Radius; - Points[i] = clipPoint; - } - - // Ensure normal points from A to B - Normal = -Normal; - } - break; - } - } - } - - private static PositionSolverManifold s_PositionSolverManifold = new PositionSolverManifold(); - - 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; - - s_PositionSolverManifold.Initialize(c); - Vector2 normal = s_PositionSolverManifold.Normal; - - // Solver normal constraints - for (int j = 0; j < c.PointCount; ++j) - { - Vector2 point = s_PositionSolverManifold.Points[j]; - float separation = s_PositionSolverManifold.Separations[j]; - - Vector2 rA = point - bodyA._sweep.C; - Vector2 rB = point - bodyB._sweep.C; - - // Track max constraint error. - minSeparation = Common.Math.Min(minSeparation, separation); - - // Prevent large corrections and allow slop. - float C = baumgarte * Box2DNet.Common.Math.Clamp(separation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); - - // Compute normal impulse - float impulse = -c.Points[j].EqualizedMass * C; - - Vector2 P = impulse * normal; - - bodyA._sweep.C -= invMassA * P; - bodyA._sweep.A -= invIA * rA.Cross(P); - bodyA.SynchronizeTransform(); - - bodyB._sweep.C += invMassB * P; - bodyB._sweep.A += invIB * rB.Cross(P); - bodyB.SynchronizeTransform(); - } - } - - // We can't expect minSpeparation >= -Settings.LinearSlop because we don't - // push the separation above -Settings.LinearSlop. - return minSeparation >= -1.5f * Settings.LinearSlop; - } - } -} +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using Box2DNet.Collision; +using Box2DNet.Collision.Shapes; +using Box2DNet.Common; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Dynamics.Contacts +{ + public sealed class ContactPositionConstraint + { + public Vector2[] localPoints = new Vector2[Settings.MaxManifoldPoints]; + public Vector2 localNormal; + public Vector2 localPoint; + public int indexA; + public int indexB; + public float invMassA, invMassB; + public Vector2 localCenterA, localCenterB; + public float invIA, invIB; + public ManifoldType type; + public float radiusA, radiusB; + public int pointCount; + } + + public sealed class VelocityConstraintPoint + { + public Vector2 rA; + public Vector2 rB; + public float normalImpulse; + public float tangentImpulse; + public float normalMass; + public float tangentMass; + public float velocityBias; + } + + public sealed class ContactVelocityConstraint + { + public VelocityConstraintPoint[] points = new VelocityConstraintPoint[Settings.MaxManifoldPoints]; + public Vector2 normal; + public Mat22 normalMass; + public Mat22 K; + public int indexA; + public int indexB; + public float invMassA, invMassB; + public float invIA, invIB; + public float friction; + public float restitution; + public float tangentSpeed; + public int pointCount; + public int contactIndex; + + public ContactVelocityConstraint() + { + for (int i = 0; i < Settings.MaxManifoldPoints; i++) + { + points[i] = new VelocityConstraintPoint(); + } + } + } + + public class ContactSolver + { + public TimeStep _step; + public Position[] _positions; + public Velocity[] _velocities; + public ContactPositionConstraint[] _positionConstraints; + public ContactVelocityConstraint[] _velocityConstraints; + public Contact[] _contacts; + public int _count; + + public void Reset(TimeStep step, int count, Contact[] contacts, Position[] positions, Velocity[] velocities, bool warmstarting = Settings.EnableWarmstarting) + { + _step = step; + _count = count; + _positions = positions; + _velocities = velocities; + _contacts = contacts; + + // grow the array + if (_velocityConstraints == null || _velocityConstraints.Length < count) + { + _velocityConstraints = new ContactVelocityConstraint[count * 2]; + _positionConstraints = new ContactPositionConstraint[count * 2]; + + for (int i = 0; i < _velocityConstraints.Length; i++) + { + _velocityConstraints[i] = new ContactVelocityConstraint(); + } + + for (int i = 0; i < _positionConstraints.Length; i++) + { + _positionConstraints[i] = new ContactPositionConstraint(); + } + } + + // Initialize position independent portions of the constraints. + for (int i = 0; i < _count; ++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; + + int pointCount = manifold.PointCount; + Debug.Assert(pointCount > 0); + + ContactVelocityConstraint vc = _velocityConstraints[i]; + vc.friction = contact.Friction; + vc.restitution = contact.Restitution; + vc.tangentSpeed = contact.TangentSpeed; + vc.indexA = bodyA.IslandIndex; + vc.indexB = bodyB.IslandIndex; + vc.invMassA = bodyA._invMass; + vc.invMassB = bodyB._invMass; + vc.invIA = bodyA._invI; + vc.invIB = bodyB._invI; + vc.contactIndex = i; + vc.pointCount = pointCount; + vc.K.SetZero(); + vc.normalMass.SetZero(); + + ContactPositionConstraint pc = _positionConstraints[i]; + pc.indexA = bodyA.IslandIndex; + pc.indexB = bodyB.IslandIndex; + pc.invMassA = bodyA._invMass; + pc.invMassB = bodyB._invMass; + pc.localCenterA = bodyA._sweep.LocalCenter; + pc.localCenterB = bodyB._sweep.LocalCenter; + pc.invIA = bodyA._invI; + pc.invIB = bodyB._invI; + pc.localNormal = manifold.LocalNormal; + pc.localPoint = manifold.LocalPoint; + pc.pointCount = pointCount; + pc.radiusA = radiusA; + pc.radiusB = radiusB; + pc.type = manifold.Type; + + for (int j = 0; j < pointCount; ++j) + { + ManifoldPoint cp = manifold.Points[j]; + VelocityConstraintPoint vcp = vc.points[j]; + + if (Settings.EnableWarmstarting) + { + vcp.normalImpulse = _step.dtRatio * cp.NormalImpulse; + vcp.tangentImpulse = _step.dtRatio * cp.TangentImpulse; + } + else + { + vcp.normalImpulse = 0.0f; + vcp.tangentImpulse = 0.0f; + } + + vcp.rA = Vector2.Zero; + vcp.rB = Vector2.Zero; + vcp.normalMass = 0.0f; + vcp.tangentMass = 0.0f; + vcp.velocityBias = 0.0f; + + pc.localPoints[j] = cp.LocalPoint; + } + } + } + + public void InitializeVelocityConstraints() + { + for (int i = 0; i < _count; ++i) + { + ContactVelocityConstraint vc = _velocityConstraints[i]; + ContactPositionConstraint pc = _positionConstraints[i]; + + float radiusA = pc.radiusA; + float radiusB = pc.radiusB; + Manifold manifold = _contacts[vc.contactIndex].Manifold; + + int indexA = vc.indexA; + int indexB = vc.indexB; + + float mA = vc.invMassA; + float mB = vc.invMassB; + float iA = vc.invIA; + float iB = vc.invIB; + Vector2 localCenterA = pc.localCenterA; + Vector2 localCenterB = pc.localCenterB; + + Vector2 cA = _positions[indexA].c; + float aA = _positions[indexA].a; + Vector2 vA = _velocities[indexA].v; + float wA = _velocities[indexA].w; + + Vector2 cB = _positions[indexB].c; + float aB = _positions[indexB].a; + Vector2 vB = _velocities[indexB].v; + float wB = _velocities[indexB].w; + + Debug.Assert(manifold.PointCount > 0); + + Transform xfA = new Transform(); + Transform xfB = new Transform(); + xfA.q.Set(aA); + xfB.q.Set(aB); + xfA.p = cA - MathUtils.Mul(xfA.q, localCenterA); + xfB.p = cB - MathUtils.Mul(xfB.q, localCenterB); + + Vector2 normal; + FixedArray2 points; + WorldManifold.Initialize(ref manifold, ref xfA, radiusA, ref xfB, radiusB, out normal, out points); + + vc.normal = normal; + + int pointCount = vc.pointCount; + for (int j = 0; j < pointCount; ++j) + { + VelocityConstraintPoint vcp = vc.points[j]; + + vcp.rA = points[j] - cA; + vcp.rB = points[j] - cB; + + float rnA = MathUtils.Cross(vcp.rA, vc.normal); + float rnB = MathUtils.Cross(vcp.rB, vc.normal); + + float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB; + + vcp.normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; + + Vector2 tangent = MathUtils.Cross(vc.normal, 1.0f); + + float rtA = MathUtils.Cross(vcp.rA, tangent); + float rtB = MathUtils.Cross(vcp.rB, tangent); + + float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB; + + vcp.tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; + + // Setup a velocity bias for restitution. + vcp.velocityBias = 0.0f; + float vRel = Vector2.Dot(vc.normal, vB + MathUtils.Cross(wB, vcp.rB) - vA - MathUtils.Cross(wA, vcp.rA)); + if (vRel < -Settings.VelocityThreshold) + { + vcp.velocityBias = -vc.restitution * vRel; + } + } + + // If we have two points, then prepare the block solver. + if (vc.pointCount == 2) + { + VelocityConstraintPoint vcp1 = vc.points[0]; + VelocityConstraintPoint vcp2 = vc.points[1]; + + float rn1A = MathUtils.Cross(vcp1.rA, vc.normal); + float rn1B = MathUtils.Cross(vcp1.rB, vc.normal); + float rn2A = MathUtils.Cross(vcp2.rA, vc.normal); + float rn2B = MathUtils.Cross(vcp2.rB, vc.normal); + + float k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B; + float k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B; + float k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B; + + // Ensure a reasonable condition number. + const float k_maxConditionNumber = 1000.0f; + if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) + { + // K is safe to invert. + vc.K.ex = new Vector2(k11, k12); + vc.K.ey = new Vector2(k12, k22); + vc.normalMass = vc.K.Inverse; + } + else + { + // The constraints are redundant, just use one. + // TODO_ERIN use deepest? + vc.pointCount = 1; + } + } + } + } + + public void WarmStart() + { + // Warm start. + for (int i = 0; i < _count; ++i) + { + ContactVelocityConstraint vc = _velocityConstraints[i]; + + int indexA = vc.indexA; + int indexB = vc.indexB; + float mA = vc.invMassA; + float iA = vc.invIA; + float mB = vc.invMassB; + float iB = vc.invIB; + int pointCount = vc.pointCount; + + Vector2 vA = _velocities[indexA].v; + float wA = _velocities[indexA].w; + Vector2 vB = _velocities[indexB].v; + float wB = _velocities[indexB].w; + + Vector2 normal = vc.normal; + Vector2 tangent = MathUtils.Cross(normal, 1.0f); + + for (int j = 0; j < pointCount; ++j) + { + VelocityConstraintPoint vcp = vc.points[j]; + Vector2 P = vcp.normalImpulse * normal + vcp.tangentImpulse * tangent; + wA -= iA * MathUtils.Cross(vcp.rA, P); + vA -= mA * P; + wB += iB * MathUtils.Cross(vcp.rB, P); + vB += mB * P; + } + + _velocities[indexA].v = vA; + _velocities[indexA].w = wA; + _velocities[indexB].v = vB; + _velocities[indexB].w = wB; + } + } + + public void SolveVelocityConstraints() + { + for (int i = 0; i < _count; ++i) + { + ContactVelocityConstraint vc = _velocityConstraints[i]; + + int indexA = vc.indexA; + int indexB = vc.indexB; + float mA = vc.invMassA; + float iA = vc.invIA; + float mB = vc.invMassB; + float iB = vc.invIB; + int pointCount = vc.pointCount; + + Vector2 vA = _velocities[indexA].v; + float wA = _velocities[indexA].w; + Vector2 vB = _velocities[indexB].v; + float wB = _velocities[indexB].w; + + Vector2 normal = vc.normal; + Vector2 tangent = MathUtils.Cross(normal, 1.0f); + float friction = vc.friction; + + Debug.Assert(pointCount == 1 || pointCount == 2); + + // Solve tangent constraints first because non-penetration is more important + // than friction. + for (int j = 0; j < pointCount; ++j) + { + VelocityConstraintPoint vcp = vc.points[j]; + + // Relative velocity at contact + Vector2 dv = vB + MathUtils.Cross(wB, vcp.rB) - vA - MathUtils.Cross(wA, vcp.rA); + + // Compute tangent force + float vt = Vector2.Dot(dv, tangent) - vc.tangentSpeed; + float lambda = vcp.tangentMass * (-vt); + + // b2Clamp the accumulated force + float maxFriction = friction * vcp.normalImpulse; + float newImpulse = MathUtils.Clamp(vcp.tangentImpulse + lambda, -maxFriction, maxFriction); + lambda = newImpulse - vcp.tangentImpulse; + vcp.tangentImpulse = newImpulse; + + // Apply contact impulse + Vector2 P = lambda * tangent; + + vA -= mA * P; + wA -= iA * MathUtils.Cross(vcp.rA, P); + + vB += mB * P; + wB += iB * MathUtils.Cross(vcp.rB, P); + } + + // Solve normal constraints + if (vc.pointCount == 1) + { + VelocityConstraintPoint vcp = vc.points[0]; + + // Relative velocity at contact + Vector2 dv = vB + MathUtils.Cross(wB, vcp.rB) - vA - MathUtils.Cross(wA, vcp.rA); + + // Compute normal impulse + float vn = Vector2.Dot(dv, normal); + float lambda = -vcp.normalMass * (vn - vcp.velocityBias); + + // b2Clamp the accumulated impulse + float newImpulse = Math.Max(vcp.normalImpulse + lambda, 0.0f); + lambda = newImpulse - vcp.normalImpulse; + vcp.normalImpulse = newImpulse; + + // Apply contact impulse + Vector2 P = lambda * normal; + vA -= mA * P; + wA -= iA * MathUtils.Cross(vcp.rA, P); + + vB += mB * P; + wB += iB * MathUtils.Cross(vcp.rB, P); + } + 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 = vn0 - 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 = a + d + // + // a := old total impulse + // x := new total impulse + // d := incremental impulse + // + // For the current iteration we extend the formula for the incremental impulse + // to compute the new total impulse: + // + // vn = A * d + b + // = A * (x - a) + b + // = A * x + b - A * a + // = A * x + b' + // b' = b - A * a; + + VelocityConstraintPoint cp1 = vc.points[0]; + VelocityConstraintPoint cp2 = vc.points[1]; + + Vector2 a = new Vector2(cp1.normalImpulse, cp2.normalImpulse); + Debug.Assert(a.X >= 0.0f && a.Y >= 0.0f); + + // Relative velocity at contact + Vector2 dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA); + Vector2 dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA); + + // Compute normal velocity + float vn1 = Vector2.Dot(dv1, normal); + float vn2 = Vector2.Dot(dv2, normal); + + Vector2 b = new Vector2(); + b.X = vn1 - cp1.velocityBias; + b.Y = vn2 - cp2.velocityBias; + + // Compute b' + b -= MathUtils.Mul(ref vc.K, a); + + const float k_errorTol = 1e-3f; + //B2_NOT_USED(k_errorTol); + + for (; ; ) + { + // + // Case 1: vn = 0 + // + // 0 = A * x + b' + // + // Solve for x: + // + // x = - inv(A) * b' + // + Vector2 x = -MathUtils.Mul(ref vc.normalMass, b); + + if (x.X >= 0.0f && x.Y >= 0.0f) + { + // Get the incremental impulse + Vector2 d = x - a; + + // Apply incremental impulse + Vector2 P1 = d.X * normal; + Vector2 P2 = d.Y * normal; + vA -= mA * (P1 + P2); + wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2)); + + vB += mB * (P1 + P2); + wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2)); + + // Accumulate + cp1.normalImpulse = x.X; + cp2.normalImpulse = x.Y; + +#if B2_DEBUG_SOLVER + // 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); + + b2Assert(b2Abs(vn1 - cp1.velocityBias) < k_errorTol); + b2Assert(b2Abs(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' + // + x.X = -cp1.normalMass * b.X; + x.Y = 0.0f; + vn1 = 0.0f; + vn2 = vc.K.ex.Y * x.X + b.Y; + + if (x.X >= 0.0f && vn2 >= 0.0f) + { + // Get the incremental impulse + Vector2 d = x - a; + + // Apply incremental impulse + Vector2 P1 = d.X * normal; + Vector2 P2 = d.Y * normal; + vA -= mA * (P1 + P2); + wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2)); + + vB += mB * (P1 + P2); + wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2)); + + // Accumulate + cp1.normalImpulse = x.X; + cp2.normalImpulse = x.Y; + +#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); + + b2Assert(b2Abs(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' + // + x.X = 0.0f; + x.Y = -cp2.normalMass * b.Y; + vn1 = vc.K.ey.X * x.Y + b.X; + vn2 = 0.0f; + + if (x.Y >= 0.0f && vn1 >= 0.0f) + { + // Resubstitute for the incremental impulse + Vector2 d = x - a; + + // Apply incremental impulse + Vector2 P1 = d.X * normal; + Vector2 P2 = d.Y * normal; + vA -= mA * (P1 + P2); + wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2)); + + vB += mB * (P1 + P2); + wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2)); + + // Accumulate + cp1.normalImpulse = x.X; + cp2.normalImpulse = x.Y; + +#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); + + b2Assert(b2Abs(vn2 - cp2.velocityBias) < k_errorTol); +#endif + break; + } + + // + // Case 4: x1 = 0 and x2 = 0 + // + // vn1 = b1 + // vn2 = b2; + x.X = 0.0f; + x.Y = 0.0f; + vn1 = b.X; + vn2 = b.Y; + + if (vn1 >= 0.0f && vn2 >= 0.0f) + { + // Resubstitute for the incremental impulse + Vector2 d = x - a; + + // Apply incremental impulse + Vector2 P1 = d.X * normal; + Vector2 P2 = d.Y * normal; + vA -= mA * (P1 + P2); + wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2)); + + vB += mB * (P1 + P2); + wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2)); + + // Accumulate + cp1.normalImpulse = x.X; + cp2.normalImpulse = x.Y; + + break; + } + + // No solution, give up. This is hit sometimes, but it doesn't seem to matter. + break; + } + } + + _velocities[indexA].v = vA; + _velocities[indexA].w = wA; + _velocities[indexB].v = vB; + _velocities[indexB].w = wB; + } + } + + public void StoreImpulses() + { + for (int i = 0; i < _count; ++i) + { + ContactVelocityConstraint vc = _velocityConstraints[i]; + Manifold manifold = _contacts[vc.contactIndex].Manifold; + + for (int j = 0; j < vc.pointCount; ++j) + { + ManifoldPoint point = manifold.Points[j]; + point.NormalImpulse = vc.points[j].normalImpulse; + point.TangentImpulse = vc.points[j].tangentImpulse; + manifold.Points[j] = point; + } + + _contacts[vc.contactIndex].Manifold = manifold; + } + } + + public bool SolvePositionConstraints() + { + float minSeparation = 0.0f; + + for (int i = 0; i < _count; ++i) + { + ContactPositionConstraint pc = _positionConstraints[i]; + + int indexA = pc.indexA; + int indexB = pc.indexB; + Vector2 localCenterA = pc.localCenterA; + float mA = pc.invMassA; + float iA = pc.invIA; + Vector2 localCenterB = pc.localCenterB; + float mB = pc.invMassB; + float iB = pc.invIB; + int pointCount = pc.pointCount; + + Vector2 cA = _positions[indexA].c; + float aA = _positions[indexA].a; + + Vector2 cB = _positions[indexB].c; + float aB = _positions[indexB].a; + + // Solve normal constraints + for (int j = 0; j < pointCount; ++j) + { + Transform xfA = new Transform(); + Transform xfB = new Transform(); + xfA.q.Set(aA); + xfB.q.Set(aB); + xfA.p = cA - MathUtils.Mul(xfA.q, localCenterA); + xfB.p = cB - MathUtils.Mul(xfB.q, localCenterB); + + Vector2 normal; + Vector2 point; + float separation; + + PositionSolverManifold.Initialize(pc, xfA, xfB, j, out normal, out point, out separation); + + Vector2 rA = point - cA; + Vector2 rB = point - cB; + + // Track max constraint error. + minSeparation = Math.Min(minSeparation, separation); + + // Prevent large corrections and allow slop. + float C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f); + + // Compute the effective mass. + float rnA = MathUtils.Cross(rA, normal); + float rnB = MathUtils.Cross(rB, normal); + float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; + + // Compute normal impulse + float impulse = K > 0.0f ? -C / K : 0.0f; + + Vector2 P = impulse * normal; + + cA -= mA * P; + aA -= iA * MathUtils.Cross(rA, P); + + cB += mB * P; + aB += iB * MathUtils.Cross(rB, P); + } + + _positions[indexA].c = cA; + _positions[indexA].a = aA; + + _positions[indexB].c = cB; + _positions[indexB].a = aB; + } + + // We can't expect minSpeparation >= -b2_linearSlop because we don't + // push the separation above -b2_linearSlop. + return minSeparation >= -3.0f * Settings.LinearSlop; + } + + // Sequential position solver for position constraints. + public bool SolveTOIPositionConstraints(int toiIndexA, int toiIndexB) + { + float minSeparation = 0.0f; + + for (int i = 0; i < _count; ++i) + { + ContactPositionConstraint pc = _positionConstraints[i]; + + int indexA = pc.indexA; + int indexB = pc.indexB; + Vector2 localCenterA = pc.localCenterA; + Vector2 localCenterB = pc.localCenterB; + int pointCount = pc.pointCount; + + float mA = 0.0f; + float iA = 0.0f; + if (indexA == toiIndexA || indexA == toiIndexB) + { + mA = pc.invMassA; + iA = pc.invIA; + } + + float mB = 0.0f; + float iB = 0.0f; + if (indexB == toiIndexA || indexB == toiIndexB) + { + mB = pc.invMassB; + iB = pc.invIB; + } + + Vector2 cA = _positions[indexA].c; + float aA = _positions[indexA].a; + + Vector2 cB = _positions[indexB].c; + float aB = _positions[indexB].a; + + // Solve normal constraints + for (int j = 0; j < pointCount; ++j) + { + Transform xfA = new Transform(); + Transform xfB = new Transform(); + xfA.q.Set(aA); + xfB.q.Set(aB); + xfA.p = cA - MathUtils.Mul(xfA.q, localCenterA); + xfB.p = cB - MathUtils.Mul(xfB.q, localCenterB); + + Vector2 normal; + Vector2 point; + float separation; + + PositionSolverManifold.Initialize(pc, xfA, xfB, j, out normal, out point, out separation); + + Vector2 rA = point - cA; + Vector2 rB = point - cB; + + // Track max constraint error. + minSeparation = Math.Min(minSeparation, separation); + + // Prevent large corrections and allow slop. + float C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f); + + // Compute the effective mass. + float rnA = MathUtils.Cross(rA, normal); + float rnB = MathUtils.Cross(rB, normal); + float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; + + // Compute normal impulse + float impulse = K > 0.0f ? -C / K : 0.0f; + + Vector2 P = impulse * normal; + + cA -= mA * P; + aA -= iA * MathUtils.Cross(rA, P); + + cB += mB * P; + aB += iB * MathUtils.Cross(rB, P); + } + + _positions[indexA].c = cA; + _positions[indexA].a = aA; + + _positions[indexB].c = cB; + _positions[indexB].a = aB; + } + + // We can't expect minSpeparation >= -b2_linearSlop because we don't + // push the separation above -b2_linearSlop. + return minSeparation >= -1.5f * Settings.LinearSlop; + } + + public static class WorldManifold + { + /// + /// Evaluate the manifold with supplied transforms. This assumes + /// modest motion from the original state. This does not change the + /// point count, impulses, etc. The radii must come from the Shapes + /// that generated the manifold. + /// + /// The manifold. + /// The transform for A. + /// The radius for A. + /// The transform for B. + /// The radius for B. + /// World vector pointing from A to B + /// Torld contact point (point of intersection). + public static void Initialize(ref Manifold manifold, ref Transform xfA, float radiusA, ref Transform xfB, float radiusB, out Vector2 normal, out FixedArray2 points) + { + normal = Vector2.Zero; + points = new FixedArray2(); + + if (manifold.PointCount == 0) + { + return; + } + + switch (manifold.Type) + { + case ManifoldType.Circles: + { + normal = new Vector2(1.0f, 0.0f); + Vector2 pointA = MathUtils.Mul(ref xfA, manifold.LocalPoint); + Vector2 pointB = MathUtils.Mul(ref xfB, manifold.Points[0].LocalPoint); + if (Vector2.DistanceSquared(pointA, pointB) > Settings.Epsilon * Settings.Epsilon) + { + normal = pointB - pointA; + normal.Normalize(); + } + + Vector2 cA = pointA + radiusA * normal; + Vector2 cB = pointB - radiusB * normal; + points[0] = 0.5f * (cA + cB); + } + break; + + case ManifoldType.FaceA: + { + normal = MathUtils.Mul(xfA.q, manifold.LocalNormal); + Vector2 planePoint = MathUtils.Mul(ref xfA, manifold.LocalPoint); + + for (int i = 0; i < manifold.PointCount; ++i) + { + Vector2 clipPoint = MathUtils.Mul(ref xfB, manifold.Points[i].LocalPoint); + Vector2 cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, normal)) * normal; + Vector2 cB = clipPoint - radiusB * normal; + points[i] = 0.5f * (cA + cB); + } + } + break; + + case ManifoldType.FaceB: + { + normal = MathUtils.Mul(xfB.q, manifold.LocalNormal); + Vector2 planePoint = MathUtils.Mul(ref xfB, manifold.LocalPoint); + + for (int i = 0; i < manifold.PointCount; ++i) + { + Vector2 clipPoint = MathUtils.Mul(ref xfA, manifold.Points[i].LocalPoint); + Vector2 cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, normal)) * normal; + Vector2 cA = clipPoint - radiusA * normal; + points[i] = 0.5f * (cA + cB); + } + + // Ensure normal points from A to B. + normal = -normal; + } + break; + } + } + } + + private static class PositionSolverManifold + { + public static void Initialize(ContactPositionConstraint pc, Transform xfA, Transform xfB, int index, out Vector2 normal, out Vector2 point, out float separation) + { + Debug.Assert(pc.pointCount > 0); + + switch (pc.type) + { + case ManifoldType.Circles: + { + Vector2 pointA = MathUtils.Mul(ref xfA, pc.localPoint); + Vector2 pointB = MathUtils.Mul(ref xfB, pc.localPoints[0]); + normal = pointB - pointA; + normal.Normalize(); + point = 0.5f * (pointA + pointB); + separation = Vector2.Dot(pointB - pointA, normal) - pc.radiusA - pc.radiusB; + } + break; + + case ManifoldType.FaceA: + { + normal = MathUtils.Mul(xfA.q, pc.localNormal); + Vector2 planePoint = MathUtils.Mul(ref xfA, pc.localPoint); + + Vector2 clipPoint = MathUtils.Mul(ref xfB, pc.localPoints[index]); + separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB; + point = clipPoint; + } + break; + + case ManifoldType.FaceB: + { + normal = MathUtils.Mul(xfB.q, pc.localNormal); + Vector2 planePoint = MathUtils.Mul(ref xfB, pc.localPoint); + + Vector2 clipPoint = MathUtils.Mul(ref xfA, pc.localPoints[index]); + separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB; + point = clipPoint; + + // Ensure normal points from A to B + normal = -normal; + } + break; + default: + normal = Vector2.Zero; + point = Vector2.Zero; + separation = 0; + break; + + } + } + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/Contacts/EdgeAndCircleContact.cs b/src/Box2DNet/Dynamics/Contacts/EdgeAndCircleContact.cs deleted file mode 100644 index 4382c4f..0000000 --- a/src/Box2DNet/Dynamics/Contacts/EdgeAndCircleContact.cs +++ /dev/null @@ -1,55 +0,0 @@ -/* - Box2DNet Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version 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 Box2DNet.Collision; -using Box2DNet.Common; - -namespace Box2DNet.Dynamics -{ - public class EdgeAndCircleContact : Contact - { - public EdgeAndCircleContact(Fixture fixtureA, Fixture fixtureB) - : base(fixtureA, fixtureB) - { - Box2DNetDebug.Assert(fixtureA.ShapeType == ShapeType.EdgeShape); - Box2DNetDebug.Assert(fixtureB.ShapeType == ShapeType.CircleShape); - _manifold.PointCount = 0; - _manifold.Points[0].NormalImpulse = 0.0f; - _manifold.Points[0].TangentImpulse = 0.0f; - CollideShapeFunction = CollideEdgeAndCircle; - } - - private static void CollideEdgeAndCircle(ref Manifold manifold, Shape shape1, Transform xf1, Shape shape2, Transform xf2) - { - Collision.Collision.CollideEdgeAndCircle(ref manifold, (EdgeShape)shape1, xf1, (CircleShape)shape2, xf2); - } - - new public static Contact Create(Fixture fixtureA, Fixture fixtureB) - { - return new EdgeAndCircleContact(fixtureA, fixtureB); - } - - new public static void Destroy(ref Contact contact) - { - contact = null; - } - } -} \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/Contacts/NullContact.cs b/src/Box2DNet/Dynamics/Contacts/NullContact.cs deleted file mode 100644 index 6ad117e..0000000 --- a/src/Box2DNet/Dynamics/Contacts/NullContact.cs +++ /dev/null @@ -1,35 +0,0 @@ -/* - Box2DNet Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version 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 Box2DNet.Collision; -using Box2DNet.Common; - -namespace Box2DNet.Dynamics -{ - public class NullContact : Contact - { - public NullContact() - { - CollideShapeFunction = Collide; - } - private static void Collide(ref Manifold manifold, Shape shape1, Transform xf1, Shape shape2, Transform xf2) { } - } -} \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/Contacts/PolyAndCircleContact.cs b/src/Box2DNet/Dynamics/Contacts/PolyAndCircleContact.cs deleted file mode 100644 index eac34ad..0000000 --- a/src/Box2DNet/Dynamics/Contacts/PolyAndCircleContact.cs +++ /dev/null @@ -1,54 +0,0 @@ -/* - Box2DNet Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version 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 Box2DNet.Collision; -using Box2DNet.Common; - -using Transform = Box2DNet.Common.Transform; - -namespace Box2DNet.Dynamics -{ - public class PolyAndCircleContact : Contact - { - public PolyAndCircleContact(Fixture fixtureA, Fixture fixtureB) - : base(fixtureA, fixtureB) - { - Box2DNetDebug.Assert(fixtureA.ShapeType == ShapeType.PolygonShape); - Box2DNetDebug.Assert(fixtureB.ShapeType == ShapeType.CircleShape); - CollideShapeFunction = CollidePolygonCircle; - } - - private static void CollidePolygonCircle(ref Manifold manifold, Shape shape1, Transform xf1, Shape shape2, Transform xf2) - { - Collision.Collision.CollidePolygonAndCircle(ref manifold, (PolygonShape)shape1, xf1, (CircleShape)shape2, xf2); - } - - new public static Contact Create(Fixture fixtureA, Fixture fixtureB) - { - return new PolyAndCircleContact(fixtureA, fixtureB); - } - - new public static void Destroy(ref Contact contact) - { - contact = null; - } - } -} diff --git a/src/Box2DNet/Dynamics/Contacts/PolyAndEdgeContact.cs b/src/Box2DNet/Dynamics/Contacts/PolyAndEdgeContact.cs deleted file mode 100644 index 1f7f669..0000000 --- a/src/Box2DNet/Dynamics/Contacts/PolyAndEdgeContact.cs +++ /dev/null @@ -1,54 +0,0 @@ -/* - Box2DNet Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version 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 Box2DNet.Collision; -using Box2DNet.Common; - -using Transform = Box2DNet.Common.Transform; - -namespace Box2DNet.Dynamics -{ - public class PolyAndEdgeContact : Contact - { - public PolyAndEdgeContact(Fixture fixtureA, Fixture fixtureB) - : base(fixtureA, fixtureB) - { - Box2DNetDebug.Assert(fixtureA.ShapeType == ShapeType.PolygonShape); - Box2DNetDebug.Assert(fixtureB.ShapeType == ShapeType.EdgeShape); - CollideShapeFunction = CollidePolyAndEdgeContact; - } - - private static void CollidePolyAndEdgeContact(ref Manifold manifold, Shape shape1, Transform xf1, Shape shape2, Transform xf2) - { - Collision.Collision.CollidePolyAndEdge(ref manifold, (PolygonShape)shape1, xf1, (EdgeShape)shape2, xf2); - } - - new public static Contact Create(Fixture fixtureA, Fixture fixtureB) - { - return new PolyAndEdgeContact(fixtureA, fixtureB); - } - - new public static void Destroy(ref Contact contact) - { - contact = null; - } - } -} \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/Contacts/PolyContact.cs b/src/Box2DNet/Dynamics/Contacts/PolyContact.cs deleted file mode 100644 index e60ed56..0000000 --- a/src/Box2DNet/Dynamics/Contacts/PolyContact.cs +++ /dev/null @@ -1,52 +0,0 @@ -/* - Box2DNet Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version 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 Box2DNet.Collision; -using Box2DNet.Common; - -namespace Box2DNet.Dynamics -{ - public class PolygonContact : Contact - { - public PolygonContact(Fixture fixtureA, Fixture fixtureB) - : base(fixtureA, fixtureB) - { - Box2DNetDebug.Assert(fixtureA.ShapeType == ShapeType.PolygonShape); - Box2DNetDebug.Assert(fixtureB.ShapeType == ShapeType.PolygonShape); - CollideShapeFunction = CollidePolygons; - } - - private static void CollidePolygons(ref Manifold manifold, Shape shape1, Transform xf1, Shape shape2, Transform xf2) - { - Collision.Collision.CollidePolygons(ref manifold, (PolygonShape)shape1, xf1, (PolygonShape)shape2, xf2); - } - - new public static Contact Create(Fixture fixtureA, Fixture fixtureB) - { - return new PolygonContact(fixtureA, fixtureB); - } - - new public static void Destroy(ref Contact contact) - { - contact = null; - } - } -} \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/Controllers/BuoyancyController.cs b/src/Box2DNet/Dynamics/Controllers/BuoyancyController.cs deleted file mode 100644 index 0cf776e..0000000 --- a/src/Box2DNet/Dynamics/Controllers/BuoyancyController.cs +++ /dev/null @@ -1,158 +0,0 @@ -using System.Numerics; -using Box2DNet.Common; - - -namespace Box2DNet.Dynamics.Controllers -{ - /// - /// This class is used to build buoyancy controllers - /// - public class BuoyancyControllerDef - { - /// The outer surface normal - public Vector2 Normal; - /// The height of the fluid surface along the normal - public float Offset; - /// The fluid density - public float Density; - /// Fluid velocity, for drag calculations - public Vector2 Velocity; - /// Linear drag co-efficient - public float LinearDrag; - /// Linear drag co-efficient - public float AngularDrag; - /// If false, bodies are assumed to be uniformly dense, otherwise use the shapes densities - public bool UseDensity; //False by default to prevent a gotcha - /// If true, gravity is taken from the world instead of the gravity parameter. - public bool UseWorldGravity; - /// Gravity vector, if the world's gravity is not used - public Vector2 Gravity; - - public BuoyancyControllerDef() - { - Normal = new Vector2(0, 1); - Offset = 0; - Density = 0; - Velocity = Vector2.Zero; - LinearDrag = 0; - AngularDrag = 0; - UseDensity = false; - UseWorldGravity = true; - Gravity = Vector2.Zero; - } - } - - /// - /// Calculates buoyancy forces for fluids in the form of a half plane. - /// - public class BuoyancyController : Controller - { - /// The outer surface normal - public Vector2 Normal; - /// The height of the fluid surface along the normal - public float Offset; - /// The fluid density - public float Density; - /// Fluid velocity, for drag calculations - public Vector2 Velocity; - /// Linear drag co-efficient - public float LinearDrag; - /// Linear drag co-efficient - public float AngularDrag; - /// If false, bodies are assumed to be uniformly dense, otherwise use the shapes densities - public bool UseDensity; //False by default to prevent a gotcha - /// If true, gravity is taken from the world instead of the gravity parameter. - public bool UseWorldGravity; - /// Gravity vector, if the world's gravity is not used - public Vector2 Gravity; - - public BuoyancyController(BuoyancyControllerDef buoyancyControllerDef) - { - Normal = buoyancyControllerDef.Normal; - Offset = buoyancyControllerDef.Offset; - Density = buoyancyControllerDef.Density; - Velocity = buoyancyControllerDef.Velocity; - LinearDrag = buoyancyControllerDef.LinearDrag; - AngularDrag = buoyancyControllerDef.AngularDrag; - UseDensity = buoyancyControllerDef.UseDensity; - UseWorldGravity = buoyancyControllerDef.UseWorldGravity; - Gravity = buoyancyControllerDef.Gravity; - } - - public override void Step(TimeStep step) - { - //B2_NOT_USED(step); - if (_bodyList == null) - return; - - if (UseWorldGravity) - { - Gravity = _world.Gravity; - } - for (ControllerEdge i = _bodyList; i != null; i = i.nextBody) - { - Body body = i.body; - if (body.IsSleeping()) - { - //Buoyancy force is just a function of position, - //so unlike most forces, it is safe to ignore sleeping bodes - continue; - } - Vector2 areac = Vector2.Zero; - Vector2 massc = Vector2.Zero; - float area = 0; - float mass = 0; - for (Fixture shape = body.GetFixtureList(); shape != null; shape = shape.Next) - { - Vector2 sc; - float sarea = shape.ComputeSubmergedArea(Normal, Offset, out sc); - area += sarea; - areac.X += sarea * sc.X; - areac.Y += sarea * sc.Y; - float shapeDensity = 0; - if (UseDensity) - { - //TODO: Expose density publicly - shapeDensity = shape.Density; - } - else - { - shapeDensity = 1; - } - mass += sarea * shapeDensity; - massc.X += sarea * sc.X * shapeDensity; - massc.Y += sarea * sc.Y * shapeDensity; - } - areac.X /= area; - areac.Y /= area; - //Vec2 localCentroid = Math.MulT(body.GetTransform(), areac); - massc.X /= mass; - massc.Y /= mass; - if (area < Settings.FLT_EPSILON) - continue; - //Buoyancy - Vector2 buoyancyForce = -Density * area * Gravity; - body.ApplyForce(buoyancyForce, massc); - //Linear drag - Vector2 dragForce = body.GetLinearVelocityFromWorldPoint(areac) - Velocity; - dragForce *= -LinearDrag * area; - body.ApplyForce(dragForce, areac); - //Angular drag - //TODO: Something that makes more physical sense? - body.ApplyTorque(-body.GetInertia() / body.GetMass() * area * body.GetAngularVelocity() * AngularDrag); - - } - } - - public override void Draw(DebugDraw debugDraw) - { - float r = 1000; - Vector2 p1 = Offset * Normal + Normal.CrossScalarPostMultiply(r); - Vector2 p2 = Offset * Normal - Normal.CrossScalarPostMultiply(r); - - Color color = new Color(0, 0, 0.8f); - - debugDraw.DrawSegment(p1, p2, color); - } - } -} \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/Controllers/ConstantAccelController.cs b/src/Box2DNet/Dynamics/Controllers/ConstantAccelController.cs deleted file mode 100644 index 8998763..0000000 --- a/src/Box2DNet/Dynamics/Controllers/ConstantAccelController.cs +++ /dev/null @@ -1,57 +0,0 @@ -/* -* Copyright (c) 2006-2007 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 Box2DNet.Common; -using System.Numerics; - -namespace Box2DNet.Dynamics.Controllers -{ - - /// This class is used to build constant acceleration controllers - public class ConstantAccelControllerDef - { - /// - /// The force to apply - /// - public Vector2 A; - } - - public class ConstantAccelController : Controller - { - /// - /// The force to apply - /// - public Vector2 A; - - public ConstantAccelController(ConstantAccelControllerDef def) - { - A = def.A; - } - - public override void Step(TimeStep step) - { - for (ControllerEdge i = _bodyList; i != null; i = i.nextBody) - { - Body body = i.body; - if (body.IsSleeping()) - continue; - body.SetLinearVelocity(body.GetLinearVelocity() + step.Dt * A); - } - } - } -} diff --git a/src/Box2DNet/Dynamics/Controllers/ConstantForceController.cs b/src/Box2DNet/Dynamics/Controllers/ConstantForceController.cs deleted file mode 100644 index 415d14e..0000000 --- a/src/Box2DNet/Dynamics/Controllers/ConstantForceController.cs +++ /dev/null @@ -1,58 +0,0 @@ -/* -* Copyright (c) 2006-2007 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 Box2DNet.Common; - -using System.Numerics; -namespace Box2DNet.Dynamics.Controllers -{ - - /// - /// This class is used to build constant force controllers - /// - public class ConstantForceControllerDef - { - /// The force to apply - public Vector2 F; - } - - public class ConstantForceController : Controller - { - /// - /// The force to apply - /// - Vector2 F; - - public ConstantForceController(ConstantForceControllerDef def) - { - F = def.F; - } - - public override void Step(TimeStep step) - { - //B2_NOT_USED(step); - for (ControllerEdge i = _bodyList; i != null; i = i.nextBody) - { - Body body = i.body; - if (body.IsSleeping()) - continue; - body.ApplyForce(F, body.GetWorldCenter()); - } - } - } -} diff --git a/src/Box2DNet/Dynamics/Controllers/Controller.cs b/src/Box2DNet/Dynamics/Controllers/Controller.cs deleted file mode 100644 index 4f6e91d..0000000 --- a/src/Box2DNet/Dynamics/Controllers/Controller.cs +++ /dev/null @@ -1,174 +0,0 @@ -using System; using System.Numerics; - -namespace Box2DNet.Dynamics.Controllers -{ - /// - /// A controller edge is used to connect bodies and controllers together - /// in a bipartite graph. - /// - public class ControllerEdge - { - public Controller controller; // provides quick access to other end of this edge. - public Body body; // the body - public ControllerEdge prevBody; // the previous controller edge in the controllers's joint list - public ControllerEdge nextBody; // the next controller edge in the controllers's joint list - public ControllerEdge prevController; // the previous controller edge in the body's joint list - public ControllerEdge nextController; // the next controller edge in the body's joint list - } - - /// - /// Base class for controllers. Controllers are a convience for encapsulating common - /// per-step functionality. - /// - public abstract class Controller : IDisposable - { - internal Controller _prev; - internal Controller _next; - - internal World _world; - protected ControllerEdge _bodyList; - protected int _bodyCount; - - public Controller() - { - _bodyList = null; - _bodyCount = 0; - } - - public Controller(World world) - { - _bodyList = null; - _bodyCount = 0; - - _world = world; - } - - public void Dispose() - { - //Remove attached bodies - - //Previus implementation: - //while (_bodyCount > 0) - // RemoveBody(_bodyList.body); - - Clear(); - } - - /// - /// Controllers override this to implement per-step functionality. - /// - public abstract void Step(TimeStep step); - - /// - /// Controllers override this to provide debug drawing. - /// - public virtual void Draw(DebugDraw debugDraw) { } - - /// - /// Adds a body to the controller list. - /// - public void AddBody(Body body) - { - ControllerEdge edge = new ControllerEdge(); - - edge.body = body; - edge.controller = this; - - //Add edge to controller list - edge.nextBody = _bodyList; - edge.prevBody = null; - if (_bodyList != null) - _bodyList.prevBody = edge; - _bodyList = edge; - ++_bodyCount; - - //Add edge to body list - edge.nextController = body._controllerList; - edge.prevController = null; - if (body._controllerList != null) - body._controllerList.prevController = edge; - body._controllerList = edge; - } - - /// - /// Removes a body from the controller list. - /// - public void RemoveBody(Body body) - { - //Assert that the controller is not empty - Box2DNetDebug.Assert(_bodyCount > 0); - - //Find the corresponding edge - ControllerEdge edge = _bodyList; - while (edge != null && edge.body != body) - edge = edge.nextBody; - - //Assert that we are removing a body that is currently attached to the controller - Box2DNetDebug.Assert(edge != null); - - //Remove edge from controller list - if (edge.prevBody != null) - edge.prevBody.nextBody = edge.nextBody; - if (edge.nextBody != null) - edge.nextBody.prevBody = edge.prevBody; - if (edge == _bodyList) - _bodyList = edge.nextBody; - --_bodyCount; - - //Remove edge from body list - if (edge.prevController != null) - edge.prevController.nextController = edge.nextController; - if (edge.nextController != null) - edge.nextController.prevController = edge.prevController; - if (edge == body._controllerList) - body._controllerList = edge.nextController; - - //Free the edge - edge = null; - } - - /// - /// Removes all bodies from the controller list. - /// - public void Clear() - { -#warning "Check this" - ControllerEdge current = _bodyList; - while (current != null) - { - ControllerEdge edge = current; - - //Remove edge from controller list - _bodyList = edge.nextBody; - - //Remove edge from body list - if (edge.prevController != null) - edge.prevController.nextController = edge.nextController; - if (edge.nextController != null) - edge.nextController.prevController = edge.prevController; - if (edge == edge.body._controllerList) - edge.body._controllerList = edge.nextController; - - //Free the edge - //m_world->m_blockAllocator.Free(edge, sizeof(b2ControllerEdge)); - } - - _bodyCount = 0; - } - - /// - /// Get the next body in the world's body list. - /// - internal Controller GetNext() { return _next; } - - /// - /// Get the parent world of this body. - /// - internal World GetWorld() { return _world; } - - /// - /// Get the attached body list - /// - internal ControllerEdge GetBodyList() { return _bodyList; } - } -} \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/Controllers/GravityController.cs b/src/Box2DNet/Dynamics/Controllers/GravityController.cs deleted file mode 100644 index 29e9d80..0000000 --- a/src/Box2DNet/Dynamics/Controllers/GravityController.cs +++ /dev/null @@ -1,99 +0,0 @@ -/* -* Copyright (c) 2006-2007 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.Numerics; -using Box2DNet.Common; -using Math = Box2DNet.Common.Math; - - - -namespace Box2DNet.Dynamics.Controllers -{ - /// This class is used to build gravity controllers - public class GravityControllerDef - { - /// - /// Specifies the strength of the gravitiation force - /// - public float G; - - /// - /// If true, gravity is proportional to r^-2, otherwise r^-1 - /// - public bool InvSqr; - } - - public class GravityController : Controller - { - /// - /// Specifies the strength of the gravitiation force - /// - public float G; - - /// If true, gravity is proportional to r^-2, otherwise r^-1 - public bool InvSqr; - - public GravityController(GravityControllerDef def) - { - G = def.G; - InvSqr = def.InvSqr; - } - - public override void Step(TimeStep step) - { - //B2_NOT_USED(step); - if (InvSqr) - { - for (ControllerEdge i = _bodyList; i != null; i = i.nextBody) - { - Body body1 = i.body; - for (ControllerEdge j = _bodyList; j != i; j = j.nextBody) - { - Body body2 = j.body; - Vector2 d = body2.GetWorldCenter() - body1.GetWorldCenter(); - float r2 = d.LengthSquared(); - if (r2 < Settings.FLT_EPSILON) - continue; - - Vector2 f = G / r2 / Math.Sqrt(r2) * body1.GetMass() * body2.GetMass() * d; - body1.ApplyForce(f, body1.GetWorldCenter()); - body2.ApplyForce(-1.0f * f, body2.GetWorldCenter()); - } - } - } - else - { - for (ControllerEdge i = _bodyList; i != null; i = i.nextBody) - { - Body body1 = i.body; - for (ControllerEdge j = _bodyList; j != i; j = j.nextBody) - { - Body body2 = j.body; - Vector2 d = body2.GetWorldCenter() - body1.GetWorldCenter(); - float r2 = d.LengthSquared(); - if (r2 < Settings.FLT_EPSILON) - continue; - Vector2 f = G / r2 * body1.GetMass() * body2.GetMass() * d; - body1.ApplyForce(f, body1.GetWorldCenter()); - body2.ApplyForce(-1.0f * f, body2.GetWorldCenter()); - } - } - } - } - } -} diff --git a/src/Box2DNet/Dynamics/Controllers/TensorDampingController.cs b/src/Box2DNet/Dynamics/Controllers/TensorDampingController.cs deleted file mode 100644 index 9b43041..0000000 --- a/src/Box2DNet/Dynamics/Controllers/TensorDampingController.cs +++ /dev/null @@ -1,91 +0,0 @@ -/* -* Copyright (c) 2006-2007 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 Box2DNet.Common; - -using System.Numerics; - -namespace Box2DNet.Dynamics.Controllers -{ - - /// - /// This class is used to build tensor damping controllers - /// - public class b2TensorDampingControllerDef - { - /// Tensor to use in damping model - Mat22 T; - /// Set this to a positive number to clamp the maximum amount of damping done. - float maxTimestep; - }; - - public class TensorDampingController : Controller - { - - /// - /// Tensor to use in damping model - /// Some examples (matrixes in format (row1; row2) ) - ///(-a 0;0 -a) Standard isotropic damping with strength a - ///(0 a;-a 0) Electron in fixed field - a force at right angles to velocity with proportional magnitude - ///(-a 0;0 -b) Differing x and y damping. Useful e.g. for top-down wheels. - ///By the way, tensor in this case just means matrix, don't let the terminology get you down. - /// - Mat22 T; - - /// - /// Set this to a positive number to clamp the maximum amount of damping done. - /// Typically one wants maxTimestep to be 1/(max eigenvalue of T), so that damping will never cause something to reverse direction - /// - float MaxTimestep; - - /// Sets damping independantly along the x and y axes - public void SetAxisAligned(float xDamping, float yDamping) - { - T.Col1.X = -xDamping; - T.Col1.Y = 0; - T.Col2.X = 0; - T.Col2.Y = -yDamping; - if (xDamping > 0 || yDamping > 0) - { - MaxTimestep = 1 / Math.Max(xDamping, yDamping); - } - else - { - MaxTimestep = 0; - } - } - - public override void Step(TimeStep step) - { - float timestep = step.Dt; - if (timestep <= Settings.FLT_EPSILON) - return; - if (timestep > MaxTimestep && MaxTimestep > 0) - timestep = MaxTimestep; - for (ControllerEdge i = _bodyList; i != null; i = i.nextBody) - { - Body body = i.body; - if (body.IsSleeping()) - continue; - - Vector2 damping = body.GetWorldVector(T.Multiply(body.GetLocalVector(body.GetLinearVelocity()))); - body.SetLinearVelocity(body.GetLinearVelocity() + timestep*damping); - } - } - } -} diff --git a/src/Box2DNet/Dynamics/Fixture.cs b/src/Box2DNet/Dynamics/Fixture.cs index 3eafbde..b53e89b 100644 --- a/src/Box2DNet/Dynamics/Fixture.cs +++ b/src/Box2DNet/Dynamics/Fixture.cs @@ -1,484 +1,618 @@ -/* - Box2DNet Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version 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.Numerics; -using System.Collections.Generic; -using System.Text; -using Box2DNet.Collision; -using Box2DNet.Common; - - -using Transform = Box2DNet.Common.Transform; - -namespace Box2DNet.Dynamics -{ - /// - /// This holds contact filtering data. - /// - public struct FilterData - { - /// - /// The collision category bits. Normally you would just set one bit. - /// - public ushort CategoryBits; - - /// - /// The collision mask bits. This states the categories that this - /// shape would accept for collision. - /// - public ushort MaskBits; - - /// - /// 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. - /// - public short GroupIndex; - } - - /// - /// A fixture definition is used to create a fixture. This class defines an - /// abstract fixture definition. You can reuse fixture definitions safely. - /// - public class FixtureDef - { - /// - /// The constructor sets the default fixture definition values. - /// - public FixtureDef() - { - Type = ShapeType.UnknownShape; - UserData = null; - Friction = 0.2f; - Restitution = 0.0f; - Density = 0.0f; - Filter.CategoryBits = 0x0001; - Filter.MaskBits = 0xFFFF; - Filter.GroupIndex = 0; - IsSensor = false; - } - - /// - /// Holds the shape type for down-casting. - /// - public ShapeType Type; - - /// - /// Use this to store application specific fixture data. - /// - public object UserData; - - /// - /// The friction coefficient, usually in the range [0,1]. - /// - public float Friction; - - /// - /// The restitution (elasticity) usually in the range [0,1]. - /// - public float Restitution; - - /// - /// The density, usually in kg/m^2. - /// - public float Density; - - /// - /// A sensor shape collects contact information but never generates a collision response. - /// - public bool IsSensor; - - /// - /// Contact filtering data. - /// - public FilterData Filter; - } - - /// - /// This structure is used to build a fixture with a circle shape. - /// - public class CircleDef : FixtureDef - { - public Vector2 LocalPosition; - public float Radius; - - public CircleDef() - { - Type = ShapeType.CircleShape; - LocalPosition = Vector2.Zero; - Radius = 1.0f; - } - } - - /// - /// Convex polygon. The vertices must be ordered so that the outside of - /// the polygon is on the right side of the edges (looking along the edge - /// from start to end). - /// - public class PolygonDef : FixtureDef - { - /// - /// The number of polygon vertices. - /// - public int VertexCount; - - /// - /// The polygon vertices in local coordinates. - /// - public Vector2[] Vertices = new Vector2[Settings.MaxPolygonVertices]; - - public PolygonDef() - { - Type = ShapeType.PolygonShape; - VertexCount = 0; - } - - /// - /// Build vertices to represent an axis-aligned box. - /// - /// The half-width - /// The half-height. - public void SetAsBox(float hx, float hy) - { - VertexCount = 4; - Vertices[0] = new Vector2(-hx, -hy); - Vertices[1] = new Vector2(hx, -hy); - Vertices[2] = new Vector2(hx, hy); - Vertices[3] = new Vector2(-hx, hy); - } - - - /// - /// Build vertices to represent an oriented box. - /// - /// The half-width - /// The half-height. - /// The center of the box in local coordinates. - /// The rotation of the box in local coordinates. - public void SetAsBox(float hx, float hy, Vector2 center, float angle) - { - SetAsBox(hx, hy); - - Transform xf = new Transform(); - xf.position = center; - xf.rotation = Box2DNet.Common.Math.AngleToRotation(angle); - //xf.R = new Mat22(angle); - - //Debug.Log(string.Format("xf.position = ({0},{1}) xf.rotation = ({2},{3},{4},{5})", xf.position.x, xf.position.y, xf.rotation.x, xf.rotation.y, xf.rotation.z, xf.rotation.w)); - - for (int i = 0; i < VertexCount; ++i) - { - Vertices[i] = xf.TransformPoint(Vertices[i]); - } - } - } - - /// - /// This structure is used to build a chain of edges. - /// - public class EdgeDef : FixtureDef - { - public EdgeDef() - { - Type = ShapeType.EdgeShape; - } - - /// - /// The start vertex. - /// - public Vector2 Vertex1; - - /// - /// The end vertex. - /// - public Vector2 Vertex2; - } - - /// - /// 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 - { - protected ShapeType _type; - protected bool _isSensor; - protected UInt16 _proxyId; - - internal Body _body; - protected Shape _shape; - internal Fixture _next; - - /// - /// Contact filtering data. You must call b2World::Refilter to correct - /// existing contacts/non-contacts. - /// - public FilterData Filter; - - /// - /// Is this fixture a sensor (non-solid)? - /// - public bool IsSensor { get { return _isSensor; } } - - /// - /// 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. - /// - public Shape Shape { get { return _shape; } } - - /// - /// Get the type of this shape. You can use this to down cast to the concrete shape. - /// - public ShapeType ShapeType { get { return _type; } } - - /// - /// Get the next fixture in the parent body's fixture list. - /// - public Fixture Next { get { return _next; } } - - /// - /// Get the parent body of this fixture. This is NULL if the fixture is not attached. - /// - public Body Body { get { return _body; } } - - /// - /// User data that was assigned in the fixture definition. Use this to - /// store your application specific data. - /// - public object UserData; - - /// - /// Friction coefficient, usually in the range [0,1]. - /// - public float Friction; - - /// - /// Restitution (elasticity) usually in the range [0,1]. - /// - public float Restitution; - - /// - /// Density, usually in kg/m^2. - /// - public float Density; - - public Fixture() - { - _proxyId = PairManager.NullProxy; - } - - public void Create(BroadPhase broadPhase, Body body, Transform xf, FixtureDef def) - { - UserData = def.UserData; - Friction = def.Friction; - Restitution = def.Restitution; - Density = def.Density; - - _body = body; - _next = null; - - Filter = def.Filter; - - _isSensor = def.IsSensor; - - _type = def.Type; - - // Allocate and initialize the child shape. - switch (_type) - { - case ShapeType.CircleShape: - { - CircleShape circle = new CircleShape(); - CircleDef circleDef = (CircleDef)def; - circle._position = circleDef.LocalPosition; - circle._radius = circleDef.Radius; - _shape = circle; - } - break; - - case ShapeType.PolygonShape: - { - PolygonShape polygon = new PolygonShape(); - PolygonDef polygonDef = (PolygonDef)def; - polygon.Set(polygonDef.Vertices, polygonDef.VertexCount); - _shape = polygon; - } - break; - - case ShapeType.EdgeShape: - { - EdgeShape edge = new EdgeShape(); - EdgeDef edgeDef = (EdgeDef)def; - edge.Set(edgeDef.Vertex1, edgeDef.Vertex2); - _shape = edge; - } - break; - - default: - Box2DNetDebug.Assert(false); - break; - } - - // Create proxy in the broad-phase. - AABB aabb; - _shape.ComputeAABB(out aabb, xf); - - bool inRange = broadPhase.InRange(aabb); - - // You are creating a shape outside the world box. - Box2DNetDebug.Assert(inRange); - - if (inRange) - { - _proxyId = broadPhase.CreateProxy(aabb, this); - } - else - { - _proxyId = PairManager.NullProxy; - } - } - - public void Destroy(BroadPhase broadPhase) - { - // Remove proxy from the broad-phase. - if (_proxyId != PairManager.NullProxy) - { - broadPhase.DestroyProxy(_proxyId); - _proxyId = PairManager.NullProxy; - } - - // Free the child shape. - _shape.Dispose(); - _shape = null; - } - - internal bool Synchronize(BroadPhase broadPhase, Transform Transform1, Transform Transform2) - { - if (_proxyId == PairManager.NullProxy) - { - return false; - } - - // Compute an AABB that covers the swept shape (may miss some rotation effect). - AABB aabb1, aabb2; - _shape.ComputeAABB(out aabb1, Transform1); - _shape.ComputeAABB(out aabb2, Transform2); - - AABB aabb = new AABB(); - aabb.Combine(aabb1, aabb2); - - if (broadPhase.InRange(aabb)) - { - broadPhase.MoveProxy(_proxyId, aabb); - return true; - } - else - { - return false; - } - } - - internal void RefilterProxy(BroadPhase broadPhase, Transform Transform) - { - if (_proxyId == PairManager.NullProxy) - { - return; - } - - broadPhase.DestroyProxy(_proxyId); - - AABB aabb; - _shape.ComputeAABB(out aabb, Transform); - - bool inRange = broadPhase.InRange(aabb); - - if (inRange) - { - _proxyId = broadPhase.CreateProxy(aabb, this); - } - else - { - _proxyId = PairManager.NullProxy; - } - } - - public virtual void Dispose() - { - Box2DNetDebug.Assert(_proxyId == PairManager.NullProxy); - Box2DNetDebug.Assert(_shape == null); - } - - /// - /// Compute the mass properties of this shape using its dimensions and density. - /// The inertia tensor is computed about the local origin, not the centroid. - /// - /// Returns the mass data for this shape. - public void ComputeMass(out MassData massData) - { - _shape.ComputeMass(out massData, Density); - } - - /// - /// Compute the volume and centroid of this fixture intersected with a half plane. - /// - /// Normal the surface normal. - /// Offset the surface offset along normal. - /// Returns the centroid. - /// The total volume less than offset along normal. - public float ComputeSubmergedArea(Vector2 normal, float offset, out Vector2 c) - { - return _shape.ComputeSubmergedArea(normal, offset, _body.GetTransform(), out c); - } - - /// - /// Test a point for containment in this fixture. This only works for convex shapes. - /// - /// A point in world coordinates. - public bool TestPoint(Vector2 p) - { - return _shape.TestPoint(_body.GetTransform(), p); - } - - /// - /// Perform a ray cast against this shape. - /// - /// Returns the hit fraction. You can use this to compute the contact point - /// p = (1 - lambda) * segment.p1 + lambda * segment.p2. - /// Returns the normal at the contact point. If there is no intersection, the normal - /// is not set. - /// Defines the begin and end point of the ray cast. - /// A number typically in the range [0,1]. - public SegmentCollide TestSegment(out float lambda, out Vector2 normal, Segment segment, float maxLambda) - { - return _shape.TestSegment(_body.GetTransform(), out lambda, out normal, segment, maxLambda); - } - - /// - /// Get the maximum radius about the parent body's center of mass. - /// - public float ComputeSweepRadius(Vector2 pivot) - { - return _shape.ComputeSweepRadius(pivot); - } - } -} +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ +//#define USE_IGNORE_CCD_CATEGORIES + +using System; +using System.Collections.Generic; +using System.Diagnostics; +using Box2DNet.Collision; +using Box2DNet.Collision.Shapes; +using Box2DNet.Common; +using Box2DNet.Dynamics.Contacts; +using Microsoft.Xna.Framework; + +namespace Box2DNet.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 + { + [ThreadStatic] + private static int _fixtureIdCounter; + private bool _isSensor; + private float _friction; + private float _restitution; + + internal Category _collidesWith; + internal Category _collisionCategories; + internal short _collisionGroup; + internal HashSet _collisionIgnores; + + public FixtureProxy[] Proxies; + public int ProxyCount; + public Category IgnoreCCDWith; + + /// + /// 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: This can in some cases be called multiple times, as a fixture can have multiple contacts. + /// Note The first fixture argument is always the fixture that the delegate is subscribed to. + /// + public OnSeparationEventHandler OnSeparation; + + internal Fixture() + { + FixtureId = _fixtureIdCounter++; + + _collisionCategories = Settings.DefaultFixtureCollisionCategories; + _collidesWith = Settings.DefaultFixtureCollidesWith; + _collisionGroup = 0; + _collisionIgnores = new HashSet(); + + IgnoreCCDWith = Settings.DefaultFixtureIgnoreCCDWith; + + //Fixture defaults + Friction = 0.2f; + Restitution = 0; + } + + internal Fixture(Body body, Shape shape, object userData = null) + : this() + { +#if DEBUG + if (shape.ShapeType == ShapeType.Polygon) + ((PolygonShape)shape).Vertices.AttachedToBody = true; +#endif + + Body = body; + UserData = userData; + Shape = shape.Clone(); + + 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 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 { return _isSensor; } + set + { + if (Body != null) + Body.Awake = true; + + _isSensor = value; + } + } + + /// + /// 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; } + + /// + /// Set the coefficient of friction. This will _not_ change the friction of + /// existing contacts. + /// + /// The friction. + public float Friction + { + get { return _friction; } + set + { + Debug.Assert(!float.IsNaN(value)); + + _friction = value; + } + } + + /// + /// Set the coefficient of restitution. This will not change the restitution of + /// existing contacts. + /// + /// 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; internal 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.Contains(fixture.FixtureId)) + { + _collisionIgnores.Remove(fixture.FixtureId); + Refilter(); + } + } + + /// + /// Ignores collisions between this fixture and the provided fixture. + /// + /// The fixture. + public void IgnoreCollisionWith(Fixture fixture) + { + if (!_collisionIgnores.Contains(fixture.FixtureId)) + { + _collisionIgnores.Add(fixture.FixtureId); + 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) + { + return _collisionIgnores.Contains(fixture.FixtureId); + } + + /// + /// 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. + /// + private 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.FilterFlag = true; + } + + 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; + + if (Body.Enabled) + { + IBroadPhase broadPhase = Body._world.ContactManager.BroadPhase; + CreateProxies(broadPhase, ref Body._xf); + } + + Body.FixtureList.Add(this); + + // Adjust mass properties if needed. + if (Shape._density > 0.0f) + { + Body.ResetMassData(); + } + + // Let the world know we have a new fixture. This will cause new contacts + // to be created at the beginning of the next time step. + Body._world._worldHasNewFixture = true; + + //FPE: Added event + 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; + } + + internal void Destroy() + { +#if DEBUG + if (Shape.ShapeType == ShapeType.Polygon) + ((PolygonShape)Shape).Vertices.AttachedToBody = false; +#endif + + // The proxies must be destroyed before calling this. + Debug.Assert(ProxyCount == 0); + + // Free the proxy array. + Proxies = null; + Shape = null; + + //FPE: We set the userdata to null here to help prevent bugs related to stale references in GC + UserData = null; + + BeforeCollision = null; + OnCollision = null; + OnSeparation = null; + AfterCollision = null; + + if (Body._world.FixtureRemoved != null) + { + Body._world.FixtureRemoved(this); + } + + Body._world.FixtureAdded = null; + Body._world.FixtureRemoved = null; + OnSeparation = null; + OnCollision = null; + } + + // These support body activation/deactivation. + internal void CreateProxies(IBroadPhase broadPhase, ref Transform xf) + { + Debug.Assert(ProxyCount == 0); + + // 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; + + //FPE note: This line needs to be after the previous two because FixtureProxy is a struct + 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.p - transform1.p; + + broadPhase.MoveProxy(proxy.ProxyId, ref proxy.AABB, displacement); + } + } + + /// + /// Only compares the values of this fixture, and not the attached shape or body. + /// This is used for deduplication in serialization only. + /// + internal bool CompareTo(Fixture fixture) + { + return (_collidesWith == fixture._collidesWith && + _collisionCategories == fixture._collisionCategories && + _collisionGroup == fixture._collisionGroup && + Friction == fixture.Friction && + IsSensor == fixture.IsSensor && + Restitution == fixture.Restitution && + UserData == fixture.UserData && + IgnoreCCDWith == fixture.IgnoreCCDWith && + SequenceEqual(_collisionIgnores, fixture._collisionIgnores)); + } + + private bool SequenceEqual(HashSet first, HashSet second) + { + if (first.Count != second.Count) + return false; + + using (IEnumerator enumerator1 = first.GetEnumerator()) + { + using (IEnumerator enumerator2 = second.GetEnumerator()) + { + while (enumerator1.MoveNext()) + { + if (!enumerator2.MoveNext() || !Equals(enumerator1.Current, enumerator2.Current)) + return false; + } + + if (enumerator2.MoveNext()) + return false; + } + } + + return true; + } + + /// + /// Clones the fixture and attached shape onto the specified body. + /// + /// The body you wish to clone the fixture onto. + /// The cloned fixture. + public Fixture CloneOnto(Body body) + { + Fixture fixture = new Fixture(); + fixture.Body = body; + fixture.Shape = Shape.Clone(); + fixture.UserData = UserData; + fixture.Restitution = Restitution; + fixture.Friction = Friction; + fixture.IsSensor = IsSensor; + fixture._collisionGroup = _collisionGroup; + fixture._collisionCategories = _collisionCategories; + fixture._collidesWith = _collidesWith; + fixture.IgnoreCCDWith = IgnoreCCDWith; + + foreach (int ignore in _collisionIgnores) + { + fixture._collisionIgnores.Add(ignore); + } + + fixture.RegisterFixture(); + return fixture; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/Island.cs b/src/Box2DNet/Dynamics/Island.cs index 5b7cdf6..dc0a488 100644 --- a/src/Box2DNet/Dynamics/Island.cs +++ b/src/Box2DNet/Dynamics/Island.cs @@ -1,522 +1,449 @@ -/* - Box2DNet Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version 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. -*/ - -/* -Position Correction Notes -========================= -I tried the several algorithms for position correction of the 2D revolute joint. -I looked at these systems: -- simple pendulum (1m diameter sphere on massless 5m stick) with initial angular velocity of 100 rad/s. -- suspension bridge with 30 1m long planks of length 1m. -- multi-link chain with 30 1m long links. - -Here are the algorithms: - -Baumgarte - A fraction of the position error is added to the velocity error. There is no -separate position solver. - -Pseudo Velocities - After the velocity solver and position integration, -the position error, Jacobian, and effective mass are recomputed. Then -the velocity constraints are solved with pseudo velocities and a fraction -of the position error is added to the pseudo velocity error. The pseudo -velocities are initialized to zero and there is no warm-starting. After -the position solver, the pseudo velocities are added to the positions. -This is also called the First Order World method or the Position LCP method. - -Modified Nonlinear Gauss-Seidel (NGS) - Like Pseudo Velocities except the -position error is re-computed for each constraint and the positions are updated -after the constraint is solved. The radius vectors (aka Jacobians) are -re-computed too (otherwise the algorithm has horrible instability). The pseudo -velocity states are not needed because they are effectively zero at the beginning -of each iteration. Since we have the current position error, we allow the -iterations to terminate early if the error becomes smaller than b2_linearSlop. - -Full NGS or just NGS - Like Modified NGS except the effective mass are re-computed -each time a constraint is solved. - -Here are the results: -Baumgarte - this is the cheapest algorithm but it has some stability problems, -especially with the bridge. The chain links separate easily close to the root -and they jitter as they struggle to pull together. This is one of the most common -methods in the field. The big drawback is that the position correction artificially -affects the momentum, thus leading to instabilities and false bounce. I used a -bias factor of 0.2. A larger bias factor makes the bridge less stable, a smaller -factor makes joints and contacts more spongy. - -Pseudo Velocities - the is more stable than the Baumgarte method. The bridge is -stable. However, joints still separate with large angular velocities. Drag the -simple pendulum in a circle quickly and the joint will separate. The chain separates -easily and does not recover. I used a bias factor of 0.2. A larger value lead to -the bridge collapsing when a heavy cube drops on it. - -Modified NGS - this algorithm is better in some ways than Baumgarte and Pseudo -Velocities, but in other ways it is worse. The bridge and chain are much more -stable, but the simple pendulum goes unstable at high angular velocities. - -Full NGS - stable in all tests. The joints display good stiffness. The bridge -still sags, but this is better than infinite forces. - -Recommendations -Pseudo Velocities are not really worthwhile because the bridge and chain cannot -recover from joint separation. In other cases the benefit over Baumgarte is small. - -Modified NGS is not a robust method for the revolute joint due to the violent -instability seen in the simple pendulum. Perhaps it is viable with other constraint -types, especially scalar constraints where the effective mass is a scalar. - -This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable instabilities -and is very fast. I don't think we can escape Baumgarte, especially in highly -demanding cases where high constraint fidelity is not needed. - -Full NGS is robust and easy on the eyes. I recommend this as an option for -higher fidelity simulation and certainly for suspension bridges and long chains. -Full NGS might be a good choice for ragdolls, especially motorized ragdolls where -joint separation can be problematic. The number of NGS iterations can be reduced -for better performance without harming robustness much. - -Each joint in a can be handled differently in the position solver. So I recommend -a system where the user can select the algorithm on a per joint basis. I would -probably default to the slower Full NGS and let the user select the faster -Baumgarte method in performance critical scenarios. -*/ - -/* -Cache Performance - -The Box2D solvers are dominated by cache misses. Data structures are designed -to increase the number of cache hits. Much of misses are due to random access -to body data. The constraint structures are iterated over linearly, which leads -to few cache misses. - -The _bodies are not accessed during iteration. Instead read only data, such as -the mass values are stored with the constraints. The mutable data are the constraint -impulses and the _bodies velocities/positions. The impulses are held inside the -constraint structures. The body velocities/positions are held in compact, temporary -arrays to increase the number of cache hits. Linear and angular velocity are -stored in a single array since multiple arrays lead to multiple misses. -*/ - -/* -2D Rotation - -R = [cos(theta) -sin(theta)] - [sin(theta) cos(theta) ] - -thetaDot = omega - -Let q1 = cos(theta), q2 = sin(theta). -R = [q1 -q2] - [q2 q1] - -q1Dot = -thetaDot * q2 -q2Dot = thetaDot * q1 - -q1_new = q1_old - dt * w * q2 -q2_new = q2_old + dt * w * q1 -then normalize. - -This might be faster than computing sin+cos. -However, we can compute sin+cos of the same angle fast. -*/ - -using System; using System.Numerics; -using System.Collections.Generic; -using System.Text; - -using Box2DNet.Common; -using Box2DNet.Collision; - - - -namespace Box2DNet.Dynamics -{ - public struct Position - { - public Vector2 x; - public float a; - } - - public struct Velocity - { - public Vector2 v; - public float w; - } - - public class Island : IDisposable - { - public ContactListener _listener; - - public Body[] _bodies; - public Contact[] _contacts; - public Joint[] _joints; - - public Position[] _positions; - public Velocity[] _velocities; - - public int _bodyCount; - public int _jointCount; - public int _contactCount; - - public int _bodyCapacity; - public int _contactCapacity; - public int _jointCapacity; - - public int _positionIterationCount; - - public Island(int bodyCapacity, int contactCapacity, int jointCapacity, ContactListener listener) - { - _bodyCapacity = bodyCapacity; - _contactCapacity = contactCapacity; - _jointCapacity = jointCapacity; - //__bodyCount = 0; - //_contactCount = 0; - //_jointCount = 0; - - _listener = listener; - - _bodies = new Body[bodyCapacity]; - _contacts = new Contact[contactCapacity]; - _joints = new Joint[jointCapacity]; - - _velocities = new Velocity[_bodyCapacity]; - _positions = new Position[_bodyCapacity]; - } - - public void Dispose() - { - // Warning: the order should reverse the constructor order. - _positions = null; - _velocities = null; - _joints = null; - _contacts = null; - _bodies = null; - } - - public void Clear() - { - _bodyCount = 0; - _contactCount = 0; - _jointCount = 0; - } - - public void Solve(TimeStep step, Vector2 gravity, bool allowSleep) - { - // Integrate velocities and apply damping. - for (int i = 0; i < _bodyCount; ++i) - { - Body b = _bodies[i]; - - if (b.IsStatic()) - continue; - - // Integrate velocities. - b._linearVelocity += step.Dt * (gravity + b._invMass * b._force); - b._angularVelocity += step.Dt * b._invI * b._torque; - - // Reset forces. - b._force = Vector2.Zero; - b._torque = 0.0f; - - // 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._linearVelocity *= Box2DNet.Common.Math.Clamp(1.0f - step.Dt * b._linearDamping, 0.0f, 1.0f); - b._angularVelocity *= Box2DNet.Common.Math.Clamp(1.0f - step.Dt * b._angularDamping, 0.0f, 1.0f); - } - - ContactSolver contactSolver = new ContactSolver(step, _contacts, _contactCount); - - // Initialize velocity constraints. - contactSolver.InitVelocityConstraints(step); - - for (int i = 0; i < _jointCount; ++i) - { - _joints[i].InitVelocityConstraints(step); - } - - // Solve velocity constraints. - for (int i = 0; i < step.VelocityIterations; ++i) - { - for (int j = 0; j < _jointCount; ++j) - { - _joints[j].SolveVelocityConstraints(step); - } - contactSolver.SolveVelocityConstraints(); - } - - // Post-solve (store impulses for warm starting). - contactSolver.FinalizeVelocityConstraints(); - - // Integrate positions. - for (int i = 0; i < _bodyCount; ++i) - { - Body b = _bodies[i]; - - if (b.IsStatic()) - continue; - - // Check for large velocities. - Vector2 translation = step.Dt * b._linearVelocity; - if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared) - { - b._linearVelocity = (Settings.MaxTranslation * step.Inv_Dt) * translation.Normalized(); - } - - float rotation = step.Dt * b._angularVelocity; - if (rotation * rotation > Settings.MaxRotationSquared) - { - if (rotation < 0.0) - { - b._angularVelocity = -step.Inv_Dt * Settings.MaxRotation; - } - else - { - b._angularVelocity = step.Inv_Dt * Settings.MaxRotation; - } - } - - // Store positions for continuous collision. - b._sweep.C0 = b._sweep.C; - b._sweep.A0 = b._sweep.A; - - // Integrate - b._sweep.C += step.Dt * b._linearVelocity; - b._sweep.A += step.Dt * b._angularVelocity; - - // Compute new Transform - b.SynchronizeTransform(); - - // Note: shapes are synchronized later. - } - - // Iterate over constraints. - for (int i = 0; i < step.PositionIterations; ++i) - { - bool contactsOkay = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte); - - bool jointsOkay = true; - for (int j = 0; j < _jointCount; ++j) - { - bool jointOkay = _joints[j].SolvePositionConstraints(Settings.ContactBaumgarte); - jointsOkay = jointsOkay && jointOkay; - } - - if (contactsOkay && jointsOkay) - { - // Exit early if the position errors are small. - break; - } - } - - Report(contactSolver._constraints); - - if (allowSleep) - { - float minSleepTime = Settings.FLT_MAX; - -#if !TARGET_FLOAT32_IS_FIXED - float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance; - float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance; -#endif - - for (int i = 0; i < _bodyCount; ++i) - { - Body b = _bodies[i]; - if (b._invMass == 0.0f) - { - continue; - } - - if ((b._flags & Body.BodyFlags.AllowSleep) == 0) - { - b._sleepTime = 0.0f; - minSleepTime = 0.0f; - } - - if ((b._flags & Body.BodyFlags.AllowSleep) == 0 || -#if TARGET_FLOAT32_IS_FIXED - Common.Math.Abs(b._angularVelocity) > Settings.AngularSleepTolerance || - Common.Math.Abs(b._linearVelocity.X) > Settings.LinearSleepTolerance || - Common.Math.Abs(b._linearVelocity.Y) > Settings.LinearSleepTolerance) -#else - b._angularVelocity * b._angularVelocity > angTolSqr || - Vector2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr) -#endif - { - b._sleepTime = 0.0f; - minSleepTime = 0.0f; - } - else - { - b._sleepTime += step.Dt; - minSleepTime = Common.Math.Min(minSleepTime, b._sleepTime); - } - } - - if (minSleepTime >= Settings.TimeToSleep) - { - for (int i = 0; i < _bodyCount; ++i) - { - Body b = _bodies[i]; - b._flags |= Body.BodyFlags.Sleep; - b._linearVelocity = Vector2.Zero; - b._angularVelocity = 0.0f; - } - } - } - } - - public void SolveTOI(ref TimeStep subStep) - { - ContactSolver contactSolver = new ContactSolver(subStep, _contacts, _contactCount); - - // No warm starting is needed for TOI events because warm - // starting impulses were applied in the discrete solver. - - // Warm starting for joints is off for now, but we need to - // call this function to compute Jacobians. - for (int i = 0; i < _jointCount; ++i) - { - _joints[i].InitVelocityConstraints(subStep); - } - - // Solve velocity constraints. - for (int i = 0; i < subStep.VelocityIterations; ++i) - { - contactSolver.SolveVelocityConstraints(); - for (int j = 0; j < _jointCount; ++j) - { - _joints[j].SolveVelocityConstraints(subStep); - } - } - - // 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.IsStatic()) - continue; - - // Check for large velocities. - Vector2 translation = subStep.Dt * b._linearVelocity; - if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared) - { - b._linearVelocity = (Settings.MaxTranslation * subStep.Inv_Dt) * translation.Normalized(); - } - - float rotation = subStep.Dt * b._angularVelocity; - if (rotation * rotation > Settings.MaxRotationSquared) - { - if (rotation < 0.0) - { - b._angularVelocity = -subStep.Inv_Dt * Settings.MaxRotation; - } - else - { - b._angularVelocity = subStep.Inv_Dt * Settings.MaxRotation; - } - } - - // Store positions for continuous collision. - b._sweep.C0 = b._sweep.C; - b._sweep.A0 = b._sweep.A; - - // Integrate - b._sweep.C += subStep.Dt * b._linearVelocity; - b._sweep.A += subStep.Dt * b._angularVelocity; - - // Compute new Transform - b.SynchronizeTransform(); - - // Note: shapes are synchronized later. - } - - // Solve position constraints. - const float k_toiBaumgarte = 0.75f; - for (int i = 0; i < subStep.PositionIterations; ++i) - { - bool contactsOkay = contactSolver.SolvePositionConstraints(k_toiBaumgarte); - bool jointsOkay = true; - for (int j = 0; j < _jointCount; ++j) - { - bool jointOkay = _joints[j].SolvePositionConstraints(k_toiBaumgarte); - jointsOkay = jointsOkay && jointOkay; - } - - if (contactsOkay && jointsOkay) - { - break; - } - } - - Report(contactSolver._constraints); - } - - public void Add(Body body) - { - Box2DNetDebug.Assert(_bodyCount < _bodyCapacity); - body._islandIndex = _bodyCount; - _bodies[_bodyCount++] = body; - } - - public void Add(Contact contact) - { - Box2DNetDebug.Assert(_contactCount < _contactCapacity); - _contacts[_contactCount++] = contact; - } - - public void Add(Joint joint) - { - Box2DNetDebug.Assert(_jointCount < _jointCapacity); - _joints[_jointCount++] = joint; - } - - public void Report(ContactConstraint[] constraints) - { - if (_listener == null) - { - return; - } - - for (int i = 0; i < _contactCount; ++i) - { - Contact c = _contacts[i]; - ContactConstraint cc = constraints[i]; - ContactImpulse impulse = new ContactImpulse(); - for (int j = 0; j < cc.PointCount; ++j) - { - impulse.normalImpulses[j] = cc.Points[j].NormalImpulse; - impulse.tangentImpulses[j] = cc.Points[j].TangentImpulse; - } - - _listener.PostSolve(c, impulse); - } - } - } +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using Box2DNet.Common; +using Box2DNet.Dynamics.Contacts; +using Box2DNet.Dynamics.Joints; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Dynamics +{ + /// + /// This is an internal class. + /// + public class Island + { + private ContactManager _contactManager; + private ContactSolver _contactSolver = new ContactSolver(); + private Contact[] _contacts; + private Joint[] _joints; + + private const float LinTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance; + private const float AngTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance; + private Stopwatch _watch = new Stopwatch(); + + public Body[] Bodies; + public int BodyCount; + public int ContactCount; + public int JointCount; + + public Velocity[] _velocities; + public Position[] _positions; + + public int BodyCapacity; + public int ContactCapacity; + public int JointCapacity; + public float JointUpdateTime; + + 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]; + _velocities = new Velocity[bodyCapacity]; + _positions = new Position[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; + } + + public void Solve(ref TimeStep step, ref Vector2 gravity) + { + float h = step.dt; + + // Integrate velocities and apply damping. Initialize the body state. + for (int i = 0; i < BodyCount; ++i) + { + Body b = Bodies[i]; + + Vector2 c = b._sweep.C; + float a = b._sweep.A; + Vector2 v = b._linearVelocity; + float w = b._angularVelocity; + + // Store positions for continuous collision. + b._sweep.C0 = b._sweep.C; + b._sweep.A0 = b._sweep.A; + + if (b.BodyType == BodyType.Dynamic) + { + // Integrate velocities. + + // FPE: Only apply gravity if the body wants it. + if (b.IgnoreGravity) + v += h * (b._invMass * b._force); + else + v += h * (b.GravityScale * gravity + b._invMass * b._force); + + w += h * 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 + v *= MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f); + w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f); + } + + _positions[i].c = c; + _positions[i].a = a; + _velocities[i].v = v; + _velocities[i].w = w; + } + + // Solver data + SolverData solverData = new SolverData(); + solverData.step = step; + solverData.positions = _positions; + solverData.velocities = _velocities; + + _contactSolver.Reset(step, ContactCount, _contacts, _positions, _velocities); + _contactSolver.InitializeVelocityConstraints(); + + if (Settings.EnableWarmstarting) + { + _contactSolver.WarmStart(); + } + + if (Settings.EnableDiagnostics) + _watch.Start(); + + for (int i = 0; i < JointCount; ++i) + { + if (_joints[i].Enabled) + _joints[i].InitVelocityConstraints(ref solverData); + } + + if (Settings.EnableDiagnostics) + _watch.Stop(); + + // Solve velocity constraints. + for (int i = 0; i < Settings.VelocityIterations; ++i) + { + for (int j = 0; j < JointCount; ++j) + { + Joint joint = _joints[j]; + + if (!joint.Enabled) + continue; + + if (Settings.EnableDiagnostics) + _watch.Start(); + + joint.SolveVelocityConstraints(ref solverData); + joint.Validate(step.inv_dt); + + if (Settings.EnableDiagnostics) + _watch.Stop(); + } + + _contactSolver.SolveVelocityConstraints(); + } + + // Store impulses for warm starting. + _contactSolver.StoreImpulses(); + + // Integrate positions + for (int i = 0; i < BodyCount; ++i) + { + Vector2 c = _positions[i].c; + float a = _positions[i].a; + Vector2 v = _velocities[i].v; + float w = _velocities[i].w; + + // Check for large velocities + Vector2 translation = h * v; + if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared) + { + float ratio = Settings.MaxTranslation / translation.Length(); + v *= ratio; + } + + float rotation = h * w; + if (rotation * rotation > Settings.MaxRotationSquared) + { + float ratio = Settings.MaxRotation / Math.Abs(rotation); + w *= ratio; + } + + // Integrate + c += h * v; + a += h * w; + + _positions[i].c = c; + _positions[i].a = a; + _velocities[i].v = v; + _velocities[i].w = w; + } + + + // Solve position constraints + bool positionSolved = false; + for (int i = 0; i < Settings.PositionIterations; ++i) + { + bool contactsOkay = _contactSolver.SolvePositionConstraints(); + + bool jointsOkay = true; + for (int j = 0; j < JointCount; ++j) + { + Joint joint = _joints[j]; + + if (!joint.Enabled) + continue; + + if (Settings.EnableDiagnostics) + _watch.Start(); + + bool jointOkay = joint.SolvePositionConstraints(ref solverData); + + if (Settings.EnableDiagnostics) + _watch.Stop(); + + jointsOkay = jointsOkay && jointOkay; + } + + if (contactsOkay && jointsOkay) + { + // Exit early if the position errors are small. + positionSolved = true; + break; + } + } + + if (Settings.EnableDiagnostics) + { + JointUpdateTime = _watch.ElapsedTicks; + _watch.Reset(); + } + + // Copy state buffers back to the bodies + for (int i = 0; i < BodyCount; ++i) + { + Body body = Bodies[i]; + body._sweep.C = _positions[i].c; + body._sweep.A = _positions[i].a; + body._linearVelocity = _velocities[i].v; + body._angularVelocity = _velocities[i].w; + body.SynchronizeTransform(); + } + + Report(_contactSolver._velocityConstraints); + + 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.SleepingAllowed || b._angularVelocity * b._angularVelocity > AngTolSqr || Vector2.Dot(b._linearVelocity, b._linearVelocity) > LinTolSqr) + { + b._sleepTime = 0.0f; + minSleepTime = 0.0f; + } + else + { + b._sleepTime += h; + minSleepTime = Math.Min(minSleepTime, b._sleepTime); + } + } + + if (minSleepTime >= Settings.TimeToSleep && positionSolved) + { + for (int i = 0; i < BodyCount; ++i) + { + Body b = Bodies[i]; + b.Awake = false; + } + } + } + } + + internal void SolveTOI(ref TimeStep subStep, int toiIndexA, int toiIndexB, bool warmstarting) + { + Debug.Assert(toiIndexA < BodyCount); + Debug.Assert(toiIndexB < BodyCount); + + // Initialize the body state. + for (int i = 0; i < BodyCount; ++i) + { + Body b = Bodies[i]; + _positions[i].c = b._sweep.C; + _positions[i].a = b._sweep.A; + _velocities[i].v = b._linearVelocity; + _velocities[i].w = b._angularVelocity; + } + + _contactSolver.Reset(subStep, ContactCount, _contacts, _positions, _velocities, warmstarting); + + // Solve position constraints. + for (int i = 0; i < Settings.TOIPositionIterations; ++i) + { + bool contactsOkay = _contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB); + if (contactsOkay) + { + break; + } + } + + // Leap of faith to new safe state. + Bodies[toiIndexA]._sweep.C0 = _positions[toiIndexA].c; + Bodies[toiIndexA]._sweep.A0 = _positions[toiIndexA].a; + Bodies[toiIndexB]._sweep.C0 = _positions[toiIndexB].c; + Bodies[toiIndexB]._sweep.A0 = _positions[toiIndexB].a; + + // 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. + + float h = subStep.dt; + + // Integrate positions. + for (int i = 0; i < BodyCount; ++i) + { + Vector2 c = _positions[i].c; + float a = _positions[i].a; + Vector2 v = _velocities[i].v; + float w = _velocities[i].w; + + // Check for large velocities + Vector2 translation = h * v; + if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared) + { + float ratio = Settings.MaxTranslation / translation.Length(); + v *= ratio; + } + + float rotation = h * w; + if (rotation * rotation > Settings.MaxRotationSquared) + { + float ratio = Settings.MaxRotation / Math.Abs(rotation); + w *= ratio; + } + + // Integrate + c += h * v; + a += h * w; + + _positions[i].c = c; + _positions[i].a = a; + _velocities[i].v = v; + _velocities[i].w = w; + + // Sync bodies + Body body = Bodies[i]; + body._sweep.C = c; + body._sweep.A = a; + body._linearVelocity = v; + body._angularVelocity = w; + body.SynchronizeTransform(); + } + + Report(_contactSolver._velocityConstraints); + } + + public void Add(Body body) + { + Debug.Assert(BodyCount < BodyCapacity); + body.IslandIndex = BodyCount; + 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(ContactVelocityConstraint[] constraints) + { + if (_contactManager == null) + return; + + for (int i = 0; i < ContactCount; ++i) + { + Contact c = _contacts[i]; + + //FPE optimization: We don't store the impulses and send it to the delegate. We just send the whole contact. + //FPE feature: added after collision + if (c.FixtureA.AfterCollision != null) + c.FixtureA.AfterCollision(c.FixtureA, c.FixtureB, c, constraints[i]); + + if (c.FixtureB.AfterCollision != null) + c.FixtureB.AfterCollision(c.FixtureB, c.FixtureA, c, constraints[i]); + + if (_contactManager.PostSolve != null) + { + _contactManager.PostSolve(c, constraints[i]); + } + } + } + } } \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/Joints/AngleJoint.cs b/src/Box2DNet/Dynamics/Joints/AngleJoint.cs new file mode 100644 index 0000000..b27e9fd --- /dev/null +++ b/src/Box2DNet/Dynamics/Joints/AngleJoint.cs @@ -0,0 +1,128 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +*/ + +using System; +using System.Diagnostics; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Dynamics.Joints +{ + /// + /// Maintains a fixed angle between two bodies + /// + public class AngleJoint : Joint + { + private float _bias; + private float _jointError; + private float _massFactor; + private float _targetAngle; + + internal AngleJoint() + { + JointType = JointType.Angle; + } + + /// + /// Constructor for AngleJoint + /// + /// The first body + /// The second body + public AngleJoint(Body bodyA, Body bodyB) + : base(bodyA, bodyB) + { + JointType = JointType.Angle; + BiasFactor = .2f; + MaxImpulse = float.MaxValue; + } + + public override Vector2 WorldAnchorA + { + get { return BodyA.Position; } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.Position; } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + /// + /// The desired angle between BodyA and BodyB + /// + public float TargetAngle + { + get { return _targetAngle; } + set + { + if (value != _targetAngle) + { + _targetAngle = value; + WakeBodies(); + } + } + } + + /// + /// Gets or sets the bias factor. + /// Defaults to 0.2 + /// + public float BiasFactor { get; set; } + + /// + /// Gets or sets the maximum impulse + /// Defaults to float.MaxValue + /// + public float MaxImpulse { get; set; } + + /// + /// Gets or sets the softness of the joint + /// Defaults to 0 + /// + public float Softness { get; set; } + + public override Vector2 GetReactionForce(float invDt) + { + //TODO + //return _inv_dt * _impulse; + return Vector2.Zero; + } + + public override float GetReactionTorque(float invDt) + { + return 0; + } + + internal override void InitVelocityConstraints(ref SolverData data) + { + int indexA = BodyA.IslandIndex; + int indexB = BodyB.IslandIndex; + + float aW = data.positions[indexA].a; + float bW = data.positions[indexB].a; + + _jointError = (bW - aW - TargetAngle); + _bias = -BiasFactor * data.step.inv_dt * _jointError; + _massFactor = (1 - Softness) / (BodyA._invI + BodyB._invI); + } + + internal override void SolveVelocityConstraints(ref SolverData data) + { + int indexA = BodyA.IslandIndex; + int indexB = BodyB.IslandIndex; + + float p = (_bias - data.velocities[indexB].w + data.velocities[indexA].w) * _massFactor; + + data.velocities[indexA].w -= BodyA._invI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse); + data.velocities[indexB].w += BodyB._invI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse); + } + + internal override bool SolvePositionConstraints(ref SolverData data) + { + //no position solving for this joint + return true; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/Joints/DistanceJoint.cs b/src/Box2DNet/Dynamics/Joints/DistanceJoint.cs index 133b731..c930b3e 100644 --- a/src/Box2DNet/Dynamics/Joints/DistanceJoint.cs +++ b/src/Box2DNet/Dynamics/Joints/DistanceJoint.cs @@ -1,279 +1,331 @@ -/* - Box2DNet Copyright (c) 2018 codeyu https://github.com/codeyu/Box2DNet - Box2D original C++ version Copyright (c) 2006-2007 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. -*/ - -// 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 - -using System; using System.Numerics; -using System.Collections.Generic; -using System.Text; - -using Box2DNet.Common; - - -namespace Box2DNet.Dynamics -{ - /// - /// Distance joint definition. This requires defining an - /// anchor point on both bodies and the non-zero length of the - /// distance joint. The definition uses local anchor points - /// 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. - /// - public class DistanceJointDef : JointDef - { - public DistanceJointDef() - { - Type = JointType.DistanceJoint; - LocalAnchor1 = Vector2.Zero; - LocalAnchor2 = Vector2.Zero; - Length = 1.0f; - FrequencyHz = 0.0f; - DampingRatio = 0.0f; - } - - /// - /// Initialize the bodies, anchors, and length using the world anchors. - /// - public void Initialize(Body body1, Body body2, Vector2 anchor1, Vector2 anchor2) - { - Body1 = body1; - Body2 = body2; - LocalAnchor1 = body1.GetLocalPoint(anchor1); - LocalAnchor2 = body2.GetLocalPoint(anchor2); - var d = anchor2 - anchor1; - Length = d.Length(); - } - - /// - /// The local anchor point relative to body1's origin. - /// - public Vector2 LocalAnchor1; - - /// - /// The local anchor point relative to body2's origin. - /// - public Vector2 LocalAnchor2; - - /// - /// The equilibrium length between the anchor points. - /// - public float Length; - - /// - /// The response speed. - /// - public float FrequencyHz; - - /// - /// The damping ratio. 0 = no damping, 1 = critical damping. - /// - public float DampingRatio; - } - - /// - /// A distance joint constrains 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 - { - public Vector2 _localAnchor1; - public Vector2 _localAnchor2; - public Vector2 _u; - public float _frequencyHz; - public float _dampingRatio; - public float _gamma; - public float _bias; - public float _impulse; - public float _mass; // effective mass for the constraint. - public float _length; - - public override Vector2 Anchor1 - { - get { return _body1.GetWorldPoint(_localAnchor1);} - } - - public override Vector2 Anchor2 - { - get { return _body2.GetWorldPoint(_localAnchor2);} - } - - public override Vector2 GetReactionForce(float inv_dt) - { - return (inv_dt * _impulse) * _u; - } - - public override float GetReactionTorque(float inv_dt) - { - return 0.0f; - } - - public DistanceJoint(DistanceJointDef def) - : base(def) - { - _localAnchor1 = def.LocalAnchor1; - _localAnchor2 = def.LocalAnchor2; - _length = def.Length; - _frequencyHz = def.FrequencyHz; - _dampingRatio = def.DampingRatio; - _impulse = 0.0f; - _gamma = 0.0f; - _bias = 0.0f; - } - - internal override void InitVelocityConstraints(TimeStep step) - { - Body b1 = _body1; - Body b2 = _body2; - - // Compute the effective mass matrix. - Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter()); - Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter()); - _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 = r1.Cross(_u); - float cr2u = r2.Cross(_u); - float invMass = b1._invMass + b1._invI * cr1u * cr1u + b2._invMass + b2._invI * cr2u * cr2u; - Box2DNetDebug.Assert(invMass > Settings.FLT_EPSILON); - _mass = 1.0f / invMass; - - if (_frequencyHz > 0.0f) - { - float C = length - _length; - - // Frequency - float omega = 2.0f * Settings.Pi * _frequencyHz; - - // Damping coefficient - float d = 2.0f * _mass * _dampingRatio * omega; - - // Spring stiffness - float k = _mass * omega * omega; - - // magic formulas - _gamma = 1.0f / (step.Dt * (d + step.Dt * k)); - _bias = C * step.Dt * k * _gamma; - - _mass = 1.0f / (invMass + _gamma); - } - - if (step.WarmStarting) - { - //Scale the inpulse to support a variable timestep. - _impulse *= step.DtRatio; - Vector2 P = _impulse * _u; - b1._linearVelocity -= b1._invMass * P; - b1._angularVelocity -= b1._invI * r1.Cross(P); - b2._linearVelocity += b2._invMass * P; - b2._angularVelocity += b2._invI * r2.Cross(P); - } - else - { - _impulse = 0.0f; - } - } - - internal override bool SolvePositionConstraints(float baumgarte) - { - if (_frequencyHz > 0.0f) - { - //There is no possition correction for soft distace constraint. - return true; - } - - Body b1 = _body1; - Body b2 = _body2; - - Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter()); - Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter()); - - Vector2 d = b2._sweep.C + r2 - b1._sweep.C - r1; - - var length = d.Length(); - d.Normalize(); - var C = length - _length; - C = Box2DNet.Common.Math.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); - - var impulse = -_mass * C; - _u = d; - var P = impulse * _u; - - b1._sweep.C -= b1._invMass * P; - b1._sweep.A -= b1._invI * r1.Cross(P); - b2._sweep.C += b2._invMass * P; - b2._sweep.A += b2._invI * r2.Cross(P); - - b1.SynchronizeTransform(); - b2.SynchronizeTransform(); - - return System.Math.Abs(C) < Settings.LinearSlop; - } - - internal override void SolveVelocityConstraints(TimeStep step) - { - //B2_NOT_USED(step); - - var b1 = _body1; - var b2 = _body2; - - var r1 = b1.GetTransform().TransformDirection( _localAnchor1 - b1.GetLocalCenter()); - var r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter()); - - // Cdot = dot(u, v + cross(w, r)) - var v1 = b1._linearVelocity + r1.CrossScalarPreMultiply(b1._angularVelocity); - var v2 = b2._linearVelocity + r2.CrossScalarPreMultiply(b2._angularVelocity); - var cdot = Vector2.Dot(_u, v2 - v1); - var impulse = -_mass * (cdot + _bias + _gamma * _impulse); - _impulse += impulse; - - var p = impulse * _u; - b1._linearVelocity -= b1._invMass * p; - b1._angularVelocity -= b1._invI * r1.Cross(p); - b2._linearVelocity += b2._invMass * p; - b2._angularVelocity += b2._invI * r2.Cross(p); - } - } -} +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using Box2DNet.Common; +using Microsoft.Xna.Framework; + +namespace Box2DNet.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 + { + // Solver shared + private float _bias; + private float _gamma; + private float _impulse; + + // Solver temp + private int _indexA; + private int _indexB; + private Vector2 _u; + private Vector2 _rA; + private Vector2 _rB; + private Vector2 _localCenterA; + private Vector2 _localCenterB; + private float _invMassA; + private float _invMassB; + private float _invIA; + private float _invIB; + private float _mass; + + 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 + /// Set to true if you are using world coordinates as anchors. + public DistanceJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false) + : base(bodyA, bodyB) + { + JointType = JointType.Distance; + + if (useWorldCoordinates) + { + LocalAnchorA = bodyA.GetLocalPoint(ref anchorA); + LocalAnchorB = bodyB.GetLocalPoint(ref anchorB); + Length = (anchorB - anchorA).Length(); + } + else + { + LocalAnchorA = anchorA; + LocalAnchorB = anchorB; + Length = (BodyB.GetWorldPoint(ref anchorB) - BodyA.GetWorldPoint(ref anchorA)).Length(); + } + } + + /// + /// The local anchor point relative to bodyA's origin. + /// + public Vector2 LocalAnchorA { get; set; } + + /// + /// The local anchor point relative to bodyB's origin. + /// + public Vector2 LocalAnchorB { get; set; } + + public override sealed Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + 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."); } + } + + /// + /// 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. A value of 0 + /// disables softness. + /// + public float Frequency { get; set; } + + /// + /// The damping ratio. 0 = no damping, 1 = critical damping. + /// + public float DampingRatio { get; set; } + + /// + /// Get the reaction force given the inverse time step. Unit is N. + /// + /// + /// + public override Vector2 GetReactionForce(float invDt) + { + Vector2 F = (invDt * _impulse) * _u; + return F; + } + + /// + /// Get the reaction torque given the inverse time step. + /// Unit is N*m. This is always zero for a distance joint. + /// + /// + /// + public override float GetReactionTorque(float invDt) + { + return 0.0f; + } + + internal override void InitVelocityConstraints(ref SolverData data) + { + _indexA = BodyA.IslandIndex; + _indexB = BodyB.IslandIndex; + _localCenterA = BodyA._sweep.LocalCenter; + _localCenterB = BodyB._sweep.LocalCenter; + _invMassA = BodyA._invMass; + _invMassB = BodyB._invMass; + _invIA = BodyA._invI; + _invIB = BodyB._invI; + + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + _u = cB + _rB - cA - _rA; + + // Handle singularity. + float length = _u.Length(); + if (length > Settings.LinearSlop) + { + _u *= 1.0f / length; + } + else + { + _u = Vector2.Zero; + } + + float crAu = MathUtils.Cross(_rA, _u); + float crBu = MathUtils.Cross(_rB, _u); + float invMass = _invMassA + _invIA * crAu * crAu + _invMassB + _invIB * crBu * crBu; + + // Compute the effective mass matrix. + _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 + float h = data.step.dt; + _gamma = h * (d + h * k); + _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f; + _bias = C * h * k * _gamma; + + invMass += _gamma; + _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; + } + else + { + _gamma = 0.0f; + _bias = 0.0f; + } + + if (Settings.EnableWarmstarting) + { + // Scale the impulse to support a variable time step. + _impulse *= data.step.dtRatio; + + Vector2 P = _impulse * _u; + vA -= _invMassA * P; + wA -= _invIA * MathUtils.Cross(_rA, P); + vB += _invMassB * P; + wB += _invIB * MathUtils.Cross(_rB, P); + } + else + { + _impulse = 0.0f; + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override void SolveVelocityConstraints(ref SolverData data) + { + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + // Cdot = dot(u, v + cross(w, r)) + Vector2 vpA = vA + MathUtils.Cross(wA, _rA); + Vector2 vpB = vB + MathUtils.Cross(wB, _rB); + float Cdot = Vector2.Dot(_u, vpB - vpA); + + float impulse = -_mass * (Cdot + _bias + _gamma * _impulse); + _impulse += impulse; + + Vector2 P = impulse * _u; + vA -= _invMassA * P; + wA -= _invIA * MathUtils.Cross(_rA, P); + vB += _invMassB * P; + wB += _invIB * MathUtils.Cross(_rB, P); + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + + } + + internal override bool SolvePositionConstraints(ref SolverData data) + { + if (Frequency > 0.0f) + { + // There is no position correction for soft distance constraints. + return true; + } + + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + Vector2 u = cB + rB - cA - rA; + + float length = u.Length(); u.Normalize(); + float C = length - Length; + C = MathUtils.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); + + float impulse = -_mass * C; + Vector2 P = impulse * u; + + cA -= _invMassA * P; + aA -= _invIA * MathUtils.Cross(rA, P); + cB += _invMassB * P; + aB += _invIB * MathUtils.Cross(rB, P); + + data.positions[_indexA].c = cA; + data.positions[_indexA].a = aA; + data.positions[_indexB].c = cB; + data.positions[_indexB].a = aB; + + return Math.Abs(C) < Settings.LinearSlop; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/Joints/FixedMouseJoint.cs b/src/Box2DNet/Dynamics/Joints/FixedMouseJoint.cs new file mode 100644 index 0000000..289338b --- /dev/null +++ b/src/Box2DNet/Dynamics/Joints/FixedMouseJoint.cs @@ -0,0 +1,261 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System.Diagnostics; +using Box2DNet.Common; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Dynamics.Joints +{ + // p = attached point, m = mouse point + // C = p - m + // Cdot = v + // = v + cross(w, r) + // J = [I r_skew] + // Identity used: + // w k % (rx i + ry j) = w * (-ry i + rx j) + + /// + /// 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 + { + private Vector2 _worldAnchor; + private float _frequency; + private float _dampingRatio; + private float _beta; + + // Solver shared + private Vector2 _impulse; + private float _maxForce; + private float _gamma; + + // Solver temp + private int _indexA; + private Vector2 _rA; + private Vector2 _localCenterA; + private float _invMassA; + private float _invIA; + private Mat22 _mass; + private Vector2 _C; + + /// + /// 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; + MaxForce = 1000 * body.Mass; + + Debug.Assert(worldAnchor.IsValid()); + + _worldAnchor = worldAnchor; + LocalAnchorA = MathUtils.MulT(BodyA._xf, worldAnchor); + } + + /// + /// The local anchor point on BodyA + /// + public Vector2 LocalAnchorA { get; set; } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + set { LocalAnchorA = BodyA.GetLocalPoint(value); } + } + + public override Vector2 WorldAnchorB + { + get { return _worldAnchor; } + set + { + WakeBodies(); + _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 { return _maxForce; } + set + { + Debug.Assert(MathUtils.IsValid(value) && value >= 0.0f); + _maxForce = value; + } + } + + /// + /// The response speed. + /// + public float Frequency + { + get { return _frequency; } + set + { + Debug.Assert(MathUtils.IsValid(value) && value >= 0.0f); + _frequency = value; + } + } + + /// + /// The damping ratio. 0 = no damping, 1 = critical damping. + /// + public float DampingRatio + { + get { return _dampingRatio; } + set + { + Debug.Assert(MathUtils.IsValid(value) && value >= 0.0f); + _dampingRatio = value; + } + } + + public override Vector2 GetReactionForce(float invDt) + { + return invDt * _impulse; + } + + public override float GetReactionTorque(float invDt) + { + return invDt * 0.0f; + } + + internal override void InitVelocityConstraints(ref SolverData data) + { + _indexA = BodyA.IslandIndex; + _localCenterA = BodyA._sweep.LocalCenter; + _invMassA = BodyA._invMass; + _invIA = BodyA._invI; + + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + + Rot qA = new Rot(aA); + + float mass = BodyA.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. + float h = data.step.dt; + Debug.Assert(d + h * k > Settings.Epsilon); + _gamma = h * (d + h * k); + if (_gamma != 0.0f) + { + _gamma = 1.0f / _gamma; + } + + _beta = h * k * _gamma; + + // Compute the effective mass matrix. + _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + // 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] + Mat22 K = new Mat22(); + K.ex.X = _invMassA + _invIA * _rA.Y * _rA.Y + _gamma; + K.ex.Y = -_invIA * _rA.X * _rA.Y; + K.ey.X = K.ex.Y; + K.ey.Y = _invMassA + _invIA * _rA.X * _rA.X + _gamma; + + _mass = K.Inverse; + + _C = cA + _rA - _worldAnchor; + _C *= _beta; + + // Cheat with some damping + wA *= 0.98f; + + if (Settings.EnableWarmstarting) + { + _impulse *= data.step.dtRatio; + vA += _invMassA * _impulse; + wA += _invIA * MathUtils.Cross(_rA, _impulse); + } + else + { + _impulse = Vector2.Zero; + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + } + + internal override void SolveVelocityConstraints(ref SolverData data) + { + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + + // Cdot = v + cross(w, r) + Vector2 Cdot = vA + MathUtils.Cross(wA, _rA); + Vector2 impulse = MathUtils.Mul(ref _mass, -(Cdot + _C + _gamma * _impulse)); + + Vector2 oldImpulse = _impulse; + _impulse += impulse; + float maxImpulse = data.step.dt * MaxForce; + if (_impulse.LengthSquared() > maxImpulse * maxImpulse) + { + _impulse *= maxImpulse / _impulse.Length(); + } + impulse = _impulse - oldImpulse; + + vA += _invMassA * impulse; + wA += _invIA * MathUtils.Cross(_rA, impulse); + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + } + + internal override bool SolvePositionConstraints(ref SolverData data) + { + return true; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/Joints/FrictionJoint.cs b/src/Box2DNet/Dynamics/Joints/FrictionJoint.cs new file mode 100644 index 0000000..6805ebb --- /dev/null +++ b/src/Box2DNet/Dynamics/Joints/FrictionJoint.cs @@ -0,0 +1,272 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using Box2DNet.Common; +using Microsoft.Xna.Framework; + +namespace Box2DNet.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 + { + // Solver shared + private Vector2 _linearImpulse; + private float _angularImpulse; + + // Solver temp + private int _indexA; + private int _indexB; + private Vector2 _rA; + private Vector2 _rB; + private Vector2 _localCenterA; + private Vector2 _localCenterB; + private float _invMassA; + private float _invMassB; + private float _invIA; + private float _invIB; + private float _angularMass; + private Mat22 _linearMass; + + internal FrictionJoint() + { + JointType = JointType.Friction; + } + + /// + /// Constructor for FrictionJoint. + /// + /// + /// + /// + /// Set to true if you are using world coordinates as anchors. + public FrictionJoint(Body bodyA, Body bodyB, Vector2 anchor, bool useWorldCoordinates = false) + : base(bodyA, bodyB) + { + JointType = JointType.Friction; + + if (useWorldCoordinates) + { + LocalAnchorA = BodyA.GetLocalPoint(anchor); + LocalAnchorB = BodyB.GetLocalPoint(anchor); + } + else + { + LocalAnchorA = anchor; + LocalAnchorB = anchor; + } + } + + /// + /// The local anchor point on BodyA + /// + public Vector2 LocalAnchorA { get; set; } + + /// + /// The local anchor point on BodyB + /// + public Vector2 LocalAnchorB { get; set; } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + set { LocalAnchorA = BodyA.GetLocalPoint(value); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { LocalAnchorB = BodyB.GetLocalPoint(value); } + } + + /// + /// 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 invDt) + { + return invDt * _linearImpulse; + } + + public override float GetReactionTorque(float invDt) + { + return invDt * _angularImpulse; + } + + internal override void InitVelocityConstraints(ref SolverData data) + { + _indexA = BodyA.IslandIndex; + _indexB = BodyB.IslandIndex; + _localCenterA = BodyA._sweep.LocalCenter; + _localCenterB = BodyB._sweep.LocalCenter; + _invMassA = BodyA._invMass; + _invMassB = BodyB._invMass; + _invIA = BodyA._invI; + _invIB = BodyB._invI; + + float aA = data.positions[_indexA].a; + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + + float aB = data.positions[_indexB].a; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + // Compute the effective mass matrix. + _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + + // 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 = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + Mat22 K = new Mat22(); + K.ex.X = mA + mB + iA * _rA.Y * _rA.Y + iB * _rB.Y * _rB.Y; + K.ex.Y = -iA * _rA.X * _rA.Y - iB * _rB.X * _rB.Y; + K.ey.X = K.ex.Y; + K.ey.Y = mA + mB + iA * _rA.X * _rA.X + iB * _rB.X * _rB.X; + + _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 *= data.step.dtRatio; + _angularImpulse *= data.step.dtRatio; + + Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y); + vA -= mA * P; + wA -= iA * (MathUtils.Cross(_rA, P) + _angularImpulse); + vB += mB * P; + wB += iB * (MathUtils.Cross(_rB, P) + _angularImpulse); + } + else + { + _linearImpulse = Vector2.Zero; + _angularImpulse = 0.0f; + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override void SolveVelocityConstraints(ref SolverData data) + { + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + float h = data.step.dt; + + // Solve angular friction + { + float Cdot = wB - wA; + float impulse = -_angularMass * Cdot; + + float oldImpulse = _angularImpulse; + float maxImpulse = h * 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.Mul(ref _linearMass, Cdot); + Vector2 oldImpulse = _linearImpulse; + _linearImpulse += impulse; + + float maxImpulse = h * 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); + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override bool SolvePositionConstraints(ref SolverData data) + { + return true; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/Joints/GearJoint.cs b/src/Box2DNet/Dynamics/Joints/GearJoint.cs index b3384cd..f8b4da8 100644 --- a/src/Box2DNet/Dynamics/Joints/GearJoint.cs +++ b/src/Box2DNet/Dynamics/Joints/GearJoint.cs @@ -1,323 +1,478 @@ -/* - Box2DNet Copyright (c) 2018 codeyu https://github.com/codeyu/Box2DNet - Box2D original C++ version Copyright (c) 2006-2007 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. -*/ - -// Gear Joint: -// C0 = (coordinate1 + ratio * coordinate2)_initial -// C = C0 - (cordinate1 + ratio * coordinate2) = 0 -// Cdot = -(Cdot1 + ratio * Cdot2) -// J = -[J1 ratio * J2] -// K = J * invM * JT -// = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T -// -// Revolute: -// coordinate = rotation -// Cdot = angularVelocity -// J = [0 0 1] -// K = J * invM * JT = invI -// -// Prismatic: -// coordinate = dot(p - pg, ug) -// Cdot = dot(v + cross(w, r), ug) -// J = [ug cross(r, ug)] -// K = J * invM * JT = invMass + invI * cross(r, ug)^2 - -using System; using System.Numerics; -using System.Collections.Generic; -using System.Text; - -using Box2DNet.Common; - - -namespace Box2DNet.Dynamics -{ - /// - /// Gear joint definition. This definition requires two existing - /// revolute or prismatic joints (any combination will work). - /// The provided joints must attach a dynamic body to a static body. - /// - public class GearJointDef : JointDef - { - public GearJointDef() - { - Type = JointType.GearJoint; - Joint1 = null; - Joint2 = null; - Ratio = 1.0f; - } - - /// - /// The first revolute/prismatic joint attached to the gear joint. - /// - public Joint Joint1; - - /// - /// The second revolute/prismatic joint attached to the gear joint. - /// - public Joint Joint2; - - /// - /// The gear ratio. - /// @see GearJoint for explanation. - /// - public float Ratio; - } - - /// - /// 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 = constant - /// 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 - { - public Body _ground1; - public Body _ground2; - - // One of these is NULL. - public RevoluteJoint _revolute1; - public PrismaticJoint _prismatic1; - - // One of these is NULL. - public RevoluteJoint _revolute2; - public PrismaticJoint _prismatic2; - - public Vector2 _groundAnchor1; - public Vector2 _groundAnchor2; - - public Vector2 _localAnchor1; - public Vector2 _localAnchor2; - - public Jacobian _J; - - public float _constant; - public float _ratio; - - // Effective mass - public float _mass; - - // Impulse for accumulation/warm starting. - public float _impulse; - - public override Vector2 Anchor1 { get { return _body1.GetWorldPoint(_localAnchor1); } } - public override Vector2 Anchor2 { get { return _body2.GetWorldPoint(_localAnchor2); } } - - public override Vector2 GetReactionForce(float inv_dt) - { - // TODO_ERIN not tested - Vector2 P = _impulse * _J.Linear2; - return inv_dt * P; - } - - public override float GetReactionTorque(float inv_dt) - { - // TODO_ERIN not tested - Vector2 r = _body2.GetTransform().TransformDirection(_localAnchor2 - _body2.GetLocalCenter()); - Vector2 P = _impulse * _J.Linear2; - float L = _impulse * _J.Angular2 - r.Cross(P); - return inv_dt * L; - } - - /// - /// Get the gear ratio. - /// - public float Ratio { get { return _ratio; } } - - public GearJoint(GearJointDef def) - : base(def) - { - JointType type1 = def.Joint1.GetType(); - JointType type2 = def.Joint2.GetType(); - - Box2DNetDebug.Assert(type1 == JointType.RevoluteJoint || type1 == JointType.PrismaticJoint); - Box2DNetDebug.Assert(type2 == JointType.RevoluteJoint || type2 == JointType.PrismaticJoint); - Box2DNetDebug.Assert(def.Joint1.GetBody1().IsStatic()); - Box2DNetDebug.Assert(def.Joint2.GetBody1().IsStatic()); - - _revolute1 = null; - _prismatic1 = null; - _revolute2 = null; - _prismatic2 = null; - - float coordinate1, coordinate2; - - _ground1 = def.Joint1.GetBody1(); - _body1 = def.Joint1.GetBody2(); - if (type1 == JointType.RevoluteJoint) - { - _revolute1 = (RevoluteJoint)def.Joint1; - _groundAnchor1 = _revolute1._localAnchor1; - _localAnchor1 = _revolute1._localAnchor2; - coordinate1 = _revolute1.JointAngle; - } - else - { - _prismatic1 = (PrismaticJoint)def.Joint1; - _groundAnchor1 = _prismatic1._localAnchor1; - _localAnchor1 = _prismatic1._localAnchor2; - coordinate1 = _prismatic1.JointTranslation; - } - - _ground2 = def.Joint2.GetBody1(); - _body2 = def.Joint2.GetBody2(); - if (type2 == JointType.RevoluteJoint) - { - _revolute2 = (RevoluteJoint)def.Joint2; - _groundAnchor2 = _revolute2._localAnchor1; - _localAnchor2 = _revolute2._localAnchor2; - coordinate2 = _revolute2.JointAngle; - } - else - { - _prismatic2 = (PrismaticJoint)def.Joint2; - _groundAnchor2 = _prismatic2._localAnchor1; - _localAnchor2 = _prismatic2._localAnchor2; - coordinate2 = _prismatic2.JointTranslation; - } - - _ratio = def.Ratio; - - _constant = coordinate1 + _ratio * coordinate2; - - _impulse = 0.0f; - } - - internal override void InitVelocityConstraints(TimeStep step) - { - Body g1 = _ground1; - Body g2 = _ground2; - Body b1 = _body1; - Body b2 = _body2; - - float K = 0.0f; - _J.SetZero(); - - if (_revolute1!=null) - { - _J.Angular1 = -1.0f; - K += b1._invI; - } - else - { - Vector2 ug = g1.GetTransform().TransformDirection(_prismatic1._localXAxis1); - Vector2 r = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter()); - float crug = r.Cross(ug); - _J.Linear1 = -ug; - _J.Angular1 = -crug; - K += b1._invMass + b1._invI * crug * crug; - } - - if (_revolute2!=null) - { - _J.Angular2 = -_ratio; - K += _ratio * _ratio * b2._invI; - } - else - { - Vector2 ug = g2.GetTransform().TransformDirection(_prismatic2._localXAxis1); - Vector2 r = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter()); - float crug = r.Cross(ug); - _J.Linear2 = -_ratio * ug; - _J.Angular2 = -_ratio * crug; - K += _ratio * _ratio * (b2._invMass + b2._invI * crug * crug); - } - - // Compute effective mass. - Box2DNetDebug.Assert(K > 0.0f); - _mass = 1.0f / K; - - if (step.WarmStarting) - { - // Warm starting. - b1._linearVelocity += b1._invMass * _impulse * _J.Linear1; - b1._angularVelocity += b1._invI * _impulse * _J.Angular1; - b2._linearVelocity += b2._invMass * _impulse * _J.Linear2; - b2._angularVelocity += b2._invI * _impulse * _J.Angular2; - } - else - { - _impulse = 0.0f; - } - } - - internal override void SolveVelocityConstraints(TimeStep step) - { - Body b1 = _body1; - Body b2 = _body2; - - float Cdot = _J.Compute(b1._linearVelocity, b1._angularVelocity, b2._linearVelocity, b2._angularVelocity); - - float impulse = _mass * (-Cdot); - _impulse += impulse; - - b1._linearVelocity += b1._invMass * impulse * _J.Linear1; - b1._angularVelocity += b1._invI * impulse * _J.Angular1; - b2._linearVelocity += b2._invMass * impulse * _J.Linear2; - b2._angularVelocity += b2._invI * impulse * _J.Angular2; - } - - internal override bool SolvePositionConstraints(float baumgarte) - { - float linearError = 0.0f; - - Body b1 = _body1; - Body b2 = _body2; - - float coordinate1, coordinate2; - if (_revolute1 != null) - { - coordinate1 = _revolute1.JointAngle; - } - else - { - coordinate1 = _prismatic1.JointTranslation; - } - - if (_revolute2 != null) - { - coordinate2 = _revolute2.JointAngle; - } - else - { - coordinate2 = _prismatic2.JointTranslation; - } - - float C = _constant - (coordinate1 + _ratio * coordinate2); - - float impulse = _mass * (-C); - - b1._sweep.C += b1._invMass * impulse * _J.Linear1; - b1._sweep.A += b1._invI * impulse * _J.Angular1; - b2._sweep.C += b2._invMass * impulse * _J.Linear2; - b2._sweep.A += b2._invI * impulse * _J.Angular2; - - b1.SynchronizeTransform(); - b2.SynchronizeTransform(); - - //TODO_ERIN not implemented - return linearError < Settings.LinearSlop; - } - } -} +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System.Diagnostics; +using Box2DNet.Common; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Dynamics.Joints +{ + // Gear Joint: + // C0 = (coordinate1 + ratio * coordinate2)_initial + // C = (coordinate1 + ratio * coordinate2) - C0 = 0 + // J = [J1 ratio * J2] + // K = J * invM * JT + // = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T + // + // Revolute: + // coordinate = rotation + // Cdot = angularVelocity + // J = [0 0 1] + // K = J * invM * JT = invI + // + // Prismatic: + // coordinate = dot(p - pg, ug) + // Cdot = dot(v + cross(w, r), ug) + // J = [ug cross(r, ug)] + // K = J * invM * JT = invMass + invI * cross(r, ug)^2 + + /// + /// 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: + /// + /// 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: You have to manually destroy the gear joint if jointA or jointB is destroyed. + /// + public class GearJoint : Joint + { + private JointType _typeA; + private JointType _typeB; + + private Body _bodyA; + private Body _bodyB; + private Body _bodyC; + private Body _bodyD; + + // Solver shared + private Vector2 _localAnchorA; + private Vector2 _localAnchorB; + private Vector2 _localAnchorC; + private Vector2 _localAnchorD; + + private Vector2 _localAxisC; + private Vector2 _localAxisD; + + private float _referenceAngleA; + private float _referenceAngleB; + + private float _constant; + private float _ratio; + + private float _impulse; + + // Solver temp + private int _indexA, _indexB, _indexC, _indexD; + private Vector2 _lcA, _lcB, _lcC, _lcD; + private float _mA, _mB, _mC, _mD; + private float _iA, _iB, _iC, _iD; + private Vector2 _JvAC, _JvBD; + private float _JwA, _JwB, _JwC, _JwD; + private float _mass; + + /// + /// 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. + /// The first body + /// The second body + public GearJoint(Body bodyA, Body bodyB, Joint jointA, Joint jointB, float ratio = 1f) + { + JointType = JointType.Gear; + BodyA = bodyA; + BodyB = bodyB; + JointA = jointA; + JointB = jointB; + Ratio = ratio; + + _typeA = jointA.JointType; + _typeB = jointB.JointType; + + Debug.Assert(_typeA == JointType.Revolute || _typeA == JointType.Prismatic || _typeA == JointType.FixedRevolute || _typeA == JointType.FixedPrismatic); + Debug.Assert(_typeB == JointType.Revolute || _typeB == JointType.Prismatic || _typeB == JointType.FixedRevolute || _typeB == JointType.FixedPrismatic); + + float coordinateA, coordinateB; + + // TODO_ERIN there might be some problem with the joint edges in b2Joint. + + _bodyC = JointA.BodyA; + _bodyA = JointA.BodyB; + + // Get geometry of joint1 + Transform xfA = _bodyA._xf; + float aA = _bodyA._sweep.A; + Transform xfC = _bodyC._xf; + float aC = _bodyC._sweep.A; + + if (_typeA == JointType.Revolute) + { + RevoluteJoint revolute = (RevoluteJoint)jointA; + _localAnchorC = revolute.LocalAnchorA; + _localAnchorA = revolute.LocalAnchorB; + _referenceAngleA = revolute.ReferenceAngle; + _localAxisC = Vector2.Zero; + + coordinateA = aA - aC - _referenceAngleA; + } + else + { + PrismaticJoint prismatic = (PrismaticJoint)jointA; + _localAnchorC = prismatic.LocalAnchorA; + _localAnchorA = prismatic.LocalAnchorB; + _referenceAngleA = prismatic.ReferenceAngle; + _localAxisC = prismatic.LocalXAxis; + + Vector2 pC = _localAnchorC; + Vector2 pA = MathUtils.MulT(xfC.q, MathUtils.Mul(xfA.q, _localAnchorA) + (xfA.p - xfC.p)); + coordinateA = Vector2.Dot(pA - pC, _localAxisC); + } + + _bodyD = JointB.BodyA; + _bodyB = JointB.BodyB; + + // Get geometry of joint2 + Transform xfB = _bodyB._xf; + float aB = _bodyB._sweep.A; + Transform xfD = _bodyD._xf; + float aD = _bodyD._sweep.A; + + if (_typeB == JointType.Revolute) + { + RevoluteJoint revolute = (RevoluteJoint)jointB; + _localAnchorD = revolute.LocalAnchorA; + _localAnchorB = revolute.LocalAnchorB; + _referenceAngleB = revolute.ReferenceAngle; + _localAxisD = Vector2.Zero; + + coordinateB = aB - aD - _referenceAngleB; + } + else + { + PrismaticJoint prismatic = (PrismaticJoint)jointB; + _localAnchorD = prismatic.LocalAnchorA; + _localAnchorB = prismatic.LocalAnchorB; + _referenceAngleB = prismatic.ReferenceAngle; + _localAxisD = prismatic.LocalXAxis; + + Vector2 pD = _localAnchorD; + Vector2 pB = MathUtils.MulT(xfD.q, MathUtils.Mul(xfB.q, _localAnchorB) + (xfB.p - xfD.p)); + coordinateB = Vector2.Dot(pB - pD, _localAxisD); + } + + _ratio = ratio; + _constant = coordinateA + _ratio * coordinateB; + _impulse = 0.0f; + } + + public override Vector2 WorldAnchorA + { + get { return _bodyA.GetWorldPoint(_localAnchorA); } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + 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 gear ratio. + /// + public float Ratio + { + get { return _ratio; } + set + { + Debug.Assert(MathUtils.IsValid(value)); + _ratio = value; + } + } + + /// + /// The first revolute/prismatic joint attached to the gear joint. + /// + public Joint JointA { get; private set; } + + /// + /// The second revolute/prismatic joint attached to the gear joint. + /// + public Joint JointB { get; private set; } + + public override Vector2 GetReactionForce(float invDt) + { + Vector2 P = _impulse * _JvAC; + return invDt * P; + } + + public override float GetReactionTorque(float invDt) + { + float L = _impulse * _JwA; + return invDt * L; + } + + internal override void InitVelocityConstraints(ref SolverData data) + { + _indexA = _bodyA.IslandIndex; + _indexB = _bodyB.IslandIndex; + _indexC = _bodyC.IslandIndex; + _indexD = _bodyD.IslandIndex; + _lcA = _bodyA._sweep.LocalCenter; + _lcB = _bodyB._sweep.LocalCenter; + _lcC = _bodyC._sweep.LocalCenter; + _lcD = _bodyD._sweep.LocalCenter; + _mA = _bodyA._invMass; + _mB = _bodyB._invMass; + _mC = _bodyC._invMass; + _mD = _bodyD._invMass; + _iA = _bodyA._invI; + _iB = _bodyB._invI; + _iC = _bodyC._invI; + _iD = _bodyD._invI; + + float aA = data.positions[_indexA].a; + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + + float aB = data.positions[_indexB].a; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + float aC = data.positions[_indexC].a; + Vector2 vC = data.velocities[_indexC].v; + float wC = data.velocities[_indexC].w; + + float aD = data.positions[_indexD].a; + Vector2 vD = data.velocities[_indexD].v; + float wD = data.velocities[_indexD].w; + + Rot qA = new Rot(aA), qB = new Rot(aB), qC = new Rot(aC), qD = new Rot(aD); + + _mass = 0.0f; + + if (_typeA == JointType.Revolute) + { + _JvAC = Vector2.Zero; + _JwA = 1.0f; + _JwC = 1.0f; + _mass += _iA + _iC; + } + else + { + Vector2 u = MathUtils.Mul(qC, _localAxisC); + Vector2 rC = MathUtils.Mul(qC, _localAnchorC - _lcC); + Vector2 rA = MathUtils.Mul(qA, _localAnchorA - _lcA); + _JvAC = u; + _JwC = MathUtils.Cross(rC, u); + _JwA = MathUtils.Cross(rA, u); + _mass += _mC + _mA + _iC * _JwC * _JwC + _iA * _JwA * _JwA; + } + + if (_typeB == JointType.Revolute) + { + _JvBD = Vector2.Zero; + _JwB = _ratio; + _JwD = _ratio; + _mass += _ratio * _ratio * (_iB + _iD); + } + else + { + Vector2 u = MathUtils.Mul(qD, _localAxisD); + Vector2 rD = MathUtils.Mul(qD, _localAnchorD - _lcD); + Vector2 rB = MathUtils.Mul(qB, _localAnchorB - _lcB); + _JvBD = _ratio * u; + _JwD = _ratio * MathUtils.Cross(rD, u); + _JwB = _ratio * MathUtils.Cross(rB, u); + _mass += _ratio * _ratio * (_mD + _mB) + _iD * _JwD * _JwD + _iB * _JwB * _JwB; + } + + // Compute effective mass. + _mass = _mass > 0.0f ? 1.0f / _mass : 0.0f; + + if (Settings.EnableWarmstarting) + { + vA += (_mA * _impulse) * _JvAC; + wA += _iA * _impulse * _JwA; + vB += (_mB * _impulse) * _JvBD; + wB += _iB * _impulse * _JwB; + vC -= (_mC * _impulse) * _JvAC; + wC -= _iC * _impulse * _JwC; + vD -= (_mD * _impulse) * _JvBD; + wD -= _iD * _impulse * _JwD; + } + else + { + _impulse = 0.0f; + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + data.velocities[_indexC].v = vC; + data.velocities[_indexC].w = wC; + data.velocities[_indexD].v = vD; + data.velocities[_indexD].w = wD; + } + + internal override void SolveVelocityConstraints(ref SolverData data) + { + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + Vector2 vC = data.velocities[_indexC].v; + float wC = data.velocities[_indexC].w; + Vector2 vD = data.velocities[_indexD].v; + float wD = data.velocities[_indexD].w; + + float Cdot = Vector2.Dot(_JvAC, vA - vC) + Vector2.Dot(_JvBD, vB - vD); + Cdot += (_JwA * wA - _JwC * wC) + (_JwB * wB - _JwD * wD); + + float impulse = -_mass * Cdot; + _impulse += impulse; + + vA += (_mA * impulse) * _JvAC; + wA += _iA * impulse * _JwA; + vB += (_mB * impulse) * _JvBD; + wB += _iB * impulse * _JwB; + vC -= (_mC * impulse) * _JvAC; + wC -= _iC * impulse * _JwC; + vD -= (_mD * impulse) * _JvBD; + wD -= _iD * impulse * _JwD; + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + data.velocities[_indexC].v = vC; + data.velocities[_indexC].w = wC; + data.velocities[_indexD].v = vD; + data.velocities[_indexD].w = wD; + } + + internal override bool SolvePositionConstraints(ref SolverData data) + { + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + Vector2 cC = data.positions[_indexC].c; + float aC = data.positions[_indexC].a; + Vector2 cD = data.positions[_indexD].c; + float aD = data.positions[_indexD].a; + + Rot qA = new Rot(aA), qB = new Rot(aB), qC = new Rot(aC), qD = new Rot(aD); + + const float linearError = 0.0f; + + float coordinateA, coordinateB; + + Vector2 JvAC, JvBD; + float JwA, JwB, JwC, JwD; + float mass = 0.0f; + + if (_typeA == JointType.Revolute) + { + JvAC = Vector2.Zero; + JwA = 1.0f; + JwC = 1.0f; + mass += _iA + _iC; + + coordinateA = aA - aC - _referenceAngleA; + } + else + { + Vector2 u = MathUtils.Mul(qC, _localAxisC); + Vector2 rC = MathUtils.Mul(qC, _localAnchorC - _lcC); + Vector2 rA = MathUtils.Mul(qA, _localAnchorA - _lcA); + JvAC = u; + JwC = MathUtils.Cross(rC, u); + JwA = MathUtils.Cross(rA, u); + mass += _mC + _mA + _iC * JwC * JwC + _iA * JwA * JwA; + + Vector2 pC = _localAnchorC - _lcC; + Vector2 pA = MathUtils.MulT(qC, rA + (cA - cC)); + coordinateA = Vector2.Dot(pA - pC, _localAxisC); + } + + if (_typeB == JointType.Revolute) + { + JvBD = Vector2.Zero; + JwB = _ratio; + JwD = _ratio; + mass += _ratio * _ratio * (_iB + _iD); + + coordinateB = aB - aD - _referenceAngleB; + } + else + { + Vector2 u = MathUtils.Mul(qD, _localAxisD); + Vector2 rD = MathUtils.Mul(qD, _localAnchorD - _lcD); + Vector2 rB = MathUtils.Mul(qB, _localAnchorB - _lcB); + JvBD = _ratio * u; + JwD = _ratio * MathUtils.Cross(rD, u); + JwB = _ratio * MathUtils.Cross(rB, u); + mass += _ratio * _ratio * (_mD + _mB) + _iD * JwD * JwD + _iB * JwB * JwB; + + Vector2 pD = _localAnchorD - _lcD; + Vector2 pB = MathUtils.MulT(qD, rB + (cB - cD)); + coordinateB = Vector2.Dot(pB - pD, _localAxisD); + } + + float C = (coordinateA + _ratio * coordinateB) - _constant; + + float impulse = 0.0f; + if (mass > 0.0f) + { + impulse = -C / mass; + } + + cA += _mA * impulse * JvAC; + aA += _iA * impulse * JwA; + cB += _mB * impulse * JvBD; + aB += _iB * impulse * JwB; + cC -= _mC * impulse * JvAC; + aC -= _iC * impulse * JwC; + cD -= _mD * impulse * JvBD; + aD -= _iD * impulse * JwD; + + data.positions[_indexA].c = cA; + data.positions[_indexA].a = aA; + data.positions[_indexB].c = cB; + data.positions[_indexB].a = aB; + data.positions[_indexC].c = cC; + data.positions[_indexC].a = aC; + data.positions[_indexD].c = cD; + data.positions[_indexD].a = aD; + + // TODO_ERIN not implemented + return linearError < Settings.LinearSlop; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/Joints/Joint.cs b/src/Box2DNet/Dynamics/Joints/Joint.cs index a977061..9e409c9 100644 --- a/src/Box2DNet/Dynamics/Joints/Joint.cs +++ b/src/Box2DNet/Dynamics/Joints/Joint.cs @@ -1,321 +1,253 @@ -/* - Box2DNet Copyright (c) 2018 codeyu https://github.com/codeyu/Box2DNet - Box2D original C++ version Copyright (c) 2006-2007 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.Numerics; -using System.Collections.Generic; -using System.Text; - -using Box2DNet.Common; - -using Transform = Box2DNet.Common.Transform; - -namespace Box2DNet.Dynamics -{ - public enum JointType - { - UnknownJoint, - RevoluteJoint, - PrismaticJoint, - DistanceJoint, - PulleyJoint, - MouseJoint, - GearJoint, - LineJoint - } - - public enum LimitState - { - InactiveLimit, - AtLowerLimit, - AtUpperLimit, - EqualLimits - } - - public struct Jacobian - { - public Vector2 Linear1; - public float Angular1; - public Vector2 Linear2; - public float Angular2; - - public void SetZero() - { - Linear1 = Vector2.Zero; - Angular1 = 0.0f; - Linear2 = Vector2.Zero; - Angular2 = 0.0f; - } - - public void Set(Vector2 x1, float a1, Vector2 x2, float a2) - { - Linear1 = x1; Angular1 = a1; - Linear2 = x2; Angular2 = a2; - } - - public float Compute(Vector2 x1, float a1, Vector2 x2, float a2) - { - return Vector2.Dot(Linear1, x1) + Angular1 * a1 + Vector2.Dot(Linear2, x2) + Angular2 * a2; - } - } - -#warning "CAS" - /// - /// 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 class JointEdge - { - /// - /// Provides quick access to the other body attached. - /// - public Body Other; - - /// - /// The joint. - /// - public Joint Joint; - - /// - /// The previous joint edge in the body's joint list. - /// - public JointEdge Prev; - - /// - /// The next joint edge in the body's joint list. - /// - public JointEdge Next; - } - -#warning "CAS" - /// - /// Joint definitions are used to construct joints. - /// - public class JointDef - { - public JointDef() - { - Type = JointType.UnknownJoint; - UserData = null; - Body1 = null; - Body2 = null; - CollideConnected = false; - } - - /// - /// The joint type is set automatically for concrete joint types. - /// - public JointType Type; - - /// - /// Use this to attach application specific data to your joints. - /// - public object UserData; - - /// - /// The first attached body. - /// - public Body Body1; - - /// - /// The second attached body. - /// - public Body Body2; - - /// - /// Set this flag to true if the attached bodies should collide. - /// - public bool CollideConnected; - } - - /// - /// The base joint class. Joints are used to constraint two bodies together in - /// various fashions. Some joints also feature limits and motors. - /// - public abstract class Joint - { - protected JointType _type; - internal Joint _prev; - internal Joint _next; - internal JointEdge _node1 = new JointEdge(); - internal JointEdge _node2 = new JointEdge(); - internal Body _body1; - internal Body _body2; - - internal bool _islandFlag; - internal bool _collideConnected; - - protected object _userData; - - // Cache here per time step to reduce cache misses. - protected Vector2 _localCenter1, _localCenter2; - protected float _invMass1, _invI1; - protected float _invMass2, _invI2; - - /// - /// Get the type of the concrete joint. - /// - public new JointType GetType() - { - return _type; - } - - /// - /// Get the first body attached to this joint. - /// - /// - public Body GetBody1() - { - return _body1; - } - - /// - /// Get the second body attached to this joint. - /// - /// - public Body GetBody2() - { - return _body2; - } - - /// - /// Get the anchor point on body1 in world coordinates. - /// - /// - public abstract Vector2 Anchor1 { get; } - - /// - /// Get the anchor point on body2 in world coordinates. - /// - /// - public abstract Vector2 Anchor2 { get; } - - /// - /// Get the reaction force on body2 at the joint anchor. - /// - public abstract Vector2 GetReactionForce(float inv_dt); - - /// - /// Get the reaction torque on body2. - /// - public abstract float GetReactionTorque(float inv_dt); - - /// - /// Get the next joint the world joint list. - /// - /// - public Joint GetNext() - { - return _next; - } - - /// - /// Get/Set the user data pointer. - /// - /// - public object UserData - { - get { return _userData; } - set { _userData = value; } - } - - protected Joint(JointDef def) - { - _type = def.Type; - _prev = null; - _next = null; - _body1 = def.Body1; - _body2 = def.Body2; - _collideConnected = def.CollideConnected; - _islandFlag = false; - _userData = def.UserData; - } - - internal static Joint Create(JointDef def) - { - Joint joint = null; - - switch (def.Type) - { - case JointType.DistanceJoint: - { - joint = new DistanceJoint((DistanceJointDef)def); - } - break; - case JointType.MouseJoint: - { - joint = new MouseJoint((MouseJointDef)def); - } - break; - case JointType.PrismaticJoint: - { - joint = new PrismaticJoint((PrismaticJointDef)def); - } - break; - case JointType.RevoluteJoint: - { - joint = new RevoluteJoint((RevoluteJointDef)def); - } - break; - case JointType.PulleyJoint: - { - joint = new PulleyJoint((PulleyJointDef)def); - } - break; - case JointType.GearJoint: - { - joint = new GearJoint((GearJointDef)def); - } - break; - case JointType.LineJoint: - { - joint = new LineJoint((LineJointDef)def); - } - break; - default: - Box2DNetDebug.Assert(false); - break; - } - - return joint; - } - - internal static void Destroy(Joint joint) - { - joint = null; - } - - internal abstract void InitVelocityConstraints(TimeStep step); - internal abstract void SolveVelocityConstraints(TimeStep step); - - // This returns true if the position errors are within tolerance. - internal abstract bool SolvePositionConstraints(float baumgarte); - - internal void ComputeTransform(ref Transform xf, Vector2 center, Vector2 localCenter, float angle) - { - xf.rotation = Box2DNet.Common.Math.AngleToRotation(angle); - //xf.R = new Mat22(angle); - xf.position = center - xf.TransformDirection(localCenter); - } - } -} +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Dynamics.Joints +{ + public enum JointType + { + Unknown, + Revolute, + Prismatic, + Distance, + Pulley, + //Mouse, <- We have fixed mouse + Gear, + Wheel, + Weld, + Friction, + Rope, + Motor, + + //FPE note: From here on and down, it is only FPE joints + Angle, + FixedMouse, + FixedRevolute, + FixedDistance, + FixedLine, + FixedPrismatic, + FixedAngle, + FixedFriction, + } + + public enum LimitState + { + Inactive, + AtLower, + AtUpper, + Equal, + } + + /// + /// 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 + { + private float _breakpoint; + private double _breakpointSquared; + + /// + /// Indicate if this join is enabled or not. Disabling a joint + /// means it is still in the simulation, but inactive. + /// + public bool Enabled = true; + + internal JointEdge EdgeA = new JointEdge(); + internal JointEdge EdgeB = new JointEdge(); + internal bool IslandFlag; + + protected Joint() + { + Breakpoint = float.MaxValue; + + //Connected bodies should not collide by default + CollideConnected = false; + } + + protected Joint(Body bodyA, Body bodyB) : this() + { + //Can't connect a joint to the same body twice. + Debug.Assert(bodyA != bodyB); + + BodyA = bodyA; + BodyB = bodyB; + } + + /// + /// Constructor for fixed joint + /// + protected Joint(Body body) : this() + { + BodyA = body; + } + + /// + /// 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; internal set; } + + /// + /// Get the second body attached to this joint. + /// + public Body BodyB { get; internal set; } + + /// + /// Get the anchor point on bodyA in world coordinates. + /// On some joints, this value indicate the anchor point within the world. + /// + public abstract Vector2 WorldAnchorA { get; set; } + + /// + /// Get the anchor point on bodyB in world coordinates. + /// On some joints, this value indicate the anchor point within the world. + /// + public abstract Vector2 WorldAnchorB { get; set; } + + /// + /// Set the user data pointer. + /// + /// The data. + public object UserData { get; set; } + + /// + /// Set this flag to true if the attached bodies should collide. + /// + public bool CollideConnected { get; set; } + + /// + /// The Breakpoint simply indicates the maximum Value the JointError can be before it breaks. + /// The default value is float.MaxValue, which means it never breaks. + /// + public float Breakpoint + { + get { return _breakpoint; } + set + { + _breakpoint = value; + _breakpointSquared = _breakpoint * _breakpoint; + } + } + + /// + /// Fires when the joint is broken. + /// + public event Action Broke; + + /// + /// Get the reaction force on body at the joint anchor in Newtons. + /// + /// The inverse delta time. + public abstract Vector2 GetReactionForce(float invDt); + + /// + /// Get the reaction torque on the body at the joint anchor in N*m. + /// + /// The inverse delta time. + public abstract float GetReactionTorque(float invDt); + + protected void WakeBodies() + { + if (BodyA != null) + 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 SolverData data); + + internal void Validate(float invDt) + { + if (!Enabled) + return; + + float jointErrorSquared = GetReactionForce(invDt).LengthSquared(); + + if (Math.Abs(jointErrorSquared) <= _breakpointSquared) + return; + + Enabled = false; + + if (Broke != null) + Broke(this, (float)Math.Sqrt(jointErrorSquared)); + } + + internal abstract void SolveVelocityConstraints(ref SolverData data); + + /// + /// Solves the position constraints. + /// + /// + /// returns true if the position errors are within tolerance. + internal abstract bool SolvePositionConstraints(ref SolverData data); + } +} \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/Joints/LineJoint.cs b/src/Box2DNet/Dynamics/Joints/LineJoint.cs deleted file mode 100644 index ac4aefc..0000000 --- a/src/Box2DNet/Dynamics/Joints/LineJoint.cs +++ /dev/null @@ -1,713 +0,0 @@ -/* - Box2DNet Copyright (c) 2018 codeyu https://github.com/codeyu/Box2DNet - Box2D original C++ version Copyright (c) 2006-2007 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. -*/ - -// 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)] -// -// K = J * invM * JT -// -// J = [-a -s1 a s2] -// 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 -// [-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(2) = max(f2(2), 0) -// upper: f2(2) = min(f2(2), 0) -// -// Solve for correct f2(1) -// K(1,1) * f2(1) = -Cdot(1) - K(1,2) * f2(2) + K(1,1:2) * f1 -// = -Cdot(1) - K(1,2) * f2(2) + K(1,1) * f1(1) + K(1,2) * f1(2) -// K(1,1) * f2(1) = -Cdot(1) - K(1,2) * (f2(2) - f1(2)) + K(1,1) * f1(1) -// f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1) -// -// Now compute impulse to be applied: -// df = f2 - f1 - -using System; using System.Numerics; -using System.Collections.Generic; -using System.Text; - -using Box2DNet.Common; - - -using Transform = Box2DNet.Common.Transform; - -namespace Box2DNet.Dynamics -{ - /// - /// Line joint definition. 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. - /// - public class LineJointDef : JointDef - { - public LineJointDef() - { - Type = JointType.LineJoint; - localAnchor1 = Vector2.Zero; - localAnchor2 = Vector2.Zero; - localAxis1 = new Vector2(1.0f, 0.0f); - enableLimit = false; - lowerTranslation = 0.0f; - upperTranslation = 0.0f; - enableMotor = false; - maxMotorForce = 0.0f; - motorSpeed = 0.0f; - } - - /// - /// Initialize the bodies, anchors, axis, and reference angle using the world - /// anchor and world axis. - /// - public void Initialize(Body body1, Body body2, Vector2 anchor, Vector2 axis) - { - Body1 = body1; - Body2 = body2; - localAnchor1 = body1.GetLocalPoint(anchor); - localAnchor2 = body2.GetLocalPoint(anchor); - localAxis1 = body1.GetLocalVector(axis); - } - - /// - /// The local anchor point relative to body1's origin. - /// - public Vector2 localAnchor1; - - /// - /// The local anchor point relative to body2's origin. - /// - public Vector2 localAnchor2; - - /// - /// The local translation axis in body1. - /// - public Vector2 localAxis1; - - /// - /// Enable/disable the joint limit. - /// - public bool enableLimit; - - /// - /// The lower translation limit, usually in meters. - /// - public float lowerTranslation; - - /// - /// The upper translation limit, usually in meters. - /// - public float upperTranslation; - - /// - /// Enable/disable the joint motor. - /// - public bool enableMotor; - - /// - /// The maximum motor torque, usually in N-m. - /// - public float maxMotorForce; - - /// - /// The desired motor speed in radians per second. - /// - public float motorSpeed; - } - - /// - /// A line joint. This joint provides one degree of freedom: translation - /// along an axis fixed in body1. 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 LineJoint : Joint - { - public Vector2 _localAnchor1; - public Vector2 _localAnchor2; - public Vector2 _localXAxis1; - public Vector2 _localYAxis1; - - public Vector2 _axis, _perp; - public float _s1, _s2; - public float _a1, _a2; - - public Mat22 _K; - public Vector2 _impulse; - - public float _motorMass; // effective mass for motor/limit translational constraint. - public float _motorImpulse; - - public float _lowerTranslation; - public float _upperTranslation; - public float _maxMotorForce; - public float _motorSpeed; - - public bool _enableLimit; - public bool _enableMotor; - public LimitState _limitState; - - public LineJoint(LineJointDef def) - : base(def) - { - _localAnchor1 = def.localAnchor1; - _localAnchor2 = def.localAnchor2; - _localXAxis1 = def.localAxis1; - _localYAxis1 = _localXAxis1.CrossScalarPreMultiply(1.0f); - - _impulse = Vector2.Zero; - _motorMass = 0.0f; - _motorImpulse = 0.0f; - - _lowerTranslation = def.lowerTranslation; - _upperTranslation = def.upperTranslation; - _maxMotorForce = Settings.FORCE_INV_SCALE(def.maxMotorForce); - _motorSpeed = def.motorSpeed; - _enableLimit = def.enableLimit; - _enableMotor = def.enableMotor; - _limitState = LimitState.InactiveLimit; - - _axis = Vector2.Zero; - _perp = Vector2.Zero; - } - - public override Vector2 Anchor1 - { - get { return _body1.GetWorldPoint(_localAnchor1); } - } - - public override Vector2 Anchor2 - { - get { return _body2.GetWorldPoint(_localAnchor2); } - } - - public override Vector2 GetReactionForce(float inv_dt) - { - return inv_dt * (_impulse.X * _perp + (_motorImpulse + _impulse.Y) * _axis); - } - - public override float GetReactionTorque(float inv_dt) - { - return 0.0f; - } - - /// - /// Get the current joint translation, usually in meters. - /// - public float GetJointTranslation() - { - Body b1 = _body1; - Body b2 = _body2; - - Vector2 p1 = b1.GetWorldPoint(_localAnchor1); - Vector2 p2 = b2.GetWorldPoint(_localAnchor2); - Vector2 d = p2 - p1; - Vector2 axis = b1.GetWorldVector(_localXAxis1); - - float translation = Vector2.Dot(d, axis); - return translation; - } - - /// - /// Get the current joint translation speed, usually in meters per second. - /// - public float GetJointSpeed() - { - Body b1 = _body1; - Body b2 = _body2; - - Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter()); - Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter()); - Vector2 p1 = b1._sweep.C + r1; - Vector2 p2 = b2._sweep.C + r2; - Vector2 d = p2 - p1; - Vector2 axis = b1.GetWorldVector(_localXAxis1); - - Vector2 v1 = b1._linearVelocity; - Vector2 v2 = b2._linearVelocity; - float w1 = b1._angularVelocity; - float w2 = b2._angularVelocity; - - float speed = Vector2.Dot(d, axis.CrossScalarPreMultiply(w1)) + Vector2.Dot(axis, v2 + r2.CrossScalarPreMultiply(w2) - v1 - r1.CrossScalarPreMultiply(w1)); - return speed; - } - - /// - /// Is the joint limit enabled? - /// - public bool IsLimitEnabled() - { - return _enableLimit; - } - - /// - /// Enable/disable the joint limit. - /// - public void EnableLimit(bool flag) - { - _body1.WakeUp(); - _body2.WakeUp(); - _enableLimit = flag; - } - - /// - /// Get the lower joint limit, usually in meters. - /// - public float GetLowerLimit() - { - return _lowerTranslation; - } - - /// - /// Get the upper joint limit, usually in meters. - /// - public float GetUpperLimit() - { - return _upperTranslation; - } - - /// - /// Set the joint limits, usually in meters. - /// - public void SetLimits(float lower, float upper) - { - Box2DNetDebug.Assert(lower <= upper); - _body1.WakeUp(); - _body2.WakeUp(); - _lowerTranslation = lower; - _upperTranslation = upper; - } - - /// - /// Is the joint motor enabled? - /// - public bool IsMotorEnabled() - { - return _enableMotor; - } - - /// - /// Enable/disable the joint motor. - /// - public void EnableMotor(bool flag) - { - _body1.WakeUp(); - _body2.WakeUp(); - _enableMotor = flag; - } - - /// - /// Set the motor speed, usually in meters per second. - /// - public void SetMotorSpeed(float speed) - { - _body1.WakeUp(); - _body2.WakeUp(); - _motorSpeed = speed; - } - - /// - /// Set the maximum motor force, usually in N. - /// - public void SetMaxMotorForce(float force) - { - _body1.WakeUp(); - _body2.WakeUp(); - _maxMotorForce = Settings.FORCE_SCALE(1.0f) * force; - } - - /// - /// Get the current motor force, usually in N. - /// - public float GetMotorForce() - { - return _motorImpulse; - } - - /// - /// Get the motor speed, usually in meters per second. - /// - public float GetMotorSpeed() - { - return _motorSpeed; - } - - internal override void InitVelocityConstraints(TimeStep step) - { - Body b1 = _body1; - Body b2 = _body2; - - _localCenter1 = b1.GetLocalCenter(); - _localCenter2 = b2.GetLocalCenter(); - - Transform xf1 = b1.GetTransform(); - Transform xf2 = b2.GetTransform(); - - // Compute the effective masses. - Vector2 r1 = xf1.TransformDirection(_localAnchor1 - _localCenter1); - Vector2 r2 = xf2.TransformDirection(_localAnchor2 - _localCenter2); - Vector2 d = b2._sweep.C + r2 - b1._sweep.C - r1; - - _invMass1 = b1._invMass; - _invI1 = b1._invI; - _invMass2 = b2._invMass; - _invI2 = b2._invI; - - // Compute motor Jacobian and effective mass. - { - _axis = xf1.TransformDirection(_localXAxis1); - _a1 = (d + r1).Cross(_axis); - _a2 = r2.Cross(_axis); - - _motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2; - Box2DNetDebug.Assert(_motorMass > Settings.FLT_EPSILON); - _motorMass = 1.0f / _motorMass; - } - - // Prismatic constraint. - { - _perp = xf1.TransformDirection(_localYAxis1); - - _s1 = (d + r1).Cross(_perp); - _s2 = r2.Cross(_perp); - - float m1 = _invMass1, m2 = _invMass2; - float i1 = _invI1, i2 = _invI2; - - float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; - float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2; - float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2; - - _K.Col1 = new Vector2(k11, k12); - _K.Col2 = new Vector2(k12, k22); - } - - // Compute motor and limit terms. - if (_enableLimit) - { - float jointTranslation = Vector2.Dot(_axis, d); - if (Box2DNet.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) - { - _limitState = LimitState.EqualLimits; - } - else if (jointTranslation <= _lowerTranslation) - { - if (_limitState != LimitState.AtLowerLimit) - { - _limitState = LimitState.AtLowerLimit; - _impulse.Y = 0.0f; - } - } - else if (jointTranslation >= _upperTranslation) - { - if (_limitState != LimitState.AtUpperLimit) - { - _limitState = LimitState.AtUpperLimit; - _impulse.Y = 0.0f; - } - } - else - { - _limitState = LimitState.InactiveLimit; - _impulse.Y = 0.0f; - } - } - else - { - _limitState = LimitState.InactiveLimit; - } - - if (_enableMotor == false) - { - _motorImpulse = 0.0f; - } - - if (step.WarmStarting) - { - // Account for variable time step. - _impulse *= step.DtRatio; - _motorImpulse *= step.DtRatio; - - Vector2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Y) * _axis; - float L1 = _impulse.X * _s1 + (_motorImpulse + _impulse.Y) * _a1; - float L2 = _impulse.X * _s2 + (_motorImpulse + _impulse.Y) * _a2; - - b1._linearVelocity -= _invMass1 * P; - b1._angularVelocity -= _invI1 * L1; - - b2._linearVelocity += _invMass2 * P; - b2._angularVelocity += _invI2 * L2; - } - else - { - _impulse = Vector2.Zero; - _motorImpulse = 0.0f; - } - } - - internal override void SolveVelocityConstraints(TimeStep step) - { - Body b1 = _body1; - Body b2 = _body2; - - Vector2 v1 = b1._linearVelocity; - float w1 = b1._angularVelocity; - Vector2 v2 = b2._linearVelocity; - float w2 = b2._angularVelocity; - - // Solve linear motor constraint. - if (_enableMotor && _limitState != LimitState.EqualLimits) - { - 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 = Box2DNet.Common.Math.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); - impulse = _motorImpulse - oldImpulse; - - Vector2 P = impulse * _axis; - float L1 = impulse * _a1; - float L2 = impulse * _a2; - - v1 -= _invMass1 * P; - w1 -= _invI1 * L1; - - v2 += _invMass2 * P; - w2 += _invI2 * L2; - } - - float Cdot1 = Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1; - - if (_enableLimit && _limitState != LimitState.InactiveLimit) - { - // Solve prismatic and limit constraint in block form. - float Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; - Vector2 Cdot = new Vector2(Cdot1, Cdot2); - - Vector2 f1 = _impulse; - Vector2 df = _K.Solve(-Cdot); - _impulse += df; - - if (_limitState == LimitState.AtLowerLimit) - { - _impulse.Y = System.Math.Max(_impulse.Y, 0.0f); - } - else if (_limitState == LimitState.AtUpperLimit) - { - _impulse.Y = System.Math.Min(_impulse.Y, 0.0f); - } - - // f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1) - float b = -Cdot1 - (_impulse.Y - f1.Y) * _K.Col2.Y; - float f2r = b / _K.Col1.X + f1.X; - _impulse.X = f2r; - - df = _impulse - f1; - - Vector2 P = df.X * _perp + df.Y * _axis; - float L1 = df.X * _s1 + df.Y * _a1; - float L2 = df.X * _s2 + df.Y * _a2; - - v1 -= _invMass1 * P; - w1 -= _invI1 * L1; - - v2 += _invMass2 * P; - w2 += _invI2 * L2; - } - else - { - // Limit is inactive, just solve the prismatic constraint in block form. - float df = (-Cdot1) / _K.Col1.X; - _impulse.X += df; - - Vector2 P = df * _perp; - float L1 = df * _s1; - float L2 = df * _s2; - - v1 -= _invMass1 * P; - w1 -= _invI1 * L1; - - v2 += _invMass2 * P; - w2 += _invI2 * L2; - } - - b1._linearVelocity = v1; - b1._angularVelocity = w1; - b2._linearVelocity = v2; - b2._angularVelocity = w2; - } - - internal override bool SolvePositionConstraints(float baumgarte) - { - Body b1 = _body1; - Body b2 = _body2; - - 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, angularError = 0.0f; - bool active = false; - float C2 = 0.0f; - - Mat22 R1 = new Mat22(a1), R2 = new Mat22(a2); - - Vector2 r1 = R1.Multiply(_localAnchor1 - _localCenter1); - Vector2 r2 = R2.Multiply(_localAnchor2 - _localCenter2); - Vector2 d = c2 + r2 - c1 - r1; - - if (_enableLimit) - { - _axis = R1.Multiply(_localXAxis1); - - _a1 = (d + r1).Cross(_axis); - _a2 = r2.Cross(_axis); - - float translation = Vector2.Dot(_axis, d); - if (System.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) - { - // Prevent large angular corrections - C2 = Box2DNet.Common.Math.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); - linearError = Box2DNet.Common.Math.Abs(translation); - active = true; - } - else if (translation <= _lowerTranslation) - { - // Prevent large linear corrections and allow some slop. - C2 = Box2DNet.Common.Math.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 = Box2DNet.Common.Math.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection); - linearError = translation - _upperTranslation; - active = true; - } - } - - _perp = R1.Multiply(_localYAxis1); - - _s1 = (d + r1).Cross(_perp); - _s2 = r2.Cross(_perp); - - Vector2 impulse; - float C1; - C1 = Vector2.Dot(_perp, d); - - linearError = Box2DNet.Common.Math.Max(linearError, Box2DNet.Common.Math.Abs(C1)); - angularError = 0.0f; - - if (active) - { - float m1 = _invMass1, m2 = _invMass2; - float i1 = _invI1, i2 = _invI2; - - float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; - float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2; - float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2; - - _K.Col1 = new Vector2(k11, k12); - _K.Col2 = new Vector2(k12, k22); - - Vector2 C = new Vector2(); - C.X = C1; - C.Y = C2; - - impulse = _K.Solve(-C); - } - else - { - float m1 = _invMass1, m2 = _invMass2; - float i1 = _invI1, i2 = _invI2; - - float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; - - float impulse1 = (-C1) / k11; - impulse.X = impulse1; - impulse.Y = 0.0f; - } - - Vector2 P = impulse.X * _perp + impulse.Y * _axis; - float L1 = impulse.X * _s1 + impulse.Y * _a1; - float L2 = impulse.X * _s2 + impulse.Y * _a2; - - c1 -= _invMass1 * P; - a1 -= _invI1 * L1; - c2 += _invMass2 * P; - a2 += _invI2 * 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/src/Box2DNet/Dynamics/Joints/MotorJoint.cs b/src/Box2DNet/Dynamics/Joints/MotorJoint.cs new file mode 100644 index 0000000..3a1e1ae --- /dev/null +++ b/src/Box2DNet/Dynamics/Joints/MotorJoint.cs @@ -0,0 +1,320 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System.Diagnostics; +using Box2DNet.Common; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Dynamics.Joints +{ + /// + /// A motor joint is used to control the relative motion + /// between two bodies. A typical usage is to control the movement + /// of a dynamic body with respect to the ground. + /// + public class MotorJoint : Joint + { + // Solver shared + private Vector2 _linearOffset; + private float _angularOffset; + private Vector2 _linearImpulse; + private float _angularImpulse; + private float _maxForce; + private float _maxTorque; + + // Solver temp + private int _indexA; + private int _indexB; + private Vector2 _rA; + private Vector2 _rB; + private Vector2 _localCenterA; + private Vector2 _localCenterB; + private Vector2 _linearError; + private float _angularError; + private float _invMassA; + private float _invMassB; + private float _invIA; + private float _invIB; + private Mat22 _linearMass; + private float _angularMass; + + internal MotorJoint() + { + JointType = JointType.Motor; + } + + /// + /// Constructor for MotorJoint. + /// + /// The first body + /// The second body + /// Set to true if you are using world coordinates as anchors. + public MotorJoint(Body bodyA, Body bodyB, bool useWorldCoordinates = false) + : base(bodyA, bodyB) + { + JointType = JointType.Motor; + + Vector2 xB = BodyB.Position; + + if (useWorldCoordinates) + _linearOffset = BodyA.GetLocalPoint(xB); + else + _linearOffset = xB; + + //Defaults + _angularOffset = 0.0f; + _maxForce = 1.0f; + _maxTorque = 1.0f; + CorrectionFactor = 0.3f; + + _angularOffset = BodyB.Rotation - BodyA.Rotation; + } + + public override Vector2 WorldAnchorA + { + get { return BodyA.Position; } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.Position; } + set { Debug.Assert(false, "You can't set the world anchor on this joint type."); } + } + + /// + /// The maximum amount of force that can be applied to BodyA + /// + public float MaxForce + { + set + { + Debug.Assert(MathUtils.IsValid(value) && value >= 0.0f); + _maxForce = value; + } + get { return _maxForce; } + } + + /// + /// The maximum amount of torque that can be applied to BodyA + /// + public float MaxTorque + { + set + { + Debug.Assert(MathUtils.IsValid(value) && value >= 0.0f); + _maxTorque = value; + } + get { return _maxTorque; } + } + + /// + /// The linear (translation) offset. + /// + public Vector2 LinearOffset + { + set + { + if (_linearOffset.X != value.X || _linearOffset.Y != value.Y) + { + WakeBodies(); + _linearOffset = value; + } + } + get { return _linearOffset; } + } + + /// + /// Get or set the angular offset. + /// + public float AngularOffset + { + set + { + if (_angularOffset != value) + { + WakeBodies(); + _angularOffset = value; + } + } + get { return _angularOffset; } + } + + //FPE note: Used for serialization. + internal float CorrectionFactor { get; set; } + + public override Vector2 GetReactionForce(float invDt) + { + return invDt * _linearImpulse; + } + + public override float GetReactionTorque(float invDt) + { + return invDt * _angularImpulse; + } + + internal override void InitVelocityConstraints(ref SolverData data) + { + _indexA = BodyA.IslandIndex; + _indexB = BodyB.IslandIndex; + _localCenterA = BodyA._sweep.LocalCenter; + _localCenterB = BodyB._sweep.LocalCenter; + _invMassA = BodyA._invMass; + _invMassB = BodyB._invMass; + _invIA = BodyA._invI; + _invIB = BodyB._invI; + + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + Rot qA = new Rot(aA); + Rot qB = new Rot(aB); + + // Compute the effective mass matrix. + _rA = MathUtils.Mul(qA, -_localCenterA); + _rB = MathUtils.Mul(qB, -_localCenterB); + + // 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 = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + Mat22 K = new Mat22(); + K.ex.X = mA + mB + iA * _rA.Y * _rA.Y + iB * _rB.Y * _rB.Y; + K.ex.Y = -iA * _rA.X * _rA.Y - iB * _rB.X * _rB.Y; + K.ey.X = K.ex.Y; + K.ey.Y = mA + mB + iA * _rA.X * _rA.X + iB * _rB.X * _rB.X; + + _linearMass = K.Inverse; + + _angularMass = iA + iB; + if (_angularMass > 0.0f) + { + _angularMass = 1.0f / _angularMass; + } + + _linearError = cB + _rB - cA - _rA - MathUtils.Mul(qA, _linearOffset); + _angularError = aB - aA - _angularOffset; + + if (Settings.EnableWarmstarting) + { + // Scale impulses to support a variable time step. + _linearImpulse *= data.step.dtRatio; + _angularImpulse *= data.step.dtRatio; + + Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y); + + vA -= mA * P; + wA -= iA * (MathUtils.Cross(_rA, P) + _angularImpulse); + vB += mB * P; + wB += iB * (MathUtils.Cross(_rB, P) + _angularImpulse); + } + else + { + _linearImpulse = Vector2.Zero; + _angularImpulse = 0.0f; + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override void SolveVelocityConstraints(ref SolverData data) + { + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + float h = data.step.dt; + float inv_h = data.step.inv_dt; + + // Solve angular friction + { + float Cdot = wB - wA + inv_h * CorrectionFactor * _angularError; + float impulse = -_angularMass * Cdot; + + float oldImpulse = _angularImpulse; + float maxImpulse = h * _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) + inv_h * CorrectionFactor * _linearError; + + Vector2 impulse = -MathUtils.Mul(ref _linearMass, ref Cdot); + Vector2 oldImpulse = _linearImpulse; + _linearImpulse += impulse; + + float maxImpulse = h * _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); + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override bool SolvePositionConstraints(ref SolverData data) + { + return true; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/Joints/MouseJoint.cs b/src/Box2DNet/Dynamics/Joints/MouseJoint.cs deleted file mode 100644 index 1217a85..0000000 --- a/src/Box2DNet/Dynamics/Joints/MouseJoint.cs +++ /dev/null @@ -1,230 +0,0 @@ -/* - Box2DNet Copyright (c) 2018 codeyu https://github.com/codeyu/Box2DNet - Box2D original C++ version Copyright (c) 2006-2007 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. -*/ - -// p = attached point, m = mouse point -// C = p - m -// Cdot = v -// = v + cross(w, r) -// J = [I r_skew] -// Identity used: -// w k % (rx i + ry j) = w * (-ry i + rx j) - -using System; using System.Numerics; -using System.Collections.Generic; -using System.Text; - -using Box2DNet.Common; - - -namespace Box2DNet.Dynamics -{ - /// - /// Mouse joint definition. This requires a world target point, - /// tuning parameters, and the time step. - /// - public class MouseJointDef : JointDef - { - public MouseJointDef() - { - Type = JointType.MouseJoint; - Target = Vector2.Zero; - MaxForce = 0.0f; - FrequencyHz = 5.0f; - DampingRatio = 0.7f; - } - - /// - /// The initial world target point. This is assumed - /// to coincide with the body anchor initially. - /// - public Vector2 Target; - - /// - /// 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; - - /// - /// The response speed. - /// - public float FrequencyHz; - - /// - /// The damping ratio. 0 = no damping, 1 = critical damping. - /// - public float DampingRatio; - } - - /// - /// 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. - /// - public class MouseJoint : Joint - { - public Vector2 _localAnchor; - public Vector2 _target; - public Vector2 _impulse; - - public Mat22 _mass; // effective mass for point-to-point constraint. - public Vector2 _C; // position error - public float _maxForce; - public float _frequencyHz; - public float _dampingRatio; - public float _beta; - public float _gamma; - - public override Vector2 Anchor1 - { - get { return _target; } - } - - public override Vector2 Anchor2 - { - get { return _body2.GetWorldPoint(_localAnchor); } - } - - public override Vector2 GetReactionForce(float inv_dt) - { - return inv_dt * _impulse; - } - - public override float GetReactionTorque(float inv_dt) - { - return inv_dt * 0.0f; - } - - /// - /// Use this to update the target point. - /// - public void SetTarget(Vector2 target) - { - if (_body2.IsSleeping()) - { - _body2.WakeUp(); - } - _target = target; - } - - public MouseJoint(MouseJointDef def) - : base(def) - { - _target = def.Target; - _localAnchor = _body2.GetTransform().InverseTransformPoint(_target); - - _maxForce = def.MaxForce; - _impulse = Vector2.Zero; - - _frequencyHz = def.FrequencyHz; - _dampingRatio = def.DampingRatio; - - _beta = 0.0f; - _gamma = 0.0f; - } - - internal override void InitVelocityConstraints(TimeStep step) - { - Body b = _body2; - - float mass = b.GetMass(); - - // Frequency - float omega = 2.0f * Settings.Pi * _frequencyHz; - - // 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. - Box2DNetDebug.Assert(d + step.Dt * k > Settings.FLT_EPSILON); - _gamma = 1.0f / (step.Dt * (d + step.Dt * k)); - _beta = step.Dt * k * _gamma; - - // Compute the effective mass matrix. - Vector2 r = b.GetTransform().TransformDirection(_localAnchor - b.GetLocalCenter()); - - // 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(); - K1.Col1.X = invMass; K1.Col2.X = 0.0f; - K1.Col1.Y = 0.0f; K1.Col2.Y = invMass; - - Mat22 K2 = new Mat22(); - K2.Col1.X = invI * r.Y * r.Y; K2.Col2.X = -invI * r.X * r.Y; - K2.Col1.Y = -invI * r.X * r.Y; K2.Col2.Y = invI * r.X * r.X; - - Mat22 K = K1 + K2; - K.Col1.X += _gamma; - K.Col2.Y += _gamma; - - _mass = K.GetInverse(); - - _C = b._sweep.C + r - _target; - - // Cheat with some damping - b._angularVelocity *= 0.98f; - - // Warm starting. - _impulse *= step.DtRatio; - b._linearVelocity += invMass * _impulse; - b._angularVelocity += invI * r.Cross(_impulse); - } - - internal override void SolveVelocityConstraints(TimeStep step) - { - Body b = _body2; - - Vector2 r = b.GetTransform().TransformDirection(_localAnchor - b.GetLocalCenter()); - - // Cdot = v + cross(w, r) - Vector2 Cdot = b._linearVelocity + r.CrossScalarPreMultiply(b._angularVelocity); - Vector2 impulse = _mass.Multiply(-(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._linearVelocity += b._invMass * impulse; - b._angularVelocity += b._invI * r.Cross(impulse); - } - - internal override bool SolvePositionConstraints(float baumgarte) - { - return true; - } - } -} diff --git a/src/Box2DNet/Dynamics/Joints/PrismaticJoint.cs b/src/Box2DNet/Dynamics/Joints/PrismaticJoint.cs index 455d5b1..7e88ead 100644 --- a/src/Box2DNet/Dynamics/Joints/PrismaticJoint.cs +++ b/src/Box2DNet/Dynamics/Joints/PrismaticJoint.cs @@ -1,759 +1,768 @@ -/* - Box2DNet Copyright (c) 2018 codeyu https://github.com/codeyu/Box2DNet - Box2D original C++ version Copyright (c) 2006-2007 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. -*/ - -// 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 - -using System; using System.Numerics; -using System.Collections.Generic; -using System.Text; - -using Box2DNet.Common; - - -using Transform = Box2DNet.Common.Transform; - -namespace Box2DNet.Dynamics -{ - using Box2DNetMath = Box2DNet.Common.Math; - using SystemMath = System.Math; - - /// - /// Prismatic joint definition. 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. - /// - public class PrismaticJointDef : JointDef - { - public PrismaticJointDef() - { - Type = JointType.PrismaticJoint; - LocalAnchor1 = Vector2.Zero; - LocalAnchor2 = Vector2.Zero; - LocalAxis1 = new Vector2(1.0f, 0.0f); - ReferenceAngle = 0.0f; - EnableLimit = false; - LowerTranslation = 0.0f; - UpperTranslation = 0.0f; - EnableMotor = false; - MaxMotorForce = 0.0f; - MotorSpeed = 0.0f; - } - - /// - /// Initialize the bodies, anchors, axis, and reference angle using the world - /// anchor and world axis. - /// - public void Initialize(Body body1, Body body2, Vector2 anchor, Vector2 axis) - { - Body1 = body1; - Body2 = body2; - LocalAnchor1 = body1.GetLocalPoint(anchor); - LocalAnchor2 = body2.GetLocalPoint(anchor); - LocalAxis1 = body1.GetLocalVector(axis); - ReferenceAngle = body2.GetAngle() - body1.GetAngle(); - } - - /// - /// The local anchor point relative to body1's origin. - /// - public Vector2 LocalAnchor1; - - /// - /// The local anchor point relative to body2's origin. - /// - public Vector2 LocalAnchor2; - - /// - /// The local translation axis in body1. - /// - public Vector2 LocalAxis1; - - /// - /// The constrained angle between the bodies: body2_angle - body1_angle. - /// - public float ReferenceAngle; - - /// - /// Enable/disable the joint limit. - /// - public bool EnableLimit; - - /// - /// The lower translation limit, usually in meters. - /// - public float LowerTranslation; - - /// - /// The upper translation limit, usually in meters. - /// - public float UpperTranslation; - - /// - /// Enable/disable the joint motor. - /// - public bool EnableMotor; - - /// - /// The maximum motor torque, usually in N-m. - /// - public float MaxMotorForce; - - /// - /// The desired motor speed in radians per second. - /// - public float MotorSpeed; - } - - /// - /// 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 _localAnchor1; - public Vector2 _localAnchor2; - public Vector2 _localXAxis1; - public Vector2 _localYAxis1; - public float _refAngle; - - public Vector2 _axis, _perp; - public float _s1, _s2; - public float _a1, _a2; - - public Mat33 _K; - public Vector3 _impulse; - - public float _motorMass; // effective mass for motor/limit translational constraint. - public float _motorImpulse; - - public float _lowerTranslation; - public float _upperTranslation; - public float _maxMotorForce; - public float _motorSpeed; - - public bool _enableLimit; - public bool _enableMotor; - public LimitState _limitState; - - public override Vector2 Anchor1 - { - get { return _body1.GetWorldPoint(_localAnchor1); } - } - - public override Vector2 Anchor2 - { - get { return _body2.GetWorldPoint(_localAnchor2); } - } - - 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; - } - - /// - /// Get the current joint translation, usually in meters. - /// - public float JointTranslation - { - get - { - Body b1 = _body1; - Body b2 = _body2; - - Vector2 p1 = b1.GetWorldPoint(_localAnchor1); - Vector2 p2 = b2.GetWorldPoint(_localAnchor2); - Vector2 d = p2 - p1; - Vector2 axis = b1.GetWorldVector(_localXAxis1); - - float translation = Vector2.Dot(d, axis); - return translation; - } - } - - /// - /// Get the current joint translation speed, usually in meters per second. - /// - public float JointSpeed - { - get - { - Body b1 = _body1; - Body b2 = _body2; - - Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter()); - Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter()); - Vector2 p1 = b1._sweep.C + r1; - Vector2 p2 = b2._sweep.C + r2; - Vector2 d = p2 - p1; - Vector2 axis = b1.GetWorldVector(_localXAxis1); - - Vector2 v1 = b1._linearVelocity; - Vector2 v2 = b2._linearVelocity; - float w1 = b1._angularVelocity; - float w2 = b2._angularVelocity; - - float speed = Vector2.Dot(d, axis.CrossScalarPreMultiply(w1)) + Vector2.Dot(axis, v2 + r2.CrossScalarPreMultiply(w2) - v1 - r1.CrossScalarPreMultiply(w1)); - return speed; - } - } - - /// - /// Is the joint limit enabled? - /// - public bool IsLimitEnabled - { - get { return _enableLimit; } - } - - /// - /// Enable/disable the joint limit. - /// - public void EnableLimit(bool flag) - { - _body1.WakeUp(); - _body2.WakeUp(); - _enableLimit = flag; - } - - /// - /// Get the lower joint limit, usually in meters. - /// - public float LowerLimit - { - get { return _lowerTranslation; } - } - - /// - /// Get the upper joint limit, usually in meters. - /// - public float UpperLimit - { - get { return _upperTranslation; } - } - - /// - /// Set the joint limits, usually in meters. - /// - public void SetLimits(float lower, float upper) - { - Box2DNetDebug.Assert(lower <= upper); - _body1.WakeUp(); - _body2.WakeUp(); - _lowerTranslation = lower; - _upperTranslation = upper; - } - - /// - /// Is the joint motor enabled? - /// - public bool IsMotorEnabled - { - get { return _enableMotor; } - } - - /// - /// Enable/disable the joint motor. - /// - public void EnableMotor(bool flag) - { - _body1.WakeUp(); - _body2.WakeUp(); - _enableMotor = flag; - } - - /// - /// Get\Set the motor speed, usually in meters per second. - /// - public float MotorSpeed - { - get { return _motorSpeed; } - set - { - _body1.WakeUp(); - _body2.WakeUp(); - _motorSpeed = value; - } - } - - /// - /// Set the maximum motor force, usually in N. - /// - public void SetMaxMotorForce(float force) - { - _body1.WakeUp(); - _body2.WakeUp(); - _maxMotorForce = Settings.FORCE_SCALE(1.0f) * force; - } - - /// - /// Get the current motor force, usually in N. - /// - public float MotorForce - { - get { return _motorImpulse; } - } - - public PrismaticJoint(PrismaticJointDef def) - : base(def) - { - _localAnchor1 = def.LocalAnchor1; - _localAnchor2 = def.LocalAnchor2; - _localXAxis1 = def.LocalAxis1; - _localYAxis1 = _localXAxis1.CrossScalarPreMultiply(1.0f); - _refAngle = def.ReferenceAngle; - - _impulse = Vector3.Zero; - _motorMass = 0.0f; - _motorImpulse = 0.0f; - - _lowerTranslation = def.LowerTranslation; - _upperTranslation = def.UpperTranslation; - _maxMotorForce = Settings.FORCE_INV_SCALE(def.MaxMotorForce); - _motorSpeed = def.MotorSpeed; - _enableLimit = def.EnableLimit; - _enableMotor = def.EnableMotor; - _limitState = LimitState.InactiveLimit; - - _axis = Vector2.Zero; - _perp= Vector2.Zero; - } - - internal override void InitVelocityConstraints(TimeStep step) - { - Body b1 = _body1; - Body b2 = _body2; - - // You cannot create a prismatic joint between bodies that - // both have fixed rotation. - Box2DNetDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f); - - _localCenter1 = b1.GetLocalCenter(); - _localCenter2 = b2.GetLocalCenter(); - - Transform xf1 = b1.GetTransform(); - Transform xf2 = b2.GetTransform(); - - // Compute the effective masses. - Vector2 r1 = xf1.TransformDirection(_localAnchor1 - _localCenter1); - Vector2 r2 = xf2.TransformDirection(_localAnchor2 - _localCenter2); - Vector2 d = b2._sweep.C + r2 - b1._sweep.C - r1; - - _invMass1 = b1._invMass; - _invI1 = b1._invI; - _invMass2 = b2._invMass; - _invI2 = b2._invI; - - // Compute motor Jacobian and effective mass. - { - _axis = xf1.TransformDirection(_localXAxis1); - _a1 = (d + r1).Cross(_axis); - _a2 = r2.Cross(_axis); - - _motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2; - Box2DNetDebug.Assert(_motorMass > Settings.FLT_EPSILON); - _motorMass = 1.0f / _motorMass; - } - - // Prismatic constraint. - { - _perp = xf1.TransformDirection(_localYAxis1); - - _s1 = (d + r1).Cross(_perp); - _s2 = r2.Cross(_perp); - - float m1 = _invMass1, m2 = _invMass2; - float i1 = _invI1, i2 = _invI2; - - 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 (Box2DNet.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) - { - _limitState = LimitState.EqualLimits; - } - else if (jointTranslation <= _lowerTranslation) - { - if (_limitState != LimitState.AtLowerLimit) - { - _limitState = LimitState.AtLowerLimit; - _impulse.Z = 0.0f; - } - } - else if (jointTranslation >= _upperTranslation) - { - if (_limitState != LimitState.AtUpperLimit) - { - _limitState = LimitState.AtUpperLimit; - _impulse.Z = 0.0f; - } - } - else - { - _limitState = LimitState.InactiveLimit; - _impulse.Z = 0.0f; - } - } - else - { - _limitState = LimitState.InactiveLimit; - } - - if (_enableMotor == false) - { - _motorImpulse = 0.0f; - } - - if (step.WarmStarting) - { - // 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._linearVelocity -= _invMass1 * P; - b1._angularVelocity -= _invI1 * L1; - - b2._linearVelocity += _invMass2 * P; - b2._angularVelocity += _invI2 * L2; - } - else - { - _impulse = Vector3.Zero; - _motorImpulse = 0.0f; - } - } - - internal override void SolveVelocityConstraints(TimeStep step) - { - Body b1 = _body1; - Body b2 = _body2; - - Vector2 v1 = b1._linearVelocity; - float w1 = b1._angularVelocity; - Vector2 v2 = b2._linearVelocity; - float w2 = b2._angularVelocity; - - // Solve linear motor constraint. - if (_enableMotor && _limitState != LimitState.EqualLimits) - { - 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 = Box2DNet.Common.Math.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); - impulse = _motorImpulse - oldImpulse; - - Vector2 P = impulse * _axis; - float L1 = impulse * _a1; - float L2 = impulse * _a2; - - v1 -= _invMass1 * P; - w1 -= _invI1 * L1; - - v2 += _invMass2 * P; - w2 += _invI2 * L2; - } - - Vector2 Cdot1; - Cdot1.X = Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1; - Cdot1.Y = w2 - w1; - - if (_enableLimit && _limitState != LimitState.InactiveLimit) - { - // Solve prismatic and limit constraint in block form. - float Cdot2; - 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.AtLowerLimit) - { - _impulse.Z = System.Math.Max(_impulse.Z, 0.0f); - } - else if (_limitState == LimitState.AtUpperLimit) - { - _impulse.Z = System.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 -= _invMass1 * P; - w1 -= _invI1 * L1; - - v2 += _invMass2 * P; - w2 += _invI2 * 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 -= _invMass1 * P; - w1 -= _invI1 * L1; - - v2 += _invMass2 * P; - w2 += _invI2 * L2; - } - - b1._linearVelocity = v1; - b1._angularVelocity = w1; - b2._linearVelocity = v2; - b2._angularVelocity = w2; - } - - internal override bool SolvePositionConstraints(float baumgarte) - { - Body b1 = _body1; - Body b2 = _body2; - - 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, angularError = 0.0f; - bool active = false; - float C2 = 0.0f; - - Mat22 R1 = new Mat22(a1), R2 = new Mat22(a2); - - Vector2 r1 = Box2DNet.Common.Math.Mul(R1, _localAnchor1 - _localCenter1); - Vector2 r2 = Box2DNet.Common.Math.Mul(R2, _localAnchor2 - _localCenter2); - Vector2 d = c2 + r2 - c1 - r1; - - if (_enableLimit) - { - _axis = Box2DNet.Common.Math.Mul(R1, _localXAxis1); - - _a1 = (d + r1).Cross(_axis); - _a2 = r2.Cross(_axis); - - float translation = Vector2.Dot(_axis, d); - if (Box2DNet.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) - { - // Prevent large angular corrections - C2 = Box2DNet.Common.Math.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); - linearError = Box2DNet.Common.Math.Abs(translation); - active = true; - } - else if (translation <= _lowerTranslation) - { - // Prevent large linear corrections and allow some slop. - C2 = Box2DNet.Common.Math.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 = Box2DNet.Common.Math.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection); - linearError = translation - _upperTranslation; - active = true; - } - } - - _perp = Box2DNet.Common.Math.Mul(R1, _localYAxis1); - - _s1 = (d + r1).Cross(_perp); - _s2 = r2.Cross(_perp); - - Vector3 impulse; - Vector2 C1 = new Vector2(); - C1.X = Vector2.Dot(_perp, d); - C1.Y = a2 - a1 - _refAngle; - - linearError = Box2DNet.Common.Math.Max(linearError, Box2DNet.Common.Math.Abs(C1.X)); - angularError = Box2DNet.Common.Math.Abs(C1.Y); - - if (active) - { - float m1 = _invMass1, m2 = _invMass2; - float i1 = _invI1, i2 = _invI2; - - 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(); - C.X = C1.X; - C.Y = C1.Y; - C.Z = C2; - - impulse = _K.Solve33(-C); - } - else - { - float m1 = _invMass1, m2 = _invMass2; - float i1 = _invI1, i2 = _invI2; - - 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 -= _invMass1 * P; - a1 -= _invI1 * L1; - c2 += _invMass2 * P; - a2 += _invI2 * 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; - } - } -} +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using Box2DNet.Common; +using Microsoft.Xna.Framework; + +namespace Box2DNet.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 bodyA. 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 + { + private Vector2 _localYAxisA; + private Vector3 _impulse; + private float _lowerTranslation; + private float _upperTranslation; + private float _maxMotorForce; + private float _motorSpeed; + private bool _enableLimit; + private bool _enableMotor; + private LimitState _limitState; + + // Solver temp + private int _indexA; + private int _indexB; + private Vector2 _localCenterA; + private Vector2 _localCenterB; + private float _invMassA; + private float _invMassB; + private float _invIA; + private float _invIB; + private Vector2 _axis, _perp; + private float _s1, _s2; + private float _a1, _a2; + private Mat33 _K; + private float _motorMass; + private Vector2 _axis1; + + 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. + /// Set to true if you are using world coordinates as anchors. + public PrismaticJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, Vector2 axis, bool useWorldCoordinates = false) + : base(bodyA, bodyB) + { + Initialize(anchorA, anchorB, axis, useWorldCoordinates); + } + + public PrismaticJoint(Body bodyA, Body bodyB, Vector2 anchor, Vector2 axis, bool useWorldCoordinates = false) + : base(bodyA, bodyB) + { + Initialize(anchor, anchor, axis, useWorldCoordinates); + } + + private void Initialize(Vector2 localAnchorA, Vector2 localAnchorB, Vector2 axis, bool useWorldCoordinates) + { + JointType = JointType.Prismatic; + + if (useWorldCoordinates) + { + LocalAnchorA = BodyA.GetLocalPoint(localAnchorA); + LocalAnchorB = BodyB.GetLocalPoint(localAnchorB); + } + else + { + LocalAnchorA = localAnchorA; + LocalAnchorB = localAnchorB; + } + + Axis = axis; //FPE only: store the orignal value for use in Serialization + ReferenceAngle = BodyB.Rotation - BodyA.Rotation; + + _limitState = LimitState.Inactive; + } + + /// + /// The local anchor point on BodyA + /// + public Vector2 LocalAnchorA { get; set; } + + /// + /// The local anchor point on BodyB + /// + public Vector2 LocalAnchorB { get; set; } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + set { LocalAnchorA = BodyA.GetLocalPoint(value); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { LocalAnchorB = BodyB.GetLocalPoint(value); } + } + + /// + /// Get the current joint translation, usually in meters. + /// + /// + public float JointTranslation + { + get + { + Vector2 d = BodyB.GetWorldPoint(LocalAnchorB) - BodyA.GetWorldPoint(LocalAnchorA); + Vector2 axis = BodyA.GetWorldVector(LocalXAxis); + + 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.Mul(ref xf1.q, LocalAnchorA - BodyA.LocalCenter); + Vector2 r2 = MathUtils.Mul(ref xf2.q, LocalAnchorB - BodyB.LocalCenter); + Vector2 p1 = BodyA._sweep.C + r1; + Vector2 p2 = BodyB._sweep.C + r2; + Vector2 d = p2 - p1; + Vector2 axis = BodyA.GetWorldVector(LocalXAxis); + + Vector2 v1 = BodyA._linearVelocity; + Vector2 v2 = BodyB._linearVelocity; + float w1 = BodyA._angularVelocity; + float w2 = BodyB._angularVelocity; + + float speed = Vector2.Dot(d, MathUtils.Cross(w1, axis)) + Vector2.Dot(axis, v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1)); + 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"); + + if (value != _enableLimit) + { + WakeBodies(); + _enableLimit = value; + _impulse.Z = 0; + } + } + } + + /// + /// Get the lower joint limit, usually in meters. + /// + /// + public float LowerLimit + { + get { return _lowerTranslation; } + set + { + if (value != _lowerTranslation) + { + WakeBodies(); + _lowerTranslation = value; + _impulse.Z = 0.0f; + } + } + } + + /// + /// Get the upper joint limit, usually in meters. + /// + /// + public float UpperLimit + { + get { return _upperTranslation; } + set + { + if (value != _upperTranslation) + { + WakeBodies(); + _upperTranslation = value; + _impulse.Z = 0.0f; + } + } + } + + /// + /// Set the joint limits, usually in meters. + /// + /// The lower limit + /// The upper limit + public void SetLimits(float lower, float upper) + { + if (upper != _upperTranslation || lower != _lowerTranslation) + { + WakeBodies(); + _upperTranslation = upper; + _lowerTranslation = lower; + _impulse.Z = 0.0f; + } + } + + /// + /// 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 impulse, usually in N. + /// + /// + public float MotorImpulse { get; set; } + + /// + /// Gets the motor force. + /// + /// The inverse delta time + public float GetMotorForce(float invDt) + { + return invDt * MotorImpulse; + } + + /// + /// The axis at which the joint moves. + /// + public Vector2 Axis + { + get { return _axis1; } + set + { + _axis1 = value; + LocalXAxis = BodyA.GetLocalVector(_axis1); + LocalXAxis.Normalize(); + _localYAxisA = MathUtils.Cross(1.0f, LocalXAxis); + } + } + + /// + /// The axis in local coordinates relative to BodyA + /// + public Vector2 LocalXAxis { get; private set; } + + /// + /// The reference angle. + /// + public float ReferenceAngle { get; set; } + + public override Vector2 GetReactionForce(float invDt) + { + return invDt * (_impulse.X * _perp + (MotorImpulse + _impulse.Z) * _axis); + } + + public override float GetReactionTorque(float invDt) + { + return invDt * _impulse.Y; + } + + internal override void InitVelocityConstraints(ref SolverData data) + { + _indexA = BodyA.IslandIndex; + _indexB = BodyB.IslandIndex; + _localCenterA = BodyA._sweep.LocalCenter; + _localCenterB = BodyB._sweep.LocalCenter; + _invMassA = BodyA._invMass; + _invMassB = BodyB._invMass; + _invIA = BodyA._invI; + _invIB = BodyB._invI; + + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + // Compute the effective masses. + Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + Vector2 d = (cB - cA) + rB - rA; + + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + // Compute motor Jacobian and effective mass. + { + _axis = MathUtils.Mul(qA, LocalXAxis); + _a1 = MathUtils.Cross(d + rA, _axis); + _a2 = MathUtils.Cross(rB, _axis); + + _motorMass = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2; + if (_motorMass > 0.0f) + { + _motorMass = 1.0f / _motorMass; + } + } + + // Prismatic constraint. + { + _perp = MathUtils.Mul(qA, _localYAxisA); + + _s1 = MathUtils.Cross(d + rA, _perp); + _s2 = MathUtils.Cross(rB, _perp); + + float k11 = mA + mB + iA * _s1 * _s1 + iB * _s2 * _s2; + float k12 = iA * _s1 + iB * _s2; + float k13 = iA * _s1 * _a1 + iB * _s2 * _a2; + float k22 = iA + iB; + if (k22 == 0.0f) + { + // For bodies with fixed rotation. + k22 = 1.0f; + } + float k23 = iA * _a1 + iB * _a2; + float k33 = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2; + + _K.ex = new Vector3(k11, k12, k13); + _K.ey = new Vector3(k12, k22, k23); + _K.ez = 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; + _impulse.Z = 0.0f; + } + + if (_enableMotor == false) + { + MotorImpulse = 0.0f; + } + + if (Settings.EnableWarmstarting) + { + // Account for variable time step. + _impulse *= data.step.dtRatio; + MotorImpulse *= data.step.dtRatio; + + Vector2 P = _impulse.X * _perp + (MotorImpulse + _impulse.Z) * _axis; + float LA = _impulse.X * _s1 + _impulse.Y + (MotorImpulse + _impulse.Z) * _a1; + float LB = _impulse.X * _s2 + _impulse.Y + (MotorImpulse + _impulse.Z) * _a2; + + vA -= mA * P; + wA -= iA * LA; + + vB += mB * P; + wB += iB * LB; + } + else + { + _impulse = Vector3.Zero; + MotorImpulse = 0.0f; + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override void SolveVelocityConstraints(ref SolverData data) + { + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + // Solve linear motor constraint. + if (_enableMotor && _limitState != LimitState.Equal) + { + float Cdot = Vector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA; + float impulse = _motorMass * (_motorSpeed - Cdot); + float oldImpulse = MotorImpulse; + float maxImpulse = data.step.dt * _maxMotorForce; + MotorImpulse = MathUtils.Clamp(MotorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = MotorImpulse - oldImpulse; + + Vector2 P = impulse * _axis; + float LA = impulse * _a1; + float LB = impulse * _a2; + + vA -= mA * P; + wA -= iA * LA; + + vB += mB * P; + wB += iB * LB; + } + + Vector2 Cdot1 = new Vector2(); + Cdot1.X = Vector2.Dot(_perp, vB - vA) + _s2 * wB - _s1 * wA; + Cdot1.Y = wB - wA; + + if (_enableLimit && _limitState != LimitState.Inactive) + { + // Solve prismatic and limit constraint in block form. + float Cdot2; + Cdot2 = Vector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA; + 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.ez.X, _K.ez.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 LA = df.X * _s1 + df.Y + df.Z * _a1; + float LB = df.X * _s2 + df.Y + df.Z * _a2; + + vA -= mA * P; + wA -= iA * LA; + + vB += mB * P; + wB += iB * LB; + } + 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 LA = df.X * _s1 + df.Y; + float LB = df.X * _s2 + df.Y; + + vA -= mA * P; + wA -= iA * LA; + + vB += mB * P; + wB += iB * LB; + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override bool SolvePositionConstraints(ref SolverData data) + { + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + // Compute fresh Jacobians + Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + Vector2 d = cB + rB - cA - rA; + + Vector2 axis = MathUtils.Mul(qA, LocalXAxis); + float a1 = MathUtils.Cross(d + rA, axis); + float a2 = MathUtils.Cross(rB, axis); + Vector2 perp = MathUtils.Mul(qA, _localYAxisA); + + float s1 = MathUtils.Cross(d + rA, perp); + float s2 = MathUtils.Cross(rB, perp); + + Vector3 impulse; + Vector2 C1 = new Vector2(); + C1.X = Vector2.Dot(perp, d); + C1.Y = aB - aA - ReferenceAngle; + + float linearError = Math.Abs(C1.X); + float angularError = Math.Abs(C1.Y); + + bool active = false; + float C2 = 0.0f; + if (_enableLimit) + { + 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.Max(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 = Math.Max(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 = Math.Max(linearError, translation - _upperTranslation); + active = true; + } + } + + if (active) + { + float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; + float k12 = iA * s1 + iB * s2; + float k13 = iA * s1 * a1 + iB * s2 * a2; + float k22 = iA + iB; + if (k22 == 0.0f) + { + // For fixed rotation + k22 = 1.0f; + } + float k23 = iA * a1 + iB * a2; + float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2; + + Mat33 K = new Mat33(); + K.ex = new Vector3(k11, k12, k13); + K.ey = new Vector3(k12, k22, k23); + K.ez = new Vector3(k13, k23, k33); + + Vector3 C = new Vector3(); + C.X = C1.X; + C.Y = C1.Y; + C.Z = C2; + + impulse = K.Solve33(-C); + } + else + { + float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; + float k12 = iA * s1 + iB * s2; + float k22 = iA + iB; + if (k22 == 0.0f) + { + k22 = 1.0f; + } + + Mat22 K = new Mat22(); + K.ex = new Vector2(k11, k12); + K.ey = new Vector2(k12, k22); + + Vector2 impulse1 = K.Solve(-C1); + impulse = new Vector3(); + impulse.X = impulse1.X; + impulse.Y = impulse1.Y; + impulse.Z = 0.0f; + } + + Vector2 P = impulse.X * perp + impulse.Z * axis; + float LA = impulse.X * s1 + impulse.Y + impulse.Z * a1; + float LB = impulse.X * s2 + impulse.Y + impulse.Z * a2; + + cA -= mA * P; + aA -= iA * LA; + cB += mB * P; + aB += iB * LB; + + data.positions[_indexA].c = cA; + data.positions[_indexA].a = aA; + data.positions[_indexB].c = cB; + data.positions[_indexB].a = aB; + + return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/Joints/PulleyJoint.cs b/src/Box2DNet/Dynamics/Joints/PulleyJoint.cs index 1fc64ff..be7fab8 100644 --- a/src/Box2DNet/Dynamics/Joints/PulleyJoint.cs +++ b/src/Box2DNet/Dynamics/Joints/PulleyJoint.cs @@ -1,567 +1,396 @@ -/* - Box2DNet Copyright (c) 2018 codeyu https://github.com/codeyu/Box2DNet - Box2D original C++ version Copyright (c) 2006-2007 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. -*/ - -// Pulley: -// length1 = norm(p1 - s1) -// length2 = norm(p2 - s2) -// C0 = (length1 + ratio * length2)_initial -// C = C0 - (length1 + ratio * length2) >= 0 -// u1 = (p1 - s1) / norm(p1 - s1) -// u2 = (p2 - s2) / norm(p2 - s2) -// Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2)) -// J = -[u1 cross(r1, u1) ratio * u2 ratio * cross(r2, u2)] -// K = J * invM * JT -// = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2) -// -// Limit: -// C = maxLength - length -// u = (p - s) / norm(p - s) -// Cdot = -dot(u, v + cross(w, r)) -// K = invMass + invI * cross(r, u)^2 -// 0 <= impulse - -using System; using System.Numerics; -using System.Collections.Generic; -using System.Text; - -using Box2DNet.Common; - - -namespace Box2DNet.Dynamics -{ - using Box2DNetMath = Box2DNet.Common.Math; - using SystemMath = System.Math; - - /// - /// Pulley joint definition. This requires two ground anchors, - /// two dynamic body anchor points, max lengths for each side, - /// and a pulley ratio. - /// - public class PulleyJointDef : JointDef - { - public PulleyJointDef() - { - Type = JointType.PulleyJoint; - GroundAnchor1 = new Vector2(-1.0f, 1.0f); - GroundAnchor2 = new Vector2(1.0f, 1.0f); - LocalAnchor1 = new Vector2(-1.0f, 0.0f); - LocalAnchor2 = new Vector2(1.0f, 0.0f); - Length1 = 0.0f; - MaxLength1 = 0.0f; - Length2 = 0.0f; - MaxLength2 = 0.0f; - Ratio = 1.0f; - CollideConnected = true; - } - - /// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors. - public void Initialize(Body body1, Body body2, - Vector2 groundAnchor1, Vector2 groundAnchor2, - Vector2 anchor1, Vector2 anchor2, - float ratio) - { - Body1 = body1; - Body2 = body2; - GroundAnchor1 = groundAnchor1; - GroundAnchor2 = groundAnchor2; - LocalAnchor1 = body1.GetLocalPoint(anchor1); - LocalAnchor2 = body2.GetLocalPoint(anchor2); - Vector2 d1 = anchor1 - groundAnchor1; - Length1 = d1.Length(); - Vector2 d2 = anchor2 - groundAnchor2; - Length2 = d2.Length(); - Ratio = ratio; - Box2DNetDebug.Assert(ratio > Settings.FLT_EPSILON); - float C = Length1 + ratio * Length2; - MaxLength1 = C - ratio * PulleyJoint.MinPulleyLength; - MaxLength2 = (C - PulleyJoint.MinPulleyLength) / ratio; - } - - /// - /// The first ground anchor in world coordinates. This point never moves. - /// - public Vector2 GroundAnchor1; - - /// - /// The second ground anchor in world coordinates. This point never moves. - /// - public Vector2 GroundAnchor2; - - /// - /// The local anchor point relative to body1's origin. - /// - public Vector2 LocalAnchor1; - - /// - /// The local anchor point relative to body2's origin. - /// - public Vector2 LocalAnchor2; - - /// - /// The a reference length for the segment attached to body1. - /// - public float Length1; - - /// - /// The maximum length of the segment attached to body1. - /// - public float MaxLength1; - - /// - /// The a reference length for the segment attached to body2. - /// - public float Length2; - - /// - /// The maximum length of the segment attached to body2. - /// - public float MaxLength2; - - /// - /// The pulley ratio, used to simulate a block-and-tackle. - /// - public float Ratio; - } - - /// - /// The pulley joint is connected to two bodies and two fixed ground points. - /// The pulley supports a ratio such that: - /// length1 + ratio * length2 <= constant - /// 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 - { - public static readonly float MinPulleyLength = 2.0f; - - public Body _ground; - public Vector2 _groundAnchor1; - public Vector2 _groundAnchor2; - public Vector2 _localAnchor1; - public Vector2 _localAnchor2; - - public Vector2 _u1; - public Vector2 _u2; - - public float _constant; - public float _ratio; - - public float _maxLength1; - public float _maxLength2; - - // Effective masses - public float _pulleyMass; - public float _limitMass1; - public float _limitMass2; - - // Impulses for accumulation/warm starting. - public float _impulse; - public float _limitImpulse1; - public float _limitImpulse2; - - public LimitState _state; - public LimitState _limitState1; - public LimitState _limitState2; - - public override Vector2 Anchor1 - { - get { return _body1.GetWorldPoint(_localAnchor1); } - } - - public override Vector2 Anchor2 - { - get { return _body2.GetWorldPoint(_localAnchor2); } - } - - 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; - } - - /// - /// Get the first ground anchor. - /// - public Vector2 GroundAnchor1 - { - get { return _ground.GetTransform().position + _groundAnchor1; } - } - - /// - /// Get the second ground anchor. - /// - public Vector2 GroundAnchor2 - { - get { return _ground.GetTransform().position + _groundAnchor2; } - } - - /// - /// Get the current length of the segment attached to body1. - /// - public float Length1 - { - get - { - Vector2 p = _body1.GetWorldPoint(_localAnchor1); - Vector2 s = _ground.GetTransform().position + _groundAnchor1; - Vector2 d = p - s; - return d.Length(); - } - } - - /// - /// Get the current length of the segment attached to body2. - /// - public float Length2 - { - get - { - Vector2 p = _body2.GetWorldPoint(_localAnchor2); - Vector2 s = _ground.GetTransform().position + _groundAnchor2; - Vector2 d = p - s; - return d.Length(); - } - } - - /// - /// Get the pulley ratio. - /// - public float Ratio - { - get { return _ratio; } - } - - public PulleyJoint(PulleyJointDef def) - : base(def) - { - _ground = _body1.GetWorld().GetGroundBody(); - _groundAnchor1 = def.GroundAnchor1 - _ground.GetTransform().position; - _groundAnchor2 = def.GroundAnchor2 - _ground.GetTransform().position; - _localAnchor1 = def.LocalAnchor1; - _localAnchor2 = def.LocalAnchor2; - - Box2DNetDebug.Assert(def.Ratio != 0.0f); - _ratio = def.Ratio; - - _constant = def.Length1 + _ratio * def.Length2; - - _maxLength1 = Common.Math.Min(def.MaxLength1, _constant - _ratio * PulleyJoint.MinPulleyLength); - _maxLength2 = Common.Math.Min(def.MaxLength2, (_constant - PulleyJoint.MinPulleyLength) / _ratio); - - _impulse = 0.0f; - _limitImpulse1 = 0.0f; - _limitImpulse2 = 0.0f; - } - - internal override void InitVelocityConstraints(TimeStep step) - { - Body b1 = _body1; - Body b2 = _body2; - - Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter()); - Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter()); - - Vector2 p1 = b1._sweep.C + r1; - Vector2 p2 = b2._sweep.C + r2; - - Vector2 s1 = _ground.GetTransform().position + _groundAnchor1; - Vector2 s2 = _ground.GetTransform().position + _groundAnchor2; - - // 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 = _constant - length1 - _ratio * length2; - if (C > 0.0f) - { - _state = LimitState.InactiveLimit; - _impulse = 0.0f; - } - else - { - _state = LimitState.AtUpperLimit; - } - - if (length1 < _maxLength1) - { - _limitState1 = LimitState.InactiveLimit; - _limitImpulse1 = 0.0f; - } - else - { - _limitState1 = LimitState.AtUpperLimit; - } - - if (length2 < _maxLength2) - { - _limitState2 = LimitState.InactiveLimit; - _limitImpulse2 = 0.0f; - } - else - { - _limitState2 = LimitState.AtUpperLimit; - } - - // Compute effective mass. - float cr1u1 = r1.Cross(_u1); - float cr2u2 = r2.Cross(_u2); - - _limitMass1 = b1._invMass + b1._invI * cr1u1 * cr1u1; - _limitMass2 = b2._invMass + b2._invI * cr2u2 * cr2u2; - _pulleyMass = _limitMass1 + _ratio * _ratio * _limitMass2; - Box2DNetDebug.Assert(_limitMass1 > Settings.FLT_EPSILON); - Box2DNetDebug.Assert(_limitMass2 > Settings.FLT_EPSILON); - Box2DNetDebug.Assert(_pulleyMass > Settings.FLT_EPSILON); - _limitMass1 = 1.0f / _limitMass1; - _limitMass2 = 1.0f / _limitMass2; - _pulleyMass = 1.0f / _pulleyMass; - - if (step.WarmStarting) - { - // 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._linearVelocity += b1._invMass * P1; - b1._angularVelocity += b1._invI * r1.Cross(P1); - b2._linearVelocity += b2._invMass * P2; - b2._angularVelocity += b2._invI * r2.Cross(P2); - } - else - { - _impulse = 0.0f; - _limitImpulse1 = 0.0f; - _limitImpulse2 = 0.0f; - } - } - - internal override void SolveVelocityConstraints(TimeStep step) - { - Body b1 = _body1; - Body b2 = _body2; - - Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter()); - Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter()); - - if (_state == LimitState.AtUpperLimit) - { - Vector2 v1 = b1._linearVelocity + r1.CrossScalarPreMultiply(b1._angularVelocity); - Vector2 v2 = b2._linearVelocity + r1.CrossScalarPreMultiply(b2._angularVelocity); - - float Cdot = -Vector2.Dot(_u1, v1) - _ratio * Vector2.Dot(_u2, v2); - float impulse = _pulleyMass * (-Cdot); - float oldImpulse = _impulse; - _impulse = Box2DNet.Common.Math.Max(0.0f, _impulse + impulse); - impulse = _impulse - oldImpulse; - - Vector2 P1 = -impulse * _u1; - Vector2 P2 = -_ratio * impulse * _u2; - b1._linearVelocity += b1._invMass * P1; - b1._angularVelocity += b1._invI * r1.Cross(P1); - b2._linearVelocity += b2._invMass * P2; - b2._angularVelocity += b2._invI * r2.Cross(P2); - } - - if (_limitState1 == LimitState.AtUpperLimit) - { - Vector2 v1 = b1._linearVelocity + r1.CrossScalarPreMultiply(b1._angularVelocity); - - float Cdot = -Vector2.Dot(_u1, v1); - float impulse = -_limitMass1 * Cdot; - float oldImpulse = _limitImpulse1; - _limitImpulse1 = Box2DNet.Common.Math.Max(0.0f, _limitImpulse1 + impulse); - impulse = _limitImpulse1 - oldImpulse; - - Vector2 P1 = -impulse * _u1; - b1._linearVelocity += b1._invMass * P1; - b1._angularVelocity += b1._invI * r1.Cross(P1); - } - - if (_limitState2 == LimitState.AtUpperLimit) - { - Vector2 v2 = b2._linearVelocity + r2.CrossScalarPreMultiply(b2._angularVelocity); - - float Cdot = -Vector2.Dot(_u2, v2); - float impulse = -_limitMass2 * Cdot; - float oldImpulse = _limitImpulse2; - _limitImpulse2 = Box2DNet.Common.Math.Max(0.0f, _limitImpulse2 + impulse); - impulse = _limitImpulse2 - oldImpulse; - - Vector2 P2 = -impulse * _u2; - b2._linearVelocity += b2._invMass * P2; - b2._angularVelocity += b2._invI * r2.Cross(P2); - } - } - - internal override bool SolvePositionConstraints(float baumgarte) - { - Body b1 = _body1; - Body b2 = _body2; - - Vector2 s1 = _ground.GetTransform().position + _groundAnchor1; - Vector2 s2 = _ground.GetTransform().position + _groundAnchor2; - - float linearError = 0.0f; - - if (_state == LimitState.AtUpperLimit) - { - Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter()); - Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter()); - - 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 = _constant - length1 - _ratio * length2; - linearError = Box2DNetMath.Max(linearError, -C); - - C = Box2DNet.Common.Math.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 * r1.Cross(P1); - b2._sweep.C += b2._invMass * P2; - b2._sweep.A += b2._invI * r2.Cross(P2); - - b1.SynchronizeTransform(); - b2.SynchronizeTransform(); - } - - if (_limitState1 == LimitState.AtUpperLimit) - { - Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter()); - 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 = _maxLength1 - length1; - linearError = System.Math.Max(linearError, -C); - C = Box2DNet.Common.Math.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 * r1.Cross(P1); - - b1.SynchronizeTransform(); - } - - if (_limitState2 == LimitState.AtUpperLimit) - { - Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter()); - 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 = _maxLength2 - length2; - linearError = Box2DNetMath.Max(linearError, -C); - C = Box2DNet.Common.Math.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 * r2.Cross(P2); - - b2.SynchronizeTransform(); - } - - return linearError < Settings.LinearSlop; - } - } -} +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using System.Diagnostics; +using Box2DNet.Common; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Dynamics.Joints +{ + // Pulley: + // length1 = norm(p1 - s1) + // length2 = norm(p2 - s2) + // C0 = (length1 + ratio * length2)_initial + // C = C0 - (length1 + ratio * length2) + // u1 = (p1 - s1) / norm(p1 - s1) + // u2 = (p2 - s2) / norm(p2 - s2) + // Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2)) + // J = -[u1 cross(r1, u1) ratio * u2 ratio * cross(r2, u2)] + // K = J * invM * JT + // = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2) + + /// + /// The pulley joint is connected to two bodies and two fixed world points. + /// The pulley supports a ratio such that: + /// + /// Yes, the force transmitted is scaled by the ratio. + /// + /// Warning: the pulley joint can get a bit squirrelly by itself. They often + /// work better when combined with prismatic joints. You should also cover the + /// the anchor points with static shapes to prevent one side from going to zero length. + /// + public class PulleyJoint : Joint + { + // Solver shared + private float _impulse; + + // Solver temp + private int _indexA; + private int _indexB; + private Vector2 _uA; + private Vector2 _uB; + private Vector2 _rA; + private Vector2 _rB; + private Vector2 _localCenterA; + private Vector2 _localCenterB; + private float _invMassA; + private float _invMassB; + private float _invIA; + private float _invIB; + private float _mass; + + internal PulleyJoint() + { + JointType = JointType.Pulley; + } + + /// + /// Constructor for PulleyJoint. + /// + /// The first body. + /// The second body. + /// The anchor on the first body. + /// The anchor on the second body. + /// The world anchor for the first body. + /// The world anchor for the second body. + /// The ratio. + /// Set to true if you are using world coordinates as anchors. + public PulleyJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, Vector2 worldAnchorA, Vector2 worldAnchorB, float ratio, bool useWorldCoordinates = false) + : base(bodyA, bodyB) + { + JointType = JointType.Pulley; + + WorldAnchorA = worldAnchorA; + WorldAnchorB = worldAnchorB; + + if (useWorldCoordinates) + { + LocalAnchorA = BodyA.GetLocalPoint(anchorA); + LocalAnchorB = BodyB.GetLocalPoint(anchorB); + + Vector2 dA = anchorA - worldAnchorA; + LengthA = dA.Length(); + Vector2 dB = anchorB - worldAnchorB; + LengthB = dB.Length(); + } + else + { + LocalAnchorA = anchorA; + LocalAnchorB = anchorB; + + Vector2 dA = anchorA - BodyA.GetLocalPoint(worldAnchorA); + LengthA = dA.Length(); + Vector2 dB = anchorB - BodyB.GetLocalPoint(worldAnchorB); + LengthB = dB.Length(); + } + + Debug.Assert(ratio != 0.0f); + Debug.Assert(ratio > Settings.Epsilon); + + Ratio = ratio; + Constant = LengthA + ratio * LengthB; + _impulse = 0.0f; + } + + /// + /// The local anchor point on BodyA + /// + public Vector2 LocalAnchorA { get; set; } + + /// + /// The local anchor point on BodyB + /// + public Vector2 LocalAnchorB { get; set; } + + /// + /// Get the first world anchor. + /// + /// + public override sealed Vector2 WorldAnchorA { get; set; } + + /// + /// Get the second world anchor. + /// + /// + public override sealed Vector2 WorldAnchorB { get; set; } + + /// + /// Get the current length of the segment attached to body1. + /// + /// + public float LengthA { get; set; } + + /// + /// Get the current length of the segment attached to body2. + /// + /// + public float LengthB { get; set; } + + /// + /// The current length between the anchor point on BodyA and WorldAnchorA + /// + public float CurrentLengthA + { + get + { + Vector2 p = BodyA.GetWorldPoint(LocalAnchorA); + Vector2 s = WorldAnchorA; + Vector2 d = p - s; + return d.Length(); + } + } + + /// + /// The current length between the anchor point on BodyB and WorldAnchorB + /// + public float CurrentLengthB + { + get + { + Vector2 p = BodyB.GetWorldPoint(LocalAnchorB); + Vector2 s = WorldAnchorB; + Vector2 d = p - s; + return d.Length(); + } + } + + /// + /// Get the pulley ratio. + /// + /// + public float Ratio { get; set; } + + //FPE note: Only used for serialization. + internal float Constant { get; set; } + + public override Vector2 GetReactionForce(float invDt) + { + Vector2 P = _impulse * _uB; + return invDt * P; + } + + public override float GetReactionTorque(float invDt) + { + return 0.0f; + } + + internal override void InitVelocityConstraints(ref SolverData data) + { + _indexA = BodyA.IslandIndex; + _indexB = BodyB.IslandIndex; + _localCenterA = BodyA._sweep.LocalCenter; + _localCenterB = BodyB._sweep.LocalCenter; + _invMassA = BodyA._invMass; + _invMassB = BodyB._invMass; + _invIA = BodyA._invI; + _invIB = BodyB._invI; + + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + + // Get the pulley axes. + _uA = cA + _rA - WorldAnchorA; + _uB = cB + _rB - WorldAnchorB; + + float lengthA = _uA.Length(); + float lengthB = _uB.Length(); + + if (lengthA > 10.0f * Settings.LinearSlop) + { + _uA *= 1.0f / lengthA; + } + else + { + _uA = Vector2.Zero; + } + + if (lengthB > 10.0f * Settings.LinearSlop) + { + _uB *= 1.0f / lengthB; + } + else + { + _uB = Vector2.Zero; + } + + // Compute effective mass. + float ruA = MathUtils.Cross(_rA, _uA); + float ruB = MathUtils.Cross(_rB, _uB); + + float mA = _invMassA + _invIA * ruA * ruA; + float mB = _invMassB + _invIB * ruB * ruB; + + _mass = mA + Ratio * Ratio * mB; + + if (_mass > 0.0f) + { + _mass = 1.0f / _mass; + } + + if (Settings.EnableWarmstarting) + { + // Scale impulses to support variable time steps. + _impulse *= data.step.dtRatio; + + // Warm starting. + Vector2 PA = -(_impulse) * _uA; + Vector2 PB = (-Ratio * _impulse) * _uB; + + vA += _invMassA * PA; + wA += _invIA * MathUtils.Cross(_rA, PA); + vB += _invMassB * PB; + wB += _invIB * MathUtils.Cross(_rB, PB); + } + else + { + _impulse = 0.0f; + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override void SolveVelocityConstraints(ref SolverData data) + { + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + Vector2 vpA = vA + MathUtils.Cross(wA, _rA); + Vector2 vpB = vB + MathUtils.Cross(wB, _rB); + + float Cdot = -Vector2.Dot(_uA, vpA) - Ratio * Vector2.Dot(_uB, vpB); + float impulse = -_mass * Cdot; + _impulse += impulse; + + Vector2 PA = -impulse * _uA; + Vector2 PB = -Ratio * impulse * _uB; + vA += _invMassA * PA; + wA += _invIA * MathUtils.Cross(_rA, PA); + vB += _invMassB * PB; + wB += _invIB * MathUtils.Cross(_rB, PB); + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override bool SolvePositionConstraints(ref SolverData data) + { + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + + // Get the pulley axes. + Vector2 uA = cA + rA - WorldAnchorA; + Vector2 uB = cB + rB - WorldAnchorB; + + float lengthA = uA.Length(); + float lengthB = uB.Length(); + + if (lengthA > 10.0f * Settings.LinearSlop) + { + uA *= 1.0f / lengthA; + } + else + { + uA = Vector2.Zero; + } + + if (lengthB > 10.0f * Settings.LinearSlop) + { + uB *= 1.0f / lengthB; + } + else + { + uB = Vector2.Zero; + } + + // Compute effective mass. + float ruA = MathUtils.Cross(rA, uA); + float ruB = MathUtils.Cross(rB, uB); + + float mA = _invMassA + _invIA * ruA * ruA; + float mB = _invMassB + _invIB * ruB * ruB; + + float mass = mA + Ratio * Ratio * mB; + + if (mass > 0.0f) + { + mass = 1.0f / mass; + } + + float C = Constant - lengthA - Ratio * lengthB; + float linearError = Math.Abs(C); + + float impulse = -mass * C; + + Vector2 PA = -impulse * uA; + Vector2 PB = -Ratio * impulse * uB; + + cA += _invMassA * PA; + aA += _invIA * MathUtils.Cross(rA, PA); + cB += _invMassB * PB; + aB += _invIB * MathUtils.Cross(rB, PB); + + data.positions[_indexA].c = cA; + data.positions[_indexA].a = aA; + data.positions[_indexB].c = cB; + data.positions[_indexB].a = aB; + + return linearError < Settings.LinearSlop; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/Joints/RevoluteJoint.cs b/src/Box2DNet/Dynamics/Joints/RevoluteJoint.cs index a8b7041..1842583 100644 --- a/src/Box2DNet/Dynamics/Joints/RevoluteJoint.cs +++ b/src/Box2DNet/Dynamics/Joints/RevoluteJoint.cs @@ -1,637 +1,619 @@ -/* - Box2DNet Copyright (c) 2018 codeyu https://github.com/codeyu/Box2DNet - Box2D original C++ version Copyright (c) 2006-2007 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. -*/ - -// 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) - -// Motor constraint -// Cdot = w2 - w1 -// J = [0 0 -1 0 0 1] -// K = invI1 + invI2 - -using System; using System.Numerics; -using System.Collections.Generic; -using System.Text; - -using Box2DNet.Common; - - -namespace Box2DNet.Dynamics -{ - using Box2DNetMath = Box2DNet.Common.Math; - using SystemMath = System.Math; - - /// - /// Revolute joint definition. 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. - /// - public class RevoluteJointDef : JointDef - { - public RevoluteJointDef() - { - Type = JointType.RevoluteJoint; - LocalAnchor1 = new Vector2(0.0f, 0.0f); - LocalAnchor2 = new Vector2(0.0f, 0.0f); - ReferenceAngle = 0.0f; - LowerAngle = 0.0f; - UpperAngle = 0.0f; - MaxMotorTorque = 0.0f; - MotorSpeed = 0.0f; - EnableLimit = false; - EnableMotor = false; - } - - /// - /// Initialize the bodies, anchors, and reference angle using the world - /// anchor. - /// - public void Initialize(Body body1, Body body2, Vector2 anchor) - { - Body1 = body1; - Body2 = body2; - LocalAnchor1 = body1.GetLocalPoint(anchor); - LocalAnchor2 = body2.GetLocalPoint(anchor); - ReferenceAngle = body2.GetAngle() - body1.GetAngle(); - } - - /// - /// The local anchor point relative to body1's origin. - /// - public Vector2 LocalAnchor1; - - /// - /// The local anchor point relative to body2's origin. - /// - public Vector2 LocalAnchor2; - - /// - /// The body2 angle minus body1 angle in the reference state (radians). - /// - public float ReferenceAngle; - - /// - /// A flag to enable joint limits. - /// - public bool EnableLimit; - - /// - /// The lower angle for the joint limit (radians). - /// - public float LowerAngle; - - /// - /// The upper angle for the joint limit (radians). - /// - public float UpperAngle; - - /// - /// A flag to enable the joint motor. - /// - public bool EnableMotor; - - /// - /// The desired motor speed. Usually in radians per second. - /// - public float MotorSpeed; - - /// - /// The maximum motor torque used to achieve the desired motor speed. - /// Usually in N-m. - /// - public float MaxMotorTorque; - } - - /// - /// A revolute joint constrains 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 _localAnchor1; // relative - public Vector2 _localAnchor2; - public Vector3 _impulse; - public float _motorImpulse; - public Mat33 _mass; //effective mass for p2p constraint. - public float _motorMass; // effective mass for motor/limit angular constraint. - - public bool _enableMotor; - public float _maxMotorTorque; - public float _motorSpeed; - - public bool _enableLimit; - public float _referenceAngle; - public float _lowerAngle; - public float _upperAngle; - public LimitState _limitState; - - public override Vector2 Anchor1 - { - get { return _body1.GetWorldPoint(_localAnchor1); } - } - - public override Vector2 Anchor2 - { - get { return _body2.GetWorldPoint(_localAnchor2); } - } - - public override Vector2 GetReactionForce(float inv_dt) - { - Vector2 P = _impulse.ToVector2(); - return inv_dt * P; - } - - public override float GetReactionTorque(float inv_dt) - { - return inv_dt * _impulse.Z; - } - - /// - /// Get the current joint angle in radians. - /// - public float JointAngle - { - get - { - Body b1 = _body1; - Body b2 = _body2; - return b2._sweep.A - b1._sweep.A - _referenceAngle; - } - } - - - /// - /// Get the current joint angle speed in radians per second. - /// - public float JointSpeed - { - get - { - Body b1 = _body1; - Body b2 = _body2; - return b2._angularVelocity - b1._angularVelocity; - } - } - - /// - /// Is the joint limit enabled? - /// - public bool IsLimitEnabled - { - get { return _enableLimit; } - } - - /// - /// Enable/disable the joint limit. - /// - public void EnableLimit(bool flag) - { - _body1.WakeUp(); - _body2.WakeUp(); - _enableLimit = flag; - } - - /// - /// Get the lower joint limit in radians. - /// - public float LowerLimit - { - get { return _lowerAngle; } - } - - /// - /// Get the upper joint limit in radians. - /// - public float UpperLimit - { - get { return _upperAngle; } - } - - /// - /// Set the joint limits in radians. - /// - public void SetLimits(float lower, float upper) - { - Box2DNetDebug.Assert(lower <= upper); - _body1.WakeUp(); - _body2.WakeUp(); - _lowerAngle = lower; - _upperAngle = upper; - } - - /// - /// Is the joint motor enabled? - /// - public bool IsMotorEnabled - { - get { return _enableMotor; } - } - - /// - /// Enable/disable the joint motor. - /// - public void EnableMotor(bool flag) - { - _body1.WakeUp(); - _body2.WakeUp(); - _enableMotor = flag; - } - - /// - /// Get\Set the motor speed in radians per second. - /// - public float MotorSpeed - { - get { return _motorSpeed; } - set - { - _body1.WakeUp(); - _body2.WakeUp(); - _motorSpeed = value; - } - } - - /// - /// Set the maximum motor torque, usually in N-m. - /// - public void SetMaxMotorTorque(float torque) - { - _body1.WakeUp(); - _body2.WakeUp(); - _maxMotorTorque = torque; - } - - /// - /// Get the current motor torque, usually in N-m. - /// - public float MotorTorque - { - get { return _motorImpulse; } - } - - public RevoluteJoint(RevoluteJointDef def) - : base(def) - { - _localAnchor1 = def.LocalAnchor1; - _localAnchor2 = def.LocalAnchor2; - _referenceAngle = def.ReferenceAngle; - - _impulse = Vector3.Zero; - _motorImpulse = 0.0f; - - _lowerAngle = def.LowerAngle; - _upperAngle = def.UpperAngle; - _maxMotorTorque = def.MaxMotorTorque; - _motorSpeed = def.MotorSpeed; - _enableLimit = def.EnableLimit; - _enableMotor = def.EnableMotor; - _limitState = LimitState.InactiveLimit; - } - - internal override void InitVelocityConstraints(TimeStep step) - { - Body b1 = _body1; - Body b2 = _body2; - - if (_enableMotor || _enableLimit) - { - // You cannot create a rotation limit between bodies that - // both have fixed rotation. - Box2DNetDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f); - } - - // Compute the effective mass matrix. - Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter()); - Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter()); - - // 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.Y * i2; - _mass.Col1.Z = _mass.Col3.X; - _mass.Col2.Z = _mass.Col3.Y; - _mass.Col3.Z = i1 + i2; - - _motorMass = 1.0f / (i1 + i2); - - if (_enableMotor == false) - { - _motorImpulse = 0.0f; - } - - if (_enableLimit) - { - float jointAngle = b2._sweep.A - b1._sweep.A - _referenceAngle; - if (Box2DNetMath.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop) - { - _limitState = LimitState.EqualLimits; - } - else if (jointAngle <= _lowerAngle) - { - if (_limitState != LimitState.AtLowerLimit) - { - _impulse.Z = 0.0f; - } - _limitState = LimitState.AtLowerLimit; - } - else if (jointAngle >= _upperAngle) - { - if (_limitState != LimitState.AtUpperLimit) - { - _impulse.Z = 0.0f; - } - _limitState = LimitState.AtUpperLimit; - } - else - { - _limitState = LimitState.InactiveLimit; - _impulse.Z = 0.0f; - } - } - else - { - _limitState = LimitState.InactiveLimit; - } - - if (step.WarmStarting) - { - // Scale impulses to support a variable time step. - _impulse *= step.DtRatio; - _motorImpulse *= step.DtRatio; - - Vector2 P = _impulse.ToVector2(); - - b1._linearVelocity -= m1 * P; - b1._angularVelocity -= i1 * (r1.Cross(P) + _motorImpulse + _impulse.Z); - - b2._linearVelocity += m2 * P; - b2._angularVelocity += i2 * (r2.Cross(P) + _motorImpulse + _impulse.Z); - } - else - { - _impulse = Vector3.Zero; - _motorImpulse = 0.0f; - } - } - - internal override void SolveVelocityConstraints(TimeStep step) - { - Body b1 = _body1; - Body b2 = _body2; - - Vector2 v1 = b1._linearVelocity; - float w1 = b1._angularVelocity; - Vector2 v2 = b2._linearVelocity; - float w2 = b2._angularVelocity; - - float m1 = b1._invMass, m2 = b2._invMass; - float i1 = b1._invI, i2 = b2._invI; - - //Solve motor constraint. - if (_enableMotor && _limitState != LimitState.EqualLimits) - { - float Cdot = w2 - w1 - _motorSpeed; - float impulse = _motorMass * (-Cdot); - float oldImpulse = _motorImpulse; - float maxImpulse = step.Dt * _maxMotorTorque; - _motorImpulse = Box2DNet.Common.Math.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); - impulse = _motorImpulse - oldImpulse; - - w1 -= i1 * impulse; - w2 += i2 * impulse; - } - - //Solve limit constraint. - if (_enableLimit && _limitState != LimitState.InactiveLimit) - { - Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter()); - Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter()); - - // Solve point-to-point constraint - Vector2 Cdot1 = v2 + r2.CrossScalarPreMultiply(w2) - v1 - r1.CrossScalarPreMultiply(w1); - float Cdot2 = w2 - w1; - Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); - - Vector3 impulse = _mass.Solve33(-Cdot); - - if (_limitState == LimitState.EqualLimits) - { - _impulse += impulse; - } - else if (_limitState == LimitState.AtLowerLimit) - { - 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.AtUpperLimit) - { - 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 = impulse.ToVector2(); - - v1 -= m1 * P; - w1 -= i1 * (r1.Cross(P) + impulse.Z); - - v2 += m2 * P; - w2 += i2 * (r2.Cross(P) + impulse.Z); - } - else - { - Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter()); - Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter()); - - // Solve point-to-point constraint - Vector2 Cdot = v2 + r2.CrossScalarPreMultiply(w2) - v1 - r1.CrossScalarPreMultiply(w1); - Vector2 impulse = _mass.Solve22(-Cdot); - - _impulse.X += impulse.X; - _impulse.Y += impulse.Y; - - v1 -= m1 * impulse; - w1 -= i1 * r1.Cross(impulse); - - v2 += m2 * impulse; - w2 += i2 * r2.Cross(impulse); - } - - b1._linearVelocity = v1; - b1._angularVelocity = w1; - b2._linearVelocity = v2; - b2._angularVelocity = w2; - } - - internal override bool SolvePositionConstraints(float baumgarte) - { - // TODO_ERIN block solve with limit. - - Body b1 = _body1; - Body b2 = _body2; - - float angularError = 0.0f; - float positionError = 0.0f; - - // Solve angular limit constraint. - if (_enableLimit && _limitState != LimitState.InactiveLimit) - { - float angle = b2._sweep.A - b1._sweep.A - _referenceAngle; - float limitImpulse = 0.0f; - - if (_limitState == LimitState.EqualLimits) - { - // Prevent large angular corrections - float C = Box2DNet.Common.Math.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection); - limitImpulse = -_motorMass * C; - angularError = Box2DNetMath.Abs(C); - } - else if (_limitState == LimitState.AtLowerLimit) - { - float C = angle - _lowerAngle; - angularError = -C; - - // Prevent large angular corrections and allow some slop. - C = Box2DNet.Common.Math.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f); - limitImpulse = -_motorMass * C; - } - else if (_limitState == LimitState.AtUpperLimit) - { - float C = angle - _upperAngle; - angularError = C; - - // Prevent large angular corrections and allow some slop. - C = Box2DNet.Common.Math.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. - { - Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter()); - Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter()); - - 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. - 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; - Box2DNetDebug.Assert(k > Settings.FLT_EPSILON); - float m = 1.0f / k; - Vector2 impulse = m * (-C); - float k_beta = 0.5f; - b1._sweep.C -= k_beta * invMass1 * impulse; - b2._sweep.C += k_beta * invMass2 * impulse; - - C = b2._sweep.C + r2 - b1._sweep.C - r1; - } - - Mat22 K1 = new Mat22(); - K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f; - K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2; - - Mat22 K2 = new Mat22(); - K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y; - K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X; - - Mat22 K3 = new Mat22(); - K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y; - K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X; - - Mat22 K = K1 + K2 + K3; - Vector2 impulse_ = K.Solve(-C); - - b1._sweep.C -= b1._invMass * impulse_; - b1._sweep.A -= b1._invI * r1.Cross(impulse_); - - b2._sweep.C += b2._invMass * impulse_; - b2._sweep.A += b2._invI * r2.Cross(impulse_); - - b1.SynchronizeTransform(); - b2.SynchronizeTransform(); - } - - return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; - } - } -} +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using Box2DNet.Common; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Dynamics.Joints +{ + /// + /// A revolute joint constrains 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 + { + // Solver shared + private Vector3 _impulse; + private float _motorImpulse; + + private bool _enableMotor; + private float _maxMotorTorque; + private float _motorSpeed; + + private bool _enableLimit; + private float _referenceAngle; + private float _lowerAngle; + private float _upperAngle; + + // Solver temp + private int _indexA; + private int _indexB; + private Vector2 _rA; + private Vector2 _rB; + private Vector2 _localCenterA; + private Vector2 _localCenterB; + private float _invMassA; + private float _invMassB; + private float _invIA; + private float _invIB; + private Mat33 _mass; // effective mass for point-to-point constraint. + private float _motorMass; // effective mass for motor/limit angular constraint. + private LimitState _limitState; + + internal RevoluteJoint() + { + JointType = JointType.Revolute; + } + + /// + /// Constructor of RevoluteJoint. + /// + /// The first body. + /// The second body. + /// The first body anchor. + /// The second anchor. + /// Set to true if you are using world coordinates as anchors. + public RevoluteJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false) + : base(bodyA, bodyB) + { + JointType = JointType.Revolute; + + if (useWorldCoordinates) + { + LocalAnchorA = BodyA.GetLocalPoint(anchorA); + LocalAnchorB = BodyB.GetLocalPoint(anchorB); + } + else + { + LocalAnchorA = anchorA; + LocalAnchorB = anchorB; + } + + ReferenceAngle = BodyB.Rotation - BodyA.Rotation; + + _impulse = Vector3.Zero; + _limitState = LimitState.Inactive; + } + + /// + /// Constructor of RevoluteJoint. + /// + /// The first body. + /// The second body. + /// The shared anchor. + /// + public RevoluteJoint(Body bodyA, Body bodyB, Vector2 anchor, bool useWorldCoordinates = false) + : this(bodyA, bodyB, anchor, anchor, useWorldCoordinates) + { + } + + /// + /// The local anchor point on BodyA + /// + public Vector2 LocalAnchorA { get; set; } + + /// + /// The local anchor point on BodyB + /// + public Vector2 LocalAnchorB { get; set; } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + set { LocalAnchorA = BodyA.GetLocalPoint(value); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { LocalAnchorB = BodyB.GetLocalPoint(value); } + } + + /// + /// The referance angle computed as BodyB angle minus BodyA angle. + /// + 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._angularVelocity - BodyA._angularVelocity; } + } + + /// + /// Is the joint limit enabled? + /// + /// true if [limit enabled]; otherwise, false. + public bool LimitEnabled + { + get { return _enableLimit; } + set + { + if (_enableLimit != value) + { + WakeBodies(); + _enableLimit = value; + _impulse.Z = 0.0f; + } + } + } + + /// + /// Get the lower joint limit in radians. + /// + public float LowerLimit + { + get { return _lowerAngle; } + set + { + if (_lowerAngle != value) + { + WakeBodies(); + _lowerAngle = value; + _impulse.Z = 0.0f; + } + } + } + + /// + /// Get the upper joint limit in radians. + /// + public float UpperLimit + { + get { return _upperAngle; } + set + { + if (_upperAngle != value) + { + WakeBodies(); + _upperAngle = value; + _impulse.Z = 0.0f; + } + } + } + + /// + /// Set the joint limits, usually in meters. + /// + /// The lower limit + /// The upper limit + public void SetLimits(float lower, float upper) + { + if (lower != _lowerAngle || upper != _upperAngle) + { + WakeBodies(); + _upperAngle = upper; + _lowerAngle = lower; + _impulse.Z = 0.0f; + } + } + + /// + /// Is the joint motor enabled? + /// + /// true if [motor enabled]; otherwise, false. + public bool MotorEnabled + { + get { return _enableMotor; } + set + { + WakeBodies(); + _enableMotor = value; + } + } + + /// + /// Get or set the motor speed in radians per second. + /// + public float MotorSpeed + { + set + { + WakeBodies(); + _motorSpeed = value; + } + get { return _motorSpeed; } + } + + /// + /// Get or set the maximum motor torque, usually in N-m. + /// + public float MaxMotorTorque + { + set + { + WakeBodies(); + _maxMotorTorque = value; + } + get { return _maxMotorTorque; } + } + + /// + /// Get or set the current motor impulse, usually in N-m. + /// + public float MotorImpulse + { + get { return _motorImpulse; } + set + { + WakeBodies(); + _motorImpulse = value; + } + } + + /// + /// Gets the motor torque in N-m. + /// + /// The inverse delta time + public float GetMotorTorque(float invDt) + { + return invDt * _motorImpulse; + } + + public override Vector2 GetReactionForce(float invDt) + { + Vector2 p = new Vector2(_impulse.X, _impulse.Y); + return invDt * p; + } + + public override float GetReactionTorque(float invDt) + { + return invDt * _impulse.Z; + } + + internal override void InitVelocityConstraints(ref SolverData data) + { + _indexA = BodyA.IslandIndex; + _indexB = BodyB.IslandIndex; + _localCenterA = BodyA._sweep.LocalCenter; + _localCenterB = BodyB._sweep.LocalCenter; + _invMassA = BodyA._invMass; + _invMassB = BodyB._invMass; + _invIA = BodyA._invI; + _invIB = BodyB._invI; + + float aA = data.positions[_indexA].a; + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + + float aB = data.positions[_indexB].a; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + + // 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 = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + bool fixedRotation = (iA + iB == 0.0f); + + _mass.ex.X = mA + mB + _rA.Y * _rA.Y * iA + _rB.Y * _rB.Y * iB; + _mass.ey.X = -_rA.Y * _rA.X * iA - _rB.Y * _rB.X * iB; + _mass.ez.X = -_rA.Y * iA - _rB.Y * iB; + _mass.ex.Y = _mass.ey.X; + _mass.ey.Y = mA + mB + _rA.X * _rA.X * iA + _rB.X * _rB.X * iB; + _mass.ez.Y = _rA.X * iA + _rB.X * iB; + _mass.ex.Z = _mass.ez.X; + _mass.ey.Z = _mass.ez.Y; + _mass.ez.Z = iA + iB; + + _motorMass = iA + iB; + if (_motorMass > 0.0f) + { + _motorMass = 1.0f / _motorMass; + } + + if (_enableMotor == false || fixedRotation) + { + _motorImpulse = 0.0f; + } + + if (_enableLimit && fixedRotation == false) + { + float jointAngle = aB - aA - 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 *= data.step.dtRatio; + _motorImpulse *= data.step.dtRatio; + + Vector2 P = new Vector2(_impulse.X, _impulse.Y); + + vA -= mA * P; + wA -= iA * (MathUtils.Cross(_rA, P) + MotorImpulse + _impulse.Z); + + vB += mB * P; + wB += iB * (MathUtils.Cross(_rB, P) + MotorImpulse + _impulse.Z); + } + else + { + _impulse = Vector3.Zero; + _motorImpulse = 0.0f; + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override void SolveVelocityConstraints(ref SolverData data) + { + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + bool fixedRotation = (iA + iB == 0.0f); + + // Solve motor constraint. + if (_enableMotor && _limitState != LimitState.Equal && fixedRotation == false) + { + float Cdot = wB - wA - _motorSpeed; + float impulse = _motorMass * (-Cdot); + float oldImpulse = _motorImpulse; + float maxImpulse = data.step.dt * _maxMotorTorque; + _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = _motorImpulse - oldImpulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + // Solve limit constraint. + if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false) + { + 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); + + if (_limitState == LimitState.Equal) + { + _impulse += impulse; + } + else if (_limitState == LimitState.AtLower) + { + float newImpulse = _impulse.Z + impulse.Z; + if (newImpulse < 0.0f) + { + Vector2 rhs = -Cdot1 + _impulse.Z * new Vector2(_mass.ez.X, _mass.ez.Y); + Vector2 reduced = _mass.Solve22(rhs); + 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 + { + _impulse += impulse; + } + } + else if (_limitState == LimitState.AtUpper) + { + float newImpulse = _impulse.Z + impulse.Z; + if (newImpulse > 0.0f) + { + Vector2 rhs = -Cdot1 + _impulse.Z * new Vector2(_mass.ez.X, _mass.ez.Y); + Vector2 reduced = _mass.Solve22(rhs); + 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 + { + _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); + } + else + { + // Solve point-to-point constraint + Vector2 Cdot = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA); + Vector2 impulse = _mass.Solve22(-Cdot); + + _impulse.X += impulse.X; + _impulse.Y += impulse.Y; + + vA -= mA * impulse; + wA -= iA * MathUtils.Cross(_rA, impulse); + + vB += mB * impulse; + wB += iB * MathUtils.Cross(_rB, impulse); + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override bool SolvePositionConstraints(ref SolverData data) + { + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + float angularError = 0.0f; + float positionError; + + bool fixedRotation = (_invIA + _invIB == 0.0f); + + // Solve angular limit constraint. + if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false) + { + float angle = aB - aA - 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; + } + + aA -= _invIA * limitImpulse; + aB += _invIB * limitImpulse; + } + + // Solve point-to-point constraint. + { + qA.Set(aA); + qB.Set(aB); + Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + + Vector2 C = cB + rB - cA - rA; + positionError = C.Length(); + + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + Mat22 K = new Mat22(); + K.ex.X = mA + mB + iA * rA.Y * rA.Y + iB * rB.Y * rB.Y; + K.ex.Y = -iA * rA.X * rA.Y - iB * rB.X * rB.Y; + K.ey.X = K.ex.Y; + K.ey.Y = mA + mB + iA * rA.X * rA.X + iB * rB.X * rB.X; + + Vector2 impulse = -K.Solve(C); + + cA -= mA * impulse; + aA -= iA * MathUtils.Cross(rA, impulse); + + cB += mB * impulse; + aB += iB * MathUtils.Cross(rB, impulse); + } + + data.positions[_indexA].c = cA; + data.positions[_indexA].a = aA; + data.positions[_indexB].c = cB; + data.positions[_indexB].a = aB; + + return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/Joints/RopeJoint.cs b/src/Box2DNet/Dynamics/Joints/RopeJoint.cs new file mode 100644 index 0000000..95cbd43 --- /dev/null +++ b/src/Box2DNet/Dynamics/Joints/RopeJoint.cs @@ -0,0 +1,291 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using Box2DNet.Common; +using Microsoft.Xna.Framework; + +namespace Box2DNet.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. + /// It can be used on ropes that are made up of several connected bodies, and if there is a need to support a heavy body. + /// This joint is used for stabiliation of heavy objects on soft constraint joints. + /// + /// Warning: if you attempt to change the maximum length during the simulation you will get some non-physical behavior. + /// Use the DistanceJoint instead if you want to dynamically control the length. + /// + public class RopeJoint : Joint + { + // Solver shared + private float _impulse; + private float _length; + + // Solver temp + private int _indexA; + private int _indexB; + private Vector2 _localCenterA; + private Vector2 _localCenterB; + private float _invMassA; + private float _invMassB; + private float _invIA; + private float _invIB; + private float _mass; + private Vector2 _rA, _rB; + private Vector2 _u; + + internal RopeJoint() + { + JointType = JointType.Rope; + } + + /// + /// Constructor for RopeJoint. + /// + /// The first body + /// The second body + /// The anchor on the first body + /// The anchor on the second body + /// Set to true if you are using world coordinates as anchors. + public RopeJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false) + : base(bodyA, bodyB) + { + JointType = JointType.Rope; + + if (useWorldCoordinates) + { + LocalAnchorA = bodyA.GetLocalPoint(anchorA); + LocalAnchorB = bodyB.GetLocalPoint(anchorB); + } + else + { + LocalAnchorA = anchorA; + LocalAnchorB = anchorB; + } + + //FPE feature: Setting default MaxLength + Vector2 d = WorldAnchorB - WorldAnchorA; + MaxLength = d.Length(); + } + + /// + /// The local anchor point on BodyA + /// + public Vector2 LocalAnchorA { get; set; } + + /// + /// The local anchor point on BodyB + /// + public Vector2 LocalAnchorB { get; set; } + + public override sealed Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + set { LocalAnchorA = BodyA.GetLocalPoint(value); } + } + + public override sealed Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { LocalAnchorB = BodyB.GetLocalPoint(value); } + } + + /// + /// Get or set the maximum length of the rope. + /// By default, it is the distance between the two anchor points. + /// + public float MaxLength { get; set; } + + /// + /// Gets the state of the joint. + /// + public LimitState State { get; private set; } + + public override Vector2 GetReactionForce(float invDt) + { + return (invDt * _impulse) * _u; + } + + public override float GetReactionTorque(float invDt) + { + return 0; + } + + internal override void InitVelocityConstraints(ref SolverData data) + { + _indexA = BodyA.IslandIndex; + _indexB = BodyB.IslandIndex; + _localCenterA = BodyA._sweep.LocalCenter; + _localCenterB = BodyB._sweep.LocalCenter; + _invMassA = BodyA._invMass; + _invMassB = BodyB._invMass; + _invIA = BodyA._invI; + _invIB = BodyB._invI; + + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + _u = cB + _rB - cA - _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 = _invMassA + _invIA * crA * crA + _invMassB + _invIB * crB * crB; + + _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; + + if (Settings.EnableWarmstarting) + { + // Scale the impulse to support a variable time step. + _impulse *= data.step.dtRatio; + + Vector2 P = _impulse * _u; + vA -= _invMassA * P; + wA -= _invIA * MathUtils.Cross(_rA, P); + vB += _invMassB * P; + wB += _invIB * MathUtils.Cross(_rB, P); + } + else + { + _impulse = 0.0f; + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override void SolveVelocityConstraints(ref SolverData data) + { + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + // Cdot = dot(u, v + cross(w, r)) + Vector2 vpA = vA + MathUtils.Cross(wA, _rA); + Vector2 vpB = vB + MathUtils.Cross(wB, _rB); + float C = _length - MaxLength; + float Cdot = Vector2.Dot(_u, vpB - vpA); + + // Predictive constraint. + if (C < 0.0f) + { + Cdot += data.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; + vA -= _invMassA * P; + wA -= _invIA * MathUtils.Cross(_rA, P); + vB += _invMassB * P; + wB += _invIB * MathUtils.Cross(_rB, P); + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override bool SolvePositionConstraints(ref SolverData data) + { + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + Vector2 u = cB + rB - cA - 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; + + cA -= _invMassA * P; + aA -= _invIA * MathUtils.Cross(rA, P); + cB += _invMassB * P; + aB += _invIB * MathUtils.Cross(rB, P); + + data.positions[_indexA].c = cA; + data.positions[_indexA].a = aA; + data.positions[_indexB].c = cB; + data.positions[_indexB].a = aB; + + return length - MaxLength < Settings.LinearSlop; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/Joints/WeldJoint.cs b/src/Box2DNet/Dynamics/Joints/WeldJoint.cs new file mode 100644 index 0000000..5862acd --- /dev/null +++ b/src/Box2DNet/Dynamics/Joints/WeldJoint.cs @@ -0,0 +1,388 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using Box2DNet.Common; +using Microsoft.Xna.Framework; + +namespace Box2DNet.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. + /// + /// The joint is soft constraint based, which means the two bodies will move + /// relative to each other, when a force is applied. To combine two bodies + /// in a rigid fashion, combine the fixtures to a single body instead. + /// + public class WeldJoint : Joint + { + // Solver shared + private Vector3 _impulse; + private float _gamma; + private float _bias; + + // Solver temp + private int _indexA; + private int _indexB; + private Vector2 _rA; + private Vector2 _rB; + private Vector2 _localCenterA; + private Vector2 _localCenterB; + private float _invMassA; + private float _invMassB; + private float _invIA; + private float _invIB; + private Mat33 _mass; + + internal WeldJoint() + { + JointType = JointType.Weld; + } + + /// + /// You need to specify an anchor point where they are attached. + /// The position of the anchor point is important for computing the reaction torque. + /// + /// The first body + /// The second body + /// The first body anchor. + /// The second body anchor. + /// Set to true if you are using world coordinates as anchors. + public WeldJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false) + : base(bodyA, bodyB) + { + JointType = JointType.Weld; + + if (useWorldCoordinates) + { + LocalAnchorA = bodyA.GetLocalPoint(anchorA); + LocalAnchorB = bodyB.GetLocalPoint(anchorB); + } + else + { + LocalAnchorA = anchorA; + LocalAnchorB = anchorB; + } + + ReferenceAngle = BodyB.Rotation - BodyA.Rotation; + } + + /// + /// The local anchor point on BodyA + /// + public Vector2 LocalAnchorA { get; set; } + + /// + /// The local anchor point on BodyB + /// + public Vector2 LocalAnchorB { get; set; } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + set { LocalAnchorA = BodyA.GetLocalPoint(value); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { LocalAnchorB = BodyB.GetLocalPoint(value); } + } + + /// + /// The bodyB angle minus bodyA angle in the reference state (radians). + /// + public float ReferenceAngle { get; set; } + + /// + /// The frequency of the joint. A higher frequency means a stiffer joint, but + /// a too high value can cause the joint to oscillate. + /// Default is 0, which means the joint does no spring calculations. + /// + public float FrequencyHz { get; set; } + + /// + /// The damping on the joint. The damping is only used when + /// the joint has a frequency (> 0). A higher value means more damping. + /// + public float DampingRatio { get; set; } + + public override Vector2 GetReactionForce(float invDt) + { + return invDt * new Vector2(_impulse.X, _impulse.Y); + } + + public override float GetReactionTorque(float invDt) + { + return invDt * _impulse.Z; + } + + internal override void InitVelocityConstraints(ref SolverData data) + { + _indexA = BodyA.IslandIndex; + _indexB = BodyB.IslandIndex; + _localCenterA = BodyA._sweep.LocalCenter; + _localCenterB = BodyB._sweep.LocalCenter; + _invMassA = BodyA._invMass; + _invMassB = BodyB._invMass; + _invIA = BodyA._invI; + _invIB = BodyB._invI; + + float aA = data.positions[_indexA].a; + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + + float aB = data.positions[_indexB].a; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + + // 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 = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + Mat33 K = new Mat33(); + K.ex.X = mA + mB + _rA.Y * _rA.Y * iA + _rB.Y * _rB.Y * iB; + K.ey.X = -_rA.Y * _rA.X * iA - _rB.Y * _rB.X * iB; + K.ez.X = -_rA.Y * iA - _rB.Y * iB; + K.ex.Y = K.ey.X; + K.ey.Y = mA + mB + _rA.X * _rA.X * iA + _rB.X * _rB.X * iB; + K.ez.Y = _rA.X * iA + _rB.X * iB; + K.ex.Z = K.ez.X; + K.ey.Z = K.ez.Y; + K.ez.Z = iA + iB; + + if (FrequencyHz > 0.0f) + { + K.GetInverse22(ref _mass); + + float invM = iA + iB; + float m = invM > 0.0f ? 1.0f / invM : 0.0f; + + float C = aB - aA - ReferenceAngle; + + // Frequency + float omega = 2.0f * Settings.Pi * FrequencyHz; + + // Damping coefficient + float d = 2.0f * m * DampingRatio * omega; + + // Spring stiffness + float k = m * omega * omega; + + // magic formulas + float h = data.step.dt; + _gamma = h * (d + h * k); + _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f; + _bias = C * h * k * _gamma; + + invM += _gamma; + _mass.ez.Z = invM != 0.0f ? 1.0f / invM : 0.0f; + } + else + { + K.GetSymInverse33(ref _mass); + _gamma = 0.0f; + _bias = 0.0f; + } + + if (Settings.EnableWarmstarting) + { + // Scale impulses to support a variable time step. + _impulse *= data.step.dtRatio; + + 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); + } + else + { + _impulse = Vector3.Zero; + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override void SolveVelocityConstraints(ref SolverData data) + { + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + if (FrequencyHz > 0.0f) + { + float Cdot2 = wB - wA; + + float impulse2 = -_mass.ez.Z * (Cdot2 + _bias + _gamma * _impulse.Z); + _impulse.Z += impulse2; + + wA -= iA * impulse2; + wB += iB * impulse2; + + Vector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA); + + Vector2 impulse1 = -MathUtils.Mul22(_mass, Cdot1); + _impulse.X += impulse1.X; + _impulse.Y += impulse1.Y; + + Vector2 P = impulse1; + + vA -= mA * P; + wA -= iA * MathUtils.Cross(_rA, P); + + vB += mB * P; + wB += iB * MathUtils.Cross(_rB, P); + } + else + { + 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 = -MathUtils.Mul(_mass, 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); + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override bool SolvePositionConstraints(ref SolverData data) + { + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + + float positionError, angularError; + + Mat33 K = new Mat33(); + K.ex.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB; + K.ey.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB; + K.ez.X = -rA.Y * iA - rB.Y * iB; + K.ex.Y = K.ey.X; + K.ey.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB; + K.ez.Y = rA.X * iA + rB.X * iB; + K.ex.Z = K.ez.X; + K.ey.Z = K.ez.Y; + K.ez.Z = iA + iB; + + if (FrequencyHz > 0.0f) + { + Vector2 C1 = cB + rB - cA - rA; + + positionError = C1.Length(); + angularError = 0.0f; + + Vector2 P = -K.Solve22(C1); + + cA -= mA * P; + aA -= iA * MathUtils.Cross(rA, P); + + cB += mB * P; + aB += iB * MathUtils.Cross(rB, P); + } + else + { + Vector2 C1 = cB + rB - cA - rA; + float C2 = aB - aA - ReferenceAngle; + + positionError = C1.Length(); + angularError = Math.Abs(C2); + + Vector3 C = new Vector3(C1.X, C1.Y, C2); + + Vector3 impulse = -K.Solve33(C); + Vector2 P = new Vector2(impulse.X, impulse.Y); + + cA -= mA * P; + aA -= iA * (MathUtils.Cross(rA, P) + impulse.Z); + + cB += mB * P; + aB += iB * (MathUtils.Cross(rB, P) + impulse.Z); + } + + data.positions[_indexA].c = cA; + data.positions[_indexA].a = aA; + data.positions[_indexB].c = cB; + data.positions[_indexB].a = aB; + + return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/Joints/WheelJoint.cs b/src/Box2DNet/Dynamics/Joints/WheelJoint.cs new file mode 100644 index 0000000..6ceee6c --- /dev/null +++ b/src/Box2DNet/Dynamics/Joints/WheelJoint.cs @@ -0,0 +1,513 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using Box2DNet.Common; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Dynamics.Joints +{ + // 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] + + /// + /// A wheel joint. This joint provides two degrees of freedom: translation + /// along an axis fixed in bodyA and rotation in the plane. You can use a + /// joint limit to restrict the range of motion and a joint motor to drive + /// the rotation or to model rotational friction. + /// This joint is designed for vehicle suspensions. + /// + public class WheelJoint : Joint + { + // Solver shared + private Vector2 _localYAxis; + + private float _impulse; + private float _motorImpulse; + private float _springImpulse; + + private float _maxMotorTorque; + private float _motorSpeed; + private bool _enableMotor; + + // Solver temp + private int _indexA; + private int _indexB; + private Vector2 _localCenterA; + private Vector2 _localCenterB; + private float _invMassA; + private float _invMassB; + private float _invIA; + private float _invIB; + + private Vector2 _ax, _ay; + private float _sAx, _sBx; + private float _sAy, _sBy; + + private float _mass; + private float _motorMass; + private float _springMass; + + private float _bias; + private float _gamma; + private Vector2 _axis; + + internal WheelJoint() + { + JointType = JointType.Wheel; + } + + /// + /// Constructor for WheelJoint + /// + /// The first body + /// The second body + /// The anchor point + /// The axis + /// Set to true if you are using world coordinates as anchors. + public WheelJoint(Body bodyA, Body bodyB, Vector2 anchor, Vector2 axis, bool useWorldCoordinates = false) + : base(bodyA, bodyB) + { + JointType = JointType.Wheel; + + if (useWorldCoordinates) + { + LocalAnchorA = bodyA.GetLocalPoint(anchor); + LocalAnchorB = bodyB.GetLocalPoint(anchor); + } + else + { + LocalAnchorA = bodyA.GetLocalPoint(bodyB.GetWorldPoint(anchor)); + LocalAnchorB = anchor; + } + + Axis = axis; //FPE only: We maintain the original value as it is supposed to. + } + + /// + /// The local anchor point on BodyA + /// + public Vector2 LocalAnchorA { get; set; } + + /// + /// The local anchor point on BodyB + /// + public Vector2 LocalAnchorB { get; set; } + + public override Vector2 WorldAnchorA + { + get { return BodyA.GetWorldPoint(LocalAnchorA); } + set { LocalAnchorA = BodyA.GetLocalPoint(value); } + } + + public override Vector2 WorldAnchorB + { + get { return BodyB.GetWorldPoint(LocalAnchorB); } + set { LocalAnchorB = BodyB.GetLocalPoint(value); } + } + + /// + /// The axis at which the suspension moves. + /// + public Vector2 Axis + { + get { return _axis; } + set + { + _axis = value; + LocalXAxis = BodyA.GetLocalVector(_axis); + _localYAxis = MathUtils.Cross(1.0f, LocalXAxis); + } + } + + /// + /// The axis in local coordinates relative to BodyA + /// + public Vector2 LocalXAxis { get; private set; } + + /// + /// The desired motor speed in radians per second. + /// + public float MotorSpeed + { + get { return _motorSpeed; } + set + { + WakeBodies(); + _motorSpeed = value; + } + } + + /// + /// The maximum motor torque, usually in N-m. + /// + public float MaxMotorTorque + { + get { return _maxMotorTorque; } + set + { + WakeBodies(); + _maxMotorTorque = value; + } + } + + /// + /// Suspension frequency, zero indicates no suspension + /// + public float Frequency { get; set; } + + /// + /// Suspension damping ratio, one indicates critical damping + /// + public float DampingRatio { get; set; } + + /// + /// Gets the translation along the axis + /// + 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; + } + } + + /// + /// Gets the angular velocity of the joint + /// + public float JointSpeed + { + get + { + float wA = BodyA.AngularVelocity; + float wB = BodyB.AngularVelocity; + return wB - wA; + } + } + + /// + /// Enable/disable the joint motor. + /// + public bool MotorEnabled + { + get { return _enableMotor; } + set + { + WakeBodies(); + _enableMotor = value; + } + } + + /// + /// Gets the torque of the motor + /// + /// inverse delta time + public float GetMotorTorque(float invDt) + { + return invDt * _motorImpulse; + } + + 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 SolverData data) + { + _indexA = BodyA.IslandIndex; + _indexB = BodyB.IslandIndex; + _localCenterA = BodyA._sweep.LocalCenter; + _localCenterB = BodyB._sweep.LocalCenter; + _invMassA = BodyA._invMass; + _invMassB = BodyB._invMass; + _invIA = BodyA._invI; + _invIB = BodyB._invI; + + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + // Compute the effective masses. + Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + Vector2 d1 = cB + rB - cA - rA; + + // Point to line constraint + { + _ay = MathUtils.Mul(qA, _localYAxis); + _sAy = MathUtils.Cross(d1 + rA, _ay); + _sBy = MathUtils.Cross(rB, _ay); + + _mass = mA + mB + iA * _sAy * _sAy + iB * _sBy * _sBy; + + if (_mass > 0.0f) + { + _mass = 1.0f / _mass; + } + } + + // Spring constraint + _springMass = 0.0f; + _bias = 0.0f; + _gamma = 0.0f; + if (Frequency > 0.0f) + { + _ax = MathUtils.Mul(qA, LocalXAxis); + _sAx = MathUtils.Cross(d1 + rA, _ax); + _sBx = MathUtils.Cross(rB, _ax); + + float invMass = mA + mB + iA * _sAx * _sAx + iB * _sBx * _sBx; + + if (invMass > 0.0f) + { + _springMass = 1.0f / invMass; + + float C = Vector2.Dot(d1, _ax); + + // Frequency + float omega = 2.0f * Settings.Pi * Frequency; + + // Damping coefficient + float d = 2.0f * _springMass * DampingRatio * omega; + + // Spring stiffness + float k = _springMass * omega * omega; + + // magic formulas + float h = data.step.dt; + _gamma = h * (d + h * k); + if (_gamma > 0.0f) + { + _gamma = 1.0f / _gamma; + } + + _bias = C * h * k * _gamma; + + _springMass = invMass + _gamma; + if (_springMass > 0.0f) + { + _springMass = 1.0f / _springMass; + } + } + } + else + { + _springImpulse = 0.0f; + } + + // Rotational motor + if (_enableMotor) + { + _motorMass = iA + iB; + if (_motorMass > 0.0f) + { + _motorMass = 1.0f / _motorMass; + } + } + else + { + _motorMass = 0.0f; + _motorImpulse = 0.0f; + } + + if (Settings.EnableWarmstarting) + { + // Account for variable time step. + _impulse *= data.step.dtRatio; + _springImpulse *= data.step.dtRatio; + _motorImpulse *= data.step.dtRatio; + + Vector2 P = _impulse * _ay + _springImpulse * _ax; + float LA = _impulse * _sAy + _springImpulse * _sAx + _motorImpulse; + float LB = _impulse * _sBy + _springImpulse * _sBx + _motorImpulse; + + vA -= _invMassA * P; + wA -= _invIA * LA; + + vB += _invMassB * P; + wB += _invIB * LB; + } + else + { + _impulse = 0.0f; + _springImpulse = 0.0f; + _motorImpulse = 0.0f; + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override void SolveVelocityConstraints(ref SolverData data) + { + float mA = _invMassA, mB = _invMassB; + float iA = _invIA, iB = _invIB; + + Vector2 vA = data.velocities[_indexA].v; + float wA = data.velocities[_indexA].w; + Vector2 vB = data.velocities[_indexB].v; + float wB = data.velocities[_indexB].w; + + // 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 -= mA * P; + wA -= iA * LA; + + vB += mB * P; + wB += iB * LB; + } + + // Solve rotational motor constraint + { + float Cdot = wB - wA - _motorSpeed; + float impulse = -_motorMass * Cdot; + + float oldImpulse = _motorImpulse; + float maxImpulse = data.step.dt * _maxMotorTorque; + _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = _motorImpulse - oldImpulse; + + wA -= iA * impulse; + wB += iB * 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 -= mA * P; + wA -= iA * LA; + + vB += mB * P; + wB += iB * LB; + } + + data.velocities[_indexA].v = vA; + data.velocities[_indexA].w = wA; + data.velocities[_indexB].v = vB; + data.velocities[_indexB].w = wB; + } + + internal override bool SolvePositionConstraints(ref SolverData data) + { + Vector2 cA = data.positions[_indexA].c; + float aA = data.positions[_indexA].a; + Vector2 cB = data.positions[_indexB].c; + float aB = data.positions[_indexB].a; + + Rot qA = new Rot(aA), qB = new Rot(aB); + + Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); + Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); + Vector2 d = (cB - cA) + rB - rA; + + Vector2 ay = MathUtils.Mul(qA, _localYAxis); + + 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; + + cA -= _invMassA * P; + aA -= _invIA * LA; + cB += _invMassB * P; + aB += _invIB * LB; + + data.positions[_indexA].c = cA; + data.positions[_indexA].a = aA; + data.positions[_indexB].c = cB; + data.positions[_indexB].a = aB; + + return Math.Abs(C) <= Settings.LinearSlop; + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/TimeStep.cs b/src/Box2DNet/Dynamics/TimeStep.cs new file mode 100644 index 0000000..2b3e240 --- /dev/null +++ b/src/Box2DNet/Dynamics/TimeStep.cs @@ -0,0 +1,66 @@ +/* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using Microsoft.Xna.Framework; + +namespace Box2DNet.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; + } + + /// This is an internal structure. + public struct Position + { + public Vector2 c; + public float a; + } + + /// This is an internal structure. + public struct Velocity + { + public Vector2 v; + public float w; + } + + /// Solver Data + public struct SolverData + { + public TimeStep step; + public Position[] positions; + public Velocity[] velocities; + } +} \ No newline at end of file diff --git a/src/Box2DNet/Dynamics/World.cs b/src/Box2DNet/Dynamics/World.cs index 84c7a8f..7cea9e8 100644 --- a/src/Box2DNet/Dynamics/World.cs +++ b/src/Box2DNet/Dynamics/World.cs @@ -1,1449 +1,1504 @@ -/* - Box2DNet Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version 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.Numerics; -using System.Numerics; -using Box2DNet.Common; -using Box2DNet.Collision; - - -using Transform = Box2DNet.Common.Transform; - -namespace Box2DNet.Dynamics -{ - public struct TimeStep - { - public float Dt; // time step - public float Inv_Dt; // inverse time step (0 if dt == 0). - public float DtRatio; // dt * inv_dt0 - public int VelocityIterations; - public int PositionIterations; - public bool WarmStarting; - } - - /// - /// The world class manages all physics entities, dynamic simulation, - /// and asynchronous queries. - /// - public class World : IDisposable - { - internal bool _lock; - - internal BroadPhase _broadPhase; - private ContactManager _contactManager; - - private Body _bodyList; - private Joint _jointList; - private Controllers.Controller _controllerList; - - private Vector2 _raycastNormal; - private object _raycastUserData; - private Segment _raycastSegment; - private bool _raycastSolidShape; - - // Do not access - internal Contact _contactList; - - private int _bodyCount; - internal int _contactCount; - private int _jointCount; - private int _controllerCount; - - private Vector2 _gravity; - /// - /// Get\Set global gravity vector. - /// - public Vector2 Gravity { get { return _gravity; } set { _gravity = value; } } - - private bool _allowSleep; - - private Body _groundBody; - - private DestructionListener _destructionListener; - private BoundaryListener _boundaryListener; - internal ContactFilter _contactFilter; - internal ContactListener _contactListener; - private DebugDraw _debugDraw; - - // This is used to compute the time step ratio to - // support a variable time step. - private float _inv_dt0; - - // This is for debugging the solver. - private bool _warmStarting; - - // This is for debugging the solver. - private bool _continuousPhysics; - - /// - /// Construct a world object. - /// - /// A bounding box that completely encompasses all your shapes. - /// The world gravity vector. - /// Improve performance by not simulating inactive bodies. - public World(AABB worldAABB, Vector2 gravity, bool doSleep) - { - _destructionListener = null; - _boundaryListener = null; - _contactFilter = null; - _contactListener = null; - _debugDraw = null; - - _bodyList = null; - _contactList = null; - _jointList = null; - - _bodyCount = 0; - _contactCount = 0; - _jointCount = 0; - - _warmStarting = true; - _continuousPhysics = true; - - _allowSleep = doSleep; - _gravity = gravity; - - _lock = false; - - _inv_dt0 = 0.0f; - - _contactManager = new ContactManager(); - _contactManager._world = this; - _broadPhase = new BroadPhase(worldAABB, _contactManager); - - BodyDef bd = new BodyDef(); - _groundBody = CreateBody(bd); - } - - /// - /// Destruct the world. All physics entities are destroyed. - /// - public void Dispose() - { - DestroyBody(_groundBody); - if (_broadPhase is IDisposable) - (_broadPhase as IDisposable).Dispose(); - _broadPhase = null; - } - - /// - /// Register a destruction listener. - /// - /// - public void SetDestructionListener(DestructionListener listener) - { - _destructionListener = listener; - } - - /// - /// Register a broad-phase boundary listener. - /// - /// - public void SetBoundaryListener(BoundaryListener listener) - { - _boundaryListener = listener; - } - - /// - /// Register a contact filter to provide specific control over collision. - /// Otherwise the default filter is used (b2_defaultFilter). - /// - /// - public void SetContactFilter(ContactFilter filter) - { - _contactFilter = filter; - } - - /// - /// Register a contact event listener - /// - /// - public void SetContactListener(ContactListener listener) - { - _contactListener = listener; - } - - /// - /// Register a routine for debug drawing. The debug draw functions are called - /// inside the World.Step method, so make sure your renderer is ready to - /// consume draw commands when you call Step(). - /// - /// - public void SetDebugDraw(DebugDraw debugDraw) - { - _debugDraw = debugDraw; - } - - /// - /// Create a rigid body given a definition. No reference to the definition - /// is retained. - /// @warning This function is locked during callbacks. - /// - /// - /// - public Body CreateBody(BodyDef def) - { - Box2DNetDebug.Assert(_lock == false); - if (_lock == true) - { - return null; - } - - Body b = new Body(def, this); - - // Add to world doubly linked list. - b._prev = null; - b._next = _bodyList; - if (_bodyList != null) - { - _bodyList._prev = b; - } - _bodyList = b; - ++_bodyCount; - - return b; - } - - /// - /// Destroy a rigid body given a definition. No reference to the definition - /// is retained. This function is locked during callbacks. - /// @warning This automatically deletes all associated shapes and joints. - /// @warning This function is locked during callbacks. - /// - /// - public void DestroyBody(Body b) - { - Box2DNetDebug.Assert(_bodyCount > 0); - Box2DNetDebug.Assert(_lock == false); - if (_lock == true) - { - return; - } - - // Delete the attached joints. - JointEdge jn = null; - if (b._jointList != null) - jn = b._jointList; - while (jn != null) - { - JointEdge jn0 = jn; - jn = jn.Next; - - if (_destructionListener != null) - { - _destructionListener.SayGoodbye(jn0.Joint); - } - - DestroyJoint(jn0.Joint); - } - - //Detach controllers attached to this body - Controllers.ControllerEdge ce = b._controllerList; - while (ce != null) - { - Controllers.ControllerEdge ce0 = ce; - ce = ce.nextController; - - ce0.controller.RemoveBody(b); - } - - // Delete the attached fixtures. This destroys broad-phase - // proxies and pairs, leading to the destruction of contacts. - Fixture f = b._fixtureList; - while (f != null) - { - Fixture f0 = f; - f = f.Next; - - if (_destructionListener != null) - { - _destructionListener.SayGoodbye(f0); - } - - f0.Destroy(_broadPhase); - } - - // Remove world body list. - if (b._prev != null) - { - b._prev._next = b._next; - } - - if (b._next != null) - { - b._next._prev = b._prev; - } - - if (b == _bodyList) - { - _bodyList = b._next; - } - - --_bodyCount; - if (b is IDisposable) - (b as IDisposable).Dispose(); - b = null; - } - - /// - /// Create a joint to constrain bodies together. No reference to the definition - /// is retained. This may cause the connected bodies to cease colliding. - /// @warning This function is locked during callbacks. - /// - /// - /// - public Joint CreateJoint(JointDef def) - { - Box2DNetDebug.Assert(_lock == false); - - Joint j = Joint.Create(def); - - // Connect to the world list. - j._prev = null; - j._next = _jointList; - if (_jointList != null) - { - _jointList._prev = j; - } - _jointList = j; - ++_jointCount; - - // Connect to the bodies' doubly linked lists. - j._node1.Joint = j; - j._node1.Other = j._body2; - j._node1.Prev = null; - j._node1.Next = j._body1._jointList; - if (j._body1._jointList != null) - j._body1._jointList.Prev = j._node1; - j._body1._jointList = j._node1; - - j._node2.Joint = j; - j._node2.Other = j._body1; - j._node2.Prev = null; - j._node2.Next = j._body2._jointList; - if (j._body2._jointList != null) - j._body2._jointList.Prev = j._node2; - j._body2._jointList = j._node2; - - // If the joint prevents collisions, then reset collision filtering. - if (def.CollideConnected == false) - { - // Reset the proxies on the body with the minimum number of shapes. - Body b = def.Body1._fixtureCount < def.Body2._fixtureCount ? def.Body1 : def.Body2; - for (Fixture f = b._fixtureList; f != null; f = f.Next) - { - f.RefilterProxy(_broadPhase, b.GetTransform()); - } - } - - return j; - } - - /// - /// Destroy a joint. This may cause the connected bodies to begin colliding. - /// @warning This function is locked during callbacks. - /// - /// - public void DestroyJoint(Joint j) - { - Box2DNetDebug.Assert(_lock == false); - - bool collideConnected = j._collideConnected; - - // Remove from the doubly linked list. - if (j._prev != null) - { - j._prev._next = j._next; - } - - if (j._next != null) - { - j._next._prev = j._prev; - } - - if (j == _jointList) - { - _jointList = j._next; - } - - // Disconnect from island graph. - Body body1 = j._body1; - Body body2 = j._body2; - - // Wake up connected bodies. - body1.WakeUp(); - body2.WakeUp(); - - // Remove from body 1. - if (j._node1.Prev != null) - { - j._node1.Prev.Next = j._node1.Next; - } - - if (j._node1.Next != null) - { - j._node1.Next.Prev = j._node1.Prev; - } - - if (j._node1 == body1._jointList) - { - body1._jointList = j._node1.Next; - } - - j._node1.Prev = null; - j._node1.Next = null; - - // Remove from body 2 - if (j._node2.Prev != null) - { - j._node2.Prev.Next = j._node2.Next; - } - - if (j._node2.Next != null) - { - j._node2.Next.Prev = j._node2.Prev; - } - - if (j._node2 == body2._jointList) - { - body2._jointList = j._node2.Next; - } - - j._node2.Prev = null; - j._node2.Next = null; - - Joint.Destroy(j); - - Box2DNetDebug.Assert(_jointCount > 0); - --_jointCount; - - // If the joint prevents collisions, then reset collision filtering. - if (collideConnected == false) - { - // Reset the proxies on the body with the minimum number of shapes. - Body b = body1._fixtureCount < body2._fixtureCount ? body1 : body2; - for (Fixture f = b._fixtureList; f != null; f = f.Next) - { - f.RefilterProxy(_broadPhase, b.GetTransform()); - } - } - } - - public Controllers.Controller AddController(Controllers.Controller def) - { - def._next = _controllerList; - def._prev = null; - if (_controllerList != null) - _controllerList._prev = def; - _controllerList = def; - ++_controllerCount; - - def._world = this; - - return def; - } - - public void RemoveController(Controllers.Controller controller) - { - Box2DNetDebug.Assert(_controllerCount > 0); - if (controller._next != null) - controller._next._prev = controller._prev; - if (controller._prev != null) - controller._prev._next = controller._next; - if (controller == _controllerList) - _controllerList = controller._next; - --_controllerCount; - } - - /// - /// The world provides a single static ground body with no collision shapes. - /// You can use this to simplify the creation of joints and static shapes. - /// - /// - public Body GetGroundBody() - { - return _groundBody; - } - - /// - /// Get the world body list. With the returned body, use Body.GetNext to get - /// the next body in the world list. A null body indicates the end of the list. - /// - /// The head of the world body list. - public Body GetBodyList() - { - return _bodyList; - } - - /// - /// Get the world joint list. With the returned joint, use Joint.GetNext to get - /// the next joint in the world list. A null joint indicates the end of the list. - /// - /// The head of the world joint list. - public Joint GetJointList() - { - return _jointList; - } - - public Controllers.Controller GetControllerList() - { - return _controllerList; - } - - public int GetControllerCount() - { - return _controllerCount; - } - - /// - /// Re-filter a fixture. This re-runs contact filtering on a fixture. - /// - public void Refilter(Fixture fixture) - { - Box2DNetDebug.Assert(_lock == false); - fixture.RefilterProxy(_broadPhase, fixture.Body.GetTransform()); - } - - /// - /// Enable/disable warm starting. For testing. - /// - public void SetWarmStarting(bool flag) { _warmStarting = flag; } - - /// - /// Enable/disable continuous physics. For testing. - /// - public void SetContinuousPhysics(bool flag) { _continuousPhysics = flag; } - - /// - /// Perform validation of internal data structures. - /// - public void Validate() { _broadPhase.Validate(); } - - /// - /// Get the number of broad-phase proxies. - /// - public int GetProxyCount() { return _broadPhase._proxyCount; } - - /// - /// Get the number of broad-phase pairs. - /// - /// - public int GetPairCount() { return _broadPhase._pairManager._pairCount; } - - /// - /// Get the number of bodies. - /// - /// - public int GetBodyCount() { return _bodyCount; } - - /// - /// Get the number joints. - /// - /// - public int GetJointCount() { return _jointCount; } - - /// - /// Get the number of contacts (each may have 0 or more contact points). - /// - /// - public int GetContactCount() { return _contactCount; } - - /// - /// Take a time step. This performs collision detection, integration, - /// and constraint solution. - /// - /// The amount of time to simulate, this should not vary. - /// For the velocity constraint solver. - /// For the positionconstraint solver. - public void Step(float dt, int velocityIterations, int positionIteration) - { - _lock = true; - - TimeStep step = new TimeStep(); - step.Dt = dt; - step.VelocityIterations = velocityIterations; - step.PositionIterations = positionIteration; - if (dt > 0.0f) - { - step.Inv_Dt = 1.0f / dt; - } - else - { - step.Inv_Dt = 0.0f; - } - - step.DtRatio = _inv_dt0 * dt; - - step.WarmStarting = _warmStarting; - - // Update contacts. - _contactManager.Collide(); - - // Integrate velocities, solve velocity constraints, and integrate positions. - if (step.Dt > 0.0f) - { - Solve(step); - } - - // Handle TOI events. - if (_continuousPhysics && step.Dt > 0.0f) - { - SolveTOI(step); - } - - // Draw debug information. - DrawDebugData(); - - _inv_dt0 = step.Inv_Dt; - _lock = false; - } - - /// Query the world for all shapes that potentially overlap the - /// provided AABB. You provide a shape pointer buffer of specified - /// size. The number of shapes found is returned. - /// @param aabb the query box. - /// @param shapes a user allocated shape pointer array of size maxCount (or greater). - /// @param maxCount the capacity of the shapes array. - /// @return the number of shapes found in aabb. - public int Query(AABB aabb, Fixture[] fixtures, int maxCount) - { - //using (object[] results = new object[maxCount]) - { - object[] results = new object[maxCount]; - - int count = _broadPhase.Query(aabb, results, maxCount); - - for (int i = 0; i < count; ++i) - { - fixtures[i] = (Fixture)results[i]; - } - - results = null; - return count; - } - } - - /// - /// Query the world for all shapes that intersect a given segment. You provide a shap - /// pointer buffer of specified size. The number of shapes found is returned, and the buffer - /// is filled in order of intersection. - /// - /// Defines the begin and end point of the ray cast, from p1 to p2. - /// Use Segment.Extend to create (semi-)infinite rays. - /// A user allocated shape pointer array of size maxCount (or greater). - /// The capacity of the shapes array. - /// Determines if shapes that the ray starts in are counted as hits. - /// Passed through the worlds contact filter, with method RayCollide. This can be used to filter valid shapes. - /// The number of shapes found - public int Raycast(Segment segment, out Fixture[] fixtures, int maxCount, bool solidShapes, object userData) - { -#warning "PTR" - _raycastSegment = segment; - _raycastUserData = userData; - _raycastSolidShape = solidShapes; - - object[] results = new object[maxCount]; - fixtures = new Fixture[maxCount]; - int count = _broadPhase.QuerySegment(segment, results, maxCount, RaycastSortKey); - - for (int i = 0; i < count; ++i) - { - fixtures[i] = (Fixture)results[i]; - } - - return count; - } - - /// - /// Performs a raycast as with Raycast, finding the first intersecting shape. - /// - /// Defines the begin and end point of the ray cast, from p1 to p2. - /// Use Segment.Extend to create (semi-)infinite rays. - /// Returns the hit fraction. You can use this to compute the contact point - /// p = (1 - lambda) * segment.p1 + lambda * segment.p2. - /// Returns the normal at the contact point. If there is no intersection, the normal is not set. - /// Determines if shapes that the ray starts in are counted as hits. - /// - /// Returns the colliding shape shape, or null if not found. - public Fixture RaycastOne(Segment segment, out float lambda, out Vector2 normal, bool solidShapes, object userData) - { - int maxCount = 1; - Fixture[] fixture; - lambda = 0.0f; - normal = new Vector2(); - - int count = Raycast(segment, out fixture, maxCount, solidShapes, userData); - - if (count == 0) - return null; - - Box2DNetDebug.Assert(count == 1); - - //Redundantly do TestSegment a second time, as the previous one's results are inaccessible - - fixture[0].TestSegment(out lambda, out normal, segment, 1); - //We already know it returns true - return fixture[0]; - } - - // Find islands, integrate and solve constraints, solve position constraints - private void Solve(TimeStep step) - { - // Step all controlls - for (Controllers.Controller controller = _controllerList; controller != null; controller = controller._next) - { - controller.Step(step); - } - - // Size the island for the worst case. - Island island = new Island(_bodyCount, _contactCount, _jointCount, _contactListener); - - // Clear all the island flags. - for (Body b = _bodyList; b != null; b = b._next) - { - b._flags &= ~Body.BodyFlags.Island; - } - for (Contact c = _contactList; c != null; c = c._next) - { - c._flags &= ~Contact.CollisionFlags.Island; - } - for (Joint j = _jointList; j != null; j = j._next) - { - j._islandFlag = false; - } - - // Build and simulate all awake islands. - int stackSize = _bodyCount; - { - Body[] stack = new Body[stackSize]; - - for (Body seed = _bodyList; seed != null; seed = seed._next) - { - if ((seed._flags & (Body.BodyFlags.Island | Body.BodyFlags.Sleep | Body.BodyFlags.Frozen)) != 0) - { - continue; - } - - if (seed.IsStatic()) - { - continue; - } - - // Reset island and stack. - island.Clear(); - int stackCount = 0; - stack[stackCount++] = seed; - seed._flags |= Body.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]; - island.Add(b); - - // Make sure the body is awake. - b._flags &= ~Body.BodyFlags.Sleep; - - // To keep islands as small as possible, we don't - // propagate islands across static bodies. - if (b.IsStatic()) - { - continue; - } - - // Search all contacts connected to this body. - for (ContactEdge cn = b._contactList; cn != null; cn = cn.Next) - { - // Has this contact already been added to an island? - if ((cn.Contact._flags & (Contact.CollisionFlags.Island | Contact.CollisionFlags.NonSolid)) != 0) - { - continue; - } - - // Is this contact touching? - if ((cn.Contact._flags & Contact.CollisionFlags.Touch) == (Contact.CollisionFlags)0) - { - continue; - } - - island.Add(cn.Contact); - cn.Contact._flags |= Contact.CollisionFlags.Island; - - Body other = cn.Other; - - // Was the other body already added to this island? - if ((other._flags & Body.BodyFlags.Island) != 0) - { - continue; - } - - Box2DNetDebug.Assert(stackCount < stackSize); - stack[stackCount++] = other; - other._flags |= Body.BodyFlags.Island; - } - - // Search all joints connect to this body. - for (JointEdge jn = b._jointList; jn != null; jn = jn.Next) - { - if (jn.Joint._islandFlag == true) - { - continue; - } - - island.Add(jn.Joint); - jn.Joint._islandFlag = true; - - Body other = jn.Other; - if ((other._flags & Body.BodyFlags.Island) != 0) - { - continue; - } - - Box2DNetDebug.Assert(stackCount < stackSize); - stack[stackCount++] = other; - other._flags |= Body.BodyFlags.Island; - } - } - - island.Solve(step, _gravity, _allowSleep); - - // 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.IsStatic()) - { - b._flags &= ~Body.BodyFlags.Island; - } - } - } - - stack = null; - } - - // Synchronize shapes, check for out of range bodies. - for (Body b = _bodyList; b != null; b = b.GetNext()) - { - if ((b._flags & (Body.BodyFlags.Sleep | Body.BodyFlags.Frozen)) != 0) - { - continue; - } - - if (b.IsStatic()) - { - continue; - } - - // Update shapes (for broad-phase). If the shapes go out of - // the world AABB then shapes and contacts may be destroyed, - // including contacts that are - bool inRange = b.SynchronizeFixtures(); - - // Did the body's shapes leave the world? - if (inRange == false && _boundaryListener != null) - { - _boundaryListener.Violation(b); - } - } - - // Commit shape proxy movements to the broad-phase so that new contacts are created. - // Also, some contacts can be destroyed. - _broadPhase.Commit(); - } - - // Find TOI contacts and solve them. - private void SolveTOI(TimeStep step) - { - // Reserve an island and a queue for TOI island solution. - Island island = new Island(_bodyCount, Settings.MaxTOIContactsPerIsland, Settings.MaxTOIJointsPerIsland, _contactListener); - - //Simple one pass queue - //Relies on the fact that we're only making one pass - //through and each body can only be pushed/popped once. - //To push: - // queue[queueStart+queueSize++] = newElement; - //To pop: - // poppedElement = queue[queueStart++]; - // --queueSize; - int queueCapacity = _bodyCount; - Body[] queue = new Body[queueCapacity]; - - for (Body b = _bodyList; b != null; b = b._next) - { - b._flags &= ~Body.BodyFlags.Island; - b._sweep.T0 = 0.0f; - } - - for (Contact c = _contactList; c != null; c = c._next) - { - // Invalidate TOI - c._flags &= ~(Contact.CollisionFlags.Toi | Contact.CollisionFlags.Island); - } - - for (Joint j = _jointList; j != null; j = j._next) - { - j._islandFlag = false; - } - - // Find TOI events and solve them. - for (; ; ) - { - // Find the first TOI. - Contact minContact = null; - float minTOI = 1.0f; - - for (Contact c = _contactList; c != null; c = c._next) - { - if ((int)(c._flags & (Contact.CollisionFlags.Slow | Contact.CollisionFlags.NonSolid)) == 1) - { - continue; - } - - // TODO_ERIN keep a counter on the contact, only respond to M TOIs per contact. - - float toi = 1.0f; - if ((int)(c._flags & Contact.CollisionFlags.Toi) == 1) - { - // This contact has a valid cached TOI. - toi = c._toi; - } - else - { - // Compute the TOI for this contact. - Fixture s1 = c.FixtureA; - Fixture s2 = c.FixtureB; - Body b1 = s1.Body; - Body b2 = s2.Body; - - if ((b1.IsStatic() || b1.IsSleeping()) && (b2.IsStatic() || b2.IsSleeping())) - { - continue; - } - - // Put the sweeps onto the same time interval. - float t0 = b1._sweep.T0; - - if (b1._sweep.T0 < b2._sweep.T0) - { - t0 = b2._sweep.T0; - b1._sweep.Advance(t0); - } - else if (b2._sweep.T0 < b1._sweep.T0) - { - t0 = b1._sweep.T0; - b2._sweep.Advance(t0); - } - - Box2DNetDebug.Assert(t0 < 1.0f); - - // Compute the time of impact. - toi = c.ComputeTOI(b1._sweep, b2._sweep); - //b2TimeOfImpact(c->m_fixtureA->GetShape(), b1->m_sweep, c->m_fixtureB->GetShape(), b2->m_sweep); - - Box2DNetDebug.Assert(0.0f <= toi && toi <= 1.0f); - - // If the TOI is in range ... - if (0.0f < toi && toi < 1.0f) - { - // Interpolate on the actual range. - toi = Common.Math.Min((1.0f - toi) * t0 + toi, 1.0f); - } - - - c._toi = toi; - c._flags |= Contact.CollisionFlags.Toi; - } - - if (Box2DNet.Common.Math.Epsilon < toi && toi < minTOI) - { - // This is the minimum TOI found so far. - minContact = c; - minTOI = toi; - } - } - - if (minContact == null || 1.0f - 100.0f * Box2DNet.Common.Math.Epsilon < minTOI) - { - // No more TOI events. Done! - break; - } - - // Advance the bodies to the TOI. - Fixture f1 = minContact.FixtureA; - Fixture f2 = minContact.FixtureB; - Body b3 = f1.Body; - Body b4 = f2.Body; - b3.Advance(minTOI); - b4.Advance(minTOI); - - // The TOI contact likely has some new contact points. - minContact.Update(_contactListener); - minContact._flags &= ~Contact.CollisionFlags.Toi; - - if ((minContact._flags & Contact.CollisionFlags.Touch) == 0) - { - // This shouldn't happen. Numerical error? - //b2Assert(false); - continue; - } - - // Build the TOI island. We need a dynamic seed. - Body seed = b3; - if (seed.IsStatic()) - { - seed = b4; - } - - // Reset island and queue. - island.Clear(); - - int queueStart = 0; // starting index for queue - int queueSize = 0; // elements in queue - queue[queueStart + queueSize++] = seed; - seed._flags |= Body.BodyFlags.Island; - - // Perform a breadth first search (BFS) on the contact/joint graph. - while (queueSize > 0) - { - // Grab the next body off the stack and add it to the island. - Body b = queue[queueStart++]; - --queueSize; - - island.Add(b); - - // Make sure the body is awake. - b._flags &= ~Body.BodyFlags.Sleep; - - // To keep islands as small as possible, we don't - // propagate islands across static bodies. - if (b.IsStatic()) - { - continue; - } - - // Search all contacts connected to this body. - for (ContactEdge cEdge = b._contactList; cEdge != null; cEdge = cEdge.Next) - { - // Does the TOI island still have space for contacts? - if (island._contactCount == island._contactCapacity) - { - continue; - } - - // Has this contact already been added to an island? Skip slow or non-solid contacts. - if ((int)(cEdge.Contact._flags & (Contact.CollisionFlags.Island | Contact.CollisionFlags.Slow | Contact.CollisionFlags.NonSolid)) != 0) - { - continue; - } - - // Is this contact touching? For performance we are not updating this contact. - if ((cEdge.Contact._flags & Contact.CollisionFlags.Touch) == 0) - { - continue; - } - - island.Add(cEdge.Contact); - cEdge.Contact._flags |= Contact.CollisionFlags.Island; - - // Update other body. - Body other = cEdge.Other; - - // Was the other body already added to this island? - if ((int)(other._flags & Body.BodyFlags.Island) == 1) - { - continue; - } - - // March forward, this can do no harm since this is the min TOI. - if (other.IsStatic() == false) - { - other.Advance(minTOI); - other.WakeUp(); - } - - //Box2DNetDebug.Assert(queueStart + queueSize < queueCapacity); - queue[queueStart + queueSize] = other; - ++queueSize; - other._flags |= Body.BodyFlags.Island; - } - - for (JointEdge jEdge = b._jointList; jEdge != null; jEdge = jEdge.Next) - { - if (island._jointCount == island._jointCapacity) - { - continue; - } - - if (jEdge.Joint._islandFlag == true) - { - continue; - } - - island.Add(jEdge.Joint); - - jEdge.Joint._islandFlag = true; - - Body other = jEdge.Other; - - if ((int)(other._flags & Body.BodyFlags.Island) == 1) - { - continue; - } - - if (!other.IsStatic()) - { - other.Advance(minTOI); - other.WakeUp(); - } - - //Box2DNetDebug.Assert(queueStart + queueSize < queueCapacity); - queue[queueStart + queueSize] = other; - ++queueSize; - other._flags |= Body.BodyFlags.Island; - } - } - - TimeStep subStep; - subStep.WarmStarting = false; - subStep.Dt = (1.0f - minTOI) * step.Dt; - subStep.Inv_Dt = 1.0f / subStep.Dt; - subStep.DtRatio = 0.0f; - subStep.VelocityIterations = step.VelocityIterations; - subStep.PositionIterations = step.PositionIterations; - - island.SolveTOI(ref subStep); - - // Post solve cleanup. - for (int i = 0; i < island._bodyCount; ++i) - { - // Allow bodies to participate in future TOI islands. - Body b = island._bodies[i]; - b._flags &= ~Body.BodyFlags.Island; - - if ((int)(b._flags & (Body.BodyFlags.Sleep | Body.BodyFlags.Frozen)) == 1) - { - continue; - } - - if (b.IsStatic()) - { - continue; - } - - // Update fixtures (for broad-phase). If the fixtures go out of - // the world AABB then fixtures and contacts may be destroyed, - // including contacts that are - bool inRange = b.SynchronizeFixtures(); - - // Did the body's fixtures leave the world? - if (inRange == false && _boundaryListener != null) - { - _boundaryListener.Violation(b); - } - - // Invalidate all contact TOIs associated with this body. Some of these - // may not be in the island because they were not touching. - for (ContactEdge cn = b._contactList; cn != null; cn = cn.Next) - { - cn.Contact._flags &= ~Contact.CollisionFlags.Toi; - } - } - - for (int i = 0; i < island._contactCount; ++i) - { - // Allow contacts to participate in future TOI islands. - Contact c = island._contacts[i]; - c._flags &= ~(Contact.CollisionFlags.Toi | Contact.CollisionFlags.Island); - } - - for (int i = 0; i < island._jointCount; ++i) - { - // Allow joints to participate in future TOI islands. - Joint j = island._joints[i]; - j._islandFlag = false; - } - - // Commit fixture proxy movements to the broad-phase so that new contacts are created. - // Also, some contacts can be destroyed. - _broadPhase.Commit(); - } - - queue = null; - } - - private void DrawJoint(Joint joint) - { - Body b1 = joint.GetBody1(); - Body b2 = joint.GetBody2(); - Transform xf1 = b1.GetTransform(); - Transform xf2 = b2.GetTransform(); - Vector2 x1 = xf1.position; - Vector2 x2 = xf2.position; - Vector2 p1 = joint.Anchor1; - Vector2 p2 = joint.Anchor2; - - Color color = new Color(0.5f, 0.8f, 0.8f); - - switch (joint.GetType()) - { - case JointType.DistanceJoint: - _debugDraw.DrawSegment(p1, p2, color); - break; - - case JointType.PulleyJoint: - { - PulleyJoint pulley = (PulleyJoint)joint; - Vector2 s1 = pulley.GroundAnchor1; - Vector2 s2 = pulley.GroundAnchor2; - _debugDraw.DrawSegment(s1, p1, color); - _debugDraw.DrawSegment(s2, p2, color); - _debugDraw.DrawSegment(s1, s2, color); - } - break; - - case JointType.MouseJoint: - // don't draw this - break; - - default: - _debugDraw.DrawSegment(x1, p1, color); - _debugDraw.DrawSegment(p1, p2, color); - _debugDraw.DrawSegment(x2, p2, color); - break; - } - } - - private void DrawFixture(Fixture fixture, Transform xf, Color color, bool core) - { -#warning "the core argument is not used, the coreColor variable is also not used" - Color coreColor = new Color(0.9f, 0.6f, 0.6f); - - switch (fixture.ShapeType) - { - case ShapeType.CircleShape: - { - CircleShape circle = (CircleShape)fixture.Shape; - - Vector2 center = xf.TransformPoint(circle._position); - float radius = circle._radius; - // [CHRISK] FIXME Vector2 axis = xf.R.Col1; - - //_debugDraw.DrawSolidCircle(center, radius, axis, color); - } - break; - - case ShapeType.PolygonShape: - { - PolygonShape poly = (PolygonShape)fixture.Shape; - int vertexCount = poly._vertexCount; - Vector2[] localVertices = poly._vertices; - - Box2DNetDebug.Assert(vertexCount <= Settings.MaxPolygonVertices); - Vector2[] vertices = new Vector2[Settings.MaxPolygonVertices]; - - for (int i = 0; i < vertexCount; ++i) - { - vertices[i] = xf.TransformPoint(localVertices[i]); - } - - _debugDraw.DrawSolidPolygon(vertices, vertexCount, color); - } - break; - - case ShapeType.EdgeShape: - { - EdgeShape edge = (EdgeShape)fixture.Shape; - - _debugDraw.DrawSegment(xf.TransformPoint(edge.Vertex1), xf.TransformPoint(edge.Vertex2), color); - } - break; - } - } - - private void DrawDebugData() - { - if (_debugDraw == null) - { - return; - } - - DebugDraw.DrawFlags flags = _debugDraw.Flags; - - if ((flags & DebugDraw.DrawFlags.Shape) != 0) - { - bool core = (flags & DebugDraw.DrawFlags.CoreShape) == DebugDraw.DrawFlags.CoreShape; - - for (Body b = _bodyList; b != null; b = b.GetNext()) - { - Transform xf = b.GetTransform(); - for (Fixture f = b.GetFixtureList(); f != null; f = f.Next) - { - if (b.IsStatic()) - { - DrawFixture(f, xf, new Color(0.5f, 0.9f, 0.5f), core); - } - else if (b.IsSleeping()) - { - DrawFixture(f, xf, new Color(0.5f, 0.5f, 0.9f), core); - } - else - { - DrawFixture(f, xf, new Color(0.9f, 0.9f, 0.9f), core); - } - } - } - } - - if ((flags & DebugDraw.DrawFlags.Joint) != 0) - { - for (Joint j = _jointList; j != null; j = j.GetNext()) - { - if (j.GetType() != JointType.MouseJoint) - { - DrawJoint(j); - } - } - } - - if ((flags & DebugDraw.DrawFlags.Controller) != 0) - { - for (Controllers.Controller c = _controllerList; c != null; c = c.GetNext()) - { - c.Draw(_debugDraw); - } - } - - if ((flags & DebugDraw.DrawFlags.Pair) != 0) - { - BroadPhase bp = _broadPhase; - Vector2 invQ = new Vector2(1.0f / bp._quantizationFactor.X, 1.0f / bp._quantizationFactor.Y); - Color color = new Color(0.9f, 0.9f, 0.3f); - - for (int i = 0; i < PairManager.TableCapacity; ++i) - { - ushort index = bp._pairManager._hashTable[i]; - while (index != PairManager.NullPair) - { - Pair pair = bp._pairManager._pairs[index]; - Proxy p1 = bp._proxyPool[pair.ProxyId1]; - Proxy p2 = bp._proxyPool[pair.ProxyId2]; - - AABB b1 = new AABB(), b2 = new AABB(); - b1.LowerBound.X = bp._worldAABB.LowerBound.X + invQ.X * bp._bounds[0][p1.LowerBounds[0]].Value; - b1.LowerBound.Y = bp._worldAABB.LowerBound.Y + invQ.Y * bp._bounds[1][p1.LowerBounds[1]].Value; - b1.UpperBound.X = bp._worldAABB.LowerBound.X + invQ.X * bp._bounds[0][p1.UpperBounds[0]].Value; - b1.UpperBound.Y = bp._worldAABB.LowerBound.Y + invQ.Y * bp._bounds[1][p1.UpperBounds[1]].Value; - b2.LowerBound.X = bp._worldAABB.LowerBound.X + invQ.X * bp._bounds[0][p2.LowerBounds[0]].Value; - b2.LowerBound.Y = bp._worldAABB.LowerBound.Y + invQ.Y * bp._bounds[1][p2.LowerBounds[1]].Value; - b2.UpperBound.X = bp._worldAABB.LowerBound.X + invQ.X * bp._bounds[0][p2.UpperBounds[0]].Value; - b2.UpperBound.Y = bp._worldAABB.LowerBound.Y + invQ.Y * bp._bounds[1][p2.UpperBounds[1]].Value; - - Vector2 x1 = 0.5f * (b1.LowerBound + b1.UpperBound); - Vector2 x2 = 0.5f * (b2.LowerBound + b2.UpperBound); - - _debugDraw.DrawSegment(x1, x2, color); - - index = pair.Next; - } - } - } - - if ((flags & DebugDraw.DrawFlags.Aabb) != 0) - { - BroadPhase bp = _broadPhase; - Vector2 worldLower = bp._worldAABB.LowerBound; - Vector2 worldUpper = bp._worldAABB.UpperBound; - - Vector2 invQ = new Vector2(1.0f / bp._quantizationFactor.X, 1.0f / bp._quantizationFactor.Y); - Color color = new Color(0.9f, 0.3f, 0.9f); - for (int i = 0; i < Settings.MaxProxies; ++i) - { - Proxy p = bp._proxyPool[i]; - if (p.IsValid == false) - { - continue; - } - - AABB b = new AABB(); - b.LowerBound.X = worldLower.X + invQ.X * bp._bounds[0][p.LowerBounds[0]].Value; - b.LowerBound.Y = worldLower.Y + invQ.Y * bp._bounds[1][p.LowerBounds[1]].Value; - b.UpperBound.X = worldLower.X + invQ.X * bp._bounds[0][p.UpperBounds[0]].Value; - b.UpperBound.Y = worldLower.Y + invQ.Y * bp._bounds[1][p.UpperBounds[1]].Value; - - Vector2[] vs1 = new Vector2[4]; - vs1[0] = new Vector2(b.LowerBound.X, b.LowerBound.Y); - vs1[1] = new Vector2(b.UpperBound.X, b.LowerBound.Y); - vs1[2] = new Vector2(b.UpperBound.X, b.UpperBound.Y); - vs1[3] = new Vector2(b.LowerBound.X, b.UpperBound.Y); - - _debugDraw.DrawPolygon(vs1, 4, color); - } - - Vector2[] vs = new Vector2[4]; - vs[0] = new Vector2(worldLower.X, worldLower.Y); - vs[1] = new Vector2(worldUpper.X, worldLower.Y); - vs[2] = new Vector2(worldUpper.X, worldUpper.Y); - vs[3] = new Vector2(worldLower.X, worldUpper.Y); - _debugDraw.DrawPolygon(vs, 4, new Color(0.3f, 0.9f, 0.9f)); - } - - if ((flags & DebugDraw.DrawFlags.CenterOfMass) != 0) - { - for (Body b = _bodyList; b != null; b = b.GetNext()) - { - Transform xf = b.GetTransform(); - xf.position = b.GetWorldCenter(); - _debugDraw.DrawTransform(xf); - } - } - } - - //Is it safe to pass private static function pointers? - private static float RaycastSortKey(object data) - { - Fixture fixture = data as Fixture; - Box2DNetDebug.Assert(fixture != null); - Body body = fixture.Body; - World world = body.GetWorld(); - - if (world._contactFilter != null && !world._contactFilter.RayCollide(world._raycastUserData, fixture)) - return -1; - - float lambda; - - SegmentCollide collide = fixture.TestSegment(out lambda, out world._raycastNormal, world._raycastSegment, 1); - - if (world._raycastSolidShape && collide == SegmentCollide.MissCollide) - return -1; - if (!world._raycastSolidShape && collide != SegmentCollide.HitCollide) - return -1; - - return lambda; - } - - public bool InRange(AABB aabb) - { - return _broadPhase.InRange(aabb); - } - } -} +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ +//#define USE_ACTIVE_CONTACT_SET +//#define USE_AWAKE_BODY_SET +//#define USE_ISLAND_SET +//#define OPTIMIZE_TOI +//#define USE_IGNORE_CCD_CATEGORIES + +using System; +using System.Collections.Generic; +using System.Diagnostics; +using Box2DNet.Collision; +using Box2DNet.Common; +using Box2DNet.Controllers; +using Box2DNet.Dynamics.Contacts; +using Box2DNet.Dynamics.Joints; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Dynamics +{ + /// + /// The world class manages all physics entities, dynamic simulation, + /// and asynchronous queries. + /// + public class World + { + private float _invDt0; + 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 Func _queryAABBCallback; + private Func _queryAABBCallbackWrapper; + private TOIInput _input = new TOIInput(); + private Fixture _myFixture; + private Vector2 _point1; + private Vector2 _point2; + private List _testPointAllFixtures; + private Stopwatch _watch = new Stopwatch(); + private Func _rayCastCallback; + private Func _rayCastCallbackWrapper; + + internal Queue _contactPool = new Queue(256); + internal bool _worldHasNewFixture; + + /// + /// Fires whenever a body has been added + /// + public BodyDelegate BodyAdded; + + /// + /// Fires whenever a body has been removed + /// + public BodyDelegate BodyRemoved; + + /// + /// Fires whenever a fixture has been added + /// + public FixtureDelegate FixtureAdded; + + /// + /// Fires whenever a fixture has been removed + /// + public FixtureDelegate FixtureRemoved; + + /// + /// Fires whenever a joint has been added + /// + public JointDelegate JointAdded; + + /// + /// Fires whenever a joint has been removed + /// + public JointDelegate JointRemoved; + + /// + /// Fires every time a controller is added to the World. + /// + public ControllerDelegate ControllerAdded; + + /// + /// Fires every time a controlelr is removed form the World. + /// + public ControllerDelegate ControllerRemoved; + + /// + /// Initializes a new instance of the class. + /// + public World(Vector2 gravity) + { + Island = new Island(); + Enabled = true; + ControllerList = new List(); + BreakableBodyList = new List(); + BodyList = new List(32); + JointList = new List(32); + +#if USE_AWAKE_BODY_SET + AwakeBodySet = new HashSet(); + AwakeBodyList = new List(32); +#endif +#if USE_ISLAND_SET + IslandSet = new HashSet(); +#endif +#if OPTIMIZE_TOI + TOISet = new HashSet(); +#endif + + _queryAABBCallbackWrapper = QueryAABBCallbackWrapper; + _rayCastCallbackWrapper = RayCastCallbackWrapper; + + ContactManager = new ContactManager(new DynamicTreeBroadPhase()); + Gravity = gravity; + } + + 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.FilterFlag = true; + } + + 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.FilterFlag = true; + } + + 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) + { +#if USE_AWAKE_BODY_SET + Debug.Assert(!body.IsDisposed); + if (body.Awake) + { + if (!AwakeBodySet.Contains(body)) + AwakeBodySet.Add(body); + } + else + { + if (AwakeBodySet.Contains(body)) + AwakeBodySet.Remove(body); + } +#endif + // 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)); + +#if USE_AWAKE_BODY_SET + Debug.Assert(!AwakeBodySet.Contains(body)); +#endif + // 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); + +#if USE_AWAKE_BODY_SET + Debug.Assert(!AwakeBodySet.Contains(body)); +#endif + } + + _bodyRemoveList.Clear(); + } + } + + private bool QueryAABBCallbackWrapper(int proxyId) + { + FixtureProxy proxy = ContactManager.BroadPhase.GetProxy(proxyId); + return _queryAABBCallback(proxy.Fixture); + } + + private float RayCastCallbackWrapper(RayCastInput rayCastInput, int 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) * rayCastInput.Point1 + fraction * rayCastInput.Point2; + return _rayCastCallback(fixture, point, output.Normal, fraction); + } + + return rayCastInput.MaxFraction; + } + + 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. +#if USE_ISLAND_SET + Debug.Assert(IslandSet.Count == 0); +#else + foreach (Body b in BodyList) + { + b._island = false; + } +#endif + +#if USE_ACTIVE_CONTACT_SET + foreach (var c in ContactManager.ActiveContacts) + { + c.Flags &= ~ContactFlags.Island; + } +#else + foreach (Contact c in ContactManager.ContactList) + { + c.IslandFlag = false; + } +#endif + 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)]; + +#if USE_AWAKE_BODY_SET + + // If AwakeBodyList is empty, the Island code will not have a chance + // to update the diagnostics timer so reset the timer here. + Island.JointUpdateTime = 0; + + Debug.Assert(AwakeBodyList.Count == 0); + AwakeBodyList.AddRange(AwakeBodySet); + + foreach (var seed in AwakeBodyList) + { +#else + for (int index = BodyList.Count - 1; index >= 0; index--) + { + Body seed = BodyList[index]; +#endif + if (seed._island) + { + 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; + +#if USE_ISLAND_SET + if (!IslandSet.Contains(body)) + IslandSet.Add(body); +#endif + seed._island = true; + + // 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.IslandFlag) + { + continue; + } + + // Is this contact solid and touching? + if (ce.Contact.Enabled == false || ce.Contact.IsTouching == false) + { + continue; + } + + // Skip sensors. + bool sensorA = contact.FixtureA.IsSensor; + bool sensorB = contact.FixtureB.IsSensor; + if (sensorA || sensorB) + { + continue; + } + + Island.Add(contact); + contact.IslandFlag = true; + + Body other = ce.Other; + + // Was the other body already added to this island? + if (other._island) + { + continue; + } + + Debug.Assert(stackCount < stackSize); + _stack[stackCount++] = other; + +#if USE_ISLAND_SET + if (!IslandSet.Contains(body)) + IslandSet.Add(body); +#endif + other._island = true; + } + + // 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._island) + { + continue; + } + + Debug.Assert(stackCount < stackSize); + _stack[stackCount++] = other; +#if USE_ISLAND_SET + if (!IslandSet.Contains(body)) + IslandSet.Add(body); +#endif + other._island = true; + } + 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._island = false; + } + } + } + + // Synchronize fixtures, check for out of range bodies. +#if USE_ISLAND_SET + foreach (var b in IslandSet) +#else + foreach (Body b in BodyList) +#endif + { + // If a body was not in an island then it did not move. + if (!b._island) + { + continue; + } +#if USE_ISLAND_SET + Debug.Assert(b.BodyType != BodyType.Static); +#else + if (b.BodyType == BodyType.Static) + { + continue; + } +#endif + + // Update fixtures (for broad-phase). + b.SynchronizeFixtures(); + } +#if OPTIMIZE_TOI + foreach (var b in IslandSet) + { + if (!TOISet.Contains(b)) + { + TOISet.Add(b); + } + } +#endif +#if USE_ISLAND_SET + IslandSet.Clear(); +#endif + + // Look for new contacts. + ContactManager.FindNewContacts(); + +#if USE_AWAKE_BODY_SET + AwakeBodyList.Clear(); +#endif + } + + private void SolveTOI(ref TimeStep step) + { + Island.Reset(2 * Settings.MaxTOIContacts, Settings.MaxTOIContacts, 0, ContactManager); + +#if OPTIMIZE_TOI + bool wasStepComplete = _stepComplete; +#endif + if (_stepComplete) + { +#if OPTIMIZE_TOI + foreach (var b in TOISet) + { + b.Flags &= ~BodyFlags.Island; + b.Sweep.Alpha0 = 0.0f; + } +#else + for (int i = 0; i < BodyList.Count; i++) + { + BodyList[i]._island = false; + BodyList[i]._sweep.Alpha0 = 0.0f; + } +#endif +#if USE_ACTIVE_CONTACT_SET + foreach (var c in ContactManager.ActiveContacts) + { +#else + for (int i = 0; i < ContactManager.ContactList.Count; i++) + { + Contact c = ContactManager.ContactList[i]; +#endif + // Invalidate TOI + c.IslandFlag = false; + c.TOIFlag = false; + 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; + +#if USE_ACTIVE_CONTACT_SET + foreach (var c in ContactManager.ActiveContacts) + { +#else + for (int i = 0; i < ContactManager.ContactList.Count; i++) + { + Contact c = ContactManager.ContactList[i]; +#endif + + // Is this contact disabled? + if (c.Enabled == false) + { + continue; + } + + // Prevent excessive sub-stepping. + if (c._toiCount > Settings.MaxSubSteps) + { + continue; + } + + float alpha; + if (c.TOIFlag) + { + // 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 activeA = bA.Awake && typeA != BodyType.Static; + bool activeB = bB.Awake && typeB != BodyType.Static; + + // Is at least one body active (awake and dynamic or kinematic)? + if (activeA == false && activeB == false) + { + continue; + } + + bool collideA = (bA.IsBullet || typeA != BodyType.Dynamic) && ((fA.IgnoreCCDWith & fB.CollisionCategories) == 0) && !bA.IgnoreCCD; + bool collideB = (bB.IsBullet || typeB != BodyType.Dynamic) && ((fB.IgnoreCCDWith & fA.CollisionCategories) == 0) && !bB.IgnoreCCD; + + // Are these two non-bullet dynamic bodies? + if (collideA == false && collideB == false) + { + continue; + } + +#if OPTIMIZE_TOI + if (_stepComplete) + { + if (!TOISet.Contains(bA)) + { + TOISet.Add(bA); + bA.Flags &= ~BodyFlags.Island; + bA.Sweep.Alpha0 = 0.0f; + } + if (!TOISet.Contains(bB)) + { + TOISet.Add(bB); + bB.Flags &= ~BodyFlags.Island; + bB.Sweep.Alpha0 = 0.0f; + } + } +#endif + // 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.TOIFlag = true; + } + + 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 bA0 = fA1.Body; + Body bB0 = fB1.Body; + + Sweep backup1 = bA0._sweep; + Sweep backup2 = bB0._sweep; + + bA0.Advance(minAlpha); + bB0.Advance(minAlpha); + + // The TOI contact likely has some new contact points. + minContact.Update(ContactManager); + minContact.TOIFlag = false; + ++minContact._toiCount; + + // Is the contact solid? + if (minContact.Enabled == false || minContact.IsTouching == false) + { + // Restore the sweeps. + minContact.Enabled = false; + bA0._sweep = backup1; + bB0._sweep = backup2; + bA0.SynchronizeTransform(); + bB0.SynchronizeTransform(); + continue; + } + + bA0.Awake = true; + bB0.Awake = true; + + // Build the island + Island.Clear(); + Island.Add(bA0); + Island.Add(bB0); + Island.Add(minContact); + + bA0._island = true; + bB0._island = true; + minContact.IslandFlag = true; + + // Get contacts on bodyA and bodyB. + Body[] bodies = { bA0, bB0 }; + for (int i = 0; i < 2; ++i) + { + Body body = bodies[i]; + if (body.BodyType == BodyType.Dynamic) + { + for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next) + { + Contact contact = ce.Contact; + + if (Island.BodyCount == Island.BodyCapacity) + { + break; + } + + if (Island.ContactCount == Island.ContactCapacity) + { + break; + } + + // Has this contact already been added to the island? + if (contact.IslandFlag) + { + 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._island) + { + 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.IslandFlag = true; + Island.Add(contact); + + // Has the other body already been added to the island? + if (other._island) + { + continue; + } + + // Add the other body to the island. + other._island = true; + + if (other.BodyType != BodyType.Static) + { + other.Awake = true; + } +#if OPTIMIZE_TOI + if (_stepComplete) + { + if (!TOISet.Contains(other)) + { + TOISet.Add(other); + other.Sweep.Alpha0 = 0.0f; + } + } +#endif + Island.Add(other); + } + } + } + + TimeStep subStep; + subStep.dt = (1.0f - minAlpha) * step.dt; + subStep.inv_dt = 1.0f / subStep.dt; + subStep.dtRatio = 1.0f; + Island.SolveTOI(ref subStep, bA0.IslandIndex, bB0.IslandIndex, false); + + // Reset island flags and synchronize broad-phase proxies. + for (int i = 0; i < Island.BodyCount; ++i) + { + Body body = Island.Bodies[i]; + body._island = false; + + 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.TOIFlag = false; + ce.Contact.IslandFlag = false; + } + } + + // Commit fixture proxy movements to the broad-phase so that new contacts are created. + // Also, some contacts can be destroyed. + ContactManager.FindNewContacts(); + + if (Settings.EnableSubStepping) + { + _stepComplete = false; + break; + } + } +#if OPTIMIZE_TOI + if (wasStepComplete) + { + TOISet.Clear(); + } +#endif + } + + 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 NewContactsTime { 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; + + /// + /// 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; } + +#if USE_AWAKE_BODY_SET + public HashSet AwakeBodySet { get; private set; } + List AwakeBodyList; +#endif +#if USE_ISLAND_SET + HashSet IslandSet; +#endif +#if OPTIMIZE_TOI + HashSet TOISet; +#endif + + /// + /// 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; } + } + + /// + /// If false, the whole simulation stops. It still processes added and removed geometries. + /// + public bool Enabled { get; set; } + + public Island Island { get; private set; } + + /// + /// 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); + +#if USE_AWAKE_BODY_SET + if (AwakeBodySet.Contains(body)) + { + AwakeBodySet.Remove(body); + } +#endif + } + + /// + /// 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(); +#if DEBUG && USE_AWAKE_BODY_SET + foreach (var b in AwakeBodySet) + { + Debug.Assert(BodyList.Contains(b)); + } +#endif + } + + /// + /// 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 (!Enabled) + return; + + if (Settings.EnableDiagnostics) + _watch.Start(); + + ProcessChanges(); + + if (Settings.EnableDiagnostics) + AddRemoveTime = _watch.ElapsedTicks; + + // If new fixtures were added, we need to find the new contacts. + if (_worldHasNewFixture) + { + ContactManager.FindNewContacts(); + _worldHasNewFixture = false; + } + + if (Settings.EnableDiagnostics) + NewContactsTime = _watch.ElapsedTicks - AddRemoveTime; + + //FPE only: moved position and velocity iterations into Settings.cs + TimeStep step; + step.inv_dt = dt > 0.0f ? 1.0f / dt : 0.0f; + step.dt = dt; + step.dtRatio = _invDt0 * dt; + + //Update controllers + for (int i = 0; i < ControllerList.Count; i++) + { + ControllerList[i].Update(dt); + } + + if (Settings.EnableDiagnostics) + ControllersUpdateTime = _watch.ElapsedTicks - (AddRemoveTime + NewContactsTime); + + // Update contacts. This is where some contacts are destroyed. + ContactManager.Collide(); + + if (Settings.EnableDiagnostics) + ContactsUpdateTime = _watch.ElapsedTicks - (AddRemoveTime + NewContactsTime + ControllersUpdateTime); + + // Integrate velocities, solve velocity raints, and integrate positions. + Solve(ref step); + + if (Settings.EnableDiagnostics) + SolveUpdateTime = _watch.ElapsedTicks - (AddRemoveTime + NewContactsTime + ControllersUpdateTime + ContactsUpdateTime); + + // Handle TOI events. + if (Settings.ContinuousPhysics) + { + SolveTOI(ref step); + } + + if (Settings.EnableDiagnostics) + ContinuousPhysicsTime = _watch.ElapsedTicks - (AddRemoveTime + NewContactsTime + ControllersUpdateTime + ContactsUpdateTime + SolveUpdateTime); + + if (Settings.AutoClearForces) + ClearForces(); + + for (int i = 0; i < BreakableBodyList.Count; i++) + { + BreakableBodyList[i].Update(); + } + + _invDt0 = step.inv_dt; + + if (Settings.EnableDiagnostics) + { + _watch.Stop(); + UpdateTime = _watch.ElapsedTicks; + _watch.Reset(); + } + } + + /// + /// 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) + { + _queryAABBCallback = callback; + ContactManager.BroadPhase.Query(_queryAABBCallbackWrapper, ref aabb); + _queryAABBCallback = null; + } + + /// + /// Query the world for all fixtures that potentially overlap the provided AABB. + /// Use the overload with a callback for filtering and better performance. + /// + /// The aabb query box. + /// A list of fixtures that were in the affected area. + public List QueryAABB(ref AABB aabb) + { + List affected = new List(); + + QueryAABB(fixture => + { + affected.Add(fixture); + return true; + }, ref aabb); + + return affected; + } + + /// + /// 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(Func callback, Vector2 point1, Vector2 point2) + { + RayCastInput input = new RayCastInput(); + input.MaxFraction = 1.0f; + input.Point1 = point1; + input.Point2 = point2; + + _rayCastCallback = callback; + ContactManager.BroadPhase.RayCast(_rayCastCallbackWrapper, ref input); + _rayCastCallback = null; + } + + public List RayCast(Vector2 point1, Vector2 point2) + { + List affected = new List(); + + RayCast((f, p, n, fr) => + { + affected.Add(f); + return 1; + }, point1, point2); + + return affected; + } + + 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; + + _myFixture = null; + _point1 = point; + + // Query the world for overlapping shapes. + QueryAABB(TestPointCallback, ref aabb); + + return _myFixture; + } + + private bool TestPointCallback(Fixture fixture) + { + bool inside = fixture.TestPoint(ref _point1); + if (inside) + { + _myFixture = fixture; + return false; + } + + // Continue the query. + return true; + } + + /// + /// 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; + + _point2 = point; + _testPointAllFixtures = new List(); + + // Query the world for overlapping shapes. + QueryAABB(TestPointAllCallback, ref aabb); + + return _testPointAllFixtures; + } + + private bool TestPointAllCallback(Fixture fixture) + { + bool inside = fixture.TestPoint(ref _point2); + if (inside) + _testPointAllFixtures.Add(fixture); + + // Continue the query. + return true; + } + + /// Shift the world origin. Useful for large worlds. + /// The body shift formula is: position -= newOrigin + /// @param newOrigin the new origin with respect to the old origin + /// Warning: Calling this method mid-update might cause a crash. + public void ShiftOrigin(Vector2 newOrigin) + { + foreach (Body b in BodyList) + { + b._xf.p -= newOrigin; + b._sweep.C0 -= newOrigin; + b._sweep.C -= newOrigin; + } + + foreach (Joint joint in JointList) + { + //joint.ShiftOrigin(newOrigin); //TODO: uncomment + } + + ContactManager.BroadPhase.ShiftOrigin(newOrigin); + } + + 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/src/Box2DNet/Dynamics/WorldCallbacks.cs b/src/Box2DNet/Dynamics/WorldCallbacks.cs index 16516f7..d322149 100644 --- a/src/Box2DNet/Dynamics/WorldCallbacks.cs +++ b/src/Box2DNet/Dynamics/WorldCallbacks.cs @@ -1,243 +1,63 @@ -/* - Box2DNet Copyright (c) 2009 Ihar Kalasouski http://code.google.com/p/box2dx - Box2D original C++ version 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.Numerics; -using Box2DNet.Common; -using Box2DNet.Collision; - - -using Transform = Box2DNet.Common.Transform; - -namespace Box2DNet.Dynamics -{ - /// - /// Joints and shapes are destroyed when their associated - /// body is destroyed. Implement this listener so that you - /// may nullify references to these joints and shapes. - /// - public abstract class DestructionListener - { - /// - /// Called when any joint is about to be destroyed due - /// to the destruction of one of its attached bodies. - /// - public abstract void SayGoodbye(Joint joint); - - /// - /// Called when any shape is about to be destroyed due - /// to the destruction of its parent body. - /// - public abstract void SayGoodbye(Fixture fixture); - } - - /// - /// This is called when a body's shape passes outside of the world boundary. - /// - public abstract class BoundaryListener - { - /// - /// This is called for each body that leaves the world boundary. - /// @warning you can't modify the world inside this callback. - /// - public abstract void Violation(Body body); - } - - /// - /// Implement this class to provide collision filtering. In other words, you can implement - /// this class if you want finer control over contact creation. - /// - public class ContactFilter - { - /// - /// Return true if contact calculations should be performed between these two shapes. - /// If you implement your own collision filter you may want to build from this implementation. - /// @warning for performance reasons this is only called when the AABBs begin to overlap. - /// - public virtual bool ShouldCollide(Fixture fixtureA, Fixture fixtureB) - { - FilterData filterA = fixtureA.Filter; - FilterData filterB = fixtureB.Filter; - - if (filterA.GroupIndex == filterB.GroupIndex && filterA.GroupIndex != 0) - { - return filterA.GroupIndex > 0; - } - - bool collide = (filterA.MaskBits & filterB.CategoryBits) != 0 && (filterA.CategoryBits & filterB.MaskBits) != 0; - return collide; - } - - /// - /// Return true if the given shape should be considered for ray intersection. - /// - public bool RayCollide(object userData, Fixture fixture) - { - //By default, cast userData as a shape, and then collide if the shapes would collide - if (userData == null) - { - return true; - } - - return ShouldCollide((Fixture)userData, fixture); - } - } - - /// Contact impulses for reporting. Impulses are used instead of forces because - /// sub-step forces may approach infinity for rigid body collisions. These - /// match up one-to-one with the contact points in b2Manifold. - public class ContactImpulse - { - public float[] normalImpulses = new float[Settings.MaxManifoldPoints]; - public float[] tangentImpulses = new float[Settings.MaxManifoldPoints]; - } - - /// Implement this class to get contact information. You can use these results for - /// things like sounds and game logic. You can also get contact results by - /// traversing the contact lists after the time step. However, you might miss - /// some contacts because continuous physics leads to sub-stepping. - /// Additionally you may receive multiple callbacks for the same contact in a - /// single time step. - /// You should strive to make your callbacks efficient because there may be - /// many callbacks per time step. - /// @warning You cannot create/destroy Box2DNet entities inside these callbacks. - public interface ContactListener - { - /// Called when two fixtures begin to touch. - void BeginContact(Contact contact); - - /// Called when two fixtures cease to touch. - void EndContact(Contact contact); - - /// This is called after a contact is updated. This allows you to inspect a - /// contact before it goes to the solver. If you are careful, you can modify the - /// contact manifold (e.g. disable contact). - /// A copy of the old manifold is provided so that you can detect changes. - /// Note: this is called only for awake bodies. - /// Note: this is called even when the number of contact points is zero. - /// Note: this is not called for sensors. - /// Note: if you set the number of contact points to zero, you will not - /// get an EndContact callback. However, you may get a BeginContact callback - /// the next step. - void PreSolve(Contact contact, Manifold oldManifold); - - /// This lets you inspect a contact after the solver is finished. This is useful - /// for inspecting impulses. - /// Note: the contact manifold does not include time of impact impulses, which can be - /// arbitrarily large if the sub-step is small. Hence the impulse is provided explicitly - /// in a separate data structure. - /// Note: this is only called for contacts that are touching, solid, and awake. - void PostSolve(Contact contact, ContactImpulse impulse); - } - - /// - /// Color for debug drawing. Each value has the range [0,1]. - /// - public struct Color - { - public float R, G, B; - - public Color(float r, float g, float b) - { - R = r; G = g; B = b; - } - public void Set(float r, float g, float b) - { - R = r; G = g; B = b; - } - } - - /// - /// Implement and register this class with a b2World to provide debug drawing of physics - /// entities in your game. - /// - public abstract class DebugDraw - { - [Flags] - public enum DrawFlags - { - Shape = 0x0001, // draw shapes - Joint = 0x0002, // draw joint connections - CoreShape = 0x0004, // draw core (TOI) shapes // should be removed in this revision? - Aabb = 0x0008, // draw axis aligned bounding boxes - Obb = 0x0010, // draw oriented bounding boxes // should be removed in this revision? - Pair = 0x0020, // draw broad-phase pairs - CenterOfMass = 0x0040, // draw center of mass frame - Controller = 0x0080 // draw center of mass frame - }; - - protected DrawFlags _drawFlags; - - public DebugDraw() - { - _drawFlags = 0; - } - - public DrawFlags Flags { get { return _drawFlags; } set { _drawFlags = value; } } - - /// - /// Append flags to the current flags. - /// - public void AppendFlags(DrawFlags flags) - { - _drawFlags |= flags; - } - - /// - /// Clear flags from the current flags. - /// - public void ClearFlags(DrawFlags flags) - { - _drawFlags &= ~flags; - } - - /// - /// Draw a closed polygon provided in CCW order. - /// - public abstract void DrawPolygon(Vector2[] vertices, int vertexCount, Color color); - - /// - /// Draw a solid closed polygon provided in CCW order. - /// - public abstract void DrawSolidPolygon(Vector2[] vertices, int vertexCount, Color color); - - /// - /// Draw a circle. - /// - public abstract void DrawCircle(Vector2 center, float radius, Color color); - - /// - /// Draw a solid circle. - /// - public abstract void DrawSolidCircle(Vector2 center, float radius, Vector2 axis, Color color); - - /// - /// Draw a line segment. - /// - public abstract void DrawSegment(Vector2 p1, Vector2 p2, Color color); - - /// - /// Draw a Transform. Choose your own length scale. - /// - /// A Transform. - public abstract void DrawTransform(Transform xf); - } -} +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using Box2DNet.Collision; +using Box2DNet.Controllers; +using Box2DNet.Dynamics.Contacts; +using Box2DNet.Dynamics.Joints; + +namespace Box2DNet.Dynamics +{ + /// + /// 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, ContactVelocityConstraint 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, ContactVelocityConstraint impulse); + + public delegate void OnSeparationEventHandler(Fixture fixtureA, Fixture fixtureB); +} \ No newline at end of file diff --git a/src/Box2DNet/Factories/BodyFactory.cs b/src/Box2DNet/Factories/BodyFactory.cs new file mode 100644 index 0000000..b832ff9 --- /dev/null +++ b/src/Box2DNet/Factories/BodyFactory.cs @@ -0,0 +1,189 @@ +using System; +using System.Collections.Generic; +using Box2DNet.Collision.Shapes; +using Box2DNet.Common; +using Box2DNet.Common.Decomposition; +using Box2DNet.Dynamics; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Factories +{ + public static class BodyFactory + { + public static Body CreateBody(World world, Vector2 position = new Vector2(), float rotation = 0, BodyType bodyType = BodyType.Static, object userData = null) + { + return new Body(world, position, rotation, bodyType, userData); + } + + public static Body CreateEdge(World world, Vector2 start, Vector2 end, object userData = null) + { + Body body = CreateBody(world); + FixtureFactory.AttachEdge(start, end, body, userData); + return body; + } + + public static Body CreateChainShape(World world, Vertices vertices, Vector2 position = new Vector2(), object userData = null) + { + Body body = CreateBody(world, position); + FixtureFactory.AttachChainShape(vertices, body, userData); + return body; + } + + public static Body CreateLoopShape(World world, Vertices vertices, Vector2 position = new Vector2(), object userData = null) + { + Body body = CreateBody(world, position); + FixtureFactory.AttachLoopShape(vertices, body, userData); + return body; + } + + public static Body CreateRectangle(World world, float width, float height, float density, Vector2 position = new Vector2(), float rotation = 0, BodyType bodyType = BodyType.Static, object userData = null) + { + if (width <= 0) + throw new ArgumentOutOfRangeException("width", "Width must be more than 0 meters"); + + if (height <= 0) + throw new ArgumentOutOfRangeException("height", "Height must be more than 0 meters"); + + Body newBody = CreateBody(world, position, rotation, bodyType); + newBody.UserData = userData; + + Vertices rectangleVertices = PolygonTools.CreateRectangle(width / 2, height / 2); + PolygonShape rectangleShape = new PolygonShape(rectangleVertices, density); + newBody.CreateFixture(rectangleShape); + + return newBody; + } + + public static Body CreateCircle(World world, float radius, float density, Vector2 position = new Vector2(), BodyType bodyType = BodyType.Static, object userData = null) + { + Body body = CreateBody(world, position, 0, bodyType); + FixtureFactory.AttachCircle(radius, density, body, userData); + return body; + } + + public static Body CreateEllipse(World world, float xRadius, float yRadius, int edges, float density, Vector2 position = new Vector2(), float rotation = 0, BodyType bodyType = BodyType.Static, object userData = null) + { + Body body = CreateBody(world, position, rotation, bodyType); + FixtureFactory.AttachEllipse(xRadius, yRadius, edges, density, body, userData); + return body; + } + + public static Body CreatePolygon(World world, Vertices vertices, float density, Vector2 position = new Vector2(), float rotation = 0, BodyType bodyType = BodyType.Static, object userData = null) + { + Body body = CreateBody(world, position, rotation, bodyType); + FixtureFactory.AttachPolygon(vertices, density, body, userData); + return body; + } + + public static Body CreateCompoundPolygon(World world, List list, float density, Vector2 position = new Vector2(), float rotation = 0, BodyType bodyType = BodyType.Static, object userData = null) + { + //We create a single body + Body polygonBody = CreateBody(world, position, rotation, bodyType); + FixtureFactory.AttachCompoundPolygon(list, density, polygonBody, userData); + return polygonBody; + } + + public static Body CreateGear(World world, float radius, int numberOfTeeth, float tipPercentage, float toothHeight, float density, Vector2 position = new Vector2(), float rotation = 0, BodyType bodyType = BodyType.Static, object userData = null) + { + Vertices gearPolygon = PolygonTools.CreateGear(radius, numberOfTeeth, tipPercentage, toothHeight); + + //Gears can in some cases be convex + if (!gearPolygon.IsConvex()) + { + //Decompose the gear: + List list = Triangulate.ConvexPartition(gearPolygon, TriangulationAlgorithm.Earclip); + + return CreateCompoundPolygon(world, list, density, position, rotation, bodyType, userData); + } + + return CreatePolygon(world, gearPolygon, density, position, rotation, bodyType, userData); + } + + public static Body CreateCapsule(World world, float height, float topRadius, int topEdges, float bottomRadius, int bottomEdges, float density, Vector2 position = new Vector2(), float rotation = 0, BodyType bodyType = BodyType.Static, object userData = null) + { + Vertices verts = PolygonTools.CreateCapsule(height, topRadius, topEdges, bottomRadius, bottomEdges); + + //There are too many vertices in the capsule. We decompose it. + if (verts.Count >= Settings.MaxPolygonVertices) + { + List vertList = Triangulate.ConvexPartition(verts, TriangulationAlgorithm.Earclip); + return CreateCompoundPolygon(world, vertList, density, position, rotation, bodyType, userData); + } + + return CreatePolygon(world, verts, density, position, rotation, bodyType, userData); + } + + public static Body CreateCapsule(World world, float height, float endRadius, float density, Vector2 position = new Vector2(), float rotation = 0, BodyType bodyType = BodyType.Static, object userData = null) + { + //Create the middle rectangle + Vertices rectangle = PolygonTools.CreateRectangle(endRadius, height / 2); + + List list = new List(); + list.Add(rectangle); + + Body body = CreateCompoundPolygon(world, list, density, position, rotation, bodyType, userData); + FixtureFactory.AttachCircle(endRadius, density, body, new Vector2(0, height / 2)); + FixtureFactory.AttachCircle(endRadius, density, body, new Vector2(0, -(height / 2))); + + //Create the two circles + //CircleShape topCircle = new CircleShape(endRadius, density); + //topCircle.Position = new Vector2(0, height / 2); + //body.CreateFixture(topCircle); + + //CircleShape bottomCircle = new CircleShape(endRadius, density); + //bottomCircle.Position = new Vector2(0, -(height / 2)); + //body.CreateFixture(bottomCircle); + return body; + } + + public static Body CreateRoundedRectangle(World world, float width, float height, float xRadius, float yRadius, int segments, float density, Vector2 position = new Vector2(), float rotation = 0, BodyType bodyType = BodyType.Static, object userData = null) + { + Vertices verts = PolygonTools.CreateRoundedRectangle(width, height, xRadius, yRadius, segments); + + //There are too many vertices in the capsule. We decompose it. + if (verts.Count >= Settings.MaxPolygonVertices) + { + List vertList = Triangulate.ConvexPartition(verts, TriangulationAlgorithm.Earclip); + return CreateCompoundPolygon(world, vertList, density, position, rotation, bodyType, userData); + } + + return CreatePolygon(world, verts, density, position, rotation, bodyType, userData); + } + + public static Body CreateLineArc(World world, float radians, int sides, float radius, bool closed = false, Vector2 position = new Vector2(), float rotation = 0, BodyType bodyType = BodyType.Static) + { + Body body = CreateBody(world, position, rotation, bodyType); + FixtureFactory.AttachLineArc(radians, sides, radius, closed, body); + return body; + } + + public static Body CreateSolidArc(World world, float density, float radians, int sides, float radius, Vector2 position = new Vector2(), float rotation = 0, BodyType bodyType = BodyType.Static) + { + Body body = CreateBody(world, position, rotation, bodyType); + FixtureFactory.AttachSolidArc(density, radians, sides, radius, body); + + return body; + } + + public static BreakableBody CreateBreakableBody(World world, Vertices vertices, float density, Vector2 position = new Vector2(), float rotation = 0) + { + //TODO: Implement a Voronoi diagram algorithm to split up the vertices + List triangles = Triangulate.ConvexPartition(vertices, TriangulationAlgorithm.Earclip); + + BreakableBody breakableBody = new BreakableBody(world, triangles, density, position, rotation); + breakableBody.MainBody.Position = position; + world.AddBreakableBody(breakableBody); + return breakableBody; + } + + public static BreakableBody CreateBreakableBody(World world, IEnumerable shapes, Vector2 position = new Vector2(), float rotation = 0) + { + BreakableBody breakableBody = new BreakableBody(world, shapes, position, rotation); + breakableBody.MainBody.Position = position; + world.AddBreakableBody(breakableBody); + return breakableBody; + } + + + } +} \ No newline at end of file diff --git a/src/Box2DNet/Factories/FixtureFactory.cs b/src/Box2DNet/Factories/FixtureFactory.cs new file mode 100644 index 0000000..fcf1356 --- /dev/null +++ b/src/Box2DNet/Factories/FixtureFactory.cs @@ -0,0 +1,125 @@ +using System; +using System.Collections.Generic; +using Box2DNet.Collision.Shapes; +using Box2DNet.Common; +using Box2DNet.Common.Decomposition; +using Box2DNet.Dynamics; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Factories +{ + /// + /// An easy to use factory for creating bodies + /// + public static class FixtureFactory + { + public static Fixture AttachEdge(Vector2 start, Vector2 end, Body body, object userData = null) + { + EdgeShape edgeShape = new EdgeShape(start, end); + return body.CreateFixture(edgeShape, userData); + } + + public static Fixture AttachChainShape(Vertices vertices, Body body, object userData = null) + { + ChainShape shape = new ChainShape(vertices); + return body.CreateFixture(shape, userData); + } + + public static Fixture AttachLoopShape(Vertices vertices, Body body, object userData = null) + { + ChainShape shape = new ChainShape(vertices, true); + return body.CreateFixture(shape, userData); + } + + public static Fixture AttachRectangle(float width, float height, float density, Vector2 offset, Body body, object userData = null) + { + Vertices rectangleVertices = PolygonTools.CreateRectangle(width / 2, height / 2); + rectangleVertices.Translate(ref offset); + PolygonShape rectangleShape = new PolygonShape(rectangleVertices, density); + return body.CreateFixture(rectangleShape, userData); + } + + public static Fixture AttachCircle(float radius, float density, Body body, object userData = null) + { + if (radius <= 0) + throw new ArgumentOutOfRangeException("radius", "Radius must be more than 0 meters"); + + CircleShape circleShape = new CircleShape(radius, density); + return body.CreateFixture(circleShape, userData); + } + + public static Fixture AttachCircle(float radius, float density, Body body, Vector2 offset, object userData = null) + { + if (radius <= 0) + throw new ArgumentOutOfRangeException("radius", "Radius must be more than 0 meters"); + + CircleShape circleShape = new CircleShape(radius, density); + circleShape.Position = offset; + return body.CreateFixture(circleShape, userData); + } + + public static Fixture AttachPolygon(Vertices vertices, float density, Body body, object userData = null) + { + if (vertices.Count <= 1) + throw new ArgumentOutOfRangeException("vertices", "Too few points to be a polygon"); + + PolygonShape polygon = new PolygonShape(vertices, density); + return body.CreateFixture(polygon, userData); + } + + public static Fixture AttachEllipse(float xRadius, float yRadius, int edges, float density, Body body, object userData = null) + { + if (xRadius <= 0) + throw new ArgumentOutOfRangeException("xRadius", "X-radius must be more than 0"); + + if (yRadius <= 0) + throw new ArgumentOutOfRangeException("yRadius", "Y-radius must be more than 0"); + + Vertices ellipseVertices = PolygonTools.CreateEllipse(xRadius, yRadius, edges); + PolygonShape polygonShape = new PolygonShape(ellipseVertices, density); + return body.CreateFixture(polygonShape, userData); + } + + public static List AttachCompoundPolygon(List list, float density, Body body, object userData = null) + { + List res = new List(list.Count); + + //Then we create several fixtures using the body + foreach (Vertices vertices in list) + { + if (vertices.Count == 2) + { + EdgeShape shape = new EdgeShape(vertices[0], vertices[1]); + res.Add(body.CreateFixture(shape, userData)); + } + else + { + PolygonShape shape = new PolygonShape(vertices, density); + res.Add(body.CreateFixture(shape, userData)); + } + } + + return res; + } + + public static Fixture AttachLineArc(float radians, int sides, float radius, bool closed, Body body) + { + Vertices arc = PolygonTools.CreateArc(radians, sides, radius); + arc.Rotate((MathHelper.Pi - radians) / 2); + return closed ? AttachLoopShape(arc, body) : AttachChainShape(arc, body); + } + + public static List AttachSolidArc(float density, float radians, int sides, float radius, Body body) + { + Vertices arc = PolygonTools.CreateArc(radians, sides, radius); + arc.Rotate((MathHelper.Pi - radians) / 2); + + //Close the arc + arc.Add(arc[0]); + + List triangles = Triangulate.ConvexPartition(arc, TriangulationAlgorithm.Earclip); + + return AttachCompoundPolygon(triangles, density, body); + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Factories/JointFactory.cs b/src/Box2DNet/Factories/JointFactory.cs new file mode 100644 index 0000000..e1a2ada --- /dev/null +++ b/src/Box2DNet/Factories/JointFactory.cs @@ -0,0 +1,168 @@ +using Box2DNet.Dynamics; +using Box2DNet.Dynamics.Joints; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Factories +{ + /// + /// An easy to use factory for using joints. + /// + public static class JointFactory + { + #region Motor Joint + + public static MotorJoint CreateMotorJoint(World world, Body bodyA, Body bodyB, bool useWorldCoordinates = false) + { + MotorJoint joint = new MotorJoint(bodyA, bodyB, useWorldCoordinates); + world.AddJoint(joint); + return joint; + } + + #endregion + + #region Revolute Joint + + public static RevoluteJoint CreateRevoluteJoint(World world, Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false) + { + RevoluteJoint joint = new RevoluteJoint(bodyA, bodyB, anchorA, anchorB, useWorldCoordinates); + world.AddJoint(joint); + return joint; + } + + public static RevoluteJoint CreateRevoluteJoint(World world, Body bodyA, Body bodyB, Vector2 anchor) + { + Vector2 localanchorA = bodyA.GetLocalPoint(bodyB.GetWorldPoint(anchor)); + RevoluteJoint joint = new RevoluteJoint(bodyA, bodyB, localanchorA, anchor); + world.AddJoint(joint); + return joint; + } + + + #endregion + + #region Rope Joint + + public static RopeJoint CreateRopeJoint(World world, Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false) + { + RopeJoint ropeJoint = new RopeJoint(bodyA, bodyB, anchorA, anchorB, useWorldCoordinates); + world.AddJoint(ropeJoint); + return ropeJoint; + } + + #endregion + + #region Weld Joint + + public static WeldJoint CreateWeldJoint(World world, Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false) + { + WeldJoint weldJoint = new WeldJoint(bodyA, bodyB, anchorA, anchorB, useWorldCoordinates); + world.AddJoint(weldJoint); + return weldJoint; + } + + #endregion + + #region Prismatic Joint + + public static PrismaticJoint CreatePrismaticJoint(World world, Body bodyA, Body bodyB, Vector2 anchor, Vector2 axis, bool useWorldCoordinates = false) + { + PrismaticJoint joint = new PrismaticJoint(bodyA, bodyB, anchor, axis, useWorldCoordinates); + world.AddJoint(joint); + return joint; + } + + #endregion + + #region Wheel Joint + + public static WheelJoint CreateWheelJoint(World world, Body bodyA, Body bodyB, Vector2 anchor, Vector2 axis, bool useWorldCoordinates = false) + { + WheelJoint joint = new WheelJoint(bodyA, bodyB, anchor, axis, useWorldCoordinates); + world.AddJoint(joint); + return joint; + } + + public static WheelJoint CreateWheelJoint(World world, Body bodyA, Body bodyB, Vector2 axis) + { + return CreateWheelJoint(world, bodyA, bodyB, Vector2.Zero, axis); + } + + #endregion + + #region Angle Joint + + public static AngleJoint CreateAngleJoint(World world, Body bodyA, Body bodyB) + { + AngleJoint angleJoint = new AngleJoint(bodyA, bodyB); + world.AddJoint(angleJoint); + return angleJoint; + } + + #endregion + + #region Distance Joint + + public static DistanceJoint CreateDistanceJoint(World world, Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false) + { + DistanceJoint distanceJoint = new DistanceJoint(bodyA, bodyB, anchorA, anchorB, useWorldCoordinates); + world.AddJoint(distanceJoint); + return distanceJoint; + } + + public static DistanceJoint CreateDistanceJoint(World world, Body bodyA, Body bodyB) + { + return CreateDistanceJoint(world, bodyA, bodyB, Vector2.Zero, Vector2.Zero); + } + + #endregion + + #region Friction Joint + + public static FrictionJoint CreateFrictionJoint(World world, Body bodyA, Body bodyB, Vector2 anchor, bool useWorldCoordinates = false) + { + FrictionJoint frictionJoint = new FrictionJoint(bodyA, bodyB, anchor, useWorldCoordinates); + world.AddJoint(frictionJoint); + return frictionJoint; + } + + public static FrictionJoint CreateFrictionJoint(World world, Body bodyA, Body bodyB) + { + return CreateFrictionJoint(world, bodyA, bodyB, Vector2.Zero); + } + + #endregion + + #region Gear Joint + + public static GearJoint CreateGearJoint(World world, Body bodyA, Body bodyB, Joint jointA, Joint jointB, float ratio) + { + GearJoint gearJoint = new GearJoint(bodyA, bodyB, jointA, jointB, ratio); + world.AddJoint(gearJoint); + return gearJoint; + } + + #endregion + + #region Pulley Joint + + public static PulleyJoint CreatePulleyJoint(World world, Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, Vector2 worldAnchorA, Vector2 worldAnchorB, float ratio, bool useWorldCoordinates = false) + { + PulleyJoint pulleyJoint = new PulleyJoint(bodyA, bodyB, anchorA, anchorB, worldAnchorA, worldAnchorB, ratio, useWorldCoordinates); + world.AddJoint(pulleyJoint); + return pulleyJoint; + } + + #endregion + + #region MouseJoint + + public static FixedMouseJoint CreateFixedMouseJoint(World world, Body body, Vector2 worldAnchor) + { + FixedMouseJoint joint = new FixedMouseJoint(body, worldAnchor); + world.AddJoint(joint); + return joint; + } + + #endregion + } +} \ No newline at end of file diff --git a/src/Box2DNet/Factories/LinkFactory.cs b/src/Box2DNet/Factories/LinkFactory.cs new file mode 100644 index 0000000..69ec022 --- /dev/null +++ b/src/Box2DNet/Factories/LinkFactory.cs @@ -0,0 +1,64 @@ +using System.Collections.Generic; +using System.Diagnostics; +using Box2DNet.Collision.Shapes; +using Box2DNet.Common; +using Box2DNet.Dynamics; +using Microsoft.Xna.Framework; + +namespace Box2DNet.Factories +{ + public static class LinkFactory + { + /// + /// Creates a chain. + /// + /// The world. + /// The start. + /// The end. + /// The width. + /// The height. + /// The number of links. + /// The link density. + /// Creates a rope joint between start and end. This enforces the length of the rope. Said in another way: it makes the rope less bouncy. + /// + public static Path CreateChain(World world, Vector2 start, Vector2 end, float linkWidth, float linkHeight, int numberOfLinks, float linkDensity, bool attachRopeJoint) + { + Debug.Assert(numberOfLinks >= 2); + + //Chain start / end + Path path = new Path(); + path.Add(start); + path.Add(end); + + //A single chainlink + PolygonShape shape = new PolygonShape(PolygonTools.CreateRectangle(linkWidth, linkHeight), linkDensity); + + //Use PathManager to create all the chainlinks based on the chainlink created before. + List chainLinks = PathManager.EvenlyDistributeShapesAlongPath(world, path, shape, BodyType.Dynamic, numberOfLinks); + + //TODO + //if (fixStart) + //{ + // //Fix the first chainlink to the world + // JointFactory.CreateFixedRevoluteJoint(world, chainLinks[0], new Vector2(0, -(linkHeight / 2)), + // chainLinks[0].Position); + //} + + //if (fixEnd) + //{ + // //Fix the last chainlink to the world + // JointFactory.CreateFixedRevoluteJoint(world, chainLinks[chainLinks.Count - 1], + // new Vector2(0, (linkHeight / 2)), + // chainLinks[chainLinks.Count - 1].Position); + //} + + //Attach all the chainlinks together with a revolute joint + PathManager.AttachBodiesWithRevoluteJoint(world, chainLinks, new Vector2(0, -linkHeight), new Vector2(0, linkHeight), false, false); + + if (attachRopeJoint) + JointFactory.CreateRopeJoint(world, chainLinks[0], chainLinks[chainLinks.Count - 1], Vector2.Zero, Vector2.Zero); + + return (path); + } + } +} \ No newline at end of file diff --git a/src/Box2DNet/Settings.cs b/src/Box2DNet/Settings.cs new file mode 100644 index 0000000..353e7a9 --- /dev/null +++ b/src/Box2DNet/Settings.cs @@ -0,0 +1,285 @@ +/* +* Farseer Physics Engine: +* Copyright (c) 2012 Ian Qvist +* +* Original source Box2D: +* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +using System; +using Box2DNet.Dynamics; + +namespace Box2DNet +{ + public static class Settings + { + public const float MaxFloat = 3.402823466e+38f; + public const float Epsilon = 1.192092896e-07f; + public const float Pi = 3.14159265359f; + + // Common + + /// + /// If true, all collision callbacks have to return the same value, and agree + /// if there was a collision or not. Swtich this to false to revert to the + /// collision agreement used in FPE 3.3.x + /// + public const bool AllCollisionCallbacksAgree = true; + + /// + /// 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 const bool EnableDiagnostics = true; + + /// + /// Set this to true to skip sanity checks in the engine. This will speed up the + /// tools by removing the overhead of the checks, but you will need to handle checks + /// yourself where it is needed. + /// + public const bool SkipSanityChecks = false; + + /// + /// 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; + + /// + /// If true, it will run a GiftWrap convex hull on all polygon inputs. + /// This makes for a more stable engine when given random input, + /// but if speed of the creation of polygons are more important, + /// you might want to set this to false. + /// + public static bool UseConvexHullPolygons = true; + + /// + /// The number of velocity iterations in the TOI solver + /// + public static int TOIVelocityIterations = VelocityIterations; + + /// + /// 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 const 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 and DefaultFixtureCollisionCategories + /// to Category.All. + /// + public static bool UseFPECollisionCategories; + + /// + /// This is used by the Fixture constructor as the default value + /// for Fixture.CollisionCategories member. Note that you may need to change this depending + /// on the setting of UseFPECollisionCategories, above. + /// + public static Category DefaultFixtureCollisionCategories = Category.Cat1; + + /// + /// This is used by the Fixture constructor as the default value + /// for Fixture.CollidesWith member. + /// + public static Category DefaultFixtureCollidesWith = Category.All; + + + /// + /// This is used by the Fixture constructor as the default value + /// for Fixture.IgnoreCCDWith member. + /// + public static Category DefaultFixtureIgnoreCCDWith = Category.None; + + /// + /// The maximum number of contact points between two convex shapes. + /// DO NOT CHANGE THIS VALUE! + /// + 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 Baumgarte = 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); + + /// + /// Defines the maximum number of iterations made by the GJK algorithm. + /// + public const int MaxGJKIterations = 20; + + /// + /// This is only for debugging the solver + /// + public const bool EnableSubStepping = false; + + /// + /// By default, forces are cleared automatically after each call to Step. + /// The default behavior is modified with this setting. + /// The purpose of this setting is to support sub-stepping. Sub-stepping is often used to maintain + /// a fixed sized time step under a variable frame-rate. + /// When you perform sub-stepping you should disable auto clearing of forces and instead call + /// ClearForces after all sub-steps are complete in one pass of your game loop. + /// + public const bool AutoClearForces = true; + + /// + /// 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