deformation.cpp and xyzgridshift.cpp: factor out common code (addresses https://github.com/OSGeo/PROJ/pull/1790#discussion_r363748615)

4 files changed, 107 insertions, 128 deletions

diff --git a/src/grids.cpp b/src/grids.cpp
index 0904a3c2..092b6903 100644
--- a/src/grids.cpp
+++ b/src/grids.cpp
@@ -2770,7 +2770,7 @@ ListOfGenericGrids pj_generic_grid_init(PJ *P, const char *gridkey) {
 // ---------------------------------------------------------------------------
 
 static const HorizontalShiftGrid *
-findGrid(const ListOfHGrids &grids, const PJ_LP& input,
+findGrid(const ListOfHGrids &grids, const PJ_LP &input,
          HorizontalShiftGridSet *&gridSetOut) {
     for (const auto &gridset : grids) {
         auto grid = gridset->gridAt(input.lam, input.phi);
@@ -3085,7 +3085,7 @@ PJ_LP pj_hgrid_value(PJ *P, const ListOfHGrids &grids, PJ_LP lp) {
 // ---------------------------------------------------------------------------
 
 static double read_vgrid_value(PJ_CONTEXT *ctx, const ListOfVGrids &grids,
-                               const PJ_LP& input, const double vmultiplier) {
+                               const PJ_LP &input, const double vmultiplier) {
 
     /* do not deal with NaN coordinates */
     /* cppcheck-suppress duplicateExpression */
@@ -3257,6 +3257,80 @@ double pj_vgrid_value(PJ *P, const ListOfVGrids &grids, PJ_LP lp,
     return value;
 }
 
+// ---------------------------------------------------------------------------
+
+const GenericShiftGrid *pj_find_generic_grid(const ListOfGenericGrids &grids,
+                                             const PJ_LP &input,
+                                             GenericShiftGridSet *&gridSetOut) {
+    for (const auto &gridset : grids) {
+        auto grid = gridset->gridAt(input.lam, input.phi);
+        if (grid) {
+            gridSetOut = gridset.get();
+            return grid;
+        }
+    }
+    return nullptr;
+}
+
+// ---------------------------------------------------------------------------
+
+bool pj_bilinear_interpolation_three_samples(const GenericShiftGrid *grid,
+                                             const PJ_LP &lp, int idx1,
+                                             int idx2, int idx3, double &v1,
+                                             double &v2, double &v3,
+                                             bool &must_retry) {
+    must_retry = false;
+
+    const auto &extent = grid->extentAndRes();
+    double grid_x = (lp.lam - extent.westLon) / extent.resLon;
+    double grid_y = (lp.phi - extent.southLat) / extent.resLat;
+    int ix = static_cast<int>(grid_x);
+    int iy = static_cast<int>(grid_y);
+    int ix2 = std::min(ix + 1, grid->width() - 1);
+    int iy2 = std::min(iy + 1, grid->height() - 1);
+
+    float dx1, dy1, dz1;
+    float dx2, dy2, dz2;
+    float dx3, dy3, dz3;
+    float dx4, dy4, dz4;
+    bool error = (!grid->valueAt(ix, iy, idx1, dx1) ||
+                  !grid->valueAt(ix, iy, idx2, dy1) ||
+                  !grid->valueAt(ix, iy, idx3, dz1) ||
+                  !grid->valueAt(ix2, iy, idx1, dx2) ||
+                  !grid->valueAt(ix2, iy, idx2, dy2) ||
+                  !grid->valueAt(ix2, iy, idx3, dz2) ||
+                  !grid->valueAt(ix, iy2, idx1, dx3) ||
+                  !grid->valueAt(ix, iy2, idx2, dy3) ||
+                  !grid->valueAt(ix, iy2, idx3, dz3) ||
+                  !grid->valueAt(ix2, iy2, idx1, dx4) ||
+                  !grid->valueAt(ix2, iy2, idx2, dy4) ||
+                  !grid->valueAt(ix2, iy2, idx3, dz4));
+    if (grid->hasChanged()) {
+        must_retry = true;
+        return false;
+    }
+    if (error) {
+        return false;
+    }
+
+    double frct_lam = grid_x - ix;
+    double frct_phi = grid_y - iy;
+    double m10 = frct_lam;
+    double m11 = m10;
+    double m01 = 1. - frct_lam;
+    double m00 = m01;
+    m11 *= frct_phi;
+    m01 *= frct_phi;
+    frct_phi = 1. - frct_phi;
+    m00 *= frct_phi;
+    m10 *= frct_phi;
+
+    v1 = m00 * dx1 + m10 * dx2 + m01 * dx3 + m11 * dx4;
+    v2 = m00 * dy1 + m10 * dy2 + m01 * dy3 + m11 * dy4;
+    v3 = m00 * dz1 + m10 * dz2 + m01 * dz3 + m11 * dz4;
+    return true;
+}
+
 NS_PROJ_END
 
 /************************************************************************/
diff --git a/src/grids.hpp b/src/grids.hpp
index ef365f06..6b6ee0d2 100644
--- a/src/grids.hpp
+++ b/src/grids.hpp
@@ -239,6 +239,15 @@ double pj_vgrid_value(PJ *P, const ListOfVGrids &, PJ_LP lp,
 PJ_LP pj_hgrid_apply(PJ_CONTEXT *ctx, const ListOfHGrids &grids, PJ_LP lp,
                      PJ_DIRECTION direction);
 
+const GenericShiftGrid *pj_find_generic_grid(const ListOfGenericGrids &grids,
+                                             const PJ_LP &input,
+                                             GenericShiftGridSet *&gridSetOut);
+bool pj_bilinear_interpolation_three_samples(const GenericShiftGrid *grid,
+                                             const PJ_LP &lp, int idx1,
+                                             int idx2, int idx3, double &v1,
+                                             double &v2, double &v3,
+                                             bool &must_retry);
+
 NS_PROJ_END
 
 #endif // GRIDS_HPP_INCLUDED
diff --git a/src/transformations/deformation.cpp b/src/transformations/deformation.cpp
index 778c1a2e..8aee50c9 100644
--- a/src/transformations/deformation.cpp
+++ b/src/transformations/deformation.cpp
@@ -80,23 +80,6 @@ struct deformationData {
 
 // ---------------------------------------------------------------------------
 
-static const GenericShiftGrid* findGrid(const ListOfGenericGrids& grids,
-                                        const PJ_LP& input,
-                                        GenericShiftGridSet *&gridSetOut)
-{
-    for( const auto& gridset: grids )
-    {
-        auto grid = gridset->gridAt(input.lam, input.phi);
-        if( grid ) {
-            gridSetOut = gridset.get();
-            return grid;
-        }
-    }
-    return nullptr;
-}
-
-// ---------------------------------------------------------------------------
-
 static bool get_grid_values(PJ* P,
                             deformationData* Q,
                             const PJ_LP& lp,
@@ -105,7 +88,7 @@ static bool get_grid_values(PJ* P,
                             double& vz)
 {
     GenericShiftGridSet* gridset = nullptr;
-    auto grid = findGrid(Q->grids, lp, gridset);
+    auto grid = pj_find_generic_grid(Q->grids, lp, gridset);
     if( !grid ) {
         return false;
     }
@@ -140,55 +123,20 @@ static bool get_grid_values(PJ* P,
         return false;
     }
 
-    const auto& extent = grid->extentAndRes();
-    double grid_x = (lp.lam - extent.westLon) / extent.resLon;
-    double grid_y = (lp.phi - extent.southLat) / extent.resLat;
-    int ix = static_cast<int>(grid_x);
-    int iy = static_cast<int>(grid_y);
-    int ix2 = std::min(ix + 1, grid->width() - 1);
-    int iy2 = std::min(iy + 1, grid->height() - 1);
-
-    float dx1, dy1, dz1;
-    float dx2, dy2, dz2;
-    float dx3, dy3, dz3;
-    float dx4, dy4, dz4;
-    bool error =( !grid->valueAt(ix, iy, sampleE, dx1) ||
-        !grid->valueAt(ix, iy, sampleN, dy1) ||
-        !grid->valueAt(ix, iy, sampleU, dz1) ||
-        !grid->valueAt(ix2, iy, sampleE, dx2) ||
-        !grid->valueAt(ix2, iy, sampleN, dy2) ||
-        !grid->valueAt(ix2, iy, sampleU, dz2) ||
-        !grid->valueAt(ix, iy2, sampleE, dx3) ||
-        !grid->valueAt(ix, iy2, sampleN, dy3) ||
-        !grid->valueAt(ix, iy2, sampleU, dz3) ||
-        !grid->valueAt(ix2, iy2, sampleE, dx4) ||
-        !grid->valueAt(ix2, iy2, sampleN, dy4) ||
-        !grid->valueAt(ix2, iy2, sampleU, dz4) );
-    if( grid->hasChanged() ) {
-        if( gridset->reopen(P->ctx) ) {
+    bool must_retry = false;
+    if( !pj_bilinear_interpolation_three_samples(grid, lp,
+                                                 sampleE, sampleN, sampleU,
+                                                 vx, vy, vz,
+                                                 must_retry) )
+    {
+        if( must_retry )
             return get_grid_values( P, Q, lp, vx, vy, vz);
-        }
-        error = true;
-    }
-    if( error ) {
         return false;
     }
-
-    double frct_lam = grid_x - ix;
-    double frct_phi = grid_y - iy;
-    double m10 = frct_lam;
-    double m11 = m10;
-    double m01 = 1. - frct_lam;
-    double m00 = m01;
-    m11 *= frct_phi;
-    m01 *= frct_phi;
-    frct_phi = 1. - frct_phi;
-    m00 *= frct_phi;
-    m10 *= frct_phi;
     // divide by 1000 to get m/year
-    vx = (m00 * dx1 + m10 * dx2 + m01 * dx3 + m11 * dx4) / 1000;
-    vy = (m00 * dy1 + m10 * dy2 + m01 * dy3 + m11 * dy4) / 1000;
-    vz = (m00 * dz1 + m10 * dz2 + m01 * dz3 + m11 * dz4) / 1000;
+    vx /= 1000;
+    vy /= 1000;
+    vz /= 1000;
     return true;
 }
 
diff --git a/src/transformations/xyzgridshift.cpp b/src/transformations/xyzgridshift.cpp
index 4a85fb4c..e1c76518 100644
--- a/src/transformations/xyzgridshift.cpp
+++ b/src/transformations/xyzgridshift.cpp
@@ -51,24 +51,6 @@ struct xyzgridshiftData {
 };
 } // anonymous namespace
 
-
-// ---------------------------------------------------------------------------
-
-static const GenericShiftGrid* findGrid(const ListOfGenericGrids& grids,
-                                        const PJ_LP& input,
-                                        GenericShiftGridSet *&gridSetOut)
-{
-    for( const auto& gridset: grids )
-    {
-        auto grid = gridset->gridAt(input.lam, input.phi);
-        if( grid ) {
-            gridSetOut = gridset.get();
-            return grid;
-        }
-    }
-    return nullptr;
-}
-
 // ---------------------------------------------------------------------------
 
 static bool get_grid_values(PJ* P,
@@ -87,7 +69,7 @@ static bool get_grid_values(PJ* P,
     }
 
     GenericShiftGridSet* gridset = nullptr;
-    auto grid = findGrid(Q->grids, lp, gridset);
+    auto grid = pj_find_generic_grid(Q->grids, lp, gridset);
     if( !grid ) {
         return false;
     }
@@ -122,54 +104,20 @@ static bool get_grid_values(PJ* P,
         return false;
     }
 
-    const auto& extent = grid->extentAndRes();
-    double grid_x = (lp.lam - extent.westLon) / extent.resLon;
-    double grid_y = (lp.phi - extent.southLat) / extent.resLat;
-    int ix = static_cast<int>(grid_x);
-    int iy = static_cast<int>(grid_y);
-    int ix2 = std::min(ix + 1, grid->width() - 1);
-    int iy2 = std::min(iy + 1, grid->height() - 1);
-
-    float dx1, dy1, dz1;
-    float dx2, dy2, dz2;
-    float dx3, dy3, dz3;
-    float dx4, dy4, dz4;
-    bool error =( !grid->valueAt(ix, iy, sampleX, dx1) ||
-        !grid->valueAt(ix, iy, sampleY, dy1) ||
-        !grid->valueAt(ix, iy, sampleZ, dz1) ||
-        !grid->valueAt(ix2, iy, sampleX, dx2) ||
-        !grid->valueAt(ix2, iy, sampleY, dy2) ||
-        !grid->valueAt(ix2, iy, sampleZ, dz2) ||
-        !grid->valueAt(ix, iy2, sampleX, dx3) ||
-        !grid->valueAt(ix, iy2, sampleY, dy3) ||
-        !grid->valueAt(ix, iy2, sampleZ, dz3) ||
-        !grid->valueAt(ix2, iy2, sampleX, dx4) ||
-        !grid->valueAt(ix2, iy2, sampleY, dy4) ||
-        !grid->valueAt(ix2, iy2, sampleZ, dz4) );
-    if( grid->hasChanged() ) {
-        if( gridset->reopen(P->ctx) ) {
+    bool must_retry = false;
+    if( !pj_bilinear_interpolation_three_samples(grid, lp,
+                                                 sampleX, sampleY, sampleZ,
+                                                 dx, dy, dz,
+                                                 must_retry) )
+    {
+        if( must_retry )
             return get_grid_values( P, Q, lp, dx, dy, dz);
-        }
-        error = true;
-    }
-    if( error ) {
         return false;
     }
 
-    double frct_lam = grid_x - ix;
-    double frct_phi = grid_y - iy;
-    double m10 = frct_lam;
-    double m11 = m10;
-    double m01 = 1. - frct_lam;
-    double m00 = m01;
-    m11 *= frct_phi;
-    m01 *= frct_phi;
-    frct_phi = 1. - frct_phi;
-    m00 *= frct_phi;
-    m10 *= frct_phi;
-    dx = (m00 * dx1 + m10 * dx2 + m01 * dx3 + m11 * dx4) * Q->multiplier;
-    dy = (m00 * dy1 + m10 * dy2 + m01 * dy3 + m11 * dy4) * Q->multiplier;
-    dz = (m00 * dz1 + m10 * dz2 + m01 * dz3 + m11 * dz4) * Q->multiplier;
+    dx *= Q->multiplier;
+    dy *= Q->multiplier;
+    dz *= Q->multiplier;
     return true;
 }