| 1 |
//---------------------------------------------------------------------- |
| 2 |
// File: kd_util.cpp |
| 3 |
// Programmer: Sunil Arya and David Mount |
| 4 |
// Description: Common utilities for kd-trees |
| 5 |
// Last modified: 01/04/05 (Version 1.0) |
| 6 |
//---------------------------------------------------------------------- |
| 7 |
// Copyright (c) 1997-2005 University of Maryland and Sunil Arya and |
| 8 |
// David Mount. All Rights Reserved. |
| 9 |
// |
| 10 |
// This software and related documentation is part of the Approximate |
| 11 |
// Nearest Neighbor Library (ANN). This software is provided under |
| 12 |
// the provisions of the Lesser GNU Public License (LGPL). See the |
| 13 |
// file ../ReadMe.txt for further information. |
| 14 |
// |
| 15 |
// The University of Maryland (U.M.) and the authors make no |
| 16 |
// representations about the suitability or fitness of this software for |
| 17 |
// any purpose. It is provided "as is" without express or implied |
| 18 |
// warranty. |
| 19 |
//---------------------------------------------------------------------- |
| 20 |
// History: |
| 21 |
// Revision 0.1 03/04/98 |
| 22 |
// Initial release |
| 23 |
//---------------------------------------------------------------------- |
| 24 |
|
| 25 |
#include "kd_util.h" // kd-utility declarations |
| 26 |
|
| 27 |
#include <ANN/ANNperf.h> // performance evaluation |
| 28 |
|
| 29 |
//---------------------------------------------------------------------- |
| 30 |
// The following routines are utility functions for manipulating |
| 31 |
// points sets, used in determining splitting planes for kd-tree |
| 32 |
// construction. |
| 33 |
//---------------------------------------------------------------------- |
| 34 |
|
| 35 |
//---------------------------------------------------------------------- |
| 36 |
// NOTE: Virtually all point indexing is done through an index (i.e. |
| 37 |
// permutation) array pidx. Consequently, a reference to the d-th |
| 38 |
// coordinate of the i-th point is pa[pidx[i]][d]. The macro PA(i,d) |
| 39 |
// is a shorthand for this. |
| 40 |
//---------------------------------------------------------------------- |
| 41 |
// standard 2-d indirect indexing |
| 42 |
#define PA(i,d) (pa[pidx[(i)]][(d)]) |
| 43 |
// accessing a single point |
| 44 |
#define PP(i) (pa[pidx[(i)]]) |
| 45 |
|
| 46 |
//---------------------------------------------------------------------- |
| 47 |
// annAspectRatio |
| 48 |
// Compute the aspect ratio (ratio of longest to shortest side) |
| 49 |
// of a rectangle. |
| 50 |
//---------------------------------------------------------------------- |
| 51 |
|
| 52 |
double annAspectRatio( |
| 53 |
int dim, // dimension |
| 54 |
const ANNorthRect &bnd_box) // bounding cube |
| 55 |
{ |
| 56 |
ANNcoord length = bnd_box.hi[0] - bnd_box.lo[0]; |
| 57 |
ANNcoord min_length = length; // min side length |
| 58 |
ANNcoord max_length = length; // max side length |
| 59 |
for (int d = 0; d < dim; d++) { |
| 60 |
length = bnd_box.hi[d] - bnd_box.lo[d]; |
| 61 |
if (length < min_length) min_length = length; |
| 62 |
if (length > max_length) max_length = length; |
| 63 |
} |
| 64 |
return max_length/min_length; |
| 65 |
} |
| 66 |
|
| 67 |
//---------------------------------------------------------------------- |
| 68 |
// annEnclRect, annEnclCube |
| 69 |
// These utilities compute the smallest rectangle and cube enclosing |
| 70 |
// a set of points, respectively. |
| 71 |
//---------------------------------------------------------------------- |
| 72 |
|
| 73 |
void annEnclRect( |
| 74 |
ANNpointArray pa, // point array |
| 75 |
ANNidxArray pidx, // point indices |
| 76 |
int n, // number of points |
| 77 |
int dim, // dimension |
| 78 |
ANNorthRect &bnds) // bounding cube (returned) |
| 79 |
{ |
| 80 |
for (int d = 0; d < dim; d++) { // find smallest enclosing rectangle |
| 81 |
ANNcoord lo_bnd = PA(0,d); // lower bound on dimension d |
| 82 |
ANNcoord hi_bnd = PA(0,d); // upper bound on dimension d |
| 83 |
for (int i = 0; i < n; i++) { |
| 84 |
if (PA(i,d) < lo_bnd) lo_bnd = PA(i,d); |
| 85 |
else if (PA(i,d) > hi_bnd) hi_bnd = PA(i,d); |
| 86 |
} |
| 87 |
bnds.lo[d] = lo_bnd; |
| 88 |
bnds.hi[d] = hi_bnd; |
| 89 |
} |
| 90 |
} |
| 91 |
|
| 92 |
void annEnclCube( // compute smallest enclosing cube |
| 93 |
ANNpointArray pa, // point array |
| 94 |
ANNidxArray pidx, // point indices |
| 95 |
int n, // number of points |
| 96 |
int dim, // dimension |
| 97 |
ANNorthRect &bnds) // bounding cube (returned) |
| 98 |
{ |
| 99 |
int d; |
| 100 |
// compute smallest enclosing rect |
| 101 |
annEnclRect(pa, pidx, n, dim, bnds); |
| 102 |
|
| 103 |
ANNcoord max_len = 0; // max length of any side |
| 104 |
for (d = 0; d < dim; d++) { // determine max side length |
| 105 |
ANNcoord len = bnds.hi[d] - bnds.lo[d]; |
| 106 |
if (len > max_len) { // update max_len if longest |
| 107 |
max_len = len; |
| 108 |
} |
| 109 |
} |
| 110 |
for (d = 0; d < dim; d++) { // grow sides to match max |
| 111 |
ANNcoord len = bnds.hi[d] - bnds.lo[d]; |
| 112 |
ANNcoord half_diff = (max_len - len) / 2; |
| 113 |
bnds.lo[d] -= half_diff; |
| 114 |
bnds.hi[d] += half_diff; |
| 115 |
} |
| 116 |
} |
| 117 |
|
| 118 |
//---------------------------------------------------------------------- |
| 119 |
// annBoxDistance - utility routine which computes distance from point to |
| 120 |
// box (Note: most distances to boxes are computed using incremental |
| 121 |
// distance updates, not this function.) |
| 122 |
//---------------------------------------------------------------------- |
| 123 |
|
| 124 |
ANNdist annBoxDistance( // compute distance from point to box |
| 125 |
const ANNpoint q, // the point |
| 126 |
const ANNpoint lo, // low point of box |
| 127 |
const ANNpoint hi, // high point of box |
| 128 |
int dim) // dimension of space |
| 129 |
{ |
| 130 |
register ANNdist dist = 0.0; // sum of squared distances |
| 131 |
register ANNdist t; |
| 132 |
|
| 133 |
for (register int d = 0; d < dim; d++) { |
| 134 |
if (q[d] < lo[d]) { // q is left of box |
| 135 |
t = ANNdist(lo[d]) - ANNdist(q[d]); |
| 136 |
dist = ANN_SUM(dist, ANN_POW(t)); |
| 137 |
} |
| 138 |
else if (q[d] > hi[d]) { // q is right of box |
| 139 |
t = ANNdist(q[d]) - ANNdist(hi[d]); |
| 140 |
dist = ANN_SUM(dist, ANN_POW(t)); |
| 141 |
} |
| 142 |
} |
| 143 |
ANN_FLOP(4*dim) // increment floating op count |
| 144 |
|
| 145 |
return dist; |
| 146 |
} |
| 147 |
|
| 148 |
//---------------------------------------------------------------------- |
| 149 |
// annSpread - find spread along given dimension |
| 150 |
// annMinMax - find min and max coordinates along given dimension |
| 151 |
// annMaxSpread - find dimension of max spread |
| 152 |
//---------------------------------------------------------------------- |
| 153 |
|
| 154 |
ANNcoord annSpread( // compute point spread along dimension |
| 155 |
ANNpointArray pa, // point array |
| 156 |
ANNidxArray pidx, // point indices |
| 157 |
int n, // number of points |
| 158 |
int d) // dimension to check |
| 159 |
{ |
| 160 |
ANNcoord min = PA(0,d); // compute max and min coords |
| 161 |
ANNcoord max = PA(0,d); |
| 162 |
for (int i = 1; i < n; i++) { |
| 163 |
ANNcoord c = PA(i,d); |
| 164 |
if (c < min) min = c; |
| 165 |
else if (c > max) max = c; |
| 166 |
} |
| 167 |
return (max - min); // total spread is difference |
| 168 |
} |
| 169 |
|
| 170 |
void annMinMax( // compute min and max coordinates along dim |
| 171 |
ANNpointArray pa, // point array |
| 172 |
ANNidxArray pidx, // point indices |
| 173 |
int n, // number of points |
| 174 |
int d, // dimension to check |
| 175 |
ANNcoord &min, // minimum value (returned) |
| 176 |
ANNcoord &max) // maximum value (returned) |
| 177 |
{ |
| 178 |
min = PA(0,d); // compute max and min coords |
| 179 |
max = PA(0,d); |
| 180 |
for (int i = 1; i < n; i++) { |
| 181 |
ANNcoord c = PA(i,d); |
| 182 |
if (c < min) min = c; |
| 183 |
else if (c > max) max = c; |
| 184 |
} |
| 185 |
} |
| 186 |
|
| 187 |
int annMaxSpread( // compute dimension of max spread |
| 188 |
ANNpointArray pa, // point array |
| 189 |
ANNidxArray pidx, // point indices |
| 190 |
int n, // number of points |
| 191 |
int dim) // dimension of space |
| 192 |
{ |
| 193 |
int max_dim = 0; // dimension of max spread |
| 194 |
ANNcoord max_spr = 0; // amount of max spread |
| 195 |
|
| 196 |
if (n == 0) return max_dim; // no points, who cares? |
| 197 |
|
| 198 |
for (int d = 0; d < dim; d++) { // compute spread along each dim |
| 199 |
ANNcoord spr = annSpread(pa, pidx, n, d); |
| 200 |
if (spr > max_spr) { // bigger than current max |
| 201 |
max_spr = spr; |
| 202 |
max_dim = d; |
| 203 |
} |
| 204 |
} |
| 205 |
return max_dim; |
| 206 |
} |
| 207 |
|
| 208 |
//---------------------------------------------------------------------- |
| 209 |
// annMedianSplit - split point array about its median |
| 210 |
// Splits a subarray of points pa[0..n] about an element of given |
| 211 |
// rank (median: n_lo = n/2) with respect to dimension d. It places |
| 212 |
// the element of rank n_lo-1 correctly (because our splitting rule |
| 213 |
// takes the mean of these two). On exit, the array is permuted so |
| 214 |
// that: |
| 215 |
// |
| 216 |
// pa[0..n_lo-2][d] <= pa[n_lo-1][d] <= pa[n_lo][d] <= pa[n_lo+1..n-1][d]. |
| 217 |
// |
| 218 |
// The mean of pa[n_lo-1][d] and pa[n_lo][d] is returned as the |
| 219 |
// splitting value. |
| 220 |
// |
| 221 |
// All indexing is done indirectly through the index array pidx. |
| 222 |
// |
| 223 |
// This function uses the well known selection algorithm due to |
| 224 |
// C.A.R. Hoare. |
| 225 |
//---------------------------------------------------------------------- |
| 226 |
|
| 227 |
// swap two points in pa array |
| 228 |
#define PASWAP(a,b) { int tmp = pidx[a]; pidx[a] = pidx[b]; pidx[b] = tmp; } |
| 229 |
|
| 230 |
void annMedianSplit( |
| 231 |
ANNpointArray pa, // points to split |
| 232 |
ANNidxArray pidx, // point indices |
| 233 |
int n, // number of points |
| 234 |
int d, // dimension along which to split |
| 235 |
ANNcoord &cv, // cutting value |
| 236 |
int n_lo) // split into n_lo and n-n_lo |
| 237 |
{ |
| 238 |
int l = 0; // left end of current subarray |
| 239 |
int r = n-1; // right end of current subarray |
| 240 |
while (l < r) { |
| 241 |
register int i = (r+l)/2; // select middle as pivot |
| 242 |
register int k; |
| 243 |
|
| 244 |
if (PA(i,d) > PA(r,d)) // make sure last > pivot |
| 245 |
PASWAP(i,r) |
| 246 |
PASWAP(l,i); // move pivot to first position |
| 247 |
|
| 248 |
ANNcoord c = PA(l,d); // pivot value |
| 249 |
i = l; |
| 250 |
k = r; |
| 251 |
for(;;) { // pivot about c |
| 252 |
while (PA(++i,d) < c) ; |
| 253 |
while (PA(--k,d) > c) ; |
| 254 |
if (i < k) PASWAP(i,k) else break; |
| 255 |
} |
| 256 |
PASWAP(l,k); // pivot winds up in location k |
| 257 |
|
| 258 |
if (k > n_lo) r = k-1; // recurse on proper subarray |
| 259 |
else if (k < n_lo) l = k+1; |
| 260 |
else break; // got the median exactly |
| 261 |
} |
| 262 |
if (n_lo > 0) { // search for next smaller item |
| 263 |
ANNcoord c = PA(0,d); // candidate for max |
| 264 |
int k = 0; // candidate's index |
| 265 |
for (int i = 1; i < n_lo; i++) { |
| 266 |
if (PA(i,d) > c) { |
| 267 |
c = PA(i,d); |
| 268 |
k = i; |
| 269 |
} |
| 270 |
} |
| 271 |
PASWAP(n_lo-1, k); // max among pa[0..n_lo-1] to pa[n_lo-1] |
| 272 |
} |
| 273 |
// cut value is midpoint value |
| 274 |
cv = (PA(n_lo-1,d) + PA(n_lo,d))/2.0; |
| 275 |
} |
| 276 |
|
| 277 |
//---------------------------------------------------------------------- |
| 278 |
// annPlaneSplit - split point array about a cutting plane |
| 279 |
// Split the points in an array about a given plane along a |
| 280 |
// given cutting dimension. On exit, br1 and br2 are set so |
| 281 |
// that: |
| 282 |
// |
| 283 |
// pa[ 0 ..br1-1] < cv |
| 284 |
// pa[br1..br2-1] == cv |
| 285 |
// pa[br2.. n -1] > cv |
| 286 |
// |
| 287 |
// All indexing is done indirectly through the index array pidx. |
| 288 |
// |
| 289 |
//---------------------------------------------------------------------- |
| 290 |
|
| 291 |
void annPlaneSplit( // split points by a plane |
| 292 |
ANNpointArray pa, // points to split |
| 293 |
ANNidxArray pidx, // point indices |
| 294 |
int n, // number of points |
| 295 |
int d, // dimension along which to split |
| 296 |
ANNcoord cv, // cutting value |
| 297 |
int &br1, // first break (values < cv) |
| 298 |
int &br2) // second break (values == cv) |
| 299 |
{ |
| 300 |
int l = 0; |
| 301 |
int r = n-1; |
| 302 |
for(;;) { // partition pa[0..n-1] about cv |
| 303 |
while (l < n && PA(l,d) < cv) l++; |
| 304 |
while (r >= 0 && PA(r,d) >= cv) r--; |
| 305 |
if (l > r) break; |
| 306 |
PASWAP(l,r); |
| 307 |
l++; r--; |
| 308 |
} |
| 309 |
br1 = l; // now: pa[0..br1-1] < cv <= pa[br1..n-1] |
| 310 |
r = n-1; |
| 311 |
for(;;) { // partition pa[br1..n-1] about cv |
| 312 |
while (l < n && PA(l,d) <= cv) l++; |
| 313 |
while (r >= br1 && PA(r,d) > cv) r--; |
| 314 |
if (l > r) break; |
| 315 |
PASWAP(l,r); |
| 316 |
l++; r--; |
| 317 |
} |
| 318 |
br2 = l; // now: pa[br1..br2-1] == cv < pa[br2..n-1] |
| 319 |
} |
| 320 |
|
| 321 |
|
| 322 |
//---------------------------------------------------------------------- |
| 323 |
// annBoxSplit - split point array about a orthogonal rectangle |
| 324 |
// Split the points in an array about a given orthogonal |
| 325 |
// rectangle. On exit, n_in is set to the number of points |
| 326 |
// that are inside (or on the boundary of) the rectangle. |
| 327 |
// |
| 328 |
// All indexing is done indirectly through the index array pidx. |
| 329 |
// |
| 330 |
//---------------------------------------------------------------------- |
| 331 |
|
| 332 |
void annBoxSplit( // split points by a box |
| 333 |
ANNpointArray pa, // points to split |
| 334 |
ANNidxArray pidx, // point indices |
| 335 |
int n, // number of points |
| 336 |
int dim, // dimension of space |
| 337 |
ANNorthRect &box, // the box |
| 338 |
int &n_in) // number of points inside (returned) |
| 339 |
{ |
| 340 |
int l = 0; |
| 341 |
int r = n-1; |
| 342 |
for(;;) { // partition pa[0..n-1] about box |
| 343 |
while (l < n && box.inside(dim, PP(l))) l++; |
| 344 |
while (r >= 0 && !box.inside(dim, PP(r))) r--; |
| 345 |
if (l > r) break; |
| 346 |
PASWAP(l,r); |
| 347 |
l++; r--; |
| 348 |
} |
| 349 |
n_in = l; // now: pa[0..n_in-1] inside and rest outside |
| 350 |
} |
| 351 |
|
| 352 |
//---------------------------------------------------------------------- |
| 353 |
// annSplitBalance - compute balance factor for a given plane split |
| 354 |
// Balance factor is defined as the number of points lying |
| 355 |
// below the splitting value minus n/2 (median). Thus, a |
| 356 |
// median split has balance 0, left of this is negative and |
| 357 |
// right of this is positive. (The points are unchanged.) |
| 358 |
//---------------------------------------------------------------------- |
| 359 |
|
| 360 |
int annSplitBalance( // determine balance factor of a split |
| 361 |
ANNpointArray pa, // points to split |
| 362 |
ANNidxArray pidx, // point indices |
| 363 |
int n, // number of points |
| 364 |
int d, // dimension along which to split |
| 365 |
ANNcoord cv) // cutting value |
| 366 |
{ |
| 367 |
int n_lo = 0; |
| 368 |
for(int i = 0; i < n; i++) { // count number less than cv |
| 369 |
if (PA(i,d) < cv) n_lo++; |
| 370 |
} |
| 371 |
return n_lo - n/2; |
| 372 |
} |
| 373 |
|
| 374 |
//---------------------------------------------------------------------- |
| 375 |
// annBox2Bnds - convert bounding box to list of bounds |
| 376 |
// Given two boxes, an inner box enclosed within a bounding |
| 377 |
// box, this routine determines all the sides for which the |
| 378 |
// inner box is strictly contained with the bounding box, |
| 379 |
// and adds an appropriate entry to a list of bounds. Then |
| 380 |
// we allocate storage for the final list of bounds, and return |
| 381 |
// the resulting list and its size. |
| 382 |
//---------------------------------------------------------------------- |
| 383 |
|
| 384 |
void annBox2Bnds( // convert inner box to bounds |
| 385 |
const ANNorthRect &inner_box, // inner box |
| 386 |
const ANNorthRect &bnd_box, // enclosing box |
| 387 |
int dim, // dimension of space |
| 388 |
int &n_bnds, // number of bounds (returned) |
| 389 |
ANNorthHSArray &bnds) // bounds array (returned) |
| 390 |
{ |
| 391 |
int i; |
| 392 |
n_bnds = 0; // count number of bounds |
| 393 |
for (i = 0; i < dim; i++) { |
| 394 |
if (inner_box.lo[i] > bnd_box.lo[i]) // low bound is inside |
| 395 |
n_bnds++; |
| 396 |
if (inner_box.hi[i] < bnd_box.hi[i]) // high bound is inside |
| 397 |
n_bnds++; |
| 398 |
} |
| 399 |
|
| 400 |
bnds = new ANNorthHalfSpace[n_bnds]; // allocate appropriate size |
| 401 |
|
| 402 |
int j = 0; |
| 403 |
for (i = 0; i < dim; i++) { // fill the array |
| 404 |
if (inner_box.lo[i] > bnd_box.lo[i]) { |
| 405 |
bnds[j].cd = i; |
| 406 |
bnds[j].cv = inner_box.lo[i]; |
| 407 |
bnds[j].sd = +1; |
| 408 |
j++; |
| 409 |
} |
| 410 |
if (inner_box.hi[i] < bnd_box.hi[i]) { |
| 411 |
bnds[j].cd = i; |
| 412 |
bnds[j].cv = inner_box.hi[i]; |
| 413 |
bnds[j].sd = -1; |
| 414 |
j++; |
| 415 |
} |
| 416 |
} |
| 417 |
} |
| 418 |
|
| 419 |
//---------------------------------------------------------------------- |
| 420 |
// annBnds2Box - convert list of bounds to bounding box |
| 421 |
// Given an enclosing box and a list of bounds, this routine |
| 422 |
// computes the corresponding inner box. It is assumed that |
| 423 |
// the box points have been allocated already. |
| 424 |
//---------------------------------------------------------------------- |
| 425 |
|
| 426 |
void annBnds2Box( |
| 427 |
const ANNorthRect &bnd_box, // enclosing box |
| 428 |
int dim, // dimension of space |
| 429 |
int n_bnds, // number of bounds |
| 430 |
ANNorthHSArray bnds, // bounds array |
| 431 |
ANNorthRect &inner_box) // inner box (returned) |
| 432 |
{ |
| 433 |
annAssignRect(dim, inner_box, bnd_box); // copy bounding box to inner |
| 434 |
|
| 435 |
for (int i = 0; i < n_bnds; i++) { |
| 436 |
bnds[i].project(inner_box.lo); // project each endpoint |
| 437 |
bnds[i].project(inner_box.hi); |
| 438 |
} |
| 439 |
} |