static PJ_XYZ get_grid_shift(PJ* P, PJ_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 PJ_XYZ-space. ENU -> PJ_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 -> PJ_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 PJ_LPZ reverse_3d(PJ_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_XYZ forward_3d(PJ_LPZ lpz, PJ *P) {
struct pj_opaque *Q = (struct pj_opaque *) P->opaque;
PJ_COORD out, in;
PJ_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;
}
int pj_apply_vgridshift( PJ *defn, const char *listname,
PJ_GRIDINFO ***gridlist_p,
int *gridlist_count_p,
int inverse,
long point_count, int point_offset,
double *x, double *y, double *z )
{
int i;
static int debug_count = 0;
PJ_GRIDINFO **tables;
struct CTABLE ct;
if( *gridlist_p == NULL )
{
*gridlist_p =
pj_gridlist_from_nadgrids( pj_get_ctx(defn),
pj_param(defn->ctx,defn->params,listname).s,
gridlist_count_p );
if( *gridlist_p == NULL || *gridlist_count_p == 0 )
return defn->ctx->last_errno;
}
if( *gridlist_count_p == 0 )
{
pj_ctx_set_errno( defn->ctx, PJD_ERR_FAILED_TO_LOAD_GRID);
return PJD_ERR_FAILED_TO_LOAD_GRID;
}
tables = *gridlist_p;
defn->ctx->last_errno = 0;
for( i = 0; i < point_count; i++ )
{
double value;
long io = i * point_offset;
LP input;
input.phi = y[io];
input.lam = x[io];
value = read_vgrid_value(defn, input, gridlist_count_p, tables, &ct);
if( inverse )
z[io] -= value;
else
z[io] += value;
if( value != HUGE_VAL )
{
if( debug_count++ < 20 ) {
proj_log_trace(defn, "pj_apply_gridshift(): used %s", ct.id);
break;
}
}
if( value == HUGE_VAL )
{
int itable;
char gridlist[3000];
proj_log_debug(defn,
"pj_apply_vgridshift(): failed to find a grid shift table for\n"
" location (%.7fdW,%.7fdN)",
x[io] * RAD_TO_DEG,
y[io] * RAD_TO_DEG );
gridlist[0] = '\0';
for( itable = 0; itable < *gridlist_count_p; itable++ )
{
PJ_GRIDINFO *gi = tables[itable];
if( strlen(gridlist) + strlen(gi->gridname) > sizeof(gridlist)-100 )
{
strcat( gridlist, "..." );
break;
}
if( itable == 0 )
sprintf( gridlist, " tried: %s", gi->gridname );
else
sprintf( gridlist+strlen(gridlist), ",%s", gi->gridname );
}
proj_log_debug(defn, "%s", gridlist);
pj_ctx_set_errno( defn->ctx, PJD_ERR_GRID_AREA );
return PJD_ERR_GRID_AREA;
}
}
return 0;
}