More on K-D trees

Previous post shows how to properly and efficiently find closest point using k-d tree algorithm for latitude-logitude coordinates. Here is supplement to the topic. Additional features are:

• finding not one, but arbitrary number of closest points,
• finding all points within specified range from given location,
• make code easy to use for metrics different than lat-lng.

To find arbitrary number of points, what we need to store is not one but list of closest points. Additionally what we need to know is distance to the most distant point - to know whether newly checked shall replace the worst of stored. To achieve this property list of point has to be sorted according to distance from the source point. Below is listing of the method.

private void findNearest(PointNode<P> root, P point, int depth, LimitedSortedArrayList<D> results) {
D distance = point.getDistance(root.point);
if (root.left == null && root.right == null) {
results.addSorted(distance);
return;
}

final int axis = depth % 2;
boolean left = false;

if ((root.left != null) && (point.compareAxisTo(axis, root.point) < 0)) {
findNearest(root.left, point, depth+1, results);
left = true;
}
else if (root.right != null) {
findNearest(root.right, point, depth+1, results);
}

results.addSorted(distance);

if (!results.isFilled() || results.getWorst().compareAxisTo(axis, distance) >= 0) {
if (left) {
if (root.right != null) {
findNearest(root.right, point, depth+1, results);
}
}
else {
if (root.left != null) {
findNearest(root.left, point, depth+1, results);
}
}
}
}

Key point for algorithm above is "addSorted" method. This method is convenient extension of ArrayList implemented in LimitedSortedArrayList class, that allow only for limited storage of sorted items:

private static class LimitedSortedArrayList<T extends Comparable<T>> extends SortedArrayList<T> {
private int limit;

public LimitedSortedArrayList(int max_number) {
limit = max_number;
}
   public void addSorted(T value) {
      if (size() < limit) {
         insertSorted(value);
      }
      else {
        overwriteSorted(value);
      }
   }
   public boolean isFilled() {
      return size() >= limit;
   }
}

LimitedSortedArrayList extends ArrayList, but not directly only via SortedArrayList class. Responsibility of SortedArrayList is to keep order according to "compareTo", allow insertion of new elements and overwriting existing (to keep constant size of the container).

private static class SortedArrayList<T extends Comparable<T>> extends ArrayList<T> {
   private void sort() {
      for (int i = size()-1; i > 0 && get(i).compareTo(get(i-1)) < 0; i--)
        Collections.swap(this, i, i-1);
   }
   public T getWorst() {
      if (size() > 0) {
        return get(size()-1);
     }
      return null;
   }
   public void overwriteSorted(T value) {
      if (value.compareTo(getWorst()) < 0) {
        set(size()-1, value);
        sort();
      }
   }
   public void insertSorted(T value) {
add(value);
sort();
   }
}

Methods presented above do not contains exact types of point and means of distance calculation. These are provided via template parameters. P stands for point which must extend Metric interface and D stands for Distance and extends Distance interface.

public interface Metric<P, D extends Distance<D, P>> extends ComparableAxis<P> {
D getDistance(P other);
}
public interface Distance<D, P> extends Comparable<D>, ComparableAxis<D> {
P getToPoint();
}

where ComparableAxis is interface that allows for comparisons along specified axis:

public interface ComparableAxis<T> {
int compareAxisTo(int axis, T o);
}

For latitude-longitude we have following implementations of the interfaces:

public class LatLngCommon implements Metric<LatLngCommon, CosDistance> {
public double lat;
public double lng;
public double sin_lat;
public double cos_lat;
public double sin_lng;
public double cos_lng;

public LatLngCommon(double latitude, double longitude) {
lat = latitude;
lng = longitude;
double lat_rad = Math.toRadians(lat);
double lng_rad = Math.toRadians(lng);
sin_lat = Math.sin(lat_rad);
cos_lat = Math.cos(lat_rad);
sin_lng = Math.sin(lng_rad);
cos_lng = Math.cos(lng_rad);
}

@Override
public int compareAxisTo(int axis, LatLngCommon o) {
return (0==axis)
?(Double.compare(lat, o.lat))
:(Double.compare(lng, o.lng));
}
@Override
public CosDistance getDistance(LatLngCommon other) {
return new CosDistance(this, other);
}
}


