henry/SimCore
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ADD: new track message, Entity class and Position class

Browse Source
This commit is contained in:
Henry Winkel
2022-12-20 17:20:35 +01:00
parent 469ecfb099
commit 98ebb563a8
2114 changed files with 482360 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

399
libs/geographiclib/maxima/tm.mac Normal file
View File

@@ -0,0 +1,399 @@
/*
Arbitrary precision Transverse Mercator Projection
Copyright (c) Charles Karney (2009-2017) <charles@karney.com> and
licensed under the MIT/X11 License. For more information, see
https://geographiclib.sourceforge.io/
Reference:
Charles F. F. Karney,
Transverse Mercator with an accuracy of a few nanometers,
J. Geodesy 85(8), 475-485 (Aug. 2011).
DOI 10.1007/s00190-011-0445-3
preprint https://arxiv.org/abs/1002.1417
resource page https://geographiclib.sourceforge.io/tm.html
The parameters for the transformation are set by
setparams(a,f,k0)$
sets the equatorial radius, inverse flattening, and central scale
factor. The default is
setparams(6378137b0, 1/298.257223563b0, 0.9996b0)$
appropriate for UTM applications.
tm(lat,lon);
takes lat and lon args (in degrees) and returns
[x, y, convergence, scale]
[x, y] do not include false eastings/northings but do include the
scale factor k0. convergence is in degrees.
ll(x,y);
takes x and y args (in meters) and returns
[lat, lon, convergence, scale].
Example:
$ maxima
Maxima 5.15.0 http://maxima.sourceforge.net
Using Lisp CLISP 2.43 (2007-11-18)
Distributed under the GNU Public License. See the file COPYING.
Dedicated to the memory of William Schelter.
The function bug_report() provides bug reporting information.
(%i1) load("tm.mac")$
(%i2) tm(10b0,20b0);
(%o2) [2.235209504622466691587930831718465965864199221939781808953597771095103\
6690000464b6, 1.17529734503138466792126931904154130080533935727351398258511134\
68541970512119385b6, 3.6194756227592979778565787394402350354250845160819430786\
093514889500602612857052b0, 1.062074627142564335518604915718789933200854739344\
8664109599248189291146283796933b0]
(%i3) ll(%[1],%[2]);
(%o3) [1.0b1, 2.0b1, 3.6194756227592979778565787394402350354250845160819430786\
093514889500602612857053b0, 1.062074627142564335518604915718789933200854739344\
8664109599248189291146283796933b0]
(%i4) float(%o2);
(%o4) [2235209.504622467, 1175297.345031385, 3.619475622759298,
1.062074627142564]
(%i5) float(%o3);
(%o5) [10.0, 20.0, 3.619475622759298, 1.062074627142564]
This implements GeographicLib::TransverseMercatorExact (i.e., Lee, 1976)
using bfloats. However fewer changes from Lee 1976 have been made since
we rely more heavily on the high precision to deal with problem cases.
To change the precision, change fpprec below and reload.
*/
fpprec:80$
load("ellint.mac")$ /* Load elliptic functions */
tol:0.1b0^fpprec$
tol1:0.1b0*sqrt(tol)$ /* For Newton's method */
tol2:sqrt(0.01*tol*tol1)$ /* Also for Newton's method but more conservative */
ahypover:log(10b0^fpprec)+2$
pi:bfloat(%pi)$
degree:pi/180$
ratprint:false$
debugprint:false$
setparams(a1,f1,k1):=(a:bfloat(a1),f:bfloat(f1),k0:bfloat(k1),
e2:f*(2-f),
e:sqrt(e2),
kcu:kc(e2),
kcv:kc(1-e2),
ecu:ec(e2),
ecv:ec(1-e2),
n:f/(2-f),
'done)$
setparams(6378137b0, 1/298.257223563b0, 0.9996b0)$ /* WGS 84 */
/* setparams(6378388b0, 1/297b0, 0.9996b0)$ International */
/* setparams(1/ec(0.01b0), 1/(30*sqrt(11b0)+100), 1b0)$ testing, eps = 0.1*/
/*
Interpret x_y(y) as x <- y, i.e., "transform quantity y to quantity x"
Let
phi = geodetic latitude
psi = isometric latitude ( + i * lambda )
sigma = TM coords
thom = Thompson coords
*/
/* sqrt(x^2 + y^2) -- Real only */
hypot(x,y):=sqrt(x^2 + y^2)$
/* log(1 + x) -- Real only */
log1p(x) := block([y : 1b0+x],
if y = 1b0 then x else x*log(y)/(y - 1))$
/* Real only */
/* Some versions of Maxima have a buggy atanh
atnh(x) := block([y : abs(x)],
y : log1p(2 * y/(1 - y))/2,
if x < 0 then -y else y)$ */
atnh(x) := atanh(x)$
/* exp(x)-1 -- Real only */
expm1(x) := block([y : exp(bfloat(x)),z],
z : y - 1b0,
if abs(x) > 1b0 then z else if z = 0b0 then x else x * z/log(y))$
/* Real only */
/* Some versions of Maxima have a buggy sinh */
snh(x) := block([u : expm1(x)],
(u / (u + 1)) * (u + 2) /2);
/* Real only */
psi_phi(phi):=block([s:sin(phi)],
asinh(s/max(cos(phi),0.1b0*tol)) - e * atnh(e * s))$
/* Real only */
phi_psi(psi):=block([q:psi,t,dq],
for i do (
t:tanh(q),
dq : -(q - e * atnh(e * t) - psi) * (1 - e2 * t^2) / (1 - e2),
q : q + dq,
if debugprint then print(float(q), float(dq)),
if abs(dq) < tol1 then return(false)),
atan(snh(q)))$
psi_thom_comb(w):=block([jacr:sncndn(bfloat(realpart(w)),1-e2),
jaci:sncndn(bfloat(imagpart(w)),e2),d,d1,d2],
d:(1-e2)*(jaci[2]^2 + e2 * (jacr[1] * jaci[1])^2)^2,
d1:sqrt(jacr[2]^2 + (1-e2) * (jacr[1] * jaci[1])^2),
d2:sqrt(e2 * jacr[2]^2 + (1-e2) * jaci[2]^2),
[
(if d1 > 0b0 then asinh(jacr[1]*jaci[3]/ d1) else signnum(snu) * ahypover)
- (if d2 > 0b0 then e * asinh(e * jacr[1] / d2) else signnum(snu) * ahypover)
+ %i * (if d1 > 0b0 and d2 > 0b0 then
atan2(jacr[3]*jaci[1],jacr[2]*jaci[2])
- e * atan2(e*jacr[2]*jaci[1],jacr[3]*jaci[2]) else 0),
jacr[2]*jacr[3]*jaci[3]*(jaci[2]^2-e2*(jacr[1]*jaci[1])^2)/d
-%i * jacr[1]*jaci[1]*jaci[2]*((jacr[3]*jaci[3])^2+e2*jacr[2]^2)/d]
)$
psi_thom(w):=block([tt:psi_thom_comb(w)],tt[1])$
inv_diff_psi_thom(w):=block([tt:psi_thom_comb(w)],tt[2])$
w0a(psi):=block([lam:bfloat(imagpart(psi)),psia:bfloat(realpart(psi))],
rectform(kcu/(pi/2)*( atan2(snh(psia),cos(lam))
+%i*asinh(sin(lam)/sqrt(cos(lam)^2 + snh(psia)^2)))))$
w0c(psi):=block([m,a,dlam],
dlam:bfloat(imagpart(psi))-pi/2*(1-e),
psi:bfloat(realpart(psi)),
m:sqrt(psi^2+dlam^2)*3/(1-e2)/e,
a:if m = 0b0 then 0 else atan2(dlam-psi, psi+dlam) - 0.75b0*pi,
m:m^(1/3), a:a/3,
m*cos(a)+%i*(m*sin(a)+kcv))$
w0d(psi):=block([psir:-realpart(psi)/e+1b0,lam:(pi/2-imagpart(psi))/e,uu,vv],
uu:asinh(sin(lam)/sqrt(cos(lam)^2+snh(psir)^2))*(1+e2/2),
vv:atan2(cos(lam), snh(psir)) *(1+e2/2),
(-uu+kcu) + %i * (-vv+kcv))$
w0m(psi):=if realpart(psi)<-e/2*pi/2 and
imagpart(psi)>pi/2*(1-2*e) and
realpart(psi) < imagpart(psi)-(pi/2*(1-e)) then w0d(psi) else
if realpart(psi)<e*pi/2 and imagpart(psi)>pi/2*(1-2*e)
then w0c(psi) else w0a(psi)$
w0(psi):=w0m(psi)$
thom_psi(psi):=block([w:w0(psi),dw,v,vv],
if not(abs(psi-pi/2*(1-e)*%i) < e * tol^0.6b0) then
for i do (
if i > 100 then error("too many iterations"),
vv:psi_thom_comb(w),
v:vv[1],
dw:-rectform((v-psi)*vv[2]),
w:w+dw,
dw:abs(dw),
if debugprint then print(float(w),float(dw)),
/* error is approx dw^2/2 */
if dw < tol2 then return(false)
),
w
)$
sigma_thom_comb(w):=block([u:bfloat(realpart(w)),v:bfloat(imagpart(w)),
jacr,jaci,phi,iu,iv,den,den1,er,ei,dnr,dni],
jacr:sncndn(u,1-e2),jaci:sncndn(v,e2),
er:eirx(jacr[1],jacr[2],jacr[3],e2,ecu),
ei:eirx(jaci[1],jaci[2],jaci[3],1-e2,ecv),
den:e2*jacr[2]^2+(1-e2)*jaci[2]^2,
den1:(1-e2)*(jaci[2]^2 + e2 * (jacr[1] * jaci[1])^2)^2,
dnr:jacr[3]*jaci[2]*jaci[3],
dni:-e2*jacr[1]*jacr[2]*jaci[1],
[ er - e2*jacr[1]*jacr[2]*jacr[3]/den
+ %i*(v - ei + (1-e2)*jaci[1]*jaci[2]*jaci[3]/den),
(dnr^2-dni^2)/den1 + %i * 2*dnr*dni/den1])$
sigma_thom(w):=block([tt:sigma_thom_comb(w)],tt[1])$
inv_diff_sigma_thom(w):=block([tt:sigma_thom_comb(w)],tt[2])$
wx0a(sigma):=rectform(sigma*kcu/ecu)$
wx0b(sigma):=block([m,aa],
sigma:rectform(sigma-%i*(kcv-ecv)),
m:abs(sigma)*3/(1-e2),
aa:atan2(imagpart(sigma),realpart(sigma)),
if aa<-pi/2 then aa:aa+2*pi,
aa:aa-pi,
rectform(m^(1/3)*(cos(aa/3b0)+%i*sin(aa/3b0))+%i*kcv))$
wx0c(sigma):=rectform(1/(sigma-(ecu+%i*(kcv-ecv))) + kcu+%i*kcv)$
wx0m(sigma):=block([eta:bfloat(imagpart(sigma)),
xi:bfloat(realpart(sigma))],
if eta > 1.25b0 * (kcv-ecv) or (xi < -0.25*ecu and xi < eta-(kcv-ecv)) then
wx0c(sigma) else
if (eta > 0.75b0 * (kcv-ecv) and xi < 0.25b0 * ecu) or
eta > kcv-ecv or xi < 0 then wx0b(sigma) else wx0a(sigma))$
wx0(sigma):=wx0m(sigma)$
thom_sigma(sigma):=block([w:wx0(sigma),dw,v,vv],
for i do (
if i > 100 then error("too many iterations"),
vv:sigma_thom_comb(w),
v:vv[1],
dw:-rectform((v-sigma)*vv[2]),
w:w+dw,
dw:abs(dw),
if debugprint then print(float(w),float(dw)),
/* error is approx dw^2/2 */
if dw < tol2 then return(false)
),
w
)$
/* Lee/Thompson's method forward */
tm(phi,lam):=block([psi,thom,jacr,jaci,sigma,gam,scale,c],
phi:phi*degree,
lam:lam*degree,
psi:psi_phi(phi),
thom:thom_psi(psi+%i*lam),
jacr:sncndn(bfloat(realpart(thom)),1-e2),
jaci:sncndn(bfloat(imagpart(thom)),e2),
sigma:sigma_thom(thom),
c:cos(phi),
if c > tol1 then (
gam:atan2((1-e2)*jacr[1]*jaci[1]*jaci[2],
jacr[2]*jacr[3]*jaci[3]),
scale:sqrt(1-e2 + e2 * c^2)/c*
sqrt(((1-e2)*jaci[1]^2 + (jacr[2]*jaci[3])^2)/
(e2*jacr[2]^2 + (1-e2)*jaci[2]^2)))
else (gam : lam, scale : 1b0),
[imagpart(sigma)*k0*a,realpart(sigma)*k0*a,gam/degree,k0*scale])$
/* Lee/Thompson's method reverse */
ll(x,y):=block([sigma,thom,jacr,jaci,psi,lam,phi,gam,scale,c],
sigma:y/(a*k0)+%i*x/(a*k0),
thom:thom_sigma(sigma),
jacr:sncndn(bfloat(realpart(thom)),1-e2),
jaci:sncndn(bfloat(imagpart(thom)),e2),
psi:psi_thom(thom),
lam:bfloat(imagpart(psi)),
psi:bfloat(realpart(psi)),
phi:phi_psi(psi),
c:cos(phi),
if c > tol1 then (
gam:atan2((1-e2)*jacr[1]*jaci[1]*jaci[2],
jacr[2]*jacr[3]*jaci[3]),
scale:sqrt(1-e2 + e2 * c^2)/c*
sqrt(((1-e2)*jaci[1]^2 + (jacr[2]*jaci[3])^2)/
(e2*jacr[2]^2 + (1-e2)*jaci[2]^2)))
else (gam : lam, scale : 1b0),
[phi/degree,lam/degree,gam/degree,k0*scale])$
/* Return lat/lon/x/y for a point specified in Thompson coords */
/* Pick u in [0, kcu] and v in [0, kcv] */
lltm(u,v):=block([jacr,jaci,psi,lam,phi,c,gam,scale,sigma,x,y],
u:bfloat(u), v:bfloat(v),
jacr:sncndn(u,1-e2),
jaci:sncndn(v,e2),
psi:psi_thom(u+%i*v),
sigma:sigma_thom(u+%i*v),
x:imagpart(sigma)*k0*a,y:realpart(sigma)*k0*a,
lam:bfloat(imagpart(psi)),
psi:bfloat(realpart(psi)),
phi:phi_psi(psi),
c:cos(phi),
if c > tol1 then (
gam:atan2((1-e2)*jacr[1]*jaci[1]*jaci[2],
jacr[2]*jacr[3]*jaci[3]),
scale:sqrt(1-e2 + e2 * c^2)/c*
sqrt(((1-e2)*jaci[1]^2 + (jacr[2]*jaci[3])^2)/
(e2*jacr[2]^2 + (1-e2)*jaci[2]^2)))
else (gam : lam, scale : 1b0),
[phi/degree,lam/degree,x,y,gam/degree,k0*scale])$
/* Gauss-Krueger series to order n^i forward
Uses the array functions
a1_a[i](n), zeta_a[i](z,n), zeta_d[i](z,n), zetap_a[i](s,n), zetap_d[i](s,n),
defined in tmseries.mac.
*/
tms(phi,lam,i):=block([psi,xip,etap,z,sigma,sp,gam,k,b1],
phi:phi*degree,
lam:lam*degree,
psi:psi_phi(phi),
xip:atan2(snh(psi), cos(lam)),
etap:asinh(sin(lam)/hypot(snh(psi),cos(lam))),
k:sqrt(1 - e2*sin(phi)^2)/(cos(phi)*hypot(snh(psi),cos(lam))),
gam:atan(tan(xip)*tanh(etap)),
z:xip+%i*etap,
b1:a1_a[i](n),
sigma:rectform(b1*zeta_a[i](z,n)),
sp:rectform(zeta_d[i](z,n)),
gam : gam - atan2(imagpart(sp),realpart(sp)),
k : k * b1 * cabs(sp),
[imagpart(sigma)*k0*a,realpart(sigma)*k0*a,gam/degree,k*k0])$
/* Gauss-Krueger series to order n^i reverse */
lls(x,y,i):=block([sigma,b1,s,z,zp,xip,etap,s,c,r,gam,k,lam,psi,phi],
sigma:y/(a*k0)+%i*x/(a*k0),
b1:a1_a[i](n),
s:rectform(sigma/b1),
z:rectform(zetap_a[i](s,n)),
zp:rectform(zetap_d[i](s,n)),
gam : atan2(imagpart(zp), realpart(zp)),
k : b1 / cabs(zp),
xip:realpart(z),
etap:imagpart(z),
s:snh(etap),
c:cos(xip),
r:hypot(s, c),
lam:atan2(s, c),
psi : asinh(sin(xip)/r),
phi :phi_psi(psi),
k : k * sqrt(1 - e2*sin(phi)^2) * r/cos(phi),
gam : gam + atan(tan(xip) * tanh(etap)),
[phi/degree,lam/degree,gam/degree,k*k0])$
/* Approx geodesic distance valid for small displacements */
dist(phi0,lam0,phi,lam):=block([dphi,dlam,nn,hlon,hlat],
dphi:(phi-phi0)*degree,
dlam:(lam-lam0)*degree,
phi0:phi0*degree,
lam0:lam0*degree,
nn : 1/sqrt(1 - e2 * sin(phi0)^2),
hlon : cos(phi0) * nn,
hlat : (1 - e2) * nn^3,
a * hypot(dphi*hlat, dlam*hlon))$
/* Compute truncation errors for all truncation levels */
check(phi,lam):=block([vv,x,y,gam,k,vf,vb,errf,errr,err2,errlist],
phi:min(90-0.01b0,phi),
lam:min(90-0.01b0,lam),
vv:tm(phi,lam),
errlist:[],
x:vv[1], y:vv[2], gam:vv[3], k:vv[4],
for i:1 thru maxpow do (
vf:tms(phi,lam,i),
errf:hypot(vf[1]-x,vf[2]-y)/k,
errfg:abs(vf[3]-gam),
errfk:abs((vf[4]-k)/k),
vb:lls(x,y,i),
errr:dist(phi,lam,vb[1],vb[2]),
errrg:abs(vb[3]-gam),
errrk:abs((vb[4]-k)/k),
errlist:append(errlist,
[max(errf, errr), max(errfg, errrg), max(errfk, errrk)])),
errlist)$
/* Max of output of check over a set of points */
checka(lst):=block([errlist:[],errx],
for i:1 thru 3*maxpow do errlist:cons(0b0,errlist),
for vv in lst do (
errx:check(vv[1],vv[2]),
for i:1 thru 3*maxpow do errlist[i]:max(errlist[i],errx[i])),
errlist)$