navit/navit/map/csv/quadtree.c

   1 /**
   2  * Navit, a modular navigation system.
   3  * Copyright (C) 2005-2011 Navit Team
   4  *
   5  * This program is free software; you can redistribute it and/or
   6  * modify it under the terms of the GNU General Public License
   7  * version 2 as published by the Free Software Foundation.
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with this program; if not, write to the
  16  * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
  17  * Boston, MA  02110-1301, USA.
  18  */
  19
  20 #include <stdlib.h>
  21 #include <glib.h>
  22
  23 #include "debug.h"
  24
  25 #include "quadtree.h"
  26
  27 #define QUADTREE_NODE_CAPACITY 10
  28
  29 #define MAX_DOUBLE 9999999
  30 /* Check if two given line segments overlap (1-d case) */
  31 #define segments_overlap(x11,x12, x21,x22) (((x11)<=(x21) && (x21)<=(x12)) || ((x21)<=(x11) && (x11)<=(x22)))
  32 /* Check if two given rectanlgles overlap (2-d case) */
  33 #define rects_overlap(x11,y11,x12,y12, x21,y21,x22,y22) (segments_overlap(x11,x12, x21,x22) && segments_overlap(y11,y12, y21,y22))
  34
  35 /* Structure describing quadtree iterative query */
  36 struct quadtree_iter {
  37         /* List representing stack of quad_tree_iter_nodes referring to higher-level quadtree_nodes */
  38         GList *iter_nodes;
  39         double xmin,xmax,ymin,ymax;
  40         /* Current item pointer */
  41         struct quadtree_item *item;
  42         void (*item_free)(void *context, struct quadtree_item *qitem);
  43         void *item_free_context;
  44 };
  45
  46 /* Structure describing one level of the quadtree iterative query */
  47 struct quadtree_iter_node {
  48         struct quadtree_node *node;
  49         /* Number of subnode being analyzed (for non-leafs) */
  50         int subnode;
  51         /* Number of item being analyzed (for leafs) */
  52         int item;
  53         /* Number of subitems in items array (for leafs) */
  54         int node_num;
  55         /* If the node referenced was a leaf when it was analyzed */
  56         int is_leaf;
  57         struct quadtree_item *items[QUADTREE_NODE_CAPACITY];
  58 };
  59
  60
  61 static double
  62 dist_sq(double x1,double y1,double x2,double y2)
  63 {
  64   return (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1);
  65 }
  66
  67 struct quadtree_node*
  68 quadtree_node_new(struct quadtree_node* parent, double xmin, double xmax, double ymin, double ymax ) {
  69     struct quadtree_node*ret = g_new0(struct quadtree_node,1);
  70     ret->xmin = xmin;
  71     ret->xmax = xmax;
  72     ret->ymin = ymin;
  73     ret->ymax = ymax;
  74     ret->is_leaf = 1;
  75     ret->parent = parent;
  76     return ret;
  77 }
  78
  79 /*
  80  * searches all four subnodes recursively for the list of items within a rectangle
  81  */
  82 void
  83 quadtree_find_rect_items(struct quadtree_node* this_, double dXMin, double dXMax, double dYMin, double dYMax, GList**out) {
  84
  85     struct quadtree_node* nodes[4] = { this_->aa, this_->ab, this_->ba, this_->bb };
  86     if( this_->is_leaf ) {
  87         int i;
  88         for(i=0;i<this_->node_num;++i) {        //select only items within input rectangle
  89                 if(dXMin<=this_->items[i]->longitude && this_->items[i]->longitude<=dXMax &&
  90                    dYMin<=this_->items[i]->latitude && this_->items[i]->latitude<=dYMax
  91                   ) {
  92                     *out=g_list_prepend(*out,this_->items[i]);
  93                 }
  94         }
  95     }
  96     else {
  97       int i;
  98       for( i=0;i<4;++i) {
  99         if(nodes[i] ) {
 100           //limit flooding
 101           if(nodes[i]->xmax<dXMin || dXMax<nodes[i]->xmin ||
 102              nodes[i]->ymax<dYMin || dYMax<nodes[i]->ymin
 103             ) {
 104                 continue;
 105           }
 106           //recurse into subtiles if there is at least one subtile corner within input rectangle
 107           quadtree_find_rect_items(nodes[i],dXMin,dXMax,dYMin,dYMax,out);
 108         }
 109       }
 110     }
 111 }
 112
 113 /*
 114  * searches all four subnodes recursively for the closest item
 115  */
 116 struct quadtree_item*
 117 quadtree_find_nearest_flood(struct quadtree_node* this_, struct quadtree_item* item, double current_max, struct quadtree_node* toSkip) {
 118     struct quadtree_node* nodes[4] = { this_->aa, this_->ab, this_->ba, this_->bb };
 119     struct quadtree_item*res = NULL;
 120     if( this_->is_leaf ) {
 121         int i;
 122         double distance_sq = current_max;
 123         for(i=0;i<this_->node_num;++i) {
 124             double curr_dist_sq = dist_sq(item->longitude,item->latitude,this_->items[i]->longitude,this_->items[i]->latitude);
 125             if(curr_dist_sq<distance_sq) {
 126                 distance_sq = curr_dist_sq;
 127                 res = this_->items[i];
 128             }
 129         }
 130     }
 131     else {
 132       int i;
 133       for( i=0;i<4;++i) {
 134         if(nodes[i] && nodes[i]!=toSkip) {
 135           //limit flooding
 136           struct quadtree_item*res_tmp = NULL;
 137           if(
 138              dist_sq(nodes[i]->xmin,nodes[i]->ymin,item->longitude,item->latitude)<current_max ||
 139              dist_sq(nodes[i]->xmax,nodes[i]->ymin,item->longitude,item->latitude)<current_max ||
 140              dist_sq(nodes[i]->xmax,nodes[i]->ymax,item->longitude,item->latitude)<current_max ||
 141              dist_sq(nodes[i]->xmin,nodes[i]->ymax,item->longitude,item->latitude)<current_max
 142             ) {
 143             res_tmp = quadtree_find_nearest_flood(nodes[i],item,current_max,NULL);
 144           }
 145           if(res_tmp) {
 146           double curr_dist_sq;
 147           res = res_tmp;
 148           curr_dist_sq = dist_sq(item->longitude,item->latitude,res->longitude,res->latitude);
 149             if(curr_dist_sq<current_max) {
 150                 current_max = curr_dist_sq;
 151             }
 152           }
 153         }
 154       }
 155     }
 156   return res;
 157 }
 158
 159 /*
 160  * tries to find an item exactly
 161  */
 162 struct quadtree_item*
 163 quadtree_find_item(struct quadtree_node* this_, struct quadtree_item* item) {
 164     struct quadtree_item*res = NULL;
 165     if( ! this_ ) {
 166       return NULL;
 167     }
 168     if( this_->is_leaf ) {
 169         int i;
 170         for(i=0;i<this_->node_num;++i) {
 171             //TODO equality check may not be correct on float values! maybe it can be replaced by range check with some tolerance
 172             if(item->longitude==this_->items[i]->longitude && item->latitude==this_->items[i]->latitude) {
 173                 res = this_->items[i];
 174                 return res;
 175             }
 176         }
 177         return NULL;
 178     }
 179     else {
 180         if(
 181            this_->aa &&
 182            this_->aa->xmin<=item->longitude && item->longitude<this_->aa->xmax &&
 183            this_->aa->ymin<=item->latitude && item->latitude<this_->aa->ymax
 184            ) {
 185           res = quadtree_find_item(this_->aa,item);
 186         }
 187         else if(
 188            this_->ab &&
 189            this_->ab->xmin<=item->longitude && item->longitude<this_->ab->xmax &&
 190            this_->ab->ymin<=item->latitude && item->latitude<this_->ab->ymax
 191            ) {
 192           res = quadtree_find_item(this_->ab,item);
 193         }
 194         else if(
 195            this_->ba &&
 196            this_->ba->xmin<=item->longitude && item->longitude<this_->ba->xmax &&
 197            this_->ba->ymin<=item->latitude && item->latitude<this_->ba->ymax
 198            ) {
 199           res = quadtree_find_item(this_->ba,item);
 200         }
 201         else if(
 202            this_->bb &&
 203            this_->bb->xmin<=item->longitude && item->longitude<this_->bb->xmax &&
 204            this_->bb->ymin<=item->latitude && item->latitude<this_->bb->ymax
 205            ) {
 206           res = quadtree_find_item(this_->bb,item);
 207         }
 208         else {
 209           return NULL;
 210         }
 211     }
 212   return res;
 213 }
 214
 215 /*
 216  * returns the containing node for an item
 217  */
 218 struct quadtree_node*
 219 quadtree_find_containing_node(struct quadtree_node* root, struct quadtree_item* item)
 220 {
 221     struct quadtree_node*res = NULL;
 222
 223     if( ! root ) {
 224       return NULL;
 225     }
 226
 227     if( root->is_leaf ) {
 228         int i;
 229         for(i=0;i<root->node_num;++i) {
 230             if(item == root->items[i]) {
 231                 res = root;
 232             }
 233         }
 234     }
 235     else {
 236         if(
 237            root->aa &&
 238            root->aa->xmin<=item->longitude && item->longitude<root->aa->xmax &&
 239            root->aa->ymin<=item->latitude && item->latitude<root->aa->ymax
 240            ) {
 241           res = quadtree_find_containing_node(root->aa,item);
 242         }
 243         else if(
 244            root->ab &&
 245            root->ab->xmin<=item->longitude && item->longitude<root->ab->xmax &&
 246            root->ab->ymin<=item->latitude && item->latitude<root->ab->ymax
 247            ) {
 248           res = quadtree_find_containing_node(root->ab,item);
 249         }
 250         else if(
 251            root->ba &&
 252            root->ba->xmin<=item->longitude && item->longitude<root->ba->xmax &&
 253            root->ba->ymin<=item->latitude && item->latitude<root->ba->ymax
 254            ) {
 255           res = quadtree_find_containing_node(root->ba,item);
 256         }
 257         else if(
 258            root->bb &&
 259            root->bb->xmin<=item->longitude && item->longitude<root->bb->xmax &&
 260            root->bb->ymin<=item->latitude && item->latitude<root->bb->ymax
 261            ) {
 262           res = quadtree_find_containing_node(root->bb,item);
 263         }
 264         else {
 265           //this should not happen
 266         }
 267     }
 268   return res;
 269 }
 270
 271
 272 int  quadtree_delete_item(struct quadtree_node* root, struct quadtree_item* item)
 273 {
 274
 275   struct quadtree_node* qn = quadtree_find_containing_node(root,item);
 276   int i, bFound=0;
 277
 278   if(!qn || !qn->node_num) {
 279     return 0;
 280   }
 281
 282   for(i=0;i<qn->node_num;++i) {
 283     if( qn->items[i] == item) {
 284         qn->items[i]->deleted=1;
 285         bFound=1;
 286     }
 287   }
 288   return bFound;
 289 }
 290
 291
 292 /*
 293  * tries to find closest item, first it descend into the quadtree as much as possible, then if no point is found go up n levels and flood
 294  */
 295 struct quadtree_item*
 296 quadtree_find_nearest(struct quadtree_node* this_, struct quadtree_item* item) {
 297     struct quadtree_item*res = NULL;
 298     double distance_sq = MAX_DOUBLE;
 299     if( ! this_ ) {
 300       return NULL;
 301     }
 302     if( this_->is_leaf ) {
 303         int i;
 304         for(i=0;i<this_->node_num;++i) {
 305             double curr_dist_sq = dist_sq(item->longitude,item->latitude,this_->items[i]->longitude,this_->items[i]->latitude);
 306             if(curr_dist_sq<distance_sq) {
 307                 distance_sq = curr_dist_sq;
 308                 res = this_->items[i];
 309             }
 310         }
 311       //go up n levels
 312           if(!res && this_->parent) {
 313           struct quadtree_item*res2 = NULL;
 314                   struct quadtree_node* anchestor = this_->parent;
 315                   int cnt = 0;
 316                   while (anchestor->parent && cnt<4) {
 317                     anchestor = anchestor->parent;
 318                     ++cnt;
 319                   }
 320                 res2 = quadtree_find_nearest_flood(anchestor,item,distance_sq,NULL);
 321                 if(res2) {
 322                   res = res2;
 323                 }
 324           }
 325     } else {
 326         if(
 327            this_->aa &&
 328            this_->aa->xmin<=item->longitude && item->longitude<this_->aa->xmax &&
 329            this_->aa->ymin<=item->latitude && item->latitude<this_->aa->ymax
 330            ) {
 331           res = quadtree_find_nearest(this_->aa,item);
 332         }
 333         else if(
 334            this_->ab &&
 335            this_->ab->xmin<=item->longitude && item->longitude<this_->ab->xmax &&
 336            this_->ab->ymin<=item->latitude && item->latitude<this_->ab->ymax
 337            ) {
 338           res = quadtree_find_nearest(this_->ab,item);
 339         }
 340         else if(
 341            this_->ba &&
 342            this_->ba->xmin<=item->longitude && item->longitude<this_->ba->xmax &&
 343            this_->ba->ymin<=item->latitude && item->latitude<this_->ba->ymax
 344            ) {
 345           res = quadtree_find_nearest(this_->ba,item);
 346         }
 347         else if(
 348            this_->bb &&
 349            this_->bb->xmin<=item->longitude && item->longitude<this_->bb->xmax &&
 350            this_->bb->ymin<=item->latitude && item->latitude<this_->bb->ymax
 351            ) {
 352           res = quadtree_find_nearest(this_->bb,item);
 353         }
 354         else {
 355         if(this_->parent) {
 356             //go up two levels
 357             struct quadtree_node* anchestor = this_->parent;
 358             int cnt = 0;
 359             while (anchestor->parent && cnt<4) {
 360               anchestor = anchestor->parent;
 361               ++cnt;
 362             }
 363         res = quadtree_find_nearest_flood(anchestor,item,distance_sq,NULL);
 364         }
 365       }
 366     }
 367   return res;
 368
 369 }
 370
 371
 372 /**
 373  * @brief Free space occupied by deleted unreferenced items.
 374  * @param node pointer to the quadtree node
 375  * @param iter Quadtree iteration context.
 376  * @return nothing
 377  */
 378 void quadtree_node_drop_garbage(struct quadtree_node* node, struct quadtree_iter *iter)
 379 {
 380         int i,j;
 381         int node_num=node->node_num;
 382         dbg(1,"Processing unreferenced subnode children...\n");
 383         for(i=0,j=0;i<node_num;i++) {
 384                 if(node->items[i]->deleted && !node->items[i]->ref_count) {
 385                         if(iter->item_free) {
 386                                 (iter->item_free)(iter->item_free_context, node->items[i]);
 387                         } else {
 388                                 g_free(node->items[i]);
 389                         }
 390                         node->node_num--;
 391                         node->items[i]=NULL;
 392                 } else {
 393                         node->items[j++]=node->items[i];
 394                 }
 395                 if(i>j)
 396                         node->items[i]=NULL;
 397         }
 398 }
 399
 400 /**
 401  * @brief Add new node to quadtree.
 402  * @param this_ pointer to the quadtree (root) node
 403  * @param item item to add
 404  * @param iter Quadtree iteration context. Can be NULL if no garbage collection is needed.
 405  * @return nothing
 406  */
 407 void
 408 quadtree_add(struct quadtree_node* this_, struct quadtree_item* item, struct quadtree_iter *iter) {
 409     if( this_->is_leaf ) {
 410         int bSame = 1;
 411         int i;
 412
 413         if(iter)
 414                 quadtree_node_drop_garbage(this_, iter);
 415
 416         if(QUADTREE_NODE_CAPACITY-1 == this_->node_num) {
 417           double lon, lat;
 418           //avoid infinite recursion when all elements have the same coordinate
 419           lon = this_->items[0]->longitude;
 420           lat = this_->items[0]->latitude;
 421           for(i=1;i<this_->node_num;++i) {
 422             if (lon != this_->items[i]->longitude || lat != this_->items[i]->latitude) {
 423               bSame = 0;
 424               break;
 425             }
 426           }
 427           if (bSame) {
 428             //FIXME: memleak and items thrown away if more than QUADTREE_NODE_CAPACITY-1 items with same coordinates added.
 429             dbg(0,"Unable to add another item with same coordinates. Throwing item to the ground. Will leak %p.\n",item);
 430             return;
 431           }
 432           this_->items[this_->node_num++] = item;
 433           quadtree_split(this_);
 434         } else {
 435           this_->items[this_->node_num++] = item;
 436         }
 437     }
 438     else {
 439         if(
 440            this_->xmin<=item->longitude && item->longitude<this_->xmin+(this_->xmax-this_->xmin)/2.0 &&
 441            this_->ymin<=item->latitude && item->latitude<this_->ymin+(this_->ymax-this_->ymin)/2.0
 442            ) {
 443             if(!this_->aa) {
 444               this_->aa = quadtree_node_new( this_, this_->xmin, this_->xmin+(this_->xmax-this_->xmin)/2.0 , this_->ymin, this_->ymin+(this_->ymax-this_->ymin)/2.0 );
 445             }
 446           quadtree_add(this_->aa,item,iter);
 447         }
 448         else if(
 449            this_->xmin+(this_->xmax-this_->xmin)/2.0<=item->longitude && item->longitude<this_->xmax &&
 450            this_->ymin<=item->latitude && item->latitude<this_->ymin+(this_->ymax-this_->ymin)/2.0
 451            ) {
 452             if(!this_->ab) {
 453               this_->ab = quadtree_node_new( this_, this_->xmin+(this_->xmax-this_->xmin)/2.0, this_->xmax , this_->ymin, this_->ymin+(this_->ymax-this_->ymin)/2.0 );
 454             }
 455           quadtree_add(this_->ab,item,iter);
 456         }
 457         else if(
 458            this_->xmin<=item->longitude && item->longitude<this_->xmin+(this_->xmax-this_->xmin)/2.0 &&
 459            this_->ymin+(this_->ymax-this_->ymin)/2.0<=item->latitude && item->latitude<this_->ymax
 460            ) {
 461             if(!this_->ba) {
 462               this_->ba = quadtree_node_new( this_, this_->xmin, this_->xmin+(this_->xmax-this_->xmin)/2.0 , this_->ymin+(this_->ymax-this_->ymin)/2.0 , this_->ymax);
 463             }
 464           quadtree_add(this_->ba,item,iter);
 465         }
 466         else if(
 467            this_->xmin+(this_->xmax-this_->xmin)/2.0<=item->longitude && item->longitude<this_->xmax &&
 468            this_->ymin+(this_->ymax-this_->ymin)/2.0<=item->latitude && item->latitude<this_->ymax
 469            ) {
 470             if(!this_->bb) {
 471               this_->bb = quadtree_node_new( this_, this_->xmin+(this_->xmax-this_->xmin)/2.0, this_->xmax , this_->ymin+(this_->ymax-this_->ymin)/2.0 , this_->ymax);
 472             }
 473           quadtree_add(this_->bb,item,iter);
 474         }
 475     }
 476 }
 477
 478 void
 479 quadtree_split(struct quadtree_node* this_) {
 480     int i;
 481     this_->is_leaf = 0;
 482     for(i=0;i<this_->node_num;++i) {
 483         if(
 484            this_->xmin<=this_->items[i]->longitude && this_->items[i]->longitude<this_->xmin+(this_->xmax-this_->xmin)/2.0 &&
 485            this_->ymin<=this_->items[i]->latitude && this_->items[i]->latitude<this_->ymin+(this_->ymax-this_->ymin)/2.0
 486            ) {
 487           if(!this_->aa) {
 488             this_->aa = quadtree_node_new( this_, this_->xmin, this_->xmin+(this_->xmax-this_->xmin)/2.0 , this_->ymin, this_->ymin+(this_->ymax-this_->ymin)/2.0 );
 489           }
 490           quadtree_add(this_->aa,this_->items[i],NULL);
 491         }
 492         else if(
 493            this_->xmin+(this_->xmax-this_->xmin)/2.0<=this_->items[i]->longitude && this_->items[i]->longitude<this_->xmax &&
 494            this_->ymin<=this_->items[i]->latitude && this_->items[i]->latitude<this_->ymin+(this_->ymax-this_->ymin)/2.0
 495            ) {
 496           if(!this_->ab) {
 497             this_->ab = quadtree_node_new( this_, this_->xmin+(this_->xmax-this_->xmin)/2.0, this_->xmax , this_->ymin, this_->ymin+(this_->ymax-this_->ymin)/2.0 );
 498           }
 499           quadtree_add(this_->ab,this_->items[i],NULL);
 500         }
 501         else if(
 502            this_->xmin<=this_->items[i]->longitude && this_->items[i]->longitude<this_->xmin+(this_->xmax-this_->xmin)/2.0 &&
 503            this_->ymin+(this_->ymax-this_->ymin)/2.0<=this_->items[i]->latitude && this_->items[i]->latitude<this_->ymax
 504            ) {
 505           if(!this_->ba) {
 506             this_->ba = quadtree_node_new( this_, this_->xmin, this_->xmin+(this_->xmax-this_->xmin)/2.0 , this_->ymin+(this_->ymax-this_->ymin)/2.0 , this_->ymax);
 507           }
 508           quadtree_add(this_->ba,this_->items[i],NULL);
 509         }
 510         else if(
 511            this_->xmin+(this_->xmax-this_->xmin)/2.0<=this_->items[i]->longitude && this_->items[i]->longitude<this_->xmax &&
 512            this_->ymin+(this_->ymax-this_->ymin)/2.0<=this_->items[i]->latitude && this_->items[i]->latitude<this_->ymax
 513            ) {
 514           if(!this_->bb) {
 515             this_->bb = quadtree_node_new( this_, this_->xmin+(this_->xmax-this_->xmin)/2.0, this_->xmax , this_->ymin+(this_->ymax-this_->ymin)/2.0 , this_->ymax);
 516           }
 517           quadtree_add(this_->bb,this_->items[i],NULL);
 518         }
 519         this_->items[i]=NULL;
 520     }
 521     this_->node_num = 0;
 522 }
 523
 524 void
 525 quadtree_destroy(struct quadtree_node* this_) {
 526     if(this_->aa) {
 527         quadtree_destroy(this_->aa);
 528         this_->aa = NULL;
 529     }
 530     if(this_->ab) {
 531         quadtree_destroy(this_->ab);
 532         this_->ab = NULL;
 533     }
 534     if(this_->ba) {
 535         quadtree_destroy(this_->ba);
 536         this_->ba = NULL;
 537     }
 538     if(this_->bb) {
 539         quadtree_destroy(this_->bb);
 540         this_->bb = NULL;
 541     }
 542     free(this_);
 543 }
 544
 545
 546 /*
 547  * @brief Start iterating over quadtree items not marked for deletion.
 548  * @param this_ Pointer to the quadtree root node.
 549  * @param dXMin,dXMax,dYMin,dYMax boundig box of area of interest.
 550  * @param item_free function to be called to free the qitem, first parameter would be context, second parameter - actual qitem pointer to free
 551  *              if this parameter is NULL, nothing would be called to free qitem pointer (possible massive memleak unless you store each qitem pointer in some
 552  *              alternate storage)
 553  * @param item_free_context data to be passed as a first parameter to item_free function
 554  * @return pointer to the quad tree iteration structure.
 555  */
 556 struct quadtree_iter *quadtree_query(struct quadtree_node *this_, double dXMin, double dXMax, double dYMin, double dYMax,void (*item_free)(void *context, struct quadtree_item *qitem), void *item_free_context)
 557 {
 558         struct quadtree_iter *ret=g_new0(struct quadtree_iter,1);
 559         struct quadtree_iter_node *n=g_new0(struct quadtree_iter_node,1);
 560         ret->xmin=dXMin;
 561         ret->xmax=dXMax;
 562         ret->ymin=dYMin;
 563         ret->ymax=dYMax;
 564         dbg(1,"%f %f %f %f\n",dXMin,dXMax,dYMin,dYMax)
 565         ret->item_free=item_free;
 566         ret->item_free_context=item_free_context;
 567         n->node=this_;
 568         ret->iter_nodes=g_list_prepend(ret->iter_nodes,n);
 569         n->is_leaf=this_->is_leaf;
 570         if(this_->is_leaf) {
 571                 int i;
 572                 n->node_num=this_->node_num;
 573                 for(i=0;i<n->node_num;i++) {
 574                         n->items[i]=this_->items[i];
 575                         n->items[i]->ref_count++;
 576                 }
 577         }
 578
 579         this_->ref_count++;
 580         dbg(1,"Query %p \n",this_)
 581         return ret;
 582
 583 }
 584
 585 /*
 586  * @brief End iteration.
 587  * @param iter Pointer to the quadtree iteration structure.
 588  */
 589 void quadtree_query_free(struct quadtree_iter *iter)
 590 {
 591         //Get the rest of the data to collect garbage and dereference all the items/nodes
 592         //TODO: No need to iterate the whole tree here. Just dereference nodes/items (if any).
 593         while(quadtree_item_next(iter));
 594         g_free(iter);
 595 }
 596
 597
 598 /*
 599  * @brief Mark current item of iterator for deletion.
 600  * @param iter Pointer to the quadtree iteration structure.
 601  */
 602 void quadtree_item_delete(struct quadtree_iter *iter)
 603 {
 604         if(iter->item)
 605                 iter->item->deleted=1;
 606 }
 607
 608 /*
 609  * @brief Get next item from the quadtree. Any  met unreferenced items/nodes marked for deletion will be freed here.
 610  * @param iter Pointer to the quadtree iteration structure.
 611  * @return pointer to the next item, or NULL if no items are left.
 612  */
 613 struct quadtree_item * quadtree_item_next(struct quadtree_iter *iter)
 614 {
 615         struct quadtree_iter_node *iter_node;
 616         struct quadtree_node *subnode;
 617
 618         if(iter->item) {
 619                 iter->item->ref_count--;
 620                 iter->item=NULL;
 621         }
 622
 623         while(iter->iter_nodes)  {
 624                 struct quadtree_node *nodes[4];
 625                 int i;
 626                 iter_node=iter->iter_nodes->data;
 627
 628                 if(iter_node->is_leaf) {
 629                         /* Try to find undeleted item in the current node */
 630                         dbg(1,"find item %p %p ...\n",iter->iter_nodes,iter->iter_nodes->data);
 631                         while(iter_node->item<iter_node->node_num) {
 632                                 dbg(1,"%d %d\n",iter_node->item,iter_node->items[iter_node->item]->deleted);
 633                                 if(iter_node->items[iter_node->item]->deleted) {
 634                                         iter_node->item++;
 635                                         continue;
 636                                 }
 637                                 iter->item=iter_node->items[iter_node->item];
 638                                 iter_node->item++;
 639                                 dbg(1,"Returning %p\n",iter->item);
 640                                 iter->item->ref_count++;
 641                                 return iter->item;
 642                         }
 643                         for(i=0;i<iter_node->node_num;i++) {
 644                                 iter_node->items[i]->ref_count--;
 645                         }
 646                 } else {
 647                         /* No items left, try to find non-empty subnode */
 648                         nodes[0]=iter_node->node->aa;
 649                         nodes[1]=iter_node->node->ab;
 650                         nodes[2]=iter_node->node->ba;
 651                         nodes[3]=iter_node->node->bb;
 652
 653                         for(subnode=NULL;!subnode && iter_node->subnode<4;iter_node->subnode++) {
 654                                 i=iter_node->subnode;
 655                                 if(!nodes[i] || !rects_overlap(nodes[i]->xmin, nodes[i]->ymin, nodes[i]->xmax, nodes[i]->ymax, iter->xmin, iter->ymin, iter->xmax, iter->ymax))
 656                                         continue;
 657                                 dbg(1,"%f %f %f %f\n",nodes[i]->xmin, nodes[i]->xmax, nodes[i]->ymin, nodes[i]->ymax)
 658                                 subnode=nodes[i];
 659                         }
 660
 661                         if(subnode) {
 662                                 /* Go one level deeper */
 663                                 dbg(1,"Go one level deeper...\n");
 664                                 iter_node=g_new0(struct quadtree_iter_node, 1);
 665                                 iter_node->node=subnode;
 666                                 iter_node->is_leaf=subnode->is_leaf;
 667                                 if(iter_node->is_leaf) {
 668                                         int i;
 669                                         iter_node->node_num=subnode->node_num;
 670                                         for(i=0;i<iter_node->node_num;i++) {
 671                                                 iter_node->items[i]=subnode->items[i];
 672                                                 iter_node->items[i]->ref_count++;
 673                                         }
 674                                 }
 675                                 subnode->ref_count++;
 676                                 iter->iter_nodes=g_list_prepend(iter->iter_nodes,iter_node);
 677                                 continue;
 678                         }
 679                 }
 680
 681                 /* No nodes and items left, fix up current subnode... */
 682                 iter_node=iter->iter_nodes->data;
 683                 subnode=iter_node->node;
 684                 subnode->ref_count--;
 685
 686                 if(!subnode->aa && !subnode->ab && !subnode->ba && !subnode->bb)
 687                         subnode->is_leaf=1;
 688
 689                 /*  1. free deleted unreferenced items */
 690                 quadtree_node_drop_garbage(subnode, iter);
 691
 692                 /* 2. remove empty leaf subnode if it's unreferenced */
 693
 694                 if(!subnode->ref_count && !subnode->node_num && subnode->is_leaf ) {
 695                         dbg(1,"Going to delete an empty unreferenced leaf subnode...\n");
 696
 697                         if(subnode->parent) {
 698                                 if(subnode->parent->aa==subnode) {
 699                                         subnode->parent->aa=NULL;
 700                                 } else if(subnode->parent->ab==subnode) {
 701                                         subnode->parent->ab=NULL;
 702                                 } else if(subnode->parent->ba==subnode) {
 703                                         subnode->parent->ba=NULL;
 704                                 } else if(subnode->parent->bb==subnode) {
 705                                         subnode->parent->bb=NULL;
 706                                 } else {
 707                                         dbg(0,"Found Quadtree structure corruption while trying to free an empty node.\n");
 708                                 }
 709
 710                                 if(!subnode->parent->aa && !subnode->parent->ab && !subnode->parent->ba && !subnode->parent->bb )
 711                                         subnode->parent->is_leaf=1;
 712                                 g_free(subnode);
 713                         } else
 714                                 dbg(1,"Quadtree is empty. NOT deleting the root subnode...\n");
 715
 716                 }
 717
 718                 /* Go one step towards root */
 719                 dbg(2,"Going towards root...\n");
 720                 g_free(iter->iter_nodes->data);
 721                 iter->iter_nodes=g_list_delete_link(iter->iter_nodes,iter->iter_nodes);
 722         }
 723
 724         iter->item=NULL;
 725         return NULL;
 726 }
 727