You cannot select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

782 lines
23 KiB
Java

package com.jwetherell.algorithms.data_structures;
import static java.lang.Math.cos;
import static java.lang.Math.sin;
import java.util.ArrayDeque;
import java.util.ArrayList;
import java.util.Collection;
import java.util.Collections;
import java.util.Comparator;
import java.util.Deque;
import java.util.HashSet;
import java.util.Iterator;
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.
*
* @author Justin Wetherell <phishman3579@gmail.com>
* @see <a href="http://en.wikipedia.org/wiki/K-d_tree">K-d_tree (Wikipedia)</a>
*/
public class KdTree<T extends KdTree.XYZPoint> implements Iterable<T> {
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) {
super();
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) {
super();
root = createNode(list, k, 0);
}
/**
* Creates 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 <= list.size()-1) && more.size() > 0) {
node.greater = createNode(more, k, depth + 1);
node.greater.parent = node;
}
}
return node;
}
/**
* Adds 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);
}
/**
* Locates 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;
}
}
}
/**
* Removes 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;
}
/**
* Gets 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;
}
/**
* Searches the K nearest neighbor.
*
* @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);
}
}
/**
* Adds, in a specified queue, a given node and its related nodes (lesser, greater).
*
* @param node
* Node to check. May be null.
*
* @param results
* Queue containing all found entries. Must not be null.
*/
@SuppressWarnings("unchecked")
private static <T extends XYZPoint> void search(final KdNode node, final Deque<T> results) {
if (node != null) {
results.add((T) node.id);
search(node.greater, results);
search(node.lesser, results);
}
}
/**
* {@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);
}
}
/**
* Searches all entries from the first to the last entry.
*
* @return Iterator
* allowing to iterate through a collection containing all found entries.
*/
public Iterator<T> iterator() {
final Deque<T> results = new ArrayDeque<T>();
search(root, results);
return results.iterator();
}
/**
* Searches all entries from the last to the first entry.
*
* @return Iterator
* allowing to iterate through a collection containing all found entries.
*/
public Iterator<T> reverse_iterator() {
final Deque<T> results = new ArrayDeque<T>();
search(root, results);
return results.descendingIterator();
}
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;
/**
* z is defaulted to zero.
*
* @param x
* @param y
*/
public XYZPoint(double x, double y) {
this.x = x;
this.y = y;
this.z = 0;
}
/**
* Default constructor
*
* @param x
* @param y
* @param z
*/
public XYZPoint(double x, double y, double z) {
this.x = x;
this.y = y;
this.z = z;
}
/**
* Does not use R to calculate x, y, and z. Where R is the approximate radius of earth (e.g. 6371KM).
* @param latitude
* @param longitude
*/
public XYZPoint(Double latitude, Double longitude) {
x = cos(Math.toRadians(latitude)) * cos(Math.toRadians(longitude));
y = cos(Math.toRadians(latitude)) * sin(Math.toRadians(longitude));
z = 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;
if (Double.compare(this.x, xyzPoint.x)!=0)
return false;
if (Double.compare(this.y, xyzPoint.y)!=0)
return false;
if (Double.compare(this.z, xyzPoint.z)!=0)
return false;
return true;
}
/**
* {@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();
}
}
}