/// <summary>
/// Recursively gather candidates.
/// </summary>
/// <param name="location">location we are searching around</param>
/// <param name="node">node</param>
/// <param name="radius">radius in which we are searching</param>
/// <param name="candidates"></param>
private void GatherCandidates(Vector3 location, KdNode <T> node, float radius, IList <T> candidates)
{
// Leaf node: add all candiadtes.
if (node.IsLeaf)
{
foreach (var element in node.LeafElements)
{
candidates.Add(element);
}
return;
}
var value = KdNode <T> .GetDimension(node.Dimension, location);
if (Math.Abs(value - node.Threshold) < radius)
{
// Case: we're within (radius) of the boundary, so check both sides.
GatherCandidates(location, node.Left, radius, candidates);
GatherCandidates(location, node.Right, radius, candidates);
}
else if (value > node.Threshold)
{
// Case: we're a safe distance on the left hand side.
GatherCandidates(location, node.Left, radius, candidates);
}
else
{
// Case: we're a safe distance on the right hand side.
GatherCandidates(location, node.Right, radius, candidates);
}
}
public KdNode(byte dimension, float threshold, KdNode <T> left, KdNode <T> right)
{
Dimension = dimension;
Threshold = threshold;
Left = left;
Right = right;
}
/// <summary>
/// Creates a node from a set of elements.
/// </summary>
/// <typeparam name="T"></typeparam>
/// <param name="splitOver"></param>
/// <param name="elements"></param>
/// <param name="locatorFunction"></param>
/// <returns></returns>
private KdNode <T> CreateNode(IEnumerable <T> elements, byte dimension = 0)
{
if (elements.Count() <= _splitOver)
{
return(new KdNode <T>(elements));
}
var positions = elements.Select(_locatorFunc);
var values = positions.Select(x => KdNode <T> .GetDimension(dimension, x));
var threshold = GetCenter(values);
var leftElements = new List <T>();
var rightElements = new List <T>();
// Sort elements into left and right elements.
foreach (var el in elements)
{
var value = KdNode <T> .GetDimension(dimension, _locatorFunc(el));
if (value > threshold)
{
leftElements.Add(el);
}
else
{
rightElements.Add(el);
}
}
// To prevent a bug occuring when all items are in the same position.
if (leftElements.Count() == 0)
{
return(new KdNode <T>(rightElements));
}
if (rightElements.Count() == 0)
{
return(new KdNode <T>(leftElements));
}
var nextDimension = (byte)((dimension + 1) % _dimensions);
return(new KdNode <T>(
dimension,
threshold,
CreateNode(leftElements, nextDimension),
CreateNode(rightElements, nextDimension)
));
}
/// <summary>
/// Decides whether a location is on the left or right of a node based on it's location for non-leaf nodes.
/// For leaf nodes the leaf node itself is returned.
/// </summary>
/// <param name="location">world location</param>
/// <param name="node">node</param>
/// <returns></returns>
private KdNode <T> Decide(Vector3 location, KdNode <T> node)
{
if (node.IsLeaf)
{
return(node);
}
var value = KdNode <T> .GetDimension(node.Dimension, location);
if (value > node.Threshold)
{
return(Decide(location, node.Left));
}
else
{
return(Decide(location, node.Right));
}
}
internal KdTree(KdNode <T> root, Func <T, Vector3> locatorFunc)
{
Root = root;
_locatorFunc = locatorFunc;
}