// David Eberly, Geometric Tools, Redmond WA 98052
// Copyright (c) 1998-2020
// Distributed under the Boost Software License, Version 1.0.
// https://www.boost.org/LICENSE_1_0.txt
// https://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
// Version: 4.0.2019.08.16

#pragma once

#include <Mathematics/Math.h>
#include <algorithm>
#include <array>

// The document
// https://www.geometrictools.com/Documentation/RobustEigenSymmetric3x3.pdf
// describes algorithms for solving the eigensystem associated with a 3x3
// symmetric real-valued matrix.  The iterative algorithm is implemented
// by class SymmmetricEigensolver3x3.  The noniterative algorithm is
// implemented by class NISymmetricEigensolver3x3.  The code does not use
// GTEngine objects.

namespace WwiseGTE
{
    template <typename Real>
    class SortEigenstuff
    {
    public:
        void operator()(int sortType, bool isRotation,
            std::array<Real, 3>& eval, std::array<std::array<Real, 3>, 3>& evec)
        {
            if (sortType != 0)
            {
                // Sort the eigenvalues to eval[0] <= eval[1] <= eval[2].
                std::array<size_t, 3> index;
                if (eval[0] < eval[1])
                {
                    if (eval[2] < eval[0])
                    {
                        // even permutation
                        index[0] = 2;
                        index[1] = 0;
                        index[2] = 1;
                    }
                    else if (eval[2] < eval[1])
                    {
                        // odd permutation
                        index[0] = 0;
                        index[1] = 2;
                        index[2] = 1;
                        isRotation = !isRotation;
                    }
                    else
                    {
                        // even permutation
                        index[0] = 0;
                        index[1] = 1;
                        index[2] = 2;
                    }
                }
                else
                {
                    if (eval[2] < eval[1])
                    {
                        // odd permutation
                        index[0] = 2;
                        index[1] = 1;
                        index[2] = 0;
                        isRotation = !isRotation;
                    }
                    else if (eval[2] < eval[0])
                    {
                        // even permutation
                        index[0] = 1;
                        index[1] = 2;
                        index[2] = 0;
                    }
                    else
                    {
                        // odd permutation
                        index[0] = 1;
                        index[1] = 0;
                        index[2] = 2;
                        isRotation = !isRotation;
                    }
                }

                if (sortType == -1)
                {
                    // The request is for eval[0] >= eval[1] >= eval[2]. This
                    // requires an odd permutation, (i0,i1,i2) -> (i2,i1,i0).
                    std::swap(index[0], index[2]);
                    isRotation = !isRotation;
                }

                std::array<Real, 3> unorderedEVal = eval;
                std::array<std::array<Real, 3>, 3> unorderedEVec = evec;
                for (size_t j = 0; j < 3; ++j)
                {
                    size_t i = index[j];
                    eval[j] = unorderedEVal[i];
                    evec[j] = unorderedEVec[i];
                }
            }

            // Ensure the ordered eigenvectors form a right-handed basis.
            if (!isRotation)
            {
                for (size_t j = 0; j < 3; ++j)
                {
                    evec[2][j] = -evec[2][j];
                }
            }
        }
    };

    template <typename Real>
    class SymmetricEigensolver3x3
    {
    public:
        // The input matrix must be symmetric, so only the unique elements
        // must be specified: a00, a01, a02, a11, a12, and a22.
        //
        // If 'aggressive' is 'true', the iterations occur until a
        // superdiagonal entry is exactly zero.  If 'aggressive' is 'false',
        // the iterations occur until a superdiagonal entry is effectively
        // zero compared to the/ sum of magnitudes of its diagonal neighbors.
        // Generally, the nonaggressive convergence is acceptable.
        //
        // The order of the eigenvalues is specified by sortType:
        // -1 (decreasing), 0 (no sorting) or +1 (increasing).  When sorted,
        // the eigenvectors are ordered accordingly, and
        // {evec[0], evec[1], evec[2]} is guaranteed to/ be a right-handed
        // orthonormal set.  The return value is the number of iterations
        // used by the algorithm.

