direct Subroutine

subroutine direct(a, f, lat1, lon1, azi1, s12a12, flags, &
                  lat2, lon2, azi2, outmask, a12s12, m12, MM12, MM21, SS12)

  real(wp), intent(in) :: a !! the equatorial radius (meters).
  real(wp), intent(in) :: f !! the flattening of the ellipsoid.  Setting `f = 0` gives
                                    !! a sphere.  Negative `f` gives a prolate ellipsoid.
  real(wp), intent(in) :: lat1 !! lat1 latitude of point 1 (degrees). `lat1` should be in the range [-90 deg, 90 deg].
  real(wp), intent(in) :: lon1 !! lon1 longitude of point 1 (degrees).
  real(wp), intent(in) :: azi1 !! azi1 azimuth at point 1 (degrees).
  real(wp), intent(in) :: s12a12 !! if `arcmode` is not set, this is the distance
                                         !! from point 1 to point 2 (meters); otherwise it is the arc
                                         !! length from point 1 to point 2 (degrees); it can be negative.
  integer, intent(in) :: flags !! a bitor'ed combination of the `arcmode` and `unroll` flags.
                               !!
                               !! `flags` is an integer in [0, 4) whose binary bits are interpreted as follows:
                               !!
                               !!  * 1 the `arcmode` flag
                               !!  * 2 the `unroll` flag
  integer, intent(in) :: outmask !! a bitor'ed combination of mask values
                               !! specifying which of the following parameters should be set.
                               !!
                               !! outmask is an integer in [0, 16) whose binary bits are interpreted
                               !! as follows:
                               !!
                               !!  * 1 return `a12`
                               !!  * 2 return `m12`
                               !!  * 4 return `MM12` and `MM21`
                               !!  * 8 return `SS12`
  real(wp), intent(out) :: lat2 !! latitude of point 2 (degrees).
  real(wp), intent(out) :: lon2 !! longitude of point 2 (degrees).
  real(wp), intent(out) :: azi2 !! (forward) azimuth at point 2 (degrees). The value `azi2` returned is in the range [-180 deg, 180 deg].
  real(wp), intent(out) :: a12s12 !! if `arcmode` is not set, this is the arc length
                                          !! from point 1 to point 2 (degrees); otherwise it is the distance
                                          !! from point 1 to point 2 (meters).
  real(wp), intent(out) :: m12 !! reduced length of geodesic (meters).
  real(wp), intent(out) :: MM12 !! geodesic scale of point 2 relative to point 1 (dimensionless).
  real(wp), intent(out) :: MM21 !! geodesic scale of point 1 relative to point 2 (dimensionless).
  real(wp), intent(out) :: SS12 !! area under the geodesic (\(m^2\)).

  integer,parameter :: ord = 6
  integer,parameter :: nC1 = ord
  integer,parameter :: nC1p = ord
  integer,parameter :: nC2 = ord
  integer,parameter :: nA3 = ord
  integer,parameter :: nA3x = nA3
  integer,parameter :: nC3 = ord
  integer,parameter :: nC3x = (nC3 * (nC3 - 1)) / 2
  integer,parameter :: nC4 = ord
  integer,parameter :: nC4x = (nC4 * (nC4 + 1)) / 2

  real(wp) :: A3x(0:nA3x-1), C3x(0:nC3x-1), C4x(0:nC4x-1), &
              C1a(nC1), C1pa(nC1p), C2a(nC2), C3a(nC3-1), C4a(0:nC4-1)

  logical :: arcmode, unroll, arcp, redlp, scalp, areap
  real(wp) :: e2, f1, ep2, n, b, c2, &
              salp0, calp0, k2, eps, &
              salp1, calp1, ssig1, csig1, cbet1, sbet1, dn1, somg1, comg1, &
              salp2, calp2, ssig2, csig2, sbet2, cbet2, dn2, somg2, comg2, &
              ssig12, csig12, salp12, calp12, omg12, lam12, lon12, &
              sig12, stau1, ctau1, tau12, t, s, c, serr, E, &
              A1m1, A2m1, A3c, A4, AB1, AB2, &
              B11, B12, B21, B22, B31, B41, B42, J12

e2 = f * (2.0_wp - f)
ep2 = e2 / (1.0_wp - e2)
f1 = 1.0_wp - f
n = f / (2.0_wp - f)
b = a * f1
c2 = 0

arcmode = mod(flags/1, 2) == 1
unroll = mod(flags/2, 2) == 1

arcp = mod(outmask/1, 2) == 1
redlp = mod(outmask/2, 2) == 1
scalp = mod(outmask/4, 2) == 1
areap = mod(outmask/8, 2) == 1

if (areap) then
  if (e2 == 0) then
    c2 = a**2
  else if (e2 > 0) then
    c2 = (a**2 + b**2 * atanh(sqrt(e2)) / sqrt(e2)) / 2.0_wp
  else
    c2 = (a**2 + b**2 * atan(sqrt(abs(e2))) / sqrt(abs(e2))) / 2.0_wp
  end if
end if

call A3coeff(n, A3x)
call C3coeff(n, C3x)
if (areap) call C4coeff(n, C4x)

