1 /*
2 # This file is part of libkd.
3 # Licensed under a 3-clause BSD style license - see LICENSE
4 */
5
6 #include <assert.h>
7 #include <stdlib.h>
8 #include <math.h>
9 #include <string.h>
10
11 #include "os-features.h"
12 #include "kdtree.h"
13 #include "kdtree_internal.h"
14 #include "kdtree_mem.h"
15 #include "keywords.h"
16 #include "errors.h"
17
18 #define KDTREE_MAX_RESULTS 1000
19 #define KDTREE_MAX_DIM 100
20
21 #define WARNING(x, ...) fprintf(stderr, x, ## __VA_ARGS__)
22
23 #define MANGLE(x) KDMANGLE(x, ETYPE, DTYPE, TTYPE)
24
25 /*
26 The "external" type is the data type that the outside world works in.
27
28 The "data" type is the type in which we store the points.
29
30 The "tree" type is the type in which we store the bounding boxes or splitting planes.
31
32 Recall that etype >= dtype >= ttype:
33 . (etype >= dtype: because there's no point storing more precision than needed by the
34 . outside world;
35 . dtype >= ttype: because there's no point keeping more precision in the splitting
36 . plane than exists in the data.)
37
38
39 The following will be defined:
40
41 - etype: typedef, the "external" type.
42 - ETYPE_INTEGER: 1 if the "external" space is integral (not floating-point)
43 - ETYPE_MIN
44 - ETYPE_MAX: the limits of the "external" space.
45 - ETYPE: the "external" type, in a form that the preprocessor can make C
46 . identifiers out of it: eg 'd' for double; use this to get to the
47 . particular data type in the kdtree unions:
48 . kdtree_qres_t* query_results = ...;
49 . etype* mydata = query_results.results.ETYPE;
50
51 - dtype: typedef, the "data" type.
52 - DTYPE_INTEGER: 1 if the "data" space is integral (not floating-point)
53 - DTYPE_DOUBLE: 1 if the "data" space has type 'double'.
54 - DTYPE_MIN
55 - DTYPE_MAX: the limits of the "data" space.
56 - DTYPE_KDT_DATA: eg KDT_DATA_DOUBLE
57 - DTYPE: the "data" type, in a form that the preprocessor can make C
58 . identifiers out of it: eg 'd' for double:
59 . dtype* mydata = kd->data.DTYPE;
60
61 - ttype: typedef, the "tree" type.
62 - TTYPE_INTEGER: 1 if the "tree" space is integral (not floating-point)
63 - TTYPE_MIN
64 - TTYPE_MAX: the limits of the "tree" space.
65 - TTYPE_SQRT_MAX: the square root of the maximum value of the "tree" space.
66 - TTYPE: the "tree" type, in a form that the preprocessor can make C
67 . identifiers out of it: eg 'd' for double.
68 . ttype* mybb = kd->bb.TTYPE;
69
70 - bigttype: typedef, a type that can hold a "ttype" squared.
71 - BIGTTYPE: #define the bigttype; used for "STRINGIFY" macro.
72 - BIGTTYPE_MAX: maximum value of a 'bigttype'.
73
74
75 - EQUAL_ED: 1 if "external" and "data" spaces are the same.
76 - EQUAL_DT: 1 if "data" and "tree" spaces are the same.
77 - EQUAL_ET: 1 if "external" and "tree" spaces are the same; implies EQUAL_ED && EQUAL_DT.
78
79
80 - POINT_ED(kd, d, c, func): d: dimension;
81 . c: coordinate in that dimension,
82 . func: function to apply (may be empty)
83 . Converts a coordinate from "external" to "data" space.
84 . eg POINT_ED(kd, 0, 0.1, floor);
85 . POINT_ED(kd, 1, 0.0, );
86
87 - POINT_DT(kd, d, c, func):
88 . Converts a coordinate from "data" to "tree" space.
89
90 - POINT_ET(kd, d, c, func):
91 . Converts a coordinate from "external" to "tree" space.
92
93 - POINT_TD(kd, d, c):
94 . Converts a coordinate from "tree" to "data" space.
95
96 - POINT_DE(kd, d, c):
97 . Converts a coordinate from "data" to "external" space.
98
99 - POINT_TE(kd, d, c):
100 . Converts a coordinate from "tree" to "external" space.
101
102
103 - DIST_ED(kd, dist, func):
104 - DIST2_ED(kd, dist2, func):
105 . Convert distance or distance-squared from "external" to "data" space,
106 . optionally applying a function to the result.
107
108 - DIST_DT(kd, dist, func):
109 - DIST2_DT(kd, dist2, func):
110 . Convert distance or distance-squared from "data" to "tree" space.
111
112 - DIST_ET(kd, dist, func):
113 - DIST2_ET(kd, dist2, func):
114 . Convert distance or distance-squared from "external" to "tree" space.
115
116 - DIST_TD(kd, dist):
117 - DIST2_TD(kd, dist2):
118 . Convert distance or distance-squared from "tree" to "data" space.
119
120 - DIST_DE(kd, dist):
121 - DIST2_DE(kd, dist2):
122 . Convert distance or distance-squared from "data" to "external" space.
123
124 - DIST_TE(kd, dist):
125 - DIST2_TE(kd, dist2):
126 . Convert distance or distance-squared from "tree" to "external" space.
127 */
128
129 // Which function do we use for rounding?
130 #define KD_ROUND rint
131
132 // Get the low corner of the bounding box
133 #define LOW_HR( kd, D, i) ((kd)->bb.TTYPE + (2*(size_t)(i)*(size_t)(D)))
134
135 // Get the high corner of the bounding box
136 #define HIGH_HR(kd, D, i) ((kd)->bb.TTYPE + ((2*(size_t)(i)+1)*(size_t)(D)))
137
138 // Get the splitting-plane position
139 #define KD_SPLIT(kd, i) ((kd)->split.TTYPE + (size_t)(i))
140
141 // Get a pointer to the 'i'-th data point.
142 #define KD_DATA(kd, D, i) ((kd)->data.DTYPE + ((size_t)(D)*(size_t)(i)))
143
144 // Dereference an array of dimension D at element i, dimension d.
145 #define KD_ARRAY_VAL(arr, D, i, d) ((arr)[(((size_t)(D)*(size_t)(i)) + (size_t)(d))])
146
147 #define KD_ARRAY_REF(arr, D, i, d) ((arr) + (((size_t)(D)*(size_t)(i)) + (size_t)(d)))
148
149 // Get the 'i'-th element of the permutation vector, or 'i' if there is no permutation vector.
150 #define KD_PERM(kd, i) ((kd)->perm ? (kd)->perm[i] : i)
151
152 // Get the dimension of this tree.
153 #if defined(KD_DIM)
154 #define DIMENSION(kd) (KD_DIM)
155 #else
156 #define DIMENSION(kd) (kd->ndim)
157 #endif
158
159 // Get the size of a single point in the tree.
160 #define SIZEOF_PT(kd) (sizeof(dtype)*DIMENSION(kd))
161
162 // compatibility macros (for DEPRECATED old-fashioned trees)
163 #define COMPAT_NODE_SIZE(kd) (sizeof(kdtree_node_t) + (SIZEOF_PT(kd) * 2))
164 #define COMPAT_HIGH_HR(kd, i) ((ttype*)(((char*)(kd)->nodes) \
165 + COMPAT_NODE_SIZE(kd)*(i) \
166 + sizeof(kdtree_node_t) \
167 + SIZEOF_PT(kd)))
168 #define COMPAT_LOW_HR(kd, i) ((ttype*) (((char*)(kd)->nodes) \
169 + COMPAT_NODE_SIZE(kd)*(i) \
170 + sizeof(kdtree_node_t)))
171
172 // "converted" <-> "dtype" conversion.
173 #define POINT_CTOR(kd, d, val) (((val) - ((kd)->minval[(d)])) * (kd)->scale)
174 #define POINT_RTOC(kd, d, val) (((val) * ((kd)->invscale)) + (kd)->minval[d])
175
176 #define DIST_CTOR(kd, dist) ((dist) * (kd)->scale)
177 #define DIST2_CTOR(kd, dist2) ((dist2) * (kd)->scale * (kd)->scale)
178 #define DIST2_RTOC(kd, dist2) ((dist2) * (kd)->invscale * (kd)->invscale)
179
180
181 // this must be at least as big as the biggest integer TTYPE.
182 typedef u32 bigint;
183
184 #define ACTUAL_STRINGIFY(x) (#x)
185 #define STRINGIFY(x) ACTUAL_STRINGIFY(x)
186
187 #define MYGLUE2(a, b) a ## b
188 #define DIST_FUNC_MANGLE(x, suffix) MYGLUE2(x, suffix)
189
190 /*
191 static void split_dim_and_value(kdtree_t* kd, int node,
192 uint8_t* splitdim, ttype* splitval) {
193 if (kd->splitdim) {
194 *splitdim = kd->splitdim[node];
195 *splitval = *KD_SPLIT(kd, node);
196 } else {
197 bigint tmpsplit = *KD_SPLIT(kd, node);
198 *splitdim = (uint8_t)(tmpsplit & kd->dimmask);
199 *splitval = (ttype)(tmpsplit & kd->splitmask);
200 }
201 }
202 */
203
204 /* min/maxdist functions. */
205 #define CAN_OVERFLOW 0
206 #undef DELTAMAX
207
208 #define PTYPE etype
209 #define DISTTYPE double
210 #define FUNC_SUFFIX
211 #include "kdtree_internal_dists.c"
212 #undef PTYPE
213 #undef DISTTYPE
214 #undef FUNC_SUFFIX
215
216 #undef CAN_OVERFLOW
217 #define CAN_OVERFLOW 1
218
219 #define PTYPE ttype
220 #define DISTTYPE ttype
221 #define FUNC_SUFFIX _ttype
222 #define DELTAMAX TTYPE_SQRT_MAX
223 #include "kdtree_internal_dists.c"
224 #undef PTYPE
225 #undef DISTTYPE
226 #undef FUNC_SUFFIX
227 #undef DELTAMAX
228
229 #define PTYPE ttype
230 #define DISTTYPE bigttype
231 #define FUNC_SUFFIX _bigttype
232 #undef DELTAMAX
233 #include "kdtree_internal_dists.c"
234 #undef PTYPE
235 #undef DISTTYPE
236 #undef FUNC_SUFFIX
237
238 #undef CAN_OVERFLOW
239
240
241
242 void MANGLE(kdtree_update_funcs)(kdtree_t* kd);
243
bboxes(const kdtree_t * kd,int node,ttype ** p_tlo,ttype ** p_thi,int D)244 static anbool bboxes(const kdtree_t* kd, int node,
245 ttype** p_tlo, ttype** p_thi, int D) {
246 if (kd->bb.any) {
247 // bb trees
248 *p_tlo = LOW_HR(kd, D, node);
249 *p_thi = HIGH_HR(kd, D, node);
250 return TRUE;
251 } else {
252 return FALSE;
253 }
254 }
255
dist2(const kdtree_t * kd,const etype * q,const dtype * p,int D)256 static inline double dist2(const kdtree_t* kd, const etype* q, const dtype* p, int D) {
257 int d;
258 double d2 = 0.0;
259 #if defined(KD_DIM)
260 D = KD_DIM;
261 #endif
262 for (d=0; d<D; d++) {
263 etype pp = POINT_DE(kd, d, p[d]);
264 double delta;
265 if (TTYPE_INTEGER) {
266 if (q[d] > pp)
267 delta = q[d] - pp;
268 else
269 delta = pp - q[d];
270 } else {
271 delta = q[d] - pp;
272 }
273 d2 += delta * delta;
274 }
275 return d2;
276 }
277
dist2_bailout(const kdtree_t * kd,const etype * q,const dtype * p,int D,double maxd2,anbool * bailedout,double * d2res)278 static inline void dist2_bailout(const kdtree_t* kd, const etype* q, const dtype* p,
279 int D, double maxd2, anbool* bailedout, double* d2res) {
280 int d;
281 double d2 = 0.0;
282 #if defined(KD_DIM)
283 D = KD_DIM;
284 #endif
285 for (d=0; d<D; d++) {
286 double delta;
287 etype pp = POINT_DE(kd, d, p[d]);
288 // But wait... "q" and "pp" are both "etype"...
289 /*
290 if (TTYPE_INTEGER) {
291 if (q[d] > pp)
292 delta = q[d] - pp;
293 else
294 delta = pp - q[d];
295 } else {
296 delta = q[d] - pp;
297 }
298 */
299 delta = q[d] - pp;
300 d2 += delta * delta;
301 if (d2 > maxd2) {
302 *bailedout = TRUE;
303 return;
304 }
305 }
306 *d2res = d2;
307 }
308
ddist2_bailout(const kdtree_t * kd,const dtype * q,const dtype * p,int D,bigttype maxd2,anbool * bailedout,bigttype * d2res)309 static inline void ddist2_bailout(const kdtree_t* kd,
310 const dtype* q, const dtype* p,
311 int D, bigttype maxd2, anbool* bailedout,
312 bigttype* d2res) {
313 int d;
314 bigttype d2 = 0;
315 #if defined(KD_DIM)
316 D = KD_DIM;
317 #endif
318 for (d=0; d<D; d++) {
319 dtype delta;
320 if (q[d] > p[d])
321 delta = q[d] - p[d];
322 else
323 delta = p[d] - q[d];
324
325 d2 += (bigttype)delta * (bigttype)delta;
326 if (d2 > maxd2) {
327 *bailedout = TRUE;
328 return;
329 }
330 }
331 *d2res = d2;
332 }
333
334
dist2_exceeds(const kdtree_t * kd,const etype * q,const dtype * p,int D,double maxd2)335 static inline anbool dist2_exceeds(const kdtree_t* kd, const etype* q, const dtype* p, int D, double maxd2) {
336 int d;
337 double d2 = 0.0;
338 #if defined(KD_DIM)
339 D = KD_DIM;
340 #endif
341 for (d=0; d<D; d++) {
342 double delta;
343 double pp = POINT_DE(kd, d, p[d]);
344 if (TTYPE_INTEGER) {
345 if (q[d] > pp)
346 delta = q[d] - pp;
347 else
348 delta = pp - q[d];
349 } else {
350 delta = q[d] - pp;
351 }
352 d2 += delta * delta;
353 if (d2 > maxd2)
354 return 1;
355 }
356 return 0;
357 }
358
bb_point_l1mindist_exceeds_ttype(ttype * lo,ttype * hi,ttype * query,int D,ttype maxl1,ttype maxlinf)359 static anbool bb_point_l1mindist_exceeds_ttype(ttype* lo, ttype* hi,
360 ttype* query, int D,
361 ttype maxl1, ttype maxlinf) {
362 ttype dist = 0;
363 ttype newdist;
364 ttype delta;
365 int d;
366 #if defined(KD_DIM)
367 D = KD_DIM;
368 #endif
369 for (d=0; d<D; d++) {
370 if (query[d] < lo[d])
371 delta = lo[d] - query[d];
372 else if (query[d] > hi[d])
373 delta = query[d] - hi[d];
374 else
375 continue;
376 if (delta > maxlinf) {
377 //printf("linf: %u > %u.\n", (unsigned int)delta, (unsigned int)maxlinf);
378 return TRUE;
379 }
380 newdist = dist + delta;
381 if (newdist < dist) {
382 // overflow!
383 return TRUE;
384 }
385 if (newdist > maxl1) {
386 //printf("l1: %u > %u\n", (unsigned int)newdist, (unsigned int)maxl1);
387 return TRUE;
388 }
389 dist = newdist;
390 }
391 return FALSE;
392 }
393
compute_splitbits(kdtree_t * kd)394 static void compute_splitbits(kdtree_t* kd) {
395 int D;
396 int bits;
397 u32 val;
398 D = kd->ndim;
399 bits = 0;
400 val = 1;
401 while (val < D) {
402 bits++;
403 val *= 2;
404 }
405 kd->dimmask = val - 1;
406 kd->dimbits = bits;
407 kd->splitmask = ~kd->dimmask;
408 }
409
410 /* Sorts results by kq->sdists */
kdtree_qsort_results(kdtree_qres_t * kq,int D)411 static int kdtree_qsort_results(kdtree_qres_t *kq, int D) {
412 int beg[KDTREE_MAX_RESULTS], end[KDTREE_MAX_RESULTS], i = 0, j, L, R;
413 static etype piv_vec[KDTREE_MAX_DIM];
414 unsigned int piv_perm;
415 double piv;
416
417 beg[0] = 0;
418 end[0] = kq->nres - 1;
419 while (i >= 0) {
420 L = beg[i];
421 R = end[i];
422 if (L < R) {
423 piv = kq->sdists[L];
424 for (j=0; j<D; j++)
425 piv_vec[j] = kq->results.ETYPE[L*D + j];
426 piv_perm = kq->inds[L];
427 if (i == KDTREE_MAX_RESULTS - 1) /* Sanity */
428 assert(0);
429 while (L < R) {
430 while (kq->sdists[R] >= piv && L < R)
431 R--;
432 if (L < R) {
433 for (j=0; j<D; j++)
434 kq->results.ETYPE[L*D + j] = kq->results.ETYPE[R*D + j];
435 kq->inds [L] = kq->inds [R];
436 kq->sdists[L] = kq->sdists[R];
437 L++;
438 }
439 while (kq->sdists[L] <= piv && L < R)
440 L++;
441 if (L < R) {
442 for (j=0; j<D; j++)
443 kq->results.ETYPE[R*D + j] = kq->results.ETYPE[L*D + j];
444 kq->inds [R] = kq->inds [L];
445 kq->sdists[R] = kq->sdists[L];
446 R--;
447 }
448 }
449 for (j=0; j<D; j++)
450 kq->results.ETYPE[D*L + j] = piv_vec[j];
451 kq->inds [L] = piv_perm;
452 kq->sdists[L] = piv;
453 beg[i + 1] = L + 1;
454 end[i + 1] = end[i];
455 end[i++] = L;
456 } else
457 i--;
458 }
459 return 1;
460 }
461
print_results(kdtree_qres_t * res,int D)462 static void print_results(kdtree_qres_t* res, int D) {
463 if (TRUE) {
464 int i, d;
465 printf("%i results.\n", res->nres);
466 for (i=0; i<res->nres; i++) {
467 printf(" ind %i", res->inds[i]);
468 if (res->sdists)
469 printf(", dist %g", res->sdists[i]);
470 if (res->results.any) {
471 printf(", pt [ ");
472 for (d=0; d<D; d++)
473 printf("%g ", res->results.ETYPE[i*D + d]);
474 printf("]");
475 }
476 printf("\n");
477 }
478 printf("\n");
479 }
480 }
481
482 static
resize_results(kdtree_qres_t * res,int newsize,int D,anbool do_dists,anbool do_points)483 anbool resize_results(kdtree_qres_t* res, int newsize, int D,
484 anbool do_dists, anbool do_points) {
485
486 if (FALSE) {
487 printf("resize results: before:\n");
488 print_results(res, D);
489 }
490
491 if (do_dists)
492 res->sdists = REALLOC(res->sdists , newsize * sizeof(double));
493 if (do_points)
494 res->results.any = REALLOC(res->results.any, newsize * D * sizeof(etype));
495 res->inds = REALLOC(res->inds, newsize * sizeof(u32));
496 if (newsize && (!res->results.any || (do_dists && !res->sdists) || !res->inds))
497 SYSERROR("Failed to resize kdtree results arrays");
498 res->capacity = newsize;
499
500 if (FALSE) {
501 printf("resize results: after:\n");
502 print_results(res, D);
503 }
504
505 return TRUE;
506 }
507
508 static
add_result(const kdtree_t * kd,kdtree_qres_t * res,double sdist,unsigned int ind,const dtype * pt,int D,anbool do_dists,anbool do_points)509 anbool add_result(const kdtree_t* kd, kdtree_qres_t* res, double sdist,
510 unsigned int ind, const dtype* pt,
511 int D, anbool do_dists, anbool do_points) {
512
513 if (FALSE) {
514 printf("Before adding new result:\n");
515 print_results(res, D);
516 }
517
518 if (do_dists)
519 res->sdists[res->nres] = sdist;
520 res->inds [res->nres] = ind;
521 if (do_points) {
522 int d;
523 for (d=0; d<D; d++)
524 res->results.ETYPE[res->nres * D + d] = POINT_DE(kd, d, pt[d]);
525 }
526 res->nres++;
527 if (res->nres == res->capacity) {
528 // enlarge arrays.
529 return resize_results(res, res->capacity * 2, D, do_dists, do_points);
530 }
531
532 if (FALSE) {
533 printf("After adding new result:\n");
534 print_results(res, D);
535 }
536
537 return TRUE;
538 }
539
540 /*
541 Can the query be represented as a ttype?
542
543 If so, place the converted value in "tquery".
544 */
ttype_query(const kdtree_t * kd,const etype * query,ttype * tquery)545 static anbool ttype_query(const kdtree_t* kd, const etype* query, ttype* tquery) {
546 etype val;
547 int d, D=kd->ndim;
548 for (d=0; d<D; d++) {
549 val = POINT_ET(kd, d, query[d], );
550 if (val < TTYPE_MIN || val > TTYPE_MAX)
551 return FALSE;
552 tquery[d] = (ttype)val;
553 }
554 return TRUE;
555 }
556
MANGLE(kdtree_get_splitval)557 double MANGLE(kdtree_get_splitval)(const kdtree_t* kd, int nodeid) {
558 Unused int dim;
559 ttype split = *KD_SPLIT(kd, nodeid);
560 if (EQUAL_ET) {
561 return split;
562 }
563 if (!kd->splitdim && TTYPE_INTEGER) {
564 bigint tmpsplit = split;
565 dim = tmpsplit & kd->dimmask;
566 return POINT_TE(kd, dim, tmpsplit & kd->splitmask);
567 } else {
568 dim = kd->splitdim[nodeid];
569 }
570 return POINT_TE(kd, dim, split);
571 }
572
573
kdtree_nn_bb(const kdtree_t * kd,const etype * query,double * p_bestd2,int * p_ibest)574 static void kdtree_nn_bb(const kdtree_t* kd, const etype* query,
575 double* p_bestd2, int* p_ibest) {
576 int nodestack[100];
577 double dist2stack[100];
578 int stackpos = 0;
579 int D = (kd ? kd->ndim : 0);
580 anbool use_tquery = FALSE;
581 anbool use_tmath = FALSE;
582 anbool use_bigtmath = FALSE;
583 #ifndef _MSC_VER //# Modified by Robert Lancaster for the SexySolver Internal Library
584 ttype tquery[D];
585 #else
586 ttype *tquery = (ttype*) malloc(sizeof(ttype)*D);
587 #endif
588 double bestd2 = *p_bestd2;
589 int ibest = *p_ibest;
590 ttype tl2 = 0;
591 bigttype bigtl2 = 0;
592
593 #if defined(KD_DIM)
594 assert(kd->ndim == KD_DIM);
595 D = KD_DIM;
596 #else
597 D = kd->ndim;
598 #endif
599
600 if (TTYPE_INTEGER) {
601 use_tquery = ttype_query(kd, query, tquery);
602 }
603 if (TTYPE_INTEGER && use_tquery) {
604 double dtl2 = DIST2_ET(kd, bestd2, );
605 if (dtl2 < TTYPE_MAX) {
606 use_tmath = TRUE;
607 } else if (dtl2 < BIGTTYPE_MAX) {
608 use_bigtmath = TRUE;
609 }
610 bigtl2 = ceil(dtl2);
611 tl2 = bigtl2;
612 }
613
614 // queue root.
615 nodestack[0] = 0;
616 dist2stack[0] = 0.0;
617 if (kd->fun.nn_enqueue)
618 kd->fun.nn_enqueue(kd, 0, 1);
619
620 while (stackpos >= 0) {
621 int nodeid;
622 int i;
623 int L, R;
624 ttype *tlo=NULL, *thi=NULL;
625 int child;
626 double childd2[2];
627 double firstd2, secondd2;
628 int firstid, secondid;
629
630 if (dist2stack[stackpos] > bestd2) {
631 // pruned!
632 if (kd->fun.nn_prune)
633 kd->fun.nn_prune(kd, nodestack[stackpos], dist2stack[stackpos], bestd2, 1);
634 stackpos--;
635 continue;
636 }
637 nodeid = nodestack[stackpos];
638 stackpos--;
639 if (kd->fun.nn_explore)
640 kd->fun.nn_explore(kd, nodeid, dist2stack[stackpos+1], bestd2);
641
642 if (KD_IS_LEAF(kd, nodeid)) {
643 // Back when leaf nodes didn't have BBoxes:
644 //|| KD_IS_LEAF(kd, KD_CHILD_LEFT(nodeid)))
645 dtype* data;
646 L = kdtree_left(kd, nodeid);
647 R = kdtree_right(kd, nodeid);
648 for (i=L; i<=R; i++) {
649 anbool bailedout = FALSE;
650 double dsqd;
651 if (kd->fun.nn_point)
652 kd->fun.nn_point(kd, nodeid, i);
653 data = KD_DATA(kd, D, i);
654 dist2_bailout(kd, query, data, D, bestd2, &bailedout, &dsqd);
655 if (bailedout)
656 continue;
657 // new best
658 ibest = i;
659 bestd2 = dsqd;
660 if (kd->fun.nn_new_best)
661 kd->fun.nn_new_best(kd, nodeid, i, bestd2);
662 }
663 continue;
664 }
665
666 childd2[0] = childd2[1] = HUGE_VAL;
667 for (child=0; child<2; child++) {
668 anbool bailed;
669 double dist2;
670 int childid = (child ? KD_CHILD_RIGHT(nodeid) : KD_CHILD_LEFT(nodeid));
671
672 bboxes(kd, childid, &tlo, &thi, D);
673
674 bailed = FALSE;
675 if (TTYPE_INTEGER && use_tmath) {
676 ttype newd2 = 0;
677 bb_point_mindist2_bailout_ttype(tlo, thi, tquery, D, tl2, &bailed, &newd2);
678 if (bailed) {
679 if (kd->fun.nn_prune)
680 kd->fun.nn_prune(kd, nodeid, newd2, bestd2, 2);
681 continue;
682 }
683 dist2 = DIST2_TE(kd, newd2);
684 } else if (TTYPE_INTEGER && use_bigtmath) {
685 bigttype newd2 = 0;
686 bb_point_mindist2_bailout_bigttype(tlo, thi, tquery, D, bigtl2, &bailed, &newd2);
687 if (bailed) {
688 if (kd->fun.nn_prune)
689 kd->fun.nn_prune(kd, nodeid, newd2, bestd2, 3);
690 continue;
691 }
692 dist2 = DIST2_TE(kd, newd2);
693 } else {
694 etype bblo, bbhi;
695 int d;
696 // this is just bb_point_mindist2_bailout...
697 dist2 = 0.0;
698 for (d=0; d<D; d++) {
699 bblo = POINT_TE(kd, d, tlo[d]);
700 if (query[d] < bblo) {
701 dist2 += (bblo - query[d])*(bblo - query[d]);
702 } else {
703 bbhi = POINT_TE(kd, d, thi[d]);
704 if (query[d] > bbhi) {
705 dist2 += (query[d] - bbhi)*(query[d] - bbhi);
706 } else
707 continue;
708 }
709 if (dist2 > bestd2) {
710 bailed = TRUE;
711 break;
712 }
713 }
714 if (bailed) {
715 if (kd->fun.nn_prune)
716 kd->fun.nn_prune(kd, childid, dist2, bestd2, 4);
717 continue;
718 }
719 }
720 childd2[child] = dist2;
721 }
722
723 if (childd2[0] <= childd2[1]) {
724 firstd2 = childd2[0];
725 secondd2 = childd2[1];
726 firstid = KD_CHILD_LEFT(nodeid);
727 secondid = KD_CHILD_RIGHT(nodeid);
728 } else {
729 firstd2 = childd2[1];
730 secondd2 = childd2[0];
731 firstid = KD_CHILD_RIGHT(nodeid);
732 secondid = KD_CHILD_LEFT(nodeid);
733 }
734
735 if (firstd2 == HUGE_VAL)
736 continue;
737
738 // it's a stack, so put the "second" one on first.
739 if (secondd2 != HUGE_VAL) {
740 stackpos++;
741 nodestack[stackpos] = secondid;
742 dist2stack[stackpos] = secondd2;
743 if (kd->fun.nn_enqueue)
744 kd->fun.nn_enqueue(kd, secondid, 2);
745 }
746
747 stackpos++;
748 nodestack[stackpos] = firstid;
749 dist2stack[stackpos] = firstd2;
750 if (kd->fun.nn_enqueue)
751 kd->fun.nn_enqueue(kd, firstid, 2);
752
753 }
754 *p_bestd2 = bestd2;
755 *p_ibest = ibest;
756
757 #ifdef _MSC_VER //# Modified by Robert Lancaster for the SexySolver Internal Library
758 free(tquery);
759 #endif
760 }
761
kdtree_nn_int_split(const kdtree_t * kd,const etype * query,const ttype * tquery,double * p_bestd2,int * p_ibest)762 static void kdtree_nn_int_split(const kdtree_t* kd, const etype* query,
763 const ttype* tquery,
764 double* p_bestd2, int* p_ibest) {
765 int nodestack[100];
766 ttype mindists[100];
767
768 int stackpos = 0;
769 int D = kd->ndim;
770
771 ttype closest_so_far;
772 bigttype closest2;
773
774 int ibest = -1;
775
776 dtype* data;
777 dtype* dquery = (dtype*)tquery;
778
779 /** FIXME **/
780 assert(sizeof(dtype) == sizeof(ttype));
781
782 {
783 double closest;
784 closest = DIST_ET(kd, sqrt(*p_bestd2), );
785 if (closest > TTYPE_MAX) {
786 closest_so_far = TTYPE_MAX;
787 closest2 = BIGTTYPE_MAX;
788 } else {
789 closest_so_far = ceil(closest);
790 closest2 = (bigttype)closest_so_far * (bigttype)closest_so_far;
791 }
792 }
793
794 // queue root.
795 nodestack[0] = 0;
796 mindists[0] = 0;
797
798 while (stackpos >= 0) {
799 int nodeid;
800 int i;
801 int dim = -1;
802 int L, R;
803 ttype split = 0;
804
805 if (mindists[stackpos] > closest_so_far) {
806 // pruned!
807 stackpos--;
808 continue;
809 }
810 nodeid = nodestack[stackpos];
811 stackpos--;
812
813 if (KD_IS_LEAF(kd, nodeid)) {
814 int oldbest = ibest;
815
816 L = kdtree_left(kd, nodeid);
817 R = kdtree_right(kd, nodeid);
818 for (i=L; i<=R; i++) {
819 anbool bailedout = FALSE;
820 bigttype dsqd;
821 data = KD_DATA(kd, D, i);
822 ddist2_bailout(kd, dquery, data, D, closest2, &bailedout, &dsqd);
823 if (bailedout)
824 continue;
825 // new best
826 ibest = i;
827 closest2 = dsqd;
828 }
829
830 if (oldbest != ibest) {
831 // FIXME - replace with int sqrt
832 closest_so_far = ceil(sqrt((double)closest2));
833 }
834 continue;
835 }
836
837 // split/dim trees
838 split = *KD_SPLIT(kd, nodeid);
839
840 if (kd->splitdim)
841 dim = kd->splitdim[nodeid];
842 else {
843 bigint tmpsplit;
844 tmpsplit = split;
845 dim = tmpsplit & kd->dimmask;
846 split = tmpsplit & kd->splitmask;
847 }
848
849
850 if (tquery[dim] < split) {
851 // query is on the "left" side of the split.
852 assert(query[dim] < POINT_TE(kd, dim, split));
853 // is the right child within range?
854 // look mum, no int overflow!
855 if (split - tquery[dim] <= closest_so_far) {
856 // visit right child - it is within range.
857 assert(POINT_TE(kd, dim, split) - query[dim] > 0.0);
858 //assert(POINT_TE(kd, dim, split) - query[dim] <= bestdist);
859 stackpos++;
860 nodestack[stackpos] = KD_CHILD_RIGHT(nodeid);
861 mindists[stackpos] = split - tquery[dim];
862 }
863 stackpos++;
864 nodestack[stackpos] = KD_CHILD_LEFT(nodeid);
865 mindists[stackpos] = 0;
866
867 } else {
868 // query is on "right" side.
869 assert(POINT_TE(kd, dim, split) <= query[dim]);
870 // is the left child within range?
871 if (tquery[dim] - split < closest_so_far) {
872 assert(query[dim] - POINT_TE(kd, dim, split) >= 0.0);
873 //assert(query[dim] - POINT_TE(kd, dim, split) < bestdist);
874 stackpos++;
875 nodestack[stackpos] = KD_CHILD_LEFT(nodeid);
876 mindists[stackpos] = tquery[dim] - split;
877 }
878 stackpos++;
879
880 nodestack[stackpos] = KD_CHILD_RIGHT(nodeid);
881 mindists[stackpos] = 0;
882 }
883 }
884 if (ibest != -1) {
885 //*p_bestd2 = DIST2_TE(kd, closest2);
886 // Recompute the d2 more precisely in "etype":
887 data = KD_DATA(kd, D, ibest);
888 *p_bestd2 = dist2(kd, query, data, D);
889 *p_ibest = ibest;
890 }
891 }
892
MANGLE(kdtree_nn)893 void MANGLE(kdtree_nn)(const kdtree_t* kd, const void* vquery,
894 double* p_bestd2, int* p_ibest) {
895 int nodestack[100];
896 double dist2stack[100];
897 int stackpos = 0;
898 int D = (kd ? kd->ndim : 0);
899
900 double bestd2 = *p_bestd2;
901 int ibest = *p_ibest;
902 const etype* query = vquery;
903
904 if (!kd) {
905 WARNING("kdtree_nn: null tree!\n");
906 return;
907 }
908
909 // Bounding boxes
910 if (!kd->split.any) {
911 kdtree_nn_bb(kd, query, p_bestd2, p_ibest);
912 return;
913 }
914
915 #if defined(KD_DIM)
916 assert(kd->ndim == KD_DIM);
917 D = KD_DIM;
918 #else
919 D = kd->ndim;
920 #endif
921
922 // Integers.
923 if (TTYPE_INTEGER) {
924
925 #ifndef _MSC_VER //# Modified by Robert Lancaster for the SexySolver Internal Library
926 ttype tquery[D];
927 #else
928 ttype *tquery = (ttype*) malloc(sizeof(ttype)*D);
929 #endif
930 if (ttype_query(kd, query, tquery)) {
931 kdtree_nn_int_split(kd, query, tquery, p_bestd2, p_ibest);
932 return;
933 }
934 #ifdef _MSC_VER //# Modified by Robert Lancaster for the SexySolver Internal Library
935 free(tquery);
936 #endif
937 }
938
939 // We got splitting planes, and the splits are either doubles, or ints
940 // but the query doesn't find into the integer range.
941
942 // queue root.
943 nodestack[0] = 0;
944 dist2stack[0] = 0.0;
945 if (kd->fun.nn_enqueue)
946 kd->fun.nn_enqueue(kd, 0, 1);
947
948 while (stackpos >= 0) {
949 int nodeid;
950 int i;
951 int dim = -1;
952 int L, R;
953 ttype split = 0;
954 double del;
955 etype rsplit;
956
957 int nearchild;
958 int farchild;
959 double fard2;
960
961 if (dist2stack[stackpos] > bestd2) {
962 // pruned!
963 if (kd->fun.nn_prune)
964 kd->fun.nn_prune(kd, nodestack[stackpos], dist2stack[stackpos], bestd2, 1);
965 stackpos--;
966 continue;
967 }
968 nodeid = nodestack[stackpos];
969 stackpos--;
970
971 if (kd->fun.nn_explore)
972 kd->fun.nn_explore(kd, nodeid, dist2stack[stackpos+1], bestd2);
973
974 if (KD_IS_LEAF(kd, nodeid)) {
975 dtype* data;
976 L = kdtree_left(kd, nodeid);
977 R = kdtree_right(kd, nodeid);
978 for (i=L; i<=R; i++) {
979 anbool bailedout = FALSE;
980 double dsqd;
981
982 if (kd->fun.nn_point)
983 kd->fun.nn_point(kd, nodeid, i);
984
985 data = KD_DATA(kd, D, i);
986 dist2_bailout(kd, query, data, D, bestd2, &bailedout, &dsqd);
987 if (bailedout)
988 continue;
989 // new best
990 ibest = i;
991 bestd2 = dsqd;
992
993 if (kd->fun.nn_new_best)
994 kd->fun.nn_new_best(kd, nodeid, i, bestd2);
995 }
996 continue;
997 }
998
999 // split/dim trees
1000 split = *KD_SPLIT(kd, nodeid);
1001 if (kd->splitdim) {
1002 dim = kd->splitdim[nodeid];
1003 } else {
1004 // packed int
1005 bigint tmpsplit = split;
1006 dim = tmpsplit & kd->dimmask;
1007 split = tmpsplit & kd->splitmask;
1008 }
1009 rsplit = POINT_TE(kd, dim, split);
1010 del = query[dim] - rsplit;
1011 fard2 = del*del;
1012 if (query[dim] < rsplit) {
1013 nearchild = KD_CHILD_LEFT (nodeid);
1014 farchild = KD_CHILD_RIGHT(nodeid);
1015 } else {
1016 nearchild = KD_CHILD_RIGHT(nodeid);
1017 farchild = KD_CHILD_LEFT (nodeid);
1018 }
1019
1020 if (fard2 <= bestd2) {
1021 // is the far child within range?
1022 stackpos++;
1023 nodestack[stackpos] = farchild;
1024 dist2stack[stackpos] = fard2;
1025 if (kd->fun.nn_enqueue)
1026 kd->fun.nn_enqueue(kd, farchild, 8);
1027 } else {
1028 if (kd->fun.nn_prune)
1029 kd->fun.nn_prune(kd, farchild, fard2, bestd2, 7);
1030 }
1031
1032 // stack near child.
1033 stackpos++;
1034 nodestack[stackpos] = nearchild;
1035 dist2stack[stackpos] = 0.0;
1036 if (kd->fun.nn_enqueue)
1037 kd->fun.nn_enqueue(kd, nearchild, 9);
1038 }
1039 *p_bestd2 = bestd2;
1040 *p_ibest = ibest;
1041 }
1042
1043
MANGLE(kdtree_rangesearch_options)1044 kdtree_qres_t* MANGLE(kdtree_rangesearch_options)
1045 (const kdtree_t* kd, kdtree_qres_t* res, const void* vquery,
1046 double maxd2, int options)
1047 {
1048 int nodestack[100];
1049 int stackpos = 0;
1050 int D = (kd ? kd->ndim : 0);
1051 anbool do_dists;
1052 anbool do_points = TRUE;
1053 anbool do_wholenode_check;
1054 double maxdist = 0.0;
1055 ttype tlinf = 0;
1056 ttype tl1 = 0;
1057 ttype tl2 = 0;
1058 bigttype bigtl2 = 0;
1059
1060 anbool use_tquery = FALSE;
1061 anbool use_tsplit = FALSE;
1062 anbool use_tmath = FALSE;
1063 anbool use_bigtmath = FALSE;
1064
1065 anbool do_precheck = FALSE;
1066 anbool do_l1precheck = FALSE;
1067
1068 anbool use_bboxes = FALSE;
1069 Unused anbool use_splits = FALSE;
1070
1071 double dtl1=0.0, dtl2=0.0, dtlinf=0.0;
1072
1073 const etype* query = vquery;
1074
1075 //dtype dquery[D];
1076
1077 #ifndef _MSC_VER //# Modified by Robert Lancaster for the SexySolver Internal Library
1078 ttype tquery[D];
1079 #else
1080 ttype *tquery = (ttype*) malloc(sizeof(ttype)*D);
1081 #endif
1082
1083 if (!kd || !query)
1084 return NULL;
1085 #if defined(KD_DIM)
1086 assert(kd->ndim == KD_DIM);
1087 D = KD_DIM;
1088 #else
1089 D = kd->ndim;
1090 #endif
1091
1092 if (options & KD_OPTIONS_SORT_DISTS)
1093 // gotta compute 'em if ya wanna sort 'em!
1094 options |= KD_OPTIONS_COMPUTE_DISTS;
1095 do_dists = options & KD_OPTIONS_COMPUTE_DISTS;
1096 do_wholenode_check = !(options & KD_OPTIONS_SMALL_RADIUS);
1097
1098 if ((options & KD_OPTIONS_SPLIT_PRECHECK) &&
1099 kd->bb.any && kd->splitdim) {
1100 do_precheck = TRUE;
1101 }
1102
1103 if ((options & KD_OPTIONS_L1_PRECHECK) &&
1104 kd->bb.any) {
1105 do_l1precheck = TRUE;
1106 }
1107
1108 if (!kd->split.any) {
1109 assert(kd->bb.any);
1110 use_bboxes = TRUE;
1111 } else {
1112 if (kd->bb.any) {
1113 // Got both BBoxes and Splits.
1114 if (options & KD_OPTIONS_USE_SPLIT) {
1115 use_splits = TRUE;
1116 } else {
1117 // Use bboxes by default.
1118 use_bboxes = TRUE;
1119 }
1120 } else {
1121 use_splits = TRUE;
1122 assert(kd->splitdim || TTYPE_INTEGER);
1123 }
1124 }
1125
1126 assert(use_splits || use_bboxes);
1127
1128 maxdist = sqrt(maxd2);
1129
1130 if (TTYPE_INTEGER &&
1131 (kd->split.any || do_precheck || do_l1precheck)) {
1132 use_tquery = ttype_query(kd, query, tquery);
1133 }
1134
1135 if (TTYPE_INTEGER && use_tquery) {
1136 dtl1 = DIST_ET(kd, maxdist * sqrt(D),);
1137 dtl2 = DIST2_ET(kd, maxd2, );
1138 dtlinf = DIST_ET(kd, maxdist, );
1139 tl1 = ceil(dtl1);
1140 tlinf = ceil(dtlinf);
1141 bigtl2 = ceil(dtl2);
1142 tl2 = bigtl2;
1143 }
1144
1145 use_tsplit = use_tquery && (dtlinf < TTYPE_MAX);
1146
1147 if (do_l1precheck)
1148 if (dtl1 > TTYPE_MAX) {
1149 //printf("L1 maxdist %g overflows ttype representation. L1 precheck disabled.\n", dtl1);
1150 do_l1precheck = FALSE;
1151 }
1152
1153 if (TTYPE_INTEGER && use_tquery && kd->bb.any) {
1154 if (dtl2 < TTYPE_MAX) {
1155 use_tmath = TRUE;
1156 /*
1157 printf("Using %s integer math.\n", STRINGIFY(TTYPE));
1158 printf("(tl2 = %u).\n", (unsigned int)tl2);
1159 */
1160 } else if (dtl2 < BIGTTYPE_MAX) {
1161 use_bigtmath = TRUE;
1162 } else {
1163 /*
1164 printf("L2 maxdist overflows u16 and u32 representation; not using int math. %g -> %g > %u\n",
1165 maxd2, dtl2, UINT32_MAX);
1166 */
1167 }
1168 if (use_bigtmath) {
1169 if (options & KD_OPTIONS_NO_BIG_INT_MATH)
1170 use_bigtmath = FALSE;
1171 else {
1172 /*
1173 printf("Using %s/%s integer math.\n", STRINGIFY(TTYPE), STRINGIFY(BIGTTYPE));
1174 printf("(bigtl2 = %llu).\n", (long long unsigned int)bigtl2);
1175 */
1176 }
1177 }
1178 }
1179
1180
1181 if (res) {
1182 if (!res->capacity) {
1183 resize_results(res, KDTREE_MAX_RESULTS, D, do_dists, do_points);
1184 } else {
1185 // call the resize routine just in case the old result struct was
1186 // from a tree of different type or dimensionality.
1187 resize_results(res, res->capacity, D, do_dists, do_points);
1188 }
1189 res->nres = 0;
1190 } else {
1191 res = CALLOC(1, sizeof(kdtree_qres_t));
1192 if (!res) {
1193 SYSERROR("Failed to allocate kdtree_qres_t struct");
1194 return NULL;
1195 }
1196 resize_results(res, KDTREE_MAX_RESULTS, D, do_dists, do_points);
1197 }
1198
1199 // queue root.
1200 nodestack[0] = 0;
1201
1202 while (stackpos >= 0) {
1203 int nodeid;
1204 int i;
1205 int dim = -1;
1206 int L, R;
1207 ttype split = 0;
1208 ttype *tlo=NULL, *thi=NULL;
1209
1210 nodeid = nodestack[stackpos];
1211 stackpos--;
1212
1213 if (KD_IS_LEAF(kd, nodeid)) {
1214 dtype* data;
1215 L = kdtree_left(kd, nodeid);
1216 R = kdtree_right(kd, nodeid);
1217
1218 if (do_dists) {
1219 for (i=L; i<=R; i++) {
1220 anbool bailedout = FALSE;
1221 double dsqd;
1222 data = KD_DATA(kd, D, i);
1223 // FIXME benchmark dist2 vs dist2_bailout.
1224
1225 // HACK - should do "use_dtype", just like "use_ttype".
1226 dist2_bailout(kd, query, data, D, maxd2, &bailedout, &dsqd);
1227 if (bailedout)
1228 continue;
1229 if (!add_result(kd, res, dsqd, KD_PERM(kd, i), data,
1230 D, do_dists, do_points))
1231 return NULL;
1232 }
1233 } else {
1234 for (i=L; i<=R; i++) {
1235 data = KD_DATA(kd, D, i);
1236 // HACK - should do "use_dtype", just like "use_ttype".
1237 if (dist2_exceeds(kd, query, data, D, maxd2))
1238 continue;
1239 if (!add_result(kd, res, HUGE_VAL, KD_PERM(kd, i), data,
1240 D, do_dists, do_points))
1241 return NULL;
1242 }
1243 }
1244 continue;
1245 }
1246
1247 if (kd->splitdim)
1248 dim = kd->splitdim[nodeid];
1249
1250 if (use_bboxes) {
1251 anbool wholenode = FALSE;
1252
1253 bboxes(kd, nodeid, &tlo, &thi, D);
1254 assert(tlo && thi);
1255
1256 if (do_precheck && nodeid) {
1257 anbool isleftchild = KD_IS_LEFT_CHILD(nodeid);
1258 // we need to use the dimension our _parent_ split on, not ours!
1259 int pdim;
1260 anbool cut;
1261 if (kd->splitdim)
1262 pdim = kd->splitdim[KD_PARENT(nodeid)];
1263 else {
1264 pdim = kd->split.TTYPE[KD_PARENT(nodeid)];
1265 pdim &= kd->dimmask;
1266 }
1267 if (TTYPE_INTEGER && use_tquery) {
1268 if (isleftchild)
1269 cut = ((tquery[pdim] > thi[pdim]) &&
1270 (tquery[pdim] - thi[pdim] > tlinf));
1271 else
1272 cut = ((tlo[pdim] > tquery[pdim]) &&
1273 (tlo[pdim] - tquery[pdim] > tlinf));
1274 } else {
1275 etype bb;
1276 if (isleftchild) {
1277 bb = POINT_TE(kd, pdim, thi[pdim]);
1278 cut = (query[pdim] - bb > maxdist);
1279 } else {
1280 bb = POINT_TE(kd, pdim, tlo[pdim]);
1281 cut = (bb - query[pdim] > maxdist);
1282 }
1283 }
1284 if (cut) {
1285 // precheck failed!
1286 //printf("precheck failed!\n");
1287 continue;
1288 }
1289 }
1290
1291 if (TTYPE_INTEGER && do_l1precheck && use_tquery)
1292 if (bb_point_l1mindist_exceeds_ttype(tlo, thi, tquery, D, tl1, tlinf)) {
1293 //printf("l1 precheck failed!\n");
1294 continue;
1295 }
1296
1297 if (TTYPE_INTEGER && use_tmath) {
1298 if (bb_point_mindist2_exceeds_ttype(tlo, thi, tquery, D, tl2))
1299 continue;
1300 wholenode = do_wholenode_check &&
1301 !bb_point_maxdist2_exceeds_ttype(tlo, thi, tquery, D, tl2);
1302 } else if (TTYPE_INTEGER && use_bigtmath) {
1303 if (bb_point_mindist2_exceeds_bigttype(tlo, thi, tquery, D, bigtl2))
1304 continue;
1305 wholenode = do_wholenode_check &&
1306 !bb_point_maxdist2_exceeds_bigttype(tlo, thi, tquery, D, bigtl2);
1307 } else {
1308
1309 #ifndef _MSC_VER //# Modified by Robert Lancaster for the SexySolver Internal Library
1310 etype bblo[D], bbhi[D];
1311 #else
1312 etype *bblo = (etype*) malloc(sizeof(etype)*D);
1313 etype *bbhi = (etype*) malloc(sizeof(etype)*D);
1314 #endif
1315 int d;
1316 for (d=0; d<D; d++) {
1317 bblo[d] = POINT_TE(kd, d, tlo[d]);
1318 bbhi[d] = POINT_TE(kd, d, thi[d]);
1319 }
1320 if (bb_point_mindist2_exceeds(bblo, bbhi, query, D, maxd2))
1321 continue;
1322 wholenode = do_wholenode_check &&
1323 !bb_point_maxdist2_exceeds(bblo, bbhi, query, D, maxd2);
1324
1325 #ifdef _MSC_VER //# Modified by Robert Lancaster for the SexySolver Internal Library
1326 free(bblo);
1327 free(bbhi);
1328 #endif
1329 }
1330
1331 if (wholenode) {
1332 L = kdtree_left(kd, nodeid);
1333 R = kdtree_right(kd, nodeid);
1334 if (do_dists) {
1335 for (i=L; i<=R; i++) {
1336 double dsqd = dist2(kd, query, KD_DATA(kd, D, i), D);
1337 if (!add_result(kd, res, dsqd, KD_PERM(kd, i),
1338 KD_DATA(kd, D, i), D,
1339 do_dists, do_points))
1340 return NULL;
1341 }
1342 } else {
1343 for (i=L; i<=R; i++)
1344 if (!add_result(kd, res, HUGE_VAL, KD_PERM(kd, i),
1345 KD_DATA(kd, D, i), D,
1346 do_dists, do_points))
1347 return NULL;
1348 }
1349 continue;
1350 }
1351
1352 stackpos++;
1353 nodestack[stackpos] = KD_CHILD_LEFT(nodeid);
1354 stackpos++;
1355 nodestack[stackpos] = KD_CHILD_RIGHT(nodeid);
1356
1357 } else {
1358 // use_splits.
1359
1360 split = *KD_SPLIT(kd, nodeid);
1361 if (!kd->splitdim && TTYPE_INTEGER) {
1362 bigint tmpsplit;
1363 tmpsplit = split;
1364 dim = tmpsplit & kd->dimmask;
1365 split = tmpsplit & kd->splitmask;
1366 }
1367
1368 if (TTYPE_INTEGER && use_tsplit) {
1369
1370 if (tquery[dim] < split) {
1371 // query is on the "left" side of the split.
1372 assert(query[dim] < POINT_TE(kd, dim, split));
1373 stackpos++;
1374 nodestack[stackpos] = KD_CHILD_LEFT(nodeid);
1375 // look mum, no int overflow!
1376 if (split - tquery[dim] <= tlinf) {
1377 // right child is okay.
1378 assert(POINT_TE(kd, dim, split) - query[dim] >= 0.0);
1379 // This may fail due to rounding?
1380 //assert(POINT_TE(kd, dim, split) - query[dim] <= maxdist);
1381 stackpos++;
1382 nodestack[stackpos] = KD_CHILD_RIGHT(nodeid);
1383 }
1384
1385 } else {
1386 // query is on "right" side.
1387 //assert(POINT_TE(kd, dim, split) <= query[dim]);
1388 assert(POINT_TE(kd, dim, split) <= query[dim] + kd->invscale/2.0);
1389 stackpos++;
1390 nodestack[stackpos] = KD_CHILD_RIGHT(nodeid);
1391 if (tquery[dim] - split <= tlinf) {
1392 //assert(query[dim] - POINT_TE(kd, dim, split) >= 0.0);
1393 assert((query[dim] - POINT_TE(kd, dim, split)) >= -kd->invscale/2.0);
1394 // This may fail due to rounding?
1395 //assert(query[dim] - POINT_TE(kd, dim, split) <= maxdist);
1396 stackpos++;
1397 nodestack[stackpos] = KD_CHILD_LEFT(nodeid);
1398 }
1399 }
1400 } else {
1401 dtype rsplit = POINT_TE(kd, dim, split);
1402 if (query[dim] < rsplit) {
1403 // query is on the "left" side of the split.
1404 stackpos++;
1405 nodestack[stackpos] = KD_CHILD_LEFT(nodeid);
1406 if (rsplit - query[dim] <= maxdist) {
1407 stackpos++;
1408 nodestack[stackpos] = KD_CHILD_RIGHT(nodeid);
1409 }
1410 } else {
1411 // query is on the "right" side
1412 stackpos++;
1413 nodestack[stackpos] = KD_CHILD_RIGHT(nodeid);
1414 if (query[dim] - rsplit <= maxdist) {
1415 stackpos++;
1416 nodestack[stackpos] = KD_CHILD_LEFT(nodeid);
1417 }
1418 }
1419 }
1420 }
1421 }
1422
1423 /* Resize result arrays. */
1424 if (!(options & KD_OPTIONS_NO_RESIZE_RESULTS))
1425 resize_results(res, res->nres, D, do_dists, do_points);
1426
1427 /* Sort by ascending distance away from target point before returning */
1428 if (options & KD_OPTIONS_SORT_DISTS) {
1429 if (FALSE) {
1430 printf("before sorting results:\n");
1431 print_results(res, D);
1432 }
1433 kdtree_qsort_results(res, kd->ndim);
1434 if (FALSE) {
1435 printf("after sorting results:\n");
1436 print_results(res, D);
1437 }
1438 }
1439
1440 #ifdef _MSC_VER //# Modified by Robert Lancaster for the SexySolver Internal Library
1441 free(tquery);
1442 #endif
1443
1444 return res;
1445 }
1446
1447
get_data(const kdtree_t * kd,int i)1448 static void* get_data(const kdtree_t* kd, int i) {
1449 return KD_DATA(kd, kd->ndim, i);
1450 }
1451
copy_data_double(const kdtree_t * kd,int start,int N,double * dest)1452 static void copy_data_double(const kdtree_t* kd, int start, int N,
1453 double* dest) {
1454 Unused int i, j, d;
1455 int D;
1456
1457 D = kd->ndim;
1458 #if DTYPE_DOUBLE
1459 //#warning "Data type is double; just copying data."
1460 memcpy(dest, kd->data.DTYPE + start*D, N*D*sizeof(etype));
1461 #elif (!DTYPE_INTEGER && !ETYPE_INTEGER)
1462 //#warning "Etype and Dtype are both reals; just casting values."
1463 for (i=0; i<(N * D); i++)
1464 dest[i] = kd->data.DTYPE[start*D + i];
1465 #else
1466 //#warning "Using POINT_DE to convert data."
1467 j=0;
1468 for (i=0; i<N; i++)
1469 for (d=0; d<D; d++) {
1470 dest[j] = POINT_DE(kd, D, kd->data.DTYPE[(start + i)*D + d]);
1471 j++;
1472 }
1473 #endif
1474 }
1475
1476 static dtype* kdqsort_arr;
1477 static int kdqsort_D;
1478
kdqsort_compare(const void * v1,const void * v2)1479 static int kdqsort_compare(const void* v1, const void* v2)
1480 {
1481 int i1, i2;
1482 dtype val1, val2;
1483 i1 = *((int*)v1);
1484 i2 = *((int*)v2);
1485 val1 = kdqsort_arr[(size_t)i1 * (size_t)kdqsort_D];
1486 val2 = kdqsort_arr[(size_t)i2 * (size_t)kdqsort_D];
1487 if (val1 < val2)
1488 return -1;
1489 else if (val1 > val2)
1490 return 1;
1491 return 0;
1492 }
1493
kdtree_qsort(dtype * arr,unsigned int * parr,int l,int r,int D,int d)1494 static int kdtree_qsort(dtype *arr, unsigned int *parr, int l, int r, int D, int d)
1495 {
1496 int* permute;
1497 int i, j, N;
1498 dtype* tmparr;
1499 int* tmpparr;
1500
1501 N = r - l + 1;
1502 permute = MALLOC((size_t)N * sizeof(int));
1503 if (!permute) {
1504 SYSERROR("Failed to allocate extra permutation array");
1505 return -1;
1506 }
1507 for (i = 0; i < N; i++)
1508 permute[i] = i;
1509 kdqsort_arr = arr + (size_t)l * (size_t)D + (size_t)d;
1510 kdqsort_D = D;
1511
1512 qsort(permute, N, sizeof(int), kdqsort_compare);
1513
1514 // permute the data one dimension at a time...
1515 tmparr = MALLOC(N * sizeof(dtype));
1516 if (!tmparr) {
1517 SYSERROR("Failed to allocate temp permutation array");
1518 return -1;
1519 }
1520 for (j = 0; j < D; j++) {
1521 for (i = 0; i < N; i++) {
1522 int pi = permute[i];
1523 tmparr[i] = arr[(size_t)(l + pi) * (size_t)D + (size_t)j];
1524 }
1525 for (i = 0; i < N; i++)
1526 arr[(size_t)(l + i) * (size_t)D + (size_t)j] = tmparr[i];
1527 }
1528 FREE(tmparr);
1529 tmpparr = MALLOC(N * sizeof(int));
1530 if (!tmpparr) {
1531 SYSERROR("Failed to allocate temp permutation array");
1532 return -1;
1533 }
1534 for (i = 0; i < N; i++) {
1535 int pi = permute[i];
1536 tmpparr[i] = parr[l + pi];
1537 }
1538 memcpy(parr + l, tmpparr, (size_t)N*(size_t)sizeof(int));
1539 FREE(tmpparr);
1540 FREE(permute);
1541 return 0;
1542 }
1543
1544
1545 #define GET(x) (arr[(size_t)(x)*(size_t)D+(size_t)d])
1546 #if defined(KD_DIM)
1547 #define ELEM_SWAP(il, ir) { \
1548 if ((il) != (ir)) { \
1549 tmpperm = parr[il]; \
1550 assert(tmpperm != -1); \
1551 parr[il] = parr[ir]; \
1552 parr[ir] = tmpperm; \
1553 { int d; for (d=0; d<D; d++) { \
1554 tmpdata[0] = KD_ARRAY_VAL(arr, D, il, d); \
1555 *KD_ARRAY_REF(arr, D, il, d) = KD_ARRAY_VAL(arr, D, ir, d); \
1556 *KD_ARRAY_REF(arr, D, ir, d) = tmpdata[0]; }}}}
1557 #else
1558 #define ELEM_SWAP(il, ir) { \
1559 if ((il) != (ir)) { \
1560 tmpperm = parr[il]; \
1561 assert(tmpperm != -1); \
1562 parr[il] = parr[ir]; \
1563 parr[ir] = tmpperm; \
1564 memcpy(tmpdata, KD_ARRAY_REF(arr, D, il, 0), D*sizeof(dtype)); \
1565 memcpy(KD_ARRAY_REF(arr, D, il, 0), KD_ARRAY_REF(arr, D, ir, 0), D*sizeof(dtype)); \
1566 memcpy(KD_ARRAY_REF(arr, D, ir, 0), tmpdata, D*sizeof(dtype)); }}
1567 #endif
1568 #define ELEM_ROT(iA, iB, iC) { \
1569 tmpperm = parr[iC]; \
1570 parr[iC] = parr[iB]; \
1571 parr[iB] = parr[iA]; \
1572 parr[iA] = tmpperm; \
1573 assert(tmpperm != -1); \
1574 memcpy(tmpdata, KD_ARRAY_REF(arr, D, iC, 0), D*sizeof(dtype)); \
1575 memcpy(KD_ARRAY_REF(arr, D, iC, 0), KD_ARRAY_REF(arr, D, iB, 0), D*sizeof(dtype)); \
1576 memcpy(KD_ARRAY_REF(arr, D, iB, 0), KD_ARRAY_REF(arr, D, iA, 0), D*sizeof(dtype)); \
1577 memcpy(KD_ARRAY_REF(arr, D, iA, 0), tmpdata, D*sizeof(dtype)); }
1578
kdtree_quickselect_partition(dtype * arr,unsigned int * parr,int L,int R,int D,int d,int rank)1579 static void kdtree_quickselect_partition(dtype *arr, unsigned int *parr,
1580 int L, int R, int D, int d,
1581 int rank) {
1582 int i;
1583 int low, high;
1584
1585 #if defined(KD_DIM)
1586 // tell the compiler this is a constant...
1587 D = KD_DIM;
1588 #endif
1589
1590 /* sanity is good */
1591 assert(R >= L);
1592
1593 /* Find the "rank"th point and partition the data. */
1594 /* For us, "rank" is usually the median of L and R. */
1595 low = L;
1596 high = R;
1597 while(1) {
1598 dtype vals[3];
1599 dtype tmp;
1600 dtype pivot;
1601 int i,j;
1602 int iless, iequal, igreater;
1603 int endless, endequal, endgreater;
1604 int middle;
1605 int nless, nequal;
1606 // temp storage for ELEM_SWAP and ELEM_ROT macros.
1607 #ifndef _MSC_VER //# Modified by Robert Lancaster for the SexySolver Internal Library
1608 dtype tmpdata[D];
1609 #else
1610 dtype *tmpdata = (dtype*) malloc(sizeof(dtype)*D);
1611 #endif
1612 int tmpperm;
1613
1614 if (high == low)
1615 break;
1616
1617 /* Choose the pivot: find the median of the values in low,
1618 middle, and high positions. */
1619 middle = (low + high) / 2;
1620 vals[0] = GET(low);
1621 vals[1] = GET(middle);
1622 vals[2] = GET(high);
1623 /* (Bubblesort the three elements.) */
1624 for (i=0; i<2; i++)
1625 for (j=0; j<(2-i); j++)
1626 if (vals[j] > vals[j+1]) {
1627 tmp = vals[j];
1628 vals[j] = vals[j+1];
1629 vals[j+1] = tmp;
1630 }
1631 assert(vals[0] <= vals[1]);
1632 assert(vals[1] <= vals[2]);
1633 pivot = vals[1];
1634
1635 /* Count the number of items that are less than, and equal to, the pivot. */
1636 nless = nequal = 0;
1637 for (i=low; i<=high; i++) {
1638 if (GET(i) < pivot)
1639 nless++;
1640 else if (GET(i) == pivot)
1641 nequal++;
1642 }
1643
1644 /* These are the indices where the <, =, and > entries will start. */
1645 iless = low;
1646 iequal = low + nless;
1647 igreater = low + nless + nequal;
1648
1649 /* These are the indices where they will end; ie the elements less than the
1650 pivot will live in [iless, endless). (But note that we'll be incrementing
1651 "iequal" et al in the loop below.) */
1652 endless = iequal;
1653 endequal = igreater;
1654 endgreater = high+1;
1655
1656 while (1) {
1657 /* Find an element in the "less" section that is out of place. */
1658 while ( (iless < endless) && (GET(iless) < pivot) )
1659 iless++;
1660
1661 /* Find an element in the "equal" section that is out of place. */
1662 while ( (iequal < endequal) && (GET(iequal) == pivot) )
1663 iequal++;
1664
1665 /* Find an element in the "greater" section that is out of place. */
1666 while ( (igreater < endgreater) && (GET(igreater) > pivot) )
1667 igreater++;
1668
1669
1670 /* We're looking at three positions, and each one has three cases:
1671 we're finished that segment, or the element we're looking at belongs in
1672 one of the other two segments. This yields 27 cases, but many of them
1673 are ruled out because, eg, if the element at "iequal" belongs in the "less"
1674 segment, then we can't be done the "less" segment.
1675
1676 It turns out there are only 6 cases to handle:
1677
1678 ---------------------------------------------
1679 case iless iequal igreater action
1680 ---------------------------------------------
1681 1 D D D done
1682 2 G ? L swap l,g
1683 3 E L ? swap l,e
1684 4 ? G E swap e,g
1685 5 E G L rotate A
1686 6 G L E rotate B
1687 ---------------------------------------------
1688
1689 legend:
1690 D: done
1691 ?: don't care
1692 L: (element < pivot)
1693 E: (element == pivot)
1694 G: (element > pivot)
1695 */
1696
1697 /* case 1: done? */
1698 if ((iless == endless) && (iequal == endequal) && (igreater == endgreater))
1699 break;
1700
1701 /* case 2: swap l,g */
1702 if ((iless < endless) && (igreater < endgreater) &&
1703 (GET(iless) > pivot) && (GET(igreater) < pivot)) {
1704 ELEM_SWAP(iless, igreater);
1705 assert(GET(iless) < pivot);
1706 assert(GET(igreater) > pivot);
1707 continue;
1708 }
1709
1710 /* cases 3,4,5,6 */
1711 assert(iequal < endequal);
1712 if (GET(iequal) > pivot) {
1713 /* cases 4,5: */
1714 assert(igreater < endgreater);
1715 if (GET(igreater) == pivot) {
1716 /* case 4: swap e,g */
1717 ELEM_SWAP(iequal, igreater);
1718 assert(GET(iequal) == pivot);
1719 assert(GET(igreater) > pivot);
1720 } else {
1721 /* case 5: rotate. */
1722 assert(GET(iless) == pivot);
1723 assert(GET(iequal) > pivot);
1724 assert(GET(igreater) < pivot);
1725 ELEM_ROT(iless, iequal, igreater);
1726 assert(GET(iless) < pivot);
1727 assert(GET(iequal) == pivot);
1728 assert(GET(igreater) > pivot);
1729 }
1730 } else {
1731 /* cases 3,6 */
1732 assert(GET(iequal) < pivot);
1733 assert(iless < endless);
1734 if (GET(iless) == pivot) {
1735 /* case 3: swap l,e */
1736 ELEM_SWAP(iless, iequal);
1737 assert(GET(iless) < pivot);
1738 assert(GET(iequal) == pivot);
1739 } else {
1740 /* case 6: rotate. */
1741 assert(GET(iless) > pivot);
1742 assert(GET(iequal) < pivot);
1743 assert(GET(igreater) == pivot);
1744 ELEM_ROT(igreater, iequal, iless);
1745 assert(GET(iless) < pivot);
1746 assert(GET(iequal) == pivot);
1747 assert(GET(igreater) > pivot);
1748 }
1749 }
1750 }
1751
1752 /* Reset the indices of where the segments start. */
1753 iless = low;
1754 iequal = low + nless;
1755 igreater = low + nless + nequal;
1756
1757 /* Assert that "<" values are in the "less" partition, "=" values are in the
1758 "equal" partition, and ">" values are in the "greater" partition. */
1759 for (i=iless; i<iequal; i++)
1760 assert(GET(i) < pivot);
1761 for (i=iequal; i<igreater; i++)
1762 assert(GET(i) == pivot);
1763 for (i=igreater; i<=high; i++)
1764 assert(GET(i) > pivot);
1765
1766 /* Is the median in the "<", "=", or ">" partition? */
1767 if (rank < iequal)
1768 /* median is in the "<" partition. low is unchanged. */
1769 high = iequal - 1;
1770 else if (rank < igreater)
1771 /* the median is inside the "=" partition; we're done! */
1772 break;
1773 else
1774 /* median is in the ">" partition. high is unchanged. */
1775 low = igreater;
1776
1777 #ifdef _MSC_VER //# Modified by Robert Lancaster for the SexySolver Internal Library
1778 free(tmpdata);
1779 #endif
1780 }
1781
1782 /* check that it worked. */
1783 for (i=L; i<rank; i++)
1784 assert(GET(i) <= GET(rank));
1785 for (i=rank; i<=R; i++)
1786 assert(GET(i) >= GET(rank));
1787 }
1788 #undef ELEM_SWAP
1789 #undef ELEM_ROT
1790 #undef GET
1791
1792
1793
kdtree_check_node(const kdtree_t * kd,int nodeid)1794 static int kdtree_check_node(const kdtree_t* kd, int nodeid) {
1795 int sum, i;
1796 int D = kd->ndim;
1797 int L, R;
1798 int d;
1799
1800 L = kdtree_left (kd, nodeid);
1801 R = kdtree_right(kd, nodeid);
1802
1803 if (kdtree_is_node_empty(kd, nodeid)) {
1804 assert(L == (R+1));
1805 assert(L >= 0);
1806 assert(L <= kd->ndata);
1807 assert(R >= -1);
1808 assert(R < kd->ndata);
1809 if (!((L == (R+1)) && (L >= -1) && (L <= kd->ndata) && (R >= -1) && (R < kd->ndata))) {
1810 ERROR("kdtree_check: L,R out of range for empty node");
1811 return -1;
1812 }
1813 } else {
1814 assert(L < kd->ndata);
1815 assert(R < kd->ndata);
1816 assert(L >= 0);
1817 assert(R >= 0);
1818 assert(L <= R);
1819 if (!((L < kd->ndata) && (R < kd->ndata) && (L >= 0) && (R >= 0) && (L <= R))) {
1820 ERROR("kdtree_check: L,R out of range for non-empty node");
1821 return -1;
1822 }
1823 }
1824
1825 // if it's the root node, make sure that each value in the permutation
1826 // array is present exactly once.
1827 if (!nodeid && kd->perm) {
1828 unsigned char* counts = CALLOC(kd->ndata, 1);
1829 for (i=0; i<kd->ndata; i++)
1830 counts[kd->perm[i]]++;
1831 for (i=0; i<kd->ndata; i++)
1832 assert(counts[i] == 1);
1833 for (i=0; i<kd->ndata; i++)
1834 if (counts[i] != 1) {
1835 ERROR("kdtree_check: permutation vector failure");
1836 return -1;
1837 }
1838 FREE(counts);
1839 }
1840
1841 sum = 0;
1842 if (kd->perm) {
1843 for (i=L; i<=R; i++) {
1844 sum += kd->perm[i];
1845 assert(kd->perm[i] >= 0);
1846 assert(kd->perm[i] < kd->ndata);
1847 if (kd->perm[i] >= kd->ndata) {
1848 ERROR("kdtree_check: permutation vector range failure");
1849 return -1;
1850 }
1851 }
1852 }
1853
1854 if (KD_IS_LEAF(kd, nodeid)) {
1855 if ((kd->minval && !kd->maxval) ||
1856 (!kd->minval && kd->maxval)) {
1857 ERROR("kdtree_check: minval but no maxval (or vice versa)");
1858 return -1;
1859 }
1860 if (kd->minval && kd->maxval) {
1861 for (i=L; i<=R; i++) {
1862 dtype* dat = KD_DATA(kd, D, i);
1863 for (d=0; d<D; d++) {
1864 Unused etype e = POINT_DE(kd, d, dat[d]);
1865 assert(e >= kd->minval[d]);
1866 assert(e <= kd->maxval[d]);
1867 }
1868 }
1869 }
1870 return 0;
1871 }
1872
1873 if (kd->bb.any) {
1874 ttype* bb;
1875 ttype *plo, *phi;
1876 anbool ok = FALSE;
1877 plo = LOW_HR( kd, D, nodeid);
1878 phi = HIGH_HR(kd, D, nodeid);
1879
1880 // bounding-box sanity.
1881 for (d=0; d<D; d++) {
1882 assert(plo[d] <= phi[d]);
1883 if (plo[d] > phi[d]) {
1884 ERROR("kdtree_check: bounding-box sanity failure");
1885 return -1;
1886 }
1887 }
1888 // check that the points owned by this node are within its bounding box.
1889 for (i=L; i<=R; i++) {
1890 dtype* dat = KD_DATA(kd, D, i);
1891 for (d=0; d<D; d++) {
1892 Unused ttype t = POINT_DT(kd, d, dat[d], KD_ROUND);
1893 assert(plo[d] <= t);
1894 assert(t <= phi[d]);
1895 if (plo[d] > t || t > phi[d]) {
1896 ERROR("kdtree_check: bounding-box failure");
1897 return -1;
1898 }
1899 }
1900 }
1901
1902 if (!KD_IS_LEAF(kd, nodeid)) {
1903 // check that the children's bounding box corners are within
1904 // the parent's bounding box.
1905 bb = LOW_HR(kd, D, KD_CHILD_LEFT(nodeid));
1906 for (d=0; d<D; d++) {
1907 assert(plo[d] <= bb[d]);
1908 assert(bb[d] <= phi[d]);
1909 if (plo[d] > bb[d] || bb[d] > phi[d]) {
1910 ERROR("kdtree_check: bounding-box nesting failure");
1911 return -1;
1912 }
1913 }
1914 bb = HIGH_HR(kd, D, KD_CHILD_LEFT(nodeid));
1915 for (d=0; d<D; d++) {
1916 assert(plo[d] <= bb[d]);
1917 assert(bb[d] <= phi[d]);
1918 if (plo[d] > bb[d] || bb[d] > phi[d]) {
1919 ERROR("kdtree_check: bounding-box nesting failure");
1920 return -1;
1921 }
1922 }
1923 bb = LOW_HR(kd, D, KD_CHILD_RIGHT(nodeid));
1924 for (d=0; d<D; d++) {
1925 assert(plo[d] <= bb[d]);
1926 assert(bb[d] <= phi[d]);
1927 if (plo[d] > bb[d] || bb[d] > phi[d]) {
1928 ERROR("kdtree_check: bounding-box nesting failure");
1929 return -1;
1930 }
1931 }
1932 bb = HIGH_HR(kd, D, KD_CHILD_RIGHT(nodeid));
1933 for (d=0; d<D; d++) {
1934 assert(plo[d] <= bb[d]);
1935 assert(bb[d] <= phi[d]);
1936 if (plo[d] > bb[d] || bb[d] > phi[d]) {
1937 ERROR("kdtree_check: bounding-box nesting failure");
1938 return -1;
1939 }
1940 }
1941 // check that the children don't overlap with positive volume.
1942 for (d=0; d<D; d++) {
1943 ttype* bb1 = HIGH_HR(kd, D, KD_CHILD_LEFT(nodeid));
1944 ttype* bb2 = LOW_HR(kd, D, KD_CHILD_RIGHT(nodeid));
1945 if (bb2[d] >= bb1[d]) {
1946 ok = TRUE;
1947 break;
1948 }
1949 }
1950 assert(ok);
1951 if (!ok) {
1952 ERROR("kdtree_check: peer overlap failure");
1953 return -1;
1954 }
1955 }
1956 }
1957 if (kd->split.any) {
1958
1959 if (!KD_IS_LEAF(kd, nodeid)) {
1960 // check split/dim.
1961 ttype split;
1962 int dim = 0;
1963 int cL, cR;
1964 dtype dsplit;
1965
1966 split = *KD_SPLIT(kd, nodeid);
1967 if (kd->splitdim)
1968 dim = kd->splitdim[nodeid];
1969 else {
1970 if (TTYPE_INTEGER) {
1971 bigint tmpsplit;
1972 tmpsplit = split;
1973 dim = tmpsplit & kd->dimmask;
1974 split = tmpsplit & kd->splitmask;
1975 }
1976 }
1977
1978 dsplit = POINT_TD(kd, dim, split);
1979
1980 cL = kdtree_left (kd, KD_CHILD_LEFT(nodeid));
1981 cR = kdtree_right(kd, KD_CHILD_LEFT(nodeid));
1982 for (i=cL; i<=cR; i++) {
1983 Unused dtype* dat = KD_DATA(kd, D, i);
1984 assert(dat[dim] <= dsplit);
1985 if (dat[dim] > dsplit) {
1986 ERROR("kdtree_check: split-plane failure (1)");
1987 printf("Data item %i, dim %i: %g vs %g\n", i, dim, (double)dat[dim], (double)dsplit);
1988 return -1;
1989 }
1990 }
1991
1992 cL = kdtree_left (kd, KD_CHILD_RIGHT(nodeid));
1993 cR = kdtree_right(kd, KD_CHILD_RIGHT(nodeid));
1994 for (i=cL; i<=cR; i++) {
1995 Unused dtype* dat = KD_DATA(kd, D, i);
1996 assert(dat[dim] >= dsplit);
1997 if (dat[dim] < dsplit) {
1998 ERROR("kdtree_check: split-plane failure (2)");
1999 return -1;
2000 }
2001 }
2002 }
2003 }
2004
2005 return 0;
2006 }
2007
MANGLE(kdtree_check)2008 int MANGLE(kdtree_check)(const kdtree_t* kd) {
2009 int i;
2010 if (kd->split.any) {
2011 assert(kd->splitmask);
2012 if (!kd->splitdim) {
2013 assert(kd->dimmask);
2014 }
2015 }
2016 for (i=0; i<kd->nnodes; i++) {
2017 if (kdtree_check_node(kd, i))
2018 return -1;
2019 }
2020 return 0;
2021 }
2022
maxrange(double * lo,double * hi,int D)2023 static double maxrange(double* lo, double* hi, int D) {
2024 double range;
2025 int d;
2026 range = 0.0;
2027 for (d=0; d<D; d++)
2028 if (hi[d] - lo[d] > range)
2029 range = hi[d] - lo[d];
2030 if (range == 0.0)
2031 return 1.0;
2032 return range;
2033 }
2034
compute_scale(dtype * ddata,int N,int D,double * lo,double * hi)2035 static double compute_scale(dtype* ddata, int N, int D,
2036 double* lo, double* hi) {
2037 double range;
2038 int i, d;
2039 for (d=0; d<D; d++) {
2040 lo[d] = DTYPE_MAX;
2041 hi[d] = DTYPE_MIN;
2042 }
2043 for (i=0; i<N; i++) {
2044 for (d=0; d<D; d++) {
2045 if (*ddata > hi[d]) hi[d] = *ddata;
2046 if (*ddata < lo[d]) lo[d] = *ddata;
2047 ddata++;
2048 }
2049 }
2050 range = maxrange(lo, hi, D);
2051 return (double)(DTYPE_MAX) / range;
2052 }
2053
2054 // same as "compute_scale" but takes data of "etype".
compute_scale_ext(etype * edata,int N,int D,double * lo,double * hi)2055 static double compute_scale_ext(etype* edata, int N, int D,
2056 double* lo, double* hi) {
2057 double range;
2058 int i, d;
2059 for (d=0; d<D; d++) {
2060 lo[d] = ETYPE_MAX;
2061 hi[d] = ETYPE_MIN;
2062 }
2063 for (i=0; i<N; i++) {
2064 for (d=0; d<D; d++) {
2065 if (*edata > hi[d]) hi[d] = *edata;
2066 if (*edata < lo[d]) lo[d] = *edata;
2067 edata++;
2068 }
2069 }
2070 range = maxrange(lo, hi, D);
2071 return (double)DTYPE_MAX / range;
2072 }
2073
convert_data(kdtree_t * kd,etype * edata,int N,int D,int Nleaf)2074 static void convert_data(kdtree_t* kd, etype* edata, int N, int D, int Nleaf) {
2075 dtype* ddata;
2076 int i, d;
2077
2078 if (!kd->minval || !kd->maxval) {
2079 kd->minval = MALLOC(D * sizeof(double));
2080 kd->maxval = MALLOC(D * sizeof(double));
2081 kd->scale = compute_scale_ext(edata, N, D, kd->minval, kd->maxval);
2082 } else {
2083 // limits were pre-set by the user. just compute scale.
2084 double range;
2085 range = maxrange(kd->minval, kd->maxval, D);
2086 kd->scale = (double)DTYPE_MAX / range;
2087 }
2088 kd->invscale = 1.0 / kd->scale;
2089
2090 ddata = kd->data.any = MALLOC((size_t)N * (size_t)D * (size_t)sizeof(dtype));
2091 if (!ddata) {
2092 ERROR("Failed to malloc %i x %i x %i\n", N, D, (int)sizeof(dtype));
2093 return;
2094 }
2095 kd->free_data = TRUE;
2096 for (i=0; i<N; i++) {
2097 for (d=0; d<D; d++) {
2098 etype dd = POINT_ED(kd, d, *edata, KD_ROUND);
2099 if (dd > DTYPE_MAX) {
2100 WARNING("Clamping value %.12g -> %.12g to %u", (double)*edata, (double)dd, (unsigned int)DTYPE_MAX);
2101 dd = DTYPE_MAX;
2102 }
2103 if (dd < DTYPE_MIN) {
2104 WARNING("Clamping value %.12g -> %.12g to %u.\n", (double)*edata, (double)dd, (unsigned int)DTYPE_MIN);
2105 dd = DTYPE_MIN;
2106 }
2107 // Right place for this? Not really....
2108 if (!ETYPE_INTEGER) {
2109 // NaN and Inf detection...
2110 if (!isfinite(dd) || isnan(dd)) {
2111 WARNING("Replacing inf/nan value (element %i,%i) = %g with %g\n", i, d, (double)dd, (double)DTYPE_MAX);
2112 dd = DTYPE_MAX;
2113 }
2114 }
2115 *ddata = (dtype)dd;
2116
2117 ddata++;
2118 edata++;
2119 }
2120 }
2121
2122 // tune up kd->maxval so that it is larger than the external-type
2123 // value of all points in the tree.
2124 for (d=0; d<D; d++) {
2125 dtype dmax = POINT_ED(kd, d, kd->maxval[d], KD_ROUND);
2126 etype emax = POINT_DE(kd, d, dmax);
2127 kd->maxval[d] = MAX(kd->maxval[d], emax);
2128 }
2129 #ifndef NDEBUG
2130 for (i=0; i<N; i++) {
2131 for (d=0; d<D; d++) {
2132 etype e = POINT_DE(kd, d, KD_DATA(kd, D, i)[d]);
2133 assert(e <= kd->maxval[d]);
2134 assert(e >= kd->minval[d]);
2135 }
2136 }
2137 #endif
2138 }
2139
compute_bb(const dtype * data,int D,int N,dtype * lo,dtype * hi)2140 static void compute_bb(const dtype* data, int D, int N, dtype* lo, dtype* hi) {
2141 int d, i;
2142
2143 for (d=0; d<D; d++) {
2144 hi[d] = DTYPE_MIN;
2145 lo[d] = DTYPE_MAX;
2146 }
2147 /* (since data is stored lexicographically we can just iterate through it) */
2148 /* (avoid doing kd->data[NODE(i)*D + d] many times; just ++ the pointer) */
2149 for (i=0; i<N; i++) {
2150 for (d=0; d<D; d++) {
2151 if (*data > hi[d]) hi[d] = *data;
2152 if (*data < lo[d]) lo[d] = *data;
2153 data++;
2154 }
2155 }
2156 }
2157
save_bb(kdtree_t * kd,int i,const dtype * lo,const dtype * hi)2158 static void save_bb(kdtree_t* kd, int i, const dtype* lo, const dtype* hi) {
2159 int D = kd->ndim;
2160 int d;
2161 for (d=0; d<D; d++) {
2162 (LOW_HR (kd, D, i))[d] = POINT_DT(kd, d, lo[d], floor);
2163 (HIGH_HR(kd, D, i))[d] = POINT_DT(kd, d, hi[d], ceil);
2164 }
2165 }
2166
needs_data_conversion()2167 static int needs_data_conversion() {
2168 return DTYPE_INTEGER && !ETYPE_INTEGER;
2169 }
2170
MANGLE(kdtree_build_2)2171 kdtree_t* MANGLE(kdtree_build_2)
2172 (kdtree_t* kd, etype* indata, int N, int D, int Nleaf, int treetype, unsigned int options, double* minval, double* maxval) {
2173 int i;
2174 int xx;
2175 int lnext, level;
2176 int maxlevel;
2177
2178 #ifndef _MSC_VER //# Modified by Robert Lancaster for the SexySolver Internal Library
2179 dtype hi[D], lo[D];
2180 #else
2181 dtype *hi = (dtype*) malloc(sizeof(dtype)*D);
2182 dtype *lo = (dtype*) malloc(sizeof(dtype)*D);
2183 #endif
2184 dtype* data = NULL;
2185
2186 #ifndef _MSC_VER //# Modified by Robert Lancaster for the SexySolver Internal Library
2187 dtype nullbb[D];
2188 #else
2189 dtype *nullbb = (dtype*) malloc(sizeof(dtype)*D);
2190 #endif
2191
2192 for (i=0; i<D; i++)
2193 nullbb[i] = 0;
2194
2195 maxlevel = kdtree_compute_levels(N, Nleaf);
2196
2197 assert(maxlevel > 0);
2198 assert(D <= KDTREE_MAX_DIM);
2199 #if defined(KD_DIM)
2200 assert(D == KD_DIM);
2201 // let the compiler know that D is a constant...
2202 D = KD_DIM;
2203 #endif
2204
2205 /* Parameters checking */
2206 if (!indata || !N || !D) {
2207 ERROR("Data, N, or D is zero");
2208 return NULL;
2209 }
2210 /* Make sure we have enough data */
2211 if ((1 << maxlevel) - 1 > N) {
2212 ERROR("Too few data points for number of tree levels (%i < %i)!", N, (1 << maxlevel) - 1);
2213 return NULL;
2214 }
2215
2216 if ((options & KD_BUILD_SPLIT) &&
2217 (options & KD_BUILD_NO_LR) &&
2218 !(options & KD_BUILD_SPLITDIM) &&
2219 TTYPE_INTEGER) {
2220 ERROR("Currently you can't set KD_BUILD_NO_LR for int trees "
2221 "unless you also set KD_BUILD_SPLITDIM");
2222 return NULL;
2223 }
2224
2225 if (!kd)
2226 kd = kdtree_new(N, D, Nleaf);
2227
2228 kd->treetype = treetype;
2229 if (minval) {
2230 kd->minval = CALLOC(D, sizeof(double));
2231 memcpy(kd->minval, minval, D*sizeof(double));
2232 }
2233 if (maxval) {
2234 kd->maxval = CALLOC(D, sizeof(double));
2235 memcpy(kd->maxval, maxval, D*sizeof(double));
2236 }
2237
2238 if (!kd->data.any) {
2239 if (needs_data_conversion()) {
2240 // need to convert the data from "etype" to "dtype".
2241 convert_data(kd, indata, N, D, Nleaf);
2242 } else {
2243 kd->data.any = indata;
2244
2245 // ???
2246 if (!ETYPE_INTEGER) {
2247 int i,d;
2248 etype* edata = indata;
2249 for (i=0; i<N; i++) {
2250 for (d=0; d<D; d++) {
2251 etype dd = edata[i*D + d];
2252 // NaN and Inf detection...
2253 if (!isfinite(dd) || isnan(dd)) {
2254 WARNING("Replacing inf/nan value (element %i,%i) = %g with %g\n", i, d, (double)dd, (double)DTYPE_MAX);
2255 edata[i*D + d] = DTYPE_MAX;
2256 }
2257 }
2258 }
2259 }
2260 }
2261 }
2262 if (needs_data_conversion()) {
2263 // compute scaling params
2264 if (!kd->minval || !kd->maxval) {
2265 free(kd->minval);
2266 free(kd->maxval);
2267 kd->minval = MALLOC(D * sizeof(double));
2268 kd->maxval = MALLOC(D * sizeof(double));
2269 assert(kd->minval);
2270 assert(kd->maxval);
2271 kd->scale = compute_scale(kd->data.any, N, D, kd->minval, kd->maxval);
2272 } else {
2273 // limits were pre-set by the user. just compute scale.
2274 double range;
2275 range = maxrange(kd->minval, kd->maxval, D);
2276 kd->scale = (double)TTYPE_MAX / range;
2277 }
2278 kd->invscale = 1.0 / kd->scale;
2279 }
2280
2281 /* perm stores the permutation indexes. This gets shuffled around during
2282 * sorts to keep track of the original index. */
2283 kd->perm = MALLOC(sizeof(u32) * N);
2284 assert(kd->perm);
2285 for (i = 0; i < N; i++)
2286 kd->perm[i] = i;
2287
2288 kd->lr = MALLOC(kd->nbottom * sizeof(int32_t));
2289 assert(kd->lr);
2290
2291 if (options & KD_BUILD_BBOX) {
2292 kd->bb.any = MALLOC(kd->nnodes * 2 * D * sizeof(ttype));
2293 kd->n_bb = kd->nnodes;
2294 assert(kd->bb.any);
2295 }
2296 if (options & KD_BUILD_SPLIT) {
2297 kd->split.any = MALLOC(kd->ninterior * sizeof(ttype));
2298 assert(kd->split.any);
2299 }
2300 if (((options & KD_BUILD_SPLIT) && !TTYPE_INTEGER) ||
2301 (options & KD_BUILD_SPLITDIM)) {
2302 kd->splitdim = MALLOC(kd->ninterior * sizeof(u8));
2303 kd->splitmask = UINT32_MAX;
2304 kd->dimmask = 0;
2305 } else if (options & KD_BUILD_SPLIT)
2306 compute_splitbits(kd);
2307
2308 if (options & KD_BUILD_LINEAR_LR)
2309 kd->has_linear_lr = TRUE;
2310
2311 /* Use the lr array as a stack while building. In place in your face! */
2312 kd->lr[0] = N - 1;
2313 lnext = 1;
2314 level = 0;
2315
2316 // shorthand
2317 data = kd->data.DTYPE;
2318
2319 /* And in one shot, make the kdtree. Because the lr pointers
2320 * are only stored for the bottom layer, we use the lr array as a
2321 * stack. At finish, it contains the r pointers for the bottom nodes.
2322 * The l pointer is simply +1 of the previous right pointer, or 0 if we
2323 * are at the first element of the lr array. */
2324 for (i = 0; i < kd->ninterior; i++) {
2325 unsigned int d;
2326 int left, right;
2327 dtype maxrange;
2328 ttype s;
2329 unsigned int c;
2330 int dim = 0;
2331 int m;
2332 dtype qsplit = 0;
2333
2334 /* Have we reached the next level in the tree? */
2335 if (i == lnext) {
2336 level++;
2337 lnext = lnext * 2 + 1;
2338 }
2339
2340 /* Since we're not storing the L pointers, we have to infer L */
2341 if (i == (1<<level)-1) {
2342 left = 0;
2343 } else {
2344 left = kd->lr[i-1] + 1;
2345 }
2346 right = kd->lr[i];
2347
2348 assert(right != (unsigned int)-1);
2349
2350 if (left >= right) {
2351 //debug("Empty node %i: left=right=%i\n", i, left);
2352 if (options & KD_BUILD_BBOX)
2353 save_bb(kd, i, nullbb, nullbb);
2354 if (kd->splitdim)
2355 kd->splitdim[i] = 0;
2356 c = 2*i;
2357 if (level == maxlevel - 2)
2358 c -= kd->ninterior;
2359 kd->lr[c+1] = right;
2360 kd->lr[c+2] = right;
2361 continue;
2362 }
2363
2364 /* More sanity */
2365 assert(0 <= left);
2366 assert(left <= right);
2367 assert(right < N);
2368
2369 /* Find the bounding-box for this node. */
2370 compute_bb(KD_DATA(kd, D, left), D, right - left + 1, lo, hi);
2371
2372 if (options & KD_BUILD_BBOX)
2373 save_bb(kd, i, lo, hi);
2374
2375 /* Split along dimension with largest range */
2376 maxrange = DTYPE_MIN;
2377 for (d=0; d<D; d++)
2378 if ((hi[d] - lo[d]) >= maxrange) {
2379 maxrange = hi[d] - lo[d];
2380 dim = d;
2381 }
2382 d = dim;
2383 assert (d < D);
2384
2385 if ((options & KD_BUILD_FORCE_SORT) ||
2386 (TTYPE_INTEGER && !(options & KD_BUILD_SPLITDIM))) {
2387
2388 /* We're packing dimension and split location into an int. */
2389
2390 /* Sort the data. */
2391
2392 /* Because the nature of the inttree is to bin the split
2393 * planes, we have to be careful. Here, we MUST sort instead
2394 * of merely partitioning, because we may not be able to
2395 * properly represent the median as a split plane. Imagine the
2396 * following on the dtype line:
2397 *
2398 * |P P | P M | P |P | PP | ------> X
2399 * 1 2
2400 * The |'s are possible split positions. If M is selected to
2401 * split on, we actually cannot select the split 1 or 2
2402 * immediately, because if we selected 2, then M would be on
2403 * the wrong side (the medians always go to the right) and we
2404 * can't select 1 because then P would be on the wrong side.
2405 * So, the solution is to try split 2, and if point M-1 is on
2406 * the correct side, great. Otherwise, we have to move shift
2407 * point M-1 into the right side and only then chose plane 1. */
2408
2409
2410 /* FIXME but qsort allocates a 2nd perm array GAH */
2411 if (kdtree_qsort(data, kd->perm, left, right, D, dim)) {
2412 ERROR("kdtree_qsort failed");
2413 // FIXME: memleak mania!
2414 return NULL;
2415 }
2416 m = (1 + (size_t)left + (size_t)right)/2;
2417 assert(m >= 0);
2418 assert(m >= left);
2419 assert(m <= right);
2420
2421 /* Make sure sort works */
2422 for(xx=left; xx<=right-1; xx++) {
2423 assert(KD_ARRAY_VAL(data, D, xx, d) <=
2424 KD_ARRAY_VAL(data, D, xx+1, d));
2425 }
2426
2427 /* Encode split dimension and value. */
2428 /* "s" is the location of the splitting plane in the "tree"
2429 data type. */
2430 s = POINT_DT(kd, d, KD_ARRAY_VAL(data, D, m, d), KD_ROUND);
2431
2432 if (kd->split.any) {
2433 /* If we are using the "split" array to store both the
2434 splitting plane and the splitting dimension, then we
2435 truncate a few bits from "s" here. */
2436 bigint tmps = s;
2437 tmps &= kd->splitmask;
2438 assert((tmps & kd->dimmask) == 0);
2439 s = tmps;
2440 }
2441 /* "qsplit" is the location of the splitting plane in the "data"
2442 type. */
2443 qsplit = POINT_TD(kd, d, s);
2444
2445 /* Play games to make sure we properly partition the data */
2446 while (m < right && KD_ARRAY_VAL(data, D, m, d) < qsplit) m++;
2447 while (left < m && qsplit < KD_ARRAY_VAL(data, D, m-1, d)) m--;
2448
2449 /* Even more sanity */
2450 assert(m >= -1);
2451 assert(left <= m);
2452 assert(m <= right);
2453 for (xx=left; m && xx<=m-1; xx++)
2454 assert(KD_ARRAY_VAL(data, D, xx, d) <= qsplit);
2455 for (xx=m; xx<=right; xx++)
2456 assert(qsplit <= KD_ARRAY_VAL(data, D, xx, d));
2457
2458 } else {
2459 /* "m-1" becomes R of the left child;
2460 "m" becomes L of the right child. */
2461 if (kd->has_linear_lr) {
2462 m = kdtree_left(kd, KD_CHILD_RIGHT(i));
2463 } else {
2464 /* Pivot the data at the median */
2465 m = (1 + (size_t)left + (size_t)right) / 2;
2466 }
2467 assert(m >= 0);
2468 assert(m >= left);
2469 assert(m <= right);
2470 kdtree_quickselect_partition(data, kd->perm, left, right, D, dim, m);
2471
2472 s = POINT_DT(kd, d, KD_ARRAY_VAL(data, D, m, d), KD_ROUND);
2473
2474 assert(m != 0);
2475 assert(left <= (m-1));
2476 assert(m <= right);
2477 for (xx=left; xx<=m-1; xx++)
2478 assert(KD_ARRAY_VAL(data, D, xx, d) <=
2479 KD_ARRAY_VAL(data, D, m, d));
2480 for (xx=left; xx<=m-1; xx++)
2481 assert(KD_ARRAY_VAL(data, D, xx, d) <= s);
2482 for (xx=m; xx<=right; xx++)
2483 assert(KD_ARRAY_VAL(data, D, m, d) <=
2484 KD_ARRAY_VAL(data, D, xx, d));
2485 for (xx=m; xx<=right; xx++)
2486 assert(s <= KD_ARRAY_VAL(data, D, xx, d));
2487 }
2488
2489 if (kd->split.any) {
2490 if (kd->splitdim)
2491 *KD_SPLIT(kd, i) = s;
2492 else {
2493 bigint tmps = s;
2494 *KD_SPLIT(kd, i) = tmps | dim;
2495 }
2496 }
2497 if (kd->splitdim)
2498 kd->splitdim[i] = dim;
2499
2500 /* Store the R pointers for each child */
2501 c = 2*i;
2502 if (level == maxlevel - 2)
2503 c -= kd->ninterior;
2504
2505 kd->lr[c+1] = m-1;
2506 kd->lr[c+2] = right;
2507
2508 assert(c+2 < kd->nbottom);
2509 }
2510
2511 for (i=0; i<kd->nbottom-1; i++)
2512 assert(kd->lr[i] <= kd->lr[i+1]);
2513
2514 if (options & KD_BUILD_BBOX) {
2515 // Compute bounding boxes for leaf nodes.
2516 int L, R = -1;
2517 for (i=0; i<kd->nbottom; i++) {
2518 L = R + 1;
2519 R = kd->lr[i];
2520 assert(L == kdtree_leaf_left(kd, i + kd->ninterior));
2521 assert(R == kdtree_leaf_right(kd, i + kd->ninterior));
2522 compute_bb(KD_DATA(kd, D, L), D, R - L + 1, lo, hi);
2523 save_bb(kd, i + kd->ninterior, lo, hi);
2524 }
2525
2526 // check that it worked...
2527 #ifndef NDEBUG
2528 for (i=0; i<kd->nbottom; i++) {
2529 ttype* lo;
2530 ttype* hi;
2531 int j, d;
2532 int nodeid = i + kd->ninterior;
2533 lo = LOW_HR(kd, kd->ndim, nodeid);
2534 hi = HIGH_HR(kd, kd->ndim, nodeid);
2535 for (j=kdtree_leaf_left(kd, nodeid); j<=kdtree_leaf_right(kd, nodeid); j++) {
2536 dtype* data = KD_DATA(kd, kd->ndim, j);
2537 for (d=0; d<kd->ndim; d++) {
2538 assert(POINT_TD(kd, d, lo[d]) <= data[d]);
2539 assert(POINT_TD(kd, d, hi[d]) >= data[d]);
2540 assert(POINT_DT(kd, d, data[d], KD_ROUND) <= hi[d]);
2541 assert(POINT_DT(kd, d, data[d], KD_ROUND) >= lo[d]);
2542 }
2543 }
2544 }
2545 #endif
2546
2547 }
2548
2549 if (options & KD_BUILD_NO_LR) {
2550 FREE(kd->lr);
2551 kd->lr = NULL;
2552 }
2553
2554 // set function table pointers.
2555 MANGLE(kdtree_update_funcs)(kd);
2556
2557 #ifdef _MSC_VER //# Modified by Robert Lancaster for the SexySolver Internal Library
2558 free(hi);
2559 free(lo);
2560 free(nullbb);
2561 #endif
2562
2563 return kd;
2564 }
2565
MANGLE(kdtree_fix_bounding_boxes)2566 void MANGLE(kdtree_fix_bounding_boxes)(kdtree_t* kd) {
2567 // FIXME - do this log(N) times more efficiently by propagating
2568 // bounding boxes up the levels of the tree...
2569 int i;
2570 int D = kd->ndim;
2571 kd->bb.any = MALLOC(kd->nnodes * sizeof(ttype) * D * 2);
2572 assert(kd->bb.any);
2573 for (i=0; i<kd->nnodes; i++) {
2574 unsigned int left, right;
2575
2576 #ifndef _MSC_VER //# Modified by Robert Lancaster for the SexySolver Internal Library
2577 dtype hi[D], lo[D];
2578 #else
2579 dtype *hi = (dtype*) malloc(sizeof(dtype)*D);
2580 dtype *lo = (dtype*) malloc(sizeof(dtype)*D);
2581 #endif
2582 left = kdtree_left(kd, i);
2583 right = kdtree_right(kd, i);
2584 compute_bb(KD_DATA(kd, D, left), D, right - left + 1, lo, hi);
2585 save_bb(kd, i, lo, hi);
2586
2587 #ifdef _MSC_VER //# Modified by Robert Lancaster for the SexySolver Internal Library
2588 free(hi);
2589 free(lo);
2590 #endif
2591 }
2592 }
2593
MANGLE(kdtree_node_point_mindist2)2594 double MANGLE(kdtree_node_point_mindist2)
2595 (const kdtree_t* kd, int node, const etype* query) {
2596 int D = kd->ndim;
2597 int d;
2598 ttype* tlo, *thi;
2599 double d2 = 0.0;
2600 if (!bboxes(kd, node, &tlo, &thi, D)) {
2601 ERROR("Error: kdtree does not have bounding boxes!");
2602 return HUGE_VAL;
2603 }
2604 for (d=0; d<D; d++) {
2605 etype delta;
2606 etype lo = POINT_TE(kd, d, tlo[d]);
2607 if (query[d] < lo)
2608 delta = lo - query[d];
2609 else {
2610 etype hi = POINT_TE(kd, d, thi[d]);
2611 if (query[d] > hi)
2612 delta = query[d] - hi;
2613 else
2614 continue;
2615 }
2616 d2 += delta * delta;
2617 }
2618 return d2;
2619 }
2620
MANGLE(kdtree_node_point_maxdist2)2621 double MANGLE(kdtree_node_point_maxdist2)
2622 (const kdtree_t* kd, int node, const etype* query) {
2623 int D = kd->ndim;
2624 int d;
2625 ttype* tlo=NULL, *thi=NULL;
2626 double d2 = 0.0;
2627 if (!bboxes(kd, node, &tlo, &thi, D)) {
2628 ERROR("Error: kdtree_node_point_maxdist2_exceeds: kdtree does not have bounding boxes!");
2629 return FALSE;
2630 }
2631 for (d=0; d<D; d++) {
2632 etype delta1, delta2, delta;
2633 etype lo = POINT_TE(kd, d, tlo[d]);
2634 etype hi = POINT_TE(kd, d, thi[d]);
2635 if (query[d] < lo)
2636 delta = hi - query[d];
2637 else if (query[d] > hi)
2638 delta = query[d] - lo;
2639 else {
2640 delta1 = hi - query[d];
2641 delta2 = query[d] - lo;
2642 delta = MAX(delta1, delta2);
2643 }
2644 d2 += delta*delta;
2645 }
2646 return d2;
2647 }
2648
MANGLE(kdtree_node_point_mindist2_exceeds)2649 anbool MANGLE(kdtree_node_point_mindist2_exceeds)
2650 (const kdtree_t* kd, int node, const etype* query, double maxd2) {
2651 int D = kd->ndim;
2652 int d;
2653 ttype* tlo, *thi;
2654 double d2 = 0.0;
2655
2656 if (!bboxes(kd, node, &tlo, &thi, D)) {
2657 //ERROR("Error: kdtree does not have bounding boxes!");
2658 return FALSE;
2659 }
2660 for (d=0; d<D; d++) {
2661 etype delta;
2662 etype lo = POINT_TE(kd, d, tlo[d]);
2663 if (query[d] < lo)
2664 delta = lo - query[d];
2665 else {
2666 etype hi = POINT_TE(kd, d, thi[d]);
2667 if (query[d] > hi)
2668 delta = query[d] - hi;
2669 else
2670 continue;
2671 }
2672 d2 += delta * delta;
2673 if (d2 > maxd2)
2674 return TRUE;
2675 }
2676 return FALSE;
2677 }
2678
MANGLE(kdtree_node_point_maxdist2_exceeds)2679 anbool MANGLE(kdtree_node_point_maxdist2_exceeds)
2680 (const kdtree_t* kd, int node, const etype* query, double maxd2) {
2681 int D = kd->ndim;
2682 int d;
2683 ttype* tlo=NULL, *thi=NULL;
2684 double d2 = 0.0;
2685
2686 if (!bboxes(kd, node, &tlo, &thi, D)) {
2687 ERROR("Error: kdtree_node_point_maxdist2_exceeds: kdtree does not have bounding boxes!");
2688 return FALSE;
2689 }
2690 for (d=0; d<D; d++) {
2691 etype delta1, delta2, delta;
2692 etype lo = POINT_TE(kd, d, tlo[d]);
2693 etype hi = POINT_TE(kd, d, thi[d]);
2694 if (query[d] < lo)
2695 delta = hi - query[d];
2696 else if (query[d] > hi)
2697 delta = query[d] - lo;
2698 else {
2699 delta1 = hi - query[d];
2700 delta2 = query[d] - lo;
2701 delta = (delta1 > delta2 ? delta1 : delta2);
2702 }
2703 d2 += delta*delta;
2704 if (d2 > maxd2)
2705 return TRUE;
2706 }
2707 return FALSE;
2708 }
2709
MANGLE(kdtree_node_node_maxdist2)2710 double MANGLE(kdtree_node_node_maxdist2)
2711 (const kdtree_t* kd1, int node1,
2712 const kdtree_t* kd2, int node2) {
2713 ttype *tlo1=NULL, *tlo2=NULL, *thi1=NULL, *thi2=NULL;
2714 double d2 = 0.0;
2715 int d, D = kd1->ndim;
2716
2717 assert(kd1->ndim == kd2->ndim);
2718 if (!bboxes(kd1, node1, &tlo1, &thi1, D)) {
2719 ERROR("Error: kdtree_node_node_maxdist2: kdtree does not have bounding boxes!");
2720 return FALSE;
2721 }
2722 if (!bboxes(kd2, node2, &tlo2, &thi2, D)) {
2723 ERROR("Error: kdtree_node_node_maxdist2: kdtree does not have bounding boxes!");
2724 return FALSE;
2725 }
2726 // Since the two trees can have different conversion factors,
2727 // we have to convert both to the external type.
2728 // FIXME - we do assume that POINT_TE works for both of them --
2729 // ie, ~we assume they are the same treetype.
2730 for (d=0; d<D; d++) {
2731 etype alo, ahi, blo, bhi;
2732 etype delta1, delta2, delta;
2733 alo = POINT_TE(kd1, d, tlo1[d]);
2734 ahi = POINT_TE(kd1, d, thi1[d]);
2735 blo = POINT_TE(kd2, d, tlo2[d]);
2736 bhi = POINT_TE(kd2, d, thi2[d]);
2737 if (ETYPE_INTEGER)
2738 WARNING("HACK - int overflow is possible here.");
2739 delta1 = bhi - alo;
2740 delta2 = ahi - blo;
2741 delta = MAX(delta1, delta2);
2742 d2 += delta*delta;
2743 }
2744 return d2;
2745 }
2746
MANGLE(kdtree_node_node_mindist2)2747 double MANGLE(kdtree_node_node_mindist2)
2748 (const kdtree_t* kd1, int node1,
2749 const kdtree_t* kd2, int node2) {
2750 ttype *tlo1=NULL, *tlo2=NULL, *thi1=NULL, *thi2=NULL;
2751 double d2 = 0.0;
2752 int d, D = kd1->ndim;
2753 assert(kd1->ndim == kd2->ndim);
2754 if (!bboxes(kd1, node1, &tlo1, &thi1, D)) {
2755 ERROR("Error: kdtree_node_node_mindist2: kdtree does not have bounding boxes!");
2756 return FALSE;
2757 }
2758 if (!bboxes(kd2, node2, &tlo2, &thi2, D)) {
2759 ERROR("Error: kdtree_node_node_mindist2: kdtree does not have bounding boxes!");
2760 return FALSE;
2761 }
2762 for (d=0; d<D; d++) {
2763 etype alo, ahi, blo, bhi;
2764 etype delta;
2765 ahi = POINT_TE(kd1, d, thi1[d]);
2766 blo = POINT_TE(kd2, d, tlo2[d]);
2767 if (ahi < blo)
2768 delta = blo - ahi;
2769 else {
2770 alo = POINT_TE(kd1, d, tlo1[d]);
2771 bhi = POINT_TE(kd2, d, thi2[d]);
2772 if (bhi < alo)
2773 delta = alo - bhi;
2774 else
2775 continue;
2776 }
2777 d2 += delta*delta;
2778 }
2779 return d2;
2780 }
2781
MANGLE(kdtree_node_node_maxdist2_exceeds)2782 anbool MANGLE(kdtree_node_node_maxdist2_exceeds)
2783 (const kdtree_t* kd1, int node1,
2784 const kdtree_t* kd2, int node2,
2785 double maxd2) {
2786 ttype *tlo1=NULL, *tlo2=NULL, *thi1=NULL, *thi2=NULL;
2787 double d2 = 0.0;
2788 int d, D = kd1->ndim;
2789
2790 //assert(kd1->treetype == kd2->treetype);
2791 assert(kd1->ndim == kd2->ndim);
2792
2793 if (!bboxes(kd1, node1, &tlo1, &thi1, D)) {
2794 ERROR("Error: kdtree_node_node_maxdist2_exceeds: kdtree does not have bounding boxes!");
2795 return FALSE;
2796 }
2797
2798 if (!bboxes(kd2, node2, &tlo2, &thi2, D)) {
2799 ERROR("Error: kdtree_node_node_maxdist2_exceeds: kdtree does not have bounding boxes!");
2800 return FALSE;
2801 }
2802
2803 for (d=0; d<D; d++) {
2804 etype alo, ahi, blo, bhi;
2805 etype delta1, delta2, delta;
2806 alo = POINT_TE(kd1, d, tlo1[d]);
2807 ahi = POINT_TE(kd1, d, thi1[d]);
2808 blo = POINT_TE(kd2, d, tlo2[d]);
2809 bhi = POINT_TE(kd2, d, thi2[d]);
2810 // HACK - if etype is integer...
2811 if (ETYPE_INTEGER)
2812 WARNING("HACK - int overflow is possible here.");
2813 delta1 = bhi - alo;
2814 delta2 = ahi - blo;
2815 delta = (delta1 > delta2 ? delta1 : delta2);
2816 d2 += delta*delta;
2817 if (d2 > maxd2)
2818 return TRUE;
2819 }
2820 return FALSE;
2821 }
2822
MANGLE(kdtree_node_node_mindist2_exceeds)2823 anbool MANGLE(kdtree_node_node_mindist2_exceeds)
2824 (const kdtree_t* kd1, int node1,
2825 const kdtree_t* kd2, int node2,
2826 double maxd2) {
2827 ttype *tlo1=NULL, *tlo2=NULL, *thi1=NULL, *thi2=NULL;
2828 double d2 = 0.0;
2829 int d, D = kd1->ndim;
2830
2831 //assert(kd1->treetype == kd2->treetype);
2832 assert(kd1->ndim == kd2->ndim);
2833
2834 if (!bboxes(kd1, node1, &tlo1, &thi1, D)) {
2835 //ERROR("Error: kdtree_node_node_mindist2_exceeds: kdtree does not have bounding boxes!");
2836 return FALSE;
2837 }
2838
2839 if (!bboxes(kd2, node2, &tlo2, &thi2, D)) {
2840 //ERROR("Error: kdtree_node_node_mindist2_exceeds: kdtree does not have bounding boxes!");
2841 return FALSE;
2842 }
2843
2844 for (d=0; d<D; d++) {
2845 etype alo, ahi, blo, bhi;
2846 etype delta;
2847 ahi = POINT_TE(kd1, d, thi1[d]);
2848 blo = POINT_TE(kd2, d, tlo2[d]);
2849 if (ahi < blo)
2850 delta = blo - ahi;
2851 else {
2852 alo = POINT_TE(kd1, d, tlo1[d]);
2853 bhi = POINT_TE(kd2, d, thi2[d]);
2854 if (bhi < alo)
2855 delta = alo - bhi;
2856 else
2857 continue;
2858 }
2859 d2 += delta*delta;
2860 if (d2 > maxd2)
2861 return TRUE;
2862 }
2863 return FALSE;
2864 }
2865
do_boxes_overlap(const ttype * lo1,const ttype * hi1,const ttype * lo2,const ttype * hi2,int D)2866 static anbool do_boxes_overlap(const ttype* lo1, const ttype* hi1,
2867 const ttype* lo2, const ttype* hi2, int D) {
2868 int d;
2869 for (d=0; d<D; d++) {
2870 if (lo1[d] > hi2[d])
2871 return FALSE;
2872 if (lo2[d] > hi1[d])
2873 return FALSE;
2874 }
2875 return TRUE;
2876 }
2877
2878 /* Is the first box contained within the second? */
is_box_contained(const ttype * lo1,const ttype * hi1,const ttype * lo2,const ttype * hi2,int D)2879 static anbool is_box_contained(const ttype* lo1, const ttype* hi1,
2880 const ttype* lo2, const ttype* hi2, int D) {
2881 int d;
2882 for (d=0; d<D; d++) {
2883 if (lo1[d] < lo2[d])
2884 return FALSE;
2885 if (hi1[d] > hi2[d])
2886 return FALSE;
2887 }
2888 return TRUE;
2889 }
2890
nodes_contained_rec(const kdtree_t * kd,int nodeid,const ttype * qlo,const ttype * qhi,void (* cb_contained)(const kdtree_t * kd,int node,void * extra),void (* cb_overlap)(const kdtree_t * kd,int node,void * extra),void * cb_extra)2891 static void nodes_contained_rec(const kdtree_t* kd,
2892 int nodeid,
2893 const ttype* qlo, const ttype* qhi,
2894 void (*cb_contained)(const kdtree_t* kd, int node, void* extra),
2895 void (*cb_overlap)(const kdtree_t* kd, int node, void* extra),
2896 void* cb_extra) {
2897 ttype *tlo=NULL, *thi=NULL;
2898 int D = kd->ndim;
2899
2900 // leaf nodes don't have bounding boxes, so we have to do this check first!
2901 if (KD_IS_LEAF(kd, nodeid)) {
2902 cb_overlap(kd, nodeid, cb_extra);
2903 return;
2904 }
2905
2906 if (!bboxes(kd, nodeid, &tlo, &thi, D)) {
2907 ERROR("Error: kdtree_nodes_contained: node %i doesn't have a bounding box", nodeid);
2908 return;
2909 }
2910
2911 if (!do_boxes_overlap(tlo, thi, qlo, qhi, D))
2912 return;
2913
2914 if (is_box_contained(tlo, thi, qlo, qhi, D)) {
2915 cb_contained(kd, nodeid, cb_extra);
2916 return;
2917 }
2918
2919 nodes_contained_rec(kd, KD_CHILD_LEFT(nodeid), qlo, qhi,
2920 cb_contained, cb_overlap, cb_extra);
2921 nodes_contained_rec(kd, KD_CHILD_RIGHT(nodeid), qlo, qhi,
2922 cb_contained, cb_overlap, cb_extra);
2923 }
2924
MANGLE(kdtree_nodes_contained)2925 void MANGLE(kdtree_nodes_contained)
2926 (const kdtree_t* kd,
2927 const void* vquerylow, const void* vqueryhi,
2928 void (*cb_contained)(const kdtree_t* kd, int node, void* extra),
2929 void (*cb_overlap)(const kdtree_t* kd, int node, void* extra),
2930 void* cb_extra) {
2931 int D = kd->ndim;
2932 int d;
2933
2934 #ifndef _MSC_VER //# Modified by Robert Lancaster for the SexySolver Internal Library
2935 ttype qlo[D], qhi[D];
2936 #else
2937 dtype *qhi = (dtype*) malloc(sizeof(dtype)*D);
2938 dtype *qlo = (dtype*) malloc(sizeof(dtype)*D);
2939 #endif
2940 const etype* querylow = vquerylow;
2941 const etype* queryhi = vqueryhi;
2942
2943 for (d=0; d<D; d++) {
2944 double q;
2945 qlo[d] = q = POINT_ET(kd, d, querylow[d], floor);
2946 if (q < TTYPE_MIN) {
2947 //WARNING("Error: query value %g is below the minimum range of the tree %g.\n", q, (double)TTYPE_MIN);
2948 qlo[d] = TTYPE_MIN;
2949 } else if (q > TTYPE_MAX) {
2950 // query's low position is more than the tree's max: no overlap is possible.
2951 goto cleanup; //# Modified by Robert Lancaster for the SexySolver Internal Library
2952 }
2953 qhi[d] = q = POINT_ET(kd, d, queryhi [d], ceil );
2954 if (q > TTYPE_MAX) {
2955 //WARNING("Error: query value %g is above the maximum range of the tree %g.\n", q, (double)TTYPE_MAX);
2956 qhi[d] = TTYPE_MAX;
2957 } else if (q < TTYPE_MIN) {
2958 // query's high position is less than the tree's min: no overlap is possible.
2959 goto cleanup; //# Modified by Robert Lancaster for the SexySolver Internal Library
2960 }
2961 }
2962
2963 nodes_contained_rec(kd, 0, qlo, qhi, cb_contained, cb_overlap, cb_extra);
2964
2965 cleanup:
2966 #ifdef _MSC_VER //# Modified by Robert Lancaster for the SexySolver Internal Library
2967 free(qhi);
2968 free(qlo);
2969 #else
2970 return;
2971 #endif
2972 }
2973
MANGLE(kdtree_get_bboxes)2974 int MANGLE(kdtree_get_bboxes)(const kdtree_t* kd, int node,
2975 void* vbblo, void* vbbhi) {
2976 etype* bblo = vbblo;
2977 etype* bbhi = vbbhi;
2978 ttype *tlo=NULL, *thi=NULL;
2979 int D = kd->ndim;
2980 int d;
2981
2982 if (!bboxes(kd, node, &tlo, &thi, D))
2983 return FALSE;
2984
2985 for (d=0; d<D; d++) {
2986 bblo[d] = POINT_TE(kd, d, tlo[d]);
2987 bbhi[d] = POINT_TE(kd, d, thi[d]);
2988 }
2989 return TRUE;
2990 }
2991
MANGLE(kdtree_update_funcs)2992 void MANGLE(kdtree_update_funcs)(kdtree_t* kd) {
2993 kd->fun.get_data = get_data;
2994 kd->fun.copy_data_double = copy_data_double;
2995 kd->fun.get_splitval = MANGLE(kdtree_get_splitval);
2996 kd->fun.get_bboxes = MANGLE(kdtree_get_bboxes);
2997 kd->fun.check = MANGLE(kdtree_check);
2998 kd->fun.fix_bounding_boxes = MANGLE(kdtree_fix_bounding_boxes);
2999 kd->fun.nearest_neighbour_internal = MANGLE(kdtree_nn);
3000 kd->fun.rangesearch = MANGLE(kdtree_rangesearch_options);
3001 kd->fun.nodes_contained = MANGLE(kdtree_nodes_contained);
3002 }
3003
3004