1 /**********************************************************************************************
2 Copyright (C) 2009 Oliver Eichler <oliver.eichler@gmx.de>
3
4 This program is free software; you can redistribute it and/or modify
5 it under the terms of the GNU General Public License as published by
6 the Free Software Foundation; either version 2 of the License, or
7 (at your option) any later version.
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 Free Software
16 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
17
18 **********************************************************************************************/
19
20 #include "canvas/IDrawContext.h"
21 #include "gis/GeoMath.h"
22 #include "units/IUnit.h"
23 #include <stdlib.h>
24
25 #include <QtGui>
26 #include <QtWidgets>
27
28 #define PI M_PI
29 #define TWOPI (2 * PI)
30
pointDP()31 pointDP::pointDP() : used(true), idx(NOIDX)
32 {
33 }
34
pointDP(const qreal & x,const qreal & y,const qreal & z)35 pointDP::pointDP(const qreal &x, const qreal &y, const qreal &z)
36 : point3D(x, y, z), used(true), idx(NOIDX)
37 {
38 }
39
distance(const QPointF & pa,const QPointF & pb)40 static inline qreal distance(const QPointF &pa, const QPointF &pb)
41 {
42 const qreal &dx = pa.x() - pb.x();
43 const qreal &dy = pa.y() - pb.y();
44 return qSqrt(dx * dx + dy * dy);
45 }
46
GPS_Math_DegMinSec_To_Deg(bool sign,const qint32 d,const qint32 m,const qreal s,qreal & deg)47 void GPS_Math_DegMinSec_To_Deg(bool sign, const qint32 d, const qint32 m, const qreal s, qreal °)
48 {
49 deg = qAbs(d) + qreal(m) / 60.0 + s / 3600;
50 if(sign)
51 {
52 deg = -deg;
53 }
54 }
55
56
GPS_Math_Deg_To_DegMin(qreal v,qint32 * deg,qreal * min)57 bool GPS_Math_Deg_To_DegMin(qreal v, qint32 *deg, qreal *min)
58 {
59 *deg = qAbs(v);
60 *min = (qAbs(v) - *deg) * 60.0;
61
62 return v < 0;
63 }
64
65
GPS_Math_DegMin_To_Deg(bool sign,const qint32 d,const qreal m,qreal & deg)66 void GPS_Math_DegMin_To_Deg(bool sign, const qint32 d, const qreal m, qreal& deg)
67 {
68 deg = qAbs(d) + m / 60.0;
69 if(sign)
70 {
71 deg = -deg;
72 }
73 }
74
75
76 // from http://www.movable-type.co.uk/scripts/LatLongVincenty.html
GPS_Math_Distance(const qreal u1,const qreal v1,const qreal u2,const qreal v2,qreal & a1,qreal & a2)77 qreal GPS_Math_Distance(const qreal u1, const qreal v1, const qreal u2, const qreal v2, qreal& a1, qreal& a2)
78 {
79 qreal cosSigma = 0.0;
80 qreal sigma = 0.0;
81 qreal sinAlpha = 0.0;
82 qreal cosSqAlpha = 0.0;
83 qreal cos2SigmaM = 0.0;
84 qreal sinSigma = 0.0;
85 qreal sinLambda = 0.0;
86 qreal cosLambda = 0.0;
87
88 qreal a = 6378137.0, b = 6356752.3142, f = 1.0 / 298.257223563; // WGS-84 ellipsiod
89 qreal L = u2 - u1;
90
91 qreal U1 = qAtan((1 - f) * qTan(v1));
92 qreal U2 = qAtan((1 - f) * qTan(v2));
93 qreal sinU1 = qSin(U1), cosU1 = qCos(U1);
94 qreal sinU2 = qSin(U2), cosU2 = qCos(U2);
95 qreal lambda = L, lambdaP = 2 * PI;
96 unsigned iterLimit = 20;
97
98 while ((qAbs(lambda - lambdaP) > 1e-12) && (--iterLimit > 0))
99 {
100 sinLambda = qSin(lambda);
101 cosLambda = qCos(lambda);
102 sinSigma = qSqrt((cosU2 * sinLambda) * (cosU2 * sinLambda) + (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) * (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda));
103
104 if (sinSigma == 0)
105 {
106 return 0; // co-incident points
107 }
108
109 cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
110 sigma = qAtan2(sinSigma, cosSigma);
111 sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
112 cosSqAlpha = 1 - sinAlpha * sinAlpha;
113 cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;
114
115 if (qIsNaN(cos2SigmaM))
116 {
117 cos2SigmaM = 0; // equatorial line: cosSqAlpha=0 (§6)
118 }
119
120 qreal C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
121 lambdaP = lambda;
122 lambda = L + (1 - C) * f * sinAlpha * (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)));
123 }
124 if (iterLimit == 0)
125 {
126 return FP_NAN; // formula failed to converge
127 }
128 qreal uSq = cosSqAlpha * (a * a - b * b) / (b * b);
129 qreal A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
130 qreal B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
131 qreal deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) - B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM)));
132 qreal s = b * A * (sigma - deltaSigma);
133
134 a1 = qAtan2(cosU2 * sinLambda, cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) * 360 / TWOPI;
135 a2 = qAtan2(cosU1 * sinLambda, -sinU1 * cosU2 + cosU1 * sinU2 * cosLambda) * 360 / TWOPI;
136 return s;
137 }
138
GPS_Math_Distance(const qreal u1,const qreal v1,const qreal u2,const qreal v2)139 qreal GPS_Math_Distance(const qreal u1, const qreal v1, const qreal u2, const qreal v2)
140 {
141 qreal cosSigma = 0.0;
142 qreal sigma = 0.0;
143 qreal sinAlpha = 0.0;
144 qreal cosSqAlpha = 0.0;
145 qreal cos2SigmaM = 0.0;
146 qreal sinSigma = 0.0;
147 qreal sinLambda = 0.0;
148 qreal cosLambda = 0.0;
149
150 qreal a = 6378137.0, b = 6356752.3142, f = 1.0 / 298.257223563; // WGS-84 ellipsiod
151 qreal L = u2 - u1;
152
153 qreal U1 = qAtan((1 - f) * qTan(v1));
154 qreal U2 = qAtan((1 - f) * qTan(v2));
155 qreal sinU1 = qSin(U1), cosU1 = qCos(U1);
156 qreal sinU2 = qSin(U2), cosU2 = qCos(U2);
157 qreal lambda = L, lambdaP = 2 * PI;
158 unsigned iterLimit = 20;
159
160 while ((qAbs(lambda - lambdaP) > 1e-12) && (--iterLimit > 0))
161 {
162 sinLambda = qSin(lambda);
163 cosLambda = qCos(lambda);
164 sinSigma = qSqrt((cosU2 * sinLambda) * (cosU2 * sinLambda) + (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) * (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda));
165
166 if (sinSigma == 0)
167 {
168 return 0; // co-incident points
169 }
170
171 cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
172 sigma = qAtan2(sinSigma, cosSigma);
173 sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
174 cosSqAlpha = 1 - sinAlpha * sinAlpha;
175 cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;
176
177 if (qIsNaN(cos2SigmaM))
178 {
179 cos2SigmaM = 0; // equatorial line: cosSqAlpha=0 (§6)
180 }
181
182 qreal C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
183 lambdaP = lambda;
184 lambda = L + (1 - C) * f * sinAlpha * (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)));
185 }
186 if (iterLimit == 0)
187 {
188 return FP_NAN; // formula failed to converge
189 }
190 qreal uSq = cosSqAlpha * (a * a - b * b) / (b * b);
191 qreal A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
192 qreal B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
193 qreal deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) - B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM)));
194 qreal s = b * A * (sigma - deltaSigma);
195
196 return s;
197 }
198
GPS_Math_DistanceQuick(const qreal u1,const qreal v1,const qreal u2,const qreal v2)199 qreal GPS_Math_DistanceQuick(const qreal u1, const qreal v1, const qreal u2, const qreal v2)
200 {
201 qreal dU = u2 - u1; // lambda
202 qreal dV = v2 - v1; // roh
203
204 qreal d = 2 * qAsin(qSqrt(qSin(dV / 2) * qSin(dV / 2) + qCos(v1) * qCos(v2) * qSin(dU / 2) * qSin(dU / 2)));
205
206 return 6371010 * d;
207 }
208
209
GPS_Math_distPointLine3D(const point3D & x1,const point3D & x2,const point3D & x0)210 static qreal GPS_Math_distPointLine3D(const point3D &x1, const point3D &x2, const point3D &x0)
211 {
212 point3D v1, v2, v3, v1x2;
213
214 // (x0 - x1)
215 v1.x = x0.x - x1.x;
216 v1.y = x0.y - x1.y;
217 v1.z = x0.z - x1.z;
218
219 // (x0 - x2)
220 v2.x = x0.x - x2.x;
221 v2.y = x0.y - x2.y;
222 v2.z = x0.z - x2.z;
223
224 // (x2 - x1)
225 v3.x = x2.x - x1.x;
226 v3.y = x2.y - x1.y;
227 v3.z = x2.z - x1.z;
228
229 // (x0 - x1)x(x0 - x2)
230 v1x2.x = v1.y * v2.z - v1.z * v2.y;
231 v1x2.y = v1.z * v2.x - v1.x * v2.z;
232 v1x2.z = v1.x * v2.y - v1.y * v2.x;
233
234 // |(x0 - x1)x(x0 - x2)|
235 qreal a1x2 = v1x2.x * v1x2.x + v1x2.y * v1x2.y + v1x2.z * v1x2.z;
236 // |(x2 - x1)|
237 qreal a3 = v3.x * v3.x + v3.y * v3.y + v3.z * v3.z;
238
239 return qSqrt(a1x2 / a3);
240 }
241
sqr(qreal a)242 static inline qreal sqr(qreal a)
243 {
244 return a * a;
245 }
sqrlen(const QPointF & a)246 static inline qreal sqrlen(const QPointF &a)
247 {
248 return sqr(a.x()) + sqr(a.y());
249 }
250
251
GPS_Math_DistPointPolyline(const QPolygonF & points,const QPointF & q)252 qreal GPS_Math_DistPointPolyline(const QPolygonF &points, const QPointF &q)
253 {
254 const qint32 count = points.size();
255
256 if(count == 0)
257 {
258 return NOFLOAT;
259 }
260
261 QPointF b = points[0];
262 QPointF dbq = b - q;
263 qreal dist = sqrlen(dbq);
264
265 for (qint32 i = 1; i < count; ++i)
266 {
267 const QPointF a = b;
268 const QPointF daq = dbq;
269 b = points[i];
270 dbq = b - q;
271
272 const QPointF dab = a - b;
273
274 const qreal inv_sqrlen = 1. / sqrlen(dab);
275 const qreal t = (dab.x() * daq.x() + dab.y() * daq.y()) * inv_sqrlen;
276 if (t < 0.)
277 {
278 continue;
279 }
280 qreal current_dist;
281 if (t <= 1.)
282 {
283 current_dist = sqr(dab.x() * dbq.y() - dab.y() * dbq.x()) * inv_sqrlen;
284 }
285 else//t>1.
286 {
287 current_dist = sqrlen(dbq);
288 }
289 if (current_dist < dist)
290 {
291 dist = current_dist;
292 }
293 }
294 return dist;
295 }
296
GPS_Math_DistPointPolyline(const QPolygonF & line,const QPointF & pt,qreal threshold)297 qreal GPS_Math_DistPointPolyline(const QPolygonF& line, const QPointF& pt, qreal threshold)
298 {
299 qreal d = threshold + 1;
300
301 const int len = line.size();
302 // see http://local.wasp.uwa.edu.au/~pbourke/geometry/pointline/
303 for(int i = 1; i < len; ++i)
304 {
305 const QPointF &p1 = line[i - 1];
306 const QPointF &p2 = line[i];
307
308 qreal dx = p2.x() - p1.x();
309 qreal dy = p2.y() - p1.y();
310
311 // distance between p1 and p2
312 qreal d_p1_p2 = qSqrt(dx * dx + dy * dy);
313
314 // ratio u the tangent point will divide d_p1_p2
315 qreal u = ((pt.x() - p1.x()) * dx + (pt.y() - p1.y()) * dy) / (d_p1_p2 * d_p1_p2);
316
317 if(u < 0.0 || u > 1.0)
318 {
319 continue;
320 }
321
322 // coord. (x,y) of the point on line defined by [p1,p2] close to pt
323 qreal x = p1.x() + u * dx;
324 qreal y = p1.y() + u * dy;
325
326 qreal distance = qSqrt((x - pt.x()) * (x - pt.x()) + (y - pt.y()) * (y - pt.y()));
327 if(distance < threshold)
328 {
329 d = threshold = distance;
330 }
331 }
332
333 return d;
334 }
335
336
337 struct segment
338 {
segmentsegment339 segment() : idx1(0), idx2(0)
340 {
341 }
segmentsegment342 segment(qint32 idx1, quint32 idx2) : idx1(idx1), idx2(idx2)
343 {
344 }
345 qint32 idx1;
346 qint32 idx2;
347 };
348
GPS_Math_DouglasPeucker(QVector<pointDP> & line,qreal d)349 void GPS_Math_DouglasPeucker(QVector<pointDP> &line, qreal d)
350 {
351 if(line.count() < 3)
352 {
353 return;
354 }
355
356 QStack<segment> stack;
357 stack << segment(0, line.size() - 1);
358
359 while(!stack.isEmpty())
360 {
361 qint32 idx = NOIDX;
362 segment seg = stack.pop();
363
364 pointDP& x1 = line[seg.idx1];
365 pointDP& x2 = line[seg.idx2];
366
367 qreal dmax = d;
368 for(qint32 i = seg.idx1 + 1; i < seg.idx2; i++)
369 {
370 qreal distance = GPS_Math_distPointLine3D(x1, x2, line[i]);
371 if(distance > dmax)
372 {
373 idx = i;
374 dmax = distance;
375 }
376 }
377
378 if(idx > 0)
379 {
380 stack << segment(seg.idx1, idx);
381 stack << segment(idx, seg.idx2);
382 }
383 else
384 {
385 for(qint32 i = seg.idx1 + 1; i < seg.idx2; i++)
386 {
387 line[i].used = false;
388 }
389 }
390 }
391 }
392
393
GPS_Math_LineCrossesRect(const QPointF & p1,const QPointF & p2,const QRectF & rect)394 bool GPS_Math_LineCrossesRect(const QPointF &p1, const QPointF &p2, const QRectF &rect)
395 {
396 // the trivial case
397 if(rect.contains(p1) || rect.contains(p2))
398 {
399 return true;
400 }
401
402 qreal slope = qreal(p2.y() - p1.y()) / (p2.x() - p1.x());
403 qreal offset = p1.y() - slope * p1.x();
404 qreal y1 = offset + slope * rect.left();
405 qreal y2 = offset + slope * rect.right();
406
407 if((y1 < rect.top()) && (y2 < rect.top()))
408 {
409 return false;
410 }
411 else if((y1 > rect.bottom()) && (y2 > rect.bottom()))
412 {
413 return false;
414 }
415
416 return true;
417 }
418
GPS_Math_Wpt_Projection(const QPointF & p1,qreal distance,qreal bearing)419 QPointF GPS_Math_Wpt_Projection(const QPointF& p1, qreal distance, qreal bearing)
420 {
421 qreal Phi1 = p1.y();
422 qreal Lambda1 = p1.x();
423 qreal Alpha1 = bearing;
424 qreal s = distance;
425
426 const qreal a = 6378137.0;
427 const qreal b = 6356752.3142;
428 const qreal f = 1.0 / 298.257223563;
429
430 qreal sinAlpha1 = qSin(Alpha1);
431 qreal cosAlpha1 = qCos(Alpha1);
432
433 qreal tanU1 = (1 - f) * qTan(Phi1);
434 qreal cosU1 = 1 / qSqrt((1 + tanU1 * tanU1));
435 qreal sinU1 = tanU1 * cosU1;
436 qreal Sigma1 = qAtan2(tanU1, cosAlpha1);
437 qreal sinAlpha = cosU1 * sinAlpha1;
438 qreal cosSqAlpha = 1 - sinAlpha * sinAlpha;
439 qreal uSq = cosSqAlpha * (a * a - b * b) / (b * b);
440 qreal A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
441 qreal B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
442
443 qreal cos2SigmaM;
444 qreal sinSigma;
445 qreal cosSigma;
446 qreal deltaSigma;
447
448 qreal Sigma = s / (b * A);
449 qreal Sigma_;
450 int iterations = 0;
451 do
452 {
453 cos2SigmaM = qCos(2 * Sigma1 + Sigma);
454 sinSigma = qSin(Sigma);
455 cosSigma = qCos(Sigma);
456 deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) -
457 B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM)));
458 Sigma_ = Sigma;
459 Sigma = s / (b * A) + deltaSigma;
460 }
461 while (qAbs(Sigma - Sigma_) > 1e-12 && ++iterations < 100);
462 if (iterations >= 100)
463 {
464 return NOPOINTF;
465 }
466 qreal x = sinU1 * sinSigma - cosU1 * cosSigma * cosAlpha1;
467 qreal Phi2 = qAtan2(sinU1 * cosSigma + cosU1 * sinSigma * cosAlpha1, (1 - f) * qSqrt(sinAlpha * sinAlpha + x * x));
468 qreal Lambda = qAtan2(sinSigma * sinAlpha1, cosU1 * cosSigma - sinU1 * sinSigma * cosAlpha1);
469 qreal C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
470 qreal L = Lambda - (1 - C) * f * sinAlpha *
471 (Sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)));
472
473 qreal Lambda2 = Lambda1 + L;
474 if(Lambda2 > PI)
475 {
476 Lambda2 = -Lambda2 + PI;
477 }
478
479 // qreal Alpha2 = qAtan2(sinAlpha, -x);
480 // Alpha2 = (Alpha2 + 2*PI) % (2*PI); // normalise to 0..360
481
482 return QPointF(Lambda2, Phi2);
483 }
484