/src/gdal/alg/gdallinearsystem.cpp
Line | Count | Source (jump to first uncovered line) |
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 | 0 | { |
40 | 0 | assert(A.getNumRows() == A.getNumCols()); |
41 | 0 | if (eps < 0) |
42 | 0 | return false; |
43 | 0 | int const m = A.getNumRows(); |
44 | 0 | int const n = RHS.getNumCols(); |
45 | | // row permutations |
46 | 0 | std::vector<int> perm(m); |
47 | 0 | for (int iRow = 0; iRow < m; ++iRow) |
48 | 0 | perm[iRow] = iRow; |
49 | |
|
50 | 0 | for (int step = 0; step < m - 1; ++step) |
51 | 0 | { |
52 | | // determine pivot element |
53 | 0 | int iMax = step; |
54 | 0 | double dMax = std::abs(A(step, step)); |
55 | 0 | for (int i = step + 1; i < m; ++i) |
56 | 0 | { |
57 | 0 | if (std::abs(A(i, step)) > dMax) |
58 | 0 | { |
59 | 0 | iMax = i; |
60 | 0 | dMax = std::abs(A(i, step)); |
61 | 0 | } |
62 | 0 | } |
63 | 0 | if (dMax <= eps) |
64 | 0 | { |
65 | 0 | CPLError(CE_Failure, CPLE_AppDefined, |
66 | 0 | "GDALLinearSystemSolve: matrix not invertible"); |
67 | 0 | return false; |
68 | 0 | } |
69 | | // swap rows |
70 | 0 | if (iMax != step) |
71 | 0 | { |
72 | 0 | std::swap(perm[iMax], perm[step]); |
73 | 0 | for (int iCol = 0; iCol < m; ++iCol) |
74 | 0 | { |
75 | 0 | std::swap(A(iMax, iCol), A(step, iCol)); |
76 | 0 | } |
77 | 0 | } |
78 | 0 | for (int iRow = step + 1; iRow < m; ++iRow) |
79 | 0 | { |
80 | 0 | A(iRow, step) /= A(step, step); |
81 | 0 | } |
82 | 0 | for (int iCol = step + 1; iCol < m; ++iCol) |
83 | 0 | { |
84 | 0 | for (int iRow = step + 1; iRow < m; ++iRow) |
85 | 0 | { |
86 | 0 | A(iRow, iCol) -= A(iRow, step) * A(step, iCol); |
87 | 0 | } |
88 | 0 | } |
89 | 0 | } |
90 | | |
91 | | // LUP solve; |
92 | 0 | for (int iCol = 0; iCol < n; ++iCol) |
93 | 0 | { |
94 | 0 | for (int iRow = 0; iRow < m; ++iRow) |
95 | 0 | { |
96 | 0 | X(iRow, iCol) = RHS(perm[iRow], iCol); |
97 | 0 | for (int k = 0; k < iRow; ++k) |
98 | 0 | { |
99 | 0 | X(iRow, iCol) -= A(iRow, k) * X(k, iCol); |
100 | 0 | } |
101 | 0 | } |
102 | 0 | for (int iRow = m - 1; iRow >= 0; --iRow) |
103 | 0 | { |
104 | 0 | for (int k = iRow + 1; k < m; ++k) |
105 | 0 | { |
106 | 0 | X(iRow, iCol) -= A(iRow, k) * X(k, iCol); |
107 | 0 | } |
108 | 0 | X(iRow, iCol) /= A(iRow, iRow); |
109 | 0 | } |
110 | 0 | } |
111 | 0 | return true; |
112 | 0 | } |
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 | | bool GDALLinearSystemSolve(GDALMatrix &A, GDALMatrix &RHS, GDALMatrix &X) |
122 | 0 | { |
123 | 0 | assert(A.getNumRows() == RHS.getNumRows()); |
124 | 0 | assert(A.getNumCols() == X.getNumRows()); |
125 | 0 | assert(RHS.getNumCols() == X.getNumCols()); |
126 | 0 | try |
127 | 0 | { |
128 | | #ifdef HAVE_ARMADILLO |
129 | | arma::mat matA(A.data(), A.getNumRows(), A.getNumCols(), false, true); |
130 | | arma::mat matRHS(RHS.data(), RHS.getNumRows(), RHS.getNumCols(), false, |
131 | | true); |
132 | | 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 | | return arma::solve(matOut, matA, matRHS, |
137 | | arma::solve_opts::equilibrate + |
138 | | arma::solve_opts::no_approx); |
139 | | #else |
140 | | return arma::solve(matOut, matA, matRHS); |
141 | | #endif |
142 | | |
143 | | #else // HAVE_ARMADILLO |
144 | 0 | return solve(A, RHS, X, 0); |
145 | 0 | #endif |
146 | 0 | } |
147 | 0 | catch (std::exception const &e) |
148 | 0 | { |
149 | 0 | CPLError(CE_Failure, CPLE_AppDefined, "GDALLinearSystemSolve: %s", |
150 | 0 | e.what()); |
151 | 0 | return false; |
152 | 0 | } |
153 | 0 | } |
154 | | |
155 | | /*! @endcond */ |