! Guard against underflow in salp0
call sincosd(AngRound(azi1), salp1, calp1)

call sincosd(AngRound(LatFix(lat1)), sbet1, cbet1)
sbet1 = f1 * sbet1
call norm(sbet1, cbet1)
! Ensure cbet1 = +dbleps at poles
cbet1 = max(tiny2, cbet1)
dn1 = sqrt(1 + ep2 * sbet1**2)

! Evaluate alp0 from sin(alp1) * cos(bet1) = sin(alp0),
! alp0 in [0, pi/2 - |bet1|]
salp0 = salp1 * cbet1
! Alt: calp0 = hypot(sbet1, calp1 * cbet1).  The following
! is slightly better (consider the case salp1 = 0).
calp0 = hypot(calp1, salp1 * sbet1)
! Evaluate sig with tan(bet1) = tan(sig1) * cos(alp1).
! sig = 0 is nearest northward crossing of equator.
! With bet1 = 0, alp1 = pi/2, we have sig1 = 0 (equatorial line).
! With bet1 =  pi/2, alp1 = -pi, sig1 =  pi/2
! With bet1 = -pi/2, alp1 =  0 , sig1 = -pi/2
! Evaluate omg1 with tan(omg1) = sin(alp0) * tan(sig1).
! With alp0 in (0, pi/2], quadrants for sig and omg coincide.
! No atan2(0,0) ambiguity at poles since cbet1 = +dbleps.
! With alp0 = 0, omg1 = 0 for alp1 = 0, omg1 = pi for alp1 = pi.
ssig1 = sbet1
somg1 = salp0 * sbet1
if (sbet1 /= 0 .or. calp1 /= 0) then
  csig1 = cbet1 * calp1
else
  csig1 = 1
end if
comg1 = csig1
! sig1 in (-pi, pi]
call norm(ssig1, csig1)
! norm(somg1, comg1); -- don't need to normalize!

k2 = calp0**2 * ep2
eps = k2 / (2 * (1 + sqrt(1 + k2)) + k2)

A1m1 = A1m1f(eps)
call C1f(eps, C1a)
B11 = SinCosSeries(.true., ssig1, csig1, C1a, nC1)
s = sin(B11)
c = cos(B11)
! tau1 = sig1 + B11
stau1 = ssig1 * c + csig1 * s
ctau1 = csig1 * c - ssig1 * s
! Not necessary because C1pa reverts C1a
!    B11 = -SinCosSeries(true, stau1, ctau1, C1pa, nC1p)

if (.not. arcmode) call C1pf(eps, C1pa)

if (redlp .or. scalp) then
  A2m1 = A2m1f(eps)
  call C2f(eps, C2a)
  B21 = SinCosSeries(.true., ssig1, csig1, C2a, nC2)
else
! Suppress bogus warnings about unitialized variables
  A2m1 = 0
  B21 = 0
end if

call C3f(eps, C3x, C3a)
A3c = -f * salp0 * A3f(eps, A3x)
B31 = SinCosSeries(.true., ssig1, csig1, C3a, nC3-1)

if (areap) then
  call C4f(eps, C4x, C4a)
! Multiplier = a^2 * e^2 * cos(alpha0) * sin(alpha0)
  A4 = a**2 * calp0 * salp0 * e2
  B41 = SinCosSeries(.false., ssig1, csig1, C4a, nC4)
else
! Suppress bogus warnings about unitialized variables
  A4 = 0
  B41 = 0
end if

if (arcmode) then
! Interpret s12a12 as spherical arc length
  sig12 = s12a12 * degree
  call sincosd(s12a12, ssig12, csig12)
! Suppress bogus warnings about unitialized variables
  B12 = 0
else
! Interpret s12a12 as distance
  tau12 = s12a12 / (b * (1 + A1m1))
  s = sin(tau12)
  c = cos(tau12)
! tau2 = tau1 + tau12
  B12 = - SinCosSeries(.true., &
      stau1 * c + ctau1 * s, ctau1 * c - stau1 * s, C1pa, nC1p)
  sig12 = tau12 - (B12 - B11)
  ssig12 = sin(sig12)
  csig12 = cos(sig12)
  if (abs(f) > 0.01_wp) then
! Reverted distance series is inaccurate for |f| > 1/100, so correct
! sig12 with 1 Newton iteration.  The following table shows the
! approximate maximum error for a = WGS_a() and various f relative to
! GeodesicExact.
!     erri = the error in the inverse solution (nm)
!     errd = the error in the direct solution (series only) (nm)
!     errda = the error in the direct solution (series + 1 Newton) (nm)
!
!       f     erri  errd errda
!     -1/5    12e6 1.2e9  69e6
!     -1/10  123e3  12e6 765e3
!     -1/20   1110 108e3  7155
!     -1/50  18.63 200.9 27.12
!     -1/100 18.63 23.78 23.37
!     -1/150 18.63 21.05 20.26
!      1/150 22.35 24.73 25.83
!      1/100 22.35 25.03 25.31
!      1/50  29.80 231.9 30.44
!      1/20   5376 146e3  10e3
!      1/10  829e3  22e6 1.5e6
!      1/5   157e6 3.8e9 280e6
    ssig2 = ssig1 * csig12 + csig1 * ssig12
    csig2 = csig1 * csig12 - ssig1 * ssig12
    B12 = SinCosSeries(.true., ssig2, csig2, C1a, nC1)
    serr = (1 + A1m1) * (sig12 + (B12 - B11)) - s12a12 / b
    sig12 = sig12 - serr / sqrt(1 + k2 * ssig2**2)
    ssig12 = sin(sig12)
    csig12 = cos(sig12)
