algorithms/data_structures/KdTree.java

package com.jwetherell.algorithms.data_structures;

/* 
 * Borrowed from 
 * https://github.com/phishman3579/java-algorithms-implementation
 */

import java.util.ArrayList;
import java.util.Collection;
import java.util.Collections;
import java.util.Comparator;
import java.util.HashSet;
import java.util.List;
import java.util.Set;
import java.util.TreeSet;

/**
 * A k-d tree (short for k-dimensional tree) is a space-partitioning data
 * structure for organizing points in a k-dimensional space. k-d trees are a
 * useful data structure for several applications, such as searches involving a
 * multidimensional search key (e.g. range searches and nearest neighbor
 * searches). k-d trees are a special case of binary space partitioning trees.
 * 
 * http://en.wikipedia.org/wiki/K-d_tree
 * 
 * @author Justin Wetherell <phishman3579@gmail.com>
 */
public class KdTree<T extends KdTree.XYZPoint> {

    private int k = 3;
    private KdNode root = null;

    private static final Comparator<XYZPoint> X_COMPARATOR = new Comparator<XYZPoint>() {
        /**
         * {@inheritDoc}
         */
        @Override
        public int compare(XYZPoint o1, XYZPoint o2) {
            if (o1.x < o2.x)
                return -1;
            if (o1.x > o2.x)
                return 1;
            return 0;
        }
    };

    private static final Comparator<XYZPoint> Y_COMPARATOR = new Comparator<XYZPoint>() {
        /**
         * {@inheritDoc}
         */
        @Override
        public int compare(XYZPoint o1, XYZPoint o2) {
            if (o1.y < o2.y)
                return -1;
            if (o1.y > o2.y)
                return 1;
            return 0;
        }
    };

    private static final Comparator<XYZPoint> Z_COMPARATOR = new Comparator<XYZPoint>() {
        /**
         * {@inheritDoc}
         */
        @Override
        public int compare(XYZPoint o1, XYZPoint o2) {
            if (o1.z < o2.z)
                return -1;
            if (o1.z > o2.z)
                return 1;
            return 0;
        }
    };

    protected static final int X_AXIS = 0;
    protected static final int Y_AXIS = 1;
    protected static final int Z_AXIS = 2;

    /**
     * Default constructor.
     */
    public KdTree() { }

    /**
     * Constructor for creating a more balanced tree. It uses the
     * "median of points" algorithm.
     * 
     * @param list
     *            of XYZPoints.
     */
    public KdTree(List<XYZPoint> list) {
        root = createNode(list, k, 0);
    }

    /**
     * Constructor for creating a more balanced tree. It uses the
     * "median of points" algorithm.
     * 
     * @param list
     *            of XYZPoints.
     * @param k
     *            of the tree.
     */
    public KdTree(List<XYZPoint> list, int k) {
        root = createNode(list, k, 0);
    }

    /**
     * Create node from list of XYZPoints.
     * 
     * @param list
     *            of XYZPoints.
     * @param k
     *            of the tree.
     * @param depth
     *            depth of the node.
     * @return node created.
     */
    private static KdNode createNode(List<XYZPoint> list, int k, int depth) {
        if (list == null || list.size() == 0)
            return null;

        int axis = depth % k;
        if (axis == X_AXIS)
            Collections.sort(list, X_COMPARATOR);
        else if (axis == Y_AXIS)
            Collections.sort(list, Y_COMPARATOR);
        else
            Collections.sort(list, Z_COMPARATOR);

        KdNode node = null;
        List<XYZPoint> less = new ArrayList<XYZPoint>(list.size());
        List<XYZPoint> more = new ArrayList<XYZPoint>(list.size());
        if (list.size() > 0) {
            int medianIndex = list.size() / 2;
            node = new KdNode(list.get(medianIndex), k, depth);
            // Process list to see where each non-median point lies
            for (int i = 0; i < list.size(); i++) {
                if (i == medianIndex)
                    continue;
                XYZPoint p = list.get(i);
                // Cannot assume points before the median are less since they could be equal
                if (KdNode.compareTo(depth, k, p, node.id) <= 0) {
                    less.add(p);
                } else {
                    more.add(p);
                }
            }

            if ((medianIndex - 1) >= 0 && less.size() > 0) {
                node.lesser = createNode(less, k, depth + 1);
                node.lesser.parent = node;
            }

            if ((medianIndex + 1) <= (list.size() - 1) && more.size() > 0) {
                node.greater = createNode(more, k, depth + 1);
                node.greater.parent = node;
            }
        }

        return node;
    }

