Merge pull request #2444 from rouault/topocentric

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

6 files changed, 235 insertions, 8 deletions

diff --git a/src/Makefile.am b/src/Makefile.am
index 1d772207..83ee4adc 100644
--- a/src/Makefile.am
+++ b/src/Makefile.am
@@ -185,6 +185,7 @@ libproj_la_SOURCES = \
 	conversions/geoc.cpp \
 	conversions/geocent.cpp \
 	conversions/noop.cpp \
+	conversions/topocentric.cpp \
 	conversions/set.cpp \
 	conversions/unitconvert.cpp \
 	\
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;
+}
diff --git a/src/iso19111/coordinateoperation.cpp b/src/iso19111/coordinateoperation.cpp
index 5ef32d31..83b626b3 100644
--- a/src/iso19111/coordinateoperation.cpp
+++ b/src/iso19111/coordinateoperation.cpp
@@ -6034,28 +6034,39 @@ void Conversion::_exportToPROJString(
         !isHeightDepthReversal;
     bool applyTargetCRSModifiers = applySourceCRSModifiers;
 
+    if (formatter->getCRSExport()) {
+        if (methodEPSGCode == EPSG_CODE_METHOD_GEOCENTRIC_TOPOCENTRIC ||
+            methodEPSGCode == EPSG_CODE_METHOD_GEOGRAPHIC_TOPOCENTRIC) {
+            throw io::FormattingException("Transformation cannot be exported "
+                                          "as a PROJ.4 string (but can be part "
+                                          "of a PROJ pipeline)");
+        }
+    }
+
     auto l_sourceCRS = sourceCRS();
+    crs::GeographicCRSPtr srcGeogCRS;
     if (!formatter->getCRSExport() && l_sourceCRS && applySourceCRSModifiers) {
 
-        crs::CRS *horiz = l_sourceCRS.get();
-        const auto compound = dynamic_cast<const crs::CompoundCRS *>(horiz);
+        crs::CRSPtr horiz = l_sourceCRS;
+        const auto compound =
+            dynamic_cast<const crs::CompoundCRS *>(l_sourceCRS.get());
         if (compound) {
             const auto &components = compound->componentReferenceSystems();
             if (!components.empty()) {
-                horiz = components.front().get();
+                horiz = components.front().as_nullable();
             }
         }
 
-        auto geogCRS = dynamic_cast<const crs::GeographicCRS *>(horiz);
-        if (geogCRS) {
+        srcGeogCRS = std::dynamic_pointer_cast<crs::GeographicCRS>(horiz);
+        if (srcGeogCRS) {
             formatter->setOmitProjLongLatIfPossible(true);
             formatter->startInversion();
-            geogCRS->_exportToPROJString(formatter);
+            srcGeogCRS->_exportToPROJString(formatter);
             formatter->stopInversion();
             formatter->setOmitProjLongLatIfPossible(false);
         }
 
-        auto projCRS = dynamic_cast<const crs::ProjectedCRS *>(horiz);
+        auto projCRS = dynamic_cast<const crs::ProjectedCRS *>(horiz.get());
         if (projCRS) {
             formatter->startInversion();
             formatter->pushOmitZUnitConversion();
@@ -6325,6 +6336,30 @@ void Conversion::_exportToPROJString(
         }
         bConversionDone = true;
         bEllipsoidParametersDone = true;
+    } else if (methodEPSGCode == EPSG_CODE_METHOD_GEOGRAPHIC_TOPOCENTRIC) {
+        if (!srcGeogCRS) {
+            throw io::FormattingException(
+                "Export of Geographic/Topocentric conversion to a PROJ string "
+                "requires an input geographic CRS");
+        }
+
+        formatter->addStep("cart");
+        srcGeogCRS->ellipsoid()->_exportToPROJString(formatter);
+
+        formatter->addStep("topocentric");
+        const auto latOrigin = parameterValueNumeric(
+            EPSG_CODE_PARAMETER_LATITUDE_TOPOGRAPHIC_ORIGIN,
+            common::UnitOfMeasure::DEGREE);
+        const auto lonOrigin = parameterValueNumeric(
+            EPSG_CODE_PARAMETER_LONGITUDE_TOPOGRAPHIC_ORIGIN,
+            common::UnitOfMeasure::DEGREE);
+        const auto heightOrigin = parameterValueNumeric(
+            EPSG_CODE_PARAMETER_ELLIPSOIDAL_HEIGHT_TOPOCENTRIC_ORIGIN,
+            common::UnitOfMeasure::METRE);
+        formatter->addParam("lat_0", latOrigin);
+        formatter->addParam("lon_0", lonOrigin);
+        formatter->addParam("h_0", heightOrigin);
+        bConversionDone = true;
     }
 
     auto l_targetCRS = targetCRS();
@@ -6449,7 +6484,9 @@ void Conversion::_exportToPROJString(
         }
 
         if (!bEllipsoidParametersDone) {
-            auto targetGeogCRS = horiz->extractGeographicCRS();
+            auto targetGeodCRS = horiz->extractGeodeticCRS();
+            auto targetGeogCRS =
+                std::dynamic_pointer_cast<crs::GeographicCRS>(targetGeodCRS);
             if (targetGeogCRS) {
                 if (formatter->getCRSExport()) {
                     targetGeogCRS->addDatumInfoToPROJString(formatter);
@@ -6458,6 +6495,8 @@ void Conversion::_exportToPROJString(
                     targetGeogCRS->primeMeridian()->_exportToPROJString(
                         formatter);
                 }
+            } else if (targetGeodCRS) {
+                targetGeodCRS->ellipsoid()->_exportToPROJString(formatter);
             }
         }
 
diff --git a/src/lib_proj.cmake b/src/lib_proj.cmake
index 7ca7f3a4..3a4880c6 100644
--- a/src/lib_proj.cmake
+++ b/src/lib_proj.cmake
@@ -173,6 +173,7 @@ set(SRC_LIBPROJ_CONVERSIONS
   conversions/geoc.cpp
   conversions/geocent.cpp
   conversions/noop.cpp
+  conversions/topocentric.cpp
   conversions/set.cpp
   conversions/unitconvert.cpp
 )
diff --git a/src/pj_list.h b/src/pj_list.h
index bcdc189e..d00e780f 100644
--- a/src/pj_list.h
+++ b/src/pj_list.h
@@ -155,6 +155,7 @@ PROJ_HEAD(tinshift, "Triangulation based transformation")
 PROJ_HEAD(tissot, "Tissot Conic")
 PROJ_HEAD(tmerc, "Transverse Mercator")
 PROJ_HEAD(tobmerc, "Tobler-Mercator")
+PROJ_HEAD(topocentric, "Geocentric/Topocentric conversion")
 PROJ_HEAD(tpeqd, "Two Point Equidistant")
 PROJ_HEAD(tpers, "Tilted perspective")
 PROJ_HEAD(unitconvert, "Unit conversion")
diff --git a/src/proj_constants.h b/src/proj_constants.h
index a3da2c10..ce3b2157 100644
--- a/src/proj_constants.h
+++ b/src/proj_constants.h
@@ -651,4 +651,24 @@
 #define EPSG_CODE_METHOD_HEIGHT_DEPTH_REVERSAL 1068
 #define EPSG_NAME_METHOD_HEIGHT_DEPTH_REVERSAL "Height Depth Reversal"
 
+/* ------------------------------------------------------------------------ */
+
+#define EPSG_NAME_METHOD_GEOCENTRIC_TOPOCENTRIC "Geocentric/topocentric conversions"
+#define EPSG_CODE_METHOD_GEOCENTRIC_TOPOCENTRIC 9836
+
+#define EPSG_NAME_PARAMETER_GEOCENTRIC_X_TOPOCENTRIC_ORIGIN "Geocentric X of topocentric origin"
+#define EPSG_CODE_PARAMETER_GEOCENTRIC_X_TOPOCENTRIC_ORIGIN 8837
+
+#define EPSG_NAME_PARAMETER_GEOCENTRIC_Y_TOPOCENTRIC_ORIGIN "Geocentric Y of topocentric origin"
+#define EPSG_CODE_PARAMETER_GEOCENTRIC_Y_TOPOCENTRIC_ORIGIN 8838
+
+#define EPSG_NAME_PARAMETER_GEOCENTRIC_Z_TOPOCENTRIC_ORIGIN "Geocentric Z of topocentric origin"
+#define EPSG_CODE_PARAMETER_GEOCENTRIC_Z_TOPOCENTRIC_ORIGIN 8839
+
+/* ------------------------------------------------------------------------ */
+
+#define EPSG_NAME_METHOD_GEOGRAPHIC_TOPOCENTRIC "Geographic/topocentric conversions"
+#define EPSG_CODE_METHOD_GEOGRAPHIC_TOPOCENTRIC 9837
+
+
 #endif /* PROJ_CONSTANTS_INCLUDED */