3x3矩阵svd

public class TestSvd
{
    private static float Sign(float a, float b)
    {
        if (!(b >= 0.0f))
        {
            return 0.0f - XMath.Abs(a);
        }
        return XMath.Abs(a);
    }
    private static float Pythag(float a, float b)
    {
        float num = XMath.Abs(a);
        float num2 = XMath.Abs(b);
        if (num > num2)
        {
            float num3 = num2 / num;
            return num * XMath.Sqrt(1.0f + num3 * num3);
        }
        if (num2 > 0.0)
        {
            float num3 = num / num2;
            return num2 * XMath.Sqrt(1.0f + num3 * num3);
        }
        return 0.0f;
    }
    static void svdcmp(float[,] a, out float[] w, out float[,] v)
    {
        int length = a.GetLength(0);
        int length2 = a.GetLength(1);
        if (length < length2)
        {
            //throw new ArgumentException("Number of rows in A must be greater or equal to number of columns");
        }
        w = new float[length2];
        v = new float[length2, length2];
        int num = 0;
        int num2 = 0;
        float[] array = new float[length2];
        float num3;
        float num4;
        float num5 = (num4 = (num3 = 0.0f));
        for (int i = 0; i < length2; i++)
        {
            num = i + 1;
            array[i] = num4 * num5;
            float num6;
            num5 = (num6 = (num4 = 0.0f));
            if (i < length)
            {
                for (int j = i; j < length; j++)
                {
                    num4 += XMath.Abs(a[j, i]);
                }
                if (num4 != 0.0)
                {
                    for (int j = i; j < length; j++)
                    {
                        a[j, i] /= num4;
                        num6 += a[j, i] * a[j, i];
                    }
                    float num7 = a[i, i];
                    num5 = 0.0f - Sign(XMath.Sqrt(num6), num7);
                    float num8 = num7 * num5 - num6;
                    a[i, i] = num7 - num5;
                    if (i != length2 - 1)
                    {
                        for (int k = num; k < length2; k++)
                        {
                            num6 = 0.0f;
                            for (int j = i; j < length; j++)
                            {
                                num6 += a[j, i] * a[j, k];
                            }
                            num7 = num6 / num8;
                            for (int j = i; j < length; j++)
                            {
                                a[j, k] += num7 * a[j, i];
                            }
                        }
                    }
                    for (int j = i; j < length; j++)
                    {
                        a[j, i] *= num4;
                    }
                }
            }
            w[i] = num4 * num5;
            num5 = (num6 = (num4 = 0.0f));
            if (i < length && i != length2 - 1)
            {
                for (int j = num; j < length2; j++)
                {
                    num4 += XMath.Abs(a[i, j]);
                }
                if (num4 != 0.0)
                {
                    for (int j = num; j < length2; j++)
                    {
                        a[i, j] /= num4;
                        num6 += a[i, j] * a[i, j];
                    }
                    float num7 = a[i, num];
                    num5 = 0.0f - Sign(XMath.Sqrt(num6), num7);
                    float num8 = num7 * num5 - num6;
                    a[i, num] = num7 - num5;
                    for (int j = num; j < length2; j++)
                    {
                        array[j] = a[i, j] / num8;
                    }
                    if (i != length - 1)
                    {
                        for (int k = num; k < length; k++)
                        {
                            num6 = 0.0f;
                            for (int j = num; j < length2; j++)
                            {
                                num6 += a[k, j] * a[i, j];
                            }
                            for (int j = num; j < length2; j++)
                            {
                                a[k, j] += num6 * array[j];
                            }
                        }
                    }
                    for (int j = num; j < length2; j++)
                    {
                        a[i, j] *= num4;
                    }
                }
            }
            num3 = XMath.Max(num3, XMath.Abs(w[i]) + XMath.Abs(array[i]));
        }
        for (int i = length2 - 1; i >= 0; i--)
        {
            if (i < length2 - 1)
            {
                if (num5 != 0.0f)
                {
                    for (int k = num; k < length2; k++)
                    {
                        v[k, i] = a[i, k] / a[i, num] / num5;
                    }
                    for (int k = num; k < length2; k++)
                    {
                        float num6 = 0.0f;
                        for (int j = num; j < length2; j++)
                        {
                            num6 += a[i, j] * v[j, k];
                        }
                        for (int j = num; j < length2; j++)
                        {
                            v[j, k] += num6 * v[j, i];
                        }
                    }
                }
                for (int k = num; k < length2; k++)
                {
                    v[i, k] = (v[k, i] = 0.0f);
                }
            }
            v[i, i] = 1.0f;
            num5 = array[i];
            num = i;
        }
        for (int i = length2 - 1; i >= 0; i--)
        {
            num = i + 1;
            num5 = w[i];
            if (i < length2 - 1)
            {
                for (int k = num; k < length2; k++)
                {
                    a[i, k] = 0.0f;
                }
            }
            if (num5 != 0.0)
            {
                num5 = 1.0f / num5;
                if (i != length2 - 1)
                {
                    for (int k = num; k < length2; k++)
                    {
                        float num6 = 0.0f;
                        for (int j = num; j < length; j++)
                        {
                            num6 += a[j, i] * a[j, k];
                        }
                        float num7 = num6 / a[i, i] * num5;
                        for (int j = i; j < length; j++)
                        {
                            a[j, k] += num7 * a[j, i];
                        }
                    }
                }
                for (int k = i; k < length; k++)
                {
                    a[k, i] *= num5;
                }
            }
            else
            {
                for (int k = i; k < length; k++)
                {
                    a[k, i] = 0.0f;
                }
            }
            a[i, i] += 1.0f;
        }
        for (int j = length2 - 1; j >= 0; j--)
        {
            for (int l = 1; l <= 30; l++)
            {
                int num9 = 1;
                for (num = j; num >= 0; num--)
                {
                    num2 = num - 1;
                    if (XMath.Abs(array[num]) + num3 == num3)
                    {
                        num9 = 0;
                        break;
                    }
                    if (XMath.Abs(w[num2]) + num3 == num3)
                    {
                        break;
                    }
                }
                float num10;
                float num6;
                float num7;
                float num8;
                float num11;
                float num12;
                if (num9 != 0)
                {
                    num10 = 0.0f;
                    num6 = 1.0f;
                    for (int i = num; i <= j; i++)
                    {
                        num7 = num6 * array[i];
                        if (XMath.Abs(num7) + num3 != num3)
                        {
                            num5 = w[i];
                            num8 = Pythag(num7, num5);
                            w[i] = num8;
                            num8 = 1.0f / num8;
                            num10 = num5 * num8;
                            num6 = (0.0f - num7) * num8;
                            for (int k = 0; k < length; k++)
                            {
                                num11 = a[k, num2];
                                num12 = a[k, i];
                                a[k, num2] = num11 * num10 + num12 * num6;
                                a[k, i] = num12 * num10 - num11 * num6;
                            }
                        }
                    }
                }
                num12 = w[j];
                if (num == j)
                {
                    if (num12 < 0.0f)
                    {
                        w[j] = 0.0f - num12;
                        for (int k = 0; k < length2; k++)
                        {
                            v[k, j] = 0.0f - v[k, j];
                        }
                    }
                    break;
                }
                if (l == 30)
                {
                    //throw new InvalidOperationException("No convergence in 30 svdcmp iterations");
                }
                float num13 = w[num];
                num2 = j - 1;
                num11 = w[num2];
                num5 = array[num2];
                num8 = array[j];
                num7 = ((num11 - num12) * (num11 + num12) + (num5 - num8) * (num5 + num8)) / (2.0f * num8 * num11);
                num5 = Pythag(num7, 1.0f);
                num7 = ((num13 - num12) * (num13 + num12) + num8 * (num11 / (num7 + Sign(num5, num7)) - num8)) / num13;
                num10 = (num6 = 1.0f);
                for (int k = num; k <= num2; k++)
                {
                    int i = k + 1;
                    num5 = array[i];
                    num11 = w[i];
                    num8 = num6 * num5;
                    num5 = num10 * num5;
                    num12 = (array[k] = Pythag(num7, num8));
                    num10 = num7 / num12;
                    num6 = num8 / num12;
                    num7 = num13 * num10 + num5 * num6;
                    num5 = num5 * num10 - num13 * num6;
                    num8 = num11 * num6;
                    num11 *= num10;
                    for (int m = 0; m < length2; m++)
                    {
                        num13 = v[m, k];
                        num12 = v[m, i];
                        v[m, k] = num13 * num10 + num12 * num6;
                        v[m, i] = num12 * num10 - num13 * num6;
                    }
                    num12 = Pythag(num7, num8);
                    w[k] = num12;
                    if (num12 != 0.0)
                    {
                        num12 = 1.0f / num12;
                        num10 = num7 * num12;
                        num6 = num8 * num12;
                    }
                    num7 = num10 * num5 + num6 * num11;
                    num13 = num10 * num11 - num6 * num5;
                    for (int m = 0; m < length; m++)
                    {
                        num11 = a[m, k];
                        num12 = a[m, i];
                        a[m, k] = num11 * num10 + num12 * num6;
                        a[m, i] = num12 * num10 - num11 * num6;
                    }
                }
                array[num] = 0.0f;
                array[j] = num7;
                w[j] = num13;
            }
        }


    }
    static void svdcmp(ref float3x3 a, out float3 w, out float3x3 v)
    {
        int length = 3;
        int length2 = 3;
        if (length < length2)
        {
            //throw new ArgumentException("Number of rows in A must be greater or equal to number of columns");
        }
        w = float3.zero;
        v = float3x3.zero;
        int num = 0;
        int num2 = 0;
        float[] array = new float[length2];
        float num3;
        float num4;
        float num5 = (num4 = (num3 = 0.0f));
        for (int i = 0; i < length2; i++)
        {
            num = i + 1;
            array[i] = num4 * num5;
            float num6;
            num5 = (num6 = (num4 = 0.0f));
            if (i < length)
            {
                for (int j = i; j < length; j++)
                {
                    num4 += XMath.Abs(a[j, i]);
                }
                if (num4 != 0.0)
                {
                    for (int j = i; j < length; j++)
                    {
                        a[j, i] /= num4;
                        num6 += a[j, i] * a[j, i];
                    }
                    float num7 = a[i, i];
                    num5 = 0.0f - Sign(XMath.Sqrt(num6), num7);
                    float num8 = num7 * num5 - num6;
                    a[i, i] = num7 - num5;
                    if (i != length2 - 1)
                    {
                        for (int k = num; k < length2; k++)
                        {
                            num6 = 0.0f;
                            for (int j = i; j < length; j++)
                            {
                                num6 += a[j, i] * a[j, k];
                            }
                            num7 = num6 / num8;
                            for (int j = i; j < length; j++)
                            {
                                a[j, k] += num7 * a[j, i];
                            }
                        }
                    }
                    for (int j = i; j < length; j++)
                    {
                        a[j, i] *= num4;
                    }
                }
            }
            w[i] = num4 * num5;
            num5 = (num6 = (num4 = 0.0f));
            if (i < length && i != length2 - 1)
            {
                for (int j = num; j < length2; j++)
                {
                    num4 += XMath.Abs(a[i, j]);
                }
                if (num4 != 0.0)
                {
                    for (int j = num; j < length2; j++)
                    {
                        a[i, j] /= num4;
                        num6 += a[i, j] * a[i, j];
                    }
                    float num7 = a[i, num];
                    num5 = 0.0f - Sign(XMath.Sqrt(num6), num7);
                    float num8 = num7 * num5 - num6;
                    a[i, num] = num7 - num5;
                    for (int j = num; j < length2; j++)
                    {
                        array[j] = a[i, j] / num8;
                    }
                    if (i != length - 1)
                    {
                        for (int k = num; k < length; k++)
                        {
                            num6 = 0.0f;
                            for (int j = num; j < length2; j++)
                            {
                                num6 += a[k, j] * a[i, j];
                            }
                            for (int j = num; j < length2; j++)
                            {
                                a[k, j] += num6 * array[j];
                            }
                        }
                    }
                    for (int j = num; j < length2; j++)
                    {
                        a[i, j] *= num4;
                    }
                }
            }
            num3 = XMath.Max(num3, XMath.Abs(w[i]) + XMath.Abs(array[i]));
        }
        for (int i = length2 - 1; i >= 0; i--)
        {
            if (i < length2 - 1)
            {
                if (num5 != 0.0f)
                {
                    for (int k = num; k < length2; k++)
                    {
                        v[k, i] = a[i, k] / a[i, num] / num5;
                    }
                    for (int k = num; k < length2; k++)
                    {
                        float num6 = 0.0f;
                        for (int j = num; j < length2; j++)
                        {
                            num6 += a[i, j] * v[j, k];
                        }
                        for (int j = num; j < length2; j++)
                        {
                            v[j, k] += num6 * v[j, i];
                        }
                    }
                }
                for (int k = num; k < length2; k++)
                {
                    v[i, k] = (v[k, i] = 0.0f);
                }
            }
            v[i, i] = 1.0f;
            num5 = array[i];
            num = i;
        }
        for (int i = length2 - 1; i >= 0; i--)
        {
            num = i + 1;
            num5 = w[i];
            if (i < length2 - 1)
            {
                for (int k = num; k < length2; k++)
                {
                    a[i, k] = 0.0f;
                }
            }
            if (num5 != 0.0)
            {
                num5 = 1.0f / num5;
                if (i != length2 - 1)
                {
                    for (int k = num; k < length2; k++)
                    {
                        float num6 = 0.0f;
                        for (int j = num; j < length; j++)
                        {
                            num6 += a[j, i] * a[j, k];
                        }
                        float num7 = num6 / a[i, i] * num5;
                        for (int j = i; j < length; j++)
                        {
                            a[j, k] += num7 * a[j, i];
                        }
                    }
                }
                for (int k = i; k < length; k++)
                {
                    a[k, i] *= num5;
                }
            }
            else
            {
                for (int k = i; k < length; k++)
                {
                    a[k, i] = 0.0f;
                }
            }
            a[i, i] += 1.0f;
        }
        for (int j = length2 - 1; j >= 0; j--)
        {
            for (int l = 1; l <= 30; l++)
            {
                int num9 = 1;
                for (num = j; num >= 0; num--)
                {
                    num2 = num - 1;
                    if (XMath.Abs(array[num]) + num3 == num3)
                    {
                        num9 = 0;
                        break;
                    }
                    if (XMath.Abs(w[num2]) + num3 == num3)
                    {
                        break;
                    }
                }
                float num10;
                float num6;
                float num7;
                float num8;
                float num11;
                float num12;
                if (num9 != 0)
                {
                    num10 = 0.0f;
                    num6 = 1.0f;
                    for (int i = num; i <= j; i++)
                    {
                        num7 = num6 * array[i];
                        if (XMath.Abs(num7) + num3 != num3)
                        {
                            num5 = w[i];
                            num8 = Pythag(num7, num5);
                            w[i] = num8;
                            num8 = 1.0f / num8;
                            num10 = num5 * num8;
                            num6 = (0.0f - num7) * num8;
                            for (int k = 0; k < length; k++)
                            {
                                num11 = a[k, num2];
                                num12 = a[k, i];
                                a[k, num2] = num11 * num10 + num12 * num6;
                                a[k, i] = num12 * num10 - num11 * num6;
                            }
                        }
                    }
                }
                num12 = w[j];
                if (num == j)
                {
                    if (num12 < 0.0f)
                    {
                        w[j] = 0.0f - num12;
                        for (int k = 0; k < length2; k++)
                        {
                            v[k, j] = 0.0f - v[k, j];
                        }
                    }
                    break;
                }
                if (l == 30)
                {
                    //throw new InvalidOperationException("No convergence in 30 svdcmp iterations");
                }
                float num13 = w[num];
                num2 = j - 1;
                num11 = w[num2];
                num5 = array[num2];
                num8 = array[j];
                num7 = ((num11 - num12) * (num11 + num12) + (num5 - num8) * (num5 + num8)) / (2.0f * num8 * num11);
                num5 = Pythag(num7, 1.0f);
                num7 = ((num13 - num12) * (num13 + num12) + num8 * (num11 / (num7 + Sign(num5, num7)) - num8)) / num13;
                num10 = (num6 = 1.0f);
                for (int k = num; k <= num2; k++)
                {
                    int i = k + 1;
                    num5 = array[i];
                    num11 = w[i];
                    num8 = num6 * num5;
                    num5 = num10 * num5;
                    num12 = (array[k] = Pythag(num7, num8));
                    num10 = num7 / num12;
                    num6 = num8 / num12;
                    num7 = num13 * num10 + num5 * num6;
                    num5 = num5 * num10 - num13 * num6;
                    num8 = num11 * num6;
                    num11 *= num10;
                    for (int m = 0; m < length2; m++)
                    {
                        num13 = v[m, k];
                        num12 = v[m, i];
                        v[m, k] = num13 * num10 + num12 * num6;
                        v[m, i] = num12 * num10 - num13 * num6;
                    }
                    num12 = Pythag(num7, num8);
                    w[k] = num12;
                    if (num12 != 0.0)
                    {
                        num12 = 1.0f / num12;
                        num10 = num7 * num12;
                        num6 = num8 * num12;
                    }
                    num7 = num10 * num5 + num6 * num11;
                    num13 = num10 * num11 - num6 * num5;
                    for (int m = 0; m < length; m++)
                    {
                        num11 = a[m, k];
                        num12 = a[m, i];
                        a[m, k] = num11 * num10 + num12 * num6;
                        a[m, i] = num12 * num10 - num11 * num6;
                    }
                }
                array[num] = 0.0f;
                array[j] = num7;
                w[j] = num13;
            }
        }
    }
    public static void SVDTest(float3x3 F, out float3x3 u, out float3x3 v, out float3x3 sigma)
    {
        float[,] array = new float[3, 3]
        {
            { F[0,0], F[0,1], F[0,2] },
            { F[1,0], F[1,1], F[1,2] },
            { F[2,0], F[2,1], F[2,2] }
        };
        svdcmp(array, out var w, out var v2);
        u = float3x3.zero;
        u[0, 0] = (float)array[0, 0];
        u[0, 1] = (float)array[0, 1];
        u[0, 2] = (float)array[0, 2];
        u[1, 0] = (float)array[1, 0];
        u[1, 1] = (float)array[1, 1];
        u[1, 2] = (float)array[1, 2];
        u[2, 0] = (float)array[2, 0];
        u[2, 1] = (float)array[2, 1];
        u[2, 2] = (float)array[2, 2];
        v = float3x3.zero;
        v[0, 0] = (float)v2[0, 0];
        v[0, 1] = (float)v2[0, 1];
        v[0, 2] = (float)v2[0, 2];
        v[1, 0] = (float)v2[1, 0];
        v[1, 1] = (float)v2[1, 1];
        v[1, 2] = (float)v2[1, 2];
        v[2, 0] = (float)v2[2, 0];
        v[2, 1] = (float)v2[2, 1];
        v[2, 2] = (float)v2[2, 2];
        sigma = float3x3.zero;
        sigma[0, 0] = (float)w[0];
        sigma[1, 1] = (float)w[1];
        sigma[2, 2] = (float)w[2];
    }
    public static void SVD(float3x3 F, out float3x3 u, out float3x3 v, out float3x3 sigma)
    {
        //float[,] array = new float[3, 3]
        //{
        //    { F[0,0], F[0,1], F[0,2] },
        //    { F[1,0], F[1,1], F[1,2] },
        //    { F[2,0], F[2,1], F[2,2] }
        //};
        float3x3 FCopy = new float3x3(F);
        svdcmp(ref FCopy, out float3 w, out float3x3 v2);
        u = FCopy;
        #region MyRegion
        //u[0, 0] = (float)array[0, 0];
        //u[0, 1] = (float)array[0, 1];
        //u[0, 2] = (float)array[0, 2];
        //u[1, 0] = (float)array[1, 0];
        //u[1, 1] = (float)array[1, 1];
        //u[1, 2] = (float)array[1, 2];
        //u[2, 0] = (float)array[2, 0];
        //u[2, 1] = (float)array[2, 1];
        //u[2, 2] = (float)array[2, 2]; 
        #endregion
        v = v2;
        #region MyRegion
        //v[0, 0] = (float)v2[0, 0];
        //v[0, 1] = (float)v2[0, 1];
        //v[0, 2] = (float)v2[0, 2];
        //v[1, 0] = (float)v2[1, 0];
        //v[1, 1] = (float)v2[1, 1];
        //v[1, 2] = (float)v2[1, 2];
        //v[2, 0] = (float)v2[2, 0];
        //v[2, 1] = (float)v2[2, 1];
        //v[2, 2] = (float)v2[2, 2]; 
        #endregion
        sigma = float3x3.zero;
        sigma[0, 0] = w[0];
        sigma[1, 1] = w[1];
        sigma[2, 2] = w[2];
    }
}