    /**
     * Add value to the tree. Tree can contain multiple equal values.
     * 
     * @param value
     *            T to add to the tree.
     * @return True if successfully added to tree.
     */
    public boolean add(T value) {
        if (value == null)
            return false;

        if (root == null) {
            root = new KdNode(value);
            return true;
        }

        KdNode node = root;
        while (true) {
            if (KdNode.compareTo(node.depth, node.k, value, node.id) <= 0) {
                // Lesser
                if (node.lesser == null) {
                    KdNode newNode = new KdNode(value, k, node.depth + 1);
                    newNode.parent = node;
                    node.lesser = newNode;
                    break;
                }
                node = node.lesser;
            } else {
                // Greater
                if (node.greater == null) {
                    KdNode newNode = new KdNode(value, k, node.depth + 1);
                    newNode.parent = node;
                    node.greater = newNode;
                    break;
                }
                node = node.greater;
            }
        }

        return true;
    }

    /**
     * Does the tree contain the value.
     * 
     * @param value
     *            T to locate in the tree.
     * @return True if tree contains value.
     */
    public boolean contains(T value) {
        if (value == null || root == null)
            return false;

        KdNode node = getNode(this, value);
        return (node != null);
    }

    /**
     * Locate T in the tree.
     * 
     * @param tree
     *            to search.
     * @param value
     *            to search for.
     * @return KdNode or NULL if not found
     */
    private static final <T extends KdTree.XYZPoint> KdNode getNode(KdTree<T> tree, T value) {
        if (tree == null || tree.root == null || value == null)
            return null;

        KdNode node = tree.root;
        while (true) {
            if (node.id.equals(value)) {
                return node;
            } else if (KdNode.compareTo(node.depth, node.k, value, node.id) <= 0) {
                // Lesser
                if (node.lesser == null) {
                    return null;
                }
                node = node.lesser;
            } else {
                // Greater
                if (node.greater == null) {
                    return null;
                }
                node = node.greater;
            }
        }
    }

    /**
     * Remove first occurrence of value in the tree.
     * 
     * @param value
     *            T to remove from the tree.
     * @return True if value was removed from the tree.
     */
    public boolean remove(T value) {
        if (value == null || root == null)
            return false;

        KdNode node = getNode(this, value);
        if (node == null)
            return false;

        KdNode parent = node.parent;
        if (parent != null) {
            if (parent.lesser != null && node.equals(parent.lesser)) {
                List<XYZPoint> nodes = getTree(node);
                if (nodes.size() > 0) {
                    parent.lesser = createNode(nodes, node.k, node.depth);
                    if (parent.lesser != null) {
                        parent.lesser.parent = parent;
                    }
                } else {
                    parent.lesser = null;
                }
            } else {
                List<XYZPoint> nodes = getTree(node);
                if (nodes.size() > 0) {
                    parent.greater = createNode(nodes, node.k, node.depth);
                    if (parent.greater != null) {
                        parent.greater.parent = parent;
                    }
                } else {
                    parent.greater = null;
                }
            }
        } else {
            // root
            List<XYZPoint> nodes = getTree(node);
            if (nodes.size() > 0)
                root = createNode(nodes, node.k, node.depth);
            else
                root = null;
        }

        return true;
    }

    /**
     * Get the (sub) tree rooted at root.
     * 
     * @param root
     *            of tree to get nodes for.
     * @return points in (sub) tree, not including root.
     */
    private static final List<XYZPoint> getTree(KdNode root) {
        List<XYZPoint> list = new ArrayList<XYZPoint>();
        if (root == null)
            return list;

        if (root.lesser != null) {
            list.add(root.lesser.id);
            list.addAll(getTree(root.lesser));
        }
        if (root.greater != null) {
            list.add(root.greater.id);
            list.addAll(getTree(root.greater));
        }

        return list;
    }

    /**
     * K Nearest Neighbor search
     * 
     * @param K
     *            Number of neighbors to retrieve. Can return more than K, if
     *            last nodes are equal distances.
     * @param value
     *            to find neighbors of.
     * @return Collection of T neighbors.
     */
    @SuppressWarnings("unchecked")
    public Collection<T> nearestNeighbourSearch(int K, T value) {
        if (value == null || root == null)
            return Collections.EMPTY_LIST;

        // Map used for results
        TreeSet<KdNode> results = new TreeSet<KdNode>(new EuclideanComparator(value));

        // Find the closest leaf node
        KdNode prev = null;
        KdNode node = root;
        while (node != null) {
            if (KdNode.compareTo(node.depth, node.k, value, node.id) <= 0) {
                // Lesser
                prev = node;
                node = node.lesser;
            } else {
                // Greater
                prev = node;
                node = node.greater;
            }
        }
        KdNode leaf = prev;

        if (leaf != null) {
            // Used to not re-examine nodes
            Set<KdNode> examined = new HashSet<KdNode>();

            // Go up the tree, looking for better solutions
            node = leaf;
            while (node != null) {
                // Search node
                searchNode(value, node, K, results, examined);
                node = node.parent;
            }
        }

        // Load up the collection of the results
        Collection<T> collection = new ArrayList<T>(K);
        for (KdNode kdNode : results)
            collection.add((T) kdNode.id);
        return collection;
    }

