/// <summary>
/// Creates a new <see cref="KDTree<T>"/>.
/// </summary>
///
/// <param name="dimension">The number of dimensions in the tree.</param>
/// <param name="root">The root node, if already existent.</param>
/// <param name="count">The number of elements in the root node.</param>
/// <param name="leaves">The number of leaves linked through the root node.</param>
///
public KDTree(int dimension, KDTreeNode <T> root, int count, int leaves)
: this(dimension)
{
this.root = root;
this.count = count;
this.leaves = leaves;
}
/// <summary>
/// Attempts to add a value to the collection. If the list is full
/// and the value is more distant than the farthest node in the
/// collection, the value will not be added.
/// </summary>
///
/// <param name="value">The node to be added.</param>
/// <param name="distance">The node distance.</param>
///
/// <returns>Returns true if the node has been added; false otherwise.</returns>
///
public bool AddFarthest(KDTreeNode <T> value, double distance)
{
// The list does have a limit. We have to check if the list
// is already full or not, to see if we can discard or keep
// the point
if (count < Capacity)
{
// The list still has room for new elements.
// Just add the value at the right position.
add(distance, value);
return(true); // a value has been added
}
// The list is at its maximum capacity. Check if the value
// to be added is farther than the current nearest point.
if (distance > range.Min)
{
// Yes, it is farther. Remove the previous nearest point
// and insert this new one at an appropriate position to
// keep the list ordered.
RemoveNearest();
add(distance, value);
return(true); // a value has been added
}
// The value is even closer
return(false); // discard it
}
/// <summary>
/// Adds the specified item to the collection.
/// </summary>
///
/// <param name="distance">The distance from the node to the query point.</param>
/// <param name="item">The item to be added.</param>
///
private void add(double distance, KDTreeNode <T> item)
{
List <KDTreeNode <T> > position;
if (!positions.TryGetValue(distance, out position))
{
positions.Add(distance, position = new List <KDTreeNode <T> >());
}
position.Add(item);
distances.Add(distance);
if (count == 0)
{
range.Max = range.Min = distance;
}
else
{
if (distance > range.Max)
{
range.Max = distance;
}
if (distance < range.Min)
{
range.Min = distance;
}
}
count++;
}
/// <summary>
/// Breadth-first tree traversal method.
/// </summary>
///
public static IEnumerator <KDTreeNode <T> > BreadthFirst <T>(KDTree <T> tree)
{
if (tree.Root == null)
{
yield break;
}
var queue = new Queue <KDTreeNode <T> >(new[] { tree.Root });
while (queue.Count != 0)
{
KDTreeNode <T> current = queue.Dequeue();
yield return(current);
if (current.Left != null)
{
queue.Enqueue(current.Left);
}
if (current.Right != null)
{
queue.Enqueue(current.Right);
}
}
}
/// <summary>
/// Pre-order tree traversal method.
/// </summary>
///
public static IEnumerator <KDTreeNode <T> > PreOrder <T>(KDTree <T> tree)
{
if (tree.Root == null)
{
yield break;
}
var stack = new Stack <KDTreeNode <T> >();
KDTreeNode <T> current = tree.Root;
while (stack.Count != 0 || current != null)
{
if (current != null)
{
stack.Push(current);
yield return(current);
current = current.Left;
}
else
{
current = stack.Pop();
current = current.Right;
}
}
}
private bool approximate(KDTreeNode <T> current, double[] position,
KDTreeNodeCollection <T> list, int maxLeaves, ref int visited)
{
// Compute distance from this node to the point
double d = distance(position, current.Position);
list.Add(current, d);
if (++visited > maxLeaves)
{
return(true);
}
// Check for leafs on the opposite sides of
// the subtrees to nearest possible neighbors.
// Prepare for recursion. The following null checks
// will be used to avoid function calls if possible
double value = position[current.Axis];
double median = current.Position[current.Axis];
double u = value - median;
if (u <= 0)
{
if (current.Left != null)
{
if (approximate(current.Left, position, list, maxLeaves, ref visited))
{
return(true);
}
}
if (current.Right != null && Math.Abs(u) <= list.Maximum)
{
if (approximate(current.Right, position, list, maxLeaves, ref visited))
{
return(true);
}
}
}
else
{
if (current.Right != null)
{
approximate(current.Right, position, list, maxLeaves, ref visited);
}
if (current.Left != null && Math.Abs(u) <= list.Maximum)
{
if (approximate(current.Left, position, list, maxLeaves, ref visited))
{
return(true);
}
}
}
return(false);
}
/// <summary>
/// Returns an enumerator that iterates through the tree.
/// </summary>
///
/// <returns>
/// An <see cref="T:System.Collections.IEnumerator"/> object that can be used to iterate through the collection.
/// </returns>
///
public IEnumerator <KDTreeNode <T> > GetEnumerator()
{
if (root == null)
{
yield break;
}
var stack = new Stack <KDTreeNode <T> >(new[] { root });
while (stack.Count != 0)
{
KDTreeNode <T> current = stack.Pop();
yield return(current);
if (current.Left != null)
{
stack.Push(current.Left);
}
if (current.Right != null)
{
stack.Push(current.Right);
}
}
}
private void insert(ref KDTreeNode <T> node, double[] position, T value, int depth)
{
if (node == null)
{
// Base case: node is null
node = new KDTreeNode <T>()
{
Axis = depth % dimensions,
Position = position,
Value = value
};
}
else
{
// Recursive case: keep looking for a position to insert
int newIndex = depth % dimensions; // with this, each depth of
// the tree will operate on one of the dimensions of the data
if (position[node.Axis] < node.Position[node.Axis])
{
KDTreeNode <T> child = node.Left;
insert(ref child, position, value, depth + 1);
node.Left = child;
}
else
{
KDTreeNode <T> child = node.Right;
insert(ref child, position, value, depth + 1);
node.Right = child;
}
}
}
/// <summary>
/// Creates the root node for a new <see cref="KDTree{T}"/> given
/// a set of data points and their respective stored values.
/// </summary>
///
/// <param name="points">The data points to be inserted in the tree.</param>
/// <param name="values">The values associated with each point.</param>
/// <param name="leaves">Return the number of leaves in the root subtree.</param>
/// <param name="inPlace">Whether the given <paramref name="points"/> vector
/// can be ordered in place. Passing true will change the original order of
/// the vector. If set to false, all operations will be performed on an extra
/// copy of the vector.</param>
///
/// <returns>The root node for a new <see cref="KDTree{T}"/>
/// contained the given <paramref name="points"/>.</returns>
///
protected static KDTreeNode<T> CreateRoot(double[][] points, T[] values, bool inPlace, out int leaves)
{
// Initial argument checks for creating the tree
if (points == null)
throw new ArgumentNullException("points");
if (values != null && points.Length != values.Length)
throw new DimensionMismatchException("values");
if (!inPlace)
{
points = (double[][])points.Clone();
if (values != null)
values = (T[])values.Clone();
}
leaves = 0;
int dimensions = points[0].Length;
// Create a comparer to compare individual array
// elements at specified positions when sorting
ElementComparer comparer = new ElementComparer();
// Call the recursive algorithm to operate on the whole array (from 0 to points.Length)
KDTreeNode<T> root = create(points, values, 0, dimensions, 0, points.Length, comparer, ref leaves);
// Create and return the newly formed tree
return root;
}
private void insert(ref KDTreeNode <T> node, double[] position, T value, int depth)
{
if (node == null)
{
// Base case: node is null
node = new KDTreeNode <T>()
{
Axis = depth % dimensions,
Position = position,
Value = value
};
}
else
{
// Recursive case: keep looking for a position to insert
if (position[node.Axis] < node.Position[node.Axis])
{
KDTreeNode <T> child = node.Left;
insert(ref child, position, value, depth + 1);
node.Left = child;
}
else
{
KDTreeNode <T> child = node.Right;
insert(ref child, position, value, depth + 1);
node.Right = child;
}
}
}
private void nearest(KDTreeNode <T> current, double[] position, KDTreeNodeCollection <T> list)
{
// Compute distance from this node to the point
double d = distance(position, current.Position);
if (current.IsLeaf)
{
// Base: node is leaf
list.Add(current, d);
}
else
{
// Check for leafs on the opposite sides of
// the subtrees to nearest possible neighbors.
// Prepare for recursion. The following null checks
// will be used to avoid function calls if possible
double value = position[current.Axis];
double median = current.Position[current.Axis];
double u = (value - median);
if (u < 0)
{
if (current.Left != null)
{
nearest(current.Left, position, list);
}
list.Add(current, d);
if (current.Right != null)
{
if ((u * u) < list.Distance.Max)
{
nearest(current.Right, position, list);
}
}
}
else
{
if (current.Right != null)
{
nearest(current.Right, position, list);
}
list.Add(current, d);
if (current.Left != null)
{
if ((u * u) < list.Distance.Max)
{
nearest(current.Left, position, list);
}
}
}
}
}
/// <summary>
/// Retrieves a fixed point of nearest points to a given point.
/// </summary>
///
/// <param name="position">The queried point.</param>
/// <param name="distance">The distance from the <paramref name="position"/>
/// to its nearest neighbor found in the tree.</param>
///
/// <returns>A list of neighbor points, ordered by distance.</returns>
///
public KDTreeNode<T> Nearest(double[] position, out double distance)
{
KDTreeNode<T> result = root;
distance = Distance.Distance(root.Position, position);
nearest(root, position, ref result, ref distance);
return result;
}
/// <summary>
/// Adds the specified item to the collection.
/// </summary>
///
/// <param name="distance">The distance from the node to the query point.</param>
/// <param name="item">The item to be added.</param>
///
private void add(double distance, KDTreeNode <T> item)
{
positions[count] = item;
distances[count] = distance;
count++;
// Ensure it is in the right place.
siftUpLast();
}
/// <summary>
/// Retrieves a fixed point of nearest points to a given point.
/// </summary>
///
/// <param name="position">The queried point.</param>
/// <param name="percentage">The maximum percentage of leaf nodes that
/// can be visited before the search finishes with an approximate answer.</param>
/// <param name="distance">The distance between the query point and its nearest neighbor.</param>
///
/// <returns>A list of neighbor points, ordered by distance.</returns>
///
public KDTreeNode<T> ApproximateNearest(double[] position, double percentage, out double distance)
{
KDTreeNode<T> result = root;
distance = Distance.Distance(root.Position, position);
int maxLeaves = (int)(leaves * percentage);
int visited = 0;
approximateNearest(root, position, ref result, ref distance, maxLeaves, ref visited);
return result;
}
/// <summary>
/// Post-order tree traversal method.
/// </summary>
///
public static IEnumerator <KDTreeNode <T> > PostOrder <T>(KDTree <T> tree)
{
if (tree.Root == null)
{
yield break;
}
var stack = new Stack <KDTreeNode <T> >(new[] { tree.Root });
KDTreeNode <T> previous = tree.Root;
while (stack.Count != 0)
{
KDTreeNode <T> current = stack.Peek();
if (previous == current || previous.Left == current || previous.Right == current)
{
if (current.Left != null)
{
stack.Push(current.Left);
}
else if (current.Right != null)
{
stack.Push(current.Right);
}
else
{
yield return(stack.Pop());
}
}
else if (current.Left == previous)
{
if (current.Right != null)
{
stack.Push(current.Right);
}
else
{
yield return(stack.Pop());
}
}
else if (current.Right == previous)
{
yield return(stack.Pop());
}
else
{
throw new InvalidOperationException();
}
previous = current;
}
}
/// <summary>
/// Creates a new <see cref="KDTree<T>"/>.
/// </summary>
///
/// <param name="dimension">The number of dimensions in the tree.</param>
/// <param name="root">The root node, if already existent.</param>
///
public KDTree(int dimension, KDTreeNode<T> root)
: this(dimension)
{
this.root = root;
foreach (var node in this)
{
count++;
if (node.IsLeaf)
leaves++;
}
}
private void find(KDTreeNode <T> current, double[] position,
double radius, ICollection <KDTreeNodeDistance <T> > list)
{
// Check if the distance of the point from this
// node is within the desired radius, and if it
// is, add to the list of nearest nodes.
double d = distance(position, current.Position);
if (d <= radius)
{
list.Add(new KDTreeNodeDistance <T>(current, d));
}
// Prepare for recursion. The following null checks
// will be used to avoid function calls if possible
double value = position[current.Axis];
double median = current.Position[current.Axis];
if (value < median)
{
if (current.Left != null)
{
find(current.Left, position, radius, list);
}
if (current.Right != null)
{
if (Math.Abs(value - median) <= radius)
{
find(current.Right, position, radius, list);
}
}
}
else
{
if (current.Right != null)
{
find(current.Right, position, radius, list);
}
if (current.Left != null)
{
if (Math.Abs(value - median) <= radius)
{
find(current.Left, position, radius, list);
}
}
}
}
private bool approximateNearest(KDTreeNode<T> current, double[] position,
ref KDTreeNode<T> match, ref double minDistance, int maxLeaves, ref int visited)
{
// Compute distance from this node to the point
double d = distance.Distance(position, current.Position);
// Base: node is leaf
if (d < minDistance)
{
minDistance = d;
match = current;
}
if (++visited > maxLeaves)
return true;
// Check for leafs on the opposite sides of
// the subtrees to nearest possible neighbors.
// Prepare for recursion. The following null checks
// will be used to avoid function calls if possible
double value = position[current.Axis];
double median = current.Position[current.Axis];
double u = value - median;
if (u <= 0)
{
if (current.Left != null)
if (approximateNearest(current.Left, position, ref match, ref minDistance, maxLeaves, ref visited))
return true;
if (current.Right != null && Math.Abs(u) <= minDistance)
if (approximateNearest(current.Right, position, ref match, ref minDistance, maxLeaves, ref visited))
return true;
}
else
{
if (current.Right != null)
approximateNearest(current.Right, position,
ref match, ref minDistance, maxLeaves, ref visited);
if (current.Left != null && Math.Abs(u) <= minDistance)
if (approximateNearest(current.Left, position, ref match, ref minDistance, maxLeaves, ref visited))
return true;
}
return false;
}
private static KDTreeNode <T> create(double[][] points, T[] values,
int depth, int k, int start, int length, ElementComparer comparer, ref int leaves)
{
if (length <= 0)
{
return(null);
}
// We will be doing sorting in place
int axis = comparer.Index = depth % k;
Array.Sort(points, values, start, length, comparer);
// Middle of the input section
int half = start + length / 2;
// Start and end of the left branch
int leftStart = start;
int leftLength = half - start;
// Start and end of the right branch
int rightStart = half + 1;
int rightLength = length - length / 2 - 1;
// The median will be located halfway in the sorted array
var median = points[half];
var value = values != null ? values[half] : default(T);
depth++;
// Continue with the recursion, passing the appropriate left and right array sections
KDTreeNode <T> left = create(points, values, depth, k, leftStart, leftLength, comparer, ref leaves);
KDTreeNode <T> right = create(points, values, depth, k, rightStart, rightLength, comparer, ref leaves);
if (left == null && right == null)
{
leaves++;
}
// Backtrack and create
return(new KDTreeNode <T>()
{
Axis = axis,
Position = median,
Value = value,
Left = left,
Right = right,
});
}
private void nearest(KDTreeNode <T> current, double[] position, ref KDTreeNode <T> match, ref double minDistance)
{
// Compute distance from this node to the point
double d = distance(position, current.Position);
if (d < minDistance)
{
minDistance = d;
match = current;
}
// Check for leafs on the opposite sides of
// the subtrees to nearest possible neighbors.
// Prepare for recursion. The following null checks
// will be used to avoid function calls if possible
double value = position[current.Axis];
double median = current.Position[current.Axis];
double u = value - median;
if (u <= 0)
{
if (current.Left != null)
{
nearest(current.Left, position, ref match, ref minDistance);
}
if (current.Right != null && u <= minDistance)
{
nearest(current.Right, position, ref match, ref minDistance);
}
}
else
{
if (current.Right != null)
{
nearest(current.Right, position, ref match, ref minDistance);
}
if (current.Left != null && u <= minDistance)
{
nearest(current.Left, position, ref match, ref minDistance);
}
}
}
internal static KDTree <T> FromData(double[][] points, T[] values, Func <double[], double[], double> distance)
{
// Initial argument checks for creating the tree
if (points == null)
{
throw new ArgumentNullException("points");
}
if (distance == null)
{
throw new ArgumentNullException("distance");
}
if (values != null && points.Length != values.Length)
{
throw new DimensionMismatchException("values");
}
int dimensions = points[0].Length;
// Create a comparer to compare individual array
// elements at specified positions when sorting
ElementComparer comparer = new ElementComparer();
// Since all sorting will be done in-place, we
// will register the original order of values
int[] idx = Matrix.Indices(0, points.Length);
// Call the recursive algorithm to operate on the whole array (from 0 to points.Length)
KDTreeNode <T> root = create(points, idx, values, 0, dimensions, 0, points.Length, comparer);
// Restore the original ordering
Array.Sort(idx, points);
// Create and return the newly formed tree
KDTree <T> tree = new KDTree <T>(dimensions);
tree.root = root;
tree.count = points.Length;
tree.distance = distance;
return(tree);
}
/// <summary>
/// Creates a new <see cref="KDTree<Object>"/>.
/// </summary>
///
/// <param name="dimension">The number of dimensions in the tree.</param>
/// <param name="root">The root node, if already existent.</param>
/// <param name="count">The number of elements in the root node.</param>
/// <param name="leaves">The number of leaves linked through the root node.</param>
///
public KDTree(int dimension, KDTreeNode <Object> root, int count, int leaves)
: base(dimension, root, count, leaves)
{
}
/// <summary>
/// Creates a new <see cref="KDTree<Object>"/>.
/// </summary>
///
/// <param name="dimension">The number of dimensions in the tree.</param>
/// <param name="root">The root node, if already existent.</param>
///
public KDTree(int dimension, KDTreeNode <Object> root)
: base(dimension, root)
{
}
/// <summary>
/// Creates a new <see cref="KDTree<Object>"/>.
/// </summary>
///
/// <param name="dimension">The number of dimensions in the tree.</param>
/// <param name="root">The root node, if already existent.</param>
///
public KDTree(int dimension, KDTreeNode root)
: base(dimension, root)
{
}
/// <summary>
/// Removes all nodes from this tree.
/// </summary>
///
public void Clear()
{
this.root = null;
}
private bool approximateNearest(KDTreeNode <T> current, double[] position,
ref KDTreeNode <T> match, ref double minDistance, int maxLeaves, ref int visited)
{
// Compute distance from this node to the point
double d = distance(position, current.Position);
if (current.IsLeaf)
{
// Base: node is leaf
if (d < minDistance)
{
minDistance = d;
match = current;
}
visited++;
if (visited > maxLeaves)
{
return(true);
}
}
else
{
// Check for leafs on the opposite sides of
// the subtrees to nearest possible neighbors.
// Prepare for recursion. The following null checks
// will be used to avoid function calls if possible
double value = position[current.Axis];
double median = current.Position[current.Axis];
double u = (value - median);
if (value < median)
{
if (current.Left != null)
{
if (approximateNearest(current.Left, position,
ref match, ref minDistance, maxLeaves, ref visited))
{
return(true);
}
}
if (u < 0)
{
minDistance = d;
match = current;
}
if (current.Right != null)
{
if ((u * u) < minDistance)
{
if (approximateNearest(current.Right, position,
ref match, ref minDistance, maxLeaves, ref visited))
{
return(true);
}
}
}
}
else
{
if (current.Right != null)
{
approximateNearest(current.Right, position,
ref match, ref minDistance, maxLeaves, ref visited);
}
if (d < minDistance)
{
minDistance = d;
match = current;
}
if (current.Left != null)
{
if ((u * u) < minDistance)
{
if (approximateNearest(current.Left, position,
ref match, ref minDistance, maxLeaves, ref visited))
{
return(true);
}
}
}
}
}
return(false);
}
/// <summary>
/// Creates a new <see cref="KDTree<T>"/>.
/// </summary>
///
/// <param name="dimension">The number of dimensions in the tree.</param>
/// <param name="root">The root node, if already existent.</param>
///
public KDTree(int dimension, KDTreeNode <T> root)
: this(dimension)
{
this.root = root;
}
/// <summary>
/// Creates a new <see cref="KDTreeNodeDistance<T>"/>.
/// </summary>
///
/// <param name="node">The node value.</param>
/// <param name="distance">The distance value.</param>
///
public KDTreeNodeDistance(KDTreeNode <T> node, double distance)
{
this.node = node;
this.distance = distance;
}