Ejemplo n.º 1
0
 /// Does this aabb contain the provided AABB.
 public bool Contains(b2AABB aabb)
 {
     bool result = true;
     result = result && m_lowerBound.x <= aabb.m_lowerBound.x;
     result = result && m_lowerBound.y <= aabb.m_lowerBound.y;
     result = result && aabb.m_upperBound.x <= m_upperBound.x;
     result = result && aabb.m_upperBound.y <= m_upperBound.y;
     return result;
 }
Ejemplo n.º 2
0
        public void DrawAABB(b2AABB aAabb, b2Color aColor)
        {
            mVertices[0] = new CCPoint(aAabb.LowerBound.x * mRatio, aAabb.LowerBound.y * mRatio);
            mVertices[1] = new CCPoint(aAabb.UpperBound.x * mRatio, aAabb.LowerBound.y * mRatio);
            mVertices[2] = new CCPoint(aAabb.UpperBound.x * mRatio, aAabb.UpperBound.y * mRatio);
            mVertices[3] = new CCPoint(aAabb.LowerBound.x * mRatio, aAabb.UpperBound.y * mRatio);

            CCDrawingPrimitives.Begin();
            CCDrawingPrimitives.DrawPoly(mVertices, 8, true, new CCColor4B(aColor.r, aColor.g, aColor.b, 1));
            CCDrawingPrimitives.End();
        }
Ejemplo n.º 3
0
        public virtual bool MouseDown(b2Vec2 p)
        {
            m_mouseWorld = p;

            if (m_mouseJoint != null)
            {
                return false;
            }

            // Make a small box.
            b2AABB aabb = new b2AABB();
            b2Vec2 d = new b2Vec2();
            d.Set(0.001f, 0.001f);
            aabb.LowerBound = p - d;
            aabb.UpperBound = p + d;

            // Query the world for overlapping shapes.
            QueryCallback callback = new QueryCallback(p);
            m_world.QueryAABB(callback, aabb);

            if (callback.m_fixture != null)
            {

                b2Body body = callback.m_fixture.Body;
                b2MouseJointDef md = new b2MouseJointDef();
                md.BodyA = m_groundBody;
                md.BodyB = body;
                md.target = p;
                md.maxForce = 1000.0f * body.Mass;
                m_mouseJoint = (b2MouseJoint)m_world.CreateJoint(md);
                body.SetAwake(true);
                return true;

            }
            else
            {

                //Como no ha encontrado un objeto empezamos el arrastre
                IsArrastring = true;
                originPosition = new CCPoint(p.x, p.y);
                return true;

            }
        }
Ejemplo n.º 4
0
 public bool Equals(b2AABB o)
 {
     return (LowerBound == o.LowerBound && UpperBound == o.UpperBound);
 }
Ejemplo n.º 5
0
        // 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.
        public int CreateProxy(b2AABB aabb, object userData)
        {
            int proxyId = AllocateNode();

            // Fatten the aabb.
            b2Vec2 r = new b2Vec2(b2Settings.b2_aabbExtension, b2Settings.b2_aabbExtension);
            m_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r;
            m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r;
            m_nodes[proxyId].userData = userData;
            m_nodes[proxyId].height = 0;

            InsertLeaf(proxyId);

            return proxyId;
        }
Ejemplo n.º 6
0
        public void LaunchBomb(b2Vec2 position, b2Vec2 velocity)
        {
            if (m_bomb != null)
            {
                m_world.DestroyBody(m_bomb);
                m_bomb = null;
            }

            b2BodyDef bd = new b2BodyDef();
            bd.type = b2BodyType.b2_dynamicBody;
            bd.position = position;
            bd.bullet = true;
            m_bomb = m_world.CreateBody(bd);
            m_bomb.LinearVelocity = velocity;

            b2CircleShape circle = new b2CircleShape();
            circle.Radius = 0.3f;

            b2FixtureDef fd = new b2FixtureDef();
            fd.shape = circle;
            fd.density = 20.0f;
            fd.restitution = 0.0f;

            b2Vec2 minV = position - new b2Vec2(0.3f, 0.3f);
            b2Vec2 maxV = position + new b2Vec2(0.3f, 0.3f);

            b2AABB aabb = new b2AABB();
            aabb.LowerBound = minV;
            aabb.UpperBound = maxV;

            m_bomb.CreateFixture(fd);
        }
Ejemplo n.º 7
0
        public static void Combine(ref b2AABB aabb1, ref b2AABB aabb2, out b2AABB output)
        {
            output.LowerBound.x = aabb1.LowerBound.x < aabb2.LowerBound.x ? aabb1.LowerBound.x : aabb2.LowerBound.x;
            output.LowerBound.y = aabb1.LowerBound.y < aabb2.LowerBound.y ? aabb1.LowerBound.y : aabb2.LowerBound.y;

            output.UpperBound.x = aabb1.UpperBound.x > aabb2.UpperBound.x ? aabb1.UpperBound.x : aabb2.UpperBound.x;
            output.UpperBound.y = aabb1.UpperBound.y > aabb2.UpperBound.y ? aabb1.UpperBound.y : aabb2.UpperBound.y;
        }
Ejemplo n.º 8
0
        public static bool b2TestOverlap(ref b2AABB a, ref b2AABB b)
        {
            b2Vec2 d1, d2;
            // No operator overloading here - do direct computation to reduce time complexity
            
            d1.m_x = b.LowerBoundX - a.UpperBoundX;
            d1.m_y = b.LowerBoundY - a.UpperBoundY;

            d2.m_x = a.LowerBoundX - b.UpperBoundX;
            d2.m_y = a.LowerBoundY - b.UpperBoundY;
            
            // d1 = b.LowerBound - a.UpperBound;
            // d2 = a.LowerBound - b.UpperBound;

            if (d1.m_x > 0.0f || d1.m_y > 0.0f)
                return false;

            if (d2.m_x > 0.0f || d2.m_y > 0.0f)
                return false;

            return true;
        }
