vincentyFormula method

double vincentyFormula(GeoLocation location, int formula)

Calculate geodesic distance in Meters between this Object and a second Object passed to this method using Thaddeus Vincenty's inverse formula See T Vincenty, "Direct and Inverse Solutions of Geodesics on the Ellipsoid with application of nested equations", Survey Review, vol XXII no 176, 1975

@param location the destination location @param formula This formula calculates initial bearing ({@link #INITIAL_BEARING}), final bearing ( {@link #FINAL_BEARING}) and distance ({@link #DISTANCE}). @return geodesic distance in Meters

Implementation

double vincentyFormula(GeoLocation location, int formula) {
  double a = 6378137;
  double b = 6356752.3142;
  double f = 1 / 298.257223563; // WGS-84 ellipsiod
  double L = radians(location.getLongitude() - getLongitude());
  double u1 = atan((1 - f) * tan(radians(getLatitude())));
  double u2 = atan((1 - f) * tan(radians(location.getLatitude())));
  double sinU1 = sin(u1), cosU1 = cos(u1);
  double sinU2 = sin(u2), cosU2 = cos(u2);

  double lambda = L;
  double lambdaP = 2 * pi;
  double iterLimit = 20;
  double sinLambda = 0;
  double cosLambda = 0;
  double sinSigma = 0;
  double cosSigma = 0;
  double sigma = 0;
  double sinAlpha = 0;
  double cosSqAlpha = 0;
  double cos2SigmaM = 0;
  double C;
  while ((lambda - lambdaP).abs() > 1e-12 && --iterLimit > 0) {
    sinLambda = sin(lambda);
    cosLambda = cos(lambda);
    sinSigma = sqrt((cosU2 * sinLambda) * (cosU2 * sinLambda) +
        (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) *
            (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda));
    if (sinSigma == 0) return 0; // co-incident points
    cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
    sigma = atan2(sinSigma, cosSigma);
    sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
    cosSqAlpha = 1 - sinAlpha * sinAlpha;
    cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;
    if (cos2SigmaM.isNaN) {
      cos2SigmaM = 0;
    } // equatorial line: cosSqAlpha=0 (§6)
    C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
    lambdaP = lambda;
    lambda = L +
        (1 - C) *
            f *
            sinAlpha *
            (sigma +
                C *
                    sinSigma *
                    (cos2SigmaM +
                        C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)));
  }
  if (iterLimit == 0) return double.nan; // formula failed to converge

  double uSq = cosSqAlpha * (a * a - b * b) / (b * b);
  double A =
      1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
  double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
  double deltaSigma = B *
      sinSigma *
      (cos2SigmaM +
          B /
              4 *
              (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) -
                  B /
                      6 *
                      cos2SigmaM *
                      (-3 + 4 * sinSigma * sinSigma) *
                      (-3 + 4 * cos2SigmaM * cos2SigmaM)));
  double distance = b * A * (sigma - deltaSigma);

  // initial bearing
  double fwdAz = degrees(
      atan2(cosU2 * sinLambda, cosU1 * sinU2 - sinU1 * cosU2 * cosLambda));
  // final bearing
  double revAz = degrees(
      atan2(cosU1 * sinLambda, -sinU1 * cosU2 + cosU1 * sinU2 * cosLambda));
  if (formula == _DISTANCE) {
    return distance;
  } else if (formula == _INITIAL_BEARING) {
    return fwdAz;
  } else if (formula == _FINAL_BEARING) {
    return revAz;
  } else {
    // should never happen
    return double.nan;
  }
}