void b2DynamicTree::DestroyProxy(int32 proxyId)
{
b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
b2Assert(m_nodes[proxyId].IsLeaf());
RemoveLeaf(proxyId);
FreeNode(proxyId);
}
void b2DynamicTree::DestroyProxy(uint16 proxyId)
{
b2Assert(proxyId < m_nodeCount);
b2Assert(m_nodes[proxyId].IsLeaf());
RemoveLeaf(proxyId);
FreeNode(proxyId);
}
bool b2DynamicTree::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement)
{
b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
b2Assert(m_nodes[proxyId].IsLeaf());
if (m_nodes[proxyId].aabb.Contains(aabb))
{
return false;
}
RemoveLeaf(proxyId);
// Extend AABB.
b2AABB b = aabb;
b2Vec2 r(b2_aabbExtension, b2_aabbExtension);
b.lowerBound = b.lowerBound - r;
b.upperBound = b.upperBound + r;
// Predict AABB displacement.
b2Vec2 d = b2_aabbMultiplier * displacement;
if (d.x < 0.0f)
{
b.lowerBound.x += d.x;
}
else
{
b.upperBound.x += d.x;
}
if (d.y < 0.0f)
{
b.lowerBound.y += d.y;
}
else
{
b.upperBound.y += d.y;
}
m_nodes[proxyId].aabb = b;
printf(" moved_aabb: %.2f %.2f to %.2f %.2f\n",
b.lowerBound.x, b.lowerBound.y,
b.upperBound.x, b.upperBound.y);
InsertLeaf(proxyId);
return true;
}
void b2DynamicTree::MoveProxy(uint16 proxyId, const b2AABB& aabb)
{
b2Assert(proxyId < m_nodeCount);
b2Assert(m_nodes[proxyId].IsLeaf());
if (m_nodes[proxyId].aabb.Contains(aabb))
{
return;
}
RemoveLeaf(proxyId);
b2Vec2 center = aabb.GetCenter();
b2Vec2 extents = b2_fatAABBFactor * aabb.GetExtents();
m_nodes[proxyId].aabb.lowerBound = center - extents;
m_nodes[proxyId].aabb.upperBound = center + extents;
InsertLeaf(proxyId);
}
void b2DynamicTree::Rebalance(int32 iterations)
{
if (m_root == b2_nullNode)
{
return;
}
for (int32 i = 0; i < iterations; ++i)
{
int32 node = m_root;
uint32 bit = 0;
while (m_nodes[node].IsLeaf() == false)
{
int32* children = &m_nodes[node].child1;
node = children[(m_path >> bit) & 1];
bit = (bit + 1) & (8* sizeof(uint32) - 1);
}
++m_path;
RemoveLeaf(node);
InsertLeaf(node);
}
}
