1 /*
2 Great Circle utility functions
3
4 Copyright (C) 2002 Robert Lipe, robertlipe+source@gpsbabel.org
5
6 This program is free software; you can redistribute it and/or modify
7 it under the terms of the GNU General Public License as published by
8 the Free Software Foundation; either version 2 of the License, or
9 (at your option) any later version.
10
11 This program is distributed in the hope that it will be useful,
12 but WITHOUT ANY WARRANTY; without even the implied warranty of
13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 GNU General Public License for more details.
15
16 You should have received a copy of the GNU General Public License
17 along with this program; if not, write to the Free Software
18 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
19
20 */
21
22 #include "defs.h"
23 #include "grtcirc.h"
24
25 #include <cerrno>
26 #include <cmath>
27 #include <cmath>
28 #include <cstdio>
29
30 static const double EARTH_RAD = 6378137.0;
31
crossproduct(double x1,double y1,double z1,double x2,double y2,double z2,double * xa,double * ya,double * za)32 static void crossproduct(double x1, double y1, double z1,
33 double x2, double y2, double z2,
34 double* xa, double* ya, double* za)
35 {
36 *xa = y1 * z2 - y2 * z1;
37 *ya = z1 * x2 - z2 * x1;
38 *za = x1 * y2 - y1 * x2;
39 }
40
dotproduct(double x1,double y1,double z1,double x2,double y2,double z2)41 static double dotproduct(double x1, double y1, double z1,
42 double x2, double y2, double z2)
43 {
44 return (x1 * x2 + y1 * y2 + z1 * z2);
45 }
46
47 /*
48 * Note: this conversion to miles uses the WGS84 value for the radius of
49 * the earth at the equator.
50 * (radius in meters)*(100cm/m) -> (radius in cm)
51 * (radius in cm) / (2.54 cm/in) -> (radius in in)
52 * (radius in in) / (12 in/ft) -> (radius in ft)
53 * (radius in ft) / (5280 ft/mi) -> (radius in mi)
54 * If the compiler is half-decent, it'll do all the math for us at compile
55 * time, so why not leave the expression human-readable?
56 */
57
radtomiles(double rads)58 double radtomiles(double rads)
59 {
60 const double radmiles = METERS_TO_MILES(EARTH_RAD);
61 return (rads * radmiles);
62 }
63
radtometers(double rads)64 double radtometers(double rads)
65 {
66 return (rads * EARTH_RAD);
67 }
68
gcdist(double lat1,double lon1,double lat2,double lon2)69 double gcdist(double lat1, double lon1, double lat2, double lon2)
70 {
71 errno = 0;
72
73 double sdlat = sin((lat1 - lat2) / 2.0);
74 double sdlon = sin((lon1 - lon2) / 2.0);
75
76 double res = sqrt(sdlat * sdlat + cos(lat1) * cos(lat2) * sdlon * sdlon);
77
78 if (res > 1.0) {
79 res = 1.0;
80 } else if (res < -1.0) {
81 res = -1.0;
82 }
83
84 res = asin(res);
85
86 if (std::isnan(res) || (errno == EDOM)) { /* this should never happen: */
87 errno = 0; /* Math argument out of domain of function, */
88 return 0; /* or value returned is not a number */
89 }
90
91 return 2.0 * res;
92 }
93
94 /* This value is the heading you'd leave point 1 at to arrive at point 2.
95 * Inputs and outputs are in radians.
96 */
heading(double lat1,double lon1,double lat2,double lon2)97 double heading(double lat1, double lon1, double lat2, double lon2)
98 {
99 double v1 = sin(lon1 - lon2) * cos(lat2);
100 double v2 = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lon1 - lon2);
101 /* rounding error protection */
102 if (fabs(v1) < 1e-15) {
103 v1 = 0.0;
104 }
105 if (fabs(v2) < 1e-15) {
106 v2 = 0.0;
107 }
108 return atan2(v1, v2);
109 }
110
111 /* As above, but outputs is in degrees from 0 - 359. Inputs are still radians. */
heading_true_degrees(double lat1,double lon1,double lat2,double lon2)112 double heading_true_degrees(double lat1, double lon1, double lat2, double lon2)
113 {
114 double h = 360.0 - DEG(heading(lat1, lon1, lat2, lon2));
115 if (h >= 360.0) {
116 h -= 360.0;
117 }
118
119 return h;
120 }
121
linedistprj(double lat1,double lon1,double lat2,double lon2,double lat3,double lon3,double * prjlat,double * prjlon,double * frac)122 double linedistprj(double lat1, double lon1,
123 double lat2, double lon2,
124 double lat3, double lon3,
125 double* prjlat, double* prjlon,
126 double* frac)
127 {
128 static double _lat1 = -9999;
129 static double _lat2 = -9999;
130 static double _lon1 = -9999;
131 static double _lon2 = -9999;
132
133 static double x1, y1, z1;
134 static double x2, y2, z2;
135 static double xa, ya, za, la;
136
137 double xa1, ya1, za1;
138 double xa2, ya2, za2;
139
140 double dot;
141
142 *prjlat = lat1;
143 *prjlon = lon1;
144 *frac = 0;
145
146 /* degrees to radians */
147 lat1 = RAD(lat1);
148 lon1 = RAD(lon1);
149 lat2 = RAD(lat2);
150 lon2 = RAD(lon2);
151 lat3 = RAD(lat3);
152 lon3 = RAD(lon3);
153
154 int newpoints = 1;
155 if (lat1 == _lat1 && lat2 == _lat2 && lon1 == _lon1 && lon2 == _lon2) {
156 newpoints = 0;
157 } else {
158 _lat1 = lat1;
159 _lat2 = lat2;
160 _lon1 = lon1;
161 _lon2 = lon2;
162 }
163
164 /* polar to ECEF rectangular */
165 if (newpoints) {
166 x1 = cos(lon1) * cos(lat1);
167 y1 = sin(lat1);
168 z1 = sin(lon1) * cos(lat1);
169 x2 = cos(lon2) * cos(lat2);
170 y2 = sin(lat2);
171 z2 = sin(lon2) * cos(lat2);
172 }
173 double x3 = cos(lon3) * cos(lat3);
174 double y3 = sin(lat3);
175 double z3 = sin(lon3) * cos(lat3);
176
177 if (newpoints) {
178 /* 'a' is the axis; the line that passes through the center of the earth
179 * and is perpendicular to the great circle through point 1 and point 2
180 * It is computed by taking the cross product of the '1' and '2' vectors.*/
181 crossproduct(x1, y1, z1, x2, y2, z2, &xa, &ya, &za);
182 la = sqrt(xa * xa + ya * ya + za * za);
183
184 if (la) {
185 xa /= la;
186 ya /= la;
187 za /= la;
188 }
189 }
190 if (la) {
191 /* dot is the component of the length of '3' that is along the axis.
192 * What's left is a non-normalized vector that lies in the plane of
193 * 1 and 2. */
194
195 dot = dotproduct(x3, y3, z3, xa, ya, za);
196
197 double xp = x3 - dot * xa;
198 double yp = y3 - dot * ya;
199 double zp = z3 - dot * za;
200
201 double lp = sqrt(xp * xp + yp * yp + zp * zp);
202
203 if (lp) {
204 /* After this, 'p' is normalized */
205 xp /= lp;
206 yp /= lp;
207 zp /= lp;
208
209 crossproduct(x1, y1, z1, xp, yp, zp, &xa1, &ya1, &za1);
210 double d1 = dotproduct(xa1, ya1, za1, xa, ya, za);
211
212 crossproduct(xp, yp, zp, x2, y2, z2, &xa2, &ya2, &za2);
213 double d2 = dotproduct(xa2, ya2, za2, xa, ya, za);
214
215 if (d1 >= 0 && d2 >= 0) {
216 /* rather than call gcdist and all its sines and cosines and
217 * worse, we can get the angle directly. It's the arctangent
218 * of the length of the component of vector 3 along the axis
219 * divided by the length of the component of vector 3 in the
220 * plane. We already have both of those numbers.
221 *
222 * atan2 would be overkill because lp and fabs(dot) are both
223 * known to be positive. */
224
225 *prjlat = DEG(asin(yp));
226 if (xp == 0 && zp == 0) {
227 *prjlon = 0;
228 } else {
229 *prjlon = DEG(atan2(zp, xp));
230 }
231 *frac = d1 / (d1 + d2);
232
233 return atan(fabs(dot) / lp);
234 }
235
236 /* otherwise, get the distance from the closest endpoint */
237 double c1 = dotproduct(x1, y1, z1, xp, yp, zp);
238 double c2 = dotproduct(x2, y2, z2, xp, yp, zp);
239 d1 = fabs(d1);
240 d2 = fabs(d2);
241
242 /* This is a hack. d$n$ is proportional to the sine of the angle
243 * between point $n$ and point p. That preserves orderedness up
244 * to an angle of 90 degrees. c$n$ is proportional to the cosine
245 * of the same angle; if the angle is over 90 degrees, c$n$ is
246 * negative. In that case, we flop the sine across the y=1 axis
247 * so that the resulting value increases as the angle increases.
248 *
249 * This only works because all of the points are on a unit sphere. */
250
251 if (c1 < 0) {
252 d1 = 2 - d1;
253 }
254 if (c2 < 0) {
255 d2 = 2 - d2;
256 }
257
258 if (fabs(d1) < fabs(d2)) {
259 return gcdist(lat1, lon1, lat3, lon3);
260 } else {
261 *prjlat = DEG(lat2);
262 *prjlon = DEG(lon2);
263 *frac = 1.0;
264 return gcdist(lat2, lon2, lat3, lon3);
265 }
266 } else {
267 /* lp is 0 when 3 is 90 degrees from the great circle */
268 return M_PI / 2;
269 }
270 } else {
271 /* la is 0 when 1 and 2 are either the same point or 180 degrees apart */
272 dot = dotproduct(x1, y1, z1, x2, y2, z2);
273 if (dot >= 0) {
274 return gcdist(lat1, lon1, lat3, lon3);
275 } else {
276 return 0;
277 }
278 }
279 }
280
linedist(double lat1,double lon1,double lat2,double lon2,double lat3,double lon3)281 double linedist(double lat1, double lon1,
282 double lat2, double lon2,
283 double lat3, double lon3)
284 {
285 double dummy;
286 return linedistprj(lat1, lon1, lat2, lon2, lat3, lon3, &dummy, &dummy, &dummy);
287 }
288
289 /*
290 * Compute the position of a point partially along the geodesic from
291 * lat1,lon1 to lat2,lon2
292 *
293 * Ref: http://mathworld.wolfram.com/RotationFormula.html
294 */
295
linepart(double lat1,double lon1,double lat2,double lon2,double frac,double * reslat,double * reslon)296 void linepart(double lat1, double lon1,
297 double lat2, double lon2,
298 double frac,
299 double* reslat, double* reslon)
300 {
301 double xa, ya, za;
302
303 /* result must be in degrees */
304 *reslat = lat1;
305 *reslon = lon1;
306
307 /* degrees to radians */
308 lat1 = RAD(lat1);
309 lon1 = RAD(lon1);
310 lat2 = RAD(lat2);
311 lon2 = RAD(lon2);
312
313 /* polar to ECEF rectangular */
314 double x1 = cos(lon1) * cos(lat1);
315 double y1 = sin(lat1);
316 double z1 = sin(lon1) * cos(lat1);
317 double x2 = cos(lon2) * cos(lat2);
318 double y2 = sin(lat2);
319 double z2 = sin(lon2) * cos(lat2);
320
321 /* 'a' is the axis; the line that passes through the center of the earth
322 * and is perpendicular to the great circle through point 1 and point 2
323 * It is computed by taking the cross product of the '1' and '2' vectors.*/
324 crossproduct(x1, y1, z1, x2, y2, z2, &xa, &ya, &za);
325 double la = sqrt(xa * xa + ya * ya + za * za);
326
327 if (la) {
328 xa /= la;
329 ya /= la;
330 za /= la;
331 }
332 /* if la is zero, the points are either equal or directly opposite
333 * each other. Either way, there's no single geodesic, so we punt. */
334 if (la) {
335 double xx, yx, zx;
336 crossproduct(x1, y1, z1, xa, ya, za, &xx, &yx, &zx);
337
338 double theta = atan2(dotproduct(xx,yx,zx,x2,y2,z2),
339 dotproduct(x1,y1,z1,x2,y2,z2));
340
341 double phi = frac * theta;
342 double cosphi = cos(phi);
343 double sinphi = sin(phi);
344
345 /* The second term of the formula from the mathworld reference is always
346 * zero, because r (lat1,lon1) is always perpendicular to n (a here) */
347 double xr = x1*cosphi + xx * sinphi;
348 double yr = y1*cosphi + yx * sinphi;
349 double zr = z1*cosphi + zx * sinphi;
350
351 if (xr > 1) {
352 xr = 1;
353 }
354 if (xr < -1) {
355 xr = -1;
356 }
357 if (yr > 1) {
358 yr = 1;
359 }
360 if (yr < -1) {
361 yr = -1;
362 }
363 if (zr > 1) {
364 zr = 1;
365 }
366 if (zr < -1) {
367 zr = -1;
368 }
369
370 *reslat = DEG(asin(yr));
371 if (xr == 0 && zr == 0) {
372 *reslon = 0;
373 } else {
374 *reslon = DEG(atan2(zr, xr));
375 }
376 }
377 }
378