! Update B12 below
  end if
end if

! sig2 = sig1 + sig12
ssig2 = ssig1 * csig12 + csig1 * ssig12
csig2 = csig1 * csig12 - ssig1 * ssig12
dn2 = sqrt(1 + k2 * ssig2**2)
if (arcmode .or. abs(f) > 0.01_wp) &
    B12 = SinCosSeries(.true., ssig2, csig2, C1a, nC1)
AB1 = (1 + A1m1) * (B12 - B11)

! sin(bet2) = cos(alp0) * sin(sig2)
sbet2 = calp0 * ssig2
! Alt: cbet2 = hypot(csig2, salp0 * ssig2)
cbet2 = hypot(salp0, calp0 * csig2)
if (cbet2 == 0) then
! I.e., salp0 = 0, csig2 = 0.  Break the degeneracy in this case
  cbet2 = tiny2
  csig2 = cbet2
end if
! tan(omg2) = sin(alp0) * tan(sig2)
! No need to normalize
somg2 = salp0 * ssig2
comg2 = csig2
! tan(alp0) = cos(sig2)*tan(alp2)
! No need to normalize
salp2 = salp0
calp2 = calp0 * csig2
! East or west going?
E = sign(1.0_wp, salp0)
! omg12 = omg2 - omg1
if (unroll) then
  omg12 = E * (sig12 &
      - (atan2(    ssig2, csig2) - atan2(    ssig1, csig1)) &
      + (atan2(E * somg2, comg2) - atan2(E * somg1, comg1)))
else
  omg12 = atan2(somg2 * comg1 - comg2 * somg1, &
      comg2 * comg1 + somg2 * somg1)
end if

lam12 = omg12 + A3c * &
    ( sig12 + (SinCosSeries(.true., ssig2, csig2, C3a, nC3-1) &
    - B31))
lon12 = lam12 / degree
if (unroll) then
  lon2 = lon1 + lon12
else
  lon2 = AngNormalize(AngNormalize(lon1) + AngNormalize(lon12))
end if
lat2 = atan2d(sbet2, f1 * cbet2)
azi2 = atan2d(salp2, calp2)

if (redlp .or. scalp) then
  B22 = SinCosSeries(.true., ssig2, csig2, C2a, nC2)
  AB2 = (1 + A2m1) * (B22 - B21)
  J12 = (A1m1 - A2m1) * sig12 + (AB1 - AB2)
end if
! Add parens around (csig1 * ssig2) and (ssig1 * csig2) to ensure
! accurate cancellation in the case of coincident points.
if (redlp) m12 = b * ((dn2 * (csig1 * ssig2) - &
    dn1 * (ssig1 * csig2)) - csig1 * csig2 * J12)
if (scalp) then
  t = k2 * (ssig2 - ssig1) * (ssig2 + ssig1) / (dn1 + dn2)
  MM12 = csig12 + (t * ssig2 - csig2 * J12) * ssig1 / dn1
  MM21 = csig12 - (t * ssig1 - csig1 * J12) * ssig2 / dn2
end if

if (areap) then
  B42 = SinCosSeries(.false., ssig2, csig2, C4a, nC4)
  if (calp0 == 0 .or. salp0 == 0) then
! alp12 = alp2 - alp1, used in atan2 so no need to normalize
    salp12 = salp2 * calp1 - calp2 * salp1
    calp12 = calp2 * calp1 + salp2 * salp1
  else
! tan(alp) = tan(alp0) * sec(sig)
! tan(alp2-alp1) = (tan(alp2) -tan(alp1)) / (tan(alp2)*tan(alp1)+1)
! = calp0 * salp0 * (csig1-csig2) / (salp0^2 + calp0^2 * csig1*csig2)
! If csig12 > 0, write
!   csig1 - csig2 = ssig12 * (csig1 * ssig12 / (1 + csig12) + ssig1)
! else
!   csig1 - csig2 = csig1 * (1 - csig12) + ssig12 * ssig1
! No need to normalize
    if (csig12 <= 0) then
      salp12 = csig1 * (1 - csig12) + ssig12 * ssig1
    else
      salp12 = ssig12 * (csig1 * ssig12 / (1 + csig12) + ssig1)
    end if
    salp12 = calp0 * salp0 * salp12
    calp12 = salp0**2 + calp0**2 * csig1 * csig2
  end if
  SS12 = c2 * atan2(salp12, calp12) + A4 * (B42 - B41)
end if

if (arcp) then
  if (arcmode) then
    a12s12 = b * ((1 + A1m1) * sig12 + AB1)
  else
    a12s12 = sig12 / degree
  end if
end if

end subroutine direct