diff --git a/src/svd.hh b/src/svd.hh new file mode 100644 index 0000000000000000000000000000000000000000..6c96a6423dbde247e183ad84638e5dfba3e80281 --- /dev/null +++ b/src/svd.hh @@ -0,0 +1,269 @@ +/** \file + \brief Singular value decomposition code taken from 'Numerical Recipes in C' +*/ +#ifndef SVD_HH +#define SVD_HH + +#include <math.h> + + +template <class T> +T SQR(const T& a) {return a*a;} + +// template <class T> +// int SIGN(const T& a, const T& b) {return 1;} +#define SIGN(a,b)((b)>=0.0 ? fabs(a) : -fabs(a)) + +/** Computes (a^2 + b^2 )1/2 without destructive underflow or overflow. */ +template <class T> +T pythag(T a, T b) +{ + T absa,absb; + absa=std::abs(a); + absb=std::abs(b); + if (absa > absb) + return absa*sqrt(1.0+SQR(absb/absa)); + else + return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+SQR(absa/absb))); +} + + +/** + Given a matrix a[1..m][1..n], this routine computes its singular value decomposition, A = + U ·W ·V^T . The matrix U replaces a on output. The diagonal matrix of singular values W is out- + put as a vector w[1..n]. The matrix V (not the transpose V T ) is output as v[1..n][1..n]. +*/ +template <class T, int m, int n> +void svdcmp(Dune::FieldMatrix<T,m,n>& a_, Dune::FieldVector<T,n>& w, Dune::FieldMatrix<T,n,n>& v_) +{ + Dune::FieldMatrix<T,m+1,n+1> a(0); + Dune::FieldMatrix<T,n+1,n+1> v(0); + + for (int i=0; i<m; i++) + for (int j=0; j<n; j++) + a[i+1][j+1] = a_[i][j]; + + int flag,i,its,j,jj,k,l,nm; + T anorm,c,f,g,h,s,scale,x,y,z,*rv1; + T* rv1_c = new T[n]; + rv1 = rv1_c-1; + + //Householder reduction to bidiagonal form. + g=scale=anorm=0.0; + + for (i=1;i<=n;i++) { + + l=i+1; + rv1[i]=scale*g; + g=s=scale=0.0; + + if (i <= m) { + for (k=i;k<=m;k++) + scale += std::abs(a[k][i]); + if (scale) { + for (k=i;k<=m;k++) { + a[k][i] /= scale; + s += a[k][i]*a[k][i]; + } + f=a[i][i]; + g = -SIGN(std::sqrt(s),f); + h=f*g-s; + a[i][i]=f-g; + for (j=l;j<=n;j++) { + for (s=0.0,k=i;k<=m;k++) + s += a[k][i]*a[k][j]; + f=s/h; + for (k=i;k<=m;k++) + a[k][j] += f*a[k][i]; + } + for (k=i;k<=m;k++) + a[k][i] *= scale; + } + } + + w[i-1]=scale *g; + g=s=scale=0.0; + + if (i <= m && i != n) { + + for (k=l;k<=n;k++) scale += fabs(a[i][k]); + if (scale) { + + for (k=l;k<=n;k++) { + a[i][k] /= scale; + s += a[i][k]*a[i][k]; + } + f=a[i][l]; + g = -SIGN(std::sqrt(s),f); + h=f*g-s; + a[i][l]=f-g; + for (k=l;k<=n;k++) rv1[k]=a[i][k]/h; + for (j=l;j<=m;j++) { + for (s=0.0,k=l;k<=n;k++) + s += a[j][k]*a[i][k]; + for (k=l;k<=n;k++) + a[j][k] += s*rv1[k]; + } + for (k=l;k<=n;k++) + a[i][k] *= scale; + } + } + + anorm = std::max(anorm,(std::abs(w[i-1])+std::abs(rv1[i]))); + + } + + // Accumulation of right-hand transformations. + for (i=n;i>=1;i--) { + if (i < n) { + if (g) { + // Double division to avoid possible underflow. + for (j=l;j<=n;j++) + v[j][i]=(a[i][j]/a[i][l])/g; + for (j=l;j<=n;j++) { + for (s=0.0,k=l;k<=n;k++) s += a[i][k]*v[k][j]; + for (k=l;k<=n;k++) v[k][j] += s*v[k][i]; + } + } + for (j=l;j<=n;j++) v[i][j]=v[j][i]=0.0; + } + v[i][i]=1.0; + g=rv1[i]; + l=i; + } + + // Accumulation of left-hand transformations. + //for (i=IMIN(m,n);i>=1;i--) { + for (i=std::min(m,n);i>=1;i--) { + l=i+1; + g=w[i-1]; + for (j=l;j<=n;j++) a[i][j]=0.0; + if (g) { + g=1.0/g; + for (j=l;j<=n;j++) { + for (s=0.0,k=l;k<=m;k++) s += a[k][i]*a[k][j]; + f=(s/a[i][i])*g; + for (k=i;k<=m;k++) a[k][j] += f*a[k][i]; + } + for (j=i;j<=m;j++) a[j][i] *= g; + } else for (j=i;j<=m;j++) a[j][i]=0.0; + ++a[i][i]; + } + + // Diagonalization of the bidiagonal form: Loop over + //singular values, and over allowed iterations. + for (k=n;k>=1;k--) { + + for (its=1;its<=30;its++) { + flag=1; + // Test for splitting. + for (l=k;l>=1;l--) { + // Note that rv1[1] is always zero. + nm=l-1; + if ((T)(fabs(rv1[l])+anorm) == anorm) { + flag=0; + break; + } + if ((T)(fabs(w[nm-1])+anorm) == anorm) break; + } + if (flag) { + // Cancellation of rv1[l], if l > 1. + c=0.0; + s=1.0; + for (i=l;i<=k;i++) { + + f=s*rv1[i]; + rv1[i]=c*rv1[i]; + if ((T)(fabs(f)+anorm) == anorm) break; + g=w[i-1]; + h=pythag(f,g); + w[i-1]=h; + h=1.0/h; + c=g*h; + s = -f*h; + for (j=1;j<=m;j++) { + y=a[j][nm]; + z=a[j][i]; + a[j][nm]=y*c+z*s; + a[j][i]=z*c-y*s; + } + } + } + z=w[k-1]; + //Convergence. + if (l == k) { + // Singular value is made nonnegative. + if (z < 0.0) { + w[k-1] = -z; + for (j=1;j<=n;j++) v[j][k] = -v[j][k]; + } + break; + } + if (its == 30) + std::cerr << "no convergence in 30 svdcmp iterations" << std::endl; + // Shift from bottom 2-by-2 minor. + x=w[l-1]; + nm=k-1; + y=w[nm-1]; + g=rv1[nm]; + h=rv1[k]; + f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y); + g=pythag(f,T(1.0)); + f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x; + // Next QR transformation: + c=s=1.0; + for (j=l;j<=nm;j++) { + i=j+1; + g=rv1[i]; + y=w[i-1]; + h=s*g; + g=c*g; + z=pythag(f,h); + rv1[j]=z; + c=f/z; + s=h/z; + f=x*c+g*s; + g = g*c-x*s; + h=y*s; + y *= c; + for (jj=1;jj<=n;jj++) { + x=v[jj][j]; + z=v[jj][i]; + v[jj][j]=x*c+z*s; + v[jj][i]=z*c-x*s; + } + z=pythag(f,h); + // Rotation can be arbitrary if z = 0. + w[j-1]=z; + if (z) { + z=1.0/z; + c=f*z; + s=h*z; + } + f=c*g+s*y; + x=c*y-s*g; + + for (jj=1;jj<=m;jj++) { + y=a[jj][j]; + z=a[jj][i]; + a[jj][j]=y*c+z*s; + a[jj][i]=z*c-y*s; + } + } + rv1[l]=0.0; + rv1[k]=f; + w[k-1]=x; + } + } + delete[](rv1_c); + + for (int i=0; i<m; i++) + for (int j=0; j<n; j++) + a_[i][j] = a[i+1][j+1]; + + for (int i=0; i<n; i++) + for (int j=0; j<n; j++) + v_[i][j] = v[i+1][j+1]; +} + +#endif