//----------------------------------------------------------------------
// 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);
}
}