Ejemplo n.º 9
0
 public void GetFatAABB(int proxyId, out b2AABB output)
 {
     Debug.Assert(0 <= proxyId && proxyId < m_nodeCapacity);
     output = m_nodes[proxyId].aabb;
 }
Ejemplo n.º 10
0
        public void RebuildBottomUp()
        {
            int[] nodes = new int[m_nodeCount];
            int   count = 0;

            // Build array of leaves. Free the rest.
            for (int i = 0; i < m_nodeCapacity; ++i)
            {
                if (m_nodes[i].height < 0)
                {
                    // free node in pool
                    continue;
                }

                if (m_nodes[i].IsLeaf())
                {
                    m_nodes[i].parentOrNext = b2TreeNode.b2_nullNode;
                    nodes[count]            = i;
                    ++count;
                }
                else
                {
                    FreeNode(i);
                }
            }

            while (count > 1)
            {
                float minCost = b2Settings.b2_maxFloat;
                int   iMin = -1, jMin = -1;
                for (int i = 0; i < count; ++i)
                {
                    b2AABB aabbi = m_nodes[nodes[i]].aabb;

                    for (int j = i + 1; j < count; ++j)
                    {
                        b2AABB aabbj = m_nodes[nodes[j]].aabb;
                        b2AABB b     = b2AABB.Default;
                        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];
                b2TreeNode child1 = m_nodes[index1];
                b2TreeNode child2 = m_nodes[index2];

                int        parentIndex = AllocateNode();
                b2TreeNode parent      = m_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 = b2TreeNode.b2_nullNode;

                child1.parentOrNext = parentIndex;
                child2.parentOrNext = parentIndex;

                nodes[jMin] = nodes[count - 1];
                nodes[iMin] = parentIndex;
                --count;
            }

            m_root = nodes[0];

            Validate();
        }
Ejemplo n.º 11
0
        public void ValidateMetrics(int index)
        {
            if (index == b2TreeNode.b2_nullNode)
            {
                return;
            }

            b2TreeNode node = m_nodes[index];

            int child1 = node.child1;
            int child2 = node.child2;

            if (node.IsLeaf())
            {
                Debug.Assert(child1 == b2TreeNode.b2_nullNode);
                Debug.Assert(child2 == b2TreeNode.b2_nullNode);
                Debug.Assert(node.height == 0);
                return;
            }

            Debug.Assert(0 <= child1 && child1 < m_nodeCapacity);
            Debug.Assert(0 <= child2 && child2 < m_nodeCapacity);

            int height1 = m_nodes[child1].height;
            int height2 = m_nodes[child2].height;
            int height;
            height = 1 + Math.Max(height1, height2);
            Debug.Assert(node.height == height);

            b2AABB aabb = new b2AABB();
            aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);

            Debug.Assert(aabb.lowerBound == node.aabb.lowerBound);
            Debug.Assert(aabb.upperBound == node.aabb.upperBound);

            ValidateMetrics(child1);
            ValidateMetrics(child2);
        }
Ejemplo n.º 12
0
 /// Combine an AABB into this one.
 public void Combine(b2AABB aabb)
 {
     m_lowerBound = b2Math.b2Min(m_lowerBound, aabb.m_lowerBound);
     m_upperBound = b2Math.b2Max(m_upperBound, aabb.m_upperBound);
 }
