Difference between revisions of "User:Chase-san/Kd-Tree"

From Robowiki
Jump to navigation Jump to search
(use <syntaxhighlight>)
m (→‎KDTreeF: (no major change, just some minor tweaks, i++ to ++i, removing the enhanced for loop))
 
(6 intermediate revisions by the same user not shown)
Line 1: Line 1:
 
[[Category:Code Snippets|Kd-Tree]]
 
[[Category:Code Snippets|Kd-Tree]]
 
Everyone and their brother has one of these now, me and Simonton started it, but I was to inexperienced to get anything written, I took an hour or two to rewrite it today, because I am no longer completely terrible at these things. So here is mine if you care to see it.
 
Everyone and their brother has one of these now, me and Simonton started it, but I was to inexperienced to get anything written, I took an hour or two to rewrite it today, because I am no longer completely terrible at these things. So here is mine if you care to see it.
 +
 +
This and all my other code in which I display on the robowiki falls under the [http://en.wikipedia.org/wiki/Zlib_License ZLIB License].
  
 
Oh yeah, am I the only one that has a Range function?
 
Oh yeah, am I the only one that has a Range function?
  
=== KDTreeC ===
+
=== KDTreeF ===
 
<syntaxhighlight>
 
<syntaxhighlight>
package org.csdgn.util.kd2;
+
package org.csdgn.util;
  
 +
import java.util.ArrayList;
 
import java.util.Arrays;
 
import java.util.Arrays;
 +
import java.util.List;
  
 
/**
 
/**
  * This is a KD Bucket Tree, for fast sorting and searching of K dimensional data.
+
  * This is a KD Bucket Tree, for fast sorting and searching of K dimensional
 +
* data.
 +
*
 
  * @author Chase
 
  * @author Chase
  *
+
  *  
 
  */
 
  */
public class KDTreeC {
+
public class KDTree<T> {
/**
+
protected static final int defaultBucketSize = 48;
* Item, for moving data around.
+
 
* @author Chase
 
*/
 
public class Item {
 
public double[] pnt;
 
public Object obj;
 
public double distance;
 
private Item(double[] p, Object o) {
 
pnt = p; obj = o;
 
}
 
}
 
 
private final int dimensions;
 
private final int dimensions;
private final int bucket_size;
+
private final int bucketSize;
 
private NodeKD root;
 
private NodeKD root;
+
 
 
/**
 
/**
 
* Constructor with value for dimensions.
 
* Constructor with value for dimensions.
* @param dimensions - Number of dimensions
+
*
 +
* @param dimensions
 +
*            - Number of dimensions
 
*/
 
*/
public KDTreeC(int dimensions) {
+
public KDTree(int dimensions) {
 
this.dimensions = dimensions;
 
this.dimensions = dimensions;
this.bucket_size = 64;
+
this.bucketSize = defaultBucketSize;
this.root = new NodeKD(this);
+
this.root = new NodeKD();
 
}
 
}
+
 
 
/**
 
/**
 
* Constructor with value for dimensions and bucket size.
 
* Constructor with value for dimensions and bucket size.
* @param dimensions - Number of dimensions
+
*
* @param bucket - Size of the buckets.
+
* @param dimensions
 +
*            - Number of dimensions
 +
* @param bucket
 +
*            - Size of the buckets.
 
*/
 
*/
public KDTreeC(int dimensions, int bucket) {
+
public KDTree(int dimensions, int bucket) {
 
this.dimensions = dimensions;
 
this.dimensions = dimensions;
this.bucket_size = bucket;
+
this.bucketSize = bucket;
this.root = new NodeKD(this);
+
this.root = new NodeKD();
 
}
 
}
+
 
 
/**
 
/**
 
* Add a key and its associated value to the tree.
 
* Add a key and its associated value to the tree.
* @param key - Key to add
+
*
* @param val - object to add
+
* @param key
 +
*            - Key to add
 +
* @param val
 +
*            - object to add
 
*/
 
*/
public void add(double[] key, Object val) {
+
public void add(double[] key, T val) {
root.add(new Item(key,val));
+
root.addPoint(key, val);
 
}
 
}
+
 
 
/**
 
/**
* Returns all PointKD within a certain range defined by an upper and lower PointKD.
+
* Returns all PointKD within a certain range defined by an upper and lower
* @param low - lower bounds of area
+
* PointKD.
* @param high - upper bounds of area
+
*
 +
* @param low
 +
*            - lower bounds of area
 +
* @param high
 +
*            - upper bounds of area
 
* @return - All PointKD between low and high.
 
* @return - All PointKD between low and high.
 
*/
 
*/
public Item[] getRange(double[] low, double[] high) {
+
@SuppressWarnings("unchecked")
return root.range(high, low);
+
public List<T> getRange(double[] low, double[] high) {
 +
Object[] objs = root.range(high, low);
 +
ArrayList<T> range = new ArrayList<T>(objs.length);
 +
for(int i=0; i<objs.length; ++i) {
 +
range.add((T)objs[i]);
 +
}
 +
return range;
 
}
 
}
+
 
 
/**
 
/**
 
* Gets the N nearest neighbors to the given key.
 
* Gets the N nearest neighbors to the given key.
* @param key - Key
 
* @param num - Number of results
 
* @return Array of Item Objects, distances within the items
 
* are the square of the actual distance between them and the key
 
*/
 
public Item[] getNearestNeighbor(double[] key, int num) {
 
ShiftArray arr = new ShiftArray(num);
 
root.nearestn(arr, key);
 
return arr.getArray();
 
}
 
/**
 
* Compares arrays of double and returns the euclidean distance
 
* between them.
 
 
*  
 
*  
* @param a - The first set of numbers
+
* @param key
* @param b - The second set of numbers
+
*            - Key
* @return The distance squared between <b>a</b> and <b>b</b>.
+
* @param num
 +
*            - Number of results
 +
* @return Array of Item Objects, distances within the items are the square
 +
*        of the actual distance between them and the key
 
*/
 
*/
public static final double distance(double[] a, double[] b) {
+
public ResultHeap<T> getNearestNeighbors(double[] key, int num) {
double total = 0;
+
ResultHeap<T> heap = new ResultHeap<T>(num);
for (int i = 0; i < a.length; ++i)
+
root.nearest(heap, key);
total += (b[i] - a[i]) * (b[i] - a[i]);
+
return heap;
return Math.sqrt(total);
 
 
}
 
}
/**
+
 
* Compares arrays of double and returns the squared euclidean distance
+
 
* between them.
+
// Internal tree node
*
 
* @param a - The first set of numbers
 
* @param b - The second set of numbers
 
* @return The distance squared between <b>a</b> and <b>b</b>.
 
*/
 
public static final double distanceSq(double[] a, double[] b) {
 
double total = 0;
 
for (int i = 0; i < a.length; ++i)
 
total += (b[i] - a[i]) * (b[i] - a[i]);
 
return total;
 
}
 
 
//Internal tree node
 
 
private class NodeKD {
 
private class NodeKD {
private KDTreeC owner;
 
 
private NodeKD left, right;
 
private NodeKD left, right;
private double[] upper, lower;
+
private double[] maxBounds, minBounds;
private Item[] bucket;
+
private Object[] bucketValues;
private int current, dim;
+
private double[][] bucketKeys;
 +
private boolean isLeaf;
 +
private int current, sliceDimension;
 
private double slice;
 
private double slice;
+
 
//note: we always start as a bucket
+
private NodeKD() {
private NodeKD(KDTreeC own) {
+
bucketValues = new Object[bucketSize];
owner = own;
+
bucketKeys = new double[bucketSize][];
upper = lower = null;
+
 
 
left = right = null;
 
left = right = null;
bucket = new Item[own.bucket_size];
+
maxBounds = minBounds = null;
 +
 +
isLeaf = true;
 +
 
current = 0;
 
current = 0;
dim = 0;
 
 
}
 
}
//when we create non-root nodes within this class
+
 
//we use this one here
+
// what it says on the tin
private NodeKD(NodeKD node) {
+
private void addPoint(double[] key, Object val) {
owner = node.owner;
+
if(isLeaf) {
dim = node.dim + 1;
+
addLeafPoint(key,val);
bucket = new Item[owner.bucket_size];
 
if(dim + 1 > owner.dimensions) dim = 0;
 
left = right = null;
 
upper = lower = null;
 
current = 0;
 
}
 
//what it says on the tin
 
private void add(Item m) {
 
if(bucket == null) {
 
//Branch
 
if(m.pnt[dim] > slice)
 
right.add(m);
 
else left.add(m);
 
 
} else {
 
} else {
//Bucket
+
extendBounds(key);
if(current+1>owner.bucket_size) {
+
if (key[sliceDimension] > slice) {
split(m);
+
right.addPoint(key, val);
return;
+
} else {
 +
left.addPoint(key, val);
 
}
 
}
bucket[current++] = m;
 
 
}
 
}
expand(m.pnt);
 
 
}
 
}
//nearest neighbor thing
+
private void nearestn(ShiftArray arr, double[] data) {
+
private void addLeafPoint(double[] key, Object val) {
if(bucket == null) {
+
extendBounds(key);
//Branch
+
if (current + 1 > bucketSize) {
if(data[dim] > slice) {
+
splitLeaf();
right.nearestn(arr, data);
+
addPoint(key, val);
if(left.current != 0) {
+
return;
if(KDTreeC.distanceSq(left.nearestRect(data),data)
+
}
< arr.getLongest()) {
+
bucketKeys[current] = key;
left.nearestn(arr, data);
+
bucketValues[current] = val;
}
+
++current;
 +
}
 +
 +
/**
 +
* Find the nearest neighbor recursively.
 +
*/
 +
@SuppressWarnings("unchecked")
 +
private void nearest(ResultHeap<T> heap, double[] data) {
 +
if(current == 0)
 +
return;
 +
if(isLeaf) {
 +
//IS LEAF
 +
for (int i = 0; i < current; ++i) {
 +
double dist = pointDistSq(bucketKeys[i], data);
 +
heap.offer(dist, (T) bucketValues[i]);
 +
}
 +
} else {
 +
//IS BRANCH
 +
if (data[sliceDimension] > slice) {
 +
right.nearest(heap, data);
 +
if(left.current == 0)
 +
return;
 +
if (!heap.isFull() || regionDistSq(data,left.minBounds,left.maxBounds) < heap.getMaxKey()) {
 +
left.nearest(heap, data);
 
}
 
}
 
 
} else {
 
} else {
left.nearestn(arr, data);
+
left.nearest(heap, data);
if(right.current != 0) {
+
if (right.current == 0)
if(KDTreeC.distanceSq(right.nearestRect(data),data)  
+
return;
< arr.getLongest()) {
+
if (!heap.isFull() || regionDistSq(data,right.minBounds,right.maxBounds) < heap.getMaxKey()) {
right.nearestn(arr, data);
+
right.nearest(heap, data);
}
 
 
}
 
}
}
 
} else {
 
//Bucket
 
for(int i = 0; i < current; i++) {
 
bucket[i].distance = KDTreeC.distanceSq(bucket[i].pnt, data);
 
arr.add(bucket[i]);
 
 
}
 
}
 
}
 
}
 
}
 
}
//gets all items from within a range
+
 
private Item[] range(double[] upper, double[] lower) {
+
// gets all items from within a range
//TODO: clean this up a bit
+
private Object[] range(double[] upper, double[] lower) {
if(bucket == null) {
+
if (bucketValues == null) {
//Branch
+
// Branch
Item[] tmp = new Item[0];
+
Object[] tmp = new Object[0];
if (intersects(upper,lower,left.upper,left.lower)) {
+
if (intersects(upper, lower, left.maxBounds, left.minBounds)) {
Item[] tmpl = left.range(upper,lower);
+
Object[] tmpl = left.range(upper, lower);
if(0 == tmp.length)
+
if (0 == tmp.length) tmp = tmpl;
tmp = tmpl;
 
 
}
 
}
if (intersects(upper,lower,right.upper,right.lower)) {
+
if (intersects(upper, lower, right.maxBounds, right.minBounds)) {
Item[] tmpr = right.range(upper,lower);
+
Object[] tmpr = right.range(upper, lower);
 
if (0 == tmp.length)
 
if (0 == tmp.length)
 
tmp = tmpr;
 
tmp = tmpr;
 
else if (0 < tmpr.length) {
 
else if (0 < tmpr.length) {
Item[] tmp2 = new Item[tmp.length + tmpr.length];
+
Object[] tmp2 = new Object[tmp.length + tmpr.length];
 
System.arraycopy(tmp, 0, tmp2, 0, tmp.length);
 
System.arraycopy(tmp, 0, tmp2, 0, tmp.length);
 
System.arraycopy(tmpr, 0, tmp2, tmp.length, tmpr.length);
 
System.arraycopy(tmpr, 0, tmp2, tmp.length, tmpr.length);
Line 213: Line 206:
 
return tmp;
 
return tmp;
 
}
 
}
//Bucket
+
// Leaf
Item[] tmp = new Item[current];
+
Object[] tmp = new Object[current];
 
int n = 0;
 
int n = 0;
for (int i = 0; i < current; i++) {
+
for (int i = 0; i < current; ++i) {
if (contains(upper, lower, bucket[i].pnt)) {
+
if (contains(upper, lower, bucketKeys[i])) {
tmp[n++] = bucket[i];
+
tmp[n++] = bucketValues[i];
 
}
 
}
 
}
 
}
Item[] tmp2 = new Item[n];
+
Object[] tmp2 = new Object[n];
 
System.arraycopy(tmp, 0, tmp2, 0, n);
 
System.arraycopy(tmp, 0, tmp2, 0, n);
 
return tmp2;
 
return tmp2;
 
}
 
}
+
 
//These are helper functions from here down
+
// These are helper functions from here down
//check if this hyper rectangle contains a give hyper-point
+
// check if this hyper rectangle contains a give hyper-point
 
public boolean contains(double[] upper, double[] lower, double[] point) {
 
public boolean contains(double[] upper, double[] lower, double[] point) {
if(current == 0) return false;
+
if (current == 0) return false;
for(int i=0; i<point.length; ++i) {
+
for (int i = 0; i < point.length; ++i) {
if(point[i] > upper[i] ||
+
if (point[i] > upper[i] || point[i] < lower[i]) return false;
point[i] < lower[i])  
 
return false;
 
 
}
 
}
 
return true;
 
return true;
 
}
 
}
//checks if two hyper-rectangles intersect
+
 
public boolean intersects(double[] up0, double[] low0,
+
// checks if two hyper-rectangles intersect
double[] up1, double[] low1) {
+
public boolean intersects(double[] up0, double[] low0, double[] up1, double[] low1) {
for(int i=0; i<up0.length; ++i) {
+
for (int i = 0; i < up0.length; ++i) {
if(up1[i] < low0[i] || low1[i] > up0[i]) return false;
+
if (up1[i] < low0[i] || low1[i] > up0[i]) return false;
 
}
 
}
 
return true;
 
return true;
 
}
 
}
//splits a bucket into a branch
+
 
private void split(Item m) {
+
private void splitLeaf() {
//split based on our bound data
+
double bestRange = 0;
slice = (upper[dim]+lower[dim])/2.0;
+
for(int i=0;i<dimensions;++i) {
left = new NodeKD(this);
+
double range = maxBounds[i] - minBounds[i];
right = new NodeKD(this);
+
if(range > bestRange) {
for(int i=0; i<current; ++i) {
+
sliceDimension = i;
if(bucket[i].pnt[dim] > slice)
+
bestRange = range;
right.add(bucket[i]);
+
}
else left.add(bucket[i]);
 
 
}
 
}
bucket = null;
+
add(m);
+
left = new NodeKD();
}
+
right = new NodeKD();
//gets nearest point to data within this hyper rectangle
+
private double[] nearestRect(double[] data) {
+
slice = (maxBounds[sliceDimension] + minBounds[sliceDimension]) * 0.5;
double[] nearest = data.clone();
+
for(int i = 0; i < data.length; ++i) {
+
for (int i = 0; i < current; ++i) {
if(nearest[i] > upper[i]) nearest[i] = upper[i];
+
if (bucketKeys[i][sliceDimension] > slice) {
if(nearest[i] < lower[i]) nearest[i] = lower[i];
+
right.addLeafPoint(bucketKeys[i], bucketValues[i]);
 +
} else {
 +
left.addLeafPoint(bucketKeys[i], bucketValues[i]);
 +
}
 
}
 
}
return nearest;
+
bucketKeys = null;
 +
bucketValues = null;
 +
isLeaf = false;
 
}
 
}
//expands this hyper rectangle
+
 
private void expand(double[] data) {
+
// expands this hyper rectangle
if(upper == null) {
+
private void extendBounds(double[] key) {
upper = Arrays.copyOf(data, owner.dimensions);
+
if (maxBounds == null) {
lower = Arrays.copyOf(data, owner.dimensions);
+
maxBounds = Arrays.copyOf(key, dimensions);
 +
minBounds = Arrays.copyOf(key, dimensions);
 
return;
 
return;
 
}
 
}
for(int i=0; i<data.length; ++i) {
+
for (int i = 0; i < key.length; ++i) {
if(upper[i] < data[i]) upper[i] = data[i];
+
if (maxBounds[i] < key[i]) maxBounds[i] = key[i];
if(lower[i] > data[i]) lower[i] = data[i];
+
if (minBounds[i] > key[i]) minBounds[i] = key[i];
 
}
 
}
 
}
 
}
 
}
 
}
//A simple shift array that sifts data up
+
//as we add new ones to lower in the array.
+
/* I may have borrowed these from an early version of Red's tree. I however forget. */
private class ShiftArray {
+
private static final double pointDistSq(double[] p1, double[] p2) {
private Item[] items;
+
        double d = 0;
private final int max;
+
        double q = 0;
private int current;
+
        for (int i = 0; i < p1.length; ++i) {
private ShiftArray(int maximum) {
+
            d += (q=(p1[i] - p2[i]))*q;
max = maximum;
+
        }
current = 0;
+
        return d;
items = new Item[max];
+
    }
}
+
 
private void add(Item m) {
+
    private static final double regionDistSq(double[] point, double[] min, double[] max) {
int i;
+
        double d = 0;
for(i=current;i>0&&items[i-1].distance > m.distance; --i);
+
        double q = 0;
if(i >= max) return;
+
        for (int i = 0; i < point.length; ++i) {
if(current < max) ++current;
+
            if (point[i] > max[i]) {
System.arraycopy(items, i, items, i+1, current-(i+1));
+
            d += (q = (point[i] - max[i]))*q;
items[i] = m;
+
            } else if (point[i] < min[i]) {
}
+
                d += (q = (point[i] - min[i]))*q;
private double getLongest() {
+
            }
if (current < max) return Double.POSITIVE_INFINITY;
+
        }
return items[max-1].distance;
+
        return d;
}
+
    }
private Item[] getArray() {
+
}
return items.clone();
+
</syntaxhighlight>
}
+
 
 +
=== ResultHeap ===
 +
<syntaxhighlight>
 +
package org.csdgn.util;
 +
 
 +
/**
 +
* @author Chase
 +
*
 +
* @param <T>
 +
*/
 +
public class ResultHeap<T> {
 +
private Object[] data;
 +
private double[] keys;
 +
private int capacity;
 +
private int size;
 +
 
 +
protected ResultHeap(int capacity) {
 +
this.data = new Object[capacity];
 +
this.keys = new double[capacity];
 +
this.capacity = capacity;
 +
this.size = 0;
 +
}
 +
 
 +
protected void offer(double key, T value) {
 +
int i = size;
 +
for (; i > 0 && keys[i - 1] > key; --i);
 +
if (i >= capacity) return;
 +
if (size < capacity) ++size;
 +
int j = i + 1;
 +
System.arraycopy(keys, i, keys, j, size - j);
 +
keys[i] = key;
 +
System.arraycopy(data, i, data, j, size - j);
 +
data[i] = value;
 +
}
 +
 
 +
public double getMaxKey() {
 +
return keys[size - 1];
 +
}
 +
 +
@SuppressWarnings("unchecked")
 +
public T removeMax() {
 +
if(isEmpty()) return null;
 +
return (T)data[--size];
 +
}
 +
 
 +
public boolean isEmpty() {
 +
return size == 0;
 +
}
 +
 
 +
public boolean isFull() {
 +
return size == capacity;
 +
}
 +
 
 +
public int size() {
 +
return size;
 +
}
 +
 
 +
public int capacity() {
 +
return capacity;
 
}
 
}
 
}
 
}
 
</syntaxhighlight>
 
</syntaxhighlight>

Latest revision as of 21:00, 7 November 2012

Everyone and their brother has one of these now, me and Simonton started it, but I was to inexperienced to get anything written, I took an hour or two to rewrite it today, because I am no longer completely terrible at these things. So here is mine if you care to see it.

This and all my other code in which I display on the robowiki falls under the ZLIB License.

Oh yeah, am I the only one that has a Range function?

KDTreeF

package org.csdgn.util;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;

/**
 * This is a KD Bucket Tree, for fast sorting and searching of K dimensional
 * data.
 * 
 * @author Chase
 * 
 */
public class KDTree<T> {
	protected static final int defaultBucketSize = 48;

	private final int dimensions;
	private final int bucketSize;
	private NodeKD root;

	/**
	 * Constructor with value for dimensions.
	 * 
	 * @param dimensions
	 *            - Number of dimensions
	 */
	public KDTree(int dimensions) {
		this.dimensions = dimensions;
		this.bucketSize = defaultBucketSize;
		this.root = new NodeKD();
	}

	/**
	 * Constructor with value for dimensions and bucket size.
	 * 
	 * @param dimensions
	 *            - Number of dimensions
	 * @param bucket
	 *            - Size of the buckets.
	 */
	public KDTree(int dimensions, int bucket) {
		this.dimensions = dimensions;
		this.bucketSize = bucket;
		this.root = new NodeKD();
	}

	/**
	 * Add a key and its associated value to the tree.
	 * 
	 * @param key
	 *            - Key to add
	 * @param val
	 *            - object to add
	 */
	public void add(double[] key, T val) {
		root.addPoint(key, val);
	}

	/**
	 * Returns all PointKD within a certain range defined by an upper and lower
	 * PointKD.
	 * 
	 * @param low
	 *            - lower bounds of area
	 * @param high
	 *            - upper bounds of area
	 * @return - All PointKD between low and high.
	 */
	@SuppressWarnings("unchecked")
	public List<T> getRange(double[] low, double[] high) {
		Object[] objs = root.range(high, low);
		ArrayList<T> range = new ArrayList<T>(objs.length);
		for(int i=0; i<objs.length; ++i) {
			range.add((T)objs[i]);
		}
		return range;
	}

	/**
	 * Gets the N nearest neighbors to the given key.
	 * 
	 * @param key
	 *            - Key
	 * @param num
	 *            - Number of results
	 * @return Array of Item Objects, distances within the items are the square
	 *         of the actual distance between them and the key
	 */
	public ResultHeap<T> getNearestNeighbors(double[] key, int num) {
		ResultHeap<T> heap = new ResultHeap<T>(num);
		root.nearest(heap, key);
		return heap;
	}


	// Internal tree node
	private class NodeKD {
		private NodeKD left, right;
		private double[] maxBounds, minBounds;
		private Object[] bucketValues;
		private double[][] bucketKeys;
		private boolean isLeaf;
		private int current, sliceDimension;
		private double slice;

		private NodeKD() {
			bucketValues = new Object[bucketSize];
			bucketKeys = new double[bucketSize][];

			left = right = null;
			maxBounds = minBounds = null;
			
			isLeaf = true;
			
			current = 0;
		}

		// what it says on the tin
		private void addPoint(double[] key, Object val) {
			if(isLeaf) {
				addLeafPoint(key,val);
			} else {
				extendBounds(key);
				if (key[sliceDimension] > slice) {
					right.addPoint(key, val);
				} else {
					left.addPoint(key, val);
				}
			}
		}
		
		private void addLeafPoint(double[] key, Object val) {
			extendBounds(key);
			if (current + 1 > bucketSize) {
				splitLeaf();
				addPoint(key, val);
				return;
			}
			bucketKeys[current] = key;
			bucketValues[current] = val;
			++current;
		}
		
		/**
		 * Find the nearest neighbor recursively.
		 */
		@SuppressWarnings("unchecked")
		private void nearest(ResultHeap<T> heap, double[] data) {
			if(current == 0)
				return;
			if(isLeaf) {
				//IS LEAF
				for (int i = 0; i < current; ++i) {
					double dist = pointDistSq(bucketKeys[i], data);
					heap.offer(dist, (T) bucketValues[i]);
				}
			} else {
				//IS BRANCH
				if (data[sliceDimension] > slice) {
					right.nearest(heap, data);
					if(left.current == 0)
						return;
					if (!heap.isFull() || regionDistSq(data,left.minBounds,left.maxBounds) < heap.getMaxKey()) {
						left.nearest(heap, data);
					}
				} else {
					left.nearest(heap, data);
					if (right.current == 0)
						return;
					if (!heap.isFull() || regionDistSq(data,right.minBounds,right.maxBounds) < heap.getMaxKey()) {
						right.nearest(heap, data);
					}
				}
			}
		}

		// gets all items from within a range
		private Object[] range(double[] upper, double[] lower) {
			if (bucketValues == null) {
				// Branch
				Object[] tmp = new Object[0];
				if (intersects(upper, lower, left.maxBounds, left.minBounds)) {
					Object[] tmpl = left.range(upper, lower);
					if (0 == tmp.length) tmp = tmpl;
				}
				if (intersects(upper, lower, right.maxBounds, right.minBounds)) {
					Object[] tmpr = right.range(upper, lower);
					if (0 == tmp.length)
						tmp = tmpr;
					else if (0 < tmpr.length) {
						Object[] tmp2 = new Object[tmp.length + tmpr.length];
						System.arraycopy(tmp, 0, tmp2, 0, tmp.length);
						System.arraycopy(tmpr, 0, tmp2, tmp.length, tmpr.length);
						tmp = tmp2;
					}
				}
				return tmp;
			}
			// Leaf
			Object[] tmp = new Object[current];
			int n = 0;
			for (int i = 0; i < current; ++i) {
				if (contains(upper, lower, bucketKeys[i])) {
					tmp[n++] = bucketValues[i];
				}
			}
			Object[] tmp2 = new Object[n];
			System.arraycopy(tmp, 0, tmp2, 0, n);
			return tmp2;
		}

		// These are helper functions from here down
		// check if this hyper rectangle contains a give hyper-point
		public boolean contains(double[] upper, double[] lower, double[] point) {
			if (current == 0) return false;
			for (int i = 0; i < point.length; ++i) {
				if (point[i] > upper[i] || point[i] < lower[i]) return false;
			}
			return true;
		}

		// checks if two hyper-rectangles intersect
		public boolean intersects(double[] up0, double[] low0, double[] up1, double[] low1) {
			for (int i = 0; i < up0.length; ++i) {
				if (up1[i] < low0[i] || low1[i] > up0[i]) return false;
			}
			return true;
		}

		private void splitLeaf() {
			double bestRange = 0;
			for(int i=0;i<dimensions;++i) {
				double range = maxBounds[i] - minBounds[i];
				if(range > bestRange) {
					sliceDimension = i;
					bestRange = range;
				}
			}
			
			left = new NodeKD();
			right = new NodeKD();
			
			slice = (maxBounds[sliceDimension] + minBounds[sliceDimension]) * 0.5;
			
			for (int i = 0; i < current; ++i) {
				if (bucketKeys[i][sliceDimension] > slice) {
					right.addLeafPoint(bucketKeys[i], bucketValues[i]);
				} else {
					left.addLeafPoint(bucketKeys[i], bucketValues[i]);
				}
			}
			bucketKeys = null;
			bucketValues = null;
			isLeaf = false;
		}

		// expands this hyper rectangle
		private void extendBounds(double[] key) {
			if (maxBounds == null) {
				maxBounds = Arrays.copyOf(key, dimensions);
				minBounds = Arrays.copyOf(key, dimensions);
				return;
			}
			for (int i = 0; i < key.length; ++i) {
				if (maxBounds[i] < key[i]) maxBounds[i] = key[i];
				if (minBounds[i] > key[i]) minBounds[i] = key[i];
			}
		}
	}
	
	/* I may have borrowed these from an early version of Red's tree. I however forget. */
	private static final double pointDistSq(double[] p1, double[] p2) {
        double d = 0;
        double q = 0;
        for (int i = 0; i < p1.length; ++i) {
            d += (q=(p1[i] - p2[i]))*q;
        }
        return d;
    }

    private static final double regionDistSq(double[] point, double[] min, double[] max) {
        double d = 0;
        double q = 0;
        for (int i = 0; i < point.length; ++i) {
            if (point[i] > max[i]) {
            	d += (q = (point[i] - max[i]))*q;
            } else if (point[i] < min[i]) {
                d += (q = (point[i] - min[i]))*q;
            }
        }
        return d;
    }
}

ResultHeap

package org.csdgn.util;

/**
 * @author Chase
 * 
 * @param <T>
 */
public class ResultHeap<T> {
	private Object[] data;
	private double[] keys;
	private int capacity;
	private int size;

	protected ResultHeap(int capacity) {
		this.data = new Object[capacity];
		this.keys = new double[capacity];
		this.capacity = capacity;
		this.size = 0;
	}

	protected void offer(double key, T value) {
		int i = size;
		for (; i > 0 && keys[i - 1] > key; --i);
		if (i >= capacity) return;
		if (size < capacity) ++size;
		int j = i + 1;
		System.arraycopy(keys, i, keys, j, size - j);
		keys[i] = key;
		System.arraycopy(data, i, data, j, size - j);
		data[i] = value;
	}

	public double getMaxKey() {
		return keys[size - 1];
	}
	
	@SuppressWarnings("unchecked")
	public T removeMax() {
		if(isEmpty()) return null;
		return (T)data[--size];
	}

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

	public boolean isFull() {
		return size == capacity;
	}

	public int size() {
		return size;
	}

	public int capacity() {
		return capacity;
	}
}