Exemple #1
0
        public void DrawAABB(ref AABB aabb, Color color)
        {
            FixedArray8<Vector2> verts = new FixedArray8<Vector2>();
            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(ref verts, 4, color);
        }
Exemple #2
0
        /// @see Shape::ComputeAABB
        public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
        {
            aabb = new AABB();
            Vector2 v1 = MathUtils.Multiply(ref transform, _vertex1);
            Vector2 v2 = MathUtils.Multiply(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;
        }
Exemple #3
0
        public Vector2 upperBound; ///< the upper vertex

        #endregion Fields

        #region Methods

        public static bool TestOverlap(ref AABB a, ref 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;
        }
        /// <summary>
        /// @see Shape.ComputeAABB
        /// </summary>
        /// <param name="aabb"></param>
        /// <param name="xf"></param>
        public override void ComputeAABB(out AABB aabb, ref Transform xf)
        {
            Vector2 lower = MathUtils.Multiply(ref xf, _vertices[0]);
            Vector2 upper = lower;

            for (int i = 1; i < _vertexCount; ++i)
            {
                Vector2 v = MathUtils.Multiply(ref xf, _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;
        }
Exemple #5
0
        /// @see Shape::ComputeAABB
        public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
        {
            Debug.Assert(childIndex < _count);
            aabb = new AABB();

            int i1 = childIndex;
            int i2 = childIndex + 1;
            if (i2 == _count)
            {
                i2 = 0;
            }

            Vector2 v1 = MathUtils.Multiply(ref transform, _vertices[i1]);
            Vector2 v2 = MathUtils.Multiply(ref transform, _vertices[i2]);

            aabb.lowerBound = Vector2.Min(v1, v2);
            aabb.upperBound = Vector2.Max(v1, v2);
        }
        /// <summary>
        /// Create a proxy. Provide a tight fitting AABB and a userData pointer.
        /// </summary>
        /// <param name="aabb"></param>
        /// <param name="userData"></param>
        /// <returns></returns>
        public int CreateProxy(ref AABB aabb, object userData)
        {
            int proxyId = AllocateNode();

            // Fatten the aabb.
            Vector2 r = new Vector2(Settings.b2_aabbExtension, Settings.b2_aabbExtension);
            _nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r;
            _nodes[proxyId].aabb.upperBound = aabb.upperBound + r;
            _nodes[proxyId].userData = userData;

            InsertLeaf(proxyId);

            // Rebalance if necessary.
            int iterationCount = _nodeCount >> 4;
            int tryCount = 0;
            int height = ComputeHeight();
            while (height > 64 && tryCount < 10)
            {
                Rebalance(iterationCount);
                height = ComputeHeight();
                ++tryCount;
            }

            return proxyId;
        }
Exemple #7
0
	    /// Query an AABB for overlapping proxies. The callback class
	    /// is called for each proxy that overlaps the supplied AABB.
	    public void Query(Func<int, bool> callback, ref AABB aabb)
        {
            stack.Clear();
	        stack.Push(_root);

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

		        DynamicTreeNode node = _nodes[nodeId];

                if (AABB.TestOverlap(ref node.aabb, ref aabb))
		        {
			        if (node.IsLeaf())
			        {
				        bool proceed = callback(nodeId);
                        if (!proceed)
                        {
                            return;
                        }
			        }
			        else
			        {
                        stack.Push(node.child1);
				        stack.Push(node.child2);
			        }
		        }
	        }
        }
Exemple #8
0
        private void InsertLeaf(int leaf)
        {
            ++_insertionCount;

	        if (_root == NullNode)
	        {
		        _root = leaf;
		        _nodes[_root].parentOrNext = NullNode;
		        return;
	        }

	        // Find the best sibling for this node
	        AABB leafAABB = _nodes[leaf].aabb;
	        Vector2 leafCenter = leafAABB.GetCenter();
	        int sibling = _root;
	        while (_nodes[sibling].IsLeaf() == false)
	        {
		        // Expand the node's AABB.
		        _nodes[sibling].aabb.Combine(ref leafAABB);
		        _nodes[sibling].leafCount += 1;

		        int child1 = _nodes[sibling].child1;
		        int child2 = _nodes[sibling].child2;

#if false
		        // This seems to create imbalanced trees
		        Vector2 delta1 = Math.Abs(_nodes[child1].aabb.GetCenter() - leafCenter);
		        Vector2 delta2 = Math.Abs(_nodes[child2].aabb.GetCenter() - leafCenter);

		        float norm1 = delta1.x + delta1.y;
		        float norm2 = delta2.x + delta2.y;
#else
		        // Surface area heuristic
		        AABB aabb1 = new AABB();
                AABB  aabb2 = new AABB();
		        aabb1.Combine(ref leafAABB, ref _nodes[child1].aabb);
		        aabb2.Combine(ref leafAABB, ref _nodes[child2].aabb);
		        float norm1 = (_nodes[child1].leafCount + 1) * aabb1.GetPerimeter();
		        float norm2 = (_nodes[child2].leafCount + 1) * aabb2.GetPerimeter();
#endif

		        if (norm1 < norm2)
		        {
			        sibling = child1;
		        }
		        else
		        {
			        sibling = child2;
		        }
	        }

	        // Create a new parent for the siblings.
	        int oldParent = _nodes[sibling].parentOrNext;
	        int newParent = AllocateNode();
            _nodes[newParent].parentOrNext = oldParent;
	        _nodes[newParent].userData = null;
	        _nodes[newParent].aabb.Combine(ref leafAABB, ref _nodes[sibling].aabb);
	        _nodes[newParent].leafCount = _nodes[sibling].leafCount + 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;
	        }
        }
Exemple #9
0
 /// Combine an AABB into this one.
 public void Combine(ref AABB aabb)
 {
     lowerBound = Vector2.Min(lowerBound, aabb.lowerBound);
     upperBound = Vector2.Max(upperBound, aabb.upperBound);
 }
 /// <summary>
 /// Given a transform, compute the associated axis aligned bounding box for this shape.
 /// </summary>
 /// <param name="aabb">returns the axis aligned box.</param>
 /// <param name="xf">the world transform of the shape.</param>
 public abstract void ComputeAABB(out AABB aabb, ref Transform xf);
 /// <summary>
 /// 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.
 /// </summary>
 /// <param name="aabb"></param>
 public void GetAABB(out AABB aabb)
 {
     aabb = _aabb;
 }
Exemple #12
0
 /// <summary> Query the world for all fixtures that potentially overlap the
 /// provided AABB.
 /// </summary> <param name="callback"> a user implemented callback class.
 /// </param> <param name="aabb"> the query box.
 /// </param>
 public void QueryAABB(Func <FixtureProxy, bool> callback, ref AABB aabb)
 {
     _queryAABBCallback = callback;
     _contactManager._broadPhase.Query(_queryAABBCallbackWrapper, ref aabb);
     _queryAABBCallback = null;
 }
 /// <summary>
 /// Combine two AABBs into this one.
 /// </summary>
 /// <param name="aabb1"></param>
 /// <param name="aabb2"></param>
 public void Combine(ref AABB aabb1, ref AABB aabb2)
 {
     lowerBound = Vector2.Min(aabb1.lowerBound, aabb2.lowerBound);
     upperBound = Vector2.Max(aabb1.upperBound, aabb2.upperBound);
 }
 /// Query an AABB for overlapping proxies. The callback class
 /// is called for each proxy that overlaps the supplied AABB.
 public void Query(Func <int, bool> callback, ref AABB aabb)
 {
     _tree.Query(callback, ref aabb);
 }
Exemple #15
0
 /// <summary> Given a transform, compute the associated axis aligned bounding box for a child shape.
 /// </summary> <param name="aabb"> returns the axis aligned box.
 /// </param> <param name="xf"> the world transform of the shape.
 /// </param>
 public abstract void ComputeAABB(out AABB aabb, ref Transform xf, int childIndex);
        /// <summary>
        /// Query an AABB for overlapping proxies. The callback class
        /// is called for each proxy that overlaps the supplied AABB.
        /// </summary>
        /// <param name="callback"></param>
        /// <param name="aabb"></param>
        public void Query(Func<int, bool> callback, ref AABB aabb)
        {
            int count = 0;
            stack[count++] = _root;

            while (count > 0)
            {
                int nodeId = stack[--count];
                if (nodeId == NullNode)
                {
                    continue;
                }

                DynamicTreeNode node = _nodes[nodeId];

                if (AABB.TestOverlap(ref node.aabb, ref aabb))
                {
                    if (node.IsLeaf())
                    {
                        bool proceed = callback(nodeId);
                        if (!proceed)
                        {
                            return;
                        }
                    }
                    else
                    {
                        if (count < k_stackSize)
                        {
                            stack[count++] = node.child1;
                        }

                        if (count < k_stackSize)
                        {
                            stack[count++] = node.child2;
                        }
                    }
                }
            }
        }
        private void CreateStem()
        {
            AddBranch = true;

            AABB aabb = new AABB(); body.GetFixtureList().GetAABB(out aabb);
            Vector2 v = aabb.GetCenter() + new Vector2(0, 15);

            Path2D p = new Path2D();
            p.AddPoint(v); p.AddPoint(v + new Vector2(0, -10));

            stem = new Branch(p, gameContent, body.GetWorld(), null);
            RevoluteJointDef revJd = new RevoluteJointDef();
            revJd.bodyA = stem.body;
            revJd.bodyB = ground;

            revJd.collideConnected = true;

            revJd.localAnchorA = Vector2.Zero;
            revJd.localAnchorB = stem.body.Position;
            revJd.enableMotor = true;

            stem.revoJoint = (RevoluteJoint)world.CreateJoint(revJd);

            // A Small rotation sets the body in motion
            stem.body.Rotation = (float)Math.PI / 360;
        }
        public bool ApplesIntersectingCollectables(Vector2 collectable, float nearestDistance)
        {
            foreach (Apple a in apples)
            {
                AABB aabb = new AABB();
                a.body.GetFixtureList().GetAABB(out aabb);

                float d = Vector2.Distance(collectable, aabb.GetCenter());
                if (d < nearestDistance) return true;
            }

            return false;
        }
Exemple #19
0
	    /// Given a transform, compute the associated axis aligned bounding box for a child shape.
	    /// @param aabb returns the axis aligned box.
	    /// @param xf the world transform of the shape.
	    public abstract void ComputeAABB(out AABB aabb, ref Transform xf, int childIndex);
 /// <summary>
 /// Get the fat AABB for a proxy.
 /// </summary>
 /// <param name="proxyId"></param>
 /// <param name="fatAABB"></param>
 public void GetFatAABB(int proxyId, out AABB fatAABB)
 {
     Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity);
     fatAABB = _nodes[proxyId].aabb;
 }
 /// <summary>
 /// Query the world for all fixtures that potentially overlap the
 /// provided AABB.
 /// </summary>
 /// <param name="callback">a user implemented callback class.</param>
 /// <param name="aabb">the query box.</param>
 public void QueryAABB(Func<Fixture, bool> callback, ref AABB aabb)
 {
     _queryAABBCallback = callback;
     _contactManager._broadPhase.Query(_queryAABBCallbackWrapper, ref aabb);
     _queryAABBCallback = null;
 }
        /// <summary>
        /// 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.
        /// @return true if the proxy was re-inserted.
        /// </summary>
        /// <param name="proxyId"></param>
        /// <param name="aabb"></param>
        /// <param name="displacement"></param>
        /// <returns></returns>
        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.b2_aabbExtension, Settings.b2_aabbExtension);
            b.lowerBound = b.lowerBound - r;
            b.upperBound = b.upperBound + r;

            // Predict AABB displacement.
            Vector2 d = Settings.b2_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;
        }
 /// <summary>
 /// @see Shape.ComputeAABB
 /// </summary>
 /// <param name="aabb"></param>
 /// <param name="transform"></param>
 public override void ComputeAABB(out AABB aabb, ref Transform transform)
 {
     Vector2 p = transform.Position + MathUtils.Multiply(ref transform.R, _p);
     aabb.lowerBound = new Vector2(p.X - _radius, p.Y - _radius);
     aabb.upperBound = new Vector2(p.X + _radius, p.Y + _radius);
 }
        /// <summary>
        /// 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.
        /// </summary>
        /// <param name="callback"></param>
        /// <param name="input"></param>
        internal void RayCast(RayCastCallbackInternal callback, ref RayCastInput input)
        {
            Vector2 p1 = input.p1;
            Vector2 p2 = input.p2;
            Vector2 r = p2 - p1;
            Debug.Assert(r.LengthSquared() > 0.0f);
            r.Normalize();

            // v is perpendicular to the segment.
            Vector2 v = MathUtils.Cross(1.0f, r);
            Vector2 abs_v = MathUtils.Abs(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.
            AABB segmentAABB = new AABB();
            {
                Vector2 t = p1 + maxFraction * (p2 - p1);
                segmentAABB.lowerBound = Vector2.Min(p1, t);
                segmentAABB.upperBound = Vector2.Max(p1, t);
            }

            int count = 0;
            stack[count++] = _root;

            while (count > 0)
            {
                int nodeId = stack[--count];
                if (nodeId == NullNode)
                {
                    continue;
                }

                DynamicTreeNode 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.GetCenter();
                Vector2 h = node.aabb.GetExtents();
                float separation = Math.Abs(Vector2.Dot(v, p1 - c)) - Vector2.Dot(abs_v, h);
                if (separation > 0.0f)
                {
                    continue;
                }

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

                    float value = callback(ref 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
                {
                    if (count < k_stackSize)
                    {
                        stack[count++] = node.child1;
                    }

                    if (count < k_stackSize)
                    {
                        stack[count++] = node.child2;
                    }
                }
            }
        }
Exemple #25
0
        /// 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.
	    public void GetAABB(out AABB aabb, int childIndex)
        {
            Debug.Assert(0 <= childIndex && childIndex < _proxyCount);
            aabb = _proxies[childIndex].aabb;
        }
 /// <summary>
 /// Does this aabb contain the provided AABB.
 /// </summary>
 /// <param name="aabb"></param>
 /// <returns></returns>
 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;
 }
 /// Get the AABB for a proxy.
 public void GetFatAABB(int proxyId, out AABB aabb)
 {
     _tree.GetFatAABB(proxyId, out aabb);
 }