Ejemplo n.º 13
0
        public void InsertLeaf(int leaf)
        {
            ++m_insertionCount;

            if (m_root == b2TreeNode.b2_nullNode)
            {
                m_root = leaf;
                m_nodes[m_root].parentOrNext = b2TreeNode.b2_nullNode;
                return;
            }

            // Find the best sibling for this node
            b2AABB leafAABB = m_nodes[leaf].aabb;
            int    index    = m_root;

            while (m_nodes[index].IsLeaf() == false)
            {
                int child1 = m_nodes[index].child1;
                int child2 = m_nodes[index].child2;

                float area = m_nodes[index].aabb.Perimeter;

                b2AABB combinedAABB = b2AABB.Default;
                combinedAABB.Combine(ref m_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 (m_nodes[child1].IsLeaf())
                {
                    b2AABB aabb = b2AABB.Default;
                    aabb.Combine(ref leafAABB, ref m_nodes[child1].aabb);
                    cost1 = aabb.Perimeter + inheritanceCost;
                }
                else
                {
                    b2AABB aabb = b2AABB.Default;
                    aabb.Combine(ref leafAABB, ref m_nodes[child1].aabb);
                    float oldArea = m_nodes[child1].aabb.Perimeter;
                    float newArea = aabb.Perimeter;
                    cost1 = (newArea - oldArea) + inheritanceCost;
                }

                // Cost of descending into child2
                float cost2;
                if (m_nodes[child2].IsLeaf())
                {
                    b2AABB aabb = b2AABB.Default;
                    aabb.Combine(ref leafAABB, ref m_nodes[child2].aabb);
                    cost2 = aabb.Perimeter + inheritanceCost;
                }
                else
                {
                    b2AABB aabb = b2AABB.Default;
                    aabb.Combine(ref leafAABB, ref m_nodes[child2].aabb);
                    float oldArea = m_nodes[child2].aabb.Perimeter;
                    float newArea = aabb.Perimeter;
                    cost2 = newArea - oldArea + inheritanceCost;
                }

                // Descend according to the minimum cost.
                if (cost < cost1 && cost < cost2)
                {
                    break;
                }

                // Descend
                if (cost1 < cost2)
                {
                    index = child1;
                }
                else
                {
                    index = child2;
                }
            }

            int sibling = index;


            // Create a new parent.
            int oldParent = m_nodes[sibling].parentOrNext;
            int newParent = AllocateNode();

            m_nodes[newParent].parentOrNext = oldParent;
            m_nodes[newParent].userData     = null;
            m_nodes[newParent].aabb.Combine(ref leafAABB, ref m_nodes[sibling].aabb);
            m_nodes[newParent].height = m_nodes[sibling].height + 1;

            if (oldParent != b2TreeNode.b2_nullNode)
            {
                // The sibling was not the root.
                if (m_nodes[oldParent].child1 == sibling)
                {
                    m_nodes[oldParent].child1 = newParent;
                }
                else
                {
                    m_nodes[oldParent].child2 = newParent;
                }

                m_nodes[newParent].child1     = sibling;
                m_nodes[newParent].child2     = leaf;
                m_nodes[sibling].parentOrNext = newParent;
                m_nodes[leaf].parentOrNext    = newParent;
            }
            else
            {
                // The sibling was the root.
                m_nodes[newParent].child1     = sibling;
                m_nodes[newParent].child2     = leaf;
                m_nodes[sibling].parentOrNext = newParent;
                m_nodes[leaf].parentOrNext    = newParent;
                m_root = newParent;
            }

            // Walk back up the tree fixing heights and AABBs
            index = m_nodes[leaf].parentOrNext;
            while (index != b2TreeNode.b2_nullNode)
            {
                index = Balance(index);

                int child1 = m_nodes[index].child1;
                int child2 = m_nodes[index].child2;

                Debug.Assert(child1 != b2TreeNode.b2_nullNode);
                Debug.Assert(child2 != b2TreeNode.b2_nullNode);

                m_nodes[index].height = 1 + Math.Max(m_nodes[child1].height, m_nodes[child2].height);
                m_nodes[index].aabb.Combine(ref m_nodes[child1].aabb, ref m_nodes[child2].aabb);

                index = m_nodes[index].parentOrNext;
            }

            //Validate();
        }
Ejemplo n.º 14
0
        public bool MoveProxy(int proxyId, b2AABB aabb, b2Vec2 displacement)
        {
            Debug.Assert(0 <= proxyId && proxyId < m_nodeCapacity);

            Debug.Assert(m_nodes[proxyId].IsLeaf());

            if (m_nodes[proxyId].aabb.Contains(ref aabb))
            {
                return false;
            }

            RemoveLeaf(proxyId);

            // Extend AABB.
            b2AABB b = aabb;
            b.Fatten();

            // Predict AABB displacement.
            b2Vec2 d = b2Settings.b2_aabbMultiplier * displacement;

            if (d.x < 0.0f)
            {
                b.LowerBoundX += d.x;
            }
            else
            {
                b.UpperBoundX += d.x;
            }

            if (d.y < 0.0f)
            {
                b.LowerBoundY += d.y;
            }
            else
            {
                b.UpperBoundY += d.y;
            }

            b.UpdateAttributes();
            m_nodes[proxyId].aabb = b;

            InsertLeaf(proxyId);
            return true;
        }
Ejemplo n.º 15
0
        // 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.
        public int CreateProxy(b2AABB aabb, object userData)
        {
            int proxyId = AllocateNode();

            // Fatten the aabb.
            m_nodes[proxyId].aabb = aabb;
            m_nodes[proxyId].aabb.Fatten();
            m_nodes[proxyId].userData = userData;
            m_nodes[proxyId].height = 0;

            InsertLeaf(proxyId);

            return proxyId;
        }
Ejemplo n.º 16
0
 /**
  * Extends or clips the segment so that it's ends lie on the boundary of the AABB
  */
 public void Extend(b2AABB aabb)
 {
     ExtendForward(aabb);
     ExtendBackward(aabb);
 }
Ejemplo n.º 17
0
        public bool MoveProxy(int proxyId, b2AABB aabb, b2Vec2 displacement)
        {
            Debug.Assert(0 <= proxyId && proxyId < m_nodeCapacity);

            Debug.Assert(m_nodes[proxyId].IsLeaf());

            if (m_nodes[proxyId].aabb.Contains(aabb))
            {
                return false;
            }

            RemoveLeaf(proxyId);

            // Extend AABB.
            b2AABB b = aabb;
            b2Vec2 r = new b2Vec2(b2Settings.b2_aabbExtension, b2Settings.b2_aabbExtension);
            b.lowerBound = b.lowerBound - r;
            b.upperBound = b.upperBound + r;

            // Predict AABB displacement.
            b2Vec2 d = b2Settings.b2_aabbMultiplier * displacement;

            if (d.x < 0.0f)
            {
                b.lowerBoundx += d.x;
            }
            else
            {
                b.upperBoundx += d.x;
            }

            if (d.y < 0.0f)
            {
                b.lowerBoundy += d.y;
            }
            else
            {
                b.upperBoundy += d.y;
            }

            m_nodes[proxyId].aabb = b;

            InsertLeaf(proxyId);
            return true;
        }
Ejemplo n.º 18
0
        public void RayCast(b2WorldRayCastWrapper callback, b2RayCastInput input)
        {
            b2Vec2 p1 = input.p1;
            b2Vec2 p2 = input.p2;
            b2Vec2 r  = p2 - p1;

            Debug.Assert(r.LengthSquared > 0.0f);
            r.Normalize();

            // v is perpendicular to the segment.
            b2Vec2 v     = r.NegUnitCross(); // b2Math.b2Cross(1.0f, r);
            b2Vec2 abs_v = b2Math.b2Abs(v);

            // Separating axis for segment (Gino, p80).
            // |dot(v, p1 - c)| > dot(|v|, h)

            float maxFraction = input.maxFraction;

            // Build a bounding box for the segment.
            b2AABB segmentAABB = b2AABB.Default;
            {
                b2Vec2 t = p1 + maxFraction * (p2 - p1);
                segmentAABB.Set(b2Math.b2Min(p1, t), b2Math.b2Max(p1, t));
            }

            Stack <int> stack = new Stack <int>();

            stack.Push(m_root);

            while (stack.Count > 0)
            {
                int nodeId = stack.Pop();
                if (nodeId == b2TreeNode.b2_nullNode)
                {
                    continue;
                }

                b2TreeNode node = m_nodes[nodeId];

                if (b2Collision.b2TestOverlap(ref node.aabb, ref segmentAABB) == false)
                {
                    continue;
                }

                // Separating axis for segment (Gino, p80).
                // |dot(v, p1 - c)| > dot(|v|, h)
                b2Vec2 c          = node.aabb.Center;
                b2Vec2 h          = node.aabb.Extents;
                float  separation = b2Math.b2Abs(b2Math.b2Dot(v, p1 - c)) - b2Math.b2Dot(abs_v, h);
                if (separation > 0.0f)
                {
                    continue;
                }

                if (node.IsLeaf())
                {
                    b2RayCastInput subInput = new b2RayCastInput();
                    subInput.p1          = input.p1;
                    subInput.p2          = input.p2;
                    subInput.maxFraction = maxFraction;

                    float value = callback.RayCastCallback(subInput, nodeId);

                    if (value == 0.0f)
                    {
                        // The client has terminated the ray cast.
                        return;
                    }

                    if (value > 0.0f)
                    {
                        // Update segment bounding box.
                        maxFraction = value;
                        b2Vec2 t = p1 + maxFraction * (p2 - p1);
                        segmentAABB.Set(b2Math.b2Min(p1, t), b2Math.b2Max(p1, t));
                    }
                }
                else
                {
                    stack.Push(node.child1);
                    stack.Push(node.child2);
                }
            }
        }
Ejemplo n.º 19
0
        public void RayCast(b2WorldRayCastWrapper callback, b2RayCastInput input)
        {
            b2Vec2 p1 = input.p1;
            b2Vec2 p2 = input.p2;
            b2Vec2 r = p2 - p1;
            Debug.Assert(r.LengthSquared() > 0.0f);
            r.Normalize();

            // v is perpendicular to the segment.
            b2Vec2 v = b2Math.b2Cross(1.0f, r);
            b2Vec2 abs_v = b2Math.b2Abs(v);

            // Separating axis for segment (Gino, p80).
            // |dot(v, p1 - c)| > dot(|v|, h)

            float maxFraction = input.maxFraction;

            // Build a bounding box for the segment.
            b2AABB segmentAABB = new b2AABB();
            {
                b2Vec2 t = p1 + maxFraction * (p2 - p1);
                segmentAABB.lowerBound = b2Math.b2Min(p1, t);
                segmentAABB.upperBound = b2Math.b2Max(p1, t);
            }

            Stack<int> stack = new Stack<int>();
            stack.Push(m_root);

            while (stack.Count > 0)
            {
                int nodeId = stack.Pop();
                if (nodeId == b2TreeNode.b2_nullNode)
                {
                    continue;
                }

                b2TreeNode node = m_nodes[nodeId];

                if (b2Collision.b2TestOverlap(ref node.aabb, ref segmentAABB) == false)
                {
                    continue;
                }

                // Separating axis for segment (Gino, p80).
                // |dot(v, p1 - c)| > dot(|v|, h)
                b2Vec2 c = node.aabb.GetCenter();
                b2Vec2 h = node.aabb.GetExtents();
                float separation = b2Math.b2Abs(b2Math.b2Dot(v, p1 - c)) - b2Math.b2Dot(abs_v, h);
                if (separation > 0.0f)
                {
                    continue;
                }

                if (node.IsLeaf())
                {
                    b2RayCastInput subInput = new b2RayCastInput();
                    subInput.p1 = input.p1;
                    subInput.p2 = input.p2;
                    subInput.maxFraction = maxFraction;

                    float value = callback.RayCastCallback(subInput, nodeId);

                    if (value == 0.0f)
                    {
                        // The client has terminated the ray cast.
                        return;
                    }

                    if (value > 0.0f)
                    {
                        // Update segment bounding box.
                        maxFraction = value;
                        b2Vec2 t = p1 + maxFraction * (p2 - p1);
                        segmentAABB.lowerBound = b2Math.b2Min(p1, t);
                        segmentAABB.upperBound = b2Math.b2Max(p1, t);
                    }
                }
                else
                {
                    stack.Push(node.child1);
                    stack.Push(node.child2);
                }
            }
        }
Ejemplo n.º 20
0
        private void MoveAABB(b2AABB aabb)
        {
            b2Vec2 d = new b2Vec2();
            d.x = Rand.RandomFloat(-0.5f, 0.5f);
            d.y = Rand.RandomFloat(-0.5f, 0.5f);
            //d.x = 2.0f;
            //d.y = 0.0f;
            aabb.LowerBound += d;
            aabb.UpperBound += d;

            b2Vec2 c0 = 0.5f * (aabb.LowerBound + aabb.UpperBound);
            b2Vec2 min = new b2Vec2();
            min.Set(-m_worldExtent, 0.0f);
            b2Vec2 max = new b2Vec2();
            max.Set(m_worldExtent, 2.0f * m_worldExtent);
            b2Vec2 c = b2Math.b2Clamp(c0, min, max);

            aabb.LowerBound += c - c0;
            aabb.UpperBound += c - c0;
        }
Ejemplo n.º 21
0
        public b2Vec2 m_upperBound; //< the upper vertex

        #endregion Fields

        #region Methods

        /// Combine an AABB into this one.
        public void Combine(b2AABB aabb)
        {
            m_lowerBound = b2Math.b2Min(m_lowerBound, aabb.m_lowerBound);
            m_upperBound = b2Math.b2Max(m_upperBound, aabb.m_upperBound);
        }
Ejemplo n.º 22
0
        public static bool b2TestOverlap(ref b2AABB a, ref b2AABB b)
        {
            b2Vec2 d1, d2;
            d1 = b.m_lowerBound - a.m_upperBound;
            d2 = a.m_lowerBound - b.m_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;
        }
        /**
         * Update the pairs. This results in pair callbacks. This can only add pairs.
         */
        public void UpdatePairs(Action <object, object> callback)
        {
            m_pairCount = 0;
            // Perform tree queries for all moving queries
            foreach (b2DynamicTreeNode queryProxy in m_moveBuffer)
            {
                /*bool QueryCallback(b2DynamicTreeNode proxy)
                 * {
                 *      // A proxy cannot form a pair with itself.
                 *      if (proxy == queryProxy)
                 *              return true;
                 *
                 *      // Grow the pair buffer as needed
                 *      if (m_pairCount == m_pairBuffer.Count)
                 *      {
                 *              m_pairBuffer[m_pairCount] = new b2DynamicTreePair();
                 *      }
                 *
                 *      b2DynamicTreePair pair = m_pairBuffer[m_pairCount];
                 *      pair.proxyA = proxy < queryProxy?proxy:queryProxy;
                 *      pair.proxyB = proxy >= queryProxy?proxy:queryProxy;
                 ++m_pairCount;
                 *
                 *      return true;
                 * }*/
                BroadPhaseQueryCallback QueryCallback = delegate(object proxy){
                    // A proxy cannot form a pair with itself.
                    if (proxy == queryProxy)
                    {
                        return(true);
                    }

                    // Grow the pair buffer as needed
                    if (m_pairCount == m_pairBuffer.Count)
                    {
                        m_pairBuffer.Add(new b2DynamicTreePair());
                    }

                    b2DynamicTreePair pair = m_pairBuffer[m_pairCount];
                    //pair.proxyA = proxy < queryProxy?proxy:queryProxy;
                    //pair.proxyB = proxy >= queryProxy?proxy:queryProxy;
                    //改
                    pair.proxyA = queryProxy;
                    pair.proxyB = (b2DynamicTreeNode)proxy;

                    ++m_pairCount;

                    return(true);
                };
                // We have to query the tree with the fat AABB so that
                // we don't fail to create a pair that may touch later.
                b2AABB fatAABB = m_tree.GetFatAABB(queryProxy);
                m_tree.Query(QueryCallback, fatAABB);
            }

            // Reset move buffer
            //m_moveBuffer.length = 0;
            m_moveBuffer.RemoveRange(0, m_moveBuffer.Count);

            // Sort the pair buffer to expose duplicates.
            // TODO: Something more sensible
            //m_pairBuffer.sort(ComparePairs);

            // Send the pair buffer
            for (int i = 0; i < m_pairCount;)
            {
                b2DynamicTreePair primaryPair = m_pairBuffer[i];
                object            userDataA   = m_tree.GetUserData(primaryPair.proxyA);
                object            userDataB   = m_tree.GetUserData(primaryPair.proxyB);
                callback(userDataA, userDataB);
                ++i;

                // Skip any duplicate pairs
                while (i < m_pairCount)
                {
                    b2DynamicTreePair pair = m_pairBuffer[i];
                    if (pair.proxyA != primaryPair.proxyA || pair.proxyB != primaryPair.proxyB)
                    {
                        break;
                    }
                    ++i;
                }
            }
        }
Ejemplo n.º 24
0
 /// Combine two AABBs into this one.
 public void Combine(b2AABB aabb1, b2AABB aabb2)
 {
     m_lowerBound = b2Math.b2Min(aabb1.m_lowerBound, aabb2.m_lowerBound);
     m_upperBound = b2Math.b2Max(aabb1.m_upperBound, aabb2.m_upperBound);
 }
Ejemplo n.º 25
0
        public static bool b2TestOverlap(ref b2AABB a, ref b2AABB b)
        {
            // No operator overloading here - do direct computation to reduce time complexity
            if (b.LowerBound.x - a.UpperBound.x > 0.0f || b.LowerBound.y - a.UpperBound.y > 0.0f)
                return false;

            if (a.LowerBound.x - b.UpperBound.x > 0.0f || a.LowerBound.y - b.UpperBound.y > 0.0f)
                return false;

            return true;
        }
Ejemplo n.º 26
0
 public bool Equals(b2AABB o)
 {
     return(LowerBound == o.LowerBound && UpperBound == o.UpperBound);
 }
Ejemplo n.º 27
0
        public virtual bool MouseDown(b2Vec2 p)
        {
            m_mouseWorld = p;

            if (m_mouseJoint != null)
            {
                return false;
            }

            // Make a small box.
            b2AABB aabb = new b2AABB();
            b2Vec2 d = new b2Vec2();
            d.Set(0.001f, 0.001f);
            aabb.LowerBound = p - d;
            aabb.UpperBound = p + d;

            // Query the world for overlapping shapes.
            QueryCallback callback = new QueryCallback(p);
            m_world.QueryAABB(callback, aabb);

            if (callback.m_fixture != null)
            {
                b2Body body = callback.m_fixture.Body;
                b2MouseJointDef md = new b2MouseJointDef();
                md.BodyA = m_groundBody;
                md.BodyB = body;
                md.target = p;
                md.maxForce = 1000.0f * body.Mass;
                m_mouseJoint = (b2MouseJoint) m_world.CreateJoint(md);
                body.SetAwake(true);
                return true;
            }
            return false;
        }
Ejemplo n.º 28
0
        public override bool Equals(object obj)
        {
            b2AABB o = (b2AABB)obj;

            return(LowerBound == o.LowerBound && UpperBound == o.UpperBound);
        }
Ejemplo n.º 29
0
        public void Combine(ref b2AABB aabb1, ref b2AABB aabb2)
        {
            LowerBound.x = aabb1.LowerBound.x < aabb2.LowerBound.x ? aabb1.LowerBound.x : aabb2.LowerBound.x;
            LowerBound.y = aabb1.LowerBound.y < aabb2.LowerBound.y ? aabb1.LowerBound.y : aabb2.LowerBound.y;

            UpperBound.x = aabb1.UpperBound.x > aabb2.UpperBound.x ? aabb1.UpperBound.x : aabb2.UpperBound.x;
            UpperBound.y = aabb1.UpperBound.y > aabb2.UpperBound.y ? aabb1.UpperBound.y : aabb2.UpperBound.y;
        }
Ejemplo n.º 30
0
        public void Combine(ref b2AABB aabb)
        {
            m_lowerBound.x = aabb.LowerBoundX < LowerBoundX ? aabb.LowerBoundX : LowerBoundX;
            m_lowerBound.y = aabb.LowerBoundY < LowerBoundY ? aabb.LowerBoundY : LowerBoundY;

            m_upperBound.x = aabb.UpperBoundX > UpperBoundX ? aabb.UpperBoundX : UpperBoundX;
            m_upperBound.y = aabb.UpperBoundY > UpperBoundY ? aabb.UpperBoundY : UpperBoundY;

            //m_lowerBound = b2Math.b2Min(m_lowerBound, aabb.LowerBound);
            //m_upperBound = b2Math.b2Max(m_upperBound, aabb.UpperBound);

            _Dirty = true;
        }
Ejemplo n.º 31
0
 public bool Contains(ref b2AABB aabb)
 {
     bool result = true;
     result = result && LowerBound.x <= aabb.LowerBoundX;
     if (result)
         result = result && LowerBound.y <= aabb.LowerBoundY;
     if (result)
         result = result && aabb.UpperBoundX <= UpperBound.x;
     if (result)
         result = result && aabb.UpperBoundY <= UpperBound.y;
     return result;
 }
Ejemplo n.º 32
0
        public void Combine(ref b2AABB aabb1, ref b2AABB aabb2)
        {
            m_lowerBound.x = aabb1.LowerBoundX < aabb2.LowerBoundX ? aabb1.LowerBoundX : aabb2.LowerBoundX;
            m_lowerBound.y = aabb1.LowerBoundY < aabb2.LowerBoundY ? aabb1.LowerBoundY : aabb2.LowerBoundY;

            m_upperBound.x = aabb1.UpperBoundX > aabb2.UpperBoundX ? aabb1.UpperBoundX : aabb2.UpperBoundX;
            m_upperBound.y = aabb1.UpperBoundY > aabb2.UpperBoundY ? aabb1.UpperBoundY : aabb2.UpperBoundY;

//            m_lowerBound = b2Math.b2Min(aabb1.LowerBound, aabb2.LowerBound);
//            m_upperBound = b2Math.b2Max(aabb1.UpperBound, aabb2.UpperBound);

            _Dirty = true;
        }
Ejemplo n.º 33
0
        public void Combine(ref b2AABB aabb)
        {
            LowerBound.x = aabb.LowerBound.x < LowerBound.x ? aabb.LowerBound.x : LowerBound.x;
            LowerBound.y = aabb.LowerBound.y < LowerBound.y ? aabb.LowerBound.y : LowerBound.y;

            UpperBound.x = aabb.UpperBound.x > UpperBound.x ? aabb.UpperBound.x : UpperBound.x;
            UpperBound.y = aabb.UpperBound.y > UpperBound.y ? aabb.UpperBound.y : UpperBound.y;
        }
Ejemplo n.º 34
0
 public bool Equals(b2AABB o)
 {
     return (m_lowerBound == o.LowerBound && m_upperBound == o.UpperBound);
 }
Ejemplo n.º 35
0
        public void InsertLeaf(int leaf)
        {
            ++m_insertionCount;

            if (m_root == b2TreeNode.b2_nullNode)
            {
                m_root = leaf;
                m_nodes[m_root].parentOrNext = b2TreeNode.b2_nullNode;
                return;
            }

            // Find the best sibling for this node
            b2AABB leafAABB = m_nodes[leaf].aabb;
            int index = m_root;
            while (m_nodes[index].IsLeaf() == false)
            {
                int child1 = m_nodes[index].child1;
                int child2 = m_nodes[index].child2;

                float area = m_nodes[index].aabb.GetPerimeter();

                b2AABB combinedAABB = new b2AABB();
                combinedAABB.Combine(m_nodes[index].aabb, leafAABB);
                float combinedArea = combinedAABB.GetPerimeter();

                // 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 (m_nodes[child1].IsLeaf())
                {
                    b2AABB aabb = new b2AABB();
                    aabb.Combine(leafAABB, m_nodes[child1].aabb);
                    cost1 = aabb.GetPerimeter() + inheritanceCost;
                }
                else
                {
                    b2AABB aabb = new b2AABB();
                    aabb.Combine(leafAABB, m_nodes[child1].aabb);
                    float oldArea = m_nodes[child1].aabb.GetPerimeter();
                    float newArea = aabb.GetPerimeter();
                    cost1 = (newArea - oldArea) + inheritanceCost;
                }

                // Cost of descending into child2
                float cost2;
                if (m_nodes[child2].IsLeaf())
                {
                    b2AABB aabb = new b2AABB();
                    aabb.Combine(leafAABB, m_nodes[child2].aabb);
                    cost2 = aabb.GetPerimeter() + inheritanceCost;
                }
                else
                {
                    b2AABB aabb = new b2AABB();
                    aabb.Combine(leafAABB, m_nodes[child2].aabb);
                    float oldArea = m_nodes[child2].aabb.GetPerimeter();
                    float newArea = aabb.GetPerimeter();
                    cost2 = newArea - oldArea + inheritanceCost;
                }

                // Descend according to the minimum cost.
                if (cost < cost1 && cost < cost2)
                {
                    break;
                }

                // Descend
                if (cost1 < cost2)
                {
                    index = child1;
                }
                else
                {
                    index = child2;
                }
            }

            int sibling = index;

            // Create a new parent.
            int oldParent = m_nodes[sibling].parentOrNext;
            int newParent = AllocateNode();
            m_nodes[newParent].parentOrNext = oldParent;
            m_nodes[newParent].userData = null;
            m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb);
            m_nodes[newParent].height = m_nodes[sibling].height + 1;

            if (oldParent != b2TreeNode.b2_nullNode)
            {
                // The sibling was not the root.
                if (m_nodes[oldParent].child1 == sibling)
                {
                    m_nodes[oldParent].child1 = newParent;
                }
                else
                {
                    m_nodes[oldParent].child2 = newParent;
                }

                m_nodes[newParent].child1 = sibling;
                m_nodes[newParent].child2 = leaf;
                m_nodes[sibling].parentOrNext = newParent;
                m_nodes[leaf].parentOrNext = newParent;
            }
            else
            {
                // The sibling was the root.
                m_nodes[newParent].child1 = sibling;
                m_nodes[newParent].child2 = leaf;
                m_nodes[sibling].parentOrNext = newParent;
                m_nodes[leaf].parentOrNext = newParent;
                m_root = newParent;
            }

            // Walk back up the tree fixing heights and AABBs
            index = m_nodes[leaf].parentOrNext;
            while (index != b2TreeNode.b2_nullNode)
            {
                index = Balance(index);

                int child1 = m_nodes[index].child1;
                int child2 = m_nodes[index].child2;

                Debug.Assert(child1 != b2TreeNode.b2_nullNode);
                Debug.Assert(child2 != b2TreeNode.b2_nullNode);

                m_nodes[index].height = 1 + Math.Max(m_nodes[child1].height, m_nodes[child2].height);
                m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);

                index = m_nodes[index].parentOrNext;
            }

            //Validate();
        }
