/// <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>
/// Finds all dynamic bodies that overlap with the explosion AABB
/// and pass the target filter test. Does not test actual shapes.
/// </summary>
private void PopulateFiltered(
TSVector2 origin,
FP radius,
VoltBodyFilter targetFilter,
int ticksBehind,
ref VoltBuffer <VoltBody> filterBuffer)
{
if (filterBuffer == null)
{
filterBuffer = new VoltBuffer <VoltBody>();
}
filterBuffer.Clear();
this.reusableBuffer.Clear();
this.staticBroadphase.QueryCircle(origin, radius, this.reusableBuffer);
this.dynamicBroadphase.QueryCircle(origin, radius, this.reusableBuffer);
VoltAABB aabb = new VoltAABB(origin, radius);
for (int i = 0; i < this.reusableBuffer.Count; i++)
{
VoltBody body = this.reusableBuffer[i];
if ((targetFilter == null) || targetFilter.Invoke(body))
{
if (body.QueryAABBOnly(aabb, ticksBehind))
{
filterBuffer.Add(body);
}
}
}
}
public static VoltAABB CreateSwept(VoltAABB source, Vector2 vector)
{
float top = source.top;
float bottom = source.bottom;
float left = source.left;
float right = source.right;
if (vector.x < 0.0f)
{
left += vector.x;
}
else
{
right += vector.x;
}
if (vector.y < 0.0f)
{
bottom += vector.y;
}
else
{
top += vector.y;
}
return(new VoltAABB(top, bottom, left, right));
}
private void UpdateRotated(
Node P,
Node Q,
Node R,
int iP,
int iX,
int iY,
bool left)
{
Node X = this.nodes[iX];
Node Y = this.nodes[iY];
R.right = iX;
if (left)
{
P.left = iY;
}
else
{
P.right = iY;
}
Y.parentOrNext = iP;
P.aabb = VoltAABB.CreateMerged(Q.aabb, Y.aabb);
R.aabb = VoltAABB.CreateMerged(P.aabb, X.aabb);
P.height = 1 + Math.Max(Q.height, Y.height);
R.height = 1 + Math.Max(P.height, X.height);
}
public static VoltAABB CreateSwept(VoltAABB source, VoltVector2 vector)
{
Fix64 top = source.top;
Fix64 bottom = source.bottom;
Fix64 left = source.left;
Fix64 right = source.right;
if (vector.x < Fix64.Zero)
{
left += vector.x;
}
else
{
right += vector.x;
}
if (vector.y < Fix64.Zero)
{
bottom += vector.y;
}
else
{
top += vector.y;
}
return(new VoltAABB(top, bottom, left, right));
}
public bool Contains(VoltAABB other)
{
return
(this.top >= other.Top &&
this.bottom <= other.Bottom &&
this.right >= other.right &&
this.left <= other.left);
}
public static VoltAABB CreateMerged(VoltAABB aabb1, VoltAABB aabb2)
{
return(new VoltAABB(
Mathf.Max(aabb1.top, aabb2.top),
Mathf.Min(aabb1.bottom, aabb2.bottom),
Mathf.Min(aabb1.left, aabb2.left),
Mathf.Max(aabb1.right, aabb2.right)));
}
public static VoltAABB CreateExpanded(VoltAABB aabb, float expansionAmount)
{
return(new VoltAABB(
aabb.top + expansionAmount,
aabb.bottom - expansionAmount,
aabb.left - expansionAmount,
aabb.right + expansionAmount));
}
internal void Reset()
{
this.aabb = default(VoltAABB);
this.left = NULL_NODE;
this.right = NULL_NODE;
this.height = 0;
this.parentOrNext = NULL_NODE;
this.body = null;
}
/// <summary>
/// Checks if an AABB overlaps with our AABB.
/// </summary>
internal bool QueryAABBOnly(
VoltAABB worldBounds,
int ticksBehind)
{
HistoryRecord record = this.GetState(ticksBehind);
// AABB check done in world space (because it keeps changing)
return(record.aabb.Intersect(worldBounds));
}
/// <summary>
/// Note: This doesn't take rounded edges into account.
/// </summary>
public bool CircleCastApprox(ref VoltRayCast ray, float radius)
{
return(VoltAABB.RayCast(
ref ray,
this.top + radius,
this.bottom - radius,
this.left - radius,
this.right + radius));
}
public bool RayCast(ref VoltRayCast ray)
{
return(VoltAABB.RayCast(
ref ray,
this.top,
this.bottom,
this.left,
this.right));
}
public bool Intersect(VoltAABB other)
{
bool outside =
this.right <= other.left ||
this.left >= other.right ||
this.bottom >= other.top ||
this.top <= other.bottom;
return(outside == false);
}
private void InsertLeaf(int leafId)
{
if (this.rootId == NULL_NODE)
{
this.rootId = leafId;
this.nodes[this.rootId].parentOrNext = NULL_NODE;
return;
}
// Find the best sibling for this node
Node leafNode = this.nodes[leafId];
VoltAABB leafAABB = leafNode.aabb;
int siblingId = this.FindBestSibling(ref leafAABB);
Node sibling = this.nodes[siblingId];
// Create a new parent
int oldParentId = sibling.parentOrNext;
int newParentId;
Node newParent = this.AllocateNode(out newParentId);
newParent.Initialize(oldParentId, sibling.height + 1);
newParent.aabb = VoltAABB.CreateMerged(leafAABB, sibling.aabb);
if (oldParentId != NULL_NODE)
{
Node oldParent = this.nodes[oldParentId];
// The sibling was not the root
if (oldParent.left == siblingId)
{
oldParent.left = newParentId;
}
else
{
oldParent.right = newParentId;
}
}
else
{
// The sibling was the root
this.rootId = newParentId;
}
newParent.left = siblingId;
newParent.right = leafId;
sibling.parentOrNext = newParentId;
leafNode.parentOrNext = newParentId;
// Walk back up the tree fixing heights and AABBs
this.FixAncestors(leafNode.parentOrNext);
}
public void QueryOverlap(
VoltAABB aabb,
VoltBuffer <VoltBody> outBuffer)
{
this.StartQuery(outBuffer);
while (this.queryStack.Count > 0)
{
Node node = this.GetNextNode();
if (node.aabb.Intersect(aabb))
{
this.ExpandNode(node, outBuffer);
}
}
}
private void FixAncestors(int index)
{
while (index != NULL_NODE)
{
index = this.Balance(index);
Node indexNode = this.nodes[index];
Node left = this.nodes[indexNode.left];
Node right = this.nodes[indexNode.right];
indexNode.aabb = VoltAABB.CreateMerged(left.aabb, right.aabb);
indexNode.height = 1 + Math.Max(left.height, right.height);
index = indexNode.parentOrNext;
}
}
private Fix64 GetCost(int index, ref VoltAABB leafAABB)
{
if (this.nodes[index].IsLeaf)
{
VoltAABB aabb =
VoltAABB.CreateMerged(leafAABB, this.nodes[index].aabb);
return(aabb.Perimeter);
}
else
{
VoltAABB aabb =
VoltAABB.CreateMerged(leafAABB, this.nodes[index].aabb);
Fix64 oldArea = this.nodes[index].aabb.Perimeter;
Fix64 newArea = aabb.Perimeter;
return(newArea - oldArea);
}
}
/// <summary>
/// Builds the AABB by combining all the shape AABBs.
/// </summary>
private void UpdateAABB()
{
float top = float.NegativeInfinity;
float right = float.NegativeInfinity;
float bottom = float.PositiveInfinity;
float left = float.PositiveInfinity;
for (int i = 0; i < this.shapeCount; i++)
{
VoltAABB aabb = this.shapes[i].AABB;
top = Mathf.Max(top, aabb.Top);
right = Mathf.Max(right, aabb.Right);
bottom = Mathf.Min(bottom, aabb.Bottom);
left = Mathf.Min(left, aabb.Left);
}
this.AABB = new VoltAABB(top, bottom, left, right);
}
/// <summary>
/// Builds the AABB by combining all the shape AABBs.
/// </summary>
private void UpdateAABB()
{
FP top = FP.NegativeInfinity;
FP right = FP.NegativeInfinity;
FP bottom = FP.PositiveInfinity;
FP left = FP.PositiveInfinity;
for (int i = 0; i < this.shapeCount; i++)
{
VoltAABB aabb = this.shapes[i].AABB;
top = TSMath.Max(top, aabb.Top);
right = TSMath.Max(right, aabb.Right);
bottom = TSMath.Min(bottom, aabb.Bottom);
left = TSMath.Min(left, aabb.Left);
}
this.AABB = new VoltAABB(top, bottom, left, right);
}
/// <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;
}
private int FindBestSibling(ref VoltAABB leafAABB)
{
int index = this.rootId;
while (this.nodes[index].IsLeaf == false)
{
Node indexNode = this.nodes[index];
int child1 = indexNode.left;
int child2 = indexNode.right;
Fix64 area = indexNode.aabb.Perimeter;
VoltAABB combinedAABB = new VoltAABB();
VoltAABB.CreateMerged(indexNode.aabb, leafAABB);
Fix64 combinedArea = combinedAABB.Perimeter;
// Cost of creating a new parent for this node and the new leaf
Fix64 cost = (Fix64)2 * combinedArea;
// Minimum cost of pushing the leaf further down the tree
Fix64 inheritanceCost = (Fix64)2 * (combinedArea - area);
Fix64 cost1 = this.GetCost(child1, ref leafAABB) + inheritanceCost;
Fix64 cost2 = this.GetCost(child2, ref leafAABB) + inheritanceCost;
// Descend according to the minimum cost.
if ((cost < cost1) && (cost1 < cost2))
{
break;
}
// Descend
if (cost1 < cost2)
{
index = child1;
}
else
{
index = child2;
}
}
return(index);
}
/// <summary>
/// Builds the AABB by combining all the shape AABBs.
/// </summary>
private void UpdateAABB()
{
Fix64 top = Fix64.MinValue;
Fix64 right = Fix64.MaxValue;
Fix64 bottom = Fix64.MaxValue;
Fix64 left = Fix64.MinValue;
for (int i = 0; i < this.shapeCount; i++)
{
VoltAABB aabb = this.shapes[i].AABB;
top = VoltMath.Max(top, aabb.Top);
right = VoltMath.Max(right, aabb.Right);
bottom = VoltMath.Min(bottom, aabb.Bottom);
left = VoltMath.Min(left, aabb.Left);
}
this.AABB = new VoltAABB(top, bottom, left, right);
}
protected virtual void Reset()
{
#if DEBUG
this.IsInitialized = false;
#endif
this.UserData = null;
this.Body = null;
this.Density = 0.0f;
this.Friction = 0.0f;
this.Restitution = 0.0f;
this.Area = 0.0f;
this.Mass = 0.0f;
this.Inertia = 0.0f;
this.bodySpaceAABB = default(VoltAABB);
this.worldSpaceAABB = default(VoltAABB);
}
private int FindBestSibling(ref VoltAABB leafAABB)
{
int index = this.rootId;
while (this.nodes[index].IsLeaf == false)
{
Node indexNode = this.nodes[index];
int child1 = indexNode.left;
int child2 = indexNode.right;
float area = indexNode.aabb.Perimeter;
VoltAABB combinedAABB = new VoltAABB();
VoltAABB.CreateMerged(indexNode.aabb, leafAABB);
float combinedArea = combinedAABB.Perimeter;
// Cost of creating a new parent for this node and the new leaf
float cost = 2.0f * combinedArea;
// Minimum cost of pushing the leaf further down the tree
float inheritanceCost = 2.0f * (combinedArea - area);
float cost1 = this.GetCost(child1, ref leafAABB) + inheritanceCost;
float cost2 = this.GetCost(child2, ref leafAABB) + inheritanceCost;
// Descend according to the minimum cost.
if ((cost < cost1) && (cost1 < cost2))
break;
// Descend
if (cost1 < cost2)
index = child1;
else
index = child2;
}
return index;
}
public bool Contains(VoltAABB other)
{
return
this.top >= other.Top &&
this.bottom <= other.Bottom &&
this.right >= other.right &&
this.left <= other.left;
}
internal void Reset()
{
this.aabb = default(VoltAABB);
this.left = NULL_NODE;
this.right = NULL_NODE;
this.height = 0;
this.parentOrNext = NULL_NODE;
this.body = null;
}
private float GetCost(int index, ref VoltAABB leafAABB)
{
if (this.nodes[index].IsLeaf)
{
VoltAABB aabb =
VoltAABB.CreateMerged(leafAABB, this.nodes[index].aabb);
return aabb.Perimeter;
}
else
{
VoltAABB aabb =
VoltAABB.CreateMerged(leafAABB, this.nodes[index].aabb);
float oldArea = this.nodes[index].aabb.Perimeter;
float newArea = aabb.Perimeter;
return newArea - oldArea;
}
}
internal void Store(ref HistoryRecord other)
{
this.aabb = other.aabb;
this.position = other.position;
this.facing = other.facing;
}
/// <summary>
/// Finds all dynamic bodies that overlap with the explosion AABB
/// and pass the target filter test. Does not test actual shapes.
/// </summary>
private void PopulateFiltered(
Vector2 origin,
float radius,
VoltBodyFilter targetFilter,
int ticksBehind,
ref VoltBuffer<VoltBody> filterBuffer)
{
if (filterBuffer == null)
filterBuffer = new VoltBuffer<VoltBody>();
filterBuffer.Clear();
this.reusableBuffer.Clear();
this.staticBroadphase.QueryCircle(origin, radius, this.reusableBuffer);
this.dynamicBroadphase.QueryCircle(origin, radius, this.reusableBuffer);
VoltAABB aabb = new VoltAABB(origin, radius);
for (int i = 0; i < this.reusableBuffer.Count; i++)
{
VoltBody body = this.reusableBuffer[i];
if ((targetFilter == null) || targetFilter.Invoke(body))
if (body.QueryAABBOnly(aabb, ticksBehind))
filterBuffer.Add(body);
}
}
public static VoltAABB CreateMerged(VoltAABB aabb1, VoltAABB aabb2)
{
return new VoltAABB(
Mathf.Max(aabb1.top, aabb2.top),
Mathf.Min(aabb1.bottom, aabb2.bottom),
Mathf.Min(aabb1.left, aabb2.left),
Mathf.Max(aabb1.right, aabb2.right));
}
public static VoltAABB CreateExpanded(VoltAABB aabb, float expansionAmount)
{
return new VoltAABB(
aabb.top + expansionAmount,
aabb.bottom - expansionAmount,
aabb.left - expansionAmount,
aabb.right + expansionAmount);
}
public static VoltAABB CreateSwept(VoltAABB source, Vector2 vector)
{
float top = source.top;
float bottom = source.bottom;
float left = source.left;
float right = source.right;
if (vector.x < 0.0f)
left += vector.x;
else
right += vector.x;
if (vector.y < 0.0f)
bottom += vector.y;
else
top += vector.y;
return new VoltAABB(top, bottom, left, right);
}
public void QueryOverlap(
VoltAABB aabb,
VoltBuffer<VoltBody> outBuffer)
{
outBuffer.Add(this.bodies, this.count);
}
public void QueryOverlap(
VoltAABB aabb,
VoltBuffer <VoltBody> outBuffer)
{
outBuffer.Add(this.bodies, this.count);
}
public void QueryOverlap(
VoltAABB aabb,
VoltBuffer<VoltBody> outBuffer)
{
this.StartQuery(outBuffer);
while (this.queryStack.Count > 0)
{
Node node = this.GetNextNode();
if (node.aabb.Intersect(aabb))
this.ExpandNode(node, outBuffer);
}
}
internal void Store(ref HistoryRecord other)
{
this.aabb = other.aabb;
this.position = other.position;
this.facing = other.facing;
}
public static void Draw(VoltAABB aabb)
{
aabb.GizmoDraw(
new Color(1.0f, 0.0f, 0.5f, 1.0f)); // AABB Color
}
public bool Intersect(VoltAABB other)
{
bool outside =
this.right <= other.left ||
this.left >= other.right ||
this.bottom >= other.top ||
this.top <= other.bottom;
return (outside == false);
}