        int operator()(Real a00, Real a01, Real a02, Real a11, Real a12, Real a22,
            bool aggressive, int sortType, std::array<Real, 3>& eval,
            std::array<std::array<Real, 3>, 3>& evec) const
        {
            // Compute the Householder reflection H and B = H*A*H, where
            // b02 = 0.
            Real const zero = (Real)0, one = (Real)1, half = (Real)0.5;
            bool isRotation = false;
            Real c, s;
            GetCosSin(a12, -a02, c, s);
            Real Q[3][3] = { { c, s, zero }, { s, -c, zero }, { zero, zero, one } };
            Real term0 = c * a00 + s * a01;
            Real term1 = c * a01 + s * a11;
            Real b00 = c * term0 + s * term1;
            Real b01 = s * term0 - c * term1;
            term0 = s * a00 - c * a01;
            term1 = s * a01 - c * a11;
            Real b11 = s * term0 - c * term1;
            Real b12 = s * a02 - c * a12;
            Real b22 = a22;

            // Givens reflections, B' = G^T*B*G, preserve tridiagonal
            // matrices.
            int const maxIteration = 2 * (1 + std::numeric_limits<Real>::digits -
                std::numeric_limits<Real>::min_exponent);
            int iteration;
            Real c2, s2;

            if (std::fabs(b12) <= std::fabs(b01))
            {
                Real saveB00, saveB01, saveB11;
                for (iteration = 0; iteration < maxIteration; ++iteration)
                {
                    // Compute the Givens reflection.
                    GetCosSin(half * (b00 - b11), b01, c2, s2);
                    s = std::sqrt(half * (one - c2));  // >= 1/sqrt(2)
                    c = half * s2 / s;

                    // Update Q by the Givens reflection.
                    Update0(Q, c, s);
                    isRotation = !isRotation;

                    // Update B <- Q^T*B*Q, ensuring that b02 is zero and
                    // |b12| has strictly decreased.
                    saveB00 = b00;
                    saveB01 = b01;
                    saveB11 = b11;
                    term0 = c * saveB00 + s * saveB01;
                    term1 = c * saveB01 + s * saveB11;
                    b00 = c * term0 + s * term1;
                    b11 = b22;
                    term0 = c * saveB01 - s * saveB00;
                    term1 = c * saveB11 - s * saveB01;
                    b22 = c * term1 - s * term0;
                    b01 = s * b12;
                    b12 = c * b12;

                    if (Converged(aggressive, b00, b11, b01))
                    {
                        // Compute the Householder reflection.
                        GetCosSin(half * (b00 - b11), b01, c2, s2);
                        s = std::sqrt(half * (one - c2));
                        c = half * s2 / s;  // >= 1/sqrt(2)

                        // Update Q by the Householder reflection.
                        Update2(Q, c, s);
                        isRotation = !isRotation;

                        // Update D = Q^T*B*Q.
                        saveB00 = b00;
                        saveB01 = b01;
                        saveB11 = b11;
                        term0 = c * saveB00 + s * saveB01;
                        term1 = c * saveB01 + s * saveB11;
                        b00 = c * term0 + s * term1;
                        term0 = s * saveB00 - c * saveB01;
                        term1 = s * saveB01 - c * saveB11;
                        b11 = s * term0 - c * term1;
                        break;
                    }
                }
            }
            else
            {
                Real saveB11, saveB12, saveB22;
                for (iteration = 0; iteration < maxIteration; ++iteration)
                {
                    // Compute the Givens reflection.
                    GetCosSin(half * (b22 - b11), b12, c2, s2);
                    s = std::sqrt(half * (one - c2));  // >= 1/sqrt(2)
                    c = half * s2 / s;

                    // Update Q by the Givens reflection.
                    Update1(Q, c, s);
                    isRotation = !isRotation;

                    // Update B <- Q^T*B*Q, ensuring that b02 is zero and
                    // |b12| has strictly decreased.  MODIFY...
                    saveB11 = b11;
                    saveB12 = b12;
                    saveB22 = b22;
                    term0 = c * saveB22 + s * saveB12;
                    term1 = c * saveB12 + s * saveB11;
                    b22 = c * term0 + s * term1;
                    b11 = b00;
                    term0 = c * saveB12 - s * saveB22;
                    term1 = c * saveB11 - s * saveB12;
                    b00 = c * term1 - s * term0;
                    b12 = s * b01;
                    b01 = c * b01;

                    if (Converged(aggressive, b11, b22, b12))
                    {
                        // Compute the Householder reflection.
                        GetCosSin(half * (b11 - b22), b12, c2, s2);
                        s = std::sqrt(half * (one - c2));
                        c = half * s2 / s;  // >= 1/sqrt(2)

                        // Update Q by the Householder reflection.
                        Update3(Q, c, s);
                        isRotation = !isRotation;

                        // Update D = Q^T*B*Q.
                        saveB11 = b11;
                        saveB12 = b12;
                        saveB22 = b22;
                        term0 = c * saveB11 + s * saveB12;
                        term1 = c * saveB12 + s * saveB22;
                        b11 = c * term0 + s * term1;
                        term0 = s * saveB11 - c * saveB12;
                        term1 = s * saveB12 - c * saveB22;
                        b22 = s * term0 - c * term1;
                        break;
                    }
                }
            }

            eval = { b00, b11, b22 };
            for (size_t row = 0; row < 3; ++row)
            {
                for (size_t col = 0; col < 3; ++col)
                {
                    evec[row][col] = Q[col][row];
                }
            }

            SortEigenstuff<Real>()(sortType, isRotation, eval, evec);
            return iteration;
        }

