/**
* 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);
}
}
// 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);
}
}
