Unstable 1.8.4.0

Libraries/Farseer Physics Engine 3.5/Collision/DynamicTree.cs

@@ -53,7 +53,7 @@ namespace FarseerPhysics.Collision
         internal int Height;
         internal int ParentOrNext;
 
-        public object Body;
+        public Body Body;
 
         internal T UserData;
 
@@ -319,7 +319,7 @@ namespace FarseerPhysics.Collision
             return _nodes[proxyId].AABB;
         }
 
-        public object GetBody(int proxyId)
+        public Body GetBody(int proxyId)
         {
             Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity);
             return _nodes[proxyId].Body;
@@ -344,7 +344,7 @@ namespace FarseerPhysics.Collision
         /// </summary>
         /// <param name="callback">The callback.</param>
         /// <param name="aabb">The aabb.</param>
-        public void Query(Func<int, bool> callback, ref AABB aabb, ref object body)
+        public void Query(Func<int, bool> callback, ref AABB aabb, ref Body body)
         {
             _queryStack.Clear();
             _queryStack.Push(_root);
@@ -357,22 +357,38 @@ namespace FarseerPhysics.Collision
                     continue;
                 }
 
-                //TreeNode<T>* node = &_nodes[nodeId];
-                if (!ReferenceEquals(_nodes[nodeId].Body, body) && AABB.TestOverlap(ref _nodes[nodeId].AABB, ref aabb))
+                TreeNode<T> node = _nodes[nodeId];
+                if (node.Body != body && AABB.TestOverlap(ref node.AABB, ref aabb))
                 {
-                    if (_nodes[nodeId].IsLeaf())
+                    if (node.IsLeaf())
                     {
+                        if (node.Body.CollidesWithMatchesBetweenFixtures && 
+                            body.CollisionCategoriesMatchBetweenFixtures)
+                        {
+                            //equivalent to
+                            //collide = node.Body.CollidesWith.HasAnyFlag(body.CollisionCategories) && body.CollidesWith.HasAnyFlag(node.Body.CollisionCategories)
+                            //same check as in ContactManager.ShouldCollide
+                            //inlined here using binary operations because this is performance critical code
+                            bool collide = 
+                                (node.Body.CollidesWith & body.CollisionCategories) != 0 &&
+                                (body.CollidesWith & node.Body.CollisionCategories) != 0;
+                            if (!collide)
+                            {
+                                continue;
+                            }                         
+                        }
                         bool proceed = callback(nodeId);
                         if (proceed == false)
                         {
                             return;
                         }
+                        
                     }
                     else
                     {
-                        _queryStack.Push(_nodes[nodeId].Child1);
-                        _queryStack.Push(_nodes[nodeId].Child2);
-                    }
+                        _queryStack.Push(node.Child1);
+                        _queryStack.Push(node.Child2);
+                    }                    
                 }
             }
         }

Libraries/Farseer Physics Engine 3.5/Collision/DynamicTreeBroadPhase.cs

@@ -263,7 +263,7 @@ namespace FarseerPhysics.Collision
                 // we don't fail to create a pair that may touch later.
                 AABB fatAABB = _tree.GetFatAABB(_queryProxyId);
 
-                object body = _tree.GetBody(_queryProxyId);
+                Body body = _tree.GetBody(_queryProxyId);
 
                 // Query tree, create pairs and add them pair buffer.
                 _tree.Query(_queryCallback, ref fatAABB, ref body);

Libraries/Farseer Physics Engine 3.5/Collision/Shapes/CircleShape.cs

@@ -115,7 +115,7 @@ namespace FarseerPhysics.Collision.Shapes
             }
 
             // Find the point of intersection of the line with the circle.
-            float a = -(c + (float)Math.Sqrt(sigma));
+            float a = -(c + MathF.Sqrt(sigma));
 
             // Is the intersection point on the segment?
             if (0.0f <= a && a <= input.MaxFraction * rr)
@@ -177,8 +177,8 @@ namespace FarseerPhysics.Collision.Shapes
 
             //Magic
             float l2 = l * l;
-            float area = _2radius * (float)((Math.Asin(l / Radius) + MathHelper.Pi / 2) + l * Math.Sqrt(_2radius - l2));
-            float com = -2.0f / 3.0f * (float)Math.Pow(_2radius - l2, 1.5f) / area;
+            float area = _2radius * ((MathF.Asin(l / Radius) + MathHelper.Pi / 2) + l * MathF.Sqrt(_2radius - l2));
+            float com = -2.0f / 3.0f * MathF.Pow(_2radius - l2, 1.5f) / area;
 
             sc.X = p.X + normal.X * com;
             sc.Y = p.Y + normal.Y * com;