00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
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
00047 template<typename SquareMatrix>
00048 void LUDecomp(SquareMatrix& mat, int num_rows, int* perm, bool& parity)
00049
00050 {
00051 parity = true;
00052 const int num_cols = num_rows;
00053
00054
00055 for (int k = 0; k < num_rows; k++)
00056 perm[k] = k;
00057
00058
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;
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
00075 for (int j = 0; j < num_cols; ++j) {
00076
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
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
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
00117 pivot_val = double(1) / mat[j][j];
00118 for (int i = j+1; i < num_rows; ++i) {
00119 mat[i][j] *= pivot_val;
00120 }
00121 }
00122 }
00123 }
00124
00125
00126
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
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 };
00209
00210 #endif // _LUDECOMP_IMPLEMENTATION_H
00211
00212