Ejemplo n.º 36
0
        /**
         * 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.
         * @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
         * @param callback a callback class that is called for each proxy that is hit by the ray.
         * It should be of signature:
         * ----- <code>function callback(input:b2RayCastInput, proxy:*):void</code>
         * <code>function callback(input:b2RayCastInput, proxy:*):Number</code>
         */
        public void RayCast(BroadPhaseRayCastCallback callback, b2RayCastInput input)
        {
            if (m_root == null)
            {
                return;
            }

            b2Vec2 p1 = input.p1;
            b2Vec2 p2 = input.p2;
            b2Vec2 r  = b2Math.SubtractVV(p1, p2);

            //b2Settings.b2Assert(r.LengthSquared() > 0.0);
            r.Normalize();

            // v is perpendicular to the segment
            b2Vec2 v     = b2Math.CrossFV(1.0f, r);
            b2Vec2 abs_v = b2Math.AbsV(v);

            float maxFraction = input.maxFraction;

            // Build a bounding box for the segment
            b2AABB segmentAABB = new b2AABB();
            float  tX;
            float  tY;
            {
                tX = p1.x + maxFraction * (p2.x - p1.x);
                tY = p1.y + maxFraction * (p2.y - p1.y);
                segmentAABB.lowerBound.x = Mathf.Min(p1.x, tX);
                segmentAABB.lowerBound.y = Mathf.Min(p1.y, tY);
                segmentAABB.upperBound.x = Mathf.Max(p1.x, tX);
                segmentAABB.upperBound.y = Mathf.Max(p1.y, tY);
            }

            Dictionary <int, b2DynamicTreeNode> stack = new Dictionary <int, b2DynamicTreeNode>();

            int count = 0;

            stack[count++] = m_root;

            while (count > 0)
            {
                b2DynamicTreeNode node = stack[--count];

                if (node.aabb.TestOverlap(segmentAABB) == false)
                {
                    continue;
                }

                // Separating axis for segment (Gino, p80)
                // |dot(v, p1 - c)| > dot(|v|,h)

                b2Vec2 c          = node.aabb.GetCenter();
                b2Vec2 h          = node.aabb.GetExtents();
                float  separation = Mathf.Abs(v.x * (p1.x - c.x) + v.y * (p1.y - c.y))
                                    - abs_v.x * h.x - abs_v.y * h.y;
                if (separation > 0.0f)
                {
                    continue;
                }

                if (node.IsLeaf())
                {
                    b2RayCastInput subInput = new b2RayCastInput();
                    subInput.p1 = input.p1;
                    subInput.p2 = input.p2;
                    //*************by kingBook 2015/10/22 16:17*************
                    subInput.maxFraction = maxFraction;
                    float value = callback(subInput, node);
                    if (value == 0)
                    {
                        return;
                    }
                    if (value > 0)
                    {
                        //Update the segment bounding box
                        maxFraction = value;
                        //******************************************************
                        tX = p1.x + maxFraction * (p2.x - p1.x);
                        tY = p1.y + maxFraction * (p2.y - p1.y);
                        segmentAABB.lowerBound.x = Mathf.Min(p1.x, tX);
                        segmentAABB.lowerBound.y = Mathf.Min(p1.y, tY);
                        segmentAABB.upperBound.x = Mathf.Max(p1.x, tX);
                        segmentAABB.upperBound.y = Mathf.Max(p1.y, tY);
                    }
                }
                else
                {
                    // No stack limit, so no assert
                    stack[count++] = node.child1;
                    stack[count++] = node.child2;
                }
            }
        }
