lilypond-0.0.1
[lilypond.git] / choleski.cc
blobdb3fe82d60e370eb7bd7a6e06b749dcb776f4a6b
1 #include "choleski.hh"
3 Vector
4 Choleski_decomposition::solve(Vector rhs)const
6 int n= rhs.dim();
7 assert(n == L.dim());
8 Vector y(n);
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);
17 for (int i=0; i < n; i++) {
18 assert(D(i));
19 y(i) /= D(i);
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);
28 return x;
31 Choleski_decomposition::Choleski_decomposition(Matrix P)
32 : L(P.dim()), D(P.dim())
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);
44 Real sum=0.0;
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;
52 #ifdef NDEBUG
53 assert((original()-P).norm() < EPS);
54 #endif
57 Matrix
58 Choleski_decomposition::original() const
60 Matrix T(L.dim());
61 T.set_diag(D);
62 return L*T*L.transposed();
65 Matrix
66 Choleski_decomposition::inverse() const
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);
78 #ifdef NDEBUG
79 Matrix I1(n), I2(original());
80 I1.unit();
81 assert((I1-original()*invm).norm() < EPS);
82 #endif
84 return invm;