flower/choleski.cc

   1 /*
   2   choleski.cc -- implement Choleski_decomposition
   3
   4   source file of the Flower Library
   5
   6   (c) 1997 Han-Wen Nienhuys <hanwen@stack.nl>
   7 */
   8
   9 #include "choleski.hh"
  10 const Real EPS = 1e-7;          // so sue me. Hard coded
  11
  12 // for testing new Matrix_storage.
  13 //#define PARANOID
  14
  15 Vector
  16 Choleski_decomposition::solve(Vector rhs)const
  17 {
  18     int n= rhs.dim();
  19     assert(n == L.dim());
  20     Vector y(n);
  21
  22     // forward substitution
  23     for (int i=0; i < n; i++) {
  24         Real sum(0.0);
  25         for (int j=0; j < i; j++)
  26             sum += y(j) * L(i,j);
  27         y(i) = (rhs(i) - sum)/L(i,i);
  28     }
  29     for (int i=0; i < n; i++)
  30         y(i) /= D(i);
  31
  32     // backward subst
  33     Vector &x(rhs);             // using input as return val.
  34     for (int i=n-1; i >= 0; i--) {
  35         Real sum(0.0);
  36         for (int j=i+1; j < n; j++)
  37             sum += L(j,i)*x(j);
  38         x(i) = (y(i) - sum)/L(i,i);
  39     }
  40     return x;
  41 }
  42
  43 void
  44 Choleski_decomposition::full_matrix_decompose(Matrix const & P)
  45 {
  46
  47    int n = P.dim();
  48     L.unit();
  49     for (int k= 0; k < n; k++) {
  50         for (int j = 0; j < k; j++){
  51             Real sum(0.0);
  52             for (int l=0; l < j; l++)
  53                 sum += L(k,l)*L(j,l)*D(l);
  54             L(k,j) = (P(k,j) - sum)/D(j);
  55         }
  56         Real sum=0.0;
  57
  58         for (int l=0; l < k; l++)
  59             sum += sqr(L(k,l))*D(l);
  60         Real d = P(k,k) - sum;
  61         D(k) = d;
  62     }
  63
  64 }
  65
  66 void
  67 Choleski_decomposition::band_matrix_decompose(Matrix const &P)
  68 {
  69     int n = P.dim();
  70     int b = P.band_i();
  71     L.unit();
  72
  73     for (int i= 0; i < n; i++) {
  74         for (int j = 0 >? i - b; j < i; j++){
  75             Real sum(0.0);
  76             for (int l=0 >? i - b; l < j; l++)
  77                 sum += L(i,l)*L(j,l)*D(l);
  78             L(i,j) = (P(i,j) - sum)/D(j);
  79         }
  80         Real sum=0.0;
  81
  82         for (int l=0 >? i - b; l < i; l++)
  83             sum += sqr(L(i,l))*D(l);
  84         Real d = P(i,i) - sum;
  85         D(i) = d;
  86     }
  87     L.try_set_band();
  88     assert ( L.band_i() == P.band_i());
  89 }
  90
  91
  92
  93
  94 /*
  95   Standard matrix algorithm.
  96    */
  97
  98 Choleski_decomposition::Choleski_decomposition(Matrix const & P)
  99    : L(P.dim()), D(P.dim())
 100 {
 101 #ifdef PARANOID
 102     assert((P-P.transposed()).norm()/P.norm() < EPS);
 103 #endif
 104     if  (P.band_b())
 105         band_matrix_decompose(P);
 106     else
 107         full_matrix_decompose(P);
 108
 109
 110 #ifdef PARANOID
 111     assert((original()-P).norm() / P.norm() < EPS);
 112 #endif
 113 }
 114
 115 Matrix
 116 Choleski_decomposition::original() const
 117 {
 118     Matrix T(L.dim());
 119     T.set_diag(D);
 120     return L*T*L.transposed();
 121 }
 122
 123 Matrix
 124 Choleski_decomposition::inverse() const
 125 {
 126     int n=L.dim();
 127     Matrix invm(n);
 128     Vector e_i(n);
 129     for (int i = 0; i < n; i++) {
 130         e_i.set_unit(i);
 131         Vector inv(solve(e_i));
 132         for (int j = 0 ; j<n; j++)
 133             invm(i,j) = inv(j);
 134     }
 135
 136 #ifdef PARANOID
 137     Matrix I1(n), I2(original());
 138     I1.unit();
 139     assert((I1-I2*invm).norm()/I2.norm() < EPS);
 140 #endif
 141
 142     return invm;
 143 }