Inverse cart: speed-up computation by 33%

Saves 2 sincos() and 1 atan2() calls.

With the following bench
```
 #include "proj.h"
 #include <stdlib.h>
 #include <stdio.h>
 #include <string.h>

int main(int argc, char* argv[])
{
    if( argc != 2 ) {
        fprintf(stderr, "Usage: bench_inv_cart fwd/inv\n");
        exit(1);
    }
    PJ* p = proj_create(0, "+proj=cart");
    const int dir = strcmp(argv[1], "inv") == 0 ? PJ_INV : PJ_FWD;
    PJ_COORD coord;
    if( dir == PJ_FWD )
    {
        coord.xyz.x = 3.14159/2;
        coord.xyz.y = 3.14159/2;
        coord.xyz.z = 100;
    }
    else
    {
        coord.xyz.x = 3e6;
        coord.xyz.y = 3e6;
        coord.xyz.z = 3e6;
    }
    for(int i = 0; i < 10* 1024 * 1024; i++ )
    {
        proj_trans(p, dir, coord);
    }

    proj_destroy(p);
    return 0;
}
```

On Intel(R) Core(TM) i7-6700HQ CPU @ 2.60GHz
Time before: 2.37s
Time after:  1.57s

1 files changed, 38 insertions, 22 deletions

diff --git a/src/conversions/cart.cpp b/src/conversions/cart.cpp
index 537fc29f..364bbb92 100644
--- a/src/conversions/cart.cpp
+++ b/src/conversions/cart.cpp
@@ -107,17 +107,16 @@ PROJ_HEAD(cart,    "Geodetic/cartesian conversions");
 
 
 /*********************************************************************/
-static double normal_radius_of_curvature (double a, double es, double phi) {
+static double normal_radius_of_curvature (double a, double es, double sinphi) {
 /*********************************************************************/
-    double s = sin(phi);
     if (es==0)
         return a;
     /* This is from WP.  HM formula 2-149 gives an a,b version */
-    return a / sqrt (1 - es*s*s);
+    return a / sqrt (1 - es*sinphi*sinphi);
 }
 
 /*********************************************************************/
-static double geocentric_radius (double a, double b, double phi) {
+static double geocentric_radius (double a, double b, double cosphi, double sinphi) {
 /*********************************************************************
     Return the geocentric radius at latitude phi, of an ellipsoid
     with semimajor axis a and semiminor axis b.
@@ -125,22 +124,23 @@ static double geocentric_radius (double a, double b, double phi) {
     This is from WP2, but uses hypot() for potentially better
     numerical robustness
 ***********************************************************************/
-    return hypot (a*a*cos (phi), b*b*sin(phi)) / hypot (a*cos(phi), b*sin(phi));
+    return hypot (a*a*cosphi, b*b*sinphi) / hypot (a*cosphi, b*sinphi);
 }
 
 
 /*********************************************************************/
 static PJ_XYZ cartesian (PJ_LPZ geod,  PJ *P) {
 /*********************************************************************/
-    double N, cosphi = cos(geod.phi);
     PJ_XYZ xyz;
 
-    N   =  normal_radius_of_curvature(P->a, P->es, geod.phi);
+    const double cosphi = cos(geod.phi);
+    const double sinphi = sin(geod.phi);
+    const double N   =  normal_radius_of_curvature(P->a, P->es, sinphi);
 
     /* HM formula 5-27 (z formula follows WP) */
     xyz.x = (N + geod.z) * cosphi      * cos(geod.lam);
     xyz.y = (N + geod.z) * cosphi      * sin(geod.lam);
-    xyz.z = (N * (1 - P->es) + geod.z) * sin(geod.phi);
+    xyz.z = (N * (1 - P->es) + geod.z) * sinphi;
 
     return xyz;
 }
@@ -149,41 +149,57 @@ static PJ_XYZ cartesian (PJ_LPZ geod,  PJ *P) {
 /*********************************************************************/
 static PJ_LPZ geodetic (PJ_XYZ cart,  PJ *P) {
 /*********************************************************************/
-    double N, p, theta, c, s;
     PJ_LPZ lpz;
 
     /* Perpendicular distance from point to Z-axis (HM eq. 5-28) */
-    p = hypot (cart.x, cart.y);
+    const double p = hypot (cart.x, cart.y);
 
+#if 0
     /* HM eq. (5-37) */
-    theta  =  atan2 (cart.z * P->a,  p * P->b);
+    const double theta  =  atan2 (cart.z * P->a,  p * P->b);
 
     /* HM eq. (5-36) (from BB, 1976) */
-    c  =  cos(theta);
-    s  =  sin(theta);
-    lpz.phi  =  atan2 (cart.z + P->e2s*P->b*s*s*s,  p - P->es*P->a*c*c*c);
-    if( fabs(lpz.phi) > M_HALFPI ) {
+    const double c  =  cos(theta);
+    const double s  =  sin(theta);
+#else
+    const double y_theta = cart.z * P->a;
+    const double x_theta = p * P->b;
+    const double norm = hypot(y_theta, x_theta);
+    const double c = norm == 0 ? 1 : x_theta / norm;
+    const double s = norm == 0 ? 0 : y_theta / norm;
+#endif
+
+    const double y_phi = cart.z + P->e2s*P->b*s*s*s;
+    const double x_phi = p - P->es*P->a*c*c*c;
+    const double norm_phi = hypot(y_phi, x_phi);
+    double cosphi   = norm_phi == 0 ? 1 : x_phi / norm_phi;
+    double sinphi   = norm_phi == 0 ? 0 : y_phi / norm_phi;
+    if( x_phi <= 0 ) {
         // this happen on non-sphere ellipsoid when x,y,z is very close to 0
         // there is no single solution to the cart->geodetic conversion in
         // that case, clamp to -90/90 deg and avoid a discontinuous boundary
         // near the poles
-        lpz.phi = copysign(M_HALFPI, lpz.phi);
+        lpz.phi = cart.z >= 0 ? M_HALFPI : -M_HALFPI;
+        cosphi = 0;
+        sinphi = cart.z >= 0 ? 1 : -1;
+    } else {
+        lpz.phi  =  atan (y_phi / x_phi);
     }
     lpz.lam  =  atan2 (cart.y, cart.x);
-    N        =  normal_radius_of_curvature (P->a, P->es, lpz.phi);
 
-
-    c  =  cos(lpz.phi);
-    if (fabs(c) < 1e-6) {
+    if (cosphi < 1e-6) {
         /* poleward of 89.99994 deg, we avoid division by zero   */
         /* by computing the height as the cartesian z value      */
         /* minus the geocentric radius of the Earth at the given */
         /* latitude                                              */
-        double r = geocentric_radius (P->a, P->b, lpz.phi);
+        const double r = geocentric_radius (P->a, P->b, cosphi, sinphi);
         lpz.z = fabs (cart.z) - r;
     }
     else
-        lpz.z =  p / c  -  N;
+    {
+        const double N  =  normal_radius_of_curvature (P->a, P->es, sinphi);
+        lpz.z =  p / cosphi  -  N;
+    }
 
     return lpz;
 }