eleni.mutantstargoat.com Git - hair/blob - src/kdtree.c

   1 /*
   2 This file is part of ``kdtree'', a library for working with kd-trees.
   3 Copyright (C) 2007-2011 John Tsiombikas <nuclear@member.fsf.org>
   4
   5 Redistribution and use in source and binary forms, with or without
   6 modification, are permitted provided that the following conditions are met:
   7
   8 1. Redistributions of source code must retain the above copyright notice, this
   9    list of conditions and the following disclaimer.
  10 2. Redistributions in binary form must reproduce the above copyright notice,
  11    this list of conditions and the following disclaimer in the documentation
  12    and/or other materials provided with the distribution.
  13 3. The name of the author may not be used to endorse or promote products
  14    derived from this software without specific prior written permission.
  15
  16 THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
  17 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
  18 MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO
  19 EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  20 EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
  21 OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  22 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  23 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
  24 IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
  25 OF SUCH DAMAGE.
  26 */
  27 /* single nearest neighbor search written by Tamas Nepusz <tamas@cs.rhul.ac.uk> */
  28
  29 #ifdef HAVE_CONFIG_H
  30 #include <config.h>
  31 #endif
  32
  33 #include <stdio.h>
  34 #include <stdlib.h>
  35 #include <string.h>
  36 #include <math.h>
  37 #include "kdtree.h"
  38
  39 #if defined(WIN32) || defined(__WIN32__)
  40 #include <malloc.h>
  41 #endif
  42
  43 #ifdef USE_LIST_NODE_ALLOCATOR
  44
  45 #ifndef NO_PTHREADS
  46 #include <pthread.h>
  47 #else
  48
  49 #ifndef I_WANT_THREAD_BUGS
  50 #error "You are compiling with the fast list node allocator, with pthreads disabled! This WILL break if used from multiple threads."
  51 #endif  /* I want thread bugs */
  52
  53 #endif  /* pthread support */
  54 #endif  /* use list node allocator */
  55
  56 struct kdhyperrect {
  57         int dim;
  58         double *min, *max;              /* minimum/maximum coords */
  59 };
  60
  61 struct kdnode {
  62         double *pos;
  63         int dir;
  64         void *data;
  65
  66         struct kdnode *left, *right;    /* negative/positive side */
  67 };
  68
  69 struct res_node {
  70         struct kdnode *item;
  71         double dist_sq;
  72         struct res_node *next;
  73 };
  74
  75 struct kdtree {
  76         int dim;
  77         struct kdnode *root;
  78         struct kdhyperrect *rect;
  79         void (*destr)(void*);
  80 };
  81
  82 struct kdres {
  83         struct kdtree *tree;
  84         struct res_node *rlist, *riter;
  85         int size;
  86 };
  87
  88 #define SQ(x)                   ((x) * (x))
  89
  90
  91 static void clear_rec(struct kdnode *node, void (*destr)(void*));
  92 static int insert_rec(struct kdnode **node, const double *pos, void *data, int dir, int dim);
  93 static int rlist_insert(struct res_node *list, struct kdnode *item, double dist_sq);
  94 static void clear_results(struct kdres *set);
  95
  96 static struct kdhyperrect* hyperrect_create(int dim, const double *min, const double *max);
  97 static void hyperrect_free(struct kdhyperrect *rect);
  98 static struct kdhyperrect* hyperrect_duplicate(const struct kdhyperrect *rect);
  99 static void hyperrect_extend(struct kdhyperrect *rect, const double *pos);
 100 static double hyperrect_dist_sq(struct kdhyperrect *rect, const double *pos);
 101
 102 #ifdef USE_LIST_NODE_ALLOCATOR
 103 static struct res_node *alloc_resnode(void);
 104 static void free_resnode(struct res_node*);
 105 #else
 106 #define alloc_resnode()         malloc(sizeof(struct res_node))
 107 #define free_resnode(n)         free(n)
 108 #endif
 109
 110
 111
 112 struct kdtree *kd_create(int k)
 113 {
 114         struct kdtree *tree;
 115
 116         if(!(tree = malloc(sizeof *tree))) {
 117                 return 0;
 118         }
 119
 120         tree->dim = k;
 121         tree->root = 0;
 122         tree->destr = 0;
 123         tree->rect = 0;
 124
 125         return tree;
 126 }
 127
 128 void kd_free(struct kdtree *tree)
 129 {
 130         if(tree) {
 131                 kd_clear(tree);
 132                 free(tree);
 133         }
 134 }
 135
 136 static void clear_rec(struct kdnode *node, void (*destr)(void*))
 137 {
 138         if(!node) return;
 139
 140         clear_rec(node->left, destr);
 141         clear_rec(node->right, destr);
 142
 143         if(destr) {
 144                 destr(node->data);
 145         }
 146         free(node->pos);
 147         free(node);
 148 }
 149
 150 void kd_clear(struct kdtree *tree)
 151 {
 152         clear_rec(tree->root, tree->destr);
 153         tree->root = 0;
 154
 155         if (tree->rect) {
 156                 hyperrect_free(tree->rect);
 157                 tree->rect = 0;
 158         }
 159 }
 160
 161 void kd_data_destructor(struct kdtree *tree, void (*destr)(void*))
 162 {
 163         tree->destr = destr;
 164 }
 165
 166
 167 static int insert_rec(struct kdnode **nptr, const double *pos, void *data, int dir, int dim)
 168 {
 169         int new_dir;
 170         struct kdnode *node;
 171
 172         if(!*nptr) {
 173                 if(!(node = malloc(sizeof *node))) {
 174                         return -1;
 175                 }
 176                 if(!(node->pos = malloc(dim * sizeof *node->pos))) {
 177                         free(node);
 178                         return -1;
 179                 }
 180                 memcpy(node->pos, pos, dim * sizeof *node->pos);
 181                 node->data = data;
 182                 node->dir = dir;
 183                 node->left = node->right = 0;
 184                 *nptr = node;
 185                 return 0;
 186         }
 187
 188         node = *nptr;
 189         new_dir = (node->dir + 1) % dim;
 190         if(pos[node->dir] < node->pos[node->dir]) {
 191                 return insert_rec(&(*nptr)->left, pos, data, new_dir, dim);
 192         }
 193         return insert_rec(&(*nptr)->right, pos, data, new_dir, dim);
 194 }
 195
 196 int kd_insert(struct kdtree *tree, const double *pos, void *data)
 197 {
 198         if (insert_rec(&tree->root, pos, data, 0, tree->dim)) {
 199                 return -1;
 200         }
 201
 202         if (tree->rect == 0) {
 203                 tree->rect = hyperrect_create(tree->dim, pos, pos);
 204         } else {
 205                 hyperrect_extend(tree->rect, pos);
 206         }
 207
 208         return 0;
 209 }
 210
 211 int kd_insertf(struct kdtree *tree, const float *pos, void *data)
 212 {
 213         static double sbuf[16];
 214         double *bptr, *buf = 0;
 215         int res, dim = tree->dim;
 216
 217         if(dim > 16) {
 218 #ifndef NO_ALLOCA
 219                 if(dim <= 256)
 220                         bptr = buf = alloca(dim * sizeof *bptr);
 221                 else
 222 #endif
 223                         if(!(bptr = buf = malloc(dim * sizeof *bptr))) {
 224                                 return -1;
 225                         }
 226         } else {
 227                 bptr = buf = sbuf;
 228         }
 229
 230         while(dim-- > 0) {
 231                 *bptr++ = *pos++;
 232         }
 233
 234         res = kd_insert(tree, buf, data);
 235 #ifndef NO_ALLOCA
 236         if(tree->dim > 256)
 237 #else
 238         if(tree->dim > 16)
 239 #endif
 240                 free(buf);
 241         return res;
 242 }
 243
 244 int kd_insert3(struct kdtree *tree, double x, double y, double z, void *data)
 245 {
 246         double buf[3];
 247         buf[0] = x;
 248         buf[1] = y;
 249         buf[2] = z;
 250         return kd_insert(tree, buf, data);
 251 }
 252
 253 int kd_insert3f(struct kdtree *tree, float x, float y, float z, void *data)
 254 {
 255         double buf[3];
 256         buf[0] = x;
 257         buf[1] = y;
 258         buf[2] = z;
 259         return kd_insert(tree, buf, data);
 260 }
 261
 262 static int find_nearest(struct kdnode *node, const double *pos, double range, struct res_node *list, int ordered, int dim)
 263 {
 264         double dist_sq, dx;
 265         int i, ret, added_res = 0;
 266
 267         if(!node) return 0;
 268
 269         dist_sq = 0;
 270         for(i=0; i<dim; i++) {
 271                 dist_sq += SQ(node->pos[i] - pos[i]);
 272         }
 273         if(dist_sq <= SQ(range)) {
 274                 if(rlist_insert(list, node, ordered ? dist_sq : -1.0) == -1) {
 275                         return -1;
 276                 }
 277                 added_res = 1;
 278         }
 279
 280         dx = pos[node->dir] - node->pos[node->dir];
 281
 282         ret = find_nearest(dx <= 0.0 ? node->left : node->right, pos, range, list, ordered, dim);
 283         if(ret >= 0 && fabs(dx) < range) {
 284                 added_res += ret;
 285                 ret = find_nearest(dx <= 0.0 ? node->right : node->left, pos, range, list, ordered, dim);
 286         }
 287         if(ret == -1) {
 288                 return -1;
 289         }
 290         added_res += ret;
 291
 292         return added_res;
 293 }
 294
 295 #if 0
 296 static int find_nearest_n(struct kdnode *node, const double *pos, double range, int num, struct rheap *heap, int dim)
 297 {
 298         double dist_sq, dx;
 299         int i, ret, added_res = 0;
 300
 301         if(!node) return 0;
 302
 303         /* if the photon is close enough, add it to the result heap */
 304         dist_sq = 0;
 305         for(i=0; i<dim; i++) {
 306                 dist_sq += SQ(node->pos[i] - pos[i]);
 307         }
 308         if(dist_sq <= range_sq) {
 309                 if(heap->size >= num) {
 310                         /* get furthest element */
 311                         struct res_node *maxelem = rheap_get_max(heap);
 312
 313                         /* and check if the new one is closer than that */
 314                         if(maxelem->dist_sq > dist_sq) {
 315                                 rheap_remove_max(heap);
 316
 317                                 if(rheap_insert(heap, node, dist_sq) == -1) {
 318                                         return -1;
 319                                 }
 320                                 added_res = 1;
 321
 322                                 range_sq = dist_sq;
 323                         }
 324                 } else {
 325                         if(rheap_insert(heap, node, dist_sq) == -1) {
 326                                 return =1;
 327                         }
 328                         added_res = 1;
 329                 }
 330         }
 331
 332
 333         /* find signed distance from the splitting plane */
 334         dx = pos[node->dir] - node->pos[node->dir];
 335
 336         ret = find_nearest_n(dx <= 0.0 ? node->left : node->right, pos, range, num, heap, dim);
 337         if(ret >= 0 && fabs(dx) < range) {
 338                 added_res += ret;
 339                 ret = find_nearest_n(dx <= 0.0 ? node->right : node->left, pos, range, num, heap, dim);
 340         }
 341
 342 }
 343 #endif
 344
 345 static void kd_nearest_i(struct kdnode *node, const double *pos, struct kdnode **result, double *result_dist_sq, struct kdhyperrect* rect)
 346 {
 347         int dir = node->dir;
 348         int i;
 349         double dummy, dist_sq;
 350         struct kdnode *nearer_subtree, *farther_subtree;
 351         double *nearer_hyperrect_coord, *farther_hyperrect_coord;
 352
 353         /* Decide whether to go left or right in the tree */
 354         dummy = pos[dir] - node->pos[dir];
 355         if (dummy <= 0) {
 356                 nearer_subtree = node->left;
 357                 farther_subtree = node->right;
 358                 nearer_hyperrect_coord = rect->max + dir;
 359                 farther_hyperrect_coord = rect->min + dir;
 360         } else {
 361                 nearer_subtree = node->right;
 362                 farther_subtree = node->left;
 363                 nearer_hyperrect_coord = rect->min + dir;
 364                 farther_hyperrect_coord = rect->max + dir;
 365         }
 366
 367         if (nearer_subtree) {
 368                 /* Slice the hyperrect to get the hyperrect of the nearer subtree */
 369                 dummy = *nearer_hyperrect_coord;
 370                 *nearer_hyperrect_coord = node->pos[dir];
 371                 /* Recurse down into nearer subtree */
 372                 kd_nearest_i(nearer_subtree, pos, result, result_dist_sq, rect);
 373                 /* Undo the slice */
 374                 *nearer_hyperrect_coord = dummy;
 375         }
 376
 377         /* Check the distance of the point at the current node, compare it
 378          * with our best so far */
 379         dist_sq = 0;
 380         for(i=0; i < rect->dim; i++) {
 381                 dist_sq += SQ(node->pos[i] - pos[i]);
 382         }
 383         if (dist_sq < *result_dist_sq) {
 384                 *result = node;
 385                 *result_dist_sq = dist_sq;
 386         }
 387
 388         if (farther_subtree) {
 389                 /* Get the hyperrect of the farther subtree */
 390                 dummy = *farther_hyperrect_coord;
 391                 *farther_hyperrect_coord = node->pos[dir];
 392                 /* Check if we have to recurse down by calculating the closest
 393                  * point of the hyperrect and see if it's closer than our
 394                  * minimum distance in result_dist_sq. */
 395                 if (hyperrect_dist_sq(rect, pos) < *result_dist_sq) {
 396                         /* Recurse down into farther subtree */
 397                         kd_nearest_i(farther_subtree, pos, result, result_dist_sq, rect);
 398                 }
 399                 /* Undo the slice on the hyperrect */
 400                 *farther_hyperrect_coord = dummy;
 401         }
 402 }
 403
 404 struct kdres *kd_nearest(struct kdtree *kd, const double *pos)
 405 {
 406         struct kdhyperrect *rect;
 407         struct kdnode *result;
 408         struct kdres *rset;
 409         double dist_sq;
 410         int i;
 411
 412         if (!kd) return 0;
 413         if (!kd->rect) return 0;
 414
 415         /* Allocate result set */
 416         if(!(rset = malloc(sizeof *rset))) {
 417                 return 0;
 418         }
 419         if(!(rset->rlist = alloc_resnode())) {
 420                 free(rset);
 421                 return 0;
 422         }
 423         rset->rlist->next = 0;
 424         rset->tree = kd;
 425
 426         /* Duplicate the bounding hyperrectangle, we will work on the copy */
 427         if (!(rect = hyperrect_duplicate(kd->rect))) {
 428                 kd_res_free(rset);
 429                 return 0;
 430         }
 431
 432         /* Our first guesstimate is the root node */
 433         result = kd->root;
 434         dist_sq = 0;
 435         for (i = 0; i < kd->dim; i++)
 436                 dist_sq += SQ(result->pos[i] - pos[i]);
 437
 438         /* Search for the nearest neighbour recursively */
 439         kd_nearest_i(kd->root, pos, &result, &dist_sq, rect);
 440
 441         /* Free the copy of the hyperrect */
 442         hyperrect_free(rect);
 443
 444         /* Store the result */
 445         if (result) {
 446                 if (rlist_insert(rset->rlist, result, -1.0) == -1) {
 447                         kd_res_free(rset);
 448                         return 0;
 449                 }
 450                 rset->size = 1;
 451                 kd_res_rewind(rset);
 452                 return rset;
 453         } else {
 454                 kd_res_free(rset);
 455                 return 0;
 456         }
 457 }
 458
 459 struct kdres *kd_nearestf(struct kdtree *tree, const float *pos)
 460 {
 461         static double sbuf[16];
 462         double *bptr, *buf = 0;
 463         int dim = tree->dim;
 464         struct kdres *res;
 465
 466         if(dim > 16) {
 467 #ifndef NO_ALLOCA
 468                 if(dim <= 256)
 469                         bptr = buf = alloca(dim * sizeof *bptr);
 470                 else
 471 #endif
 472                         if(!(bptr = buf = malloc(dim * sizeof *bptr))) {
 473                                 return 0;
 474                         }
 475         } else {
 476                 bptr = buf = sbuf;
 477         }
 478
 479         while(dim-- > 0) {
 480                 *bptr++ = *pos++;
 481         }
 482
 483         res = kd_nearest(tree, buf);
 484 #ifndef NO_ALLOCA
 485         if(tree->dim > 256)
 486 #else
 487         if(tree->dim > 16)
 488 #endif
 489                 free(buf);
 490         return res;
 491 }
 492
 493 struct kdres *kd_nearest3(struct kdtree *tree, double x, double y, double z)
 494 {
 495         double pos[3];
 496         pos[0] = x;
 497         pos[1] = y;
 498         pos[2] = z;
 499         return kd_nearest(tree, pos);
 500 }
 501
 502 struct kdres *kd_nearest3f(struct kdtree *tree, float x, float y, float z)
 503 {
 504         double pos[3];
 505         pos[0] = x;
 506         pos[1] = y;
 507         pos[2] = z;
 508         return kd_nearest(tree, pos);
 509 }
 510
 511 /* ---- nearest N search ---- */
 512 /*
 513 static kdres *kd_nearest_n(struct kdtree *kd, const double *pos, int num)
 514 {
 515         int ret;
 516         struct kdres *rset;
 517
 518         if(!(rset = malloc(sizeof *rset))) {
 519                 return 0;
 520         }
 521         if(!(rset->rlist = alloc_resnode())) {
 522                 free(rset);
 523                 return 0;
 524         }
 525         rset->rlist->next = 0;
 526         rset->tree = kd;
 527
 528         if((ret = find_nearest_n(kd->root, pos, range, num, rset->rlist, kd->dim)) == -1) {
 529                 kd_res_free(rset);
 530                 return 0;
 531         }
 532         rset->size = ret;
 533         kd_res_rewind(rset);
 534         return rset;
 535 }*/
 536
 537 struct kdres *kd_nearest_range(struct kdtree *kd, const double *pos, double range)
 538 {
 539         int ret;
 540         struct kdres *rset;
 541
 542         if(!(rset = malloc(sizeof *rset))) {
 543                 return 0;
 544         }
 545         if(!(rset->rlist = alloc_resnode())) {
 546                 free(rset);
 547                 return 0;
 548         }
 549         rset->rlist->next = 0;
 550         rset->tree = kd;
 551
 552         if((ret = find_nearest(kd->root, pos, range, rset->rlist, 0, kd->dim)) == -1) {
 553                 kd_res_free(rset);
 554                 return 0;
 555         }
 556         rset->size = ret;
 557         kd_res_rewind(rset);
 558         return rset;
 559 }
 560
 561 struct kdres *kd_nearest_rangef(struct kdtree *kd, const float *pos, float range)
 562 {
 563         static double sbuf[16];
 564         double *bptr, *buf = 0;
 565         int dim = kd->dim;
 566         struct kdres *res;
 567
 568         if(dim > 16) {
 569 #ifndef NO_ALLOCA
 570                 if(dim <= 256)
 571                         bptr = buf = alloca(dim * sizeof *bptr);
 572                 else
 573 #endif
 574                         if(!(bptr = buf = malloc(dim * sizeof *bptr))) {
 575                                 return 0;
 576                         }
 577         } else {
 578                 bptr = buf = sbuf;
 579         }
 580
 581         while(dim-- > 0) {
 582                 *bptr++ = *pos++;
 583         }
 584
 585         res = kd_nearest_range(kd, buf, range);
 586 #ifndef NO_ALLOCA
 587         if(kd->dim > 256)
 588 #else
 589         if(kd->dim > 16)
 590 #endif
 591                 free(buf);
 592         return res;
 593 }
 594
 595 struct kdres *kd_nearest_range3(struct kdtree *tree, double x, double y, double z, double range)
 596 {
 597         double buf[3];
 598         buf[0] = x;
 599         buf[1] = y;
 600         buf[2] = z;
 601         return kd_nearest_range(tree, buf, range);
 602 }
 603
 604 struct kdres *kd_nearest_range3f(struct kdtree *tree, float x, float y, float z, float range)
 605 {
 606         double buf[3];
 607         buf[0] = x;
 608         buf[1] = y;
 609         buf[2] = z;
 610         return kd_nearest_range(tree, buf, range);
 611 }
 612
 613 void kd_res_free(struct kdres *rset)
 614 {
 615         clear_results(rset);
 616         free_resnode(rset->rlist);
 617         free(rset);
 618 }
 619
 620 int kd_res_size(struct kdres *set)
 621 {
 622         return (set->size);
 623 }
 624
 625 void kd_res_rewind(struct kdres *rset)
 626 {
 627         rset->riter = rset->rlist->next;
 628 }
 629
 630 int kd_res_end(struct kdres *rset)
 631 {
 632         return rset->riter == 0;
 633 }
 634
 635 int kd_res_next(struct kdres *rset)
 636 {
 637         rset->riter = rset->riter->next;
 638         return rset->riter != 0;
 639 }
 640
 641 void *kd_res_item(struct kdres *rset, double *pos)
 642 {
 643         if(rset->riter) {
 644                 if(pos) {
 645                         memcpy(pos, rset->riter->item->pos, rset->tree->dim * sizeof *pos);
 646                 }
 647                 return rset->riter->item->data;
 648         }
 649         return 0;
 650 }
 651
 652 void *kd_res_itemf(struct kdres *rset, float *pos)
 653 {
 654         if(rset->riter) {
 655                 if(pos) {
 656                         int i;
 657                         for(i=0; i<rset->tree->dim; i++) {
 658                                 pos[i] = rset->riter->item->pos[i];
 659                         }
 660                 }
 661                 return rset->riter->item->data;
 662         }
 663         return 0;
 664 }
 665
 666 void *kd_res_item3(struct kdres *rset, double *x, double *y, double *z)
 667 {
 668         if(rset->riter) {
 669                 if(*x) *x = rset->riter->item->pos[0];
 670                 if(*y) *y = rset->riter->item->pos[1];
 671                 if(*z) *z = rset->riter->item->pos[2];
 672                 return rset->riter->item->data;
 673         }
 674         return 0;
 675 }
 676
 677 void *kd_res_item3f(struct kdres *rset, float *x, float *y, float *z)
 678 {
 679         if(rset->riter) {
 680                 if(*x) *x = rset->riter->item->pos[0];
 681                 if(*y) *y = rset->riter->item->pos[1];
 682                 if(*z) *z = rset->riter->item->pos[2];
 683                 return rset->riter->item->data;
 684         }
 685         return 0;
 686 }
 687
 688 void *kd_res_item_data(struct kdres *set)
 689 {
 690         return kd_res_item(set, 0);
 691 }
 692
 693 /* ---- hyperrectangle helpers ---- */
 694 static struct kdhyperrect* hyperrect_create(int dim, const double *min, const double *max)
 695 {
 696         size_t size = dim * sizeof(double);
 697         struct kdhyperrect* rect = 0;
 698
 699         if (!(rect = malloc(sizeof(struct kdhyperrect)))) {
 700                 return 0;
 701         }
 702
 703         rect->dim = dim;
 704         if (!(rect->min = malloc(size))) {
 705                 free(rect);
 706                 return 0;
 707         }
 708         if (!(rect->max = malloc(size))) {
 709                 free(rect->min);
 710                 free(rect);
 711                 return 0;
 712         }
 713         memcpy(rect->min, min, size);
 714         memcpy(rect->max, max, size);
 715
 716         return rect;
 717 }
 718
 719 static void hyperrect_free(struct kdhyperrect *rect)
 720 {
 721         free(rect->min);
 722         free(rect->max);
 723         free(rect);
 724 }
 725
 726 static struct kdhyperrect* hyperrect_duplicate(const struct kdhyperrect *rect)
 727 {
 728         return hyperrect_create(rect->dim, rect->min, rect->max);
 729 }
 730
 731 static void hyperrect_extend(struct kdhyperrect *rect, const double *pos)
 732 {
 733         int i;
 734
 735         for (i=0; i < rect->dim; i++) {
 736                 if (pos[i] < rect->min[i]) {
 737                         rect->min[i] = pos[i];
 738                 }
 739                 if (pos[i] > rect->max[i]) {
 740                         rect->max[i] = pos[i];
 741                 }
 742         }
 743 }
 744
 745 static double hyperrect_dist_sq(struct kdhyperrect *rect, const double *pos)
 746 {
 747         int i;
 748         double result = 0;
 749
 750         for (i=0; i < rect->dim; i++) {
 751                 if (pos[i] < rect->min[i]) {
 752                         result += SQ(rect->min[i] - pos[i]);
 753                 } else if (pos[i] > rect->max[i]) {
 754                         result += SQ(rect->max[i] - pos[i]);
 755                 }
 756         }
 757
 758         return result;
 759 }
 760
 761 /* ---- static helpers ---- */
 762
 763 #ifdef USE_LIST_NODE_ALLOCATOR
 764 /* special list node allocators. */
 765 static struct res_node *free_nodes;
 766
 767 #ifndef NO_PTHREADS
 768 static pthread_mutex_t alloc_mutex = PTHREAD_MUTEX_INITIALIZER;
 769 #endif
 770
 771 static struct res_node *alloc_resnode(void)
 772 {
 773         struct res_node *node;
 774
 775 #ifndef NO_PTHREADS
 776         pthread_mutex_lock(&alloc_mutex);
 777 #endif
 778
 779         if(!free_nodes) {
 780                 node = malloc(sizeof *node);
 781         } else {
 782                 node = free_nodes;
 783                 free_nodes = free_nodes->next;
 784                 node->next = 0;
 785         }
 786
 787 #ifndef NO_PTHREADS
 788         pthread_mutex_unlock(&alloc_mutex);
 789 #endif
 790
 791         return node;
 792 }
 793
 794 static void free_resnode(struct res_node *node)
 795 {
 796 #ifndef NO_PTHREADS
 797         pthread_mutex_lock(&alloc_mutex);
 798 #endif
 799
 800         node->next = free_nodes;
 801         free_nodes = node;
 802
 803 #ifndef NO_PTHREADS
 804         pthread_mutex_unlock(&alloc_mutex);
 805 #endif
 806 }
 807 #endif  /* list node allocator or not */
 808
 809
 810 /* inserts the item. if dist_sq is >= 0, then do an ordered insert */
 811 /* TODO make the ordering code use heapsort */
 812 static int rlist_insert(struct res_node *list, struct kdnode *item, double dist_sq)
 813 {
 814         struct res_node *rnode;
 815
 816         if(!(rnode = alloc_resnode())) {
 817                 return -1;
 818         }
 819         rnode->item = item;
 820         rnode->dist_sq = dist_sq;
 821
 822         if(dist_sq >= 0.0) {
 823                 while(list->next && list->next->dist_sq < dist_sq) {
 824                         list = list->next;
 825                 }
 826         }
 827         rnode->next = list->next;
 828         list->next = rnode;
 829         return 0;
 830 }
 831
 832 static void clear_results(struct kdres *rset)
 833 {
 834         struct res_node *tmp, *node = rset->rlist->next;
 835
 836         while(node) {
 837                 tmp = node;
 838                 node = node->next;
 839                 free_resnode(tmp);
 840         }
 841
 842         rset->rlist->next = 0;
 843 }