# shithub: 9ferno

View raw version
```implement Geodesy;

include "sys.m";
sys: Sys;
include "math.m";
maths: Math;
Pi: import Math;
sin, cos, tan, asin, acos, atan, atan2, sqrt, fabs: import maths;
include "math/geodesy.m";

Approx: con 0;

Epsilon: con 0.000001;
Mperft: con 0.3048;
Earthrad: con 10800.0/Pi*6076.115*Mperft;	# in feet (about 4000 miles) : now metres
Δt: con 16.0;	# now-1989

# lalo0: con "53:57:45N 01:04:55W";
# os0: con "SE6022552235";

# ellipsoids
Airy1830, Airy1830m, Int1924, GRS80: con iota;

Ngrid: con 100000;	# in metres

x, y, z: real;
};

la: real;	# -Pi to Pi
lo: real;	# -Pi to Pi
x: real;
y: real;
};

name: string;
a: real;
b: real;
};

name: string;
e: int;
# X, Y, Z axes etc
};

name: string;
F0: real;
φ0λ0: string;
E0: real;
N0: real;
e: int;
};

tx, ty, tz: real;	# metres
s: real;		# ppm
rx, ry, rz: real;	# secs
};

dat: int;	# datum
cdat: int;	# converting datum
prj: int;		# projection
tmp: ref Mercator;	# actual projection
orig: Lalo;	# origin of above projection
zone: int;	# UTM zone
};

# ellipsoids
ells := array[] of {
Airy1830 => Ellipsoid("Airy1830", 6377563.396, 6356256.910),
Airy1830m => Ellipsoid("Airy1830 modified", 6377340.189, 6356034.447),
Int1924 => Ellipsoid("International 1924", 6378388.000, 6356911.946),
GRS80 => Ellipsoid("GRS80", 6378137.000, 6356752.3141),
};

# datums
dats := array[] of {
OSGB36 => Datum("OSGB36", Airy1830),
Ireland65 => Datum("Ireland65", Airy1830m),
ED50 => Datum("ED50", Int1924),
WGS84 => Datum("WGS84", GRS80),
ITRS2000 => Datum("ITRS2000", GRS80),
ETRS89 => Datum("ETRS89", GRS80),
};

# transverse Mercator projections
tmps := array[] of {
Natgrid => Mercator("National Grid", 0.9996012717, "49:00:00N 02:00:00W", real(4*Ngrid), real(-Ngrid), Airy1830),
IrishNatgrid => Mercator("Irish National Grid", 1.000035, "53:30:00N 08:00:00W", real(2*Ngrid), real(5*Ngrid/2), Airy1830m),
UTMEur => Mercator("UTM Europe", 0.9996, nil, real(5*Ngrid), real(0), Int1924),
UTM => Mercator("UTM", 0.9996, nil, real(5*Ngrid), real(0), GRS80),
};

# Helmert tranformations
HT_WGS84_OSGB36: con Helmert(-446.448, 125.157, -542.060, 20.4894, -0.1502, -0.2470, -0.8421);
HT_ITRS2000_ETRS89: con Helmert(0.054, 0.051, -0.048, 0.0, 0.000081*Δt, 0.00049*Δt, -0.000792*Δt);

# Helmert matrices
HM_WGS84_OSGB36, HM_OSGB36_WGS84, HM_ITRS2000_ETRS89, HM_ETRS89_ITRS2000, HM_ETRS89_OSGB36, HM_OSGB36_ETRS89, HM_IDENTITY: array of array of real;

fmt: ref Format;

# latlong: ref Latlong;

init(d: int, t: int, z: int)
{

helmertinit();
format(d, t, z);
# (nil, (la, lo)) := str2lalo(lalo0);
# (nil, (E, N)) := os2en(os0);
# latlong = ref Latlong(la, lo, real E, real N);
}

format(d: int, t: int, z: int)
{
if(fmt == nil)
fmt = ref Format(WGS84, 0, Natgrid, nil, (0.0, 0.0), 30);
if(d >= 0 && d <= ETRS89)
fmt.dat = d;
if(t >= 0 && t <= UTM)
fmt.prj = t;
if(z >= 1 && z <= 60)
fmt.zone = z;
fmt.cdat = fmt.dat;
fmt.tmp = ref Mercator(tmps[fmt.prj]);
if(fmt.tmp.φ0λ0 == nil)
fmt.orig = utmlaloz(fmt.zone);
else
(nil, fmt.orig) = str2lalo(fmt.tmp.φ0λ0);
e := fmt.tmp.e;
if(e != dats[fmt.dat].e){
for(i := 0; i <= ETRS89; i++)
if(e == dats[i].e){
fmt.cdat = i;
break;
}
}
}

str2en(s: string): (int, Eano)
{
s = trim(s, " \t\n\r");
if(s == nil)
return (0, (0.0, 0.0));
os := s[0] >= 'A' && s[0] <= 'Z' || strchrs(s, "NSEW:") < 0;
en: Eano;
if(os){
(ok, p) := os2en(s);
if(!ok)
return (0, (0.0, 0.0));
en = p;
}
else{
(ok, lalo) := str2lalo(s);
if(!ok)
return (0, (0.0, 0.0));
en = lalo2en(lalo);
}
return (1, en);
}

str2ll(s: string, pos: int, neg: int): (int, real)
{
(n, ls) := sys->tokenize(s, ": \t");
if(n < 1 || n > 3)
return (0, 0.0);
t := hd ls; ls = tl ls;
v := real t;
if(ls != nil){
t = hd ls; ls = tl ls;
v += (real t)/60.0;
}
if(ls != nil){
t = hd ls; ls = tl ls;
v += (real t)/3600.0;
}
c := t[len t-1];
if(c == pos)
;
else if(c == neg)
v = -v;
else
return (0, 0.0);
}

str2lalo(s: string): (int, Lalo)
{
s = trim(s, " \t\n\r");
p := strchr(s, 'N');
if(p < 0)
p = strchr(s, 'S');
if(p < 0)
return (0, (0.0, 0.0));
(ok1, la) := str2ll(s[0: p+1], 'N', 'S');
(ok2, lo) := str2ll(s[p+1: ], 'E', 'W');
if(!ok1 || !ok2 || la < -Pi/2.0 || la > Pi/2.0)
return (0, (0.0, 0.0));
return (1, (la, lo));
}

ll2str(ll: int, dir: string): string
{
d := ll/360000;
ll -= 360000*d;
m := ll/6000;
ll -= 6000*m;
s := ll/100;
ll -= 100*s;
return d2(d) + ":" + d2(m) + ":" + d2(s) + "." + d2(ll) + dir;
}

lalo2str(lalo: Lalo): string
{
lod := "E";
if(la < 0){
la = -la;
}
if(lo < 0){
lod = "W";
lo = -lo;
}
return ll2str(la, lad) + " " + ll2str(lo, lod);
}

en2os(p: Eano): string
{
E := trunc(p.e);
N := trunc(p.n);
es := E/Ngrid;
ns := N/Ngrid;
e := E-Ngrid*es;
n := N-Ngrid*ns;
d1 := 5*(4-ns/5)+es/5+'A'-3;
d2 := 5*(4-ns%5)+es%5+'A';
# now account for 'I' missing
if(d1 >= 'I')
d1++;
if(d2 >= 'I')
d2++;
return sys->sprint("%c%c%5.5d%5.5d", d1, d2, e, n);
}

os2en(s: string): (int, Eano)
{
s = trim(s, " \t\n\r");
if((m := len s) != 4 && m != 6 && m != 8 && m != 10 && m != 12)
return (0, (0.0, 0.0));
m = m/2-1;
u := Ngrid/10**m;
d1 := s[0];
d2 := s[1];
if(d1 < 'A' || d2 < 'A' || d1 > 'Z' || d2 > 'Z'){
# error(sys->sprint("bad os reference %s", s));
e := u*int s[0: 1+m];
n := u*int s[1+m: 2+2*m];
return (1, (real e, real n));
}
e := u*int s[2: 2+m];
n := u*int s[2+m: 2+2*m];
if(d1 >= 'I')
d1--;
if(d2 >= 'I')
d2--;
d1 -= 'A'-3;
d2 -= 'A';
es := 5*(d1%5)+d2%5;
ns := 5*(4-d1/5)+4-d2/5;
return (1, (real(Ngrid*es+e), real(Ngrid*ns+n)));
}

utmlalo(lalo: Lalo): Lalo
{
(nil, zn) := utmzone(lalo);
return utmlaloz(zn);
}

utmlaloz(zn: int): Lalo
{
}

utmzone(lalo: Lalo): (int, int)
{
(la, lo) := lalo;
zlo := trunc(lo+180.0)/6+1;
if(la < -80.0)
zla := 'B';
else if(la >= 84.0)
zla = 'Y';
else if(la >= 72.0)
zla = 'X';
else{
zla = trunc(la+80.0)/8+'C';
if(zla >= 'I')
zla++;
if(zla >= 'O')
zla++;
}
return (zla, zlo);
}

helmertinit()
{
(HM_WGS84_OSGB36, HM_OSGB36_WGS84) = helminit(HT_WGS84_OSGB36);
(HM_ITRS2000_ETRS89, HM_ETRS89_ITRS2000) = helminit(HT_ITRS2000_ETRS89);
HM_ETRS89_OSGB36 = mulmm(HM_WGS84_OSGB36, HM_ETRS89_ITRS2000);
HM_OSGB36_ETRS89 = mulmm(HM_ITRS2000_ETRS89, HM_OSGB36_WGS84);
HM_IDENTITY = m := matrix(3, 4);
m[0][0] = m[1][1] = m[2][2] = 1.0;
# mprint(HM_WGS84_OSGB36);
# mprint(HM_OSGB36_WGS84);
}

helminit(h: Helmert): (array of array of real, array of array of real)
{
m := matrix(3, 4);

s := 1.0+h.s/1000000.0;

m[0][0] = s;
m[0][1] = -rz;
m[0][2] = ry;
m[0][3] = h.tx;
m[1][0] = rz;
m[1][1] = s;
m[1][2] = -rx;
m[1][3] = h.ty;
m[2][0] = -ry;
m[2][1] = rx;
m[2][2] = s;
m[2][3] = h.tz;

return (m, inv(m));
}

trans(f: int, t: int): array of array of real
{
case(f){
WGS84 =>
case(t){
WGS84 =>
return HM_IDENTITY;
OSGB36 =>
return HM_WGS84_OSGB36;
ITRS2000 =>
return HM_IDENTITY;
ETRS89 =>
return HM_ITRS2000_ETRS89;
}
OSGB36 =>
case(t){
WGS84 =>
return HM_OSGB36_WGS84;
OSGB36 =>
return HM_IDENTITY;
ITRS2000 =>
return HM_OSGB36_WGS84;
ETRS89 =>
return HM_OSGB36_ETRS89;
}
ITRS2000 =>
case(t){
WGS84 =>
return HM_IDENTITY;
OSGB36 =>
return HM_WGS84_OSGB36;
ITRS2000 =>
return HM_IDENTITY;
ETRS89 =>
return HM_ITRS2000_ETRS89;
}
ETRS89 =>
case(t){
WGS84 =>
return HM_ETRS89_ITRS2000;
OSGB36 =>
return HM_ETRS89_OSGB36;
ITRS2000 =>
return HM_ETRS89_ITRS2000;
ETRS89 =>
return HM_IDENTITY;
}
}
return HM_IDENTITY;	# Ireland65, ED50 not done
}

datum2datum(lalo: Lalo, f: int, t: int): Lalo
{
if(f == t)
return lalo;
(la, lo) := lalo;
v := laloh2xyz(la, lo, 0.0, dats[f].e);
v = mulmv(trans(f, t), v);
(la, lo, nil) = xyz2laloh(v, dats[t].e);
return (la, lo);
}

laloh2xyz(φ: real, λ: real, H: real, e: int): Vector
{
a := ells[e].a;
b := ells[e].b;
e2 := 1.0-(b/a)**2;

s := sin(φ);
c := cos(φ);

ν := a/sqrt(1.0-e2*s*s);
x := (ν+H)*c*cos(λ);
y := (ν+H)*c*sin(λ);
z := ((1.0-e2)*ν+H)*s;

return (x, y, z);
}

xyz2laloh(v: Vector, e: int): (real, real, real)
{
x := v.x;
y := v.y;
z := v.z;

a := ells[e].a;
b := ells[e].b;
e2 := 1.0-(b/a)**2;

λ := atan2(y, x);

p := sqrt(x*x+y*y);
φ := φ1 := atan(z/(p*(1.0-e2)));
ν := 0.0;
do{
φ = φ1;
s := sin(φ);
ν = a/sqrt(1.0-e2*s*s);
φ1 = atan((z+e2*ν*s)/p);
}while(!small(fabs(φ-φ1)));

φ = φ1;
H := p/cos(φ)-ν;

return (φ, λ, H);
}

lalo2en(lalo: Lalo): Eano
{
(φ, λ) := lalo;
if(fmt.cdat != fmt.dat)
(φ, λ) = datum2datum(lalo, fmt.dat, fmt.cdat);

s := sin(φ);
c := cos(φ);
t2 := tan(φ)**2;

(nil, F0, φ0λ0, E0, N0, e) := *fmt.tmp;
a := ells[e].a;
b := ells[e].b;
e2 := 1.0-(b/a)**2;

if(φ0λ0 == nil)	# UTM
(φ0, λ0) := utmlalo((φ, λ));	# don't use fmt.zone here
else
(φ0, λ0) = fmt.orig;

n := (a-b)/(a+b);
ν := a*F0/sqrt(1.0-e2*s*s);
ρ := ν*(1.0-e2)/(1.0-e2*s*s);
η2 := ν/ρ-1.0;

φ1 := φ-φ0;
φ2 := φ+φ0;
M := b*F0*((1.0+n*(1.0+1.25*n*(1.0+n)))*φ1 - (3.0*n*(1.0+n*(1.0+0.875*n)))*sin(φ1)*cos(φ2) + 1.875*n*n*(1.0+n)*sin(2.0*φ1)*cos(2.0*φ2) - 35.0/24.0*n**3*sin(3.0*φ1)*cos(3.0*φ2));

I := M+N0;
II := ν*s*c/2.0;
III := ν*s*c**3*(5.0-t2+9.0*η2)/24.0;
IIIA := ν*s*c**5*(61.0+t2*(t2-58.0))/720.0;
IV := ν*c;
V := ν*c**3*(ν/ρ-t2)/6.0;
VI := ν*c**5*(5.0+14.0*η2+t2*(t2-18.0-58.0*η2))/120.0;

λ -= λ0;
λ2 := λ*λ;
N := I+λ2*(II+λ2*(III+IIIA*λ2));
E := E0+λ*(IV+λ2*(V+VI*λ2));

# if(E < 0.0 || E >= real(7*Ngrid))
# 	E = 0.0;
# if(N < 0.0 || N >= real(13*Ngrid))
# 	N = 0.0;
return (E, N);
}

en2lalo(en: Eano): Lalo
{
E := en.e;
N := en.n;

(nil, F0, nil, E0, N0, e) := *fmt.tmp;
a := ells[e].a;
b := ells[e].b;
e2 := 1.0-(b/a)**2;

(φ0, λ0) := fmt.orig;

n := (a-b)/(a+b);

M0 := 1.0+n*(1.0+1.25*n*(1.0+n));
M1 := 3.0*n*(1.0+n*(1.0+0.875*n));
M2 := 1.875*n*n*(1.0+n);
M3 := 35.0/24.0*n**3;

N -= N0;
M := 0.0;
φ := φold := φ0;
do{
φ = (N-M)/(a*F0)+φold;
φ1 := φ-φ0;
φ2 := φ+φ0;
M = b*F0*(M0*φ1 - M1*sin(φ1)*cos(φ2) + M2*sin(2.0*φ1)*cos(2.0*φ2) - M3*sin(3.0*φ1)*cos(3.0*φ2));
φold = φ;
}while(fabs(N-M) >= 0.01);

s := sin(φ);
c := cos(φ);
t := tan(φ);
t2 := t*t;

ν := a*F0/sqrt(1.0-e2*s*s);
ρ := ν*(1.0-e2)/(1.0-e2*s*s);
η2 := ν/ρ-1.0;

VII := t/(2.0*ρ*ν);
VIII := VII*(5.0+η2+3.0*t2*(1.0-3.0*η2))/(12.0*ν*ν);
IX := VII*(61.0+45.0*t2*(2.0+t2))/(360.0*ν**4);
X := 1.0/(ν*c);
XI := X*(ν/ρ+2.0*t2)/(6.0*ν*ν);
XII := X*(5.0+4.0*t2*(7.0+6.0*t2))/(120.0*ν**4);
XIIA := X*(61.0+2.0*t2*(331.0+60.0*t2*(11.0+6.0*t2)))/(5040.0*ν**6);

E -= E0;
E2 := E*E;
φ = φ-E2*(VII-E2*(VIII-E2*IX));
λ := λ0+E*(X-E2*(XI-E2*(XII-E2*XIIA)));

if(fmt.cdat != fmt.dat)
(φ, λ) = datum2datum((φ, λ), fmt.cdat, fmt.dat);
return (φ, λ);
}

mulmm(m1: array of array of real, m2: array of array of real): array of array of real
{
m := matrix(3, 4);
mul3x3(m, m1, m2);
for(i := 0; i < 3; i++){
sum := 0.0;
for(k := 0; k < 3; k++)
sum += m1[i][k]*m2[k][3];
m[i][3] = sum+m1[i][3];
}
return m;
}

mulmv(m: array of array of real, v: Vector): Vector
{
x := v.x;
y := v.y;
z := v.z;
v.x = m[0][0]*x + m[0][1]*y + m[0][2]*z + m[0][3];
v.y = m[1][0]*x + m[1][1]*y + m[1][2]*z + m[1][3];
v.z = m[2][0]*x + m[2][1]*y + m[2][2]*z + m[2][3];
return v;
}

inv(m: array of array of real): array of array of real
{
n := matrix(3, 4);
inv3x3(m, n);
(n[0][3], n[1][3], n[2][3]) = mulmv(n, (-m[0][3], -m[1][3], -m[2][3]));
return n;
}

mul3x3(m: array of array of real, m1: array of array of real, m2: array of array of real)
{
for(i := 0; i < 3; i++){
for(j := 0; j < 3; j++){
sum := 0.0;
for(k := 0; k < 3; k++)
sum += m1[i][k]*m2[k][j];
m[i][j] = sum;
}
}
}

inv3x3(m: array of array of real, n: array of array of real)
{
t00 := m[0][0];
t01 := m[0][1];
t02 := m[0][2];
t10 := m[1][0];
t11 := m[1][1];
t12 := m[1][2];
t20 := m[2][0];
t21 := m[2][1];
t22 := m[2][2];

n[0][0] = t11*t22-t12*t21;
n[1][0] = t12*t20-t10*t22;
n[2][0] = t10*t21-t11*t20;
n[0][1] = t02*t21-t01*t22;
n[1][1] = t00*t22-t02*t20;
n[2][1] = t01*t20-t00*t21;
n[0][2] = t01*t12-t02*t11;
n[1][2] = t02*t10-t00*t12;
n[2][2] = t00*t11-t01*t10;

d := t00*n[0][0]+t01*n[1][0]+t02*n[2][0];
for(i := 0; i < 3; i++)
for(j := 0; j < 3; j++)
n[i][j] /= d;
}

matrix(rows: int, cols: int): array of array of real
{
m := array[rows] of array of real;
for(i := 0; i < rows; i++)
m[i] = array[cols] of { * => 0.0 };
return m;
}

vprint(v: Vector)
{
sys->print("	%f	%f	%f\n", v.x, v.y, v.z);
}

mprint(m: array of array of real)
{
for(i := 0; i < len m; i++){
for(j := 0; j < len m[i]; j++)
sys->print("	%f", m[i][j]);
sys->print("\n");
}
}

# lalo2xy(la: real, lo: real, lalo: ref Latlong): Eano
# {
# 	x, y: real;
#
# 	la0 := lalo.la;
# 	lo0 := lalo.lo;
# 	if(Approx){
# 	}
# 	else{
# 	}
# 	return (x, y);
# }

# lalo2xyz(la: real, lo: real, lalo: ref Latlong): (int, int, int)
# {
# 	z: real;
#
# 	la0 := lalo.la;
#     	lo0 := lalo.lo;
# 	(x, y) := lalo2xy(la, lo, lalo);
# 	if(Approx)
# 	else
# 	return (x, y, int z);
# }

# xy2lalo(p: Eano, lalo: ref Latlong): (real, real)
# {
# 	la, lo: real;
#
# 	x := p.e;
# 	y := p.n;
# 	la0 := lalo.la;
# 	lo0 := lalo.lo;
# 	if(Approx){
# 		la = la0 + (y-lalo.y)/Earthrad;
# 		lo = lo0 + (x-lalo.x)/(Earthrad*cos(la0));
# 	}
# 	else{
# 		a, b, c, d, bestd, r, r1, r2, lat, lon, tmp: real;
# 		i, n: int;
#
# 		bestd = -1.0;
# 		la = lo = 0.0;
# 		(n, r1, r2) = quad(1.0, -2.0*b*cos(la0), (a*a-1.0)*sin(la0)*sin(la0)+b*b);
# 		if(n == 0)
# 			return (la, lo);
# 		while(--n >= 0){
# 			if(n == 1)
# 				r = r2;
# 			else
# 				r = r1;
# 			if(fabs(r) <= 1.0){
# 				lat = asin(r);
# 				c = cos(lat);
# 				if(small(c))
# 					tmp = 0.0;	# lat = +90, -90, lon = lo0
# 				else
# 					tmp = a/c;
# 				if(fabs(tmp) <= 1.0){
# 					for(i = 0; i < 2; i++){
# 						if(i == 0)
# 							lon = norm(asin(tmp)+lo0);
# 						else
# 							lon = norm(Pi-asin(tmp)+lo0);
# 						(X, Y, Z) := lalo2xyz(lat, lon, lalo);
# 						# eliminate non-roots by d, root on other side of earth by Z
# 						d = (real X-x)**2+(real Y-y)**2;
# 						if(Z >= 0 && (bestd < 0.0 || d < bestd)){
# 							bestd = d;
# 							la = lat;
# 							lo = lon;
# 						}
# 					}
# 				}
# 			}
# 		}
# 	}
# 	return (la, lo);
# }

# quad(a: real, b: real, c: real): (int, real, real)
# {
# 	r1, r2: real;
#
# 	D := b*b-4.0*a*c;
# 	if(small(a)){
# 		if(small(b))
# 			return (0, r1, r2);
# 		r1 = r2 = -c/b;
# 		return (1, r1, r2);
# 	}
# 	if(D < 0.0)
# 		return (0, r1, r2);
# 	D = sqrt(D);
# 	r1 = (-b+D)/(2.0*a);
# 	r2 = (-b-D)/(2.0*a);
# 	if(small(D))
# 		return (1, r1, r2);
# 	else
# 		return (2, r1, r2);
# }

d2(v: int): string
{
s := string v;
if(v < 10)
s = "0" + s;
return s;
}

trim(s: string, t: string): string
{
while(s != nil && strchr(t, s[0]) >= 0)
s = s[1: ];
while(s != nil && strchr(t, s[len s-1]) >= 0)
s = s[0: len s-1];
return s;
}

strchrs(s: string, t: string): int
{
for(i := 0; i < len t; i++){
p := strchr(s, t[i]);
if(p >= 0)
return p;
}
return -1;
}

strchr(s: string, c: int): int
{
for(i := 0; i < len s; i++)
if(s[i] == c)
return i;
return -1;
}

{
return d*Pi/180.0;
}

{
return r*180.0/Pi;
}

{
}

norm(r: real): real
{
while(r > Pi)
r -= 2.0*Pi;
while(r < -Pi)
r += 2.0*Pi;
return r;
}

small(r: real): int
{
return r > -Epsilon && r < Epsilon;
}

trunc(r: real): int
{
# down : assumes r >= 0
i := int r;
if(real i > r)
i--;
return i;
}

abs(x: int): int
{
if(x < 0)
return -x;
return x;
}
```