00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013 #ifndef CHOLESKYDECOMP_H
00014 #define CHOLESKYDECOMP_H
00015
00016 #include <iostream>
00017 #include <cassert>
00018 #include <cmath>
00019
00020
00021 namespace utilities {
00022 template<int ND>
00023 void choleskyDecomposition(double a[ND][ND], double p[ND]);
00024
00025 template <int ND>
00026 void choleskySolve(double a[ND][ND], double p[ND], double rhs[ND], double x[ND]);
00027 }
00028
00029
00030
00031 namespace utilities {
00032
00033 template <int ND>
00034 void choleskyDecomposition(double a[ND][ND], double p[ND])
00035 {
00036
00037
00038
00039
00040 double sum;
00041 for (int i=0; i<ND; ++i) {
00042 for (int j=i; j<ND; ++j) {
00043
00044 int k;
00045 for (sum=a[i][j], k=i-1; k>-1; --k) sum -= a[i][k]*a[j][k];
00046
00047 if (i==j) {
00048 if (sum<0.0) {
00049 std::cerr << "choleskyDecomposition non positive definite matrix."
00050 << std::endl;
00051 assert(false);
00052 }
00053 p[i] = std::sqrt(sum);
00054
00055 } else a[j][i] = sum/p[i];
00056 }
00057 }
00058 return;
00059 }
00060
00061
00062 template <int ND>
00063 void choleskySolve(double a[ND][ND], double p[ND], double rhs[ND], double x[ND])
00064 {
00065
00066
00067 double sum;
00068 int k;
00069
00070
00071 for (int i=0; i<ND; ++i) {
00072 for (sum=rhs[i], k=i-1; k>-1; --k) sum -= a[i][k]*x[k];
00073 x[i] = sum/p[i];
00074 }
00075
00076
00077 for (int i=(ND-1); i>-1; --i) {
00078 for (sum =x[i], k=i+1; k<ND; ++k) sum -= a[k][i]*x[k];
00079 x[i] = sum/p[i];
00080 }
00081 return;
00082 }
00083
00084 }
00085
00086 #endif