    private:
        // Update Q = Q*G in-place using G = {{c,0,-s},{s,0,c},{0,0,1}}.
        void Update0(Real Q[3][3], Real c, Real s) const
        {
            for (int r = 0; r < 3; ++r)
            {
                Real tmp0 = c * Q[r][0] + s * Q[r][1];
                Real tmp1 = Q[r][2];
                Real tmp2 = c * Q[r][1] - s * Q[r][0];
                Q[r][0] = tmp0;
                Q[r][1] = tmp1;
                Q[r][2] = tmp2;
            }
        }

        // Update Q = Q*G in-place using G = {{0,1,0},{c,0,s},{-s,0,c}}.
        void Update1(Real Q[3][3], Real c, Real s) const
        {
            for (int r = 0; r < 3; ++r)
            {
                Real tmp0 = c * Q[r][1] - s * Q[r][2];
                Real tmp1 = Q[r][0];
                Real tmp2 = c * Q[r][2] + s * Q[r][1];
                Q[r][0] = tmp0;
                Q[r][1] = tmp1;
                Q[r][2] = tmp2;
            }
        }

        // Update Q = Q*H in-place using H = {{c,s,0},{s,-c,0},{0,0,1}}.
        void Update2(Real Q[3][3], Real c, Real s) const
        {
            for (int r = 0; r < 3; ++r)
            {
                Real tmp0 = c * Q[r][0] + s * Q[r][1];
                Real tmp1 = s * Q[r][0] - c * Q[r][1];
                Q[r][0] = tmp0;
                Q[r][1] = tmp1;
            }
        }

        // Update Q = Q*H in-place using H = {{1,0,0},{0,c,s},{0,s,-c}}.
        void Update3(Real Q[3][3], Real c, Real s) const
        {
            for (int r = 0; r < 3; ++r)
            {
                Real tmp0 = c * Q[r][1] + s * Q[r][2];
                Real tmp1 = s * Q[r][1] - c * Q[r][2];
                Q[r][1] = tmp0;
                Q[r][2] = tmp1;
            }
        }

