1 | |
---|
2 | #include "square_mat.h" |
---|
3 | |
---|
4 | namespace bdm { |
---|
5 | |
---|
6 | using namespace itpp; |
---|
7 | |
---|
8 | using std::endl; |
---|
9 | |
---|
10 | void fsqmat::opupdt ( const vec &v, double w ) { |
---|
11 | M += outer_product ( v, v * w ); |
---|
12 | }; |
---|
13 | mat fsqmat::to_mat() const { |
---|
14 | return M; |
---|
15 | }; |
---|
16 | void fsqmat::mult_sym ( const mat &C ) { |
---|
17 | M = C * M * C.T(); |
---|
18 | }; |
---|
19 | void fsqmat::mult_sym_t ( const mat &C ) { |
---|
20 | M = C.T() * M * C; |
---|
21 | }; |
---|
22 | void fsqmat::mult_sym ( const mat &C, fsqmat &U ) const { |
---|
23 | U.M = ( C * ( M * C.T() ) ); |
---|
24 | }; |
---|
25 | void fsqmat::mult_sym_t ( const mat &C, fsqmat &U ) const { |
---|
26 | U.M = ( C.T() * ( M * C ) ); |
---|
27 | }; |
---|
28 | void fsqmat::inv ( fsqmat &Inv ) const { |
---|
29 | mat IM = itpp::inv ( M ); |
---|
30 | Inv = IM; |
---|
31 | }; |
---|
32 | void fsqmat::clear() { |
---|
33 | M.clear(); |
---|
34 | }; |
---|
35 | fsqmat::fsqmat ( const mat &M0 ) : sqmat ( M0.cols() ) { |
---|
36 | bdm_assert_debug ( ( M0.cols() == M0.rows() ), "M0 must be square" ); |
---|
37 | M = M0; |
---|
38 | }; |
---|
39 | |
---|
40 | //fsqmat::fsqmat() {}; |
---|
41 | |
---|
42 | fsqmat::fsqmat ( const int dim0 ) : sqmat ( dim0 ), M ( dim0, dim0 ) {}; |
---|
43 | |
---|
44 | std::ostream &operator<< ( std::ostream &os, const fsqmat &ld ) { |
---|
45 | os << ld.M << endl; |
---|
46 | return os; |
---|
47 | } |
---|
48 | |
---|
49 | |
---|
50 | ldmat::ldmat ( const mat &exL, const vec &exD ) : sqmat ( exD.length() ) { |
---|
51 | D = exD; |
---|
52 | L = exL; |
---|
53 | } |
---|
54 | |
---|
55 | ldmat::ldmat() : sqmat ( 0 ) {} |
---|
56 | |
---|
57 | ldmat::ldmat ( const int dim0 ) : sqmat ( dim0 ), D ( dim0 ), L ( dim0, dim0 ) {} |
---|
58 | |
---|
59 | ldmat::ldmat ( const vec D0 ) : sqmat ( D0.length() ) { |
---|
60 | D = D0; |
---|
61 | L = eye ( dim ); |
---|
62 | } |
---|
63 | |
---|
64 | ldmat::ldmat ( const mat &V ) : sqmat ( V.cols() ) { |
---|
65 | |
---|
66 | bdm_assert_debug ( dim == V.rows(), "ldmat::ldmat matrix V is not square!" ); |
---|
67 | |
---|
68 | // L and D will be allocated by ldform() |
---|
69 | //Chol is unstable |
---|
70 | this->ldform ( chol ( V ), ones ( dim ) ); |
---|
71 | } |
---|
72 | |
---|
73 | void ldmat::opupdt ( const vec &v, double w ) { |
---|
74 | int dim = D.length(); |
---|
75 | double kr; |
---|
76 | vec r = v; |
---|
77 | //beware! it is potentionally dangerous, if ITpp change _behaviour of _data()! |
---|
78 | double *Lraw = L._data(); |
---|
79 | double *Draw = D._data(); |
---|
80 | double *rraw = r._data(); |
---|
81 | |
---|
82 | bdm_assert_debug ( v.length() == dim, "LD::ldupdt vector v is not compatible with this ld." ); |
---|
83 | |
---|
84 | for ( int i = dim - 1; i >= 0; i-- ) { |
---|
85 | dydr ( rraw, Lraw + i, &w, Draw + i, rraw + i, 0, i, &kr, 1, dim ); |
---|
86 | } |
---|
87 | } |
---|
88 | |
---|
89 | std::ostream &operator<< ( std::ostream &os, const ldmat &ld ) { |
---|
90 | os << "L:" << ld.L << endl; |
---|
91 | os << "D:" << ld.D << endl; |
---|
92 | return os; |
---|
93 | } |
---|
94 | |
---|
95 | mat ldmat::to_mat() const { |
---|
96 | int dim = D.length(); |
---|
97 | mat V ( dim, dim ); |
---|
98 | double sum; |
---|
99 | int r, c, cc; |
---|
100 | |
---|
101 | for ( r = 0; r < dim; r++ ) { //row cycle |
---|
102 | for ( c = r; c < dim; c++ ) { |
---|
103 | //column cycle, using symmetricity => c=r! |
---|
104 | sum = 0.0; |
---|
105 | for ( cc = c; cc < dim; cc++ ) { //cycle over the remaining part of the vector |
---|
106 | sum += L ( cc, r ) * D ( cc ) * L ( cc, c ); |
---|
107 | //here L(cc,r) = L(r,cc)'; |
---|
108 | } |
---|
109 | V ( r, c ) = sum; |
---|
110 | // symmetricity |
---|
111 | if ( r != c ) { |
---|
112 | V ( c, r ) = sum; |
---|
113 | }; |
---|
114 | } |
---|
115 | } |
---|
116 | //mat V2 = L.transpose() * diag ( D ) * L; |
---|
117 | return V; |
---|
118 | } |
---|
119 | |
---|
120 | |
---|
121 | void ldmat::add ( const ldmat &ld2, double w ) { |
---|
122 | int dim = D.length(); |
---|
123 | |
---|
124 | bdm_assert_debug ( ld2.D.length() == dim, "LD.add() incompatible sizes of LDs" ); |
---|
125 | |
---|
126 | //Fixme can be done more efficiently either via dydr or ldform |
---|
127 | for ( int r = 0; r < dim; r++ ) { |
---|
128 | // Add columns of ld2.L' (i.e. rows of ld2.L) as dyads weighted by ld2.D |
---|
129 | this->opupdt ( ld2.L.get_row ( r ), w*ld2.D ( r ) ); |
---|
130 | } |
---|
131 | } |
---|
132 | |
---|
133 | void ldmat::clear() { |
---|
134 | L.clear(); |
---|
135 | for ( int i = 0; i < L.cols(); i++ ) { |
---|
136 | L ( i, i ) = 1; |
---|
137 | }; |
---|
138 | D.clear(); |
---|
139 | } |
---|
140 | |
---|
141 | void ldmat::inv ( ldmat &Inv ) const { |
---|
142 | Inv.clear(); //Inv = zero in LD |
---|
143 | mat U = ltuinv ( L ); |
---|
144 | |
---|
145 | Inv.ldform ( U.transpose(), 1.0 / D ); |
---|
146 | } |
---|
147 | |
---|
148 | void ldmat::mult_sym ( const mat &C ) { |
---|
149 | mat A = L * C.T(); |
---|
150 | this->ldform ( A, D ); |
---|
151 | } |
---|
152 | |
---|
153 | void ldmat::mult_sym_t ( const mat &C ) { |
---|
154 | mat A = L * C; |
---|
155 | this->ldform ( A, D ); |
---|
156 | } |
---|
157 | |
---|
158 | void ldmat::mult_sym ( const mat &C, ldmat &U ) const { |
---|
159 | mat A = L * C.T(); //could be done more efficiently using BLAS |
---|
160 | U.ldform ( A, D ); |
---|
161 | } |
---|
162 | |
---|
163 | void ldmat::mult_sym_t ( const mat &C, ldmat &U ) const { |
---|
164 | mat A = L * C; |
---|
165 | /* vec nD=zeros(U.rows()); |
---|
166 | nD.replace_mid(0, D); //I case that D < nD*/ |
---|
167 | U.ldform ( A, D ); |
---|
168 | } |
---|
169 | |
---|
170 | |
---|
171 | double ldmat::logdet() const { |
---|
172 | double ldet = 0.0; |
---|
173 | int i; |
---|
174 | // sum logarithms of diagobal elements |
---|
175 | for ( i = 0; i < D.length(); i++ ) { |
---|
176 | ldet += log ( D ( i ) ); |
---|
177 | }; |
---|
178 | return ldet; |
---|
179 | } |
---|
180 | |
---|
181 | double ldmat::qform ( const vec &v ) const { |
---|
182 | double x = 0.0, sum; |
---|
183 | int i, j; |
---|
184 | |
---|
185 | for ( i = 0; i < D.length(); i++ ) { //rows of L |
---|
186 | sum = 0.0; |
---|
187 | for ( j = 0; j <= i; j++ ) { |
---|
188 | sum += L ( i, j ) * v ( j ); |
---|
189 | } |
---|
190 | x += D ( i ) * sum * sum; |
---|
191 | }; |
---|
192 | return x; |
---|
193 | } |
---|
194 | |
---|
195 | double ldmat::invqform ( const vec &v ) const { |
---|
196 | double x = 0.0; |
---|
197 | int i; |
---|
198 | vec pom ( v.length() ); |
---|
199 | |
---|
200 | backward_substitution ( L.T(), v, pom ); |
---|
201 | |
---|
202 | for ( i = 0; i < D.length(); i++ ) { //rows of L |
---|
203 | x += pom ( i ) * pom ( i ) / D ( i ); |
---|
204 | }; |
---|
205 | return x; |
---|
206 | } |
---|
207 | |
---|
208 | ldmat& ldmat::operator *= ( double x ) { |
---|
209 | D *= x; |
---|
210 | return *this; |
---|
211 | } |
---|
212 | |
---|
213 | vec ldmat::sqrt_mult ( const vec &x ) const { |
---|
214 | int i, j; |
---|
215 | vec res ( dim ); |
---|
216 | //double sum; |
---|
217 | for ( i = 0; i < dim; i++ ) {//for each element of result |
---|
218 | res ( i ) = 0.0; |
---|
219 | for ( j = i; j < dim; j++ ) {//sum D(j)*L(:,i).*x |
---|
220 | res ( i ) += sqrt ( D ( j ) ) * L ( j, i ) * x ( j ); |
---|
221 | } |
---|
222 | } |
---|
223 | // vec res2 = L.transpose()*diag( sqrt( D ) )*x; |
---|
224 | return res; |
---|
225 | } |
---|
226 | |
---|
227 | void ldmat::ldform ( const mat &A, const vec &D0 ) { |
---|
228 | int m = A.rows(); |
---|
229 | int n = A.cols(); |
---|
230 | int mn = ( m < n ) ? m : n ; |
---|
231 | |
---|
232 | bdm_assert_debug ( D0.length() == A.rows(), "ldmat::ldform Vector D must have the length as row count of A" ); |
---|
233 | |
---|
234 | L = concat_vertical ( zeros ( n, n ), diag ( sqrt ( D0 ) ) * A ); |
---|
235 | D = zeros ( n + m ); |
---|
236 | |
---|
237 | //unnecessary big L and D will be made smaller at the end of file |
---|
238 | vec w = zeros ( n + m ); |
---|
239 | |
---|
240 | double sum, beta, pom; |
---|
241 | |
---|
242 | int cc = 0; |
---|
243 | int i = n; // indexovani o 1 niz, nez v matlabu |
---|
244 | int ii, j, jj; |
---|
245 | while ( ( i > n - mn - cc ) && ( i > 0 ) ) { |
---|
246 | i--; |
---|
247 | sum = 0.0; |
---|
248 | |
---|
249 | int last_v = m + i - n + cc + 1; |
---|
250 | |
---|
251 | vec v = zeros ( last_v + 1 ); //prepare v |
---|
252 | for ( ii = n - cc - 1; ii < m + i + 1; ii++ ) { |
---|
253 | sum += L ( ii, i ) * L ( ii, i ); |
---|
254 | v ( ii - n + cc + 1 ) = L ( ii, i ); //assign v |
---|
255 | } |
---|
256 | |
---|
257 | if ( L ( m + i, i ) == 0 ) |
---|
258 | beta = sqrt ( sum ); |
---|
259 | else |
---|
260 | beta = L ( m + i, i ) + sign ( L ( m + i, i ) ) * sqrt ( sum ); |
---|
261 | |
---|
262 | if ( std::fabs ( beta ) < eps ) { |
---|
263 | cc++; |
---|
264 | L.set_row ( n - cc, L.get_row ( m + i ) ); |
---|
265 | L.set_row ( m + i, zeros ( L.cols() ) ); |
---|
266 | D ( m + i ) = 0; |
---|
267 | L ( m + i, i ) = 1; |
---|
268 | L.set_submatrix ( n - cc, m + i - 1, i, i, 0 ); |
---|
269 | continue; |
---|
270 | } |
---|
271 | |
---|
272 | sum -= v ( last_v ) * v ( last_v ); |
---|
273 | sum /= beta * beta; |
---|
274 | sum++; |
---|
275 | |
---|
276 | v /= beta; |
---|
277 | v ( last_v ) = 1; |
---|
278 | |
---|
279 | pom = -2.0 / sum; |
---|
280 | // echo to venca |
---|
281 | |
---|
282 | for ( j = i; j >= 0; j-- ) { |
---|
283 | double w_elem = 0; |
---|
284 | for ( ii = n - cc; ii <= m + i + 1; ii++ ) |
---|
285 | w_elem += v ( ii - n + cc ) * L ( ii - 1, j ); |
---|
286 | w ( j ) = w_elem * pom; |
---|
287 | } |
---|
288 | |
---|
289 | for ( ii = n - cc - 1; ii <= m + i; ii++ ) |
---|
290 | for ( jj = 0; jj < i; jj++ ) |
---|
291 | L ( ii, jj ) += v ( ii - n + cc + 1 ) * w ( jj ); |
---|
292 | |
---|
293 | for ( ii = n - cc - 1; ii < m + i; ii++ ) |
---|
294 | L ( ii, i ) = 0; |
---|
295 | |
---|
296 | L ( m + i, i ) += w ( i ); |
---|
297 | D ( m + i ) = L ( m + i, i ) * L ( m + i, i ); |
---|
298 | |
---|
299 | for ( ii = 0; ii <= i; ii++ ) |
---|
300 | L ( m + i, ii ) /= L ( m + i, i ); |
---|
301 | } |
---|
302 | |
---|
303 | if ( i >= 0 ) |
---|
304 | for ( ii = 0; ii < i; ii++ ) { |
---|
305 | jj = D.length() - 1 - n + ii; |
---|
306 | D ( jj ) = 0; |
---|
307 | L.set_row ( jj, zeros ( L.cols() ) ); //TODO: set_row accepts Num_T |
---|
308 | L ( jj, jj ) = 1; |
---|
309 | } |
---|
310 | |
---|
311 | L.del_rows ( 0, m - 1 ); |
---|
312 | D.del ( 0, m - 1 ); |
---|
313 | |
---|
314 | dim = L.rows(); |
---|
315 | } |
---|
316 | |
---|
317 | //////// Auxiliary Functions |
---|
318 | |
---|
319 | mat ltuinv ( const mat &L ) { |
---|
320 | int dim = L.cols(); |
---|
321 | mat Il = eye ( dim ); |
---|
322 | int i, j, k, m; |
---|
323 | double s; |
---|
324 | |
---|
325 | //Fixme blind transcription of ltuinv.m |
---|
326 | for ( k = 1; k < ( dim ); k++ ) { |
---|
327 | for ( i = 0; i < ( dim - k ); i++ ) { |
---|
328 | j = i + k; //change in .m 1+1=2, here 0+0+1=1 |
---|
329 | s = L ( j, i ); |
---|
330 | for ( m = i + 1; m < ( j ); m++ ) { |
---|
331 | s += L ( m, i ) * Il ( j, m ); |
---|
332 | } |
---|
333 | Il ( j, i ) = -s; |
---|
334 | } |
---|
335 | } |
---|
336 | |
---|
337 | return Il; |
---|
338 | } |
---|
339 | |
---|
340 | void dydr ( double * r, double *f, double *Dr, double *Df, double *R, int jl, int jh, double *kr, int m, int mx ) |
---|
341 | /******************************************************************** |
---|
342 | |
---|
343 | dydr = dyadic reduction, performs transformation of sum of |
---|
344 | 2 dyads r*Dr*r'+ f*Df*f' so that the element of r pointed |
---|
345 | by R is zeroed. This version allows Dr to be NEGATIVE. Hence the name negdydr or dydr_withneg. |
---|
346 | |
---|
347 | Parameters : |
---|
348 | r ... pointer to reduced dyad |
---|
349 | f ... pointer to reducing dyad |
---|
350 | Dr .. pointer to the weight of reduced dyad |
---|
351 | Df .. pointer to the weight of reducing dyad |
---|
352 | R ... pointer to the element of r, which is to be reduced to |
---|
353 | zero; the corresponding element of f is assumed to be 1. |
---|
354 | jl .. lower index of the range within which the dyads are |
---|
355 | modified |
---|
356 | ju .. upper index of the range within which the dyads are |
---|
357 | modified |
---|
358 | kr .. pointer to the coefficient used in the transformation of r |
---|
359 | rnew = r + kr*f |
---|
360 | m .. number of rows of modified matrix (part of which is r) |
---|
361 | Remark : Constant mzero means machine zero and should be modified |
---|
362 | according to the precision of particular machine |
---|
363 | |
---|
364 | V. Peterka 17-7-89 |
---|
365 | |
---|
366 | Added: |
---|
367 | mx .. number of rows of modified matrix (part of which is f) -PN |
---|
368 | |
---|
369 | ********************************************************************/ |
---|
370 | { |
---|
371 | int j, jm; |
---|
372 | double kD, r0; |
---|
373 | double mzero = 2.2e-16; |
---|
374 | double threshold = 1e-4; |
---|
375 | |
---|
376 | if ( fabs ( *Dr ) < mzero ) *Dr = 0; |
---|
377 | r0 = *R; |
---|
378 | *R = 0.0; |
---|
379 | kD = *Df; |
---|
380 | *kr = r0 * *Dr; |
---|
381 | *Df = kD + r0 * ( *kr ); |
---|
382 | if ( *Df > mzero ) { |
---|
383 | kD /= *Df; |
---|
384 | *kr /= *Df; |
---|
385 | } else { |
---|
386 | kD = 1.0; |
---|
387 | *kr = 0.0; |
---|
388 | if ( *Df < -threshold ) { |
---|
389 | bdm_warning ( "Problem in dydr: subraction of dyad results in negative definitness. Likely mistake in calling function." ); |
---|
390 | } |
---|
391 | *Df = 0.0; |
---|
392 | } |
---|
393 | *Dr *= kD; |
---|
394 | jm = mx * jl; |
---|
395 | for ( j = m * jl; j < m*jh; j += m ) { |
---|
396 | r[j] -= r0 * f[jm]; |
---|
397 | f[jm] += *kr * r[j]; |
---|
398 | jm += mx; |
---|
399 | } |
---|
400 | } |
---|
401 | |
---|
402 | } |
---|