/// Does this aabb contain the provided AABB.
public bool Contains(b2AABB aabb)
{
bool result = true;
result = result && m_lowerBound.x <= aabb.m_lowerBound.x;
result = result && m_lowerBound.y <= aabb.m_lowerBound.y;
result = result && aabb.m_upperBound.x <= m_upperBound.x;
result = result && aabb.m_upperBound.y <= m_upperBound.y;
return result;
}
public void DrawAABB(b2AABB aAabb, b2Color aColor)
{
mVertices[0] = new CCPoint(aAabb.LowerBound.x * mRatio, aAabb.LowerBound.y * mRatio);
mVertices[1] = new CCPoint(aAabb.UpperBound.x * mRatio, aAabb.LowerBound.y * mRatio);
mVertices[2] = new CCPoint(aAabb.UpperBound.x * mRatio, aAabb.UpperBound.y * mRatio);
mVertices[3] = new CCPoint(aAabb.LowerBound.x * mRatio, aAabb.UpperBound.y * mRatio);
CCDrawingPrimitives.Begin();
CCDrawingPrimitives.DrawPoly(mVertices, 8, true, new CCColor4B(aColor.r, aColor.g, aColor.b, 1));
CCDrawingPrimitives.End();
}
public virtual bool MouseDown(b2Vec2 p)
{
m_mouseWorld = p;
if (m_mouseJoint != null)
{
return false;
}
// Make a small box.
b2AABB aabb = new b2AABB();
b2Vec2 d = new b2Vec2();
d.Set(0.001f, 0.001f);
aabb.LowerBound = p - d;
aabb.UpperBound = p + d;
// Query the world for overlapping shapes.
QueryCallback callback = new QueryCallback(p);
m_world.QueryAABB(callback, aabb);
if (callback.m_fixture != null)
{
b2Body body = callback.m_fixture.Body;
b2MouseJointDef md = new b2MouseJointDef();
md.BodyA = m_groundBody;
md.BodyB = body;
md.target = p;
md.maxForce = 1000.0f * body.Mass;
m_mouseJoint = (b2MouseJoint)m_world.CreateJoint(md);
body.SetAwake(true);
return true;
}
else
{
//Como no ha encontrado un objeto empezamos el arrastre
IsArrastring = true;
originPosition = new CCPoint(p.x, p.y);
return true;
}
}
public bool Equals(b2AABB o)
{
return (LowerBound == o.LowerBound && UpperBound == o.UpperBound);
}
// Create a proxy in the tree as a leaf node. We return the index
// of the node instead of a pointer so that we can grow
// the node pool.
public int CreateProxy(b2AABB aabb, object userData)
{
int proxyId = AllocateNode();
// Fatten the aabb.
b2Vec2 r = new b2Vec2(b2Settings.b2_aabbExtension, b2Settings.b2_aabbExtension);
m_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r;
m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r;
m_nodes[proxyId].userData = userData;
m_nodes[proxyId].height = 0;
InsertLeaf(proxyId);
return proxyId;
}
public void LaunchBomb(b2Vec2 position, b2Vec2 velocity)
{
if (m_bomb != null)
{
m_world.DestroyBody(m_bomb);
m_bomb = null;
}
b2BodyDef bd = new b2BodyDef();
bd.type = b2BodyType.b2_dynamicBody;
bd.position = position;
bd.bullet = true;
m_bomb = m_world.CreateBody(bd);
m_bomb.LinearVelocity = velocity;
b2CircleShape circle = new b2CircleShape();
circle.Radius = 0.3f;
b2FixtureDef fd = new b2FixtureDef();
fd.shape = circle;
fd.density = 20.0f;
fd.restitution = 0.0f;
b2Vec2 minV = position - new b2Vec2(0.3f, 0.3f);
b2Vec2 maxV = position + new b2Vec2(0.3f, 0.3f);
b2AABB aabb = new b2AABB();
aabb.LowerBound = minV;
aabb.UpperBound = maxV;
m_bomb.CreateFixture(fd);
}
public static void Combine(ref b2AABB aabb1, ref b2AABB aabb2, out b2AABB output)
{
output.LowerBound.x = aabb1.LowerBound.x < aabb2.LowerBound.x ? aabb1.LowerBound.x : aabb2.LowerBound.x;
output.LowerBound.y = aabb1.LowerBound.y < aabb2.LowerBound.y ? aabb1.LowerBound.y : aabb2.LowerBound.y;
output.UpperBound.x = aabb1.UpperBound.x > aabb2.UpperBound.x ? aabb1.UpperBound.x : aabb2.UpperBound.x;
output.UpperBound.y = aabb1.UpperBound.y > aabb2.UpperBound.y ? aabb1.UpperBound.y : aabb2.UpperBound.y;
}
public static bool b2TestOverlap(ref b2AABB a, ref b2AABB b)
{
b2Vec2 d1, d2;
// No operator overloading here - do direct computation to reduce time complexity
d1.m_x = b.LowerBoundX - a.UpperBoundX;
d1.m_y = b.LowerBoundY - a.UpperBoundY;
d2.m_x = a.LowerBoundX - b.UpperBoundX;
d2.m_y = a.LowerBoundY - b.UpperBoundY;
// d1 = b.LowerBound - a.UpperBound;
// d2 = a.LowerBound - b.UpperBound;
if (d1.m_x > 0.0f || d1.m_y > 0.0f)
return false;
if (d2.m_x > 0.0f || d2.m_y > 0.0f)
return false;
return true;
}
public void GetFatAABB(int proxyId, out b2AABB output)
{
Debug.Assert(0 <= proxyId && proxyId < m_nodeCapacity);
output = m_nodes[proxyId].aabb;
}
public void RebuildBottomUp()
{
int[] 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;
}
if (m_nodes[i].IsLeaf())
{
m_nodes[i].parentOrNext = b2TreeNode.b2_nullNode;
nodes[count] = i;
++count;
}
else
{
FreeNode(i);
}
}
while (count > 1)
{
float minCost = b2Settings.b2_maxFloat;
int iMin = -1, jMin = -1;
for (int i = 0; i < count; ++i)
{
b2AABB aabbi = m_nodes[nodes[i]].aabb;
for (int j = i + 1; j < count; ++j)
{
b2AABB aabbj = m_nodes[nodes[j]].aabb;
b2AABB b = b2AABB.Default;
b.Combine(ref aabbi, ref aabbj);
float cost = b.Perimeter;
if (cost < minCost)
{
iMin = i;
jMin = j;
minCost = cost;
}
}
}
int index1 = nodes[iMin];
int index2 = nodes[jMin];
b2TreeNode child1 = m_nodes[index1];
b2TreeNode child2 = m_nodes[index2];
int parentIndex = AllocateNode();
b2TreeNode parent = m_nodes[parentIndex];
parent.child1 = index1;
parent.child2 = index2;
parent.height = 1 + Math.Max(child1.height, child2.height);
parent.aabb.Combine(ref child1.aabb, ref child2.aabb);
parent.parentOrNext = b2TreeNode.b2_nullNode;
child1.parentOrNext = parentIndex;
child2.parentOrNext = parentIndex;
nodes[jMin] = nodes[count - 1];
nodes[iMin] = parentIndex;
--count;
}
m_root = nodes[0];
Validate();
}
public void ValidateMetrics(int index)
{
if (index == b2TreeNode.b2_nullNode)
{
return;
}
b2TreeNode node = m_nodes[index];
int child1 = node.child1;
int child2 = node.child2;
if (node.IsLeaf())
{
Debug.Assert(child1 == b2TreeNode.b2_nullNode);
Debug.Assert(child2 == b2TreeNode.b2_nullNode);
Debug.Assert(node.height == 0);
return;
}
Debug.Assert(0 <= child1 && child1 < m_nodeCapacity);
Debug.Assert(0 <= child2 && child2 < m_nodeCapacity);
int height1 = m_nodes[child1].height;
int height2 = m_nodes[child2].height;
int height;
height = 1 + Math.Max(height1, height2);
Debug.Assert(node.height == height);
b2AABB aabb = new b2AABB();
aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
Debug.Assert(aabb.lowerBound == node.aabb.lowerBound);
Debug.Assert(aabb.upperBound == node.aabb.upperBound);
ValidateMetrics(child1);
ValidateMetrics(child2);
}
/// Combine an AABB into this one.
public void Combine(b2AABB aabb)
{
m_lowerBound = b2Math.b2Min(m_lowerBound, aabb.m_lowerBound);
m_upperBound = b2Math.b2Max(m_upperBound, aabb.m_upperBound);
}
public void InsertLeaf(int leaf)
{
++m_insertionCount;
if (m_root == b2TreeNode.b2_nullNode)
{
m_root = leaf;
m_nodes[m_root].parentOrNext = b2TreeNode.b2_nullNode;
return;
}
// Find the best sibling for this node
b2AABB leafAABB = m_nodes[leaf].aabb;
int index = m_root;
while (m_nodes[index].IsLeaf() == false)
{
int child1 = m_nodes[index].child1;
int child2 = m_nodes[index].child2;
float area = m_nodes[index].aabb.Perimeter;
b2AABB combinedAABB = b2AABB.Default;
combinedAABB.Combine(ref m_nodes[index].aabb, ref 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);
// Cost of descending into child1
float cost1;
if (m_nodes[child1].IsLeaf())
{
b2AABB aabb = b2AABB.Default;
aabb.Combine(ref leafAABB, ref m_nodes[child1].aabb);
cost1 = aabb.Perimeter + inheritanceCost;
}
else
{
b2AABB aabb = b2AABB.Default;
aabb.Combine(ref leafAABB, ref m_nodes[child1].aabb);
float oldArea = m_nodes[child1].aabb.Perimeter;
float newArea = aabb.Perimeter;
cost1 = (newArea - oldArea) + inheritanceCost;
}
// Cost of descending into child2
float cost2;
if (m_nodes[child2].IsLeaf())
{
b2AABB aabb = b2AABB.Default;
aabb.Combine(ref leafAABB, ref m_nodes[child2].aabb);
cost2 = aabb.Perimeter + inheritanceCost;
}
else
{
b2AABB aabb = b2AABB.Default;
aabb.Combine(ref leafAABB, ref m_nodes[child2].aabb);
float oldArea = m_nodes[child2].aabb.Perimeter;
float newArea = aabb.Perimeter;
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;
}
}
int sibling = index;
// Create a new parent.
int oldParent = m_nodes[sibling].parentOrNext;
int newParent = AllocateNode();
m_nodes[newParent].parentOrNext = oldParent;
m_nodes[newParent].userData = null;
m_nodes[newParent].aabb.Combine(ref leafAABB, ref m_nodes[sibling].aabb);
m_nodes[newParent].height = m_nodes[sibling].height + 1;
if (oldParent != b2TreeNode.b2_nullNode)
{
// The sibling was not the root.
if (m_nodes[oldParent].child1 == sibling)
{
m_nodes[oldParent].child1 = newParent;
}
else
{
m_nodes[oldParent].child2 = newParent;
}
m_nodes[newParent].child1 = sibling;
m_nodes[newParent].child2 = leaf;
m_nodes[sibling].parentOrNext = newParent;
m_nodes[leaf].parentOrNext = newParent;
}
else
{
// The sibling was the root.
m_nodes[newParent].child1 = sibling;
m_nodes[newParent].child2 = leaf;
m_nodes[sibling].parentOrNext = newParent;
m_nodes[leaf].parentOrNext = newParent;
m_root = newParent;
}
// Walk back up the tree fixing heights and AABBs
index = m_nodes[leaf].parentOrNext;
while (index != b2TreeNode.b2_nullNode)
{
index = Balance(index);
int child1 = m_nodes[index].child1;
int child2 = m_nodes[index].child2;
Debug.Assert(child1 != b2TreeNode.b2_nullNode);
Debug.Assert(child2 != b2TreeNode.b2_nullNode);
m_nodes[index].height = 1 + Math.Max(m_nodes[child1].height, m_nodes[child2].height);
m_nodes[index].aabb.Combine(ref m_nodes[child1].aabb, ref m_nodes[child2].aabb);
index = m_nodes[index].parentOrNext;
}
//Validate();
}
public bool MoveProxy(int proxyId, b2AABB aabb, b2Vec2 displacement)
{
Debug.Assert(0 <= proxyId && proxyId < m_nodeCapacity);
Debug.Assert(m_nodes[proxyId].IsLeaf());
if (m_nodes[proxyId].aabb.Contains(ref aabb))
{
return false;
}
RemoveLeaf(proxyId);
// Extend AABB.
b2AABB b = aabb;
b.Fatten();
// Predict AABB displacement.
b2Vec2 d = b2Settings.b2_aabbMultiplier * displacement;
if (d.x < 0.0f)
{
b.LowerBoundX += d.x;
}
else
{
b.UpperBoundX += d.x;
}
if (d.y < 0.0f)
{
b.LowerBoundY += d.y;
}
else
{
b.UpperBoundY += d.y;
}
b.UpdateAttributes();
m_nodes[proxyId].aabb = b;
InsertLeaf(proxyId);
return true;
}
// Create a proxy in the tree as a leaf node. We return the index
// of the node instead of a pointer so that we can grow
// the node pool.
public int CreateProxy(b2AABB aabb, object userData)
{
int proxyId = AllocateNode();
// Fatten the aabb.
m_nodes[proxyId].aabb = aabb;
m_nodes[proxyId].aabb.Fatten();
m_nodes[proxyId].userData = userData;
m_nodes[proxyId].height = 0;
InsertLeaf(proxyId);
return proxyId;
}
/**
* Extends or clips the segment so that it's ends lie on the boundary of the AABB
*/
public void Extend(b2AABB aabb)
{
ExtendForward(aabb);
ExtendBackward(aabb);
}
public bool MoveProxy(int proxyId, b2AABB aabb, b2Vec2 displacement)
{
Debug.Assert(0 <= proxyId && proxyId < m_nodeCapacity);
Debug.Assert(m_nodes[proxyId].IsLeaf());
if (m_nodes[proxyId].aabb.Contains(aabb))
{
return false;
}
RemoveLeaf(proxyId);
// Extend AABB.
b2AABB b = aabb;
b2Vec2 r = new b2Vec2(b2Settings.b2_aabbExtension, b2Settings.b2_aabbExtension);
b.lowerBound = b.lowerBound - r;
b.upperBound = b.upperBound + r;
// Predict AABB displacement.
b2Vec2 d = b2Settings.b2_aabbMultiplier * displacement;
if (d.x < 0.0f)
{
b.lowerBoundx += d.x;
}
else
{
b.upperBoundx += d.x;
}
if (d.y < 0.0f)
{
b.lowerBoundy += d.y;
}
else
{
b.upperBoundy += d.y;
}
m_nodes[proxyId].aabb = b;
InsertLeaf(proxyId);
return true;
}
public void RayCast(b2WorldRayCastWrapper callback, b2RayCastInput input)
{
b2Vec2 p1 = input.p1;
b2Vec2 p2 = input.p2;
b2Vec2 r = p2 - p1;
Debug.Assert(r.LengthSquared > 0.0f);
r.Normalize();
// v is perpendicular to the segment.
b2Vec2 v = r.NegUnitCross(); // b2Math.b2Cross(1.0f, r);
b2Vec2 abs_v = b2Math.b2Abs(v);
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
float maxFraction = input.maxFraction;
// Build a bounding box for the segment.
b2AABB segmentAABB = b2AABB.Default;
{
b2Vec2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.Set(b2Math.b2Min(p1, t), b2Math.b2Max(p1, t));
}
Stack <int> stack = new Stack <int>();
stack.Push(m_root);
while (stack.Count > 0)
{
int nodeId = stack.Pop();
if (nodeId == b2TreeNode.b2_nullNode)
{
continue;
}
b2TreeNode node = m_nodes[nodeId];
if (b2Collision.b2TestOverlap(ref node.aabb, ref segmentAABB) == false)
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
b2Vec2 c = node.aabb.Center;
b2Vec2 h = node.aabb.Extents;
float separation = b2Math.b2Abs(b2Math.b2Dot(v, p1 - c)) - b2Math.b2Dot(abs_v, h);
if (separation > 0.0f)
{
continue;
}
if (node.IsLeaf())
{
b2RayCastInput subInput = new b2RayCastInput();
subInput.p1 = input.p1;
subInput.p2 = input.p2;
subInput.maxFraction = maxFraction;
float value = callback.RayCastCallback(subInput, nodeId);
if (value == 0.0f)
{
// The client has terminated the ray cast.
return;
}
if (value > 0.0f)
{
// Update segment bounding box.
maxFraction = value;
b2Vec2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.Set(b2Math.b2Min(p1, t), b2Math.b2Max(p1, t));
}
}
else
{
stack.Push(node.child1);
stack.Push(node.child2);
}
}
}
public void RayCast(b2WorldRayCastWrapper callback, b2RayCastInput input)
{
b2Vec2 p1 = input.p1;
b2Vec2 p2 = input.p2;
b2Vec2 r = p2 - p1;
Debug.Assert(r.LengthSquared() > 0.0f);
r.Normalize();
// v is perpendicular to the segment.
b2Vec2 v = b2Math.b2Cross(1.0f, r);
b2Vec2 abs_v = b2Math.b2Abs(v);
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
float maxFraction = input.maxFraction;
// Build a bounding box for the segment.
b2AABB segmentAABB = new b2AABB();
{
b2Vec2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.lowerBound = b2Math.b2Min(p1, t);
segmentAABB.upperBound = b2Math.b2Max(p1, t);
}
Stack<int> stack = new Stack<int>();
stack.Push(m_root);
while (stack.Count > 0)
{
int nodeId = stack.Pop();
if (nodeId == b2TreeNode.b2_nullNode)
{
continue;
}
b2TreeNode node = m_nodes[nodeId];
if (b2Collision.b2TestOverlap(ref node.aabb, ref 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 = b2Math.b2Abs(b2Math.b2Dot(v, p1 - c)) - b2Math.b2Dot(abs_v, h);
if (separation > 0.0f)
{
continue;
}
if (node.IsLeaf())
{
b2RayCastInput subInput = new b2RayCastInput();
subInput.p1 = input.p1;
subInput.p2 = input.p2;
subInput.maxFraction = maxFraction;
float value = callback.RayCastCallback(subInput, nodeId);
if (value == 0.0f)
{
// The client has terminated the ray cast.
return;
}
if (value > 0.0f)
{
// Update segment bounding box.
maxFraction = value;
b2Vec2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.lowerBound = b2Math.b2Min(p1, t);
segmentAABB.upperBound = b2Math.b2Max(p1, t);
}
}
else
{
stack.Push(node.child1);
stack.Push(node.child2);
}
}
}
private void MoveAABB(b2AABB aabb)
{
b2Vec2 d = new b2Vec2();
d.x = Rand.RandomFloat(-0.5f, 0.5f);
d.y = Rand.RandomFloat(-0.5f, 0.5f);
//d.x = 2.0f;
//d.y = 0.0f;
aabb.LowerBound += d;
aabb.UpperBound += d;
b2Vec2 c0 = 0.5f * (aabb.LowerBound + aabb.UpperBound);
b2Vec2 min = new b2Vec2();
min.Set(-m_worldExtent, 0.0f);
b2Vec2 max = new b2Vec2();
max.Set(m_worldExtent, 2.0f * m_worldExtent);
b2Vec2 c = b2Math.b2Clamp(c0, min, max);
aabb.LowerBound += c - c0;
aabb.UpperBound += c - c0;
}
public b2Vec2 m_upperBound; //< the upper vertex
#endregion Fields
#region Methods
/// Combine an AABB into this one.
public void Combine(b2AABB aabb)
{
m_lowerBound = b2Math.b2Min(m_lowerBound, aabb.m_lowerBound);
m_upperBound = b2Math.b2Max(m_upperBound, aabb.m_upperBound);
}
public static bool b2TestOverlap(ref b2AABB a, ref b2AABB b)
{
b2Vec2 d1, d2;
d1 = b.m_lowerBound - a.m_upperBound;
d2 = a.m_lowerBound - b.m_upperBound;
if (d1.x > 0.0f || d1.y > 0.0f)
return false;
if (d2.x > 0.0f || d2.y > 0.0f)
return false;
return true;
}
/**
* Update the pairs. This results in pair callbacks. This can only add pairs.
*/
public void UpdatePairs(Action <object, object> callback)
{
m_pairCount = 0;
// Perform tree queries for all moving queries
foreach (b2DynamicTreeNode queryProxy in m_moveBuffer)
{
/*bool QueryCallback(b2DynamicTreeNode proxy)
* {
* // A proxy cannot form a pair with itself.
* if (proxy == queryProxy)
* return true;
*
* // Grow the pair buffer as needed
* if (m_pairCount == m_pairBuffer.Count)
* {
* m_pairBuffer[m_pairCount] = new b2DynamicTreePair();
* }
*
* b2DynamicTreePair pair = m_pairBuffer[m_pairCount];
* pair.proxyA = proxy < queryProxy?proxy:queryProxy;
* pair.proxyB = proxy >= queryProxy?proxy:queryProxy;
++m_pairCount;
*
* return true;
* }*/
BroadPhaseQueryCallback QueryCallback = delegate(object proxy){
// A proxy cannot form a pair with itself.
if (proxy == queryProxy)
{
return(true);
}
// Grow the pair buffer as needed
if (m_pairCount == m_pairBuffer.Count)
{
m_pairBuffer.Add(new b2DynamicTreePair());
}
b2DynamicTreePair pair = m_pairBuffer[m_pairCount];
//pair.proxyA = proxy < queryProxy?proxy:queryProxy;
//pair.proxyB = proxy >= queryProxy?proxy:queryProxy;
//改
pair.proxyA = queryProxy;
pair.proxyB = (b2DynamicTreeNode)proxy;
++m_pairCount;
return(true);
};
// We have to query the tree with the fat AABB so that
// we don't fail to create a pair that may touch later.
b2AABB fatAABB = m_tree.GetFatAABB(queryProxy);
m_tree.Query(QueryCallback, fatAABB);
}
// Reset move buffer
//m_moveBuffer.length = 0;
m_moveBuffer.RemoveRange(0, m_moveBuffer.Count);
// Sort the pair buffer to expose duplicates.
// TODO: Something more sensible
//m_pairBuffer.sort(ComparePairs);
// Send the pair buffer
for (int i = 0; i < m_pairCount;)
{
b2DynamicTreePair primaryPair = m_pairBuffer[i];
object userDataA = m_tree.GetUserData(primaryPair.proxyA);
object userDataB = m_tree.GetUserData(primaryPair.proxyB);
callback(userDataA, userDataB);
++i;
// Skip any duplicate pairs
while (i < m_pairCount)
{
b2DynamicTreePair pair = m_pairBuffer[i];
if (pair.proxyA != primaryPair.proxyA || pair.proxyB != primaryPair.proxyB)
{
break;
}
++i;
}
}
}
/// Combine two AABBs into this one.
public void Combine(b2AABB aabb1, b2AABB aabb2)
{
m_lowerBound = b2Math.b2Min(aabb1.m_lowerBound, aabb2.m_lowerBound);
m_upperBound = b2Math.b2Max(aabb1.m_upperBound, aabb2.m_upperBound);
}
public static bool b2TestOverlap(ref b2AABB a, ref b2AABB b)
{
// No operator overloading here - do direct computation to reduce time complexity
if (b.LowerBound.x - a.UpperBound.x > 0.0f || b.LowerBound.y - a.UpperBound.y > 0.0f)
return false;
if (a.LowerBound.x - b.UpperBound.x > 0.0f || a.LowerBound.y - b.UpperBound.y > 0.0f)
return false;
return true;
}
public bool Equals(b2AABB o)
{
return(LowerBound == o.LowerBound && UpperBound == o.UpperBound);
}
public virtual bool MouseDown(b2Vec2 p)
{
m_mouseWorld = p;
if (m_mouseJoint != null)
{
return false;
}
// Make a small box.
b2AABB aabb = new b2AABB();
b2Vec2 d = new b2Vec2();
d.Set(0.001f, 0.001f);
aabb.LowerBound = p - d;
aabb.UpperBound = p + d;
// Query the world for overlapping shapes.
QueryCallback callback = new QueryCallback(p);
m_world.QueryAABB(callback, aabb);
if (callback.m_fixture != null)
{
b2Body body = callback.m_fixture.Body;
b2MouseJointDef md = new b2MouseJointDef();
md.BodyA = m_groundBody;
md.BodyB = body;
md.target = p;
md.maxForce = 1000.0f * body.Mass;
m_mouseJoint = (b2MouseJoint) m_world.CreateJoint(md);
body.SetAwake(true);
return true;
}
return false;
}
public override bool Equals(object obj)
{
b2AABB o = (b2AABB)obj;
return(LowerBound == o.LowerBound && UpperBound == o.UpperBound);
}
public void Combine(ref b2AABB aabb1, ref b2AABB aabb2)
{
LowerBound.x = aabb1.LowerBound.x < aabb2.LowerBound.x ? aabb1.LowerBound.x : aabb2.LowerBound.x;
LowerBound.y = aabb1.LowerBound.y < aabb2.LowerBound.y ? aabb1.LowerBound.y : aabb2.LowerBound.y;
UpperBound.x = aabb1.UpperBound.x > aabb2.UpperBound.x ? aabb1.UpperBound.x : aabb2.UpperBound.x;
UpperBound.y = aabb1.UpperBound.y > aabb2.UpperBound.y ? aabb1.UpperBound.y : aabb2.UpperBound.y;
}
public void Combine(ref b2AABB aabb)
{
m_lowerBound.x = aabb.LowerBoundX < LowerBoundX ? aabb.LowerBoundX : LowerBoundX;
m_lowerBound.y = aabb.LowerBoundY < LowerBoundY ? aabb.LowerBoundY : LowerBoundY;
m_upperBound.x = aabb.UpperBoundX > UpperBoundX ? aabb.UpperBoundX : UpperBoundX;
m_upperBound.y = aabb.UpperBoundY > UpperBoundY ? aabb.UpperBoundY : UpperBoundY;
//m_lowerBound = b2Math.b2Min(m_lowerBound, aabb.LowerBound);
//m_upperBound = b2Math.b2Max(m_upperBound, aabb.UpperBound);
_Dirty = true;
}
public bool Contains(ref b2AABB aabb)
{
bool result = true;
result = result && LowerBound.x <= aabb.LowerBoundX;
if (result)
result = result && LowerBound.y <= aabb.LowerBoundY;
if (result)
result = result && aabb.UpperBoundX <= UpperBound.x;
if (result)
result = result && aabb.UpperBoundY <= UpperBound.y;
return result;
}
public void Combine(ref b2AABB aabb1, ref b2AABB aabb2)
{
m_lowerBound.x = aabb1.LowerBoundX < aabb2.LowerBoundX ? aabb1.LowerBoundX : aabb2.LowerBoundX;
m_lowerBound.y = aabb1.LowerBoundY < aabb2.LowerBoundY ? aabb1.LowerBoundY : aabb2.LowerBoundY;
m_upperBound.x = aabb1.UpperBoundX > aabb2.UpperBoundX ? aabb1.UpperBoundX : aabb2.UpperBoundX;
m_upperBound.y = aabb1.UpperBoundY > aabb2.UpperBoundY ? aabb1.UpperBoundY : aabb2.UpperBoundY;
// m_lowerBound = b2Math.b2Min(aabb1.LowerBound, aabb2.LowerBound);
// m_upperBound = b2Math.b2Max(aabb1.UpperBound, aabb2.UpperBound);
_Dirty = true;
}
public void Combine(ref b2AABB aabb)
{
LowerBound.x = aabb.LowerBound.x < LowerBound.x ? aabb.LowerBound.x : LowerBound.x;
LowerBound.y = aabb.LowerBound.y < LowerBound.y ? aabb.LowerBound.y : LowerBound.y;
UpperBound.x = aabb.UpperBound.x > UpperBound.x ? aabb.UpperBound.x : UpperBound.x;
UpperBound.y = aabb.UpperBound.y > UpperBound.y ? aabb.UpperBound.y : UpperBound.y;
}
public bool Equals(b2AABB o)
{
return (m_lowerBound == o.LowerBound && m_upperBound == o.UpperBound);
}
public void InsertLeaf(int leaf)
{
++m_insertionCount;
if (m_root == b2TreeNode.b2_nullNode)
{
m_root = leaf;
m_nodes[m_root].parentOrNext = b2TreeNode.b2_nullNode;
return;
}
// Find the best sibling for this node
b2AABB leafAABB = m_nodes[leaf].aabb;
int index = m_root;
while (m_nodes[index].IsLeaf() == false)
{
int child1 = m_nodes[index].child1;
int child2 = m_nodes[index].child2;
float area = m_nodes[index].aabb.GetPerimeter();
b2AABB combinedAABB = new b2AABB();
combinedAABB.Combine(m_nodes[index].aabb, leafAABB);
float combinedArea = combinedAABB.GetPerimeter();
// 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);
// Cost of descending into child1
float cost1;
if (m_nodes[child1].IsLeaf())
{
b2AABB aabb = new b2AABB();
aabb.Combine(leafAABB, m_nodes[child1].aabb);
cost1 = aabb.GetPerimeter() + inheritanceCost;
}
else
{
b2AABB aabb = new b2AABB();
aabb.Combine(leafAABB, m_nodes[child1].aabb);
float oldArea = m_nodes[child1].aabb.GetPerimeter();
float newArea = aabb.GetPerimeter();
cost1 = (newArea - oldArea) + inheritanceCost;
}
// Cost of descending into child2
float cost2;
if (m_nodes[child2].IsLeaf())
{
b2AABB aabb = new b2AABB();
aabb.Combine(leafAABB, m_nodes[child2].aabb);
cost2 = aabb.GetPerimeter() + inheritanceCost;
}
else
{
b2AABB aabb = new b2AABB();
aabb.Combine(leafAABB, m_nodes[child2].aabb);
float oldArea = m_nodes[child2].aabb.GetPerimeter();
float newArea = aabb.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;
}
}
int sibling = index;
// Create a new parent.
int oldParent = m_nodes[sibling].parentOrNext;
int newParent = AllocateNode();
m_nodes[newParent].parentOrNext = oldParent;
m_nodes[newParent].userData = null;
m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb);
m_nodes[newParent].height = m_nodes[sibling].height + 1;
if (oldParent != b2TreeNode.b2_nullNode)
{
// The sibling was not the root.
if (m_nodes[oldParent].child1 == sibling)
{
m_nodes[oldParent].child1 = newParent;
}
else
{
m_nodes[oldParent].child2 = newParent;
}
m_nodes[newParent].child1 = sibling;
m_nodes[newParent].child2 = leaf;
m_nodes[sibling].parentOrNext = newParent;
m_nodes[leaf].parentOrNext = newParent;
}
else
{
// The sibling was the root.
m_nodes[newParent].child1 = sibling;
m_nodes[newParent].child2 = leaf;
m_nodes[sibling].parentOrNext = newParent;
m_nodes[leaf].parentOrNext = newParent;
m_root = newParent;
}
// Walk back up the tree fixing heights and AABBs
index = m_nodes[leaf].parentOrNext;
while (index != b2TreeNode.b2_nullNode)
{
index = Balance(index);
int child1 = m_nodes[index].child1;
int child2 = m_nodes[index].child2;
Debug.Assert(child1 != b2TreeNode.b2_nullNode);
Debug.Assert(child2 != b2TreeNode.b2_nullNode);
m_nodes[index].height = 1 + Math.Max(m_nodes[child1].height, m_nodes[child2].height);
m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
index = m_nodes[index].parentOrNext;
}
//Validate();
}
/**
* 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;
}
}
}
public void Query(Ib2QueryCallback w, b2AABB aabb)
{
Stack<int> stack = new Stack<int>();
stack.Push(m_root);
while (stack.Count > 0)
{
int nodeId = stack.Pop();
if (nodeId == b2TreeNode.b2_nullNode)
{
continue;
}
b2TreeNode node = m_nodes[nodeId];
if (b2Collision.b2TestOverlap(ref node.aabb, ref aabb))
{
if (node.IsLeaf())
{
bool proceed = w.QueryCallback(nodeId);
if (proceed == false)
{
return;
}
}
else
{
if(node.child1 != b2TreeNode.b2_nullNode)
stack.Push(node.child1);
if (node.child2 != b2TreeNode.b2_nullNode)
stack.Push(node.child2);
}
}
}
}
public void GetFatAABB(int proxyId, out b2AABB output)
{
m_tree.GetFatAABB(proxyId, out output);
}
public void RebuildBottomUp()
{
int[] 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;
}
if (m_nodes[i].IsLeaf())
{
m_nodes[i].parentOrNext = b2TreeNode.b2_nullNode;
nodes[count] = i;
++count;
}
else
{
FreeNode(i);
}
}
while (count > 1)
{
float minCost = b2Settings.b2_maxFloat;
int iMin = -1, jMin = -1;
for (int i = 0; i < count; ++i)
{
b2AABB aabbi = m_nodes[nodes[i]].aabb;
for (int j = i + 1; j < count; ++j)
{
b2AABB aabbj = m_nodes[nodes[j]].aabb;
b2AABB b = new b2AABB();
b.Combine(aabbi, aabbj);
float cost = b.GetPerimeter();
if (cost < minCost)
{
iMin = i;
jMin = j;
minCost = cost;
}
}
}
int index1 = nodes[iMin];
int index2 = nodes[jMin];
b2TreeNode child1 = m_nodes[index1];
b2TreeNode child2 = m_nodes[index2];
int parentIndex = AllocateNode();
b2TreeNode parent = m_nodes[parentIndex];
parent.child1 = index1;
parent.child2 = index2;
parent.height = 1 + Math.Max(child1.height, child2.height);
parent.aabb.Combine(child1.aabb, child2.aabb);
parent.parentOrNext = b2TreeNode.b2_nullNode;
child1.parentOrNext = parentIndex;
child2.parentOrNext = parentIndex;
nodes[jMin] = nodes[count - 1];
nodes[iMin] = parentIndex;
--count;
}
m_root = nodes[0];
Validate();
}
public virtual void Query(Ib2QueryCallback q, b2AABB aabb)
{
m_tree.Query(q, aabb);
}
public void DrawAABB(b2AABB aabb, b2Color p1)
{
b2Vec2[] verts = new b2Vec2[4];
verts[0] = new b2Vec2(aabb.LowerBound.x, aabb.LowerBound.y);
verts[1] = new b2Vec2(aabb.UpperBound.x, aabb.LowerBound.y);
verts[2] = new b2Vec2(aabb.UpperBound.x, aabb.UpperBound.y);
verts[3] = new b2Vec2(aabb.LowerBound.x, aabb.UpperBound.y);
DrawPolygon(verts, 4, p1);
}
public void Query <T>(T callback, b2AABB aabb)
{
m_tree.Query(callback, aabb);
}
/// Combine two AABBs into this one.
public void Combine(b2AABB aabb1, b2AABB aabb2)
{
m_lowerBound = b2Math.b2Min(aabb1.m_lowerBound, aabb2.m_lowerBound);
m_upperBound = b2Math.b2Max(aabb1.m_upperBound, aabb2.m_upperBound);
}
public bool Equals(b2AABB o)
{
return(m_lowerBound == o.LowerBound && m_upperBound == o.UpperBound);
}
private void GetRandomAABB(ref b2AABB aabb)
{
b2Vec2 w = new b2Vec2();
w.Set(2.0f * m_proxyExtent, 2.0f * m_proxyExtent);
//aabb->lowerBound.x = -m_proxyExtent;
//aabb->lowerBound.y = -m_proxyExtent + m_worldExtent;
aabb.LowerBoundX = Rand.RandomFloat(-m_worldExtent, m_worldExtent);
aabb.LowerBoundY = Rand.RandomFloat(0.0f, 2.0f * m_worldExtent);
aabb.UpperBound = aabb.LowerBound + w;
}
/**
* @inheritDoc
*/
public void Query(BroadPhaseQueryCallback callback, b2AABB aabb)
{
m_tree.Query(callback, aabb);
}