        // Normalize (u,v) robustly, avoiding floating-point overflow in the
        // sqrt call.  The normalized pair is (cs,sn) with cs <= 0.  If
        // (u,v) = (0,0), the function returns (cs,sn) = (-1,0).  When used
        // to generate a Householder reflection, it does not matter whether
        // (cs,sn) or (-cs,-sn) is used.  When generating a Givens reflection,
        // cs = cos(2*theta) and sn = sin(2*theta).  Having a negative cosine
        // for the double-angle term ensures that the single-angle terms
        // c = cos(theta) and s = sin(theta) satisfy |c| <= |s|.
        void GetCosSin(Real u, Real v, Real& cs, Real& sn) const
        {
            Real maxAbsComp = std::max(std::fabs(u), std::fabs(v));
            if (maxAbsComp > (Real)0)
            {
                u /= maxAbsComp;  // in [-1,1]
                v /= maxAbsComp;  // in [-1,1]
                Real length = std::sqrt(u * u + v * v);
                cs = u / length;
                sn = v / length;
                if (cs > (Real)0)
                {
                    cs = -cs;
                    sn = -sn;
                }
            }
            else
            {
                cs = (Real)-1;
                sn = (Real)0;
            }
        }

        // The convergence test.  When 'aggressive' is 'true', the
        // superdiagonal test is "bSuper == 0".  When 'aggressive' is 'false',
        // the superdiagonal test is
        //   |bDiag0| + |bDiag1| + |bSuper| == |bDiag0| + |bDiag1|
        // which means bSuper is effectively zero compared to the sizes of the
        // diagonal entries.
        bool Converged(bool aggressive, Real bDiag0, Real bDiag1, Real bSuper) const
        {
            if (aggressive)
            {
                return bSuper == (Real)0;
            }
            else
            {
                Real sum = std::fabs(bDiag0) + std::fabs(bDiag1);
                return sum + std::fabs(bSuper) == sum;
            }
        }
    };


    template <typename Real>
    class NISymmetricEigensolver3x3
    {
    public:
        // The input matrix must be symmetric, so only the unique elements
        // must be specified: a00, a01, a02, a11, a12, and a22.  The
        // eigenvalues are sorted in ascending order: eval0 <= eval1 <= eval2.

