/** * Find KD-tree node whose key is nearest neighbor to * key. Implements the Nearest Neighbor algorithm (Table 6.4) of * * <PRE> * @techreport{AndrewMooreNearestNeighbor, * author = {Andrew Moore}, * title = {An introductory tutorial on kd-trees}, * institution = {Robotics Institute, Carnegie Mellon University}, * year = {1991}, * number = {Technical Report No. 209, Computer Laboratory, * University of Cambridge}, * address = {Pittsburgh, PA} * } * </PRE> * * @param key key for KD-tree node * * @return object at node nearest to key, or null on failure * * @throws KeySizeException if key.length mismatches K * */ public Object nearest(double[] key) { if (key.Length != 3) { throw new KeySizeException(); } // initial call is with infinite rectangle and max distance Rect3 hr = Rect3.infiniteHRect(); double max_dist_sqd = Double.MaxValue; Point3 keyp = new Point3(key); KDNode best = null; double best_dist_sq = Double.MaxValue; KDNode.nnbr(m_root, keyp, hr, max_dist_sqd, 0, ref best, ref best_dist_sq, null); Debug.Assert(best_dist_sq != Double.MaxValue); Debug.Assert(best != null); return(best.v); }
// Method Nearest Neighbor from Andrew Moore's thesis. Numbered // comments are direct quotes from there. Step "SDL" is added to // make the algorithm work correctly. NearestNeighborList solution // courtesy of Bjoern Heckel. // The nearest neighbor is returned in best, with distance // sqrt(best_dist_sq). Tmp is a temporary point, passed around // as an optimization so it doesn't need to be recreated all the // time. Can be passed in as null by callers. public static void nnbr(KDNode kd, Point3 target, Rect3 hr, double max_dist_sqd, int lev, ref KDNode best, ref double best_dist_sq, Point3 tmp) { // 1. if kd is empty then set dist-sqd to infinity and exit. if (kd == null) { return; } if (tmp == null) { tmp = new Point3(); } // 2. s := split field of kd int s = lev % 3; // 3. pivot := dom-elt field of kd Point3 pivot = kd.k; double pivot_to_target = Point3.sqrDist(pivot, target); // 4. Cut hr into to sub-hyperrectangles left-hr and right-hr. // The cut plane is through pivot and perpendicular to the s // dimension. Rect3 left_hr = hr; // optimize by not cloning Rect3 right_hr = (Rect3)hr.clone(); left_hr.max.setCoord(s, pivot.coord(s)); right_hr.min.setCoord(s, pivot.coord(s)); // 5. target-in-left := target_s <= pivot_s bool target_in_left = target.coord(s) < pivot.coord(s); KDNode nearer_kd; Rect3 nearer_hr; KDNode further_kd; Rect3 further_hr; // 6. if target-in-left then // 6.1. nearer-kd := left field of kd and nearer-hr := left-hr // 6.2. further-kd := right field of kd and further-hr := right-hr if (target_in_left) { nearer_kd = kd.left; nearer_hr = left_hr; further_kd = kd.right; further_hr = right_hr; } // // 7. if not target-in-left then // 7.1. nearer-kd := right field of kd and nearer-hr := right-hr // 7.2. further-kd := left field of kd and further-hr := left-hr else { nearer_kd = kd.right; nearer_hr = right_hr; further_kd = kd.left; further_hr = left_hr; } right_hr = null; // 8. Recursively call Nearest Neighbor with paramters // (nearer-kd, target, nearer-hr, max-dist-sqd), storing the // results in nearest and dist-sqd nnbr(nearer_kd, target, nearer_hr, max_dist_sqd, lev + 1, ref best, ref best_dist_sq, tmp); nearer_hr = null; KDNode nearest = best; double dist_sqd; dist_sqd = best_dist_sq; // 9. max-dist-sqd := minimum of max-dist-sqd and dist-sqd max_dist_sqd = Math.Min(max_dist_sqd, dist_sqd); // 10. A nearer point could only lie in further-kd if there were some // part of further-hr within distance sqrt(max-dist-sqd) of // target. If this is the case then Point3 closest = further_hr.closest(target, tmp); if (Point3.sqrDist(closest, target) < max_dist_sqd) { // 10.1 if (pivot-target)^2 < dist-sqd then if (pivot_to_target < dist_sqd) { // 10.1.1 nearest := (pivot, range-elt field of kd) nearest = kd; // 10.1.2 dist-sqd = (pivot-target)^2 dist_sqd = pivot_to_target; // add to nnl if (!kd.deleted) { best = kd; best_dist_sq = dist_sqd; } max_dist_sqd = best_dist_sq; } // 10.2 Recursively call Nearest Neighbor with parameters // (further-kd, target, further-hr, max-dist_sqd), // storing results in temp-nearest and temp-dist-sqd nnbr(further_kd, target, further_hr, max_dist_sqd, lev + 1, ref best, ref best_dist_sq, tmp); } }