SVD分解C实现

int svd(int m,int n,int withu,int withv,double eps,double tol,
double *a, double *q, double *u, double *v, double *vt)
{

int i,j,k,l,l1,iter,retval;
double c,f,g,h,s,x,y,z;
double *e;

e = (double *)calloc(n,sizeof(double));

retval = 0;

/* Copy 'a' to 'u' */
for (i=0;i<m;i++) {
for (j=0;j<n;j++)
u[i*n + j] = a[i*n + j];
}

/* Householder's reduction to bidiagonal form. */
g = x = 0.0;

for (i=0;i<n;i++) {

e[i] = g;
s = 0.0;
l = i+1;

for (j=i;j<m;j++)
s += (u[j*n+i]*u[j*n+i]);

if (s < tol)
g = 0.0;

else {

f = u[i*n+i];

g = (f < 0) ? sqrt(s) : -sqrt(s);

h = f * g - s;

u[i*n+i] = f - g;

for (j=l;j<n;j++) {

s = 0.0;

for (k=i;k<m;k++)
s += (u[k*n+i] * u[k*n+j]);

f = s / h;

for (k=i;k<m;k++)
u[k*n+j] += (f * u[k*n+i]);

}
/* end j */
}
/* end s */
q[i] = g;

s = 0.0;

for (j=l;j<n;j++)
s += (u[i*n+j] * u[i*n+j]);

if (s < tol)
g = 0.0;

else {

f = u[i*n+i+1];

g = (f < 0) ? sqrt(s) : -sqrt(s);

h = f * g - s;

u[i*n+i+1] = f - g;

for (j=l;j<n;j++)
e[j] = u[i*n+j]/h;

for (j=l;j<m;j++) {

s = 0.0;

for (k=l;k<n;k++)
s += (u[j*n+k] * u[i*n+k]);

for (k=l;k<n;k++)
u[j*n+k] += (s * e[k]);

}
/* end j */
}
/* end s */
y = fabs(q[i]) + fabs(e[i]);

if (y > x)
x = y;

}
/* end i */

/* accumulation of right-hand transformations */
if (withv) {

for (i=n-1;i>=0;i--) {

if (g != 0.0) {

h = u[i*n+i+1] * g;

for (j=l;j<n;j++)
v[j*n+i] = u[i*n+j]/h;

for (j=l;j<n;j++) {

s = 0.0;

for (k=l;k<n;k++)
s += (u[i*n+k] * v[k*n+j]);

for (k=l;k<n;k++)
v[k*n+j] += (s * v[k*n+i]);

}
/* end j */
}
/* end g */
for (j=l;j<n;j++)
v[i*n+j] = v[j*n+i] = 0.0;

v[i*n+i] = 1.0;

g = e[i];

l = i;

}
/* end i */

}
/* end withv, parens added for clarity */

/* accumulation of left-hand transformations */
if (withu) {

for (i=n;i<m;i++) {

for (j=n;j<m;j++)
u[i*n+j] = 0.0;

u[i*n+i] = 1.0;

}

}

if (withu) {

for (i=n-1;i>=0;i--) {

l = i + 1;

g = q[i];

for (j=l;j<m;j++)  /* upper limit was 'n' */
u[i*n+j] = 0.0;

if (g != 0.0) {

h = u[i*n+i] * g;

for (j=l;j<m;j++) {
/* upper limit was 'n' */
s = 0.0;

for (k=l;k<m;k++)
s += (u[k*n+i] * u[k*n+j]);

f = s / h;

for (k=i;k<m;k++)
u[k*n+j] += (f * u[k*n+i]);

}
/* end j */
for (j=i;j<m;j++)
u[j*n+i] /= g;

}
/* end g */
else {

for (j=i;j<m;j++)
u[j*n+i] = 0.0;

}

u[i*n+i] += 1.0;

}
/* end i*/
}
/* end withu, parens added for clarity */

/* diagonalization of the bidiagonal form */
eps *= x;

for (k=n-1;k>=0;k--) {

iter = 0;

test_f_splitting:
for (l=k;l>=0;l--) {

if (fabs(e[l]) <= eps) goto test_f_convergence;

if (fabs(q[l-1]) <= eps) goto cancellation;

}
/* end l */

/* cancellation of e[l] if l > 0 */
cancellation:
c = 0.0;

s = 1.0;

l1 = l - 1;

for (i=l;i<=k;i++) {

f = s * e[i];

e[i] *= c;

if (fabs(f) <= eps) goto test_f_convergence;

g = q[i];

h = q[i] = sqrt(f*f + g*g);

c = g / h;

s = -f / h;

if (withu) {

for (j=0;j<m;j++) {

y = u[j*n+l1];

z = u[j*n+i];

u[j*n+l1] = y * c + z * s;

u[j*n+i] = -y * s + z * c;

}
/* end j */
}
/* end withu, parens added for clarity */
}
/* end i */
test_f_convergence:
z = q[k];

if (l == k) goto convergence;

/* shift from bottom 2x2 minor */
iter++;

if (iter > 30) {

retval = k;

break;

}

x = q[l];

y = q[k-1];

g = e[k-1];

h = e[k];

f = ((y-z)*(y+z) + (g-h)*(g+h)) / (2*h*y);

g = sqrt(f*f + 1.0);

f = ((x-z)*(x+z) + h*(y/((f<0)?(f-g):(f+g))-h))/x;

/* next QR transformation */
c = s = 1.0;

for (i=l+1;i<=k;i++) {

g = e[i];

y = q[i];

h = s * g;

g *= c;

e[i-1] = z = sqrt(f*f+h*h);

c = f / z;

s = h / z;

f = x * c + g * s;

g = -x * s + g * c;

h = y * s;

y *= c;

if (withv) {

for (j=0;j<n;j++) {

x = v[j*n+i-1];

z = v[j*n+i];

v[j*n+i-1] = x * c + z * s;

v[j*n+i] = -x * s + z * c;

}
/* end j */
}
/* end withv, parens added for clarity */
q[i-1] = z = sqrt(f*f + h*h);

c = f/z;

s = h/z;

f = c * g + s * y;

x = -s * g + c * y;

if (withu) {

for (j=0;j<m;j++) {

y = u[j*n+i-1];

z = u[j*n+i];

u[j*n+i-1] = y * c + z * s;

u[j*n+i] = -y * s + z * c;

}
/* end j */
}
/* end withu, parens added for clarity */
}
/* end i */
e[l] = 0.0;

e[k] = f;

q[k] = x;

goto test_f_splitting;

convergence:
if (z < 0.0) {

/* q[k] is made non-negative */
q[k] = - z;

if (withv) {

for (j=0;j<n;j++)
v[j*n+k] = -v[j*n+k];

}
/* end withv, parens added for clarity */
}
/* end z */
}
/* end k */

free(e);

matrix_transpose(m, n, v, vt);

return retval;

}