Ejemplo n.º 37
0
        public void Query(Ib2QueryCallback w, b2AABB aabb)
        {
            Stack<int> stack = new Stack<int>();
            stack.Push(m_root);

            while (stack.Count > 0)
            {
                int nodeId = stack.Pop();
                if (nodeId == b2TreeNode.b2_nullNode)
                {
                    continue;
                }

                b2TreeNode node = m_nodes[nodeId];

                if (b2Collision.b2TestOverlap(ref node.aabb, ref aabb))
                {
                    if (node.IsLeaf())
                    {
                        bool proceed = w.QueryCallback(nodeId);
                        if (proceed == false)
                        {
                            return;
                        }
                    }
                    else
                    {
                        if(node.child1 != b2TreeNode.b2_nullNode)
                            stack.Push(node.child1);
                        if (node.child2 != b2TreeNode.b2_nullNode)
                            stack.Push(node.child2);
                    }
                }
            }
        }
Ejemplo n.º 38
0
 public void GetFatAABB(int proxyId, out b2AABB output)
 {
     m_tree.GetFatAABB(proxyId, out output);
 }
Ejemplo n.º 39
0
        public void RebuildBottomUp()
        {
            int[] nodes = new int[m_nodeCount];
            int count = 0;

            // Build array of leaves. Free the rest.
            for (int i = 0; i < m_nodeCapacity; ++i)
            {
                if (m_nodes[i].height < 0)
                {
                    // free node in pool
                    continue;
                }

                if (m_nodes[i].IsLeaf())
                {
                    m_nodes[i].parentOrNext = b2TreeNode.b2_nullNode;
                    nodes[count] = i;
                    ++count;
                }
                else
                {
                    FreeNode(i);
                }
            }

            while (count > 1)
            {
                float minCost = b2Settings.b2_maxFloat;
                int iMin = -1, jMin = -1;
                for (int i = 0; i < count; ++i)
                {
                    b2AABB aabbi = m_nodes[nodes[i]].aabb;

                    for (int j = i + 1; j < count; ++j)
                    {
                        b2AABB aabbj = m_nodes[nodes[j]].aabb;
                        b2AABB b = new b2AABB();
                        b.Combine(aabbi, aabbj);
                        float cost = b.GetPerimeter();
                        if (cost < minCost)
                        {
                            iMin = i;
                            jMin = j;
                            minCost = cost;
                        }
                    }
                }

                int index1 = nodes[iMin];
                int index2 = nodes[jMin];
                b2TreeNode child1 = m_nodes[index1];
                b2TreeNode child2 = m_nodes[index2];

                int parentIndex = AllocateNode();
                b2TreeNode parent = m_nodes[parentIndex];
                parent.child1 = index1;
                parent.child2 = index2;
                parent.height = 1 + Math.Max(child1.height, child2.height);
                parent.aabb.Combine(child1.aabb, child2.aabb);
                parent.parentOrNext = b2TreeNode.b2_nullNode;

                child1.parentOrNext = parentIndex;
                child2.parentOrNext = parentIndex;

                nodes[jMin] = nodes[count - 1];
                nodes[iMin] = parentIndex;
                --count;
            }

            m_root = nodes[0];

            Validate();
        }
