private void Range(KdNode node, RectHV nrect, RectHV rect, Queue<Point2D> queue) {
if (node == null) {
return;
}
if (rect.Intersects(nrect)) {
var p = new Point2D(node.X, node.Y);
if (rect.Contains(p)) {
queue.Enqueue(p);
}
Range(node.Left, LeftRect(nrect, node), rect, queue);
Range(node.Right, RightRect(nrect, node), rect, queue);
}
}
private KdNode Insert(KdNode node, Point2D p, bool vertical) {
if (node == null) {
size++;
return new KdNode(p.X, p.Y, vertical);
}
if (node.X.Equals(p.X) && node.Y.Equals(p.Y)) {
return node;
}
if (node.Vertical && p.X < node.X || !node.Vertical && p.Y < node.Y) {
node.Left = Insert(node.Left, p, !node.Vertical);
}
else {
node.Right = Insert(node.Right, p, !node.Vertical);
}
return node;
}
public bool Contains(Point2D p) {
return Contains(root, p.X, p.Y);
}
public void Insert(Point2D point) {
root = Insert(root, point, vertical:true);
}
public double DistanceSquaredTo(Point2D p) {
double dx = 0, dy = 0;
if (p.X < Xmin) {
dx = p.X - Xmin;
}
else if (p.X > Xmax) {
dx = p.X - Xmax;
}
if (p.Y < Ymin) {
dy = p.Y - Ymin;
}
else if (p.Y > Ymax) {
dy = p.Y - Ymax;
}
return dx * dx + dy * dy;
}
public double DistanceTo(Point2D p) {
return Math.Sqrt(DistanceSquaredTo(p));
}
public bool Contains(Point2D p) {
return (p.X >= Xmin && p.X <= Xmax)
&& (p.Y >= Ymin && p.Y <= Ymax);
}
public double DistanceSquaredTo(Point2D that) {
var dx = X - that.X;
var dy = Y - that.Y;
return dx * dx + dy * dy;
}
public double DistanceTo(Point2D that) {
return Math.Sqrt(DistanceSquaredTo(that));
}
private Point2D Nearest(KdNode node, RectHV rect, double x, double y, Point2D candidate) {
if (node == null) {
return candidate;
}
double dqn = 0;
double drq = 0;
var query = new Point2D(x, y);
var nearest = candidate;
if (nearest != null) {
dqn = query.DistanceSquaredTo(nearest);
drq = rect.DistanceSquaredTo(query);
}
if (nearest == null || dqn < drq) {
var point = new Point2D(node.X, node.Y);
if (nearest == null || dqn > query.DistanceSquaredTo(point)) {
nearest = point;
}
}
var left = LeftRect(rect, node);
var right = RightRect(rect, node);
if (node.Vertical) {
if (x < node.X) {
nearest = Nearest(node.Left, left, x, y, nearest);
nearest = Nearest(node.Right, right, x, y, nearest);
}
else {
nearest = Nearest(node.Right, right, x, y, nearest);
nearest = Nearest(node.Left, left, x, y, nearest);
}
}
else {
if (y < node.Y) {
nearest = Nearest(node.Left, left, x, y, nearest);
nearest = Nearest(node.Right, right, x, y, nearest);
}
else {
nearest = Nearest(node.Right, right, x, y, nearest);
nearest = Nearest(node.Left, left, x, y, nearest);
}
}
return nearest;
}
public Point2D Nearest(Point2D p) {
return Nearest(root, Container, p.X, p.Y, null);
}