henry/SimCore
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
b546628663b439e40e9d68e00760709433cb9744
SimCore/libs/geographiclib/maxima/geodesic.mac

1210 lines
49 KiB
Plaintext
Raw Blame History

/*
Solve the direct and inverse geodesic problems accurately.
Copyright (c) Charles Karney (2013-2021) <charles@karney.com> and
licensed under the MIT/X11 License. For more information, see
https://geographiclib.sourceforge.io/
References:
Charles F. F. Karney,
Algorithms for geodesics, J. Geodesy 87, 43-55 (2013),
https://doi.org/10.1007/s00190-012-0578-z
Addenda: https://geographiclib.sourceforge.io/geod-addenda.html
This program solves the geodesic problem either using series expansions
(exact : false) or using elliptic integrals (exact : true). Elliptic
integrals give reliably high accuracy (especially when f is large).
Note that the area calculation always uses the series expansion (I don't
know how to express the integrals in terms of elliptic integrals).
Before running this file, you need to compute and save the series
expansions by editing geod.mac setting maxpow appropriately (near the
end of the file) and uncommenting the last line (to save the results).
If you're testing the accuracy of the series expansions (exact : false)
or if you're interested in accurate results for the area, that pick a
largish value of maxpow (e.g., 20). This program can truncate the
series to a smaller power. If you just want to compute accurate
geodesics and are not interested in the area, then use elliptic
integrals (exact : true) and leave maxpow at some small value (6 or 8).
To use this program,
(1) Edit the file name for the series "geod30.lsp" to reflect the value
of maxpow that you used.
(2) Set fpprec (the number of decimal digits of precision).
(3) Set exact (true for elliptic integrals, false for series).
(4) If exact = false, set the order of the series you want to use, by
replacing the "20" in min(maxpow,20) below.
(5) Start maxima and run
load("geodesic.mac")$
(If you want to change fpprec, exact, or the order of the series, you
should edit this file and run this command again.)
(6) Define an ellipsoid with
g:geod_init(radius, flattening)$
The ellipsoids wgs84 and grs80 are pre-defined.
(7) To solve a direct problem, run
geod_direct(ellipsoid, lat1, lon1, azi1, s12);
e.g.,
geod_direct(wgs84, -30, 0, 45, 1b7);
This returns a list, [a12, lat2, lon2, azi2, s12, m12, M12, M21, S12], e.g.,
[9.00979560785581153132573611946234278938679821643464129576496b1,
3.795350501490084914310911431201705948430953526031024848204b1,
6.3403810943391419431089434638892210208040664981080107562114b1,
5.09217379721155238753530133334186917347878103616352193700526b1,1.0b7,
6.35984161356437923135381788735707599997546833534230510111197b6,
-1.42475315175601879366432145888870774855600761387337970018946b-3,
-7.47724030796032158868881196081763293754486469000152919698785b-4,
4.18229766667689593851692491830627309580972454148317773382384b12]
(8) To solve an inverse problem, run
geod_inverse(ellipsoid, lat1, lon1, lat2, lon2);
e.g.,
geod_inverse(wgs84, -30, 0, 29.9b0, 179.9b0);
This returns a list, [a12, s12, azi1, azi2, m12, M12, M21, S12], e.g.,
[1.79898924051433853264945993266804037171884583041584874134816b2,
1.99920903023269266279365620501124020214744990997998731526732b7,
1.70994569965518052741917124376016361591705298855243347424863b2,
8.99634915141674951478756137150809390696858860117887233257945b0,
6.04691725017600149958466836698242794713940408239599801996017b4,
-9.95488849775559128989753386111595867497859420132749768254471b-1,
-1.00448979492598025351148808245250847420628601706577993586242b0,
-1.14721359300439474273586680489951630835335433189068889945966b14]
(9) Use geod_polygonarea(ellipsoid, points) to compute polygon areas.
(10) The interface is copied from the C library for geodesics which is
documented at
https://geographiclib.sourceforge.io/C/doc/index.html
*/
/* The corresponding version of GeographicLib */
geod_version:[1,52,0]$
/* Load series created by geod.mac (NEED TO UNCOMMENT THE LAST LINE OF
geod.mac TO GENERATE THIS FILE). */
load("geod30.lsp")$
/* Edit to reflect precision and order of the series to be used */
( fpprec:60, exact:true,
GEOGRAPHICLIB_GEODESIC_ORDER:if exact then maxpow else min(maxpow,20))$
if exact then load("ellint.mac")$
( nA1 :GEOGRAPHICLIB_GEODESIC_ORDER,
nC1 :GEOGRAPHICLIB_GEODESIC_ORDER,
nC1p :GEOGRAPHICLIB_GEODESIC_ORDER,
nA2 :GEOGRAPHICLIB_GEODESIC_ORDER,
nC2 :GEOGRAPHICLIB_GEODESIC_ORDER,
nA3 :GEOGRAPHICLIB_GEODESIC_ORDER,
nA3x :nA3,
nC3 :GEOGRAPHICLIB_GEODESIC_ORDER,
nC3x :((nC3 * (nC3 - 1)) / 2),
nC4 :GEOGRAPHICLIB_GEODESIC_ORDER,
nC4x :((nC4 * (nC4 + 1)) / 2) )$
taylordepth:5$
jtaylor(expr,var1,var2,ord):=expand(subst([zz=1],
ratdisrep(taylor(subst([var1=zz*var1,var2=zz*var2],expr),zz,0,ord))))$
ataylor(expr,var,ord):=expand(ratdisrep(taylor(expr,var,0,ord)))$
if not exact then (
A1m1f(eps):=''((horner(ataylor(A1*(1-eps)-1,eps,nA1))
+eps)/(1-eps)),
C1f(eps):=''(block([l:[]],for i:1 thru nC1 do
l:endcons(horner(ataylor(C1[i],eps,nC1)),l),l)),
C1pf(eps):=''(block([l:[]],for i:1 thru nC1p do
l:endcons(horner(ataylor(C1p[i],eps,nC1p)),l),l)),
A2m1f(eps):=''((horner(ataylor(A2*(1+eps)-1,eps,nA2))
-eps)/(1+eps)),
C2f(eps):=''(block([l:[]],for i:1 thru nC2 do
l:endcons(horner(ataylor(C2[i],eps,nC2)),l),l)),
A3coeff(n):=
''(block([q:jtaylor(A3,n,eps,nA3-1),l:[]],
for i:0 thru nA3-1 do l:endcons(horner(coeff(q,eps,i)),l),
l)),
C3coeff(n):=
''(block([q,l:[]],
for m:1 thru nC3-1 do (
q:jtaylor(C3[m],n,eps,nC3-1),
for j:m thru nC3-1 do l:endcons(horner(coeff(q,eps,j)),l)),
l)))$
C4coeff(n):=
''(block([q,l:[]],
for m:0 thru nC4-1 do (
q:jtaylor(C4[m],n,eps,nC4-1),
for j:m thru nC4-1 do l:endcons(horner(coeff(q,eps,j)),l)),
l))$
( digits:floor((fpprec-1)*log(10.0)/log(2.0)),
epsilon : 0.5b0^(digits - 1),
realmin : 0.1b0^(3*fpprec),
pi : bfloat(%pi),
maxit1 : 100,
maxit2 : maxit1 + digits + 10,
tiny : sqrt(realmin),
tol0 : epsilon,
tol1 : 200 * tol0,
tol2 : sqrt(tol0),
tolb : tol0 * tol2,
xthresh : 1000 * tol2,
degree : pi/180,
NaN : 'nan )$
sq(x):=x^2$
hypotx(x, y):=sqrt(x * x + y * y)$
/* doesn't handle -0.0 */
copysign(x, y):=abs(x) * (if y < 0b0 then -1 else 1)$
/*
pow(x,y):=x^y$
cbrtx(x) := block([y:pow(abs(x), 1/3b0)],
if x < 0b0 then -y else y)$
*/
cbrtx(x):=x^(1/3)$
sumx(u, v):=block([s,up,vpp,t],
s : u + v,
up : s - v,
vpp : s - up,
up : up-u,
vpp : vpp-v,
t : -(up + vpp),
[s,t])$
swapx(x, y):=[y,x]$
norm2(x, y):=block([r : hypotx(x, y)], [x/r, y/r])$
AngNormalize(x):=block([y:x-360b0*round(x/360b0)],
if y <= -180b0 then y+360b0 else if y <= 180b0 then y else y-360b0)$
AngDiff(x, y) := block([t,d,r:sumx(AngNormalize(-x),AngNormalize(y))],
d:AngNormalize(r[1]), t:r[2],
sumx(if d = 180b0 and t > 0b0 then -180b0 else d, t))$
AngRound(x) := block([z:1/16b0, y:abs(x)],
if x = 0b0 then return(x),
y : if y < z then z - (z - y) else y,
if x < 0b0 then -y else y)$
sincosdx(x):=block([r,q:round(x/90b0),s,c],
r:(x-q*90b0)*degree,
s:sin(r), c:cos(r),
q:mod(q,4),
r:
if q = 0 then [ s, c]
elseif q = 1 then [ c, -s]
elseif q = 2 then [-s, -c]
else [-c, s],
if x # 0b0 then r:0b0+r,
r)$
atan2dx(y,x):=block([q,xx,yy,ang],
if abs(y) > abs(x)
then (xx:y, yy:x, q:2)
else (xx:x, yy:y, q:0),
if xx < 0
then (xx:-xx, q:q+1),
ang:atan2(yy, xx) / degree,
if q = 0 then ang
elseif q = 1 then (if y >= 0b0 then 180b0 else -180b0) - ang
elseif q = 2 then 90 - ang
else -90 + ang)$
/* Indices in geodesic struct */
block([i:0], g_a:(i:i+1), g_f:(i:i+1), g_f1:(i:i+1), g_e2:(i:i+1),
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),
g_A3x:(i:i+1), g_C3x:(i:i+1), g_C4x:(i:i+1) )$
geod_init(a, f):= (a:bfloat(a),f:bfloat(f),
block([f1,e2,ep2,n,b,c2,etol2],
f1:1-f, e2:f*(2-f), ep2:e2/f1^2, n:f/(2-f), b:a*f1,
c2 : (sq(a) + sq(b) *
(if e2 = 0b0 then 1b0 else
(if e2 > 0b0 then atanh(sqrt(e2)) else atan(sqrt(-e2))) /
sqrt(abs(e2))))/2, /* authalic radius squared */
/* The sig12 threshold for "really short". Using the auxiliary sphere
solution with dnm computed at (bet1 + bet2) / 2, the relative error in
the azimuth consistency check is sig12^2 * abs(f) * min(1, 1-f/2) / 2.
(Error measured for 1/100 < b/a < 100 and abs(f) >= 1/1000. For a
given f and sig12, the max error occurs for lines near the pole. If
the old rule for computing dnm = (dn1 + dn2)/2 is used, then the error
increases by a factor of 2.) Setting this equal to epsilon gives
sig12 = etol2. Here 0.1 is a safety factor (error decreased by 100)
and max(0.001, abs(f)) stops etol2 getting too large in the nearly
spherical case. */
etol2 : 0.1b0 * tol2 / sqrt( max(0.001b0, abs(f)) * min(1b0, 1-f/2) / 2 ),
[ a, f, f1, e2,
ep2, n, b, c2, etol2,
if exact then [] else bfloat(A3coeff(n)),
if exact then [] else bfloat(C3coeff(n)),
bfloat(C4coeff(n))]))$
/* Indices into geodesicline struct */
block([i:0],
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),
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),
l_k2:(i:i+1), l_salp1:(i:i+1), l_calp1:(i:i+1),
l_ssig1:(i:i+1), l_csig1:(i:i+1), l_dn1:(i:i+1),
l_stau1:(i:i+1), l_ctau1:(i:i+1), l_somg1:(i:i+1), l_comg1:(i:i+1),
if exact then (l_e2:(i:i+1), l_cchi1:(i:i+1), l_A4:(i:i+1), l_B41:(i:i+1),
l_E0:(i:i+1), l_D0:(i:i+1), l_H0:(i:i+1),
l_E1:(i:i+1), l_D1:(i:i+1), l_H1:(i:i+1),
l_C4a:(i:i+1), l_E:(i:i+1),
e_k2:1, e_alpha2:2, e_ec:3, e_dc:4, e_hc:5)
else (l_A1m1:(i:i+1), l_A2m1:(i:i+1), l_A3c:(i:i+1),
l_B11:(i:i+1), l_B21:(i:i+1), l_B31:(i:i+1),
l_A4:(i:i+1), l_B41:(i:i+1), l_C1a:(i:i+1), l_C1pa:(i:i+1),
l_C2a:(i:i+1), l_C3a:(i:i+1),
l_C4a:(i:i+1) ))$
Ef(k2, alpha2):=if exact then [k2, alpha2, ec(k2), dc(k2), hc(k2, alpha2)]
else []$
geod_lineinit(g,lat1,lon1,azi1):=block([a, f,
b, c2, f1, salp0, calp0,
k2, salp1, calp1,
ssig1, csig1, dn1,
stau1, ctau1, somg1, comg1,
A1m1, A2m1, A3c, B11, B21, B31,
A4, B41, C1a, C1pa, C2a, C3a,
C4a,
cbet1, sbet1, eps,
e2, cchi1, A4, B41, E0, D0, H0, E1, D1, H1, C4a, E],
lat1:bfloat(lat1),lon1:bfloat(lon1), azi1:bfloat(azi1),
a : g[g_a],
f : g[g_f],
b : g[g_b],
c2 : g[g_c2],
f1 : g[g_f1],
e2 : g[g_e2],
lat1 : lat1,
/* If caps is 0 assume the standard direct calculation
caps = (caps ? caps : GEOD_DISTANCE_IN | GEOD_LONGITUDE) |
GEOD_LATITUDE | GEOD_AZIMUTH, Always allow latitude and azimuth
Guard against underflow in salp0 */
azi1 : AngNormalize(azi1),
/* Don't normalize lon1... */
block([t:sincosdx(AngRound(azi1))], salp1:t[1], calp1:t[2]),
block([t:sincosdx(AngRound(lat1))],
sbet1:t[1], cbet1:t[2]), sbet1 : f1 * sbet1,
block([t:norm2(sbet1, cbet1)], sbet1:t[1], cbet1:t[2]),
/* Ensure cbet1 = +epsilon at poles */
cbet1 = max(tiny, cbet1),
dn1 : sqrt(1 + g[g_ep2] * sq(sbet1)),
/* Evaluate alp0 from sin(alp1) * cos(bet1) = sin(alp0), */
salp0 : salp1 * cbet1, /* alp0 in [0, pi/2 - |bet1|] */
/* Alt: calp0 : hypot(sbet1, calp1 * cbet1). The following
is slightly better (consider the case salp1 = 0). */
calp0 : hypotx(calp1, salp1 * sbet1),
/* Evaluate sig with tan(bet1) = tan(sig1) * cos(alp1).
sig = 0 is nearest northward crossing of equator.
With bet1 = 0, alp1 = pi/2, we have sig1 = 0 (equatorial line).
With bet1 = pi/2, alp1 = -pi, sig1 = pi/2
With bet1 = -pi/2, alp1 = 0 , sig1 = -pi/2
Evaluate omg1 with tan(omg1) = sin(alp0) * tan(sig1).
With alp0 in (0, pi/2], quadrants for sig and omg coincide.
No atan2(0,0) ambiguity at poles since cbet1 = +epsilon.
With alp0 = 0, omg1 = 0 for alp1 = 0, omg1 = pi for alp1 = pi. */
ssig1 : sbet1, somg1 : salp0 * sbet1,
csig1 : comg1 : if sbet1 # 0b0 or calp1 # 0b0 then cbet1 * calp1 else 1b0,
/* Without normalization we have schi1 = somg1. */
cchi1 : f1 * dn1 * comg1,
/* sig1 in (-pi, pi] */
block([t:norm2(ssig1, csig1)], ssig1:t[1], csig1:t[2]),
/* norm2 (somg1, comg1); -- don't need to normalize!
norm2 (schi1, cchi1); -- don't need to normalize! */
k2 : sq(calp0) * g[g_ep2],
eps : k2 / (2 * (1 + sqrt(1 + k2)) + k2),
E:Ef(-k2,-g[g_ep2]),
block([s,c],
if exact then (E0 : E[e_ec]/(pi/2),
E1 : deltae(ssig1,csig1,dn1,E[e_k2],E[e_ec]),
s:sin(E1),c:cos(E1))
else ( A1m1 : A1m1f(eps),
C1a : C1f(eps),
B11 : SinCosSeries(true, ssig1, csig1, C1a),
s : sin(B11), c : cos(B11)),
/* tau1 = sig1 + B11 */
stau1 : ssig1 * c + csig1 * s,
ctau1 : csig1 * c - ssig1 * s
/* Not necessary because C1pa reverts C1a
B11 = -SinCosSeries(true, stau1, ctau1, C1pa, nC1p) */
),
if exact then (D0 : E[e_dc]/(pi/2),
D1 : deltad(ssig1, csig1, dn1, E[e_k2], E[e_dc]),
H0 : E[e_hc]/(pi/2),
H1 : deltah(ssig1, csig1, dn1, E[e_k2], E[e_alpha2], E[e_hc]))
else ( C1pa: C1pf(eps),
A2m1 : A2m1f(eps),
C2a : C2f(eps),
B21 : SinCosSeries(true, ssig1, csig1, C2a),
C3a : C3f(g, eps),
A3c : -f * salp0 * A3f(g, eps),
B31 : SinCosSeries(true, ssig1, csig1, C3a)),
C4a : C4f(g, eps),
/* Multiplier = a^2 * e^2 * cos(alpha0) * sin(alpha0) */
A4 : sq(a) * calp0 * salp0 * e2,
B41 : SinCosSeries(false, ssig1, csig1, C4a),
if exact then
[ lat1, lon1, azi1, a, f,
b, c2, f1, salp0, calp0,
k2, salp1, calp1,
ssig1, csig1, dn1,
stau1, ctau1, somg1, comg1,
e2, cchi1, A4, B41, E0, D0, H0, E1, D1, H1, C4a, E]
else
[ lat1, lon1, azi1, a, f,
b, c2, f1, salp0, calp0,
k2, salp1, calp1,
ssig1, csig1, dn1,
stau1, ctau1, somg1, comg1,
A1m1, A2m1, A3c, B11, B21, B31,
A4, B41, C1a, C1pa, C2a, C3a,
C4a] )$
/* Return [a12, lat2, lon2, azi2, s12, m12, M12, M21, S12] */
geod_genposition(l, arcmode, s12_a12):=block(
[ lat2 : 0, lon2 : 0, azi2 : 0, s12 : 0,
m12 : 0, M12 : 0, M21 : 0, S12 : 0,
/* Avoid warning about uninitialized B12. */
sig12, ssig12, csig12, B12 : 0, E2 : 0, AB1 : 0,
omg12, lam12, lon12,
ssig2, csig2, sbet2, cbet2, somg2, comg2, salp2, calp2, dn2, E],
s12_a12 : bfloat(s12_a12),
if (arcmode) then (
/* Interpret s12_a12 as spherical arc length */
sig12 : s12_a12 * degree,
block([t:sincosdx(s12_a12)], ssig12:t[1], csig12:t[2]))
else block([tau12 : s12_a12 / (l[l_b] *
(if exact then l[l_E0] else (1 + l[l_A1m1]))),s,c],
/* Interpret s12_a12 as distance */
s : sin(tau12),
c : cos(tau12),
/* tau2 = tau1 + tau12 */
if exact then (E2 : - deltaeinv(l[l_stau1] * c + l[l_ctau1] * s,
l[l_ctau1] * c - l[l_stau1] * s,
l[l_E][e_k2], l[l_E][e_ec]),
sig12 : tau12 - (E2 - l[l_E1]), ssig12 : sin(sig12), csig12 : cos(sig12))
else (B12 : - SinCosSeries(true,
l[l_stau1] * c + l[l_ctau1] * s,
l[l_ctau1] * c - l[l_stau1] * s,
l[l_C1pa]),
sig12 : tau12 - (B12 - l[l_B11]),
ssig12 : sin(sig12), csig12 : cos(sig12),
if abs(l[l_f]) > 0.01 then block(
/* Reverted distance series is inaccurate for |f| > 1/100, so correct
sig12 with 1 Newton iteration. The following table shows the
approximate maximum error for a = WGS_a() and various f relative to
GeodesicExact.
erri = the error in the inverse solution (nm)
errd = the error in the direct solution (series only) (nm)
errda = the error in the direct solution (series + 1 Newton) (nm)
f erri errd errda
-1/5 12e6 1.2e9 69e6
-1/10 123e3 12e6 765e3
-1/20 1110 108e3 7155
-1/50 18.63 200.9 27.12
-1/100 18.63 23.78 23.37
-1/150 18.63 21.05 20.26
1/150 22.35 24.73 25.83
1/100 22.35 25.03 25.31
1/50 29.80 231.9 30.44
1/20 5376 146e3 10e3
1/10 829e3 22e6 1.5e6
1/5 157e6 3.8e9 280e6
*/
[ssig2 : l[l_ssig1] * csig12 + l[l_csig1] * ssig12,
csig2 : l[l_csig1] * csig12 - l[l_ssig1] * ssig12,
serr],
B12 : SinCosSeries(true, ssig2, csig2, l[l_C1a]),
serr : (1 + l[l_A1m1]) * (sig12 + (B12 - l[l_B11])) - s12_a12 / l[l_b],
sig12 : sig12 - serr / sqrt(1 + l[l_k2] * sq(ssig2)),
ssig12 : sin(sig12), csig12 : cos(sig12)
/* Update B12 below */
))),
/* sig2 = sig1 + sig12 */
ssig2 : l[l_ssig1] * csig12 + l[l_csig1] * ssig12,
csig2 : l[l_csig1] * csig12 - l[l_ssig1] * ssig12,
dn2 : sqrt(1 + l[l_k2] * sq(ssig2)),
if exact then (if arcmode
then E2 : deltae(ssig2, csig2, dn2, l[l_E][e_k2], l[l_E][e_ec]),
AB1 : l[l_E0] * (E2 - l[l_E1]))
else (if arcmode or abs(l[l_f]) > 0.01b0
then B12 : SinCosSeries(true, ssig2, csig2, l[l_C1a]),
AB1 : (1 + l[l_A1m1]) * (B12 - l[l_B11])),
/* sin(bet2) = cos(alp0) * sin(sig2) */
sbet2 : l[l_calp0] * ssig2,
/* Alt: cbet2 = hypot(csig2, salp0 * ssig2); */
cbet2 : hypotx(l[l_salp0], l[l_calp0] * csig2),
if cbet2 = 0b0 then
/* I.e., salp0 = 0, csig2 = 0. Break the degeneracy in this case */
cbet2 : csig2 : tiny,
if not exact then (
/* tan(omg2) = sin(alp0) * tan(sig2) */
somg2 : l[l_salp0] * ssig2, comg2 : csig2), /* No need to normalize */
/* tan(alp0) = cos(sig2)*tan(alp2) */
salp2 : l[l_salp0], calp2 : l[l_calp0] * csig2, /* No need to normalize */
E : copysign(1b0, l[l_salp0]),
if not exact then
/* omg12 = omg2 - omg1 */
omg12 : E * (sig12
- (atan2( ssig2, csig2) - atan2( l[l_ssig1], l[l_csig1]))
+ (atan2(E*somg2, comg2) - atan2(E*l[l_somg1], l[l_comg1]))),
s12 : if arcmode then l[l_b] *
((if exact then l[l_E0] else (1 + l[l_A1m1])) * sig12 + AB1) else s12_a12,
if exact then block([somg2:l[l_salp0] * ssig2,
comg2 : csig2, /* No need to normalize */
cchi2],
/* Without normalization we have schi2 = somg2. */
cchi2 : l[l_f1] * dn2 * comg2,
lam12 : E * (sig12
- (atan2( ssig2, csig2) - atan2( l[l_ssig1], l[l_csig1]))
+ (atan2(E*somg2, cchi2) - atan2(E*l[l_somg1], l[l_cchi1]))) -
l[l_e2]/l[l_f1] * l[l_salp0] * l[l_H0] *
(sig12 + deltah(ssig2, csig2, dn2,
l[l_E][e_k2], l[l_E][e_alpha2], l[l_E][e_hc]) - l[l_H1] ) )
else lam12 : omg12 + l[l_A3c] *
( sig12 + (SinCosSeries(true, ssig2, csig2, l[l_C3a]) - l[l_B31])),
lon12 : lam12 / degree,
/* Don't normalize lon2... */
lon2 : l[l_lon1] + lon12,
lat2 : atan2dx(sbet2, l[l_f1] * cbet2),
azi2 : atan2dx(salp2, calp2),
block([B22, AB2, J12],
if exact then J12 : l[l_k2] * l[l_D0] *
(sig12 + deltad(ssig2, csig2, dn2, l[l_E][e_k2], l[l_E][e_dc]) - l[l_D1])
else ( B22 : SinCosSeries(true, ssig2, csig2, l[l_C2a]),
AB2 : (1 + l[l_A2m1]) * (B22 - l[l_B21]),
J12 : (l[l_A1m1] - l[l_A2m1]) * sig12 + (AB1 - AB2)),
/* Add parens around (csig1 * ssig2) and (ssig1 * csig2) to ensure
accurate cancellation in the case of coincident points. */
m12 : l[l_b] * ((dn2 * (l[l_csig1] * ssig2) -
l[l_dn1] * (l[l_ssig1] * csig2))
- l[l_csig1] * csig2 * J12),
block([t : l[l_k2] * (ssig2 - l[l_ssig1]) *
(ssig2 + l[l_ssig1]) / (l[l_dn1] + dn2)],
M12 : csig12 + (t * ssig2 - csig2 * J12) * l[l_ssig1] / l[l_dn1],
M21 : csig12 - (t * l[l_ssig1] - l[l_csig1] * J12) * ssig2 / dn2)),
block([ B42 : SinCosSeries(false, ssig2, csig2, l[l_C4a]), salp12, calp12],
if l[l_calp0] = 0b0 or l[l_salp0] = 0b0 then (
/* alp12 = alp2 - alp1, used in atan2 so no need to normalize */
salp12 : salp2 * l[l_calp1] - calp2 * l[l_salp1],
calp12 : calp2 * l[l_calp1] + salp2 * l[l_salp1])
else (
/* tan(alp) = tan(alp0) * sec(sig)
tan(alp2-alp1) = (tan(alp2) -tan(alp1)) / (tan(alp2)*tan(alp1)+1)
= calp0 * salp0 * (csig1-csig2) / (salp0^2 + calp0^2 * csig1*csig2)
If csig12 > 0, write
csig1 - csig2 = ssig12 * (csig1 * ssig12 / (1 + csig12) + ssig1)
else
csig1 - csig2 = csig1 * (1 - csig12) + ssig12 * ssig1
No need to normalize */
salp12 : l[l_calp0] * l[l_salp0] *
( if csig12 <= 0b0 then l[l_csig1] * (1 - csig12) + ssig12 * l[l_ssig1]
else ssig12 * (l[l_csig1] * ssig12 / (1 + csig12) + l[l_ssig1])),
calp12 : sq(l[l_salp0]) + sq(l[l_calp0]) * l[l_csig1] * csig2),
S12 : l[l_c2] * atan2(salp12, calp12) + l[l_A4] * (B42 - l[l_B41])),
[if arcmode then s12_a12 else sig12 / degree,
lat2, lon2, azi2, s12, m12, M12, M21, S12])$
geod_position(l, s12) := geod_genposition(l, false, s12)$
geod_gendirect(g, lat1, lon1, azi1, arcmode, s12_a12):=
block([l:geod_lineinit(g, lat1, lon1, azi1)],
geod_genposition(l, arcmode, s12_a12))$
geod_direct(g, lat1, lon1, azi1, s12):=
geod_gendirect(g, lat1, lon1, azi1, false, s12)$
/* Return [a12, s12, azi1, azi2, m12, M12, M21, S12] */
geod_geninverse(g, lat1, lon1, lat2, lon2):=block(
[s12 : 0b0, azi1 : 0b0, azi2 : 0b0,
m12 : 0b0, M12 : 0b0, M21 : 0b0, S12 : 0b0,
lon12, lon12s, latsign, lonsign, swapp,
sbet1, cbet1, sbet2, cbet2, s12x : 0b0, m12x : 0b0,
dn1, dn2, lam12, slam12, clam12,
a12 : 0b0, sig12, calp1 : 0b0, salp1 : 0b0, calp2 : 0b0, salp2 : 0b0,
meridian, omg12 : 0b0, somg12 : 2b0, comg12,
/* Initialize for the meridian. No longitude calculation is done in this
case to let the parameter default to 0. */
E : Ef(-g[g_ep2], 0b0)],
lat1:bfloat(lat1),lon1:bfloat(lon1),
lat2:bfloat(lat2),lon2:bfloat(lon2),
/* Compute longitude difference (AngDiff does this carefully). Result is
in [-180, 180] but -180 is only for west-going geodesics. 180 is for
east-going and meridional geodesics. */
lon12 : AngDiff(lon1, lon2), lon12s:lon12[2], lon12:lon12[1],
/* Make longitude difference positive. */
lonsign : if lon12 >= 0b0 then 1 else -1,
/* If very close to being on the same half-meridian, then make it so. */
lon12 : lonsign * AngRound(lon12),
lon12s : AngRound((180 - lon12) - lonsign * lon12s),
lam12 : lon12 * degree,
if lon12 > 90 then block([t:sincosdx(lon12s)], slam12:t[1], clam12:-t[2])
else block([t:sincosdx(lon12)], slam12:t[1], clam12:t[2]),
/* If really close to the equator, treat as on equator. */
lat1 : AngRound(lat1),
lat2 : AngRound(lat2),
/* Swap points so that point with higher (abs) latitude is point 1 */
swapp : if abs(lat1) >= abs(lat2) then 1 else -1,
if swapp < 0 then (lonsign : -lonsign,
block([t:swapx(lat1, lat2)], lat1:t[1], lat2:t[2])),
/* Make lat1 <= 0 */
latsign : if lat1 < 0b0 then 1 else -1,
lat1 : lat1 * latsign,
lat2 : lat2 * latsign,
/* Now we have
0 <= lon12 <= 180
-90 <= lat1 <= 0
lat1 <= lat2 <= -lat1
longsign, swapp, latsign register the transformation to bring the
coordinates to this canonical form. In all cases, 1 means no change was
made. We make these transformations so that there are few cases to
check, e.g., on verifying quadrants in atan2. In addition, this
enforces some symmetries in the results returned. */
block([t:sincosdx(lat1)], sbet1:t[1], cbet1:t[2]), sbet1 : g[g_f1] * sbet1,
block([t:norm2(sbet1, cbet1)], sbet1:t[1], cbet1:t[2]),
/* Ensure cbet1 = +epsilon at poles */
cbet1 = max(tiny, cbet1),
block([t:sincosdx(lat2)], sbet2:t[1], cbet2:t[2]), sbet2 : g[g_f1] * sbet2,
block([t:norm2(sbet2, cbet2)], sbet2:t[1], cbet2:t[2]),
/* Ensure cbet2 = +epsilon at poles */
cbet2 = max(tiny, cbet2),
/* If cbet1 < -sbet1, then cbet2 - cbet1 is a sensitive measure of the
|bet1| - |bet2|. Alternatively (cbet1 >= -sbet1), abs(sbet2) + sbet1 is
a better measure. This logic is used in assigning calp2 in Lambda12.
Sometimes these quantities vanish and in that case we force bet2 = +/-
bet1 exactly. An example where is is necessary is the inverse problem
48.522876735459 0 -48.52287673545898293 179.599720456223079643
which failed with Visual Studio 10 (Release and Debug) */
if cbet1 < -sbet1 then
( if cbet2 = cbet1 then sbet2 : if sbet2 < 0b0 then sbet1 else -sbet1 )
else ( if abs(sbet2) = -sbet1 then cbet2 : cbet1 ),
dn1 : sqrt(1 + g[g_ep2] * sq(sbet1)),
dn2 : sqrt(1 + g[g_ep2] * sq(sbet2)),
meridian : is(lat1 = -90b0 or slam12 = 0b0),
if meridian then block(
/* Endpoints are on a single full meridian, so the geodesic might lie on
a meridian. */
[ ssig1, csig1, ssig2, csig2],
calp1 : clam12, salp1 : slam12, /* Head to the target longitude */
calp2 : 1b0, salp2 : 0b0, /* At the target we're heading north */
/* tan(bet) : tan(sig) * cos(alp) */
ssig1 : sbet1, csig1 : calp1 * cbet1,
ssig2 : sbet2, csig2 : calp2 * cbet2,
/* sig12 = sig2 - sig1 */
sig12 : atan2(0b0 + max(0b0, csig1 * ssig2 - ssig1 * csig2),
csig1 * csig2 + ssig1 * ssig2),
block([r],
r:Lengths(g, g[g_n], E, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2,
cbet1, cbet2),
s12x:r[1], m12x:r[2], M12:r[4], M21:r[5]),
/* Add the check for sig12 since zero length geodesics might yield m12 <
0. Test case was
echo 20.001 0 20.001 0 | GeodTest -i
In fact, we will have sig12 > pi/2 for meridional geodesic which is
not a shortest path. */
if sig12 < 1b0 or m12x >= 0b0 then (
if sig12 < 3 * tiny or
(sig12 < tol0 and (s12x < 0 or m12x < 0)) then
sig12 : m12x : s12x : 0b0,
m12x : m12x * g[g_b],
s12x : s12x * g[g_b],
a12 : sig12 / degree )
else
/* m12 < 0, i.e., prolate and too close to anti-podal */
meridian : false ),
if not meridian and
sbet1 = 0b0 and /* and sbet2 == 0 */
/* Mimic the way Lambda12 works with calp1 = 0 */
(g[g_f] <= 0b0 or lon12s >= g[g_f] * 180) then (
/* Geodesic runs along equator */
calp1 : calp2 : 0b0, salp1 : salp2 : 1b0,
s12x : g[g_a] * lam12,
sig12 : omg12 : lam12 / g[g_f1],
m12x : g[g_b] * sin(sig12),
M12 : M21 : cos(sig12),
a12 : lon12 / g[g_f1] )
else if not meridian then block([dnm],
/* Now point1 and point2 belong within a hemisphere bounded by a
meridian and geodesic is neither meridional or equatorial.
Figure a starting point for Newton's method */
block([r],
r : InverseStart(g, sbet1, cbet1, dn1, sbet2, cbet2, dn2,
lam12, slam12, clam12),
sig12:r[1], salp1:r[2], calp1:r[3], salp2:r[4], calp2:r[5],
dnm:r[6]),
if sig12 >= 0b0 then (
/* Short lines (InverseStart sets salp2, calp2, dnm) */
s12x : sig12 * g[g_b] * dnm,
m12x : sq(dnm) * g[g_b] * sin(sig12 / dnm),
M12 : M21 : cos(sig12 / dnm),
a12 : sig12 / degree,
omg12 : lam12 / (g[g_f1] * dnm))
else block(
/* Newton's method. This is a straightforward solution of f(alp1) =
lambda12(alp1) - lam12 = 0 with one wrinkle. f(alp) has exactly one
root in the interval (0, pi) and its derivative is positive at the
root. Thus f(alp) is positive for alp > alp1 and negative for alp <
alp1. During the course of the iteration, a range (alp1a, alp1b) is
maintained which brackets the root and with each evaluation of
f(alp) the range is shrunk, if possible. Newton's method is
restarted whenever the derivative of f is negative (because the new
value of alp1 is then further from the solution) or if the new
estimate of alp1 lies outside (0,pi); in this case, the new starting
guess is taken to be (alp1a + alp1b) / 2. */
[ssig1 : 0b0, csig1 : 0b0, ssig2 : 0b0, csig2 : 0b0, eps : 0b0,
domg12 : 0b0, numit : 0,
/* Bracketing range */
salp1a : tiny, calp1a : 1b0, salp1b : tiny, calp1b : -1b0,
tripn : false, tripb : false, contflag, dv, v],
for i:0 thru maxit2-1 do (
contflag:false,
numit:i,
/* the WGS84 test set: mean : 1.47, sd = 1.25, max = 16
WGS84 and random input: mean = 2.85, sd = 0.60 */
block([r],
r : Lambda12(g, sbet1, cbet1, dn1, sbet2, cbet2, dn2, salp1, calp1,
slam12, clam12, is(numit < maxit1)),
v : r[1],
salp2:r[2], calp2:r[3], sig12:r[4],
ssig1:r[5], csig1:r[6], ssig2:r[7], csig2:r[8],
if exact then E:r[9] else eps:r[9], domg12:r[10],
dv:r[11]),
/* Reversed test to allow escape with NaNs */
if tripb or not(abs(v) >= (if tripn then 8 else 1) * tol0) then
return(true),
/* Update bracketing values */
if v > 0b0 and (numit > maxit1 or calp1/salp1 > calp1b/salp1b)
then ( salp1b : salp1, calp1b : calp1 )
else if v < 0b0 and (numit > maxit1 or calp1/salp1 < calp1a/salp1a)
then ( salp1a : salp1, calp1a : calp1 ),
if numit < maxit1 and dv > 0b0 then block(
[dalp1, sdalp1, cdalp1,nsalp1],
dalp1 : -v/dv,
sdalp1 : sin(dalp1), cdalp1 : cos(dalp1),
nsalp1 : salp1 * cdalp1 + calp1 * sdalp1,
if nsalp1 > 0b0 and abs(dalp1) < pi then (
calp1 : calp1 * cdalp1 - salp1 * sdalp1,
salp1 : nsalp1,
block([t:norm2(salp1, calp1)], salp1:t[1], calp1:t[2]),
/* In some regimes we don't get quadratic convergence because
slope -> 0. So use convergence conditions based on epsilon
instead of sqrt(epsilon). */
tripn : is(abs(v) <= 16 * tol0),
contflag:true ) ),
if not contflag then (
/* Either dv was not positive or updated value was outside legal
range. Use the midpoint of the bracket as the next estimate.
This mechanism is not needed for the WGS84 ellipsoid, but it does
catch problems with more eccentric ellipsoids. Its efficacy is
such for the WGS84 test set with the starting guess set to alp1 =
90deg:
the WGS84 test set: mean = 5.21, sd = 3.93, max = 24
WGS84 and random input: mean = 4.74, sd = 0.99 */
salp1 : (salp1a + salp1b)/2,
calp1 : (calp1a + calp1b)/2,
block([t:norm2(salp1, calp1)], salp1:t[1], calp1:t[2]),
tripn : false,
tripb : is(abs(salp1a - salp1) + (calp1a - calp1) < tolb or
abs(salp1 - salp1b) + (calp1 - calp1b) < tolb)) ),
block([r],
r:Lengths(g, eps, E, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2,
cbet1, cbet2),
s12x:r[1], m12x:r[2], M12:r[4], M21:r[5]),
m12x : m12x * g[g_b],
s12x : s12x * g[g_b],
a12 : sig12 / degree,
block([sdomg12 : sin(domg12), cdomg12 : cos(domg12)],
somg12 : slam12 * cdomg12 - clam12 * sdomg12,
comg12 : clam12 * cdomg12 + slam12 * sdomg12) ) ),
s12 : 0b0 + s12x, /* Convert -0 to 0 */
m12 : 0b0 + m12x, /* Convert -0 to 0 */
block(
/* From Lambda12: sin(alp1) * cos(bet1) = sin(alp0) */
[ salp0 : salp1 * cbet1,
calp0 : hypotx(calp1, salp1 * sbet1), /* calp0 > 0 */
alp12],
if calp0 # 0b0 and salp0 # 0b0 then block(
/* From Lambda12: tan(bet) = tan(sig) * cos(alp) */
[ssig1 : sbet1, csig1 : calp1 * cbet1,
ssig2 : sbet2, csig2 : calp2 * cbet2,
k2 : sq(calp0) * g[g_ep2], eps,
/* Multiplier = a^2 * e^2 * cos(alpha0) * sin(alpha0). */
A4 : sq(g[g_a]) * calp0 * salp0 * g[g_e2],
Ca, B41, B42],
eps : k2 / (2 * (1 + sqrt(1 + k2)) + k2),
block([t:norm2(ssig1, csig1)], ssig1:t[1], csig1:t[2]),
block([t:norm2(ssig2, csig2)], ssig2:t[1], csig2:t[2]),
Ca : C4f(g, eps),
B41 : SinCosSeries(false, ssig1, csig1, Ca),
B42 : SinCosSeries(false, ssig2, csig2, Ca),
S12 : A4 * (B42 - B41))
else S12 : 0, /* Avoid problems with indeterminate sig1, sig2 on equator */
if not meridian and somg12 > 1 then
(somg12 : sin(omg12), comg12 : cos(omg12)),
if not meridian and
/* omg12 < 3/4 * pi */
comg12 > -0.7071b0 and /* Long difference not too big */
sbet2 - sbet1 < 1.75b0 then block( /* Lat difference not too big */
/* Use tan(Gamma/2) = tan(omg12/2)
* (tan(bet1/2)+tan(bet2/2))/(1+tan(bet1/2)*tan(bet2/2))
with tan(x/2) = sin(x)/(1+cos(x)) */
[domg12 : 1 + comg12, dbet1 : 1 + cbet1, dbet2 : 1 + cbet2],
alp12 : 2 * atan2( somg12 * ( sbet1 * dbet2 + sbet2 * dbet1 ),
domg12 * ( sbet1 * sbet2 + dbet1 * dbet2 ) ))
else block(
/* alp12 = alp2 - alp1, used in atan2 so no need to normalize */
[salp12 : salp2 * calp1 - calp2 * salp1,
calp12 : calp2 * calp1 + salp2 * salp1],
/* The right thing appears to happen if alp1 = +/-180 and alp2 = 0, viz
salp12 = -0 and alp12 = -180. However this depends on the sign
being attached to 0 correctly. The following ensures the correct
behavior. */
if salp12 = 0b0 and calp12 < 0b0 then (
salp12 : tiny * calp1,
calp12 : -1b0),
alp12 : atan2(salp12, calp12) ),
S12 : S12 + g[g_c2] * alp12,
S12 : S12 * swapp * lonsign * latsign,
/* Convert -0 to 0 */
S12 : S12 + 0b0 ),
/* Convert calp, salp to azimuth accounting for lonsign, swapp, latsign. */
if swapp < 0 then (
block([t:swapx(salp1, salp2)], salp1:t[1], salp2:t[2]),
block([t:swapx(calp1, calp2)], calp1:t[1], calp2:t[2]),
block([t:swapx(M12, M21)], M12:t[1], M21:t[2])),
salp1 : salp1 * swapp * lonsign, calp1 : calp1 * swapp * latsign,
salp2 : salp2 * swapp * lonsign, calp2 : calp2 * swapp * latsign,
azi1 : atan2dx(salp1, calp1),
azi2 : atan2dx(salp2, calp2),
[a12, s12, azi1, azi2, m12, M12, M21, S12]
)$
geod_inverse(g, lat1, lon1, lat2, lon2):=
geod_geninverse(g, lat1, lon1, lat2, lon2)$
SinCosSeries(sinp, sinx, cosx, c):=block([n:length(c), ar, y0, y1, n2, k],
/* Evaluate
y = sinp ? sum(c[i] * sin( 2*i * x), i, 1, n) :
sum(c[i] * cos((2*i-1) * x), i, 1, n)
using Clenshaw summation. where n = length(c)
Approx operation count = (n + 5) mult and (2 * n + 2) add */
/* 2 * cos(2 * x) */
ar : 2 * (cosx - sinx) * (cosx + sinx),
/* accumulators for sum */
if mod(n, 2)=1 then (y0 : c[n], n2 : n - 1)
else (y0 : 0b0, n2 : n),
y1 : 0b0,
/* Now n2 is even */
for k : n2 thru 1 step -2 do (
/* Unroll loop x 2, so accumulators return to their original role */
y1 : ar * y0 - y1 + c[k],
y0 : ar * y1 - y0 + c[k-1]),
if sinp then 2 * sinx * cosx * y0 /* sin(2 * x) * y0 */
else cosx * (y0 - y1) /* cos(x) * (y0 - y1) */
)$
/* Return [s12b, m12b, m0, M12, M21] */
Lengths(g, eps, E, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2, cbet1, cbet2):=
block([Ca,
s12b : 0b0, m12b : 0b0, m0 : 0b0, M12 : 0b0, M21 : 0b0,
A1m1, AB1, A2m1, AB2, J12],
/* Return m12b = (reduced length)/b; also calculate s12b = distance/b,
and m0 = coefficient of secular term in expression for reduced length. */
if exact then (
m0 : - E[e_k2] * E[e_dc] / (pi/2),
J12 : m0 *
(sig12 + deltad(ssig2, csig2, dn2, E[e_k2], E[e_dc])
- deltad(ssig1, csig1, dn1, E[e_k2], E[e_dc])) )
else (
A1m1 : A1m1f(eps), Ca : C1f(eps),
AB1 : (1 + A1m1) * (SinCosSeries(true, ssig2, csig2, Ca) -
SinCosSeries(true, ssig1, csig1, Ca)),
A2m1 : A2m1f(eps), Ca : C2f(eps),
AB2 : (1 + A2m1) * (SinCosSeries(true, ssig2, csig2, Ca) -
SinCosSeries(true, ssig1, csig1, Ca)),
m0 : A1m1 - A2m1,
J12 : m0 * sig12 + (AB1 - AB2)),
/* Missing a factor of b.
Add parens around (csig1 * ssig2) and (ssig1 * csig2) to ensure accurate
cancellation in the case of coincident points. */
m12b : dn2 * (csig1 * ssig2) - dn1 * (ssig1 * csig2) - csig1 * csig2 * J12,
/* Missing a factor of b */
s12b : if exact then
E[e_ec] / (pi/2) *
(sig12 + deltae(ssig2, csig2, dn2, E[e_k2], E[e_ec])
- deltae(ssig1, csig1, dn1, E[e_k2], E[e_ec]))
else (1 + A1m1) * sig12 + AB1,
block([csig12 : csig1 * csig2 + ssig1 * ssig2,
t : g[g_ep2] * (cbet1 - cbet2) * (cbet1 + cbet2) / (dn1 + dn2)],
M12 : csig12 + (t * ssig2 - csig2 * J12) * ssig1 / dn1,
M21 : csig12 - (t * ssig1 - csig1 * J12) * ssig2 / dn2 ),
[s12b, m12b, m0, M12, M21])$
Astroid(x, y):= block(
/* Solve k^4+2*k^3-(x^2+y^2-1)*k^2-2*y^2*k-y^2 = 0 for positive root k.
This solution is adapted from Geocentric::Reverse. */
[k, p : sq(x), q : sq(y), r],
r : (p + q - 1) / 6,
if not(q = 0b0 and r <= 0b0) then block(
/* Avoid possible division by zero when r = 0 by multiplying equations
for s and t by r^3 and r, resp. */
[S : p * q / 4, /* S = r^3 * s */
r2 : sq(r),
r3, disc, u, v, uv, w],
r3 : r * r2,
/* The discriminant of the quadratic equation for T3. This is zero on
the evolute curve p^(1/3)+q^(1/3) = 1 */
disc : S * (S + 2 * r3),
u : r,
v, uv, w,
if disc >= 0b0 then block([T3 : S + r3, T],
/* Pick the sign on the sqrt to maximize abs(T3). This minimizes loss
of precision due to cancellation. The result is unchanged because
of the way the T is used in definition of u. */
/* T3 = (r * t)^3 */
T3 : T3 + (if T3 < 0b0 then -sqrt(disc) else sqrt(disc)),
/* N.B. cbrtx always returns the real root. cbrtx(-8) = -2. */
T : cbrtx(T3), /* T = r * t */
/* T can be zero, but then r2 / T -> 0. */
u : u + T + (if T # 0b0 then r2 / T else 0b0))
else block(
/* T is complex, but the way u is defined the result is real. */
[ang : atan2(sqrt(-disc), -(S + r3))],
/* There are three possible cube roots. We choose the root which
avoids cancellation. Note that disc < 0 implies that r < 0. */
u : u + 2 * r * cos(ang / 3)),
v : sqrt(sq(u) + q), /* guaranteed positive */
/* Avoid loss of accuracy when u < 0. */
uv : if u < 0b0 then q / (v - u) else u + v, /* u+v, guaranteed positive */
w : (uv - q) / (2 * v), /* positive? */
/* Rearrange expression for k to avoid loss of accuracy due to
subtraction. Division by 0 not possible because uv > 0, w >= 0. */
k : uv / (sqrt(uv + sq(w)) + w)) /* guaranteed positive */
else /* q == 0 && r <= 0 */
/* y = 0 with |x| <= 1. Handle this case directly.
for y small, positive root is k = abs(y)/sqrt(1-x^2) */
k : 0b0,
k)$
/* Return [sig12, salp1, calp1, salp2, calp2, dnm] */
InverseStart(g, sbet1, cbet1, dn1, sbet2, cbet2, dn2,
lam12, slam12, clam12):=block(
[ salp1 : 0b0, calp1 : 0b0, salp2 : 0b0, calp2 : 0b0, dnm : 0b0,
/* Return a starting point for Newton's method in salp1 and calp1 (function
value is -1). If Newton's method doesn't need to be used, return also
salp2 and calp2 and function value is sig12. */
sig12 : -1b0, /* Return value */
/* bet12 = bet2 - bet1 in [0, pi); bet12a = bet2 + bet1 in (-pi, 0] */
sbet12 : sbet2 * cbet1 - cbet2 * sbet1,
cbet12 : cbet2 * cbet1 + sbet2 * sbet1,
sbet12a : sbet2 * cbet1 + cbet2 * sbet1,
shortline, somg12, comg12, ssig12, csig12, E:[]],
shortline : is(cbet12 >= 0b0 and sbet12 < 0.5b0 and cbet2 * lam12 < 0.5b0),
if shortline then block([sbetm2 : sq(sbet1 + sbet2), omg12],
/* sin((bet1+bet2)/2)^2
= (sbet1 + sbet2)^2 / ((sbet1 + sbet2)^2 + (cbet1 + cbet2)^2) */
sbetm2 : sbetm2 / (sbetm2 + sq(cbet1 + cbet2)),
dnm : sqrt(1 + g[g_ep2] * sbetm2),
omg12 : lam12 / (g[g_f1] * dnm),
somg12 : sin(omg12), comg12 : cos(omg12))
else (somg12 : slam12, comg12 : clam12),
salp1 : cbet2 * somg12,
calp1 : if comg12 >= 0b0
then sbet12 + cbet2 * sbet1 * sq(somg12) / (1 + comg12)
else sbet12a - cbet2 * sbet1 * sq(somg12) / (1 - comg12),
ssig12 : hypotx(salp1, calp1),
csig12 : sbet1 * sbet2 + cbet1 * cbet2 * comg12,
if shortline and ssig12 < g[g_etol2] then (
/* really short lines */
salp2 : cbet1 * somg12,
calp2 : sbet12 - cbet1 * sbet2 *
(if comg12 >= 0 then sq(somg12) / (1 + comg12) else 1 - comg12),
block([t:norm2(salp2, calp2)], salp2:t[1], calp2:t[2]),
/* Set return value */
sig12 : atan2(ssig12, csig12))
else if abs(g[g_n]) > 0.1b0 or /* No astroid calc if too eccentric */
csig12 >= 0 or
ssig12 >= 6 * abs(g[g_n]) * pi * sq(cbet1) then true
/* Nothing to do, zeroth order spherical approximation is OK */
else block(
/* Scale lam12 and bet2 to x, y coordinate system where antipodal point
is at origin and singular point is at y = 0, x = -1. */
[y, lamscale, betscale,x, lam12x],
lam12x : atan2(-slam12, -clam12), /* lam12 - pi */
if g[g_f] >= 0 then ( /* In fact f == 0 does not get here */
/* x = dlong, y = dlat */
if exact then block([k2 : sq(sbet1) * g[g_ep2]],
E : Ef(-k2, -g[g_ep2]),
lamscale : g[g_e2]/g[g_f1] * cbet1 * 2 * E[e_hc])
else block([k2 : sq(sbet1) * g[g_ep2],eps],
eps : k2 / (2 * (1 + sqrt(1 + k2)) + k2),
lamscale : g[g_f] * cbet1 * A3f(g, eps) * pi),
betscale : lamscale * cbet1,
x : lam12x / lamscale,
y : sbet12a / betscale)
else block( /* f < 0 */
/* x = dlat, y = dlong */
[cbet12a : cbet2 * cbet1 - sbet2 * sbet1,bet12a,m12b, m0],
bet12a : atan2(sbet12a, cbet12a),
/* In the case of lon12 = 180, this repeats a calculation made in
* Inverse. */
block([r],
r:Lengths(g, g[g_n], E, pi + bet12a,
sbet1, -cbet1, dn1, sbet2, cbet2, dn2,
cbet1, cbet2),
m12b:r[2], m0:r[3]),
x : -1 + m12b / (cbet1 * cbet2 * m0 * pi),
betscale : if x < -0.01b0 then sbet12a / x else
-g[g_f] * sq(cbet1) * pi,
lamscale : betscale / cbet1,
y : lam12x / lamscale),
if y > -tol1 and x > -1 - xthresh then (
/* strip near cut */
if g[g_f] >= 0b0 then (
salp1 : min(1b0, -x), calp1 : - sqrt(1 - sq(salp1)))
else (
calp1 : max(if x > -tol1 then 0b0 else -1b0, x),
salp1 : sqrt(1 - sq(calp1))))
else block(
/* Estimate alp1, by solving the astroid problem.
Could estimate alpha1 = theta + pi/2, directly, i.e.,
calp1 = y/k; salp1 = -x/(1+k); for f >= 0
calp1 = x/(1+k); salp1 = -y/k; for f < 0 (need to check)
However, it's better to estimate omg12 from astroid and use
spherical formula to compute alp1. This reduces the mean number of
Newton iterations for astroid cases from 2.24 (min 0, max 6) to 2.12
(min 0 max 5). The changes in the number of iterations are as
follows:
change percent
1 5
0 78
-1 16
-2 0.6
-3 0.04
-4 0.002
The histogram of iterations is (m = number of iterations estimating
alp1 directly, n = number of iterations estimating via omg12, total
number of trials = 148605):
iter m n
0 148 186
1 13046 13845
2 93315 102225
3 36189 32341
4 5396 7
5 455 1
6 56 0
Because omg12 is near pi, estimate work with omg12a = pi - omg12 */
[k : Astroid(x, y),omg12a],
omg12a : lamscale *
( if g[g_f] >= 0b0 then -x * k/(1 + k) else -y * (1 + k)/k ),
somg12 : sin(omg12a), comg12 : -cos(omg12a),
/* Update spherical estimate of alp1 using omg12 instead of lam12 */
salp1 : cbet2 * somg12,
calp1 : sbet12a - cbet2 * sbet1 * sq(somg12) / (1 - comg12))),
if salp1 > 0 then /* Sanity check on starting guess */
block([t:norm2(salp1, calp1)], salp1:t[1], calp1:t[2])
else ( salp1 : 1b0, calp1 : 0b0 ),
[sig12, salp1, calp1, salp2, calp2, dnm]
)$
/* Return [lam12, salp2, calp2, sig12, ssig1, csig1, ssig2, csig2,
(E or eps), domg12, dlam12]
*/
Lambda12(g, sbet1, cbet1, dn1, sbet2, cbet2, dn2, salp1, calp1,
slam120, clam120, diffp):=
block([salp2 : 0b0, calp2 : 0b0, sig12 : 0b0,
ssig1 : 0b0, csig1 : 0b0, ssig2 : 0b0, csig2 : 0b0, eps : 0b0, E : [],
domg12 : 0b0, somg12 : 0b0, comg12 : 0b0, dlam12 : 0b0,
salp0, calp0,
somg1, comg1, somg2, comg2, cchi1, cchi2, lam12,
B312, k2, Ca, eta],
if sbet1 = 0b0 and calp1 = 0b0 then
/* Break degeneracy of equatorial line. This case has already been
handled. */
calp1 : -tiny,
/* sin(alp1) * cos(bet1) = sin(alp0) */
salp0 : salp1 * cbet1,
calp0 : hypotx(calp1, salp1 * sbet1), /* calp0 > 0 */
/* tan(bet1) = tan(sig1) * cos(alp1)
tan(omg1) = sin(alp0) * tan(sig1) = tan(omg1)=tan(alp1)*sin(bet1) */
ssig1 : sbet1, somg1 : salp0 * sbet1,
csig1 : comg1 : calp1 * cbet1,
/* Without normalization we have schi1 = somg1. */
if exact then cchi1 : g[g_f1] * dn1 * comg1,
block([t:norm2(ssig1, csig1)], ssig1:t[1], csig1:t[2]),
/* norm2 (&somg1, &comg1); -- don't need to normalize! */
/* Enforce symmetries in the case abs(bet2) = -bet1. Need to be careful
about this case, since this can yield singularities in the Newton
iteration.
sin(alp2) * cos(bet2) = sin(alp0) */
salp2 : if cbet2 # cbet1 then salp0 / cbet2 else salp1,
/* calp2 = sqrt(1 - sq(salp2))
= sqrt(sq(calp0) - sq(sbet2)) / cbet2
and subst for calp0 and rearrange to give; choose positive sqrt
to give alp2 in [0, pi/2]. */
calp2 : if cbet2 # cbet1 or abs(sbet2) # -sbet1 then
sqrt(sq(calp1 * cbet1) +
(if cbet1 < -sbet1 then
(cbet2 - cbet1) * (cbet1 + cbet2) else
(sbet1 - sbet2) * (sbet1 + sbet2))) / cbet2 else
abs(calp1),
/* tan(bet2) = tan(sig2) * cos(alp2)
tan(omg2) = sin(alp0) * tan(sig2). */
ssig2 : sbet2, somg2 : salp0 * sbet2,
csig2 : comg2 : calp2 * cbet2,
/* Without normalization we have schi2 = somg2. */
if exact then cchi2 : g[g_f1] * dn2 * comg2,
block([t:norm2(ssig2, csig2)], ssig2:t[1], csig2:t[2]),
/* norm2(&somg2, &comg2); -- don't need to normalize! */
/* sig12 = sig2 - sig1, limit to [0, pi] */
sig12 : atan2(0b0 + max(0b0, csig1 * ssig2 - ssig1 * csig2),
csig1 * csig2 + ssig1 * ssig2),
/* omg12 = omg2 - omg1, limit to [0, pi] */
somg12 : 0b0 + max(0b0, comg1 * somg2 - somg1 * comg2),
comg12 : comg1 * comg2 + somg1 * somg2,
k2 : sq(calp0) * g[g_ep2],
if exact then block([schi12, cchi12, deta12],
E : Ef(-k2, -g[g_ep2]),
schi12 : 0b0 + max(0b0, cchi1 * somg2 - somg1 * cchi2),
cchi12 : cchi1 * cchi2 + somg1 * somg2,
/* eta = chi12 - lam120 */
eta : atan2(schi12 * clam120 - cchi12 * slam120,
cchi12 * clam120 + schi12 * slam120),
deta12 : -g[g_e2]/g[g_f1] * salp0 * E[e_hc] / (pi/2) *
(sig12 + deltah(ssig2, csig2, dn2, E[e_k2], E[e_alpha2], E[e_hc]) -
deltah(ssig1, csig1, dn1, E[e_k2], E[e_alpha2], E[e_hc]) ),
lam12 : eta + deta12,
domg12 : deta12 + atan2(schi12 * comg12 - cchi12 * somg12,
cchi12 * comg12 + schi12 * somg12))
else ( /* eta = omg12 - lam120 */
eta : atan2(somg12 * clam120 - comg12 * slam120,
comg12 * clam120 + somg12 * slam120),
eps : k2 / (2 * (1 + sqrt(1 + k2)) + k2),
Ca : C3f(g, eps),
B312 : (SinCosSeries(true, ssig2, csig2, Ca) -
SinCosSeries(true, ssig1, csig1, Ca)),
domg12 : -g[g_f] * A3f(g, eps) * salp0 * (sig12 + B312),
lam12 : eta + domg12),
if diffp then (
if calp2 = 0b0 then
dlam12 : - 2 * g[g_f1] * dn1 / sbet1
else (block([r],
r:Lengths(g, eps, E, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2,
cbet1, cbet2),
dlam12:r[2]),
dlam12 : dlam12 * g[g_f1] / (calp2 * cbet2))),
[lam12, salp2, calp2, sig12, ssig1, csig1, ssig2, csig2,
if exact then E else eps, domg12, dlam12])$
A3f(g, eps):=block(
/* Evaluate sum(A3x[k] * eps^k, k, 0, nA3x-1) by Horner's method */
[v : 0b0],
for i:nA3x step -1 thru 1 do
v : eps * v + g[g_A3x][i],
v)$
C3f(g, eps):=block(
/* Evaluate C3 coeffs by Horner's method
* Elements c[1] thru c[nC3 - 1] are set */
[i, j, k, mult : 1b0, c : makelist(0, i, 1, nC3-1)],
j : nC3x,
for k : nC3-1 step -1 thru 1 do block(
[t : 0b0],
for i : nC3 - k step -1 thru 1 do (
t : eps * t + g[g_C3x][j],
j : j - 1),
c[k] : t),
for k:1 thru nC3-1 do (
mult : mult * eps,
c[k] : c[k] * mult),
c)$
C4f(g, eps):=block(
/* Evaluate C4 coeffs by Horner's method
* Elements c[1] thru c[nC4] are set */
[i, j, k, mult : 1b0, c : makelist(0, i, 1, nC4)],
j : nC4x,
for k : nC4-1 step -1 thru 0 do block(
[t : 0b0],
for i : nC4 - k step -1 thru 1 do (
t : eps * t + g[g_C4x][j],
j : j - 1),
c[k+1] : t),
for k : 2 thru nC4 do (
mult : mult * eps,
c[k] : c[k] * mult),
c)$
transit(lon1, lon2):=block([lon12],
/* Return 1 or -1 if crossing prime meridian in east or west direction.
Otherwise return zero. */
/* Compute lon12 the same way as Geodesic::Inverse. */
lon1 : AngNormalize(lon1),
lon2 : AngNormalize(lon2),
lon12 : AngDiff(lon1, lon2)[1],
if lon1 <= 0b0 and lon2 > 0b0 and lon12 > 0b0 then 1 else
(if lon2 <= 0b0 and lon1 > 0b0 and lon12 < 0b0 then -1 else 0))$
/* Return [P, A, mins, maxs] */
geod_polygonarea(g, points) := block([n:length(points), crossings : 0,
area0 : 4 * pi * g[g_c2], A : 0, P : 0, mins : g[g_a] * 100, maxs : 0b0],
for i : 1 thru n do block(
[ s12, S12, r ],
r:geod_geninverse(g,
points[i][1], points[i][2],
points[mod(i,n)+1][1], points[mod(i,n)+1][2]),
s12:r[2], S12:r[8],
if s12 > maxs then maxs:s12,
if s12 < mins then mins:s12,
P : P + s12,
/* The minus sign is due to the counter-clockwise convention */
A : A - S12,
crossings : crossings + transit(points[i][2], points[mod(i,n)+1][2])),
A : mod(A+area0/2, area0) - area0/2,
if mod(crossings, 2) = 1 then
A : A + (if A < 0b0 then 1 else -1) * area0/2,
/* Put area in (-area0/2, area0/2] */
if A > area0/2 then
A : A - area0
else if A <= -area0/2 then
A : A + area0,
[P, A, mins, maxs])$
wgs84:geod_init(6378137b0, 1/298.257223563b0)$
flat(a, GM, omega, J2):=block(
[e2:3*J2, K : 2 * (a*omega)^2 * a / (15 * GM), e2a, q0],
for j:0 thru 100 do (
e2a:e2,q0:qf(e2/(1-e2)),
e2:3*J2+K*e2*sqrt(e2)/q0,
if e2 = e2a then return(done)),
e2/(1+sqrt(1-e2)))$
qf(ep2):=block([ep,fpprec:3*fpprec],ep:sqrt(ep2),
((1 + 3/ep2) * atan(ep) - 3/ep)/2)$
/* 1/298.257222100882711243162836607614495 */
fgrs80:flat(6378137b0, 3986005b8, 7292115b-11, 108263b-8)$
grs80:geod_init(6378137b0, fgrs80)$
Reference in New Issue View Git Blame Copy Permalink