void PrintHelper(KdNode root, int level, bool isLeft = false) { if (root == null) { return; } StringBuilder sb = new StringBuilder(); for (int i = 0; i < level; i++) { sb.Append("-"); } if (level > 0) { if (isLeft) { sb.Append("L:"); } else { sb.Append("R:"); } } sb.Append(root); Debug.Log(sb.ToString()); PrintHelper(root.mLeft, level + 1, true); PrintHelper(root.mRight, level + 1, false); }
/// <summary> /// clear tree /// </summary> public void Clear() { //rest for the garbage collection _root = null; _last = null; _count = 0; }
/// <summary> /// Test invariants on children /// </summary> private static void AreMetByIntermediate <T>(KdNode <T> n) where T : IVector { // Each intermediate node has two children Assert.IsNotNull(n.Left); Assert.IsNotNull(n.Right); // If not root, each node has a parent which itself references the node in either left or righ // child property if (!n.Root) { Assert.IsNotNull(n.Parent); Assert.IsTrue(n.Parent.Left == n || n.Parent.Right == n); } // Split dimension must be within bounds Assert.Less(n.SplitDimension, n.InternalBounds.Dimensions); // Split location must be within bounds Assert.LessOrEqual(n.SplitBounds.Lower[n.SplitDimension], n.SplitLocation); Assert.GreaterOrEqual(n.SplitBounds.Upper[n.SplitDimension], n.SplitLocation); // The split bounding volume of the children must represent the split plane of the node Assert.AreEqual(n.Left.SplitBounds.Upper[n.SplitDimension], n.SplitLocation, FloatComparison.DefaultEps); Assert.AreEqual(n.Right.SplitBounds.Lower[n.SplitDimension], n.SplitLocation, FloatComparison.DefaultEps); // The inner bounding volume must be contained in the split bounding volume for (int i = 0; i < n.InternalBounds.Dimensions; ++i) { Assert.LessOrEqual(n.SplitBounds.Lower[i], n.InternalBounds.Lower[i]); Assert.GreaterOrEqual(n.SplitBounds.Upper[i], n.InternalBounds.Upper[i]); } }
/// <summary> /// Select axis by cycling through dimensions at each depth level of tree /// </summary> public int Select <T> (KdNode <T> target) where T : IVector { IVector diagonal = target.InternalBounds.Diagonal; int dims = target.InternalBounds.Dimensions; // Setup initial axis // On root level this is zero on each other level it the depth of the node module its dimensions int axis = target.Depth % dims; double spread = diagonal[axis]; int i = 0; while (FloatComparison.CloseZero(spread, FloatComparison.DefaultEps) && i < dims) { axis = (axis + 1) % dims; spread = diagonal[axis]; i += 1; } if (i == dims) { // Cycle completed without finding an axis that has a spread greater than zero throw new DegenerateDatasetException(); } return(axis); }
/// <summary> /// Selects the split location based on the midpoint of the chosen split axis. /// This might generate leaf nodes that are empty. Assumes split dimension has a positive spread /// </summary> public double Select <T>(KdNode <T> target, int split_dimension) where T : IVector { AABB split_bounds = target.SplitBounds; double split = split_bounds.Lower[split_dimension] + split_bounds.Extension(split_dimension) * 0.5; return(split); }
private static KdNode RemoveNode(KdNode tree, V pos, int depth, out KdNode removed) { if (tree == null) { removed = null; return(null); } if (tree.Position.Equals(pos)) { removed = tree; return(ConstructTree( EnumerateTree(tree).Skip(1).Select( node => new KeyValuePair <V, T> (node.Position, node.Data)), depth).Item1); } var k = depth % pos.Dimensions; if (pos[k] < tree.Position[k]) { tree.Left = RemoveNode(tree.Left, pos, depth + 1, out removed); } else { tree.Right = RemoveNode(tree.Right, pos, depth + 1, out removed); } return(tree); }
/// <summary> /// Determine if the chosen split is a trivial one. /// </summary> private ETrivialSplitType IsTrivialSplit <T>(KdNode <T> target, int split_dimension, double split_location) where T : IVector { // Test for trivial split O(n) int count_left = 0; int count_right = 0; int i = 0; // Loop over vectors and try to early exit the loop when both left and right are assigned at least one element. while ((i < target.Vectors.Count) && (count_left == 0 || count_right == 0)) { T t = target.Vectors[i]; if (t[split_dimension] <= split_location) { count_left += 1; } else { count_right += 1; } ++i; } if (count_left == 0) { return(ETrivialSplitType.EmptyLeft); } else if (count_right == 0) { return(ETrivialSplitType.EmptyRight); } else { return(ETrivialSplitType.NoTrivial); } }
void ShowGraphHelper(KdNode root, int dim, Material mat, int minX, int maxX, int minY, int maxY) { if (root == null) { return; } mat.SetColor("_Color", Color.red); GraphicsTool.DrawPoint(new Vector2(root.mValues[0], root.mValues[1]), 5, mat); mat.SetColor("_Color", Color.green); if (dim % 2 == 0) { int x = root.mValues[0]; Vector2 begin = new Vector2(x, maxY); Vector2 end = new Vector2(x, minY); GraphicsTool.DrawLine(begin, end, mat); ShowGraphHelper(root.mLeft, dim + 1, mat, minX, x, minY, maxY); ShowGraphHelper(root.mRight, dim + 1, mat, x, maxX, minY, maxY); } else { int y = root.mValues[1]; Vector2 begin = new Vector2(minX, y); Vector2 end = new Vector2(maxX, y); GraphicsTool.DrawLine(begin, end, mat); ShowGraphHelper(root.mLeft, dim + 1, mat, minX, maxX, minY, y); ShowGraphHelper(root.mRight, dim + 1, mat, minX, maxX, y, maxY); } }
public CubeBehaviour FindClosest(Vector3 pos) { float _nearestDis = float.MaxValue; KdNode _nearestNode = null; var current = _mRoot; while (null != current) { var dis = CalDistance(current.component.transform.position, pos); if (dis < _nearestDis) { _nearestDis = dis; _nearestNode = current; } var curValue = _getSplitValue(current); var nodeValue = _getSplitValue(current.level, pos); if (nodeValue < curValue) { current = current.left; } else { current = current.right; } } if (null == _nearestNode) { return(null); } return(_nearestNode.component as CubeBehaviour); }
/// <summary> /// Specialized exact search for the first matching item. This method /// does not carry the overhead of creating IEnumerables for its result set. /// </summary> public bool TryFindExactFirst(IVector x, out T first) { // If point is not within root-bounds we can exit early if (!this.Tree.InternalBounds.Inside(x)) { first = default(T); return(false); } // Else we fetch the leaf x possibly resides in KdNode <T> leaf = _cls.FindClosestLeaf(x); // And test for containment int index = leaf.Vectors.FindIndex(delegate(T obj) { return(VectorComparison.Equal(x, obj)); }); if (index < 0) { first = default(T); return(false); } else { first = leaf.Vectors[index]; return(true); } }
private static KdNode[] NearestNeighbours(KdNode tree, V pos, Aabb <V> bounds, int depth, KdNode[] bests, Func <V, V, float> distance) { if (tree != null) { var k = depth % tree.Position.Dimensions; var split = tree.Position [k]; var leftBounds = new Aabb <V> (bounds.Min, bounds.Max.With(k, split)); var rightBounds = new Aabb <V> (bounds.Min.With(k, split), bounds.Max); bests = pos [k] < split? NearestNeighbours(tree.Left, pos, leftBounds, depth + 1, bests, distance) : NearestNeighbours(tree.Right, pos, rightBounds, depth + 1, bests, distance); var currDist = distance(tree.Position, pos); var bestDist = LastBestDistance(bests, pos, distance); if (currDist < bestDist) { var i = Array.FindLastIndex(bests, n => n != null && distance(n.Position, pos) < currDist) + 1; bests = bests.Insert(i, tree); bestDist = LastBestDistance(bests, pos, distance); } if (pos [k] < split && DistanceToBBox(pos, rightBounds, distance) < bestDist) { bests = NearestNeighbours(tree.Right, pos, rightBounds, depth + 1, bests, distance); } if (pos [k] >= split && DistanceToBBox(pos, leftBounds, distance) < bestDist) { bests = NearestNeighbours(tree.Left, pos, leftBounds, depth + 1, bests, distance); } } return(bests); }
public void TestSplitOneDimensional() { ISubdivisionPolicy p = SubdivisionPolicyConnector.CreatePolicy <AxisOfMaximumSpreadSelector, MidpointSelector, SlidingPlaneResolver>(1); KdNode <Vector> n = new KdNode <Vector>(); n.Vectors = new List <Vector>(new Vector[] { Vector.Create(-1.0), Vector.Create(1.0), Vector.Create(3.0), Vector.Create(2.0) }); n.InternalBounds = new AABB(1); n.InternalBounds.Enlarge <Vector>(n.Vectors); p.Split(n); Assert.AreEqual(1.0, n.SplitLocation, FloatComparison.DefaultEps); Assert.AreEqual(0, n.SplitDimension); KdNode <Vector> left = n.Left; KdNode <Vector> right = n.Right; Assert.AreEqual(2, left.Vectors.Count); Assert.AreEqual(-1.0, left.Vectors[0][0], FloatComparison.DefaultEps); Assert.AreEqual(1.0, left.Vectors[1][0], FloatComparison.DefaultEps); Assert.AreEqual(2, right.Vectors.Count); Assert.AreEqual(3.0, right.Vectors[0][0], FloatComparison.DefaultEps); Assert.AreEqual(2.0, right.Vectors[1][0], FloatComparison.DefaultEps); Assert.AreEqual(-1.0, left.SplitBounds.Lower[0], FloatComparison.DefaultEps); Assert.AreEqual(1.0, left.SplitBounds.Upper[0], FloatComparison.DefaultEps); Assert.AreEqual(1.0, right.SplitBounds.Lower[0], FloatComparison.DefaultEps); Assert.AreEqual(3.0, right.SplitBounds.Upper[0], FloatComparison.DefaultEps); }
public KdTree(IEnumerable <KeyValuePair <V, T> > values) { var t = ConstructTree(values, 0); _root = t.Item1; _count = t.Item2; }
public override void Visit(KdNode<HotPixel> node) { var hp = node.Data; /* * If the hot pixel is not a node, and it contains one of the segment vertices, * then that vertex is the source for the hot pixel. * To avoid over-noding a node is not added at this point. * The hot pixel may be subsequently marked as a node, * in which case the intersection will be added during the final vertex noding phase. */ if (!hp.IsNode) { if (hp.Intersects(P0) || hp.Intersects(P1)) return; } /* * Add a node if the segment intersects the pixel. * Mark the HotPixel as a node (since it may not have been one before). * This ensures the vertex for it is added as a node during the final vertex noding phase. */ if (hp.Intersects(P0, P1)) { //System.out.println("Added intersection: " + hp.getCoordinate()); SS.AddIntersection(hp.Coordinate, SegIndex); hp.IsNode = true; } }
private static IEnumerable <KdNode> OverlappingNodes(KdNode tree, Aabb <V> bbox, int depth) { if (tree == null) { yield break; } if (bbox & tree.Position) { yield return(tree); } var k = depth % tree.Position.Dimensions; if (bbox.Min[k] < tree.Position[k]) { foreach (var node in OverlappingNodes(tree.Left, bbox, depth + 1)) { yield return(node); } } if (bbox.Max[k] >= tree.Position[k]) { foreach (var node in OverlappingNodes(tree.Right, bbox, depth + 1)) { yield return(node); } } }
private static bool ValidateKdt(KdNode node, int depth = 0) { bool even = depth % 2 == 0; var nodeValue = even ? node.Value.X : node.Value.Y; if (node.Left != null) { var leftValue = even ? node.Left.Value.X : node.Left.Value.Y; if (nodeValue < leftValue || ValidateKdt(node.Left, ++depth) == false) { return(false); } } if (node.Right != null) { var rightValue = even ? node.Right.Value.X : node.Right.Value.Y; if (nodeValue >= rightValue || ValidateKdt(node.Right, ++depth) == false) { return(false); } } return(true); }
public void TestSplitBounds() { // +-------------+ // | | | // | | | // +--+---| | // | | x | | // +--+---+------+ KdNode <Vector> root = new KdNode <Vector>(); root.InternalBounds = new AABB(Vector.Create(-5, -5), Vector.Create(5, 5)); root.SplitDimension = 0; root.SplitLocation = 0; KdNode <Vector> left = root.SetLeftChild(new KdNode <Vector>()); left.SplitDimension = 1; left.SplitLocation = 0; left = left.SetLeftChild(new KdNode <Vector>()); left.SplitDimension = 0; left.SplitLocation = -2.5; KdNode <Vector> right = left.SetRightChild(new KdNode <Vector>()); right.InternalBounds = new AABB(2); AABB bounds = right.SplitBounds; Assert.AreEqual(bounds.Lower[0], -2.5, FloatComparison.DefaultEps); Assert.AreEqual(bounds.Lower[1], -5, FloatComparison.DefaultEps); Assert.AreEqual(bounds.Upper[0], 0, FloatComparison.DefaultEps); Assert.AreEqual(bounds.Upper[1], 0, FloatComparison.DefaultEps); }
private KdNode InsertRange(List <Point> points, int depth) { if (points.Count == 0) { return(null); } if (depth % 2 == 0) { points.Sort((a, b) => a.X.CompareTo(b.X)); } else { points.Sort((a, b) => a.Y.CompareTo(b.Y)); } int midIndex = points.Count / 2; KdNode node = new KdNode(points[midIndex]); var smaller = points.Take(midIndex).ToList(); var bigger = points.Skip(midIndex + 1).ToList(); node.Left = InsertRange(smaller, depth + 1); node.Right = InsertRange(bigger, depth + 1); return(node); }
internal bool intersects(KdNode v) { Point d = v.med.Center - med.Center; double l = d.Length; return(l < v.med.Radius + med.Radius); }
private static RectHV RightRect(RectHV rect, KdNode node) { if (node.Vertical) { return(new RectHV(node.X, rect.Ymin, rect.Xmax, rect.Ymax)); } return(new RectHV(rect.Xmin, node.Y, rect.Xmax, rect.Ymax)); }
private Point2D Nearest(KdNode node, RectHV rect, double x, double y, Point2D candidate) { if (node == null) { return(candidate); } double dqn = 0; double drq = 0; var query = new Point2D(x, y); var nearest = candidate; if (nearest != null) { dqn = query.DistanceSquaredTo(nearest); drq = rect.DistanceSquaredTo(query); } if (nearest == null || dqn < drq) { var point = new Point2D(node.X, node.Y); if (nearest == null || dqn > query.DistanceSquaredTo(point)) { nearest = point; } } var left = LeftRect(rect, node); var right = RightRect(rect, node); if (node.Vertical) { if (x < node.X) { nearest = Nearest(node.Left, left, x, y, nearest); nearest = Nearest(node.Right, right, x, y, nearest); } else { nearest = Nearest(node.Right, right, x, y, nearest); nearest = Nearest(node.Left, left, x, y, nearest); } } else { if (y < node.Y) { nearest = Nearest(node.Left, left, x, y, nearest); nearest = Nearest(node.Right, right, x, y, nearest); } else { nearest = Nearest(node.Right, right, x, y, nearest); nearest = Nearest(node.Left, left, x, y, nearest); } } return(nearest); }
/// <summary> /// Splits a node based on bucket size, the chosen split dimension selector and chosen split location selector /// </summary> public void Split <T>(KdNode <T> target) where T : IVector { // Sanity check node if (target.Intermediate) { throw new IntermediateNodeException(); } if (target.Vectors.Count <= this.MaximumBucketSize) { throw new BucketSizeException(); } // Find axis of split plane int split_dim = _dim_selector.Select(target); // Find location of split plane double split_loc = _loc_selector.Select(target, split_dim); // Possibly resolve a trivial split ETrivialSplitType split_type = this.IsTrivialSplit(target, split_dim, split_loc); if (split_type == ETrivialSplitType.EmptyLeft || split_type == ETrivialSplitType.EmptyRight) { split_loc = _trivial_resolver.Resolve(target, split_dim, split_loc, split_type); } // Pass over vectors, create leaves (one is possibly empty) and update parent KdNode <T> left = target.SetLeftChild(new KdNode <T>()); KdNode <T> right = target.SetRightChild(new KdNode <T>()); // Assign vectors left.Vectors = new List <T>(); right.Vectors = new List <T>(); // Classify each vector foreach (T t in target.Vectors) { if (t[split_dim] <= split_loc) { left.Vectors.Add(t); } else { right.Vectors.Add(t); } } // Update internal bounds left.InternalBounds = new AABB(target.InternalBounds.Dimensions); left.InternalBounds.Enlarge <T>(left.Vectors); right.InternalBounds = new AABB(target.InternalBounds.Dimensions); right.InternalBounds.Enlarge <T>(right.Vectors); // Update target target.SplitDimension = split_dim; target.SplitLocation = split_loc; }
public void TestDegenerated2() { KdNode <Vector> n = new KdNode <Vector>(); n.Vectors = new List <Vector>(new Vector[] { Vector.Create(1.0, 1.0), Vector.Create(2.0, 2.0), Vector.Create(2.0, 2.0), Vector.Create(2.0, 2.0) }); n.InternalBounds = new AABB(2); n.InternalBounds.Enlarge <Vector>(n.Vectors); new MedianSelector().Select(n, 0); }
private void _add(KdNode newNode) { _count++; newNode.left = null; newNode.right = null; newNode.level = 0; var parent = _findParent(newNode.component.transform.position); //set last if (_last != null) { _last.next = newNode; } _last = newNode; //set root if (parent == null) { _root = newNode; return; } var splitParent = _getSplitValue(parent); var splitNew = _getSplitValue(parent.level, newNode.component.transform.position); newNode.level = parent.level + 1; if (splitNew < splitParent) { parent.left = newNode; //go left } else { parent.right = newNode; //go right } }
/// <summary> /// Resolves a split that caused the right partion to be empty. /// </summary> private double ResolveRightEmpty <T> (KdNode <T> target, int split_dimension, double split_location) where T : IVector { // Because of the property that all elements > split_location are put in the right partition, we need to search for the // closest element to the split location with the restriction that the distance is greater than zero. // First pass, find the closest element double best = this.FindClosest(target, split_dimension, split_location, false); return(this.FindClosest(target, split_dimension, best, true)); }
public override void Visit(KdNode<HotPixel> node) { var hp = node.Data; /* * If vertex pixel is a node, add it. */ if (hp.IsNode && hp.Coordinate.Equals2D(P0)) { SS.AddIntersection(P0, SegIndex); } }
public void TestSplitMultiDimensional() { MedianSelector s = new MedianSelector(); KdNode <Vector> n = new KdNode <Vector>(); n.Vectors = new List <Vector>(new Vector[] { Vector.Create(1.0, 1.0), Vector.Create(1.0, -1.0), Vector.Create(1.0, 3.0), Vector.Create(1.0, 2.0) }); n.InternalBounds = new AABB(2); n.InternalBounds.Enlarge <Vector>(n.Vectors); Assert.AreEqual(2.0, s.Select(n, 1), FloatComparison.DefaultEps); }
public void TestAllCoordinatesSame() { KdNode <IVector> n = new KdNode <IVector>(); n.Vectors = new List <IVector>(new IVector[] { Vector.Create(1.0f, 0.0f), Vector.Create(1.0f, 0.0f), Vector.Create(1.0f, 0.0f), Vector.Create(1.0f, 0.0f) }); n.InternalBounds = new AABB(2); n.InternalBounds.Enlarge <IVector>(n.Vectors); PeriodicAxisSelector s = new PeriodicAxisSelector(); s.Select(n); }
private Visual TreeVisual(KdNode tree, Visual parent) { if (tree == null) { return(Visual.Margin(NodeVisual("-", Color.DarkGray, parent), right: 4, bottom: 4)); } var node = NodeVisual(tree.ToString(), Color.Black, parent); return(Visual.VStack(HAlign.Center, Visual.Margin(node, right: 4, bottom: 20), Visual.HStack(VAlign.Top, TreeVisual(tree.Left, node), TreeVisual(tree.Right, node)))); }
public bool TryAdd(V pos, T data) { bool added = false; _root = AddNode(_root, new KdNode(pos, data), 0, ref added); if (added) { _count++; } return(added); }
private bool Contains(KdNode node, double x, double y) { if (node == null) { return false; } if (node.X.Equals(x) && node.Y.Equals(y)) { return true; } if (node.Vertical && x < node.X || !node.Vertical && y < node.Y) { return Contains(node.Left, x, y); } return Contains(node.Right, x, y); }
private void Range(KdNode node, RectHV nrect, RectHV rect, Queue<Point2D> queue) { if (node == null) { return; } if (rect.Intersects(nrect)) { var p = new Point2D(node.X, node.Y); if (rect.Contains(p)) { queue.Enqueue(p); } Range(node.Left, LeftRect(nrect, node), rect, queue); Range(node.Right, RightRect(nrect, node), rect, queue); } }
private KdNode Insert(KdNode node, Point2D p, bool vertical) { if (node == null) { size++; return new KdNode(p.X, p.Y, vertical); } if (node.X.Equals(p.X) && node.Y.Equals(p.Y)) { return node; } if (node.Vertical && p.X < node.X || !node.Vertical && p.Y < node.Y) { node.Left = Insert(node.Left, p, !node.Vertical); } else { node.Right = Insert(node.Right, p, !node.Vertical); } return node; }
public KdTree() { root = null; size = 0; }
public void Insert(Point2D point) { root = Insert(root, point, vertical:true); }
private static RectHV RightRect(RectHV rect, KdNode node) { if (node.Vertical) { return new RectHV(node.X, rect.Ymin, rect.Xmax, rect.Ymax); } return new RectHV(rect.Xmin, node.Y, rect.Xmax, rect.Ymax); }
private Point2D Nearest(KdNode node, RectHV rect, double x, double y, Point2D candidate) { if (node == null) { return candidate; } double dqn = 0; double drq = 0; var query = new Point2D(x, y); var nearest = candidate; if (nearest != null) { dqn = query.DistanceSquaredTo(nearest); drq = rect.DistanceSquaredTo(query); } if (nearest == null || dqn < drq) { var point = new Point2D(node.X, node.Y); if (nearest == null || dqn > query.DistanceSquaredTo(point)) { nearest = point; } } var left = LeftRect(rect, node); var right = RightRect(rect, node); if (node.Vertical) { if (x < node.X) { nearest = Nearest(node.Left, left, x, y, nearest); nearest = Nearest(node.Right, right, x, y, nearest); } else { nearest = Nearest(node.Right, right, x, y, nearest); nearest = Nearest(node.Left, left, x, y, nearest); } } else { if (y < node.Y) { nearest = Nearest(node.Left, left, x, y, nearest); nearest = Nearest(node.Right, right, x, y, nearest); } else { nearest = Nearest(node.Right, right, x, y, nearest); nearest = Nearest(node.Left, left, x, y, nearest); } } return nearest; }