/** * Compute Distance and Bearings Between Two Points on The Earth * @author Robert John Morton * @version 20 August 1998 revised 30 March 2010 */ /* Solution to the geodetic inverse problem after T. Vincenty. Modified Rainsford's Method with Helmert's elliptical terms. Effective in any azimuth and at any distance short of antipodal. Neither the observer nor the observed can be at a geographic pole. Latitudes and longitudes in radians, positive north and east. Distance between observer and observed is returned in metres. Forward azimuths at observer and observed returned in radians clockwise from north. Originally programmed for CDC6600 by Lcdr L Pfeifer NGS Rockville MD 18FEB75. Modified for IBM 360 by John G Gergen NGS Rockville MD JUL75 */ class dandb{ private static final double π = 3.14159265358979323846264338327950288, // convergence limit for distance accuracy better than 1mm δ = 0.5e-13, // polar limit: 1 minute less than a right-angle P = 1.57050543858623089763516774317834; private static dandb db; // reference to current instance of this class private double α1, // forward azimuth α2, // back-azimuth s, // ellipsoidal distance observer to observed a, // equitorial radius of the Earth in metres f, // flattening factor r; // equitorial radius * r = polar radius private int ic; /* iteration count for vincenty loop: 3 to 6 itera- tions are normally sufficient to achieve convergence */ dandb(double a, double f){ // set which geo-ellipsoid to use (above) this.a = a; // equitorial radius of the Earth in metres this.f = f; // flattening factor r = 1 - f; // equitorial radius * r = polar radius db = this; // reference to current instance of this class } public void setaf(double a, double f){ this.a = a; // set new value for semi-major axis this.f = f; // set new value for flattening r = 1 - f; // equitorial radius * r = polar radius } public double getDist(){return s;} // get ellipsoidal distance in metres // return bearing of observed from observer public double getForwardAzimuth() { return α1; } // return bearing of observer from observed public double getBackAzimuth(){return α2;} // to check for non-convergence public int getIterations(){return ic;} /* Code fragment for calling the Vincenty dandb class: dandb db = dandb.getCurrent(); int x = db.vincenty(φ1, λ1, φ2, λ2); // compute distance & bearings if(x == 0) { // if dandb computation is valid if((s = db.getDist()) != 0) { / /provided distance isn't zero s /= 1000; // if you need distance in kilometres, s /= 6366197.723857773; // or in mean Earth-radians α1 = db.getForwardAzimuth(); // both azumuths are valid α2 = db.getBackAzimuth(); // else the points are coincident, so } // leave α1, α2 as they were last time } // dandb invalid so leave all as was Note that if s = 0, then the observer and the observed are coincident and therefore the azimuths are meaningless. In this case, for flight control applications, leave both azimuths at their previous values. If the computation returns a non-zero error code, look at the premature returns in the following method to see their meanings. VINCENTY INVERSE METHOD parameters: latitude of observer, longitude of observer, longitude of observed, latitude of observed. */ public int vincenty( double φ1, double λ1, double φ2, double λ2 ) { // Return because cannot cope when observer's lattitude is: if(φ1 > P) return 1; // > 89°59'00"N else if(φ1 < -P) return 2; // > 89°59'00"S else if(φ2 > P) return 3; // > 89°59'00"N else if(φ2 < -P) return 4; // > 89°59'00"S else if(λ1 > π || λ1 < -π) return 5; // > 180° E or W else if(λ2 > π || λ2 < -π) return 6; // > 180° E or W double Δλ = λ2 - λ1, // longitude difference tanU1 = r * Math.tan(φ1), // tan(reduced latitude of observer) tanU2 = r * Math.tan(φ2), // tan(reduced latitude of observed) // Cosine and sine of the reduced latitude of the observer. cosU1 = 1 / Math.sqrt(1 + tanU1 * tanU1), sinU1 = cosU1 * tanU1, // Cosine of the reduced latitude of the observed. cosU2 = 1 / Math.sqrt(1 + tanU2 * tanU2), θ = Δλ, // prime θ before entering the loop χ, // holds previous value of θ for each iteration sinβ, cosβ, sinθ, cosθ, sinσ, cosσ, ψ, e, y, c; s = cosU1 * cosU2; // prime ellipsoidal distance (observer to observed) α2 = s * tanU2; // prime the back-azimuth α1 = α2 * tanU1; // prime the forward azimuth ic = 0; // zero the iteration counter /* ITERATE THE APPROPRIATE EQUATIONSUNTIL THERE IS NO FURTHER SIGNIFICANT CHANGE IN θ. That is, WHILE χ [the old value of θ] minus the newly cal- culated value of θ is greater than δ AND we have not yet passed through here 100 times [iteration limit in case θ doesn't converge]. */ do { sinθ = Math.sin(θ); cosθ = Math.cos(θ); tanU1 = cosU2 * sinθ; tanU2 = α2 - sinU1 * cosU2 * cosθ; sinβ = Math.sqrt(tanU1 * tanU1 + tanU2 * tanU2); cosβ = s * cosθ + α1; y = Math.atan2(sinβ, cosβ); sinσ = s * sinθ / sinβ; cosσ = 1 - sinσ * sinσ; ψ = α1 + α1; if(cosσ > 0) ψ = cosβ - ψ / cosσ; e = ψ * ψ * 2 - 1; c = ((4 - 3 * cosσ) * f + 4) * cosσ * f / 16; χ = θ; // save old θ before computing new value θ = (1 - c) * ((e * cosβ * c + ψ) * sinβ * c + y) * sinσ * f + Δλ; } while(Math.abs(χ - θ) > δ && ++ic < 100); //Bail out because θ didn't converge sufficiently within 100 iterations. if(ic >= 100) return 7; θ = Math.sqrt((1 / r / r - 1) * cosσ + 1) + 1; // distance calculation θ = (θ - 2) / θ; c = (θ * θ / 4 + 1) / (1 - θ); θ = (0.375 * θ * θ - 1) * θ; s = ((((sinβ * sinβ * 4 - 3) * (1 - e - e) * ψ * θ / 6 - e * cosβ) * θ / 4 + ψ) * sinβ * θ + y) * c * a * r; if(Double.isNaN(s)) { // if distance calculation was indeterminate if(Math.abs(Δλ) > 1) // if the points are antipodal return 8; // then bailout; can't cope s = 0; // else they are coincident } else { // else distance calculation yielded a valid value // so compute the forward and backward azimuths. α1 = Math.atan2(tanU1, tanU2); α2 = Math.atan2(cosU1 * sinθ, α2 * cosθ - sinU1 * cosU2) + π; } return 0; // success } }