src/rtree.c

   1 /* $Id$ */
   2
   3 /*
   4  *                            COPYRIGHT
   5  *
   6  *  PCB, interactive printed circuit board design
   7  *  Copyright (C) 1994,1995,1996 Thomas Nau
   8  *  Copyright (C) 1998,1999,2000,2001,2002,2003,2004 harry eaton
   9  *
  10  *  This program is free software; you can redistribute it and/or modify
  11  *  it under the terms of the GNU General Public License as published by
  12  *  the Free Software Foundation; either version 2 of the License, or
  13  *  (at your option) any later version.
  14  *
  15  *  This program is distributed in the hope that it will be useful,
  16  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  *  GNU General Public License for more details.
  19  *
  20  *  You should have received a copy of the GNU General Public License
  21  *  along with this program; if not, write to the Free Software
  22  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  23  *
  24  *  Contact addresses for paper mail and Email:
  25  *  harry eaton, 6697 Buttonhole Ct, Columbia, MD 21044 USA
  26  *  haceaton@aplcomm.jhuapl.edu
  27  *
  28  */
  29
  30 /* this file, rtree.c, was written and is
  31  * Copyright (c) 2004, harry eaton
  32  */
  33
  34 /* implements r-tree structures.
  35  * these should be much faster for the auto-router
  36  * because the recursive search is much more efficient
  37  * and that's where the auto-router spends all its time.
  38  */
  39 #ifdef HAVE_CONFIG_H
  40 #include "config.h"
  41 #endif
  42
  43 #include "global.h"
  44
  45 #include <assert.h>
  46 #include <setjmp.h>
  47
  48 /* #define DRAWBOX */
  49
  50 #ifdef DRAWBOX
  51 #include "clip.h"
  52 #include "data.h"
  53 #endif
  54 #include "mymem.h"
  55
  56 #include "rtree.h"
  57
  58 #ifdef HAVE_LIBDMALLOC
  59 #include <dmalloc.h>
  60 #endif
  61
  62 RCSID ("$Id$");
  63
  64
  65 #define SLOW_ASSERTS
  66 /* All rectangles are closed on the bottom left and open on the
  67  * top right. i.e. they contain one corner point, but not the other.
  68  * This requires that the corner points not be equal!
  69  */
  70
  71 /* the number of entries in each rtree node
  72  * 4 - 7 seem to be pretty good settings
  73  */
  74 #define M_SIZE 6
  75
  76 /* it seems that sorting the leaf order slows us down
  77  * but sometimes gives better routes
  78  */
  79 #undef SORT
  80 #define SORT_NONLEAF
  81
  82 #define DELETE_BY_POINTER
  83
  84 typedef struct
  85 {
  86   const BoxType *bptr;          /* pointer to the box */
  87   BoxType bounds;               /* copy of the box for locality of reference */
  88 } Rentry;
  89
  90 struct rtree_node
  91 {
  92   BoxType box;                  /* bounds rectangle of this node */
  93   struct rtree_node *parent;    /* parent of this node, NULL = root */
  94   struct
  95   {
  96     unsigned is_leaf:1;         /* this is a leaf node */
  97     unsigned manage:31;         /* true==should free 'rect.bptr' if node is destroyed */
  98   }
  99   flags;
 100   union
 101   {
 102     struct rtree_node *kids[M_SIZE + 1];        /* when not leaf */
 103     Rentry rects[M_SIZE + 1];   /* when leaf */
 104   } u;
 105 };
 106
 107 #ifdef SLOW_ASSERTS
 108 static int
 109 __r_node_is_good (struct rtree_node *node)
 110 {
 111   int i, flag = 1;
 112   int kind = -1;
 113   bool last = false;
 114
 115   if (node == NULL)
 116     return 1;
 117   for (i = 0; i < M_SIZE; i++)
 118     {
 119       if (node->flags.is_leaf)
 120         {
 121           if (!node->u.rects[i].bptr)
 122             {
 123               last = true;
 124               continue;
 125             }
 126           /* check that once one entry is empty, all the rest are too */
 127           if (node->u.rects[i].bptr && last)
 128             assert (0);
 129           /* check that the box makes sense */
 130           if (node->box.X1 > node->box.X2)
 131             assert (0);
 132           if (node->box.Y1 > node->box.Y2)
 133             assert (0);
 134           /* check that bounds is the same as the pointer */
 135           if (node->u.rects[i].bounds.X1 != node->u.rects[i].bptr->X1)
 136             assert (0);
 137           if (node->u.rects[i].bounds.Y1 != node->u.rects[i].bptr->Y1)
 138             assert (0);
 139           if (node->u.rects[i].bounds.X2 != node->u.rects[i].bptr->X2)
 140             assert (0);
 141           if (node->u.rects[i].bounds.Y2 != node->u.rects[i].bptr->Y2)
 142             assert (0);
 143           /* check that entries are within node bounds */
 144           if (node->u.rects[i].bounds.X1 < node->box.X1)
 145             assert (0);
 146           if (node->u.rects[i].bounds.X2 > node->box.X2)
 147             assert (0);
 148           if (node->u.rects[i].bounds.Y1 < node->box.Y1)
 149             assert (0);
 150           if (node->u.rects[i].bounds.Y2 > node->box.Y2)
 151             assert (0);
 152         }
 153       else
 154         {
 155           if (!node->u.kids[i])
 156             {
 157               last = true;
 158               continue;
 159             }
 160           /* make sure all children are the same type */
 161           if (kind == -1)
 162             kind = node->u.kids[i]->flags.is_leaf;
 163           else if (kind != node->u.kids[i]->flags.is_leaf)
 164             assert (0);
 165           /* check that once one entry is empty, all the rest are too */
 166           if (node->u.kids[i] && last)
 167             assert (0);
 168           /* check that entries are within node bounds */
 169           if (node->u.kids[i]->box.X1 < node->box.X1)
 170             assert (0);
 171           if (node->u.kids[i]->box.X2 > node->box.X2)
 172             assert (0);
 173           if (node->u.kids[i]->box.Y1 < node->box.Y1)
 174             assert (0);
 175           if (node->u.kids[i]->box.Y2 > node->box.Y2)
 176             assert (0);
 177         }
 178       flag <<= 1;
 179     }
 180   /* check that we're completely in the parent's bounds */
 181   if (node->parent)
 182     {
 183       if (node->parent->box.X1 > node->box.X1)
 184         assert (0);
 185       if (node->parent->box.X2 < node->box.X2)
 186         assert (0);
 187       if (node->parent->box.Y1 > node->box.Y1)
 188         assert (0);
 189       if (node->parent->box.Y2 < node->box.Y2)
 190         assert (0);
 191     }
 192   /* make sure overflow is empty */
 193   if (!node->flags.is_leaf && node->u.kids[i])
 194     assert (0);
 195   if (node->flags.is_leaf && node->u.rects[i].bptr)
 196     assert (0);
 197   return 1;
 198 }
 199
 200
 201 /* check the whole tree from this node down for consistency */
 202 static bool
 203 __r_tree_is_good (struct rtree_node *node)
 204 {
 205   int i;
 206
 207   if (!node)
 208     return 1;
 209   if (!__r_node_is_good (node))
 210     assert (0);
 211   if (node->flags.is_leaf)
 212     return 1;
 213   for (i = 0; i < M_SIZE; i++)
 214     {
 215       if (!__r_tree_is_good (node->u.kids[i]))
 216         return 0;
 217     }
 218   return 1;
 219 }
 220 #endif
 221 #ifndef NDEBUG
 222 /* print out the tree */
 223 void
 224 __r_dump_tree (struct rtree_node *node, int depth)
 225 {
 226   int i, j;
 227   static int count;
 228   static double area;
 229
 230   if (depth == 0)
 231     {
 232       area = 0;
 233       count = 0;
 234     }
 235   area +=
 236     (node->box.X2 - node->box.X1) * (double) (node->box.Y2 - node->box.Y1);
 237   count++;
 238   for (i = 0; i < depth; i++)
 239     printf ("  ");
 240   if (!node->flags.is_leaf)
 241     {
 242       printf ("p=0x%p node X(%d, %d) Y(%d, %d)\n", (void *) node,
 243               node->box.X1, node->box.X2, node->box.Y1, node->box.Y2);
 244 #ifdef DRAWBOX
 245       gdk_gc_set_line_attributes (Output.fgGC, 4,
 246                                   GDK_LINE_SOLID, GDK_CAP_ROUND,
 247                                   GDK_JOIN_ROUND);
 248
 249       if (depth < max_layer + 1)
 250         gdk_gc_set_foreground (Output.fgGC, (LAYER_PTR (depth)->Color));
 251       else
 252         gdk_gc_set_foreground (Output.fgGC, PCB->WarnColor);
 253       XDrawCLine (Output.top_window->window, Output.fgGC,
 254                   node->box.X1, node->box.Y1, node->box.X2, node->box.Y1);
 255       XDrawCLine (Output.top_window->window, Output.fgGC,
 256                   node->box.X2, node->box.Y1, node->box.X2, node->box.Y2);
 257       XDrawCLine (Output.top_window->window, Output.fgGC,
 258                   node->box.X2, node->box.Y2, node->box.X1, node->box.Y2);
 259       XDrawCLine (Output.top_window->window, Output.fgGC,
 260                   node->box.X1, node->box.Y2, node->box.X1, node->box.Y1);
 261 #endif
 262     }
 263   else
 264     {
 265 #ifdef DRAWBOX
 266       gdk_gc_set_line_attributes (Output.fgGC, 2,
 267                                   GDK_LINE_SOLID, GDK_CAP_ROUND,
 268                                   GDK_JOIN_ROUND);
 269       gdk_gc_set_foreground (Output.fgGC, PCB->MaskColor);
 270       XDrawCLine (Output.top_window->window, Output.fgGC,
 271                   node->box.X1, node->box.Y1, node->box.X2, node->box.Y1);
 272       XDrawCLine (Output.top_window->window, Output.fgGC,
 273                   node->box.X2, node->box.Y1, node->box.X2, node->box.Y2);
 274       XDrawCLine (Output.top_window->window, Output.fgGC,
 275                   node->box.X2, node->box.Y2, node->box.X1, node->box.Y2);
 276       XDrawCLine (Output.top_window->window, Output.fgGC,
 277                   node->box.X1, node->box.Y2, node->box.X1, node->box.Y1);
 278 #endif
 279       printf ("p=0x%p leaf manage(%02x) X(%d, %d) Y(%d, %d)\n", (void *) node,
 280               node->flags.manage, node->box.X1, node->box.X2, node->box.Y1,
 281               node->box.Y2);
 282       for (j = 0; j < M_SIZE; j++)
 283         {
 284           if (!node->u.rects[j].bptr)
 285             break;
 286           area +=
 287             (node->u.rects[j].bounds.X2 -
 288              node->u.rects[j].bounds.X1) *
 289             (double) (node->u.rects[j].bounds.Y2 -
 290                       node->u.rects[j].bounds.Y1);
 291           count++;
 292 #ifdef DRAWBOX
 293           gdk_gc_set_line_attributes (Output.fgGC, 1,
 294                                       GDK_LINE_SOLID, GDK_CAP_ROUND,
 295                                       GDK_JOIN_ROUND);
 296           gdk_gc_set_foreground (Output.fgGC, PCB->ViaSelectedColor);
 297           XDrawCLine (Output.top_window->window, Output.fgGC,
 298                       node->u.rects[j].bounds.X1, node->u.rects[j].bounds.Y1,
 299                       node->u.rects[j].bounds.X2, node->u.rects[j].bounds.Y1);
 300           XDrawCLine (Output.top_window->window, Output.fgGC,
 301                       node->u.rects[j].bounds.X2, node->u.rects[j].bounds.Y1,
 302                       node->u.rects[j].bounds.X2, node->u.rects[j].bounds.Y2);
 303           XDrawCLine (Output.top_window->window, Output.fgGC,
 304                       node->u.rects[j].bounds.X2, node->u.rects[j].bounds.Y2,
 305                       node->u.rects[j].bounds.X1, node->u.rects[j].bounds.Y2);
 306           XDrawCLine (Output.top_window->window, Output.fgGC,
 307                       node->u.rects[j].bounds.X1, node->u.rects[j].bounds.Y2,
 308                       node->u.rects[j].bounds.X1, node->u.rects[j].bounds.Y1);
 309 #endif
 310           for (i = 0; i < depth + 1; i++)
 311             printf ("  ");
 312           printf ("entry 0x%p X(%d, %d) Y(%d, %d)\n",
 313                   (void *) (node->u.rects[j].bptr),
 314                   node->u.rects[j].bounds.X1, node->u.rects[j].bounds.X2,
 315                   node->u.rects[j].bounds.Y1, node->u.rects[j].bounds.Y2);
 316         }
 317       return;
 318     }
 319   for (i = 0; i < M_SIZE; i++)
 320     if (node->u.kids[i])
 321       __r_dump_tree (node->u.kids[i], depth + 1);
 322   if (depth == 0)
 323     printf ("average box area is %g\n", area / count);
 324 }
 325 #endif
 326
 327 /* Sort the children or entries of a node
 328  * according to the largest side.
 329  */
 330 #ifdef SORT
 331 static int
 332 cmp_box (const BoxType * a, const BoxType * b)
 333 {
 334   /* compare two box coordinates so that the __r_search
 335    * will fail at the earliest comparison possible.
 336    * It needs to see the biggest X1 first, then the
 337    * smallest X2, the biggest Y1 and smallest Y2
 338    */
 339   if (a->X1 < b->X1)
 340     return 0;
 341   if (a->X1 > b->X1)
 342     return 1;
 343   if (a->X2 > b->X2)
 344     return 0;
 345   if (a->X2 < b->X2)
 346     return 1;
 347   if (a->Y1 < b->Y1)
 348     return 0;
 349   if (a->Y1 > b->Y1)
 350     return 1;
 351   if (a->Y2 > b->Y2)
 352     return 0;
 353   return 1;
 354 }
 355
 356 static void
 357 sort_node (struct rtree_node *node)
 358 {
 359   if (node->flags.is_leaf)
 360     {
 361       register Rentry *r, *i, temp;
 362
 363       for (r = &node->u.rects[1]; r->bptr; r++)
 364         {
 365           temp = *r;
 366           i = r - 1;
 367           while (i >= &node->u.rects[0])
 368             {
 369               if (cmp_box (&i->bounds, &r->bounds))
 370                 break;
 371               *(i + 1) = *i;
 372               i--;
 373             }
 374           *(i + 1) = temp;
 375         }
 376     }
 377 #ifdef SORT_NONLEAF
 378   else
 379     {
 380       register struct rtree_node **r, **i, *temp;
 381
 382       for (r = &node->u.kids[1]; *r; r++)
 383         {
 384           temp = *r;
 385           i = r - 1;
 386           while (i >= &node->u.kids[0])
 387             {
 388               if (cmp_box (&(*i)->box, &(*r)->box))
 389                 break;
 390               *(i + 1) = *i;
 391               i--;
 392             }
 393           *(i + 1) = temp;
 394         }
 395     }
 396 #endif
 397 }
 398 #else
 399 #define sort_node(x)
 400 #endif
 401
 402 /* set the node bounds large enough to encompass all
 403  * of the children's rectangles
 404  */
 405 static void
 406 adjust_bounds (struct rtree_node *node)
 407 {
 408   int i;
 409
 410   assert (node);
 411   assert (node->u.kids[0]);
 412   if (node->flags.is_leaf)
 413     {
 414       node->box = node->u.rects[0].bounds;
 415       for (i = 1; i < M_SIZE + 1; i++)
 416         {
 417           if (!node->u.rects[i].bptr)
 418             return;
 419           MAKEMIN (node->box.X1, node->u.rects[i].bounds.X1);
 420           MAKEMAX (node->box.X2, node->u.rects[i].bounds.X2);
 421           MAKEMIN (node->box.Y1, node->u.rects[i].bounds.Y1);
 422           MAKEMAX (node->box.Y2, node->u.rects[i].bounds.Y2);
 423         }
 424     }
 425   else
 426     {
 427       node->box = node->u.kids[0]->box;
 428       for (i = 1; i < M_SIZE + 1; i++)
 429         {
 430           if (!node->u.kids[i])
 431             return;
 432           MAKEMIN (node->box.X1, node->u.kids[i]->box.X1);
 433           MAKEMAX (node->box.X2, node->u.kids[i]->box.X2);
 434           MAKEMIN (node->box.Y1, node->u.kids[i]->box.Y1);
 435           MAKEMAX (node->box.Y2, node->u.kids[i]->box.Y2);
 436         }
 437     }
 438 }
 439
 440 /* create an r-tree from an unsorted list of boxes.
 441  * the r-tree will keep pointers into
 442  * it, so don't free the box list until you've called r_destroy_tree.
 443  * if you set 'manage' to true, r_destroy_tree will free your boxlist.
 444  */
 445 rtree_t *
 446 r_create_tree (const BoxType * boxlist[], int N, int manage)
 447 {
 448   rtree_t *rtree;
 449   struct rtree_node *node;
 450   int i;
 451
 452   assert (N >= 0);
 453   rtree = calloc (1, sizeof (*rtree));
 454   /* start with a single empty leaf node */
 455   node = calloc (1, sizeof (*node));
 456   node->flags.is_leaf = 1;
 457   node->parent = NULL;
 458   rtree->root = node;
 459   /* simple, just insert all of the boxes! */
 460   for (i = 0; i < N; i++)
 461     {
 462       assert (boxlist[i]);
 463       r_insert_entry (rtree, boxlist[i], manage);
 464     }
 465 #ifdef SLOW_ASSERTS
 466   assert (__r_tree_is_good (rtree->root));
 467 #endif
 468   return rtree;
 469 }
 470
 471 static void
 472 __r_destroy_tree (struct rtree_node *node)
 473 {
 474   int i, flag = 1;
 475
 476   if (node->flags.is_leaf)
 477     for (i = 0; i < M_SIZE; i++)
 478       {
 479         if (!node->u.rects[i].bptr)
 480           break;
 481         if (node->flags.manage & flag)
 482           free ((void *) node->u.rects[i].bptr);
 483         flag = flag << 1;
 484       }
 485   else
 486     for (i = 0; i < M_SIZE; i++)
 487       {
 488         if (!node->u.kids[i])
 489           break;
 490         __r_destroy_tree (node->u.kids[i]);
 491       }
 492   free (node);
 493 }
 494
 495 /* free the memory associated with an rtree. */
 496 void
 497 r_destroy_tree (rtree_t ** rtree)
 498 {
 499
 500   __r_destroy_tree ((*rtree)->root);
 501   free (*rtree);
 502   *rtree = NULL;
 503 }
 504
 505 typedef struct
 506 {
 507   int (*check_it) (const BoxType * region, void *cl);
 508   int (*found_it) (const BoxType * box, void *cl);
 509   void *closure;
 510 } r_arg;
 511
 512 /* most of the auto-routing time is spent in this routine
 513  * so some careful thought has been given to maximizing the speed
 514  *
 515  */
 516 int
 517 __r_search (struct rtree_node *node, const BoxType * query, r_arg * arg)
 518 {
 519   assert (node);
 520   /** assert that starting_region is well formed */
 521   assert (query->X1 < query->X2 && query->Y1 < query->Y2);
 522   assert (node->box.X1 < query->X2 && node->box.X2 > query->X1 &&
 523           node->box.Y1 < query->Y2 && node->box.Y2 > query->Y1);
 524 #ifdef SLOW_ASSERTS
 525   /** assert that node is well formed */
 526   assert (__r_node_is_good (node));
 527 #endif
 528   /* the check for bounds is done before entry. This saves the overhead
 529    * of building/destroying the stack frame for each bounds that fails
 530    * to intersect, which is the most common condition.
 531    */
 532   if (node->flags.is_leaf)
 533     {
 534       register int i;
 535       if (arg->found_it)        /* test this once outside of loop */
 536         {
 537           register int seen = 0;
 538           for (i = 0; node->u.rects[i].bptr; i++)
 539             {
 540               if ((node->u.rects[i].bounds.X1 < query->X2) &&
 541                   (node->u.rects[i].bounds.X2 > query->X1) &&
 542                   (node->u.rects[i].bounds.Y1 < query->Y2) &&
 543                   (node->u.rects[i].bounds.Y2 > query->Y1) &&
 544                   arg->found_it (node->u.rects[i].bptr, arg->closure))
 545                 seen++;
 546             }
 547           return seen;
 548         }
 549       else
 550         {
 551           register int seen = 0;
 552           for (i = 0; node->u.rects[i].bptr; i++)
 553             {
 554               if ((node->u.rects[i].bounds.X1 < query->X2) &&
 555                   (node->u.rects[i].bounds.X2 > query->X1) &&
 556                   (node->u.rects[i].bounds.Y1 < query->Y2) &&
 557                   (node->u.rects[i].bounds.Y2 > query->Y1))
 558                 seen++;
 559             }
 560           return seen;
 561         }
 562     }
 563
 564   /* not a leaf, recurse on lower nodes */
 565   if (arg->check_it)
 566     {
 567       int seen = 0;
 568       struct rtree_node **n;
 569       for (n = &node->u.kids[0]; *n; n++)
 570         {
 571           if ((*n)->box.X1 >= query->X2 ||
 572               (*n)->box.X2 <= query->X1 ||
 573               (*n)->box.Y1 >= query->Y2 || (*n)->box.Y2 <= query->Y1 ||
 574               !arg->check_it (&(*n)->box, arg->closure))
 575             continue;
 576           seen += __r_search (*n, query, arg);
 577         }
 578       return seen;
 579     }
 580   else
 581     {
 582       int seen = 0;
 583       struct rtree_node **n;
 584       for (n = &node->u.kids[0]; *n; n++)
 585         {
 586           if ((*n)->box.X1 >= query->X2 ||
 587               (*n)->box.X2 <= query->X1 ||
 588               (*n)->box.Y1 >= query->Y2 || (*n)->box.Y2 <= query->Y1)
 589             continue;
 590           seen += __r_search (*n, query, arg);
 591         }
 592       return seen;
 593     }
 594 }
 595
 596 /* Parameterized search in the rtree.
 597  * Returns the number of rectangles found.
 598  * calls found_rectangle for each intersection seen
 599  * and calls check_region with the current sub-region
 600  * to see whether deeper searching is desired
 601  */
 602 int
 603 r_search (rtree_t * rtree, const BoxType * query,
 604           int (*check_region) (const BoxType * region, void *cl),
 605           int (*found_rectangle) (const BoxType * box, void *cl), void *cl)
 606 {
 607   r_arg arg;
 608
 609   if (!rtree || rtree->size < 1)
 610     return 0;
 611   if (query)
 612     {
 613 #ifdef SLOW_ASSERTS
 614   assert (__r_tree_is_good (rtree->root));
 615 #endif
 616 #ifdef DEBUG
 617       if (query->X2 <= query->X1 || query->Y2 <= query->Y1)
 618         return 0;
 619 #endif
 620       /* check this box. If it's not touched we're done here */
 621       if (rtree->root->box.X1 >= query->X2 ||
 622           rtree->root->box.X2 <= query->X1 ||
 623           rtree->root->box.Y1 >= query->Y2
 624           || rtree->root->box.Y2 <= query->Y1)
 625         return 0;
 626       arg.check_it = check_region;
 627       arg.found_it = found_rectangle;
 628       arg.closure = cl;
 629       return __r_search (rtree->root, query, &arg);
 630     }
 631   else
 632     {
 633       arg.check_it = check_region;
 634       arg.found_it = found_rectangle;
 635       arg.closure = cl;
 636       return __r_search (rtree->root, &rtree->root->box, &arg);
 637     }
 638 }
 639
 640 /*------ r_region_is_empty ------*/
 641 static int
 642 __r_region_is_empty_rect_in_reg (const BoxType * box, void *cl)
 643 {
 644   jmp_buf *envp = (jmp_buf *) cl;
 645   longjmp (*envp, 1);           /* found one! */
 646 }
 647
 648 /* return 0 if there are any rectangles in the given region. */
 649 int
 650 r_region_is_empty (rtree_t * rtree, const BoxType * region)
 651 {
 652   jmp_buf env;
 653   int r;
 654
 655   if (setjmp (env))
 656     return 0;
 657   r = r_search (rtree, region, NULL, __r_region_is_empty_rect_in_reg, &env);
 658   assert (r == 0);              /* otherwise longjmp would have been called */
 659   return 1;                     /* no rectangles found */
 660 }
 661
 662 struct centroid
 663 {
 664   float x, y, area;
 665 };
 666
 667 /* split the node into two nodes putting clusters in each
 668  * use the k-means clustering algorithm
 669  */
 670 struct rtree_node *
 671 find_clusters (struct rtree_node *node)
 672 {
 673   float total_a, total_b;
 674   float a_X, a_Y, b_X, b_Y;
 675   bool belong[M_SIZE + 1];
 676   struct centroid center[M_SIZE + 1];
 677   int clust_a, clust_b, tries;
 678   int a_manage = 0, b_manage = 0;
 679   int i, old_ax, old_ay, old_bx, old_by;
 680   struct rtree_node *new_node;
 681   BoxType *b;
 682
 683   for (i = 0; i < M_SIZE + 1; i++)
 684     {
 685       if (node->flags.is_leaf)
 686         b = &(node->u.rects[i].bounds);
 687       else
 688         b = &(node->u.kids[i]->box);
 689       center[i].x = 0.5 * (b->X1 + b->X2);
 690       center[i].y = 0.5 * (b->Y1 + b->Y2);
 691       /* adding 1 prevents zero area */
 692       center[i].area = 1. + (float) (b->X2 - b->X1) * (float) (b->Y2 - b->Y1);
 693     }
 694   /* starting 'A' cluster center */
 695   a_X = center[0].x;
 696   a_Y = center[0].y;
 697   /* starting 'B' cluster center */
 698   b_X = center[M_SIZE].x;
 699   b_Y = center[M_SIZE].y;
 700   /* don't allow the same cluster centers */
 701   if (b_X == a_X && b_Y == a_Y)
 702     {
 703       b_X += 10000;
 704       a_Y -= 10000;
 705     }
 706   for (tries = 0; tries < M_SIZE; tries++)
 707     {
 708       old_ax = (int) a_X;
 709       old_ay = (int) a_Y;
 710       old_bx = (int) b_X;
 711       old_by = (int) b_Y;
 712       clust_a = clust_b = 0;
 713       for (i = 0; i < M_SIZE + 1; i++)
 714         {
 715           float dist1, dist2;
 716
 717           dist1 = SQUARE (a_X - center[i].x) + SQUARE (a_Y - center[i].y);
 718           dist2 = SQUARE (b_X - center[i].x) + SQUARE (b_Y - center[i].y);
 719           if (dist1 * (clust_a + M_SIZE / 2) < dist2 * (clust_b + M_SIZE / 2))
 720             {
 721               belong[i] = true;
 722               clust_a++;
 723             }
 724           else
 725             {
 726               belong[i] = false;
 727               clust_b++;
 728             }
 729         }
 730       /* kludge to fix degenerate cases */
 731       if (clust_a == M_SIZE + 1)
 732         belong[M_SIZE / 2] = false;
 733       else if (clust_b == M_SIZE + 1)
 734         belong[M_SIZE / 2] = true;
 735       /* compute new center of gravity of clusters */
 736       total_a = total_b = 0;
 737       a_X = a_Y = b_X = b_Y = 0;
 738       for (i = 0; i < M_SIZE + 1; i++)
 739         {
 740           if (belong[i])
 741             {
 742               a_X += center[i].x * center[i].area;
 743               a_Y += center[i].y * center[i].area;
 744               total_a += center[i].area;
 745             }
 746           else
 747             {
 748               b_X += center[i].x * center[i].area;
 749               b_Y += center[i].y * center[i].area;
 750               total_b += center[i].area;
 751             }
 752         }
 753       a_X /= total_a;
 754       a_Y /= total_a;
 755       b_X /= total_b;
 756       b_Y /= total_b;
 757       if (old_ax == (int) a_X && old_ay == (int) a_Y &&
 758           old_bx == (int) b_X && old_by == (int) b_Y)
 759         break;
 760     }
 761   /* Now 'belong' has the partition map */
 762   new_node = calloc (1, sizeof (*new_node));
 763   new_node->parent = node->parent;
 764   new_node->flags.is_leaf = node->flags.is_leaf;
 765   clust_a = clust_b = 0;
 766   if (node->flags.is_leaf)
 767     {
 768       int flag, a_flag, b_flag;
 769       flag = a_flag = b_flag = 1;
 770       for (i = 0; i < M_SIZE + 1; i++)
 771         {
 772           if (belong[i])
 773             {
 774               node->u.rects[clust_a++] = node->u.rects[i];
 775               if (node->flags.manage & flag)
 776                 a_manage |= a_flag;
 777               a_flag <<= 1;
 778             }
 779           else
 780             {
 781               new_node->u.rects[clust_b++] = node->u.rects[i];
 782               if (node->flags.manage & flag)
 783                 b_manage |= b_flag;
 784               b_flag <<= 1;
 785             }
 786           flag <<= 1;
 787         }
 788     }
 789   else
 790     {
 791       for (i = 0; i < M_SIZE + 1; i++)
 792         {
 793           if (belong[i])
 794             node->u.kids[clust_a++] = node->u.kids[i];
 795           else
 796             {
 797               node->u.kids[i]->parent = new_node;
 798               new_node->u.kids[clust_b++] = node->u.kids[i];
 799             }
 800         }
 801     }
 802   node->flags.manage = a_manage;
 803   new_node->flags.manage = b_manage;
 804   assert (clust_a != 0);
 805   assert (clust_b != 0);
 806   if (node->flags.is_leaf)
 807     for (; clust_a < M_SIZE + 1; clust_a++)
 808       node->u.rects[clust_a].bptr = NULL;
 809   else
 810     for (; clust_a < M_SIZE + 1; clust_a++)
 811       node->u.kids[clust_a] = NULL;
 812   adjust_bounds (node);
 813   sort_node (node);
 814   adjust_bounds (new_node);
 815   sort_node (new_node);
 816   return (new_node);
 817 }
 818
 819 /* split a node according to clusters
 820  */
 821 static void
 822 split_node (struct rtree_node *node)
 823 {
 824   int i;
 825   struct rtree_node *new_node;
 826
 827   assert (node);
 828   assert (node->flags.is_leaf ? (void *) node->u.rects[M_SIZE].
 829           bptr : (void *) node->u.kids[M_SIZE]);
 830   new_node = find_clusters (node);
 831   if (node->parent == NULL)     /* split root node */
 832     {
 833       struct rtree_node *second;
 834
 835       second = calloc (1, sizeof (*second));
 836       *second = *node;
 837       if (!second->flags.is_leaf)
 838         for (i = 0; i < M_SIZE; i++)
 839           if (second->u.kids[i])
 840             second->u.kids[i]->parent = second;
 841       node->flags.is_leaf = 0;
 842       node->flags.manage = 0;
 843       second->parent = new_node->parent = node;
 844       node->u.kids[0] = new_node;
 845       node->u.kids[1] = second;
 846       for (i = 2; i < M_SIZE + 1; i++)
 847         node->u.kids[i] = NULL;
 848       adjust_bounds (node);
 849       sort_node (node);
 850 #ifdef SLOW_ASSERTS
 851       assert (__r_tree_is_good (node));
 852 #endif
 853       return;
 854     }
 855   for (i = 0; i < M_SIZE; i++)
 856     if (!node->parent->u.kids[i])
 857       break;
 858   node->parent->u.kids[i] = new_node;
 859 #ifdef SLOW_ASSERTS
 860   assert (__r_node_is_good (node));
 861   assert (__r_node_is_good (new_node));
 862 #endif
 863   if (i < M_SIZE)
 864     {
 865 #ifdef SLOW_ASSERTS
 866       assert (__r_node_is_good (node->parent));
 867 #endif
 868       sort_node (node->parent);
 869       return;
 870     }
 871   split_node (node->parent);
 872 }
 873
 874 static inline int
 875 contained (struct rtree_node *node, const BoxType * query)
 876 {
 877   if (node->box.X1 > query->X1 || node->box.X2 < query->X2 ||
 878       node->box.Y1 > query->Y1 || node->box.Y2 < query->Y2)
 879     return 0;
 880   return 1;
 881 }
 882
 883
 884 static inline double
 885 penalty (struct rtree_node *node, const BoxType * query)
 886 {
 887   double score;
 888
 889   /* Compute the area penalty for inserting here and return.
 890    * The penalty is the increase in area necessary
 891    * to include the query->
 892    */
 893   score  = (MAX (node->box.X2, query->X2) - MIN (node->box.X1, query->X1));
 894   score *= (MAX (node->box.Y2, query->Y2) - MIN (node->box.Y1, query->Y1));
 895   score -=
 896     (double)(node->box.X2 - node->box.X1) *
 897     (double)(node->box.Y2 - node->box.Y1);
 898   return score;
 899 }
 900
 901 static void
 902 __r_insert_node (struct rtree_node *node, const BoxType * query,
 903                  int manage, bool force)
 904 {
 905
 906 #ifdef SLOW_ASSERTS
 907   assert (__r_node_is_good (node));
 908 #endif
 909   /* Ok, at this point we must already enclose the query or we're forcing
 910    * this node to expand to enclose it, so if we're a leaf, simply store
 911    * the query here
 912    */
 913
 914   if (node->flags.is_leaf)
 915     {
 916       register int i;
 917
 918       if (UNLIKELY (manage))
 919         {
 920           register int flag = 1;
 921
 922           for (i = 0; i < M_SIZE; i++)
 923             {
 924               if (!node->u.rects[i].bptr)
 925                 break;
 926               flag <<= 1;
 927             }
 928           node->flags.manage |= flag;
 929         }
 930       else
 931         {
 932           for (i = 0; i < M_SIZE; i++)
 933             if (!node->u.rects[i].bptr)
 934               break;
 935         }
 936       /* the node always has an extra space available */
 937       node->u.rects[i].bptr = query;
 938       node->u.rects[i].bounds = *query;
 939       /* first entry in node determines initial bounding box */
 940       if (i == 0)
 941         node->box = *query;
 942       else if (force)
 943         {
 944           MAKEMIN (node->box.X1, query->X1);
 945           MAKEMAX (node->box.X2, query->X2);
 946           MAKEMIN (node->box.Y1, query->Y1);
 947           MAKEMAX (node->box.Y2, query->Y2);
 948         }
 949       if (i < M_SIZE)
 950         {
 951           sort_node (node);
 952           return;
 953         }
 954       /* we must split the node */
 955       split_node (node);
 956       return;
 957     }
 958   else
 959     {
 960       int i;
 961       struct rtree_node *best_node;
 962       double score, best_score;
 963
 964       if (force)
 965         {
 966           MAKEMIN (node->box.X1, query->X1);
 967           MAKEMAX (node->box.X2, query->X2);
 968           MAKEMIN (node->box.Y1, query->Y1);
 969           MAKEMAX (node->box.Y2, query->Y2);
 970         }
 971
 972       /* this node encloses it, but it's not a leaf, so descend the tree */
 973
 974       /* First check if any children actually encloses it */
 975       assert (node->u.kids[0]);
 976       for (i = 0; i < M_SIZE; i++)
 977         {
 978           if (!node->u.kids[i])
 979             break;
 980           if (contained (node->u.kids[i], query))
 981             {
 982               __r_insert_node (node->u.kids[i], query, manage, false);
 983               sort_node (node);
 984               return;
 985             }
 986         }
 987
 988       /* see if there is room for a new leaf node */
 989       if (node->u.kids[0]->flags.is_leaf && i < M_SIZE)
 990         {
 991           struct rtree_node *new_node;
 992           new_node = calloc (1, sizeof (*new_node));
 993           new_node->parent = node;
 994           new_node->flags.is_leaf = true;
 995           node->u.kids[i] = new_node;
 996           new_node->u.rects[0].bptr = query;
 997           new_node->u.rects[0].bounds = *query;
 998           new_node->box = *query;
 999           if (UNLIKELY (manage))
1000             new_node->flags.manage = 1;
1001           sort_node (node);
1002           return;
1003         }
1004
1005       /* Ok, so we're still here - look for the best child to push it into */
1006       best_score = penalty (node->u.kids[0], query);
1007       best_node = node->u.kids[0];
1008       for (i = 1; i < M_SIZE; i++)
1009         {
1010           if (!node->u.kids[i])
1011             break;
1012           score = penalty (node->u.kids[i], query);
1013           if (score < best_score)
1014             {
1015               best_score = score;
1016               best_node = node->u.kids[i];
1017             }
1018         }
1019       __r_insert_node (best_node, query, manage, true);
1020       sort_node (node);
1021       return;
1022     }
1023 }
1024
1025 void
1026 r_insert_entry (rtree_t * rtree, const BoxType * which, int man)
1027 {
1028   assert (which);
1029   assert (which->X1 <= which->X2);
1030   assert (which->Y1 <= which->Y2);
1031   /* recursively search the tree for the best leaf node */
1032   assert (rtree->root);
1033   __r_insert_node (rtree->root, which, man,
1034                    rtree->root->box.X1 > which->X1
1035                    || rtree->root->box.X2 < which->X2
1036                    || rtree->root->box.Y1 > which->Y1
1037                    || rtree->root->box.Y2 < which->Y2);
1038   rtree->size++;
1039 }
1040
1041 bool
1042 __r_delete (struct rtree_node *node, const BoxType * query)
1043 {
1044   int i, flag, mask, a;
1045
1046   /* the tree might be inconsistent during delete */
1047   if (query->X1 < node->box.X1 || query->Y1 < node->box.Y1
1048       || query->X2 > node->box.X2 || query->Y2 > node->box.Y2)
1049     return false;
1050   if (!node->flags.is_leaf)
1051     {
1052       for (i = 0; i < M_SIZE; i++)
1053         {
1054           /* if this is us being removed, free and copy over */
1055           if (node->u.kids[i] == (struct rtree_node *) query)
1056             {
1057               free ((void *) query);
1058               for (; i < M_SIZE; i++)
1059                 {
1060                   node->u.kids[i] = node->u.kids[i + 1];
1061                   if (!node->u.kids[i])
1062                     break;
1063                 }
1064               /* nobody home here now ? */
1065               if (!node->u.kids[0])
1066                 {
1067                   if (!node->parent)
1068                     {
1069                       /* wow, the root is empty! */
1070                       node->flags.is_leaf = 1;
1071                       /* changing type of node, be sure it's all zero */
1072                       for (i = 1; i < M_SIZE + 1; i++)
1073                         node->u.rects[i].bptr = NULL;
1074                       return true;
1075                     }
1076                   return (__r_delete (node->parent, &node->box));
1077                 }
1078               else
1079                 /* propegate boundary adjust upward */
1080                 while (node)
1081                   {
1082                     adjust_bounds (node);
1083                     node = node->parent;
1084                   }
1085               return true;
1086             }
1087           if (node->u.kids[i])
1088             {
1089               if (__r_delete (node->u.kids[i], query))
1090                 return true;
1091             }
1092           else
1093             break;
1094         }
1095       return false;
1096     }
1097   /* leaf node here */
1098   mask = 0;
1099   a = 1;
1100   for (i = 0; i < M_SIZE; i++)
1101     {
1102 #ifdef DELETE_BY_POINTER
1103       if (!node->u.rects[i].bptr || node->u.rects[i].bptr == query)
1104 #else
1105       if (node->u.rects[i].bounds.X1 == query->X1 &&
1106           node->u.rects[i].bounds.X2 == query->X2 &&
1107           node->u.rects[i].bounds.Y1 == query->Y1 &&
1108           node->u.rects[i].bounds.Y2 == query->Y2)
1109 #endif
1110         break;
1111       mask |= a;
1112       a <<= 1;
1113     }
1114   if (!node->u.rects[i].bptr)
1115     return false;               /* not at this leaf */
1116   if (node->flags.manage & a)
1117     {
1118       free ((void *) node->u.rects[i].bptr);
1119       node->u.rects[i].bptr = NULL;
1120     }
1121   /* squeeze the manage flags together */
1122   flag = node->flags.manage & mask;
1123   mask = (~mask) << 1;
1124   node->flags.manage = flag | ((node->flags.manage & mask) >> 1);
1125   /* remove the entry */
1126   for (; i < M_SIZE; i++)
1127     {
1128       node->u.rects[i] = node->u.rects[i + 1];
1129       if (!node->u.rects[i].bptr)
1130         break;
1131     }
1132   if (!node->u.rects[0].bptr)
1133     {
1134       if (node->parent)
1135         __r_delete (node->parent, &node->box);
1136       return true;
1137     }
1138   else
1139     /* propagate boundary adjustment upward */
1140     while (node)
1141       {
1142         adjust_bounds (node);
1143         node = node->parent;
1144       }
1145   return true;
1146 }
1147
1148 bool
1149 r_delete_entry (rtree_t * rtree, const BoxType * box)
1150 {
1151   bool r;
1152
1153   assert (box);
1154   assert (rtree);
1155   r = __r_delete (rtree->root, box);
1156   if (r)
1157     rtree->size--;
1158 #ifdef SLOW_ASSERTS
1159   assert (__r_tree_is_good (rtree->root));
1160 #endif
1161   return r;
1162 }
1163
1164 int
1165 __r_sub (struct rtree_node *node, const BoxType * b, const BoxType * a,
1166          jmp_buf * e)
1167 {
1168   int i;
1169
1170   if (node->flags.is_leaf)
1171     {
1172       for (i = 0; i < M_SIZE; i++)
1173         if (node->u.rects[i].bptr == b)
1174           {
1175             node->u.rects[i].bptr = a;
1176             assert (a->X1 == node->u.rects[i].bounds.X1);
1177             assert (a->X2 == node->u.rects[i].bounds.X2);
1178             assert (a->Y1 == node->u.rects[i].bounds.Y1);
1179             assert (a->Y2 == node->u.rects[i].bounds.Y2);
1180             longjmp (*e, 1);
1181           }
1182       return 0;
1183     }
1184   for (i = 0; i < M_SIZE; i++)
1185     if (node->u.kids[i] && __r_sub (node->u.kids[i], b, a, e))
1186       return 1;
1187   return 0;
1188 }
1189
1190
1191 void
1192 r_substitute (rtree_t * rtree, const BoxType * before, const BoxType * after)
1193 {
1194   jmp_buf env;
1195
1196   if (before == after)
1197     return;
1198   if (setjmp (env) == 0)
1199     __r_sub (rtree->root, before, after, &env);
1200 }