//=========================================================================== // GoTools Core - SINTEF Geometry Tools Core library, version 2.0.1 // // Copyright (C) 2000-2007, 2010 SINTEF ICT, Applied Mathematics, Norway. // // This program is free software; you can redistribute it and/or // modify it under the terms of the GNU General Public License // as published by the Free Software Foundation version 2 of the License. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program; if not, write to the Free Software // Foundation, Inc., // 59 Temple Place - Suite 330, // Boston, MA 02111-1307, USA. // // Contact information: E-mail: tor.dokken@sintef.no // SINTEF ICT, Department of Applied Mathematics, // P.O. Box 124 Blindern, // 0314 Oslo, Norway. // // Other licenses are also available for this software, notably licenses // for: // - Building commercial software. // - Building software whose source code you wish to keep private. //=========================================================================== 00015 #ifndef _LUDECOMP_IMPLEMENTATION_H 00016 #define _LUDECOMP_IMPLEMENTATION_H 00017 00018 #include <vector> 00019 #include <cmath> 00020 00021 namespace Go 00022 { 00023 00024 //=========================================================================== 00025 // LU decomposition algorithm, based on Crout's algorithm 00026 template<typename SquareMatrix> 00027 void LUDecomp(SquareMatrix& mat, int num_rows, int* perm, bool& parity) 00028 //=========================================================================== 00029 { 00030 parity = true; // as for now, number of row transpositions is 0, evidently 00031 const int num_cols = num_rows; // for clarity (should be optimized away by compiler...) 00032 00033 // filling out perm with sequence 0, 1,... 00034 for (int k = 0; k < num_rows; k++) 00035 perm[k] = k; 00036 00037 // determining scaling factor of each row 00038 std::vector<double> scaling(num_rows, 0); 00039 for (int i = 0; i < num_rows; ++i) { 00040 for (int j = 0; j < num_cols; ++j) { 00041 double temp = fabs(mat[i][j]); 00042 if (temp > scaling[i]) { 00043 scaling[i] = temp; // scaling[i] is max. absolute elem on row i. 00044 } 00045 } 00046 if (scaling[i] == 0) { 00047 throw std::runtime_error("Unable to LU decompose matrix. Null row detected."); 00048 } else { 00049 scaling[i] = double(1) / scaling[i]; 00050 } 00051 } 00052 00053 // executing Crout's algorithm 00054 for (int j = 0; j < num_cols; ++j) { 00055 // determining elements of UPPER matrix on this column 00056 for (int i = 0; i < j; ++i) { 00057 double sum = mat[i][j]; 00058 for (int k = 0; k <= i-1; ++k) { 00059 sum -= mat[i][k] * mat[k][j]; 00060 } 00061 mat[i][j] = sum; 00062 } 00063 00064 // compute rest of this column, before division by pivot 00065 double pivot_val = 0; 00066 int pivot_row = j; 00067 for (int i = j; i < num_rows; ++i) { 00068 double sum = mat[i][j]; 00069 for (int k = 0; k <= j-1; ++k) { 00070 sum -= mat[i][k] * mat[k][j]; 00071 } 00072 mat[i][j] = sum; 00073 double temp = std::fabs(sum * scaling[i]); 00074 if (temp > pivot_val) { 00075 pivot_val = temp; 00076 pivot_row = i; 00077 } 00078 } 00079 00080 if (mat[pivot_row][j] == 0) { 00081 throw std::runtime_error("Unable to LU decompose singular matrix."); 00082 } 00083 00084 // permute rows to position pivot correctly 00085 if (pivot_row != j) { 00086 for (int k = 0; k < num_cols; ++k) { 00087 std::swap(mat[pivot_row][k], mat[j][k]); 00088 } 00089 parity = !parity; 00090 std::swap(scaling[j], scaling[pivot_row]); 00091 std::swap(perm[j], perm[pivot_row]); 00092 } 00093 00094 if (j < num_rows - 1) { 00095 // dividing LOWER matrix elements by pivot 00096 pivot_val = double(1) / mat[j][j]; // inverse value, without scaling 00097 for (int i = j+1; i < num_rows; ++i) { 00098 mat[i][j] *= pivot_val; 00099 } 00100 } 00101 } 00102 } 00103 00104 //=========================================================================== 00105 // Solve the system Ax = b for x, using LU decomposition of the matrix A. 00106 template<typename SquareMatrix, typename T> 00107 void LUsolveSystem(SquareMatrix& A, int num_unknowns, T* vec) 00108 //=========================================================================== 00109 { 00110 bool parity; 00111 std::vector<int> permutation(num_unknowns); 00112 00113 LUDecomp(A, num_unknowns, &permutation[0], parity); 00114 00115 // permuting b 00116 std::vector<T> vec_old(vec, vec + num_unknowns); 00117 for (int i = 0; i < num_unknowns; ++i) { 00118 swap(vec[i], vec_old[permutation[i]]); 00119 } 00120 forwardSubstitution(A, vec, num_unknowns); 00121 backwardSubstitution(A, vec, num_unknowns); 00122 } 00123 00124 //=========================================================================== 00125 template<typename SquareMatrix, typename T> 00126 void forwardSubstitution(const SquareMatrix& A, T* x, int num_unknowns) 00127 //=========================================================================== 00128 { 00129 for (int i = 1; i < num_unknowns; ++i) { 00130 for (int j = 0; j < i; ++j) { 00131 x[i] -= A[i][j] * x[j]; 00132 } 00133 } 00134 } 00135 00136 //=========================================================================== 00137 template<typename SquareMatrix> 00138 void forwardSubstitution(const SquareMatrix& A, std::vector<double>* x, int num_unknowns) 00139 //=========================================================================== 00140 { 00141 const int dim = int(x[0].size()); 00142 for (int i = 1; i < num_unknowns; ++i) { 00143 for (int j = 0; j < i; ++j) { 00144 for (int dd = 0; dd < dim; ++dd) { 00145 x[i][dd] -= A[i][j] * x[j][dd]; 00146 } 00147 } 00148 } 00149 } 00150 00151 //=========================================================================== 00152 template<typename SquareMatrix, typename T> 00153 void backwardSubstitution(const SquareMatrix& A, T* x, int num_unknowns) 00154 //=========================================================================== 00155 { 00156 x[num_unknowns-1] /= A[num_unknowns-1][num_unknowns-1]; 00157 for (int i = num_unknowns - 2; i >= 0; --i) { 00158 for (int j = i+1; j < num_unknowns; ++j) { 00159 x[i] -= A[i][j] * x[j]; 00160 } 00161 x[i] /= A[i][i]; 00162 } 00163 } 00164 00165 //=========================================================================== 00166 template<typename SquareMatrix> 00167 void backwardSubstitution(const SquareMatrix& A, std::vector<double>* x, int num_unknowns) 00168 //=========================================================================== 00169 { 00170 const int dim = int(x[0].size()); 00171 for (int dd = 0; dd < dim; ++dd) { 00172 x[num_unknowns-1][dd] /= A[num_unknowns-1][num_unknowns-1]; 00173 } 00174 for (int i = num_unknowns - 2; i >= 0; --i) { 00175 for (int j = i+1; j < num_unknowns; ++j) { 00176 for (int dd = 0; dd < dim; ++dd) { 00177 x[i][dd] -= A[i][j] * x[j][dd]; 00178 } 00179 } 00180 for (int dd = 0; dd < dim; ++dd) { 00181 x[i][dd] /= A[i][i]; 00182 } 00183 } 00184 } 00185 00186 }; // end namespace Go 00187 00188 #endif // _LUDECOMP_IMPLEMENTATION_H 00189