/[zanavi_public1]/navit/navit/transform.c
ZANavi

Contents of /navit/navit/transform.c

Parent Directory Parent Directory | Revision Log Revision Log


Revision 31 - (show annotations) (download)
Mon Feb 4 17:41:59 2013 UTC (7 years, 6 months ago) by zoff99
File MIME type: text/plain
File size: 41727 byte(s)
new map version, lots of fixes and experimental new features
1 /**
2 * ZANavi, Zoff Android Navigation system.
3 * Copyright (C) 2011-2012 Zoff <zoff@zoff.cc>
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 /**
21 * Navit, a modular navigation system.
22 * Copyright (C) 2005-2008 Navit Team
23 *
24 * This program is free software; you can redistribute it and/or
25 * modify it under the terms of the GNU General Public License
26 * version 2 as published by the Free Software Foundation.
27 *
28 * This program is distributed in the hope that it will be useful,
29 * but WITHOUT ANY WARRANTY; without even the implied warranty of
30 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
31 * GNU General Public License for more details.
32 *
33 * You should have received a copy of the GNU General Public License
34 * along with this program; if not, write to the
35 * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
36 * Boston, MA 02110-1301, USA.
37 */
38
39 #define _USE_MATH_DEFINES 1
40 #include <assert.h>
41 #include <stdio.h>
42 #include <math.h>
43 #include <limits.h>
44 #include <glib.h>
45 #include <string.h>
46 #include "config.h"
47 #include "coord.h"
48 #include "debug.h"
49 #include "item.h"
50 #include "map.h"
51 #include "transform.h"
52 #include "projection.h"
53 #include "point.h"
54 #include "navit.h"
55
56 #define POST_SHIFT 8
57
58 #ifdef ENABLE_ROLL
59 #define HOG(t) ((t).hog)
60 #else
61 #define HOG(t) 0
62 #endif
63
64 static void transform_set_screen_dist(struct transformation *t, int dist)
65 {
66 t->screen_dist = dist;
67 t->xscale3d = dist;
68 t->yscale3d = dist;
69 t->wscale3d = dist << POST_SHIFT;
70 }
71
72 static void transform_setup_matrix(struct transformation *t)
73 {
74 navit_float det;
75 navit_float fac;
76 navit_float yawc = navit_cos(-M_PI * t->yaw / 180);
77 navit_float yaws = navit_sin(-M_PI * t->yaw / 180);
78 navit_float pitchc = navit_cos(-M_PI * t->pitch / 180);
79 navit_float pitchs = navit_sin(-M_PI * t->pitch / 180);
80 #ifdef ENABLE_ROLL
81 navit_float rollc=navit_cos(M_PI*t->roll/180);
82 navit_float rolls=navit_sin(M_PI*t->roll/180);
83 #else
84 navit_float rollc = 1;
85 navit_float rolls = 0;
86 #endif
87
88 int scale = t->scale;
89 int order_dir = -1;
90
91 //dbg(1,"yaw=%d pitch=%d center=0x%x,0x%x\n", t->yaw, t->pitch, t->map_center.x, t->map_center.y);
92 t->znear = 1 << POST_SHIFT;
93 t->zfar = 300 * t->znear;
94 t->scale_shift = 0;
95 t->order = t->order_base;
96
97 if (t->scale >= 1)
98 {
99 scale = t->scale;
100 }
101 else
102 {
103 scale = 1.0 / t->scale;
104 order_dir = 1;
105 }
106
107 while (scale > 1)
108 {
109 if (order_dir < 0)
110 t->scale_shift++;
111 t->order += order_dir;
112 scale >>= 1;
113 }
114
115 fac = (1 << POST_SHIFT) * (1 << t->scale_shift) / t->scale;
116 //dbg(1,"scale_shift=%d order=%d scale=%f fac=%f\n", t->scale_shift, t->order,t->scale,fac);
117
118 t->m00 = rollc * yawc * fac;
119 t->m01 = rollc * yaws * fac;
120 t->m02 = -rolls * fac;
121 t->m10 = (pitchs * rolls * yawc - pitchc * yaws) * (-fac);
122 t->m11 = (pitchs * rolls * yaws + pitchc * yawc) * (-fac);
123 t->m12 = pitchs * rollc * (-fac);
124 t->m20 = (pitchc * rolls * yawc + pitchs * yaws) * fac;
125 t->m21 = (pitchc * rolls * yaws - pitchs * yawc) * fac;
126 t->m22 = pitchc * rollc * fac;
127
128 t->offx = t->screen_center.x;
129 t->offy = t->screen_center.y;
130 if (t->pitch)
131 {
132 t->ddd = 1;
133 t->offz = t->screen_dist;
134 //dbg(1,"near %d far %d\n",t->znear,t->zfar);
135 t->xscale = t->xscale3d;
136 t->yscale = t->yscale3d;
137 t->wscale = t->wscale3d;
138 }
139 else
140 {
141 t->ddd = 0;
142 t->offz = 0;
143 t->xscale = 1;
144 t->yscale = 1;
145 t->wscale = 1;
146 }
147 det = (navit_float) t->m00 * (navit_float) t->m11 * (navit_float) t->m22 + (navit_float) t->m01 * (navit_float) t->m12 * (navit_float) t->m20 + (navit_float) t->m02 * (navit_float) t->m10 * (navit_float) t->m21 - (navit_float) t->m02 * (navit_float) t->m11 * (navit_float) t->m20 - (navit_float) t->m01 * (navit_float) t->m10 * (navit_float) t->m22 - (navit_float) t->m00 * (navit_float) t->m12 * (navit_float) t->m21;
148
149 t->im00 = (t->m11 * t->m22 - t->m12 * t->m21) / det;
150 t->im01 = (t->m02 * t->m21 - t->m01 * t->m22) / det;
151 t->im02 = (t->m01 * t->m12 - t->m02 * t->m11) / det;
152 t->im10 = (t->m12 * t->m20 - t->m10 * t->m22) / det;
153 t->im11 = (t->m00 * t->m22 - t->m02 * t->m20) / det;
154 t->im12 = (t->m02 * t->m10 - t->m00 * t->m12) / det;
155 t->im20 = (t->m10 * t->m21 - t->m11 * t->m20) / det;
156 t->im21 = (t->m01 * t->m20 - t->m00 * t->m21) / det;
157 t->im22 = (t->m00 * t->m11 - t->m01 * t->m10) / det;
158 }
159
160 struct transformation *
161 transform_new(void)
162 {
163 struct transformation *this_;
164
165 this_=g_new0(struct transformation, 1);
166 transform_set_screen_dist(this_, 100);
167 this_->order_base = 14;
168 #if 0
169 this_->pitch=20;
170 #endif
171 #if 0
172 this_->roll=30;
173 this_->hog=1000;
174 #endif
175 transform_setup_matrix(this_);
176 return this_;
177 }
178
179 int transform_get_hog(struct transformation *this_)
180 {
181 return HOG(*this_);
182 }
183
184 void transform_set_hog(struct transformation *this_, int hog)
185 {
186 #ifdef ENABLE_ROLL
187 this_->hog=hog;
188 #else
189 dbg(0, "not supported\n");
190 #endif
191
192 }
193
194 int transform_get_attr(struct transformation *this_, enum attr_type type, struct attr *attr, struct attr_iter *iter)
195 {
196 switch (type)
197 {
198 #ifdef ENABLE_ROLL
199 case attr_hog:
200 attr->u.num=this_->hog;
201 break;
202 #endif
203 default:
204 return 0;
205 }
206 attr->type = type;
207 return 1;
208 }
209
210 int transform_set_attr(struct transformation *this_, struct attr *attr)
211 {
212 switch (attr->type)
213 {
214 #ifdef ENABLE_ROLL
215 case attr_hog:
216 this_->hog=attr->u.num;
217 return 1;
218 #endif
219 default:
220 return 0;
221 }
222 }
223
224 int transformation_get_order_base(struct transformation *this_)
225 {
226 return this_->order_base;
227 }
228
229 void transform_set_order_base(struct transformation *this_, int order_base)
230 {
231 this_->order_base = order_base;
232 }
233
234 struct transformation *
235 transform_dup(struct transformation *t)
236 {
237 struct transformation *ret=g_new0(struct transformation, 1);
238 *ret = *t;
239 return ret;
240 }
241
242 static const navit_float gar2geo_units = 360.0 / (1 << 24);
243 static const navit_float geo2gar_units = 1 / (360.0 / (1 << 24));
244
245 void transform_to_geo(enum projection pro, struct coord *c, struct coord_geo *g)
246 {
247 // dbg(0,"enter\n");
248
249 int x, y, northern, zone;
250
251 #if 0
252 int hash_id;
253 int s;
254 struct hash_entry_transform *v = NULL;
255 #endif
256
257 switch (pro)
258 {
259 case projection_mg:
260
261 #if 0
262 hash_id = (long)(c->x) << 16 | (int)(c->y);
263 dbg(0,"h=%d\n", hash_id);
264
265 if (global_transform_hash2)
266 {
267 s=g_hash_table_size(global_transform_hash2);
268 dbg(0,"size=%d\n",s);
269 if (s > 30000)
270 {
271 g_hash_table_remove_all(global_transform_hash2);
272 }
273 v = g_hash_table_lookup(global_transform_hash2, &hash_id);
274 }
275
276 if (v != NULL)
277 {
278 if ((v->x == c->x)&&(v->y == c->y))
279 {
280 g->lng = v->lng;
281 g->lat = v->lat;
282 }
283 else
284 {
285 // g->lng=c->x/6371000.0/M_PI*180;
286 g->lng = c->x * 0.00000899322; // simpler
287 g->lat = navit_atan(exp(c->y / 6371000.0)) / M_PI * 360 - 90;
288
289 if (global_transform_hash2)
290 {
291 v = g_new0(struct hash_entry_transform,1);
292 v->id=hash_id;
293 v->x=c->x;
294 v->y=c->y;
295 v->lat=g->lat;
296 v->lng=g->lng;
297 g_hash_table_insert(global_transform_hash2, &v->id, v);
298 }
299 }
300 }
301 else
302 {
303 // g->lng=c->x/6371000.0/M_PI*180;
304 g->lng = c->x * 0.00000899322; // simpler
305 g->lat = navit_atan(exp(c->y / 6371000.0)) / M_PI * 360 - 90;
306
307 if (global_transform_hash2)
308 {
309 v = g_new0(struct hash_entry_transform,1);
310 v->id=hash_id;
311 v->x=c->x;
312 v->y=c->y;
313 v->lat=g->lat;
314 v->lng=g->lng;
315 g_hash_table_insert(global_transform_hash2, &v->id, v);
316 }
317 }
318 #endif
319 // g->lng=c->x/6371000.0/M_PI*180;
320 g->lng = c->x * 0.00000899322; // simpler
321 g->lat = navit_atan(exp(c->y / 6371000.0)) / M_PI * 360 - 90;
322
323 break;
324 case projection_garmin:
325 g->lng = c->x * gar2geo_units;
326 g->lat = c->y * gar2geo_units;
327 break;
328 case projection_utm:
329 x = c->x;
330 y = c->y;
331 northern = y >= 0;
332 if (!northern)
333 {
334 y += 10000000;
335 }
336 zone = (x / 1000000);
337 x = x % 1000000;
338 transform_utm_to_geo(x, y, zone, northern, g);
339 break;
340 default:
341 break;
342 }
343 }
344
345 void transform_from_geo(enum projection pro, struct coord_geo *g, struct coord *c)
346 {
347 // dbg(0,"enter\n");
348
349 #if 0
350 int hash_id;
351 int s;
352 struct hash_entry_transform *v = NULL;
353 #endif
354
355 switch (pro)
356 {
357 case projection_mg:
358 #if 0
359 // check if value already in hash
360 hash_id = (long)(g->lat*1000000) << 16 | (int)(g->lng*1000000);
361 dbg(0,"h=%d\n", hash_id);
362
363 //dbg(0,"tt 001 %f %d %f %d %lu %d\n", g->lat, (int)g->lat, g->lng, (int)g->lng, hash_id, (int)hash_id);
364 //dbg(0,"%lu %d\n",(long)(g->lat*1000000) << 16, (int)(g->lng*1000000));
365 if (global_transform_hash)
366 {
367 //dbg(0,"tt 001.1\n");
368 s=g_hash_table_size(global_transform_hash);
369 dbg(0,"size=%d\n",s);
370 if (s > 30000)
371 {
372 g_hash_table_remove_all(global_transform_hash);
373 }
374 v = g_hash_table_lookup(global_transform_hash, &hash_id);
375 }
376
377 if (v != NULL)
378 {
379 if ((v->lat == g->lat)&&(v->lng == g->lng))
380 {
381 c->x = v->x;
382 c->y = v->y;
383 }
384 else
385 {
386 // c->x=g->lng*6371000.0*M_PI/180;
387 c->x = g->lng * 111194.9266445587373; // already calced (6371000.0*M_PI/180)
388 // c->y=log(navit_tan(M_PI_4+g->lat*M_PI/360))*6371000.0;
389 c->y = log(navit_tan(M_PI_4 + g->lat * 0.008726646259971647884618)) * 6371000.0; // already calced (M_PI/360)
390
391 if (global_transform_hash)
392 {
393 v = g_new0(struct hash_entry_transform,1);
394 v->id=hash_id;
395 v->x=c->x;
396 v->y=c->y;
397 v->lat=g->lat;
398 v->lng=g->lng;
399 g_hash_table_insert(global_transform_hash, &v->id, v);
400 }
401 }
402 }
403 else
404 {
405 // c->x=g->lng*6371000.0*M_PI/180;
406 c->x = g->lng * 111194.9266445587373; // already calced (6371000.0*M_PI/180)
407 // c->y=log(navit_tan(M_PI_4+g->lat*M_PI/360))*6371000.0;
408 c->y = log(navit_tan(M_PI_4 + g->lat * 0.008726646259971647884618)) * 6371000.0; // already calced (M_PI/360)
409
410 if (global_transform_hash)
411 {
412 v = g_new0(struct hash_entry_transform,1);
413 v->id=hash_id;
414 v->x=c->x;
415 v->y=c->y;
416 v->lat=g->lat;
417 v->lng=g->lng;
418 g_hash_table_insert(global_transform_hash, &v->id, v);
419 }
420 }
421 #endif
422 // c->x=g->lng*6371000.0*M_PI/180;
423 c->x = g->lng * 111194.9266445587373; // already calced (6371000.0*M_PI/180)
424 // c->y=log(navit_tan(M_PI_4+g->lat*M_PI/360))*6371000.0;
425 c->y = log(navit_tan(M_PI_4 + g->lat * 0.008726646259971647884618)) * 6371000.0; // already calced (M_PI/360)
426
427 break;
428 case projection_garmin:
429 c->x = g->lng * geo2gar_units;
430 c->y = g->lat * geo2gar_units;
431 break;
432 default:
433 break;
434 }
435 }
436
437 void transform_from_to_count(struct coord *cfrom, enum projection from, struct coord *cto, enum projection to, int count)
438 {
439 struct coord_geo g;
440 int i;
441
442 for (i = 0; i < count; i++)
443 {
444 transform_to_geo(from, cfrom, &g);
445 transform_from_geo(to, &g, cto);
446 cfrom++;
447 cto++;
448 }
449 }
450
451 void transform_from_to(struct coord *cfrom, enum projection from, struct coord *cto, enum projection to)
452 {
453 struct coord_geo g;
454
455 transform_to_geo(from, cfrom, &g);
456 transform_from_geo(to, &g, cto);
457 }
458
459 void transform_geo_to_cart(struct coord_geo *geo, navit_float a, navit_float b, struct coord_geo_cart *cart)
460 {
461 navit_float n, ee = 1 - b * b / (a * a);
462 n = a / sqrtf(1 - ee * navit_sin(geo->lat) * navit_sin(geo->lat));
463 cart->x = n * navit_cos(geo->lat) * navit_cos(geo->lng);
464 cart->y = n * navit_cos(geo->lat) * navit_sin(geo->lng);
465 cart->z = n * (1 - ee) * navit_sin(geo->lat);
466 }
467
468 void transform_cart_to_geo(struct coord_geo_cart *cart, navit_float a, navit_float b, struct coord_geo *geo)
469 {
470 navit_float lat, lati, n, ee = 1 - b * b / (a * a), lng = navit_tan(cart->y / cart->x);
471
472 lat = navit_tan(cart->z / navit_sqrt((cart->x * cart->x) + (cart->y * cart->y)));
473 do
474 {
475 lati = lat;
476
477 n = a / navit_sqrt(1 - ee * navit_sin(lat) * navit_sin(lat));
478 lat = navit_atan((cart->z + ee * n * navit_sin(lat)) / navit_sqrt(cart->x * cart->x + cart->y * cart->y));
479 }
480 while (fabs(lat - lati) >= 0.000000000000001);
481
482 geo->lng = lng / M_PI * 180;
483 geo->lat = lat / M_PI * 180;
484 }
485
486 void transform_utm_to_geo(const double UTMEasting, const double UTMNorthing, int ZoneNumber, int NorthernHemisphere, struct coord_geo *geo)
487 {
488 //converts UTM coords to lat/long. Equations from USGS Bulletin 1532
489 //East Longitudes are positive, West longitudes are negative.
490 //North latitudes are positive, South latitudes are negative
491 //Lat and Long are in decimal degrees.
492 //Written by Chuck Gantz- chuck.gantz@globalstar.com
493
494 double Lat, Long;
495 double k0 = 0.99960000000000004;
496 double a = 6378137;
497 double eccSquared = 0.0066943799999999998;
498 double eccPrimeSquared;
499 double e1 = (1 - sqrt(1 - eccSquared)) / (1 + sqrt(1 - eccSquared));
500 double N1, T1, C1, R1, D, M;
501 double LongOrigin;
502 double mu, phi1, phi1Rad;
503 double x, y;
504 double rad2deg = 180 / M_PI;
505
506 x = UTMEasting - 500000.0; //remove 500,000 meter offset for longitude
507 y = UTMNorthing;
508
509 if (!NorthernHemisphere)
510 {
511 y -= 10000000.0;//remove 10,000,000 meter offset used for southern hemisphere
512 }
513
514 LongOrigin = (ZoneNumber - 1) * 6 - 180 + 3; //+3 puts origin in middle of zone
515
516 eccPrimeSquared = (eccSquared) / (1 - eccSquared);
517
518 M = y / k0;
519 mu = M / (a * (1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 - 5 * eccSquared * eccSquared * eccSquared / 256));
520 phi1Rad = mu + (3 * e1 / 2 - 27 * e1 * e1 * e1 / 32) * sin(2 * mu) + (21 * e1 * e1 / 16 - 55 * e1 * e1 * e1 * e1 / 32) * sin(4 * mu) + (151 * e1 * e1 * e1 / 96) * sin(6 * mu);
521 phi1 = phi1Rad * rad2deg;
522
523 N1 = a / sqrt(1 - eccSquared * sin(phi1Rad) * sin(phi1Rad));
524 T1 = tan(phi1Rad) * tan(phi1Rad);
525 C1 = eccPrimeSquared * cos(phi1Rad) * cos(phi1Rad);
526 R1 = a * (1 - eccSquared) / pow(1 - eccSquared * sin(phi1Rad) * sin(phi1Rad), 1.5);
527 D = x / (N1 * k0);
528
529 Lat = phi1Rad - (N1 * tan(phi1Rad) / R1) * (D * D / 2 - (5 + 3 * T1 + 10 * C1 - 4 * C1 * C1 - 9 * eccPrimeSquared) * D * D * D * D / 24 + (61 + 90 * T1 + 298 * C1 + 45 * T1 * T1 - 252 * eccPrimeSquared - 3 * C1 * C1) * D * D * D * D * D * D / 720);
530 Lat = Lat * rad2deg;
531
532 Long = (D - (1 + 2 * T1 + C1) * D * D * D / 6 + (5 - 2 * C1 + 28 * T1 - 3 * C1 * C1 + 8 * eccPrimeSquared + 24 * T1 * T1) * D * D * D * D * D / 120) / cos(phi1Rad);
533 Long = LongOrigin + Long * rad2deg;
534
535 geo->lat = Lat;
536 geo->lng = Long;
537 }
538
539 void transform_datum(struct coord_geo *from, enum map_datum from_datum, struct coord_geo *to, enum map_datum to_datum)
540 {
541 }
542
543 int transform(struct transformation *t, enum projection pro, struct coord *c, struct point *p, int count, int mindist, int width, int *width_return)
544 {
545 struct coord c1;
546 int xcn, ycn;
547 struct coord_geo g;
548 int xc, yc, zc = 0, xco = 0, yco = 0, zco = 0;
549 int xm, ym, zct;
550 int zlimit = t->znear;
551 int visible, visibleo = -1;
552 int i, j = 0, k = 0;
553
554 //dbg(0,"count=%d\n", count);
555 for (i = 0; i < count; i++)
556 {
557 if (pro == t->pro)
558 {
559 xc = c[i].x;
560 yc = c[i].y;
561 }
562 else
563 {
564 //dbg(0,"to from geo\n");
565 transform_to_geo(pro, &c[i], &g);
566 transform_from_geo(t->pro, &g, &c1);
567 xc = c1.x;
568 yc = c1.y;
569 }
570
571 if (i != 0 && i != count - 1 && mindist)
572 {
573 if (xc > c[k].x - mindist && xc < c[k].x + mindist && yc > c[k].y - mindist && yc < c[k].y + mindist && (c[i + 1].x != c[0].x || c[i + 1].y != c[0].y))
574 {
575 continue;
576 }
577 k = i;
578 }
579 xm = xc;
580 ym = yc;
581 // dbg(2,"0x%x, 0x%x - 0x%x,0x%x contains 0x%x,0x%x\n", t->r.lu.x, t->r.lu.y, t->r.rl.x, t->r.rl.y, c->x, c->y);
582 // ret=coord_rect_contains(&t->r, c);
583 xc -= t->map_center.x;
584 yc -= t->map_center.y;
585 xc >>= t->scale_shift;
586 yc >>= t->scale_shift;
587 xm = xc;
588 ym = yc;
589
590 xcn = xc * t->m00 + yc * t->m01 + HOG(*t) * t->m02;
591 ycn = xc * t->m10 + yc * t->m11 + HOG(*t) * t->m12;
592
593 if (t->ddd)
594 {
595 zc = (xc * t->m20 + yc * t->m21 + HOG(*t) * t->m22);
596 zct = zc;
597 zc += t->offz << POST_SHIFT;
598 //dbg(1,"zc=%d\n", zc);
599 //dbg(1,"zc(%d)=xc(%d)*m20(%d)+yc(%d)*m21(%d)\n", (xc*t->m20+yc*t->m21), xc, t->m20, yc, t->m21);
600 /* visibility */
601 visible = (zc < zlimit ? 0 : 1);
602 //dbg(1,"visible=%d old %d\n", visible, visibleo);
603 if (visible != visibleo && visibleo != -1)
604 {
605 //dbg(1,"clipping (%d,%d,%d)-(%d,%d,%d) (%d,%d,%d)\n", xcn, ycn, zc, xco, yco, zco, xco-xcn, yco-ycn, zco-zc);
606 if (zco != zc)
607 {
608 xcn = xcn + (long long) (xco - xcn) * (zlimit - zc) / (zco - zc);
609 ycn = ycn + (long long) (yco - ycn) * (zlimit - zc) / (zco - zc);
610 }
611 //dbg(1,"result (%d,%d,%d) * %d / %d\n", xcn,ycn,zc,zlimit-zc,zco-zc);
612 zc = zlimit;
613 xco = xcn;
614 yco = ycn;
615 zco = zc;
616 if (visible)
617 {
618 i--;
619 }
620 visibleo = visible;
621 }
622 else
623 {
624 xco = xcn;
625 yco = ycn;
626 zco = zc;
627 visibleo = visible;
628
629 if (!visible)
630 {
631 continue;
632 }
633 }
634 //dbg(1,"zc=%d\n", zc);
635 //dbg(1,"xcn %d ycn %d\n", xcn, ycn);
636 //dbg(1,"%d,%d %d\n",xc,yc,zc);
637 //#if 0
638 // dbg(0,"%d/%d=%d %d/%d=%d\n",xcn,xc,xcn/xc,ycn,yc,ycn/yc);
639 //#endif
640
641 //#if 1
642 xc = (long long) xcn * t->xscale / zc;
643 yc = (long long) ycn * t->yscale / zc;
644 //#else
645 // xc=xcn/(1000+zc);
646 // yc=ycn/(1000+zc);
647 //#endif
648
649
650 //#if 0
651 // dbg(1,"%d,%d %d\n",xc,yc,zc);
652 //#endif
653 }
654 else
655 {
656 xc = xcn;
657 yc = ycn;
658 xc >>= POST_SHIFT;
659 yc >>= POST_SHIFT;
660 }
661
662 xc += t->offx;
663 yc += t->offy;
664 p[j].x = xc;
665 p[j].y = yc;
666
667 if (width_return)
668 {
669 if (t->ddd)
670 {
671 width_return[j] = width * t->wscale / zc;
672 }
673 else
674 {
675 width_return[j] = width;
676 }
677 }
678 j++;
679 } // ------- END for loop ----------
680
681 return j;
682 }
683
684 static void transform_apply_inverse_matrix(struct transformation *t, struct coord_geo_cart *in, struct coord_geo_cart *out)
685 {
686 out->x = in->x * t->im00 + in->y * t->im01 + in->z * t->im02;
687 out->y = in->x * t->im10 + in->y * t->im11 + in->z * t->im12;
688 out->z = in->x * t->im20 + in->y * t->im21 + in->z * t->im22;
689 }
690
691 static int transform_zplane_intersection(struct coord_geo_cart *p1, struct coord_geo_cart *p2, navit_float z, struct coord_geo_cart *result)
692 {
693 navit_float dividend = z - p1->z;
694 navit_float divisor = p2->z - p1->z;
695 navit_float q;
696 if (!divisor)
697 {
698 if (dividend)
699 return 0; /* no intersection */
700 else
701 return 3; /* identical planes */
702 }
703 q = dividend / divisor;
704 result->x = p1->x + q * (p2->x - p1->x);
705 result->y = p1->y + q * (p2->y - p1->y);
706 result->z = z;
707 if (q >= 0 && q <= 1)
708 return 1; /* intersection within [p1,p2] */
709 return 2; /* intersection without [p1,p2] */
710 }
711
712 static void transform_screen_to_3d(struct transformation *t, struct point *p, navit_float z, struct coord_geo_cart *cg)
713 {
714 double xc, yc;
715 double offz = t->offz << POST_SHIFT;
716 xc = p->x - t->offx;
717 yc = p->y - t->offy;
718 cg->x = xc * z / t->xscale;
719 cg->y = yc * z / t->yscale;
720 cg->z = z - offz;
721 }
722
723 static int transform_reverse_near_far(struct transformation *t, struct point *p, struct coord *c, int near, int far)
724 {
725 double xc, yc;
726
727 //dbg(1,"%d,%d\n",p->x,p->y);
728
729 if (t->ddd)
730 {
731 struct coord_geo_cart nearc, farc, nears, fars, intersection;
732 transform_screen_to_3d(t, p, near, &nearc);
733 transform_screen_to_3d(t, p, far, &farc);
734 transform_apply_inverse_matrix(t, &nearc, &nears);
735 transform_apply_inverse_matrix(t, &farc, &fars);
736 if (transform_zplane_intersection(&nears, &fars, HOG(*t), &intersection) != 1)
737 {
738 return 0;
739 }
740 xc = intersection.x;
741 yc = intersection.y;
742 }
743 else
744 {
745 double xcn, ycn;
746 xcn = p->x - t->offx;
747 ycn = p->y - t->offy;
748 xc = (xcn * t->im00 + ycn * t->im01) * (1 << POST_SHIFT);
749 yc = (xcn * t->im10 + ycn * t->im11) * (1 << POST_SHIFT);
750 }
751
752 c->x = xc * (1 << t->scale_shift) + t->map_center.x;
753 c->y = yc * (1 << t->scale_shift) + t->map_center.y;
754
755 return 1;
756 }
757
758 int transform_reverse(struct transformation *t, struct point *p, struct coord *c)
759 {
760 return transform_reverse_near_far(t, p, c, t->znear, t->zfar);
761 }
762
763 enum projection transform_get_projection(struct transformation *this_)
764 {
765 return this_->pro;
766 }
767
768 void transform_set_projection(struct transformation *this_, enum projection pro)
769 {
770 this_->pro = pro;
771 }
772
773 static int min4(int v1, int v2, int v3, int v4)
774 {
775 int res = v1;
776 if (v2 < res)
777 res = v2;
778 if (v3 < res)
779 res = v3;
780 if (v4 < res)
781 res = v4;
782 return res;
783 }
784
785 static int max4(int v1, int v2, int v3, int v4)
786 {
787 int res = v1;
788 if (v2 > res)
789 res = v2;
790 if (v3 > res)
791 res = v3;
792 if (v4 > res)
793 res = v4;
794 return res;
795 }
796
797 struct map_selection *
798 transform_get_selection(struct transformation *this_, enum projection pro, int order)
799 {
800
801 struct map_selection *ret, *curri, *curro;
802 struct coord_geo g;
803
804 ret = map_selection_dup(this_->map_sel);
805 curri = this_->map_sel;
806 curro = ret;
807 while (curri)
808 {
809 if (this_->pro != pro)
810 {
811 transform_to_geo(this_->pro, &curri->u.c_rect.lu, &g);
812 transform_from_geo(pro, &g, &curro->u.c_rect.lu);
813 // dbg(1,"%f,%f", g.lat, g.lng);
814 transform_to_geo(this_->pro, &curri->u.c_rect.rl, &g);
815 transform_from_geo(pro, &g, &curro->u.c_rect.rl);
816 // dbg(1,": - %f,%f\n", g.lat, g.lng);
817 }
818 // dbg(1,"transform rect for %d is %d,%d - %d,%d\n", pro, curro->u.c_rect.lu.x, curro->u.c_rect.lu.y, curro->u.c_rect.rl.x, curro->u.c_rect.rl.y);
819 curro->order += order;
820 #if 0
821 curro->u.c_rect.lu.x-=500;
822 curro->u.c_rect.lu.y+=500;
823 curro->u.c_rect.rl.x+=500;
824 curro->u.c_rect.rl.y-=500;
825 #endif
826 curro->range = item_range_all;
827 curri = curri->next;
828 curro = curro->next;
829 }
830 return ret;
831 }
832
833 struct coord *
834 transform_center(struct transformation *this_)
835 {
836 return &this_->map_center;
837 }
838
839 struct coord *
840 transform_get_center(struct transformation *this_)
841 {
842 return &this_->map_center;
843 }
844
845 void transform_set_center(struct transformation *this_, struct coord *c)
846 {
847 this_->map_center = *c;
848 }
849
850 void transform_set_yaw(struct transformation *t, int yaw)
851 {
852 t->yaw = yaw;
853 transform_setup_matrix(t);
854 }
855
856 int transform_get_yaw(struct transformation *this_)
857 {
858 return this_->yaw;
859 }
860
861 void transform_set_pitch(struct transformation *this_, int pitch)
862 {
863 this_->pitch = pitch;
864 transform_setup_matrix(this_);
865 }
866 int transform_get_pitch(struct transformation *this_)
867 {
868 return this_->pitch;
869 }
870
871 void transform_set_roll(struct transformation *this_, int roll)
872 {
873 #ifdef ENABLE_ROLL
874 this_->roll=roll;
875 transform_setup_matrix(this_);
876 #else
877 //dbg(0, "not supported\n");
878 #endif
879 }
880
881 int transform_get_roll(struct transformation *this_)
882 {
883 #ifdef ENABLE_ROLL
884 return this_->roll;
885 #else
886 return 0;
887 #endif
888 }
889
890 void transform_set_distance(struct transformation *this_, int distance)
891 {
892 transform_set_screen_dist(this_, distance);
893 transform_setup_matrix(this_);
894 }
895
896 int transform_get_distance(struct transformation *this_)
897 {
898 return this_->screen_dist;
899 }
900
901 void transform_set_scales(struct transformation *this_, int xscale, int yscale, int wscale)
902 {
903 this_->xscale3d = xscale;
904 this_->yscale3d = yscale;
905 this_->wscale3d = wscale;
906 }
907
908 void transform_set_screen_selection(struct transformation *t, struct map_selection *sel)
909 {
910 map_selection_destroy(t->screen_sel);
911 t->screen_sel = map_selection_dup(sel);
912 if (sel)
913 {
914 t->screen_center.x = (sel->u.p_rect.rl.x - sel->u.p_rect.lu.x) / 2;
915 t->screen_center.y = (sel->u.p_rect.rl.y - sel->u.p_rect.lu.y) / 2;
916 transform_setup_matrix(t);
917 }
918 }
919
920 void transform_set_screen_center(struct transformation *t, struct point *p)
921 {
922 t->screen_center = *p;
923 }
924
925 #if 0
926 void
927 transform_set_size(struct transformation *t, int width, int height)
928 {
929 t->width=width;
930 t->height=height;
931 }
932 #endif
933
934 void transform_get_size(struct transformation *t, int *width, int *height)
935 {
936 struct point_rect *r;
937 if (t->screen_sel)
938 {
939 r = &t->screen_sel->u.p_rect;
940 *width = r->rl.x - r->lu.x;
941 *height = r->rl.y - r->lu.y;
942 }
943 }
944
945 void transform_setup(struct transformation *t, struct pcoord *c, int scale, int yaw)
946 {
947 t->pro = c->pro;
948 t->map_center.x = c->x;
949 t->map_center.y = c->y;
950 t->scale = scale / 16.0;
951 transform_set_yaw(t, yaw);
952 }
953
954 #if 0
955
956 void
957 transform_setup_source_rect_limit(struct transformation *t, struct coord *center, int limit)
958 {
959 t->center=*center;
960 t->scale=1;
961 t->angle=0;
962 t->r.lu.x=center->x-limit;
963 t->r.rl.x=center->x+limit;
964 t->r.rl.y=center->y-limit;
965 t->r.lu.y=center->y+limit;
966 }
967 #endif
968
969 void transform_setup_source_rect(struct transformation *t)
970 {
971 int i;
972 struct coord screen[4];
973 struct point screen_pnt[4];
974 struct point_rect *pr;
975 struct map_selection *ms, *msm, *next, **msm_last;
976 ms = t->map_sel;
977 while (ms)
978 {
979 next = ms->next;
980 g_free(ms);
981 ms = next;
982 }
983 t->map_sel = NULL;
984 msm_last = &t->map_sel;
985 ms = t->screen_sel;
986 while (ms)
987 {
988 msm=g_new0(struct map_selection, 1);
989 *msm = *ms;
990 pr = &ms->u.p_rect;
991 screen_pnt[0].x = pr->lu.x; /* left upper */
992 screen_pnt[0].y = pr->lu.y;
993 screen_pnt[1].x = pr->rl.x; /* right upper */
994 screen_pnt[1].y = pr->lu.y;
995 screen_pnt[2].x = pr->rl.x; /* right lower */
996 screen_pnt[2].y = pr->rl.y;
997 screen_pnt[3].x = pr->lu.x; /* left lower */
998 screen_pnt[3].y = pr->rl.y;
999 if (t->ddd)
1000 {
1001 struct coord_geo_cart tmp, cg[8];
1002 struct coord c;
1003 int valid = 0;
1004 unsigned char edgenodes[] = { 0, 1, 1, 2, 2, 3, 3, 0, 4, 5, 5, 6, 6, 7, 7, 4, 0, 4, 1, 5, 2, 6, 3, 7 };
1005 for (i = 0; i < 8; i++)
1006 {
1007 transform_screen_to_3d(t, &screen_pnt[i % 4], (i >= 4 ? t->zfar : t->znear), &tmp);
1008 transform_apply_inverse_matrix(t, &tmp, &cg[i]);
1009 }
1010 msm->u.c_rect.lu.x = 0;
1011 msm->u.c_rect.lu.y = 0;
1012 msm->u.c_rect.rl.x = 0;
1013 msm->u.c_rect.rl.y = 0;
1014 for (i = 0; i < 12; i++)
1015 {
1016 if (transform_zplane_intersection(&cg[edgenodes[i * 2]], &cg[edgenodes[i * 2 + 1]], HOG(*t), &tmp) == 1)
1017 {
1018 c.x = tmp.x * (1 << t->scale_shift) + t->map_center.x;
1019 c.y = tmp.y * (1 << t->scale_shift) + t->map_center.y;
1020 //dbg(1,"intersection with edge %d at 0x%x,0x%x\n",i,c.x,c.y);
1021 if (valid)
1022 coord_rect_extend(&msm->u.c_rect, &c);
1023 else
1024 {
1025 msm->u.c_rect.lu = c;
1026 msm->u.c_rect.rl = c;
1027 valid = 1;
1028 }
1029 //dbg(1,"rect 0x%x,0x%x - 0x%x,0x%x\n",msm->u.c_rect.lu.x,msm->u.c_rect.lu.y,msm->u.c_rect.rl.x,msm->u.c_rect.rl.y);
1030 }
1031 }
1032 }
1033 else
1034 {
1035 for (i = 0; i < 4; i++)
1036 {
1037 transform_reverse(t, &screen_pnt[i], &screen[i]);
1038 //dbg(1,"map(%d) %d,%d=0x%x,0x%x\n", i,screen_pnt[i].x, screen_pnt[i].y, screen[i].x, screen[i].y);
1039 }
1040 msm->u.c_rect.lu.x = min4(screen[0].x, screen[1].x, screen[2].x, screen[3].x);
1041 msm->u.c_rect.rl.x = max4(screen[0].x, screen[1].x, screen[2].x, screen[3].x);
1042 msm->u.c_rect.rl.y = min4(screen[0].y, screen[1].y, screen[2].y, screen[3].y);
1043 msm->u.c_rect.lu.y = max4(screen[0].y, screen[1].y, screen[2].y, screen[3].y);
1044 }
1045 //dbg(1,"%dx%d\n", msm->u.c_rect.rl.x-msm->u.c_rect.lu.x,
1046 // msm->u.c_rect.lu.y-msm->u.c_rect.rl.y);
1047 *msm_last = msm;
1048 msm_last = &msm->next;
1049 ms = ms->next;
1050 }
1051 }
1052
1053 long transform_get_scale(struct transformation *t)
1054 {
1055 return (int) (t->scale * 16);
1056 }
1057
1058 void transform_set_scale(struct transformation *t, long scale)
1059 {
1060 t->scale = scale / 16.0;
1061 transform_setup_matrix(t);
1062 }
1063
1064 int transform_get_order(struct transformation *t)
1065 {
1066 //dbg(1,"order %d\n", t->order);
1067 return t->order;
1068 }
1069
1070 #define TWOPI (M_PI*2)
1071 #define GC2RAD(c) ((c) * TWOPI/(1<<24))
1072 #define minf(a,b) ((a) < (b) ? (a) : (b))
1073
1074 static double transform_distance_garmin(struct coord *c1, struct coord *c2)
1075 {
1076 #ifdef USE_HALVESINE
1077 static const int earth_radius = 6371*1000; //m change accordingly
1078 // static const int earth_radius = 3960; //miles
1079
1080 //Point 1 cords
1081 navit_float lat1 = GC2RAD(c1->y);
1082 navit_float long1 = GC2RAD(c1->x);
1083
1084 //Point 2 cords
1085 navit_float lat2 = GC2RAD(c2->y);
1086 navit_float long2 = GC2RAD(c2->x);
1087
1088 //Haversine Formula
1089 navit_float dlong = long2-long1;
1090 navit_float dlat = lat2-lat1;
1091
1092 navit_float sinlat = navit_sin(dlat/2);
1093 navit_float sinlong = navit_sin(dlong/2);
1094
1095 navit_float a=(sinlat*sinlat)+navit_cos(lat1)*navit_cos(lat2)*(sinlong*sinlong);
1096 navit_float c=2*navit_asin(minf(1,navit_sqrt(a)));
1097 #ifdef AVOID_FLOAT
1098 return round(earth_radius*c);
1099 #else
1100 return earth_radius*c;
1101 #endif
1102 #else
1103 #define GMETER 2.3887499999999999
1104 navit_float dx, dy;
1105 dx = c1->x - c2->x;
1106 dy = c1->y - c2->y;
1107 return navit_sqrt(dx * dx + dy * dy) * GMETER;
1108 #undef GMETER
1109 #endif
1110 }
1111
1112 double transform_scale(int y)
1113 {
1114 struct coord c;
1115 struct coord_geo g;
1116 c.x = 0;
1117 c.y = y;
1118 transform_to_geo(projection_mg, &c, &g);
1119 return 1 / navit_cos(g.lat / 180 * M_PI);
1120 }
1121
1122 #ifdef AVOID_FLOAT
1123 static int
1124 tab_sqrt[]=
1125 { 14142,13379,12806,12364,12018,11741,11517,11333,11180,11051,10943,10850,10770,10701,10640,10587,10540,10499,10462,10429,10400,10373,10349,10327,10307,10289,10273,10257,10243,10231,10219,10208};
1126
1127 static int tab_int_step = 0x20000;
1128 static int tab_int_scale[]=
1129 { 10000,10002,10008,10019,10033,10052,10076,10103,10135,10171,10212,10257,10306,10359,10417,10479,10546,10617,10693,10773,10858,10947,11041,11140,11243,11352,11465,11582,11705,11833,11965,12103,12246,12394,12547,12706,12870,13039,13214,13395,13581,13773,13971,14174,14384,14600,14822,15050,15285,15526,15774,16028,16289,16557,16832,17114,17404,17700,18005,18316,18636,18964,19299,19643,19995,20355,20724,21102,21489,21885,22290,22705,23129,23563,24007,24461,24926,25401,25886,26383,26891};
1130
1131 int transform_int_scale(int y)
1132 {
1133 int i,size = sizeof(tab_int_scale)/sizeof(int);
1134 if (y < 0)
1135 y=-y;
1136 i=y/tab_int_step;
1137 if (i < size-1)
1138 return tab_int_scale[i]+((tab_int_scale[i+1]-tab_int_scale[i])*(y-i*tab_int_step))/tab_int_step;
1139 return tab_int_scale[size-1];
1140 }
1141 #endif
1142
1143 double transform_distance(enum projection pro, struct coord *c1, struct coord *c2)
1144 {
1145 if (pro == projection_mg)
1146 {
1147 #ifndef AVOID_FLOAT
1148 double dx, dy, scale = transform_scale((c1->y + c2->y) / 2);
1149 dx = c1->x - c2->x;
1150 dy = c1->y - c2->y;
1151 return sqrt(dx * dx + dy * dy) / scale;
1152 #else
1153 int dx,dy,f,scale=transform_int_scale((c1->y+c2->y)/2);
1154 dx=c1->x-c2->x;
1155 dy=c1->y-c2->y;
1156 if (dx < 0)
1157 dx=-dx;
1158 if (dy < 0)
1159 dy=-dy;
1160 while (dx > 20000 || dy > 20000)
1161 {
1162 dx/=10;
1163 dy/=10;
1164 scale/=10;
1165 }
1166 if (! dy)
1167 return dx*10000/scale;
1168 if (! dx)
1169 return dy*10000/scale;
1170 if (dx > dy)
1171 {
1172 f=dx*8/dy-8;
1173 if (f >= 32)
1174 return dx*10000/scale;
1175 return dx*tab_sqrt[f]/scale;
1176 }
1177 else
1178 {
1179 f=dy*8/dx-8;
1180 if (f >= 32)
1181 return dy*10000/scale;
1182 return dy*tab_sqrt[f]/scale;
1183 }
1184 #endif
1185 }
1186 else if (pro == projection_garmin)
1187 {
1188 return transform_distance_garmin(c1, c2);
1189 }
1190 else
1191 {
1192 dbg(0, "Unknown projection: %d\n", pro);
1193 return 0;
1194 }
1195 }
1196
1197 void transform_project(enum projection pro, struct coord *c, int distance, int angle, struct coord *res)
1198 {
1199 double scale;
1200 switch (pro)
1201 {
1202 case projection_mg:
1203 scale = transform_scale(c->y);
1204 res->x = c->x + distance * sin(angle * M_PI / 180) * scale;
1205 res->y = c->y + distance * cos(angle * M_PI / 180) * scale;
1206 break;
1207 default:
1208 dbg(0, "Unsupported projection: %d\n", pro);
1209 return;
1210 }
1211
1212 }
1213
1214 double transform_polyline_length(enum projection pro, struct coord *c, int count)
1215 {
1216 double ret = 0;
1217 int i;
1218
1219 for (i = 0; i < count - 1; i++)
1220 ret += transform_distance(pro, &c[i], &c[i + 1]);
1221 return ret;
1222 }
1223
1224 // calc the distance (squared) of a point (p) to a line segment (l1 .. l2)
1225 // return (int) distance squared
1226 int transform_distance_point2line_sq(struct coord *p, struct coord *l1, struct coord *l2)
1227 {
1228 int A = p->x - l1->x;
1229 int B = p->y - l1->y;
1230 float C = l2->x - l1->x;
1231 float D = l2->y - l1->y;
1232
1233 int dot = A * C + B * D;
1234 int len_sq = C * C + D * D;
1235 float param = (float) dot / (float) len_sq;
1236
1237 int xx, yy;
1238
1239 if (param < 0 || (l1->x == l2->x && l1->y == l2->y))
1240 {
1241 xx = l1->x;
1242 yy = l1->y;
1243 }
1244 else if (param > 1)
1245 {
1246 xx = l2->x;
1247 yy = l2->y;
1248 }
1249 else
1250 {
1251 xx = l1->x + param * C;
1252 yy = l1->y + param * D;
1253 }
1254
1255 int dx = p->x - xx;
1256 int dy = p->y - yy;
1257
1258 if (dx > 32767 || dy > 32767 || dx < -32767 || dy < -32767)
1259 {
1260 return INT_MAX;
1261 }
1262
1263 return (dx * dx + dy * dy);
1264
1265 }
1266
1267 int transform_distance_sq(struct coord *c1, struct coord *c2)
1268 {
1269 int dx = c1->x - c2->x;
1270 int dy = c1->y - c2->y;
1271
1272 if (dx > 32767 || dy > 32767 || dx < -32767 || dy < -32767)
1273 return INT_MAX;
1274 else
1275 return dx * dx + dy * dy;
1276 }
1277
1278 navit_float transform_distance_sq_float(struct coord *c1, struct coord *c2)
1279 {
1280 int dx = c1->x - c2->x;
1281 int dy = c1->y - c2->y;
1282 return (navit_float) dx * dx + dy * dy;
1283 }
1284
1285 int transform_distance_sq_pc(struct pcoord *c1, struct pcoord *c2)
1286 {
1287 struct coord p1, p2;
1288 p1.x = c1->x;
1289 p1.y = c1->y;
1290 p2.x = c2->x;
1291 p2.y = c2->y;
1292 return transform_distance_sq(&p1, &p2);
1293 }
1294
1295 int transform_distance_line_sq(struct coord *l0, struct coord *l1, struct coord *ref, struct coord *lpnt)
1296 {
1297 int vx, vy, wx, wy;
1298 int c1, c2;
1299 int climit = 1000000;
1300 struct coord l;
1301
1302 vx = l1->x - l0->x;
1303 vy = l1->y - l0->y;
1304 wx = ref->x - l0->x;
1305 wy = ref->y - l0->y;
1306
1307 c1 = vx * wx + vy * wy;
1308 if (c1 <= 0)
1309 {
1310 if (lpnt)
1311 *lpnt = *l0;
1312 return transform_distance_sq(l0, ref);
1313 }
1314 c2 = vx * vx + vy * vy;
1315 if (c2 <= c1)
1316 {
1317 if (lpnt)
1318 *lpnt = *l1;
1319 return transform_distance_sq(l1, ref);
1320 }
1321 while (c1 > climit || c2 > climit)
1322 {
1323 c1 /= 256;
1324 c2 /= 256;
1325 }
1326 l.x = l0->x + vx * c1 / c2;
1327 l.y = l0->y + vy * c1 / c2;
1328 if (lpnt)
1329 *lpnt = l;
1330 return transform_distance_sq(&l, ref);
1331 }
1332
1333 navit_float transform_distance_line_sq_float(struct coord *l0, struct coord *l1, struct coord *ref, struct coord *lpnt)
1334 {
1335 navit_float vx, vy, wx, wy;
1336 navit_float c1, c2;
1337 struct coord l;
1338
1339 vx = l1->x - l0->x;
1340 vy = l1->y - l0->y;
1341 wx = ref->x - l0->x;
1342 wy = ref->y - l0->y;
1343
1344 c1 = vx * wx + vy * wy;
1345 if (c1 <= 0)
1346 {
1347 if (lpnt)
1348 *lpnt = *l0;
1349 return transform_distance_sq_float(l0, ref);
1350 }
1351 c2 = vx * vx + vy * vy;
1352 if (c2 <= c1)
1353 {
1354 if (lpnt)
1355 *lpnt = *l1;
1356 return transform_distance_sq_float(l1, ref);
1357 }
1358 l.x = l0->x + vx * c1 / c2;
1359 l.y = l0->y + vy * c1 / c2;
1360 if (lpnt)
1361 *lpnt = l;
1362 return transform_distance_sq_float(&l, ref);
1363 }
1364
1365 int transform_distance_polyline_sq__v2(struct coord *c, int count, struct coord *ref)
1366 {
1367 int i, dist, distn;
1368
1369 if (count < 2)
1370 {
1371 int d;
1372 d = transform_distance_sq(&c[0], ref);
1373 //dbg(0,"d=%d\n", d);
1374 return d;
1375 }
1376
1377 dist = transform_distance_point2line_sq(ref, &c[0], &c[1]);
1378 //dbg(0,"dist1:%d\n", dist);
1379
1380 for (i = 2; i < count; i++)
1381 {
1382 distn = transform_distance_point2line_sq(ref, &c[i - 1], &c[i]);
1383 //dbg(0,"dist2:%d\n", dist);
1384 if (distn < dist)
1385 {
1386 dist = distn;
1387 }
1388 }
1389 //dbg(0,"dist final:%d\n", dist);
1390 return dist;
1391 }
1392
1393 int transform_distance_polyline_sq(struct coord *c, int count, struct coord *ref, struct coord *lpnt, int *pos)
1394 {
1395 int i, dist, distn;
1396 struct coord lp;
1397 if (count < 2)
1398 {
1399 // dbg(0,"1\n");
1400 return INT_MAX;
1401 }
1402 if (pos)
1403 {
1404 *pos = 0;
1405 }
1406
1407 dist = transform_distance_line_sq(&c[0], &c[1], ref, lpnt);
1408 // dbg(0,"dist:%d\n", dist);
1409
1410 for (i = 2; i < count; i++)
1411 {
1412 distn = transform_distance_line_sq(&c[i - 1], &c[i], ref, &lp);
1413 if (distn < dist)
1414 {
1415 dist = distn;
1416 if (lpnt)
1417 {
1418 *lpnt = lp;
1419 }
1420 if (pos)
1421 {
1422 *pos = i - 1;
1423 }
1424 }
1425 }
1426 return dist;
1427 }
1428
1429 int transform_douglas_peucker(struct coord *in, int count, int dist_sq, struct coord *out)
1430 {
1431 int ret = 0;
1432 int i, d, dmax = 0, idx = 0;
1433 for (i = 1; i < count - 2; i++)
1434 {
1435 d = transform_distance_line_sq(&in[0], &in[count - 1], &in[i], NULL);
1436 if (d > dmax)
1437 {
1438 idx = i;
1439 dmax = d;
1440 }
1441 }
1442 if (dmax > dist_sq)
1443 {
1444 ret = transform_douglas_peucker(in, idx, dist_sq, out) - 1;
1445 ret += transform_douglas_peucker(in + idx, count - idx, dist_sq, out + ret);
1446 }
1447 else
1448 {
1449 if (count > 0)
1450 out[ret++] = in[0];
1451 if (count > 1)
1452 out[ret++] = in[count - 1];
1453 }
1454 return ret;
1455 }
1456
1457 int transform_douglas_peucker_float(struct coord *in, int count, navit_float dist_sq, struct coord *out)
1458 {
1459 int ret = 0;
1460 int i, idx = 0;
1461 navit_float d, dmax = 0;
1462 for (i = 1; i < count - 2; i++)
1463 {
1464 d = transform_distance_line_sq_float(&in[0], &in[count - 1], &in[i], NULL);
1465 if (d > dmax)
1466 {
1467 idx = i;
1468 dmax = d;
1469 }
1470 }
1471 if (dmax > dist_sq)
1472 {
1473 ret = transform_douglas_peucker_float(in, idx, dist_sq, out) - 1;
1474 ret += transform_douglas_peucker_float(in + idx, count - idx, dist_sq, out + ret);
1475 }
1476 else
1477 {
1478 if (count > 0)
1479 out[ret++] = in[0];
1480 if (count > 1)
1481 out[ret++] = in[count - 1];
1482 }
1483 return ret;
1484 }
1485
1486 void transform_print_deg(double deg)
1487 {
1488 printf("%2.0f:%2.0f:%2.4f", floor(deg), fmod(deg * 60, 60), fmod(deg * 3600, 60));
1489 }
1490
1491 #ifdef AVOID_FLOAT
1492 static int tab_atan[]=
1493 { 0,262,524,787,1051,1317,1584,1853,2126,2401,2679,2962,3249,3541,3839,4142,4452,4770,5095,5430,5774,6128,6494,6873,7265,7673,8098,8541,9004,9490,10000,10538};
1494
1495 static int
1496 atan2_int_lookup(int val)
1497 {
1498 int len=sizeof(tab_atan)/sizeof(int);
1499 int i=len/2;
1500 int p=i-1;
1501 for (;;)
1502 {
1503 i>>=1;
1504 if (val < tab_atan[p])
1505 p-=i;
1506 else
1507 if (val < tab_atan[p+1])
1508 return p+(p>>1);
1509 else
1510 p+=i;
1511 }
1512 }
1513
1514 static int
1515 atan2_int(int dx, int dy)
1516 {
1517 int mul=1,add=0,ret;
1518 if (! dx)
1519 {
1520 return dy < 0 ? 180 : 0;
1521 }
1522 if (! dy)
1523 {
1524 return dx < 0 ? -90 : 90;
1525 }
1526 if (dx < 0)
1527 {
1528 dx=-dx;
1529 mul=-1;
1530 }
1531 if (dy < 0)
1532 {
1533 dy=-dy;
1534 add=180*mul;
1535 mul*=-1;
1536 }
1537 while (dx > 20000 || dy > 20000)
1538 {
1539 dx/=10;
1540 dy/=10;
1541 }
1542 if (dx > dy)
1543 {
1544 ret=90-atan2_int_lookup(dy*10000/dx);
1545 }
1546 else
1547 {
1548 ret=atan2_int_lookup(dx*10000/dy);
1549 }
1550 return ret*mul+add;
1551 }
1552 #endif
1553
1554 int transform_get_angle_delta(struct coord *c1, struct coord *c2, int dir)
1555 {
1556 int dx = c2->x - c1->x;
1557 int dy = c2->y - c1->y;
1558 #ifndef AVOID_FLOAT
1559 double angle;
1560 angle = atan2(dx, dy);
1561 angle *= 180 / M_PI;
1562 #else
1563 int angle;
1564 angle=atan2_int(dx,dy);
1565 #endif
1566 if (dir == -1)
1567 angle = angle - 180;
1568 if (angle < 0)
1569 angle += 360;
1570 return angle;
1571 }
1572
1573 int transform_within_border(struct transformation *this_, struct point *p, int border)
1574 {
1575 struct map_selection *ms = this_->screen_sel;
1576 while (ms)
1577 {
1578 struct point_rect *r = &ms->u.p_rect;
1579 if (p->x >= r->lu.x + border && p->x <= r->rl.x - border && p->y >= r->lu.y + border && p->y <= r->rl.y - border)
1580 return 1;
1581 ms = ms->next;
1582 }
1583 return 0;
1584 }
1585
1586 int transform_within_dist_point(struct coord *ref, struct coord *c, int dist)
1587 {
1588 if (c->x - dist > ref->x)
1589 return 0;
1590 if (c->x + dist < ref->x)
1591 return 0;
1592 if (c->y - dist > ref->y)
1593 return 0;
1594 if (c->y + dist < ref->y)
1595 return 0;
1596 if ((c->x - ref->x) * (c->x - ref->x) + (c->y - ref->y) * (c->y - ref->y) <= dist * dist)
1597 return 1;
1598 return 0;
1599 }
1600
1601 int transform_within_dist_line(struct coord *ref, struct coord *c0, struct coord *c1, int dist)
1602 {
1603 int vx, vy, wx, wy;
1604 int n1, n2;
1605 struct coord lc;
1606
1607 if (c0->x < c1->x)
1608 {
1609 if (c0->x - dist > ref->x)
1610 return 0;
1611 if (c1->x + dist < ref->x)
1612 return 0;
1613 }
1614 else
1615 {
1616 if (c1->x - dist > ref->x)
1617 return 0;
1618 if (c0->x + dist < ref->x)
1619 return 0;
1620 }
1621 if (c0->y < c1->y)
1622 {
1623 if (c0->y - dist > ref->y)
1624 return 0;
1625 if (c1->y + dist < ref->y)
1626 return 0;
1627 }
1628 else
1629 {
1630 if (c1->y - dist > ref->y)
1631 return 0;
1632 if (c0->y + dist < ref->y)
1633 return 0;
1634 }
1635 vx = c1->x - c0->x;
1636 vy = c1->y - c0->y;
1637 wx = ref->x - c0->x;
1638 wy = ref->y - c0->y;
1639
1640 n1 = vx * wx + vy * wy;
1641 if (n1 <= 0)
1642 return transform_within_dist_point(ref, c0, dist);
1643 n2 = vx * vx + vy * vy;
1644 if (n2 <= n1)
1645 return transform_within_dist_point(ref, c1, dist);
1646
1647 lc.x = c0->x + vx * n1 / n2;
1648 lc.y = c0->y + vy * n1 / n2;
1649 return transform_within_dist_point(ref, &lc, dist);
1650 }
1651
1652 int transform_within_dist_polyline(struct coord *ref, struct coord *c, int count, int close, int dist)
1653 {
1654 int i;
1655 for (i = 0; i < count - 1; i++)
1656 {
1657 if (transform_within_dist_line(ref, c + i, c + i + 1, dist))
1658 {
1659 return 1;
1660 }
1661 }
1662 if (close)
1663 return (transform_within_dist_line(ref, c, c + count - 1, dist));
1664 return 0;
1665 }
1666
1667 int transform_within_dist_polygon(struct coord *ref, struct coord *c, int count, int dist)
1668 {
1669 int i, j, ci = 0;
1670 for (i = 0, j = count - 1; i < count; j = i++)
1671 {
1672 if ((((c[i].y <= ref->y) && (ref->y < c[j].y)) || ((c[j].y <= ref->y) && (ref->y < c[i].y))) && (ref->x < (c[j].x - c[i].x) * (ref->y - c[i].y) / (c[j].y - c[i].y) + c[i].x))
1673 ci = !ci;
1674 }
1675 if (!ci)
1676 {
1677 if (dist)
1678 return transform_within_dist_polyline(ref, c, count, dist, 1);
1679 else
1680 return 0;
1681 }
1682 return 1;
1683 }
1684
1685 int transform_within_dist_item(struct coord *ref, enum item_type type, struct coord *c, int count, int dist)
1686 {
1687 if (type < type_line)
1688 return transform_within_dist_point(ref, c, dist);
1689 if (type < type_area)
1690 return transform_within_dist_polyline(ref, c, count, 0, dist);
1691 return transform_within_dist_polygon(ref, c, count, dist);
1692 }
1693
1694 void transform_copy(struct transformation *src, struct transformation *dst)
1695 {
1696 memcpy(dst, src, sizeof(*src));
1697 }
1698
1699 void transform_destroy(struct transformation *t)
1700 {
1701 g_free(t);
1702 }
1703
1704 /*
1705 Note: there are many mathematically equivalent ways to express these formulas. As usual, not all of them are computationally equivalent.
1706
1707 L = latitude in radians (positive north)
1708 Lo = longitude in radians (positive east)
1709 E = easting (meters)
1710 N = northing (meters)
1711
1712 For the sphere
1713
1714 E = r Lo
1715 N = r ln [ tan (pi/4 + L/2) ]
1716
1717 where
1718
1719 r = radius of the sphere (meters)
1720 ln() is the natural logarithm
1721
1722 For the ellipsoid
1723
1724 E = a Lo
1725 N = a * ln ( tan (pi/4 + L/2) * ( (1 - e * sin (L)) / (1 + e * sin (L))) ** (e/2) )
1726
1727
1728 e
1729 -
1730 pi L 1 - e sin(L) 2
1731 = a ln( tan( ---- + ---) (--------------) )
1732 4 2 1 + e sin(L)
1733
1734
1735 where
1736
1737 a = the length of the semi-major axis of the ellipsoid (meters)
1738 e = the first eccentricity of the ellipsoid
1739
1740
1741 */
1742
1743 void transform_init(void)
1744 {
1745 #if 0
1746 if (global_transform_hash == NULL)
1747 {
1748 dbg(0,"enter\n");
1749 global_transform_hash=g_hash_table_new_full(g_int_hash, g_int_equal, NULL, g_free_func);
1750 dbg(0,"leave\n");
1751 }
1752 if (global_transform_hash2 == NULL)
1753 {
1754 dbg(0,"enter\n");
1755 global_transform_hash2=g_hash_table_new_full(g_int_hash, g_int_equal, NULL, g_free_func);
1756 dbg(0,"leave\n");
1757 }
1758 #endif
1759 }
1760

   
Visit the ZANavi Wiki