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