Ejemplo n.º 40
0
 public virtual void Query(Ib2QueryCallback q, b2AABB aabb)
 {
     m_tree.Query(q, aabb);
 }
Ejemplo n.º 41
0
        public void DrawAABB(b2AABB aabb, b2Color p1)
        {
            b2Vec2[] verts = new b2Vec2[4];
            verts[0] = new b2Vec2(aabb.LowerBound.x, aabb.LowerBound.y);
            verts[1] = new b2Vec2(aabb.UpperBound.x, aabb.LowerBound.y);
            verts[2] = new b2Vec2(aabb.UpperBound.x, aabb.UpperBound.y);
            verts[3] = new b2Vec2(aabb.LowerBound.x, aabb.UpperBound.y);

            DrawPolygon(verts, 4, p1);
        }
Ejemplo n.º 42
0
 public void Query <T>(T callback, b2AABB aabb)
 {
     m_tree.Query(callback, aabb);
 }
Ejemplo n.º 43
0
 /// Combine two AABBs into this one.
 public void Combine(b2AABB aabb1, b2AABB aabb2)
 {
     m_lowerBound = b2Math.b2Min(aabb1.m_lowerBound, aabb2.m_lowerBound);
     m_upperBound = b2Math.b2Max(aabb1.m_upperBound, aabb2.m_upperBound);
 }
Ejemplo n.º 44
0
 public bool Equals(b2AABB o)
 {
     return(m_lowerBound == o.LowerBound && m_upperBound == o.UpperBound);
 }
Ejemplo n.º 45
0
 private void GetRandomAABB(ref b2AABB aabb)
 {
     b2Vec2 w = new b2Vec2();
     w.Set(2.0f * m_proxyExtent, 2.0f * m_proxyExtent);
     //aabb->lowerBound.x = -m_proxyExtent;
     //aabb->lowerBound.y = -m_proxyExtent + m_worldExtent;
     aabb.LowerBoundX = Rand.RandomFloat(-m_worldExtent, m_worldExtent);
     aabb.LowerBoundY = Rand.RandomFloat(0.0f, 2.0f * m_worldExtent);
     aabb.UpperBound = aabb.LowerBound + w;
 }
 /**
  * @inheritDoc
  */
 public void Query(BroadPhaseQueryCallback callback, b2AABB aabb)
 {
     m_tree.Query(callback, aabb);
 }