1/* 2Solve the direct and inverse geodesic problems accurately. 3 4Copyright (c) Charles Karney (2013-2021) <charles@karney.com> and 5licensed under the MIT/X11 License. For more information, see 6https://geographiclib.sourceforge.io/ 7 8References: 9 10 Charles F. F. Karney, 11 Algorithms for geodesics, J. Geodesy 87, 43-55 (2013), 12 https://doi.org/10.1007/s00190-012-0578-z 13 Addenda: https://geographiclib.sourceforge.io/geod-addenda.html 14 15This program solves the geodesic problem either using series expansions 16(exact : false) or using elliptic integrals (exact : true). Elliptic 17integrals give reliably high accuracy (especially when f is large). 18Note that the area calculation always uses the series expansion (I don't 19know how to express the integrals in terms of elliptic integrals). 20 21Before running this file, you need to compute and save the series 22expansions by editing geod.mac setting maxpow appropriately (near the 23end of the file) and uncommenting the last line (to save the results). 24If you're testing the accuracy of the series expansions (exact : false) 25or if you're interested in accurate results for the area, that pick a 26largish value of maxpow (e.g., 20). This program can truncate the 27series to a smaller power. If you just want to compute accurate 28geodesics and are not interested in the area, then use elliptic 29integrals (exact : true) and leave maxpow at some small value (6 or 8). 30 31To use this program, 32 33(1) Edit the file name for the series "geod30.lsp" to reflect the value 34of maxpow that you used. 35 36(2) Set fpprec (the number of decimal digits of precision). 37 38(3) Set exact (true for elliptic integrals, false for series). 39 40(4) If exact = false, set the order of the series you want to use, by 41replacing the "20" in min(maxpow,20) below. 42 43(5) Start maxima and run 44 45 load("geodesic.mac")$ 46 47(If you want to change fpprec, exact, or the order of the series, you 48should edit this file and run this command again.) 49 50(6) Define an ellipsoid with 51 52 g:geod_init(radius, flattening)$ 53 54The ellipsoids wgs84 and grs80 are pre-defined. 55 56(7) To solve a direct problem, run 57 58 geod_direct(ellipsoid, lat1, lon1, azi1, s12); 59 60e.g., 61 62 geod_direct(wgs84, -30, 0, 45, 1b7); 63 64This returns a list, [a12, lat2, lon2, azi2, s12, m12, M12, M21, S12], e.g., 65 66[9.00979560785581153132573611946234278938679821643464129576496b1, 673.795350501490084914310911431201705948430953526031024848204b1, 686.3403810943391419431089434638892210208040664981080107562114b1, 695.09217379721155238753530133334186917347878103616352193700526b1,1.0b7, 706.35984161356437923135381788735707599997546833534230510111197b6, 71-1.42475315175601879366432145888870774855600761387337970018946b-3, 72-7.47724030796032158868881196081763293754486469000152919698785b-4, 734.18229766667689593851692491830627309580972454148317773382384b12] 74 75(8) To solve an inverse problem, run 76 77 geod_inverse(ellipsoid, lat1, lon1, lat2, lon2); 78 79e.g., 80 81 geod_inverse(wgs84, -30, 0, 29.9b0, 179.9b0); 82 83This returns a list, [a12, s12, azi1, azi2, m12, M12, M21, S12], e.g., 84 85[1.79898924051433853264945993266804037171884583041584874134816b2, 861.99920903023269266279365620501124020214744990997998731526732b7, 871.70994569965518052741917124376016361591705298855243347424863b2, 888.99634915141674951478756137150809390696858860117887233257945b0, 896.04691725017600149958466836698242794713940408239599801996017b4, 90-9.95488849775559128989753386111595867497859420132749768254471b-1, 91-1.00448979492598025351148808245250847420628601706577993586242b0, 92-1.14721359300439474273586680489951630835335433189068889945966b14] 93 94(9) Use geod_polygonarea(ellipsoid, points) to compute polygon areas. 95 96(10) The interface is copied from the C library for geodesics which is 97documented at 98 99 https://geographiclib.sourceforge.io/html/C/index.html 100 101*/ 102 103/* The corresponding version of GeographicLib */ 104geod_version:[1,52,0]$ 105 106/* Load series created by geod.mac (NEED TO UNCOMMENT THE LAST LINE OF 107geod.mac TO GENERATE THIS FILE). */ 108load("geod30.lsp")$ 109 110/* Edit to reflect precision and order of the series to be used */ 111( fpprec:60, exact:true, 112 GEOGRAPHICLIB_GEODESIC_ORDER:if exact then maxpow else min(maxpow,20))$ 113 114if exact then load("ellint.mac")$ 115 116( nA1 :GEOGRAPHICLIB_GEODESIC_ORDER, 117 nC1 :GEOGRAPHICLIB_GEODESIC_ORDER, 118 nC1p :GEOGRAPHICLIB_GEODESIC_ORDER, 119 nA2 :GEOGRAPHICLIB_GEODESIC_ORDER, 120 nC2 :GEOGRAPHICLIB_GEODESIC_ORDER, 121 nA3 :GEOGRAPHICLIB_GEODESIC_ORDER, 122 nA3x :nA3, 123 nC3 :GEOGRAPHICLIB_GEODESIC_ORDER, 124 nC3x :((nC3 * (nC3 - 1)) / 2), 125 nC4 :GEOGRAPHICLIB_GEODESIC_ORDER, 126 nC4x :((nC4 * (nC4 + 1)) / 2) )$ 127 128taylordepth:5$ 129jtaylor(expr,var1,var2,ord):=expand(subst([zz=1], 130 ratdisrep(taylor(subst([var1=zz*var1,var2=zz*var2],expr),zz,0,ord))))$ 131ataylor(expr,var,ord):=expand(ratdisrep(taylor(expr,var,0,ord)))$ 132if not exact then ( 133 A1m1f(eps):=''((horner(ataylor(A1*(1-eps)-1,eps,nA1)) 134 +eps)/(1-eps)), 135 C1f(eps):=''(block([l:[]],for i:1 thru nC1 do 136 l:endcons(horner(ataylor(C1[i],eps,nC1)),l),l)), 137 C1pf(eps):=''(block([l:[]],for i:1 thru nC1p do 138 l:endcons(horner(ataylor(C1p[i],eps,nC1p)),l),l)), 139 A2m1f(eps):=''((horner(ataylor(A2*(1+eps)-1,eps,nA2)) 140 -eps)/(1+eps)), 141 C2f(eps):=''(block([l:[]],for i:1 thru nC2 do 142 l:endcons(horner(ataylor(C2[i],eps,nC2)),l),l)), 143 A3coeff(n):= 144 ''(block([q:jtaylor(A3,n,eps,nA3-1),l:[]], 145 for i:0 thru nA3-1 do l:endcons(horner(coeff(q,eps,i)),l), 146 l)), 147 C3coeff(n):= 148 ''(block([q,l:[]], 149 for m:1 thru nC3-1 do ( 150 q:jtaylor(C3[m],n,eps,nC3-1), 151 for j:m thru nC3-1 do l:endcons(horner(coeff(q,eps,j)),l)), 152 l)))$ 153C4coeff(n):= 154''(block([q,l:[]], 155 for m:0 thru nC4-1 do ( 156 q:jtaylor(C4[m],n,eps,nC4-1), 157 for j:m thru nC4-1 do l:endcons(horner(coeff(q,eps,j)),l)), 158 l))$ 159 160( digits:floor((fpprec-1)*log(10.0)/log(2.0)), 161 epsilon : 0.5b0^(digits - 1), 162 realmin : 0.1b0^(3*fpprec), 163 pi : bfloat(%pi), 164 maxit1 : 100, 165 maxit2 : maxit1 + digits + 10, 166 tiny : sqrt(realmin), 167 tol0 : epsilon, 168 tol1 : 200 * tol0, 169 tol2 : sqrt(tol0), 170 tolb : tol0 * tol2, 171 xthresh : 1000 * tol2, 172 degree : pi/180, 173 NaN : 'nan )$ 174 175sq(x):=x^2$ 176hypotx(x, y):=sqrt(x * x + y * y)$ 177/* doesn't handle -0.0 */ 178copysign(x, y):=abs(x) * (if y < 0b0 then -1 else 1)$ 179/* 180pow(x,y):=x^y$ 181cbrtx(x) := block([y:pow(abs(x), 1/3b0)], 182 if x < 0b0 then -y else y)$ 183*/ 184cbrtx(x):=x^(1/3)$ 185 186sumx(u, v):=block([s,up,vpp,t], 187 s : u + v, 188 up : s - v, 189 vpp : s - up, 190 up : up-u, 191 vpp : vpp-v, 192 t : -(up + vpp), 193 [s,t])$ 194 195swapx(x, y):=[y,x]$ 196 197norm2(x, y):=block([r : hypotx(x, y)], [x/r, y/r])$ 198 199AngNormalize(x):=block([y:x-360b0*round(x/360b0)], 200 if y <= -180b0 then y+360b0 else if y <= 180b0 then y else y-360b0)$ 201 202AngDiff(x, y) := block([t,d,r:sumx(AngNormalize(-x),AngNormalize(y))], 203 d:AngNormalize(r[1]), t:r[2], 204 sumx(if d = 180b0 and t > 0b0 then -180b0 else d, t))$ 205 206AngRound(x) := block([z:1/16b0, y:abs(x)], 207 if x = 0b0 then return(x), 208 y : if y < z then z - (z - y) else y, 209 if x < 0b0 then -y else y)$ 210 211sincosdx(x):=block([r,q:round(x/90b0),s,c], 212 r:(x-q*90b0)*degree, 213 s:sin(r), c:cos(r), 214 q:mod(q,4), 215 r: 216 if q = 0 then [ s, c] 217 elseif q = 1 then [ c, -s] 218 elseif q = 2 then [-s, -c] 219 else [-c, s], 220 if x # 0b0 then r:0b0+r, 221 r)$ 222 223atan2dx(y,x):=block([q,xx,yy,ang], 224 if abs(y) > abs(x) 225 then (xx:y, yy:x, q:2) 226 else (xx:x, yy:y, q:0), 227 if xx < 0 228 then (xx:-xx, q:q+1), 229 ang:atan2(yy, xx) / degree, 230 if q = 0 then ang 231 elseif q = 1 then (if y >= 0b0 then 180b0 else -180b0) - ang 232 elseif q = 2 then 90 - ang 233 else -90 + ang)$ 234 235/* Indices in geodesic struct */ 236block([i:0], g_a:(i:i+1), g_f:(i:i+1), g_f1:(i:i+1), g_e2:(i:i+1), 237 g_ep2:(i:i+1), g_n:(i:i+1), g_b:(i:i+1), g_c2:(i:i+1), g_etol2:(i:i+1), 238 g_A3x:(i:i+1), g_C3x:(i:i+1), g_C4x:(i:i+1) )$ 239geod_init(a, f):= (a:bfloat(a),f:bfloat(f), 240 block([f1,e2,ep2,n,b,c2,etol2], 241 f1:1-f, e2:f*(2-f), ep2:e2/f1^2, n:f/(2-f), b:a*f1, 242 c2 : (sq(a) + sq(b) * 243 (if e2 = 0b0 then 1b0 else 244 (if e2 > 0b0 then atanh(sqrt(e2)) else atan(sqrt(-e2))) / 245 sqrt(abs(e2))))/2, /* authalic radius squared */ 246 /* The sig12 threshold for "really short". Using the auxiliary sphere 247 solution with dnm computed at (bet1 + bet2) / 2, the relative error in 248 the azimuth consistency check is sig12^2 * abs(f) * min(1, 1-f/2) / 2. 249 (Error measured for 1/100 < b/a < 100 and abs(f) >= 1/1000. For a 250 given f and sig12, the max error occurs for lines near the pole. If 251 the old rule for computing dnm = (dn1 + dn2)/2 is used, then the error 252 increases by a factor of 2.) Setting this equal to epsilon gives 253 sig12 = etol2. Here 0.1 is a safety factor (error decreased by 100) 254 and max(0.001, abs(f)) stops etol2 getting too large in the nearly 255 spherical case. */ 256 etol2 : 0.1b0 * tol2 / sqrt( max(0.001b0, abs(f)) * min(1b0, 1-f/2) / 2 ), 257 [ a, f, f1, e2, 258 ep2, n, b, c2, etol2, 259 if exact then [] else bfloat(A3coeff(n)), 260 if exact then [] else bfloat(C3coeff(n)), 261 bfloat(C4coeff(n))]))$ 262 263/* Indices into geodesicline struct */ 264block([i:0], 265 l_lat1:(i:i+1), l_lon1:(i:i+1), l_azi1:(i:i+1), l_a:(i:i+1), l_f:(i:i+1), 266 l_b:(i:i+1), l_c2:(i:i+1), l_f1:(i:i+1), l_salp0:(i:i+1), l_calp0:(i:i+1), 267 l_k2:(i:i+1), l_salp1:(i:i+1), l_calp1:(i:i+1), 268 l_ssig1:(i:i+1), l_csig1:(i:i+1), l_dn1:(i:i+1), 269 l_stau1:(i:i+1), l_ctau1:(i:i+1), l_somg1:(i:i+1), l_comg1:(i:i+1), 270 if exact then (l_e2:(i:i+1), l_cchi1:(i:i+1), l_A4:(i:i+1), l_B41:(i:i+1), 271 l_E0:(i:i+1), l_D0:(i:i+1), l_H0:(i:i+1), 272 l_E1:(i:i+1), l_D1:(i:i+1), l_H1:(i:i+1), 273 l_C4a:(i:i+1), l_E:(i:i+1), 274 e_k2:1, e_alpha2:2, e_ec:3, e_dc:4, e_hc:5) 275 else (l_A1m1:(i:i+1), l_A2m1:(i:i+1), l_A3c:(i:i+1), 276 l_B11:(i:i+1), l_B21:(i:i+1), l_B31:(i:i+1), 277 l_A4:(i:i+1), l_B41:(i:i+1), l_C1a:(i:i+1), l_C1pa:(i:i+1), 278 l_C2a:(i:i+1), l_C3a:(i:i+1), 279 l_C4a:(i:i+1) ))$ 280 281Ef(k2, alpha2):=if exact then [k2, alpha2, ec(k2), dc(k2), hc(k2, alpha2)] 282else []$ 283 284geod_lineinit(g,lat1,lon1,azi1):=block([a, f, 285 b, c2, f1, salp0, calp0, 286 k2, salp1, calp1, 287 ssig1, csig1, dn1, 288 stau1, ctau1, somg1, comg1, 289 A1m1, A2m1, A3c, B11, B21, B31, 290 A4, B41, C1a, C1pa, C2a, C3a, 291 C4a, 292 cbet1, sbet1, eps, 293 e2, cchi1, A4, B41, E0, D0, H0, E1, D1, H1, C4a, E], 294 lat1:bfloat(lat1),lon1:bfloat(lon1), azi1:bfloat(azi1), 295 a : g[g_a], 296 f : g[g_f], 297 b : g[g_b], 298 c2 : g[g_c2], 299 f1 : g[g_f1], 300 e2 : g[g_e2], 301 lat1 : lat1, 302 /* If caps is 0 assume the standard direct calculation 303 caps = (caps ? caps : GEOD_DISTANCE_IN | GEOD_LONGITUDE) | 304 GEOD_LATITUDE | GEOD_AZIMUTH, Always allow latitude and azimuth 305 Guard against underflow in salp0 */ 306 azi1 : AngNormalize(azi1), 307 /* Don't normalize lon1... */ 308 block([t:sincosdx(AngRound(azi1))], salp1:t[1], calp1:t[2]), 309 block([t:sincosdx(AngRound(lat1))], 310 sbet1:t[1], cbet1:t[2]), sbet1 : f1 * sbet1, 311 block([t:norm2(sbet1, cbet1)], sbet1:t[1], cbet1:t[2]), 312 /* Ensure cbet1 = +epsilon at poles */ 313 cbet1 = max(tiny, cbet1), 314 dn1 : sqrt(1 + g[g_ep2] * sq(sbet1)), 315 /* Evaluate alp0 from sin(alp1) * cos(bet1) = sin(alp0), */ 316 salp0 : salp1 * cbet1, /* alp0 in [0, pi/2 - |bet1|] */ 317 /* Alt: calp0 : hypot(sbet1, calp1 * cbet1). The following 318 is slightly better (consider the case salp1 = 0). */ 319 calp0 : hypotx(calp1, salp1 * sbet1), 320 /* Evaluate sig with tan(bet1) = tan(sig1) * cos(alp1). 321 sig = 0 is nearest northward crossing of equator. 322 With bet1 = 0, alp1 = pi/2, we have sig1 = 0 (equatorial line). 323 With bet1 = pi/2, alp1 = -pi, sig1 = pi/2 324 With bet1 = -pi/2, alp1 = 0 , sig1 = -pi/2 325 Evaluate omg1 with tan(omg1) = sin(alp0) * tan(sig1). 326 With alp0 in (0, pi/2], quadrants for sig and omg coincide. 327 No atan2(0,0) ambiguity at poles since cbet1 = +epsilon. 328 With alp0 = 0, omg1 = 0 for alp1 = 0, omg1 = pi for alp1 = pi. */ 329 ssig1 : sbet1, somg1 : salp0 * sbet1, 330 csig1 : comg1 : if sbet1 # 0b0 or calp1 # 0b0 then cbet1 * calp1 else 1b0, 331 /* Without normalization we have schi1 = somg1. */ 332 cchi1 : f1 * dn1 * comg1, 333 /* sig1 in (-pi, pi] */ 334 block([t:norm2(ssig1, csig1)], ssig1:t[1], csig1:t[2]), 335 /* norm2 (somg1, comg1); -- don't need to normalize! 336 norm2 (schi1, cchi1); -- don't need to normalize! */ 337 k2 : sq(calp0) * g[g_ep2], 338 eps : k2 / (2 * (1 + sqrt(1 + k2)) + k2), 339 E:Ef(-k2,-g[g_ep2]), 340 block([s,c], 341 if exact then (E0 : E[e_ec]/(pi/2), 342 E1 : deltae(ssig1,csig1,dn1,E[e_k2],E[e_ec]), 343 s:sin(E1),c:cos(E1)) 344 else ( A1m1 : A1m1f(eps), 345 C1a : C1f(eps), 346 B11 : SinCosSeries(true, ssig1, csig1, C1a), 347 s : sin(B11), c : cos(B11)), 348 /* tau1 = sig1 + B11 */ 349 stau1 : ssig1 * c + csig1 * s, 350 ctau1 : csig1 * c - ssig1 * s 351 /* Not necessary because C1pa reverts C1a 352 B11 = -SinCosSeries(true, stau1, ctau1, C1pa, nC1p) */ 353 ), 354 if exact then (D0 : E[e_dc]/(pi/2), 355 D1 : deltad(ssig1, csig1, dn1, E[e_k2], E[e_dc]), 356 H0 : E[e_hc]/(pi/2), 357 H1 : deltah(ssig1, csig1, dn1, E[e_k2], E[e_alpha2], E[e_hc])) 358 else ( C1pa: C1pf(eps), 359 A2m1 : A2m1f(eps), 360 C2a : C2f(eps), 361 B21 : SinCosSeries(true, ssig1, csig1, C2a), 362 C3a : C3f(g, eps), 363 A3c : -f * salp0 * A3f(g, eps), 364 B31 : SinCosSeries(true, ssig1, csig1, C3a)), 365 C4a : C4f(g, eps), 366 /* Multiplier = a^2 * e^2 * cos(alpha0) * sin(alpha0) */ 367 A4 : sq(a) * calp0 * salp0 * e2, 368 B41 : SinCosSeries(false, ssig1, csig1, C4a), 369 if exact then 370 [ lat1, lon1, azi1, a, f, 371 b, c2, f1, salp0, calp0, 372 k2, salp1, calp1, 373 ssig1, csig1, dn1, 374 stau1, ctau1, somg1, comg1, 375 e2, cchi1, A4, B41, E0, D0, H0, E1, D1, H1, C4a, E] 376 else 377 [ lat1, lon1, azi1, a, f, 378 b, c2, f1, salp0, calp0, 379 k2, salp1, calp1, 380 ssig1, csig1, dn1, 381 stau1, ctau1, somg1, comg1, 382 A1m1, A2m1, A3c, B11, B21, B31, 383 A4, B41, C1a, C1pa, C2a, C3a, 384 C4a] )$ 385 386/* Return [a12, lat2, lon2, azi2, s12, m12, M12, M21, S12] */ 387geod_genposition(l, arcmode, s12_a12):=block( 388 [ lat2 : 0, lon2 : 0, azi2 : 0, s12 : 0, 389 m12 : 0, M12 : 0, M21 : 0, S12 : 0, 390 /* Avoid warning about uninitialized B12. */ 391 sig12, ssig12, csig12, B12 : 0, E2 : 0, AB1 : 0, 392 omg12, lam12, lon12, 393 ssig2, csig2, sbet2, cbet2, somg2, comg2, salp2, calp2, dn2, E], 394 s12_a12 : bfloat(s12_a12), 395 if (arcmode) then ( 396 /* Interpret s12_a12 as spherical arc length */ 397 sig12 : s12_a12 * degree, 398 block([t:sincosdx(s12_a12)], ssig12:t[1], csig12:t[2])) 399 else block([tau12 : s12_a12 / (l[l_b] * 400 (if exact then l[l_E0] else (1 + l[l_A1m1]))),s,c], 401 /* Interpret s12_a12 as distance */ 402 s : sin(tau12), 403 c : cos(tau12), 404 /* tau2 = tau1 + tau12 */ 405 if exact then (E2 : - deltaeinv(l[l_stau1] * c + l[l_ctau1] * s, 406 l[l_ctau1] * c - l[l_stau1] * s, 407 l[l_E][e_k2], l[l_E][e_ec]), 408 sig12 : tau12 - (E2 - l[l_E1]), ssig12 : sin(sig12), csig12 : cos(sig12)) 409 else (B12 : - SinCosSeries(true, 410 l[l_stau1] * c + l[l_ctau1] * s, 411 l[l_ctau1] * c - l[l_stau1] * s, 412 l[l_C1pa]), 413 sig12 : tau12 - (B12 - l[l_B11]), 414 ssig12 : sin(sig12), csig12 : cos(sig12), 415 if abs(l[l_f]) > 0.01 then block( 416 /* Reverted distance series is inaccurate for |f| > 1/100, so correct 417 sig12 with 1 Newton iteration. The following table shows the 418 approximate maximum error for a = WGS_a() and various f relative to 419 GeodesicExact. 420 erri = the error in the inverse solution (nm) 421 errd = the error in the direct solution (series only) (nm) 422 errda = the error in the direct solution (series + 1 Newton) (nm) 423 f erri errd errda 424 -1/5 12e6 1.2e9 69e6 425 -1/10 123e3 12e6 765e3 426 -1/20 1110 108e3 7155 427 -1/50 18.63 200.9 27.12 428 -1/100 18.63 23.78 23.37 429 -1/150 18.63 21.05 20.26 430 1/150 22.35 24.73 25.83 431 1/100 22.35 25.03 25.31 432 1/50 29.80 231.9 30.44 433 1/20 5376 146e3 10e3 434 1/10 829e3 22e6 1.5e6 435 1/5 157e6 3.8e9 280e6 436 */ 437 [ssig2 : l[l_ssig1] * csig12 + l[l_csig1] * ssig12, 438 csig2 : l[l_csig1] * csig12 - l[l_ssig1] * ssig12, 439 serr], 440 B12 : SinCosSeries(true, ssig2, csig2, l[l_C1a]), 441 serr : (1 + l[l_A1m1]) * (sig12 + (B12 - l[l_B11])) - s12_a12 / l[l_b], 442 sig12 : sig12 - serr / sqrt(1 + l[l_k2] * sq(ssig2)), 443 ssig12 : sin(sig12), csig12 : cos(sig12) 444 /* Update B12 below */ 445 ))), 446 /* sig2 = sig1 + sig12 */ 447 ssig2 : l[l_ssig1] * csig12 + l[l_csig1] * ssig12, 448 csig2 : l[l_csig1] * csig12 - l[l_ssig1] * ssig12, 449 dn2 : sqrt(1 + l[l_k2] * sq(ssig2)), 450 if exact then (if arcmode 451 then E2 : deltae(ssig2, csig2, dn2, l[l_E][e_k2], l[l_E][e_ec]), 452 AB1 : l[l_E0] * (E2 - l[l_E1])) 453 else (if arcmode or abs(l[l_f]) > 0.01b0 454 then B12 : SinCosSeries(true, ssig2, csig2, l[l_C1a]), 455 AB1 : (1 + l[l_A1m1]) * (B12 - l[l_B11])), 456 /* sin(bet2) = cos(alp0) * sin(sig2) */ 457 sbet2 : l[l_calp0] * ssig2, 458 /* Alt: cbet2 = hypot(csig2, salp0 * ssig2); */ 459 cbet2 : hypotx(l[l_salp0], l[l_calp0] * csig2), 460 if cbet2 = 0b0 then 461 /* I.e., salp0 = 0, csig2 = 0. Break the degeneracy in this case */ 462 cbet2 : csig2 : tiny, 463 if not exact then ( 464 /* tan(omg2) = sin(alp0) * tan(sig2) */ 465 somg2 : l[l_salp0] * ssig2, comg2 : csig2), /* No need to normalize */ 466 /* tan(alp0) = cos(sig2)*tan(alp2) */ 467 salp2 : l[l_salp0], calp2 : l[l_calp0] * csig2, /* No need to normalize */ 468 E : copysign(1b0, l[l_salp0]), 469 if not exact then 470 /* omg12 = omg2 - omg1 */ 471 omg12 : E * (sig12 472 - (atan2( ssig2, csig2) - atan2( l[l_ssig1], l[l_csig1])) 473 + (atan2(E*somg2, comg2) - atan2(E*l[l_somg1], l[l_comg1]))), 474 s12 : if arcmode then l[l_b] * 475 ((if exact then l[l_E0] else (1 + l[l_A1m1])) * sig12 + AB1) else s12_a12, 476 if exact then block([somg2:l[l_salp0] * ssig2, 477 comg2 : csig2, /* No need to normalize */ 478 cchi2], 479 /* Without normalization we have schi2 = somg2. */ 480 cchi2 : l[l_f1] * dn2 * comg2, 481 lam12 : E * (sig12 482 - (atan2( ssig2, csig2) - atan2( l[l_ssig1], l[l_csig1])) 483 + (atan2(E*somg2, cchi2) - atan2(E*l[l_somg1], l[l_cchi1]))) - 484 l[l_e2]/l[l_f1] * l[l_salp0] * l[l_H0] * 485 (sig12 + deltah(ssig2, csig2, dn2, 486 l[l_E][e_k2], l[l_E][e_alpha2], l[l_E][e_hc]) - l[l_H1] ) ) 487 else lam12 : omg12 + l[l_A3c] * 488 ( sig12 + (SinCosSeries(true, ssig2, csig2, l[l_C3a]) - l[l_B31])), 489 lon12 : lam12 / degree, 490 /* Don't normalize lon2... */ 491 lon2 : l[l_lon1] + lon12, 492 lat2 : atan2dx(sbet2, l[l_f1] * cbet2), 493 azi2 : atan2dx(salp2, calp2), 494 block([B22, AB2, J12], 495 if exact then J12 : l[l_k2] * l[l_D0] * 496 (sig12 + deltad(ssig2, csig2, dn2, l[l_E][e_k2], l[l_E][e_dc]) - l[l_D1]) 497 else ( B22 : SinCosSeries(true, ssig2, csig2, l[l_C2a]), 498 AB2 : (1 + l[l_A2m1]) * (B22 - l[l_B21]), 499 J12 : (l[l_A1m1] - l[l_A2m1]) * sig12 + (AB1 - AB2)), 500 /* Add parens around (csig1 * ssig2) and (ssig1 * csig2) to ensure 501 accurate cancellation in the case of coincident points. */ 502 m12 : l[l_b] * ((dn2 * (l[l_csig1] * ssig2) - 503 l[l_dn1] * (l[l_ssig1] * csig2)) 504 - l[l_csig1] * csig2 * J12), 505 block([t : l[l_k2] * (ssig2 - l[l_ssig1]) * 506 (ssig2 + l[l_ssig1]) / (l[l_dn1] + dn2)], 507 M12 : csig12 + (t * ssig2 - csig2 * J12) * l[l_ssig1] / l[l_dn1], 508 M21 : csig12 - (t * l[l_ssig1] - l[l_csig1] * J12) * ssig2 / dn2)), 509 block([ B42 : SinCosSeries(false, ssig2, csig2, l[l_C4a]), salp12, calp12], 510 if l[l_calp0] = 0b0 or l[l_salp0] = 0b0 then ( 511 /* alp12 = alp2 - alp1, used in atan2 so no need to normalize */ 512 salp12 : salp2 * l[l_calp1] - calp2 * l[l_salp1], 513 calp12 : calp2 * l[l_calp1] + salp2 * l[l_salp1]) 514 else ( 515 /* tan(alp) = tan(alp0) * sec(sig) 516 tan(alp2-alp1) = (tan(alp2) -tan(alp1)) / (tan(alp2)*tan(alp1)+1) 517 = calp0 * salp0 * (csig1-csig2) / (salp0^2 + calp0^2 * csig1*csig2) 518 If csig12 > 0, write 519 csig1 - csig2 = ssig12 * (csig1 * ssig12 / (1 + csig12) + ssig1) 520 else 521 csig1 - csig2 = csig1 * (1 - csig12) + ssig12 * ssig1 522 No need to normalize */ 523 salp12 : l[l_calp0] * l[l_salp0] * 524 ( if csig12 <= 0b0 then l[l_csig1] * (1 - csig12) + ssig12 * l[l_ssig1] 525 else ssig12 * (l[l_csig1] * ssig12 / (1 + csig12) + l[l_ssig1])), 526 calp12 : sq(l[l_salp0]) + sq(l[l_calp0]) * l[l_csig1] * csig2), 527 S12 : l[l_c2] * atan2(salp12, calp12) + l[l_A4] * (B42 - l[l_B41])), 528 [if arcmode then s12_a12 else sig12 / degree, 529 lat2, lon2, azi2, s12, m12, M12, M21, S12])$ 530 531geod_position(l, s12) := geod_genposition(l, false, s12)$ 532 533geod_gendirect(g, lat1, lon1, azi1, arcmode, s12_a12):= 534block([l:geod_lineinit(g, lat1, lon1, azi1)], 535 geod_genposition(l, arcmode, s12_a12))$ 536 537geod_direct(g, lat1, lon1, azi1, s12):= 538 geod_gendirect(g, lat1, lon1, azi1, false, s12)$ 539 540/* Return [a12, s12, azi1, azi2, m12, M12, M21, S12] */ 541geod_geninverse(g, lat1, lon1, lat2, lon2):=block( 542 [s12 : 0b0, azi1 : 0b0, azi2 : 0b0, 543 m12 : 0b0, M12 : 0b0, M21 : 0b0, S12 : 0b0, 544 lon12, lon12s, latsign, lonsign, swapp, 545 sbet1, cbet1, sbet2, cbet2, s12x : 0b0, m12x : 0b0, 546 dn1, dn2, lam12, slam12, clam12, 547 a12 : 0b0, sig12, calp1 : 0b0, salp1 : 0b0, calp2 : 0b0, salp2 : 0b0, 548 meridian, omg12 : 0b0, somg12 : 2b0, comg12, 549 /* Initialize for the meridian. No longitude calculation is done in this 550 case to let the parameter default to 0. */ 551 E : Ef(-g[g_ep2], 0b0)], 552 lat1:bfloat(lat1),lon1:bfloat(lon1), 553 lat2:bfloat(lat2),lon2:bfloat(lon2), 554 /* Compute longitude difference (AngDiff does this carefully). Result is 555 in [-180, 180] but -180 is only for west-going geodesics. 180 is for 556 east-going and meridional geodesics. */ 557 lon12 : AngDiff(lon1, lon2), lon12s:lon12[2], lon12:lon12[1], 558 /* Make longitude difference positive. */ 559 lonsign : if lon12 >= 0b0 then 1 else -1, 560 /* If very close to being on the same half-meridian, then make it so. */ 561 lon12 : lonsign * AngRound(lon12), 562 lon12s : AngRound((180 - lon12) - lonsign * lon12s), 563 lam12 : lon12 * degree, 564 if lon12 > 90 then block([t:sincosdx(lon12s)], slam12:t[1], clam12:-t[2]) 565 else block([t:sincosdx(lon12)], slam12:t[1], clam12:t[2]), 566 /* If really close to the equator, treat as on equator. */ 567 lat1 : AngRound(lat1), 568 lat2 : AngRound(lat2), 569 /* Swap points so that point with higher (abs) latitude is point 1 */ 570 swapp : if abs(lat1) >= abs(lat2) then 1 else -1, 571 if swapp < 0 then (lonsign : -lonsign, 572 block([t:swapx(lat1, lat2)], lat1:t[1], lat2:t[2])), 573 /* Make lat1 <= 0 */ 574 latsign : if lat1 < 0b0 then 1 else -1, 575 lat1 : lat1 * latsign, 576 lat2 : lat2 * latsign, 577 /* Now we have 578 0 <= lon12 <= 180 579 -90 <= lat1 <= 0 580 lat1 <= lat2 <= -lat1 581 longsign, swapp, latsign register the transformation to bring the 582 coordinates to this canonical form. In all cases, 1 means no change was 583 made. We make these transformations so that there are few cases to 584 check, e.g., on verifying quadrants in atan2. In addition, this 585 enforces some symmetries in the results returned. */ 586 block([t:sincosdx(lat1)], sbet1:t[1], cbet1:t[2]), sbet1 : g[g_f1] * sbet1, 587 block([t:norm2(sbet1, cbet1)], sbet1:t[1], cbet1:t[2]), 588 /* Ensure cbet1 = +epsilon at poles */ 589 cbet1 = max(tiny, cbet1), 590 block([t:sincosdx(lat2)], sbet2:t[1], cbet2:t[2]), sbet2 : g[g_f1] * sbet2, 591 block([t:norm2(sbet2, cbet2)], sbet2:t[1], cbet2:t[2]), 592 /* Ensure cbet2 = +epsilon at poles */ 593 cbet2 = max(tiny, cbet2), 594 /* If cbet1 < -sbet1, then cbet2 - cbet1 is a sensitive measure of the 595 |bet1| - |bet2|. Alternatively (cbet1 >= -sbet1), abs(sbet2) + sbet1 is 596 a better measure. This logic is used in assigning calp2 in Lambda12. 597 Sometimes these quantities vanish and in that case we force bet2 = +/- 598 bet1 exactly. An example where is is necessary is the inverse problem 599 48.522876735459 0 -48.52287673545898293 179.599720456223079643 600 which failed with Visual Studio 10 (Release and Debug) */ 601 if cbet1 < -sbet1 then 602 ( if cbet2 = cbet1 then sbet2 : if sbet2 < 0b0 then sbet1 else -sbet1 ) 603 else ( if abs(sbet2) = -sbet1 then cbet2 : cbet1 ), 604 dn1 : sqrt(1 + g[g_ep2] * sq(sbet1)), 605 dn2 : sqrt(1 + g[g_ep2] * sq(sbet2)), 606 meridian : is(lat1 = -90b0 or slam12 = 0b0), 607 if meridian then block( 608 /* Endpoints are on a single full meridian, so the geodesic might lie on 609 a meridian. */ 610 [ ssig1, csig1, ssig2, csig2], 611 calp1 : clam12, salp1 : slam12, /* Head to the target longitude */ 612 calp2 : 1b0, salp2 : 0b0, /* At the target we're heading north */ 613 /* tan(bet) : tan(sig) * cos(alp) */ 614 ssig1 : sbet1, csig1 : calp1 * cbet1, 615 ssig2 : sbet2, csig2 : calp2 * cbet2, 616 /* sig12 = sig2 - sig1 */ 617 sig12 : atan2(0b0 + max(0b0, csig1 * ssig2 - ssig1 * csig2), 618 csig1 * csig2 + ssig1 * ssig2), 619 block([r], 620 r:Lengths(g, g[g_n], E, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2, 621 cbet1, cbet2), 622 s12x:r[1], m12x:r[2], M12:r[4], M21:r[5]), 623 /* Add the check for sig12 since zero length geodesics might yield m12 < 624 0. Test case was 625 echo 20.001 0 20.001 0 | GeodTest -i 626 In fact, we will have sig12 > pi/2 for meridional geodesic which is 627 not a shortest path. */ 628 if sig12 < 1b0 or m12x >= 0b0 then ( 629 if sig12 < 3 * tiny or 630 (sig12 < tol0 and (s12x < 0 or m12x < 0)) then 631 sig12 : m12x : s12x : 0b0, 632 m12x : m12x * g[g_b], 633 s12x : s12x * g[g_b], 634 a12 : sig12 / degree ) 635 else 636 /* m12 < 0, i.e., prolate and too close to anti-podal */ 637 meridian : false ), 638 if not meridian and 639 sbet1 = 0b0 and /* and sbet2 == 0 */ 640 /* Mimic the way Lambda12 works with calp1 = 0 */ 641 (g[g_f] <= 0b0 or lon12s >= g[g_f] * 180) then ( 642 /* Geodesic runs along equator */ 643 calp1 : calp2 : 0b0, salp1 : salp2 : 1b0, 644 s12x : g[g_a] * lam12, 645 sig12 : omg12 : lam12 / g[g_f1], 646 m12x : g[g_b] * sin(sig12), 647 M12 : M21 : cos(sig12), 648 a12 : lon12 / g[g_f1] ) 649 else if not meridian then block([dnm], 650 /* Now point1 and point2 belong within a hemisphere bounded by a 651 meridian and geodesic is neither meridional or equatorial. 652 Figure a starting point for Newton's method */ 653 block([r], 654 r : InverseStart(g, sbet1, cbet1, dn1, sbet2, cbet2, dn2, 655 lam12, slam12, clam12), 656 sig12:r[1], salp1:r[2], calp1:r[3], salp2:r[4], calp2:r[5], 657 dnm:r[6]), 658 if sig12 >= 0b0 then ( 659 /* Short lines (InverseStart sets salp2, calp2, dnm) */ 660 s12x : sig12 * g[g_b] * dnm, 661 m12x : sq(dnm) * g[g_b] * sin(sig12 / dnm), 662 M12 : M21 : cos(sig12 / dnm), 663 a12 : sig12 / degree, 664 omg12 : lam12 / (g[g_f1] * dnm)) 665 else block( 666 /* Newton's method. This is a straightforward solution of f(alp1) = 667 lambda12(alp1) - lam12 = 0 with one wrinkle. f(alp) has exactly one 668 root in the interval (0, pi) and its derivative is positive at the 669 root. Thus f(alp) is positive for alp > alp1 and negative for alp < 670 alp1. During the course of the iteration, a range (alp1a, alp1b) is 671 maintained which brackets the root and with each evaluation of 672 f(alp) the range is shrunk, if possible. Newton's method is 673 restarted whenever the derivative of f is negative (because the new 674 value of alp1 is then further from the solution) or if the new 675 estimate of alp1 lies outside (0,pi); in this case, the new starting 676 guess is taken to be (alp1a + alp1b) / 2. */ 677 [ssig1 : 0b0, csig1 : 0b0, ssig2 : 0b0, csig2 : 0b0, eps : 0b0, 678 domg12 : 0b0, numit : 0, 679 /* Bracketing range */ 680 salp1a : tiny, calp1a : 1b0, salp1b : tiny, calp1b : -1b0, 681 tripn : false, tripb : false, contflag, dv, v], 682 for i:0 thru maxit2-1 do ( 683 contflag:false, 684 numit:i, 685 /* the WGS84 test set: mean : 1.47, sd = 1.25, max = 16 686 WGS84 and random input: mean = 2.85, sd = 0.60 */ 687 block([r], 688 r : Lambda12(g, sbet1, cbet1, dn1, sbet2, cbet2, dn2, salp1, calp1, 689 slam12, clam12, is(numit < maxit1)), 690 v : r[1], 691 salp2:r[2], calp2:r[3], sig12:r[4], 692 ssig1:r[5], csig1:r[6], ssig2:r[7], csig2:r[8], 693 if exact then E:r[9] else eps:r[9], domg12:r[10], 694 dv:r[11]), 695 /* Reversed test to allow escape with NaNs */ 696 if tripb or not(abs(v) >= (if tripn then 8 else 1) * tol0) then 697 return(true), 698 /* Update bracketing values */ 699 if v > 0b0 and (numit > maxit1 or calp1/salp1 > calp1b/salp1b) 700 then ( salp1b : salp1, calp1b : calp1 ) 701 else if v < 0b0 and (numit > maxit1 or calp1/salp1 < calp1a/salp1a) 702 then ( salp1a : salp1, calp1a : calp1 ), 703 if numit < maxit1 and dv > 0b0 then block( 704 [dalp1, sdalp1, cdalp1,nsalp1], 705 dalp1 : -v/dv, 706 sdalp1 : sin(dalp1), cdalp1 : cos(dalp1), 707 nsalp1 : salp1 * cdalp1 + calp1 * sdalp1, 708 if nsalp1 > 0b0 and abs(dalp1) < pi then ( 709 calp1 : calp1 * cdalp1 - salp1 * sdalp1, 710 salp1 : nsalp1, 711 block([t:norm2(salp1, calp1)], salp1:t[1], calp1:t[2]), 712 /* In some regimes we don't get quadratic convergence because 713 slope -> 0. So use convergence conditions based on epsilon 714 instead of sqrt(epsilon). */ 715 tripn : is(abs(v) <= 16 * tol0), 716 contflag:true ) ), 717 if not contflag then ( 718 /* Either dv was not positive or updated value was outside legal 719 range. Use the midpoint of the bracket as the next estimate. 720 This mechanism is not needed for the WGS84 ellipsoid, but it does 721 catch problems with more eccentric ellipsoids. Its efficacy is 722 such for the WGS84 test set with the starting guess set to alp1 = 723 90deg: 724 the WGS84 test set: mean = 5.21, sd = 3.93, max = 24 725 WGS84 and random input: mean = 4.74, sd = 0.99 */ 726 salp1 : (salp1a + salp1b)/2, 727 calp1 : (calp1a + calp1b)/2, 728 block([t:norm2(salp1, calp1)], salp1:t[1], calp1:t[2]), 729 tripn : false, 730 tripb : is(abs(salp1a - salp1) + (calp1a - calp1) < tolb or 731 abs(salp1 - salp1b) + (calp1 - calp1b) < tolb)) ), 732 block([r], 733 r:Lengths(g, eps, E, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2, 734 cbet1, cbet2), 735 s12x:r[1], m12x:r[2], M12:r[4], M21:r[5]), 736 m12x : m12x * g[g_b], 737 s12x : s12x * g[g_b], 738 a12 : sig12 / degree, 739 block([sdomg12 : sin(domg12), cdomg12 : cos(domg12)], 740 somg12 : slam12 * cdomg12 - clam12 * sdomg12, 741 comg12 : clam12 * cdomg12 + slam12 * sdomg12) ) ), 742 s12 : 0b0 + s12x, /* Convert -0 to 0 */ 743 m12 : 0b0 + m12x, /* Convert -0 to 0 */ 744 block( 745 /* From Lambda12: sin(alp1) * cos(bet1) = sin(alp0) */ 746 [ salp0 : salp1 * cbet1, 747 calp0 : hypotx(calp1, salp1 * sbet1), /* calp0 > 0 */ 748 alp12], 749 if calp0 # 0b0 and salp0 # 0b0 then block( 750 /* From Lambda12: tan(bet) = tan(sig) * cos(alp) */ 751 [ssig1 : sbet1, csig1 : calp1 * cbet1, 752 ssig2 : sbet2, csig2 : calp2 * cbet2, 753 k2 : sq(calp0) * g[g_ep2], eps, 754 /* Multiplier = a^2 * e^2 * cos(alpha0) * sin(alpha0). */ 755 A4 : sq(g[g_a]) * calp0 * salp0 * g[g_e2], 756 Ca, B41, B42], 757 eps : k2 / (2 * (1 + sqrt(1 + k2)) + k2), 758 block([t:norm2(ssig1, csig1)], ssig1:t[1], csig1:t[2]), 759 block([t:norm2(ssig2, csig2)], ssig2:t[1], csig2:t[2]), 760 Ca : C4f(g, eps), 761 B41 : SinCosSeries(false, ssig1, csig1, Ca), 762 B42 : SinCosSeries(false, ssig2, csig2, Ca), 763 S12 : A4 * (B42 - B41)) 764 else S12 : 0, /* Avoid problems with indeterminate sig1, sig2 on equator */ 765 if not meridian and somg12 > 1 then 766 (somg12 : sin(omg12), comg12 : cos(omg12)), 767 if not meridian and 768 /* omg12 < 3/4 * pi */ 769 comg12 > -0.7071b0 and /* Long difference not too big */ 770 sbet2 - sbet1 < 1.75b0 then block( /* Lat difference not too big */ 771 /* Use tan(Gamma/2) = tan(omg12/2) 772 * (tan(bet1/2)+tan(bet2/2))/(1+tan(bet1/2)*tan(bet2/2)) 773 with tan(x/2) = sin(x)/(1+cos(x)) */ 774 [domg12 : 1 + comg12, dbet1 : 1 + cbet1, dbet2 : 1 + cbet2], 775 alp12 : 2 * atan2( somg12 * ( sbet1 * dbet2 + sbet2 * dbet1 ), 776 domg12 * ( sbet1 * sbet2 + dbet1 * dbet2 ) )) 777 else block( 778 /* alp12 = alp2 - alp1, used in atan2 so no need to normalize */ 779 [salp12 : salp2 * calp1 - calp2 * salp1, 780 calp12 : calp2 * calp1 + salp2 * salp1], 781 /* The right thing appears to happen if alp1 = +/-180 and alp2 = 0, viz 782 salp12 = -0 and alp12 = -180. However this depends on the sign 783 being attached to 0 correctly. The following ensures the correct 784 behavior. */ 785 if salp12 = 0b0 and calp12 < 0b0 then ( 786 salp12 : tiny * calp1, 787 calp12 : -1b0), 788 alp12 : atan2(salp12, calp12) ), 789 S12 : S12 + g[g_c2] * alp12, 790 S12 : S12 * swapp * lonsign * latsign, 791 /* Convert -0 to 0 */ 792 S12 : S12 + 0b0 ), 793 /* Convert calp, salp to azimuth accounting for lonsign, swapp, latsign. */ 794 if swapp < 0 then ( 795 block([t:swapx(salp1, salp2)], salp1:t[1], salp2:t[2]), 796 block([t:swapx(calp1, calp2)], calp1:t[1], calp2:t[2]), 797 block([t:swapx(M12, M21)], M12:t[1], M21:t[2])), 798 salp1 : salp1 * swapp * lonsign, calp1 : calp1 * swapp * latsign, 799 salp2 : salp2 * swapp * lonsign, calp2 : calp2 * swapp * latsign, 800 azi1 : atan2dx(salp1, calp1), 801 azi2 : atan2dx(salp2, calp2), 802 [a12, s12, azi1, azi2, m12, M12, M21, S12] 803)$ 804 805geod_inverse(g, lat1, lon1, lat2, lon2):= 806geod_geninverse(g, lat1, lon1, lat2, lon2)$ 807 808SinCosSeries(sinp, sinx, cosx, c):=block([n:length(c), ar, y0, y1, n2, k], 809 /* Evaluate 810 y = sinp ? sum(c[i] * sin( 2*i * x), i, 1, n) : 811 sum(c[i] * cos((2*i-1) * x), i, 1, n) 812 using Clenshaw summation. where n = length(c) 813 Approx operation count = (n + 5) mult and (2 * n + 2) add */ 814 /* 2 * cos(2 * x) */ 815 ar : 2 * (cosx - sinx) * (cosx + sinx), 816 /* accumulators for sum */ 817 if mod(n, 2)=1 then (y0 : c[n], n2 : n - 1) 818 else (y0 : 0b0, n2 : n), 819 y1 : 0b0, 820 /* Now n2 is even */ 821 for k : n2 thru 1 step -2 do ( 822 /* Unroll loop x 2, so accumulators return to their original role */ 823 y1 : ar * y0 - y1 + c[k], 824 y0 : ar * y1 - y0 + c[k-1]), 825 if sinp then 2 * sinx * cosx * y0 /* sin(2 * x) * y0 */ 826 else cosx * (y0 - y1) /* cos(x) * (y0 - y1) */ 827 )$ 828 829/* Return [s12b, m12b, m0, M12, M21] */ 830Lengths(g, eps, E, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2, cbet1, cbet2):= 831block([Ca, 832 s12b : 0b0, m12b : 0b0, m0 : 0b0, M12 : 0b0, M21 : 0b0, 833 A1m1, AB1, A2m1, AB2, J12], 834 /* Return m12b = (reduced length)/b; also calculate s12b = distance/b, 835 and m0 = coefficient of secular term in expression for reduced length. */ 836 if exact then ( 837 m0 : - E[e_k2] * E[e_dc] / (pi/2), 838 J12 : m0 * 839 (sig12 + deltad(ssig2, csig2, dn2, E[e_k2], E[e_dc]) 840 - deltad(ssig1, csig1, dn1, E[e_k2], E[e_dc])) ) 841 else ( 842 A1m1 : A1m1f(eps), Ca : C1f(eps), 843 AB1 : (1 + A1m1) * (SinCosSeries(true, ssig2, csig2, Ca) - 844 SinCosSeries(true, ssig1, csig1, Ca)), 845 A2m1 : A2m1f(eps), Ca : C2f(eps), 846 AB2 : (1 + A2m1) * (SinCosSeries(true, ssig2, csig2, Ca) - 847 SinCosSeries(true, ssig1, csig1, Ca)), 848 m0 : A1m1 - A2m1, 849 J12 : m0 * sig12 + (AB1 - AB2)), 850 /* Missing a factor of b. 851 Add parens around (csig1 * ssig2) and (ssig1 * csig2) to ensure accurate 852 cancellation in the case of coincident points. */ 853 m12b : dn2 * (csig1 * ssig2) - dn1 * (ssig1 * csig2) - csig1 * csig2 * J12, 854 /* Missing a factor of b */ 855 s12b : if exact then 856 E[e_ec] / (pi/2) * 857 (sig12 + deltae(ssig2, csig2, dn2, E[e_k2], E[e_ec]) 858 - deltae(ssig1, csig1, dn1, E[e_k2], E[e_ec])) 859 else (1 + A1m1) * sig12 + AB1, 860 block([csig12 : csig1 * csig2 + ssig1 * ssig2, 861 t : g[g_ep2] * (cbet1 - cbet2) * (cbet1 + cbet2) / (dn1 + dn2)], 862 M12 : csig12 + (t * ssig2 - csig2 * J12) * ssig1 / dn1, 863 M21 : csig12 - (t * ssig1 - csig1 * J12) * ssig2 / dn2 ), 864 [s12b, m12b, m0, M12, M21])$ 865 866Astroid(x, y):= block( 867 /* Solve k^4+2*k^3-(x^2+y^2-1)*k^2-2*y^2*k-y^2 = 0 for positive root k. 868 This solution is adapted from Geocentric::Reverse. */ 869 [k, p : sq(x), q : sq(y), r], 870 r : (p + q - 1) / 6, 871 if not(q = 0b0 and r <= 0b0) then block( 872 /* Avoid possible division by zero when r = 0 by multiplying equations 873 for s and t by r^3 and r, resp. */ 874 [S : p * q / 4, /* S = r^3 * s */ 875 r2 : sq(r), 876 r3, disc, u, v, uv, w], 877 r3 : r * r2, 878 /* The discriminant of the quadratic equation for T3. This is zero on 879 the evolute curve p^(1/3)+q^(1/3) = 1 */ 880 disc : S * (S + 2 * r3), 881 u : r, 882 v, uv, w, 883 if disc >= 0b0 then block([T3 : S + r3, T], 884 /* Pick the sign on the sqrt to maximize abs(T3). This minimizes loss 885 of precision due to cancellation. The result is unchanged because 886 of the way the T is used in definition of u. */ 887 /* T3 = (r * t)^3 */ 888 T3 : T3 + (if T3 < 0b0 then -sqrt(disc) else sqrt(disc)), 889 /* N.B. cbrtx always returns the real root. cbrtx(-8) = -2. */ 890 T : cbrtx(T3), /* T = r * t */ 891 /* T can be zero, but then r2 / T -> 0. */ 892 u : u + T + (if T # 0b0 then r2 / T else 0b0)) 893 else block( 894 /* T is complex, but the way u is defined the result is real. */ 895 [ang : atan2(sqrt(-disc), -(S + r3))], 896 /* There are three possible cube roots. We choose the root which 897 avoids cancellation. Note that disc < 0 implies that r < 0. */ 898 u : u + 2 * r * cos(ang / 3)), 899 v : sqrt(sq(u) + q), /* guaranteed positive */ 900 /* Avoid loss of accuracy when u < 0. */ 901 uv : if u < 0b0 then q / (v - u) else u + v, /* u+v, guaranteed positive */ 902 w : (uv - q) / (2 * v), /* positive? */ 903 /* Rearrange expression for k to avoid loss of accuracy due to 904 subtraction. Division by 0 not possible because uv > 0, w >= 0. */ 905 k : uv / (sqrt(uv + sq(w)) + w)) /* guaranteed positive */ 906 else /* q == 0 && r <= 0 */ 907 /* y = 0 with |x| <= 1. Handle this case directly. 908 for y small, positive root is k = abs(y)/sqrt(1-x^2) */ 909 k : 0b0, 910 k)$ 911 912/* Return [sig12, salp1, calp1, salp2, calp2, dnm] */ 913InverseStart(g, sbet1, cbet1, dn1, sbet2, cbet2, dn2, 914 lam12, slam12, clam12):=block( 915 [ salp1 : 0b0, calp1 : 0b0, salp2 : 0b0, calp2 : 0b0, dnm : 0b0, 916 /* Return a starting point for Newton's method in salp1 and calp1 (function 917 value is -1). If Newton's method doesn't need to be used, return also 918 salp2 and calp2 and function value is sig12. */ 919 sig12 : -1b0, /* Return value */ 920 /* bet12 = bet2 - bet1 in [0, pi); bet12a = bet2 + bet1 in (-pi, 0] */ 921 sbet12 : sbet2 * cbet1 - cbet2 * sbet1, 922 cbet12 : cbet2 * cbet1 + sbet2 * sbet1, 923 sbet12a : sbet2 * cbet1 + cbet2 * sbet1, 924 shortline, somg12, comg12, ssig12, csig12, E:[]], 925 shortline : is(cbet12 >= 0b0 and sbet12 < 0.5b0 and cbet2 * lam12 < 0.5b0), 926 if shortline then block([sbetm2 : sq(sbet1 + sbet2), omg12], 927 /* sin((bet1+bet2)/2)^2 928 = (sbet1 + sbet2)^2 / ((sbet1 + sbet2)^2 + (cbet1 + cbet2)^2) */ 929 sbetm2 : sbetm2 / (sbetm2 + sq(cbet1 + cbet2)), 930 dnm : sqrt(1 + g[g_ep2] * sbetm2), 931 omg12 : lam12 / (g[g_f1] * dnm), 932 somg12 : sin(omg12), comg12 : cos(omg12)) 933 else (somg12 : slam12, comg12 : clam12), 934 salp1 : cbet2 * somg12, 935 calp1 : if comg12 >= 0b0 936 then sbet12 + cbet2 * sbet1 * sq(somg12) / (1 + comg12) 937 else sbet12a - cbet2 * sbet1 * sq(somg12) / (1 - comg12), 938 ssig12 : hypotx(salp1, calp1), 939 csig12 : sbet1 * sbet2 + cbet1 * cbet2 * comg12, 940 if shortline and ssig12 < g[g_etol2] then ( 941 /* really short lines */ 942 salp2 : cbet1 * somg12, 943 calp2 : sbet12 - cbet1 * sbet2 * 944 (if comg12 >= 0 then sq(somg12) / (1 + comg12) else 1 - comg12), 945 block([t:norm2(salp2, calp2)], salp2:t[1], calp2:t[2]), 946 /* Set return value */ 947 sig12 : atan2(ssig12, csig12)) 948 else if abs(g[g_n]) > 0.1b0 or /* No astroid calc if too eccentric */ 949 csig12 >= 0 or 950 ssig12 >= 6 * abs(g[g_n]) * pi * sq(cbet1) then true 951 /* Nothing to do, zeroth order spherical approximation is OK */ 952 else block( 953 /* Scale lam12 and bet2 to x, y coordinate system where antipodal point 954 is at origin and singular point is at y = 0, x = -1. */ 955 [y, lamscale, betscale,x, lam12x], 956 lam12x : atan2(-slam12, -clam12), /* lam12 - pi */ 957 if g[g_f] >= 0 then ( /* In fact f == 0 does not get here */ 958 /* x = dlong, y = dlat */ 959 if exact then block([k2 : sq(sbet1) * g[g_ep2]], 960 E : Ef(-k2, -g[g_ep2]), 961 lamscale : g[g_e2]/g[g_f1] * cbet1 * 2 * E[e_hc]) 962 else block([k2 : sq(sbet1) * g[g_ep2],eps], 963 eps : k2 / (2 * (1 + sqrt(1 + k2)) + k2), 964 lamscale : g[g_f] * cbet1 * A3f(g, eps) * pi), 965 betscale : lamscale * cbet1, 966 x : lam12x / lamscale, 967 y : sbet12a / betscale) 968 else block( /* f < 0 */ 969 /* x = dlat, y = dlong */ 970 [cbet12a : cbet2 * cbet1 - sbet2 * sbet1,bet12a,m12b, m0], 971 bet12a : atan2(sbet12a, cbet12a), 972 /* In the case of lon12 = 180, this repeats a calculation made in 973 * Inverse. */ 974 block([r], 975 r:Lengths(g, g[g_n], E, pi + bet12a, 976 sbet1, -cbet1, dn1, sbet2, cbet2, dn2, 977 cbet1, cbet2), 978 m12b:r[2], m0:r[3]), 979 x : -1 + m12b / (cbet1 * cbet2 * m0 * pi), 980 betscale : if x < -0.01b0 then sbet12a / x else 981 -g[g_f] * sq(cbet1) * pi, 982 lamscale : betscale / cbet1, 983 y : lam12x / lamscale), 984 if y > -tol1 and x > -1 - xthresh then ( 985 /* strip near cut */ 986 if g[g_f] >= 0b0 then ( 987 salp1 : min(1b0, -x), calp1 : - sqrt(1 - sq(salp1))) 988 else ( 989 calp1 : max(if x > -tol1 then 0b0 else -1b0, x), 990 salp1 : sqrt(1 - sq(calp1)))) 991 else block( 992 /* Estimate alp1, by solving the astroid problem. 993 Could estimate alpha1 = theta + pi/2, directly, i.e., 994 calp1 = y/k; salp1 = -x/(1+k); for f >= 0 995 calp1 = x/(1+k); salp1 = -y/k; for f < 0 (need to check) 996 However, it's better to estimate omg12 from astroid and use 997 spherical formula to compute alp1. This reduces the mean number of 998 Newton iterations for astroid cases from 2.24 (min 0, max 6) to 2.12 999 (min 0 max 5). The changes in the number of iterations are as 1000 follows: 1001 change percent 1002 1 5 1003 0 78 1004 -1 16 1005 -2 0.6 1006 -3 0.04 1007 -4 0.002 1008 The histogram of iterations is (m = number of iterations estimating 1009 alp1 directly, n = number of iterations estimating via omg12, total 1010 number of trials = 148605): 1011 iter m n 1012 0 148 186 1013 1 13046 13845 1014 2 93315 102225 1015 3 36189 32341 1016 4 5396 7 1017 5 455 1 1018 6 56 0 1019 Because omg12 is near pi, estimate work with omg12a = pi - omg12 */ 1020 [k : Astroid(x, y),omg12a], 1021 omg12a : lamscale * 1022 ( if g[g_f] >= 0b0 then -x * k/(1 + k) else -y * (1 + k)/k ), 1023 somg12 : sin(omg12a), comg12 : -cos(omg12a), 1024 /* Update spherical estimate of alp1 using omg12 instead of lam12 */ 1025 salp1 : cbet2 * somg12, 1026 calp1 : sbet12a - cbet2 * sbet1 * sq(somg12) / (1 - comg12))), 1027 if salp1 > 0 then /* Sanity check on starting guess */ 1028 block([t:norm2(salp1, calp1)], salp1:t[1], calp1:t[2]) 1029 else ( salp1 : 1b0, calp1 : 0b0 ), 1030 [sig12, salp1, calp1, salp2, calp2, dnm] 1031 )$ 1032 1033/* Return [lam12, salp2, calp2, sig12, ssig1, csig1, ssig2, csig2, 1034(E or eps), domg12, dlam12] 1035*/ 1036Lambda12(g, sbet1, cbet1, dn1, sbet2, cbet2, dn2, salp1, calp1, 1037 slam120, clam120, diffp):= 1038block([salp2 : 0b0, calp2 : 0b0, sig12 : 0b0, 1039 ssig1 : 0b0, csig1 : 0b0, ssig2 : 0b0, csig2 : 0b0, eps : 0b0, E : [], 1040 domg12 : 0b0, somg12 : 0b0, comg12 : 0b0, dlam12 : 0b0, 1041 salp0, calp0, 1042 somg1, comg1, somg2, comg2, cchi1, cchi2, lam12, 1043 B312, k2, Ca, eta], 1044 if sbet1 = 0b0 and calp1 = 0b0 then 1045 /* Break degeneracy of equatorial line. This case has already been 1046 handled. */ 1047 calp1 : -tiny, 1048 /* sin(alp1) * cos(bet1) = sin(alp0) */ 1049 salp0 : salp1 * cbet1, 1050 calp0 : hypotx(calp1, salp1 * sbet1), /* calp0 > 0 */ 1051 /* tan(bet1) = tan(sig1) * cos(alp1) 1052 tan(omg1) = sin(alp0) * tan(sig1) = tan(omg1)=tan(alp1)*sin(bet1) */ 1053 ssig1 : sbet1, somg1 : salp0 * sbet1, 1054 csig1 : comg1 : calp1 * cbet1, 1055 /* Without normalization we have schi1 = somg1. */ 1056 if exact then cchi1 : g[g_f1] * dn1 * comg1, 1057 block([t:norm2(ssig1, csig1)], ssig1:t[1], csig1:t[2]), 1058 /* norm2 (&somg1, &comg1); -- don't need to normalize! */ 1059 /* Enforce symmetries in the case abs(bet2) = -bet1. Need to be careful 1060 about this case, since this can yield singularities in the Newton 1061 iteration. 1062 sin(alp2) * cos(bet2) = sin(alp0) */ 1063 salp2 : if cbet2 # cbet1 then salp0 / cbet2 else salp1, 1064 /* calp2 = sqrt(1 - sq(salp2)) 1065 = sqrt(sq(calp0) - sq(sbet2)) / cbet2 1066 and subst for calp0 and rearrange to give; choose positive sqrt 1067 to give alp2 in [0, pi/2]. */ 1068 calp2 : if cbet2 # cbet1 or abs(sbet2) # -sbet1 then 1069 sqrt(sq(calp1 * cbet1) + 1070 (if cbet1 < -sbet1 then 1071 (cbet2 - cbet1) * (cbet1 + cbet2) else 1072 (sbet1 - sbet2) * (sbet1 + sbet2))) / cbet2 else 1073 abs(calp1), 1074 /* tan(bet2) = tan(sig2) * cos(alp2) 1075 tan(omg2) = sin(alp0) * tan(sig2). */ 1076 ssig2 : sbet2, somg2 : salp0 * sbet2, 1077 csig2 : comg2 : calp2 * cbet2, 1078 /* Without normalization we have schi2 = somg2. */ 1079 if exact then cchi2 : g[g_f1] * dn2 * comg2, 1080 block([t:norm2(ssig2, csig2)], ssig2:t[1], csig2:t[2]), 1081 /* norm2(&somg2, &comg2); -- don't need to normalize! */ 1082 /* sig12 = sig2 - sig1, limit to [0, pi] */ 1083 sig12 : atan2(0b0 + max(0b0, csig1 * ssig2 - ssig1 * csig2), 1084 csig1 * csig2 + ssig1 * ssig2), 1085 /* omg12 = omg2 - omg1, limit to [0, pi] */ 1086 somg12 : 0b0 + max(0b0, comg1 * somg2 - somg1 * comg2), 1087 comg12 : comg1 * comg2 + somg1 * somg2, 1088 k2 : sq(calp0) * g[g_ep2], 1089 if exact then block([schi12, cchi12, deta12], 1090 E : Ef(-k2, -g[g_ep2]), 1091 schi12 : 0b0 + max(0b0, cchi1 * somg2 - somg1 * cchi2), 1092 cchi12 : cchi1 * cchi2 + somg1 * somg2, 1093 /* eta = chi12 - lam120 */ 1094 eta : atan2(schi12 * clam120 - cchi12 * slam120, 1095 cchi12 * clam120 + schi12 * slam120), 1096 deta12 : -g[g_e2]/g[g_f1] * salp0 * E[e_hc] / (pi/2) * 1097 (sig12 + deltah(ssig2, csig2, dn2, E[e_k2], E[e_alpha2], E[e_hc]) - 1098 deltah(ssig1, csig1, dn1, E[e_k2], E[e_alpha2], E[e_hc]) ), 1099 lam12 : eta + deta12, 1100 domg12 : deta12 + atan2(schi12 * comg12 - cchi12 * somg12, 1101 cchi12 * comg12 + schi12 * somg12)) 1102 else ( /* eta = omg12 - lam120 */ 1103 eta : atan2(somg12 * clam120 - comg12 * slam120, 1104 comg12 * clam120 + somg12 * slam120), 1105 eps : k2 / (2 * (1 + sqrt(1 + k2)) + k2), 1106 Ca : C3f(g, eps), 1107 B312 : (SinCosSeries(true, ssig2, csig2, Ca) - 1108 SinCosSeries(true, ssig1, csig1, Ca)), 1109 domg12 : -g[g_f] * A3f(g, eps) * salp0 * (sig12 + B312), 1110 lam12 : eta + domg12), 1111 if diffp then ( 1112 if calp2 = 0b0 then 1113 dlam12 : - 2 * g[g_f1] * dn1 / sbet1 1114 else (block([r], 1115 r:Lengths(g, eps, E, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2, 1116 cbet1, cbet2), 1117 dlam12:r[2]), 1118 dlam12 : dlam12 * g[g_f1] / (calp2 * cbet2))), 1119 [lam12, salp2, calp2, sig12, ssig1, csig1, ssig2, csig2, 1120 if exact then E else eps, domg12, dlam12])$ 1121 1122A3f(g, eps):=block( 1123 /* Evaluate sum(A3x[k] * eps^k, k, 0, nA3x-1) by Horner's method */ 1124 [v : 0b0], 1125 for i:nA3x step -1 thru 1 do 1126 v : eps * v + g[g_A3x][i], 1127 v)$ 1128 1129C3f(g, eps):=block( 1130 /* Evaluate C3 coeffs by Horner's method 1131 * Elements c[1] thru c[nC3 - 1] are set */ 1132 [i, j, k, mult : 1b0, c : makelist(0, i, 1, nC3-1)], 1133 j : nC3x, 1134 for k : nC3-1 step -1 thru 1 do block( 1135 [t : 0b0], 1136 for i : nC3 - k step -1 thru 1 do ( 1137 t : eps * t + g[g_C3x][j], 1138 j : j - 1), 1139 c[k] : t), 1140 for k:1 thru nC3-1 do ( 1141 mult : mult * eps, 1142 c[k] : c[k] * mult), 1143 c)$ 1144 1145C4f(g, eps):=block( 1146 /* Evaluate C4 coeffs by Horner's method 1147 * Elements c[1] thru c[nC4] are set */ 1148 [i, j, k, mult : 1b0, c : makelist(0, i, 1, nC4)], 1149 j : nC4x, 1150 for k : nC4-1 step -1 thru 0 do block( 1151 [t : 0b0], 1152 for i : nC4 - k step -1 thru 1 do ( 1153 t : eps * t + g[g_C4x][j], 1154 j : j - 1), 1155 c[k+1] : t), 1156 for k : 2 thru nC4 do ( 1157 mult : mult * eps, 1158 c[k] : c[k] * mult), 1159 c)$ 1160 1161transit(lon1, lon2):=block([lon12], 1162 /* Return 1 or -1 if crossing prime meridian in east or west direction. 1163 Otherwise return zero. */ 1164 /* Compute lon12 the same way as Geodesic::Inverse. */ 1165 lon1 : AngNormalize(lon1), 1166 lon2 : AngNormalize(lon2), 1167 lon12 : AngDiff(lon1, lon2)[1], 1168 if lon1 <= 0b0 and lon2 > 0b0 and lon12 > 0b0 then 1 else 1169 (if lon2 <= 0b0 and lon1 > 0b0 and lon12 < 0b0 then -1 else 0))$ 1170 1171/* Return [P, A, mins, maxs] */ 1172geod_polygonarea(g, points) := block([n:length(points), crossings : 0, 1173 area0 : 4 * pi * g[g_c2], A : 0, P : 0, mins : g[g_a] * 100, maxs : 0b0], 1174 for i : 1 thru n do block( 1175 [ s12, S12, r ], 1176 r:geod_geninverse(g, 1177 points[i][1], points[i][2], 1178 points[mod(i,n)+1][1], points[mod(i,n)+1][2]), 1179 s12:r[2], S12:r[8], 1180 if s12 > maxs then maxs:s12, 1181 if s12 < mins then mins:s12, 1182 P : P + s12, 1183 /* The minus sign is due to the counter-clockwise convention */ 1184 A : A - S12, 1185 crossings : crossings + transit(points[i][2], points[mod(i,n)+1][2])), 1186 A : mod(A+area0/2, area0) - area0/2, 1187 if mod(crossings, 2) = 1 then 1188 A : A + (if A < 0b0 then 1 else -1) * area0/2, 1189 /* Put area in (-area0/2, area0/2] */ 1190 if A > area0/2 then 1191 A : A - area0 1192 else if A <= -area0/2 then 1193 A : A + area0, 1194 [P, A, mins, maxs])$ 1195 1196wgs84:geod_init(6378137b0, 1/298.257223563b0)$ 1197 1198flat(a, GM, omega, J2):=block( 1199 [e2:3*J2, K : 2 * (a*omega)^2 * a / (15 * GM), e2a, q0], 1200 for j:0 thru 100 do ( 1201 e2a:e2,q0:qf(e2/(1-e2)), 1202 e2:3*J2+K*e2*sqrt(e2)/q0, 1203 if e2 = e2a then return(done)), 1204 e2/(1+sqrt(1-e2)))$ 1205qf(ep2):=block([ep,fpprec:3*fpprec],ep:sqrt(ep2), 1206 ((1 + 3/ep2) * atan(ep) - 3/ep)/2)$ 1207/* 1/298.257222100882711243162836607614495 */ 1208fgrs80:flat(6378137b0, 3986005b8, 7292115b-11, 108263b-8)$ 1209grs80:geod_init(6378137b0, fgrs80)$ 1210