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