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