lib/kdtree.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <assert.h>
   4 #include <memory.h>
   5 #include <limits.h>
   6 #include "q.h"
   7 #include "kdtree.h"
   8
   9 /*
  10               |          |
  11               |          |
  12   kdarea      |          k
  13               k          d
  14               d  kdarea  b---kdbranch-
  15               b          r
  16 --kdbranch----r          a
  17               a          n  kdarea
  18               n          c
  19               c          h
  20               h          |
  21               |------kdbranch---------
  22    kdarea     |
  23               |      kdarea
  24               |
  25               |
  26
  27 */
  28
  29 /* 0=right 1=down 2=left 3=up */
  30 static int vx_and[4] = {INT_MAX, 0, INT_MAX, 0};
  31 static int vy_and[4] = {0, INT_MAX, 0, INT_MAX};
  32 static int vx[4] = {1, 0, -1, 0};
  33 static int vy[4] = {0, 1, 0, -1};
  34 static int vsign[4] = {1,1,-1,-1};
  35 static char* vname[4] = {"right", "down", "left", "up"};
  36
  37 kdarea_t* kdarea_new(void*data)
  38 {
  39     NEW(kdarea_t,area);
  40     area->bbox.xmin = INT_MIN;
  41     area->bbox.ymin = INT_MIN;
  42     area->bbox.xmax = INT_MAX;
  43     area->bbox.ymax = INT_MAX;
  44     area->data = data;
  45     return area;
  46 }
  47
  48 kdtree_t* kdtree_new()
  49 {
  50     NEW(kdtree_t,tree);
  51     tree->root = kdarea_new(0);
  52     return tree;
  53 }
  54
  55 static inline int32_t max32(int32_t v1, int32_t v2) {return v1>v2?v1:v2;}
  56 static inline int32_t min32(int32_t v1, int32_t v2) {return v1<v2?v1:v2;}
  57
  58 kdbranch_t*kdbranch_new(int32_t xy, kdtype_t type)
  59 {
  60     NEW(kdbranch_t,b);
  61     b->type = type;
  62     b->xy = xy;
  63     return b;
  64 }
  65
  66 kdarea_t*kdbranch_follow(const kdbranch_t*tree, int32_t x, int32_t y)
  67 {
  68     /*
  69     int follow = 0;
  70     switch(tree->type) {
  71         case KD_LEFT:
  72             follow = (x < tree->xy);
  73         break;
  74         case KD_RIGHT:
  75             follow = (x > tree->xy);
  76         break;
  77         case KD_UP:
  78             follow = (y < tree->xy);
  79         break;
  80         case KD_DOWN:
  81             follow = (y > tree->xy);
  82         break;
  83     }
  84     return &tree->side[follow];
  85     */
  86     int32_t s = x*vx[tree->type] + y*vy[tree->type];
  87     int32_t v = tree->xy*vsign[tree->type];
  88     if(s == v)
  89         return 0; //point is on the boundary
  90     return tree->side[s < v];
  91 }
  92
  93 static kdarea_list_t* kdarea_list_new(kdarea_t*area)
  94 {
  95     NEW(kdarea_list_t,b);
  96     b->area = area;
  97     b->next = b->prev = b;
  98     return b;
  99 }
 100
 101 static kdarea_list_t*kdarea_list_concatenate(kdarea_list_t*l1, kdarea_list_t*l2)
 102 {
 103     if(!l1) return l2;
 104     if(!l2) return l1;
 105     l2->prev->next = l1->next;
 106     l1->next->prev = l2->prev;
 107     l2->prev = l1;
 108     l1->next = l2;
 109     return l1;
 110 }
 111
 112 static kdbbox_t bbox_for_halfplane(int xy, int dir)
 113 {
 114     kdbbox_t b = {INT_MIN,INT_MIN,INT_MAX,INT_MAX};
 115     switch(dir) {
 116         case KD_LEFT:
 117             b.xmax = xy;
 118         break;
 119         case KD_RIGHT:
 120             b.xmin = xy;
 121         break;
 122         case KD_UP:
 123             b.ymax = xy;
 124         break;
 125         case KD_DOWN:
 126             b.ymin = xy;
 127         break;
 128     }
 129     return b;
 130 }
 131
 132 static kdbbox_t intersect_bbox(const kdbbox_t*box1, const kdbbox_t*box2)
 133 {
 134     kdbbox_t b = *box1;
 135     if(box2->xmin > b.xmin)
 136         b.xmin = box2->xmin;
 137     if(box2->ymin > b.ymin)
 138         b.ymin = box2->ymin;
 139     if(box2->xmax < b.xmax)
 140         b.xmax = box2->xmax;
 141     if(box2->ymax < b.ymax)
 142         b.ymax = box2->ymax;
 143     if(b.xmin > b.xmax)
 144         b.xmax = b.xmin;
 145     if(b.ymin > b.ymax)
 146         b.ymax = b.ymin;
 147     return b;
 148 }
 149
 150 static void kdarea_split(kdarea_t*area, int xy, int dir,
 151                              int32_t x1, int32_t y1,
 152                              int32_t x2, int32_t y2)
 153 {
 154     if(!area->split) {
 155         kdbranch_t*b = area->split = kdbranch_new(xy, dir);
 156         kdbbox_t b1 = bbox_for_halfplane(xy, dir);
 157         kdbbox_t b2 = bbox_for_halfplane(xy, dir^2);
 158         b->side[0] = kdarea_new(area->data);
 159         b->side[1] = kdarea_new(area->data);
 160         b->side[0]->bbox = intersect_bbox(&area->bbox,&b1);
 161         b->side[1]->bbox = intersect_bbox(&area->bbox,&b2);
 162         memcpy(b->side[0]->neighbors, area->neighbors, sizeof(area->neighbors));
 163         memcpy(b->side[1]->neighbors, area->neighbors, sizeof(area->neighbors));
 164         b->side[0]->neighbors[dir^2] = b->side[1];
 165         b->side[1]->neighbors[dir] = b->side[0];
 166         area->data = 0;
 167     } else {
 168         kdbranch_t*split = area->split;
 169         kdarea_t*first = kdbranch_follow(split, x1,y1);
 170         kdarea_t*second = kdbranch_follow(split, x2,y2);
 171
 172         if(!first) {
 173             if(!second) {
 174                 /* line is on top of an already existing segment */
 175                 return;
 176             } else {
 177                 /* first point is directly on the split */
 178                 kdarea_split(second, xy, dir, x1,y1, x2,y2);
 179                 return;
 180             }
 181         } else {
 182             if(!second) {
 183                 /* second point is directly on the split */
 184                 kdarea_split(first, xy, dir, x1,y1, x2,y2);
 185                 return;
 186             } else if(first == second) {
 187                 /* both points are to the same side of this split */
 188                 kdarea_split(first, xy, dir, x1,y1, x2,y2);
 189                 return;
 190             } else {
 191                 kdarea_split(first, xy, dir, x1,y1, x2,y2);
 192                 kdarea_split(second, xy, dir, x1,y1, x2,y2);
 193                 return;
 194             }
 195         }
 196     }
 197 }
 198
 199 static kdarea_list_t* kdarea_filter(kdarea_t*area, int xy, int dir)
 200 {
 201     if(!area->split) {
 202         return kdarea_list_new(area);
 203     } else {
 204         kdbranch_t*branch = area->split;
 205         if((branch->type^dir) == 0) {
 206             /* both filter as well as branch point into the same direction */
 207             if(xy*vsign[dir] >= branch->xy*vsign[dir]) {
 208                 /* filter splits the primary node. We can skip the other one. */
 209                 return kdarea_filter(branch->side[0], xy, dir);
 210             } else {
 211                 /* filter splits the secondary node. the primary node is left completely intact,
 212                    and returned as such */
 213                 kdarea_list_t*l1 = kdarea_list_new(branch->side[0]);
 214                 kdarea_list_t*l2 = kdarea_filter(branch->side[1], xy, dir);
 215                 return kdarea_list_concatenate(l1,l2);
 216             }
 217         } else if((branch->type^dir) == 1) {
 218             /* filter and branch point into opposite directions */
 219             if(xy*vsign[dir] <= branch->xy*vsign[dir]) {
 220                 // filter splits the secondary node. We can skip the primary node.
 221                 return kdarea_filter(branch->side[1], xy, dir);
 222             } else {
 223                 /* filter splits the primary node. the secondary node is left completely intact,
 224                    and returned as such */
 225                 kdarea_list_t*l1 = kdarea_filter(branch->side[0], xy, dir);
 226                 kdarea_list_t*l2 = kdarea_list_new(branch->side[1]);
 227                 return kdarea_list_concatenate(l1,l2);
 228             }
 229         } else {
 230             /* filter segment is perpendicular to the node */
 231             kdarea_list_t*l1 = kdarea_filter(branch->side[0], xy, dir);
 232             kdarea_list_t*l2 = kdarea_filter(branch->side[1], xy, dir);
 233             return kdarea_list_concatenate(l1,l2);
 234         }
 235     }
 236 }
 237
 238 static kdarea_t* kdarea_find(kdarea_t*node, int x, int y)
 239 {
 240     while(node) {
 241         if(!node->split)
 242             break;
 243         node = kdbranch_follow(node->split, x,y);
 244     }
 245     return node;
 246 }
 247
 248 kdarea_t*kdtree_find(kdtree_t*tree, int x, int y)
 249 {
 250     return kdarea_find(tree->root, x,y);
 251 }
 252
 253 void kdarea_list_destroy(kdarea_list_t*list)
 254 {
 255     kdarea_list_t*i = list;
 256     if(i) do {
 257         kdarea_list_t*next = i->next;
 258         free(i);
 259         i = next;
 260     } while(i!=list);
 261 }
 262
 263 static kdarea_list_t* kdarea_list_add(kdarea_list_t*l, kdarea_t*area)
 264 {
 265     return kdarea_list_concatenate(l,kdarea_list_new(area));
 266 }
 267
 268 static kdarea_list_t* kdarea_all_children(kdarea_t*area, kdarea_list_t*result)
 269 {
 270     if(!area->split) {
 271         result = kdarea_list_add(result, area);
 272     } else {
 273         result = kdarea_all_children(area->split->side[0], result);
 274         result = kdarea_all_children(area->split->side[1], result);
 275     }
 276     return result;
 277 }
 278
 279 /* return all areas that are contained in, or partly intersect, the given bounding box */
 280 kdarea_list_t* kdtree_filter(kdtree_t*tree, int32_t x1, int32_t y1, int32_t x2, int32_t y2, char leafs)
 281 {
 282     kdarea_list_t*result = 0;
 283     kdarea_list_t*branches1 = kdarea_filter(tree->root, x2, KD_LEFT);
 284     kdarea_list_t*i = branches1;
 285     if(i) do {
 286         kdarea_list_t*branches2 = kdarea_filter(i->area, y2, KD_UP);
 287         kdarea_list_t*j = branches2;
 288         if(j) do {
 289             kdarea_list_t*branches3 = kdarea_filter(i->area, x1, KD_RIGHT);
 290             kdarea_list_t*k = branches3;
 291             if(k) do {
 292                 kdarea_list_t*branches4 = kdarea_filter(i->area, y1, KD_DOWN);
 293                 kdarea_list_t*l = branches4;
 294                 if(leafs) {
 295                     if(l) do {
 296                         result = kdarea_list_concatenate(result, kdarea_all_children(l->area, 0));
 297                         l = l->next;
 298                     } while(l!=branches4);
 299                     kdarea_list_destroy(branches4);
 300                 } else {
 301                     result = kdarea_list_concatenate(result, l);
 302                 }
 303                 k = k->next;
 304             } while(k!=branches3);
 305             kdarea_list_destroy(branches3);
 306             j = j->next;
 307         } while(j!=branches2);
 308         kdarea_list_destroy(branches2);
 309         i = i->next;
 310     } while(i!=branches1);
 311     kdarea_list_destroy(branches1);
 312     return result;
 313 }
 314
 315 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)
 316 {
 317     kdarea_split(tree->root, x2, KD_LEFT,  x2,y1, x2,y2);
 318     kdarea_split(tree->root, y2, KD_UP,    x1,y2, x2,y2);
 319     kdarea_split(tree->root, x1, KD_RIGHT, x1,y1, x1,y2);
 320     kdarea_split(tree->root, y1, KD_DOWN,  x1,y1, x2,y1);
 321     kdarea_list_t*l = kdtree_filter(tree, x1, y1, x2, y2, 1);
 322     kdarea_list_t*i = l;
 323     if(l) do {
 324         l->area->data = f(user, l->area->data);
 325         i = i->next;
 326     } while(i!=l);
 327     kdarea_list_destroy(l);
 328 }
 329
 330 static void* overwrite(void*user, void*data)
 331 {
 332     return user;
 333 }
 334
 335 void kdtree_add_box(kdtree_t*tree, int32_t x1, int32_t y1, int32_t x2, int32_t y2, void*data)
 336 {
 337     kdtree_modify_box(tree, x1, y1, x2, y2, overwrite, data);
 338 }
 339
 340 kdarea_t*kdarea_neighbor(kdarea_t*area, int dir, int xy)
 341 {
 342     int x,y;
 343     switch(dir) {
 344         case KD_LEFT:
 345             x = area->bbox.xmin;
 346             y = xy;
 347         break;
 348         case KD_RIGHT:
 349             x = area->bbox.xmax;
 350             y = xy;
 351         break;
 352         case KD_UP:
 353             x = xy;
 354             y = area->bbox.ymin;
 355         break;
 356         case KD_DOWN:
 357             x = xy;
 358             y = area->bbox.ymax;
 359         break;
 360     }
 361     kdarea_t*n = area->neighbors[dir];
 362     if(!n)
 363         return 0;
 364     return kdarea_find(n, x, y);
 365 }
 366
 367 static void do_indent(int l)
 368 {
 369     int i;
 370     for(i=0;i<l;i++)
 371         printf(" ");
 372 }
 373
 374 void kdarea_print(kdarea_t*area, int indent);
 375 void kdbranch_print(kdbranch_t*branch, int indent)
 376 {
 377     do_indent(indent);printf("[%p] branch (%s, %d)\n", branch, vname[branch->type], branch->xy);
 378     kdbbox_t b = bbox_for_halfplane(branch->xy, branch->type);
 379     kdarea_print(branch->side[0], indent+4);
 380     kdarea_print(branch->side[1], indent+4);
 381 }
 382
 383 void kdarea_print(kdarea_t*area, int indent)
 384 {
 385     int i;
 386     assert(area);
 387     do_indent(indent);printf("[%p] leaf (%d,%d,%d,%d) %p (l:%p r:%p u:%p d:%p)\n", area,
 388             area->bbox.xmin,
 389             area->bbox.ymin,
 390             area->bbox.xmax,
 391             area->bbox.ymax,
 392             area->data,
 393             area->neighbors[KD_LEFT],
 394             area->neighbors[KD_RIGHT],
 395             area->neighbors[KD_UP],
 396             area->neighbors[KD_DOWN]);
 397     if(area->split) {
 398         kdbranch_print(area->split, indent+4);
 399     }
 400 }
 401
 402 void kdtree_print(kdtree_t*tree)
 403 {
 404     kdarea_print(tree->root, 0);
 405 }
 406
 407
 408 void kdbranch_destroy(kdbranch_t*b)
 409 {
 410     if(b->side[0]) {
 411         kdarea_destroy(b->side[0]);
 412         b->side[0] = 0;
 413     }
 414     if(b->side[1]) {
 415         kdarea_destroy(b->side[1]);
 416         b->side[1] = 0;
 417     }
 418     free(b);
 419 }
 420
 421 void kdarea_destroy(kdarea_t*area)
 422 {
 423     if(area->split) {
 424         kdbranch_destroy(area->split);
 425     }
 426     free(area);
 427 }
 428
 429 void kdtree_destroy(kdtree_t*tree)
 430 {
 431     kdarea_destroy(tree->root);
 432     tree->root = 0;
 433     free(tree);
 434 }
 435
 436 #ifdef MAIN
 437 int main()
 438 {
 439     assert((1^vx[2]) < 0);
 440
 441     kdtree_t*tree = kdtree_new();
 442     kdtree_add_box(tree, 10,30,20,40, "hello world");
 443     kdtree_print(tree);
 444     kdarea_t*a = kdtree_find(tree, 15,35);
 445     kdarea_t*left = kdarea_neighbor(a, KD_LEFT, /*y*/35);
 446     kdarea_t*right = kdarea_neighbor(a, KD_RIGHT, /*y*/35);
 447     kdarea_t*up = kdarea_neighbor(a, KD_UP, /*x*/15);
 448     kdarea_t*down = kdarea_neighbor(a, KD_DOWN, /*x*/15);
 449     kdtree_destroy(tree);
 450 }
 451 #endif