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
115
116
117
118
119
|
/******************************************************************************
*
* Project: GDAL
* Purpose: Generic method to compute inverse coordinate transformation from
* forward method
* Author: Even Rouault <even dot rouault at spatialys dot com>
*
******************************************************************************
* Copyright (c) 2023, Even Rouault <even dot rouault at spatialys dot com>
*
* SPDX-License-Identifier: MIT
****************************************************************************/
#include <algorithm>
#include <cmath>
#include "gdalgenericinverse.h"
#include <cstdio>
/** Compute (xOut, yOut) corresponding to input (xIn, yIn) using
* the provided forward transformation to emulate the reverse direction.
*
* 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 forward method evaluated at close points.
*
* Starts with initial guess provided by user in (guessedXOut, guessedYOut)
*
* It iterates at most for 15 iterations or as soon as we get below the specified
* tolerance (on input coordinates)
*/
bool GDALGenericInverse2D(double xIn, double yIn, double guessedXOut,
double guessedYOut,
GDALForwardCoordTransformer pfnForwardTranformer,
void *pfnForwardTranformerUserData, double &xOut,
double &yOut,
bool computeJacobianMatrixOnlyAtFirstIter,
double toleranceOnInputCoordinates,
double toleranceOnOutputCoordinates)
{
const double dfAbsValOut = std::max(fabs(guessedXOut), fabs(guessedYOut));
const double dfEps = dfAbsValOut > 0 ? dfAbsValOut * 1e-6 : 1e-6;
if (toleranceOnInputCoordinates == 0)
{
const double dfAbsValIn = std::max(fabs(xIn), fabs(yIn));
toleranceOnInputCoordinates =
dfAbsValIn > 0 ? dfAbsValIn * 1e-12 : 1e-12;
}
xOut = guessedXOut;
yOut = guessedYOut;
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++)
{
double xApprox;
double yApprox;
if (!pfnForwardTranformer(xOut, yOut, xApprox, yApprox,
pfnForwardTranformerUserData))
return false;
const double deltaX = xApprox - xIn;
const double deltaY = yApprox - yIn;
if (fabs(deltaX) < toleranceOnInputCoordinates &&
fabs(deltaY) < toleranceOnInputCoordinates)
{
return true;
}
if (i == 0 || !computeJacobianMatrixOnlyAtFirstIter)
{
// Compute Jacobian matrix
double xTmp = xOut + dfEps;
double yTmp = yOut;
double xTmpOut;
double yTmpOut;
if (!pfnForwardTranformer(xTmp, yTmp, xTmpOut, yTmpOut,
pfnForwardTranformerUserData))
return false;
const double deriv_X_lam = (xTmpOut - xApprox) / dfEps;
const double deriv_Y_lam = (yTmpOut - yApprox) / dfEps;
xTmp = xOut;
yTmp = yOut + dfEps;
if (!pfnForwardTranformer(xTmp, yTmp, xTmpOut, yTmpOut,
pfnForwardTranformerUserData))
return false;
const double deriv_X_phi = (xTmpOut - xApprox) / dfEps;
const double deriv_Y_phi = (yTmpOut - yApprox) / dfEps;
// 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;
}
else
{
return false;
}
}
const double xOutDelta = deltaX * deriv_lam_X + deltaY * deriv_lam_Y;
const double yOutDelta = deltaX * deriv_phi_X + deltaY * deriv_phi_Y;
xOut -= xOutDelta;
yOut -= yOutDelta;
if (toleranceOnOutputCoordinates > 0 &&
fabs(xOutDelta) < toleranceOnOutputCoordinates &&
fabs(yOutDelta) < toleranceOnOutputCoordinates)
{
return true;
}
}
return false;
}
|
