Os' moje rishenn'a...

KdTree.java

import edu.princeton.cs.algs4.Point2D;
import edu.princeton.cs.algs4.RectHV;
import edu.princeton.cs.algs4.StdDraw;

import java.awt.Color;
import java.util.ArrayList;
import java.util.List;

public class KdTree {
    private TreeNode root;
    private int size;
    private static final long EPSILON = 1000;

    public boolean isEmpty() {
        return size == 0;
    }

    public int size() {
        return size;
    }

    public void insert(Point2D p) {
        if (p == null) {
            throw new IllegalArgumentException();
        }
        if (root == null) {
            root = new TreeNode(p, new RectHV(0, 0, 1, 1));
            size++;
            return;
        }
        TreeNode i = root;
        boolean compareX = true;
        while (true) {
            long ix    = Double.doubleToLongBits(i.p.x());
            long px = Double.doubleToLongBits(p.x());
            long iy    = Double.doubleToLongBits(i.p.y());
            long py = Double.doubleToLongBits(p.y());

            if (Math.abs(ix - px) < EPSILON && Math.abs(iy - py) < EPSILON) {
                return;
            }
            if (compareX ? (i.p.x() <= p.x()) : (i.p.y() <= p.y())) {
                if (i.left == null) {
                    i.left = new TreeNode(p, divideRectangle(i.rect, i.p, p, compareX));
                    size++;
                    return;
                }
                i = i.left;
                compareX = !compareX;
            }
            if (compareX ? (i.p.x() >= p.x()) : (i.p.y() >= p.y())) {
                if (i.right == null) {
                    i.right = new TreeNode(p, divideRectangle(i.rect, i.p, p, compareX));
                    size++;
                    return;
                }
                i = i.right;
                compareX = !compareX;
            }
        }
    }

    private RectHV divideRectangle(RectHV rect, Point2D parent, Point2D p, boolean vertical) {
        if (vertical) {
            final RectHV top =
                    new RectHV(rect.xmin(), Math.min(parent.y(), rect.ymax()),
                               rect.xmax(), Math.max(parent.y(), rect.ymax()));
            return top.contains(p) ? top :
                   new RectHV(rect.xmin(), Math.min(parent.y(), rect.ymin()),
                              rect.xmax(), Math.max(parent.y(), rect.ymin()));
        }
        else {
            final RectHV left =
                    new RectHV(Math.min(parent.x(), rect.xmin()), rect.ymin(),
                               Math.max(parent.x(), rect.xmin()), rect.ymax());
            return left.contains(p) ? left :
                   new RectHV(Math.min(parent.x(), rect.xmax()), rect.ymin(),
                              Math.max(parent.x(), rect.xmax()), rect.ymax());
        }
    }

    public boolean contains(Point2D p) {
        if (p == null) {
            throw new IllegalArgumentException();
        }
        if (root == null) {
            return false;
        }
        TreeNode i = root;
        boolean compareX = true;
        while (true) {
            long ix    = Double.doubleToLongBits(i.p.x());
            long px = Double.doubleToLongBits(p.x());
            long iy    = Double.doubleToLongBits(i.p.y());
            long py = Double.doubleToLongBits(p.y());

            if (Math.abs(ix - px) < EPSILON && Math.abs(iy - py) < EPSILON) {
                return true;
            }
            if (compareX ? (i.p.x() <= p.x()) : (i.p.y() <= p.y())) {
                if (i.left == null) {
                    return false;
                }
                i = i.left;
                compareX = !compareX;
            }
            if (compareX ? (i.p.x() >= p.x()) : (i.p.y() >= p.y())) {
                if (i.right == null) {
                    return false;
                }
                i = i.right;
                compareX = !compareX;
            }
        }
    }

    public void draw() {
        drawRecursive(root, null, true);
    }

    private static void drawRecursive(TreeNode i, TreeNode parent, boolean vertical) {
        if (i == null) {
            return;
        }
        final double ix = i.p.x();
        final double iy = i.p.y();
        StdDraw.filledCircle(ix, iy, 0.01);
        if (parent == null) {
            StdDraw.setPenColor(Color.RED);
            StdDraw.line(ix, 0, ix, 1);
            StdDraw.setPenColor(Color.BLACK);
        }
        else {
            if (vertical) {
                StdDraw.setPenColor(Color.RED);
                StdDraw.line(ix, i.rect.ymin(), ix, i.rect.ymax());
                StdDraw.setPenColor(Color.BLACK);
            }
            else {
                StdDraw.setPenColor(Color.BLUE);
                StdDraw.line(i.rect.xmin(), iy, i.rect.xmax(), iy);
                StdDraw.setPenColor(Color.BLACK);
            }
        }
        drawRecursive(i.left, i, !vertical);
        drawRecursive(i.right, i, !vertical);
    }

    public Iterable<Point2D> range(RectHV rect) {
        if (rect == null) {
            throw new IllegalArgumentException();
        }
        if (root == null) {
            return null;
        }
        List<Point2D> result = new ArrayList<>();
        inspect(root, rect, result);
        return result;
    }

    private void inspect(TreeNode i, RectHV rect, List<Point2D> result) {
        if (i != null && i.rect.intersects(rect)) {
            if (rect.contains(i.p)) {
                result.add(i.p);
            }
            inspect(i.left, rect, result);
            inspect(i.right, rect, result);
        }
    }

    public Point2D nearest(Point2D p) {
        if (p == null) {
            throw new IllegalArgumentException();
        }
        return isEmpty() ? null : closest(p, root.p, root);
    }

    private static Point2D closest(Point2D target, Point2D nearest, TreeNode node) {
        if (node == null) {
            return nearest;
        }
        double nearestDist = nearest.distanceSquaredTo(target);
        if (node.rect.distanceSquaredTo(target) < nearestDist) {
            double nodeDist = node.p.distanceSquaredTo(target);
            if (nodeDist < nearestDist) {
                nearest = node.p;
            }
            nearest = closest(target, nearest, node.left);
            nearest = closest(target, nearest, node.right);
        }
        return nearest;
    }

    private static class TreeNode {
        private final Point2D p;
        private TreeNode left;
        private TreeNode right;
        private final RectHV rect;

        public TreeNode(Point2D p, RectHV rect) {
            this.p = p;
            this.rect = rect;
        }
    }
}