choleski.cc

   1 #include "choleski.hh"
   2
   3 Vector
   4 Choleski_decomposition::solve(Vector rhs)const
   5 {
   6     int n= rhs.dim();
   7     assert(n == L.dim());
   8     Vector y(n);
   9
  10     // forward substitution
  11     for (int i=0; i < n; i++) {
  12         Real sum(0.0);
  13         for (int j=0; j < i; j++)
  14             sum += y(j) * L(i,j);
  15         y(i) = (rhs(i) - sum)/L(i,i);
  16     }
  17     for (int i=0; i < n; i++) {
  18         assert(D(i));
  19         y(i) /= D(i);
  20     }
  21
  22     // backward subst
  23     Vector x(n);
  24     for (int i=n-1; i >= 0; i--) {
  25         Real sum(0.0);
  26         for (int j=i+1; j < n; j++)
  27             sum += L(j,i)*x(j);
  28         x(i) = (y(i) - sum)/L(i,i);
  29     }
  30     return x;
  31 }
  32
  33 /*
  34   Standard matrix algorithm.
  35   */
  36
  37 Choleski_decomposition::Choleski_decomposition(Matrix P)
  38     : L(P.dim()), D(P.dim())
  39 {
  40     int n = P.dim();
  41     assert((P-P.transposed()).norm() < EPS);
  42     L.unit();
  43     for (int k= 0; k < n; k++) {
  44         for (int j = 0; j < k; j++){
  45             Real sum(0.0);
  46             for (int l=0; l < j; l++)
  47                 sum += L(k,l)*L(j,l)*D(l);
  48             L(k,j) = (P(k,j) - sum)/D(j);
  49         }
  50         Real sum=0.0;
  51
  52         for (int l=0; l < k; l++)
  53             sum += sqr(L(k,l))*D(l);
  54         Real d = P(k,k) - sum;
  55         D(k) = d;
  56     }
  57
  58 #ifdef NDEBUG
  59     assert((original()-P).norm() < EPS);
  60 #endif
  61 }
  62
  63 Matrix
  64 Choleski_decomposition::original() const
  65 {
  66     Matrix T(L.dim());
  67     T.set_diag(D);
  68     return L*T*L.transposed();
  69 }
  70
  71 Matrix
  72 Choleski_decomposition::inverse() const
  73 {
  74     int n=L.dim();
  75     Matrix invm(n);
  76     Vector e_i(n);
  77     for (int i = 0; i < n; i++) {
  78         e_i.set_unit(i);
  79         Vector inv(solve(e_i));
  80         for (int j = 0 ; j<n; j++)
  81             invm(i,j) = inv(j);
  82     }
  83
  84 #ifdef NDEBUG
  85     Matrix I1(n), I2(original());
  86     I1.unit();
  87     assert((I1-original()*invm).norm() < EPS);
  88 #endif
  89
  90     return invm;
  91 }
  92
  93
  94
  95