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 |
|