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); }
/// @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; }
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; }
/// @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; }
/// 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); } } } }
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; } }
/// 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; }
/// <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); }
/// <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; }
/// 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; } } } }
/// 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); }