Add +proj=topocentric geocentric->topocentric conversion (fixes #500)

1 files changed, 165 insertions, 0 deletions

diff --git a/src/conversions/topocentric.cpp b/src/conversions/topocentric.cpp
new file mode 100644
index 00000000..214f8221
--- /dev/null
+++ b/src/conversions/topocentric.cpp
@@ -0,0 +1,165 @@
+/******************************************************************************
+ * Project:  PROJ
+ * Purpose:  Convert between geocentric coordinates and topocentric (ENU) coordinates
+ *
+ ******************************************************************************
+ * Copyright (c) 2020, Even Rouault <even.rouault at spatialys.com>
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included
+ * in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+ * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *****************************************************************************/
+
+#define PJ_LIB__
+
+#include "proj_internal.h"
+#include <errno.h>
+#include <math.h>
+
+PROJ_HEAD(topocentric, "Geocentric/Topocentric conversion");
+
+// Notations and formulas taken from IOGP Publication 373-7-2 -
+// Geomatics Guidance Note number 7, part 2 - October 2020
+
+namespace { // anonymous namespace
+struct pj_opaque {
+    double X0;
+    double Y0;
+    double Z0;
+    double sinphi0;
+    double cosphi0;
+    double sinlam0;
+    double coslam0;
+};
+} // anonymous namespace
+
+// Convert from geocentric to topocentric
+static PJ_COORD topocentric_fwd(PJ_COORD in, PJ * P)
+{
+    struct pj_opaque *Q = static_cast<struct pj_opaque*>(P->opaque);
+    PJ_COORD out;
+    const double dX = in.xyz.x - Q->X0;
+    const double dY = in.xyz.y - Q->Y0;
+    const double dZ = in.xyz.z - Q->Z0;
+    out.xyz.x = -dX * Q->sinlam0              + dY * Q->coslam0;
+    out.xyz.y = -dX * Q->sinphi0 * Q->coslam0 - dY * Q->sinphi0 * Q->sinlam0 + dZ * Q->cosphi0;
+    out.xyz.z =  dX * Q->cosphi0 * Q->coslam0 + dY * Q->cosphi0 * Q->sinlam0 + dZ * Q->sinphi0;
+    return out;
+}
+
+// Convert from topocentric to geocentric
+static PJ_COORD topocentric_inv(PJ_COORD in, PJ * P)
+{
+    struct pj_opaque *Q = static_cast<struct pj_opaque*>(P->opaque);
+    PJ_COORD out;
+    out.xyz.x = Q->X0 - in.xyz.x * Q->sinlam0 - in.xyz.y * Q->sinphi0 * Q->coslam0 + in.xyz.z * Q->cosphi0 * Q->coslam0;
+    out.xyz.y = Q->Y0 + in.xyz.x * Q->coslam0 - in.xyz.y * Q->sinphi0 * Q->sinlam0 + in.xyz.z * Q->cosphi0 * Q->sinlam0;
+    out.xyz.z = Q->Z0                         + in.xyz.y * Q->cosphi0              + in.xyz.z * Q->sinphi0;
+    return out;
+}
+
+
+/*********************************************************************/
+PJ *CONVERSION(topocentric,1) {
+/*********************************************************************/
+    struct pj_opaque *Q = static_cast<struct pj_opaque*>(pj_calloc (1, sizeof (struct pj_opaque)));
+    if (nullptr==Q)
+        return pj_default_destructor (P, ENOMEM);
+    P->opaque = static_cast<void *>(Q);
+
+    // The topocentric origin can be specified either in geocentric coordinates
+    // (X_0,Y_0,Z_0) or as geographic coordinates (lon_0,lat_0,h_0)
+    // Checks:
+    // - X_0 or lon_0 must be specified
+    // - If X_0 is specified, the Y_0 and Z_0 must also be
+    // - If lon_0 is specified, then lat_0 must also be
+    // - If any of X_0, Y_0, Z_0 is specified, then any of lon_0,lat_0,h_0 must
+    //   not be, and vice versa.
+    const auto hasX0 = pj_param_exists(P->params, "X_0");
+    const auto hasY0 = pj_param_exists(P->params, "Y_0");
+    const auto hasZ0 = pj_param_exists(P->params, "Z_0");
+    const auto hasLon0 = pj_param_exists(P->params, "lon_0");
+    const auto hasLat0 = pj_param_exists(P->params, "lat_0");
+    const auto hash0 = pj_param_exists(P->params, "h_0");
+    if( !hasX0 && !hasLon0 )
+    {
+        return pj_default_destructor(P, PJD_ERR_MISSING_ARGS);
+    }
+    if ( (hasX0 || hasY0 || hasZ0) &&
+         (hasLon0 || hasLat0 || hash0) )
+    {
+        return pj_default_destructor(P, PJD_ERR_MUTUALLY_EXCLUSIVE_ARGS);
+    }
+    if( hasX0 && (!hasY0 || !hasZ0) )
+    {
+        return pj_default_destructor(P, PJD_ERR_MISSING_ARGS);
+    }
+    if( hasLon0 && !hasLat0 ) // allow missing h_0
+    {
+        return pj_default_destructor(P, PJD_ERR_MISSING_ARGS);
+    }
+
+    // Pass a dummy ellipsoid definition that will be overridden just afterwards
+    PJ* cart = proj_create(P->ctx, "+proj=cart +a=1");
+    if (cart == nullptr)
+        return pj_default_destructor(P, ENOMEM);
+    /* inherit ellipsoid definition from P to cart */
+    pj_inherit_ellipsoid_def (P, cart);
+
+    if( hasX0 )
+    {
+        Q->X0 = pj_param(P->ctx, P->params, "dX_0").f;
+        Q->Y0 = pj_param(P->ctx, P->params, "dY_0").f;
+        Q->Z0 = pj_param(P->ctx, P->params, "dZ_0").f;
+
+        // Compute lam0, phi0 from X0,Y0,Z0
+        PJ_XYZ xyz;
+        xyz.x = Q->X0;
+        xyz.y = Q->Y0;
+        xyz.z = Q->Z0;
+        const auto lpz = pj_inv3d(xyz, cart);
+        Q->sinphi0 = sin(lpz.phi);
+        Q->cosphi0 = cos(lpz.phi);
+        Q->sinlam0 = sin(lpz.lam);
+        Q->coslam0 = cos(lpz.lam);
+    }
+    else
+    {
+        // Compute X0,Y0,Z0 from lam0, phi0, h0
+        PJ_LPZ lpz;
+        lpz.lam = P->lam0;
+        lpz.phi = P->phi0;
+        lpz.z = pj_param(P->ctx, P->params, "dh_0").f;
+        const auto xyz = pj_fwd3d(lpz, cart);
+        Q->X0 = xyz.x;
+        Q->Y0 = xyz.y;
+        Q->Z0 = xyz.z;
+
+        Q->sinphi0 = sin(P->phi0);
+        Q->cosphi0 = cos(P->phi0);
+        Q->sinlam0 = sin(P->lam0);
+        Q->coslam0 = cos(P->lam0);
+    }
+
+    proj_destroy(cart);
+
+    P->fwd4d  =  topocentric_fwd;
+    P->inv4d  =  topocentric_inv;
+    P->left   =  PJ_IO_UNITS_CARTESIAN;
+    P->right  =  PJ_IO_UNITS_CARTESIAN;
+    return P;
+}