[2] | 1 | #include <itpp/itbase.h> |
---|
[7] | 2 | #include "libDC.h" |
---|
[2] | 3 | |
---|
| 4 | using namespace itpp; |
---|
| 5 | |
---|
[7] | 6 | using std::endl; |
---|
[2] | 7 | |
---|
[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 ); |
---|
[2] | 10 | |
---|
[7] | 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 ); |
---|
[2] | 14 | |
---|
| 15 | |
---|
| 16 | |
---|
[7] | 17 | ldmat::ldmat( const mat &exL, const vec &exD ) { |
---|
[5] | 18 | D = exD; |
---|
| 19 | L = exL; |
---|
[2] | 20 | } |
---|
| 21 | |
---|
[7] | 22 | ldmat::ldmat() { |
---|
| 23 | vec D ; |
---|
| 24 | mat L; |
---|
[2] | 25 | } |
---|
| 26 | |
---|
[7] | 27 | ldmat::ldmat( const mat V ) { |
---|
| 28 | //TODO check if correct!! Based on heuristic observation of lu() |
---|
| 29 | |
---|
| 30 | int dim = V.cols(); |
---|
| 31 | it_assert_debug( dim == V.rows(),"ldmat::ldmat matrix V is not square!" ); |
---|
| 32 | |
---|
| 33 | mat U( dim,dim ); |
---|
| 34 | |
---|
| 35 | L = V; //Allocate space for L |
---|
| 36 | ivec p = ivec( dim ); //not clear why? |
---|
| 37 | |
---|
| 38 | lu( V,L,U,p ); |
---|
| 39 | |
---|
| 40 | //Now, if V is symmetric, L is what we seek and D is on diagonal of U |
---|
| 41 | D = diag( U ); |
---|
| 42 | |
---|
| 43 | //check if V was symmetric |
---|
| 44 | //TODO How? norm of L-U'? |
---|
| 45 | //it_assert_debug(); |
---|
| 46 | } |
---|
| 47 | |
---|
| 48 | void ldmat::opupdt( const vec &v, double w ) { |
---|
| 49 | int dim = D.length(); |
---|
| 50 | double kr; |
---|
| 51 | vec r = v; |
---|
| 52 | //beware! it is potentionally dangerous, if ITpp change _behaviour of _data()! |
---|
| 53 | double *Lraw = L._data(); |
---|
| 54 | double *Draw = D._data(); |
---|
| 55 | double *rraw = r._data(); |
---|
| 56 | |
---|
| 57 | it_assert_debug( v.length() == dim, "LD::ldupdt vector v is not compatible with this ld." ); |
---|
| 58 | |
---|
| 59 | for ( int i = dim - 1; i >= 0; i-- ) { |
---|
| 60 | dydr( rraw, Lraw + i, &w, Draw + i, rraw + i, 0, i, &kr, 1, dim ); |
---|
| 61 | } |
---|
| 62 | } |
---|
| 63 | |
---|
| 64 | std::ostream &operator<< ( std::ostream &os, sqmat &sq ) { |
---|
| 65 | os << sq.to_mat() << endl; |
---|
| 66 | } |
---|
| 67 | |
---|
| 68 | mat ldmat::to_mat() { |
---|
| 69 | int dim = D.length(); |
---|
| 70 | mat V( dim, dim ); |
---|
| 71 | double sum; |
---|
| 72 | int r, c, cc; |
---|
| 73 | |
---|
| 74 | for ( r = 0;r < dim;r++ ) { //row cycle |
---|
| 75 | for ( c = r;c < dim;c++ ) { |
---|
| 76 | //column cycle, using symmetricity => c=r! |
---|
| 77 | sum = 0.0; |
---|
| 78 | for ( cc = c;cc < dim;cc++ ) { //cycle over the remaining part of the vector |
---|
| 79 | sum += L( cc, r ) * D( cc ) * L( cc, c ); |
---|
| 80 | //here L(cc,r) = L(r,cc)'; |
---|
| 81 | } |
---|
| 82 | V( r, c ) = sum; |
---|
| 83 | // symmetricity |
---|
| 84 | if ( r != c ) {V( c, r ) = sum;}; |
---|
| 85 | } |
---|
| 86 | } |
---|
| 87 | return V; |
---|
| 88 | } |
---|
| 89 | |
---|
| 90 | |
---|
| 91 | void ldmat::add( const ldmat &ld2, double w ) { |
---|
| 92 | int dim = D.length(); |
---|
| 93 | |
---|
| 94 | it_assert_debug( ld2.D.length() == dim, "LD.add() incompatible sizes of LDs;" ); |
---|
| 95 | |
---|
| 96 | //Fixme can be done more efficiently either via dydr or ldform |
---|
| 97 | for ( int r = 0; r < dim; r++ ) { |
---|
| 98 | // Add columns of ld2.L' (i.e. rows of ld2.L) as dyads weighted by ld2.D |
---|
| 99 | this->opupdt( ld2.L.get_row( r ), w*ld2.D( r ) ); |
---|
| 100 | } |
---|
| 101 | } |
---|
| 102 | |
---|
| 103 | void ldmat::clear(){L.clear(); for ( int i=0;i<L.cols();i++ ){L( i,i )=1;}; D.clear();} |
---|
| 104 | |
---|
| 105 | void ldmat::inv( ldmat &Inv ) { |
---|
| 106 | int dim = D.length(); |
---|
| 107 | Inv.clear(); //Inv = zero in LD |
---|
| 108 | mat U = ltuinv( L ); |
---|
| 109 | |
---|
| 110 | //Fixme can be done more efficiently either via dydr or ldform |
---|
| 111 | for ( int r = 0; r < dim; r++ ) { |
---|
| 112 | // Add columns of U as dyads weighted by 1/D |
---|
| 113 | Inv.opupdt( U.get_col( r ), 1.0 / D( r ) ); |
---|
| 114 | } |
---|
| 115 | } |
---|
| 116 | |
---|
| 117 | void ldmat::mult_qform( const mat &C, bool trans ) { |
---|
| 118 | |
---|
| 119 | //TODO better |
---|
| 120 | |
---|
| 121 | it_assert_debug( C.cols()==L.cols(), "ldmat::mult_qform wrong input argument" ); |
---|
| 122 | mat Ct=C; |
---|
| 123 | |
---|
| 124 | if ( trans==false ) { // return C*this*C' |
---|
| 125 | Ct *= this->to_mat(); |
---|
| 126 | Ct *= C.transpose(); |
---|
| 127 | } else { // return C'*this*C |
---|
| 128 | Ct = C.transpose(); |
---|
| 129 | Ct *= this->to_mat(); |
---|
| 130 | Ct *= C; |
---|
| 131 | } |
---|
| 132 | |
---|
| 133 | ldmat Lnew=ldmat( Ct ); |
---|
| 134 | L = Lnew.L; |
---|
| 135 | D = Lnew.D; |
---|
| 136 | } |
---|
| 137 | |
---|
| 138 | double ldmat::logdet() { |
---|
| 139 | double ldet = 0.0; |
---|
| 140 | int i; |
---|
| 141 | // sum logarithms of diagobal elements |
---|
| 142 | for ( i=0; i<D.length(); i++ ){ldet+=log( D( i ) );}; |
---|
| 143 | } |
---|
| 144 | |
---|
| 145 | double ldmat::qform( vec &v ) { |
---|
| 146 | double x = 0.0, sum; |
---|
| 147 | int i,j; |
---|
| 148 | |
---|
| 149 | for ( i=0; i<D.length(); i++ ) { //rows of L |
---|
| 150 | sum = 0.0; |
---|
| 151 | for ( j=0; j<=i; j++ ){sum+=L( i,j )*v( j );} |
---|
| 152 | x +=D( i )*sum*sum; |
---|
| 153 | }; |
---|
| 154 | return x; |
---|
| 155 | } |
---|
| 156 | |
---|
| 157 | ldmat& ldmat::operator *= (double x){ |
---|
| 158 | int i; |
---|
| 159 | for(i=0;i<D.length();i++){D(i)*=x;}; |
---|
| 160 | } |
---|
| 161 | |
---|
| 162 | |
---|
| 163 | //////// Auxiliary Functions |
---|
| 164 | |
---|
| 165 | mat ltuinv( const mat &L ) { |
---|
| 166 | int dim = L.cols(); |
---|
| 167 | mat Il = eye( dim ); |
---|
| 168 | int i, j, k, m; |
---|
| 169 | double s; |
---|
| 170 | |
---|
| 171 | //Fixme blind transcription of ltuinv.m |
---|
| 172 | for ( k = 1; k < ( dim );k++ ) { |
---|
| 173 | for ( i = 0; i < ( dim - k );i++ ) { |
---|
| 174 | j = i + k; //change in .m 1+1=2, here 0+0+1=1 |
---|
| 175 | s = L( j, i ); |
---|
| 176 | for ( m = i + 1; m < ( j - 1 ); m++ ) { |
---|
| 177 | s += L( m, i ) * Il( j, m ); |
---|
| 178 | } |
---|
| 179 | Il( j, i ) = -s; |
---|
| 180 | } |
---|
| 181 | } |
---|
| 182 | |
---|
| 183 | return Il; |
---|
| 184 | } |
---|
| 185 | |
---|
| 186 | void dydr( double * r, double *f, double *Dr, double *Df, double *R, int jl, int jh, double *kr, int m, int mx ) |
---|
| 187 | /******************************************************************** |
---|
| 188 | |
---|
| 189 | dydr = dyadic reduction, performs transformation of sum of |
---|
| 190 | 2 dyads r*Dr*r'+ f*Df*f' so that the element of r pointed |
---|
| 191 | by R is zeroed. This version allows Dr to be NEGATIVE. Hence the name negdydr or dydr_withneg. |
---|
| 192 | |
---|
| 193 | Parameters : |
---|
| 194 | r ... pointer to reduced dyad |
---|
| 195 | f ... pointer to reducing dyad |
---|
| 196 | Dr .. pointer to the weight of reduced dyad |
---|
| 197 | Df .. pointer to the weight of reducing dyad |
---|
| 198 | R ... pointer to the element of r, which is to be reduced to |
---|
| 199 | zero; the corresponding element of f is assumed to be 1. |
---|
| 200 | jl .. lower index of the range within which the dyads are |
---|
| 201 | modified |
---|
| 202 | ju .. upper index of the range within which the dyads are |
---|
| 203 | modified |
---|
| 204 | kr .. pointer to the coefficient used in the transformation of r |
---|
| 205 | rnew = r + kr*f |
---|
| 206 | m .. number of rows of modified matrix (part of which is r) |
---|
| 207 | Remark : Constant mzero means machine zero and should be modified |
---|
| 208 | according to the precision of particular machine |
---|
| 209 | |
---|
| 210 | V. Peterka 17-7-89 |
---|
| 211 | |
---|
| 212 | Added: |
---|
| 213 | mx .. number of rows of modified matrix (part of which is f) -PN |
---|
| 214 | |
---|
| 215 | ********************************************************************/ |
---|
[5] | 216 | { |
---|
[7] | 217 | int j, jm; |
---|
| 218 | double kD, r0; |
---|
| 219 | double mzero = 2.2e-16; |
---|
| 220 | double threshold = 1e-4; |
---|
| 221 | |
---|
| 222 | if ( fabs( *Dr ) < mzero ) *Dr = 0; |
---|
| 223 | r0 = *R; |
---|
| 224 | *R = 0.0; |
---|
| 225 | kD = *Df; |
---|
| 226 | *kr = r0 * *Dr; |
---|
| 227 | *Df = kD + r0 * ( *kr ); |
---|
| 228 | if ( *Df > mzero ) { |
---|
| 229 | kD /= *Df; |
---|
| 230 | *kr /= *Df; |
---|
| 231 | } else { |
---|
| 232 | kD = 1.0; |
---|
| 233 | *kr = 0.0; |
---|
| 234 | if ( *Df < -threshold ) it_warning( "Problem in dydr: subraction of dyad results in negative definitness. Likely mistake in calling function." ); |
---|
| 235 | *Df = 0.0; |
---|
| 236 | } |
---|
| 237 | *Dr *= kD; |
---|
| 238 | jm = mx * jl; |
---|
| 239 | for ( j = m * jl; j < m*jh; j += m ) { |
---|
| 240 | r[j] -= r0 * f[jm]; |
---|
| 241 | f[jm] += *kr * r[j]; |
---|
| 242 | jm += mx; |
---|
| 243 | } |
---|
[2] | 244 | } |
---|
[7] | 245 | |
---|