tetrees/Plugins/Wwise/Source/AkAudio/Classes/GTE/Mathematics/QuarticRootsQR.h

// 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.13

#pragma once

#include <Mathematics/CubicRootsQR.h>

// An implementation of the QR algorithm described in "Matrix Computations,
// 2nd edition" by G. H. Golub and C. F. Van Loan, The Johns Hopkins
// University Press, Baltimore MD, Fourth Printing 1993.  In particular,
// the implementation is based on Chapter 7 (The Unsymmetric Eigenvalue
// Problem), Section 7.5 (The Practical QR Algorithm).  The algorithm is
// specialized for the companion matrix associated with a quartic polynomial.

namespace WwiseGTE
{
    template <typename Real>
    class QuarticRootsQR
    {
    public:
        typedef std::array<std::array<Real, 4>, 4> Matrix;

        // Solve p(x) = c0 + c1 * x + c2 * x^2 + c3 * x^3 + x^4 = 0.
        uint32_t operator() (uint32_t maxIterations, Real c0, Real c1, Real c2, Real c3,
            uint32_t& numRoots, std::array<Real, 4>& roots) const
        {
            // Create the companion matrix for the polynomial.  The matrix is
            // in upper Hessenberg form.
            Matrix A;
            A[0][0] = (Real)0;
            A[0][1] = (Real)0;
            A[0][2] = (Real)0;
            A[0][3] = -c0;
            A[1][0] = (Real)1;
            A[1][1] = (Real)0;
            A[1][2] = (Real)0;
            A[1][3] = -c1;
            A[2][0] = (Real)0;
            A[2][1] = (Real)1;
            A[2][2] = (Real)0;
            A[2][3] = -c2;
            A[3][0] = (Real)0;
            A[3][1] = (Real)0;
            A[3][2] = (Real)1;
            A[3][3] = -c3;

            // Avoid the QR-cycle when c1 = c2 = 0 and avoid the slow
            // convergence when c1 and c2 are nearly zero.
            std::array<Real, 3> V{
                (Real)1,
                (Real)0.36602540378443865,
                (Real)0.36602540378443865 };
            DoIteration(V, A);

            return operator()(maxIterations, A, numRoots, roots);
        }

        // Compute the real eigenvalues of the upper Hessenberg matrix A.  The
        // matrix is modified by in-place operations, so if you need to
        // remember A, you must make your own copy before calling this
        // function.
        uint32_t operator() (uint32_t maxIterations, Matrix& A,
            uint32_t& numRoots, std::array<Real, 4>& roots) const
        {
            numRoots = 0;
            std::fill(roots.begin(), roots.end(), (Real)0);

            for (uint32_t numIterations = 0; numIterations < maxIterations; ++numIterations)
            {
                // Apply a Francis QR iteration.
                Real tr = A[2][2] + A[3][3];
                Real det = A[2][2] * A[3][3] - A[2][3] * A[3][2];
                std::array<Real, 3> X{
                    A[0][0] * A[0][0] + A[0][1] * A[1][0] - tr * A[0][0] + det,
                    A[1][0] * (A[0][0] + A[1][1] - tr),
                    A[1][0] * A[2][1] };
                std::array<Real, 3> V = House<3>(X);
                DoIteration(V, A);

                // Test for uncoupling of A.
                Real tr12 = A[1][1] + A[2][2];
                if (tr12 + A[2][1] == tr12)
                {
                    GetQuadraticRoots(0, 1, A, numRoots, roots);
                    GetQuadraticRoots(2, 3, A, numRoots, roots);
                    return numIterations;
                }

                Real tr01 = A[0][0] + A[1][1];
                if (tr01 + A[1][0] == tr01)
                {
                    numRoots = 1;
                    roots[0] = A[0][0];

                    // TODO: The cubic solver is not designed to process 3x3
                    // submatrices of an NxN matrix, so the copy of a
                    // submatrix of A to B is a simple workaround for running
                    // the solver.  Write general root-finding/ code that
                    // avoids such copying.
                    uint32_t subMaxIterations = maxIterations - numIterations;
                    typename CubicRootsQR<Real>::Matrix B;
                    for (int r = 0; r < 3; ++r)
                    {
                        for (int c = 0; c < 3; ++c)
                        {
                            B[r][c] = A[r + 1][c + 1];
                        }
                    }

                    uint32_t numSubroots = 0;
                    std::array<Real, 3> subroots;
                    uint32_t numSubiterations = CubicRootsQR<Real>()(subMaxIterations, B,
                        numSubroots, subroots);
                    for (uint32_t i = 0; i < numSubroots; ++i)
                    {
                        roots[numRoots++] = subroots[i];
                    }
                    return numIterations + numSubiterations;
                }

                Real tr23 = A[2][2] + A[3][3];
                if (tr23 + A[3][2] == tr23)
                {
                    numRoots = 1;
                    roots[0] = A[3][3];

                    // TODO: The cubic solver is not designed to process 3x3
                    // submatrices of an NxN matrix, so the copy of a
                    // submatrix of A to B is a simple workaround for running
                    // the solver.  Write general root-finding/ code that
                    // avoids such copying.
                    uint32_t subMaxIterations = maxIterations - numIterations;
                    typename CubicRootsQR<Real>::Matrix B;
                    for (int r = 0; r < 3; ++r)
                    {
                        for (int c = 0; c < 3; ++c)
                        {
                            B[r][c] = A[r][c];
                        }
                    }

                    uint32_t numSubroots = 0;
                    std::array<Real, 3> subroots;
                    uint32_t numSubiterations = CubicRootsQR<Real>()(subMaxIterations, B,
                        numSubroots, subroots);
                    for (uint32_t i = 0; i < numSubroots; ++i)
                    {
                        roots[numRoots++] = subroots[i];
                    }
                    return numIterations + numSubiterations;
                }
            }
            return maxIterations;
        }

