Debian Codegraph @

src/kd_util.cpp

raw
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 }