private void validateMetrics(DynamicTreeNode node)
{
if (node == null)
{
return;
}
DynamicTreeNode child1 = node.child1;
DynamicTreeNode child2 = node.child2;
if (node.isLeaf())
{
return;
}
int height1 = child1.height;
int height2 = child2.height;
int height;
height = 1 + MathUtils.max(height1, height2);
var aabb = new AABB();
aabb.combine(child1.aabb, child2.aabb);
validateMetrics(child1);
validateMetrics(child2);
}
private int computeHeight(DynamicTreeNode node)
{
if (node.isLeaf())
{
return(0);
}
int height1 = computeHeight(node.child1);
int height2 = computeHeight(node.child2);
return(1 + MathUtils.max(height1, height2));
}
private void validateStructure(DynamicTreeNode node)
{
if (node == null)
{
return;
}
if (node == m_root)
{
}
DynamicTreeNode child1 = node.child1;
DynamicTreeNode child2 = node.child2;
if (node.isLeaf())
{
return;
}
validateStructure(child1);
validateStructure(child2);
}
// Perform a left or right rotation if node A is imbalanced.
// Returns the new root index.
private DynamicTreeNode balance(DynamicTreeNode iA)
{
DynamicTreeNode A = iA;
if (A.isLeaf() || A.height < 2)
{
return(iA);
}
DynamicTreeNode iB = A.child1;
DynamicTreeNode iC = A.child2;
DynamicTreeNode B = iB;
DynamicTreeNode C = iC;
int balance = C.height - B.height;
// Rotate C up
if (balance > 1)
{
DynamicTreeNode iF = C.child1;
DynamicTreeNode iG = C.child2;
DynamicTreeNode F = iF;
DynamicTreeNode G = iG;
// Swap A and C
C.child1 = iA;
C.parent = A.parent;
A.parent = iC;
// A's old parent should point to C
if (C.parent != null)
{
if (C.parent.child1 == iA)
{
C.parent.child1 = iC;
}
else
{
C.parent.child2 = iC;
}
}
else
{
m_root = iC;
}
// Rotate
if (F.height > G.height)
{
C.child2 = iF;
A.child2 = iG;
G.parent = iA;
A.aabb.combine(B.aabb, G.aabb);
C.aabb.combine(A.aabb, F.aabb);
A.height = 1 + MathUtils.max(B.height, G.height);
C.height = 1 + MathUtils.max(A.height, F.height);
}
else
{
C.child2 = iG;
A.child2 = iF;
F.parent = iA;
A.aabb.combine(B.aabb, F.aabb);
C.aabb.combine(A.aabb, G.aabb);
A.height = 1 + MathUtils.max(B.height, F.height);
C.height = 1 + MathUtils.max(A.height, G.height);
}
return(iC);
}
// Rotate B up
if (balance < -1)
{
DynamicTreeNode iD = B.child1;
DynamicTreeNode iE = B.child2;
DynamicTreeNode D = iD;
DynamicTreeNode E = iE;
// Swap A and B
B.child1 = iA;
B.parent = A.parent;
A.parent = iB;
// A's old parent should point to B
if (B.parent != null)
{
if (B.parent.child1 == iA)
{
B.parent.child1 = iB;
}
else
{
B.parent.child2 = iB;
}
}
else
{
m_root = iB;
}
// Rotate
if (D.height > E.height)
{
B.child2 = iD;
A.child1 = iE;
E.parent = iA;
A.aabb.combine(C.aabb, E.aabb);
B.aabb.combine(A.aabb, D.aabb);
A.height = 1 + MathUtils.max(C.height, E.height);
B.height = 1 + MathUtils.max(A.height, D.height);
}
else
{
B.child2 = iE;
A.child1 = iD;
D.parent = iA;
A.aabb.combine(C.aabb, D.aabb);
B.aabb.combine(A.aabb, E.aabb);
A.height = 1 + MathUtils.max(C.height, D.height);
B.height = 1 + MathUtils.max(A.height, E.height);
}
return(iB);
}
return(iA);
}
private void insertLeaf(int leaf_index)
{
m_insertionCount++;
DynamicTreeNode leaf = m_nodes[leaf_index];
if (m_root == null)
{
m_root = leaf;
m_root.parent = null;
return;
}
// find the best sibling
AABB leafAABB = leaf.aabb;
DynamicTreeNode index = m_root;
while (index.child1 != null)
{
DynamicTreeNode node = index;
DynamicTreeNode child1 = node.child1;
DynamicTreeNode child2 = node.child2;
double area = node.aabb.getPerimeter();
combinedAABB.combine(node.aabb, leafAABB);
double combinedArea = combinedAABB.getPerimeter();
// Cost of creating a new parent for this node and the new leaf
double cost = 2.0d * combinedArea;
// Minimum cost of pushing the leaf further down the tree
double inheritanceCost = 2.0d * (combinedArea - area);
// Cost of descending into child1
double cost1;
if (child1.isLeaf())
{
combinedAABB.combine(leafAABB, child1.aabb);
cost1 = combinedAABB.getPerimeter() + inheritanceCost;
}
else
{
combinedAABB.combine(leafAABB, child1.aabb);
double oldArea = child1.aabb.getPerimeter();
double newArea = combinedAABB.getPerimeter();
cost1 = (newArea - oldArea) + inheritanceCost;
}
// Cost of descending into child2
double cost2;
if (child2.isLeaf())
{
combinedAABB.combine(leafAABB, child2.aabb);
cost2 = combinedAABB.getPerimeter() + inheritanceCost;
}
else
{
combinedAABB.combine(leafAABB, child2.aabb);
double oldArea = child2.aabb.getPerimeter();
double newArea = combinedAABB.getPerimeter();
cost2 = newArea - oldArea + inheritanceCost;
}
// Descend according to the minimum cost.
if (cost < cost1 && cost < cost2)
{
break;
}
// Descend
if (cost1 < cost2)
{
index = child1;
}
else
{
index = child2;
}
}
DynamicTreeNode sibling = index;
DynamicTreeNode oldParent = m_nodes[sibling.id].parent;
DynamicTreeNode newParent = allocateNode();
newParent.parent = oldParent;
newParent.userData = null;
newParent.aabb.combine(leafAABB, sibling.aabb);
newParent.height = sibling.height + 1;
if (oldParent != null)
{
// The sibling was not the root.
if (oldParent.child1 == sibling)
{
oldParent.child1 = newParent;
}
else
{
oldParent.child2 = newParent;
}
newParent.child1 = sibling;
newParent.child2 = leaf;
sibling.parent = newParent;
leaf.parent = newParent;
}
else
{
// The sibling was the root.
newParent.child1 = sibling;
newParent.child2 = leaf;
sibling.parent = newParent;
leaf.parent = newParent;
m_root = newParent;
}
// Walk back up the tree fixing heights and AABBs
index = leaf.parent;
while (index != null)
{
index = balance(index);
DynamicTreeNode child1 = index.child1;
DynamicTreeNode child2 = index.child2;
index.height = 1 + MathUtils.max(child1.height, child2.height);
index.aabb.combine(child1.aabb, child2.aabb);
index = index.parent;
}
// validate();
}
/**
* Build an optimal tree. Very expensive. For testing.
*/
public void rebuildBottomUp()
{
var nodes = new int[m_nodeCount];
int count = 0;
// Build array of leaves. Free the rest.
for (int i = 0; i < m_nodeCapacity; ++i)
{
if (m_nodes[i].height < 0)
{
// free node in pool
continue;
}
DynamicTreeNode node = m_nodes[i];
if (node.isLeaf())
{
node.parent = null;
nodes[count] = i;
++count;
}
else
{
freeNode(node);
}
}
var b = new AABB();
while (count > 1)
{
double minCost = double.MaxValue;
int iMin = -1, jMin = -1;
for (int i = 0; i < count; ++i)
{
AABB aabbi = m_nodes[nodes[i]].aabb;
for (int j = i + 1; j < count; ++j)
{
AABB aabbj = m_nodes[nodes[j]].aabb;
b.combine(aabbi, aabbj);
double cost = b.getPerimeter();
if (cost < minCost)
{
iMin = i;
jMin = j;
minCost = cost;
}
}
}
int index1 = nodes[iMin];
int index2 = nodes[jMin];
DynamicTreeNode child1 = m_nodes[index1];
DynamicTreeNode child2 = m_nodes[index2];
DynamicTreeNode parent = allocateNode();
parent.child1 = child1;
parent.child2 = child2;
parent.height = 1 + MathUtils.max(child1.height, child2.height);
parent.aabb.combine(child1.aabb, child2.aabb);
parent.parent = null;
child1.parent = parent;
child2.parent = parent;
nodes[jMin] = nodes[count - 1];
nodes[iMin] = parent.id;
--count;
}
m_root = m_nodes[nodes[0]];
validate();
}
public void raycast(TreeRayCastCallback callback, RayCastInput input)
{
Vec2 p1 = input.p1;
Vec2 p2 = input.p2;
double p1x = p1.x, p2x = p2.x, p1y = p1.y, p2y = p2.y;
double vx, vy;
double rx, ry;
double absVx, absVy;
double cx, cy;
double hx, hy;
double tempx, tempy;
r.x = p2x - p1x;
r.y = p2y - p1y;
r.normalize();
rx = r.x;
ry = r.y;
// v is perpendicular to the segment.
vx = -1d * ry;
vy = 1d * rx;
absVx = MathUtils.abs(vx);
absVy = MathUtils.abs(vy);
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
double maxFraction = input.maxFraction;
// Build a bounding box for the segment.
AABB segAABB = aabb;
// Vec2 t = p1 + maxFraction * (p2 - p1);
// before inline
// temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
// Vec2.minToOut(p1, temp, segAABB.lowerBound);
// Vec2.maxToOut(p1, temp, segAABB.upperBound);
tempx = (p2x - p1x) * maxFraction + p1x;
tempy = (p2y - p1y) * maxFraction + p1y;
segAABB.lowerBound.x = p1x < tempx ? p1x : tempx;
segAABB.lowerBound.y = p1y < tempy ? p1y : tempy;
segAABB.upperBound.x = p1x > tempx ? p1x : tempx;
segAABB.upperBound.y = p1y > tempy ? p1y : tempy;
// end inline
nodeStack.reset();
nodeStack.push(m_root);
while (nodeStack.getCount() > 0)
{
DynamicTreeNode node = nodeStack.pop();
if (node == null)
{
continue;
}
AABB nodeAABB = node.aabb;
if (!AABB.testOverlap(nodeAABB, segAABB))
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
// node.aabb.getCenterToOut(c);
// node.aabb.getExtentsToOut(h);
cx = (nodeAABB.lowerBound.x + nodeAABB.upperBound.x) * .5d;
cy = (nodeAABB.lowerBound.y + nodeAABB.upperBound.y) * .5d;
hx = (nodeAABB.upperBound.x - nodeAABB.lowerBound.x) * .5d;
hy = (nodeAABB.upperBound.y - nodeAABB.lowerBound.y) * .5d;
tempx = p1x - cx;
tempy = p1y - cy;
double separation = MathUtils.abs(vx * tempx + vy * tempy) - (absVx * hx + absVy * hy);
if (separation > 0.0d)
{
continue;
}
if (node.isLeaf())
{
subInput.p1.x = p1x;
subInput.p1.y = p1y;
subInput.p2.x = p2x;
subInput.p2.y = p2y;
subInput.maxFraction = maxFraction;
double value = callback.raycastCallback(subInput, node.id);
if (value == 0.0d)
{
// The client has terminated the ray cast.
return;
}
if (value > 0.0d)
{
// Update segment bounding box.
maxFraction = value;
// temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
// Vec2.minToOut(p1, temp, segAABB.lowerBound);
// Vec2.maxToOut(p1, temp, segAABB.upperBound);
tempx = (p2x - p1x) * maxFraction + p1x;
tempy = (p2y - p1y) * maxFraction + p1y;
segAABB.lowerBound.x = p1x < tempx ? p1x : tempx;
segAABB.lowerBound.y = p1y < tempy ? p1y : tempy;
segAABB.upperBound.x = p1x > tempx ? p1x : tempx;
segAABB.upperBound.y = p1y > tempy ? p1y : tempy;
}
}
else
{
nodeStack.push(node.child1);
nodeStack.push(node.child2);
}
}
}