cartesian_to_geodetic_triaxial Subroutine

subroutine cartesian_to_geodetic_triaxial(ax, ay, b, r, tol, phi, lambda, h)

    real(wp),intent(in) :: ax !! semiaxes (0 < b <= ay <= ax)
    real(wp),intent(in) :: ay !! semiaxes (0 < b <= ay <= ax)
    real(wp),intent(in) :: b  !! semiaxes (0 < b <= ay <= ax)
    real(wp),dimension(3),intent(in) :: r !! Cartesian coordinates (x,y,z)
    real(wp),intent(in) :: tol !! tolerance (may be set to zero)
    real(wp),intent(out) :: phi !! geodetic latitude (radians)
    real(wp),intent(out) :: lambda !! geodetic longitude (radians)
    real(wp),intent(out) :: h !! geodetic height

    real(wp) :: kx,ky,cx,cy,cz,XX,YY,ZZ,x,y,z,Xo,Yo,Zo,m,Mm,axax,ayay,b2
    integer :: n

    if (ax < ay .or. ay < b) error stop 'error in cartesian_to_geodetic_triaxial: invalid ax,ay,b'

    axax = ax*ax
    ayay = ay*ay
    b2   = b*b
    kx   = (axax-b2)/ax
    ky   = (ayay-b2)/ay
    cx   = (axax)/(b2)
    cy   = (ayay)/(b2)
    cz   = (axax)/(ayay)

    XX = abs(r(1))
    YY = abs(r(2))
    ZZ = abs(r(3))

    ! Compute geodetic latitude/longitude
    if (ZZ == zero) then
        if (XX == zero .and. YY == zero) then
            x = zero
            y = zero
            z = b
        else if (ky*XX*ky*XX+kx*YY*kx*YY < kx*ky*kx*ky) then
            x = ax*XX/kx
            y = ay*YY/ky
            z = b*sqrt(one-((x*x)/(axax))-((y*y)/(ayay)))
        else if (XX == zero) then
            x = zero
            y = ay
            z = zero
        else if (YY == zero) then
            x = ax
            y = zero
            z = zero
        else
            Xo = XX/ax
            Yo = YY/ay
            call bisection_special_2(cz, Xo, Yo, tol, n, m, Mm)
            x = cz*XX/(cz+m)
            y = YY/(one+m)
            z = zero
        end if
    else
        if (XX == zero .and. YY == zero) then
            x = zero
            y = zero
            z = b
        else
            Xo = XX/ax
            Yo = YY/ay
            Zo = ZZ/b
            call bisection_special_3(cx, cy, Xo, Yo, Zo, tol, n, m, Mm)
            x = cx*XX/(cx+m)
            y = cy*YY/(cy+m)
            if (m < zero .and. ky*XX*ky*XX + kx*YY*kx*YY < kx*ky*kx*ky) then
                z = b*sqrt(one-((x*x)/(axax))-((y*y)/(ayay)))
            else
                z = ZZ/(one+m)
            end if
        end if
    end if

    call xyz2fl(ax, ay, b, x, y, z, phi, lambda)        ! analytic method used for initial guess
    call xyz2philambda(ax, ay, b, x, y, z, phi, lambda) ! iterative method
    call philambda_quadrant(r(1), r(2), r(3), phi, lambda)

    ! Compute geodetic height
    h = norm2([XX-x, YY-y, ZZ-z])
    if ((XX+YY+ZZ) < (x+y+z)) h = -h

end subroutine cartesian_to_geodetic_triaxial