    private:
        void DoIteration(std::array<Real, 3> const& V, Matrix& A) const
        {
            Real multV = (Real)-2 / (V[0] * V[0] + V[1] * V[1] + V[2] * V[2]);
            std::array<Real, 3> MV{ multV * V[0], multV * V[1], multV * V[2] };
            RowHouse<3>(0, 2, 0, 3, V, MV, A);
            ColHouse<3>(0, 3, 0, 2, V, MV, A);

            std::array<Real, 3> X{ A[1][0], A[2][0], A[3][0] };
            std::array<Real, 3> locV = House<3>(X);
            multV = (Real)-2 / (locV[0] * locV[0] + locV[1] * locV[1] + locV[2] * locV[2]);
            MV = { multV * locV[0], multV * locV[1], multV * locV[2] };
            RowHouse<3>(1, 3, 0, 3, locV, MV, A);
            ColHouse<3>(0, 3, 1, 3, locV, MV, A);

            std::array<Real, 2> Y{ A[2][1], A[3][1] };
            std::array<Real, 2> W = House<2>(Y);
            Real multW = (Real)-2 / (W[0] * W[0] + W[1] * W[1]);
            std::array<Real, 2> MW = { multW * W[0], multW * W[1] };
            RowHouse<2>(2, 3, 0, 3, W, MW, A);
            ColHouse<2>(0, 3, 2, 3, W, MW, A);
        }

        template <int N>
        std::array<Real, N> House(std::array<Real, N> const& X) const
        {
            std::array<Real, N> V;
            Real length = (Real)0;
            for (int i = 0; i < N; ++i)
            {
                length += X[i] * X[i];
            }
            length = std::sqrt(length);
            if (length != (Real)0)
            {
                Real sign = (X[0] >= (Real)0 ? (Real)1 : (Real)-1);
                Real denom = X[0] + sign * length;
                for (int i = 1; i < N; ++i)
                {
                    V[i] = X[i] / denom;
                }
            }
            else
            {
                V.fill((Real)0);
            }
            V[0] = (Real)1;
            return V;
        }

        template <int N>
        void RowHouse(int rmin, int rmax, int cmin, int cmax,
            std::array<Real, N> const& V, std::array<Real, N> const& MV, Matrix& A) const
        {
            // Only elements cmin through cmax are used.
            std::array<Real, 4> W;

            for (int c = cmin; c <= cmax; ++c)
            {
                W[c] = (Real)0;
                for (int r = rmin, k = 0; r <= rmax; ++r, ++k)
                {
                    W[c] += V[k] * A[r][c];
                }
            }

            for (int r = rmin, k = 0; r <= rmax; ++r, ++k)
            {
                for (int c = cmin; c <= cmax; ++c)
                {
                    A[r][c] += MV[k] * W[c];
                }
            }
        }

        template <int N>
        void ColHouse(int rmin, int rmax, int cmin, int cmax,
            std::array<Real, N> const& V, std::array<Real, N> const& MV, Matrix& A) const
        {
            // Only elements rmin through rmax are used.
            std::array<Real, 4> W;

            for (int r = rmin; r <= rmax; ++r)
            {
                W[r] = (Real)0;
                for (int c = cmin, k = 0; c <= cmax; ++c, ++k)
                {
                    W[r] += V[k] * A[r][c];
                }
            }

            for (int r = rmin; r <= rmax; ++r)
            {
                for (int c = cmin, k = 0; c <= cmax; ++c, ++k)
                {
                    A[r][c] += W[r] * MV[k];
                }
            }
        }

        void GetQuadraticRoots(int i0, int i1, Matrix const& A,
            uint32_t& numRoots, std::array<Real, 4>& roots) const
        {
            // Solve x^2 - t * x + d = 0, where t is the trace and d is the
            // determinant of the 2x2 matrix defined by indices i0 and i1.
            // The discriminant is D = (t/2)^2 - d.  When D >= 0, the roots
            // are real values t/2 - sqrt(D) and t/2 + sqrt(D).  To avoid
            // potential numerical issues with subtractive cancellation, the
            // roots are computed as
            //   r0 = t/2 + sign(t/2)*sqrt(D), r1 = trace - r0
            Real trace = A[i0][i0] + A[i1][i1];
            Real halfTrace = trace * (Real)0.5;
            Real determinant = A[i0][i0] * A[i1][i1] - A[i0][i1] * A[i1][i0];
            Real discriminant = halfTrace * halfTrace - determinant;
            if (discriminant >= (Real)0)
            {
                Real sign = (trace >= (Real)0 ? (Real)1 : (Real)-1);
                Real root = halfTrace + sign * std::sqrt(discriminant);
                roots[numRoots++] = root;
                roots[numRoots++] = trace - root;
            }
        }
    };
}