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