#include <u.h>#include <libc.h>#include <bio.h> typedef struct Lathe Lathe;struct Lathe {	double	lat;	double	lng;};#pragma	varargck	type	"φ"	Lathe double	a = 6378137.;double	b = 6356752.3142;		/* b ≡ a(1-f) */double	f = 1 / 298.257223563;double	π = 3.14159265358979323846;double	R = 6371 * 10;Lathe	Z; #define	Big	(1e30)char	*line;Biobuf	o;double	min = Big;double	(*lathe2m)(Lathe, Lathe);char	flag[127]; intφfmt(Fmt *f){	Lathe φ; 	φ = va_arg(f->args, Lathe);	if(f->flags & FmtSharp)		fmtprint(f, "lat %g lng %g", φ.lat, φ.lng);	else		fmtprint(f, "%g %g", φ.lat, φ.lng);	return 0;} doubledtorad(double d){	return d*π/180.;} doublehaversine(Lathe φ1, Lathe φ2){	double lat1, lat2, x, y;	Lathe Δ; 	Δ.lat = dtorad(φ2.lat - φ1.lat);	Δ.lng = dtorad(φ2.lng - φ1.lng);	lat1 = dtorad(φ1.lat);	lat2 = dtorad(φ2.lat); 	x = sin(Δ.lat/2) * sin(Δ.lat/2) +		sin(Δ.lng/2) * sin(Δ.lng/2) +		cos(lat1) * cos(lat2);	y = 2*atan2(sqrt(x), sqrt(1-x));	return R*y;	/* meters */} /* Thaddeus Vincenty's formula; output in m */doublevincenty(Lathe φ1, Lathe φ2){	double L, U1, U2, λ, λ′;	double sinσ, cosσ, σ, sinα, cos2α, cos2σm, C, u2, A, B, Δσ;	double x, y, δ, δ′; 	L = dtorad(φ2.lng - φ1.lng);	U1 = atan((1 - f) * tan(dtorad(φ1.lat)));	/* reduced latitude */	U2 = atan((1 - f) * tan(dtorad(φ2.lat))); 	δ = 1e30;	for(λ = L;; λ = λ′){			/* λ = lat difference on aux. sphere */		x = cos(U2)*sin(λ);		y = cos(U1)*sin(U2)-sin(U1)*cos(U2)*cos(λ);		sinσ = sqrt(x*x + y*y);		if(sinσ == 0)			return 0;			/* same pt within prec */		cosσ = sin(U1)*sin(U2) + cos(U1)*cos(U2)*cos(λ);		σ = atan2(sinσ, cosσ);		sinα = cos(U1) * cos(U2) * sin(λ) / sinσ;		cos2α = 1 - sinα*sinα;		if(cos2α != 0)			cos2σm = cosσ - 2*sin(U1)*sin(U2)/cos2α;		else			cos2σm = 0.;		/* equatorial line */		C = f/16*cos2α*(4+f*(4-3*cos2α));		λ′ = L + (1-C) * f * sinα *			(σ + C*sinσ*(cos2σm+C*cosσ*(-1+2*cos2σm*cos2σm)));		δ′ = fabs(λ - λ′);		if(δ′ < 1e-12)			break;		if(fabs(δ′) >= fabs(δ))			return haversine(φ1, φ2);	/* fallback: formula diverged */		δ = δ′;	} 	u2 = cos2α * (a*a - b*b) / (b*b);	A = 1 + u2/16384*(4096 + u2*(-768 + u2*(320 - 175*u2)));	B = u2/1024 * (256 + u2*(-128 +u2*(74-47*u2)));	x = cos2σm*cos2σm;	Δσ = B*sinσ*(cos2σm + B/4*(cosσ*(-1 + 2*x)		- B/6*cos2σm*(-3 + 4*sinσ*sinσ)*(-3 + 4*x))); 	return b*A*(σ-Δσ);	/* meters */	} Lathestrtolathe(char *s, char **r){	char *p0, *x;	Lathe φ; 	if(r == nil)		r = &x;	s += strcspn(s, "0123456789-.");	φ.lat = strtod(s, r);	if(**r == 0 || *r == s || fabs(φ.lat) > 90.){		werrstr("bad latitude");		return Z;	}	p0 = *r;	φ.lng = strtod(p0, r);	if(*r == p0 || fabs(φ.lng) > 180.){		werrstr("bad longitude");		return Z;	}	return φ;} intisZ(Lathe φ){	return memcmp(&φ, &Z, sizeof Z) == 0;} Latheskyhere(void){	char buf[128];	int fd, n;	Lathe φ; 	fd = open("/lib/sky/here", OREAD);	if(fd == -1)		return Z;	n = read(fd, buf, sizeof buf);	close(fd);	if(n == -1)		return Z;	buf[n] = 0;	φ = strtolathe(buf, nil);	φ.lng = -φ.lng;	return φ;} voidresult(char *key, double d){	if(!flag['m']){		Bprint(&o, "%s	%g\n", key, d);		return;	}	if(fabs(d) >= min)		return;	free(line);	line = nil;	if(key)		line = strdup(key);	min = d;} voidprmin(void){	if(line != nil && !flag['q'])		Bprint(&o, "%s	", line);	Bprint(&o, "%g\n", min);} voidfildist(char *file, Lathe φ1){	char *s;	Biobuf *b;	Lathe φ2; 	b = Bopen(file, OREAD);	if(b == nil)		sysfatal("Bopen: %r");	for(; s = Brdstr(b, '\n', 1); free(s)){		φ2 = strtolathe(s, nil);		if(isZ(φ2))			continue;		result(s, lathe2m(φ1, φ2));	}	Bterm(b);} voidusage(void){	fprint(2, "usage lat [-qm] [-l lat lng] [ptfile ...]\n");	exits("usage");} voidmain(int argc, char **argv){	char buf[128];	int i, ells;	Lathe φ[2]; 	lathe2m = vincenty;	fmtinstall(L'φ', φfmt);	ells = 0; 	if(Binit(&o, 1, OWRITE) == -1)		sysfatal("Binit: %r"); 	ARGBEGIN{	case 'q':	case 'm':	case 'k':		flag[ARGC()] = 1;		break;	case 'l':		φ[ells].lat = strtod(EARGF(usage()), nil);		φ[ells].lng = strtod(EARGF(usage()), nil);		if(fabs(φ[ells].lat) > 90. || fabs(φ[ells].lng) > 180.)			usage();		if(++ells == 2){			snprint(buf, sizeof buf, "%φ", φ[1]);			result(buf, lathe2m(φ[0], φ[1]));			ells = 1;		}		break;	default:		usage();	}ARGEND 	if(isZ(φ[0]))		φ[0] = skyhere();	if(isZ(φ[0]))		sysfatal("bad origin: %r"); 	if(flag['k']){		/* kat — known answer test */		φ[0] = (Lathe){33.8765, -83.3370};		φ[1] = (Lathe){33.3587, -84.5617};		Bprint(&o, "h	%g\n", haversine(φ[0], φ[1]));		Bprint(&o, "v	%g\n", vincenty(φ[0], φ[1]));	}else		for(i = 0; i < argc; i++)			fildist(argv[i], φ[0]); 	if(flag['m'])		prmin(); 	Bterm(&o);	exits("");}