        void operator()(Real a00, Real a01, Real a02, Real a11, Real a12, Real a22,
            int sortType, std::array<Real, 3>& eval, std::array<std::array<Real, 3>, 3>& evec) const
        {
            // Precondition the matrix by factoring out the maximum absolute
            // value of the components.  This guards against floating-point
            // overflow when computing the eigenvalues.
            Real max0 = std::max(std::fabs(a00), std::fabs(a01));
            Real max1 = std::max(std::fabs(a02), std::fabs(a11));
            Real max2 = std::max(std::fabs(a12), std::fabs(a22));
            Real maxAbsElement = std::max(std::max(max0, max1), max2);
            if (maxAbsElement == (Real)0)
            {
                // A is the zero matrix.
                eval[0] = (Real)0;
                eval[1] = (Real)0;
                eval[2] = (Real)0;
                evec[0] = { (Real)1, (Real)0, (Real)0 };
                evec[1] = { (Real)0, (Real)1, (Real)0 };
                evec[2] = { (Real)0, (Real)0, (Real)1 };
                return;
            }

            Real invMaxAbsElement = (Real)1 / maxAbsElement;
            a00 *= invMaxAbsElement;
            a01 *= invMaxAbsElement;
            a02 *= invMaxAbsElement;
            a11 *= invMaxAbsElement;
            a12 *= invMaxAbsElement;
            a22 *= invMaxAbsElement;

            Real norm = a01 * a01 + a02 * a02 + a12 * a12;
            if (norm > (Real)0)
            {
                // Compute the eigenvalues of A.

                // In the PDF mentioned previously, B = (A - q*I)/p, where
                // q = tr(A)/3 with tr(A) the trace of A (sum of the diagonal
                // entries of A) and where p = sqrt(tr((A - q*I)^2)/6).
                Real q = (a00 + a11 + a22) / (Real)3;

                // The matrix A - q*I is represented by the following, where
                // b00, b11 and b22 are computed after these comments,
                //   +-           -+
                //   | b00 a01 a02 |
                //   | a01 b11 a12 |
                //   | a02 a12 b22 |
                //   +-           -+
                Real b00 = a00 - q;
                Real b11 = a11 - q;
                Real b22 = a22 - q;

                // The is the variable p mentioned in the PDF.
                Real p = std::sqrt((b00 * b00 + b11 * b11 + b22 * b22 + norm * (Real)2) / (Real)6);

                // We need det(B) = det((A - q*I)/p) = det(A - q*I)/p^3.  The
                // value det(A - q*I) is computed using a cofactor expansion
                // by the first row of A - q*I.  The cofactors are c00, c01
                // and c02 and the determinant is b00*c00 - a01*c01 + a02*c02.
                // The det(B) is then computed finally by the division
                // with p^3.
                Real c00 = b11 * b22 - a12 * a12;
                Real c01 = a01 * b22 - a12 * a02;
                Real c02 = a01 * a12 - b11 * a02;
                Real det = (b00 * c00 - a01 * c01 + a02 * c02) / (p * p * p);

                // The halfDet value is cos(3*theta) mentioned in the PDF. The
                // acos(z) function requires |z| <= 1, but will fail silently
                // and return NaN if the input is larger than 1 in magnitude.
                // To avoid this problem due to rounding errors, the halfDet
                // value is clamped to [-1,1].
                Real halfDet = det * (Real)0.5;
                halfDet = std::min(std::max(halfDet, (Real)-1), (Real)1);

                // The eigenvalues of B are ordered as
                // beta0 <= beta1 <= beta2.  The number of digits in
                // twoThirdsPi is chosen so that, whether float or double,
                // the floating-point number is the closest to theoretical
                // 2*pi/3.
                Real angle = std::acos(halfDet) / (Real)3;
                Real const twoThirdsPi = (Real)2.09439510239319549;
                Real beta2 = std::cos(angle) * (Real)2;
                Real beta0 = std::cos(angle + twoThirdsPi) * (Real)2;
                Real beta1 = -(beta0 + beta2);

                // The eigenvalues of A are ordered as
                // alpha0 <= alpha1 <= alpha2.
                eval[0] = q + p * beta0;
                eval[1] = q + p * beta1;
                eval[2] = q + p * beta2;

                // Compute the eigenvectors so that the set
                // {evec[0], evec[1], evec[2]} is right handed and
                // orthonormal.
                if (halfDet >= (Real)0)
                {
                    ComputeEigenvector0(a00, a01, a02, a11, a12, a22, eval[2], evec[2]);
                    ComputeEigenvector1(a00, a01, a02, a11, a12, a22, evec[2], eval[1], evec[1]);
                    evec[0] = Cross(evec[1], evec[2]);
                }
                else
                {
                    ComputeEigenvector0(a00, a01, a02, a11, a12, a22, eval[0], evec[0]);
                    ComputeEigenvector1(a00, a01, a02, a11, a12, a22, evec[0], eval[1], evec[1]);
                    evec[2] = Cross(evec[0], evec[1]);
                }
            }
            else
            {
                // The matrix is diagonal.
                eval[0] = a00;
                eval[1] = a11;
                eval[2] = a22;
                evec[0] = { (Real)1, (Real)0, (Real)0 };
                evec[1] = { (Real)0, (Real)1, (Real)0 };
                evec[2] = { (Real)0, (Real)0, (Real)1 };
            }

            // The preconditioning scaled the matrix A, which scales the
            // eigenvalues.  Revert the scaling.
            eval[0] *= maxAbsElement;
            eval[1] *= maxAbsElement;
            eval[2] *= maxAbsElement;

            SortEigenstuff<Real>()(sortType, true, eval, evec);
        }

    private:
        static std::array<Real, 3> Multiply(Real s, std::array<Real, 3> const& U)
        {
            std::array<Real, 3> product = { s * U[0], s * U[1], s * U[2] };
            return product;
        }

