summaryrefslogtreecommitdiff
path: root/appl/math/geodesy.b
diff options
context:
space:
mode:
Diffstat (limited to 'appl/math/geodesy.b')
-rw-r--r--appl/math/geodesy.b849
1 files changed, 849 insertions, 0 deletions
diff --git a/appl/math/geodesy.b b/appl/math/geodesy.b
new file mode 100644
index 00000000..7c4cd152
--- /dev/null
+++ b/appl/math/geodesy.b
@@ -0,0 +1,849 @@
+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
+
+Vector: adt{
+ x, y, z: real;
+};
+
+Latlong: adt{
+ la: real; # -Pi to Pi
+ lo: real; # -Pi to Pi
+ x: real;
+ y: real;
+};
+
+Ellipsoid: adt{
+ name: string;
+ a: real;
+ b: real;
+};
+
+Datum: adt{
+ name: string;
+ e: int;
+ # X, Y, Z axes etc
+};
+
+Mercator: adt{
+ name: string;
+ F0: real;
+ φ0λ0: string;
+ E0: real;
+ N0: real;
+ e: int;
+};
+
+Helmert: adt{
+ tx, ty, tz: real; # metres
+ s: real; # ppm
+ rx, ry, rz: real; # secs
+};
+
+Format: adt{
+ 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)
+{
+ sys = load Sys Sys->PATH;
+ maths = load Math Math->PATH;
+
+ 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);
+ return (1, norm(deg2rad(v)));
+}
+
+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
+{
+ la := int(360000.0*rad2deg(lalo.la));
+ lo := int(360000.0*rad2deg(lalo.lo));
+ lad := "N";
+ lod := "E";
+ if(la < 0){
+ lad = "S";
+ 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
+{
+ return (0.0, deg2rad(real(6*zn-183)));
+}
+
+utmzone(lalo: Lalo): (int, int)
+{
+ (la, lo) := lalo;
+ la = rad2deg(la);
+ lo = rad2deg(lo);
+ 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;
+ rx := sec2rad(h.rx);
+ ry := sec2rad(h.ry);
+ rz := sec2rad(h.rz);
+
+ 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){
+# x = Earthrad*cos(la0)*(lo-lo0)+lalo.x;
+# y = Earthrad*(la-la0)+lalo.y;
+# }
+# else{
+# x = Earthrad*cos(la)*sin(lo-lo0)+lalo.x;
+# y = Earthrad*(sin(la)*cos(la0)-sin(la0)*cos(la)*cos(lo-lo0))+lalo.y;
+# }
+# 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)
+# z = Earthrad;
+# else
+# z = Earthrad*(sin(la)*sin(la0)+cos(la)*cos(la0)*cos(lo-lo0));
+# 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;
+# a = (x-lalo.x)/Earthrad;
+# b = (y-lalo.y)/Earthrad;
+# (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;
+}
+
+deg2rad(d: real): real
+{
+ return d*Pi/180.0;
+}
+
+rad2deg(r: real): real
+{
+ return r*180.0/Pi;
+}
+
+sec2rad(s: real): real
+{
+ return deg2rad(s/3600.0);
+}
+
+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;
+}
generated by cgit v1.2.3 (git 2.46.0) at 2025-12-10 09:23:01 +0000