private IEnumerable <Pair <T> > GetOverlapsImpl(Vector3F scale, AabbTree <T> otherTree, Vector3F otherScale, Pose otherPose)
{
// Compute transformations.
Vector3F scaleA = scale; // Rename scales for readability.
Vector3F scaleB = otherScale;
Pose bToA = otherPose;
#if !POOL_ENUMERABLES
var stack = DigitalRune.ResourcePools <Pair <Node, Node> > .Stacks.Obtain();
stack.Push(new Pair <Node, Node>(_root, otherTree._root));
while (stack.Count > 0)
{
var nodePair = stack.Pop();
var nodeA = nodePair.First;
var nodeB = nodePair.Second;
if (HaveAabbContact(nodeA, scaleA, nodeB, scaleB, bToA))
{
if (!nodeA.IsLeaf)
{
if (!nodeB.IsLeaf)
{
stack.Push(new Pair <Node, Node>(nodeA.RightChild, nodeB.RightChild));
stack.Push(new Pair <Node, Node>(nodeA.LeftChild, nodeB.RightChild));
stack.Push(new Pair <Node, Node>(nodeA.RightChild, nodeB.LeftChild));
stack.Push(new Pair <Node, Node>(nodeA.LeftChild, nodeB.LeftChild));
}
else
{
stack.Push(new Pair <Node, Node>(nodeA.RightChild, nodeB));
stack.Push(new Pair <Node, Node>(nodeA.LeftChild, nodeB));
}
}
else
{
if (!nodeB.IsLeaf)
{
stack.Push(new Pair <Node, Node>(nodeA, nodeB.RightChild));
stack.Push(new Pair <Node, Node>(nodeA, nodeB.LeftChild));
}
else
{
// Leaf overlap.
var overlap = new Pair <T>(nodeA.Item, nodeB.Item);
if (Filter == null || Filter.Filter(overlap))
{
yield return(overlap);
}
}
}
}
}
DigitalRune.ResourcePools <Pair <Node, Node> > .Stacks.Recycle(stack);
#else
// Avoiding garbage:
return(GetOverlapsWithTransformedTreeWork.Create(this, otherTree, ref scaleA, ref scaleB, ref bToA));
#endif
}
/// <summary>
/// Compresses an AABB tree.
/// </summary>
/// <param name="compressedNodes">The list of compressed AABB nodes.</param>
/// <param name="uncompressedNode">The root of the uncompressed AABB tree.</param>
private void CompressTree(List <Node> compressedNodes, AabbTree <int> .Node uncompressedNode)
{
if (uncompressedNode.IsLeaf)
{
// Compress leaf node.
Node node = new Node();
node.Item = uncompressedNode.Item;
SetAabb(ref node, uncompressedNode.Aabb);
compressedNodes.Add(node);
}
else
{
// Node is internal node.
int currentIndex = compressedNodes.Count;
Node node = new Node();
SetAabb(ref node, uncompressedNode.Aabb);
compressedNodes.Add(node);
// Compress child nodes.
CompressTree(compressedNodes, uncompressedNode.LeftChild);
CompressTree(compressedNodes, uncompressedNode.RightChild);
// Set escape offset. (Escape offset = size of subtree)
node.EscapeOffset = compressedNodes.Count - currentIndex;
compressedNodes[currentIndex] = node;
}
}
public static IEnumerable <Pair <T> > Create(AabbTree <T> partition, AabbTree <T> otherPartition)
{
var enumerable = Pool.Obtain();
enumerable._partition = partition;
enumerable._stack.Push(new Pair <Node, Node>(partition._root, otherPartition._root));
return(enumerable);
}
public static IEnumerable <Pair <T> > Create(AabbTree <T> partition, ISpatialPartition <T> otherPartition)
{
var enumerable = Pool.Obtain();
enumerable._partition = partition;
enumerable._otherPartition = otherPartition;
enumerable._leafNodes = partition.GetLeafNodes(otherPartition.Aabb).GetEnumerator();
return(enumerable);
}
public static IEnumerable <Node> Create(AabbTree <T> aabbTree, ref Aabb aabb)
{
var enumerable = Pool.Obtain();
enumerable._aabb = aabb;
if (aabbTree._root != null)
{
enumerable._stack.Push(aabbTree._root);
}
return(enumerable);
}
public static IEnumerable <Pair <T> > Create(AabbTree <T> partition, AabbTree <T> otherPartition,
ref Vector3F scaleA, ref Vector3F scaleB, ref Pose bToA)
{
var enumerable = Pool.Obtain();
enumerable._partition = partition;
enumerable._stack.Push(new Pair <Node, Node>(partition._root, otherPartition._root));
enumerable._scaleA = scaleA;
enumerable._scaleB = scaleB;
enumerable._bToA = bToA;
return(enumerable);
}
protected override void OnRecycle()
{
_partition = null;
_otherPartition = null;
_leafNodes.Dispose();
_leafNodes = null;
if (_otherCandidates != null)
{
_otherCandidates.Dispose();
_otherCandidates = null;
}
Pool.Recycle(this);
}
public static IEnumerable <Pair <T> > Create(AabbTree <T> partition,
ISpatialPartition <T> otherPartition, IEnumerable <Node> leafNodes,
ref Vector3F scale, ref Vector3F otherScaleInverse, ref Pose toOther)
{
var enumerable = Pool.Obtain();
enumerable._partition = partition;
enumerable._otherPartition = otherPartition;
enumerable._leafNodes = leafNodes.GetEnumerator();
enumerable._scale = scale;
enumerable._otherScaleInverse = otherScaleInverse;
enumerable._toOther = toOther;
return(enumerable);
}
public static IEnumerable <T> Create(AabbTree <T> aabbTree, ref Ray ray)
{
var enumerable = Pool.Obtain();
enumerable._ray = ray;
enumerable._rayDirectionInverse = new Vector3F(1 / ray.Direction.X,
1 / ray.Direction.Y,
1 / ray.Direction.Z);
enumerable._epsilon = Numeric.EpsilonF * (1 + aabbTree.Aabb.Extent.Length);
if (aabbTree._root != null)
{
enumerable._stack.Push(aabbTree._root);
}
return(enumerable);
}
public void GetOverlaps(Vector3F scale, Pose pose, AabbTree <T> otherTree, Vector3F otherScale, Pose otherPose, List <Pair <T> > overlaps)
{
// TODO: Overlaps should use Pair<T, T> (ordered pair) instead of Pair<T> (unordered pair)!
// TODO: Getting all overlaps is slow when used with boolean queries. For this case following
// interface would be the fastest:
// GetOverlaps(..., Func<T, T, bool> overlapCallback)
// overlapCallback has a bool return value which can abort GetOverlaps().
if (otherTree == null)
{
throw new ArgumentNullException("otherTree");
}
if (overlaps == null)
{
throw new ArgumentNullException("overlaps");
}
UpdateInternal();
otherTree.UpdateInternal();
if (_root == null || otherTree._root == null)
{
return;
}
// Stackless traversal works only if the height can be encoded in a 64-bit long.
// TODO: What is the exact limit? <63, <64, <=64???
if (_height < 63 && otherTree._height < 63)
{
GetOverlapsStackless(scale, pose, otherTree, otherScale, otherPose, overlaps);
}
else
{
GetOverlapsStackBased(scale, pose, otherTree, otherScale, otherPose, overlaps);
}
}
protected override void OnRecycle()
{
_partition = null;
_stack.Clear();
Pool.Recycle(this);
}
private IEnumerable <Pair <T> > GetOverlapsImpl(AabbTree <T> otherTree)
{
#if !POOL_ENUMERABLES
var stack = DigitalRune.ResourcePools <Pair <Node, Node> > .Stacks.Obtain();
stack.Push(new Pair <Node, Node>(_root, otherTree._root));
while (stack.Count > 0)
{
var nodePair = stack.Pop();
var nodeA = nodePair.First;
var nodeB = nodePair.Second;
if (nodeA == nodeB)
{
if (!nodeA.IsLeaf)
{
stack.Push(new Pair <Node, Node>(nodeA.RightChild, nodeA.RightChild));
stack.Push(new Pair <Node, Node>(nodeA.LeftChild, nodeA.RightChild));
stack.Push(new Pair <Node, Node>(nodeA.LeftChild, nodeA.LeftChild));
}
}
else if (GeometryHelper.HaveContact(nodeA.Aabb, nodeB.Aabb))
{
if (!nodeA.IsLeaf)
{
if (!nodeB.IsLeaf)
{
stack.Push(new Pair <Node, Node>(nodeA.RightChild, nodeB.RightChild));
stack.Push(new Pair <Node, Node>(nodeA.LeftChild, nodeB.RightChild));
stack.Push(new Pair <Node, Node>(nodeA.RightChild, nodeB.LeftChild));
stack.Push(new Pair <Node, Node>(nodeA.LeftChild, nodeB.LeftChild));
}
else
{
stack.Push(new Pair <Node, Node>(nodeA.RightChild, nodeB));
stack.Push(new Pair <Node, Node>(nodeA.LeftChild, nodeB));
}
}
else
{
if (!nodeB.IsLeaf)
{
stack.Push(new Pair <Node, Node>(nodeA, nodeB.RightChild));
stack.Push(new Pair <Node, Node>(nodeA, nodeB.LeftChild));
}
else
{
// Leaf overlap.
var overlap = new Pair <T>(nodeA.Item, nodeB.Item);
if (Filter == null || Filter.Filter(overlap))
{
yield return(overlap);
}
}
}
}
}
DigitalRune.ResourcePools <Pair <Node, Node> > .Stacks.Recycle(stack);
#else
// Avoiding garbage:
return(GetOverlapsWithTreeWork.Create(this, otherTree));
#endif
}
private void GetOverlapsStackBased(Vector3F scale, Pose pose, AabbTree <T> otherTree, Vector3F otherScale, Pose otherPose, List <Pair <T> > overlaps)
{
// Compute transformations.
Vector3F scaleInverse = Vector3F.One / scale;
Vector3F otherScaleInverse = Vector3F.One / otherScale;
Pose toLocal = pose.Inverse * otherPose;
Pose toOther = otherPose.Inverse * pose;
// Transform the AABB of the other partition into space of this partition.
var otherAabb = otherTree.Aabb;
otherAabb = otherAabb.GetAabb(otherScale, toLocal); // Apply local scale and transform to scaled local space of this partition.
otherAabb.Scale(scaleInverse); // Transform to unscaled local space of this partition.
var stack = Stacks.Obtain();
var stack2 = Stacks.Obtain();
var filter = Filter;
var node = _root;
while (node != null)
{
var aabb = node.Aabb;
if (aabb.Minimum.X <= otherAabb.Maximum.X &&
aabb.Maximum.X >= otherAabb.Minimum.X &&
aabb.Minimum.Y <= otherAabb.Maximum.Y &&
aabb.Maximum.Y >= otherAabb.Minimum.Y &&
aabb.Minimum.Z <= otherAabb.Maximum.Z &&
aabb.Maximum.Z >= otherAabb.Minimum.Z)
{
if (node.IsLeaf)
{
// Transform AABB of this partition into space of the other partition.
aabb = aabb.GetAabb(scale, toOther); // Apply local scale and transform to scaled local space of other partition.
aabb.Scale(otherScaleInverse); // Transform to unscaled local space of other partition.
stack2.Clear();
var otherNode = otherTree._root;
while (otherNode != null)
{
if (aabb.Minimum.X <= otherNode.Aabb.Maximum.X &&
aabb.Maximum.X >= otherNode.Aabb.Minimum.X &&
aabb.Minimum.Y <= otherNode.Aabb.Maximum.Y &&
aabb.Maximum.Y >= otherNode.Aabb.Minimum.Y &&
aabb.Minimum.Z <= otherNode.Aabb.Maximum.Z &&
aabb.Maximum.Z >= otherNode.Aabb.Minimum.Z)
{
if (otherNode.IsLeaf)
{
var overlap = new Pair <T>(node.Item, otherNode.Item);
if (filter == null || filter.Filter(overlap))
{
overlaps.Add(overlap);
}
otherNode = stack2.Pop();
}
else
{
stack2.Push(otherNode.RightChild);
otherNode = otherNode.LeftChild;
}
}
else
{
otherNode = stack2.Pop();
}
}
node = stack.Pop();
}
else
{
stack.Push(node.RightChild);
node = node.LeftChild;
}
}
else
{
node = stack.Pop();
}
}
Stacks.Recycle(stack);
Stacks.Recycle(stack2);
}
private void GetOverlapsStackless(Vector3F scale, Pose pose, AabbTree <T> otherTree, Vector3F otherScale, Pose otherPose, List <Pair <T> > overlaps)
{
// Note: Stackless traversal requires a Parent reference in the Node class!
// Compute transformations.
Vector3F scaleInverse = Vector3F.One / scale;
Vector3F otherScaleInverse = Vector3F.One / otherScale;
Pose toLocal = pose.Inverse * otherPose;
Pose toOther = otherPose.Inverse * pose;
// Transform the AABB of the other partition into space of this partition.
var otherAabb = otherTree.Aabb;
otherAabb = otherAabb.GetAabb(otherScale, toLocal); // Apply local scale and transform to scaled local space of this partition.
otherAabb.Scale(scaleInverse); // Transform to unscaled local space of this partition.
var filter = Filter;
long levelIndex = 0;
var node = _root;
do
{
var aabb = node.Aabb;
if (aabb.Minimum.X <= otherAabb.Maximum.X &&
aabb.Maximum.X >= otherAabb.Minimum.X &&
aabb.Minimum.Y <= otherAabb.Maximum.Y &&
aabb.Maximum.Y >= otherAabb.Minimum.Y &&
aabb.Minimum.Z <= otherAabb.Maximum.Z &&
aabb.Maximum.Z >= otherAabb.Minimum.Z)
{
if (node.LeftChild == null && node.RightChild == null)
{
// Transform AABB of this partition into space of the other partition.
aabb = aabb.GetAabb(scale, toOther); // Apply local scale and transform to scaled local space of other partition.
aabb.Scale(otherScaleInverse); // Transform to unscaled local space of other partition.
long otherLevelIndex = 0;
var otherNode = otherTree._root;
do
{
if (aabb.Minimum.X <= otherNode.Aabb.Maximum.X &&
aabb.Maximum.X >= otherNode.Aabb.Minimum.X &&
aabb.Minimum.Y <= otherNode.Aabb.Maximum.Y &&
aabb.Maximum.Y >= otherNode.Aabb.Minimum.Y &&
aabb.Minimum.Z <= otherNode.Aabb.Maximum.Z &&
aabb.Maximum.Z >= otherNode.Aabb.Minimum.Z)
{
if (otherNode.LeftChild == null && otherNode.RightChild == null)
{
var overlap = new Pair <T>(node.Item, otherNode.Item);
if (filter == null || filter.Filter(overlap))
{
overlaps.Add(overlap);
}
}
else
{
otherLevelIndex <<= 1;
otherNode = otherNode.LeftChild;
continue;
}
}
otherLevelIndex++;
while ((otherLevelIndex & 1) == 0)
{
otherNode = otherNode.Parent;
otherLevelIndex >>= 1;
}
if (otherNode.Parent != null)
{
otherNode = otherNode.Parent.RightChild;
}
} while (otherNode != otherTree._root);
}
else
{
levelIndex <<= 1;
node = node.LeftChild;
continue;
}
}
levelIndex++;
while ((levelIndex & 1) == 0)
{
node = node.Parent;
levelIndex >>= 1;
}
if (node.Parent != null)
{
node = node.Parent.RightChild;
}
} while (node != _root);
}
private void Build()
{
Debug.Assert(_items != null && _items.Count > 0, "Build should not be called for empty CompressedAabbTree.");
if (GetAabbForItem == null)
{
throw new GeometryException("Cannot build AABB tree. The property GetAabbForItem of the spatial partition is not set.");
}
if (_items.Count == 1)
{
// AABB tree contains exactly one item. (One leaf, no internal nodes.)
int item = _items[0];
// Determine AABB of spatial partition and prepare factors for quantization.
Aabb aabb = GetAabbForItem(item);
SetQuantizationValues(aabb);
// Create node.
Node node = new Node();
node.Item = _items[0];
SetAabb(ref node, _aabb);
_nodes = new[] { node };
_numberOfItems = 1;
}
else
{
// Default case: Several items. (Data is stored in the leaves.)
// First create a normal AabbTree<int> which is then compressed.
_numberOfItems = _items.Count;
List <IAabbTreeNode <int> > leaves = DigitalRune.ResourcePools <IAabbTreeNode <int> > .Lists.Obtain();
for (int i = 0; i < _numberOfItems; i++)
{
int item = _items[i];
Aabb aabb = GetAabbForItem(item);
leaves.Add(new AabbTree <int> .Node {
Aabb = aabb, Item = item
});
}
// Build tree.
AabbTree <int> .Node root = (AabbTree <int> .Node)AabbTreeBuilder.Build(leaves, () => new AabbTree <int> .Node(), BottomUpBuildThreshold);
// Set AABB of spatial partition and prepare the factors for quantization.
SetQuantizationValues(root.Aabb);
// Compress AABB tree.
var nodes = DigitalRune.ResourcePools <Node> .Lists.Obtain();
CompressTree(nodes, root);
_nodes = nodes.ToArray();
// Recycle temporary lists.
DigitalRune.ResourcePools <IAabbTreeNode <int> > .Lists.Recycle(leaves);
DigitalRune.ResourcePools <Node> .Lists.Recycle(nodes);
}
// Recycle items list, now that we have a valid tree.
DigitalRune.ResourcePools <int> .Lists.Recycle(_items);
_items = null;
}