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