Line data Source code
1 : /******************************************************************************
2 : *
3 : * Project: GDAL
4 : * Purpose: Linear system solver
5 : * Author: VIZRT Development Team.
6 : *
7 : * This code was provided by Gilad Ronnen (gro at visrt dot com) with
8 : * permission to reuse under the following license.
9 : *
10 : ******************************************************************************
11 : * Copyright (c) 2004, VIZRT Inc.
12 : * Copyright (c) 2008-2014, Even Rouault <even dot rouault at spatialys.com>
13 : * Copyright (c) 2019, Martin Franzke <martin dot franzke at telekom dot de>
14 : *
15 : * Permission is hereby granted, free of charge, to any person obtaining a
16 : * copy of this software and associated documentation files (the "Software"),
17 : * to deal in the Software without restriction, including without limitation
18 : * the rights to use, copy, modify, merge, publish, distribute, sublicense,
19 : * and/or sell copies of the Software, and to permit persons to whom the
20 : * Software is furnished to do so, subject to the following conditions:
21 : *
22 : * The above copyright notice and this permission notice shall be included
23 : * in all copies or substantial portions of the Software.
24 : *
25 : * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
26 : * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
27 : * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
28 : * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
29 : * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
30 : * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
31 : * DEALINGS IN THE SOFTWARE.
32 : ****************************************************************************/
33 :
34 : /*! @cond Doxygen_Suppress */
35 :
36 : #include "cpl_port.h"
37 : #include "cpl_conv.h"
38 : #include "gdallinearsystem.h"
39 :
40 : #ifdef HAVE_ARMADILLO
41 : #include "armadillo_headers.h"
42 : #endif
43 :
44 : #include <cstdio>
45 : #include <algorithm>
46 : #include <cassert>
47 : #include <cmath>
48 :
49 : #ifndef HAVE_ARMADILLO
50 : namespace
51 : {
52 : // LU decomposition of the quadratic matrix A
53 : // see https://en.wikipedia.org/wiki/LU_decomposition#C_code_examples
54 : bool solve(GDALMatrix &A, GDALMatrix &RHS, GDALMatrix &X, double eps)
55 : {
56 : assert(A.getNumRows() == A.getNumCols());
57 : if (eps < 0)
58 : return false;
59 : int const m = A.getNumRows();
60 : int const n = RHS.getNumCols();
61 : // row permutations
62 : std::vector<int> perm(m);
63 : for (int iRow = 0; iRow < m; ++iRow)
64 : perm[iRow] = iRow;
65 :
66 : for (int step = 0; step < m - 1; ++step)
67 : {
68 : // determine pivot element
69 : int iMax = step;
70 : double dMax = std::abs(A(step, step));
71 : for (int i = step + 1; i < m; ++i)
72 : {
73 : if (std::abs(A(i, step)) > dMax)
74 : {
75 : iMax = i;
76 : dMax = std::abs(A(i, step));
77 : }
78 : }
79 : if (dMax <= eps)
80 : {
81 : CPLError(CE_Failure, CPLE_AppDefined,
82 : "GDALLinearSystemSolve: matrix not invertible");
83 : return false;
84 : }
85 : // swap rows
86 : if (iMax != step)
87 : {
88 : std::swap(perm[iMax], perm[step]);
89 : for (int iCol = 0; iCol < m; ++iCol)
90 : {
91 : std::swap(A(iMax, iCol), A(step, iCol));
92 : }
93 : }
94 : for (int iRow = step + 1; iRow < m; ++iRow)
95 : {
96 : A(iRow, step) /= A(step, step);
97 : }
98 : for (int iCol = step + 1; iCol < m; ++iCol)
99 : {
100 : for (int iRow = step + 1; iRow < m; ++iRow)
101 : {
102 : A(iRow, iCol) -= A(iRow, step) * A(step, iCol);
103 : }
104 : }
105 : }
106 :
107 : // LUP solve;
108 : for (int iCol = 0; iCol < n; ++iCol)
109 : {
110 : for (int iRow = 0; iRow < m; ++iRow)
111 : {
112 : X(iRow, iCol) = RHS(perm[iRow], iCol);
113 : for (int k = 0; k < iRow; ++k)
114 : {
115 : X(iRow, iCol) -= A(iRow, k) * X(k, iCol);
116 : }
117 : }
118 : for (int iRow = m - 1; iRow >= 0; --iRow)
119 : {
120 : for (int k = iRow + 1; k < m; ++k)
121 : {
122 : X(iRow, iCol) -= A(iRow, k) * X(k, iCol);
123 : }
124 : X(iRow, iCol) /= A(iRow, iRow);
125 : }
126 : }
127 : return true;
128 : }
129 : } // namespace
130 : #endif
131 : /************************************************************************/
132 : /* GDALLinearSystemSolve() */
133 : /* */
134 : /* Solves the linear system A*X_i = RHS_i for each column i */
135 : /* where A is a square matrix. */
136 : /************************************************************************/
137 41 : bool GDALLinearSystemSolve(GDALMatrix &A, GDALMatrix &RHS, GDALMatrix &X)
138 : {
139 41 : assert(A.getNumRows() == RHS.getNumRows());
140 41 : assert(A.getNumCols() == X.getNumRows());
141 41 : assert(RHS.getNumCols() == X.getNumCols());
142 : try
143 : {
144 : #ifdef HAVE_ARMADILLO
145 82 : arma::mat matA(A.data(), A.getNumRows(), A.getNumCols(), false, true);
146 41 : arma::mat matRHS(RHS.data(), RHS.getNumRows(), RHS.getNumCols(), false,
147 82 : true);
148 41 : arma::mat matOut(X.data(), X.getNumRows(), X.getNumCols(), false, true);
149 : #if ARMA_VERSION_MAJOR > 6 || \
150 : (ARMA_VERSION_MAJOR == 6 && ARMA_VERSION_MINOR >= 500)
151 : // Perhaps available in earlier versions, but didn't check
152 41 : return arma::solve(matOut, matA, matRHS,
153 41 : arma::solve_opts::equilibrate +
154 82 : arma::solve_opts::no_approx);
155 : #else
156 : return arma::solve(matOut, matA, matRHS);
157 : #endif
158 :
159 : #else // HAVE_ARMADILLO
160 : return solve(A, RHS, X, 0);
161 : #endif
162 : }
163 0 : catch (std::exception const &e)
164 : {
165 0 : CPLError(CE_Failure, CPLE_AppDefined, "GDALLinearSystemSolve: %s",
166 0 : e.what());
167 0 : return false;
168 : }
169 : }
170 :
171 : /*! @endcond */
|