diff options
Diffstat (limited to 'geom_bottleneck/bottleneck/src/ann')
-rw-r--r-- | geom_bottleneck/bottleneck/src/ann/ANN.cpp | 230 | ||||
-rw-r--r-- | geom_bottleneck/bottleneck/src/ann/bd_fix_rad_search.cpp | 64 | ||||
-rw-r--r-- | geom_bottleneck/bottleneck/src/ann/bd_pr_search.cpp | 66 | ||||
-rw-r--r-- | geom_bottleneck/bottleneck/src/ann/bd_search.cpp | 64 | ||||
-rw-r--r-- | geom_bottleneck/bottleneck/src/ann/bd_tree.cpp | 419 | ||||
-rw-r--r-- | geom_bottleneck/bottleneck/src/ann/kd_dump.cpp | 447 | ||||
-rw-r--r-- | geom_bottleneck/bottleneck/src/ann/kd_fix_rad_search.cpp | 185 | ||||
-rw-r--r-- | geom_bottleneck/bottleneck/src/ann/kd_pr_search.cpp | 221 | ||||
-rw-r--r-- | geom_bottleneck/bottleneck/src/ann/kd_search.cpp | 298 | ||||
-rw-r--r-- | geom_bottleneck/bottleneck/src/ann/kd_split.cpp | 632 | ||||
-rw-r--r-- | geom_bottleneck/bottleneck/src/ann/kd_tree.cpp | 560 | ||||
-rw-r--r-- | geom_bottleneck/bottleneck/src/ann/kd_util.cpp | 441 |
12 files changed, 3627 insertions, 0 deletions
diff --git a/geom_bottleneck/bottleneck/src/ann/ANN.cpp b/geom_bottleneck/bottleneck/src/ann/ANN.cpp new file mode 100644 index 0000000..7bae577 --- /dev/null +++ b/geom_bottleneck/bottleneck/src/ann/ANN.cpp @@ -0,0 +1,230 @@ +//---------------------------------------------------------------------- +// File: ANN.cpp +// Programmer: Sunil Arya and David Mount +// Description: Methods for ANN.h and ANNx.h +// Last modified: 01/27/10 (Version 1.1.2) +//---------------------------------------------------------------------- +// Copyright (c) 1997-2010 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 0.1 03/04/98 +// Initial release +// Revision 1.0 04/01/05 +// Added performance counting to annDist() +// Revision 1.1.2 01/27/10 +// Fixed minor compilation bugs for new versions of gcc +//---------------------------------------------------------------------- + +#ifdef _WIN32 +#include <ciso646> // make VS more conformal +#endif + +#include <cstdlib> // C standard lib defs +#include <ANN/ANNx.h> // all ANN includes +#include <ANN/ANNperf.h> // ANN performance + + + +using namespace std; // make std:: accessible + + +namespace geom_bt { +//---------------------------------------------------------------------- +// Point methods +//---------------------------------------------------------------------- + +//---------------------------------------------------------------------- +// Distance utility. +// (Note: In the nearest neighbor search, most distances are +// computed using partial distance calculations, not this +// procedure.) +//---------------------------------------------------------------------- + +ANNdist annDist( // interpoint squared distance + int dim, + ANNpoint p, + ANNpoint q) +{ + register int d; + register ANNcoord diff; + register ANNcoord dist; + + dist = 0; + for (d = 0; d < dim; d++) { + diff = p[d] - q[d]; + dist = ANN_SUM(dist, ANN_POW(diff)); + } + ANN_FLOP(3*dim) // performance counts + ANN_PTS(1) + ANN_COORD(dim) + return dist; +} + +//---------------------------------------------------------------------- +// annPrintPoint() prints a point to a given output stream. +//---------------------------------------------------------------------- + +void annPrintPt( // print a point + ANNpoint pt, // the point + int dim, // the dimension + std::ostream &out) // output stream +{ + for (int j = 0; j < dim; j++) { + out << pt[j]; + if (j < dim-1) out << " "; + } +} + +//---------------------------------------------------------------------- +// Point allocation/deallocation: +// +// Because points (somewhat like strings in C) are stored +// as pointers. Consequently, creating and destroying +// copies of points may require storage allocation. These +// procedures do this. +// +// annAllocPt() and annDeallocPt() allocate a deallocate +// storage for a single point, and return a pointer to it. +// +// annAllocPts() allocates an array of points as well a place +// to store their coordinates, and initializes the points to +// point to their respective coordinates. It allocates point +// storage in a contiguous block large enough to store all the +// points. It performs no initialization. +// +// annDeallocPts() should only be used on point arrays allocated +// by annAllocPts since it assumes that points are allocated in +// a block. +// +// annCopyPt() copies a point taking care to allocate storage +// for the new point. +// +// annAssignRect() assigns the coordinates of one rectangle to +// another. The two rectangles must have the same dimension +// (and it is not possible to test this here). +//---------------------------------------------------------------------- + +ANNpoint annAllocPt(int dim, ANNcoord c) // allocate 1 point +{ + ANNpoint p = new ANNcoord[dim]; + for (int i = 0; i < dim; i++) p[i] = c; + return p; +} + +ANNpointArray annAllocPts(int n, int dim) // allocate n pts in dim +{ + ANNpointArray pa = new ANNpoint[n]; // allocate points + ANNpoint p = new ANNcoord[n*dim]; // allocate space for coords + for (int i = 0; i < n; i++) { + pa[i] = &(p[i*dim]); + } + return pa; +} + +void annDeallocPt(ANNpoint &p) // deallocate 1 point +{ + delete [] p; + p = NULL; +} + +void annDeallocPts(ANNpointArray &pa) // deallocate points +{ + delete [] pa[0]; // dealloc coordinate storage + delete [] pa; // dealloc points + pa = NULL; +} + +ANNpoint annCopyPt(int dim, ANNpoint source) // copy point +{ + ANNpoint p = new ANNcoord[dim]; + for (int i = 0; i < dim; i++) p[i] = source[i]; + return p; +} + + // assign one rect to another +void annAssignRect(int dim, ANNorthRect &dest, const ANNorthRect &source) +{ + for (int i = 0; i < dim; i++) { + dest.lo[i] = source.lo[i]; + dest.hi[i] = source.hi[i]; + } +} + + // is point inside rectangle? +ANNbool ANNorthRect::inside(const int dim, ANNpoint p) const +{ + for (int i = 0; i < dim; i++) { + if (p[i] < lo[i] || p[i] > hi[i]) return ANNfalse; + } + return ANNtrue; +} + +bool ANNorthRect::contains(const int dim, const ANNorthRect& r) const +{ + return this->inside(dim, r.hi) and this->inside(dim, r.lo); +} + +bool ANNorthRect::intersects(const int dim, const ANNorthRect& r) const +{ + assert(dim == 2); // works for plane only + const ANNpoint otherLo = r.lo; + const ANNpoint otherHi = r.hi; + if ( otherLo[0] > hi[0] or + otherLo[1] > hi[1] or + otherHi[0] < lo[0] or + otherHi[1] < lo[1]) { + return false; + } else { + return true; + } +} + +//---------------------------------------------------------------------- +// Error handler +//---------------------------------------------------------------------- + +void annError(const char* msg, ANNerr level) +{ + if (level == ANNabort) { + cerr << "ANN: ERROR------->" << msg << "<-------------ERROR\n"; + exit(1); + } + else { + cerr << "ANN: WARNING----->" << msg << "<-------------WARNING\n"; + } +} + +//---------------------------------------------------------------------- +// Limit on number of points visited +// We have an option for terminating the search early if the +// number of points visited exceeds some threshold. If the +// threshold is 0 (its default) this means there is no limit +// and the algorithm applies its normal termination condition. +// This is for applications where there are real time constraints +// on the running time of the algorithm. +//---------------------------------------------------------------------- + +int ANNmaxPtsVisited = 0; // maximum number of pts visited +int ANNptsVisited; // number of pts visited in search + +//---------------------------------------------------------------------- +// Global function declarations +//---------------------------------------------------------------------- + +void annMaxPtsVisit( // set limit on max. pts to visit in search + int maxPts) // the limit +{ + ANNmaxPtsVisited = maxPts; +} +} diff --git a/geom_bottleneck/bottleneck/src/ann/bd_fix_rad_search.cpp b/geom_bottleneck/bottleneck/src/ann/bd_fix_rad_search.cpp new file mode 100644 index 0000000..fe8ab78 --- /dev/null +++ b/geom_bottleneck/bottleneck/src/ann/bd_fix_rad_search.cpp @@ -0,0 +1,64 @@ +//---------------------------------------------------------------------- +// File: bd_fix_rad_search.cpp +// Programmer: David Mount +// Description: Standard bd-tree 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 "bd_tree.h" // bd-tree declarations +#include "kd_fix_rad_search.h" // kd-tree FR search declarations + +namespace geom_bt { + +//---------------------------------------------------------------------- +// Approximate searching for bd-trees. +// See the file kd_FR_search.cpp for general information on the +// approximate nearest neighbor search algorithm. Here we +// include the extensions for shrinking nodes. +//---------------------------------------------------------------------- + +//---------------------------------------------------------------------- +// bd_shrink::ann_FR_search - search a shrinking node +//---------------------------------------------------------------------- + +void ANNbd_shrink::ann_FR_search(ANNdist box_dist) +{ + // check dist calc term cond. + if (ANNmaxPtsVisited != 0 && ANNptsVisited > ANNmaxPtsVisited) return; + + ANNdist inner_dist = 0; // distance to inner box + for (int i = 0; i < n_bnds; i++) { // is query point in the box? + if (bnds[i].out(ANNkdFRQ)) { // outside this bounding side? + // add to inner distance + inner_dist = (ANNdist) ANN_SUM(inner_dist, bnds[i].dist(ANNkdFRQ)); + } + } + if (inner_dist <= box_dist) { // if inner box is closer + child[ANN_IN]->ann_FR_search(inner_dist);// search inner child first + child[ANN_OUT]->ann_FR_search(box_dist);// ...then outer child + } + else { // if outer box is closer + child[ANN_OUT]->ann_FR_search(box_dist);// search outer child first + child[ANN_IN]->ann_FR_search(inner_dist);// ...then outer child + } + ANN_FLOP(3*n_bnds) // increment floating ops + ANN_SHR(1) // one more shrinking node +} +} diff --git a/geom_bottleneck/bottleneck/src/ann/bd_pr_search.cpp b/geom_bottleneck/bottleneck/src/ann/bd_pr_search.cpp new file mode 100644 index 0000000..fb9dea6 --- /dev/null +++ b/geom_bottleneck/bottleneck/src/ann/bd_pr_search.cpp @@ -0,0 +1,66 @@ +//---------------------------------------------------------------------- +// File: bd_pr_search.cpp +// Programmer: David Mount +// Description: Priority search for bd-trees +// Last modified: 01/04/05 (Version 1.0) +//---------------------------------------------------------------------- +// 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 0.1 03/04/98 +// Initial release +//---------------------------------------------------------------------- + +#include "bd_tree.h" // bd-tree declarations +#include "kd_pr_search.h" // kd priority search declarations + + +namespace geom_bt { + +//---------------------------------------------------------------------- +// Approximate priority searching for bd-trees. +// See the file kd_pr_search.cc for general information on the +// approximate nearest neighbor priority search algorithm. Here +// we include the extensions for shrinking nodes. +//---------------------------------------------------------------------- + +//---------------------------------------------------------------------- +// bd_shrink::ann_search - search a shrinking node +//---------------------------------------------------------------------- + +void ANNbd_shrink::ann_pri_search(ANNdist box_dist) +{ + ANNdist inner_dist = 0; // distance to inner box + for (int i = 0; i < n_bnds; i++) { // is query point in the box? + if (bnds[i].out(ANNprQ)) { // outside this bounding side? + // add to inner distance + inner_dist = (ANNdist) ANN_SUM(inner_dist, bnds[i].dist(ANNprQ)); + } + } + if (inner_dist <= box_dist) { // if inner box is closer + if (child[ANN_OUT] != KD_TRIVIAL) // enqueue outer if not trivial + ANNprBoxPQ->insert(box_dist,child[ANN_OUT]); + // continue with inner child + child[ANN_IN]->ann_pri_search(inner_dist); + } + else { // if outer box is closer + if (child[ANN_IN] != KD_TRIVIAL) // enqueue inner if not trivial + ANNprBoxPQ->insert(inner_dist,child[ANN_IN]); + // continue with outer child + child[ANN_OUT]->ann_pri_search(box_dist); + } + ANN_FLOP(3*n_bnds) // increment floating ops + ANN_SHR(1) // one more shrinking node +} +} diff --git a/geom_bottleneck/bottleneck/src/ann/bd_search.cpp b/geom_bottleneck/bottleneck/src/ann/bd_search.cpp new file mode 100644 index 0000000..2935bcb --- /dev/null +++ b/geom_bottleneck/bottleneck/src/ann/bd_search.cpp @@ -0,0 +1,64 @@ +//---------------------------------------------------------------------- +// File: bd_search.cpp +// Programmer: David Mount +// Description: Standard bd-tree search +// Last modified: 01/04/05 (Version 1.0) +//---------------------------------------------------------------------- +// 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 0.1 03/04/98 +// Initial release +//---------------------------------------------------------------------- + +#include "bd_tree.h" // bd-tree declarations +#include "kd_search.h" // kd-tree search declarations + +namespace geom_bt { + +//---------------------------------------------------------------------- +// Approximate searching for bd-trees. +// See the file kd_search.cpp for general information on the +// approximate nearest neighbor search algorithm. Here we +// include the extensions for shrinking nodes. +//---------------------------------------------------------------------- + +//---------------------------------------------------------------------- +// bd_shrink::ann_search - search a shrinking node +//---------------------------------------------------------------------- + +void ANNbd_shrink::ann_search(ANNdist box_dist) +{ + // check dist calc term cond. + if (ANNmaxPtsVisited != 0 && ANNptsVisited > ANNmaxPtsVisited) return; + + ANNdist inner_dist = 0; // distance to inner box + for (int i = 0; i < n_bnds; i++) { // is query point in the box? + if (bnds[i].out(ANNkdQ)) { // outside this bounding side? + // add to inner distance + inner_dist = (ANNdist) ANN_SUM(inner_dist, bnds[i].dist(ANNkdQ)); + } + } + if (inner_dist <= box_dist) { // if inner box is closer + child[ANN_IN]->ann_search(inner_dist); // search inner child first + child[ANN_OUT]->ann_search(box_dist); // ...then outer child + } + else { // if outer box is closer + child[ANN_OUT]->ann_search(box_dist); // search outer child first + child[ANN_IN]->ann_search(inner_dist); // ...then outer child + } + ANN_FLOP(3*n_bnds) // increment floating ops + ANN_SHR(1) // one more shrinking node +} +} diff --git a/geom_bottleneck/bottleneck/src/ann/bd_tree.cpp b/geom_bottleneck/bottleneck/src/ann/bd_tree.cpp new file mode 100644 index 0000000..8c1ef6d --- /dev/null +++ b/geom_bottleneck/bottleneck/src/ann/bd_tree.cpp @@ -0,0 +1,419 @@ +//---------------------------------------------------------------------- +// File: bd_tree.cpp +// Programmer: David Mount +// Description: Basic methods for bd-trees. +// Last modified: 01/04/05 (Version 1.0) +//---------------------------------------------------------------------- +// 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 0.1 03/04/98 +// Initial release +// Revision l.0 04/01/05 +// Fixed centroid shrink threshold condition to depend on the +// dimension. +// Moved dump routine to kd_dump.cpp. +//---------------------------------------------------------------------- + +#include "bd_tree.h" // bd-tree declarations +#include "kd_util.h" // kd-tree utilities +#include "kd_split.h" // kd-tree splitting rules + +#include <ANN/ANNperf.h> // performance evaluation + +namespace geom_bt { +//---------------------------------------------------------------------- +// Printing a bd-tree +// These routines print a bd-tree. See the analogous procedure +// in kd_tree.cpp for more information. +//---------------------------------------------------------------------- + +void ANNbd_shrink::print( // print shrinking node + int level, // depth of node in tree + ostream &out) // output stream +{ + child[ANN_OUT]->print(level+1, out); // print out-child + + out << " "; + for (int i = 0; i < level; i++) // print indentation + out << ".."; + out << "Shrink"; + for (int j = 0; j < n_bnds; j++) { // print sides, 2 per line + if (j % 2 == 0) { + out << "\n"; // newline and indentation + for (int i = 0; i < level+2; i++) out << " "; + } + out << " ([" << bnds[j].cd << "]" + << (bnds[j].sd > 0 ? ">=" : "< ") + << bnds[j].cv << ")"; + } + out << "\n"; + + child[ANN_IN]->print(level+1, out); // print in-child +} + +//---------------------------------------------------------------------- +// kd_tree statistics utility (for performance evaluation) +// This routine computes various statistics information for +// shrinking nodes. See file kd_tree.cpp for more information. +//---------------------------------------------------------------------- + +void ANNbd_shrink::getStats( // get subtree statistics + int dim, // dimension of space + ANNkdStats &st, // stats (modified) + ANNorthRect &bnd_box) // bounding box +{ + ANNkdStats ch_stats; // stats for children + ANNorthRect inner_box(dim); // inner box of shrink + + annBnds2Box(bnd_box, // enclosing box + dim, // dimension + n_bnds, // number of bounds + bnds, // bounds array + inner_box); // inner box (modified) + // get stats for inner child + ch_stats.reset(); // reset + child[ANN_IN]->getStats(dim, ch_stats, inner_box); + st.merge(ch_stats); // merge them + // get stats for outer child + ch_stats.reset(); // reset + child[ANN_OUT]->getStats(dim, ch_stats, bnd_box); + st.merge(ch_stats); // merge them + + st.depth++; // increment depth + st.n_shr++; // increment number of shrinks +} + +//---------------------------------------------------------------------- +// bd-tree constructor +// This is the main constructor for bd-trees given a set of points. +// It first builds a skeleton kd-tree as a basis, then computes the +// bounding box of the data points, and then invokes rbd_tree() to +// actually build the tree, passing it the appropriate splitting +// and shrinking information. +//---------------------------------------------------------------------- + +ANNkd_ptr rbd_tree( // recursive construction of bd-tree + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices to store in subtree + int n, // number of points + int dim, // dimension of space + int bsp, // bucket space + ANNorthRect &bnd_box, // bounding box for current node + ANNkd_splitter splitter, // splitting routine + ANNshrinkRule shrink); // shrinking rule + +ANNbd_tree::ANNbd_tree( // construct from point array + ANNpointArray pa, // point array (with at least n pts) + int n, // number of points + int dd, // dimension + int bs, // bucket size + ANNsplitRule split, // splitting rule + ANNshrinkRule shrink) // shrinking rule + : ANNkd_tree(n, dd, bs) // build skeleton base tree +{ + pts = pa; // where the points are + if (n == 0) return; // no points--no sweat + + ANNorthRect bnd_box(dd); // bounding box for points + // construct bounding rectangle + annEnclRect(pa, pidx, n, dd, bnd_box); + // copy to tree structure + bnd_box_lo = annCopyPt(dd, bnd_box.lo); + bnd_box_hi = annCopyPt(dd, bnd_box.hi); + + switch (split) { // build by rule + case ANN_KD_STD: // standard kd-splitting rule + root = rbd_tree(pa, pidx, n, dd, bs, bnd_box, kd_split, shrink); + break; + case ANN_KD_MIDPT: // midpoint split + root = rbd_tree(pa, pidx, n, dd, bs, bnd_box, midpt_split, shrink); + break; + case ANN_KD_SUGGEST: // best (in our opinion) + case ANN_KD_SL_MIDPT: // sliding midpoint split + root = rbd_tree(pa, pidx, n, dd, bs, bnd_box, sl_midpt_split, shrink); + break; + case ANN_KD_FAIR: // fair split + root = rbd_tree(pa, pidx, n, dd, bs, bnd_box, fair_split, shrink); + break; + case ANN_KD_SL_FAIR: // sliding fair split + root = rbd_tree(pa, pidx, n, dd, bs, + bnd_box, sl_fair_split, shrink); + break; + default: + annError("Illegal splitting method", ANNabort); + } +} + +//---------------------------------------------------------------------- +// Shrinking rules +//---------------------------------------------------------------------- + +enum ANNdecomp {SPLIT, SHRINK}; // decomposition methods + +//---------------------------------------------------------------------- +// trySimpleShrink - Attempt a simple shrink +// +// We compute the tight bounding box of the points, and compute +// the 2*dim ``gaps'' between the sides of the tight box and the +// bounding box. If any of the gaps is large enough relative to +// the longest side of the tight bounding box, then we shrink +// all sides whose gaps are large enough. (The reason for +// comparing against the tight bounding box, is that after +// shrinking the longest box size will decrease, and if we use +// the standard bounding box, we may decide to shrink twice in +// a row. Since the tight box is fixed, we cannot shrink twice +// consecutively.) +//---------------------------------------------------------------------- +const float BD_GAP_THRESH = 0.5; // gap threshold (must be < 1) +const int BD_CT_THRESH = 2; // min number of shrink sides + +ANNdecomp trySimpleShrink( // try a simple shrink + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices to store in subtree + int n, // number of points + int dim, // dimension of space + const ANNorthRect &bnd_box, // current bounding box + ANNorthRect &inner_box) // inner box if shrinking (returned) +{ + int i; + // compute tight bounding box + annEnclRect(pa, pidx, n, dim, inner_box); + + ANNcoord max_length = 0; // find longest box side + for (i = 0; i < dim; i++) { + ANNcoord length = inner_box.hi[i] - inner_box.lo[i]; + if (length > max_length) { + max_length = length; + } + } + + int shrink_ct = 0; // number of sides we shrunk + for (i = 0; i < dim; i++) { // select which sides to shrink + // gap between boxes + ANNcoord gap_hi = bnd_box.hi[i] - inner_box.hi[i]; + // big enough gap to shrink? + if (gap_hi < max_length*BD_GAP_THRESH) + inner_box.hi[i] = bnd_box.hi[i]; // no - expand + else shrink_ct++; // yes - shrink this side + + // repeat for high side + ANNcoord gap_lo = inner_box.lo[i] - bnd_box.lo[i]; + if (gap_lo < max_length*BD_GAP_THRESH) + inner_box.lo[i] = bnd_box.lo[i]; // no - expand + else shrink_ct++; // yes - shrink this side + } + + if (shrink_ct >= BD_CT_THRESH) // did we shrink enough sides? + return SHRINK; + else return SPLIT; +} + +//---------------------------------------------------------------------- +// tryCentroidShrink - Attempt a centroid shrink +// +// We repeatedly apply the splitting rule, always to the larger subset +// of points, until the number of points decreases by the constant +// fraction BD_FRACTION. If this takes more than dim*BD_MAX_SPLIT_FAC +// splits for this to happen, then we shrink to the final inner box +// Otherwise we split. +//---------------------------------------------------------------------- + +const float BD_MAX_SPLIT_FAC = 0.5; // maximum number of splits allowed +const float BD_FRACTION = 0.5; // ...to reduce points by this fraction + // ...This must be < 1. + +ANNdecomp tryCentroidShrink( // try a centroid shrink + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices to store in subtree + int n, // number of points + int dim, // dimension of space + const ANNorthRect &bnd_box, // current bounding box + ANNkd_splitter splitter, // splitting procedure + ANNorthRect &inner_box) // inner box if shrinking (returned) +{ + int n_sub = n; // number of points in subset + int n_goal = (int) (n*BD_FRACTION); // number of point in goal + int n_splits = 0; // number of splits needed + // initialize inner box to bounding box + annAssignRect(dim, inner_box, bnd_box); + + while (n_sub > n_goal) { // keep splitting until goal reached + int cd; // cut dim from splitter (ignored) + ANNcoord cv; // cut value from splitter (ignored) + int n_lo; // number of points on low side + // invoke splitting procedure + (*splitter)(pa, pidx, inner_box, n_sub, dim, cd, cv, n_lo); + n_splits++; // increment split count + + if (n_lo >= n_sub/2) { // most points on low side + inner_box.hi[cd] = cv; // collapse high side + n_sub = n_lo; // recurse on lower points + } + else { // most points on high side + inner_box.lo[cd] = cv; // collapse low side + pidx += n_lo; // recurse on higher points + n_sub -= n_lo; + } + } + if (n_splits > dim*BD_MAX_SPLIT_FAC)// took too many splits + return SHRINK; // shrink to final subset + else + return SPLIT; +} + +//---------------------------------------------------------------------- +// selectDecomp - select which decomposition to use +//---------------------------------------------------------------------- + +ANNdecomp selectDecomp( // select decomposition method + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices to store in subtree + int n, // number of points + int dim, // dimension of space + const ANNorthRect &bnd_box, // current bounding box + ANNkd_splitter splitter, // splitting procedure + ANNshrinkRule shrink, // shrinking rule + ANNorthRect &inner_box) // inner box if shrinking (returned) +{ + ANNdecomp decomp = SPLIT; // decomposition + + switch (shrink) { // check shrinking rule + case ANN_BD_NONE: // no shrinking allowed + decomp = SPLIT; + break; + case ANN_BD_SUGGEST: // author's suggestion + case ANN_BD_SIMPLE: // simple shrink + decomp = trySimpleShrink( + pa, pidx, // points and indices + n, dim, // number of points and dimension + bnd_box, // current bounding box + inner_box); // inner box if shrinking (returned) + break; + case ANN_BD_CENTROID: // centroid shrink + decomp = tryCentroidShrink( + pa, pidx, // points and indices + n, dim, // number of points and dimension + bnd_box, // current bounding box + splitter, // splitting procedure + inner_box); // inner box if shrinking (returned) + break; + default: + annError("Illegal shrinking rule", ANNabort); + } + return decomp; +} + +//---------------------------------------------------------------------- +// rbd_tree - recursive procedure to build a bd-tree +// +// This is analogous to rkd_tree, but for bd-trees. See the +// procedure rkd_tree() in kd_split.cpp for more information. +// +// If the number of points falls below the bucket size, then a +// leaf node is created for the points. Otherwise we invoke the +// procedure selectDecomp() which determines whether we are to +// split or shrink. If splitting is chosen, then we essentially +// do exactly as rkd_tree() would, and invoke the specified +// splitting procedure to the points. Otherwise, the selection +// procedure returns a bounding box, from which we extract the +// appropriate shrinking bounds, and create a shrinking node. +// Finally the points are subdivided, and the procedure is +// invoked recursively on the two subsets to form the children. +//---------------------------------------------------------------------- + +ANNkd_ptr rbd_tree( // recursive construction of bd-tree + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices to store in subtree + int n, // number of points + int dim, // dimension of space + int bsp, // bucket space + ANNorthRect &bnd_box, // bounding box for current node + ANNkd_splitter splitter, // splitting routine + ANNshrinkRule shrink) // shrinking rule +{ + ANNdecomp decomp; // decomposition method + + ANNorthRect inner_box(dim); // inner box (if shrinking) + + if (n <= bsp) { // n small, make a leaf node + if (n == 0) // empty leaf node + return KD_TRIVIAL; // return (canonical) empty leaf + else // construct the node and return + return new ANNkd_leaf(n, pidx); + } + + decomp = selectDecomp( // select decomposition method + pa, pidx, // points and indices + n, dim, // number of points and dimension + bnd_box, // current bounding box + splitter, shrink, // splitting/shrinking methods + inner_box); // inner box if shrinking (returned) + + if (decomp == SPLIT) { // split selected + int cd; // cutting dimension + ANNcoord cv; // cutting value + int n_lo; // number on low side of cut + // invoke splitting procedure + (*splitter)(pa, pidx, bnd_box, n, dim, cd, cv, n_lo); + + ANNcoord lv = bnd_box.lo[cd]; // save bounds for cutting dimension + ANNcoord hv = bnd_box.hi[cd]; + + bnd_box.hi[cd] = cv; // modify bounds for left subtree + ANNkd_ptr lo = rbd_tree( // build left subtree + pa, pidx, n_lo, // ...from pidx[0..n_lo-1] + dim, bsp, bnd_box, splitter, shrink); + bnd_box.hi[cd] = hv; // restore bounds + + bnd_box.lo[cd] = cv; // modify bounds for right subtree + ANNkd_ptr hi = rbd_tree( // build right subtree + pa, pidx + n_lo, n-n_lo,// ...from pidx[n_lo..n-1] + dim, bsp, bnd_box, splitter, shrink); + bnd_box.lo[cd] = lv; // restore bounds + // create the splitting node + return new ANNkd_split(cd, cv, lv, hv, lo, hi); + } + else { // shrink selected + int n_in; // number of points in box + int n_bnds; // number of bounding sides + + annBoxSplit( // split points around inner box + pa, // points to split + pidx, // point indices + n, // number of points + dim, // dimension + inner_box, // inner box + n_in); // number of points inside (returned) + + ANNkd_ptr in = rbd_tree( // build inner subtree pidx[0..n_in-1] + pa, pidx, n_in, dim, bsp, inner_box, splitter, shrink); + ANNkd_ptr out = rbd_tree( // build outer subtree pidx[n_in..n] + pa, pidx+n_in, n - n_in, dim, bsp, bnd_box, splitter, shrink); + + ANNorthHSArray bnds = NULL; // bounds (alloc in Box2Bnds and + // ...freed in bd_shrink destroyer) + + annBox2Bnds( // convert inner box to bounds + inner_box, // inner box + bnd_box, // enclosing box + dim, // dimension + n_bnds, // number of bounds (returned) + bnds); // bounds array (modified) + + // return shrinking node + return new ANNbd_shrink(n_bnds, bnds, in, out); + } +} +} diff --git a/geom_bottleneck/bottleneck/src/ann/kd_dump.cpp b/geom_bottleneck/bottleneck/src/ann/kd_dump.cpp new file mode 100644 index 0000000..64db9a7 --- /dev/null +++ b/geom_bottleneck/bottleneck/src/ann/kd_dump.cpp @@ -0,0 +1,447 @@ +//---------------------------------------------------------------------- +// File: kd_dump.cc +// Programmer: David Mount +// Description: Dump and Load for kd- and bd-trees +// Last modified: 01/04/05 (Version 1.0) +//---------------------------------------------------------------------- +// 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 0.1 03/04/98 +// Initial release +// Revision 1.0 04/01/05 +// Moved dump out of kd_tree.cc into this file. +// Added kd-tree load constructor. +//---------------------------------------------------------------------- +// This file contains routines for dumping kd-trees and bd-trees and +// reloading them. (It is an abuse of policy to include both kd- and +// bd-tree routines in the same file, sorry. There should be no problem +// in deleting the bd- versions of the routines if they are not +// desired.) +//---------------------------------------------------------------------- + +#include "kd_tree.h" // kd-tree declarations +#include "bd_tree.h" // bd-tree declarations + +using namespace std; // make std:: available + +namespace geom_bt { + + //---------------------------------------------------------------------- + // Constants + //---------------------------------------------------------------------- + + const int STRING_LEN = 500; // maximum string length + const double EPSILON = 1E-5; // small number for float comparison + + enum ANNtreeType { KD_TREE, BD_TREE }; // tree types (used in loading) + + //---------------------------------------------------------------------- + // Procedure declarations + //---------------------------------------------------------------------- + + static ANNkd_ptr annReadDump( // read dump file + istream &in, // input stream + ANNtreeType tree_type, // type of tree expected + ANNpointArray &the_pts, // new points (if applic) + ANNidxArray &the_pidx, // point indices (returned) + int &the_dim, // dimension (returned) + int &the_n_pts, // number of points (returned) + int &the_bkt_size, // bucket size (returned) + ANNpoint &the_bnd_box_lo, // low bounding point + ANNpoint &the_bnd_box_hi); // high bounding point + + static ANNkd_ptr annReadTree( // read tree-part of dump file + istream &in, // input stream + ANNtreeType tree_type, // type of tree expected + ANNidxArray the_pidx, // point indices (modified) + int &next_idx); // next index (modified) + + //---------------------------------------------------------------------- + // ANN kd- and bd-tree Dump Format + // The dump file begins with a header containing the version of + // ANN, an optional section containing the points, followed by + // a description of the tree. The tree is printed in preorder. + // + // Format: + // #ANN <version number> <comments> [END_OF_LINE] + // points <dim> <n_pts> (point coordinates: this is optional) + // 0 <xxx> <xxx> ... <xxx> (point indices and coordinates) + // 1 <xxx> <xxx> ... <xxx> + // ... + // tree <dim> <n_pts> <bkt_size> + // <xxx> <xxx> ... <xxx> (lower end of bounding box) + // <xxx> <xxx> ... <xxx> (upper end of bounding box) + // If the tree is null, then a single line "null" is + // output. Otherwise the nodes of the tree are printed + // one per line in preorder. Leaves and splitting nodes + // have the following formats: + // Leaf node: + // leaf <n_pts> <bkt[0]> <bkt[1]> ... <bkt[n-1]> + // Splitting nodes: + // split <cut_dim> <cut_val> <lo_bound> <hi_bound> + // + // For bd-trees: + // + // Shrinking nodes: + // shrink <n_bnds> + // <cut_dim> <cut_val> <side> + // <cut_dim> <cut_val> <side> + // ... (repeated n_bnds times) + //---------------------------------------------------------------------- + + void ANNkd_tree::Dump( // dump entire tree + ANNbool with_pts, // print points as well? + ostream &out) // output stream + { + out << "#ANN " << ANNversion << "\n"; + out.precision(ANNcoordPrec); // use full precision in dumping + if (with_pts) { // print point coordinates + out << "points " << dim << " " << n_pts << "\n"; + for (int i = 0; i < n_pts; i++) { + out << i << " "; + annPrintPt(pts[i], dim, out); + out << "\n"; + } + } + out << "tree " // print tree elements + << dim << " " + << n_pts << " " + << bkt_size << "\n"; + + annPrintPt(bnd_box_lo, dim, out); // print lower bound + out << "\n"; + annPrintPt(bnd_box_hi, dim, out); // print upper bound + out << "\n"; + + if (root == NULL) // empty tree? + out << "null\n"; + else { + root->dump(out); // invoke printing at root + } + out.precision(0); // restore default precision + } + + void ANNkd_split::dump( // dump a splitting node + ostream &out) // output stream + { + out << "split " << cut_dim << " " << cut_val << " "; + out << cd_bnds[ANN_LO] << " " << cd_bnds[ANN_HI] << "\n"; + + child[ANN_LO]->dump(out); // print low child + child[ANN_HI]->dump(out); // print high child + } + + void ANNkd_leaf::dump( // dump a leaf node + ostream &out) // output stream + { + if (this == KD_TRIVIAL) { // canonical trivial leaf node + out << "leaf 0\n"; // leaf no points + } + else { + out << "leaf " << n_pts; + for (int j = 0; j < n_pts; j++) { + out << " " << bkt[j]; + } + out << "\n"; + } + } + + void ANNbd_shrink::dump( // dump a shrinking node + ostream &out) // output stream + { + out << "shrink " << n_bnds << "\n"; + for (int j = 0; j < n_bnds; j++) { + out << bnds[j].cd << " " << bnds[j].cv << " " << bnds[j].sd << "\n"; + } + child[ANN_IN]->dump(out); // print in-child + child[ANN_OUT]->dump(out); // print out-child + } + + //---------------------------------------------------------------------- + // Load kd-tree from dump file + // This rebuilds a kd-tree which was dumped to a file. The dump + // file contains all the basic tree information according to a + // preorder traversal. We assume that the dump file also contains + // point data. (This is to guarantee the consistency of the tree.) + // If not, then an error is generated. + // + // Indirectly, this procedure allocates space for points, point + // indices, all nodes in the tree, and the bounding box for the + // tree. When the tree is destroyed, all but the points are + // deallocated. + // + // This routine calls annReadDump to do all the work. + //---------------------------------------------------------------------- + + ANNkd_tree::ANNkd_tree( // build from dump file + istream &in) // input stream for dump file + { + int the_dim; // local dimension + int the_n_pts; // local number of points + int the_bkt_size; // local number of points + ANNpoint the_bnd_box_lo; // low bounding point + ANNpoint the_bnd_box_hi; // high bounding point + ANNpointArray the_pts; // point storage + ANNidxArray the_pidx; // point index storage + ANNkd_ptr the_root; // root of the tree + + the_root = annReadDump( // read the dump file + in, // input stream + KD_TREE, // expecting a kd-tree + the_pts, // point array (returned) + the_pidx, // point indices (returned) + the_dim, the_n_pts, the_bkt_size, // basic tree info (returned) + the_bnd_box_lo, the_bnd_box_hi); // bounding box info (returned) + + // create a skeletal tree + SkeletonTree(the_n_pts, the_dim, the_bkt_size, the_pts, the_pidx); + + bnd_box_lo = the_bnd_box_lo; + bnd_box_hi = the_bnd_box_hi; + + root = the_root; // set the root + } + + ANNbd_tree::ANNbd_tree( // build bd-tree from dump file + istream &in) : ANNkd_tree() // input stream for dump file + { + int the_dim; // local dimension + int the_n_pts; // local number of points + int the_bkt_size; // local number of points + ANNpoint the_bnd_box_lo; // low bounding point + ANNpoint the_bnd_box_hi; // high bounding point + ANNpointArray the_pts; // point storage + ANNidxArray the_pidx; // point index storage + ANNkd_ptr the_root; // root of the tree + + the_root = annReadDump( // read the dump file + in, // input stream + BD_TREE, // expecting a bd-tree + the_pts, // point array (returned) + the_pidx, // point indices (returned) + the_dim, the_n_pts, the_bkt_size, // basic tree info (returned) + the_bnd_box_lo, the_bnd_box_hi); // bounding box info (returned) + + // create a skeletal tree + SkeletonTree(the_n_pts, the_dim, the_bkt_size, the_pts, the_pidx); + bnd_box_lo = the_bnd_box_lo; + bnd_box_hi = the_bnd_box_hi; + + root = the_root; // set the root + } + + //---------------------------------------------------------------------- + // annReadDump - read a dump file + // + // This procedure reads a dump file, constructs a kd-tree + // and returns all the essential information needed to actually + // construct the tree. Because this procedure is used for + // constructing both kd-trees and bd-trees, the second argument + // is used to indicate which type of tree we are expecting. + //---------------------------------------------------------------------- + + static ANNkd_ptr annReadDump( + istream &in, // input stream + ANNtreeType tree_type, // type of tree expected + ANNpointArray &the_pts, // new points (returned) + ANNidxArray &the_pidx, // point indices (returned) + int &the_dim, // dimension (returned) + int &the_n_pts, // number of points (returned) + int &the_bkt_size, // bucket size (returned) + ANNpoint &the_bnd_box_lo, // low bounding point (ret'd) + ANNpoint &the_bnd_box_hi) // high bounding point (ret'd) + { + int j; + char str[STRING_LEN]; // storage for string + char version[STRING_LEN]; // ANN version number + ANNkd_ptr the_root = NULL; + + //------------------------------------------------------------------ + // Input file header + //------------------------------------------------------------------ + in >> str; // input header + if (strcmp(str, "#ANN") != 0) { // incorrect header + annError("Incorrect header for dump file", ANNabort); + } + in.getline(version, STRING_LEN); // get version (ignore) + + //------------------------------------------------------------------ + // Input the points + // An array the_pts is allocated and points are read from + // the dump file. + //------------------------------------------------------------------ + in >> str; // get major heading + if (strcmp(str, "points") == 0) { // points section + in >> the_dim; // input dimension + in >> the_n_pts; // number of points + // allocate point storage + the_pts = annAllocPts(the_n_pts, the_dim); + for (int i = 0; i < the_n_pts; i++) { // input point coordinates + ANNidx idx; // point index + in >> idx; // input point index + if (idx < 0 || idx >= the_n_pts) { + annError("Point index is out of range", ANNabort); + } + for (j = 0; j < the_dim; j++) { + in >> the_pts[idx][j]; // read point coordinates + } + } + in >> str; // get next major heading + } + else { // no points were input + annError("Points must be supplied in the dump file", ANNabort); + } + + //------------------------------------------------------------------ + // Input the tree + // After the basic header information, we invoke annReadTree + // to do all the heavy work. We create our own array of + // point indices (so we can pass them to annReadTree()) + // but we do not deallocate them. They will be deallocated + // when the tree is destroyed. + //------------------------------------------------------------------ + if (strcmp(str, "tree") == 0) { // tree section + in >> the_dim; // read dimension + in >> the_n_pts; // number of points + in >> the_bkt_size; // bucket size + the_bnd_box_lo = annAllocPt(the_dim); // allocate bounding box pts + the_bnd_box_hi = annAllocPt(the_dim); + + for (j = 0; j < the_dim; j++) { // read bounding box low + in >> the_bnd_box_lo[j]; + } + for (j = 0; j < the_dim; j++) { // read bounding box low + in >> the_bnd_box_hi[j]; + } + the_pidx = new ANNidx[the_n_pts]; // allocate point index array + int next_idx = 0; // number of indices filled + // read the tree and indices + the_root = annReadTree(in, tree_type, the_pidx, next_idx); + if (next_idx != the_n_pts) { // didn't see all the points? + annError("Didn't see as many points as expected", ANNwarn); + } + } + else { + annError("Illegal dump format. Expecting section heading", ANNabort); + } + return the_root; + } + + //---------------------------------------------------------------------- + // annReadTree - input tree and return pointer + // + // annReadTree reads in a node of the tree, makes any recursive + // calls as needed to input the children of this node (if internal). + // It returns a pointer to the node that was created. An array + // of point indices is given along with a pointer to the next + // available location in the array. As leaves are read, their + // point indices are stored here, and the point buckets point + // to the first entry in the array. + // + // Recall that these are the formats. The tree is given in + // preorder. + // + // Leaf node: + // leaf <n_pts> <bkt[0]> <bkt[1]> ... <bkt[n-1]> + // Splitting nodes: + // split <cut_dim> <cut_val> <lo_bound> <hi_bound> + // + // For bd-trees: + // + // Shrinking nodes: + // shrink <n_bnds> + // <cut_dim> <cut_val> <side> + // <cut_dim> <cut_val> <side> + // ... (repeated n_bnds times) + //---------------------------------------------------------------------- + + static ANNkd_ptr annReadTree( + istream &in, // input stream + ANNtreeType tree_type, // type of tree expected + ANNidxArray the_pidx, // point indices (modified) + int &next_idx) // next index (modified) + { + char tag[STRING_LEN]; // tag (leaf, split, shrink) + int n_pts; // number of points in leaf + int cd; // cut dimension + ANNcoord cv; // cut value + ANNcoord lb; // low bound + ANNcoord hb; // high bound + int n_bnds; // number of bounding sides + int sd; // which side + + in >> tag; // input node tag + + if (strcmp(tag, "null") == 0) { // null tree + return NULL; + } + //------------------------------------------------------------------ + // Read a leaf + //------------------------------------------------------------------ + if (strcmp(tag, "leaf") == 0) { // leaf node + + in >> n_pts; // input number of points + int old_idx = next_idx; // save next_idx + if (n_pts == 0) { // trivial leaf + return KD_TRIVIAL; + } + else { + for (int i = 0; i < n_pts; i++) { // input point indices + in >> the_pidx[next_idx++]; // store in array of indices + } + } + return new ANNkd_leaf(n_pts, &the_pidx[old_idx]); + } + //------------------------------------------------------------------ + // Read a splitting node + //------------------------------------------------------------------ + else if (strcmp(tag, "split") == 0) { // splitting node + + in >> cd >> cv >> lb >> hb; + + // read low and high subtrees + ANNkd_ptr lc = annReadTree(in, tree_type, the_pidx, next_idx); + ANNkd_ptr hc = annReadTree(in, tree_type, the_pidx, next_idx); + // create new node and return + return new ANNkd_split(cd, cv, lb, hb, lc, hc); + } + //------------------------------------------------------------------ + // Read a shrinking node (bd-tree only) + //------------------------------------------------------------------ + else if (strcmp(tag, "shrink") == 0) { // shrinking node + if (tree_type != BD_TREE) { + annError("Shrinking node not allowed in kd-tree", ANNabort); + } + + in >> n_bnds; // number of bounding sides + // allocate bounds array + ANNorthHSArray bds = new ANNorthHalfSpace[n_bnds]; + for (int i = 0; i < n_bnds; i++) { + in >> cd >> cv >> sd; // input bounding halfspace + // copy to array + bds[i] = ANNorthHalfSpace(cd, cv, sd); + } + // read inner and outer subtrees + ANNkd_ptr ic = annReadTree(in, tree_type, the_pidx, next_idx); + ANNkd_ptr oc = annReadTree(in, tree_type, the_pidx, next_idx); + // create new node and return + return new ANNbd_shrink(n_bnds, bds, ic, oc); + } + else { + annError("Illegal node type in dump file", ANNabort); + exit(0); // to keep the compiler happy + } + } +} 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 +} +} diff --git a/geom_bottleneck/bottleneck/src/ann/kd_pr_search.cpp b/geom_bottleneck/bottleneck/src/ann/kd_pr_search.cpp new file mode 100644 index 0000000..73d643f --- /dev/null +++ b/geom_bottleneck/bottleneck/src/ann/kd_pr_search.cpp @@ -0,0 +1,221 @@ +//---------------------------------------------------------------------- +// File: kd_pr_search.cpp +// Programmer: Sunil Arya and David Mount +// Description: Priority search for kd-trees +// Last modified: 01/04/05 (Version 1.0) +//---------------------------------------------------------------------- +// 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 0.1 03/04/98 +// Initial release +//---------------------------------------------------------------------- + +#include "kd_pr_search.h" // kd priority search declarations + +namespace geom_bt { +//---------------------------------------------------------------------- +// Approximate nearest neighbor searching by priority search. +// The kd-tree is searched for an approximate nearest neighbor. +// The point is returned through one of the arguments, and the +// distance returned is the SQUARED distance to this point. +// +// The method used for searching the kd-tree is called priority +// search. (It is described in Arya and Mount, ``Algorithms for +// fast vector quantization,'' Proc. of DCC '93: Data Compression +// Conference}, eds. J. A. Storer and M. Cohn, IEEE Press, 1993, +// 381--390.) +// +// The cell of the kd-tree containing the query point is located, +// and cells are visited in increasing order of distance from the +// query point. This is done by placing each subtree which has +// NOT been visited in a priority queue, according to the closest +// distance of the corresponding enclosing rectangle from the +// query point. The search stops when the distance to the nearest +// remaining rectangle exceeds the distance to the nearest point +// seen by a factor of more than 1/(1+eps). (Implying that any +// point found subsequently in the search cannot be closer by more +// than this factor.) +// +// The main entry point is annkPriSearch() which sets things up and +// then call the recursive routine ann_pri_search(). This is a +// recursive routine which performs the processing for one node in +// the kd-tree. There are two versions of this virtual procedure, +// one for splitting nodes and one for leaves. When a splitting node +// is visited, we determine which child to continue the search on +// (the closer one), and insert the other child into the priority +// queue. When a leaf is visited, we compute the distances to the +// points in the buckets, and update information on the closest +// points. +// +// Some trickery is used to incrementally update the distance from +// a kd-tree rectangle to the query point. This comes about from +// the fact that which each successive split, only one component +// (along the dimension that is split) of the squared distance to +// the child rectangle is different from the squared distance to +// the parent rectangle. +//---------------------------------------------------------------------- + +//---------------------------------------------------------------------- +// To keep argument lists short, a number of global variables +// are maintained which are common to all the recursive calls. +// These are given below. +//---------------------------------------------------------------------- + +double ANNprEps; // the error bound +int ANNprDim; // dimension of space +ANNpoint ANNprQ; // query point +double ANNprMaxErr; // max tolerable squared error +ANNpointArray ANNprPts; // the points +ANNpr_queue *ANNprBoxPQ; // priority queue for boxes +ANNmin_k *ANNprPointMK; // set of k closest points + +//---------------------------------------------------------------------- +// annkPriSearch - priority search for k nearest neighbors +//---------------------------------------------------------------------- + +void ANNkd_tree::annkPriSearch( + ANNpoint q, // query point + int k, // number of near neighbors to return + ANNidxArray nn_idx, // nearest neighbor indices (returned) + ANNdistArray dd, // dist to near neighbors (returned) + double eps) // error bound (ignored) +{ + // max tolerable squared error + ANNprMaxErr = ANN_POW(1.0 + eps); + ANN_FLOP(2) // increment floating ops + + ANNprDim = dim; // copy arguments to static equivs + ANNprQ = q; + ANNprPts = pts; + ANNptsVisited = 0; // initialize count of points visited + + ANNprPointMK = new ANNmin_k(k); // create set for closest k points + + // distance to root box + ANNdist box_dist = annBoxDistance(q, + bnd_box_lo, bnd_box_hi, dim); + + ANNprBoxPQ = new ANNpr_queue(n_pts);// create priority queue for boxes + ANNprBoxPQ->insert(box_dist, root); // insert root in priority queue + + while (ANNprBoxPQ->non_empty() && + (!(ANNmaxPtsVisited != 0 && ANNptsVisited > ANNmaxPtsVisited))) { + ANNkd_ptr np; // next box from prior queue + + // extract closest box from queue + ANNprBoxPQ->extr_min(box_dist, (void *&) np); + + ANN_FLOP(2) // increment floating ops + if (box_dist*ANNprMaxErr >= ANNprPointMK->max_key()) + break; + + np->ann_pri_search(box_dist); // search this subtree. + } + + for (int i = 0; i < k; i++) { // extract the k-th closest points + dd[i] = ANNprPointMK->ith_smallest_key(i); + nn_idx[i] = ANNprPointMK->ith_smallest_info(i); + } + + delete ANNprPointMK; // deallocate closest point set + delete ANNprBoxPQ; // deallocate priority queue +} + +//---------------------------------------------------------------------- +// kd_split::ann_pri_search - search a splitting node +//---------------------------------------------------------------------- + +void ANNkd_split::ann_pri_search(ANNdist box_dist) +{ + ANNdist new_dist; // distance to child visited later + // distance to cutting plane + ANNcoord cut_diff = ANNprQ[cut_dim] - cut_val; + + if (cut_diff < 0) { // left of cutting plane + ANNcoord box_diff = cd_bnds[ANN_LO] - ANNprQ[cut_dim]; + if (box_diff < 0) // within bounds - ignore + box_diff = 0; + // distance to further box + new_dist = (ANNdist) ANN_SUM(box_dist, + ANN_DIFF(ANN_POW(box_diff), ANN_POW(cut_diff))); + + if (child[ANN_HI] != KD_TRIVIAL)// enqueue if not trivial + ANNprBoxPQ->insert(new_dist, child[ANN_HI]); + // continue with closer child + child[ANN_LO]->ann_pri_search(box_dist); + } + else { // right of cutting plane + ANNcoord box_diff = ANNprQ[cut_dim] - cd_bnds[ANN_HI]; + if (box_diff < 0) // within bounds - ignore + box_diff = 0; + // distance to further box + new_dist = (ANNdist) ANN_SUM(box_dist, + ANN_DIFF(ANN_POW(box_diff), ANN_POW(cut_diff))); + + if (child[ANN_LO] != KD_TRIVIAL)// enqueue if not trivial + ANNprBoxPQ->insert(new_dist, child[ANN_LO]); + // continue with closer child + child[ANN_HI]->ann_pri_search(box_dist); + } + ANN_SPL(1) // one more splitting node visited + ANN_FLOP(8) // increment floating ops +} + +//---------------------------------------------------------------------- +// kd_leaf::ann_pri_search - search points in a leaf node +// +// This is virtually identical to the ann_search for standard search. +//---------------------------------------------------------------------- + +void ANNkd_leaf::ann_pri_search(ANNdist box_dist) +{ + register ANNdist dist; // distance to data point + register ANNcoord* pp; // data coordinate pointer + register ANNcoord* qq; // query coordinate pointer + register ANNdist min_dist; // distance to k-th closest point + register ANNcoord t; + register int d; + + min_dist = ANNprPointMK->max_key(); // k-th smallest distance so far + + for (int i = 0; i < n_pts; i++) { // check points in bucket + + pp = ANNprPts[bkt[i]]; // first coord of next data point + qq = ANNprQ; // first coord of query point + dist = 0; + + for(d = 0; d < ANNprDim; d++) { + ANN_COORD(1) // one more coordinate hit + ANN_FLOP(4) // 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))) > min_dist) { + break; + } + } + + if (d >= ANNprDim && // among the k best? + (ANN_ALLOW_SELF_MATCH || dist!=0)) { // and no self-match problem + // add it to the list + ANNprPointMK->insert(dist, bkt[i]); + min_dist = ANNprPointMK->max_key(); + } + } + ANN_LEAF(1) // one more leaf node visited + ANN_PTS(n_pts) // increment points visited + ANNptsVisited += n_pts; // increment number of points visited +} +} diff --git a/geom_bottleneck/bottleneck/src/ann/kd_search.cpp b/geom_bottleneck/bottleneck/src/ann/kd_search.cpp new file mode 100644 index 0000000..f559eb9 --- /dev/null +++ b/geom_bottleneck/bottleneck/src/ann/kd_search.cpp @@ -0,0 +1,298 @@ +//---------------------------------------------------------------------- +// File: kd_search.cpp +// Programmer: Sunil Arya and David Mount +// Description: Standard kd-tree search +// Last modified: 01/04/05 (Version 1.0) +//---------------------------------------------------------------------- +// 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 0.1 03/04/98 +// Initial release +// Revision 1.0 04/01/05 +// Changed names LO, HI to ANN_LO, ANN_HI +// -------------------------------------------------------------------- +// 2015 - modified by A. Nigmetov to support deletion of points +//---------------------------------------------------------------------- + +#include "kd_search.h" // kd-search declarations + +namespace geom_bt { +//---------------------------------------------------------------------- +// Approximate nearest neighbor searching by kd-tree search +// The kd-tree is searched for an approximate nearest neighbor. +// The point is returned through one of the arguments, and the +// distance returned is the squared distance to this point. +// +// The method used for searching the kd-tree is an approximate +// adaptation of the search algorithm described by Friedman, +// Bentley, and Finkel, ``An algorithm for finding best matches +// in logarithmic expected time,'' ACM Transactions on Mathematical +// Software, 3(3):209-226, 1977). +// +// The algorithm operates recursively. When first encountering a +// node of the kd-tree we first visit the child which is closest to +// the query point. On return, we decide whether we want to visit +// the other child. If the box containing the other child exceeds +// 1/(1+eps) times the current best distance, then we skip it (since +// any point found in this child cannot be closer to the query point +// by more than this factor.) Otherwise, we visit it recursively. +// The distance between a box and the query point is computed exactly +// (not approximated as is often done in kd-tree), using incremental +// distance updates, as described by Arya and Mount in ``Algorithms +// for fast vector quantization,'' Proc. of DCC '93: Data Compression +// Conference, eds. J. A. Storer and M. Cohn, IEEE Press, 1993, +// 381-390. +// +// The main entry points is annkSearch() which sets things up and +// then call the recursive routine ann_search(). This is a recursive +// routine which performs the processing for one node in the kd-tree. +// There are two versions of this virtual procedure, one for splitting +// nodes and one for leaves. When a splitting node is visited, we +// determine which child to visit first (the closer one), and visit +// the other child on return. When a leaf is visited, we compute +// the distances to the points in the buckets, and update information +// on the closest points. +// +// Some trickery is used to incrementally update the distance from +// a kd-tree rectangle to the query point. This comes about from +// the fact that which each successive split, only one component +// (along the dimension that is split) of the squared distance to +// the child rectangle is different from the squared distance to +// the parent rectangle. +//---------------------------------------------------------------------- + +//---------------------------------------------------------------------- +// 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 ANNkdDim; // dimension of space +ANNpoint ANNkdQ; // query point +double ANNkdMaxErr; // max tolerable squared error +ANNpointArray ANNkdPts; // the points +ANNmin_k *ANNkdPointMK; // set of k closest points + +//---------------------------------------------------------------------- +// annkSearch - search for the k nearest neighbors +//---------------------------------------------------------------------- + +void ANNkd_tree::annkSearch( + ANNpoint q, // the query point + 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 +{ + + ANNkdDim = dim; // copy arguments to static equivs + ANNkdQ = q; + ANNkdPts = pts; + ANNptsVisited = 0; // initialize count of points visited + + if (k > actual_num_points) { // too many near neighbors? + annError("Requesting more near neighbors than data points", ANNabort); + } + + ANNkdMaxErr = ANN_POW(1.0 + eps); + ANN_FLOP(2) // increment floating op count + + ANNkdPointMK = new ANNmin_k(k); // create set for closest k points + // search starting at the root + root->ann_search(annBoxDistance(q, bnd_box_lo, bnd_box_hi, dim)); + + for (int i = 0; i < k; i++) { // extract the k-th closest points + dd[i] = ANNkdPointMK->ith_smallest_key(i); + nn_idx[i] = ANNkdPointMK->ith_smallest_info(i); + } + delete ANNkdPointMK; // deallocate closest point set +} + +//---------------------------------------------------------------------- +// kd_split::ann_search - search a splitting node +//---------------------------------------------------------------------- + +void ANNkd_split::ann_search(ANNdist box_dist) +{ + // check if the subtree is empty + if (0 == actual_num_points) return; + // check dist calc term condition + if (ANNmaxPtsVisited != 0 && ANNptsVisited > ANNmaxPtsVisited) return; + + // distance to cutting plane + ANNcoord cut_diff = ANNkdQ[cut_dim] - cut_val; + + if (cut_diff < 0) { // left of cutting plane + child[ANN_LO]->ann_search(box_dist);// visit closer child first + + ANNcoord box_diff = cd_bnds[ANN_LO] - ANNkdQ[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 close enough + if (box_dist * ANNkdMaxErr < ANNkdPointMK->max_key()) + child[ANN_HI]->ann_search(box_dist); + + } + else { // right of cutting plane + child[ANN_HI]->ann_search(box_dist);// visit closer child first + + ANNcoord box_diff = ANNkdQ[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 * ANNkdMaxErr < ANNkdPointMK->max_key()) + child[ANN_LO]->ann_search(box_dist); + + } + ANN_FLOP(10) // increment floating ops + ANN_SPL(1) // one more splitting node visited +} + +//---------------------------------------------------------------------- +// kd_leaf::ann_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_search(ANNdist box_dist) +{ + register ANNdist dist; // distance to data point + register ANNcoord* pp; // data coordinate pointer + register ANNcoord* qq; // query coordinate pointer + register ANNdist min_dist; // distance to k-th closest point + register ANNcoord t; + register int d; + + min_dist = ANNkdPointMK->max_key(); // k-th smallest distance so far + + for (int i = 0; i < n_pts; i++) { // check points in bucket + + pp = ANNkdPts[bkt[i]]; // first coord of next data point + qq = ANNkdQ; // first coord of query point + dist = 0; + + for(d = 0; d < ANNkdDim; d++) { + ANN_COORD(1) // one more coordinate hit + ANN_FLOP(4) // 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))) > min_dist) { + break; + } + } + + if (d >= ANNkdDim && // among the k best? + (ANN_ALLOW_SELF_MATCH || dist!=0)) { // and no self-match problem + // add it to the list + ANNkdPointMK->insert(dist, bkt[i]); + min_dist = ANNkdPointMK->max_key(); + } + } + ANN_LEAF(1) // one more leaf node visited + ANN_PTS(n_pts) // increment points visited + ANNptsVisited += n_pts; // increment number of points visited +} + + + +//////////////////////////////////////////////// +// range search +// //////////////////////////////////////////// + +void ANNkd_tree::range_search(const ANNorthRect& region, + std::vector<size_t>& point_indices) +{ + + // get bounding box of the root + ANNorthRect bnd_box = ANNorthRect(dim, bnd_box_lo, bnd_box_hi); + root->range_search(region, dim, pts, bnd_box, point_indices); +} + +void ANNkd_split::range_search(const ANNorthRect& region, + int ANNkdDim, + ANNpointArray ANNkdPts, + ANNorthRect& bnd_box, + std::vector<size_t>& point_indices) +{ + // check if the subtree is empty + if (0 == actual_num_points) return; + + // process left child + ANNcoord old_bnd_box_val = bnd_box.hi[cut_dim]; + bnd_box.hi[cut_dim] = cut_val; + if (region.contains(ANNkdDim, bnd_box)) { + child[ANN_LO]->range_search_add(point_indices); + } else if (region.intersects(ANNkdDim, bnd_box)) { + child[ANN_LO]->range_search(region, ANNkdDim, ANNkdPts, bnd_box, point_indices); + } + // restore bounding box + bnd_box.hi[cut_dim] = old_bnd_box_val; + // process right child + old_bnd_box_val = bnd_box.lo[cut_dim]; + bnd_box.lo[cut_dim] = cut_val; + if (region.contains(ANNkdDim, bnd_box)) { + child[ANN_HI]->range_search_add(point_indices); + } else if (region.intersects(ANNkdDim, bnd_box)) { + child[ANN_HI]->range_search(region, ANNkdDim, ANNkdPts, bnd_box, point_indices); + } + // restore bounding box + bnd_box.lo[cut_dim] = old_bnd_box_val; +} + +void ANNkd_leaf::range_search(const ANNorthRect& region, + int ANNkdDim, + ANNpointArray ANNkdPts, + ANNorthRect&, // nameless parameter to suppress + // warnings and allow recursion + // in splitting node + std::vector<size_t>& point_indices) +{ + for (int i = 0; i < n_pts; i++) { // check points in bucket + if (region.inside(ANNkdDim, ANNkdPts[bkt[i]]) == ANNtrue) { + //std::cout << "adding point, i = " << i; + //std::cout << ", x = " << ANNkdPts[bkt[i]][0]; + //std::cout << ", y = " << ANNkdPts[bkt[i]][1] << std::endl; + point_indices.push_back(bkt[i]); + } + } +} + +void ANNkd_split::range_search_add(std::vector<size_t>& point_indices) +{ + if ( 0 == actual_num_points ) + return; + child[ANN_LO]->range_search_add(point_indices); + child[ANN_HI]->range_search_add(point_indices); +} + +void ANNkd_leaf::range_search_add(std::vector<size_t>& point_indices) +{ + if ( 0 == actual_num_points ) + return; + for (int i = 0; i < n_pts; i++) { // add all points in a bucket + //std::cout << "adding point without checking, i = " << i <<", bkt[i] = " << bkt[i] << std::endl; + point_indices.push_back(bkt[i]); + } +} +} diff --git a/geom_bottleneck/bottleneck/src/ann/kd_split.cpp b/geom_bottleneck/bottleneck/src/ann/kd_split.cpp new file mode 100644 index 0000000..7979aaa --- /dev/null +++ b/geom_bottleneck/bottleneck/src/ann/kd_split.cpp @@ -0,0 +1,632 @@ +//---------------------------------------------------------------------- +// File: kd_split.cpp +// Programmer: Sunil Arya and David Mount +// Description: Methods for splitting kd-trees +// Last modified: 01/04/05 (Version 1.0) +//---------------------------------------------------------------------- +// 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 0.1 03/04/98 +// Initial release +// Revision 1.0 04/01/05 +//---------------------------------------------------------------------- + +#include "kd_tree.h" // kd-tree definitions +#include "kd_util.h" // kd-tree utilities +#include "kd_split.h" // splitting functions + +namespace geom_bt { +//---------------------------------------------------------------------- +// Constants +//---------------------------------------------------------------------- + +const double ERR = 0.001; // a small value +const double FS_ASPECT_RATIO = 3.0; // maximum allowed aspect ratio + // in fair split. Must be >= 2. + +//---------------------------------------------------------------------- +// NOTE: Virtually all point indexing is done through an index (i.e. +// permutation) array pidx. Consequently, a reference to the d-th +// coordinate of the i-th point is pa[pidx[i]][d]. The macro PA(i,d) +// is a shorthand for this. +//---------------------------------------------------------------------- + // standard 2-d indirect indexing +#define PA(i,d) (pa[pidx[(i)]][(d)]) + // accessing a single point +#define PP(i) (pa[pidx[(i)]]) + + +//---------------------------------------------------------------------- +// kd_split - Bentley's standard splitting routine for kd-trees +// Find the dimension of the greatest spread, and split +// just before the median point along this dimension. +//---------------------------------------------------------------------- + +void kd_split( + ANNpointArray pa, // point array (permuted on return) + ANNidxArray pidx, // point indices + const ANNorthRect &bnds, // bounding rectangle for cell + int n, // number of points + int dim, // dimension of space + int &cut_dim, // cutting dimension (returned) + ANNcoord &cut_val, // cutting value (returned) + int &n_lo) // num of points on low side (returned) +{ + // find dimension of maximum spread + cut_dim = annMaxSpread(pa, pidx, n, dim); + n_lo = n/2; // median rank + // split about median + annMedianSplit(pa, pidx, n, cut_dim, cut_val, n_lo); +} + +//---------------------------------------------------------------------- +// midpt_split - midpoint splitting rule for box-decomposition trees +// +// This is the simplest splitting rule that guarantees boxes +// of bounded aspect ratio. It simply cuts the box with the +// longest side through its midpoint. If there are ties, it +// selects the dimension with the maximum point spread. +// +// WARNING: This routine (while simple) doesn't seem to work +// well in practice in high dimensions, because it tends to +// generate a large number of trivial and/or unbalanced splits. +// Either kd_split(), sl_midpt_split(), or fair_split() are +// recommended, instead. +//---------------------------------------------------------------------- + +void midpt_split( + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices (permuted on return) + const ANNorthRect &bnds, // bounding rectangle for cell + int n, // number of points + int dim, // dimension of space + int &cut_dim, // cutting dimension (returned) + ANNcoord &cut_val, // cutting value (returned) + int &n_lo) // num of points on low side (returned) +{ + int d; + + ANNcoord max_length = bnds.hi[0] - bnds.lo[0]; + for (d = 1; d < dim; d++) { // find length of longest box side + ANNcoord length = bnds.hi[d] - bnds.lo[d]; + if (length > max_length) { + max_length = length; + } + } + ANNcoord max_spread = -1; // find long side with most spread + for (d = 0; d < dim; d++) { + // is it among longest? + if (double(bnds.hi[d] - bnds.lo[d]) >= (1-ERR)*max_length) { + // compute its spread + ANNcoord spr = annSpread(pa, pidx, n, d); + if (spr > max_spread) { // is it max so far? + max_spread = spr; + cut_dim = d; + } + } + } + // split along cut_dim at midpoint + cut_val = (bnds.lo[cut_dim] + bnds.hi[cut_dim]) / 2; + // permute points accordingly + int br1, br2; + annPlaneSplit(pa, pidx, n, cut_dim, cut_val, br1, br2); + //------------------------------------------------------------------ + // On return: pa[0..br1-1] < cut_val + // pa[br1..br2-1] == cut_val + // pa[br2..n-1] > cut_val + // + // We can set n_lo to any value in the range [br1..br2]. + // We choose split so that points are most evenly divided. + //------------------------------------------------------------------ + if (br1 > n/2) n_lo = br1; + else if (br2 < n/2) n_lo = br2; + else n_lo = n/2; +} + +//---------------------------------------------------------------------- +// sl_midpt_split - sliding midpoint splitting rule +// +// This is a modification of midpt_split, which has the nonsensical +// name "sliding midpoint". The idea is that we try to use the +// midpoint rule, by bisecting the longest side. If there are +// ties, the dimension with the maximum spread is selected. If, +// however, the midpoint split produces a trivial split (no points +// on one side of the splitting plane) then we slide the splitting +// (maintaining its orientation) until it produces a nontrivial +// split. For example, if the splitting plane is along the x-axis, +// and all the data points have x-coordinate less than the x-bisector, +// then the split is taken along the maximum x-coordinate of the +// data points. +// +// Intuitively, this rule cannot generate trivial splits, and +// hence avoids midpt_split's tendency to produce trees with +// a very large number of nodes. +// +//---------------------------------------------------------------------- + +void sl_midpt_split( + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices (permuted on return) + const ANNorthRect &bnds, // bounding rectangle for cell + int n, // number of points + int dim, // dimension of space + int &cut_dim, // cutting dimension (returned) + ANNcoord &cut_val, // cutting value (returned) + int &n_lo) // num of points on low side (returned) +{ + int d; + + ANNcoord max_length = bnds.hi[0] - bnds.lo[0]; + for (d = 1; d < dim; d++) { // find length of longest box side + ANNcoord length = bnds.hi[d] - bnds.lo[d]; + if (length > max_length) { + max_length = length; + } + } + ANNcoord max_spread = -1; // find long side with most spread + for (d = 0; d < dim; d++) { + // is it among longest? + if ((bnds.hi[d] - bnds.lo[d]) >= (1-ERR)*max_length) { + // compute its spread + ANNcoord spr = annSpread(pa, pidx, n, d); + if (spr > max_spread) { // is it max so far? + max_spread = spr; + cut_dim = d; + } + } + } + // ideal split at midpoint + ANNcoord ideal_cut_val = (bnds.lo[cut_dim] + bnds.hi[cut_dim])/2; + + ANNcoord min, max; + annMinMax(pa, pidx, n, cut_dim, min, max); // find min/max coordinates + + if (ideal_cut_val < min) // slide to min or max as needed + cut_val = min; + else if (ideal_cut_val > max) + cut_val = max; + else + cut_val = ideal_cut_val; + + // permute points accordingly + int br1, br2; + annPlaneSplit(pa, pidx, n, cut_dim, cut_val, br1, br2); + //------------------------------------------------------------------ + // On return: pa[0..br1-1] < cut_val + // pa[br1..br2-1] == cut_val + // pa[br2..n-1] > cut_val + // + // We can set n_lo to any value in the range [br1..br2] to satisfy + // the exit conditions of the procedure. + // + // if ideal_cut_val < min (implying br2 >= 1), + // then we select n_lo = 1 (so there is one point on left) and + // if ideal_cut_val > max (implying br1 <= n-1), + // then we select n_lo = n-1 (so there is one point on right). + // Otherwise, we select n_lo as close to n/2 as possible within + // [br1..br2]. + //------------------------------------------------------------------ + if (ideal_cut_val < min) n_lo = 1; + else if (ideal_cut_val > max) n_lo = n-1; + else if (br1 > n/2) n_lo = br1; + else if (br2 < n/2) n_lo = br2; + else n_lo = n/2; +} + +//---------------------------------------------------------------------- +// fair_split - fair-split splitting rule +// +// This is a compromise between the kd-tree splitting rule (which +// always splits data points at their median) and the midpoint +// splitting rule (which always splits a box through its center. +// The goal of this procedure is to achieve both nicely balanced +// splits, and boxes of bounded aspect ratio. +// +// A constant FS_ASPECT_RATIO is defined. Given a box, those sides +// which can be split so that the ratio of the longest to shortest +// side does not exceed ASPECT_RATIO are identified. Among these +// sides, we select the one in which the points have the largest +// spread. We then split the points in a manner which most evenly +// distributes the points on either side of the splitting plane, +// subject to maintaining the bound on the ratio of long to short +// sides. To determine that the aspect ratio will be preserved, +// we determine the longest side (other than this side), and +// determine how narrowly we can cut this side, without causing the +// aspect ratio bound to be exceeded (small_piece). +// +// This procedure is more robust than either kd_split or midpt_split, +// but is more complicated as well. When point distribution is +// extremely skewed, this degenerates to midpt_split (actually +// 1/3 point split), and when the points are most evenly distributed, +// this degenerates to kd-split. +//---------------------------------------------------------------------- + +void fair_split( + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices (permuted on return) + const ANNorthRect &bnds, // bounding rectangle for cell + int n, // number of points + int dim, // dimension of space + int &cut_dim, // cutting dimension (returned) + ANNcoord &cut_val, // cutting value (returned) + int &n_lo) // num of points on low side (returned) +{ + int d; + ANNcoord max_length = bnds.hi[0] - bnds.lo[0]; + cut_dim = 0; + for (d = 1; d < dim; d++) { // find length of longest box side + ANNcoord length = bnds.hi[d] - bnds.lo[d]; + if (length > max_length) { + max_length = length; + cut_dim = d; + } + } + + ANNcoord max_spread = 0; // find legal cut with max spread + cut_dim = 0; + for (d = 0; d < dim; d++) { + ANNcoord length = bnds.hi[d] - bnds.lo[d]; + // is this side midpoint splitable + // without violating aspect ratio? + if (((double) max_length)*2.0/((double) length) <= FS_ASPECT_RATIO) { + // compute spread along this dim + ANNcoord spr = annSpread(pa, pidx, n, d); + if (spr > max_spread) { // best spread so far + max_spread = spr; + cut_dim = d; // this is dimension to cut + } + } + } + + max_length = 0; // find longest side other than cut_dim + for (d = 0; d < dim; d++) { + ANNcoord length = bnds.hi[d] - bnds.lo[d]; + if (d != cut_dim && length > max_length) + max_length = length; + } + // consider most extreme splits + ANNcoord small_piece = max_length / FS_ASPECT_RATIO; + ANNcoord lo_cut = bnds.lo[cut_dim] + small_piece;// lowest legal cut + ANNcoord hi_cut = bnds.hi[cut_dim] - small_piece;// highest legal cut + + int br1, br2; + // is median below lo_cut ? + if (annSplitBalance(pa, pidx, n, cut_dim, lo_cut) >= 0) { + cut_val = lo_cut; // cut at lo_cut + annPlaneSplit(pa, pidx, n, cut_dim, cut_val, br1, br2); + n_lo = br1; + } + // is median above hi_cut? + else if (annSplitBalance(pa, pidx, n, cut_dim, hi_cut) <= 0) { + cut_val = hi_cut; // cut at hi_cut + annPlaneSplit(pa, pidx, n, cut_dim, cut_val, br1, br2); + n_lo = br2; + } + else { // median cut preserves asp ratio + n_lo = n/2; // split about median + annMedianSplit(pa, pidx, n, cut_dim, cut_val, n_lo); + } +} + +//---------------------------------------------------------------------- +// sl_fair_split - sliding fair split splitting rule +// +// Sliding fair split is a splitting rule that combines the +// strengths of both fair split with sliding midpoint split. +// Fair split tends to produce balanced splits when the points +// are roughly uniformly distributed, but it can produce many +// trivial splits when points are highly clustered. Sliding +// midpoint never produces trivial splits, and shrinks boxes +// nicely if points are highly clustered, but it may produce +// rather unbalanced splits when points are unclustered but not +// quite uniform. +// +// Sliding fair split is based on the theory that there are two +// types of splits that are "good": balanced splits that produce +// fat boxes, and unbalanced splits provided the cell with fewer +// points is fat. +// +// This splitting rule operates by first computing the longest +// side of the current bounding box. Then it asks which sides +// could be split (at the midpoint) and still satisfy the aspect +// ratio bound with respect to this side. Among these, it selects +// the side with the largest spread (as fair split would). It +// then considers the most extreme cuts that would be allowed by +// the aspect ratio bound. This is done by dividing the longest +// side of the box by the aspect ratio bound. If the median cut +// lies between these extreme cuts, then we use the median cut. +// If not, then consider the extreme cut that is closer to the +// median. If all the points lie to one side of this cut, then +// we slide the cut until it hits the first point. This may +// violate the aspect ratio bound, but will never generate empty +// cells. However the sibling of every such skinny cell is fat, +// and hence packing arguments still apply. +// +//---------------------------------------------------------------------- + +void sl_fair_split( + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices (permuted on return) + const ANNorthRect &bnds, // bounding rectangle for cell + int n, // number of points + int dim, // dimension of space + int &cut_dim, // cutting dimension (returned) + ANNcoord &cut_val, // cutting value (returned) + int &n_lo) // num of points on low side (returned) +{ + int d; + ANNcoord min, max; // min/max coordinates + int br1, br2; // split break points + + ANNcoord max_length = bnds.hi[0] - bnds.lo[0]; + cut_dim = 0; + for (d = 1; d < dim; d++) { // find length of longest box side + ANNcoord length = bnds.hi[d] - bnds.lo[d]; + if (length > max_length) { + max_length = length; + cut_dim = d; + } + } + + ANNcoord max_spread = 0; // find legal cut with max spread + cut_dim = 0; + for (d = 0; d < dim; d++) { + ANNcoord length = bnds.hi[d] - bnds.lo[d]; + // is this side midpoint splitable + // without violating aspect ratio? + if (((double) max_length)*2.0/((double) length) <= FS_ASPECT_RATIO) { + // compute spread along this dim + ANNcoord spr = annSpread(pa, pidx, n, d); + if (spr > max_spread) { // best spread so far + max_spread = spr; + cut_dim = d; // this is dimension to cut + } + } + } + + max_length = 0; // find longest side other than cut_dim + for (d = 0; d < dim; d++) { + ANNcoord length = bnds.hi[d] - bnds.lo[d]; + if (d != cut_dim && length > max_length) + max_length = length; + } + // consider most extreme splits + ANNcoord small_piece = max_length / FS_ASPECT_RATIO; + ANNcoord lo_cut = bnds.lo[cut_dim] + small_piece;// lowest legal cut + ANNcoord hi_cut = bnds.hi[cut_dim] - small_piece;// highest legal cut + // find min and max along cut_dim + annMinMax(pa, pidx, n, cut_dim, min, max); + // is median below lo_cut? + if (annSplitBalance(pa, pidx, n, cut_dim, lo_cut) >= 0) { + if (max > lo_cut) { // are any points above lo_cut? + cut_val = lo_cut; // cut at lo_cut + annPlaneSplit(pa, pidx, n, cut_dim, cut_val, br1, br2); + n_lo = br1; // balance if there are ties + } + else { // all points below lo_cut + cut_val = max; // cut at max value + annPlaneSplit(pa, pidx, n, cut_dim, cut_val, br1, br2); + n_lo = n-1; + } + } + // is median above hi_cut? + else if (annSplitBalance(pa, pidx, n, cut_dim, hi_cut) <= 0) { + if (min < hi_cut) { // are any points below hi_cut? + cut_val = hi_cut; // cut at hi_cut + annPlaneSplit(pa, pidx, n, cut_dim, cut_val, br1, br2); + n_lo = br2; // balance if there are ties + } + else { // all points above hi_cut + cut_val = min; // cut at min value + annPlaneSplit(pa, pidx, n, cut_dim, cut_val, br1, br2); + n_lo = 1; + } + } + else { // median cut is good enough + n_lo = n/2; // split about median + annMedianSplit(pa, pidx, n, cut_dim, cut_val, n_lo); + } +} + + +///////////////////////////////////////////////////////////////////////////////// +// for kd-trees with deletion +// +//---------------------------------------------------------------------- +// kd_split - Bentley's standard splitting routine for kd-trees +// Find the dimension of the greatest spread, and split +// just before the median point along this dimension. +//---------------------------------------------------------------------- + +void kd_split_wd( + ANNpointArray pa, // point array (permuted on return) + ANNidxArray pidx, // point indices + const ANNorthRect &bnds, // bounding rectangle for cell + int n, // number of points + int dim, // dimension of space + int &cut_dim, // cutting dimension (returned) + ANNcoord &cut_val, // cutting value (returned) + int &n_lo, // num of points on low side (returned) + int &cut_pt_idx) // index of cutting point (returned) +{ + // find dimension of maximum spread + cut_dim = annMaxSpread(pa, pidx, n, dim); + n_lo = n/2; // median rank + // split about median + annMedianSplit(pa, pidx, n, cut_dim, cut_val, n_lo); + cut_pt_idx = n_lo; + cut_val = PA(cut_pt_idx, cut_dim); +} + +//---------------------------------------------------------------------- +// midpt_split - midpoint splitting rule for box-decomposition trees +// +// This is the simplest splitting rule that guarantees boxes +// of bounded aspect ratio. It simply cuts the box with the +// longest side through its midpoint. If there are ties, it +// selects the dimension with the maximum point spread. +// +// WARNING: This routine (while simple) doesn't seem to work +// well in practice in high dimensions, because it tends to +// generate a large number of trivial and/or unbalanced splits. +// Either kd_split(), sl_midpt_split(), or fair_split() are +// recommended, instead. +//---------------------------------------------------------------------- + +void midpt_split_wd( + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices (permuted on return) + const ANNorthRect &bnds, // bounding rectangle for cell + int n, // number of points + int dim, // dimension of space + int &cut_dim, // cutting dimension (returned) + ANNcoord &cut_val, // cutting value (returned) + int &n_lo, // num of points on low side (returned) + int &cut_pt_idx) // index of cutting point (returned) +{ + int d; + + ANNcoord max_length = bnds.hi[0] - bnds.lo[0]; + for (d = 1; d < dim; d++) { // find length of longest box side + ANNcoord length = bnds.hi[d] - bnds.lo[d]; + if (length > max_length) { + max_length = length; + } + } + ANNcoord max_spread = -1; // find long side with most spread + for (d = 0; d < dim; d++) { + // is it among longest? + if (double(bnds.hi[d] - bnds.lo[d]) >= (1-ERR)*max_length) { + // compute its spread + ANNcoord spr = annSpread(pa, pidx, n, d); + if (spr > max_spread) { // is it max so far? + max_spread = spr; + cut_dim = d; + } + } + } + // split along cut_dim at midpoint + cut_val = (bnds.lo[cut_dim] + bnds.hi[cut_dim]) / 2; + // permute points accordingly + int br1, br2; + annPlaneSplit(pa, pidx, n, cut_dim, cut_val, br1, br2); + //------------------------------------------------------------------ + // On return: pa[0..br1-1] < cut_val + // pa[br1..br2-1] == cut_val + // pa[br2..n-1] > cut_val + // + // We can set n_lo to any value in the range [br1..br2]. + // We choose split so that points are most evenly divided. + //------------------------------------------------------------------ + if (br1 > n/2) n_lo = br1; + else if (br2 < n/2) n_lo = br2; + else n_lo = n/2; + + cut_pt_idx = n_lo; + cut_val = PA(cut_pt_idx, cut_dim); + +} + +//---------------------------------------------------------------------- +// sl_midpt_split - sliding midpoint splitting rule +// +// This is a modification of midpt_split, which has the nonsensical +// name "sliding midpoint". The idea is that we try to use the +// midpoint rule, by bisecting the longest side. If there are +// ties, the dimension with the maximum spread is selected. If, +// however, the midpoint split produces a trivial split (no points +// on one side of the splitting plane) then we slide the splitting +// (maintaining its orientation) until it produces a nontrivial +// split. For example, if the splitting plane is along the x-axis, +// and all the data points have x-coordinate less than the x-bisector, +// then the split is taken along the maximum x-coordinate of the +// data points. +// +// Intuitively, this rule cannot generate trivial splits, and +// hence avoids midpt_split's tendency to produce trees with +// a very large number of nodes. +// +//---------------------------------------------------------------------- + +void sl_midpt_split_wd( + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices (permuted on return) + const ANNorthRect &bnds, // bounding rectangle for cell + int n, // number of points + int dim, // dimension of space + int &cut_dim, // cutting dimension (returned) + ANNcoord &cut_val, // cutting value (returned) + int &n_lo, // num of points on low side (returned) + int &cut_pt_idx) // index of cutting point (returned) +{ + int d; + + ANNcoord max_length = bnds.hi[0] - bnds.lo[0]; + for (d = 1; d < dim; d++) { // find length of longest box side + ANNcoord length = bnds.hi[d] - bnds.lo[d]; + if (length > max_length) { + max_length = length; + } + } + ANNcoord max_spread = -1; // find long side with most spread + for (d = 0; d < dim; d++) { + // is it among longest? + if ((bnds.hi[d] - bnds.lo[d]) >= (1-ERR)*max_length) { + // compute its spread + ANNcoord spr = annSpread(pa, pidx, n, d); + if (spr > max_spread) { // is it max so far? + max_spread = spr; + cut_dim = d; + } + } + } + // ideal split at midpoint + ANNcoord ideal_cut_val = (bnds.lo[cut_dim] + bnds.hi[cut_dim])/2; + + ANNcoord min, max; + annMinMax(pa, pidx, n, cut_dim, min, max); // find min/max coordinates + + if (ideal_cut_val < min) // slide to min or max as needed + cut_val = min; + else if (ideal_cut_val > max) + cut_val = max; + else + cut_val = ideal_cut_val; + + // permute points accordingly + int br1, br2; + annPlaneSplit(pa, pidx, n, cut_dim, cut_val, br1, br2); + //------------------------------------------------------------------ + // On return: pa[0..br1-1] < cut_val + // pa[br1..br2-1] == cut_val + // pa[br2..n-1] > cut_val + // + // We can set n_lo to any value in the range [br1..br2] to satisfy + // the exit conditions of the procedure. + // + // if ideal_cut_val < min (implying br2 >= 1), + // then we select n_lo = 1 (so there is one point on left) and + // if ideal_cut_val > max (implying br1 <= n-1), + // then we select n_lo = n-1 (so there is one point on right). + // Otherwise, we select n_lo as close to n/2 as possible within + // [br1..br2]. + //------------------------------------------------------------------ + if (ideal_cut_val < min) n_lo = 1; + else if (ideal_cut_val > max) n_lo = n-1; + else if (br1 > n/2) n_lo = br1; + else if (br2 < n/2) n_lo = br2; + else n_lo = n/2; +} +} diff --git a/geom_bottleneck/bottleneck/src/ann/kd_tree.cpp b/geom_bottleneck/bottleneck/src/ann/kd_tree.cpp new file mode 100644 index 0000000..ad3a82d --- /dev/null +++ b/geom_bottleneck/bottleneck/src/ann/kd_tree.cpp @@ -0,0 +1,560 @@ +//---------------------------------------------------------------------- +// File: kd_tree.cpp +// Programmer: Sunil Arya and David Mount +// Description: Basic methods for kd-trees. +// Last modified: 01/04/05 (Version 1.0) +//---------------------------------------------------------------------- +// 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 0.1 03/04/98 +// Initial release +// Revision 1.0 04/01/05 +// Increased aspect ratio bound (ANN_AR_TOOBIG) from 100 to 1000. +// Fixed leaf counts to count trivial leaves. +// Added optional pa, pi arguments to Skeleton kd_tree constructor +// for use in load constructor. +// Added annClose() to eliminate KD_TRIVIAL memory leak. +// -------------------------------------------------------------------- +// 2015 - modified by A. Nigmetov to support deletion of points +//---------------------------------------------------------------------- + +#ifdef _WIN32 +#include <ciso646> // make VS more conformal +#endif + +#include "kd_tree.h" // kd-tree declarations +#include "kd_split.h" // kd-tree splitting rules +#include "kd_util.h" // kd-tree utilities +#include <ANN/ANNperf.h> // performance evaluation + +namespace geom_bt { +//---------------------------------------------------------------------- +// Global data +// +// For some splitting rules, especially with small bucket sizes, +// it is possible to generate a large number of empty leaf nodes. +// To save storage we allocate a single trivial leaf node which +// contains no points. For messy coding reasons it is convenient +// to have it reference a trivial point index. +// +// KD_TRIVIAL is allocated when the first kd-tree is created. It +// must *never* deallocated (since it may be shared by more than +// one tree). +//---------------------------------------------------------------------- +static int IDX_TRIVIAL[] = {0}; // trivial point index +ANNkd_leaf *KD_TRIVIAL = NULL; // trivial leaf node + +//---------------------------------------------------------------------- +// Printing the kd-tree +// These routines print a kd-tree in reverse inorder (high then +// root then low). (This is so that if you look at the output +// from the right side it appear from left to right in standard +// inorder.) When outputting leaves we output only the point +// indices rather than the point coordinates. There is an option +// to print the point coordinates separately. +// +// The tree printing routine calls the printing routines on the +// individual nodes of the tree, passing in the level or depth +// in the tree. The level in the tree is used to print indentation +// for readability. +//---------------------------------------------------------------------- + +void ANNkd_split::print( // print splitting node + int level, // depth of node in tree + ostream &out) // output stream +{ + child[ANN_HI]->print(level+1, out); // print high child + out << " "; + for (int i = 0; i < level; i++) // print indentation + out << ".."; + out << "Split cd=" << cut_dim << " cv=" << cut_val; + out << " lbnd=" << cd_bnds[ANN_LO]; + out << " hbnd=" << cd_bnds[ANN_HI]; + out << " np=" << actual_num_points; + out << "\n"; + child[ANN_LO]->print(level+1, out); // print low child +} + +void ANNkd_leaf::print( // print leaf node + int level, // depth of node in tree + ostream &out) // output stream +{ + + out << " "; + for (int i = 0; i < level; i++) // print indentation + out << ".."; + + if (this == KD_TRIVIAL) { // canonical trivial leaf node + out << "Leaf (trivial)\n"; + } + else{ + out << "Leaf n=" << n_pts << " <"; + for (int j = 0; j < n_pts; j++) { + out << bkt[j]; + if (j < n_pts-1) out << ","; + } + out << ">\n"; + } +} + +void ANNkd_tree::Print( // print entire tree + ANNbool with_pts, // print points as well? + ostream &out) // output stream +{ + out << "ANN Version " << ANNversion << "\n"; + if (with_pts) { // print point coordinates + out << " Points:\n"; + for (int i = 0; i < n_pts; i++) { + out << "\t" << i << ": "; + annPrintPt(pts[i], dim, out); + out << "\n"; + } + } + if (root == NULL) // empty tree? + out << " Null tree.\n"; + else { + root->print(0, out); // invoke printing at root + } +} + +//---------------------------------------------------------------------- +// kd_tree statistics (for performance evaluation) +// This routine compute various statistics information for +// a kd-tree. It is used by the implementors for performance +// evaluation of the data structure. +//---------------------------------------------------------------------- + +#define MAX(a,b) ((a) > (b) ? (a) : (b)) + +void ANNkdStats::merge(const ANNkdStats &st) // merge stats from child +{ + n_lf += st.n_lf; n_tl += st.n_tl; + n_spl += st.n_spl; n_shr += st.n_shr; + depth = MAX(depth, st.depth); + sum_ar += st.sum_ar; +} + +//---------------------------------------------------------------------- +// Update statistics for nodes +//---------------------------------------------------------------------- + +const double ANN_AR_TOOBIG = 1000; // too big an aspect ratio + +void ANNkd_leaf::getStats( // get subtree statistics + int dim, // dimension of space + ANNkdStats &st, // stats (modified) + ANNorthRect &bnd_box) // bounding box +{ + st.reset(); + st.n_lf = 1; // count this leaf + if (this == KD_TRIVIAL) st.n_tl = 1; // count trivial leaf + double ar = annAspectRatio(dim, bnd_box); // aspect ratio of leaf + // incr sum (ignore outliers) + st.sum_ar += float(ar < ANN_AR_TOOBIG ? ar : ANN_AR_TOOBIG); +} + +void ANNkd_split::getStats( // get subtree statistics + int dim, // dimension of space + ANNkdStats &st, // stats (modified) + ANNorthRect &bnd_box) // bounding box +{ + ANNkdStats ch_stats; // stats for children + // get stats for low child + ANNcoord hv = bnd_box.hi[cut_dim]; // save box bounds + bnd_box.hi[cut_dim] = cut_val; // upper bound for low child + ch_stats.reset(); // reset + child[ANN_LO]->getStats(dim, ch_stats, bnd_box); + st.merge(ch_stats); // merge them + bnd_box.hi[cut_dim] = hv; // restore bound + // get stats for high child + ANNcoord lv = bnd_box.lo[cut_dim]; // save box bounds + bnd_box.lo[cut_dim] = cut_val; // lower bound for high child + ch_stats.reset(); // reset + child[ANN_HI]->getStats(dim, ch_stats, bnd_box); + st.merge(ch_stats); // merge them + bnd_box.lo[cut_dim] = lv; // restore bound + + st.depth++; // increment depth + st.n_spl++; // increment number of splits +} + +//---------------------------------------------------------------------- +// getStats +// Collects a number of statistics related to kd_tree or +// bd_tree. +//---------------------------------------------------------------------- + +void ANNkd_tree::getStats( // get tree statistics + ANNkdStats &st) // stats (modified) +{ + st.reset(dim, n_pts, bkt_size); // reset stats + // create bounding box + ANNorthRect bnd_box(dim, bnd_box_lo, bnd_box_hi); + if (root != NULL) { // if nonempty tree + root->getStats(dim, st, bnd_box); // get statistics + st.avg_ar = st.sum_ar / st.n_lf; // average leaf asp ratio + } +} + +//---------------------------------------------------------------------- +// kd_tree destructor +// The destructor just frees the various elements that were +// allocated in the construction process. +//---------------------------------------------------------------------- + +ANNkd_tree::~ANNkd_tree() // tree destructor +{ + if (root != NULL and root != KD_TRIVIAL) delete root; + if (pidx != NULL) delete [] pidx; + if (bnd_box_lo != NULL) annDeallocPt(bnd_box_lo); + if (bnd_box_hi != NULL) annDeallocPt(bnd_box_hi); +} + +//---------------------------------------------------------------------- +// This is called with all use of ANN is finished. It eliminates the +// minor memory leak caused by the allocation of KD_TRIVIAL. +//---------------------------------------------------------------------- +void annClose() // close use of ANN +{ + if (KD_TRIVIAL != NULL) { + delete KD_TRIVIAL; + KD_TRIVIAL = NULL; + } +} + +//---------------------------------------------------------------------- +// kd_tree constructors +// There is a skeleton kd-tree constructor which sets up a +// trivial empty tree. The last optional argument allows +// the routine to be passed a point index array which is +// assumed to be of the proper size (n). Otherwise, one is +// allocated and initialized to the identity. Warning: In +// either case the destructor will deallocate this array. +// +// As a kludge, we need to allocate KD_TRIVIAL if one has not +// already been allocated. (This is because I'm too dumb to +// figure out how to cause a pointer to be allocated at load +// time.) +//---------------------------------------------------------------------- + +void ANNkd_tree::SkeletonTree( // construct skeleton tree + int n, // number of points + int dd, // dimension + int bs, // bucket size + ANNpointArray pa, // point array + ANNidxArray pi) // point indices +{ + dim = dd; // initialize basic elements + n_pts = n; + bkt_size = bs; + pts = pa; // initialize points array + + root = NULL; // no associated tree yet + + if (pi == NULL) { // point indices provided? + pidx = new ANNidx[n]; // no, allocate space for point indices + for (int i = 0; i < n; i++) { + pidx[i] = i; // initially identity + } + } + else { + pidx = pi; // yes, use them + } + + bnd_box_lo = bnd_box_hi = NULL; // bounding box is nonexistent + if (KD_TRIVIAL == NULL) // no trivial leaf node yet? + KD_TRIVIAL = new ANNkd_leaf(0, IDX_TRIVIAL); // allocate it + + // for deletion + pointToLeafVec.clear(); + pointToLeafVec.reserve(n_pts); + for(int k = 0; k < n_pts; ++k) { + pointToLeafVec.push_back(NULL); + } +} + +ANNkd_tree::ANNkd_tree( // basic constructor + int n, // number of points + int dd, // dimension + int bs) // bucket size +{ SkeletonTree(n, dd, bs); } // construct skeleton tree + + + +//---------------------------------------------------------------------- +// rkd_tree - recursive procedure to build a kd-tree +// +// Builds a kd-tree for points in pa as indexed through the +// array pidx[0..n-1] (typically a subarray of the array used in +// the top-level call). This routine permutes the array pidx, +// but does not alter pa[]. +// +// The construction is based on a standard algorithm for constructing +// the kd-tree (see Friedman, Bentley, and Finkel, ``An algorithm for +// finding best matches in logarithmic expected time,'' ACM Transactions +// on Mathematical Software, 3(3):209-226, 1977). The procedure +// operates by a simple divide-and-conquer strategy, which determines +// an appropriate orthogonal cutting plane (see below), and splits +// the points. When the number of points falls below the bucket size, +// we simply store the points in a leaf node's bucket. +// +// One of the arguments is a pointer to a splitting routine, +// whose prototype is: +// +// void split( +// ANNpointArray pa, // complete point array +// ANNidxArray pidx, // point array (permuted on return) +// ANNorthRect &bnds, // bounds of current cell +// int n, // number of points +// int dim, // dimension of space +// int &cut_dim, // cutting dimension +// ANNcoord &cut_val, // cutting value +// int &n_lo) // no. of points on low side of cut +// +// This procedure selects a cutting dimension and cutting value, +// partitions pa about these values, and returns the number of +// points on the low side of the cut. +//---------------------------------------------------------------------- + +ANNkd_ptr rkd_tree( // recursive construction of kd-tree + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices to store in subtree + int n, // number of points + int dim, // dimension of space + int bsp, // bucket space + ANNorthRect &bnd_box, // bounding box for current node + ANNkd_splitter splitter, // splitting routine + vector<ANNkd_leaf*>* ppointToLeafVec) +{ + if (n <= bsp) { // n small, make a leaf node + if (n == 0) // empty leaf node + return KD_TRIVIAL; // return (canonical) empty leaf + else { // construct the node and return + ANNkd_leaf* res = new ANNkd_leaf(n, pidx); + if ( 1 == bsp) { + (*ppointToLeafVec)[*pidx] = res; + } + return res; + } + } + else { // n large, make a splitting node + int cd; // cutting dimension + ANNcoord cv; // cutting value + int n_lo; // number on low side of cut + ANNkd_node *lo, *hi; // low and high children + + // invoke splitting procedure + (*splitter)(pa, pidx, bnd_box, n, dim, cd, cv, n_lo); + + ANNcoord lv = bnd_box.lo[cd]; // save bounds for cutting dimension + ANNcoord hv = bnd_box.hi[cd]; + + bnd_box.hi[cd] = cv; // modify bounds for left subtree + lo = rkd_tree( // build left subtree + pa, pidx, n_lo, // ...from pidx[0..n_lo-1] + dim, bsp, bnd_box, splitter, ppointToLeafVec); + bnd_box.hi[cd] = hv; // restore bounds + + bnd_box.lo[cd] = cv; // modify bounds for right subtree + hi = rkd_tree( // build right subtree + pa, pidx + n_lo, n-n_lo,// ...from pidx[n_lo..n-1] + dim, bsp, bnd_box, splitter, ppointToLeafVec); + bnd_box.lo[cd] = lv; // restore bounds + + // create the splitting node + ANNkd_split *ptr = new ANNkd_split(cd, cv, lv, hv, lo, hi); + if (lo != KD_TRIVIAL) + lo->setParent(ptr); + if (hi != KD_TRIVIAL) + hi->setParent(ptr); + ptr->setNumPoints(lo->getNumPoints() + hi->getNumPoints()); + + return ptr; // return pointer to this node + } +} + +// for kd-trees with deletion +/* +ANNkd_ptr rkd_tree_wd( // recursive construction of kd-tree + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices to store in subtree + int n, // number of points + int dim, // dimension of space + int bsp, // bucket space + ANNorthRect &bnd_box, // bounding box for current node + ANNkd_splitter_wd splitter) // splitting routine +{ + ANNidx cut_pt_idx; + if (n <= bsp) { // n small, make a leaf node + if (n == 0) // empty leaf node + return KD_TRIVIAL; // return (canonical) empty leaf + else // construct the node and return + return new ANNkd_leaf(n, pidx); + } + else { // n large, make a splitting node + int cd; // cutting dimension + ANNcoord cv; // cutting value + int n_lo; // number on low side of cut + ANNkd_node *lo, *hi; // low and high children + + // invoke splitting procedure + (*splitter)(pa, pidx, bnd_box, n, dim, cd, cv, n_lo, cut_pt_idx); + + ANNcoord lv = bnd_box.lo[cd]; // save bounds for cutting dimension + ANNcoord hv = bnd_box.hi[cd]; + + bnd_box.hi[cd] = cv; // modify bounds for left subtree + lo = rkd_tree_wd( // build left subtree + pa, pidx, n_lo, // ...from pidx[0..n_lo-1] + dim, bsp, bnd_box, splitter); + bnd_box.hi[cd] = hv; // restore bounds + + bnd_box.lo[cd] = cv; // modify bounds for right subtree + hi = rkd_tree_wd( // build right subtree + pa, pidx + n_lo, n-n_lo,// ...from pidx[n_lo..n-1] + dim, bsp, bnd_box, splitter); + bnd_box.lo[cd] = lv; // restore bounds + + // create the splitting node + ANNkd_split *ptr = new ANNkd_split(cd, cv, lv, hv, lo, hi, cut_pt_idx); + + return ptr; // return pointer to this node + } +} +*/ + +//---------------------------------------------------------------------- +// kd-tree constructor +// This is the main constructor for kd-trees given a set of points. +// It first builds a skeleton tree, then computes the bounding box +// of the data points, and then invokes rkd_tree() to actually +// build the tree, passing it the appropriate splitting routine. +//---------------------------------------------------------------------- + +ANNkd_tree::ANNkd_tree( // construct from point array + ANNpointArray pa, // point array (with at least n pts) + int n, // number of points + int dd, // dimension + int bs, // bucket size + ANNsplitRule split) // splitting method +{ + SkeletonTree(n, dd, bs); // set up the basic stuff + pts = pa; // where the points are + actual_num_points = n; + if (n == 0) return; // no points--no sweat + + ANNorthRect bnd_box(dd); // bounding box for points + annEnclRect(pa, pidx, n, dd, bnd_box);// construct bounding rectangle + // copy to tree structure + bnd_box_lo = annCopyPt(dd, bnd_box.lo); + bnd_box_hi = annCopyPt(dd, bnd_box.hi); + + switch (split) { // build by rule + case ANN_KD_STD: // standard kd-splitting rule + root = rkd_tree(pa, pidx, n, dd, bs, bnd_box, kd_split, &pointToLeafVec); + break; + case ANN_KD_MIDPT: // midpoint split + root = rkd_tree(pa, pidx, n, dd, bs, bnd_box, midpt_split, &pointToLeafVec); + break; + case ANN_KD_FAIR: // fair split + root = rkd_tree(pa, pidx, n, dd, bs, bnd_box, fair_split, &pointToLeafVec); + break; + case ANN_KD_SUGGEST: // best (in our opinion) + case ANN_KD_SL_MIDPT: // sliding midpoint split + root = rkd_tree(pa, pidx, n, dd, bs, bnd_box, sl_midpt_split, &pointToLeafVec); + break; + case ANN_KD_SL_FAIR: // sliding fair split + root = rkd_tree(pa, pidx, n, dd, bs, bnd_box, sl_fair_split, &pointToLeafVec); + break; + // for kd-trees with deletion + /* + //case ANN_KD_SUGGEST: + case ANN_KD_STD_WD: + root = rkd_tree_wd(pa, pidx, n, dd, bs, bnd_box, kd_split_wd); + break; + case ANN_KD_MIDPT_WD: + root = rkd_tree_wd(pa, pidx, n, dd, bs, bnd_box, kd_split_wd); + break; + case ANN_KD_SL_MIDPT_WD: + root = rkd_tree_wd(pa, pidx, n, dd, bs, bnd_box, kd_split_wd); + break; + */ + default: + annError("Illegal splitting method", ANNabort); + } +} + + +// deletion code +// +// +// +// +// +void ANNkd_tree::delete_point(const int point_idx) +{ + // range check + assert(0 <= point_idx and point_idx < n_pts); + assert(actual_num_points > 0); + // if this is the first deletion, + // initialize isDeleted vector + if (isDeleted.empty()) { + isDeleted.reserve(n_pts); + for(size_t k = 0; k < n_pts; ++k) { + isDeleted.push_back(false); + } + } + // points shouldn't be deleted twice + assert(!isDeleted[point_idx]); + assert(root != NULL); + ANNkd_leaf* leafWithPoint = pointToLeafVec.at(point_idx); + assert(leafWithPoint != NULL); + // if leafWithPoint != root, + // its parent will delete the leaf + pointToLeafVec.at(point_idx)->delete_point(point_idx, leafWithPoint != root); + if (leafWithPoint == root) { + // we had only one point, + // so the tree must delete it + root = KD_TRIVIAL; + delete leafWithPoint; + } + isDeleted[point_idx] = true; + actual_num_points--; +} + +void ANNkd_leaf::delete_point(const int point_idx, const bool killYourself) +{ + assert(n_pts == 1); + assert(bkt[0] == point_idx); + ANNkd_split* myPar = parent; + while(myPar != NULL) { + myPar->decNumPoints(); + myPar = myPar->getParent(); + } + if (parent != NULL) + parent->delete_leaf(this); + if (killYourself) + delete this; +} + +void ANNkd_split::delete_leaf(ANNkd_leaf* childToDelete) +{ + assert(child[ANN_LO] == childToDelete or child[ANN_HI] == childToDelete); + if (child[ANN_LO] == childToDelete) + child[ANN_LO] = KD_TRIVIAL; + else + child[ANN_HI] = KD_TRIVIAL; +} +} diff --git a/geom_bottleneck/bottleneck/src/ann/kd_util.cpp b/geom_bottleneck/bottleneck/src/ann/kd_util.cpp new file mode 100644 index 0000000..02b35c4 --- /dev/null +++ b/geom_bottleneck/bottleneck/src/ann/kd_util.cpp @@ -0,0 +1,441 @@ +//---------------------------------------------------------------------- +// File: kd_util.cpp +// Programmer: Sunil Arya and David Mount +// Description: Common utilities for kd-trees +// Last modified: 01/04/05 (Version 1.0) +//---------------------------------------------------------------------- +// 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 0.1 03/04/98 +// Initial release +//---------------------------------------------------------------------- + +#include "kd_util.h" // kd-utility declarations + +#include <ANN/ANNperf.h> // performance evaluation + +namespace geom_bt { +//---------------------------------------------------------------------- +// The following routines are utility functions for manipulating +// points sets, used in determining splitting planes for kd-tree +// construction. +//---------------------------------------------------------------------- + +//---------------------------------------------------------------------- +// NOTE: Virtually all point indexing is done through an index (i.e. +// permutation) array pidx. Consequently, a reference to the d-th +// coordinate of the i-th point is pa[pidx[i]][d]. The macro PA(i,d) +// is a shorthand for this. +//---------------------------------------------------------------------- + // standard 2-d indirect indexing +#define PA(i,d) (pa[pidx[(i)]][(d)]) + // accessing a single point +#define PP(i) (pa[pidx[(i)]]) + +//---------------------------------------------------------------------- +// annAspectRatio +// Compute the aspect ratio (ratio of longest to shortest side) +// of a rectangle. +//---------------------------------------------------------------------- + +double annAspectRatio( + int dim, // dimension + const ANNorthRect &bnd_box) // bounding cube +{ + ANNcoord length = bnd_box.hi[0] - bnd_box.lo[0]; + ANNcoord min_length = length; // min side length + ANNcoord max_length = length; // max side length + for (int d = 0; d < dim; d++) { + length = bnd_box.hi[d] - bnd_box.lo[d]; + if (length < min_length) min_length = length; + if (length > max_length) max_length = length; + } + return max_length/min_length; +} + +//---------------------------------------------------------------------- +// annEnclRect, annEnclCube +// These utilities compute the smallest rectangle and cube enclosing +// a set of points, respectively. +//---------------------------------------------------------------------- + +void annEnclRect( + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices + int n, // number of points + int dim, // dimension + ANNorthRect &bnds) // bounding cube (returned) +{ + for (int d = 0; d < dim; d++) { // find smallest enclosing rectangle + ANNcoord lo_bnd = PA(0,d); // lower bound on dimension d + ANNcoord hi_bnd = PA(0,d); // upper bound on dimension d + for (int i = 0; i < n; i++) { + if (PA(i,d) < lo_bnd) lo_bnd = PA(i,d); + else if (PA(i,d) > hi_bnd) hi_bnd = PA(i,d); + } + bnds.lo[d] = lo_bnd; + bnds.hi[d] = hi_bnd; + } +} + +void annEnclCube( // compute smallest enclosing cube + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices + int n, // number of points + int dim, // dimension + ANNorthRect &bnds) // bounding cube (returned) +{ + int d; + // compute smallest enclosing rect + annEnclRect(pa, pidx, n, dim, bnds); + + ANNcoord max_len = 0; // max length of any side + for (d = 0; d < dim; d++) { // determine max side length + ANNcoord len = bnds.hi[d] - bnds.lo[d]; + if (len > max_len) { // update max_len if longest + max_len = len; + } + } + for (d = 0; d < dim; d++) { // grow sides to match max + ANNcoord len = bnds.hi[d] - bnds.lo[d]; + ANNcoord half_diff = (max_len - len) / 2; + bnds.lo[d] -= half_diff; + bnds.hi[d] += half_diff; + } +} + +//---------------------------------------------------------------------- +// annBoxDistance - utility routine which computes distance from point to +// box (Note: most distances to boxes are computed using incremental +// distance updates, not this function.) +//---------------------------------------------------------------------- + +ANNdist annBoxDistance( // compute distance from point to box + const ANNpoint q, // the point + const ANNpoint lo, // low point of box + const ANNpoint hi, // high point of box + int dim) // dimension of space +{ + register ANNdist dist = 0.0; // sum of squared distances + register ANNdist t; + + for (register int d = 0; d < dim; d++) { + if (q[d] < lo[d]) { // q is left of box + t = ANNdist(lo[d]) - ANNdist(q[d]); + dist = ANN_SUM(dist, ANN_POW(t)); + } + else if (q[d] > hi[d]) { // q is right of box + t = ANNdist(q[d]) - ANNdist(hi[d]); + dist = ANN_SUM(dist, ANN_POW(t)); + } + } + ANN_FLOP(4*dim) // increment floating op count + + return dist; +} + +//---------------------------------------------------------------------- +// annSpread - find spread along given dimension +// annMinMax - find min and max coordinates along given dimension +// annMaxSpread - find dimension of max spread +//---------------------------------------------------------------------- + +ANNcoord annSpread( // compute point spread along dimension + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices + int n, // number of points + int d) // dimension to check +{ + ANNcoord min = PA(0,d); // compute max and min coords + ANNcoord max = PA(0,d); + for (int i = 1; i < n; i++) { + ANNcoord c = PA(i,d); + if (c < min) min = c; + else if (c > max) max = c; + } + return (max - min); // total spread is difference +} + +void annMinMax( // compute min and max coordinates along dim + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices + int n, // number of points + int d, // dimension to check + ANNcoord &min, // minimum value (returned) + ANNcoord &max) // maximum value (returned) +{ + min = PA(0,d); // compute max and min coords + max = PA(0,d); + for (int i = 1; i < n; i++) { + ANNcoord c = PA(i,d); + if (c < min) min = c; + else if (c > max) max = c; + } +} + +int annMaxSpread( // compute dimension of max spread + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices + int n, // number of points + int dim) // dimension of space +{ + int max_dim = 0; // dimension of max spread + ANNcoord max_spr = 0; // amount of max spread + + if (n == 0) return max_dim; // no points, who cares? + + for (int d = 0; d < dim; d++) { // compute spread along each dim + ANNcoord spr = annSpread(pa, pidx, n, d); + if (spr > max_spr) { // bigger than current max + max_spr = spr; + max_dim = d; + } + } + return max_dim; +} + +//---------------------------------------------------------------------- +// annMedianSplit - split point array about its median +// Splits a subarray of points pa[0..n] about an element of given +// rank (median: n_lo = n/2) with respect to dimension d. It places +// the element of rank n_lo-1 correctly (because our splitting rule +// takes the mean of these two). On exit, the array is permuted so +// that: +// +// pa[0..n_lo-2][d] <= pa[n_lo-1][d] <= pa[n_lo][d] <= pa[n_lo+1..n-1][d]. +// +// The mean of pa[n_lo-1][d] and pa[n_lo][d] is returned as the +// splitting value. +// +// All indexing is done indirectly through the index array pidx. +// +// This function uses the well known selection algorithm due to +// C.A.R. Hoare. +//---------------------------------------------------------------------- + + // swap two points in pa array +#define PASWAP(a,b) { int tmp = pidx[a]; pidx[a] = pidx[b]; pidx[b] = tmp; } + +void annMedianSplit( + ANNpointArray pa, // points to split + ANNidxArray pidx, // point indices + int n, // number of points + int d, // dimension along which to split + ANNcoord &cv, // cutting value + int n_lo) // split into n_lo and n-n_lo +{ + int l = 0; // left end of current subarray + int r = n-1; // right end of current subarray + while (l < r) { + register int i = (r+l)/2; // select middle as pivot + register int k; + + if (PA(i,d) > PA(r,d)) // make sure last > pivot + PASWAP(i,r) + PASWAP(l,i); // move pivot to first position + + ANNcoord c = PA(l,d); // pivot value + i = l; + k = r; + for(;;) { // pivot about c + while (PA(++i,d) < c) ; + while (PA(--k,d) > c) ; + if (i < k) PASWAP(i,k) else break; + } + PASWAP(l,k); // pivot winds up in location k + + if (k > n_lo) r = k-1; // recurse on proper subarray + else if (k < n_lo) l = k+1; + else break; // got the median exactly + } + if (n_lo > 0) { // search for next smaller item + ANNcoord c = PA(0,d); // candidate for max + int k = 0; // candidate's index + for (int i = 1; i < n_lo; i++) { + if (PA(i,d) > c) { + c = PA(i,d); + k = i; + } + } + PASWAP(n_lo-1, k); // max among pa[0..n_lo-1] to pa[n_lo-1] + } + // cut value is midpoint value + cv = (PA(n_lo-1,d) + PA(n_lo,d))/2.0; +} + +//---------------------------------------------------------------------- +// annPlaneSplit - split point array about a cutting plane +// Split the points in an array about a given plane along a +// given cutting dimension. On exit, br1 and br2 are set so +// that: +// +// pa[ 0 ..br1-1] < cv +// pa[br1..br2-1] == cv +// pa[br2.. n -1] > cv +// +// All indexing is done indirectly through the index array pidx. +// +//---------------------------------------------------------------------- + +void annPlaneSplit( // split points by a plane + ANNpointArray pa, // points to split + ANNidxArray pidx, // point indices + int n, // number of points + int d, // dimension along which to split + ANNcoord cv, // cutting value + int &br1, // first break (values < cv) + int &br2) // second break (values == cv) +{ + int l = 0; + int r = n-1; + for(;;) { // partition pa[0..n-1] about cv + while (l < n && PA(l,d) < cv) l++; + while (r >= 0 && PA(r,d) >= cv) r--; + if (l > r) break; + PASWAP(l,r); + l++; r--; + } + br1 = l; // now: pa[0..br1-1] < cv <= pa[br1..n-1] + r = n-1; + for(;;) { // partition pa[br1..n-1] about cv + while (l < n && PA(l,d) <= cv) l++; + while (r >= br1 && PA(r,d) > cv) r--; + if (l > r) break; + PASWAP(l,r); + l++; r--; + } + br2 = l; // now: pa[br1..br2-1] == cv < pa[br2..n-1] +} + + +//---------------------------------------------------------------------- +// annBoxSplit - split point array about a orthogonal rectangle +// Split the points in an array about a given orthogonal +// rectangle. On exit, n_in is set to the number of points +// that are inside (or on the boundary of) the rectangle. +// +// All indexing is done indirectly through the index array pidx. +// +//---------------------------------------------------------------------- + +void annBoxSplit( // split points by a box + ANNpointArray pa, // points to split + ANNidxArray pidx, // point indices + int n, // number of points + int dim, // dimension of space + ANNorthRect &box, // the box + int &n_in) // number of points inside (returned) +{ + int l = 0; + int r = n-1; + for(;;) { // partition pa[0..n-1] about box + while (l < n && box.inside(dim, PP(l))) l++; + while (r >= 0 && !box.inside(dim, PP(r))) r--; + if (l > r) break; + PASWAP(l,r); + l++; r--; + } + n_in = l; // now: pa[0..n_in-1] inside and rest outside +} + +//---------------------------------------------------------------------- +// annSplitBalance - compute balance factor for a given plane split +// Balance factor is defined as the number of points lying +// below the splitting value minus n/2 (median). Thus, a +// median split has balance 0, left of this is negative and +// right of this is positive. (The points are unchanged.) +//---------------------------------------------------------------------- + +int annSplitBalance( // determine balance factor of a split + ANNpointArray pa, // points to split + ANNidxArray pidx, // point indices + int n, // number of points + int d, // dimension along which to split + ANNcoord cv) // cutting value +{ + int n_lo = 0; + for(int i = 0; i < n; i++) { // count number less than cv + if (PA(i,d) < cv) n_lo++; + } + return n_lo - n/2; +} + +//---------------------------------------------------------------------- +// annBox2Bnds - convert bounding box to list of bounds +// Given two boxes, an inner box enclosed within a bounding +// box, this routine determines all the sides for which the +// inner box is strictly contained with the bounding box, +// and adds an appropriate entry to a list of bounds. Then +// we allocate storage for the final list of bounds, and return +// the resulting list and its size. +//---------------------------------------------------------------------- + +void annBox2Bnds( // convert inner box to bounds + const ANNorthRect &inner_box, // inner box + const ANNorthRect &bnd_box, // enclosing box + int dim, // dimension of space + int &n_bnds, // number of bounds (returned) + ANNorthHSArray &bnds) // bounds array (returned) +{ + int i; + n_bnds = 0; // count number of bounds + for (i = 0; i < dim; i++) { + if (inner_box.lo[i] > bnd_box.lo[i]) // low bound is inside + n_bnds++; + if (inner_box.hi[i] < bnd_box.hi[i]) // high bound is inside + n_bnds++; + } + + bnds = new ANNorthHalfSpace[n_bnds]; // allocate appropriate size + + int j = 0; + for (i = 0; i < dim; i++) { // fill the array + if (inner_box.lo[i] > bnd_box.lo[i]) { + bnds[j].cd = i; + bnds[j].cv = inner_box.lo[i]; + bnds[j].sd = +1; + j++; + } + if (inner_box.hi[i] < bnd_box.hi[i]) { + bnds[j].cd = i; + bnds[j].cv = inner_box.hi[i]; + bnds[j].sd = -1; + j++; + } + } +} + +//---------------------------------------------------------------------- +// annBnds2Box - convert list of bounds to bounding box +// Given an enclosing box and a list of bounds, this routine +// computes the corresponding inner box. It is assumed that +// the box points have been allocated already. +//---------------------------------------------------------------------- + +void annBnds2Box( + const ANNorthRect &bnd_box, // enclosing box + int dim, // dimension of space + int n_bnds, // number of bounds + ANNorthHSArray bnds, // bounds array + ANNorthRect &inner_box) // inner box (returned) +{ + annAssignRect(dim, inner_box, bnd_box); // copy bounding box to inner + + for (int i = 0; i < n_bnds; i++) { + bnds[i].project(inner_box.lo); // project each endpoint + bnds[i].project(inner_box.hi); + } +} +} |