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; }