added queryNearest

hxd/impl/Partition.hx

@@ -70,11 +70,15 @@ class Partition<T : { x : Float, y : Float, partChunk : Int, partNext : T, partQ
 		Make sure not to make a second query before the first one is fully processed.
 	**/
 	public inline function query( x : Float, y : Float, ray : Float ) : PartitionIterator<T> {
-		var q = runQuery(x, y, ray);
+		var q = runQueryDist(x, y, ray);
 		return new PartitionIterator(q);
 	}
 
-	function runQuery(x:Float, y:Float, r:Float) : T {
+	function runQueryDist(x, y, r) {
+		return runQueryInline(x, y, r, function(e) { var dx = e.x - x; var dy = e.y - y; return dx * dx + dy * dy; } );
+	}
+
+	inline function runQueryInline(x:Float, y:Float, r:Float, calcDistSq : T -> Float ) : T {
 		var xMin = Math.floor(x - r) >> pbits;
 		var yMin = Math.floor(y - r) >> pbits;
 		var xMax = (Math.ceil(x + r) + (psize-1)) >> pbits;
@@ -96,10 +100,8 @@ class Partition<T : { x : Float, y : Float, partChunk : Int, partNext : T, partQ
 				var e = c.elements;
 				if( e == null ) continue;
 				while( e != null ) {
-					var dx = e.x - x;
-					var dy = e.y - y;
-					var d = dx * dx + dy * dy;
-					if( d <= rr ) {
+					var d = calcDistSq(e);
+					if( d <= rr && d >= 0. ) {
 						if( r == null ) {
 							r = last = e;
 						} else {
@@ -116,6 +118,41 @@ class Partition<T : { x : Float, y : Float, partChunk : Int, partNext : T, partQ
 		return r;
 	}
 
+
+	public inline function queryNearest( x : Float, y : Float, ray : Float, calcDistSq : T -> Float ) : T {
+		var xMin = Math.floor(x - ray) >> pbits;
+		var yMin = Math.floor(y - ray) >> pbits;
+		var xMax = (Math.ceil(x + ray) + (psize-1)) >> pbits;
+		var yMax = (Math.ceil(y + ray) + (psize-1)) >> pbits;
+		var best : T = null, bestD = ray * ray;
+		xMin -= 1;
+		yMin -= 1;
+		xMax += 2;
+		yMax += 2;
+		if( xMin < 0 ) xMin = 0;
+		if( yMin < 0 ) yMin = 0;
+		if( xMax > pwidth ) xMax = pwidth;
+		if( yMax > pheight ) yMax = pheight;
+		for( cy in yMin...yMax ) {
+			var cid = cy * pwidth + xMin;
+			for( cx in xMin...xMax ) {
+				var c = chunks[cid++];
+				var e = c.elements;
+				if( e == null ) continue;
+				while( e != null ) {
+					var d = calcDistSq(e);
+					if( d <= bestD && d >= 0. ) {
+						best = e;
+						bestD = d;
+					}
+					e = e.partNext;
+				}
+			}
+		}
+		return best;
+	}
+
+
 	/**
 		Adds an entity to our partition system.
 		If the entity is already present, it will sync() it, but in a less efficient manner.