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 : * SPDX-License-Identifier: MIT
16 : ****************************************************************************/
17 :
18 : /*! @cond Doxygen_Suppress */
19 :
20 : #include "cpl_port.h"
21 : #include "cpl_conv.h"
22 : #include "gdallinearsystem.h"
23 :
24 : #ifdef HAVE_ARMADILLO
25 : #include "armadillo_headers.h"
26 : #endif
27 :
28 : #include <cstdio>
29 : #include <algorithm>
30 : #include <cassert>
31 : #include <cmath>
32 :
33 : #ifndef HAVE_ARMADILLO
34 : namespace
35 : {
36 : // LU decomposition of the quadratic matrix A
37 : // see https://en.wikipedia.org/wiki/LU_decomposition#C_code_examples
38 : bool solve(GDALMatrix &A, GDALMatrix &RHS, GDALMatrix &X, double eps)
39 : {
40 : assert(A.getNumRows() == A.getNumCols());
41 : if (eps < 0)
42 : return false;
43 : int const m = A.getNumRows();
44 : int const n = RHS.getNumCols();
45 : // row permutations
46 : std::vector<int> perm(m);
47 : for (int iRow = 0; iRow < m; ++iRow)
48 : perm[iRow] = iRow;
49 :
50 : for (int step = 0; step < m - 1; ++step)
51 : {
52 : // determine pivot element
53 : int iMax = step;
54 : double dMax = std::abs(A(step, step));
55 : for (int i = step + 1; i < m; ++i)
56 : {
57 : if (std::abs(A(i, step)) > dMax)
58 : {
59 : iMax = i;
60 : dMax = std::abs(A(i, step));
61 : }
62 : }
63 : if (dMax <= eps)
64 : {
65 : CPLError(CE_Failure, CPLE_AppDefined,
66 : "GDALLinearSystemSolve: matrix not invertible");
67 : return false;
68 : }
69 : // swap rows
70 : if (iMax != step)
71 : {
72 : std::swap(perm[iMax], perm[step]);
73 : for (int iCol = 0; iCol < m; ++iCol)
74 : {
75 : std::swap(A(iMax, iCol), A(step, iCol));
76 : }
77 : }
78 : for (int iRow = step + 1; iRow < m; ++iRow)
79 : {
80 : A(iRow, step) /= A(step, step);
81 : }
82 : for (int iCol = step + 1; iCol < m; ++iCol)
83 : {
84 : for (int iRow = step + 1; iRow < m; ++iRow)
85 : {
86 : A(iRow, iCol) -= A(iRow, step) * A(step, iCol);
87 : }
88 : }
89 : }
90 :
91 : // LUP solve;
92 : for (int iCol = 0; iCol < n; ++iCol)
93 : {
94 : for (int iRow = 0; iRow < m; ++iRow)
95 : {
96 : X(iRow, iCol) = RHS(perm[iRow], iCol);
97 : for (int k = 0; k < iRow; ++k)
98 : {
99 : X(iRow, iCol) -= A(iRow, k) * X(k, iCol);
100 : }
101 : }
102 : for (int iRow = m - 1; iRow >= 0; --iRow)
103 : {
104 : for (int k = iRow + 1; k < m; ++k)
105 : {
106 : X(iRow, iCol) -= A(iRow, k) * X(k, iCol);
107 : }
108 : X(iRow, iCol) /= A(iRow, iRow);
109 : }
110 : }
111 : return true;
112 : }
113 : } // namespace
114 : #endif
115 : /************************************************************************/
116 : /* GDALLinearSystemSolve() */
117 : /* */
118 : /* Solves the linear system A*X_i = RHS_i for each column i */
119 : /* where A is a square matrix. */
120 : /************************************************************************/
121 41 : bool GDALLinearSystemSolve(GDALMatrix &A, GDALMatrix &RHS, GDALMatrix &X)
122 : {
123 41 : assert(A.getNumRows() == RHS.getNumRows());
124 41 : assert(A.getNumCols() == X.getNumRows());
125 41 : assert(RHS.getNumCols() == X.getNumCols());
126 : try
127 : {
128 : #ifdef HAVE_ARMADILLO
129 82 : arma::mat matA(A.data(), A.getNumRows(), A.getNumCols(), false, true);
130 41 : arma::mat matRHS(RHS.data(), RHS.getNumRows(), RHS.getNumCols(), false,
131 82 : true);
132 41 : arma::mat matOut(X.data(), X.getNumRows(), X.getNumCols(), false, true);
133 : #if ARMA_VERSION_MAJOR > 6 || \
134 : (ARMA_VERSION_MAJOR == 6 && ARMA_VERSION_MINOR >= 500)
135 : // Perhaps available in earlier versions, but didn't check
136 41 : return arma::solve(matOut, matA, matRHS,
137 41 : arma::solve_opts::equilibrate +
138 82 : arma::solve_opts::no_approx);
139 : #else
140 : return arma::solve(matOut, matA, matRHS);
141 : #endif
142 :
143 : #else // HAVE_ARMADILLO
144 : return solve(A, RHS, X, 0);
145 : #endif
146 : }
147 0 : catch (std::exception const &e)
148 : {
149 0 : CPLError(CE_Failure, CPLE_AppDefined, "GDALLinearSystemSolve: %s",
150 0 : e.what());
151 0 : return false;
152 : }
153 : }
154 :
155 : /*! @endcond */
|
