svdcmp.cpp

经典算法 主要用于程序员开发 和进行开始学习用

int sgn(double pa)
{
	if (pa>0.0)
	{
		return 1;
	}
	else
	{
		if (pa<0.0)
		{
			return -1;
		}
	}
	return 0;
}

void svdcmp(double a[], int m, int n, double w[], double v[])
{
	int i,j,k,l,its,nm,aaaaa,pp;
    double rv1[100];
    if (m < n)
	{
		cout<<"you must augment a with extra zero rows."<<endl;
	}
    double x,y,z,c,f,h,s,g = 0.0;
    double scale1 = 0.0;
    double anorm = 0.0;
    for (i = 1; i<=n;i++)
	{
        l = i + 1;
        rv1[i] = scale1 * g;
        g = 0.0;
        s = 0.0;
        scale1 = 0.0;
        if (i <= m)
		{
            for (k = i; k<=m; k++)
			{
                scale1 = scale1 + fabs(a[(k-1)*n+i]);
            }
            if (scale1 != 0.0)
			{
                for (k = i; k<=m; k++)
				{
                    a[(k-1)*n+i] = a[(k-1)*n+i] / scale1;
                    s = s + a[(k-1)*n+i] * a[(k-1)*n+i];
                }
                f = a[(i-1)*n+i];
                g = -sqrt(s) * sgn(f);
                h = f * g - s;
                a[(i-1)*n+i] = f - g;
                if (i != n)
				{
                    for (j = l; j<=n; j++)
					{
                        s = 0.0;
                        for (k = i; k<=m; k++)
						{
                            s = s + a[(k-1)*n+i] * a[(k-1)*n+j];
                        }
                        f = s / h;
                        for (k = i; k<=m; k++)
						{
                            a[(k-1)*n+j] = a[(k-1)*n+j] + f * a[(k-1)*n+i];
                        }
					}
                }
                for (k = i; k<=m; k++)
				{
                    a[(k-1)*n+i] = scale1 * a[(k-1)*n+i];
                }
            }
        }
        w[i] = scale1 * g;
        g = 0.0;
        s = 0.0;
        scale1 = 0.0;
        if ((i <= m) && (i != n))
		{
            for (k = l; k<=n; k++)
			{
                scale1 = scale1 + fabs(a[(i-1)*n+k]);
            }
            if (scale1 != 0.0)
			{
                for (k = l; k<=n; k++)
				{
                    a[(i-1)*n+k] = a[(i-1)*n+k] / scale1;
                    s = s + a[(i-1)*n+k] * a[(i-1)*n+k];
                }
                f = a[(i-1)*n+l];
                g = -sqrt(s) * sgn(f);
                h = f * g - s;
                a[(i-1)*n+l] = f - g;
                for (k = l; k<=n; k++)
				{
                    rv1[k] = a[(i-1)*n+k] / h;
                }
                if (i != m)
				{
                    for (j = l; j<=m; j++)
					{
                        s = 0.0;
                        for (k = l; k<=n; k++)
						{
                            s = s + a[(j-1)*n+k] * a[(i-1)*n+k];
                        }
                        for (k = l; k<=n; k++)
						{
                            a[(j-1)*n+k] = a[(j-1)*n+k] + s * rv1[k];
						}
					}
                }
                for (k = l; k<=n; k++)
				{
                    a[(i-1)*n+k] = scale1 * a[(i-1)*n+k];
                }
            }
        }
        if (anorm > (fabs(w[i]) + fabs(rv1[i])))
		{
            anorm = anorm;
		}
        else
		{
            anorm = fabs(w[i]) + fabs(rv1[i]);
		}
    }
    for (i=n; i>=1; i--)
	{
        if (i < n)
		{
            if (g != 0.0)
			{
                for (j = l; j<=n; j++)
				{
                    v[(j-1)*n+i] = (a[(i-1)*n+j] / a[(i-1)*n+l]) / g;
                }
                for (j = l; j<=n; j++)
				{
                    s = 0.0;
                    for (k = l; k<=n; k++)
					{
                        s = s + a[(i-1)*n+k] * v[(k-1)*n+j];
                    }
                    for (k = l; k<=n; k++)
					{
                        v[(k-1)*n+j] = v[(k-1)*n+j] + s * v[(k-1)*n+i];
                    }
                }
            }
            for (j = l; j<=n; j++)
			{
                v[(i-1)*n+j] = 0.0;
                v[(j-1)*n+i] = 0.0;
            }
        }
        v[(i-1)*n+i] = 1.0;
        g = rv1[i];
        l = i;
    }
    for (i = n; i>=1; i--)
	{
        l = i + 1;
        g = w[i];
        if (i < n)
		{
            for (j = l; j<=n; j++)
			{
                a[(i-1)*n+j] = 0.0;
			}
        }
        if (g != 0.0)
		{
            g = 1.0 / g;
            if (i != n)
			{
                for (j = l; j<=n; j++)
				{
                    s = 0.0;
                    for (k = l; k<=m; k++)
					{
                        s = s + a[(k-1)*n+i] * a[(k-1)*n+j];
                    }
                    f = (s / a[(i-1)*n+i]) * g;
                    for (k = i; k<=m; k++)
					{
                        a[(k-1)*n+j] = a[(k-1)*n+j] + f * a[(k-1)*n+i];
                    }
                }
            }
            for (j= i; j<=m; j++)
			{
                a[(j-1)*n+i] = a[(j-1)*n+i] * g;
            }
		}
        else
		{
            for (j= i; j<=m; j++)
			{
                a[(j-1)*n+i] = 0.0;
            }
        }
        a[(i-1)*n+i] = a[(i-1)*n+i] + 1.0;
    }
    for (k = n; k>=1; k--)
	{
        for (its = 1; its<=30; its++)
		{
            for (l = k; l>=1; l--)
			{
                nm = l - 1;
                if (fabs(rv1[l]) + anorm == anorm)
				{
					goto r2;
				}
                if (fabs(w[nm]) + anorm == anorm)
				{
					goto r1;
				}
            }
			r1: c = 0.0;
            s = 1.0;
            for (i = l; i<=k; i++)
			{
                f = s * rv1[i];
                if (fabs(f) + anorm != anorm)
				{
                    g = w[i];
                    h = sqrt(f * f + g * g);
                    w[i] = h;
                    h = 1.0 / h;
                    c = (g * h);
                    s = -(f * h);
                    for (j = 1; j<=m; j++)
					{
                        y = a[(j-1)*n+nm];
                        z = a[(j-1)*n+i];
                        a[(j-1)*n+nm] = (y * c) + (z * s);
                        a[(j-1)*n+i] = -(y * s) + (z * c);
                    }
                }
            }
			r2:           z = w[k];
            if (l == k)
			{
                if (z < 0.0)
				{
                    w[k] = -z;
                    for (j = 1; j<=n; j++)
					{
                        v[(j-1)*n+k] = -v[(j-1)*n+k];
                    }
                }
                goto r3;
            }
            if (its == 30)
			{
				cout<< "no convergence in 30 iterations"<<endl;
			}
            x = w[l];
            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 = sqrt(f * f + 1.0);
            f = ((x - z) * (x + z) + h * ((y / (f + fabs(g) * sgn(f))) - h)) / x;
            c = 1.0;
            s = 1.0;
            for (j = l; j<=nm; j++)
			{
                i = j + 1;
                g = rv1[i];
                y = w[i];
                h = s * g;
                g = g * c;
                z = sqrt(f * f + h * h);
                rv1[j] = z;
                c = f / z;
                s = h / z;
                f = (x * c) + (g * s);
                g = -(x * s) + (g * c);
                h = y * s;
                y = y * c;
                for (pp = 1; pp<=n; pp++)
				{
                    x = v[(pp-1)*n+j];
                    z = v[(pp-1)*n+i];
                    v[(pp-1)*n+j] = (x * c) + (z * s);
                    v[(pp-1)*n+i] = -(x * s) + (z * c);
                }
                z = sqrt(f * f + h * h);
                w[j] = z;
                if (z != 0.0)
				{
                    z = 1.0 / z;
                    c = f * z;
                    s = h * z;
                }
                f = (c * g) + (s * y);
                x = -(s * g) + (c * y);
                for (pp = 1; pp<=m; pp++)
				{
                    y = a[(pp-1)*n+j];
                    z = a[(pp-1)*n+i];
                    a[(pp-1)*n+j] = (y * c) + (z * s);
                    a[(pp-1)*n+i] = -(y * s) + (z * c);
                }
            }
            rv1[l] = 0.0;
            rv1[k] = f;
            w[k] = x;
        }
		r3:   aaaaa = 1;
    }
}