public void GetAABB(out AABB aabb, int childIndex)
{
Debug.Assert(0 <= childIndex && childIndex < this.ProxyCount);
aabb = this.Proxies[childIndex].AABB;
}
/// <summary> /// Given a transform, compute the associated axis aligned bounding box for a child shape. /// </summary> /// <param name="aabb">The aabb results.</param> /// <param name="transform">The world transform of the shape.</param> /// <param name="childIndex">The child shape index.</param> public abstract void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex);
public void RebuildBottomUp()
{
int[] array = new int[this._nodeCount];
int i = 0;
for (int j = 0; j < this._nodeCapacity; j++)
{
bool flag = this._nodes[j].Height < 0;
if (!flag)
{
bool flag2 = this._nodes[j].IsLeaf();
if (flag2)
{
this._nodes[j].ParentOrNext = -1;
array[i] = j;
i++;
}
else
{
this.FreeNode(j);
}
}
}
while (i > 1)
{
FP y = Settings.MaxFP;
int num = -1;
int num2 = -1;
for (int k = 0; k < i; k++)
{
AABB aABB = this._nodes[array[k]].AABB;
for (int l = k + 1; l < i; l++)
{
AABB aABB2 = this._nodes[array[l]].AABB;
AABB aABB3 = default(AABB);
aABB3.Combine(ref aABB, ref aABB2);
FP perimeter = aABB3.Perimeter;
bool flag3 = perimeter < y;
if (flag3)
{
num = k;
num2 = l;
y = perimeter;
}
}
}
int num3 = array[num];
int num4 = array[num2];
TreeNode <T> treeNode = this._nodes[num3];
TreeNode <T> treeNode2 = this._nodes[num4];
int num5 = this.AllocateNode();
TreeNode <T> treeNode3 = this._nodes[num5];
treeNode3.Child1 = num3;
treeNode3.Child2 = num4;
treeNode3.Height = 1 + Math.Max(treeNode.Height, treeNode2.Height);
treeNode3.AABB.Combine(ref treeNode.AABB, ref treeNode2.AABB);
treeNode3.ParentOrNext = -1;
treeNode.ParentOrNext = num5;
treeNode2.ParentOrNext = num5;
array[num2] = array[i - 1];
array[num] = num5;
i--;
}
this._root = array[0];
this.Validate();
}
private void InsertLeaf(int leaf)
{
bool flag = this._root == -1;
if (flag)
{
this._root = leaf;
this._nodes[this._root].ParentOrNext = -1;
}
else
{
AABB aABB = this._nodes[leaf].AABB;
int num = this._root;
while (!this._nodes[num].IsLeaf())
{
int child = this._nodes[num].Child1;
int child2 = this._nodes[num].Child2;
FP perimeter = this._nodes[num].AABB.Perimeter;
AABB aABB2 = default(AABB);
aABB2.Combine(ref this._nodes[num].AABB, ref aABB);
FP perimeter2 = aABB2.Perimeter;
FP x = 2f * perimeter2;
FP y = 2f * (perimeter2 - perimeter);
bool flag2 = this._nodes[child].IsLeaf();
FP fP;
if (flag2)
{
AABB aABB3 = default(AABB);
aABB3.Combine(ref aABB, ref this._nodes[child].AABB);
fP = aABB3.Perimeter + y;
}
else
{
AABB aABB4 = default(AABB);
aABB4.Combine(ref aABB, ref this._nodes[child].AABB);
FP perimeter3 = this._nodes[child].AABB.Perimeter;
FP perimeter4 = aABB4.Perimeter;
fP = perimeter4 - perimeter3 + y;
}
bool flag3 = this._nodes[child2].IsLeaf();
FP y2;
if (flag3)
{
AABB aABB5 = default(AABB);
aABB5.Combine(ref aABB, ref this._nodes[child2].AABB);
y2 = aABB5.Perimeter + y;
}
else
{
AABB aABB6 = default(AABB);
aABB6.Combine(ref aABB, ref this._nodes[child2].AABB);
FP perimeter5 = this._nodes[child2].AABB.Perimeter;
FP perimeter6 = aABB6.Perimeter;
y2 = perimeter6 - perimeter5 + y;
}
bool flag4 = x < fP && fP < y2;
if (flag4)
{
break;
}
bool flag5 = fP < y2;
if (flag5)
{
num = child;
}
else
{
num = child2;
}
}
int num2 = num;
int parentOrNext = this._nodes[num2].ParentOrNext;
int num3 = this.AllocateNode();
this._nodes[num3].ParentOrNext = parentOrNext;
this._nodes[num3].UserData = default(T);
this._nodes[num3].AABB.Combine(ref aABB, ref this._nodes[num2].AABB);
this._nodes[num3].Height = this._nodes[num2].Height + 1;
bool flag6 = parentOrNext != -1;
if (flag6)
{
bool flag7 = this._nodes[parentOrNext].Child1 == num2;
if (flag7)
{
this._nodes[parentOrNext].Child1 = num3;
}
else
{
this._nodes[parentOrNext].Child2 = num3;
}
this._nodes[num3].Child1 = num2;
this._nodes[num3].Child2 = leaf;
this._nodes[num2].ParentOrNext = num3;
this._nodes[leaf].ParentOrNext = num3;
}
else
{
this._nodes[num3].Child1 = num2;
this._nodes[num3].Child2 = leaf;
this._nodes[num2].ParentOrNext = num3;
this._nodes[leaf].ParentOrNext = num3;
this._root = num3;
}
for (num = this._nodes[leaf].ParentOrNext; num != -1; num = this._nodes[num].ParentOrNext)
{
num = this.Balance(num);
int child3 = this._nodes[num].Child1;
int child4 = this._nodes[num].Child2;
Debug.Assert(child3 != -1);
Debug.Assert(child4 != -1);
this._nodes[num].Height = 1 + Math.Max(this._nodes[child3].Height, this._nodes[child4].Height);
this._nodes[num].AABB.Combine(ref this._nodes[child3].AABB, ref this._nodes[child4].AABB);
}
}
}
public void RayCast(Func <RayCastInput, int, FP> callback, ref RayCastInput input)
{
TSVector2 point = input.Point1;
TSVector2 point2 = input.Point2;
TSVector2 tSVector = point2 - point;
Debug.Assert(tSVector.LengthSquared() > 0f);
tSVector.Normalize();
TSVector2 value = MathUtils.Abs(new TSVector2(-tSVector.y, tSVector.x));
FP fP = input.MaxFraction;
AABB aABB = default(AABB);
TSVector2 tSVector2 = point + fP * (point2 - point);
TSVector2.Min(ref point, ref tSVector2, out aABB.LowerBound);
TSVector2.Max(ref point, ref tSVector2, out aABB.UpperBound);
this._raycastStack.Clear();
this._raycastStack.Push(this._root);
while (this._raycastStack.Count > 0)
{
int num = this._raycastStack.Pop();
bool flag = num == -1;
if (!flag)
{
TreeNode <T> treeNode = this._nodes[num];
bool flag2 = !AABB.TestOverlap(ref treeNode.AABB, ref aABB);
if (!flag2)
{
TSVector2 center = treeNode.AABB.Center;
TSVector2 extents = treeNode.AABB.Extents;
FP x = FP.Abs(TSVector2.Dot(new TSVector2(-tSVector.y, tSVector.x), point - center)) - TSVector2.Dot(value, extents);
bool flag3 = x > 0f;
if (!flag3)
{
bool flag4 = treeNode.IsLeaf();
if (flag4)
{
RayCastInput arg;
arg.Point1 = input.Point1;
arg.Point2 = input.Point2;
arg.MaxFraction = fP;
FP fP2 = callback(arg, num);
bool flag5 = fP2 == 0f;
if (flag5)
{
break;
}
bool flag6 = fP2 > 0f;
if (flag6)
{
fP = fP2;
TSVector2 value2 = point + fP * (point2 - point);
aABB.LowerBound = TSVector2.Min(point, value2);
aABB.UpperBound = TSVector2.Max(point, value2);
}
}
else
{
this._raycastStack.Push(treeNode.Child1);
this._raycastStack.Push(treeNode.Child2);
}
}
}
}
}
}
public void GetFatAABB(int proxyId, out AABB fatAABB)
{
Debug.Assert(0 <= proxyId && proxyId < this._nodeCapacity);
fatAABB = this._nodes[proxyId].AABB;
}
