CartesianIntoGeodeticII Subroutine

subroutine CartesianIntoGeodeticII(ax, ay, az, r, latitude, longitude, altitude, error)

    real(wp),intent(in) :: ax, ay, az !! semiaxes of the celestial body: ax>ay>az
    real(wp),dimension(3),intent(in) :: r !! cartesian coordinates of the considered point
                                          !! in the first octant: xG, yG, zG with (xG,yG,zG)<>(0,0,0)
    real(wp),intent(out) :: latitude, longitude, altitude !! geodetic coordinates of the considered point
    real(wp),intent(in) :: error !! Values smaller than error treated as 0.0

    real(wp) :: aymaz,aypaz,axmaz,axpaz,axpaz2,ax2,ay2,az2,ax4,ay4,az4,az6,&
                az8,temp0,temp1,temp2,temp3,temp4,temp5,temp6,temp7,temp8,&
                temp9,tempa,az6ax2,az6ay2,tempb,maz10,excc,eecc
    real(wp) :: xg2,yg2,zg2,zgxg2,zgyg2,zg3,zg4,aux,xG,yG,zG
    real(wp) :: xE,yE,zE,k,B(0:6)
    logical :: done

    call special_cases(ax,ay,az,r(1),r(2),r(3),latitude,longitude,altitude,done)
    if (done) return

    ! Computations independent of xG,yG,zG. They can be precomputed, if necessary.
    aymaz = ay-az
    aypaz = ay+az
    axmaz = ax-az
    axpaz = ax+az
    axpaz2 = axpaz*axpaz
    ax2 = ax*ax
    ay2 = ay*ay
    az2 = az*az
    ax4 = ax2*ax2
    ay4 = ay2*ay2
    az4 = az2*az2
    az6 = az4*az2
    az8 = az4*az4
    temp0 = aymaz*aymaz*aypaz*aypaz*axmaz*axmaz*axpaz2
    temp1 = 2*az2*aymaz*aypaz*axmaz*axpaz*(ax2+ay2-2*az2)
    temp2 = -az2*(ax4*ay4-2*ax4*ay2*az2+ax4*az4-2*ax2*ay4*az2+4*ax2*ay2*az4-2*ay2*az6+az8-2*ax2*az6+ay4*az4)
    temp3 = -az2*(-ax2*ay4+2*ax2*ay2*az2-ax2*az4)
    temp4 = -az2*(-ax4*ay2+2*ax2*ay2*az2-ay2*az4)
    temp5 = -az2*(-ax4*az2-4*ax2*ay2*az2+6*ax2*az4-ay4*az2+6*ay2*az4-6*az6)
    temp6 =  -2*az4*(ax4*ay2-ax4*az2+ax2*ay4-4*ax2*ay2*az2+3*ax2*az4-ay4*az2+3*ay2*az4-2*az6)
    temp7 = -2*az4*(-ax2*ay2+ax2*az2)
    temp8 = -2*az4*(-ax2*ay2+ay2*az2)
    temp9 = -2*az4*(-ax2*az2-ay2*az2+2*az4)
    tempa = -az6*(ax4+4*ax2*ay2-6*ax2*az2+ay4-6*ay2*az2+6*az4)
    az6ax2 = az6*ax2
    az6ay2 = az6*ay2
    tempb = -2*az8*(ax2+ay2-2*az2)
    maz10 = -az6*az4
    excc = (ax2-az2)/(ax2)
    eecc = (ax2-ay2)/(ax2)

    xG = abs(r(1))
    yG = abs(r(2))
    zG = abs(r(3))
    xg2 = xG*xG
    yg2 = yG*yG
    zg2 = zG*zG
    zgxg2 = zG*xg2
    zgyg2 = zG*yg2
    zg3 = zg2*zG
    zg4 = zg2*zg2
    aux = xg2/ax2+yg2/ay2+zg2/az2

  if (abs(aux-1.0_wp) < error) then ! The point is on the ellipsoid

    xE = xG
    yE = yG
    zE = zG

  else if (zG > error) then ! The point is inside or outside the ellipsoid with zG != 0

    B(6) = temp0
    B(5) = temp1*zG
    B(4) = temp2+temp3*xg2+temp4*yg2+temp5*zg2
    B(3) = zG*temp6+temp7*zgxg2+temp8*zgyg2+temp9*zg3
    B(2) = zg2*(tempa+az6ax2*xg2+az6ay2*yg2+az8*zg2)
    B(1) = tempb*zg3
    B(0) = maz10*zg4

    k = solve_polynomial(B,az*zG/norm2([xG,yG,zG]),error)
    xE = ax2*xG*k/(ax2*k-az2*k+az2*zG)
    yE = ay2*yG*k/(ay2*k-az2*k+az2*zG)
    zE = k

  else if (yG > error) then

    B = [-ay4*ay2*zg2, &
         -2*ay4*(ax2-ay2)*zG, &
         -ay2*(ax4-2*ax2*ay2-ax2*yg2+ay4-ay2*zg2), &
         2*ay2*(ax2-ay2)*zG, &
         ax4+ay4-2*ax2*ay2, &
         0.0_wp, &
         0.0_wp ]

    k = solve_polynomial(B,ay*yG/norm2([xG,yG,zG]),error)
    xE = k*ax2*xG/(ax2*k-ay2*k+ay2*yG)
    yE = k
    zE = 0.0_wp

  else

    xE = ax
    yE = 0.0_wp
    zE = 0.0_wp

  end if

  ! Computing longitude

  if (xG > 0.0_wp) then
    longitude = atan(yE/((1.0_wp-eecc)*xE))
  else if (yG > 0.0_wp) then
    longitude = halfpi
  else
    longitude = huge(1.0_wp) ! undefined
  end if

  ! Computing latitude

  if (xE > 0.0_wp .or. yE > 0.0_wp) then
    latitude = atan((1.0_wp-eecc)/(1.0_wp-excc)*zE/norm2([xE*(1.0_wp-eecc),yE]))
  else
    latitude = halfpi
  end if

  ! Computing altitude

  if (aux>=1.0) then
    altitude = norm2([xE-xG,yE-yG,zE-zG])
  else
    altitude = -norm2([xE-xG,yE-yG,zE-zG])
  end if

  call philambda_quadrant(r(1), r(2), r(3), latitude, longitude)

end subroutine CartesianIntoGeodeticII