Geografic-Information-System/lib/gmath/svd.c

#include <grass/gis.h>
#include <math.h>

static double at,bt,ct;
#define PYTHAG(a,b) ((at=fabs(a)) > (bt=fabs(b)) ? \
    (ct=bt/at,at*sqrt(1.0+ct*ct)) : (bt ? (ct=at/bt,bt*sqrt(1.0+ct*ct)): 0.0))

static double maxarg1,maxarg2;
#define MAX(a,b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ?\
	(maxarg1) : (maxarg2))
#define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))

int G_svdcmp( double **a, int m,int n, double *w,double **v)
{
    int flag,i,its,j,jj,k,ii=0,nm=0;
    double c,f,h,s,x,y,z;
    double anorm=0.0,g=0.0,scale=0.0;
    double *rv1,*G_alloc_vector();


    if (m < n) return -1; /* must augment A with extra zero rows */
    rv1=G_alloc_vector(n);

    n--;
    m--;

    for (i=0;i<=n;i++) {
        ii=i+1;
        rv1[i]=scale*g;
        g=s=scale=0.0;
        if (i <= m) {
            for (k=i;k<=m;k++) scale += fabs(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(sqrt(s),f);
                h=f*g-s;
                a[i][i]=f-g;
                if (i != n) {
                    for (j=ii;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]=scale*g;
        g=s=scale=0.0;
        if (i <= m && i != n) {
            for (k=ii;k<=n;k++) scale += fabs(a[i][k]);
            if (scale) {
                for (k=ii;k<=n;k++) {
                    a[i][k] /= scale;
                    s += a[i][k]*a[i][k];
                }
                f=a[i][ii];
                g = -SIGN(sqrt(s),f);
                h=f*g-s;
                a[i][ii]=f-g;
                for (k=ii;k<=n;k++) rv1[k]=a[i][k]/h;
                if (i != m) {
                    for (j=ii;j<=m;j++) {
                        for (s=0.0,k=ii;k<=n;k++) s += a[j][k]*a[i][k];
                        for (k=ii;k<=n;k++) a[j][k] += s*rv1[k];
                    }
                }
                for (k=ii;k<=n;k++) a[i][k] *= scale;
            }
        }
        anorm=MAX(anorm,(fabs(w[i])+fabs(rv1[i])));
    }
    for (i=n;i>=0;i--) {
        if (i < n) {
            if (g) {
                for (j=ii;j<=n;j++)
                    v[j][i]=(a[i][j]/a[i][ii])/g;
                for (j=ii;j<=n;j++) {
                    for (s=0.0,k=ii;k<=n;k++) s += a[i][k]*v[k][j];
                    for (k=ii;k<=n;k++) v[k][j] += s*v[k][i];
                }
            }
            for (j=ii;j<=n;j++) v[i][j]=v[j][i]=0.0;
        }
        v[i][i]=1.0;
        g=rv1[i];
        ii=i;
    }
    for (i=n;i>=0;i--) {
        ii=i+1;
        g=w[i];
        if (i < n)
            for (j=ii;j<=n;j++) a[i][j]=0.0;
        if (g) {
            g=1.0/g;
            if (i != n) {
                for (j=ii;j<=n;j++) {
                    for (s=0.0,k=ii;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];
    }
    for (k=n;k>=0;k--) {
        for (its=1;its<=30;its++) {
            flag=1;
            for (ii=k;ii>=0;ii--) {
                nm=ii-1;
                if (fabs(rv1[ii])+anorm == anorm) {
                    flag=0;
                    break;
                }
                if (fabs(w[nm])+anorm == anorm) break;
            }
            if (flag) {
                c=0.0;
                s=1.0;
                for (i=ii;i<=k;i++) {
                    f=s*rv1[i];
                    if (fabs(f)+anorm != anorm) {
                        g=w[i];
                        h=PYTHAG(f,g);
                        w[i]=h;
                        h=1.0/h;
                        c=g*h;
                        s=(-f*h);
                        for (j=0;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];
            if (ii == k) {
                if (z < 0.0) {
                    w[k] = -z;
                    for (j=0;j<=n;j++) v[j][k]=(-v[j][k]);
                }
                break;
            }
            if (its == 30) return -2; /*No convergence in 30 SVDCMP iterations*/
            x=w[ii];
            nm=k-1;
            y=w[nm];
            g=rv1[nm];
            h=rv1[k];
            f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
            g=PYTHAG(f,1.0);
            f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
            c=s=1.0;
            for (j=ii;j<=nm;j++) {
                i=j+1;
                g=rv1[i];
                y=w[i];
                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=y*c;
                for (jj=0;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);
                w[j]=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=0;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[ii]=0.0;
            rv1[k]=f;
            w[k]=x;
        }
    }
    G_free_vector(rv1);
    return 0;
}

#undef SIGN
#undef MAX
#undef PYTHAG

int G_svbksb(
    double **u,double w[],double **v,
    int m,int n,double b[],double x[])
{
    int j,i;
    double s,*tmp,*G_alloc_vector();

    tmp=G_alloc_vector(n);
    for (j=0;j<n;j++) {
        s=0.0;
        if (w[j]) {
            for (i=0;i<m;i++) s += u[i][j]*b[i];
            s /= w[j];
        }
        tmp[j]=s;
    }
    for (j=0;j<n;j++) {
        s=0.0;
        for (i=0;i<n;i++) s += v[j][i]*tmp[i];
        x[j]=s;
    }
    G_free_vector(tmp);

    return 0;
}

#define TOL 1e-8

int G_svelim( double *w, int n)
{
    int i;
    double thresh;

    thresh=0.0;                  /* remove singularity */
    for(i=0;i<n;i++)
        if (w[i] > thresh)
	    thresh=w[i];
    thresh *= TOL;
    for(i=0;i<n;i++)
        if (w[i] < thresh)
	    w[i]=0.0;

    return 0;
}

#undef TOL