1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
|
/******************************************************************************
*
* Project: PROJ
* Purpose: Generic method to compute inverse projection from forward method
* Author: Even Rouault <even dot rouault at spatialys dot com>
*
******************************************************************************
* Copyright (c) 2018, Even Rouault <even dot rouault at spatialys dot 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.
****************************************************************************/
#include "proj_internal.h"
#include <algorithm>
#include <cmath>
/** Compute (lam, phi) corresponding to input (xy.x, xy.y) for projection P.
*
* Uses Newton-Raphson method, extended to 2D variables, that is using
* inversion of the Jacobian 2D matrix of partial derivatives. The derivatives
* are estimated numerically from the P->fwd method evaluated at close points.
*
* Note: thresholds used have been verified to work with adams_ws2 and wink2
*
* Starts with initial guess provided by user in lpInitial
*/
PJ_LP pj_generic_inverse_2d(PJ_XY xy, PJ *P, PJ_LP lpInitial) {
PJ_LP lp = lpInitial;
double deriv_lam_X = 0;
double deriv_lam_Y = 0;
double deriv_phi_X = 0;
double deriv_phi_Y = 0;
for (int i = 0; i < 15; i++) {
PJ_XY xyApprox = P->fwd(lp, P);
const double deltaX = xyApprox.x - xy.x;
const double deltaY = xyApprox.y - xy.y;
if (fabs(deltaX) < 1e-10 && fabs(deltaY) < 1e-10) {
return lp;
}
if (fabs(deltaX) > 1e-6 || fabs(deltaY) > 1e-6) {
// Compute Jacobian matrix (only if we aren't close to the final
// result to speed things a bit)
PJ_LP lp2;
PJ_XY xy2;
const double dLam = lp.lam > 0 ? -1e-6 : 1e-6;
lp2.lam = lp.lam + dLam;
lp2.phi = lp.phi;
xy2 = P->fwd(lp2, P);
const double deriv_X_lam = (xy2.x - xyApprox.x) / dLam;
const double deriv_Y_lam = (xy2.y - xyApprox.y) / dLam;
const double dPhi = lp.phi > 0 ? -1e-6 : 1e-6;
lp2.lam = lp.lam;
lp2.phi = lp.phi + dPhi;
xy2 = P->fwd(lp2, P);
const double deriv_X_phi = (xy2.x - xyApprox.x) / dPhi;
const double deriv_Y_phi = (xy2.y - xyApprox.y) / dPhi;
// Inverse of Jacobian matrix
const double det =
deriv_X_lam * deriv_Y_phi - deriv_X_phi * deriv_Y_lam;
if (det != 0) {
deriv_lam_X = deriv_Y_phi / det;
deriv_lam_Y = -deriv_X_phi / det;
deriv_phi_X = -deriv_Y_lam / det;
deriv_phi_Y = deriv_X_lam / det;
}
}
if (xy.x != 0) {
// Limit the amplitude of correction to avoid overshoots due to
// bad initial guess
const double delta_lam = std::max(
std::min(deltaX * deriv_lam_X + deltaY * deriv_lam_Y, 0.3),
-0.3);
lp.lam -= delta_lam;
if (lp.lam < -M_PI)
lp.lam = -M_PI;
else if (lp.lam > M_PI)
lp.lam = M_PI;
}
if (xy.y != 0) {
const double delta_phi = std::max(
std::min(deltaX * deriv_phi_X + deltaY * deriv_phi_Y, 0.3),
-0.3);
lp.phi -= delta_phi;
if (lp.phi < -M_HALFPI)
lp.phi = -M_HALFPI;
else if (lp.phi > M_HALFPI)
lp.phi = M_HALFPI;
}
}
pj_ctx_set_errno(P->ctx, PJD_ERR_NON_CONVERGENT);
return lp;
}
|
