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