public class CosDistance implements Distance<CosDistance, LatLngCommon> {

private LatLngCommon to_point;
private double sin_from_lat__sin_to_lat;
private double cos_from_lat__cos_to_lat;
private double sin_from_lng__sin_to_lng__cos_from_lng__cos_to_lng;
private double cos_dist;
private static final double RADIUS = 6371000.0;

public CosDistance(LatLngCommon from, LatLngCommon to) {
to_point = to;
sin_from_lat__sin_to_lat = from.sin_lat*to.sin_lat;
cos_from_lat__cos_to_lat = from.cos_lat*to.cos_lat;
sin_from_lng__sin_to_lng__cos_from_lng__cos_to_lng = from.sin_lng*to.sin_lng + from.cos_lng*to.cos_lng;
cos_dist = sin_from_lat__sin_to_lat + cos_from_lat__cos_to_lat*(sin_from_lng__sin_to_lng__cos_from_lng__cos_to_lng);
}
public CosDistance(double distance_meters) {
to_point = null;
sin_from_lat__sin_to_lat = 0;
cos_from_lat__cos_to_lat = 0;
sin_from_lng__sin_to_lng__cos_from_lng__cos_to_lng = 0;
cos_dist = Math.cos(distance_meters/RADIUS);
}
@Override
public int compareTo(CosDistance o) {
return  - Double.compare(cos_dist, o.cos_dist);
}
@Override
public int compareAxisTo(int axis, CosDistance o) {
return (0==axis)
?( - Double.compare(cos_dist, o.sin_from_lat__sin_to_lat + o.cos_from_lat__cos_to_lat)) // same longitude
:( - Double.compare(cos_dist, 1.0 + o.to_point.cos_lat*o.to_point.cos_lat*(o.sin_from_lng__sin_to_lng__cos_from_lng__cos_to_lng - 1.0))); // same latitude
}
@Override
public LatLngCommon getToPoint() {
return to_point;
}
}


Providing your own implementations of Metric and Distance interfaces you can easily calculate closest point in different coordinates (e.g. Cartesian).

Finding N-closest point example is available here. Number of points to be searched can be specified as get method parameter "number".


Last point is to solve problem stated a little differently - find all points within specified range. You can find in "CosDistance" definition additional constructor (with one double argument - distance) and Earth radius used in Google maps (see green code above).
Such distance calculated from radius in meters is required for finding points in range method:

private void findInRange(PointNode<P> root, P point, D range, int depth, CheckedArrayList<D> results) {

D distance = point.getDistance(root.point);

if (root.left == null && root.right == null) {
results.addChecked(distance);
return;
}

final int axis = depth % 2;
boolean left = false;

if ((root.left != null) && (point.compareAxisTo(axis, root.point) < 0)) {
findInRange(root.left, point, range, depth+1, results);
left = true;
}
else if (root.right != null) {
findInRange(root.right, point, range, depth+1, results);
}

results.addChecked(distance);

if (range.compareAxisTo(axis, distance) >= 0) {
if (left) {
if (root.right != null) {
findInRange(root.right, point, range, depth+1, results);
}
}
else {
if (root.left != null) {
findInRange(root.left, point, range, depth+1, results);
}
}
}
}

As you can see this method is almost the same as finding nearest points. There are two important differences:

• first - container used for results storage - now we use CheckedArrayList - number of elements stored is not limited, but point is only added if its distance is lower than llimit,
• second - searching different branch of a tree is performed only when actual distance along actual axis is smaller than given distance (range).

CheckedArrayList is thus implemented as simple extension of ArrayList:

private static class CheckedArrayList<T extends Comparable<T>> extends ArrayList<T> {
T reference;
public CheckedArrayList(T ref) {
reference = ref;
}
   public void addChecked(T value) {
      if (value.compareTo(reference) < 0) {
       add(value);
      }
   }
}

Finding points in range example is available here. Radius for search can be specified as get method parameter "radius".

All the code presented above you can find here.