///////////////////////////////////////////////////////////////////////////////
//
//          Thaddeus Vincenty's inverse formula for ellipsoids
//           Geodesic (shortest distance) between two points
//
//                Lat1, Lon1, Lat2, Lon2  in radians
//
//  Notes and Java script     ? 2002-2006 Chris Veness
//
//  Delphi Translation         May 2006  Charles Seitz

{ For the benefit of the terminally obsessive (as well as the genuinely needy),
 Thaddeus Vincenty (?TV?) devised formulae for calculating geodesic distances
 between a pair of latitude/longitude points on the earth?s surface, using an
 accurate ellipsoidal model of the earth. When I looked up the references
 (?Direct and Inverse Solutions of Geodesics on the Ellipsoid with application
 of nested equations?), I discovered to my surprise that while the mathematics
 is utterly beyond me, it is actually quite simple to program. Vincenty?s
 formula is accurate to within 0.5mm, or 0.000015? (!), on the ellipsoid being
 used. Calculations based on a spherical model, such as the (much simpler)
 Haversine, are accurate to around 0.3% (which is still good enough for most
 purposes, of course).

 Note: the accuracy quoted by Vincenty applies to the theoretical ellipsoid
 being used, which will differ (to varying degree) from the real earth geoid.
 If you happen to be located in Colorado, 2km above msl, distances will be
 0.03% greater. In the UK, if you measure the distance from Land?s End to
 John O? Groats using WGS-84, it will be 28m ? 0.003% ? greater than using the
 Airy ellipsoid, which provides a better fit for the UK.}


                                UNIT GEO84;

                                 INTERFACE

//        Lat1, Lon1, Lat2, Lon2  in radians;   Result in km
//                    W and S are negative
//              Calculates TC12 (True) in radians


function
  Geodesic(const Lat1, Lon1, Lat2, Lon2: Double; out TC12: Double): Double;
procedure
  GeoRadial(const Lat1, Lon1, Tc, D: Double; out Lat, Lon: Double);



                              IMPLEMENTATION

uses
  SysUtils, Math;

const                                       // WGS-84 (meters)

  a0 = 6378137.0000;                        // Earth radius at equator
  b0 = 6356752.3142;                        // Earth radius at poles
  f0 = 1/298.257223563;                     // Flattening
  r0 = 1-f0;

  aa = a0*a0;   bb = b0*b0;


procedure RaiseException(Msg: string);
begin  raise Exception.Create(Msg)  end;


function
  GeoDesic(const Lat1, Lon1, Lat2, Lon2: Double; out TC12: Double): Double;
var
  IterLimit: Integer;

  L, U1, U2, SinU1, CosU1, SinU2, CosU2, Lambda, LambdaP, SinLambda, CosLambda,
  Sigma, SinSigma, CosSigma, Cos2SigmaM, SinAlpha, CosSqAlpha, uSqr, A, B, C,
  DeltaSigma, TC21: Extended;

begin
  if (Lat1 = Lat2) and (Lon1 = Lon2) then            // Coincident points
    begin
      Result := 0.0;
      Exit;
    end;

  L := Lon2-Lon1;

  if (Lat1 = 0) and (Lat2 = 0) then                  // Along equator
    begin
      Result := Abs(a0*L);
      if (L > 0) then
        TC12 := PI/2
      else
        TC12 := 3*PI/2;
      Exit;
    end;

  U1 := ArcTan(r0 * Tan(Lat1));
  U2 := ArcTan(r0 * Tan(Lat2));
  SinCos(U1, SinU1, CosU1);
  SinCos(U2, SinU2, CosU2);
  Lambda := L;
  LambdaP := 2*PI;
  IterLimit := 20;
  while (Abs(Lambda-LambdaP) > 1E-12) do
    begin
      SinCos(Lambda, SinLambda, CosLambda);
      SinSigma :=
        Sqrt((Sqr(CosU2*SinLambda)) + Sqr(CosU1*SinU2-SinU1*CosU2*CosLambda));
      CosSigma := SinU1*SinU2 + CosU1*CosU2*CosLambda;
      Sigma := ArcTan2(SinSigma, CosSigma);
      SinAlpha := CosU1 * CosU2 * SinLambda / SinSigma;
      CosSqAlpha := 1 - Sqr(SinAlpha);
      Cos2SigmaM := CosSigma - 2*SinU1*SinU2/CosSqAlpha;
      C := f0/16*CosSqAlpha*(4+f0*(4-3*CosSqAlpha));
      LambdaP := Lambda;
      Lambda := L + (1-C) * f0 * SinAlpha *
        (Sigma + C*SinSigma*(Cos2SigmaM+C*CosSigma*(-1+2*Sqr(Cos2SigmaM))));
      Dec(IterLimit, 1);
      if (iterLimit<=0) then
        RaiseException('GeoDis failed to converge');
    end;
  uSqr := CosSqAlpha * (aa-bb)/bb;
  A := 1 + uSqr/16384*(4096+uSqr*(-768+uSqr*(320-175*uSqr)));
  B := uSqr/1024 * (256+uSqr*(-128+uSqr*(74-47*uSqr)));
  DeltaSigma :=
    B*SinSigma*(Cos2SigmaM + B/4*(SinSigma*(-1+2*Sqr(Cos2SigmaM))-
    B/6*Cos2SigmaM*(-3+4*SinSigma*SinSigma)*(-3+4*Sqr(Cos2SigmaM))));
  Result := b0*A*(Sigma-DeltaSigma)/1000;

  TC12 := ArcTan2(CosU2*SinLambda, CosU1*SinU2 - SinU1*CosU2*CosLambda);
  if (TC12 < 0) then TC12 := 2*Pi + TC12;

  TC21 := ArcTan2(cosU1*SinLambda, (-sinU1*cosU2 + cosU1*sinU2*cosLambda))-pi;
  if (TC21 < 0) then TC21 := 2*Pi + TC21;
 
