public override b2AABB ComputeAABB(b2Transform transform, int childIndex)
{
b2Vec2 p = transform.p + b2Math.b2Mul(transform.q, m_p);
b2AABB aabb = new b2AABB();
aabb.lowerBound.Set(p.x - m_radius, p.y - m_radius);
aabb.upperBound.Set(p.x + m_radius, p.y + m_radius);
return (aabb);
}
/// Combine an AABB into this one.
public void Combine(ref b2AABB aabb)
{
lowerBound = Utils.b2Min(lowerBound, aabb.lowerBound);
upperBound = Utils.b2Max(upperBound, aabb.upperBound);
}
public override void ComputeAABB(out b2AABB output, ref b2Transform xf, int childIndex)
{
b2Vec2 v1;
v1.x = (xf.q.c * Vertex1.x - xf.q.s * Vertex1.y) + xf.p.x;
v1.y = (xf.q.s * Vertex1.x + xf.q.c * Vertex1.y) + xf.p.y;
b2Vec2 v2;
v2.x = (xf.q.c * Vertex2.x - xf.q.s * Vertex2.y) + xf.p.x;
v2.y = (xf.q.s * Vertex2.x + xf.q.c * Vertex2.y) + xf.p.y;
b2Vec2 lower;
lower.x = v1.x < v2.x ? v1.x : v2.x;
lower.y = v1.y < v2.y ? v1.y : v2.y;
//b2Math.b2Min(v1, v2);
b2Vec2 upper;
upper.x = v1.x > v2.x ? v1.x : v2.x;
upper.y = v1.y > v2.y ? v1.y : v2.y;
// = b2Math.b2Max(v1, v2);
output.LowerBound = lower;
output.UpperBound = upper;
output.Fatten(Radius);
}
/**
* Given a transform, compute the associated axis aligned bounding box for this shape.
* @param aabb returns the axis aligned box.
* @param xf the world transform of the shape.
*/
virtual public void ComputeAABB(b2AABB aabb, b2Transform xf) {}
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(b2AABB obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
public override b2AABB ComputeAABB(b2Transform xf, int childIndex)
{
b2Vec2 v1 = b2Math.b2Mul(xf, m_vertex1);
b2Vec2 v2 = b2Math.b2Mul(xf, m_vertex2);
b2Vec2 lower = b2Math.b2Min(v1, v2);
b2Vec2 upper = b2Math.b2Max(v1, v2);
b2Vec2 r = new b2Vec2(m_radius, m_radius);
b2AABB aabb = new b2AABB();
aabb.lowerBound = lower - r;
aabb.upperBound = upper + r;
return(aabb);
}
public override void ComputeAABB(out b2AABB output, ref b2Transform xf, int childIndex)
{
#if false
b2Vec2 lower = b2Math.b2Mul(ref xf, ref Vertices[0]);
b2Vec2 upper = lower;
for (int i = 1; i < m_vertexCount; ++i)
{
b2Vec2 v = b2Math.b2Mul(ref xf, ref Vertices[i]);
lower = b2Math.b2Min(lower, v);
upper = b2Math.b2Max(upper, v);
}
b2AABB aabb = b2AABB.Default;
aabb.Set(lower, upper);
aabb.Fatten(Radius);
return(aabb);
#else
var vert = Vertices[0];
b2Vec2 lower;
lower.x = (xf.q.c * vert.x - xf.q.s * vert.y) + xf.p.x;
lower.y = (xf.q.s * vert.x + xf.q.c * vert.y) + xf.p.y;
b2Vec2 upper = lower;
for (int i = 1; i < m_vertexCount; ++i)
{
var vetr2 = Vertices[i];
b2Vec2 v;
v.x = (xf.q.c * vetr2.x - xf.q.s * vetr2.y) + xf.p.x;
v.y = (xf.q.s * vetr2.x + xf.q.c * vetr2.y) + xf.p.y;
lower.x = Math.Min(lower.x, v.x);
lower.y = Math.Min(lower.y, v.y);
upper.x = Math.Max(upper.x, v.x);
upper.y = Math.Max(upper.y, v.y);
}
output.LowerBound = lower;
output.UpperBound = upper;
output.Fatten(Radius);
#endif
}
/// Given a transform, compute the associated axis aligned bounding box for a child shape. /// @param aabb returns the axis aligned box. /// @param xf the world transform of the shape. /// @param childIndex the child shape public abstract void ComputeAABB(out b2AABB output, ref b2Transform xf, int childIndex);
/// Given a transform, compute the associated axis aligned bounding box for a child shape. /// @param aabb returns the axis aligned box. /// @param xf the world transform of the shape. /// @param childIndex the child shape public abstract void ComputeAABB(ref b2AABB aabb, b2Transform xf, int childIndex);
private void InsertLeaf(int leaf)
{
++m_insertionCount;
if (m_root == Settings.b2_nullNode)
{
m_root = leaf;
m_nodes[m_root].parentOrNext = Settings.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(ref m_nodes[index].aabb, ref leafAABB);
float combinedArea = combinedAABB.GetPerimeter();
// Cost of creating a new parentOrNext 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(ref leafAABB, ref m_nodes[child1].aabb);
cost1 = aabb.GetPerimeter() + inheritanceCost;
}
else
{
b2AABB aabb = new b2AABB();
aabb.Combine(ref leafAABB, ref 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(ref leafAABB, ref m_nodes[child2].aabb);
cost2 = aabb.GetPerimeter() + inheritanceCost;
}
else
{
b2AABB aabb = new b2AABB();
aabb.Combine(ref leafAABB, ref 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 parentOrNext.
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 != Settings.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 != Settings.b2_nullNode)
{
index = Balance(index);
int child1 = m_nodes[index].child1;
int child2 = m_nodes[index].child2;
Debug.Assert(child1 != Settings.b2_nullNode);
Debug.Assert(child2 != Settings.b2_nullNode);
m_nodes[index].height = 1 + Utils.b2Max(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();
}
/// Build an optimal tree. Very expensive. For testing.
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 = Settings.b2_nullNode;
nodes[count] = i;
++count;
}
else
{
FreeNode(i);
}
}
while (count > 1)
{
float minCost = float.MaxValue;
int iMin = -1;
int 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(ref aabbi, ref 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 parentOrNext = m_nodes[parentIndex];
parentOrNext.child1 = index1;
parentOrNext.child2 = index2;
parentOrNext.height = 1 + Utils.b2Max(child1.height, child2.height);
parentOrNext.aabb.Combine(ref child1.aabb, ref child2.aabb);
parentOrNext.parentOrNext = Settings.b2_nullNode;
child1.parentOrNext = parentIndex;
child2.parentOrNext = parentIndex;
nodes[jMin] = nodes[count - 1];
nodes[iMin] = parentIndex;
--count;
}
m_root = nodes[0];
nodes = null;
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.
public void RayCast(b2BroadphaseRayCastCallback callback, b2RayCastInput input)
{
b2Vec2 p1 = new b2Vec2(input.p1);
b2Vec2 p2 = new b2Vec2(input.p2);
b2Vec2 r = p2 - p1;
Debug.Assert(r.LengthSquared() > 0.0f);
r.Normalize();
// v is perpendicular to the segment.
b2Vec2 v = Utils.b2Cross(1.0f, r);
b2Vec2 abs_v = Utils.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 = Utils.b2Min(p1, t);
segmentAABB.upperBound = Utils.b2Max(p1, t);
}
Stack <int> stack = new Stack <int>(256);
stack.Push(m_root);
while (stack.Count > 0)
{
int nodeId = stack.Pop();
if (nodeId == Settings.b2_nullNode)
{
continue;
}
b2TreeNode node = m_nodes[nodeId];
if (Utils.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 = Utils.b2Abs(Utils.b2Dot(v, p1 - c)) - Utils.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(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 = Utils.b2Min(p1, t);
segmentAABB.upperBound = Utils.b2Max(p1, t);
}
}
else
{
stack.Push(node.child1);
stack.Push(node.child2);
}
}
}
public b2AABB GetAABB(int childIndex)
{
b2AABB ret = new b2AABB(Box2dPINVOKE.b2Fixture_GetAABB(swigCPtr, childIndex), false);
return(ret);
}
public OverlapShapeInputNative()
{
transform = Physics2DNative.GetNewTransform(Vector2.Zero, 0f);
aabb = new b2AABB();
}
/// Combine two AABBs into this one.
public void Combine(ref b2AABB aabb1, ref b2AABB aabb2)
{
lowerBound = Utils.b2Min(aabb1.lowerBound, aabb2.lowerBound);
upperBound = Utils.b2Max(aabb1.upperBound, aabb2.upperBound);
}
/// Given a transform, compute the associated axis aligned bounding box for a child shape. /// @param aabb returns the axis aligned box. /// @param xf the world transform of the shape. /// @param childIndex the child shape public abstract void ComputeAABB(out b2AABB output, ref b2Transform xf, int childIndex);
public override b2AABB ComputeAABB(b2Transform xf, int childIndex)
{
b2Vec2 lower = b2Math.b2Mul(xf, m_vertices[0]);
b2Vec2 upper = lower;
for (int i = 1; i < m_vertexCount; ++i)
{
b2Vec2 v = b2Math.b2Mul(xf, m_vertices[i]);
lower = b2Math.b2Min(lower, v);
upper = b2Math.b2Max(upper, v);
}
b2Vec2 r = new b2Vec2(m_radius, m_radius);
b2AABB aabb = new b2AABB();
aabb.m_lowerBound = lower - r;
aabb.m_upperBound = upper + r;
return(aabb);
}
public b2AABB GetFatAABB(int proxyId)
{
b2AABB ret = new b2AABB(Box2DPINVOKE.b2DynamicTree_GetFatAABB(swigCPtr, proxyId), false);
return(ret);
}
public override void ComputeAABB(out b2AABB output, ref b2Transform transform, int childIndex)
{
b2Vec2 p;
p.x = transform.p.x + transform.q.c * Position.x - transform.q.s * Position.y;
p.y = transform.p.y + transform.q.s * Position.x + transform.q.c * Position.y;
output.LowerBound.x = p.x - Radius;
output.LowerBound.y = p.y - Radius;
output.UpperBound.x = p.x + Radius;
output.UpperBound.y = p.y + Radius;
}
public override void ComputeAABB(out b2AABB output, ref b2Transform xf, int childIndex)
{
int i1 = childIndex;
int i2 = childIndex + 1;
if (i2 == Count)
{
i2 = 0;
}
b2Vec2 v1 = b2Math.b2Mul(ref xf, ref Vertices[i1]);
b2Vec2 v2 = b2Math.b2Mul(ref xf, ref Vertices[i2]);
b2Math.b2Min(ref v1, ref v2, out output.LowerBound);
b2Math.b2Max(ref v1, ref v2, out output.UpperBound);
}
public void DrawAABB(b2AABB aabb, b2Color c)
{
Gl.glColor3f(c.r, c.g, c.b);
Gl.glBegin(Gl.GL_LINE_LOOP);
Gl.glVertex2f(aabb.lowerBound.x, aabb.lowerBound.y);
Gl.glVertex2f(aabb.upperBound.x, aabb.lowerBound.y);
Gl.glVertex2f(aabb.upperBound.x, aabb.upperBound.y);
Gl.glVertex2f(aabb.lowerBound.x, aabb.upperBound.y);
Gl.glEnd();
}
public override b2AABB ComputeAABB(b2Transform xf, int childIndex)
{
int i1 = childIndex;
int i2 = childIndex + 1;
if (i2 == m_count)
{
i2 = 0;
}
b2Vec2 v1 = b2Math.b2Mul(xf, m_vertices[i1]);
b2Vec2 v2 = b2Math.b2Mul(xf, m_vertices[i2]);
b2AABB aabb = new b2AABB();
aabb.lowerBound = b2Math.b2Min(v1, v2);
aabb.upperBound = b2Math.b2Max(v1, v2);
return (aabb);
}
/// Query an AABB for overlapping proxies. The callback class
/// is called for each proxy that overlaps the supplied AABB.
public void Query(b2BroadphaseQueryCallback callback, b2AABB aabb)
{
m_tree.Query(callback, aabb);
}
