/// <summary>
/// Updates a body's position. If the body has moved outside of its
/// expanded AABB, then the body is removed from the tree and re-inserted.
/// Otherwise the function returns immediately.
/// </summary>
public void UpdateBody(VoltBody body)
{
int proxyId = body.ProxyId;
VoltDebug.Assert((0 <= proxyId) && (proxyId < this.nodeCapacity));
Node proxyNode = this.nodes[proxyId];
VoltDebug.Assert(proxyNode.IsLeaf);
if (proxyNode.aabb.Contains(body.AABB))
{
return;
}
this.RemoveLeaf(proxyId);
// Extend AABB
VoltAABB expanded =
VoltAABB.CreateExpanded(body.AABB, VoltConfig.AABB_EXTENSION);
// Predict AABB displacement and sweep the AABB
//Vector2 sweep = VoltConfig.AABB_MULTIPLIER * displacement;
//VoltAABB swept = VoltAABB.CreateSwept(expanded, sweep);
//this.nodes[proxyId].aabb = swept;
proxyNode.aabb = expanded;
this.InsertLeaf(proxyId);
return;
}
/// <summary>
/// Adds a body to the tree.
/// </summary>
public void AddBody(VoltBody body)
{
VoltDebug.Assert(body.ProxyId == TreeBroadphase.NULL_NODE);
int proxyId;
Node proxyNode = this.AllocateNode(out proxyId);
// Expand the aabb
proxyNode.aabb =
VoltAABB.CreateExpanded(
body.AABB,
VoltConfig.AABB_EXTENSION);
proxyNode.body = body;
proxyNode.height = 0;
this.InsertLeaf(proxyId);
body.ProxyId = proxyId;
}