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     Vector x(n);
  22     for (int i=n-1; i >= 0; i--) {
  23         Real sum(0.0);
  24         for (int j=i+1; j < n; j++)
  25             sum += L(j,i)*x(j);
  26         x(i) = (y(i) - sum)/L(i,i);
  27     }
  28     return x;
  29 }
  30
  31 Choleski_decomposition::Choleski_decomposition(Matrix P)
  32     : L(P.dim()), D(P.dim())
  33 {
  34     int n = P.dim();
  35     assert((P-P.transposed()).norm() < EPS);
  36     L.unit();
  37     for (int k= 0; k < n; k++) {
  38         for (int j = 0; j < k; j++){
  39             Real sum(0.0);
  40             for (int l=0; l < j; l++)
  41                 sum += L(k,l)*L(j,l)*D(l);
  42             L(k,j) = (P(k,j) - sum)/D(j);
  43         }
  44         Real sum=0.0;
  45
  46         for (int l=0; l < k; l++)
  47             sum += sqr(L(k,l))*D(l);
  48         Real d = P(k,k) - sum;
  49         D(k) = d;
  50     }
  51
  52     #ifdef NDEBUG
  53     assert((original()-P).norm() < EPS);
  54     #endif
  55 }
  56
  57 Matrix
  58 Choleski_decomposition::original() const
  59 {
  60     Matrix T(L.dim());
  61     T.set_diag(D);
  62     return L*T*L.transposed();
  63 }
  64
  65 Matrix
  66 Choleski_decomposition::inverse() const
  67 {
  68     int n=L.dim();
  69     Matrix invm(n);
  70     Vector e_i(n);
  71     for (int i = 0; i < n; i++) {
  72         e_i.set_unit(i);
  73         Vector inv(solve(e_i));
  74         for (int j = 0 ; j<n; j++)
  75             invm(i,j) = inv(j);
  76     }
  77
  78 #ifdef NDEBUG
  79     Matrix I1(n), I2(original());
  80     I1.unit();
  81     assert((I1-original()*invm).norm() < EPS);
  82     #endif
  83
  84     return invm;
  85 }