1 | |
---|
2 | #include "chmat.h" |
---|
3 | |
---|
4 | //using std::endl; |
---|
5 | |
---|
6 | namespace bdm { |
---|
7 | |
---|
8 | void chmat::add ( const chmat &A2, double w ) { |
---|
9 | bdm_assert_debug ( dim == A2.dim, "Matrices of unequal dimension" ); |
---|
10 | mat pre = concat_vertical ( Ch, sqrt ( w ) * A2.Ch ); |
---|
11 | mat post = zeros ( pre.rows(), pre.cols() ); |
---|
12 | if ( !qr ( pre, post ) ) { |
---|
13 | bdm_warning ( "Unstable QR in chmat add" ); |
---|
14 | } |
---|
15 | Ch = post ( 0, dim - 1, 0, dim - 1 ); |
---|
16 | }; |
---|
17 | |
---|
18 | void chmat::opupdt ( const vec &v, double w ) { |
---|
19 | //TODO see cholupdt in lhotse |
---|
20 | mat Z; |
---|
21 | mat R; |
---|
22 | mat V ( 1, v.length() ); |
---|
23 | V.set_row ( 0, v*sqrt ( w ) ); |
---|
24 | Z = concat_vertical ( Ch, V ); |
---|
25 | qr ( Z, R ); |
---|
26 | Ch = R ( 0, Ch.rows() - 1, 0, Ch.cols() - 1 ); |
---|
27 | } |
---|
28 | |
---|
29 | mat chmat::to_mat() const { |
---|
30 | mat F = Ch.T() * Ch; |
---|
31 | return F; |
---|
32 | } |
---|
33 | |
---|
34 | void chmat::mult_sym ( const mat &C ) { |
---|
35 | bdm_assert_debug ( C.rows() == dim, "Wrong dimension of U" ); |
---|
36 | if ( !qr ( Ch*C.T(), Ch ) ) { |
---|
37 | bdm_warning ( "QR unstable in chmat mult_sym" ); |
---|
38 | } |
---|
39 | } |
---|
40 | |
---|
41 | void chmat::mult_sym ( const mat &C , chmat &U ) const { |
---|
42 | bdm_assert_debug ( C.rows() == U.dim, "Wrong dimension of U" ); |
---|
43 | if ( !qr ( Ch*C.T(), U.Ch ) ) { |
---|
44 | bdm_warning ( "QR unstable in chmat mult_sym" ); |
---|
45 | } |
---|
46 | } |
---|
47 | |
---|
48 | void chmat::mult_sym_t ( const mat &C ) { |
---|
49 | bdm_assert_debug ( C.cols() == dim, "Wrong dimension of U" ); |
---|
50 | if ( !qr ( Ch*C, Ch ) ) { |
---|
51 | bdm_warning ( "QR unstable in chmat mult_sym" ); |
---|
52 | } |
---|
53 | } |
---|
54 | |
---|
55 | void chmat::mult_sym_t ( const mat &C, chmat &U ) const { |
---|
56 | bdm_assert_debug ( C.cols() == U.dim, "Wrong dimension of U" ); |
---|
57 | if ( !qr ( Ch*C, U.Ch ) ) { |
---|
58 | bdm_warning ( "QR unstable in chmat mult_sym" ); |
---|
59 | } |
---|
60 | } |
---|
61 | |
---|
62 | double chmat::logdet() const { |
---|
63 | double ldet = 0.0; |
---|
64 | int i; |
---|
65 | //sum of logs of (possibly negative!) diagonal entries |
---|
66 | for ( i = 0; i < Ch.rows(); i++ ) { |
---|
67 | ldet += log ( std::fabs ( Ch ( i, i ) ) ); |
---|
68 | } |
---|
69 | return 2*ldet; //compensate for Ch being sqrt() |
---|
70 | } |
---|
71 | |
---|
72 | //TODO can be done more efficiently using BLAS, see triangular matrices |
---|
73 | vec chmat::sqrt_mult ( const vec &v ) const { |
---|
74 | vec pom; |
---|
75 | pom = Ch.T() * v; |
---|
76 | return pom; |
---|
77 | } |
---|
78 | |
---|
79 | double chmat::qform ( const vec &v ) const { |
---|
80 | vec pom; |
---|
81 | pom = Ch * v; |
---|
82 | return pom*pom; |
---|
83 | } |
---|
84 | |
---|
85 | double chmat::invqform ( const vec &v ) const { |
---|
86 | vec pom ( v.length() ); |
---|
87 | forward_substitution ( Ch.T(), v, pom ); |
---|
88 | return pom*pom; |
---|
89 | } |
---|
90 | |
---|
91 | void chmat::clear() { |
---|
92 | Ch.clear(); |
---|
93 | } |
---|
94 | |
---|
95 | } |
---|