diff options
Diffstat (limited to 'geom_bottleneck/bottleneck/src/ann/kd_fix_rad_search.cpp')
-rw-r--r-- | geom_bottleneck/bottleneck/src/ann/kd_fix_rad_search.cpp | 185 |
1 files changed, 185 insertions, 0 deletions
diff --git a/geom_bottleneck/bottleneck/src/ann/kd_fix_rad_search.cpp b/geom_bottleneck/bottleneck/src/ann/kd_fix_rad_search.cpp new file mode 100644 index 0000000..1a4749e --- /dev/null +++ b/geom_bottleneck/bottleneck/src/ann/kd_fix_rad_search.cpp @@ -0,0 +1,185 @@ +//---------------------------------------------------------------------- +// File: kd_fix_rad_search.cpp +// Programmer: Sunil Arya and David Mount +// Description: Standard kd-tree fixed-radius kNN search +// Last modified: 05/03/05 (Version 1.1) +//---------------------------------------------------------------------- +// Copyright (c) 1997-2005 University of Maryland and Sunil Arya and +// David Mount. All Rights Reserved. +// +// This software and related documentation is part of the Approximate +// Nearest Neighbor Library (ANN). This software is provided under +// the provisions of the Lesser GNU Public License (LGPL). See the +// file ../ReadMe.txt for further information. +// +// The University of Maryland (U.M.) and the authors make no +// representations about the suitability or fitness of this software for +// any purpose. It is provided "as is" without express or implied +// warranty. +//---------------------------------------------------------------------- +// History: +// Revision 1.1 05/03/05 +// Initial release +//---------------------------------------------------------------------- + +#include "kd_fix_rad_search.h" // kd fixed-radius search decls + +namespace geom_bt { +//---------------------------------------------------------------------- +// Approximate fixed-radius k nearest neighbor search +// The squared radius is provided, and this procedure finds the +// k nearest neighbors within the radius, and returns the total +// number of points lying within the radius. +// +// The method used for searching the kd-tree is a variation of the +// nearest neighbor search used in kd_search.cpp, except that the +// radius of the search ball is known. We refer the reader to that +// file for the explanation of the recursive search procedure. +//---------------------------------------------------------------------- + +//---------------------------------------------------------------------- +// To keep argument lists short, a number of global variables +// are maintained which are common to all the recursive calls. +// These are given below. +//---------------------------------------------------------------------- + +int ANNkdFRDim; // dimension of space +ANNpoint ANNkdFRQ; // query point +ANNdist ANNkdFRSqRad; // squared radius search bound +double ANNkdFRMaxErr; // max tolerable squared error +ANNpointArray ANNkdFRPts; // the points +ANNmin_k* ANNkdFRPointMK; // set of k closest points +int ANNkdFRPtsVisited; // total points visited +int ANNkdFRPtsInRange; // number of points in the range + +//---------------------------------------------------------------------- +// annkFRSearch - fixed radius search for k nearest neighbors +//---------------------------------------------------------------------- + +int ANNkd_tree::annkFRSearch( + ANNpoint q, // the query point + ANNdist sqRad, // squared radius search bound + int k, // number of near neighbors to return + ANNidxArray nn_idx, // nearest neighbor indices (returned) + ANNdistArray dd, // the approximate nearest neighbor + double eps) // the error bound +{ + ANNkdFRDim = dim; // copy arguments to static equivs + ANNkdFRQ = q; + ANNkdFRSqRad = sqRad; + ANNkdFRPts = pts; + ANNkdFRPtsVisited = 0; // initialize count of points visited + ANNkdFRPtsInRange = 0; // ...and points in the range + + ANNkdFRMaxErr = ANN_POW(1.0 + eps); + ANN_FLOP(2) // increment floating op count + + ANNkdFRPointMK = new ANNmin_k(k); // create set for closest k points + // search starting at the root + root->ann_FR_search(annBoxDistance(q, bnd_box_lo, bnd_box_hi, dim)); + + for (int i = 0; i < k; i++) { // extract the k-th closest points + if (dd != NULL) + dd[i] = ANNkdFRPointMK->ith_smallest_key(i); + if (nn_idx != NULL) + nn_idx[i] = ANNkdFRPointMK->ith_smallest_info(i); + } + + delete ANNkdFRPointMK; // deallocate closest point set + return ANNkdFRPtsInRange; // return final point count +} + +//---------------------------------------------------------------------- +// kd_split::ann_FR_search - search a splitting node +// Note: This routine is similar in structure to the standard kNN +// search. It visits the subtree that is closer to the query point +// first. For fixed-radius search, there is no benefit in visiting +// one subtree before the other, but we maintain the same basic +// code structure for the sake of uniformity. +//---------------------------------------------------------------------- + +void ANNkd_split::ann_FR_search(ANNdist box_dist) +{ + // check dist calc term condition + if (ANNmaxPtsVisited != 0 && ANNkdFRPtsVisited > ANNmaxPtsVisited) return; + + // distance to cutting plane + ANNcoord cut_diff = ANNkdFRQ[cut_dim] - cut_val; + + if (cut_diff < 0) { // left of cutting plane + child[ANN_LO]->ann_FR_search(box_dist);// visit closer child first + + ANNcoord box_diff = cd_bnds[ANN_LO] - ANNkdFRQ[cut_dim]; + if (box_diff < 0) // within bounds - ignore + box_diff = 0; + // distance to further box + box_dist = (ANNdist) ANN_SUM(box_dist, + ANN_DIFF(ANN_POW(box_diff), ANN_POW(cut_diff))); + + // visit further child if in range + if (box_dist * ANNkdFRMaxErr <= ANNkdFRSqRad) + child[ANN_HI]->ann_FR_search(box_dist); + + } + else { // right of cutting plane + child[ANN_HI]->ann_FR_search(box_dist);// visit closer child first + + ANNcoord box_diff = ANNkdFRQ[cut_dim] - cd_bnds[ANN_HI]; + if (box_diff < 0) // within bounds - ignore + box_diff = 0; + // distance to further box + box_dist = (ANNdist) ANN_SUM(box_dist, + ANN_DIFF(ANN_POW(box_diff), ANN_POW(cut_diff))); + + // visit further child if close enough + if (box_dist * ANNkdFRMaxErr <= ANNkdFRSqRad) + child[ANN_LO]->ann_FR_search(box_dist); + + } + ANN_FLOP(13) // increment floating ops + ANN_SPL(1) // one more splitting node visited +} + +//---------------------------------------------------------------------- +// kd_leaf::ann_FR_search - search points in a leaf node +// Note: The unreadability of this code is the result of +// some fine tuning to replace indexing by pointer operations. +//---------------------------------------------------------------------- + +void ANNkd_leaf::ann_FR_search(ANNdist box_dist) +{ + register ANNdist dist; // distance to data point + register ANNcoord* pp; // data coordinate pointer + register ANNcoord* qq; // query coordinate pointer + register ANNcoord t; + register int d; + + for (int i = 0; i < n_pts; i++) { // check points in bucket + + pp = ANNkdFRPts[bkt[i]]; // first coord of next data point + qq = ANNkdFRQ; // first coord of query point + dist = 0; + + for(d = 0; d < ANNkdFRDim; d++) { + ANN_COORD(1) // one more coordinate hit + ANN_FLOP(5) // increment floating ops + + t = *(qq++) - *(pp++); // compute length and adv coordinate + // exceeds dist to k-th smallest? + if( (dist = ANN_SUM(dist, ANN_POW(t))) > ANNkdFRSqRad) { + break; + } + } + + if (d >= ANNkdFRDim && // among the k best? + (ANN_ALLOW_SELF_MATCH || dist!=0)) { // and no self-match problem + // add it to the list + ANNkdFRPointMK->insert(dist, bkt[i]); + ANNkdFRPtsInRange++; // increment point count + } + } + ANN_LEAF(1) // one more leaf node visited + ANN_PTS(n_pts) // increment points visited + ANNkdFRPtsVisited += n_pts; // increment number of points visited +} +} |