/// <summary>
/// Creates a new box containing the two given ones.
/// </summary>
/// <param name="original">First box.</param>
/// <param name="additional">Second box.</param>
/// <returns>A JBBox containing the two given boxes.</returns>
#region public static JBBox CreateMerged(JBBox original, JBBox additional)
public static FPBBox CreateMerged(FPBBox original, FPBBox additional)
{
FPBBox result;
FPBBox.CreateMerged(ref original, ref additional, out result);
return(result);
}
private void RemoveLeaf(int leaf)
{
if (leaf == _root)
{
_root = NullNode;
return;
}
int parent = _nodes[leaf].ParentOrNext;
int grandParent = _nodes[parent].ParentOrNext;
int sibling;
if (_nodes[parent].Child1 == leaf)
{
sibling = _nodes[parent].Child2;
}
else
{
sibling = _nodes[parent].Child1;
}
if (grandParent != NullNode)
{
// Destroy parent and connect sibling to grandParent.
if (_nodes[grandParent].Child1 == parent)
{
_nodes[grandParent].Child1 = sibling;
}
else
{
_nodes[grandParent].Child2 = sibling;
}
_nodes[sibling].ParentOrNext = grandParent;
FreeNode(parent);
// Adjust ancestor bounds.
parent = grandParent;
while (parent != NullNode)
{
//_nodes[parent].AABB.Combine(ref _nodes[_nodes[parent].Child1].AABB,
// ref _nodes[_nodes[parent].Child2].AABB);
FPBBox.CreateMerged(ref _nodes[_nodes[parent].Child1].AABB,
ref _nodes[_nodes[parent].Child2].AABB, out _nodes[parent].AABB);
Debug.Assert(_nodes[parent].LeafCount > 0);
_nodes[parent].LeafCount -= 1;
parent = _nodes[parent].ParentOrNext;
}
}
else
{
_root = sibling;
_nodes[sibling].ParentOrNext = NullNode;
FreeNode(parent);
}
}
protected void UpdateInternalBoundingBox()
{
mInternalBBox.min = new FPVector(FP.MaxValue);
mInternalBBox.max = new FPVector(FP.MinValue);
for (int i = 0; i < shapes.Length; i++)
{
shapes[i].UpdateBoundingBox();
FPBBox.CreateMerged(ref mInternalBBox, ref shapes[i].boundingBox, out mInternalBBox);
}
}
/// <summary>
/// Gets the axis aligned bounding box of the orientated shape. This includes
/// the whole shape.
/// </summary>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="box">The axis aligned bounding box of the shape.</param>
public override void GetBoundingBox(ref FPMatrix orientation, out FPBBox box)
{
FPBBox helpBox = FPBBox.LargeBox;
int length = this.Prepare(ref helpBox);
box = FPBBox.SmallBox;
for (int i = 0; i < length; i++)
{
this.SetCurrentShape(i);
base.GetBoundingBox(ref orientation, out helpBox);
FPBBox.CreateMerged(ref box, ref helpBox, out box);
}
}
private void InsertLeaf(int leaf)
{
++_insertionCount;
if (_root == NullNode)
{
_root = leaf;
_nodes[_root].ParentOrNext = NullNode;
return;
}
// Find the best sibling for this node
FPBBox leafAABB = _nodes[leaf].AABB;
int sibling = _root;
while (_nodes[sibling].IsLeaf() == false)
{
int child1 = _nodes[sibling].Child1;
int child2 = _nodes[sibling].Child2;
// Expand the node's AABB.
//_nodes[sibling].AABB.Combine(ref leafAABB);
FPBBox.CreateMerged(ref _nodes[sibling].AABB, ref leafAABB, out _nodes[sibling].AABB);
_nodes[sibling].LeafCount += 1;
FP siblingArea = _nodes[sibling].AABB.Perimeter;
FPBBox parentAABB = new FPBBox();
//parentAABB.Combine(ref _nodes[sibling].AABB, ref leafAABB);
FPBBox.CreateMerged(ref _nodes[sibling].AABB, ref leafAABB, out _nodes[sibling].AABB);
FP parentArea = parentAABB.Perimeter;
FP cost1 = (2 * FP.One) * parentArea;
FP inheritanceCost = (2 * FP.One) * (parentArea - siblingArea);
FP cost2;
if (_nodes[child1].IsLeaf())
{
FPBBox aabb = new FPBBox();
//aabb.Combine(ref leafAABB, ref _nodes[child1].AABB);
FPBBox.CreateMerged(ref leafAABB, ref _nodes[child1].AABB, out aabb);
cost2 = aabb.Perimeter + inheritanceCost;
}
else
{
FPBBox aabb = new FPBBox();
//aabb.Combine(ref leafAABB, ref _nodes[child1].AABB);
FPBBox.CreateMerged(ref leafAABB, ref _nodes[child1].AABB, out aabb);
FP oldArea = _nodes[child1].AABB.Perimeter;
FP newArea = aabb.Perimeter;
cost2 = (newArea - oldArea) + inheritanceCost;
}
FP cost3;
if (_nodes[child2].IsLeaf())
{
FPBBox aabb = new FPBBox();
//aabb.Combine(ref leafAABB, ref _nodes[child2].AABB);
FPBBox.CreateMerged(ref leafAABB, ref _nodes[child2].AABB, out aabb);
cost3 = aabb.Perimeter + inheritanceCost;
}
else
{
FPBBox aabb = new FPBBox();
//aabb.Combine(ref leafAABB, ref _nodes[child2].AABB);
FPBBox.CreateMerged(ref leafAABB, ref _nodes[child2].AABB, out aabb);
FP oldArea = _nodes[child2].AABB.Perimeter;
FP newArea = aabb.Perimeter;
cost3 = newArea - oldArea + inheritanceCost;
}
// Descend according to the minimum cost.
if (cost1 < cost2 && cost1 < cost3)
{
break;
}
// Expand the node's AABB to account for the new leaf.
//_nodes[sibling].AABB.Combine(ref leafAABB);
FPBBox.CreateMerged(ref leafAABB, ref _nodes[sibling].AABB, out _nodes[sibling].AABB);
// Descend
if (cost2 < cost3)
{
sibling = child1;
}
else
{
sibling = child2;
}
}
// Create a new parent for the siblings.
int oldParent = _nodes[sibling].ParentOrNext;
int newParent = AllocateNode();
_nodes[newParent].ParentOrNext = oldParent;
_nodes[newParent].UserData = default(T);
//_nodes[newParent].AABB.Combine(ref leafAABB, ref _nodes[sibling].AABB);
FPBBox.CreateMerged(ref leafAABB, ref _nodes[sibling].AABB, out _nodes[newParent].AABB);
_nodes[newParent].LeafCount = _nodes[sibling].LeafCount + 1;
if (oldParent != NullNode)
{
// The sibling was not the root.
if (_nodes[oldParent].Child1 == sibling)
{
_nodes[oldParent].Child1 = newParent;
}
else
{
_nodes[oldParent].Child2 = newParent;
}
_nodes[newParent].Child1 = sibling;
_nodes[newParent].Child2 = leaf;
_nodes[sibling].ParentOrNext = newParent;
_nodes[leaf].ParentOrNext = newParent;
}
else
{
// The sibling was the root.
_nodes[newParent].Child1 = sibling;
_nodes[newParent].Child2 = leaf;
_nodes[sibling].ParentOrNext = newParent;
_nodes[leaf].ParentOrNext = newParent;
_root = newParent;
}
}