private Point2D FindNearestRecursive(Node x, Point2D p, Point2D closest, bool isVertical)
{
double a = p.DistanceSquaredTo(x.p);
double b = p.DistanceSquaredTo(closest);
Point2D c = a < b ? x.p : closest;
double dpSquared = PerpendicularSquared(x, p, isVertical);
double minSquared = p.DistanceSquaredTo(c);
if (dpSquared < minSquared)
{
if (null != x.rt)
{
Point2D q = FindNearestRecursive(x.rt, p, c, !isVertical);
double qSquared = p.DistanceSquaredTo(q);
if (qSquared < minSquared)
{
c = q;
minSquared = qSquared;
}
}
if (null != x.lb)
{
Point2D q = FindNearestRecursive(x.lb, p, c, !isVertical);
double qSquared = p.DistanceSquaredTo(q);
if (qSquared < minSquared)
{
c = q;
//minSquared = qSquared;
}
}
}
else
{
bool isLB = isVertical ? p.X < x.p.X : p.Y < x.p.Y;
if (isLB)
{
if (x.lb != null)
{
return(FindNearestRecursive(x.lb, p, c, !isVertical));
}
}
else
{
if (x.rt != null)
{
return(FindNearestRecursive(x.rt, p, c, !isVertical));
}
}
}
return(c);
}
private double PerpendicularSquared(Node x, Point2D p, bool isVertical)
{
Point2D q = isVertical ? new Point2D(x.p.X, p.Y) : new Point2D(p.X, x.p.Y);
return(p.DistanceSquaredTo(q));
}