        static std::array<Real, 3> Subtract(std::array<Real, 3> const& U, std::array<Real, 3> const& V)
        {
            std::array<Real, 3> difference = { U[0] - V[0], U[1] - V[1], U[2] - V[2] };
            return difference;
        }

        static std::array<Real, 3> Divide(std::array<Real, 3> const& U, Real s)
        {
            Real invS = (Real)1 / s;
            std::array<Real, 3> division = { U[0] * invS, U[1] * invS, U[2] * invS };
            return division;
        }

        static Real Dot(std::array<Real, 3> const& U, std::array<Real, 3> const& V)
        {
            Real dot = U[0] * V[0] + U[1] * V[1] + U[2] * V[2];
            return dot;
        }

        static std::array<Real, 3> Cross(std::array<Real, 3> const& U, std::array<Real, 3> const& V)
        {
            std::array<Real, 3> cross =
            {
                U[1] * V[2] - U[2] * V[1],
                U[2] * V[0] - U[0] * V[2],
                U[0] * V[1] - U[1] * V[0]
            };
            return cross;
        }

        void ComputeOrthogonalComplement(std::array<Real, 3> const& W,
            std::array<Real, 3>& U, std::array<Real, 3>& V) const
        {
            // Robustly compute a right-handed orthonormal set { U, V, W }.
            // The vector W is guaranteed to be unit-length, in which case
            // there is no need to worry about a division by zero when
            // computing invLength.
            Real invLength;
            if (std::fabs(W[0]) > std::fabs(W[1]))
            {
                // The component of maximum absolute value is either W[0]
                // or W[2].
                invLength = (Real)1 / std::sqrt(W[0] * W[0] + W[2] * W[2]);
                U = { -W[2] * invLength, (Real)0, +W[0] * invLength };
            }
            else
            {
                // The component of maximum absolute value is either W[1]
                // or W[2].
                invLength = (Real)1 / std::sqrt(W[1] * W[1] + W[2] * W[2]);
                U = { (Real)0, +W[2] * invLength, -W[1] * invLength };
            }
            V = Cross(W, U);
        }

        void ComputeEigenvector0(Real a00, Real a01, Real a02, Real a11, Real a12, Real a22,
            Real eval0, std::array<Real, 3>& evec0) const
        {
            // Compute a unit-length eigenvector for eigenvalue[i0].  The
            // matrix is rank 2, so two of the rows are linearly independent.
            // For a robust computation of the eigenvector, select the two
            // rows whose cross product has largest length of all pairs of
            // rows.
            std::array<Real, 3> row0 = { a00 - eval0, a01, a02 };
            std::array<Real, 3> row1 = { a01, a11 - eval0, a12 };
            std::array<Real, 3> row2 = { a02, a12, a22 - eval0 };
            std::array<Real, 3>  r0xr1 = Cross(row0, row1);
            std::array<Real, 3>  r0xr2 = Cross(row0, row2);
            std::array<Real, 3>  r1xr2 = Cross(row1, row2);
            Real d0 = Dot(r0xr1, r0xr1);
            Real d1 = Dot(r0xr2, r0xr2);
            Real d2 = Dot(r1xr2, r1xr2);

            Real dmax = d0;
            int imax = 0;
            if (d1 > dmax)
            {
                dmax = d1;
                imax = 1;
            }
            if (d2 > dmax)
            {
                imax = 2;
            }

            if (imax == 0)
            {
                evec0 = Divide(r0xr1, std::sqrt(d0));
            }
            else if (imax == 1)
            {
                evec0 = Divide(r0xr2, std::sqrt(d1));
            }
            else
            {
                evec0 = Divide(r1xr2, std::sqrt(d2));
            }
        }

