jube-fork/Accord.Math/AForge.Math/SVD.cs

// AForge Math Library
// AForge.NET framework
// http://www.aforgenet.com/framework/
//
// Copyright © AForge.NET, 2007-2014
// [email protected]
//

namespace Accord.Math
{
    using System;

    // Just a copy-paste of SVD algorithm from Numerical Recipes but updated for C#
    // (as authors state, the code is aimed to be machine readable, so blame them
    // for all those c/f/g/h/s variable)
    internal class Svd
    {
        public static void svdcmp(double[,] a, out double[] w, out double[,] v)
        {
            // number of rows in A
            int m = a.GetLength(0);
            // number of columns in A
            int n = a.GetLength(1);

            if (m < n)
            {
                throw new ArgumentException("Number of rows in A must be greater or equal to number of columns");
            }

            w = new double[n];
            v = new double[n, n];


            int flag, i, its, j, jj, k, l = 0, nm = 0;
            double anorm, c, f, g, h, s, scale, x, y, z;

            double[] rv1 = new double[n];

            // householder reduction to bidiagonal form
            g = scale = anorm = 0.0;

            for (i = 0; i < n; i++)
            {
                l = i + 1;
                rv1[i] = scale * g;
                g = s = scale = 0;

                if (i < m)
                {
                    for (k = i; k < m; k++)
                    {
                        scale += System.Math.Abs(a[k, i]);
                    }

                    if (scale != 0.0)
                    {
                        for (k = i; k < m; k++)
                        {
                            a[k, i] /= scale;
                            s += a[k, i] * a[k, i];
                        }

                        f = a[i, i];
                        g = -Sign(System.Math.Sqrt(s), f);
                        h = f * g - s;
                        a[i, i] = f - g;

                        if (i != n - 1)
                        {
                            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] = scale * g;
                g = s = scale = 0.0;

                if ((i < m) && (i != n - 1))
                {
                    for (k = l; k < n; k++)
                    {
                        scale += System.Math.Abs(a[i, k]);
                    }

                    if (scale != 0.0)
                    {
                        for (k = l; k < n; k++)
                        {
                            a[i, k] /= scale;
                            s += a[i, k] * a[i, k];
                        }

                        f = a[i, l];
                        g = -Sign(System.Math.Sqrt(s), f);
                        h = f * g - s;
                        a[i, l] = f - g;

                        for (k = l; k < n; k++)
                        {
                            rv1[k] = a[i, k] / h;
                        }

                        if (i != m - 1)
                        {
                            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 = System.Math.Max(anorm, (System.Math.Abs(w[i]) + System.Math.Abs(rv1[i])));
            }

            // accumulation of right-hand transformations
            for (i = n - 1; i >= 0; i--)
            {
                if (i < n - 1)
                {
                    if (g != 0.0)
                    {
                        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, 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;
                    }
                }
                v[i, i] = 1;
                g = rv1[i];
                l = i;
            }

            // accumulation of left-hand transformations
            for (i = n - 1; i >= 0; i--)
            {
                l = i + 1;
                g = w[i];

                if (i < n - 1)
                {
                    for (j = l; j < n; j++)
                    {
                        a[i, j] = 0.0;
                    }
                }

                if (g != 0)
                {
                    g = 1.0 / g;

                    if (i != n - 1)
                    {
                        for (j = l; j < n; j++)
                        {
                            for (s = 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;
                    }
                }
                ++a[i, i];
            }

            // diagonalization of the bidiagonal form: Loop over singular values
            // and over allowed iterations
            for (k = n - 1; k >= 0; k--)
            {
                for (its = 1; its <= 30; its++)
                {
                    flag = 1;

                    for (l = k; l >= 0; l--)
                    {
                        // test for splitting
                        nm = l - 1;

                        if (System.Math.Abs(rv1[l]) + anorm == anorm)
                        {
                            flag = 0;
                            break;
                        }

                        if (System.Math.Abs(w[nm]) + anorm == anorm)
                            break;
                    }

                    if (flag != 0)
                    {
                        c = 0.0;
                        s = 1.0;
                        for (i = l; i <= k; i++)
                        {
                            f = s * rv1[i];

                            if (System.Math.Abs(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 (l == k)
                    {
                        // convergence
                        if (z < 0.0)
                        {
                            // singular value is made nonnegative
                            w[k] = -z;

                            for (j = 0; j < n; j++)
                            {
                                v[j, k] = -v[j, k];
                            }
                        }
                        break;
                    }

                    if (its == 30)
                    {
                        throw new InvalidOperationException("No convergence in 30 svdcmp iterations");
                    }

                    // shift from bottom 2-by-2 minor
                    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 = Pythag(f, 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];
                        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 = 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 != 0)
                        {
                            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[l] = 0.0;
                    rv1[k] = f;
                    w[k] = x;
                }
            }
        }

        private static double Sign(double a, double b)
        {
            return (b >= 0.0) ? System.Math.Abs(a) : -System.Math.Abs(a);
        }

        private static double Pythag(double a, double b)
        {
            double at = System.Math.Abs(a), bt = System.Math.Abs(b), ct, result;

            if (at > bt)
            {
                ct = bt / at;
                result = at * System.Math.Sqrt(1.0 + ct * ct);
            }
            else if (bt > 0.0)
            {
                ct = at / bt;
                result = bt * System.Math.Sqrt(1.0 + ct * ct);
            }
            else
            {
                result = 0.0;
            }

            return result;
        }
    }
}