public void Visit(KdNode <T> node)
{
double dist = p.Distance(node.Coordinate);
bool isInTolerance = dist <= tolerance;
if (!isInTolerance)
{
return;
}
bool update = false;
if (matchNode == null ||
dist < matchDist
// if distances are the same, record the lesser coordinate
|| (matchNode != null && dist == matchDist &&
node.Coordinate.CompareTo(matchNode.Coordinate) < 1))
{
update = true;
}
if (update)
{
matchNode = node;
matchDist = dist;
}
}
/// <summary>
/// Inserts a new point into the kd-tree.
/// </summary>
/// <param name="p">The point to insert</param>
/// <param name="data">A data item for the point</param>
/// <returns>
/// A new KdNode if a new point is inserted, else an existing
/// node is returned with its counter incremented. This can be checked
/// by testing returnedNode.getCount() > 1.
/// </returns>
public KdNode <T> Insert(Coordinate p, T data)
{
if (_root == null)
{
_root = new KdNode <T>(p, data);
return(_root);
}
/**
* Check if the point is already in the tree, up to tolerance.
* If tolerance is zero, this phase of the insertion can be skipped.
*/
if (_tolerance > 0)
{
var matchNode = FindBestMatchNode(p);
if (matchNode != null)
{
// point already in index - increment counter
matchNode.Increment();
return(matchNode);
}
}
return(InsertExact(p, data));
}
/// <summary>
/// Performs a nearest neighbor search of the points in the index.
/// </summary>
/// <param name="self">The KdTree to look for the nearest neighbor</param>
/// <param name="coord">The point to search the nearset neighbor for</param>
public static KdNode <T> NearestNeighbor <T>(this KdTree <T> self, Coordinate coord) where T : class
{
KdNode <T> result = null;
double closestDistSq = double.MaxValue;
NearestNeighbor(self.Root, coord, ref result, ref closestDistSq, true);
return(result);
}
private static bool NeedsToBeSearched <T>(Coordinate target, KdNode <T> node,
double closestDistSq, bool left, bool isOddLevel) where T : class
{
if (isOddLevel)
{
return((left ? target.X <= node.X : target.X >= node.X) || Math.Pow(target.X - node.X, 2) < closestDistSq);
}
return((left ? target.Y <= node.Y : target.Y >= node.Y) || Math.Pow(target.Y - node.Y, 2) < closestDistSq);
}
private static void NearestNeighbor <T>(
KdNode <T> currentNode, Coordinate queryCoordinate,
ref KdNode <T> closestNode, ref double closestDistanceSq,
bool isOddLevel) where T : class
{
while (true)
{
if (currentNode == null)
{
return;
}
double distSq = Math.Pow(currentNode.X - queryCoordinate.X, 2) +
Math.Pow(currentNode.Y - queryCoordinate.Y, 2);
if (distSq < closestDistanceSq)
{
closestNode = currentNode;
closestDistanceSq = distSq;
}
// Start with the branch that is more likely to produce the final result
var firstBranch = currentNode.Left;
var secondBranch = currentNode.Right;
if (currentNode.Left != null &&
(isOddLevel ? queryCoordinate.X >= currentNode.X : queryCoordinate.Y >= currentNode.Y))
{
firstBranch = currentNode.Right;
secondBranch = currentNode.Left;
}
if (firstBranch != null &&
NeedsToBeSearched(queryCoordinate, currentNode, closestDistanceSq,
firstBranch == currentNode.Left, isOddLevel))
{
NearestNeighbor(firstBranch, queryCoordinate, ref closestNode, ref closestDistanceSq, !isOddLevel);
}
if (secondBranch != null &&
NeedsToBeSearched(queryCoordinate, currentNode, closestDistanceSq,
secondBranch == currentNode.Left, isOddLevel))
{
currentNode = secondBranch;
isOddLevel = !isOddLevel;
continue;
}
break;
}
}
private static void QueryNode(KdNode <T> currentNode,
Envelope queryEnv, bool odd, IKdNodeVisitor <T> visitor)
{
if (currentNode == null)
{
return;
}
double min;
double max;
double discriminant;
if (odd)
{
min = queryEnv.MinX;
max = queryEnv.MaxX;
discriminant = currentNode.X;
}
else
{
min = queryEnv.MinY;
max = queryEnv.MaxY;
discriminant = currentNode.Y;
}
bool searchLeft = min < discriminant;
bool searchRight = discriminant <= max;
// search is computed via in-order traversal
if (searchLeft)
{
QueryNode(currentNode.Left, queryEnv, !odd, visitor);
}
if (queryEnv.Contains(currentNode.Coordinate))
{
visitor.Visit(currentNode);
}
if (searchRight)
{
QueryNode(currentNode.Right, queryEnv, !odd, visitor);
}
}
public void Visit(KdNode <T> node)
{
_result.Add(node);
}
/// <summary>
/// Inserts a point known to be beyond the distance tolerance of any existing node.
/// The point is inserted at the bottom of the exact splitting path,
/// so that tree shape is deterministic.
/// </summary>
/// <param name="p">The point to insert</param>
/// <returns>
/// <list type="bullet">
/// <item><description>The data for the point</description></item>
/// <item><description>The created node</description></item>
/// </list>
/// </returns>
public KdNode <T> InsertExact(Coordinate p, T data)
{
var currentNode = _root;
var leafNode = _root;
bool isOddLevel = true;
bool isLessThan = true;
/**
* Traverse the tree, first cutting the plane left-right (by X ordinate)
* then top-bottom (by Y ordinate)
*/
while (currentNode != null)
{
// test if point is already a node (not strictly necessary)
if (currentNode != null)
{
bool isInTolerance = p.Distance(currentNode.Coordinate) <= _tolerance;
// check if point is already in tree (up to tolerance) and if so simply
// return existing node
if (isInTolerance)
{
currentNode.Increment();
return(currentNode);
}
}
if (isOddLevel)
{
// ReSharper disable once PossibleNullReferenceException
isLessThan = p.X < currentNode.X;
}
else
{
// ReSharper disable once PossibleNullReferenceException
isLessThan = p.Y < currentNode.Y;
}
leafNode = currentNode;
currentNode = isLessThan
? currentNode.Left
: currentNode.Right;
isOddLevel = !isOddLevel;
}
// no node found, add new leaf node to tree
_numberOfNodes = _numberOfNodes + 1;
var node = new KdNode <T>(p, data);
node.Left = null;
node.Right = null;
if (isLessThan)
{
leafNode.Left = node;
}
else
{
leafNode.Right = node;
}
return(node);
}