    private static final <T extends KdTree.XYZPoint> void searchNode(T value, KdNode node, int K, TreeSet<KdNode> results, Set<KdNode> examined) {
        examined.add(node);

        // Search node
        KdNode lastNode = null;
        Double lastDistance = Double.MAX_VALUE;
        if (results.size() > 0) {
            lastNode = results.last();
            lastDistance = lastNode.id.euclideanDistance(value);
        }
        Double nodeDistance = node.id.euclideanDistance(value);
        if (nodeDistance.compareTo(lastDistance) < 0) {
            if (results.size() == K && lastNode != null)
                results.remove(lastNode);
            results.add(node);
        } else if (nodeDistance.equals(lastDistance)) {
            results.add(node);
        } else if (results.size() < K) {
            results.add(node);
        }
        lastNode = results.last();
        lastDistance = lastNode.id.euclideanDistance(value);

        int axis = node.depth % node.k;
        KdNode lesser = node.lesser;
        KdNode greater = node.greater;

        // Search children branches, if axis aligned distance is less than
        // current distance
        if (lesser != null && !examined.contains(lesser)) {
            examined.add(lesser);

            double nodePoint = Double.MIN_VALUE;
            double valuePlusDistance = Double.MIN_VALUE;
            if (axis == X_AXIS) {
                nodePoint = node.id.x;
                valuePlusDistance = value.x - lastDistance;
            } else if (axis == Y_AXIS) {
                nodePoint = node.id.y;
                valuePlusDistance = value.y - lastDistance;
            } else {
                nodePoint = node.id.z;
                valuePlusDistance = value.z - lastDistance;
            }
            boolean lineIntersectsCube = ((valuePlusDistance <= nodePoint) ? true : false);

            // Continue down lesser branch
            if (lineIntersectsCube)
                searchNode(value, lesser, K, results, examined);
        }
        if (greater != null && !examined.contains(greater)) {
            examined.add(greater);

            double nodePoint = Double.MIN_VALUE;
            double valuePlusDistance = Double.MIN_VALUE;
            if (axis == X_AXIS) {
                nodePoint = node.id.x;
                valuePlusDistance = value.x + lastDistance;
            } else if (axis == Y_AXIS) {
                nodePoint = node.id.y;
                valuePlusDistance = value.y + lastDistance;
            } else {
                nodePoint = node.id.z;
                valuePlusDistance = value.z + lastDistance;
            }
            boolean lineIntersectsCube = ((valuePlusDistance >= nodePoint) ? true : false);

            // Continue down greater branch
            if (lineIntersectsCube)
                searchNode(value, greater, K, results, examined);
        }
    }

    /**
     * {@inheritDoc}
     */
    @Override
    public String toString() {
        return TreePrinter.getString(this);
    }

    protected static class EuclideanComparator implements Comparator<KdNode> {

        private final XYZPoint point;

        public EuclideanComparator(XYZPoint point) {
            this.point = point;
        }

        /**
         * {@inheritDoc}
         */
        @Override
        public int compare(KdNode o1, KdNode o2) {
            Double d1 = point.euclideanDistance(o1.id);
            Double d2 = point.euclideanDistance(o2.id);
            if (d1.compareTo(d2) < 0)
                return -1;
            else if (d2.compareTo(d1) < 0)
                return 1;
            return o1.id.compareTo(o2.id);
        }
    }

    public static class KdNode implements Comparable<KdNode> {

        private final XYZPoint id;
        private final int k;
        private final int depth;

        private KdNode parent = null;
        private KdNode lesser = null;
        private KdNode greater = null;

        public KdNode(XYZPoint id) {
            this.id = id;
            this.k = 3;
            this.depth = 0;
        }

        public KdNode(XYZPoint id, int k, int depth) {
            this.id = id;
            this.k = k;
            this.depth = depth;
        }

        public static int compareTo(int depth, int k, XYZPoint o1, XYZPoint o2) {
            int axis = depth % k;
            if (axis == X_AXIS)
                return X_COMPARATOR.compare(o1, o2);
            if (axis == Y_AXIS)
                return Y_COMPARATOR.compare(o1, o2);
            return Z_COMPARATOR.compare(o1, o2);
        }

        /**
         * {@inheritDoc}
         */
        @Override
        public int hashCode() {
            return 31 * (this.k + this.depth + this.id.hashCode());
        }

        /**
         * {@inheritDoc}
         */
        @Override
        public boolean equals(Object obj) {
            if (obj == null)
                return false;
            if (!(obj instanceof KdNode))
                return false;

            KdNode kdNode = (KdNode) obj;
            if (this.compareTo(kdNode) == 0)
                return true;
            return false;
        }

        /**
         * {@inheritDoc}
         */
        @Override
        public int compareTo(KdNode o) {
            return compareTo(depth, k, this.id, o.id);
        }

        /**
         * {@inheritDoc}
         */
        @Override
        public String toString() {
            StringBuilder builder = new StringBuilder();
            builder.append("k=").append(k);
            builder.append(" depth=").append(depth);
            builder.append(" id=").append(id.toString());
            return builder.toString();
        }
    }

    public static class XYZPoint implements Comparable<XYZPoint> {

        protected final double x;
        protected final double y;
        protected final double z;

