diff options
Diffstat (limited to 'geom_bottleneck/bottleneck/src/ann/kd_util.cpp')
| -rw-r--r-- | geom_bottleneck/bottleneck/src/ann/kd_util.cpp | 441 |
1 files changed, 441 insertions, 0 deletions
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); + } +} +} |