Debian Codegraph @

packages/seacas/applications/mapvar-kd/optkd.c

raw
1 /*
2 * Copyright(C) 1999-2021, 2023 National Technology & Engineering Solutions
3 * of Sandia, LLC (NTESS). Under the terms of Contract DE-NA0003525 with
4 * NTESS, the U.S. Government retains certain rights in this software.
5 *
6 * See packages/seacas/LICENSE for details
7 */
8 /**************************************************************************/
9 /* Program to perform orthogonal range searches and nearest neighbor */
10 /* queries in a more sophisticated k-d tree. In this implementation the, */
11 /* nodes on any given level of the tree do not have the same */
12 /* discriminating dimension as the discrimiator is chosen based on the */
13 /* dimension with the "maxspead." */
14 /* */
15 /* References: J.H. Friedman, J.L. Bentley, R.A. Finkel "An Algorithm */
16 /* for Finding Best Matches in Logarithmic Expected Time." */
17 /* ACM Transactions on Mathematical Software, Vol 3 No. 3 Sept. 1977 */
18 /* pp. 209-226. */
19 /**************************************************************************/
20
21 /* Turn off assertions here since SEACAS compiles don't normally do it. */
22 #ifndef NDEBUG
23 #define NDEBUG
24 #endif
25
26 #if defined ADDC_
27 #define KDRECTQUERY kdrectquery_
28 #define KDKILLTREE kdkilltree_
29 #define KDBUILDTREE kdbuildtree_
30 #else
31 #define KDRECTQUERY kdrectquery
32 #define KDKILLTREE kdkilltree
33 #define KDBUILDTREE kdbuildtree
34 #endif
35
36 #include <float.h>
37 #include <math.h>
38 #include <stdbool.h>
39 #include <stdio.h>
40 #include <stdlib.h>
41 #include <string.h>
42
43 #include <assert.h>
44
45 #if defined(Build64)
46 #define real double
47 #else
48 #define real float
49 #endif
50
51 #include "optkd.h"
52
53 /* Used to create a new tree in the k-d tree */
54 #define TESTTREE(PP) ((PP) = (optkdNode *)malloc(sizeof(optkdNode)))
55 #define NEWTREE(PP) \
56 if (TESTTREE(PP) == NULL) { \
57 printf("memory error\n"); \
58 return 0; \
59 }
60
61 static optkdNode *Root = NULL;
62 static int *perm = NULL; /* permutation array */
63
64 /***************************************************************************/
65 /* Makes the perm partition the array Values along the element k. */
66 /* Adapted from Sedgewick's Algorithms in C (p. 128) */
67 /***************************************************************************/
68 #define max(a, b) ((a) > (b) ? (a) : (b))
69 #define min(a, b) ((a) < (b) ? (a) : (b))
70 #define sign(x) ((x) >= 0 ? 1 : -1)
71
72 /*
73 * Floyd and Rivest SELECT Algorithm (CACM Algorithm 489)
74 */
75 void rf_select(real *a, int count, int l, int r, int k, int discrim)
76 {
77 while (r > l) {
78 if (r - l > 600) {
79 /*
80 * Use SELECT recursively on a sample of size S to get an estimate
81 * for the (K-L+1)-th smallest element into A[K], biased slightly so
82 * that the (K-L+1)-th element is expected to lie in the smaller set
83 * after partitioning
84 */
85 int n = r - l + 1;
86 int i = k - l + 1;
87 real z = log(n);
88 int s = 0.5 * exp(2.0 * z / 3.0);
89 int sd = 0.1 * sqrt(z * s * (n - s) / n) * sign(i - n / 2.0);
90
91 int tmp = k - (i * s / n) + sd;
92 int ll = max(l, tmp);
93 tmp = k + (n - i) * s / n + sd;
94 int rr = min(r, tmp);
95 rf_select(a, count, ll, rr, k, discrim);
96 }
97 real t = a[discrim * count + perm[k]];
98
99 /* The following code partitions a[L:R] about T. It is similar to
100 PARTITION, but will run faster on most machines since subscript
101 range checking on I and J has been eliminated.
102 */
103 int i = l;
104 int j = r;
105 /* exchange(x[l], x[k]); */
106 int tmp = perm[l];
107 perm[l] = perm[k];
108 perm[k] = tmp;
109 if (a[discrim * count + perm[r]] > t) {
110 /* exchange(x[r], x[l]); */
111 tmp = perm[r];
112 perm[r] = perm[l];
113 perm[l] = tmp;
114 }
115 while (i < j) {
116 /* exchange(x[i], x[j]); */
117 tmp = perm[i];
118 perm[i] = perm[j];
119 perm[j] = tmp;
120 i = i + 1;
121 j = j - 1;
122 while (a[discrim * count + perm[i]] < t) {
123 i++;
124 }
125 while (a[discrim * count + perm[j]] > t) {
126 j--;
127 }
128 }
129 if (a[discrim * count + perm[l]] == t) {
130 /* exchange(x[l], x[j]); */
131 tmp = perm[l];
132 perm[l] = perm[j];
133 perm[j] = tmp;
134 }
135 else {
136 j++;
137 /* exchange(x[j], x[r]); */
138 tmp = perm[j];
139 perm[j] = perm[r];
140 perm[r] = tmp;
141 }
142 /* Now adjust L, R so they surround the subset containing the (k-l+1)-th element */
143 if (j <= k) {
144 l = j + 1;
145 }
146 if (k <= j) {
147 r = j - 1;
148 }
149 }
150 }
151
152 /****************************************************************************/
153 int findmaxspread(int l, int u, int dimension, real *points, int N)
154 {
155 int maxdim = 0;
156 real maxspread = -FLT_MAX;
157
158 for (int i = 0; i < dimension; i++) {
159 real max = -FLT_MAX;
160 real min = FLT_MAX;
161 for (int j = l; j <= u; j++) {
162 real val = points[N * i + perm[j]];
163 if (max < val) {
164 max = val;
165 }
166 if (min > val) {
167 min = val;
168 }
169 }
170 if (maxspread < fabs(max - min)) {
171 maxspread = fabs(max - min);
172 maxdim = i;
173 }
174 }
175 return (maxdim);
176 }
177
178 int findmaxvariance(int l, int u, int dimension, real *points, int N)
179 {
180 int maxdim = 0;
181 real max_var = 0.0;
182
183 for (int i = 0; i < dimension; i++) {
184 real mean = 0.0;
185 real variance = 0.0;
186 real count = 0.0;
187
188 for (int j = l; j <= u; j++) {
189 real val = points[N * i + perm[j]];
190 real prev_mean = mean;
191 count += 1.0;
192 mean += (val - prev_mean) / count;
193 variance += (val - prev_mean) * (val - mean);
194 }
195 /* True variance would be divided by (count-1.0) */
196
197 if (max_var < variance) {
198 max_var = variance;
199 maxdim = i;
200 }
201 }
202 return (maxdim);
203 }
204
205 /*******************************************************************************/
206 int check(real *points, int N, int l, int u, int m, int discrim)
207 {
208 for (int i = l; i < m; i++) {
209 assert(points[discrim * N + perm[i]] <= points[discrim * N + perm[m]]);
210 }
211 for (int i = m; i < u; i++) {
212 assert(points[discrim * N + perm[i]] >= points[discrim * N + perm[m]]);
213 }
214 return 1;
215 }
216
217 optkdNode *BuildkdTree(real *points, int N, int l, int u, int dimension)
218 {
219 optkdNode *p;
220
221 NEWTREE(p);
222 if (u - l + 1 <= BUCKETSIZE) {
223 p->bucket = 1;
224 p->lopt = l;
225 p->hipt = u;
226 p->loson = NULL;
227 p->hison = NULL;
228 }
229 else {
230 p->bucket = 0;
231
232 #if 1
233 p->discrim = findmaxspread(l, u, dimension, points, N);
234 #else
235 p->discrim = findmaxvariance(l, u, dimension, points, N);
236 #endif
237
238 int m = (l + u) / 2;
239
240 rf_select(points, N, l, u, m, p->discrim);
241 assert(check(points, N, l, u, m, p->discrim));
242
243 p->cutval = points[p->discrim * N + perm[m]];
244 p->loson = BuildkdTree(points, N, l, m, dimension);
245 p->hison = BuildkdTree(points, N, m + 1, u, dimension);
246 }
247 return (p);
248 }
249
250 /*******************************************************************************/
251 void KDBUILDTREE(real *points, int *numPoints, int *dimension)
252 {
253 /* initialize perm array */
254 assert(perm == NULL);
255 perm = (int *)malloc(*numPoints * sizeof(int));
256 assert(perm != NULL);
257 for (int j = 0; j < *numPoints; j++) {
258 perm[j] = j;
259 }
260 assert(Root == NULL);
261 Root = BuildkdTree(points, *numPoints, 0, *numPoints - 1, *dimension);
262 assert(Root != NULL);
263 }
264
265 /***************************************************************************/
266 void KillOptTree(optkdNode *P)
267 {
268 /* Kills a kd-tree to avoid memory holes. */
269 if (P == NULL) {
270 return;
271 } /* just to be sure */
272
273 if (P->loson != NULL) {
274 KillOptTree(P->loson);
275 }
276
277 if (P->hison != NULL) {
278 KillOptTree(P->hison);
279 }
280
281 free(P);
282 }
283
284 void KDKILLTREE(void)
285 {
286 if (perm != NULL) {
287 free(perm);
288 perm = NULL;
289 } /* free permutation array */
290
291 KillOptTree(Root);
292 Root = NULL;
293 }
294
295 /***************************************************************************/
296 /* Determines if the treenode P falls inside the rectangular query */
297 /* xmin,xmax. If so, adds the array index of the point to the found */
298 /* array. */
299 /***************************************************************************/
300 void optInRegion(optkdNode *P, int Dimension, real *Points, int N, real *xmin, real *xmax,
301 int *found, int *count)
302 {
303 for (int index = P->lopt; index <= P->hipt; index++) {
304 bool InsideRange = true;
305
306 if (Dimension == 3) {
307 int inval = perm[index];
308 if (Points[0 * N + inval] < xmin[0] || Points[0 * N + inval] > xmax[0] ||
309 Points[1 * N + inval] < xmin[1] || Points[1 * N + inval] > xmax[1] ||
310 Points[2 * N + inval] < xmin[2] ||
311 Points[2 * N + inval] > xmax[2]) { /* P is not in the region */
312 InsideRange = false;
313 }
314 }
315 else {
316 for (int dc = 0; dc < Dimension; dc++) {
317 if (Points[dc * N + perm[index]] < xmin[dc] ||
318 Points[dc * N + perm[index]] > xmax[dc]) { /* P is not in the region */
319 InsideRange = false;
320 break;
321 }
322 }
323 }
324 if (InsideRange) {
325 found[(*count)++] = perm[index] + 1;
326 }
327 }
328 }
329
330 /***************************************************************************/
331 /* Adds the array index of each point in the bucket the point to the found */
332 /* array. There is no need to check if the points are in it because we */
333 /* have proven so already. */
334 /***************************************************************************/
335 void optAddRegion(optkdNode *P, int Dimension, int *found, int *count)
336 {
337 for (int index = P->lopt; index <= P->hipt; index++) {
338 found[(*count)++] = perm[index] + 1;
339 }
340 }
341
342 /***************************************************************************/
343 /* Returns true iff the hyper-rectangle defined by bounds array B */
344 /* intersects the rectangular query xmin,xmax. */
345 /***************************************************************************/
346 int optBoundsIntersectRegion(real *B, real *xmin, real *xmax, int Dimension)
347 {
348 if (Dimension == 3) {
349 if (B[0] > xmax[0] || B[1] < xmin[0] || B[2] > xmax[1] || B[3] < xmin[1] || B[4] > xmax[2] ||
350 B[5] < xmin[2]) {
351 return 0;
352 }
353 }
354 else {
355 int dc;
356
357 for (dc = 0; dc < Dimension; dc++) {
358 if (B[2 * dc] > xmax[dc] || B[2 * dc + 1] < xmin[dc]) {
359 return (0);
360 }
361 }
362 }
363 return (1);
364 }
365
366 /***************************************************************************/
367 /* Returns true iff the hyper-rectangle defined by bounds array B */
368 /* is completely contained inside the rectangular query xmin,xmax. */
369 /***************************************************************************/
370 int optBoundsContainsRegion(real *B, real *xmin, real *xmax, int Dimension)
371 {
372 if (Dimension == 3) {
373 if (xmin[0] <= B[0] && xmax[0] >= B[1] && xmin[1] <= B[2] && xmax[1] >= B[3] &&
374 xmin[2] <= B[4] && xmax[2] >= B[5]) {
375 return 1;
376 }
377 else {
378 return 0;
379 }
380 }
381 else {
382 int dc;
383
384 for (dc = 0; dc < Dimension; dc++) {
385 if (!(B[2 * dc] >= xmin[dc] && B[2 * dc + 1] <= xmax[dc])) {
386 return (0);
387 }
388 }
389 }
390 return (1);
391 }
392
393 #define MAX_DEPTH 60
394 /***************************************************************************/
395 void optRangeSearch(optkdNode *P, real *Points, int N, int Dimension, real *xmin, real *xmax,
396 real *B, int *found, int *count, int depth)
397 {
398 static real BLow[MAX_DEPTH][6];
399 static real BHigh[MAX_DEPTH][6];
400
401 if (depth >= MAX_DEPTH) {
402 fprintf(stderr, "Internal Error in optRangeSearch -- recursion depth too large.\n");
403 abort();
404 }
405
406 if (P == NULL) {
407 fprintf(stderr, "Internal Error in optRangeSearch -- P is NULL.\n");
408 abort();
409 }
410
411 if (P->bucket) {
412 if (optBoundsContainsRegion(B, xmin, xmax, Dimension)) {
413 optAddRegion(P, Dimension, found, count);
414 }
415 else {
416 optInRegion(P, Dimension, Points, N, xmin, xmax, found, count);
417 }
418 return;
419 }
420
421 /* Claim: P is not a bucket node */
422 int disc = P->discrim;
423 /* copy the region B into BLow, BHigh */
424
425 if (Dimension == 3) {
426 memcpy(&BLow[depth][0], &B[0], 6 * sizeof(real));
427 memcpy(&BHigh[depth][0], &B[0], 6 * sizeof(real));
428 }
429 else {
430 memcpy(&BLow[depth][0], &B[0], 2 * Dimension * sizeof(real));
431 memcpy(&BHigh[depth][0], &B[0], 2 * Dimension * sizeof(real));
432 }
433
434 /* Improve the Bounds for the subtrees */
435 BLow[depth][2 * disc + 1] = P->cutval;
436 BHigh[depth][2 * disc] = P->cutval;
437
438 if (optBoundsIntersectRegion(&BLow[depth][0], xmin, xmax, Dimension)) {
439 optRangeSearch(P->loson, Points, N, Dimension, xmin, xmax, &BLow[depth][0], found, count,
440 depth + 1);
441 }
442 if (optBoundsIntersectRegion(&BHigh[depth][0], xmin, xmax, Dimension)) {
443 optRangeSearch(P->hison, Points, N, Dimension, xmin, xmax, &BHigh[depth][0], found, count,
444 depth + 1);
445 }
446 }
447
448 /***************************************************************************/
449 void KDRECTQUERY(real *Points, int *N, int *Dimension, real *xmin, real *xmax, int *found,
450 int *count)
451 {
452 real B[6] = {-FLT_MAX, FLT_MAX, -FLT_MAX, FLT_MAX, -FLT_MAX, FLT_MAX};
453 *count = 0;
454 optRangeSearch(Root, Points, *N, *Dimension, xmin, xmax, B, found, count, 0);
455 }