      /*  public XYZPoint(double x, double y) {
            this.x = x;
            this.y = y;
            this.z = 0;
        }*/

        public XYZPoint(double x, double y, double z) {
            this.x = x;
            this.y = y;
            this.z = z;
        }
        
        public XYZPoint(double latitude, double longitude) {
                this.x = Math.cos(Math.toRadians(latitude)) * Math.cos(Math.toRadians(longitude));
                this.y = Math.cos(Math.toRadians(latitude)) * Math.sin(Math.toRadians(longitude));
                this.z = Math.sin(Math.toRadians(latitude));
        }

        public double getX() {
            return x;
        }
        public double getY() {
            return y;
        }
        public double getZ() {
            return z;
        }

        /**
         * Computes the Euclidean distance from this point to the other.
         * 
         * @param o1
         *            other point.
         * @return euclidean distance.
         */
        public double euclideanDistance(XYZPoint o1) {
            return euclideanDistance(o1, this);
        }

        /**
         * Computes the Euclidean distance from one point to the other.
         * 
         * @param o1
         *            first point.
         * @param o2
         *            second point.
         * @return euclidean distance.
         */
        private static final double euclideanDistance(XYZPoint o1, XYZPoint o2) {
            return Math.sqrt(Math.pow((o1.x - o2.x), 2) + Math.pow((o1.y - o2.y), 2) + Math.pow((o1.z - o2.z), 2));
        }

        /**
         * {@inheritDoc}
         */
        @Override
        public int hashCode() {
            return 31 * (int)(this.x + this.y + this.z);
        }

        /**
         * {@inheritDoc}
         */
        @Override
        public boolean equals(Object obj) {
            if (obj == null)
                return false;
            if (!(obj instanceof XYZPoint))
                return false;

            XYZPoint xyzPoint = (XYZPoint) obj;
            return compareTo(xyzPoint) == 0;
        }

        /**
         * {@inheritDoc}
         */
        @Override
        public int compareTo(XYZPoint o) {
            int xComp = X_COMPARATOR.compare(this, o);
            if (xComp != 0)
                return xComp;
            int yComp = Y_COMPARATOR.compare(this, o);
            if (yComp != 0)
                return yComp;
            int zComp = Z_COMPARATOR.compare(this, o);
            return zComp;
        }

        /**
         * {@inheritDoc}
         */
        @Override
        public String toString() {
            StringBuilder builder = new StringBuilder();
            builder.append("(");
            builder.append(x).append(", ");
            builder.append(y).append(", ");
            builder.append(z);
            builder.append(")");
            return builder.toString();
        }
    }

    protected static class TreePrinter {

        public static <T extends XYZPoint> String getString(KdTree<T> tree) {
            if (tree.root == null)
                return "Tree has no nodes.";
            return getString(tree.root, "", true);
        }

