20060303a

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;
+}