diff options
| author | Even Rouault <even.rouault@spatialys.com> | 2018-12-19 12:25:33 +0100 |
|---|---|---|
| committer | Even Rouault <even.rouault@spatialys.com> | 2018-12-26 10:08:54 +0100 |
| commit | e6de172371ea203f6393d745641d66c82b5b13e2 (patch) | |
| tree | 791fa07f431a2d1db6f6e813ab984db982587278 /src/transformations | |
| parent | ce8075076b4e4ffebd32afaba419e1d9ab27cd03 (diff) | |
| download | PROJ-e6de172371ea203f6393d745641d66c82b5b13e2.tar.gz PROJ-e6de172371ea203f6393d745641d66c82b5b13e2.zip | |
cpp conversion: move source files in apps/ iso19111/ conversions/ projections/ transformations/ tests/ subdirectories
Diffstat (limited to 'src/transformations')
| -rw-r--r-- | src/transformations/PJ_affine.cpp | 250 | ||||
| -rw-r--r-- | src/transformations/PJ_deformation.cpp | 325 | ||||
| -rw-r--r-- | src/transformations/PJ_helmert.cpp | 755 | ||||
| -rw-r--r-- | src/transformations/PJ_hgridshift.cpp | 133 | ||||
| -rw-r--r-- | src/transformations/PJ_horner.cpp | 513 | ||||
| -rw-r--r-- | src/transformations/PJ_molodensky.cpp | 329 | ||||
| -rw-r--r-- | src/transformations/PJ_vgridshift.cpp | 144 |
7 files changed, 2449 insertions, 0 deletions
diff --git a/src/transformations/PJ_affine.cpp b/src/transformations/PJ_affine.cpp new file mode 100644 index 00000000..e2b668d3 --- /dev/null +++ b/src/transformations/PJ_affine.cpp @@ -0,0 +1,250 @@ +/************************************************************************ +* Copyright (c) 2018, 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 <errno.h> +#include <math.h> + +#include "proj_internal.h" +#include "proj.h" +#include "projects.h" + +PROJ_HEAD(affine, "Affine transformation"); +PROJ_HEAD(geogoffset, "Geographic Offset"); + +namespace { // anonymous namespace +struct pj_affine_coeffs { + double s11; + double s12; + double s13; + double s21; + double s22; + double s23; + double s31; + double s32; + double s33; + double tscale; +}; +} // anonymous namespace + +namespace { // anonymous namespace +struct pj_opaque_affine { + double xoff; + double yoff; + double zoff; + double toff; + struct pj_affine_coeffs forward; + struct pj_affine_coeffs reverse; +}; +} // anonymous namespace + + +static PJ_COORD forward_4d(PJ_COORD obs, PJ *P) { + PJ_COORD newObs; + const struct pj_opaque_affine *Q = (const struct pj_opaque_affine *) P->opaque; + const struct pj_affine_coeffs *C = &(Q->forward); + newObs.xyzt.x = Q->xoff + C->s11 * obs.xyzt.x + C->s12 * obs.xyzt.y + C->s13 * obs.xyzt.z; + newObs.xyzt.y = Q->yoff + C->s21 * obs.xyzt.x + C->s22 * obs.xyzt.y + C->s23 * obs.xyzt.z; + newObs.xyzt.z = Q->zoff + C->s31 * obs.xyzt.x + C->s32 * obs.xyzt.y + C->s33 * obs.xyzt.z; + newObs.xyzt.t = Q->toff + C->tscale * obs.xyzt.t; + return newObs; +} + +static XYZ forward_3d(LPZ lpz, PJ *P) { + PJ_COORD point = {{0,0,0,0}}; + point.lpz = lpz; + return forward_4d(point, P).xyz; +} + + +static XY forward_2d(LP lp, PJ *P) { + PJ_COORD point = {{0,0,0,0}}; + point.lp = lp; + return forward_4d(point, P).xy; +} + + +static PJ_COORD reverse_4d(PJ_COORD obs, PJ *P) { + PJ_COORD newObs; + const struct pj_opaque_affine *Q = (const struct pj_opaque_affine *) P->opaque; + const struct pj_affine_coeffs *C = &(Q->reverse); + obs.xyzt.x -= Q->xoff; + obs.xyzt.y -= Q->yoff; + obs.xyzt.z -= Q->zoff; + newObs.xyzt.x = C->s11 * obs.xyzt.x + C->s12 * obs.xyzt.y + C->s13 * obs.xyzt.z; + newObs.xyzt.y = C->s21 * obs.xyzt.x + C->s22 * obs.xyzt.y + C->s23 * obs.xyzt.z; + newObs.xyzt.z = C->s31 * obs.xyzt.x + C->s32 * obs.xyzt.y + C->s33 * obs.xyzt.z; + newObs.xyzt.t = C->tscale * (obs.xyzt.t - Q->toff); + return newObs; +} + +static LPZ reverse_3d(XYZ xyz, PJ *P) { + PJ_COORD point = {{0,0,0,0}}; + point.xyz = xyz; + return reverse_4d(point, P).lpz; +} + +static LP reverse_2d(XY xy, PJ *P) { + PJ_COORD point = {{0,0,0,0}}; + point.xy = xy; + return reverse_4d(point, P).lp; +} + +static struct pj_opaque_affine * initQ() { + struct pj_opaque_affine *Q = static_cast<struct pj_opaque_affine *>(pj_calloc(1, sizeof(struct pj_opaque_affine))); + if (nullptr==Q) + return nullptr; + + /* default values */ + Q->forward.s11 = 1.0; + Q->forward.s22 = 1.0; + Q->forward.s33 = 1.0; + Q->forward.tscale = 1.0; + + Q->reverse.s11 = 1.0; + Q->reverse.s22 = 1.0; + Q->reverse.s33 = 1.0; + Q->reverse.tscale = 1.0; + + return Q; +} + +static void computeReverseParameters(PJ* P) +{ + struct pj_opaque_affine *Q = (struct pj_opaque_affine *) P->opaque; + + /* cf https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3_%C3%97_3_matrices */ + const double a = Q->forward.s11; + const double b = Q->forward.s12; + const double c = Q->forward.s13; + const double d = Q->forward.s21; + const double e = Q->forward.s22; + const double f = Q->forward.s23; + const double g = Q->forward.s31; + const double h = Q->forward.s32; + const double i = Q->forward.s33; + const double A = e * i - f * h; + const double B = -(d * i - f * g); + const double C = (d * h - e * g); + const double D = -(b * i - c * h); + const double E = (a * i - c * g); + const double F = -(a * h - b * g); + const double G = b * f - c * e; + const double H = -(a * f - c * d); + const double I = a * e - b * d; + const double det = a * A + b * B + c * C; + if( det == 0.0 || Q->forward.tscale == 0.0 ) { + if (proj_log_level(P->ctx, PJ_LOG_TELL) >= PJ_LOG_DEBUG) { + proj_log_debug(P, "Affine: matrix non invertible"); + } + P->inv4d = nullptr; + P->inv3d = nullptr; + P->inv = nullptr; + } else { + Q->reverse.s11 = A / det; + Q->reverse.s12 = D / det; + Q->reverse.s13 = G / det; + Q->reverse.s21 = B / det; + Q->reverse.s22 = E / det; + Q->reverse.s23 = H / det; + Q->reverse.s31 = C / det; + Q->reverse.s32 = F / det; + Q->reverse.s33 = I / det; + Q->reverse.tscale = 1.0 / Q->forward.tscale; + } +} + +PJ *TRANSFORMATION(affine,0 /* no need for ellipsoid */) { + struct pj_opaque_affine *Q = initQ(); + if (nullptr==Q) + return pj_default_destructor(P, ENOMEM); + P->opaque = (void *) Q; + + P->fwd4d = forward_4d; + P->inv4d = reverse_4d; + P->fwd3d = forward_3d; + P->inv3d = reverse_3d; + P->fwd = forward_2d; + P->inv = reverse_2d; + + P->left = PJ_IO_UNITS_WHATEVER; + P->right = PJ_IO_UNITS_WHATEVER; + + /* read args */ + Q->xoff = pj_param(P->ctx, P->params, "dxoff").f; + Q->yoff = pj_param(P->ctx, P->params, "dyoff").f; + Q->zoff = pj_param(P->ctx, P->params, "dzoff").f; + Q->toff = pj_param(P->ctx, P->params, "dtoff").f; + + if(pj_param (P->ctx, P->params, "ts11").i) { + Q->forward.s11 = pj_param(P->ctx, P->params, "ds11").f; + } + Q->forward.s12 = pj_param(P->ctx, P->params, "ds12").f; + Q->forward.s13 = pj_param(P->ctx, P->params, "ds13").f; + Q->forward.s21 = pj_param(P->ctx, P->params, "ds21").f; + if(pj_param (P->ctx, P->params, "ts22").i) { + Q->forward.s22 = pj_param(P->ctx, P->params, "ds22").f; + } + Q->forward.s23 = pj_param(P->ctx, P->params, "ds23").f; + Q->forward.s31 = pj_param(P->ctx, P->params, "ds31").f; + Q->forward.s32 = pj_param(P->ctx, P->params, "ds32").f; + if(pj_param (P->ctx, P->params, "ts33").i) { + Q->forward.s33 = pj_param(P->ctx, P->params, "ds33").f; + } + if(pj_param (P->ctx, P->params, "ttscale").i) { + Q->forward.tscale = pj_param(P->ctx, P->params, "dtscale").f; + } + + computeReverseParameters(P); + + return P; +} + + +/* Arcsecond to radians */ +#define ARCSEC_TO_RAD (DEG_TO_RAD / 3600.0) + + +PJ *TRANSFORMATION(geogoffset,0 /* no need for ellipsoid */) { + struct pj_opaque_affine *Q = initQ(); + if (nullptr==Q) + return pj_default_destructor(P, ENOMEM); + P->opaque = (void *) Q; + + P->fwd4d = forward_4d; + P->inv4d = reverse_4d; + P->fwd3d = forward_3d; + P->inv3d = reverse_3d; + P->fwd = forward_2d; + P->inv = reverse_2d; + + P->left = PJ_IO_UNITS_ANGULAR; + P->right = PJ_IO_UNITS_ANGULAR; + + /* read args */ + Q->xoff = pj_param(P->ctx, P->params, "ddlon").f * ARCSEC_TO_RAD; + Q->yoff = pj_param(P->ctx, P->params, "ddlat").f * ARCSEC_TO_RAD; + Q->zoff = pj_param(P->ctx, P->params, "ddh").f; + + return P; +} diff --git a/src/transformations/PJ_deformation.cpp b/src/transformations/PJ_deformation.cpp new file mode 100644 index 00000000..6c30f21c --- /dev/null +++ b/src/transformations/PJ_deformation.cpp @@ -0,0 +1,325 @@ +/*********************************************************************** + + Kinematic datum shifting utilizing a deformation model + + Kristian Evers, 2017-10-29 + +************************************************************************ + +Perform datum shifts by means of a deformation/velocity model. + + X_out = X_in + (T_ct - T_obs)*DX + Y_out = Y_in + (T_ct - T_obs)*DY + Z_out = Z_in + (T_ct - T_obs)*DZ + + +The deformation operation takes cartesian coordinates as input and +returns cartesian coordinates as well. + +Corrections in the gridded model are in east, north, up (ENU) space. +Hence the input coordinates needs to be converted to ENU-space when +searching for corrections in the grid. The corrections are then converted +to cartesian XYZ-space and applied to the input coordinates (also in +cartesian space). + +A full deformation model is described by two grids, one for the horizontal +components and one for the vertical component. The horizontal grid is +stored in CTable/CTable2 and the vertical grid is stored in the GTX +format. The NTv2 format should not be used for this purpose since grid- +values are scaled upon reading. Both grids are expected to contain +grid-values in units of mm/year in ENU-space. + +************************************************************************ +* Copyright (c) 2017, Kristian Evers +* +* 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 <errno.h> +#include "proj.h" +#include "proj_internal.h" +#include "proj_math.h" +#include "projects.h" + +PROJ_HEAD(deformation, "Kinematic grid shift"); + +#define TOL 1e-8 +#define MAX_ITERATIONS 10 + +namespace { // anonymous namespace +struct pj_opaque { + double t_obs; + double t_epoch; + PJ *cart; +}; +} // anonymous namespace + +/********************************************************************************/ +static XYZ get_grid_shift(PJ* P, XYZ cartesian) { +/******************************************************************************** + Read correction values from grid. The cartesian input coordinates are + converted to geodetic coordinates in order look up the correction values + in the grid. Once the grid corrections are read we need to convert them + from ENU-space to cartesian XYZ-space. ENU -> XYZ formula described in: + + Nørbech, T., et al, 2003(?), "Transformation from a Common Nordic Reference + Frame to ETRS89 in Denmark, Finland, Norway, and Sweden – status report" + +********************************************************************************/ + PJ_COORD geodetic, shift, temp; + double sp, cp, sl, cl; + int previous_errno = proj_errno_reset(P); + + /* cartesian to geodetic */ + geodetic.lpz = pj_inv3d(cartesian, static_cast<struct pj_opaque*>(P->opaque)->cart); + + /* look up correction values in grids */ + shift.lp = proj_hgrid_value(P, geodetic.lp); + shift.enu.u = proj_vgrid_value(P, geodetic.lp); + + if (proj_errno(P) == PJD_ERR_GRID_AREA) + proj_log_debug(P, "deformation: coordinate (%.3f, %.3f) outside deformation model", + proj_todeg(geodetic.lp.lam), proj_todeg(geodetic.lp.phi)); + + /* grid values are stored as mm/yr, we need m/yr */ + shift.xyz.x /= 1000; + shift.xyz.y /= 1000; + shift.xyz.z /= 1000; + + /* pre-calc cosines and sines */ + sp = sin(geodetic.lp.phi); + cp = cos(geodetic.lp.phi); + sl = sin(geodetic.lp.lam); + cl = cos(geodetic.lp.lam); + + /* ENU -> XYZ */ + temp.xyz.x = -sp*cl*shift.enu.n - sl*shift.enu.e + cp*cl*shift.enu.u; + temp.xyz.y = -sp*sl*shift.enu.n + cl*shift.enu.e + cp*sl*shift.enu.u; + temp.xyz.z = cp*shift.enu.n + sp*shift.enu.u; + + shift.xyz = temp.xyz; + + proj_errno_restore(P, previous_errno); + + return shift.xyz; +} + +/********************************************************************************/ +static XYZ reverse_shift(PJ *P, XYZ input, double dt) { +/******************************************************************************** + Iteratively determine the reverse grid shift correction values. +*********************************************************************************/ + XYZ out, delta, dif; + double z0; + int i = MAX_ITERATIONS; + + delta = get_grid_shift(P, input); + + /* Store the origial z shift for later application */ + z0 = delta.z; + + /* When iterating to find the best horizontal coordinate we also carry */ + /* along the z-component, since we need it for the cartesian -> geodetic */ + /* conversion. The z-component adjustment is overwritten with z0 after */ + /* the loop has finished. */ + out.x = input.x - dt*delta.x; + out.y = input.y - dt*delta.y; + out.z = input.z + dt*delta.z; + + do { + delta = get_grid_shift(P, out); + + if (delta.x == HUGE_VAL) + break; + + dif.x = out.x + dt*delta.x - input.x; + dif.y = out.y + dt*delta.y - input.y; + dif.z = out.z - dt*delta.z - input.z; + out.x += dif.x; + out.y += dif.y; + out.z += dif.z; + + } while ( --i && hypot(dif.x, dif.y) > TOL ); + + out.z = input.z - dt*z0; + + return out; +} + +static XYZ forward_3d(LPZ lpz, PJ *P) { + struct pj_opaque *Q = (struct pj_opaque *) P->opaque; + PJ_COORD out, in; + XYZ shift; + double dt = 0.0; + in.lpz = lpz; + out = in; + + if (Q->t_obs != HUGE_VAL) { + dt = Q->t_epoch - Q->t_obs; + } else { + out = proj_coord_error(); /* in the 3D case +t_obs must be specified */ + proj_log_debug(P, "deformation: +t_obs must be specified"); + return out.xyz; + } + + shift = get_grid_shift(P, in.xyz); + + out.xyz.x += dt * shift.x; + out.xyz.y += dt * shift.y; + out.xyz.z += dt * shift.z; + + return out.xyz; +} + + +static PJ_COORD forward_4d(PJ_COORD in, PJ *P) { + struct pj_opaque *Q = (struct pj_opaque *) P->opaque; + double dt; + XYZ shift; + PJ_COORD out = in; + + if (Q->t_obs != HUGE_VAL) { + dt = Q->t_epoch - Q->t_obs; + } else { + dt = Q->t_epoch - in.xyzt.t; + } + + shift = get_grid_shift(P, in.xyz); + + out.xyzt.x += dt*shift.x; + out.xyzt.y += dt*shift.y; + out.xyzt.z += dt*shift.z; + + + return out; +} + + +static LPZ reverse_3d(XYZ in, PJ *P) { + struct pj_opaque *Q = (struct pj_opaque *) P->opaque; + PJ_COORD out; + double dt = 0.0; + out.xyz = in; + + if (Q->t_obs != HUGE_VAL) { + dt = Q->t_epoch - Q->t_obs; + } else { + out = proj_coord_error(); /* in the 3D case +t_obs must be specified */ + proj_log_debug(P, "deformation: +t_obs must be specified"); + return out.lpz; + } + + out.xyz = reverse_shift(P, in, dt); + + return out.lpz; +} + +static PJ_COORD reverse_4d(PJ_COORD in, PJ *P) { + struct pj_opaque *Q = (struct pj_opaque *) P->opaque; + PJ_COORD out = in; + double dt; + + + if (Q->t_obs != HUGE_VAL) { + dt = Q->t_epoch - Q->t_obs; + } else { + dt = Q->t_epoch - in.xyzt.t; + } + + out.xyz = reverse_shift(P, in.xyz, dt); + return out; +} + +static PJ *destructor(PJ *P, int errlev) { + if (nullptr==P) + return nullptr; + + if (nullptr==P->opaque) + return pj_default_destructor (P, errlev); + + if (static_cast<struct pj_opaque*>(P->opaque)->cart) + static_cast<struct pj_opaque*>(P->opaque)->cart->destructor (static_cast<struct pj_opaque*>(P->opaque)->cart, errlev); + + return pj_default_destructor(P, errlev); +} + + +PJ *TRANSFORMATION(deformation,1) { + int has_xy_grids = 0; + int has_z_grids = 0; + struct pj_opaque *Q = static_cast<struct pj_opaque*>(pj_calloc (1, sizeof (struct pj_opaque))); + if (nullptr==Q) + return destructor(P, ENOMEM); + P->opaque = (void *) Q; + + Q->cart = proj_create(P->ctx, "+proj=cart"); + if (Q->cart == nullptr) + return destructor(P, ENOMEM); + + /* inherit ellipsoid definition from P to Q->cart */ + pj_inherit_ellipsoid_def (P, Q->cart); + + has_xy_grids = pj_param(P->ctx, P->params, "txy_grids").i; + has_z_grids = pj_param(P->ctx, P->params, "tz_grids").i; + + /* Build gridlists. Both horizontal and vertical grids are mandatory. */ + if (!has_xy_grids || !has_z_grids) { + proj_log_error(P, "deformation: Both +xy_grids and +z_grids should be specified."); + return destructor(P, PJD_ERR_NO_ARGS ); + } + + proj_hgrid_init(P, "xy_grids"); + if (proj_errno(P)) { + proj_log_error(P, "deformation: could not find requested xy_grid(s)."); + return destructor(P, PJD_ERR_FAILED_TO_LOAD_GRID); + } + + proj_vgrid_init(P, "z_grids"); + if (proj_errno(P)) { + proj_log_error(P, "deformation: could not find requested z_grid(s)."); + return destructor(P, PJD_ERR_FAILED_TO_LOAD_GRID); + } + + Q->t_obs = HUGE_VAL; + if (pj_param(P->ctx, P->params, "tt_obs").i) { + Q->t_obs = pj_param(P->ctx, P->params, "dt_obs").f; + } + + if (pj_param(P->ctx, P->params, "tt_epoch").i) { + Q->t_epoch = pj_param(P->ctx, P->params, "dt_epoch").f; + } else { + proj_log_error(P, "deformation: +t_epoch parameter missing."); + return destructor(P, PJD_ERR_MISSING_ARGS); + } + + P->fwd4d = forward_4d; + P->inv4d = reverse_4d; + P->fwd3d = forward_3d; + P->inv3d = reverse_3d; + P->fwd = nullptr; + P->inv = nullptr; + + P->left = PJ_IO_UNITS_CARTESIAN; + P->right = PJ_IO_UNITS_CARTESIAN; + P->destructor = destructor; + + return P; +} + diff --git a/src/transformations/PJ_helmert.cpp b/src/transformations/PJ_helmert.cpp new file mode 100644 index 00000000..4a3abf4e --- /dev/null +++ b/src/transformations/PJ_helmert.cpp @@ -0,0 +1,755 @@ +/*********************************************************************** + + 3-, 4-and 7-parameter shifts, and their 6-, 8- + and 14-parameter kinematic counterparts. + + Thomas Knudsen, 2016-05-24 + +************************************************************************ + + Implements 3(6)-, 4(8) and 7(14)-parameter Helmert transformations for + 3D data. + + Also incorporates Molodensky-Badekas variant of 7-parameter Helmert + transformation, where the rotation is not applied regarding the centre + of the spheroid, but given a reference point. + + Primarily useful for implementation of datum shifts in transformation + pipelines. + +************************************************************************ + +Thomas Knudsen, thokn@sdfe.dk, 2016-05-24/06-05 +Kristian Evers, kreve@sdfe.dk, 2017-05-01 +Even Rouault, even.roault@spatialys.com +Last update: 2018-10-26 + +************************************************************************ +* Copyright (c) 2016, Thomas Knudsen / SDFE +* +* 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 <errno.h> +#include <math.h> + +#include "proj_internal.h" +#include "projects.h" +#include "geocent.h" + +PROJ_HEAD(helmert, "3(6)-, 4(8)- and 7(14)-parameter Helmert shift"); +PROJ_HEAD(molobadekas, "Molodensky-Badekas transformation"); + +static XYZ helmert_forward_3d (LPZ lpz, PJ *P); +static LPZ helmert_reverse_3d (XYZ xyz, PJ *P); + + + +/***********************************************************************/ +namespace { // anonymous namespace +struct pj_opaque_helmert { +/************************************************************************ + Projection specific elements for the "helmert" PJ object +************************************************************************/ + XYZ xyz; + XYZ xyz_0; + XYZ dxyz; + XYZ refp; + PJ_OPK opk; + PJ_OPK opk_0; + PJ_OPK dopk; + double scale; + double scale_0; + double dscale; + double theta; + double theta_0; + double dtheta; + double R[3][3]; + double t_epoch, t_obs; + int no_rotation, exact, fourparam; + int is_position_vector; /* 1 = position_vector, 0 = coordinate_frame */ +}; +} // anonymous namespace + + +/* Make the maths of the rotation operations somewhat more readable and textbook like */ +#define R00 (Q->R[0][0]) +#define R01 (Q->R[0][1]) +#define R02 (Q->R[0][2]) + +#define R10 (Q->R[1][0]) +#define R11 (Q->R[1][1]) +#define R12 (Q->R[1][2]) + +#define R20 (Q->R[2][0]) +#define R21 (Q->R[2][1]) +#define R22 (Q->R[2][2]) + +/**************************************************************************/ +static void update_parameters(PJ *P) { +/*************************************************************************** + + Update transformation parameters. + --------------------------------- + + The 14-parameter Helmert transformation is at it's core the same as the + 7-parameter transformation, since the transformation parameters are + projected forward or backwards in time via the rate of changes of the + parameters. The transformation parameters are calculated for a specific + epoch before the actual Helmert transformation is carried out. + + The transformation parameters are updated with the following equation [0]: + + . + P(t) = P(EPOCH) + P * (t - EPOCH) + + . + where EPOCH is the epoch indicated in the above table and P is the rate + of that parameter. + + [0] http://itrf.ign.fr/doc_ITRF/Transfo-ITRF2008_ITRFs.txt + +*******************************************************************************/ + + struct pj_opaque_helmert *Q = (struct pj_opaque_helmert *) P->opaque; + double dt = Q->t_obs - Q->t_epoch; + + Q->xyz.x = Q->xyz_0.x + Q->dxyz.x * dt; + Q->xyz.y = Q->xyz_0.y + Q->dxyz.y * dt; + Q->xyz.z = Q->xyz_0.z + Q->dxyz.z * dt; + + Q->opk.o = Q->opk_0.o + Q->dopk.o * dt; + Q->opk.p = Q->opk_0.p + Q->dopk.p * dt; + Q->opk.k = Q->opk_0.k + Q->dopk.k * dt; + + Q->scale = Q->scale_0 + Q->dscale * dt; + + Q->theta = Q->theta_0 + Q->dtheta * dt; + + /* debugging output */ + if (proj_log_level(P->ctx, PJ_LOG_TELL) >= PJ_LOG_TRACE) { + proj_log_trace(P, "Transformation parameters for observation " + "t_obs=%g (t_epoch=%g):", Q->t_obs, Q->t_epoch); + proj_log_trace(P, "x: %g", Q->xyz.x); + proj_log_trace(P, "y: %g", Q->xyz.y); + proj_log_trace(P, "z: %g", Q->xyz.z); + proj_log_trace(P, "s: %g", Q->scale*1e-6); + proj_log_trace(P, "rx: %g", Q->opk.o); + proj_log_trace(P, "ry: %g", Q->opk.p); + proj_log_trace(P, "rz: %g", Q->opk.k); + proj_log_trace(P, "theta: %g", Q->theta); + } +} + +/**************************************************************************/ +static void build_rot_matrix(PJ *P) { +/*************************************************************************** + + Build rotation matrix. + ---------------------- + + Here we rename rotation indices from omega, phi, kappa (opk), to + fi (i.e. phi), theta, psi (ftp), in order to reduce the mental agility + needed to implement the expression for the rotation matrix derived over + at https://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions + The relevant section is Euler angles ( z-’-x" intrinsic) -> Rotation matrix + + By default small angle approximations are used: + The matrix elements are approximated by expanding the trigonometric + functions to linear order (i.e. cos(x) = 1, sin(x) = x), and discarding + products of second order. + + This was a useful hack when calculating by hand was the only option, + but in general, today, should be avoided because: + + 1. It does not save much computation time, as the rotation matrix + is built only once and probably used many times (except when + transforming spatio-temporal coordinates). + + 2. The error induced may be too large for ultra high accuracy + applications: the Earth is huge and the linear error is + approximately the angular error multiplied by the Earth radius. + + However, in many cases the approximation is necessary, since it has + been used historically: Rotation angles from older published datum + shifts may actually be a least squares fit to the linearized rotation + approximation, hence not being strictly valid for deriving the exact + rotation matrix. In fact, most publicly available transformation + parameters are based on the approximate Helmert transform, which is why + we use that as the default setting, even though it is more correct to + use the exact form of the equations. + + So in order to fit historically derived coordinates, the access to + the approximate rotation matrix is necessary - at least in principle. + + Also, when using any published datum transformation information, one + should always check which convention (exact or approximate rotation + matrix) is expected, and whether the induced error for selecting + the opposite convention is acceptable (which it often is). + + + Sign conventions + ---------------- + + Take care: Two different sign conventions exist for the rotation terms. + + Conceptually they relate to whether we rotate the coordinate system + or the "position vector" (the vector going from the coordinate system + origin to the point being transformed, i.e. the point coordinates + interpreted as vector coordinates). + + Switching between the "position vector" and "coordinate system" + conventions is simply a matter of switching the sign of the rotation + angles, which algebraically also translates into a transposition of + the rotation matrix. + + Hence, as geodetic constants should preferably be referred to exactly + as published, the "convention" option provides the ability to switch + between the conventions. + +***************************************************************************/ + struct pj_opaque_helmert *Q = (struct pj_opaque_helmert *) P->opaque; + + double f, t, p; /* phi/fi , theta, psi */ + double cf, ct, cp; /* cos (fi, theta, psi) */ + double sf, st, sp; /* sin (fi, theta, psi) */ + + /* rename (omega, phi, kappa) to (fi, theta, psi) */ + f = Q->opk.o; + t = Q->opk.p; + p = Q->opk.k; + + /* Those equations are given assuming coordinate frame convention. */ + /* For the position vector convention, we transpose the matrix just after. */ + if (Q->exact) { + cf = cos(f); + sf = sin(f); + ct = cos(t); + st = sin(t); + cp = cos(p); + sp = sin(p); + + + R00 = ct*cp; + R01 = cf*sp + sf*st*cp; + R02 = sf*sp - cf*st*cp; + + R10 = -ct*sp; + R11 = cf*cp - sf*st*sp; + R12 = sf*cp + cf*st*sp; + + R20 = st; + R21 = -sf*ct; + R22 = cf*ct; + } else{ + R00 = 1; + R01 = p; + R02 = -t; + + R10 = -p; + R11 = 1; + R12 = f; + + R20 = t; + R21 = -f; + R22 = 1; + } + + + /* + For comparison: Description from Engsager/Poder implementation + in set_dtm_1.c (trlib) + + DATUM SHIFT: + TO = scale * ROTZ * ROTY * ROTX * FROM + TRANSLA + + ( cz sz 0) (cy 0 -sy) (1 0 0) + ROTZ=(-sz cz 0), ROTY=(0 1 0), ROTX=(0 cx sx) + ( 0 0 1) (sy 0 cy) (0 -sx cx) + + trp->r11 = cos_ry*cos_rz; + trp->r12 = cos_rx*sin_rz + sin_rx*sin_ry*cos_rz; + trp->r13 = sin_rx*sin_rz - cos_rx*sin_ry*cos_rz; + + trp->r21 = -cos_ry*sin_rz; + trp->r22 = cos_rx*cos_rz - sin_rx*sin_ry*sin_rz; + trp->r23 = sin_rx*cos_rz + cos_rx*sin_ry*sin_rz; + + trp->r31 = sin_ry; + trp->r32 = -sin_rx*cos_ry; + trp->r33 = cos_rx*cos_ry; + + trp->scale = 1.0 + scale; + */ + + + if (Q->is_position_vector) { + double r; + r = R01; R01 = R10; R10 = r; + r = R02; R02 = R20; R20 = r; + r = R12; R12 = R21; R21 = r; + } + + /* some debugging output */ + if (proj_log_level(P->ctx, PJ_LOG_TELL) >= PJ_LOG_TRACE) { + proj_log_trace(P, "Rotation Matrix:"); + proj_log_trace(P, " | % 6.6g % 6.6g % 6.6g |", R00, R01, R02); + proj_log_trace(P, " | % 6.6g % 6.6g % 6.6g |", R10, R11, R12); + proj_log_trace(P, " | % 6.6g % 6.6g % 6.6g |", R20, R21, R22); + } +} + + + + +/***********************************************************************/ +static XY helmert_forward (LP lp, PJ *P) { +/***********************************************************************/ + struct pj_opaque_helmert *Q = (struct pj_opaque_helmert *) P->opaque; + PJ_COORD point = {{0,0,0,0}}; + double x, y, cr, sr; + point.lp = lp; + + cr = cos(Q->theta) * Q->scale; + sr = sin(Q->theta) * Q->scale; + x = point.xy.x; + y = point.xy.y; + + point.xy.x = cr*x + sr*y + Q->xyz_0.x; + point.xy.y = -sr*x + cr*y + Q->xyz_0.y; + + return point.xy; +} + + +/***********************************************************************/ +static LP helmert_reverse (XY xy, PJ *P) { +/***********************************************************************/ + struct pj_opaque_helmert *Q = (struct pj_opaque_helmert *) P->opaque; + PJ_COORD point = {{0,0,0,0}}; + double x, y, sr, cr; + point.xy = xy; + + cr = cos(Q->theta) / Q->scale; + sr = sin(Q->theta) / Q->scale; + x = point.xy.x - Q->xyz_0.x; + y = point.xy.y - Q->xyz_0.y; + + point.xy.x = x*cr - y*sr; + point.xy.y = x*sr + y*cr; + + return point.lp; +} + + +/***********************************************************************/ +static XYZ helmert_forward_3d (LPZ lpz, PJ *P) { +/***********************************************************************/ + struct pj_opaque_helmert *Q = (struct pj_opaque_helmert *) P->opaque; + PJ_COORD point = {{0,0,0,0}}; + double X, Y, Z, scale; + + point.lpz = lpz; + + if (Q->fourparam) { + point.xy = helmert_forward(point.lp, P); + return point.xyz; + } + + if (Q->no_rotation) { + point.xyz.x = lpz.lam + Q->xyz.x; + point.xyz.y = lpz.phi + Q->xyz.y; + point.xyz.z = lpz.z + Q->xyz.z; + return point.xyz; + } + + scale = 1 + Q->scale * 1e-6; + + X = lpz.lam - Q->refp.x; + Y = lpz.phi - Q->refp.y; + Z = lpz.z - Q->refp.z; + + + point.xyz.x = scale * ( R00 * X + R01 * Y + R02 * Z); + point.xyz.y = scale * ( R10 * X + R11 * Y + R12 * Z); + point.xyz.z = scale * ( R20 * X + R21 * Y + R22 * Z); + + point.xyz.x += Q->xyz.x; /* for Molodensky-Badekas, Q->xyz already incorporates the Q->refp offset */ + point.xyz.y += Q->xyz.y; + point.xyz.z += Q->xyz.z; + + return point.xyz; +} + + +/***********************************************************************/ +static LPZ helmert_reverse_3d (XYZ xyz, PJ *P) { +/***********************************************************************/ + struct pj_opaque_helmert *Q = (struct pj_opaque_helmert *) P->opaque; + PJ_COORD point = {{0,0,0,0}}; + double X, Y, Z, scale; + + point.xyz = xyz; + + if (Q->fourparam) { + point.lp = helmert_reverse(point.xy, P); + return point.lpz; + } + + if (Q->no_rotation) { + point.xyz.x = xyz.x - Q->xyz.x; + point.xyz.y = xyz.y - Q->xyz.y; + point.xyz.z = xyz.z - Q->xyz.z; + return point.lpz; + } + + scale = 1 + Q->scale * 1e-6; + + /* Unscale and deoffset */ + X = (xyz.x - Q->xyz.x) / scale; + Y = (xyz.y - Q->xyz.y) / scale; + Z = (xyz.z - Q->xyz.z) / scale; + + /* Inverse rotation through transpose multiplication */ + point.xyz.x = ( R00 * X + R10 * Y + R20 * Z) + Q->refp.x; + point.xyz.y = ( R01 * X + R11 * Y + R21 * Z) + Q->refp.y; + point.xyz.z = ( R02 * X + R12 * Y + R22 * Z) + Q->refp.z; + + return point.lpz; +} + + +static PJ_COORD helmert_forward_4d (PJ_COORD point, PJ *P) { + struct pj_opaque_helmert *Q = (struct pj_opaque_helmert *) P->opaque; + + /* We only need to rebuild the rotation matrix if the + * observation time is different from the last call */ + double t_obs = (point.xyzt.t == HUGE_VAL) ? Q->t_epoch : point.xyzt.t; + if (t_obs != Q->t_obs) { + Q->t_obs = t_obs; + update_parameters(P); + build_rot_matrix(P); + } + + point.xyz = helmert_forward_3d (point.lpz, P); + + return point; +} + + +static PJ_COORD helmert_reverse_4d (PJ_COORD point, PJ *P) { + struct pj_opaque_helmert *Q = (struct pj_opaque_helmert *) P->opaque; + + /* We only need to rebuild the rotation matrix if the + * observation time is different from the last call */ + double t_obs = (point.xyzt.t == HUGE_VAL) ? Q->t_epoch : point.xyzt.t; + if (t_obs != Q->t_obs) { + Q->t_obs = t_obs; + update_parameters(P); + build_rot_matrix(P); + } + + point.lpz = helmert_reverse_3d (point.xyz, P); + + return point; +} + +/* Arcsecond to radians */ +#define ARCSEC_TO_RAD (DEG_TO_RAD / 3600.0) + + +static PJ* init_helmert_six_parameters(PJ* P) { + struct pj_opaque_helmert *Q = static_cast<struct pj_opaque_helmert*>(pj_calloc (1, sizeof (struct pj_opaque_helmert))); + if (nullptr==Q) + return pj_default_destructor (P, ENOMEM); + P->opaque = (void *) Q; + + /* In most cases, we work on 3D cartesian coordinates */ + P->left = PJ_IO_UNITS_CARTESIAN; + P->right = PJ_IO_UNITS_CARTESIAN; + + /* Translations */ + if (pj_param (P->ctx, P->params, "tx").i) + Q->xyz_0.x = pj_param (P->ctx, P->params, "dx").f; + + if (pj_param (P->ctx, P->params, "ty").i) + Q->xyz_0.y = pj_param (P->ctx, P->params, "dy").f; + + if (pj_param (P->ctx, P->params, "tz").i) + Q->xyz_0.z = pj_param (P->ctx, P->params, "dz").f; + + /* Rotations */ + if (pj_param (P->ctx, P->params, "trx").i) + Q->opk_0.o = pj_param (P->ctx, P->params, "drx").f * ARCSEC_TO_RAD; + + if (pj_param (P->ctx, P->params, "try").i) + Q->opk_0.p = pj_param (P->ctx, P->params, "dry").f * ARCSEC_TO_RAD; + + if (pj_param (P->ctx, P->params, "trz").i) + Q->opk_0.k = pj_param (P->ctx, P->params, "drz").f * ARCSEC_TO_RAD; + + /* Use small angle approximations? */ + if (pj_param (P->ctx, P->params, "bexact").i) + Q->exact = 1; + + return P; +} + + +static PJ* read_convention(PJ* P) { + + struct pj_opaque_helmert *Q = (struct pj_opaque_helmert *)P->opaque; + + /* In case there are rotational terms, we require an explicit convention + * to be provided. */ + if (!Q->no_rotation) { + const char* convention = pj_param (P->ctx, P->params, "sconvention").s; + if( !convention ) { + proj_log_error (P, "helmert: missing 'convention' argument"); + return pj_default_destructor (P, PJD_ERR_MISSING_ARGS); + } + if( strcmp(convention, "position_vector") == 0 ) { + Q->is_position_vector = 1; + } + else if( strcmp(convention, "coordinate_frame") == 0 ) { + Q->is_position_vector = 0; + } + else { + proj_log_error (P, "helmert: invalid value for 'convention' argument"); + return pj_default_destructor (P, PJD_ERR_INVALID_ARG); + } + + /* historically towgs84 in PROJ has always been using position_vector + * convention. Accepting coordinate_frame would be confusing. */ + if (pj_param_exists (P->params, "towgs84")) { + if( !Q->is_position_vector ) { + proj_log_error (P, "helmert: towgs84 should only be used with " + "convention=position_vector"); + return pj_default_destructor (P, PJD_ERR_INVALID_ARG); + } + } + } + + return P; +} + + +/***********************************************************************/ +PJ *TRANSFORMATION(helmert, 0) { +/***********************************************************************/ + + struct pj_opaque_helmert *Q; + + if( !init_helmert_six_parameters(P) ) { + return nullptr; + } + + /* In the 2D case, the coordinates are projected */ + if (pj_param_exists (P->params, "theta")) { + P->left = PJ_IO_UNITS_PROJECTED; + P->right = PJ_IO_UNITS_PROJECTED; + } + + P->fwd4d = helmert_forward_4d; + P->inv4d = helmert_reverse_4d; + P->fwd3d = helmert_forward_3d; + P->inv3d = helmert_reverse_3d; + P->fwd = helmert_forward; + P->inv = helmert_reverse; + + Q = (struct pj_opaque_helmert *)P->opaque; + + /* Detect obsolete transpose flag and error out if found */ + if (pj_param (P->ctx, P->params, "ttranspose").i) { + proj_log_error (P, "helmert: 'transpose' argument is no longer valid. " + "Use convention=position_vector/coordinate_frame"); + return pj_default_destructor (P, PJD_ERR_INVALID_ARG); + } + + /* Support the classic PROJ towgs84 parameter, but allow later overrides.*/ + /* Note that if towgs84 is specified, the datum_params array is set up */ + /* for us automagically by the pj_datum_set call in pj_init_ctx */ + if (pj_param_exists (P->params, "towgs84")) { + Q->xyz_0.x = P->datum_params[0]; + Q->xyz_0.y = P->datum_params[1]; + Q->xyz_0.z = P->datum_params[2]; + + Q->opk_0.o = P->datum_params[3]; + Q->opk_0.p = P->datum_params[4]; + Q->opk_0.k = P->datum_params[5]; + + /* We must undo conversion to absolute scale from pj_datum_set */ + if (0==P->datum_params[6]) + Q->scale_0 = 0; + else + Q->scale_0 = (P->datum_params[6] - 1) * 1e6; + } + + if (pj_param (P->ctx, P->params, "ttheta").i) { + Q->theta_0 = pj_param (P->ctx, P->params, "dtheta").f * ARCSEC_TO_RAD; + Q->fourparam = 1; + Q->scale_0 = 1.0; /* default scale for the 4-param shift */ + } + + /* Scale */ + if (pj_param (P->ctx, P->params, "ts").i) { + Q->scale_0 = pj_param (P->ctx, P->params, "ds").f; + if (pj_param (P->ctx, P->params, "ttheta").i && Q->scale_0 == 0.0) + return pj_default_destructor (P, PJD_ERR_INVALID_SCALE); + } + + /* Translation rates */ + if (pj_param(P->ctx, P->params, "tdx").i) + Q->dxyz.x = pj_param (P->ctx, P->params, "ddx").f; + + if (pj_param(P->ctx, P->params, "tdy").i) + Q->dxyz.y = pj_param (P->ctx, P->params, "ddy").f; + + if (pj_param(P->ctx, P->params, "tdz").i) + Q->dxyz.z = pj_param (P->ctx, P->params, "ddz").f; + + /* Rotations rates */ + if (pj_param (P->ctx, P->params, "tdrx").i) + Q->dopk.o = pj_param (P->ctx, P->params, "ddrx").f * ARCSEC_TO_RAD; + + if (pj_param (P->ctx, P->params, "tdry").i) + Q->dopk.p = pj_param (P->ctx, P->params, "ddry").f * ARCSEC_TO_RAD; + + if (pj_param (P->ctx, P->params, "tdrz").i) + Q->dopk.k = pj_param (P->ctx, P->params, "ddrz").f * ARCSEC_TO_RAD; + + if (pj_param (P->ctx, P->params, "tdtheta").i) + Q->dtheta = pj_param (P->ctx, P->params, "ddtheta").f * ARCSEC_TO_RAD; + + /* Scale rate */ + if (pj_param (P->ctx, P->params, "tds").i) + Q->dscale = pj_param (P->ctx, P->params, "dds").f; + + + /* Epoch */ + if (pj_param(P->ctx, P->params, "tt_epoch").i) + Q->t_epoch = pj_param (P->ctx, P->params, "dt_epoch").f; + + if (pj_param(P->ctx, P->params, "tt_obs").i) + Q->t_obs = pj_param (P->ctx, P->params, "dt_obs").f; + + Q->xyz = Q->xyz_0; + Q->opk = Q->opk_0; + Q->scale = Q->scale_0; + Q->theta = Q->theta_0; + + if ((Q->opk.o==0) && (Q->opk.p==0) && (Q->opk.k==0) && (Q->scale==0) && + (Q->dopk.o==0) && (Q->dopk.p==0) && (Q->dopk.k==0)) { + Q->no_rotation = 1; + } + + if( !read_convention(P) ) { + return nullptr; + } + + /* Let's help with debugging */ + if (proj_log_level(P->ctx, PJ_LOG_TELL) >= PJ_LOG_DEBUG) { + proj_log_debug(P, "Helmert parameters:"); + proj_log_debug(P, "x= %8.5f y= %8.5f z= %8.5f", Q->xyz.x, Q->xyz.y, Q->xyz.z); + proj_log_debug(P, "rx= %8.5f ry= %8.5f rz= %8.5f", + Q->opk.o / ARCSEC_TO_RAD, Q->opk.p / ARCSEC_TO_RAD, Q->opk.k / ARCSEC_TO_RAD); + proj_log_debug(P, "s= %8.5f exact=%d%s", Q->scale, Q->exact, + Q->no_rotation ? "" : + Q->is_position_vector ? " convention=position_vector" : + " convention=coordinate_frame"); + proj_log_debug(P, "dx= %8.5f dy= %8.5f dz= %8.5f", Q->dxyz.x, Q->dxyz.y, Q->dxyz.z); + proj_log_debug(P, "drx=%8.5f dry=%8.5f drz=%8.5f", Q->dopk.o, Q->dopk.p, Q->dopk.k); + proj_log_debug(P, "ds= %8.5f t_epoch=%8.5f t_obs=%8.5f", Q->dscale, Q->t_epoch, Q->t_obs); + } + + if (Q->no_rotation) { + return P; + } + + update_parameters(P); + build_rot_matrix(P); + + return P; +} + + +/***********************************************************************/ +PJ *TRANSFORMATION(molobadekas, 0) { +/***********************************************************************/ + + struct pj_opaque_helmert *Q; + + if( !init_helmert_six_parameters(P) ) { + return nullptr; + } + + P->fwd3d = helmert_forward_3d; + P->inv3d = helmert_reverse_3d; + + Q = (struct pj_opaque_helmert *)P->opaque; + + /* Scale */ + if (pj_param (P->ctx, P->params, "ts").i) { + Q->scale_0 = pj_param (P->ctx, P->params, "ds").f; + } + + Q->opk = Q->opk_0; + Q->scale = Q->scale_0; + + if( !read_convention(P) ) { + return nullptr; + } + + /* Reference point */ + if (pj_param (P->ctx, P->params, "tpx").i) + Q->refp.x = pj_param (P->ctx, P->params, "dpx").f; + + if (pj_param (P->ctx, P->params, "tpy").i) + Q->refp.y = pj_param (P->ctx, P->params, "dpy").f; + + if (pj_param (P->ctx, P->params, "tpz").i) + Q->refp.z = pj_param (P->ctx, P->params, "dpz").f; + + + /* Let's help with debugging */ + if (proj_log_level(P->ctx, PJ_LOG_TELL) >= PJ_LOG_DEBUG) { + proj_log_debug(P, "Molodensky-Badekas parameters:"); + proj_log_debug(P, "x= %8.5f y= %8.5f z= %8.5f", Q->xyz_0.x, Q->xyz_0.y, Q->xyz_0.z); + proj_log_debug(P, "rx= %8.5f ry= %8.5f rz= %8.5f", + Q->opk.o / ARCSEC_TO_RAD, Q->opk.p / ARCSEC_TO_RAD, Q->opk.k / ARCSEC_TO_RAD); + proj_log_debug(P, "s= %8.5f exact=%d%s", Q->scale, Q->exact, + Q->is_position_vector ? " convention=position_vector" : + " convention=coordinate_frame"); + proj_log_debug(P, "px= %8.5f py= %8.5f pz= %8.5f", Q->refp.x, Q->refp.y, Q->refp.z); + } + + /* as an optimization, we incorporate the refp in the translation terms */ + Q->xyz_0.x += Q->refp.x; + Q->xyz_0.y += Q->refp.y; + Q->xyz_0.z += Q->refp.z; + + Q->xyz = Q->xyz_0; + + build_rot_matrix(P); + + return P; +} diff --git a/src/transformations/PJ_hgridshift.cpp b/src/transformations/PJ_hgridshift.cpp new file mode 100644 index 00000000..f0e57251 --- /dev/null +++ b/src/transformations/PJ_hgridshift.cpp @@ -0,0 +1,133 @@ +#define PJ_LIB__ + +#include <errno.h> +#include <string.h> +#include <stddef.h> +#include <time.h> + +#include "proj_internal.h" +#include "projects.h" + +PROJ_HEAD(hgridshift, "Horizontal grid shift"); + +namespace { // anonymous namespace +struct pj_opaque_hgridshift { + double t_final; + double t_epoch; +}; +} // anonymous namespace + +static XYZ forward_3d(LPZ lpz, PJ *P) { + PJ_COORD point = {{0,0,0,0}}; + point.lpz = lpz; + + if (P->gridlist != nullptr) { + /* Only try the gridshift if at least one grid is loaded, + * otherwise just pass the coordinate through unchanged. */ + point.lp = proj_hgrid_apply(P, point.lp, PJ_FWD); + } + + return point.xyz; +} + + +static LPZ reverse_3d(XYZ xyz, PJ *P) { + PJ_COORD point = {{0,0,0,0}}; + point.xyz = xyz; + + if (P->gridlist != nullptr) { + /* Only try the gridshift if at least one grid is loaded, + * otherwise just pass the coordinate through unchanged. */ + point.lp = proj_hgrid_apply(P, point.lp, PJ_INV); + } + + return point.lpz; +} + +static PJ_COORD forward_4d(PJ_COORD obs, PJ *P) { + struct pj_opaque_hgridshift *Q = (struct pj_opaque_hgridshift *) P->opaque; + PJ_COORD point = obs; + + /* If transformation is not time restricted, we always call it */ + if (Q->t_final==0 || Q->t_epoch==0) { + point.xyz = forward_3d (obs.lpz, P); + return point; + } + + /* Time restricted - only apply transform if within time bracket */ + if (obs.lpzt.t < Q->t_epoch && Q->t_final > Q->t_epoch) + point.xyz = forward_3d (obs.lpz, P); + + + return point; +} + +static PJ_COORD reverse_4d(PJ_COORD obs, PJ *P) { + struct pj_opaque_hgridshift *Q = (struct pj_opaque_hgridshift *) P->opaque; + PJ_COORD point = obs; + + /* If transformation is not time restricted, we always call it */ + if (Q->t_final==0 || Q->t_epoch==0) { + point.lpz = reverse_3d (obs.xyz, P); + return point; + } + + /* Time restricted - only apply transform if within time bracket */ + if (obs.lpzt.t < Q->t_epoch && Q->t_final > Q->t_epoch) + point.lpz = reverse_3d (obs.xyz, P); + + return point; +} + + +PJ *TRANSFORMATION(hgridshift,0) { + struct pj_opaque_hgridshift *Q = static_cast<struct pj_opaque_hgridshift*>(pj_calloc (1, sizeof (struct pj_opaque_hgridshift))); + if (nullptr==Q) + return pj_default_destructor (P, ENOMEM); + P->opaque = (void *) Q; + + P->fwd4d = forward_4d; + P->inv4d = reverse_4d; + P->fwd3d = forward_3d; + P->inv3d = reverse_3d; + P->fwd = nullptr; + P->inv = nullptr; + + P->left = PJ_IO_UNITS_ANGULAR; + P->right = PJ_IO_UNITS_ANGULAR; + + if (0==pj_param(P->ctx, P->params, "tgrids").i) { + proj_log_error(P, "hgridshift: +grids parameter missing."); + return pj_default_destructor (P, PJD_ERR_NO_ARGS); + } + + /* TODO: Refactor into shared function that can be used */ + /* by both vgridshift and hgridshift */ + if (pj_param(P->ctx, P->params, "tt_final").i) { + Q->t_final = pj_param (P->ctx, P->params, "dt_final").f; + if (Q->t_final == 0) { + /* a number wasn't passed to +t_final, let's see if it was "now" */ + /* and set the time accordingly. */ + if (!strcmp("now", pj_param(P->ctx, P->params, "st_final").s)) { + time_t now; + struct tm *date; + time(&now); + date = localtime(&now); + Q->t_final = 1900.0 + date->tm_year + date->tm_yday/365.0; + } + } + } + + if (pj_param(P->ctx, P->params, "tt_epoch").i) + Q->t_epoch = pj_param (P->ctx, P->params, "dt_epoch").f; + + + proj_hgrid_init(P, "grids"); + /* Was gridlist compiled properly? */ + if ( proj_errno(P) ) { + proj_log_error(P, "hgridshift: could not find required grid(s)."); + return pj_default_destructor(P, PJD_ERR_FAILED_TO_LOAD_GRID); + } + + return P; +} diff --git a/src/transformations/PJ_horner.cpp b/src/transformations/PJ_horner.cpp new file mode 100644 index 00000000..73977de6 --- /dev/null +++ b/src/transformations/PJ_horner.cpp @@ -0,0 +1,513 @@ +/*********************************************************************** + + Interfacing to a classic piece of geodetic software + +************************************************************************ + + gen_pol is a highly efficient, classic implementation of a generic + 2D Horner's Scheme polynomial evaluation routine by Knud Poder and + Karsten Engsager, originating in the vivid geodetic environment at + what was then (1960-ish) the Danish Geodetic Institute. + + The original Poder/Engsager gen_pol implementation (where + the polynomial degree and two sets of polynomial coefficients + are packed together in one compound array, handled via a plain + double pointer) is compelling and "true to the code history": + + It has a beautiful classical 1960s ring to it, not unlike the + original fft implementations, which revolutionized spectral + analysis in twenty lines of code. + + The Poder coding sound, as classic 1960s as Phil Spector's Wall + of Sound, is beautiful and inimitable. + + On the other hand: For the uninitiated, the gen_pol code is hard + to follow, despite being compact. + + Also, since adding metadata and improving maintainability + of the code are among the implied goals of a current SDFE/DTU Space + project, the material in this file introduces a version with a + more modern (or at least 1990s) look, introducing a "double 2D + polynomial" data type, HORNER. + + Despite introducing a new data type for handling the polynomial + coefficients, great care has been taken to keep the coefficient + array organization identical to that of gen_pol. + + Hence, on one hand, the HORNER data type helps improving the + long term maintainability of the code by making the data + organization more mentally accessible. + + On the other hand, it allows us to preserve the business end of + the original gen_pol implementation - although not including the + famous "Poder dual autocheck" in all its enigmatic elegance. + + ********************************************************************** + + The material included here was written by Knud Poder, starting + around 1960, and Karsten Engsager, starting around 1970. It was + originally written in Algol 60, later (1980s) reimplemented in C. + + The HORNER data type interface, and the organization as a header + library was implemented by Thomas Knudsen, starting around 2015. + + *********************************************************************** + * + * Copyright (c) 2016, SDFE http://www.sdfe.dk / Thomas Knudsen / Karsten Engsager + * + * 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 <errno.h> +#include <math.h> +#include <stddef.h> +#include <stdio.h> +#include <string.h> + +#include "proj_internal.h" +#include "projects.h" + +PROJ_HEAD(horner, "Horner polynomial evaluation"); + +/* make horner.h interface with proj's memory management */ +#define horner_dealloc(x) pj_dealloc(x) +#define horner_calloc(n,x) pj_calloc(n,x) + +namespace { // anonymous namespace +struct horner { + int uneg; /* u axis negated? */ + int vneg; /* v axis negated? */ + int order; /* maximum degree of polynomium */ + int coefs; /* number of coefficients for each polynomium */ + double range; /* radius of the region of validity */ + + double *fwd_u; /* coefficients for the forward transformations */ + double *fwd_v; /* i.e. latitude/longitude to northing/easting */ + + double *inv_u; /* coefficients for the inverse transformations */ + double *inv_v; /* i.e. northing/easting to latitude/longitude */ + + double *fwd_c; /* coefficients for the complex forward transformations */ + double *inv_c; /* coefficients for the complex inverse transformations */ + + UV *fwd_origin; /* False longitude/latitude */ + UV *inv_origin; /* False easting/northing */ +}; +} // anonymous namespace + +typedef struct horner HORNER; +static UV horner_func (const HORNER *transformation, PJ_DIRECTION direction, UV position); +static HORNER *horner_alloc (size_t order, int complex_polynomia); +static void horner_free (HORNER *h); + +/* e.g. degree = 2: a + bx + cy + dxx + eyy + fxy, i.e. 6 coefficients */ +#define horner_number_of_coefficients(order) \ + (((order + 1)*(order + 2)/2)) + + +static void horner_free (HORNER *h) { + horner_dealloc (h->inv_v); + horner_dealloc (h->inv_u); + horner_dealloc (h->fwd_v); + horner_dealloc (h->fwd_u); + horner_dealloc (h->fwd_c); + horner_dealloc (h->inv_c); + horner_dealloc (h->fwd_origin); + horner_dealloc (h->inv_origin); + horner_dealloc (h); +} + + +static HORNER *horner_alloc (size_t order, int complex_polynomia) { + /* size_t is unsigned, so we need not check for order > 0 */ + int n = (int)horner_number_of_coefficients(order); + int polynomia_ok = 0; + HORNER *h = static_cast<HORNER*>(horner_calloc (1, sizeof (HORNER))); + + if (nullptr==h) + return nullptr; + + if (complex_polynomia) + n = 2*(int)order + 2; + h->order = (int)order; + h->coefs = n; + + if (complex_polynomia) { + h->fwd_c = static_cast<double*>(horner_calloc (n, sizeof(double))); + h->inv_c = static_cast<double*>(horner_calloc (n, sizeof(double))); + if (h->fwd_c && h->inv_c) + polynomia_ok = 1; + } + else { + h->fwd_u = static_cast<double*>(horner_calloc (n, sizeof(double))); + h->fwd_v = static_cast<double*>(horner_calloc (n, sizeof(double))); + h->inv_u = static_cast<double*>(horner_calloc (n, sizeof(double))); + h->inv_v = static_cast<double*>(horner_calloc (n, sizeof(double))); + if (h->fwd_u && h->fwd_v && h->inv_u && h->inv_v) + polynomia_ok = 1; + } + + h->fwd_origin = static_cast<UV*>(horner_calloc (1, sizeof(UV))); + h->inv_origin = static_cast<UV*>(horner_calloc (1, sizeof(UV))); + + if (polynomia_ok && h->fwd_origin && h->inv_origin) + return h; + + /* safe, since all pointers are null-initialized (by calloc) */ + horner_free (h); + return nullptr; +} + + + + +/**********************************************************************/ +static UV horner_func (const HORNER *transformation, PJ_DIRECTION direction, UV position) { +/*********************************************************************** + +A reimplementation of the classic Engsager/Poder 2D Horner polynomial +evaluation engine "gen_pol". + +This version omits the inimitable Poder "dual autocheck"-machinery, +which here is intended to be implemented at a higher level of the +library: We separate the polynomial evaluation from the quality +control (which, given the limited MTBF for "computing machinery", +typical when Knud Poder invented the dual autocheck method, +was not defensible at that time). + +Another difference from the original version is that we return the +result on the stack, rather than accepting pointers to result variables +as input. This results in code that is easy to read: + + projected = horner (s34j, 1, geographic); + geographic = horner (s34j, -1, projected ); + +and experiments have shown that on contemporary architectures, the time +taken for returning even comparatively large objects on the stack (and +the UV is not that large - typically only 16 bytes) is negligibly +different from passing two pointers (i.e. typically also 16 bytes) the +other way. + +The polynomium has the form: + +P = sum (i = [0 : order]) + sum (j = [0 : order - i]) + pow(par_1, i) * pow(par_2, j) * coef(index(order, i, j)) + +For numerical stability, the summation is carried out backwards, +summing the tiny high order elements first. + +***********************************************************************/ + + /* These variable names follow the Engsager/Poder implementation */ + int sz; /* Number of coefficients per polynomial */ + double *tcx, *tcy; /* Coefficient pointers */ + double range; /* Equivalent to the gen_pol's FLOATLIMIT constant */ + double n, e; + UV uv_error; + uv_error.u = uv_error.v = HUGE_VAL; + + if (nullptr==transformation) + return uv_error; + + /* Check for valid value of direction (-1, 0, 1) */ + switch (direction) { + case PJ_IDENT: /* no-op */ + return position; + case PJ_FWD: /* forward */ + case PJ_INV: /* inverse */ + break; + default: /* invalid */ + errno = EINVAL; + return uv_error; + } + + /* Prepare for double Horner */ + sz = horner_number_of_coefficients(transformation->order); + range = transformation->range; + + + if (direction==PJ_FWD) { /* forward */ + tcx = transformation->fwd_u + sz; + tcy = transformation->fwd_v + sz; + e = position.u - transformation->fwd_origin->u; + n = position.v - transformation->fwd_origin->v; + } else { /* inverse */ + tcx = transformation->inv_u + sz; + tcy = transformation->inv_v + sz; + e = position.u - transformation->inv_origin->u; + n = position.v - transformation->inv_origin->v; + } + + if ((fabs(n) > range) || (fabs(e) > range)) { + errno = EDOM; + return uv_error; + } + + /* The melody of this block is straight out of the great Engsager/Poder songbook */ + else { + int g = transformation->order; + int r = g, c; + double u, v, N, E; + + /* Double Horner's scheme: N = n*Cy*e -> yout, E = e*Cx*n -> xout */ + N = *--tcy; + E = *--tcx; + for (; r > 0; r--) { + u = *--tcy; + v = *--tcx; + for (c = g; c >= r; c--) { + u = n*u + *--tcy; + v = e*v + *--tcx; + } + N = e*N + u; + E = n*E + v; + } + + position.u = E; + position.v = N; + } + + return position; +} + + + + + + + +static PJ_COORD horner_forward_4d (PJ_COORD point, PJ *P) { + point.uv = horner_func ((HORNER *) P->opaque, PJ_FWD, point.uv); + return point; +} + +static PJ_COORD horner_reverse_4d (PJ_COORD point, PJ *P) { + point.uv = horner_func ((HORNER *) P->opaque, PJ_INV, point.uv); + return point; +} + + + + +/**********************************************************************/ +static UV complex_horner (const HORNER *transformation, PJ_DIRECTION direction, UV position) { +/*********************************************************************** + +A reimplementation of a classic Engsager/Poder Horner complex +polynomial evaluation engine. + +***********************************************************************/ + + /* These variable names follow the Engsager/Poder implementation */ + int sz; /* Number of coefficients */ + double *c, *cb; /* Coefficient pointers */ + double range; /* Equivalent to the gen_pol's FLOATLIMIT constant */ + double n, e, w, N, E; + UV uv_error; + uv_error.u = uv_error.v = HUGE_VAL; + + if (nullptr==transformation) + return uv_error; + + /* Check for valid value of direction (-1, 0, 1) */ + switch (direction) { + case PJ_IDENT: /* no-op */ + return position; + case PJ_FWD: /* forward */ + case PJ_INV: /* inverse */ + break; + default: /* invalid */ + errno = EINVAL; + return uv_error; + } + + /* Prepare for double Horner */ + sz = 2*transformation->order + 2; + range = transformation->range; + + if (direction==PJ_FWD) { /* forward */ + cb = transformation->fwd_c; + c = cb + sz; + e = position.u - transformation->fwd_origin->u; + n = position.v - transformation->fwd_origin->v; + if (transformation->uneg) + e = -e; + if (transformation->vneg) + n = -n; + } else { /* inverse */ + cb = transformation->inv_c; + c = cb + sz; + e = position.u - transformation->inv_origin->u; + n = position.v - transformation->inv_origin->v; + if (transformation->uneg) + e = -e; + if (transformation->vneg) + n = -n; + } + + if ((fabs(n) > range) || (fabs(e) > range)) { + errno = EDOM; + return uv_error; + } + + /* Everything's set up properly - now do the actual polynomium evaluation */ + E = *--c; + N = *--c; + while (c > cb) { + w = n*E + e*N + *--c; + N = n*N - e*E + *--c; + E = w; + } + + position.u = E; + position.v = N; + return position; +} + + + +static PJ_COORD complex_horner_forward_4d (PJ_COORD point, PJ *P) { + point.uv = complex_horner ((HORNER *) P->opaque, PJ_FWD, point.uv); + return point; +} + +static PJ_COORD complex_horner_reverse_4d (PJ_COORD point, PJ *P) { + point.uv = complex_horner ((HORNER *) P->opaque, PJ_INV, point.uv); + return point; +} + + +static PJ *horner_freeup (PJ *P, int errlev) { /* Destructor */ + if (nullptr==P) + return nullptr; + if (nullptr==P->opaque) + return pj_default_destructor (P, errlev); + horner_free ((HORNER *) P->opaque); + P->opaque = nullptr; + return pj_default_destructor (P, errlev); +} + + +static int parse_coefs (PJ *P, double *coefs, const char *param, int ncoefs) { + char *buf, *init, *next = nullptr; + int i; + + buf = static_cast<char*>(pj_calloc (strlen (param) + 2, sizeof(char))); + if (nullptr==buf) { + proj_log_error (P, "Horner: No memory left"); + return 0; + } + + sprintf (buf, "t%s", param); + if (0==pj_param (P->ctx, P->params, buf).i) { + pj_dealloc (buf); + return 0; + } + sprintf (buf, "s%s", param); + init = pj_param(P->ctx, P->params, buf).s; + pj_dealloc (buf); + + for (i = 0; i < ncoefs; i++) { + if (i > 0) { + if ( next == nullptr || ','!=*next) { + proj_log_error (P, "Horner: Malformed polynomium set %s. need %d coefs", param, ncoefs); + return 0; + } + init = ++next; + } + coefs[i] = pj_strtod (init, &next); + } + return 1; +} + + +/*********************************************************************/ +PJ *PROJECTION(horner) { +/*********************************************************************/ + int degree = 0, n, complex_polynomia = 0; + HORNER *Q; + P->fwd4d = horner_forward_4d; + P->inv4d = horner_reverse_4d; + P->fwd3d = nullptr; + P->inv3d = nullptr; + P->fwd = nullptr; + P->inv = nullptr; + P->left = P->right = PJ_IO_UNITS_PROJECTED; + P->destructor = horner_freeup; + + /* Polynomial degree specified? */ + if (pj_param (P->ctx, P->params, "tdeg").i) { /* degree specified? */ + degree = pj_param(P->ctx, P->params, "ideg").i; + if (degree < 0 || degree > 10000) { + /* What are reasonable minimum and maximums for degree? */ + proj_log_debug (P, "Horner: Degree is unreasonable: %d", degree); + return horner_freeup (P, PJD_ERR_INVALID_ARG); + } + } else { + proj_log_debug (P, "Horner: Must specify polynomial degree, (+deg=n)"); + return horner_freeup (P, PJD_ERR_MISSING_ARGS); + } + + if (pj_param (P->ctx, P->params, "tfwd_c").i || pj_param (P->ctx, P->params, "tinv_c").i) /* complex polynomium? */ + complex_polynomia = 1; + + Q = horner_alloc (degree, complex_polynomia); + if (Q == nullptr) + return horner_freeup (P, ENOMEM); + P->opaque = Q; + + if (complex_polynomia) { + /* Westings and/or southings? */ + Q->uneg = pj_param_exists (P->params, "uneg") ? 1 : 0; + Q->vneg = pj_param_exists (P->params, "vneg") ? 1 : 0; + + n = 2*degree + 2; + if (0==parse_coefs (P, Q->fwd_c, "fwd_c", n)) + return horner_freeup (P, PJD_ERR_MISSING_ARGS); + if (0==parse_coefs (P, Q->inv_c, "inv_c", n)) + return horner_freeup (P, PJD_ERR_MISSING_ARGS); + P->fwd4d = complex_horner_forward_4d; + P->inv4d = complex_horner_reverse_4d; + } + + else { + n = horner_number_of_coefficients (degree); + if (0==parse_coefs (P, Q->fwd_u, "fwd_u", n)) + return horner_freeup (P, PJD_ERR_MISSING_ARGS); + if (0==parse_coefs (P, Q->fwd_v, "fwd_v", n)) + return horner_freeup (P, PJD_ERR_MISSING_ARGS); + if (0==parse_coefs (P, Q->inv_u, "inv_u", n)) + return horner_freeup (P, PJD_ERR_MISSING_ARGS); + if (0==parse_coefs (P, Q->inv_v, "inv_v", n)) + return horner_freeup (P, PJD_ERR_MISSING_ARGS); + } + + if (0==parse_coefs (P, (double *)(Q->fwd_origin), "fwd_origin", 2)) + return horner_freeup (P, PJD_ERR_MISSING_ARGS); + if (0==parse_coefs (P, (double *)(Q->inv_origin), "inv_origin", 2)) + return horner_freeup (P, PJD_ERR_MISSING_ARGS); + if (0==parse_coefs (P, &Q->range, "range", 1)) + Q->range = 500000; + + return P; +} diff --git a/src/transformations/PJ_molodensky.cpp b/src/transformations/PJ_molodensky.cpp new file mode 100644 index 00000000..91743fda --- /dev/null +++ b/src/transformations/PJ_molodensky.cpp @@ -0,0 +1,329 @@ +/*********************************************************************** + + (Abridged) Molodensky Transform + + Kristian Evers, 2017-07-07 + +************************************************************************ + + Implements the (abridged) Molodensky transformations for 2D and 3D + data. + + Primarily useful for implementation of datum shifts in transformation + pipelines. + + The code in this file is mostly based on + + The Standard and Abridged Molodensky Coordinate Transformation + Formulae, 2004, R.E. Deaking, + http://www.mygeodesy.id.au/documents/Molodensky%20V2.pdf + + + +************************************************************************ +* Copyright (c) 2017, Kristian Evers / SDFE +* +* 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 <errno.h> +#include <math.h> + +#include "proj.h" +#include "proj_internal.h" +#include "projects.h" + +PROJ_HEAD(molodensky, "Molodensky transform"); + +static XYZ forward_3d(LPZ lpz, PJ *P); +static LPZ reverse_3d(XYZ xyz, PJ *P); + +namespace { // anonymous namespace +struct pj_opaque_molodensky { + double dx; + double dy; + double dz; + double da; + double df; + int abridged; +}; +} // anonymous namespace + + +static double RN (double a, double es, double phi) { +/********************************************************** + N(phi) - prime vertical radius of curvature + ------------------------------------------- + + This is basically the same function as in PJ_cart.c + should probably be refactored into it's own file at some + point. + +**********************************************************/ + double s = sin(phi); + if (es==0) + return a; + + return a / sqrt (1 - es*s*s); +} + + +static double RM (double a, double es, double phi) { +/********************************************************** + M(phi) - Meridian radius of curvature + ------------------------------------- + + Source: + + E.J Krakiwsky & D.B. Thomson, 1974, + GEODETIC POSITION COMPUTATIONS, + + Fredericton NB, Canada: + University of New Brunswick, + Department of Geodesy and Geomatics Engineering, + Lecture Notes No. 39, + 99 pp. + + http://www2.unb.ca/gge/Pubs/LN39.pdf + +**********************************************************/ + double s = sin(phi); + if (es==0) + return a; + + /* eq. 13a */ + if (phi == 0) + return a * (1-es); + + /* eq. 13b */ + if (fabs(phi) == M_PI_2) + return a / sqrt(1-es); + + /* eq. 13 */ + return (a * (1 - es) ) / pow(1 - es*s*s, 1.5); + +} + + +static LPZ calc_standard_params(LPZ lpz, PJ *P) { + struct pj_opaque_molodensky *Q = (struct pj_opaque_molodensky *) P->opaque; + double dphi, dlam, dh; + + /* sines and cosines */ + double slam = sin(lpz.lam); + double clam = cos(lpz.lam); + double sphi = sin(lpz.phi); + double cphi = cos(lpz.phi); + + /* ellipsoid parameters and differences */ + double f = P->f, a = P->a; + double dx = Q->dx, dy = Q->dy, dz = Q->dz; + double da = Q->da, df = Q->df; + + /* ellipsoid radii of curvature */ + double rho = RM(a, P->es, lpz.phi); + double nu = RN(a, P->e2s, lpz.phi); + + /* delta phi */ + dphi = (-dx*sphi*clam) - (dy*sphi*slam) + (dz*cphi) + + ((nu * P->es * sphi * cphi * da) / a) + + (sphi*cphi * ( rho/(1-f) + nu*(1-f))*df); + dphi /= (rho + lpz.z); + + /* delta lambda */ + dlam = (-dx*slam + dy*clam) / ((nu+lpz.z)*cphi); + + /* delta h */ + dh = dx*cphi*clam + dy*cphi*slam + dz*sphi - (a/nu)*da + nu*(1-f)*sphi*sphi*df; + + lpz.phi = dphi; + lpz.lam = dlam; + lpz.z = dh; + + return lpz; +} + + +static LPZ calc_abridged_params(LPZ lpz, PJ *P) { + struct pj_opaque_molodensky *Q = (struct pj_opaque_molodensky *) P->opaque; + double dphi, dlam, dh; + + /* sines and cosines */ + double slam = sin(lpz.lam); + double clam = cos(lpz.lam); + double sphi = sin(lpz.phi); + double cphi = cos(lpz.phi); + + /* ellipsoid parameters and differences */ + double dx = Q->dx, dy = Q->dy, dz = Q->dz; + double da = Q->da, df = Q->df; + double adffda = (P->a*df + P->f*da); + + /* delta phi */ + dphi = -dx*sphi*clam - dy*sphi*slam + dz*cphi + adffda*sin(2*lpz.phi); + dphi /= RM(P->a, P->es, lpz.phi); + + /* delta lambda */ + dlam = -dx*slam + dy*clam; + dlam /= RN(P->a, P->e2s, lpz.phi)*cphi; + + /* delta h */ + dh = dx*cphi*clam + dy*cphi*slam + dz*sphi - da + adffda*sphi*sphi; + + /* offset coordinate */ + lpz.phi = dphi; + lpz.lam = dlam; + lpz.z = dh; + + return lpz; +} + + +static XY forward_2d(LP lp, PJ *P) { + PJ_COORD point = {{0,0,0,0}}; + + point.lp = lp; + point.xyz = forward_3d(point.lpz, P); + + return point.xy; +} + + +static LP reverse_2d(XY xy, PJ *P) { + PJ_COORD point = {{0,0,0,0}}; + + point.xy = xy; + point.xyz.z = 0; + point.lpz = reverse_3d(point.xyz, P); + + return point.lp; +} + + +static XYZ forward_3d(LPZ lpz, PJ *P) { + struct pj_opaque_molodensky *Q = (struct pj_opaque_molodensky *) P->opaque; + PJ_COORD point = {{0,0,0,0}}; + + point.lpz = lpz; + + /* calculate parameters depending on the mode we are in */ + if (Q->abridged) { + lpz = calc_abridged_params(lpz, P); + } else { + lpz = calc_standard_params(lpz, P); + } + + /* offset coordinate */ + point.lpz.phi += lpz.phi; + point.lpz.lam += lpz.lam; + point.lpz.z += lpz.z; + + return point.xyz; +} + + +static PJ_COORD forward_4d(PJ_COORD obs, PJ *P) { + obs.xyz = forward_3d(obs.lpz, P); + return obs; +} + + +static LPZ reverse_3d(XYZ xyz, PJ *P) { + struct pj_opaque_molodensky *Q = (struct pj_opaque_molodensky *) P->opaque; + PJ_COORD point = {{0,0,0,0}}; + LPZ lpz; + + /* calculate parameters depending on the mode we are in */ + point.xyz = xyz; + if (Q->abridged) + lpz = calc_abridged_params(point.lpz, P); + else + lpz = calc_standard_params(point.lpz, P); + + /* offset coordinate */ + point.lpz.phi -= lpz.phi; + point.lpz.lam -= lpz.lam; + point.lpz.z -= lpz.z; + + return point.lpz; +} + + +static PJ_COORD reverse_4d(PJ_COORD obs, PJ *P) { + obs.lpz = reverse_3d(obs.xyz, P); + return obs; +} + + +PJ *TRANSFORMATION(molodensky,1) { + int count_required_params = 0; + struct pj_opaque_molodensky *Q = static_cast<struct pj_opaque_molodensky*>(pj_calloc(1, sizeof(struct pj_opaque_molodensky))); + if (nullptr==Q) + return pj_default_destructor(P, ENOMEM); + P->opaque = (void *) Q; + + P->fwd4d = forward_4d; + P->inv4d = reverse_4d; + P->fwd3d = forward_3d; + P->inv3d = reverse_3d; + P->fwd = forward_2d; + P->inv = reverse_2d; + + P->left = PJ_IO_UNITS_ANGULAR; + P->right = PJ_IO_UNITS_ANGULAR; + + /* read args */ + if (pj_param(P->ctx, P->params, "tdx").i) { + count_required_params ++; + Q->dx = pj_param(P->ctx, P->params, "ddx").f; + } + + if (pj_param(P->ctx, P->params, "tdy").i) { + count_required_params ++; + Q->dy = pj_param(P->ctx, P->params, "ddy").f; + } + + if (pj_param(P->ctx, P->params, "tdz").i) { + count_required_params ++; + Q->dz = pj_param(P->ctx, P->params, "ddz").f; + } + + if (pj_param(P->ctx, P->params, "tda").i) { + count_required_params ++; + Q->da = pj_param(P->ctx, P->params, "dda").f; + } + + if (pj_param(P->ctx, P->params, "tdf").i) { + count_required_params ++; + Q->df = pj_param(P->ctx, P->params, "ddf").f; + } + + Q->abridged = pj_param(P->ctx, P->params, "tabridged").i; + + /* We want all parameters (except +abridged) to be set */ + if (count_required_params == 0) + return pj_default_destructor(P, PJD_ERR_NO_ARGS); + + if (count_required_params != 5) + return pj_default_destructor(P, PJD_ERR_MISSING_ARGS); + + return P; +} diff --git a/src/transformations/PJ_vgridshift.cpp b/src/transformations/PJ_vgridshift.cpp new file mode 100644 index 00000000..b3da906d --- /dev/null +++ b/src/transformations/PJ_vgridshift.cpp @@ -0,0 +1,144 @@ +#define PJ_LIB__ + +#include <errno.h> +#include <stddef.h> +#include <string.h> +#include <time.h> + +#include "proj_internal.h" +#include "projects.h" + +PROJ_HEAD(vgridshift, "Vertical grid shift"); + +namespace { // anonymous namespace +struct pj_opaque_vgridshift { + double t_final; + double t_epoch; + double forward_multiplier; +}; +} // anonymous namespace + +static XYZ forward_3d(LPZ lpz, PJ *P) { + struct pj_opaque_vgridshift *Q = (struct pj_opaque_vgridshift *) P->opaque; + PJ_COORD point = {{0,0,0,0}}; + point.lpz = lpz; + + if (P->vgridlist_geoid != nullptr) { + /* Only try the gridshift if at least one grid is loaded, + * otherwise just pass the coordinate through unchanged. */ + point.xyz.z += Q->forward_multiplier * proj_vgrid_value(P, point.lp); + } + + return point.xyz; +} + + +static LPZ reverse_3d(XYZ xyz, PJ *P) { + struct pj_opaque_vgridshift *Q = (struct pj_opaque_vgridshift *) P->opaque; + PJ_COORD point = {{0,0,0,0}}; + point.xyz = xyz; + + if (P->vgridlist_geoid != nullptr) { + /* Only try the gridshift if at least one grid is loaded, + * otherwise just pass the coordinate through unchanged. */ + point.xyz.z -= Q->forward_multiplier * proj_vgrid_value(P, point.lp); + } + + return point.lpz; +} + + +static PJ_COORD forward_4d(PJ_COORD obs, PJ *P) { + struct pj_opaque_vgridshift *Q = (struct pj_opaque_vgridshift *) P->opaque; + PJ_COORD point = obs; + + /* If transformation is not time restricted, we always call it */ + if (Q->t_final==0 || Q->t_epoch==0) { + point.xyz = forward_3d (obs.lpz, P); + return point; + } + + /* Time restricted - only apply transform if within time bracket */ + if (obs.lpzt.t < Q->t_epoch && Q->t_final > Q->t_epoch) + point.xyz = forward_3d (obs.lpz, P); + + + return point; +} + +static PJ_COORD reverse_4d(PJ_COORD obs, PJ *P) { + struct pj_opaque_vgridshift *Q = (struct pj_opaque_vgridshift *) P->opaque; + PJ_COORD point = obs; + + /* If transformation is not time restricted, we always call it */ + if (Q->t_final==0 || Q->t_epoch==0) { + point.lpz = reverse_3d (obs.xyz, P); + return point; + } + + /* Time restricted - only apply transform if within time bracket */ + if (obs.lpzt.t < Q->t_epoch && Q->t_final > Q->t_epoch) + point.lpz = reverse_3d (obs.xyz, P); + + return point; +} + + +PJ *TRANSFORMATION(vgridshift,0) { + struct pj_opaque_vgridshift *Q = static_cast<struct pj_opaque_vgridshift*>(pj_calloc (1, sizeof (struct pj_opaque_vgridshift))); + if (nullptr==Q) + return pj_default_destructor (P, ENOMEM); + P->opaque = (void *) Q; + + if (!pj_param(P->ctx, P->params, "tgrids").i) { + proj_log_error(P, "vgridshift: +grids parameter missing."); + return pj_default_destructor(P, PJD_ERR_NO_ARGS); + } + + /* TODO: Refactor into shared function that can be used */ + /* by both vgridshift and hgridshift */ + if (pj_param(P->ctx, P->params, "tt_final").i) { + Q->t_final = pj_param (P->ctx, P->params, "dt_final").f; + if (Q->t_final == 0) { + /* a number wasn't passed to +t_final, let's see if it was "now" */ + /* and set the time accordingly. */ + if (!strcmp("now", pj_param(P->ctx, P->params, "st_final").s)) { + time_t now; + struct tm *date; + time(&now); + date = localtime(&now); + Q->t_final = 1900.0 + date->tm_year + date->tm_yday/365.0; + } + } + } + + if (pj_param(P->ctx, P->params, "tt_epoch").i) + Q->t_epoch = pj_param (P->ctx, P->params, "dt_epoch").f; + + /* historical: the forward direction subtracts the grid offset. */ + Q->forward_multiplier = -1.0; + if (pj_param(P->ctx, P->params, "tmultiplier").i) { + Q->forward_multiplier = pj_param(P->ctx, P->params, "dmultiplier").f; + } + + /* Build gridlist. P->vgridlist_geoid can be empty if +grids only ask for optional grids. */ + proj_vgrid_init(P, "grids"); + + /* Was gridlist compiled properly? */ + if ( proj_errno(P) ) { + proj_log_error(P, "vgridshift: could not find required grid(s)."); + return pj_default_destructor(P, PJD_ERR_FAILED_TO_LOAD_GRID); + } + + P->fwd4d = forward_4d; + P->inv4d = reverse_4d; + P->fwd3d = forward_3d; + P->inv3d = reverse_3d; + P->fwd = nullptr; + P->inv = nullptr; + + P->left = PJ_IO_UNITS_ANGULAR; + P->right = PJ_IO_UNITS_ANGULAR; + + return P; +} |
