diff options
Diffstat (limited to 'navit/map/csv/quadtree.c')
| -rw-r--r-- | navit/map/csv/quadtree.c | 959 |
1 files changed, 468 insertions, 491 deletions
diff --git a/navit/map/csv/quadtree.c b/navit/map/csv/quadtree.c index ca6068541..eda5a476d 100644 --- a/navit/map/csv/quadtree.c +++ b/navit/map/csv/quadtree.c @@ -34,34 +34,32 @@ /* Structure describing quadtree iterative query */ struct quadtree_iter { - /* List representing stack of quad_tree_iter_nodes referring to higher-level quadtree_nodes */ - GList *iter_nodes; - double xmin,xmax,ymin,ymax; - /* Current item pointer */ - struct quadtree_item *item; - void (*item_free)(void *context, struct quadtree_item *qitem); - void *item_free_context; + /* List representing stack of quad_tree_iter_nodes referring to higher-level quadtree_nodes */ + GList *iter_nodes; + double xmin,xmax,ymin,ymax; + /* Current item pointer */ + struct quadtree_item *item; + void (*item_free)(void *context, struct quadtree_item *qitem); + void *item_free_context; }; /* Structure describing one level of the quadtree iterative query */ struct quadtree_iter_node { - struct quadtree_node *node; - /* Number of subnode being analyzed (for non-leafs) */ - int subnode; - /* Number of item being analyzed (for leafs) */ - int item; - /* Number of subitems in items array (for leafs) */ - int node_num; - /* If the node referenced was a leaf when it was analyzed */ - int is_leaf; - struct quadtree_item *items[QUADTREE_NODE_CAPACITY]; -}; - - -static double -dist_sq(double x1,double y1,double x2,double y2) -{ - return (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1); + struct quadtree_node *node; + /* Number of subnode being analyzed (for non-leafs) */ + int subnode; + /* Number of item being analyzed (for leafs) */ + int item; + /* Number of subitems in items array (for leafs) */ + int node_num; + /* If the node referenced was a leaf when it was analyzed */ + int is_leaf; + struct quadtree_item *items[QUADTREE_NODE_CAPACITY]; +}; + + +static double dist_sq(double x1,double y1,double x2,double y2) { + return (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1); } struct quadtree_node* @@ -79,34 +77,33 @@ quadtree_node_new(struct quadtree_node* parent, double xmin, double xmax, double /* * searches all four subnodes recursively for the list of items within a rectangle */ -void -quadtree_find_rect_items(struct quadtree_node* this_, double dXMin, double dXMax, double dYMin, double dYMax, GList**out) { +void quadtree_find_rect_items(struct quadtree_node* this_, double dXMin, double dXMax, double dYMin, double dYMax, + GList**out) { struct quadtree_node* nodes[4] = { this_->aa, this_->ab, this_->ba, this_->bb }; - if( this_->is_leaf ) { + if( this_->is_leaf ) { int i; - for(i=0;i<this_->node_num;++i) { //select only items within input rectangle - if(dXMin<=this_->items[i]->longitude && this_->items[i]->longitude<=dXMax && - dYMin<=this_->items[i]->latitude && this_->items[i]->latitude<=dYMax - ) { - *out=g_list_prepend(*out,this_->items[i]); - } + for(i=0; i<this_->node_num; ++i) { //select only items within input rectangle + if(dXMin<=this_->items[i]->longitude && this_->items[i]->longitude<=dXMax && + dYMin<=this_->items[i]->latitude && this_->items[i]->latitude<=dYMax + ) { + *out=g_list_prepend(*out,this_->items[i]); + } + } + } else { + int i; + for( i=0; i<4; ++i) { + if(nodes[i] ) { + //limit flooding + if(nodes[i]->xmax<dXMin || dXMax<nodes[i]->xmin || + nodes[i]->ymax<dYMin || dYMax<nodes[i]->ymin + ) { + continue; + } + //recurse into subtiles if there is at least one subtile corner within input rectangle + quadtree_find_rect_items(nodes[i],dXMin,dXMax,dYMin,dYMax,out); + } } - } - else { - int i; - for( i=0;i<4;++i) { - if(nodes[i] ) { - //limit flooding - if(nodes[i]->xmax<dXMin || dXMax<nodes[i]->xmin || - nodes[i]->ymax<dYMin || dYMax<nodes[i]->ymin - ) { - continue; - } - //recurse into subtiles if there is at least one subtile corner within input rectangle - quadtree_find_rect_items(nodes[i],dXMin,dXMax,dYMin,dYMax,out); - } - } } } @@ -114,46 +111,46 @@ quadtree_find_rect_items(struct quadtree_node* this_, double dXMin, double dXMax * searches all four subnodes recursively for the closest item */ struct quadtree_item* -quadtree_find_nearest_flood(struct quadtree_node* this_, struct quadtree_item* item, double current_max, struct quadtree_node* toSkip) { +quadtree_find_nearest_flood(struct quadtree_node* this_, struct quadtree_item* item, double current_max, + struct quadtree_node* toSkip) { struct quadtree_node* nodes[4] = { this_->aa, this_->ab, this_->ba, this_->bb }; struct quadtree_item*res = NULL; - if( this_->is_leaf ) { + if( this_->is_leaf ) { int i; double distance_sq = current_max; - for(i=0;i<this_->node_num;++i) { + for(i=0; i<this_->node_num; ++i) { double curr_dist_sq = dist_sq(item->longitude,item->latitude,this_->items[i]->longitude,this_->items[i]->latitude); if(curr_dist_sq<distance_sq) { distance_sq = curr_dist_sq; res = this_->items[i]; } } - } - else { - int i; - for( i=0;i<4;++i) { - if(nodes[i] && nodes[i]!=toSkip) { - //limit flooding - struct quadtree_item*res_tmp = NULL; - if( - dist_sq(nodes[i]->xmin,nodes[i]->ymin,item->longitude,item->latitude)<current_max || - dist_sq(nodes[i]->xmax,nodes[i]->ymin,item->longitude,item->latitude)<current_max || - dist_sq(nodes[i]->xmax,nodes[i]->ymax,item->longitude,item->latitude)<current_max || - dist_sq(nodes[i]->xmin,nodes[i]->ymax,item->longitude,item->latitude)<current_max - ) { - res_tmp = quadtree_find_nearest_flood(nodes[i],item,current_max,NULL); - } - if(res_tmp) { - double curr_dist_sq; - res = res_tmp; - curr_dist_sq = dist_sq(item->longitude,item->latitude,res->longitude,res->latitude); - if(curr_dist_sq<current_max) { - current_max = curr_dist_sq; + } else { + int i; + for( i=0; i<4; ++i) { + if(nodes[i] && nodes[i]!=toSkip) { + //limit flooding + struct quadtree_item*res_tmp = NULL; + if( + dist_sq(nodes[i]->xmin,nodes[i]->ymin,item->longitude,item->latitude)<current_max || + dist_sq(nodes[i]->xmax,nodes[i]->ymin,item->longitude,item->latitude)<current_max || + dist_sq(nodes[i]->xmax,nodes[i]->ymax,item->longitude,item->latitude)<current_max || + dist_sq(nodes[i]->xmin,nodes[i]->ymax,item->longitude,item->latitude)<current_max + ) { + res_tmp = quadtree_find_nearest_flood(nodes[i],item,current_max,NULL); + } + if(res_tmp) { + double curr_dist_sq; + res = res_tmp; + curr_dist_sq = dist_sq(item->longitude,item->latitude,res->longitude,res->latitude); + if(curr_dist_sq<current_max) { + current_max = curr_dist_sq; + } + } } - } - } - } + } } - return res; + return res; } /* @@ -163,129 +160,117 @@ struct quadtree_item* quadtree_find_item(struct quadtree_node* this_, struct quadtree_item* item) { struct quadtree_item*res = NULL; if( ! this_ ) { - return NULL; + return NULL; } - if( this_->is_leaf ) { + if( this_->is_leaf ) { int i; - for(i=0;i<this_->node_num;++i) { + for(i=0; i<this_->node_num; ++i) { //TODO equality check may not be correct on float values! maybe it can be replaced by range check with some tolerance if(item->longitude==this_->items[i]->longitude && item->latitude==this_->items[i]->latitude) { res = this_->items[i]; return res; } } - return NULL; - } - else { + return NULL; + } else { if( - this_->aa && - this_->aa->xmin<=item->longitude && item->longitude<this_->aa->xmax && - this_->aa->ymin<=item->latitude && item->latitude<this_->aa->ymax - ) { - res = quadtree_find_item(this_->aa,item); - } - else if( - this_->ab && - this_->ab->xmin<=item->longitude && item->longitude<this_->ab->xmax && - this_->ab->ymin<=item->latitude && item->latitude<this_->ab->ymax - ) { - res = quadtree_find_item(this_->ab,item); - } - else if( - this_->ba && - this_->ba->xmin<=item->longitude && item->longitude<this_->ba->xmax && - this_->ba->ymin<=item->latitude && item->latitude<this_->ba->ymax - ) { - res = quadtree_find_item(this_->ba,item); - } - else if( - this_->bb && - this_->bb->xmin<=item->longitude && item->longitude<this_->bb->xmax && - this_->bb->ymin<=item->latitude && item->latitude<this_->bb->ymax - ) { - res = quadtree_find_item(this_->bb,item); - } - else { - return NULL; + this_->aa && + this_->aa->xmin<=item->longitude && item->longitude<this_->aa->xmax && + this_->aa->ymin<=item->latitude && item->latitude<this_->aa->ymax + ) { + res = quadtree_find_item(this_->aa,item); + } else if( + this_->ab && + this_->ab->xmin<=item->longitude && item->longitude<this_->ab->xmax && + this_->ab->ymin<=item->latitude && item->latitude<this_->ab->ymax + ) { + res = quadtree_find_item(this_->ab,item); + } else if( + this_->ba && + this_->ba->xmin<=item->longitude && item->longitude<this_->ba->xmax && + this_->ba->ymin<=item->latitude && item->latitude<this_->ba->ymax + ) { + res = quadtree_find_item(this_->ba,item); + } else if( + this_->bb && + this_->bb->xmin<=item->longitude && item->longitude<this_->bb->xmax && + this_->bb->ymin<=item->latitude && item->latitude<this_->bb->ymax + ) { + res = quadtree_find_item(this_->bb,item); + } else { + return NULL; } } - return res; + return res; } /* * returns the containing node for an item */ -struct quadtree_node* -quadtree_find_containing_node(struct quadtree_node* root, struct quadtree_item* item) -{ +struct quadtree_node* +quadtree_find_containing_node(struct quadtree_node* root, struct quadtree_item* item) { struct quadtree_node*res = NULL; if( ! root ) { - return NULL; + return NULL; } - if( root->is_leaf ) { + if( root->is_leaf ) { int i; - for(i=0;i<root->node_num;++i) { + for(i=0; i<root->node_num; ++i) { if(item == root->items[i]) { res = root; } } - } - else { + } else { if( - root->aa && - root->aa->xmin<=item->longitude && item->longitude<root->aa->xmax && - root->aa->ymin<=item->latitude && item->latitude<root->aa->ymax - ) { - res = quadtree_find_containing_node(root->aa,item); - } - else if( - root->ab && - root->ab->xmin<=item->longitude && item->longitude<root->ab->xmax && - root->ab->ymin<=item->latitude && item->latitude<root->ab->ymax - ) { - res = quadtree_find_containing_node(root->ab,item); - } - else if( - root->ba && - root->ba->xmin<=item->longitude && item->longitude<root->ba->xmax && - root->ba->ymin<=item->latitude && item->latitude<root->ba->ymax - ) { - res = quadtree_find_containing_node(root->ba,item); - } - else if( - root->bb && - root->bb->xmin<=item->longitude && item->longitude<root->bb->xmax && - root->bb->ymin<=item->latitude && item->latitude<root->bb->ymax - ) { - res = quadtree_find_containing_node(root->bb,item); - } - else { - //this should not happen + root->aa && + root->aa->xmin<=item->longitude && item->longitude<root->aa->xmax && + root->aa->ymin<=item->latitude && item->latitude<root->aa->ymax + ) { + res = quadtree_find_containing_node(root->aa,item); + } else if( + root->ab && + root->ab->xmin<=item->longitude && item->longitude<root->ab->xmax && + root->ab->ymin<=item->latitude && item->latitude<root->ab->ymax + ) { + res = quadtree_find_containing_node(root->ab,item); + } else if( + root->ba && + root->ba->xmin<=item->longitude && item->longitude<root->ba->xmax && + root->ba->ymin<=item->latitude && item->latitude<root->ba->ymax + ) { + res = quadtree_find_containing_node(root->ba,item); + } else if( + root->bb && + root->bb->xmin<=item->longitude && item->longitude<root->bb->xmax && + root->bb->ymin<=item->latitude && item->latitude<root->bb->ymax + ) { + res = quadtree_find_containing_node(root->bb,item); + } else { + //this should not happen } } - return res; + return res; } -int quadtree_delete_item(struct quadtree_node* root, struct quadtree_item* item) -{ +int quadtree_delete_item(struct quadtree_node* root, struct quadtree_item* item) { - struct quadtree_node* qn = quadtree_find_containing_node(root,item); - int i, bFound=0; + struct quadtree_node* qn = quadtree_find_containing_node(root,item); + int i, bFound=0; - if(!qn || !qn->node_num) { - return 0; - } + if(!qn || !qn->node_num) { + return 0; + } - for(i=0;i<qn->node_num;++i) { - if( qn->items[i] == item) { - qn->items[i]->deleted=1; - bFound=1; + for(i=0; i<qn->node_num; ++i) { + if( qn->items[i] == item) { + qn->items[i]->deleted=1; + bFound=1; + } } - } - return bFound; + return bFound; } @@ -297,247 +282,240 @@ quadtree_find_nearest(struct quadtree_node* this_, struct quadtree_item* item) { struct quadtree_item*res = NULL; double distance_sq = MAX_DOUBLE; if( ! this_ ) { - return NULL; + return NULL; } - if( this_->is_leaf ) { + if( this_->is_leaf ) { int i; - for(i=0;i<this_->node_num;++i) { + for(i=0; i<this_->node_num; ++i) { double curr_dist_sq = dist_sq(item->longitude,item->latitude,this_->items[i]->longitude,this_->items[i]->latitude); if(curr_dist_sq<distance_sq) { distance_sq = curr_dist_sq; res = this_->items[i]; } } - //go up n levels - if(!res && this_->parent) { - struct quadtree_item*res2 = NULL; - struct quadtree_node* anchestor = this_->parent; - int cnt = 0; - while (anchestor->parent && cnt<4) { - anchestor = anchestor->parent; - ++cnt; - } - res2 = quadtree_find_nearest_flood(anchestor,item,distance_sq,NULL); - if(res2) { - res = res2; - } - } - } else { - if( - this_->aa && - this_->aa->xmin<=item->longitude && item->longitude<this_->aa->xmax && - this_->aa->ymin<=item->latitude && item->latitude<this_->aa->ymax - ) { - res = quadtree_find_nearest(this_->aa,item); - } - else if( - this_->ab && - this_->ab->xmin<=item->longitude && item->longitude<this_->ab->xmax && - this_->ab->ymin<=item->latitude && item->latitude<this_->ab->ymax - ) { - res = quadtree_find_nearest(this_->ab,item); - } - else if( - this_->ba && - this_->ba->xmin<=item->longitude && item->longitude<this_->ba->xmax && - this_->ba->ymin<=item->latitude && item->latitude<this_->ba->ymax - ) { - res = quadtree_find_nearest(this_->ba,item); - } - else if( - this_->bb && - this_->bb->xmin<=item->longitude && item->longitude<this_->bb->xmax && - this_->bb->ymin<=item->latitude && item->latitude<this_->bb->ymax - ) { - res = quadtree_find_nearest(this_->bb,item); - } - else { - if(this_->parent) { - //go up two levels - struct quadtree_node* anchestor = this_->parent; + //go up n levels + if(!res && this_->parent) { + struct quadtree_item*res2 = NULL; + struct quadtree_node* anchestor = this_->parent; int cnt = 0; while (anchestor->parent && cnt<4) { - anchestor = anchestor->parent; - ++cnt; - } - res = quadtree_find_nearest_flood(anchestor,item,distance_sq,NULL); + anchestor = anchestor->parent; + ++cnt; + } + res2 = quadtree_find_nearest_flood(anchestor,item,distance_sq,NULL); + if(res2) { + res = res2; + } + } + } else { + if( + this_->aa && + this_->aa->xmin<=item->longitude && item->longitude<this_->aa->xmax && + this_->aa->ymin<=item->latitude && item->latitude<this_->aa->ymax + ) { + res = quadtree_find_nearest(this_->aa,item); + } else if( + this_->ab && + this_->ab->xmin<=item->longitude && item->longitude<this_->ab->xmax && + this_->ab->ymin<=item->latitude && item->latitude<this_->ab->ymax + ) { + res = quadtree_find_nearest(this_->ab,item); + } else if( + this_->ba && + this_->ba->xmin<=item->longitude && item->longitude<this_->ba->xmax && + this_->ba->ymin<=item->latitude && item->latitude<this_->ba->ymax + ) { + res = quadtree_find_nearest(this_->ba,item); + } else if( + this_->bb && + this_->bb->xmin<=item->longitude && item->longitude<this_->bb->xmax && + this_->bb->ymin<=item->latitude && item->latitude<this_->bb->ymax + ) { + res = quadtree_find_nearest(this_->bb,item); + } else { + if(this_->parent) { + //go up two levels + struct quadtree_node* anchestor = this_->parent; + int cnt = 0; + while (anchestor->parent && cnt<4) { + anchestor = anchestor->parent; + ++cnt; + } + res = quadtree_find_nearest_flood(anchestor,item,distance_sq,NULL); + } } - } } - return res; + return res; } /** - * @brief Free space occupied by deleted unreferenced items. + * @brief Free space occupied by deleted unreferenced items. * @param node pointer to the quadtree node * @param iter Quadtree iteration context. * @return nothing */ -void quadtree_node_drop_garbage(struct quadtree_node* node, struct quadtree_iter *iter) -{ - int i,j; - int node_num=node->node_num; - dbg(lvl_debug,"Processing unreferenced subnode children...\n"); - for(i=0,j=0;i<node_num;i++) { - if(node->items[i]->deleted && !node->items[i]->ref_count) { - if(iter->item_free) { - (iter->item_free)(iter->item_free_context, node->items[i]); - } else { - g_free(node->items[i]); - } - node->node_num--; - node->items[i]=NULL; - } else { - node->items[j++]=node->items[i]; - } - if(i>j) - node->items[i]=NULL; - } +void quadtree_node_drop_garbage(struct quadtree_node* node, struct quadtree_iter *iter) { + int i,j; + int node_num=node->node_num; + dbg(lvl_debug,"Processing unreferenced subnode children..."); + for(i=0,j=0; i<node_num; i++) { + if(node->items[i]->deleted && !node->items[i]->ref_count) { + if(iter->item_free) { + (iter->item_free)(iter->item_free_context, node->items[i]); + } else { + g_free(node->items[i]); + } + node->node_num--; + node->items[i]=NULL; + } else { + node->items[j++]=node->items[i]; + } + if(i>j) + node->items[i]=NULL; + } } /** - * @brief Add new node to quadtree. + * @brief Add new node to quadtree. * @param this_ pointer to the quadtree (root) node * @param item item to add * @param iter Quadtree iteration context. Can be NULL if no garbage collection is needed. * @return nothing */ -void -quadtree_add(struct quadtree_node* this_, struct quadtree_item* item, struct quadtree_iter *iter) { +void quadtree_add(struct quadtree_node* this_, struct quadtree_item* item, struct quadtree_iter *iter) { if( this_->is_leaf ) { int bSame = 1; int i; - + if(iter) - quadtree_node_drop_garbage(this_, iter); - + quadtree_node_drop_garbage(this_, iter); + if(QUADTREE_NODE_CAPACITY-1 == this_->node_num) { - double lon, lat; - //avoid infinite recursion when all elements have the same coordinate - lon = this_->items[0]->longitude; - lat = this_->items[0]->latitude; - for(i=1;i<this_->node_num;++i) { - if (lon != this_->items[i]->longitude || lat != this_->items[i]->latitude) { - bSame = 0; - break; + double lon, lat; + //avoid infinite recursion when all elements have the same coordinate + lon = this_->items[0]->longitude; + lat = this_->items[0]->latitude; + for(i=1; i<this_->node_num; ++i) { + if (lon != this_->items[i]->longitude || lat != this_->items[i]->latitude) { + bSame = 0; + break; + } + } + if (bSame) { + //FIXME: memleak and items thrown away if more than QUADTREE_NODE_CAPACITY-1 items with same coordinates added. + dbg(lvl_error,"Unable to add another item with same coordinates. Throwing item to the ground. Will leak %p.",item); + return; } - } - if (bSame) { - //FIXME: memleak and items thrown away if more than QUADTREE_NODE_CAPACITY-1 items with same coordinates added. - dbg(lvl_error,"Unable to add another item with same coordinates. Throwing item to the ground. Will leak %p.\n",item); - return; - } - this_->items[this_->node_num++] = item; - quadtree_split(this_); + this_->items[this_->node_num++] = item; + quadtree_split(this_); } else { - this_->items[this_->node_num++] = item; + this_->items[this_->node_num++] = item; } - } - else { + } else { if( - this_->xmin<=item->longitude && item->longitude<this_->xmin+(this_->xmax-this_->xmin)/2.0 && - this_->ymin<=item->latitude && item->latitude<this_->ymin+(this_->ymax-this_->ymin)/2.0 - ) { - if(!this_->aa) { - 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 ); - } - quadtree_add(this_->aa,item,iter); - } - else if( - this_->xmin+(this_->xmax-this_->xmin)/2.0<=item->longitude && item->longitude<this_->xmax && - this_->ymin<=item->latitude && item->latitude<this_->ymin+(this_->ymax-this_->ymin)/2.0 - ) { - if(!this_->ab) { - 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 ); - } - quadtree_add(this_->ab,item,iter); - } - else if( - this_->xmin<=item->longitude && item->longitude<this_->xmin+(this_->xmax-this_->xmin)/2.0 && - this_->ymin+(this_->ymax-this_->ymin)/2.0<=item->latitude && item->latitude<this_->ymax - ) { - if(!this_->ba) { - 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); - } - quadtree_add(this_->ba,item,iter); - } - else if( - this_->xmin+(this_->xmax-this_->xmin)/2.0<=item->longitude && item->longitude<this_->xmax && - this_->ymin+(this_->ymax-this_->ymin)/2.0<=item->latitude && item->latitude<this_->ymax - ) { - if(!this_->bb) { - 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); - } - quadtree_add(this_->bb,item,iter); + this_->xmin<=item->longitude && item->longitude<this_->xmin+(this_->xmax-this_->xmin)/2.0 && + this_->ymin<=item->latitude && item->latitude<this_->ymin+(this_->ymax-this_->ymin)/2.0 + ) { + if(!this_->aa) { + 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 ); + } + quadtree_add(this_->aa,item,iter); + } else if( + this_->xmin+(this_->xmax-this_->xmin)/2.0<=item->longitude && item->longitude<this_->xmax && + this_->ymin<=item->latitude && item->latitude<this_->ymin+(this_->ymax-this_->ymin)/2.0 + ) { + if(!this_->ab) { + 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 ); + } + quadtree_add(this_->ab,item,iter); + } else if( + this_->xmin<=item->longitude && item->longitude<this_->xmin+(this_->xmax-this_->xmin)/2.0 && + this_->ymin+(this_->ymax-this_->ymin)/2.0<=item->latitude && item->latitude<this_->ymax + ) { + if(!this_->ba) { + 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); + } + quadtree_add(this_->ba,item,iter); + } else if( + this_->xmin+(this_->xmax-this_->xmin)/2.0<=item->longitude && item->longitude<this_->xmax && + this_->ymin+(this_->ymax-this_->ymin)/2.0<=item->latitude && item->latitude<this_->ymax + ) { + if(!this_->bb) { + 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); + } + quadtree_add(this_->bb,item,iter); } } } -void -quadtree_split(struct quadtree_node* this_) { +void quadtree_split(struct quadtree_node* this_) { int i; this_->is_leaf = 0; - for(i=0;i<this_->node_num;++i) { + for(i=0; i<this_->node_num; ++i) { if( - this_->xmin<=this_->items[i]->longitude && this_->items[i]->longitude<this_->xmin+(this_->xmax-this_->xmin)/2.0 && - this_->ymin<=this_->items[i]->latitude && this_->items[i]->latitude<this_->ymin+(this_->ymax-this_->ymin)/2.0 - ) { - if(!this_->aa) { - 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 ); - } - quadtree_add(this_->aa,this_->items[i],NULL); - } - else if( - this_->xmin+(this_->xmax-this_->xmin)/2.0<=this_->items[i]->longitude && this_->items[i]->longitude<this_->xmax && - this_->ymin<=this_->items[i]->latitude && this_->items[i]->latitude<this_->ymin+(this_->ymax-this_->ymin)/2.0 - ) { - if(!this_->ab) { - 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 ); - } - quadtree_add(this_->ab,this_->items[i],NULL); - } - else if( - this_->xmin<=this_->items[i]->longitude && this_->items[i]->longitude<this_->xmin+(this_->xmax-this_->xmin)/2.0 && - this_->ymin+(this_->ymax-this_->ymin)/2.0<=this_->items[i]->latitude && this_->items[i]->latitude<this_->ymax - ) { - if(!this_->ba) { - 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); - } - quadtree_add(this_->ba,this_->items[i],NULL); - } - else if( - this_->xmin+(this_->xmax-this_->xmin)/2.0<=this_->items[i]->longitude && this_->items[i]->longitude<this_->xmax && - this_->ymin+(this_->ymax-this_->ymin)/2.0<=this_->items[i]->latitude && this_->items[i]->latitude<this_->ymax - ) { - if(!this_->bb) { - 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); - } - quadtree_add(this_->bb,this_->items[i],NULL); + this_->xmin<=this_->items[i]->longitude && this_->items[i]->longitude<this_->xmin+(this_->xmax-this_->xmin)/2.0 && + this_->ymin<=this_->items[i]->latitude && this_->items[i]->latitude<this_->ymin+(this_->ymax-this_->ymin)/2.0 + ) { + if(!this_->aa) { + 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 ); + } + quadtree_add(this_->aa,this_->items[i],NULL); + } else if( + this_->xmin+(this_->xmax-this_->xmin)/2.0<=this_->items[i]->longitude && this_->items[i]->longitude<this_->xmax && + this_->ymin<=this_->items[i]->latitude && this_->items[i]->latitude<this_->ymin+(this_->ymax-this_->ymin)/2.0 + ) { + if(!this_->ab) { + 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 ); + } + quadtree_add(this_->ab,this_->items[i],NULL); + } else if( + this_->xmin<=this_->items[i]->longitude && this_->items[i]->longitude<this_->xmin+(this_->xmax-this_->xmin)/2.0 && + this_->ymin+(this_->ymax-this_->ymin)/2.0<=this_->items[i]->latitude && this_->items[i]->latitude<this_->ymax + ) { + if(!this_->ba) { + 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); + } + quadtree_add(this_->ba,this_->items[i],NULL); + } else if( + this_->xmin+(this_->xmax-this_->xmin)/2.0<=this_->items[i]->longitude && this_->items[i]->longitude<this_->xmax && + this_->ymin+(this_->ymax-this_->ymin)/2.0<=this_->items[i]->latitude && this_->items[i]->latitude<this_->ymax + ) { + if(!this_->bb) { + 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); + } + quadtree_add(this_->bb,this_->items[i],NULL); } this_->items[i]=NULL; } this_->node_num = 0; } -void -quadtree_destroy(struct quadtree_node* this_) { +void quadtree_destroy(struct quadtree_node* this_) { if(this_->aa) { quadtree_destroy(this_->aa); - this_->aa = NULL; + this_->aa = NULL; } if(this_->ab) { quadtree_destroy(this_->ab); - this_->ab = NULL; + this_->ab = NULL; } if(this_->ba) { quadtree_destroy(this_->ba); - this_->ba = NULL; + this_->ba = NULL; } if(this_->bb) { quadtree_destroy(this_->bb); - this_->bb = NULL; + this_->bb = NULL; } free(this_); } @@ -553,32 +531,32 @@ quadtree_destroy(struct quadtree_node* this_) { * @param item_free_context data to be passed as a first parameter to item_free function * @return pointer to the quad tree iteration structure. */ -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) -{ - struct quadtree_iter *ret=g_new0(struct quadtree_iter,1); - struct quadtree_iter_node *n=g_new0(struct quadtree_iter_node,1); - ret->xmin=dXMin; - ret->xmax=dXMax; - ret->ymin=dYMin; - ret->ymax=dYMax; - dbg(lvl_debug,"%f %f %f %f\n",dXMin,dXMax,dYMin,dYMax) - ret->item_free=item_free; - ret->item_free_context=item_free_context; - n->node=this_; - ret->iter_nodes=g_list_prepend(ret->iter_nodes,n); - n->is_leaf=this_->is_leaf; - if(this_->is_leaf) { - int i; - n->node_num=this_->node_num; - for(i=0;i<n->node_num;i++) { - n->items[i]=this_->items[i]; - n->items[i]->ref_count++; - } - } - - this_->ref_count++; - dbg(lvl_debug,"Query %p \n",this_) - return ret; +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) { + struct quadtree_iter *ret=g_new0(struct quadtree_iter,1); + struct quadtree_iter_node *n=g_new0(struct quadtree_iter_node,1); + ret->xmin=dXMin; + ret->xmax=dXMax; + ret->ymin=dYMin; + ret->ymax=dYMax; + dbg(lvl_debug,"%f %f %f %f",dXMin,dXMax,dYMin,dYMax) + ret->item_free=item_free; + ret->item_free_context=item_free_context; + n->node=this_; + ret->iter_nodes=g_list_prepend(ret->iter_nodes,n); + n->is_leaf=this_->is_leaf; + if(this_->is_leaf) { + int i; + n->node_num=this_->node_num; + for(i=0; i<n->node_num; i++) { + n->items[i]=this_->items[i]; + n->items[i]->ref_count++; + } + } + + this_->ref_count++; + dbg(lvl_debug,"Query %p ",this_) + return ret; } @@ -586,12 +564,11 @@ struct quadtree_iter *quadtree_query(struct quadtree_node *this_, double dXMin, * @brief End iteration. * @param iter Pointer to the quadtree iteration structure. */ -void quadtree_query_free(struct quadtree_iter *iter) -{ - //Get the rest of the data to collect garbage and dereference all the items/nodes - //TODO: No need to iterate the whole tree here. Just dereference nodes/items (if any). - while(quadtree_item_next(iter)); - g_free(iter); +void quadtree_query_free(struct quadtree_iter *iter) { + //Get the rest of the data to collect garbage and dereference all the items/nodes + //TODO: No need to iterate the whole tree here. Just dereference nodes/items (if any). + while(quadtree_item_next(iter)); + g_free(iter); } @@ -599,10 +576,9 @@ void quadtree_query_free(struct quadtree_iter *iter) * @brief Mark current item of iterator for deletion. * @param iter Pointer to the quadtree iteration structure. */ -void quadtree_item_delete(struct quadtree_iter *iter) -{ - if(iter->item) - iter->item->deleted=1; +void quadtree_item_delete(struct quadtree_iter *iter) { + if(iter->item) + iter->item->deleted=1; } /* @@ -610,118 +586,119 @@ void quadtree_item_delete(struct quadtree_iter *iter) * @param iter Pointer to the quadtree iteration structure. * @return pointer to the next item, or NULL if no items are left. */ -struct quadtree_item * quadtree_item_next(struct quadtree_iter *iter) -{ - struct quadtree_iter_node *iter_node; - struct quadtree_node *subnode; - - if(iter->item) { - iter->item->ref_count--; - iter->item=NULL; - } - - while(iter->iter_nodes) { - struct quadtree_node *nodes[4]; - int i; - iter_node=iter->iter_nodes->data; - - if(iter_node->is_leaf) { - /* Try to find undeleted item in the current node */ - dbg(lvl_debug,"find item %p %p ...\n",iter->iter_nodes,iter->iter_nodes->data); - while(iter_node->item<iter_node->node_num) { - dbg(lvl_debug,"%d %d\n",iter_node->item,iter_node->items[iter_node->item]->deleted); - if(iter_node->items[iter_node->item]->deleted) { - iter_node->item++; - continue; - } - iter->item=iter_node->items[iter_node->item]; - iter_node->item++; - dbg(lvl_debug,"Returning %p\n",iter->item); - iter->item->ref_count++; - return iter->item; - } - for(i=0;i<iter_node->node_num;i++) { - iter_node->items[i]->ref_count--; - } - } else { - /* No items left, try to find non-empty subnode */ - nodes[0]=iter_node->node->aa; - nodes[1]=iter_node->node->ab; - nodes[2]=iter_node->node->ba; - nodes[3]=iter_node->node->bb; - - for(subnode=NULL;!subnode && iter_node->subnode<4;iter_node->subnode++) { - i=iter_node->subnode; - 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)) - continue; - dbg(lvl_debug,"%f %f %f %f\n",nodes[i]->xmin, nodes[i]->xmax, nodes[i]->ymin, nodes[i]->ymax) - subnode=nodes[i]; - } - - if(subnode) { - /* Go one level deeper */ - dbg(lvl_debug,"Go one level deeper...\n"); - iter_node=g_new0(struct quadtree_iter_node, 1); - iter_node->node=subnode; - iter_node->is_leaf=subnode->is_leaf; - if(iter_node->is_leaf) { - int i; - iter_node->node_num=subnode->node_num; - for(i=0;i<iter_node->node_num;i++) { - iter_node->items[i]=subnode->items[i]; - iter_node->items[i]->ref_count++; - } - } - subnode->ref_count++; - iter->iter_nodes=g_list_prepend(iter->iter_nodes,iter_node); - continue; - } - } - - /* No nodes and items left, fix up current subnode... */ - iter_node=iter->iter_nodes->data; - subnode=iter_node->node; - subnode->ref_count--; - - if(!subnode->aa && !subnode->ab && !subnode->ba && !subnode->bb) - subnode->is_leaf=1; - - /* 1. free deleted unreferenced items */ - quadtree_node_drop_garbage(subnode, iter); - - /* 2. remove empty leaf subnode if it's unreferenced */ - - if(!subnode->ref_count && !subnode->node_num && subnode->is_leaf ) { - dbg(lvl_debug,"Going to delete an empty unreferenced leaf subnode...\n"); - - if(subnode->parent) { - if(subnode->parent->aa==subnode) { - subnode->parent->aa=NULL; - } else if(subnode->parent->ab==subnode) { - subnode->parent->ab=NULL; - } else if(subnode->parent->ba==subnode) { - subnode->parent->ba=NULL; - } else if(subnode->parent->bb==subnode) { - subnode->parent->bb=NULL; - } else { - dbg(lvl_error,"Found Quadtree structure corruption while trying to free an empty node.\n"); - } - - if(!subnode->parent->aa && !subnode->parent->ab && !subnode->parent->ba && !subnode->parent->bb ) - subnode->parent->is_leaf=1; - g_free(subnode); - } else - dbg(lvl_debug,"Quadtree is empty. NOT deleting the root subnode...\n"); - - } - - /* Go one step towards root */ - dbg(lvl_info,"Going towards root...\n"); - g_free(iter->iter_nodes->data); - iter->iter_nodes=g_list_delete_link(iter->iter_nodes,iter->iter_nodes); - } - - iter->item=NULL; - return NULL; +struct quadtree_item * quadtree_item_next(struct quadtree_iter *iter) { + struct quadtree_iter_node *iter_node; + struct quadtree_node *subnode; + + if(iter->item) { + iter->item->ref_count--; + iter->item=NULL; + } + + while(iter->iter_nodes) { + struct quadtree_node *nodes[4]; + int i; + iter_node=iter->iter_nodes->data; + + if(iter_node->is_leaf) { + /* Try to find undeleted item in the current node */ + dbg(lvl_debug,"find item %p %p ...",iter->iter_nodes,iter->iter_nodes->data); + while(iter_node->item<iter_node->node_num) { + dbg(lvl_debug,"%d %d",iter_node->item,iter_node->items[iter_node->item]->deleted); + if(iter_node->items[iter_node->item]->deleted) { + iter_node->item++; + continue; + } + iter->item=iter_node->items[iter_node->item]; + iter_node->item++; + dbg(lvl_debug,"Returning %p",iter->item); + iter->item->ref_count++; + return iter->item; + } + for(i=0; i<iter_node->node_num; i++) { + iter_node->items[i]->ref_count--; + } + } else { + /* No items left, try to find non-empty subnode */ + nodes[0]=iter_node->node->aa; + nodes[1]=iter_node->node->ab; + nodes[2]=iter_node->node->ba; + nodes[3]=iter_node->node->bb; + + for(subnode=NULL; !subnode && iter_node->subnode<4; iter_node->subnode++) { + i=iter_node->subnode; + 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)) + continue; + dbg(lvl_debug,"%f %f %f %f",nodes[i]->xmin, nodes[i]->xmax, nodes[i]->ymin, nodes[i]->ymax) + subnode=nodes[i]; + } + + if(subnode) { + /* Go one level deeper */ + dbg(lvl_debug,"Go one level deeper..."); + iter_node=g_new0(struct quadtree_iter_node, 1); + iter_node->node=subnode; + iter_node->is_leaf=subnode->is_leaf; + if(iter_node->is_leaf) { + int i; + iter_node->node_num=subnode->node_num; + for(i=0; i<iter_node->node_num; i++) { + iter_node->items[i]=subnode->items[i]; + iter_node->items[i]->ref_count++; + } + } + subnode->ref_count++; + iter->iter_nodes=g_list_prepend(iter->iter_nodes,iter_node); + continue; + } + } + + /* No nodes and items left, fix up current subnode... */ + iter_node=iter->iter_nodes->data; + subnode=iter_node->node; + subnode->ref_count--; + + if(!subnode->aa && !subnode->ab && !subnode->ba && !subnode->bb) + subnode->is_leaf=1; + + /* 1. free deleted unreferenced items */ + quadtree_node_drop_garbage(subnode, iter); + + /* 2. remove empty leaf subnode if it's unreferenced */ + + if(!subnode->ref_count && !subnode->node_num && subnode->is_leaf ) { + dbg(lvl_debug,"Going to delete an empty unreferenced leaf subnode..."); + + if(subnode->parent) { + if(subnode->parent->aa==subnode) { + subnode->parent->aa=NULL; + } else if(subnode->parent->ab==subnode) { + subnode->parent->ab=NULL; + } else if(subnode->parent->ba==subnode) { + subnode->parent->ba=NULL; + } else if(subnode->parent->bb==subnode) { + subnode->parent->bb=NULL; + } else { + dbg(lvl_error,"Found Quadtree structure corruption while trying to free an empty node."); + } + + if(!subnode->parent->aa && !subnode->parent->ab && !subnode->parent->ba && !subnode->parent->bb ) + subnode->parent->is_leaf=1; + g_free(subnode); + } else + dbg(lvl_debug,"Quadtree is empty. NOT deleting the root subnode..."); + + } + + /* Go one step towards root */ + dbg(lvl_info,"Going towards root..."); + g_free(iter->iter_nodes->data); + iter->iter_nodes=g_list_delete_link(iter->iter_nodes,iter->iter_nodes); + } + + iter->item=NULL; + return NULL; } |