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;
}
/// @see Shape.ComputeAABB
public override void ComputeAABB(out AABB aabb, ref XForm 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 XForm 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);
}
internal void Synchronize(BroadPhase broadPhase, ref XForm transform1, ref XForm transform2)
{
if (_proxyId == BroadPhase.NullProxy)
{
return;
}
// Compute an AABB that covers the swept shape (may miss some rotation effect).
AABB aabb1, aabb2;
_shape.ComputeAABB(out aabb1, ref transform1);
_shape.ComputeAABB(out aabb2, ref transform2);
AABB aabb = new AABB();
aabb.Combine(ref aabb1, ref aabb2);
broadPhase.MoveProxy(_proxyId, ref aabb);
}
/// Does this aabb contain the provided AABB.
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;
}
/// Combine two AABBs into this one.
public void Combine(ref AABB aabb1, ref AABB aabb2)
{
lowerBound = Vector2.Min(aabb1.lowerBound, aabb2.lowerBound);
upperBound = Vector2.Max(aabb1.upperBound, aabb2.upperBound);
}
/// 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.
public void RayCast(RayCastCallback 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;
RayCastOutput output;
callback(out output, ref subInput, node.userData);
if (output.hit)
{
// Early exit.
if (output.fraction == 0.0f)
{
return;
}
maxFraction = output.fraction;
// Update segment bounding box.
{
Vector2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.lowerBound = Vector2.Min(p1, t);
segmentAABB.upperBound = Vector2.Max(p1, t);
}
}
}
else
{
Debug.Assert(count + 1 < k_stackSize);
stack[count++] = node.child1;
stack[count++] = node.child2;
}
}
}
/// Query the world for all fixtures that potentially overlap the
/// provided AABB.
/// @param callback a user implemented callback class.
/// @param aabb the query box.
public void Query(Func<Fixture, bool> callback, ref AABB aabb)
{
_contactManager._broadPhase.Query(callback, ref aabb);
}
/// Query an AABB for overlapping proxies. The callback class
/// is called for each proxy that overlaps the supplied AABB.
public void Query(Action<int> 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())
{
callback(node.userData);
}
else
{
Debug.Assert(count + 1 < k_stackSize);
stack[count++] = node.child1;
stack[count++] = node.child2;
}
}
}
}
/// Move a proxy. 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.
public void MoveProxy(int proxyId, ref AABB aabb)
{
Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity);
Debug.Assert(_nodes[proxyId].IsLeaf());
if (_nodes[proxyId].aabb.Contains(ref aabb))
{
return;
}
RemoveLeaf(proxyId);
Vector2 r = new Vector2(Settings.b2_aabbExtension, Settings.b2_aabbExtension);
_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r;
_nodes[proxyId].aabb.upperBound = aabb.upperBound + r;
InsertLeaf(proxyId);
}
/// Create a proxy. Provide a tight fitting AABB and a userData pointer.
public int CreateProxy(ref AABB aabb, int 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);
return proxyId;
}
/// Given a transform, compute the associated axis aligned bounding box for this shape. /// @param aabb returns the axis aligned box. /// @param xf the world transform of the shape. public abstract void ComputeAABB(out AABB aabb, ref XForm xf);
internal void Collide()
{
// Update awake contacts.
Contact c = _contactList;
while (c != null)
{
Fixture fixtureA = c.GetFixtureA();
Fixture fixtureB = c.GetFixtureB();
Body bodyA = fixtureA.GetBody();
Body bodyB = fixtureB.GetBody();
if (bodyA.IsSleeping && bodyB.IsSleeping)
{
c = c.GetNext();
continue;
}
// Is this contact flagged for filtering?
if ((c._flags & ContactFlags.Filter) == ContactFlags.Filter)
{
// Are both bodies static?
if (bodyA.IsStatic && bodyB.IsStatic)
{
Contact cNuke = c;
c = cNuke.GetNext();
Destroy(cNuke);
continue;
}
// Does a joint override collision?
if (bodyB.IsConnected(bodyA))
{
Contact cNuke = c;
c = cNuke.GetNext();
Destroy(cNuke);
continue;
}
// Check user filtering.
if (ContactFilter.ShouldCollide(fixtureA, fixtureB) == false)
{
Contact cNuke = c;
c = cNuke.GetNext();
Destroy(cNuke);
continue;
}
// Clear the filtering flag.
c._flags &= ~ContactFlags.Filter;
}
int proxyIdA = fixtureA._proxyId;
int proxyIdB = fixtureB._proxyId;
AABB aabbA;
_broadPhase.GetAABB(proxyIdA, out aabbA);
AABB aabbB;
_broadPhase.GetAABB(proxyIdB, out aabbB);
// Here we cull out contacts that cease to overlap.
if (AABB.TestOverlap(ref aabbA, ref aabbB) == false)
{
Contact cNuke = c;
c = cNuke.GetNext();
Destroy(cNuke);
continue;
}
// The contact persists.
c.Update(ContactListener);
c = c.GetNext();
}
}
