gtk2/demos/gtk-demo/singular_value_decomposition.c

397 lines
11 KiB
C
Raw Normal View History

#include <string.h>
#include <float.h>
#include <math.h>
#include <glib.h>
/* See Golub and Reinsch,
* "Handbook for Automatic Computation vol II - Linear Algebra",
* Springer, 1971
*/
#define MAX_ITERATION_COUNT 30
static void
householder_reduction (double *A,
int nrows,
int ncols,
double *U,
double *V,
double *diagonal,
double *superdiagonal)
{
int i, j, k, ip1;
double s, s2, si, scale;
double *pu, *pui, *pv, *pvi;
double half_norm_squared;
memcpy (U, A, sizeof (double) * nrows * ncols);
diagonal[0] = 0.0;
s = 0.0;
scale = 0.0;
for (i = 0, pui = U, ip1 = 1;
i < ncols;
pui += ncols, i++, ip1++)
{
superdiagonal[i] = scale * s;
for (j = i, pu = pui, scale = 0.0;
j < nrows;
j++, pu += ncols)
scale += fabs( *(pu + i) );
if (scale > 0.0)
{
for (j = i, pu = pui, s2 = 0.0; j < nrows; j++, pu += ncols)
{
*(pu + i) /= scale;
s2 += *(pu + i) * *(pu + i);
}
s = *(pui + i) < 0.0 ? sqrt (s2) : -sqrt (s2);
half_norm_squared = *(pui + i) * s - s2;
*(pui + i) -= s;
for (j = ip1; j < ncols; j++)
{
for (k = i, si = 0.0, pu = pui; k < nrows; k++, pu += ncols)
si += *(pu + i) * *(pu + j);
si /= half_norm_squared;
for (k = i, pu = pui; k < nrows; k++, pu += ncols)
*(pu + j) += si * *(pu + i);
}
}
for (j = i, pu = pui; j < nrows; j++, pu += ncols)
*(pu + i) *= scale;
diagonal[i] = s * scale;
s = 0.0;
scale = 0.0;
if (i >= nrows || i == ncols - 1)
continue;
for (j = ip1; j < ncols; j++)
scale += fabs (*(pui + j));
if (scale > 0.0)
{
for (j = ip1, s2 = 0.0; j < ncols; j++)
{
*(pui + j) /= scale;
s2 += *(pui + j) * *(pui + j);
}
s = *(pui + ip1) < 0.0 ? sqrt (s2) : -sqrt (s2);
half_norm_squared = *(pui + ip1) * s - s2;
*(pui + ip1) -= s;
for (k = ip1; k < ncols; k++)
superdiagonal[k] = *(pui + k) / half_norm_squared;
if (i < (nrows - 1))
{
for (j = ip1, pu = pui + ncols; j < nrows; j++, pu += ncols)
{
for (k = ip1, si = 0.0; k < ncols; k++)
si += *(pui + k) * *(pu + k);
for (k = ip1; k < ncols; k++)
*(pu + k) += si * superdiagonal[k];
}
}
for (k = ip1; k < ncols; k++)
*(pui + k) *= scale;
}
}
pui = U + ncols * (ncols - 2);
pvi = V + ncols * (ncols - 1);
*(pvi + ncols - 1) = 1.0;
s = superdiagonal[ncols - 1];
pvi -= ncols;
for (i = ncols - 2, ip1 = ncols - 1;
i >= 0;
i--, pui -= ncols, pvi -= ncols, ip1--)
{
if (s != 0.0)
{
pv = pvi + ncols;
for (j = ip1; j < ncols; j++, pv += ncols)
*(pv + i) = ( *(pui + j) / *(pui + ip1) ) / s;
for (j = ip1; j < ncols; j++)
{
si = 0.0;
for (k = ip1, pv = pvi + ncols; k < ncols; k++, pv += ncols)
si += *(pui + k) * *(pv + j);
for (k = ip1, pv = pvi + ncols; k < ncols; k++, pv += ncols)
*(pv + j) += si * *(pv + i);
}
}
pv = pvi + ncols;
for (j = ip1; j < ncols; j++, pv += ncols)
{
*(pvi + j) = 0.0;
*(pv + i) = 0.0;
}
*(pvi + i) = 1.0;
s = superdiagonal[i];
}
pui = U + ncols * (ncols - 1);
for (i = ncols - 1, ip1 = ncols;
i >= 0;
ip1 = i, i--, pui -= ncols)
{
s = diagonal[i];
for (j = ip1; j < ncols; j++)
*(pui + j) = 0.0;
if (s != 0.0)
{
for (j = ip1; j < ncols; j++)
{
si = 0.0;
pu = pui + ncols;
for (k = ip1; k < nrows; k++, pu += ncols)
si += *(pu + i) * *(pu + j);
si = (si / *(pui + i)) / s;
for (k = i, pu = pui; k < nrows; k++, pu += ncols)
*(pu + j) += si * *(pu + i);
}
for (j = i, pu = pui; j < nrows; j++, pu += ncols)
*(pu + i) /= s;
}
else
for (j = i, pu = pui; j < nrows; j++, pu += ncols)
*(pu + i) = 0.0;
*(pui + i) += 1.0;
}
}
static int
givens_reduction (int nrows,
int ncols,
double *U,
double *V,
double *diagonal,
double *superdiagonal)
{
double epsilon;
double c, s;
double f,g,h;
double x,y,z;
double *pu, *pv;
int i,j,k,m;
int rotation_test;
int iteration_count;
for (i = 0, x = 0.0; i < ncols; i++)
{
y = fabs (diagonal[i]) + fabs (superdiagonal[i]);
if (x < y)
x = y;
}
epsilon = x * DBL_EPSILON;
for (k = ncols - 1; k >= 0; k--)
{
iteration_count = 0;
while (1)
{
rotation_test = 1;
for (m = k; m >= 0; m--)
{
if (fabs (superdiagonal[m]) <= epsilon)
{
rotation_test = 0;
break;
}
if (fabs (diagonal[m-1]) <= epsilon)
break;
}
if (rotation_test)
{
c = 0.0;
s = 1.0;
for (i = m; i <= k; i++)
{
f = s * superdiagonal[i];
superdiagonal[i] *= c;
if (fabs (f) <= epsilon)
break;
g = diagonal[i];
h = sqrt (f*f + g*g);
diagonal[i] = h;
c = g / h;
s = -f / h;
for (j = 0, pu = U; j < nrows; j++, pu += ncols)
{
y = *(pu + m - 1);
z = *(pu + i);
*(pu + m - 1 ) = y * c + z * s;
*(pu + i) = -y * s + z * c;
}
}
}
z = diagonal[k];
if (m == k)
{
if (z < 0.0)
{
diagonal[k] = -z;
for (j = 0, pv = V; j < ncols; j++, pv += ncols)
*(pv + k) = - *(pv + k);
}
break;
}
else
{
if (iteration_count >= MAX_ITERATION_COUNT)
return -1;
iteration_count++;
x = diagonal[m];
y = diagonal[k-1];
g = superdiagonal[k-1];
h = superdiagonal[k];
f = ((y - z) * ( y + z ) + (g - h) * (g + h))/(2.0 * h * y);
g = sqrt (f * f + 1.0);
if (f < 0.0)
g = -g;
f = ((x - z) * (x + z) + h * (y / (f + g) - h)) / x;
c = 1.0;
s = 1.0;
for (i = m + 1; i <= k; i++)
{
g = superdiagonal[i];
y = diagonal[i];
h = s * g;
g *= c;
z = sqrt (f * f + h * h);
superdiagonal[i-1] = z;
c = f / z;
s = h / z;
f = x * c + g * s;
g = -x * s + g * c;
h = y * s;
y *= c;
for (j = 0, pv = V; j < ncols; j++, pv += ncols)
{
x = *(pv + i - 1);
z = *(pv + i);
*(pv + i - 1) = x * c + z * s;
*(pv + i) = -x * s + z * c;
}
z = sqrt (f * f + h * h);
diagonal[i - 1] = z;
if (z != 0.0)
{
c = f / z;
s = h / z;
}
f = c * g + s * y;
x = -s * g + c * y;
for (j = 0, pu = U; j < nrows; j++, pu += ncols)
{
y = *(pu + i - 1);
z = *(pu + i);
*(pu + i - 1) = c * y + s * z;
*(pu + i) = -s * y + c * z;
}
}
superdiagonal[m] = 0.0;
superdiagonal[k] = f;
diagonal[k] = x;
}
}
}
return 0;
}
static void
sort_singular_values (int nrows,
int ncols,
double *S,
double *U,
double *V)
{
int i, j, max_index;
double temp;
double *p1, *p2;
for (i = 0; i < ncols - 1; i++)
{
max_index = i;
for (j = i + 1; j < ncols; j++)
if (S[j] > S[max_index])
max_index = j;
if (max_index == i)
continue;
temp = S[i];
S[i] = S[max_index];
S[max_index] = temp;
p1 = U + max_index;
p2 = U + i;
for (j = 0; j < nrows; j++, p1 += ncols, p2 += ncols)
{
temp = *p1;
*p1 = *p2;
*p2 = temp;
}
p1 = V + max_index;
p2 = V + i;
for (j = 0; j < ncols; j++, p1 += ncols, p2 += ncols)
{
temp = *p1;
*p1 = *p2;
*p2 = temp;
}
}
}
int
singular_value_decomposition (double *A,
int nrows,
int ncols,
double *U,
double *S,
double *V)
{
double *superdiagonal;
superdiagonal = g_alloca (sizeof (double) * ncols);
if (nrows < ncols)
return -1;
householder_reduction (A, nrows, ncols, U, V, S, superdiagonal);
if (givens_reduction (nrows, ncols, U, V, S, superdiagonal) < 0)
return -1;
sort_singular_values (nrows, ncols, S, U, V);
return 0;
}
void
singular_value_decomposition_solve (double *U,
double *S,
double *V,
int nrows,
int ncols,
double *B,
double *x)
{
int i, j, k;
double *pu, *pv;
double d;
double tolerance;
tolerance = DBL_EPSILON * S[0] * (double) ncols;
for ( i = 0, pv = V; i < ncols; i++, pv += ncols)
{
x[i] = 0.0;
for (j = 0; j < ncols; j++)
{
if (S[j] > tolerance)
{
for (k = 0, d = 0.0, pu = U; k < nrows; k++, pu += ncols)
d += *(pu + j) * B[k];
x[i] += d * *(pv + j) / S[j];
}
}
}
}