        void ComputeEigenvector1(Real a00, Real a01, Real a02, Real a11, Real a12, Real a22,
            std::array<Real, 3> const& evec0, Real eval1, std::array<Real, 3>& evec1) const
        {
            // Robustly compute a right-handed orthonormal set
            // { U, V, evec0 }.
            std::array<Real, 3> U, V;
            ComputeOrthogonalComplement(evec0, U, V);

            // Let e be eval1 and let E be a corresponding eigenvector which
            // is a solution to the linear system (A - e*I)*E = 0.  The matrix
            // (A - e*I) is 3x3, not invertible (so infinitely many
            // solutions), and has rank 2 when eval1 and eval are different.
            // It has rank 1 when eval1 and eval2 are equal.  Numerically, it
            // is difficult to compute robustly the rank of a matrix.  Instead,
            // the 3x3 linear system is reduced to a 2x2 system as follows.
            // Define the 3x2 matrix J = [U V] whose columns are the U and V
            // computed previously.  Define the 2x1 vector X = J*E.  The 2x2
            // system is 0 = M * X = (J^T * (A - e*I) * J) * X where J^T is
            // the transpose of J and M = J^T * (A - e*I) * J is a 2x2 matrix.
            // The system may be written as
            //     +-                        -++-  -+       +-  -+
            //     | U^T*A*U - e  U^T*A*V     || x0 | = e * | x0 |
            //     | V^T*A*U      V^T*A*V - e || x1 |       | x1 |
            //     +-                        -++   -+       +-  -+
            // where X has row entries x0 and x1.

            std::array<Real, 3> AU =
            {
                a00 * U[0] + a01 * U[1] + a02 * U[2],
                a01 * U[0] + a11 * U[1] + a12 * U[2],
                a02 * U[0] + a12 * U[1] + a22 * U[2]
            };

            std::array<Real, 3> AV =
            {
                a00 * V[0] + a01 * V[1] + a02 * V[2],
                a01 * V[0] + a11 * V[1] + a12 * V[2],
                a02 * V[0] + a12 * V[1] + a22 * V[2]
            };

            Real m00 = U[0] * AU[0] + U[1] * AU[1] + U[2] * AU[2] - eval1;
            Real m01 = U[0] * AV[0] + U[1] * AV[1] + U[2] * AV[2];
            Real m11 = V[0] * AV[0] + V[1] * AV[1] + V[2] * AV[2] - eval1;

            // For robustness, choose the largest-length row of M to compute
            // the eigenvector.  The 2-tuple of coefficients of U and V in the
            // assignments to eigenvector[1] lies on a circle, and U and V are
            // unit length and perpendicular, so eigenvector[1] is unit length
            // (within numerical tolerance).
            Real absM00 = std::fabs(m00);
            Real absM01 = std::fabs(m01);
            Real absM11 = std::fabs(m11);
            Real maxAbsComp;
            if (absM00 >= absM11)
            {
                maxAbsComp = std::max(absM00, absM01);
                if (maxAbsComp > (Real)0)
                {
                    if (absM00 >= absM01)
                    {
                        m01 /= m00;
                        m00 = (Real)1 / std::sqrt((Real)1 + m01 * m01);
                        m01 *= m00;
                    }
                    else
                    {
                        m00 /= m01;
                        m01 = (Real)1 / std::sqrt((Real)1 + m00 * m00);
                        m00 *= m01;
                    }
                    evec1 = Subtract(Multiply(m01, U), Multiply(m00, V));
                }
                else
                {
                    evec1 = U;
                }
            }
            else
            {
                maxAbsComp = std::max(absM11, absM01);
                if (maxAbsComp > (Real)0)
                {
                    if (absM11 >= absM01)
                    {
                        m01 /= m11;
                        m11 = (Real)1 / std::sqrt((Real)1 + m01 * m01);
                        m01 *= m11;
                    }
                    else
                    {
                        m11 /= m01;
                        m01 = (Real)1 / std::sqrt((Real)1 + m11 * m11);
                        m11 *= m01;
                    }
                    evec1 = Subtract(Multiply(m11, U), Multiply(m01, V));
                }
                else
                {
                    evec1 = U;
                }
            }
        }
    };
}