flower/choleski.cc

   1 /*
   2   choleski.cc -- implement Choleski_decomposition
   3
   4   source file of the Flower Library
   5
   6   (c)  1997--1999 Han-Wen Nienhuys <hanwen@cs.uu.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 (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.set_band();
 149   band_b_ = true;
 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   band_b_ = false;
 166
 167   int b = P.calc_band_i ();
 168
 169   if (b <= P.dim ()/2)
 170     band_matrix_decompose (P);
 171   else
 172     full_matrix_decompose (P);
 173
 174 #ifdef PARANOID
 175   assert ((original()-P).norm () / P.norm () < EPS);
 176 #endif
 177 }
 178
 179 Matrix
 180 Choleski_decomposition::original() const
 181 {
 182   Matrix T(L.dim());
 183   T.set_diag (D);
 184   return L*T*L.transposed();
 185 }
 186
 187 Matrix
 188 Choleski_decomposition::inverse() const
 189 {
 190   int n=L.dim();
 191   Matrix invm (n);
 192   Vector e_i (n);
 193   Vector inv (n);
 194   for (int i = 0; i < n; i++)
 195     {
 196       e_i.set_unit (i);
 197       solve (inv, e_i);
 198       for (int j = 0 ; j<n; j++)
 199         invm (i,j) = inv (j);
 200     }
 201
 202 #ifdef PARANOID
 203   Matrix I1(n), I2(original());
 204   I1.unit();
 205   assert ((I1-I2*invm).norm()/I2.norm () < EPS);
 206 #endif
 207
 208   return invm;
 209 }