You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
440 lines
15 KiB
C++
440 lines
15 KiB
C++
//----------------------------------------------------------------------
|
|
// 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
|
|
|
|
//----------------------------------------------------------------------
|
|
// 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);
|
|
}
|
|
}
|