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