kd_util.cpp
上传用户:chinafayin
上传日期:2022-04-05
资源大小:153k
文件大小:15k
- //----------------------------------------------------------------------
- // 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 "../ANNperf.h" // performance evaluation
- //----------------------------------------------------------------------
- // 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);
- }
- }