algorithms/data_structures/KdTree.java

package com.jwetherell.algorithms.data_structures;

/* 
 * Borrowed from 
 * https://github.com/phishman3579/java-algorithms-implementation
 * 
 * Only adjustment is
 * public XYZPoint(double latitude, double longitude) { //create point from lat/lng pair
 * 
 */

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