/****************************************************************************** * Project: PROJ * Purpose: Convert between geocentric coordinates and topocentric (ENU) *coordinates * ****************************************************************************** * Copyright (c) 2020, Even Rouault * * 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. *****************************************************************************/ #include "proj_internal.h" #include #include 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 void topocentric_fwd(PJ_COORD &coo, PJ *P) { struct pj_opaque *Q = static_cast(P->opaque); const double dX = coo.xyz.x - Q->X0; const double dY = coo.xyz.y - Q->Y0; const double dZ = coo.xyz.z - Q->Z0; coo.xyz.x = -dX * Q->sinlam0 + dY * Q->coslam0; coo.xyz.y = -dX * Q->sinphi0 * Q->coslam0 - dY * Q->sinphi0 * Q->sinlam0 + dZ * Q->cosphi0; coo.xyz.z = dX * Q->cosphi0 * Q->coslam0 + dY * Q->cosphi0 * Q->sinlam0 + dZ * Q->sinphi0; } // Convert from topocentric to geocentric static void topocentric_inv(PJ_COORD &coo, PJ *P) { struct pj_opaque *Q = static_cast(P->opaque); const double x = coo.xyz.x; const double y = coo.xyz.y; const double z = coo.xyz.z; coo.xyz.x = Q->X0 - x * Q->sinlam0 - y * Q->sinphi0 * Q->coslam0 + z * Q->cosphi0 * Q->coslam0; coo.xyz.y = Q->Y0 + x * Q->coslam0 - y * Q->sinphi0 * Q->sinlam0 + z * Q->cosphi0 * Q->sinlam0; coo.xyz.z = Q->Z0 + y * Q->cosphi0 + z * Q->sinphi0; } /*********************************************************************/ PJ *PJ_CONVERSION(topocentric, 1) { /*********************************************************************/ struct pj_opaque *Q = static_cast(calloc(1, sizeof(struct pj_opaque))); if (nullptr == Q) return pj_default_destructor(P, PROJ_ERR_OTHER /*ENOMEM*/); P->opaque = static_cast(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) { proj_log_error(P, _("missing X_0 or lon_0")); return pj_default_destructor(P, PROJ_ERR_INVALID_OP_MISSING_ARG); } if ((hasX0 || hasY0 || hasZ0) && (hasLon0 || hasLat0 || hash0)) { proj_log_error( P, _("(X_0,Y_0,Z_0) and (lon_0,lat_0,h_0) are mutually exclusive")); return pj_default_destructor( P, PROJ_ERR_INVALID_OP_MUTUALLY_EXCLUSIVE_ARGS); } if (hasX0 && (!hasY0 || !hasZ0)) { proj_log_error(P, _("missing Y_0 and/or Z_0")); return pj_default_destructor(P, PROJ_ERR_INVALID_OP_MISSING_ARG); } if (hasLon0 && !hasLat0) // allow missing h_0 { proj_log_error(P, _("missing lat_0")); return pj_default_destructor(P, PROJ_ERR_INVALID_OP_MISSING_ARG); } // 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, PROJ_ERR_OTHER /*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; }