/**
* Query an AABB for overlapping proxies. The callback
* is called for each proxy that overlaps the supplied AABB.
* The callback should match function signature
* <code>fuction callback(proxy:b2DynamicTreeNode):Boolean</code>
* and should return false to trigger premature termination.
*/
public void Query(BroadPhaseQueryCallback callback, b2AABB aabb)
{
if (m_root == null)
{
return;
}
Dictionary <int, b2DynamicTreeNode> stack = new Dictionary <int, b2DynamicTreeNode>();
int count = 0;
stack.Add(count++, m_root);
while (count > 0)
{
b2DynamicTreeNode node = stack[--count];
if (node.aabb.TestOverlap(aabb))
{
if (node.IsLeaf())
{
bool proceed = callback(node);
if (!proceed)
{
return;
}
}
else
{
// No stack limit, so no assert
int key;
key = count++;
if (stack.ContainsKey(key))
{
stack.Remove(key);
}
stack.Add(key, node.child1);
key = count++;
if (stack.ContainsKey(key))
{
stack.Remove(key);
}
stack.Add(key, node.child2);
}
}
}
}
/**
* Perform some iterations to re-balance the tree.
*/
public void Rebalance(int iterations)
{
if (m_root == null)
{
return;
}
for (int i = 0; i < iterations; i++)
{
b2DynamicTreeNode node = m_root;
uint bit = 0;
while (node.IsLeaf() == false)
{
node = (((int)m_path >> (int)bit) & 1) > 0 ? node.child2 : node.child1;
bit = (bit + 1) & 31; // 0-31 bits in a uint
}
++m_path;
RemoveLeaf(node);
InsertLeaf(node);
}
}
/**
* Move a proxy with a swept AABB. If the proxy has moved outside of its fattened AABB,
* then the proxy is removed from the tree and re-inserted. Otherwise
* the function returns immediately.
*/
public bool MoveProxy(b2DynamicTreeNode proxy, b2AABB aabb, b2Vec2 displacement)
{
b2Settings.b2Assert(proxy.IsLeaf());
if (proxy.aabb.Contains(aabb))
{
return(false);
}
RemoveLeaf(proxy);
// Extend AABB
float extendX = b2Settings.b2_aabbExtension + b2Settings.b2_aabbMultiplier * (displacement.x > 0?displacement.x: -displacement.x);
float extendY = b2Settings.b2_aabbExtension + b2Settings.b2_aabbMultiplier * (displacement.y > 0?displacement.y: -displacement.y);
proxy.aabb.lowerBound.x = aabb.lowerBound.x - extendX;
proxy.aabb.lowerBound.y = aabb.lowerBound.y - extendY;
proxy.aabb.upperBound.x = aabb.upperBound.x + extendX;
proxy.aabb.upperBound.y = aabb.upperBound.y + extendY;
InsertLeaf(proxy);
return(true);
}
private void InsertLeaf(b2DynamicTreeNode leaf)
{
++m_insertionCount;
if (m_root == null)
{
m_root = leaf;
m_root.parent = null;
return;
}
b2Vec2 center = leaf.aabb.GetCenter();
b2DynamicTreeNode sibling = m_root;
if (sibling.IsLeaf() == false)
{
do
{
b2DynamicTreeNode child1 = sibling.child1;
b2DynamicTreeNode child2 = sibling.child2;
//b2Vec2 delta1 = b2Abs(m_nodes[child1].aabb.GetCenter() - center);
//b2Vec2 delta2 = b2Abs(m_nodes[child2].aabb.GetCenter() - center);
//float32 norm1 = delta1.x + delta1.y;
//float32 norm2 = delta2.x + delta2.y;
float norm1 = Mathf.Abs((child1.aabb.lowerBound.x + child1.aabb.upperBound.x) / 2.0f - center.x)
+ Mathf.Abs((child1.aabb.lowerBound.y + child1.aabb.upperBound.y) / 2.0f - center.y);
float norm2 = Mathf.Abs((child2.aabb.lowerBound.x + child2.aabb.upperBound.x) / 2.0f - center.x)
+ Mathf.Abs((child2.aabb.lowerBound.y + child2.aabb.upperBound.y) / 2.0f - center.y);
if (norm1 < norm2)
{
sibling = child1;
}
else
{
sibling = child2;
}
}while (sibling.IsLeaf() == false);
}
// Create a parent for the siblings
b2DynamicTreeNode node1 = sibling.parent;
b2DynamicTreeNode node2 = AllocateNode();
node2.parent = node1;
node2.userData = null;
node2.aabb.Combine(leaf.aabb, sibling.aabb);
if (node1 != null)
{
if (sibling.parent.child1 == sibling)
{
node1.child1 = node2;
}
else
{
node1.child2 = node2;
}
node2.child1 = sibling;
node2.child2 = leaf;
sibling.parent = node2;
leaf.parent = node2;
do
{
if (node1.aabb.Contains(node2.aabb))
{
break;
}
node1.aabb.Combine(node1.child1.aabb, node1.child2.aabb);
node2 = node1;
node1 = node1.parent;
}while (node1 != null);
}
else
{
node2.child1 = sibling;
node2.child2 = leaf;
sibling.parent = node2;
leaf.parent = node2;
m_root = node2;
}
}
/**
* Ray-cast against the proxies in the tree. This relies on the callback
* to perform a exact ray-cast in the case were the proxy contains a shape.
* The callback also performs the any collision filtering. This has performance
* roughly equal to k * log(n), where k is the number of collisions and n is the
* number of proxies in the tree.
* @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
* @param callback a callback class that is called for each proxy that is hit by the ray.
* It should be of signature:
* ----- <code>function callback(input:b2RayCastInput, proxy:*):void</code>
* <code>function callback(input:b2RayCastInput, proxy:*):Number</code>
*/
public void RayCast(BroadPhaseRayCastCallback callback, b2RayCastInput input)
{
if (m_root == null)
{
return;
}
b2Vec2 p1 = input.p1;
b2Vec2 p2 = input.p2;
b2Vec2 r = b2Math.SubtractVV(p1, p2);
//b2Settings.b2Assert(r.LengthSquared() > 0.0);
r.Normalize();
// v is perpendicular to the segment
b2Vec2 v = b2Math.CrossFV(1.0f, r);
b2Vec2 abs_v = b2Math.AbsV(v);
float maxFraction = input.maxFraction;
// Build a bounding box for the segment
b2AABB segmentAABB = new b2AABB();
float tX;
float tY;
{
tX = p1.x + maxFraction * (p2.x - p1.x);
tY = p1.y + maxFraction * (p2.y - p1.y);
segmentAABB.lowerBound.x = Mathf.Min(p1.x, tX);
segmentAABB.lowerBound.y = Mathf.Min(p1.y, tY);
segmentAABB.upperBound.x = Mathf.Max(p1.x, tX);
segmentAABB.upperBound.y = Mathf.Max(p1.y, tY);
}
Dictionary <int, b2DynamicTreeNode> stack = new Dictionary <int, b2DynamicTreeNode>();
int count = 0;
stack[count++] = m_root;
while (count > 0)
{
b2DynamicTreeNode node = stack[--count];
if (node.aabb.TestOverlap(segmentAABB) == false)
{
continue;
}
// Separating axis for segment (Gino, p80)
// |dot(v, p1 - c)| > dot(|v|,h)
b2Vec2 c = node.aabb.GetCenter();
b2Vec2 h = node.aabb.GetExtents();
float separation = Mathf.Abs(v.x * (p1.x - c.x) + v.y * (p1.y - c.y))
- abs_v.x * h.x - abs_v.y * h.y;
if (separation > 0.0f)
{
continue;
}
if (node.IsLeaf())
{
b2RayCastInput subInput = new b2RayCastInput();
subInput.p1 = input.p1;
subInput.p2 = input.p2;
//*************by kingBook 2015/10/22 16:17*************
subInput.maxFraction = maxFraction;
float value = callback(subInput, node);
if (value == 0)
{
return;
}
if (value > 0)
{
//Update the segment bounding box
maxFraction = value;
//******************************************************
tX = p1.x + maxFraction * (p2.x - p1.x);
tY = p1.y + maxFraction * (p2.y - p1.y);
segmentAABB.lowerBound.x = Mathf.Min(p1.x, tX);
segmentAABB.lowerBound.y = Mathf.Min(p1.y, tY);
segmentAABB.upperBound.x = Mathf.Max(p1.x, tX);
segmentAABB.upperBound.y = Mathf.Max(p1.y, tY);
}
}
else
{
// No stack limit, so no assert
stack[count++] = node.child1;
stack[count++] = node.child2;
}
}
}