public void CalculateBounds()
 {
     if (connectedBody == null)
     {
         _bodyBounds = new Bounds();
         return;
     }
     AABB aabb_full = new AABB();
     bool combine = false;
     for (int i = 0; i < connectedBody.FixtureList.Count; i++)
     {
         for (int j = 0; j < connectedBody.FixtureList[i].Shape.ChildCount; j++)
         {
             AABB aabb;
             connectedBody.FixtureList[i].Shape.ComputeAABB(out aabb, ref connectedBody.Xf, j);
             if (!combine)
             {
                 combine = true;
                 aabb_full = aabb;
             }
             else
             {
                 aabb_full.Combine(ref aabb);
             }
         }
     }
     _bodyBounds = Bounds.FromAABB(ref aabb_full, to2dMode, GetSize());
 }

        // Constructor
        public Particle(FluidSystem fluidSystem, int index, Vector2 position)
        {
            //this.simulation = simulation;
            this.index = index;
            this.position = position;
            active = false;
            alive = false;

            aabb = new AABB();
            neighbors = new int[FluidSystem.MAX_NEIGHBORS];
            distances = new float[FluidSystem.MAX_NEIGHBORS];
            relativePosition = new Vector2[FluidSystem.MAX_NEIGHBORS];
            oneminusq = new float[FluidSystem.MAX_NEIGHBORS];
            entitiesToInfluence = new int[MAX_INFLUENCES];
            fixturesToTest = new Fixture[MAX_FIXTURES_TO_TEST];

            collisionVertices = new Vector2[Settings.MaxPolygonVertices];
            collisionNormals = new Vector2[Settings.MaxPolygonVertices];

            int i = fluidSystem.getFluidGridX(position.X);
            int j = fluidSystem.getFluidGridY(position.Y);

            if (!fluidSystem.fluidGrid.ContainsKey(i))
                fluidSystem.fluidGrid[i] = new Dictionary<int, List<int>>();
            if (!fluidSystem.fluidGrid[i].ContainsKey(j))
                fluidSystem.fluidGrid[i][j] = new List<int>();

            fluidSystem.fluidGrid[i][j].Add(index);
            ci = i;
            cj = j;
        }

 /// <summary>
 /// Creates a new terrain.
 /// </summary>
 /// <param name="world">The World</param>
 /// <param name="area">The area of the terrain.</param>
 public Terrain(World world, AABB area)
 {
     World = world;
     Width = area.Width;
     Height = area.Height;
     Center = area.Center;
 }

        public virtual Body GetBodyAtMouse(bool includeStatic)
        {
            // Make a small box
            mousePVec.X = MouseXWorldPhys;
            mousePVec.Y = MouseYWorldPhys;
            FVector2 lowerBound = new FVector2(MouseXWorldPhys - 0.001f, MouseYWorldPhys - 0.001f);
            FVector2 upperBound = new FVector2(MouseXWorldPhys + 0.001f, MouseYWorldPhys + 0.001f);
            AABB aabb = new AABB(lowerBound, upperBound);
            Body body = null;

            // Query the world for overlapping shapes
            System.Func<Fixture, bool> GetBodyCallback = delegate (Fixture fixture0)
            {
                Shape shape = fixture0.Shape;
                if(fixture0.Body.BodyType != BodyType.Static || includeStatic)
                {
                    FarseerPhysics.Common.Transform transform0;
                    fixture0.Body.GetTransform(out transform0);
                    bool inside = shape.TestPoint(ref transform0, ref mousePVec);
                    if(inside)
                    {
                        body = fixture0.Body;
                        return false;
                    }
                }
                return true;
            };
            FSWorldComponent.PhysicsWorld.QueryAABB(GetBodyCallback, ref aabb);
            return body;
        }

        private BuoyancyTest()
        {
            World.Gravity = new Vector2(0, -9.82f);

            BodyFactory.CreateEdge(World, new Vector2(-40, 0), new Vector2(40, 0));

            float offset = 5;
            for (int i = 0; i < 3; i++)
            {
                Body rectangle = BodyFactory.CreateRectangle(World, 2, 2, 1, new Vector2(-30 + offset, 20));
                rectangle.Rotation = Rand.RandomFloat(0, 3.14f);
                rectangle.BodyType = BodyType.Dynamic;
                offset += 7;
            }

            for (int i = 0; i < 3; i++)
            {
                Body rectangle = BodyFactory.CreateCircle(World, 1, 1, new Vector2(-30 + offset, 20));
                rectangle.Rotation = Rand.RandomFloat(0, 3.14f);
                rectangle.BodyType = BodyType.Dynamic;
                offset += 7;
            }

            AABB container = new AABB(new Vector2(0, 10), 60, 10);
            BuoyancyController buoyancy = new BuoyancyController(container, 2, 2, 1, World.Gravity);
            World.AddController(buoyancy);
        }

 /// <summary>
 /// Creates a new quad tree broadphase with the specified span.
 /// </summary>
 /// <param name="span">the maximum span of the tree (world size)</param>
 public QuadTreeBroadPhase(AABB span)
 {
     QuadTree = new QuadTree<FixtureProxy>(span, 5, 10);
     _idRegister = new Dictionary<int, Element<FixtureProxy>>();
     _moveBuffer = new List<Element<FixtureProxy>>();
     _pairBuffer = new List<Pair>();
 }

        public override void Update()
        {
            base.Update();

            // udpate selected body
            if (SelectedBody != null)
            {
                if (SelectedBody.IsDisposed || !SelectedBody.Enabled)
                    SelectedBody = null;
            }

            // update highlighted body
            if(!UI.Mouse.ScreenPosition.IsStriclyInside(Render.Viewport.Area))
                return;

            var mousePos = UI.Mouse.WorldPosition;
            var aabb = new AABB(mousePos, .001f, .001f);

            HighlightedBody = null;
            Physic.World.QueryAABB(OnMouseOverFixture, ref aabb);

            if (HighlightedBody != null)
            {
                Focus();
            }
            else
            {
                Unfocus();
            }
        }

 public void GetFatAABB(int proxyID, out AABB aabb)
 {
     if (_idRegister.ContainsKey(proxyID))
         aabb = _idRegister[proxyID].Span;
     else
         throw new KeyNotFoundException("proxyID not found in register");
 }

        public void attemptRopeGrab(string levelUid, int characterId, CharacterMovementComponent characterMovementComponent, PhysicsComponent physicsComponent, RopeGrabComponent existingRopeGrabComponent)
        {
            float margin = 0.5f;
            AABB region = new AABB();
            RopeNode ropeNode = null;
            int nodeCount = 0;

            region.LowerBound = physicsComponent.body.Position - new Vector2(margin, margin);
            region.UpperBound = physicsComponent.body.Position + new Vector2(margin, margin);

            if (physicsComponent == null)
                return;

            // Query the world for a body, and check to see if it's a rope
            physicsComponent.body.World.QueryAABB((fixture) =>
                {
                    int ropeEntityId = (int)fixture.Body.UserData;
                    RopeComponent ropeComponent = (RopeComponent)_entityManager.getComponent(levelUid, ropeEntityId, ComponentType.Rope);
                    RopeGrabComponent ropeGrabComponent = null;

                    if (ropeComponent != null && !ropeComponent.doubleAnchor)
                    {
                        RopeNode current = ropeComponent.ropeNodeHead;

                        characterMovementComponent.allowRopeGrab = false;

                        while (current != null)
                        {
                            if (current.body == fixture.Body)
                            {
                                ropeNode = current;
                                break;
                            }
                            nodeCount++;
                            current = current.next;
                        }

                        if (existingRopeGrabComponent != null)
                        {
                            RopeComponent existingRopeComponent = (RopeComponent)_entityManager.getComponent(levelUid, existingRopeGrabComponent.ropeEntityId, ComponentType.Rope);

                            if (existingRopeComponent.destroyAfterRelease)
                                existingRopeComponent.timeToLive = 100;

                            _ropeSystem.releaseRope(existingRopeGrabComponent, physicsComponent.body);
                            _entityManager.removeComponent(levelUid, characterId, existingRopeGrabComponent);
                        }

                        ropeGrabComponent = new RopeGrabComponent(ropeEntityId, ropeNode, (float)nodeCount, ropeComponent.reverseClimbDirection);
                        _ropeSystem.grabRope(ropeGrabComponent, physicsComponent.body);
                        _entityManager.addComponent(levelUid, characterId, ropeGrabComponent);

                        return false;
                    }
                    return true;
                },
                ref region);
        }

        public Midgard(AABB boundBox, Vector2 gravity, int tickRate)
        {
            _tickRate = tickRate;
            _invTickRate = 1f / (float)_tickRate;

            FarseerPhysics.Settings.ContinuousPhysics = false;

            _world = new World(gravity);
        }

 /// <summary>
 /// 
 /// </summary>
 /// <param name="game"></param>
 /// <param name="world"></param>
 /// <param name="position">The top left corner</param>
 /// <param name="width">The width of the container</param>
 /// <param name="height">The height of a container</param>
 public Water(Game game, SpriteBatch spriteBatch, World world, Vector2 position, float width, float height)
 {
     sprite = new LoopSpriteAnimator(game, spriteBatch, game.Content.Load<Texture2D>("Images/Sprites/gb"), 200, 6, 1, 6);
     position = new Vector2(position.X + width/2,position.Y + height/2);
     texture = new TexturedGameEntity(game, position,0,"Images/water",0.3f);
     var container = new AABB(ConvertUnits.ToSimUnits(position),ConvertUnits.ToSimUnits(width),ConvertUnits.ToSimUnits(height));
     var buoyancy = new BuoyancyController(container, 1.1f, 2, 1, world.Gravity);
     world.AddController(buoyancy);
 }

 /// <summary>
 /// Initializes a new instance of the <see cref="BuoyancyController"/> class.
 /// </summary>
 /// <param name="container">Only bodies inside this AABB will be influenced by the controller</param>
 /// <param name="density">Density of the fluid</param>
 /// <param name="linearDragCoefficient">Linear drag coefficient of the fluid</param>
 /// <param name="rotationalDragCoefficient">Rotational drag coefficient of the fluid</param>
 /// <param name="gravity">The direction gravity acts. Buoyancy force will act in opposite direction of gravity.</param>
 public BuoyancyController(AABB container, float density, float linearDragCoefficient, float rotationalDragCoefficient, Vector2 gravity)
     : base(ControllerType.BuoyancyController)
 {
     Container = container;
     _normal = new Vector2(0, 1);
     Density = density;
     LinearDragCoefficient = linearDragCoefficient;
     AngularDragCoefficient = rotationalDragCoefficient;
     _gravity = gravity;
 }

        public void Emit()
        {
            var p = UI.Mouse.WorldPosition;
            var aabb = new AABB(p, 0.01f, 0.01f);
            var bodies = Physic.World.QueryAABB(ref aabb);
            if(bodies.Any())
                return;

            _emitter.StartEmitting();
        }

        public void DrawAABB(ref AABB aabb, Color color)
        {
            Vector2[] verts = new Vector2[4];
            verts[0] = new Vector2(aabb.LowerBound.X, aabb.LowerBound.Y);
            verts[1] = new Vector2(aabb.UpperBound.X, aabb.LowerBound.Y);
            verts[2] = new Vector2(aabb.UpperBound.X, aabb.UpperBound.Y);
            verts[3] = new Vector2(aabb.LowerBound.X, aabb.UpperBound.Y);

            DrawPolygon(verts, 4, color);
        }

        public void TestThinningAndFattening()
        {
            var aabb = new AABB(new Vector2(-10, 5), new Vector2(0, 35));
            Assert.AreEqual(aabb, aabb.Thinned.Fattened);
            Assert.AreEqual(aabb, aabb.Fattened.Thinned);

            AssertFattened(new Vector2(0, 0), new Vector2(0, 0), new Vector2(0, 0), new Vector2(0, 0));
            AssertThinned(new Vector2(0, 0), new Vector2(0, 0), new Vector2(0, 0), new Vector2(0, 0));
            AssertFattened(new Vector2(0, 0), new Vector2(40, 40), new Vector2(10, 10), new Vector2(30, 30));
            AssertThinned(new Vector2(-40, -40), new Vector2(-20, -20), new Vector2(-50, -50), new Vector2(-10, -10));
        }

        public IEnumerable<Entity> EntitiesInRegion(FloatRect rect)
        {
            var min = new Vector2(rect.Left, rect.Top) / Program.PixelsPerMeter;
            var aabb = new AABB(min, min + (new Vector2(rect.Width, rect.Height) / Program.PixelsPerMeter));
            var result = new List<Entity>(256);

            World.QueryAABB(f =>
            {
                result.Add((Entity)f.Body.UserData);
                return true;
            }, ref aabb);

            return result.Distinct().OrderBy(e => e.Depth + e.DepthBias);
        }

        private DestructibleTerrainTest()
        {
            World = new World(new Vector2(0, -10));

            _terrainArea = new AABB(new Vector2(0, 0), 100, 100);
            _terrain = new Terrain(World, _terrainArea)
                           {
                               PointsPerUnit = 10,
                               CellSize = 50,
                               SubCellSize = 5,
                               Decomposer = TriangulationAlgorithm.Earclip,
                               Iterations = 2,
                           };

            _terrain.Initialize();
        }

        public static void GetEnclosingAABB(this Body body, out AABB aabb_enclosing)
        {
            AABB aabb_fixture;
            Microsoft.Xna.Framework.Vector2 lo = new Microsoft.Xna.Framework.Vector2(float.PositiveInfinity);
            Microsoft.Xna.Framework.Vector2 hi = new Microsoft.Xna.Framework.Vector2(float.NegativeInfinity);
            for (int i = 0; i < body.FixtureList.Count; i++) {
                if(body.FixtureList[i].CollisionCategories == Category.None)
                    continue;
                body.FixtureList[i].GetAABB (out aabb_fixture, 0); // Should just be first proxy (0)
                lo.X = Math.Min (lo.X, aabb_fixture.LowerBound.X);
                lo.Y = Math.Min (lo.Y, aabb_fixture.LowerBound.Y);
                hi.X = Math.Max (hi.X, aabb_fixture.UpperBound.X);
                hi.Y = Math.Max (hi.Y, aabb_fixture.UpperBound.Y);
            }

            aabb_enclosing = new AABB(lo, hi);
        }

 //----------------------------------------------------------------
 // Constructor
 //----------------------------------------------------------------
 //
 // TODO: add Vector2 target_position as an argument??
 protected GameWorld( float width, float height, Vector2 gravity )
 {
     // Create the world's axis-aligned bounding box
     m_aabb = new AABB();
     m_aabb.LowerBound = new Vector2( -GameScreen.GAME_WIDTH,
         -GameScreen.GAME_HEIGHT ) + new Vector2( -MARGIN - width,
             -MARGIN - height );
     m_aabb.UpperBound = new Vector2( GameScreen.GAME_WIDTH,
         GameScreen.GAME_HEIGHT ) + new Vector2( width + MARGIN,
             height + MARGIN );
     m_world = new World( gravity, m_aabb );
     GameWorld.the_world = m_world;
     m_succeeded = m_failed = false;
     m_enemy_count = 0;
     m_enemy_slain = 0;
     m_collision_manager = new CollisionManager();
     m_background = GameScreen.m_background;
 }

        public override void Update(GameSettings settings, GameTime gameTime)
        {
            Vector2 min = -new Vector2(10);
            Vector2 max = new Vector2(10);

            AABB affected = new AABB(ref min, ref max);
            Fixture fix = null;
            World.QueryAABB(fixture =>
                                {
                                    fix = fixture;
                                    return true;
                                }, ref affected);

            //DebugView.BeginCustomDraw(ref GameInstance.Projection, ref GameInstance.View);
            //if (fix != null)
            //    DebugView.DrawPoint(fix.Body.Position, 1, Color.Red);

            //DebugView.DrawAABB(ref affected, Color.AliceBlue);
            //DebugView.EndCustomDraw();

            base.Update(settings, gameTime);
        }