LUDecomp_implementation.h

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

Generated on Mon Jun 11 14:48:18 2007 for GoTools Core Library by  doxygen 1.5.1