TBCI Numerical high perf. C++ Library: svd

00001 
00011 #ifndef TBCI_SOLVER_SVD_SOLVER_H
00012 #define TBCI_SOLVER_SVD_SOLVER_H
00013 
00014 #include "../matrix.h"
00015 #define SVDFLOAT typename MatrixType::value_type
00016 
00017 NAMESPACE_TBCI
00018 
00019 
00021 template <typename ARG>
00022 inline double sv_decomp_pythag(ARG a, ARG b)
00023 {
00024         const double absa = MATH__ fabs(a);
00025         const double absb = MATH__ fabs(b);
00026         if (absa > absb)
00027                 return absa * MATH__ sqrt(1.0 + (absb/absa)*(absb/absa));
00028         else 
00029                 return (absb==0.0? 0.0: absb * MATH__ sqrt(1.0 + (absa/absb)*(absa/absb)));
00030 }
00031 
00032 
00033 //TODO: Check which of the variables used should be double rather
00034 // then SVDFLOAT (which can be complex, e.g.)
00035 
00036 INST2(template <typename MAT, typename VEC > class MAT friend void sv_decomp (MAT &, MAT &, VEC &);)
00037 template <typename MatrixType /*= TBCI__ Matrix<double>*/,
00038           typename VectorType /*= TBCI__ Vector<double>*/ >
00044 void sv_decomp (MatrixType & A, MatrixType & V, VectorType & W)
00045 {
00046         const int m = A.rows();
00047         const int n = A.columns();
00048         int l = 0, i, k;        // M$ annoyance (for scoping bug)
00049         VectorType rv1(n);
00050         SVDFLOAT g = 0.0, scale = 0.0;
00051         double anorm = 0.0;
00052         for (i = 0; i < n; ++i) {
00053                 SVDFLOAT s = 0.0;
00054                 l = i+1;
00055                 rv1[i] = scale*g;
00056                 g = scale = 0.0;
00057                 if (i < m) {
00058                         for (k = i; k < m; ++k)
00059                                 scale += MATH__ fabs(A(k,i));
00060                         if (scale != (SVDFLOAT)0) {
00061                                 for (k = i; k < m; ++k) {
00062                                         A(k,i) /= scale;
00063                                         s += A(k,i)*A(k,i);
00064                                 }
00065                                 SVDFLOAT f = A(i,i);
00066                                 g = f>(SVDFLOAT)0 ? - MATH__ sqrt(s) : MATH__ sqrt(s);
00067                                 SVDFLOAT h = f*g - s;
00068                                 A(i,i) = f - g;
00069                                 for (int j = l; j < n; ++j) {
00070                                         s = (SVDFLOAT)0;
00071                                         for (k = i; k < m; ++k) 
00072                                                 s += A(k,i)*A(k,j);
00073                                         SVDFLOAT f = s/h;
00074                                         for (k = i; k < m; ++k)
00075                                                 A(k,j) += f*A(k,i);
00076                                 }
00077                                 for (k = i; k < m; ++k) 
00078                                         A(k,i) *= scale;
00079                         }
00080                 }
00081                 W[i] = scale * g;
00082                 g = s = scale = 0.0;
00083                 if (i < m && i != (n-1)) {
00084                         for (k = l; k < n; ++k) 
00085                                 scale += MATH__ fabs(A(i,k));
00086                         if (scale != (SVDFLOAT)0) {
00087                                 for (k = l; k < n; ++k) {
00088                                         A(i,k) /= scale;
00089                                         s += A(i,k)*A(i,k);
00090                                 }
00091                                 SVDFLOAT f = A(i,l);
00092                                 g = f>(SVDFLOAT)0 ? - MATH__ sqrt(s) : MATH__ sqrt(s);
00093                                 SVDFLOAT h = f*g - s;
00094                                 A(i,l) = f - g;
00095                                 for (k=l; k < n; ++k) 
00096                                         rv1[k] = A(i,k)/h;
00097                                 for (int j = l; j < m; ++j) {
00098                                         s = 0.0;
00099                                         for (k = l; k < n; ++k)
00100                                                 s += A(j,k)*A(i,k);
00101                                         for (k = l; k < n; ++k)
00102                                                 A(j,k) += s*rv1[k];
00103                                 }
00104                                 for(k = l; k < n; ++k) 
00105                                         A(i,k) *= scale;
00106                         }
00107                 }
00108                 anorm = MAX(anorm, MATH__ fabs(W[i]) + MATH__ fabs(rv1[i]));
00109         }
00110         for (i = n-1; i >= 0; --i) {
00111                 if (i < (n-1)) {
00112                         if (g != (SVDFLOAT)0) {
00113                                 int j;
00114                                 for (j = l; j < n; ++j )
00115                                         V(j,i) = (A(i,j)/A(i,l))/g;
00116                                 for (j = l; j < n; ++j) {
00117                                         SVDFLOAT s = 0.0;
00118                                         for (k = l; k < n; ++k )
00119                                                 s += A(i,k)*V(k,j);
00120                                         for (k = l; k < n; ++k )
00121                                                 V(k,j) += s*V(k,i);
00122                                 }
00123                         }
00124                         for (int j = l; j < n; ++j) 
00125                                 V(i,j) = V(j,i) = 0.0;
00126                 }
00127                 V(i,i) = 1.0;
00128                 g = rv1[i];
00129                 l = i;
00130         }
00131         for (i = MIN(m,n)-1; i >= 0; --i) {
00132                 int j;
00133                 int l = i+1;
00134                 g = W[i];
00135                 for (j = l; j < n; ++j) 
00136                         A(i,j) = 0.0;
00137                 if (g != (SVDFLOAT)0) {
00138                         g = 1.0/g;
00139                         for (j = l; j < n; ++j) {
00140                                 SVDFLOAT s = 0.0;
00141                                 for (k = l; k < m; ++k) 
00142                                         s += A(k,i)*A(k,j);
00143                                 SVDFLOAT f = (s/A(i,i))*g;
00144                                 for (k = i; k < m; ++k) 
00145                                         A(k,j) += f*A(k,i);
00146                         }
00147                         for (j = i; j < m; ++j) 
00148                                 A(j,i) *= g;
00149                 } else 
00150                         for (j = i; j < m; ++j) 
00151                                 A(j,i) = 0.0;
00152                 A(i,i) += 1.0;
00153         }
00154         for (k = n-1; k >= 0; --k) {
00155                 for (int its = 0; its < 30; ++its) {
00156                         bool flag = true;
00157                         int nm=0;
00158                         for (l = k; l >= 0; --l) {
00159                                 nm = l-1; // note that rv1[0] is always zero
00160                                 if ((MATH__ fabs(rv1[l]) + anorm) == anorm) {
00161                                         flag = false;
00162                                         break;
00163                                 }
00164                                 if ((MATH__ fabs(W[nm]) + anorm) == anorm) 
00165                                         break;
00166                         }
00167                         if (flag) {
00168                                 SVDFLOAT c = 0.0;
00169                                 SVDFLOAT s = 1.0;
00170                                 for (i = l; i < k; ++i) {
00171                                         SVDFLOAT f = s*rv1[i];
00172                                         rv1[i] *= c;
00173                                         if ((MATH__ fabs(f) + anorm) == anorm) 
00174                                                 break;
00175                                         SVDFLOAT g = W[i];
00176                                         SVDFLOAT h = sv_decomp_pythag(f, g);
00177                                         W[i] = h;
00178                                         h = 1.0/h;
00179                                         c =  g*h;
00180                                         s = -f*h;
00181                                         for (int j = 0; j < m; ++j) {
00182                                                 SVDFLOAT y = A(j,nm);
00183                                                 SVDFLOAT z = A(j,i);
00184                                                 A(j,nm) = y*c + z*s;
00185                                                 A(j,i)  = z*c - y*s;
00186                                         }
00187                                 }
00188                         }
00189                         SVDFLOAT z = W[k];
00190                         if (l == k) {   // convergence
00191                                 if (z < (SVDFLOAT)0) {
00192                                         W[k] = -z;
00193                                         for (int j = 0; j < n; ++j) 
00194                                                 V(j,k) = -V(j,k);
00195                                 }
00196                                 break;
00197                         }
00198                         if (its == 29)
00199                                 STD__ cerr << "SVD: No convergence in 30 iterations.\n";
00200                         SVDFLOAT x = W[l];
00201                         nm = k-1;
00202                         SVDFLOAT y = W[nm];
00203                         SVDFLOAT g = rv1[nm];
00204                         SVDFLOAT h = rv1[k];
00205                         SVDFLOAT f = ((y-z)*(y+z) + (g-h)*(g+h)) / (2.0*h*y);
00206                         g = sv_decomp_pythag(f, (SVDFLOAT)1.0);
00207                         f = ((x-z)*(x+z) +
00208                              h*((y / (f + (f>(SVDFLOAT)0 ? MATH__ fabs(g) : - MATH__ fabs(g))))-h))/x;
00209                         SVDFLOAT c = 1.0;
00210                         SVDFLOAT s = 1.0;
00211                         for (int j = l; j <= nm; ++j) {
00212                                 int jj; // You know who annoyance
00213                                 i = j+1;
00214                                 SVDFLOAT g = rv1[i];
00215                                 SVDFLOAT y = W[i];
00216                                 SVDFLOAT h = s*g;
00217                                 g*=c;
00218                                 SVDFLOAT z = sv_decomp_pythag(f, h);
00219                                 rv1[j] = z;
00220                                 c = f/z;
00221                                 s = h/z;
00222                                 f = x*c + g*s;
00223                                 g = g*c - x*s;
00224                                 h = y*s;
00225                                 y *= c;
00226                                 for (jj = 0; jj < n; ++jj) {
00227                                         SVDFLOAT x = V(jj,j);
00228                                         SVDFLOAT z = V(jj,i);
00229                                         V(jj,j) = x*c + z*s;
00230                                         V(jj,i) = z*c - x*s;
00231                                 }
00232                                 z = sv_decomp_pythag(f, h);
00233                                 W[j] = z;
00234                                 if (z != (SVDFLOAT)0) {
00235                                         z = 1.0/z;
00236                                         c = f*z;
00237                                         s = h*z;
00238                                 }
00239                                 f = c*g + s*y;
00240                                 x = c*y - s*g;
00241                                 for (jj = 0; jj < m; ++jj) {
00242                                         SVDFLOAT y = A(jj,j);
00243                                         SVDFLOAT z = A(jj,i);
00244                                         A(jj,j) = y*c + z*s;
00245                                         A(jj,i) = z*c - y*s;
00246                                 }
00247                         }
00248                         rv1[l] = 0.0;
00249                         rv1[k] = f;
00250                         W[k] = x;
00251                 }
00252         }
00253 }
00254 
00255 
00256 INST2(template <typename MAT, typename VEC > class MAT friend void sv_decomp_backsub (const MAT &, const MAT &, const VEC &, const VEC &, VEC &);)
00257 template <typename MatrixType /*= TBCI__ Matrix<double>*/,
00258           typename VectorType /*= TBCI__ Vector<double>*/ >
00259 void sv_decomp_backsub (const MatrixType & U, const MatrixType & V,
00260                         const VectorType & W, const VectorType & b,
00261                         VectorType & x)
00262 {
00263         const int n = W.size();
00264         const int m = b.size();
00265         VectorType tmp(n);
00266         int j;
00267         for (j = 0; j < n; ++j) {
00268                 SVDFLOAT s = 0.0;
00269                 if (W[j] != (SVDFLOAT)0) {
00270                         for (int i = 0; i < m; ++i) 
00271                                 s += U(i,j)*b[i];
00272                         s /= W[j];
00273                 }
00274                 tmp[j] = s;
00275         }
00276         for (j = 0; j < n; ++j) {
00277                 SVDFLOAT s = 0.0;
00278                 for (int i = 0; i < n; ++i) 
00279                         s += V(j,i)*tmp[i];
00280                 x[j] = s;
00281         }
00282 }
00283 
00284 INST(template <typename VEC > class VEC friend /*static*/ int fix_condition (VEC& vec, const double cndno);)
00285 /* Fix those singular values that are close to zero */
00286 template <typename VectorType >
00287 static int fix_condition (VectorType& vec, const double cndno = 1e-12)
00288 {
00289         int fixed = 0; unsigned i;
00290         double maxsv = 0.0;
00291         const unsigned size = vec.size();
00292         //typename VectorType::value_type * RESTRICT const VEC = &vec.setval(0);
00293         for (i = 0; i < size; ++i)
00294                 maxsv = MAX(maxsv, MATH__ fabs(vec[i]));
00295         double minsv = maxsv * cndno;
00296         for (i = 0; i < size; ++i)
00297                 if (MATH__ fabs(vec[i]) < minsv) {
00298                         fixed++;
00299                         vec[i] = (typename VectorType::value_type)0.0;
00300                 }
00301         return fixed;
00302 }
00303 
00304         
00305 
00306 INST2(template <typename MAT, typename VEC > class MAT friend VEC svd_solve (const MAT& mat, const VEC& b, const double cndno);)
00314 template <typename MatrixType, typename VectorType>
00315 VectorType svd_solve (const MatrixType& mat, const VectorType& b, const double cndno = 1e-12)
00316 {
00317         MatrixType A(mat);
00318         MatrixType V(mat.columns(), mat.columns());
00319         VectorType W(mat.columns());
00320         sv_decomp <MatrixType, VectorType > (A, V, W);
00321         fix_condition(W, cndno);
00322         // we could calc mat^-1 = V * diag (1/W) * A * A^T now
00323         VectorType x (mat.columns());
00324         sv_decomp_backsub(A, V, W, b, x);
00325         return x;
00326 }
00327 
00328 
00329 // Optimized version for our own matrix & vector classes, using TBCI
00330 INST2(template <typename MAT, typename T > class MAT friend TVector<T> sv_decomp_backsub (const MAT &, const MAT &, const Vector<T> &, const Vector<T> &);)
00331 template <typename MatrixType, typename T >
00332 TVector<T> sv_decomp_backsub (const MatrixType & U, const MatrixType & V,
00333                               const Vector<T> & W, const Vector<T> & b)
00334 {
00335         const int n = W.size();
00336         const int m = b.size();
00337         TVector<T> x (n);
00338         Vector<T> tmp(n);
00339         int j;
00340         for (j = 0; j < n; ++j) {
00341                 SVDFLOAT s = 0.0;
00342                 if (W[j] != (SVDFLOAT)0) {
00343                         for (int i = 0; i < m; ++i) 
00344                                 s += U(i,j)*b[i];
00345                         s /= W[j];
00346                 }
00347                 tmp[j] = s;
00348         }
00349         for (j = 0; j < n; ++j) {
00350                 SVDFLOAT s = 0.0;
00351                 for (int i = 0; i < n; ++i) 
00352                         s += V(j,i)*tmp[i];
00353                 x.set (s, j);
00354         }
00355         return x;
00356 }
00357 
00358 INST2(template <typename MAT, typename T > class MAT friend TVector<T> svd_solve (const MAT& mat, const Vector<T>& b, const double cndno);)
00369 template <typename MatrixType, typename T >
00370 TVector<T> svd_solve (const MatrixType& mat, const Vector<T>& b, const double cndno = 1e-12)
00371 {
00372         MatrixType A(mat);
00373         MatrixType V(mat.columns(), mat.columns());
00374         Vector<T>  W(mat.columns());
00375         sv_decomp <MatrixType, Vector<T> > (A, V, W);
00376         fix_condition (W, cndno);
00377         return sv_decomp_backsub (A, V, W, b);
00378 }
00379 
00380 
00385 #undef SVDFLOAT
00386 
00387 NAMESPACE_END
00388 #endif  /* TBCI_SOLVER_SVD_SOLVER_H */
svd_solver.h
