public void Solve(Matrix3d matrix, double epsilon = MathUtil.Epsilon, int jacobiIters = -1)
        {
            if (jacobiIters != -1)
            {
                NumJacobiIterations = jacobiIters;
            }

            if (ATA == null)
            {
                ATA = new SymmetricMatrix3d();
            }

            ATA.SetATA(ref matrix);

            Vector4d v = jacobiDiagonalize(ATA);

            if (AV == null)
            {
                AV = new double[9];
            }

            computeAV(ref matrix, ref v, AV);

            Vector4d u = Vector4d.Zero;

            QRFactorize(AV, ref v, epsilon, ref S, ref u);

            //u,v are quaternions in (s, x, y, z) order
            U = new Quaterniond(u[1], u[2], u[3], u[0]);
            V = new Quaterniond(v[1], v[2], v[3], v[0]);
        }
        Vector4d jacobiDiagonalize(SymmetricMatrix3d ATA)
        {
            var V = new Vector4d(1, 0, 0, 0);

            for (int i = 0; i < NumJacobiIterations; ++i)
            {
                Vector2d givens = givensAngles(ATA, 0, 1);
                ATA.quatConjugate01(givens.x, givens.y);
                quatTimesEqualCoordinateAxis(ref V, givens.x, givens.y, 2);

                givens = givensAngles(ATA, 1, 2);
                ATA.quatConjugate12(givens.x, givens.y);
                quatTimesEqualCoordinateAxis(ref V, givens.x, givens.y, 0);

                givens = givensAngles(ATA, 0, 2);
                ATA.quatConjugate02(givens.x, givens.y);
                quatTimesEqualCoordinateAxis(ref V, givens.x, givens.y, 1);
            }

            return(V);
        }
        /// <summary>
        /// compute givens angles of B for (p,q). Only
        /// ever called with p,q as [0,1], [0,2], or [1,2]
        /// </summary>
        Vector2d givensAngles(SymmetricMatrix3d B, int p, int q)
        {
            double ch = 0, sh = 0;

            if (p == 0)
            {
                if (q == 1)
                {
                    ch = B.entries[p] - B.entries[q];
                    sh = 0.5 * B.entries[3];
                }
                else
                {
                    ch = B.entries[q] - B.entries[p];
                    sh = 0.5 * B.entries[4];
                }
            }
            else if (p == 1 /* && q == 2 */)
            {
                ch = B.entries[p] - B.entries[q];
                sh = 0.5 * B.entries[5];
            }

            // [TODO] can use fast reciprocal square root here...
            double omega = 1.0 / Math.Sqrt(ch * ch + sh * sh);

            ch *= omega;
            sh *= omega;

            bool approxValid = (gamma * sh * sh) < (ch * ch);

            ch = approxValid ? ch : cosBackup;
            sh = approxValid ? sh : sinBackup;

            return(new Vector2d(ch, sh));
        }