        private static String getString(KdNode node, String prefix, boolean isTail) {
            StringBuilder builder = new StringBuilder();

            if (node.parent != null) {
                String side = "left";
                if (node.parent.greater != null && node.id.equals(node.parent.greater.id))
                    side = "right";
                builder.append(prefix + (isTail ? "└── " : "├── ") + "[" + side + "] " + "depth=" + node.depth + " id="
                        + node.id + "\n");
            } else {
                builder.append(prefix + (isTail ? "└── " : "├── ") + "depth=" + node.depth + " id=" + node.id + "\n");
            }
            List<KdNode> children = null;
            if (node.lesser != null || node.greater != null) {
                children = new ArrayList<KdNode>(2);
                if (node.lesser != null)
                    children.add(node.lesser);
                if (node.greater != null)
                    children.add(node.greater);
            }
            if (children != null) {
                for (int i = 0; i < children.size() - 1; i++) {
                    builder.append(getString(children.get(i), prefix + (isTail ? "    " : "│   "), false));
                }
                if (children.size() >= 1) {
                    builder.append(getString(children.get(children.size() - 1), prefix + (isTail ? "    " : "│   "),
                            true));
                }
            }

            return builder.toString();
        }
    }
}
1	package com.jwetherell.algorithms.data_structures;
2
3	/*
4	* Borrowed from
5	* https://github.com/phishman3579/java-algorithms-implementation
6	*/
7
8	import java.util.ArrayList;
9	import java.util.Collection;
10	import java.util.Collections;
11	import java.util.Comparator;
12	import java.util.HashSet;
13	import java.util.List;
14	import java.util.Set;
15	import java.util.TreeSet;
16
17	/**
18	* A k-d tree (short for k-dimensional tree) is a space-partitioning data
19	* structure for organizing points in a k-dimensional space. k-d trees are a
20	* useful data structure for several applications, such as searches involving a
21	* multidimensional search key (e.g. range searches and nearest neighbor
22	* searches). k-d trees are a special case of binary space partitioning trees.
23	*
24	* http://en.wikipedia.org/wiki/K-d_tree
25	*
26	* @author Justin Wetherell <phishman3579@gmail.com>
27	*/
28	public class KdTree<T extends KdTree.XYZPoint> {
29
30	private int k = 3;
31	private KdNode root = null;
32
33	private static final Comparator<XYZPoint> X_COMPARATOR = new Comparator<XYZPoint>() {
34	/**
35	* {@inheritDoc}
36	*/
37	@Override
38	public int compare(XYZPoint o1, XYZPoint o2) {
39	if (o1.x < o2.x)
40	return -1;
41	if (o1.x > o2.x)
42	return 1;
43	return 0;
44	}
45	};
46
47	private static final Comparator<XYZPoint> Y_COMPARATOR = new Comparator<XYZPoint>() {
48	/**
49	* {@inheritDoc}
50	*/
51	@Override
52	public int compare(XYZPoint o1, XYZPoint o2) {
53	if (o1.y < o2.y)
54	return -1;
55	if (o1.y > o2.y)
56	return 1;
57	return 0;
58	}
59	};
60
61	private static final Comparator<XYZPoint> Z_COMPARATOR = new Comparator<XYZPoint>() {
62	/**
63	* {@inheritDoc}
64	*/
65	@Override
66	public int compare(XYZPoint o1, XYZPoint o2) {
67	if (o1.z < o2.z)
68	return -1;
69	if (o1.z > o2.z)
70	return 1;
71	return 0;
72	}
73	};
74
75	protected static final int X_AXIS = 0;
76	protected static final int Y_AXIS = 1;
77	protected static final int Z_AXIS = 2;
78
79	/**
80	* Default constructor.
81	*/
82	public KdTree() { }
83
84	/**
85	* Constructor for creating a more balanced tree. It uses the
86	* "median of points" algorithm.
87	*
88	* @param list
89	* of XYZPoints.
90	*/
91	public KdTree(List<XYZPoint> list) {
92	root = createNode(list, k, 0);
93	}
94
95	/**
96	* Constructor for creating a more balanced tree. It uses the
97	* "median of points" algorithm.
98	*
99	* @param list
100	* of XYZPoints.
101	* @param k
102	* of the tree.
103	*/
104	public KdTree(List<XYZPoint> list, int k) {
105	root = createNode(list, k, 0);
106	}
107
108	/**
109	* Create node from list of XYZPoints.
110	*
111	* @param list
112	* of XYZPoints.
113	* @param k
114	* of the tree.
115	* @param depth
116	* depth of the node.
117	* @return node created.
118	*/
119	private static KdNode createNode(List<XYZPoint> list, int k, int depth) {
120	if (list == null \|\| list.size() == 0)
121	return null;
122
123	int axis = depth % k;
124	if (axis == X_AXIS)
125	Collections.sort(list, X_COMPARATOR);
126	else if (axis == Y_AXIS)
127	Collections.sort(list, Y_COMPARATOR);
128	else
129	Collections.sort(list, Z_COMPARATOR);
130
131	KdNode node = null;
132	List<XYZPoint> less = new ArrayList<XYZPoint>(list.size());
133	List<XYZPoint> more = new ArrayList<XYZPoint>(list.size());
134	if (list.size() > 0) {
135	int medianIndex = list.size() / 2;
136	node = new KdNode(list.get(medianIndex), k, depth);
137	// Process list to see where each non-median point lies
138	for (int i = 0; i < list.size(); i++) {
139	if (i == medianIndex)
140	continue;
141	XYZPoint p = list.get(i);
142	// Cannot assume points before the median are less since they could be equal
143	if (KdNode.compareTo(depth, k, p, node.id) <= 0) {
144	less.add(p);
145	} else {
146	more.add(p);
147	}
148	}
149
150	if ((medianIndex - 1) >= 0 && less.size() > 0) {
151	node.lesser = createNode(less, k, depth + 1);
152	node.lesser.parent = node;
153	}
154
155	if ((medianIndex + 1) <= (list.size() - 1) && more.size() > 0) {
156	node.greater = createNode(more, k, depth + 1);
157	node.greater.parent = node;
158	}
159	}
160
161	return node;
162	}
163
164	/**
165	* Add value to the tree. Tree can contain multiple equal values.
166	*
167	* @param value
168	* T to add to the tree.
169	* @return True if successfully added to tree.
170	*/
171	public boolean add(T value) {
172	if (value == null)
173	return false;
174
175	if (root == null) {
176	root = new KdNode(value);
177	return true;
178	}
179
180	KdNode node = root;
181	while (true) {
182	if (KdNode.compareTo(node.depth, node.k, value, node.id) <= 0) {
183	// Lesser
184	if (node.lesser == null) {
185	KdNode newNode = new KdNode(value, k, node.depth + 1);
186	newNode.parent = node;
187	node.lesser = newNode;
188	break;
189	}
190	node = node.lesser;
191	} else {
192	// Greater
193	if (node.greater == null) {
194	KdNode newNode = new KdNode(value, k, node.depth + 1);
195	newNode.parent = node;
196	node.greater = newNode;
197	break;
198	}
199	node = node.greater;
200	}
201	}
202
203	return true;
204	}
205
206	/**
207	* Does the tree contain the value.
208	*
209	* @param value
210	* T to locate in the tree.
211	* @return True if tree contains value.
212	*/
213	public boolean contains(T value) {
214	if (value == null \|\| root == null)
215	return false;
216
217	KdNode node = getNode(this, value);
218	return (node != null);
219	}
220
221	/**
222	* Locate T in the tree.
223	*
224	* @param tree
225	* to search.
226	* @param value
227	* to search for.
228	* @return KdNode or NULL if not found
229	*/
230	private static final <T extends KdTree.XYZPoint> KdNode getNode(KdTree<T> tree, T value) {
231	if (tree == null \|\| tree.root == null \|\| value == null)
232	return null;
233
234	KdNode node = tree.root;
235	while (true) {
236	if (node.id.equals(value)) {
237	return node;
238	} else if (KdNode.compareTo(node.depth, node.k, value, node.id) <= 0) {
239	// Lesser
240	if (node.lesser == null) {
241	return null;
242	}
243	node = node.lesser;
244	} else {
245	// Greater
246	if (node.greater == null) {
247	return null;
248	}
249	node = node.greater;
250	}
251	}
252	}
253
254	/**
255	* Remove first occurrence of value in the tree.
256	*
257	* @param value
258	* T to remove from the tree.
259	* @return True if value was removed from the tree.
260	*/
261	public boolean remove(T value) {
262	if (value == null \|\| root == null)
263	return false;
264
265	KdNode node = getNode(this, value);
266	if (node == null)
267	return false;
268
269	KdNode parent = node.parent;
270	if (parent != null) {
271	if (parent.lesser != null && node.equals(parent.lesser)) {
272	List<XYZPoint> nodes = getTree(node);
273	if (nodes.size() > 0) {
274	parent.lesser = createNode(nodes, node.k, node.depth);
275	if (parent.lesser != null) {
276	parent.lesser.parent = parent;
277	}
278	} else {
279	parent.lesser = null;
280	}
281	} else {
282	List<XYZPoint> nodes = getTree(node);
283	if (nodes.size() > 0) {
284	parent.greater = createNode(nodes, node.k, node.depth);
285	if (parent.greater != null) {
286	parent.greater.parent = parent;
287	}
288	} else {
289	parent.greater = null;
290	}
291	}
292	} else {
293	// root
294	List<XYZPoint> nodes = getTree(node);
295	if (nodes.size() > 0)
296	root = createNode(nodes, node.k, node.depth);
297	else
298	root = null;
299	}
300
301	return true;
302	}
303
304	/**
305	* Get the (sub) tree rooted at root.
306	*
307	* @param root
308	* of tree to get nodes for.
309	* @return points in (sub) tree, not including root.
310	*/
311	private static final List<XYZPoint> getTree(KdNode root) {
312	List<XYZPoint> list = new ArrayList<XYZPoint>();
313	if (root == null)
314	return list;
315
316	if (root.lesser != null) {
317	list.add(root.lesser.id);
318	list.addAll(getTree(root.lesser));
319	}
320	if (root.greater != null) {
321	list.add(root.greater.id);
322	list.addAll(getTree(root.greater));
323	}
324
325	return list;
326	}
327
328	/**
329	* K Nearest Neighbor search
330	*
331	* @param K
332	* Number of neighbors to retrieve. Can return more than K, if
333	* last nodes are equal distances.
334	* @param value
335	* to find neighbors of.
336	* @return Collection of T neighbors.
337	*/
338	@SuppressWarnings("unchecked")
339	public Collection<T> nearestNeighbourSearch(int K, T value) {
340	if (value == null \|\| root == null)
341	return Collections.EMPTY_LIST;
342
343	// Map used for results
344	TreeSet<KdNode> results = new TreeSet<KdNode>(new EuclideanComparator(value));
345
346	// Find the closest leaf node
347	KdNode prev = null;
348	KdNode node = root;
349	while (node != null) {
350	if (KdNode.compareTo(node.depth, node.k, value, node.id) <= 0) {
351	// Lesser
352	prev = node;
353	node = node.lesser;
354	} else {
355	// Greater
356	prev = node;
357	node = node.greater;
358	}
359	}
360	KdNode leaf = prev;
361
362	if (leaf != null) {
363	// Used to not re-examine nodes
364	Set<KdNode> examined = new HashSet<KdNode>();
365
366	// Go up the tree, looking for better solutions
367	node = leaf;
368	while (node != null) {
369	// Search node
370	searchNode(value, node, K, results, examined);
371	node = node.parent;
372	}
373	}
374
375	// Load up the collection of the results
376	Collection<T> collection = new ArrayList<T>(K);
377	for (KdNode kdNode : results)
378	collection.add((T) kdNode.id);
379	return collection;
380	}
381
382	private static final <T extends KdTree.XYZPoint> void searchNode(T value, KdNode node, int K, TreeSet<KdNode> results, Set<KdNode> examined) {
383	examined.add(node);
384
385	// Search node
386	KdNode lastNode = null;
387	Double lastDistance = Double.MAX_VALUE;
388	if (results.size() > 0) {
389	lastNode = results.last();
390	lastDistance = lastNode.id.euclideanDistance(value);
391	}
392	Double nodeDistance = node.id.euclideanDistance(value);
393	if (nodeDistance.compareTo(lastDistance) < 0) {
394	if (results.size() == K && lastNode != null)
395	results.remove(lastNode);
396	results.add(node);
397	} else if (nodeDistance.equals(lastDistance)) {
398	results.add(node);
399	} else if (results.size() < K) {
400	results.add(node);
401	}
402	lastNode = results.last();
403	lastDistance = lastNode.id.euclideanDistance(value);
404
405	int axis = node.depth % node.k;
406	KdNode lesser = node.lesser;
407	KdNode greater = node.greater;
408
409	// Search children branches, if axis aligned distance is less than
410	// current distance
411	if (lesser != null && !examined.contains(lesser)) {
412	examined.add(lesser);
413
414	double nodePoint = Double.MIN_VALUE;
415	double valuePlusDistance = Double.MIN_VALUE;
416	if (axis == X_AXIS) {
417	nodePoint = node.id.x;
418	valuePlusDistance = value.x - lastDistance;
419	} else if (axis == Y_AXIS) {
420	nodePoint = node.id.y;
421	valuePlusDistance = value.y - lastDistance;
422	} else {
423	nodePoint = node.id.z;
424	valuePlusDistance = value.z - lastDistance;
425	}
426	boolean lineIntersectsCube = ((valuePlusDistance <= nodePoint) ? true : false);
427
428	// Continue down lesser branch
429	if (lineIntersectsCube)
430	searchNode(value, lesser, K, results, examined);
431	}
432	if (greater != null && !examined.contains(greater)) {
433	examined.add(greater);
434
435	double nodePoint = Double.MIN_VALUE;
436	double valuePlusDistance = Double.MIN_VALUE;
437	if (axis == X_AXIS) {
438	nodePoint = node.id.x;
439	valuePlusDistance = value.x + lastDistance;
440	} else if (axis == Y_AXIS) {
441	nodePoint = node.id.y;
442	valuePlusDistance = value.y + lastDistance;
443	} else {
444	nodePoint = node.id.z;
445	valuePlusDistance = value.z + lastDistance;
446	}
447	boolean lineIntersectsCube = ((valuePlusDistance >= nodePoint) ? true : false);
448
449	// Continue down greater branch
450	if (lineIntersectsCube)
451	searchNode(value, greater, K, results, examined);
452	}
453	}
454
455	/**
456	* {@inheritDoc}
457	*/
458	@Override
459	public String toString() {
460	return TreePrinter.getString(this);
461	}
462
463	protected static class EuclideanComparator implements Comparator<KdNode> {
464
465	private final XYZPoint point;
466
467	public EuclideanComparator(XYZPoint point) {
468	this.point = point;
469	}
470
471	/**
472	* {@inheritDoc}
473	*/
474	@Override
475	public int compare(KdNode o1, KdNode o2) {
476	Double d1 = point.euclideanDistance(o1.id);
477	Double d2 = point.euclideanDistance(o2.id);
478	if (d1.compareTo(d2) < 0)
479	return -1;
480	else if (d2.compareTo(d1) < 0)
481	return 1;
482	return o1.id.compareTo(o2.id);
483	}
484	}
485
486	public static class KdNode implements Comparable<KdNode> {
487
488	private final XYZPoint id;
489	private final int k;
490	private final int depth;
491
492	private KdNode parent = null;
493	private KdNode lesser = null;
494	private KdNode greater = null;
495
496	public KdNode(XYZPoint id) {
497	this.id = id;
498	this.k = 3;
499	this.depth = 0;
500	}
501
502	public KdNode(XYZPoint id, int k, int depth) {
503	this.id = id;
504	this.k = k;
505	this.depth = depth;
506	}
507
508	public static int compareTo(int depth, int k, XYZPoint o1, XYZPoint o2) {
509	int axis = depth % k;
510	if (axis == X_AXIS)
511	return X_COMPARATOR.compare(o1, o2);
512	if (axis == Y_AXIS)
513	return Y_COMPARATOR.compare(o1, o2);
514	return Z_COMPARATOR.compare(o1, o2);
515	}
516
517	/**
518	* {@inheritDoc}
519	*/
520	@Override
521	public int hashCode() {
522	return 31 * (this.k + this.depth + this.id.hashCode());
523	}
524
525	/**
526	* {@inheritDoc}
527	*/
528	@Override
529	public boolean equals(Object obj) {
530	if (obj == null)
531	return false;
532	if (!(obj instanceof KdNode))
533	return false;
534
535	KdNode kdNode = (KdNode) obj;
536	if (this.compareTo(kdNode) == 0)
537	return true;
538	return false;
539	}
540
541	/**
542	* {@inheritDoc}
543	*/
544	@Override
545	public int compareTo(KdNode o) {
546	return compareTo(depth, k, this.id, o.id);
547	}
548
549	/**
550	* {@inheritDoc}
551	*/
552	@Override
553	public String toString() {
554	StringBuilder builder = new StringBuilder();
555	builder.append("k=").append(k);
556	builder.append(" depth=").append(depth);
557	builder.append(" id=").append(id.toString());
558	return builder.toString();
559	}
560	}
561
562	public static class XYZPoint implements Comparable<XYZPoint> {
563
564	protected final double x;
565	protected final double y;
566	protected final double z;
567
568	/* public XYZPoint(double x, double y) {
569	this.x = x;
570	this.y = y;
571	this.z = 0;
572	}*/
573
574	public XYZPoint(double x, double y, double z) {
575	this.x = x;
576	this.y = y;
577	this.z = z;
578	}
579
580	public XYZPoint(double latitude, double longitude) {
581	this.x = Math.cos(Math.toRadians(latitude)) * Math.cos(Math.toRadians(longitude));
582	this.y = Math.cos(Math.toRadians(latitude)) * Math.sin(Math.toRadians(longitude));
583	this.z = Math.sin(Math.toRadians(latitude));
584	}
585
586	public double getX() {
587	return x;
588	}
589	public double getY() {
590	return y;
591	}
592	public double getZ() {
593	return z;
594	}
595
596	/**
597	* Computes the Euclidean distance from this point to the other.
598	*
599	* @param o1
600	* other point.
601	* @return euclidean distance.
602	*/
603	public double euclideanDistance(XYZPoint o1) {
604	return euclideanDistance(o1, this);
605	}
606
607	/**
608	* Computes the Euclidean distance from one point to the other.
609	*
610	* @param o1
611	* first point.
612	* @param o2
613	* second point.
614	* @return euclidean distance.
615	*/
616	private static final double euclideanDistance(XYZPoint o1, XYZPoint o2) {
617	return Math.sqrt(Math.pow((o1.x - o2.x), 2) + Math.pow((o1.y - o2.y), 2) + Math.pow((o1.z - o2.z), 2));
618	}
619
620	/**
621	* {@inheritDoc}
622	*/
623	@Override
624	public int hashCode() {
625	return 31 * (int)(this.x + this.y + this.z);
626	}
627
628	/**
629	* {@inheritDoc}
630	*/
631	@Override
632	public boolean equals(Object obj) {
633	if (obj == null)
634	return false;
635	if (!(obj instanceof XYZPoint))
636	return false;
637
638	XYZPoint xyzPoint = (XYZPoint) obj;
639	return compareTo(xyzPoint) == 0;
640	}
641
642	/**
643	* {@inheritDoc}
644	*/
645	@Override
646	public int compareTo(XYZPoint o) {
647	int xComp = X_COMPARATOR.compare(this, o);
648	if (xComp != 0)
649	return xComp;
650	int yComp = Y_COMPARATOR.compare(this, o);
651	if (yComp != 0)
652	return yComp;
653	int zComp = Z_COMPARATOR.compare(this, o);
654	return zComp;
655	}
656
657	/**
658	* {@inheritDoc}
659	*/
660	@Override
661	public String toString() {
662	StringBuilder builder = new StringBuilder();
663	builder.append("(");
664	builder.append(x).append(", ");
665	builder.append(y).append(", ");
666	builder.append(z);
667	builder.append(")");
668	return builder.toString();
669	}
670	}
671
672	protected static class TreePrinter {
673
674	public static <T extends XYZPoint> String getString(KdTree<T> tree) {
675	if (tree.root == null)
676	return "Tree has no nodes.";
677	return getString(tree.root, "", true);
678	}
679
680	private static String getString(KdNode node, String prefix, boolean isTail) {
681	StringBuilder builder = new StringBuilder();
682
683	if (node.parent != null) {
684	String side = "left";
685	if (node.parent.greater != null && node.id.equals(node.parent.greater.id))
686	side = "right";
687	builder.append(prefix + (isTail ? "└── " : "├── ") + "[" + side + "] " + "depth=" + node.depth + " id="
688	+ node.id + "\n");
689	} else {
690	builder.append(prefix + (isTail ? "└── " : "├── ") + "depth=" + node.depth + " id=" + node.id + "\n");
691	}
692	List<KdNode> children = null;
693	if (node.lesser != null \|\| node.greater != null) {
694	children = new ArrayList<KdNode>(2);
695	if (node.lesser != null)
696	children.add(node.lesser);
697	if (node.greater != null)
698	children.add(node.greater);
699	}
700	if (children != null) {
701	for (int i = 0; i < children.size() - 1; i++) {
702	builder.append(getString(children.get(i), prefix + (isTail ? " " : "│ "), false));
703	}
704	if (children.size() >= 1) {
705	builder.append(getString(children.get(children.size() - 1), prefix + (isTail ? " " : "│ "),
706	true));
707	}
708	}
709
710	return builder.toString();
711	}
712	}
713	}