private Node AllocateNode(out int nodeId)
{
// Expand the node pool as needed
if (this.freeList == NULL_NODE)
{
VoltDebug.Assert(this.nodeCount == this.nodeCapacity);
// The free list is empty -- rebuild a bigger pool
Node[] oldNodes = this.nodes;
this.nodeCapacity = VoltUtil.ExpandArray(ref this.nodes);
Array.Copy(oldNodes, this.nodes, this.nodeCount);
// Build a linked list for the free list
// The parent pointer becomes the "next" pointer
for (int i = this.nodeCount; i < this.nodeCapacity - 1; ++i)
{
this.nodes[i] = new Node(i + 1, -1);
}
this.nodes[this.nodeCapacity - 1] = new Node(NULL_NODE, -1);
this.freeList = this.nodeCount;
}
// Peel a node off the free list.
nodeId = this.freeList;
Node result = this.nodes[nodeId];
this.freeList = result.parentOrNext;
result.Reset();
this.nodeCount++;
return(result);
}
/// <summary>
/// Perform a left or right rotation if node A is imbalanced.
/// </summary>
private int Balance(int iA)
{
VoltDebug.Assert(iA != NULL_NODE);
Node A = this.nodes[iA];
if (A.IsLeaf || A.height < 2)
{
return(iA);
}
int iB = A.left;
int iC = A.right;
VoltDebug.Assert(0 <= iB && iB < this.nodeCapacity);
VoltDebug.Assert(0 <= iC && iC < this.nodeCapacity);
Node B = this.nodes[iB];
Node C = this.nodes[iC];
int balance = C.height - B.height;
if (balance > 1) // Rotate C up
{
return(this.Rotate(A, B, C, iA, iC, false));
}
if (balance < -1) // Rotate B up
{
return(this.Rotate(A, C, B, iA, iB, true));
}
return(iA);
}
/// <summary>
/// Full reset. Clears out all data for pooling. Call FreeShapes() first.
/// </summary>
private void Reset()
{
VoltDebug.Assert(this.shapeCount == 0);
#if DEBUG
this.IsInitialized = false;
#endif
this.UserData = null;
this.World = null;
this.BodyType = VoltBodyType.Invalid;
this.CollisionFilter = null;
this.Angle = 0.0f;
this.LinearVelocity = TSVector2.zero;
this.AngularVelocity = 0.0f;
this.Force = TSVector2.zero;
this.Torque = 0.0f;
this.Mass = 0.0f;
this.Inertia = 0.0f;
this.InvMass = 0.0f;
this.InvInertia = 0.0f;
this.BiasVelocity = TSVector2.zero;
this.BiasRotation = 0.0f;
this.history = null;
this.currentState = default(HistoryRecord);
}
private void Initialize(
TSVector2 position,
FP radians,
VoltShape[] shapesToAdd)
{
this.Position = position;
this.Angle = radians;
this.Facing = VoltMath.Polar(radians);
#if DEBUG
for (int i = 0; i < shapesToAdd.Length; i++)
{
VoltDebug.Assert(shapesToAdd[i].IsInitialized);
}
#endif
if ((this.shapes == null) || (this.shapes.Length < shapesToAdd.Length))
{
this.shapes = new VoltShape[shapesToAdd.Length];
}
Array.Copy(shapesToAdd, this.shapes, shapesToAdd.Length);
this.shapeCount = shapesToAdd.Length;
for (int i = 0; i < this.shapeCount; i++)
{
this.shapes[i].AssignBody(this);
}
#if DEBUG
this.IsInitialized = true;
#endif
}
/// <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;
}
public void Deallocate(T obj)
{
VoltDebug.Assert(obj.Pool == this);
obj.Reset();
obj.Pool = null; // Prevent multiple frees
this.freeList.Push(obj);
}
/// <summary>
/// Removes a body from the world. The body will be partially reset so it
/// can be added later. The pointer is still valid and the body can be
/// returned to the world using AddBody.
/// </summary>
public void RemoveBody(VoltBody body)
{
VoltDebug.Assert(body.World == this);
body.PartialReset();
this.RemoveBodyInternal(body);
}
/// <summary>
/// Removes a body from the world and deallocates it. The pointer is
/// invalid after this point.
/// </summary>
public void DestroyBody(VoltBody body)
{
VoltDebug.Assert(body.World == this);
body.FreeShapes();
this.RemoveBodyInternal(body);
this.FreeBody(body);
}
private void FreeNode(int nodeId)
{
VoltDebug.Assert((0 <= nodeId) && (nodeId < this.nodeCapacity));
VoltDebug.Assert(0 < this.nodeCount);
this.nodes[nodeId].Initialize(this.freeList, -1);
this.freeList = nodeId;
this.nodeCount--;
}
/// <summary>
/// Adds a body to the world. Used for reintroducing bodies that
/// have been removed. For new bodies, use CreateBody.
/// </summary>
public void AddBody(
VoltBody body,
VoltVector2 position,
Fix64 radians)
{
#if DEBUG
VoltDebug.Assert(body.IsInitialized);
#endif
VoltDebug.Assert(body.World == null);
this.AddBodyInternal(body);
body.Set(position, radians);
}
/// <summary>
/// Removes a body from the tree.
/// </summary>
public void RemoveBody(VoltBody body)
{
int proxyId = body.ProxyId;
VoltDebug.Assert((0 <= proxyId) && (proxyId < this.nodeCapacity));
VoltDebug.Assert(this.nodes[proxyId].IsLeaf);
this.RemoveLeaf(proxyId);
this.FreeNode(proxyId);
body.ProxyId = TreeBroadphase.NULL_NODE;
}
/// <summary>
/// Identifies collisions for a single body. Does not keep track of
/// symmetrical duplicates (they could be counted twice).
/// </summary>
private void BroadPhase(VoltBody query, bool collideDynamic = false)
{
VoltDebug.Assert(query.IsStatic == false);
this.reusableBuffer.Clear();
this.staticBroadphase.QueryOverlap(query.AABB, this.reusableBuffer);
if (collideDynamic)
{
this.dynamicBroadphase.QueryOverlap(query.AABB, this.reusableBuffer);
}
this.TestBuffer(query);
}
private int Rotate(
Node P,
Node Q,
Node R,
int iP,
int iR,
bool left)
{
int iX = R.left;
int iY = R.right;
Node X = this.nodes[iX];
Node Y = this.nodes[iY];
VoltDebug.Assert((0 <= iX) && (iX < this.nodeCapacity));
VoltDebug.Assert((0 <= iY) && (iY < this.nodeCapacity));
// Swap P and R
R.left = iP;
R.parentOrNext = P.parentOrNext;
P.parentOrNext = iR;
// P's old parent should point to R
if (R.parentOrNext != NULL_NODE)
{
if (this.nodes[R.parentOrNext].left == iP)
{
this.nodes[R.parentOrNext].left = iR;
}
else
{
VoltDebug.Assert(this.nodes[R.parentOrNext].right == iP);
this.nodes[R.parentOrNext].right = iR;
}
}
else
{
this.rootId = iR;
}
// Rotate
if (X.height > Y.height)
{
this.UpdateRotated(P, Q, R, iP, iX, iY, left);
}
else
{
this.UpdateRotated(P, Q, R, iP, iY, iX, left);
}
return(iR);
}
/// <summary>
/// Compute the height of a sub-tree.
/// </summary>
private int ComputeHeight(int nodeId)
{
VoltDebug.Assert((0 <= nodeId) && (nodeId < this.nodeCapacity));
Node node = this.nodes[nodeId];
if (node.IsLeaf)
{
return(0);
}
int height1 = ComputeHeight(node.left);
int height2 = ComputeHeight(node.right);
return(1 + Math.Max(height1, height2));
}
/// <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;
}
/// <summary>
/// Removes the element by swapping it for the last element in the list.
/// </summary>
public void Remove(T value)
{
int index = value.Index;
VoltDebug.Assert(index >= 0);
VoltDebug.Assert(index < this.count);
int lastIndex = this.count - 1;
if (index < lastIndex)
{
T lastValue = this.values[lastIndex];
this.values[lastIndex].Index = -1;
this.values[lastIndex] = null;
this.values[index] = lastValue;
lastValue.Index = index;
}
this.count--;
}
public void RemoveBody(VoltBody body)
{
int index = body.ProxyId;
VoltDebug.Assert(index >= 0);
VoltDebug.Assert(index < this.count);
int lastIndex = this.count - 1;
if (index < lastIndex)
{
VoltBody lastBody = this.bodies[lastIndex];
this.bodies[lastIndex].ProxyId = -1;
this.bodies[lastIndex] = null;
this.bodies[index] = lastBody;
lastBody.ProxyId = index;
}
this.count--;
}
private void ExpandNode(Node node, VoltBuffer <VoltBody> outBuffer)
{
VoltDebug.Assert(node.IsLeaf == false);
this.ExpandChild(node.left, outBuffer);
this.ExpandChild(node.right, outBuffer);
}
/// <summary>
/// Initializes the buffer for storing past body states/spaces.
/// </summary>
internal void AssignHistory(HistoryBuffer history)
{
VoltDebug.Assert(this.IsStatic == false);
this.history = history;
}