1 files changed, 147 insertions, 0 deletions
diff --git a/src/PJ_vgridshift.c b/src/PJ_vgridshift.c
new file mode 100644
index 00000000..c91db968
--- /dev/null
+++ b/src/PJ_vgridshift.c
@@ -0,0 +1,147 @@
+#define PJ_LIB__
+#include <proj.h>
+#include <projects.h>
+
+PROJ_HEAD(vgridshift, "Vertical grid shift");
+
+static void *freeup_msg (PJ *P, int errlev) {
+    if (0==P)
+        return 0;
+
+    if (0!=P->ctx)
+        pj_ctx_set_errno (P->ctx, errlev);
+
+    return pj_dealloc(P);
+}
+
+
+static void freeup (PJ *P) {
+    freeup_msg (P, 0);
+    return;
+}
+
+
+static XYZ forward_3d(LPZ lpz, PJ *P) {
+    PJ_TRIPLET point;
+    point.lpz = lpz;
+
+    if (P->gridlist != NULL) {
+        /* Only try the gridshift if at least one grid is loaded,
+         * otherwise just pass the coordinate through unchanged. */
+        pj_apply_vgridshift( P, "sgrids",
+                             &(P->vgridlist_geoid),
+                             &(P->vgridlist_geoid_count),
+                             1, 1, 0,
+                             &point.xyz.x, &point.xyz.y, &point.xyz.z );
+    }
+
+    return point.xyz;
+}
+
+
+static LPZ reverse_3d(XYZ xyz, PJ *P) {
+    PJ_TRIPLET point;
+    point.xyz = xyz;
+
+    if (P->gridlist != NULL) {
+        /* Only try the gridshift if at least one grid is loaded,
+         * otherwise just pass the coordinate through unchanged. */
+        pj_apply_vgridshift( P, "sgrids",
+                             &(P->vgridlist_geoid),
+                             &(P->vgridlist_geoid_count),
+                             0, 1, 0,
+                             &point.xyz.x, &point.xyz.y, &point.xyz.z );
+    }
+
+    return point.lpz;
+}
+
+
+static PJ_OBS forward_obs(PJ_OBS obs, PJ *P) {
+    PJ_OBS point;
+    point.coo.xyz = forward_3d (obs.coo.lpz, P);
+    return point;
+}
+
+static PJ_OBS reverse_obs(PJ_OBS obs, PJ *P) {
+    PJ_OBS point;
+    point.coo.lpz = reverse_3d (obs.coo.xyz, P);
+    return point;
+}
+
+
+PJ *PROJECTION(vgridshift) {
+
+   if (!pj_param(P->ctx, P->params, "tgrids").i) {
+        pj_log_error(P, "vgridshift: +grids parameter missing.");
+        return freeup_msg(P, -1);
+    }
+
+    /* Build gridlist. P->gridlist can be empty if +grids only ask for optional grids. */
+    P->gridlist = pj_gridlist_from_nadgrids( P->ctx, pj_param(P->ctx, P->params, "sgrids").s,
+                                             &(P->gridlist_count) );
+
+    /* Was gridlist compiled properly? */
+    if ( pj_ctx_get_errno(P->ctx) ) {
+        pj_log_error(P, "vgridshift: could not find required grid(s).");
+        return freeup_msg(P, -38);
+    }
+
+    P->fwdobs = forward_obs;
+    P->invobs = reverse_obs;
+    P->fwd3d  = forward_3d;
+    P->inv3d  = reverse_3d;
+    P->fwd    = 0;
+    P->inv    = 0;
+
+    P->left  = PJ_IO_UNITS_RADIANS;
+    P->right = PJ_IO_UNITS_RADIANS;
+
+    return P;
+}
+
+
+#ifndef PJ_SELFTEST
+/* selftest stub */
+int pj_vgridshift_selftest (void) {return 0;}
+#else
+int pj_vgridshift_selftest (void) {
+    PJ *P;
+    PJ_OBS expect, a, b;
+    double dist;
+
+    /* fail on purpose: +grids parameter it mandatory*/
+    P = pj_create("+proj=vgridshift");
+    if (0!=P)
+        return 99;
+
+    /* fail on purpose: open non-existing grid */
+    P = pj_create("+proj=vgridshift +grids=nonexistinggrid.gtx");
+    if (0!=P)
+        return 999;
+
+    /* Failure most likely means the grid is missing */
+    P = pj_create ("+proj=vgridshift +grids=egm96_15.gtx +ellps=GRS80");
+    if (0==P)
+        return 10;
+
+    a = pj_obs_null;
+    a.coo.lpz.lam = TORAD(12.5);
+    a.coo.lpz.phi = TORAD(55.5);
+
+    dist = pj_roundtrip (P, PJ_FWD, 1, a);
+    if (dist > 0.00000001)
+        return 1;
+
+    expect = a;
+    expect.coo.lpz.z   = -36.021305084228515625;
+    b = pj_trans(P, PJ_FWD, a);
+    if (pj_xyz_dist(expect.coo.xyz, b.coo.xyz) > 1e-10)
+        return 2;
+
+
+    pj_free(P);
+
+    return 0;
+}
+#endif