private static KdTreeNode Construct(KdTree tree, Vector[] elements, int depth)
{
var length = elements.GetLength(0);
if (length == 0)
{
return(null);
}
// Sort array by dimension
var dimension = depth % tree.Dimensionality;
Array.Sort(
elements,
(a, b) => a[dimension].CompareTo(b[dimension]) // fancy delegate here.. ooh urr..
);
// Get median element from sorted array
var medianIndex = length / 2;
var medianElement = elements[medianIndex];
// Get sub-tree elements
var leftElements = elements.Take(medianIndex).ToArray();
var rightElements = elements.Skip(medianIndex + 1).ToArray();
// Create node and connect sub-trees
var node = new KdTreeNode(medianElement);
node.LeftChild = Construct(tree, leftElements, depth + 1);
node.RightChild = Construct(tree, rightElements, depth + 1);
return(node);
}
} // private empty constructor
/// <summary>
/// Initialises a new instance of the <see cref="KdTree"/> class with the specified root node.
/// </summary>
/// <param name="dimensionality">The dimensionality of the underlying <see cref="Vector"/>.</param>
/// <param name="root">The root node of the tree. Cannot be null.</param>
public KdTree(int dimensionality, KdTreeNode root)
: this()
{
if (root == null)
{
throw new ArgumentNullException("root");
}
this.Dimensionality = dimensionality;
this.Root = root;
}
private Vector FindNearestNeighbour(Vector location,
KdTreeNode node, Vector bestValue, float bestDistance, int depth)
{
if (node == null) // no where left to search, return best value
{
return(bestValue);
}
var dimension = depth % this.Dimensionality;
var nodeValue = node.Value;
var distance = (nodeValue - location).Norm(this.Dimensionality);
// Check if current node is better than best node.
// Current node cannot be same as search location.
if (!Precision.AlmostEqual(distance, 0) &&
(distance < bestDistance))
{
bestValue = nodeValue;
bestDistance = distance;
}
// Check for best node in sub-tree of near child. i.e., which side of Node is the query point?
var nearChildNode = (location[dimension] < nodeValue[dimension]) ?
node.LeftChild : node.RightChild;
if (nearChildNode != null)
{
var nearBestValue = FindNearestNeighbour(location, nearChildNode, bestValue, bestDistance, depth + 1);
var nearBestDistance = (nearBestValue - location).Norm(this.Dimensionality);
bestValue = nearBestValue;
bestDistance = nearBestDistance;
}
// Check whether splitting hyperplane given by current node intersects with hypersphere of current smallest
// distance around given location.
if (bestDistance > Math.Abs(nodeValue[dimension] - location[dimension]))
{
// Check for best node in sub-tree of far child.
var farChildNode = nearChildNode == node.LeftChild ? node.RightChild : node.LeftChild; // i.e. not the near child
if (farChildNode != null)
{
var farBestValue = FindNearestNeighbour(location, farChildNode, bestValue, bestDistance, depth + 1);
var farBestDistance = (farBestValue - location).Norm(this.Dimensionality);
bestValue = farBestValue;
bestDistance = farBestDistance;
}
}
return(bestValue);
}
private KdTreeNode Find(Vector value, KdTreeNode node, int depth)
{
if (node == null)
{
return(null);
}
if (node.Value.Equals(value))
{
return(node);
}
var dimension = depth % this.Dimensionality;
var foundNode = (value[dimension] < node.Value[dimension]) ?
Find(value, node.LeftChild, depth + 1) : Find(value, node.RightChild, depth + 1);
return(foundNode);
}
private void FindInRange(Vector location,
KdTreeNode node, float range, IList <Vector> valuesList, int depth)
{
if (node == null)
{
return;
}
var dimension = depth % this.Dimensionality;
var distance = (node.Value - location).Norm(this.Dimensionality);
// Add current node to list if it lies within given range.
// Current node cannot be same as search location.
if (!Precision.AlmostEqual(distance, 0.0f) &&
(distance < range))
{
valuesList.Add(node.Value);
}
// Check for nodes in sub-tree of near child.
var nearChildNode = (location[dimension] < node.Value[dimension]) ?
node.LeftChild : node.RightChild;
if (nearChildNode != null)
{
FindInRange(location, nearChildNode, range, valuesList, depth + 1);
}
// Check whether splitting hyperplane given by current node intersects with hypersphere of current
// smallest distance around given location.
if ((range > Math.Abs(node.Value[dimension] - location[dimension])))
{
// Check for nodes in sub-tree of far child.
var farChildNode = nearChildNode == node.LeftChild ? node.RightChild : node.LeftChild;
if (farChildNode != null)
{
FindInRange(location, farChildNode, range, valuesList, depth + 1);
}
}
}
private KdTreeNode Add(Vector value, KdTreeNode node, int depth)
{
if (node == null)
{
node = new KdTreeNode(value);
}
else
{
// Check if node should be added to left or right sub-tree of current node.
var dimension = depth % this.Dimensionality;
if (!(value[dimension] > node.Value[dimension]))
{
node.LeftChild = Add(value, node.LeftChild, depth + 1);
}
else
{
node.RightChild = Add(value, node.RightChild, depth + 1);
}
}
return(node);
}
