lib/kdtree.c

   1 /* kdtree.c
   2    Implementation of 2d kd trees.
   3
   4    Part of the swftools package.
   5
   6    Copyright (c) 2010 Matthias Kramm <kramm@quiss.org>
   7
   8    This program is free software; you can redistribute it and/or modify
   9    it under the terms of the GNU General Public License as published by
  10    the Free Software Foundation; either version 2 of the License, or
  11    (at your option) any later version.
  12
  13    This program is distributed in the hope that it will be useful,
  14    but WITHOUT ANY WARRANTY; without even the implied warranty of
  15    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16    GNU General Public License for more details.
  17
  18    You should have received a copy of the GNU General Public License
  19    along with this program; if not, write to the Free Software
  20    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA */
  21
  22 #include <stdio.h>
  23 #include <stdlib.h>
  24 #include <assert.h>
  25 #include <memory.h>
  26 #include <limits.h>
  27 #include "q.h"
  28 #include "kdtree.h"
  29
  30 /* 0=right 1=down 2=left 3=up */
  31 static int vx_and[4] = {INT_MAX, 0, INT_MAX, 0};
  32 static int vy_and[4] = {0, INT_MAX, 0, INT_MAX};
  33 static int vx[4] = {1, 0, -1, 0};
  34 static int vy[4] = {0, 1, 0, -1};
  35 static int vsign[4] = {1,1,-1,-1};
  36 static char* vname[4] = {"right", "down", "left", "up"};
  37
  38 kdarea_t* kdarea_new(void*data)
  39 {
  40     NEW(kdarea_t,area);
  41     area->bbox.xmin = INT_MIN;
  42     area->bbox.ymin = INT_MIN;
  43     area->bbox.xmax = INT_MAX;
  44     area->bbox.ymax = INT_MAX;
  45     area->data = data;
  46     return area;
  47 }
  48
  49 kdtree_t* kdtree_new()
  50 {
  51     NEW(kdtree_t,tree);
  52     tree->root = kdarea_new(0);
  53     return tree;
  54 }
  55
  56 static inline int32_t max32(int32_t v1, int32_t v2) {return v1>v2?v1:v2;}
  57 static inline int32_t min32(int32_t v1, int32_t v2) {return v1<v2?v1:v2;}
  58
  59 kdbranch_t*kdbranch_new(int32_t xy, kdtype_t type)
  60 {
  61     NEW(kdbranch_t,b);
  62     b->type = type;
  63     b->xy = xy;
  64     return b;
  65 }
  66
  67 kdarea_t*kdbranch_follow(const kdbranch_t*tree, int32_t x, int32_t y)
  68 {
  69     /*
  70     int follow = 0;
  71     switch(tree->type) {
  72         case KD_LEFT:
  73             follow = (x < tree->xy);
  74         break;
  75         case KD_RIGHT:
  76             follow = (x > tree->xy);
  77         break;
  78         case KD_UP:
  79             follow = (y < tree->xy);
  80         break;
  81         case KD_DOWN:
  82             follow = (y > tree->xy);
  83         break;
  84     }
  85     return &tree->side[follow];
  86     */
  87     int32_t s = x*vx[tree->type] + y*vy[tree->type];
  88     int32_t v = tree->xy*vsign[tree->type];
  89     return tree->side[s < v];
  90 }
  91
  92 static kdarea_list_t* kdarea_list_new(kdarea_t*area)
  93 {
  94     NEW(kdarea_list_t,b);
  95     b->area = area;
  96     b->next = b->prev = b;
  97     return b;
  98 }
  99
 100 static kdarea_list_t*kdarea_list_concatenate(kdarea_list_t*l1, kdarea_list_t*l2)
 101 {
 102     if(!l1) return l2;
 103     if(!l2) return l1;
 104     l2->prev->next = l1->next;
 105     l1->next->prev = l2->prev;
 106     l2->prev = l1;
 107     l1->next = l2;
 108     return l1;
 109 }
 110
 111 static kdbbox_t bbox_for_halfplane(int xy, int dir)
 112 {
 113     kdbbox_t b = {INT_MIN,INT_MIN,INT_MAX,INT_MAX};
 114     switch(dir) {
 115         case KD_LEFT:
 116             b.xmax = xy;
 117         break;
 118         case KD_RIGHT:
 119             b.xmin = xy;
 120         break;
 121         case KD_UP:
 122             b.ymax = xy;
 123         break;
 124         case KD_DOWN:
 125             b.ymin = xy;
 126         break;
 127     }
 128     return b;
 129 }
 130
 131 static kdbbox_t intersect_bbox(const kdbbox_t*box1, const kdbbox_t*box2)
 132 {
 133     kdbbox_t b = *box1;
 134     if(box2->xmin > b.xmin)
 135         b.xmin = box2->xmin;
 136     if(box2->ymin > b.ymin)
 137         b.ymin = box2->ymin;
 138     if(box2->xmax < b.xmax)
 139         b.xmax = box2->xmax;
 140     if(box2->ymax < b.ymax)
 141         b.ymax = box2->ymax;
 142     if(b.xmin > b.xmax)
 143         b.xmax = b.xmin;
 144     if(b.ymin > b.ymax)
 145         b.ymax = b.ymin;
 146     return b;
 147 }
 148
 149 static void kdarea_split(kdarea_t*area, int xy, int dir,
 150                              int32_t x1, int32_t y1,
 151                              int32_t x2, int32_t y2)
 152 {
 153     if(!area->split) {
 154         kdbranch_t*b = area->split = kdbranch_new(xy, dir);
 155         kdbbox_t b1 = bbox_for_halfplane(xy, dir);
 156         kdbbox_t b2 = bbox_for_halfplane(xy, dir^2);
 157         b->side[0] = kdarea_new(area->data);
 158         b->side[1] = kdarea_new(area->data);
 159         b->side[0]->bbox = intersect_bbox(&area->bbox,&b1);
 160         b->side[1]->bbox = intersect_bbox(&area->bbox,&b2);
 161         memcpy(b->side[0]->neighbors, area->neighbors, sizeof(area->neighbors));
 162         memcpy(b->side[1]->neighbors, area->neighbors, sizeof(area->neighbors));
 163         b->side[0]->neighbors[dir^2] = b->side[1];
 164         b->side[1]->neighbors[dir] = b->side[0];
 165         area->data = 0;
 166     } else {
 167         kdbranch_t*split = area->split;
 168         kdarea_t*first = kdbranch_follow(split, x1,y1);
 169         kdarea_t*second = kdbranch_follow(split, x2,y2);
 170
 171         if(!first) {
 172             if(!second) {
 173                 /* line is on top of an already existing segment */
 174                 return;
 175             } else {
 176                 /* first point is directly on the split */
 177                 kdarea_split(second, xy, dir, x1,y1, x2,y2);
 178                 return;
 179             }
 180         } else {
 181             if(!second) {
 182                 /* second point is directly on the split */
 183                 kdarea_split(first, xy, dir, x1,y1, x2,y2);
 184                 return;
 185             } else if(first == second) {
 186                 /* both points are to the same side of this split */
 187                 kdarea_split(first, xy, dir, x1,y1, x2,y2);
 188                 return;
 189             } else {
 190                 kdarea_split(first, xy, dir, x1,y1, x2,y2);
 191                 kdarea_split(second, xy, dir, x1,y1, x2,y2);
 192                 return;
 193             }
 194         }
 195     }
 196 }
 197
 198 static kdarea_list_t* kdarea_filter(kdarea_t*area, int xy, int dir)
 199 {
 200     if(!area->split) {
 201         return kdarea_list_new(area);
 202     } else {
 203         kdbranch_t*branch = area->split;
 204         if((branch->type^dir) == 0) {
 205             /* both filter as well as branch point into the same direction */
 206             if(xy*vsign[dir] >= branch->xy*vsign[dir]) {
 207                 /* filter splits the primary node. We can skip the other one. */
 208 #ifdef DEBUG
 209                 printf("%p: using %p, skipping %p (looking to %s of %d)\n", area, branch->side[0], branch->side[1], vname[dir], xy);
 210 #endif
 211                 return kdarea_filter(branch->side[0], xy, dir);
 212             } else {
 213                 /* filter splits the secondary node. the primary node is left completely intact,
 214                    and returned as such */
 215 #ifdef DEBUG
 216                 printf("%p: expanding %p, filtering %p (looking to %s of %d)\n", area, branch->side[0], branch->side[1], vname[dir], xy);
 217 #endif
 218                 kdarea_list_t*l1 = kdarea_list_new(branch->side[0]);
 219                 kdarea_list_t*l2 = kdarea_filter(branch->side[1], xy, dir);
 220                 return kdarea_list_concatenate(l1,l2);
 221             }
 222         } else if((branch->type^dir) == 2) {
 223             /* filter and branch point into opposite directions */
 224             if(xy*vsign[dir] >= branch->xy*vsign[dir]) {
 225                 // filter splits the secondary node. We can skip the primary node.
 226 #ifdef DEBUG
 227                 printf("%p: skipping %p, using %p (looking to %s of %d)\n", area, branch->side[0], branch->side[1], vname[dir], xy);
 228 #endif
 229                 return kdarea_filter(branch->side[1], xy, dir);
 230             } else {
 231                 /* filter splits the primary node. the secondary node is left completely intact,
 232                    and returned as such */
 233 #ifdef DEBUG
 234                 printf("%p: filtering %p, expanding %p (looking to %s of %d)\n", area, branch->side[0], branch->side[1], vname[dir], xy);
 235 #endif
 236                 kdarea_list_t*l1 = kdarea_filter(branch->side[0], xy, dir);
 237                 kdarea_list_t*l2 = kdarea_list_new(branch->side[1]);
 238                 return kdarea_list_concatenate(l1,l2);
 239             }
 240         } else {
 241             /* filter segment is perpendicular to the node */
 242             return kdarea_list_new(area);
 243         }
 244     }
 245 }
 246
 247 static kdarea_t* kdarea_find(kdarea_t*node, int x, int y)
 248 {
 249     while(node) {
 250         if(!node->split)
 251             break;
 252         node = kdbranch_follow(node->split, x,y);
 253     }
 254     return node;
 255 }
 256
 257 kdarea_t*kdtree_find(kdtree_t*tree, int x, int y)
 258 {
 259     return kdarea_find(tree->root, x,y);
 260 }
 261
 262 void kdarea_list_destroy(kdarea_list_t*list)
 263 {
 264     kdarea_list_t*i = list;
 265     if(i) do {
 266         kdarea_list_t*next = i->next;
 267         free(i);
 268         i = next;
 269     } while(i!=list);
 270 }
 271
 272 static kdarea_list_t* kdarea_list_add(kdarea_list_t*l, kdarea_t*area)
 273 {
 274     return kdarea_list_concatenate(l,kdarea_list_new(area));
 275 }
 276
 277 static kdarea_list_t* kdarea_all_children(kdarea_t*area, int32_t x1, int32_t y1, int32_t x2, int32_t y2, kdarea_list_t*result)
 278 {
 279     if(!area->split) {
 280         if(area->bbox.xmin >= x1 &&
 281            area->bbox.ymin >= y1 &&
 282            area->bbox.xmax <= x2 &&
 283            area->bbox.ymax <= y2) {
 284             result = kdarea_list_add(result, area);
 285         }
 286     } else {
 287         result = kdarea_all_children(area->split->side[0], x1, y1, x2, y2, result);
 288         result = kdarea_all_children(area->split->side[1], x1, y1, x2, y2, result);
 289     }
 290     return result;
 291 }
 292
 293 /* return all areas that are contained in, or partly intersect, the given bounding box */
 294 kdarea_list_t* kdtree_filter(kdtree_t*tree, int32_t x1, int32_t y1, int32_t x2, int32_t y2, char leafs)
 295 {
 296     kdarea_list_t*result = 0;
 297     kdarea_list_t*branches1 = kdarea_filter(tree->root, x2, KD_LEFT);
 298     kdarea_list_t*i = branches1;
 299
 300 #ifdef DEBUG
 301     kdarea_list_t*u = branches1;
 302     if(u) do {printf("%p [%d %d %d %d] is to the left of %d\n", u->area, u->area->bbox.xmin, u->area->bbox.ymin, u->area->bbox.xmax, u->area->bbox.ymax, x2);u = u->next;} while(u!=branches1);
 303 #endif
 304     if(i) do {
 305         kdarea_list_t*branches2 = kdarea_filter(i->area, y2, KD_UP);
 306         kdarea_list_t*j = branches2;
 307
 308 #ifdef DEBUG
 309         kdarea_list_t*u = branches2;
 310         if(u) do {printf("%p [%d %d %d %d] is above %d\n", u->area, u->area->bbox.xmin, u->area->bbox.ymin, u->area->bbox.xmax, u->area->bbox.ymax, y2);u = u->next;} while(u!=branches2);
 311 #endif
 312         if(j) do {
 313             kdarea_list_t*branches3 = kdarea_filter(j->area, x1, KD_RIGHT);
 314             kdarea_list_t*k = branches3;
 315 #ifdef DEBUG
 316             kdarea_list_t*u = branches3;
 317             if(u) do {printf("%p [%d %d %d %d] is to the right of %d\n", u->area, u->area->bbox.xmin, u->area->bbox.ymin, u->area->bbox.xmax, u->area->bbox.ymax, x1);u = u->next;} while(u!=branches3);
 318 #endif
 319             if(k) do {
 320                 kdarea_list_t*branches4 = kdarea_filter(k->area, y1, KD_DOWN);
 321                 kdarea_list_t*l = branches4;
 322 #ifdef DEBUG
 323                 kdarea_list_t*u = branches4;
 324                 if(u) do {printf("%p [%d %d %d %d] is below %d\n", u->area, u->area->bbox.xmin, u->area->bbox.ymin, u->area->bbox.xmax, u->area->bbox.ymax, y1);u = u->next;} while(u!=branches4);
 325 #endif
 326                 if(leafs) {
 327                     if(l) do {
 328                         result = kdarea_list_concatenate(result, kdarea_all_children(l->area, x1, y1, x2, y2, 0));
 329                         l = l->next;
 330                     } while(l!=branches4);
 331                     kdarea_list_destroy(branches4);
 332                 } else {
 333                     result = kdarea_list_concatenate(result, l);
 334                 }
 335                 k = k->next;
 336             } while(k!=branches3);
 337             kdarea_list_destroy(branches3);
 338             j = j->next;
 339         } while(j!=branches2);
 340         kdarea_list_destroy(branches2);
 341         i = i->next;
 342     } while(i!=branches1);
 343     kdarea_list_destroy(branches1);
 344     return result;
 345 }
 346
 347 static void kdtree_modify_box(kdtree_t*tree, int32_t x1, int32_t y1, int32_t x2, int32_t y2, void*(*f)(void*user,void*data), void*user)
 348 {
 349     kdarea_split(tree->root, x2, KD_LEFT,  x2,y1, x2,y2);
 350     kdarea_split(tree->root, y2, KD_UP,    x1,y2, x2,y2);
 351     kdarea_split(tree->root, x1, KD_RIGHT, x1,y1, x1,y2);
 352     kdarea_split(tree->root, y1, KD_DOWN,  x1,y1, x2,y1);
 353 #ifdef DEBUG
 354     printf("inserting (%d,%d,%d,%d) %p\n", x1, y1, x2, y2, user);
 355 #endif
 356     kdarea_list_t*l = kdtree_filter(tree, x1, y1, x2, y2, 1);
 357     kdarea_list_t*i = l;
 358     if(l) do {
 359 #ifdef DEBUG
 360         printf("%p [%d,%d,%d,%d], is contained in [%d %d %d %d]\n", i->area,
 361                 i->area->bbox.xmin,
 362                 i->area->bbox.ymin,
 363                 i->area->bbox.xmax,
 364                 i->area->bbox.ymax,
 365                 x1, y1, x2, y2);
 366 #endif
 367         i->area->data = f(user, i->area->data);
 368         i = i->next;
 369     } while(i!=l);
 370     kdarea_list_destroy(l);
 371 }
 372
 373 static void* overwrite(void*user, void*data)
 374 {
 375     return user;
 376 }
 377
 378 void kdtree_add_box(kdtree_t*tree, int32_t x1, int32_t y1, int32_t x2, int32_t y2, void*data)
 379 {
 380     kdtree_modify_box(tree, x1, y1, x2, y2, overwrite, data);
 381 }
 382
 383 static void* add_to_dict(void*user, void*data)
 384 {
 385     dict_t*items = (dict_t*)user;
 386     if(!dict_contains(items, data)) {
 387         dict_put(items, data, data);
 388     }
 389     return data;
 390 }
 391
 392 kdresult_list_t*kdtree_find_in_box(kdtree_t*tree, int32_t x1, int32_t y1, int32_t x2, int32_t y2)
 393 {
 394     dict_t*items = dict_new2(&ptr_type);
 395     kdtree_modify_box(tree, x1, y1, x2, y2, add_to_dict, items);
 396     kdresult_list_t*list = 0;
 397     DICT_ITERATE_KEY(items, void*, d) {
 398         if(d) {
 399             NEW(kdresult_list_t,r);
 400             r->data = d;
 401             r->next = list;
 402             list = r;
 403         }
 404     };
 405     dict_destroy(items);
 406     return list;
 407 }
 408
 409 kdarea_t*kdarea_neighbor(kdarea_t*area, int dir, int xy)
 410 {
 411     int x,y;
 412     switch(dir) {
 413         case KD_LEFT:
 414             x = area->bbox.xmin;
 415             y = xy;
 416         break;
 417         case KD_RIGHT:
 418             x = area->bbox.xmax;
 419             y = xy;
 420         break;
 421         case KD_UP:
 422             x = xy;
 423             y = area->bbox.ymin;
 424         break;
 425         case KD_DOWN:
 426             x = xy;
 427             y = area->bbox.ymax;
 428         break;
 429     }
 430     kdarea_t*n = area->neighbors[dir];
 431     if(!n)
 432         return 0;
 433     return kdarea_find(n, x, y);
 434 }
 435
 436 static void do_indent(int l)
 437 {
 438     int i;
 439     for(i=0;i<l;i++)
 440         printf(" ");
 441 }
 442
 443 void kdarea_print(kdarea_t*area, int indent);
 444 void kdbranch_print(kdbranch_t*branch, int indent)
 445 {
 446     do_indent(indent);printf("[%p] branch (%s, %d)\n", branch, vname[branch->type], branch->xy);
 447     kdbbox_t b = bbox_for_halfplane(branch->xy, branch->type);
 448     kdarea_print(branch->side[0], indent+4);
 449     kdarea_print(branch->side[1], indent+4);
 450 }
 451
 452 void kdarea_print(kdarea_t*area, int indent)
 453 {
 454     int i;
 455     assert(area);
 456     do_indent(indent);printf("[%p] area (%d,%d,%d,%d) %p (l:%p r:%p u:%p d:%p)\n", area,
 457             area->bbox.xmin,
 458             area->bbox.ymin,
 459             area->bbox.xmax,
 460             area->bbox.ymax,
 461             area->data,
 462             area->neighbors[KD_LEFT],
 463             area->neighbors[KD_RIGHT],
 464             area->neighbors[KD_UP],
 465             area->neighbors[KD_DOWN]);
 466     if(area->split) {
 467         kdbranch_print(area->split, indent+4);
 468     }
 469 }
 470
 471 void kdtree_print(kdtree_t*tree)
 472 {
 473     kdarea_print(tree->root, 0);
 474 }
 475
 476
 477 void kdbranch_destroy(kdbranch_t*b)
 478 {
 479     if(b->side[0]) {
 480         kdarea_destroy(b->side[0]);
 481         b->side[0] = 0;
 482     }
 483     if(b->side[1]) {
 484         kdarea_destroy(b->side[1]);
 485         b->side[1] = 0;
 486     }
 487     free(b);
 488 }
 489
 490 void kdarea_destroy(kdarea_t*area)
 491 {
 492     if(area->split) {
 493         kdbranch_destroy(area->split);
 494     }
 495     free(area);
 496 }
 497
 498 void kdtree_destroy(kdtree_t*tree)
 499 {
 500     kdarea_destroy(tree->root);
 501     tree->root = 0;
 502     free(tree);
 503 }
 504
 505 #ifdef MAIN
 506 int main()
 507 {
 508     assert((1^vx[2]) < 0);
 509
 510     kdtree_t*tree = kdtree_new();
 511     kdtree_add_box(tree, 10,30,20,40, "hello world");
 512     kdtree_add_box(tree, 12,50,15,60, "hello world");
 513     //kdtree_print(tree);
 514     kdarea_t*a = kdtree_find(tree, 15,35);
 515     kdarea_t*left = kdarea_neighbor(a, KD_LEFT, /*y*/35);
 516     kdarea_t*right = kdarea_neighbor(a, KD_RIGHT, /*y*/35);
 517     kdarea_t*up = kdarea_neighbor(a, KD_UP, /*x*/15);
 518     kdarea_t*down = kdarea_neighbor(a, KD_DOWN, /*x*/15);
 519
 520     a = kdtree_find(tree, 15,25);
 521     assert(!a || !a->data);
 522     a = kdtree_find(tree, 15,45);
 523     assert(!a || !a->data);
 524     a = kdtree_find(tree, 5,35);
 525     assert(!a || !a->data);
 526     a = kdtree_find(tree, 45,35);
 527     assert(!a || !a->data);
 528
 529     kdtree_destroy(tree);
 530 }
 531 #endif