Implement an iterative inverse method for +proj=adams_ws2

1 files changed, 120 insertions, 18 deletions

diff --git a/src/projections/adams.cpp b/src/projections/adams.cpp
index 397584c7..89eab795 100644
--- a/src/projections/adams.cpp
+++ b/src/projections/adams.cpp
@@ -27,6 +27,8 @@
 #include <math.h>
 #include <errno.h>
 
+#include <algorithm>
+
 #include "proj.h"
 #include "proj_internal.h"
 
@@ -62,9 +64,7 @@ static double ell_int_5(double phi) {
  * The approximation is performed with an even Chebyshev
  * series, thus the coefficients below are the even values
  * and where series evaluation  must be multiplied by the argument. */
-    double d1 = 0.0;
-    double d2 = 0.0;
-    static double C0 = 2.19174570831038;
+    constexpr double C0 = 2.19174570831038;
     static const double C[] = {
         -8.58691003636495e-07,
         2.02692115653689e-07,
@@ -78,6 +78,8 @@ static double ell_int_5(double phi) {
     double y = phi * M_2_PI;
     y = 2. * y * y - 1.;
     double y2 = 2. * y;
+    double d1 = 0.0;
+    double d2 = 0.0;
     for ( double c: C ) {
         double temp = d1;
         d1 = y2 * d1 - d2 + c;
@@ -88,11 +90,11 @@ static double ell_int_5(double phi) {
 
 }
 
-static PJ_XY forward(PJ_LP lp, PJ *P) {
+static PJ_XY adams_forward(PJ_LP lp, PJ *P) {
     double a=0., b=0.;
     bool sm=false, sn=false;
     PJ_XY xy;
-    struct pj_opaque *Q = static_cast<struct pj_opaque*>(P->opaque);
+    const struct pj_opaque *Q = static_cast<const struct pj_opaque*>(P->opaque);
 
     switch (Q->mode) {
     case GUYOU:
@@ -106,9 +108,9 @@ static PJ_XY forward(PJ_LP lp, PJ *P) {
             xy.y = lp.phi < 0 ? -1.85407 : 1.85407;
             return xy;
         } else {
-            double sl = sin(lp.lam);
-            double sp = sin(lp.phi);
-            double cp = cos(lp.phi);
+            const double sl = sin(lp.lam);
+            const double sp = sin(lp.phi);
+            const double cp = cos(lp.phi);
             a = aacos(P->ctx, (cp * sl - sp) * RSQRT2);
             b = aacos(P->ctx, (cp * sl + sp) * RSQRT2);
             sm = lp.lam < 0.;
@@ -116,9 +118,9 @@ static PJ_XY forward(PJ_LP lp, PJ *P) {
         }
         break;
     case PEIRCE_Q: {
-            double sl = sin(lp.lam);
-            double cl = cos(lp.lam);
-            double cp = cos(lp.phi);
+            const double sl = sin(lp.lam);
+            const double cl = cos(lp.lam);
+            const double cp = cos(lp.phi);
             a = aacos(P->ctx, cp * (sl + cl) * RSQRT2);
             b = aacos(P->ctx, cp * (sl - cl) * RSQRT2);
             sm = sl < 0.;
@@ -126,7 +128,7 @@ static PJ_XY forward(PJ_LP lp, PJ *P) {
         }
         break;
     case ADAMS_HEMI: {
-            double sp = sin(lp.phi);
+            const double sp = sin(lp.phi);
             if ((fabs(lp.lam) - TOL) > M_PI_2) {
                 proj_errno_set(P, PJD_ERR_TOLERANCE_CONDITION);
                 return proj_coord_error().xy;
@@ -139,7 +141,7 @@ static PJ_XY forward(PJ_LP lp, PJ *P) {
         }
         break;
     case ADAMS_WS1: {
-            double sp = tan(0.5 * lp.phi);
+            const double sp = tan(0.5 * lp.phi);
             b = cos(aasin(P->ctx, sp)) * sin(0.5 * lp.lam);
             a = aacos(P->ctx, (b - sp) * RSQRT2);
             b = aacos(P->ctx, (b + sp) * RSQRT2);
@@ -148,7 +150,7 @@ static PJ_XY forward(PJ_LP lp, PJ *P) {
         }
         break;
     case ADAMS_WS2: {
-            double spp = tan(0.5 * lp.phi);
+            const double spp = tan(0.5 * lp.phi);
             a = cos(aasin(P->ctx, spp)) * sin(0.5 * lp.lam);
             sm = (spp + a) < 0.;
             sn = (spp - a) < 0.;
@@ -158,17 +160,17 @@ static PJ_XY forward(PJ_LP lp, PJ *P) {
         break;
     }
 
-    double m = aasin(P->ctx, sqrt((1. + cos(a + b))));
+    double m = aasin(P->ctx, sqrt((1. + std::min(0.0, cos(a + b)))));
     if (sm) m = -m;
 
-    double n = aasin(P->ctx, sqrt(fabs(1. - cos(a - b))));
+    double n = aasin(P->ctx, sqrt(fabs(1. - std::max(0.0, cos(a - b)))));
     if (sn) n = -n;
 
     xy.x = ell_int_5(m);
     xy.y = ell_int_5(n);
 
     if (Q->mode == ADAMS_HEMI || Q->mode == ADAMS_WS2) { /* rotate by 45deg. */
-        double temp = xy.x;
+        const double temp = xy.x;
         xy.x = RSQRT2 * (xy.x - xy.y);
         xy.y = RSQRT2 * (temp + xy.y);
     }
@@ -176,6 +178,104 @@ static PJ_XY forward(PJ_LP lp, PJ *P) {
     return xy;
 }
 
+static PJ_LP inverse_with_newton_raphson(PJ_XY xy,
+                                         PJ *P,
+                                         PJ_LP lp, // inital guess
+                                         PJ_XY  (*fwd)(PJ_LP,    PJ *))
+{
+    double deriv_lam_X = 0;
+    double deriv_lam_Y = 0;
+    double deriv_phi_X = 0;
+    double deriv_phi_Y = 0;
+    for(int i = 0; i < 15; i++ )
+    {
+        PJ_XY xyApprox = fwd(lp, P);
+        const double deltaX = xyApprox.x - xy.x;
+        const double deltaY = xyApprox.y - xy.y;
+        if( fabs(deltaX) < 1e-10 && fabs(deltaY) < 1e-10 )
+        {
+            return lp;
+        }
+
+        if( fabs(deltaX) > 1e-6 || fabs(deltaY) > 1e-6 )
+        {
+            // Compute Jacobian matrix (only if we aren't close to the final
+            // result to speed things a bit)
+            PJ_LP lp2;
+            PJ_XY xy2;
+            const double dLam = lp.lam > 0 ? -1e-6 : 1e-6;
+            lp2.lam = lp.lam + dLam;
+            lp2.phi = lp.phi;
+            xy2 = fwd(lp2, P);
+            const double deriv_X_lam = (xy2.x - xyApprox.x) / dLam;
+            const double deriv_Y_lam = (xy2.y - xyApprox.y) / dLam;
+
+            const double dPhi = lp.phi > 0 ? -1e-6 : 1e-6;
+            lp2.lam = lp.lam;
+            lp2.phi = lp.phi + dPhi;
+            xy2 = fwd(lp2, P);
+            const double deriv_X_phi = (xy2.x - xyApprox.x) / dPhi;
+            const double deriv_Y_phi = (xy2.y - xyApprox.y) / dPhi;
+
+            // Inverse of Jacobian matrix
+            const double det = deriv_X_lam * deriv_Y_phi - deriv_X_phi * deriv_Y_lam;
+            if( det != 0  )
+            {
+                deriv_lam_X =  deriv_Y_phi / det;
+                deriv_lam_Y = -deriv_X_phi / det;
+                deriv_phi_X = -deriv_Y_lam / det;
+                deriv_phi_Y =  deriv_X_lam / det;
+            }
+        }
+
+        if( xy.x != 0 )
+        {
+            // Limit the amplitude of correction to avoid overshoots due to
+            // bad initial guess
+            const double delta_lam = std::max(std::min(
+                deltaX * deriv_lam_X + deltaY * deriv_lam_Y, 0.3), -0.3);
+            lp.lam -= delta_lam;
+            if( lp.lam < -M_PI )
+                lp.lam = -M_PI;
+            else if( lp.lam > M_PI )
+                lp.lam = M_PI;
+        }
+
+        if( xy.y != 0 )
+        {
+            const double delta_phi = std::max(std::min(
+                deltaX * deriv_phi_X + deltaY * deriv_phi_Y, 0.3), -0.3);
+            lp.phi -= delta_phi;
+            if( lp.phi < -M_HALFPI )
+                lp.phi = -M_HALFPI ;
+            else if( lp.phi > M_HALFPI )
+                lp.phi = M_HALFPI;
+        }
+    }
+    pj_ctx_set_errno( P->ctx, PJD_ERR_NON_CONVERGENT );
+    return lp;
+}
+
+static PJ_LP adams_inverse(PJ_XY xy, PJ *P)
+{
+    PJ_LP lp;
+
+    // Only implemented for ADAMS_WS2
+    // Uses Newton-Raphson method on the following pair of functions:
+    //      f_x(lam,phi) = adams_forward(lam, phi).x - xy.x
+    //      f_y(lam,phi) = adams_forward(lam, phi).y - xy.y
+
+    // Initial guess (very rough, especially at high northings)
+    // The magic values are got with:
+    //  echo 0   90 | src/proj -f "%.8f" +proj=adams_ws2 +R=1
+    //  echo 180 0  | src/proj -f "%.8f" +proj=adams_ws2 +R=1
+    lp.phi = std::max(std::min(xy.y / 2.62181347, 1.0), -1.0) * M_HALFPI;
+    lp.lam = fabs(lp.phi) >= M_HALFPI ? 0 :
+        std::max(std::min(xy.x / 2.62205760 / cos(lp.phi), 1.0), -1.0) * M_PI;
+
+    return inverse_with_newton_raphson(xy, P, lp, adams_forward);
+}
+
 
 static PJ *setup(PJ *P, projection_type mode) {
     struct pj_opaque *Q = static_cast<struct pj_opaque*>(
@@ -186,9 +286,11 @@ static PJ *setup(PJ *P, projection_type mode) {
     P->opaque = Q;
 
     P->es = 0;
-    P->fwd = forward;
+    P->fwd = adams_forward;
 
     Q->mode = mode;
+    if( mode == ADAMS_WS2 )
+        P->inv = adams_inverse;
 
     return P;
 }