Calculates Singular Value Decomposition

SingularValueDecomposition.cs

public static void SVD(Matrix2x2 A, out Matrix2x2 U, out Vector2 S, out Matrix2x2 V) {
	    if (Mathf.Abs(A.m01 - A.m10) < MATRIX_EPSILON && Mathf.Abs(A.m01) < MATRIX_EPSILON){
		    V = new Matrix2x2(A.m00 < 0 ? -1 : 1, 0, 0, A.m11 < 0 ? -1 : 1);
            S = new Vector2(Mathf.Abs(A.m00), Mathf.Abs(A.m11));
		    U = Identity();
        } else{
            float j = A.m00 * A.m00 + A.m01 * A.m01;
            float k = A.m10 * A.m10 + A.m11 * A.m11;
            float v_c = A.m00 * A.m10 + A.m01 * A.m11;

		    //Check to see if A^T*A is diagonal
		    if (Mathf.Abs(v_c) < MATRIX_EPSILON){
                float s1 = Mathf.Sqrt(j);
                float s2 = Mathf.Abs(j - k) < MATRIX_EPSILON ? s1 : Mathf.Sqrt(k);
                S = new Vector2(s1, s2);
                U = Identity();
                V = new Matrix2x2(A.m00/s1, A.m10/s2,A.m01/s1, A.m11/s2);
		    } else{
                float jmk = j - k;
                float jpk = j + k;
                float root = Mathf.Sqrt(jmk * jmk + 4f * v_c * v_c);
                float eig = (jpk + root) / 2f;
                float s1 = Mathf.Sqrt(eig);
                float s2 = Mathf.Abs(root) < MATRIX_EPSILON ? s1 : Mathf.Sqrt((jpk - root) / 2);
                S = new Vector2(s1, s2);

                //Use eigenvectors of A^T*A as V
                float v_s = eig - j, len = Mathf.Sqrt(v_s * v_s + v_c * v_c);
                v_c /= len;
			    v_s /= len;
			    U = new Matrix2x2(v_c, -v_s, v_s, v_c);

                //Compute w matrix as Av/s
                V = new Matrix2x2(
				    (A.m00*v_c + A.m10*v_s)/s1,
				    (A.m10* v_c - A.m00* v_s)/s2,
				    (A.m01* v_c + A.m11* v_s)/s1,
				    (A.m11* v_c - A.m01* v_s)/s2
			    );
		    }
	    }
    }