end;  // Vincinty's inverse algorithm


///////////////////////////////////////////////////////////////////////////////
//
//          Thaddeus Vincenty's direct formula for ellipsoids
//
//                Lat1, Lon1, Lat, Lon, Tc in radians
//                D in km


procedure GeoRadial(const Lat1, Lon1, Tc, D: Double; out Lat, Lon: Double);
var
  SinTc, CosTc, U1, TanU1, SinU1, CosU1, SinAlpha, CosAlpha, Usqr, S, A, B, C,
  Term1, Term2, Term3, Sigma, SinSigma, CosSigma, TanSigma1, DeltaSigma,
  Sigma1, TwoSigmaM, c2sm, LastSigma, Lambda, Omega: Extended;

  begin
    if D = 0.0 then                                  // Coincident points
      begin
        Lat := Lat1;                                 // TC21 := 0.0;
        Lon := Lon1;
        Exit;
      end;

    S := D*1000;
    TanU1 := r0 * Tan(Lat1);
    TanSigma1 := TanU1/Cos(Tc);                      //eq 1
    U1 := ArcTan(TanU1);
    SinCos(U1, SinU1, CosU1);
    SinAlpha := CosU1 * Sin(Tc);                     //eq 2
    CosAlpha := Sqrt(1 - Sqr(SinAlpha));
    Usqr := Sqr(CosAlpha) * (aa-bb)/bb;

    Term1 := Usqr/16384;
    Term2 := 4096 + Usqr*(-768 + Usqr*(320 - 175*Usqr));

    A := 1 + Term1 * Term2;
    B := Usqr / 1024*(256 + Usqr*(-128 + Usqr*(74 - 47*Usqr)));   //eq 4
                                                     
    Sigma := S / (b0*a);
    Sigma1 := ArcTan(TanSigma1);

    repeat
      Lastsigma := Sigma;
      TwoSigmaM := 2*Sigma1 + Sigma;                 //eq 5
      SinCos(Sigma, SinSigma, CosSigma);
      SinCos(Tc, SinTc, CosTc);
      c2sm := Cos(TwoSigmam);

      DeltaSigma :=                                 
        B*SinSigma*(c2sm + b/4 * (CosSigma*(-1 + 2*Sqr(c2sm)) -
        B / 6*c2sm * (-3 + 4*Sqr(SinSigma)) * (-3 + 4*Sqr(c2sm))));    //eq 6

      Sigma := S / (b0*a) + DeltaSigma;              //eq 7
    until (Abs(Sigma - LastSigma) <= 1E-12);

    TwoSigmam := 2*Sigma1 + Sigma;                   //eq 5
    c2sm := Cos(TwoSigmaM);
    SinCos(Sigma, SinSigma, CosSigma);
    Term1 := SinU1*CosSigma + CosU1*SinSigma*CosTc;
    Term2 := Sqr(SinAlpha) + Sqr(SinU1*SinSigma - CosU1*CosSigma*CosTc);
    Term3 := r0*Sqrt(Term2);
    Lat   := ArcTan2(Term1, Term3);
    Term1 := SinSigma*Sin(Tc);
    Term2 := CosU1*CosSigma - SinU1*SinSigma*CosTc;
    Lambda := ArcTan2(Term1, Term2);

    C := f0/ 16*Sqr(CosAlpha) * (4 + f0*(4 - 3*Sqr(CosAlpha)));

    Omega := Lambda - (1-c) * f0 * SinAlpha *
               (Sigma + C*SinSigma*(c2sm + C*CosSigma*(-1 + 2*Sqr(c2sm))));

    Lon := Lon1+Omega;

  end;  // Vincinty's direct algorithm

end.  //  GEO84