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 void
  16 Choleski_decomposition::full_matrix_solve(Vector &out, Vector const &rhs)const
  17 {
  18     int n= rhs.dim();
  19     assert(n == L.dim());
  20     Vector y;
  21     y.set_dim( n);
  22     out.set_dim(n);
  23
  24     // forward substitution
  25     for (int i=0; i < n; i++) {
  26         Real sum(0.0);
  27         for (int j=0; j < i; j++)
  28             sum += y(j) * L(i,j);
  29         y(i) = (rhs(i) - sum)/L(i,i);
  30     }
  31
  32     for (int i=0; i < n; i++)
  33         y(i) /= D(i);
  34
  35     // backward subst
  36     Vector &x(out);             // using input as return val.
  37     for (int i=n-1; i >= 0; i--) {
  38         Real sum(0.0);
  39         for (int j=i+1; j < n; j++)
  40             sum += L(j,i)*x(j);
  41         x(i) = (y(i) - sum)/L(i,i);
  42     }
  43 }
  44
  45 void
  46 Choleski_decomposition::band_matrix_solve(Vector &out, Vector const &rhs)const
  47 {
  48     int n= rhs.dim();
  49     int b = L.band_i();
  50     assert(n == L.dim());
  51
  52       out.set_dim(n);
  53
  54     Vector y;
  55     y.set_dim(n);
  56
  57     // forward substitution
  58     for (int i=0; i < n; i++) {
  59         Real sum(0.0);
  60         for (int j= 0 >? i - b; j < i; j++)
  61             sum += y(j) * L(i,j);
  62         y(i) = (rhs(i) - sum)/L(i,i);
  63     }
  64     for (int i=0; i < n; i++)
  65         y(i) /= D(i);
  66
  67     // backward subst
  68     Vector &x(out);             // using input as return val.
  69     for (int i=n-1; i >= 0; i--) {
  70         Real sum(0.0);
  71         for (int j=i+1; j <= i + b&&j < n ; j++)
  72             sum += L(j,i)*x(j);
  73         x(i) = (y(i) - sum)/L(i,i);
  74     }
  75 }
  76
  77 void
  78 Choleski_decomposition::solve(Vector &x, Vector const &rhs)const
  79 {
  80     if (L.band_b()) {
  81         band_matrix_solve(x,rhs);
  82     } else
  83         full_matrix_solve(x,rhs);
  84 }
  85
  86 Vector
  87 Choleski_decomposition::solve(Vector rhs)const
  88 {
  89     Vector r;
  90     solve(r, rhs);
  91     return r;
  92 }
  93
  94 void
  95 Choleski_decomposition::full_matrix_decompose(Matrix const & P)
  96 {
  97
  98    int n = P.dim();
  99     L.unit();
 100     for (int k= 0; k < n; k++) {
 101         for (int j = 0; j < k; j++){
 102             Real sum(0.0);
 103             for (int l=0; l < j; l++)
 104                 sum += L(k,l)*L(j,l)*D(l);
 105             L(k,j) = (P(k,j) - sum)/D(j);
 106         }
 107         Real sum=0.0;
 108
 109         for (int l=0; l < k; l++)
 110             sum += sqr(L(k,l))*D(l);
 111         Real d = P(k,k) - sum;
 112         D(k) = d;
 113     }
 114
 115 }
 116
 117 void
 118 Choleski_decomposition::band_matrix_decompose(Matrix const &P)
 119 {
 120     int n = P.dim();
 121     int b = P.band_i();
 122     L.unit();
 123
 124     for (int i= 0; i < n; i++) {
 125         for (int j = 0 >? i - b; j < i; j++){
 126             Real sum(0.0);
 127             for (int l=0 >? i - b; l < j; l++)
 128                 sum += L(i,l)*L(j,l)*D(l);
 129             L(i,j) = (P(i,j) - sum)/D(j);
 130         }
 131         Real sum=0.0;
 132
 133         for (int l=0 >? i - b; l < i; l++)
 134             sum += sqr(L(i,l))*D(l);
 135         Real d = P(i,i) - sum;
 136         D(i) = d;
 137     }
 138     L.try_set_band();
 139     assert ( L.band_i() == P.band_i());
 140 }
 141
 142
 143
 144
 145 /*
 146   Standard matrix algorithm.
 147    */
 148
 149 Choleski_decomposition::Choleski_decomposition(Matrix const & P)
 150    : L(P.dim()), D(P.dim())
 151 {
 152 #ifdef PARANOID
 153     assert((P-P.transposed()).norm()/P.norm() < EPS);
 154 #endif
 155     if  (P.band_b())
 156         band_matrix_decompose(P);
 157     else
 158         full_matrix_decompose(P);
 159
 160
 161 #ifdef PARANOID
 162     assert((original()-P).norm() / P.norm() < EPS);
 163 #endif
 164 }
 165
 166 Matrix
 167 Choleski_decomposition::original() const
 168 {
 169     Matrix T(L.dim());
 170     T.set_diag(D);
 171     return L*T*L.transposed();
 172 }
 173
 174 Matrix
 175 Choleski_decomposition::inverse() const
 176 {
 177     int n=L.dim();
 178     Matrix invm(n);
 179     Vector e_i(n);
 180     Vector inv(n);
 181     for (int i = 0; i < n; i++) {
 182         e_i.set_unit(i);
 183         solve(inv, e_i);
 184         for (int j = 0 ; j<n; j++)
 185             invm(i,j) = inv(j);
 186     }
 187
 188 #ifdef PARANOID
 189     Matrix I1(n), I2(original());
 190     I1.unit();
 191     assert((I1-I2*invm).norm()/I2.norm() < EPS);
 192 #endif
 193
 194     return invm;
 195 }