/[zanavi_public1]/navit/navit/map/csv/quadtree.c
ZANavi

Contents of /navit/navit/map/csv/quadtree.c

Parent Directory Parent Directory | Revision Log Revision Log


Revision 2 - (show annotations) (download)
Fri Oct 28 21:19:04 2011 UTC (12 years, 5 months ago) by zoff99
File MIME type: text/plain
File size: 15549 byte(s)
import files
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 "quadtree.h"
24
25 #define QUADTREE_NODE_CAPACITY 10
26
27 #define MAX_DOUBLE 9999999
28
29 static double
30 dist_sq(double x1,double y1,double x2,double y2)
31 {
32 return (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1);
33 }
34
35 struct quadtree_node*
36 quadtree_node_new(struct quadtree_node* parent, double xmin, double xmax, double ymin, double ymax ) {
37 struct quadtree_node*ret = g_new0(struct quadtree_node,1);
38 ret->xmin = xmin;
39 ret->xmax = xmax;
40 ret->ymin = ymin;
41 ret->ymax = ymax;
42 ret->is_leaf = 1;
43 ret->parent = parent;
44 return ret;
45 }
46
47 /*
48 * searches all four subnodes recursively for the list of items within a rectangle
49 */
50 void
51 quadtree_find_rect_items(struct quadtree_node* this_, double dXMin, double dXMax, double dYMin, double dYMax, GList**out) {
52
53 struct quadtree_node* nodes[4] = { this_->aa, this_->ab, this_->ba, this_->bb };
54 if( this_->is_leaf ) {
55 int i;
56 //double distance_sq = current_max;
57 for(i=0;i<this_->node_num;++i) { //select only items within input rectangle
58 if(dXMin<=this_->items[i].longitude && this_->items[i].longitude<=dXMax &&
59 dYMin<=this_->items[i].latitude && this_->items[i].latitude<=dYMax
60 ) {
61 *out=g_list_prepend(*out,&this_->items[i]);
62 }
63 }
64 }
65 else {
66 int i;
67 for( i=0;i<4;++i) {
68 if(nodes[i] ) {
69 //limit flooding
70 if(nodes[i]->xmax<dXMin || dXMax<nodes[i]->xmin ||
71 nodes[i]->ymax<dYMin || dYMax<nodes[i]->ymin
72 ) {
73 continue;
74 }
75 //recurse into subtiles if there is at least one subtile corner within input rectangle
76 quadtree_find_rect_items(nodes[i],dXMin,dXMax,dYMin,dYMax,out);
77 }
78 }
79 }
80 }
81
82 /*
83 * searches all four subnodes recursively for the closest item
84 */
85 struct quadtree_item*
86 quadtree_find_nearest_flood(struct quadtree_node* this_, struct quadtree_item* item, double current_max, struct quadtree_node* toSkip) {
87 struct quadtree_node* nodes[4] = { this_->aa, this_->ab, this_->ba, this_->bb };
88 struct quadtree_item*res = NULL;
89 if( this_->is_leaf ) {
90 int i;
91 double distance_sq = current_max;
92 for(i=0;i<this_->node_num;++i) {
93 double curr_dist_sq = dist_sq(item->longitude,item->latitude,this_->items[i].longitude,this_->items[i].latitude);
94 if(curr_dist_sq<distance_sq) {
95 distance_sq = curr_dist_sq;
96 res = &this_->items[i];
97 }
98 }
99 }
100 else {
101 int i;
102 for( i=0;i<4;++i) {
103 if(nodes[i] && nodes[i]!=toSkip) {
104 //limit flooding
105 struct quadtree_item*res_tmp = NULL;
106 if(
107 dist_sq(nodes[i]->xmin,nodes[i]->ymin,item->longitude,item->latitude)<current_max ||
108 dist_sq(nodes[i]->xmax,nodes[i]->ymin,item->longitude,item->latitude)<current_max ||
109 dist_sq(nodes[i]->xmax,nodes[i]->ymax,item->longitude,item->latitude)<current_max ||
110 dist_sq(nodes[i]->xmin,nodes[i]->ymax,item->longitude,item->latitude)<current_max
111 ) {
112 res_tmp = quadtree_find_nearest_flood(nodes[i],item,current_max,NULL);
113 }
114 if(res_tmp) {
115 double curr_dist_sq;
116 res = res_tmp;
117 curr_dist_sq = dist_sq(item->longitude,item->latitude,res->longitude,res->latitude);
118 if(curr_dist_sq<current_max) {
119 current_max = curr_dist_sq;
120 }
121 }
122 }
123 }
124 }
125 return res;
126 }
127
128 /*
129 * tries to find an item exactly
130 */
131 struct quadtree_item*
132 quadtree_find_item(struct quadtree_node* this_, struct quadtree_item* item) {
133 struct quadtree_item*res = NULL;
134 if( ! this_ ) {
135 return NULL;
136 }
137 if( this_->is_leaf ) {
138 int i;
139 for(i=0;i<this_->node_num;++i) {
140 //TODO equality check may not be correct on float values! maybe it can be replaced by range check with some tolerance
141 if(item->longitude==this_->items[i].longitude && item->latitude==this_->items[i].latitude) {
142 res = &this_->items[i];
143 return res;
144 }
145 }
146 return NULL;
147 }
148 else {
149 if(
150 this_->aa &&
151 this_->aa->xmin<=item->longitude && item->longitude<this_->aa->xmax &&
152 this_->aa->ymin<=item->latitude && item->latitude<this_->aa->ymax
153 ) {
154 res = quadtree_find_item(this_->aa,item);
155 }
156 else if(
157 this_->ab &&
158 this_->ab->xmin<=item->longitude && item->longitude<this_->ab->xmax &&
159 this_->ab->ymin<=item->latitude && item->latitude<this_->ab->ymax
160 ) {
161 res = quadtree_find_item(this_->ab,item);
162 }
163 else if(
164 this_->ba &&
165 this_->ba->xmin<=item->longitude && item->longitude<this_->ba->xmax &&
166 this_->ba->ymin<=item->latitude && item->latitude<this_->ba->ymax
167 ) {
168 res = quadtree_find_item(this_->ba,item);
169 }
170 else if(
171 this_->bb &&
172 this_->bb->xmin<=item->longitude && item->longitude<this_->bb->xmax &&
173 this_->bb->ymin<=item->latitude && item->latitude<this_->bb->ymax
174 ) {
175 res = quadtree_find_item(this_->bb,item);
176 }
177 else {
178 return NULL;
179 }
180 }
181 return res;
182 }
183
184 /*
185 * returns the containing node for an item
186 */
187 struct quadtree_node*
188 quadtree_find_containing_node(struct quadtree_node* root, struct quadtree_item* item)
189 {
190 struct quadtree_node*res = NULL;
191
192 if( ! root ) {
193 return NULL;
194 }
195
196 if( root->is_leaf ) {
197 int i;
198 for(i=0;i<root->node_num;++i) {
199 if(item == &root->items[i]) {
200 res = root;
201 }
202 }
203 }
204 else {
205 if(
206 root->aa &&
207 root->aa->xmin<=item->longitude && item->longitude<root->aa->xmax &&
208 root->aa->ymin<=item->latitude && item->latitude<root->aa->ymax
209 ) {
210 res = quadtree_find_containing_node(root->aa,item);
211 }
212 else if(
213 root->ab &&
214 root->ab->xmin<=item->longitude && item->longitude<root->ab->xmax &&
215 root->ab->ymin<=item->latitude && item->latitude<root->ab->ymax
216 ) {
217 res = quadtree_find_containing_node(root->ab,item);
218 }
219 else if(
220 root->ba &&
221 root->ba->xmin<=item->longitude && item->longitude<root->ba->xmax &&
222 root->ba->ymin<=item->latitude && item->latitude<root->ba->ymax
223 ) {
224 res = quadtree_find_containing_node(root->ba,item);
225 }
226 else if(
227 root->bb &&
228 root->bb->xmin<=item->longitude && item->longitude<root->bb->xmax &&
229 root->bb->ymin<=item->latitude && item->latitude<root->bb->ymax
230 ) {
231 res = quadtree_find_containing_node(root->bb,item);
232 }
233 else {
234 //this should not happen
235 }
236 }
237 return res;
238 }
239
240
241 int quadtree_delete_item(struct quadtree_node* root, struct quadtree_item* item)
242 {
243 struct quadtree_node* qn = quadtree_find_containing_node(root,item);
244 int i, bFound = 0;
245
246 if(!qn || !qn->node_num) {
247 return 0;
248 }
249
250 for(i=0;i<qn->node_num;++i) {
251 if( &qn->items[i] == item) {
252 bFound = 1;
253 }
254 if(bFound && i<qn->node_num-1) {
255 qn->items[i] = qn->items[i+1];
256 }
257 }
258 if(bFound) {
259 --qn->node_num;
260 }
261 return bFound;
262 }
263
264
265 /*
266 * 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
267 */
268 struct quadtree_item*
269 quadtree_find_nearest(struct quadtree_node* this_, struct quadtree_item* item) {
270 struct quadtree_item*res = NULL;
271 double distance_sq = MAX_DOUBLE;
272 if( ! this_ ) {
273 return NULL;
274 }
275 if( this_->is_leaf ) {
276 int i;
277 for(i=0;i<this_->node_num;++i) {
278 double curr_dist_sq = dist_sq(item->longitude,item->latitude,this_->items[i].longitude,this_->items[i].latitude);
279 if(curr_dist_sq<distance_sq) {
280 distance_sq = curr_dist_sq;
281 res = &this_->items[i];
282 }
283 }
284 //go up n levels
285 if(!res && this_->parent) {
286 struct quadtree_item*res2 = NULL;
287 struct quadtree_node* anchestor = this_->parent;
288 int cnt = 0;
289 while (anchestor->parent && cnt<4) {
290 anchestor = anchestor->parent;
291 ++cnt;
292 }
293 res2 = quadtree_find_nearest_flood(anchestor,item,distance_sq,NULL);
294 if(res2) {
295 res = res2;
296 }
297 }
298 }
299 else {
300 if(
301 this_->aa &&
302 this_->aa->xmin<=item->longitude && item->longitude<this_->aa->xmax &&
303 this_->aa->ymin<=item->latitude && item->latitude<this_->aa->ymax
304 ) {
305 res = quadtree_find_nearest(this_->aa,item);
306 }
307 else if(
308 this_->ab &&
309 this_->ab->xmin<=item->longitude && item->longitude<this_->ab->xmax &&
310 this_->ab->ymin<=item->latitude && item->latitude<this_->ab->ymax
311 ) {
312 res = quadtree_find_nearest(this_->ab,item);
313 }
314 else if(
315 this_->ba &&
316 this_->ba->xmin<=item->longitude && item->longitude<this_->ba->xmax &&
317 this_->ba->ymin<=item->latitude && item->latitude<this_->ba->ymax
318 ) {
319 res = quadtree_find_nearest(this_->ba,item);
320 }
321 else if(
322 this_->bb &&
323 this_->bb->xmin<=item->longitude && item->longitude<this_->bb->xmax &&
324 this_->bb->ymin<=item->latitude && item->latitude<this_->bb->ymax
325 ) {
326 res = quadtree_find_nearest(this_->bb,item);
327 }
328 else {
329 if(this_->parent) {
330 //go up two levels
331 struct quadtree_node* anchestor = this_->parent;
332 int cnt = 0;
333 while (anchestor->parent && cnt<4) {
334 anchestor = anchestor->parent;
335 ++cnt;
336 }
337 res = quadtree_find_nearest_flood(anchestor,item,distance_sq,NULL);
338 }
339 }
340 }
341 return res;
342
343 }
344
345 void
346 quadtree_add(struct quadtree_node* this_, struct quadtree_item* item) {
347 if( this_->is_leaf ) {
348 this_->items[this_->node_num++] = *item;
349 if(QUADTREE_NODE_CAPACITY == this_->node_num) {
350 quadtree_split(this_);
351 }
352 }
353 else {
354 if(
355 this_->xmin<=item->longitude && item->longitude<this_->xmin+(this_->xmax-this_->xmin)/2.0 &&
356 this_->ymin<=item->latitude && item->latitude<this_->ymin+(this_->ymax-this_->ymin)/2.0
357 ) {
358 if(!this_->aa) {
359 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 );
360 }
361 quadtree_add(this_->aa,item);
362 }
363 else if(
364 this_->xmin+(this_->xmax-this_->xmin)/2.0<=item->longitude && item->longitude<this_->xmax &&
365 this_->ymin<=item->latitude && item->latitude<this_->ymin+(this_->ymax-this_->ymin)/2.0
366 ) {
367 if(!this_->ab) {
368 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 );
369 }
370 quadtree_add(this_->ab,item);
371 }
372 else if(
373 this_->xmin<=item->longitude && item->longitude<this_->xmin+(this_->xmax-this_->xmin)/2.0 &&
374 this_->ymin+(this_->ymax-this_->ymin)/2.0<=item->latitude && item->latitude<this_->ymax
375 ) {
376 if(!this_->ba) {
377 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);
378 }
379 quadtree_add(this_->ba,item);
380 }
381 else if(
382 this_->xmin+(this_->xmax-this_->xmin)/2.0<=item->longitude && item->longitude<this_->xmax &&
383 this_->ymin+(this_->ymax-this_->ymin)/2.0<=item->latitude && item->latitude<this_->ymax
384 ) {
385 if(!this_->bb) {
386 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);
387 }
388 quadtree_add(this_->bb,item);
389 }
390 }
391 }
392
393 void
394 quadtree_split(struct quadtree_node* this_) {
395 int i;
396 this_->is_leaf = 0;
397 for(i=0;i<this_->node_num;++i) {
398 if(
399 this_->xmin<=this_->items[i].longitude && this_->items[i].longitude<this_->xmin+(this_->xmax-this_->xmin)/2.0 &&
400 this_->ymin<=this_->items[i].latitude && this_->items[i].latitude<this_->ymin+(this_->ymax-this_->ymin)/2.0
401 ) {
402 if(!this_->aa) {
403 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 );
404 }
405 quadtree_add(this_->aa,&this_->items[i]);
406 }
407 else if(
408 this_->xmin+(this_->xmax-this_->xmin)/2.0<=this_->items[i].longitude && this_->items[i].longitude<this_->xmax &&
409 this_->ymin<=this_->items[i].latitude && this_->items[i].latitude<this_->ymin+(this_->ymax-this_->ymin)/2.0
410 ) {
411 if(!this_->ab) {
412 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 );
413 }
414 quadtree_add(this_->ab,&this_->items[i]);
415 }
416 else if(
417 this_->xmin<=this_->items[i].longitude && this_->items[i].longitude<this_->xmin+(this_->xmax-this_->xmin)/2.0 &&
418 this_->ymin+(this_->ymax-this_->ymin)/2.0<=this_->items[i].latitude && this_->items[i].latitude<this_->ymax
419 ) {
420 if(!this_->ba) {
421 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);
422 }
423 quadtree_add(this_->ba,&this_->items[i]);
424 }
425 else if(
426 this_->xmin+(this_->xmax-this_->xmin)/2.0<=this_->items[i].longitude && this_->items[i].longitude<this_->xmax &&
427 this_->ymin+(this_->ymax-this_->ymin)/2.0<=this_->items[i].latitude && this_->items[i].latitude<this_->ymax
428 ) {
429 if(!this_->bb) {
430 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);
431 }
432 quadtree_add(this_->bb,&this_->items[i]);
433 }
434 }
435 this_->node_num = 0;
436 }
437
438 void
439 quadtree_destroy(struct quadtree_node* this_) {
440 if(this_->aa) {
441 quadtree_destroy(this_->aa);
442 this_->aa = NULL;
443 }
444 if(this_->ab) {
445 quadtree_destroy(this_->ab);
446 this_->ab = NULL;
447 }
448 if(this_->ba) {
449 quadtree_destroy(this_->ba);
450 this_->ba = NULL;
451 }
452 if(this_->bb) {
453 quadtree_destroy(this_->bb);
454 this_->bb = NULL;
455 }
456 free(this_);
457 }
458
459
460
461

   
Visit the ZANavi Wiki