tetrees/Plugins/Wwise/Source/AkAudio/Classes/GTE/Mathematics/Rotation.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/AxisAngle.h>
#include <Mathematics/EulerAngles.h>
#include <Mathematics/Matrix.h>
#include <Mathematics/Quaternion.h>

namespace WwiseGTE
{
    // Conversions among various representations of rotations.  The value of
    // N must be 3 or 4.  The latter case supports affine algebra when you use
    // 4-tuple vectors (w-component is 1 for points and 0 for vector) and 4x4
    // matrices for affine transformations.  Rotation axes must be unit
    // length.  The angles are in radians.  The Euler angles are in world
    // coordinates; we have not yet added support for body coordinates.

    template <int N, typename Real>
    class Rotation
    {
    public:
        // Create rotations from various representations.
        Rotation(Matrix<N, N, Real> const& matrix)
            :
            mType(IS_MATRIX),
            mMatrix(matrix)
        {
            static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
        }

        Rotation(Quaternion<Real> const& quaternion)
            :
            mType(IS_QUATERNION),
            mQuaternion(quaternion)
        {
            static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
        }

        Rotation(AxisAngle<N, Real> const& axisAngle)
            :
            mType(IS_AXIS_ANGLE),
            mAxisAngle(axisAngle)
        {
            static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
        }

        Rotation(EulerAngles<Real> const& eulerAngles)
            :
            mType(IS_EULER_ANGLES),
            mEulerAngles(eulerAngles)
        {
            static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
        }

        // Convert one representation to another.
        operator Matrix<N, N, Real>() const
        {
            static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

            switch (mType)
            {
            case IS_MATRIX:
                break;
            case IS_QUATERNION:
                Convert(mQuaternion, mMatrix);
                break;
            case IS_AXIS_ANGLE:
                Convert(mAxisAngle, mMatrix);
                break;
            case IS_EULER_ANGLES:
                Convert(mEulerAngles, mMatrix);
                break;
            }

            return mMatrix;
        }

        operator Quaternion<Real>() const
        {
            static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

            switch (mType)
            {
            case IS_MATRIX:
                Convert(mMatrix, mQuaternion);
                break;
            case IS_QUATERNION:
                break;
            case IS_AXIS_ANGLE:
                Convert(mAxisAngle, mQuaternion);
                break;
            case IS_EULER_ANGLES:
                Convert(mEulerAngles, mQuaternion);
                break;
            }

            return mQuaternion;
        }

        operator AxisAngle<N, Real>() const
        {
            static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

            switch (mType)
            {
            case IS_MATRIX:
                Convert(mMatrix, mAxisAngle);
                break;
            case IS_QUATERNION:
                Convert(mQuaternion, mAxisAngle);
                break;
            case IS_AXIS_ANGLE:
                break;
            case IS_EULER_ANGLES:
                Convert(mEulerAngles, mAxisAngle);
                break;
            }

            return mAxisAngle;
        }

        EulerAngles<Real> const& operator()(int i0, int i1, int i2) const
        {
            static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

            mEulerAngles.axis[0] = i0;
            mEulerAngles.axis[1] = i1;
            mEulerAngles.axis[2] = i2;

            switch (mType)
            {
            case IS_MATRIX:
                Convert(mMatrix, mEulerAngles);
                break;
            case IS_QUATERNION:
                Convert(mQuaternion, mEulerAngles);
                break;
            case IS_AXIS_ANGLE:
                Convert(mAxisAngle, mEulerAngles);
                break;
            case IS_EULER_ANGLES:
                break;
            }

            return mEulerAngles;
        }

    private:
        enum RepresentationType
        {
            IS_MATRIX,
            IS_QUATERNION,
            IS_AXIS_ANGLE,
            IS_EULER_ANGLES
        };

        RepresentationType mType;
        mutable Matrix<N, N, Real> mMatrix;
        mutable Quaternion<Real> mQuaternion;
        mutable AxisAngle<N, Real> mAxisAngle;
        mutable EulerAngles<Real> mEulerAngles;

        // Convert a rotation matrix to a quaternion.
        //
        // x^2 = (+r00 - r11 - r22 + 1)/4
        // y^2 = (-r00 + r11 - r22 + 1)/4
        // z^2 = (-r00 - r11 + r22 + 1)/4
        // w^2 = (+r00 + r11 + r22 + 1)/4
        // x^2 + y^2 = (1 - r22)/2
        // z^2 + w^2 = (1 + r22)/2
        // y^2 - x^2 = (r11 - r00)/2
        // w^2 - z^2 = (r11 + r00)/2
        // x*y = (r01 + r10)/4
        // x*z = (r02 + r20)/4
        // y*z = (r12 + r21)/4
        // [GTE_USE_MAT_VEC]
        //   x*w = (r21 - r12)/4
        //   y*w = (r02 - r20)/4
        //   z*w = (r10 - r01)/4
        // [GTE_USE_VEC_MAT]
        //   x*w = (r12 - r21)/4
        //   y*w = (r20 - r02)/4
        //   z*w = (r01 - r10)/4
        //
        // If Q is the 4x1 column vector (x,y,z,w), the previous equations
        // give us
        //         +-                  -+
        //         | x*x  x*y  x*z  x*w |
        // Q*Q^T = | y*x  y*y  y*z  y*w |
        //         | z*x  z*y  z*z  z*w |
        //         | w*x  w*y  w*z  w*w |
        //         +-                  -+
        // The code extracts the row of maximum length, normalizing it to
        // obtain the result q.
        static void Convert(Matrix<N, N, Real> const& r, Quaternion<Real>& q)
        {
            static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

            Real r22 = r(2, 2);
            if (r22 <= (Real)0)  // x^2 + y^2 >= z^2 + w^2
            {
                Real dif10 = r(1, 1) - r(0, 0);
                Real omr22 = (Real)1 - r22;
                if (dif10 <= (Real)0)  // x^2 >= y^2
                {
                    Real fourXSqr = omr22 - dif10;
                    Real inv4x = ((Real)0.5) / std::sqrt(fourXSqr);
                    q[0] = fourXSqr * inv4x;
                    q[1] = (r(0, 1) + r(1, 0)) * inv4x;
                    q[2] = (r(0, 2) + r(2, 0)) * inv4x;
#if defined(GTE_USE_MAT_VEC)
                    q[3] = (r(2, 1) - r(1, 2)) * inv4x;
#else
                    q[3] = (r(1, 2) - r(2, 1)) * inv4x;
#endif
                }
                else  // y^2 >= x^2
                {
                    Real fourYSqr = omr22 + dif10;
                    Real inv4y = ((Real)0.5) / std::sqrt(fourYSqr);
                    q[0] = (r(0, 1) + r(1, 0)) * inv4y;
                    q[1] = fourYSqr * inv4y;
                    q[2] = (r(1, 2) + r(2, 1)) * inv4y;
#if defined(GTE_USE_MAT_VEC)
                    q[3] = (r(0, 2) - r(2, 0)) * inv4y;
#else
                    q[3] = (r(2, 0) - r(0, 2)) * inv4y;
#endif
                }
            }
            else  // z^2 + w^2 >= x^2 + y^2
            {
                Real sum10 = r(1, 1) + r(0, 0);
                Real opr22 = (Real)1 + r22;
                if (sum10 <= (Real)0)  // z^2 >= w^2
                {
                    Real fourZSqr = opr22 - sum10;
                    Real inv4z = ((Real)0.5) / std::sqrt(fourZSqr);
                    q[0] = (r(0, 2) + r(2, 0)) * inv4z;
                    q[1] = (r(1, 2) + r(2, 1)) * inv4z;
                    q[2] = fourZSqr * inv4z;
#if defined(GTE_USE_MAT_VEC)
                    q[3] = (r(1, 0) - r(0, 1)) * inv4z;
#else
                    q[3] = (r(0, 1) - r(1, 0)) * inv4z;
#endif
                }
                else  // w^2 >= z^2
                {
                    Real fourWSqr = opr22 + sum10;
                    Real inv4w = ((Real)0.5) / std::sqrt(fourWSqr);
#if defined(GTE_USE_MAT_VEC)
                    q[0] = (r(2, 1) - r(1, 2)) * inv4w;
                    q[1] = (r(0, 2) - r(2, 0)) * inv4w;
                    q[2] = (r(1, 0) - r(0, 1)) * inv4w;
#else
                    q[0] = (r(1, 2) - r(2, 1)) * inv4w;
                    q[1] = (r(2, 0) - r(0, 2)) * inv4w;
                    q[2] = (r(0, 1) - r(1, 0)) * inv4w;
#endif
                    q[3] = fourWSqr * inv4w;
                }
            }
        }

        // Convert a quaterion q = x*i + y*j + z*k + w to a rotation matrix.
        // [GTE_USE_MAT_VEC]
        //     +-           -+   +-                                     -+
        // R = | r00 r01 r02 | = | 1-2y^2-2z^2  2(xy-zw)     2(xz+yw)    |
        //     | r10 r11 r12 |   | 2(xy+zw)     1-2x^2-2z^2  2(yz-xw)    |
        //     | r20 r21 r22 |   | 2(xz-yw)     2(yz+xw)     1-2x^2-2y^2 |
        //     +-           -+   +-                                     -+
        // [GTE_USE_VEC_MAT]
        //     +-           -+   +-                                     -+
        // R = | r00 r01 r02 | = | 1-2y^2-2z^2  2(xy+zw)     2(xz-yw)    |
        //     | r10 r11 r12 |   | 2(xy-zw)     1-2x^2-2z^2  2(yz+xw)    |
        //     | r20 r21 r22 |   | 2(xz+yw)     2(yz-xw)     1-2x^2-2y^2 |
        //     +-           -+   +-                                     -+
        static void Convert(Quaternion<Real> const& q, Matrix<N, N, Real>& r)
        {
            static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

            r.MakeIdentity();

            Real twoX = ((Real)2) * q[0];
            Real twoY = ((Real)2) * q[1];
            Real twoZ = ((Real)2) * q[2];
            Real twoXX = twoX * q[0];
            Real twoXY = twoX * q[1];
            Real twoXZ = twoX * q[2];
            Real twoXW = twoX * q[3];
            Real twoYY = twoY * q[1];
            Real twoYZ = twoY * q[2];
            Real twoYW = twoY * q[3];
            Real twoZZ = twoZ * q[2];
            Real twoZW = twoZ * q[3];

#if defined(GTE_USE_MAT_VEC)
            r(0, 0) = (Real)1 - twoYY - twoZZ;
            r(0, 1) = twoXY - twoZW;
            r(0, 2) = twoXZ + twoYW;
            r(1, 0) = twoXY + twoZW;
            r(1, 1) = (Real)1 - twoXX - twoZZ;
            r(1, 2) = twoYZ - twoXW;
            r(2, 0) = twoXZ - twoYW;
            r(2, 1) = twoYZ + twoXW;
            r(2, 2) = (Real)1 - twoXX - twoYY;
#else
            r(0, 0) = (Real)1 - twoYY - twoZZ;
            r(1, 0) = twoXY - twoZW;
            r(2, 0) = twoXZ + twoYW;
            r(0, 1) = twoXY + twoZW;
            r(1, 1) = (Real)1 - twoXX - twoZZ;
            r(2, 1) = twoYZ - twoXW;
            r(0, 2) = twoXZ - twoYW;
            r(1, 2) = twoYZ + twoXW;
            r(2, 2) = (Real)1 - twoXX - twoYY;
#endif
        }

        // Convert a rotation matrix to an axis-angle pair.  Let (x0,x1,x2) be
        // the axis let t be an angle of rotation.  The rotation matrix is
        // [GTE_USE_MAT_VEC]
        //   R = I + sin(t)*S + (1-cos(t))*S^2
        // or
        // [GTE_USE_VEC_MAT]
        //   R = I - sin(t)*S + (1-cos(t))*S^2
        // where I is the identity and S = {{0,-x2,x1},{x2,0,-x0},{-x1,x0,0}}
        // where the inner-brace triples are the rows of the matrix.  If
        // t > 0, R represents a counterclockwise rotation; see the comments
        // for the constructor Matrix3x3(axis,angle).  It may be shown that
        // cos(t) = (trace(R)-1)/2 and R - Transpose(R) = 2*sin(t)*S.  As long
        // as sin(t) is not zero, we may solve for S in the second equation,
        // which produces the axis direction U = (S21,S02,S10).  When t = 0,
        // the rotation is the identity, in which case any axis direction is
        // valid; we choose (1,0,0).  When t = pi, it must be that
        // R - Transpose(R) = 0, which prevents us from extracting the axis.
        // Instead, note that (R+I)/2 = I+S^2 = U*U^T, where U is a
        // unit-length axis direction.
        static void Convert(Matrix<N, N, Real> const& r, AxisAngle<N, Real>& a)
        {
            static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

            Real trace = r(0, 0) + r(1, 1) + r(2, 2);
            Real half = (Real)0.5;
            Real cs = half * (trace - (Real)1);
            cs = std::max(std::min(cs, (Real)1), (Real)-1);
            a.angle = std::acos(cs);  // The angle is in [0,pi].
            a.axis.MakeZero();

            if (a.angle > (Real)0)
            {
                if (a.angle < (Real)GTE_C_PI)
                {
                    // The angle is in (0,pi).
#if defined(GTE_USE_MAT_VEC)
                    a.axis[0] = r(2, 1) - r(1, 2);
                    a.axis[1] = r(0, 2) - r(2, 0);
                    a.axis[2] = r(1, 0) - r(0, 1);
                    Normalize(a.axis);
#else
                    a.axis[0] = r(1, 2) - r(2, 1);
                    a.axis[1] = r(2, 0) - r(0, 2);
                    a.axis[2] = r(0, 1) - r(1, 0);
                    Normalize(a.axis);
#endif
                }
                else
                {
                    // The angle is pi, in which case R is symmetric and
                    // R+I = 2*(I+S^2) = 2*U*U^T, where U = (u0,u1,u2) is the
                    // unit-length direction of the rotation axis.  Determine
                    // the largest diagonal entry of R+I and normalize the
                    // corresponding row to produce U.  It does not matter the
                    // sign on u[d] for chosen diagonal d, because
                    // R(U,pi) = R(-U,pi).
                    Real one = (Real)1;
                    if (r(0, 0) >= r(1, 1))
                    {
                        if (r(0, 0) >= r(2, 2))
                        {
                            // r00 is maximum diagonal term
                            a.axis[0] = r(0, 0) + one;
                            a.axis[1] = half * (r(0, 1) + r(1, 0));
                            a.axis[2] = half * (r(0, 2) + r(2, 0));
                        }
                        else
                        {
                            // r22 is maximum diagonal term
                            a.axis[0] = half * (r(2, 0) + r(0, 2));
                            a.axis[1] = half * (r(2, 1) + r(1, 2));
                            a.axis[2] = r(2, 2) + one;
                        }
                    }
                    else
                    {
                        if (r(1, 1) >= r(2, 2))
                        {
                            // r11 is maximum diagonal term
                            a.axis[0] = half * (r(1, 0) + r(0, 1));
                            a.axis[1] = r(1, 1) + one;
                            a.axis[2] = half * (r(1, 2) + r(2, 1));
                        }
                        else
                        {
                            // r22 is maximum diagonal term
                            a.axis[0] = half * (r(2, 0) + r(0, 2));
                            a.axis[1] = half * (r(2, 1) + r(1, 2));
                            a.axis[2] = r(2, 2) + one;
                        }
                    }
                    Normalize(a.axis);
                }
            }
            else
            {
                // The angle is 0 and the matrix is the identity.  Any axis
                // will work, so choose the Unit(0) axis.
                a.axis[0] = (Real)1;
            }
        }

        // Convert an axis-angle pair to a rotation matrix.  Assuming
        // (x0,x1,x2) is for a right-handed world (x0 to right, x1 up, x2 out
        // of plane of page), a positive angle corresponds to a
        // counterclockwise rotation from the perspective of an observer
        // looking at the origin of the plane of rotation and having view
        // direction the negative of the axis direction.  The coordinate-axis
        // rotations are the following, where unit(0) = (1,0,0),
        // unit(1) = (0,1,0), unit(2) = (0,0,1),
        // [GTE_USE_MAT_VEC]
        //   R(unit(0),t) = {{ 1, 0, 0}, { 0, c,-s}, { 0, s, c}}
        //   R(unit(1),t) = {{ c, 0, s}, { 0, 1, 0}, {-s, 0, c}}
        //   R(unit(2),t) = {{ c,-s, 0}, { s, c, 0}, { 0, 0, 1}}
        // or
        // [GTE_USE_VEC_MAT]
        //   R(unit(0),t) = {{ 1, 0, 0}, { 0, c, s}, { 0,-s, c}}
        //   R(unit(1),t) = {{ c, 0,-s}, { 0, 1, 0}, { s, 0, c}}
        //   R(unit(2),t) = {{ c, s, 0}, {-s, c, 0}, { 0, 0, 1}}
        // where c = cos(t), s = sin(t), and the inner-brace triples are rows
        // of the matrix.  The general matrix is
        // [GTE_USE_MAT_VEC]
        //      +-                                                          -+
        //  R = | (1-c)*x0^2  + c     (1-c)*x0*x1 - s*x2  (1-c)*x0*x2 + s*x1 |
        //      | (1-c)*x0*x1 + s*x2  (1-c)*x1^2  + c     (1-c)*x1*x2 - s*x0 |
        //      | (1-c)*x0*x2 - s*x1  (1-c)*x1*x2 + s*x0  (1-c)*x2^2  + c    |
        //      +-                                                          -+
        // [GTE_USE_VEC_MAT]
        //      +-                                                          -+
        //  R = | (1-c)*x0^2  + c     (1-c)*x0*x1 + s*x2  (1-c)*x0*x2 - s*x1 |
        //      | (1-c)*x0*x1 - s*x2  (1-c)*x1^2  + c     (1-c)*x1*x2 + s*x0 |
        //      | (1-c)*x0*x2 + s*x1  (1-c)*x1*x2 - s*x0  (1-c)*x2^2  + c    |
        //      +-                                                          -+
        static void Convert(AxisAngle<N, Real> const& a, Matrix<N, N, Real>& r)
        {
            static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

            r.MakeIdentity();

            Real cs = std::cos(a.angle);
            Real sn = std::sin(a.angle);
            Real oneMinusCos = ((Real)1) - cs;
            Real x0sqr = a.axis[0] * a.axis[0];
            Real x1sqr = a.axis[1] * a.axis[1];
            Real x2sqr = a.axis[2] * a.axis[2];
            Real x0x1m = a.axis[0] * a.axis[1] * oneMinusCos;
            Real x0x2m = a.axis[0] * a.axis[2] * oneMinusCos;
            Real x1x2m = a.axis[1] * a.axis[2] * oneMinusCos;
            Real x0Sin = a.axis[0] * sn;
            Real x1Sin = a.axis[1] * sn;
            Real x2Sin = a.axis[2] * sn;

#if defined(GTE_USE_MAT_VEC)
            r(0, 0) = x0sqr * oneMinusCos + cs;
            r(0, 1) = x0x1m - x2Sin;
            r(0, 2) = x0x2m + x1Sin;
            r(1, 0) = x0x1m + x2Sin;
            r(1, 1) = x1sqr * oneMinusCos + cs;
            r(1, 2) = x1x2m - x0Sin;
            r(2, 0) = x0x2m - x1Sin;
            r(2, 1) = x1x2m + x0Sin;
            r(2, 2) = x2sqr * oneMinusCos + cs;
#else
            r(0, 0) = x0sqr * oneMinusCos + cs;
            r(1, 0) = x0x1m - x2Sin;
            r(2, 0) = x0x2m + x1Sin;
            r(0, 1) = x0x1m + x2Sin;
            r(1, 1) = x1sqr * oneMinusCos + cs;
            r(2, 1) = x1x2m - x0Sin;
            r(0, 2) = x0x2m - x1Sin;
            r(1, 2) = x1x2m + x0Sin;
            r(2, 2) = x2sqr * oneMinusCos + cs;
#endif
        }

        // Convert a rotation matrix to Euler angles.  Factorization into
        // Euler angles is not necessarily unique.  If the result is
        // ER_NOT_UNIQUE_SUM, then the multiple solutions occur because
        // angleN2+angleN0 is constant.  If the result is ER_NOT_UNIQUE_DIF,
        // then the multiple solutions occur because angleN2-angleN0 is
        // constant.  In either type of nonuniqueness, the function returns
        // angleN0=0.
        static void Convert(Matrix<N, N, Real> const& r, EulerAngles<Real>& e)
        {
            static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

            if (0 <= e.axis[0] && e.axis[0] < 3
                && 0 <= e.axis[1] && e.axis[1] < 3
                && 0 <= e.axis[2] && e.axis[2] < 3
                && e.axis[1] != e.axis[0]
                && e.axis[1] != e.axis[2])
            {
                if (e.axis[0] != e.axis[2])
                {
#if defined(GTE_USE_MAT_VEC)
                    // Map (0,1,2), (1,2,0), and (2,0,1) to +1.
                    // Map (0,2,1), (2,1,0), and (1,0,2) to -1.
                    int parity = (((e.axis[2] | (e.axis[1] << 2)) >> e.axis[0]) & 1);
                    Real const sgn = (parity & 1 ? (Real)-1 : (Real)+1);

                    if (r(e.axis[2], e.axis[0]) < (Real)1)
                    {
                        if (r(e.axis[2], e.axis[0]) > (Real)-1)
                        {
                            e.angle[2] = std::atan2(sgn * r(e.axis[1], e.axis[0]),
                                r(e.axis[0], e.axis[0]));
                            e.angle[1] = std::asin(-sgn * r(e.axis[2], e.axis[0]));
                            e.angle[0] = std::atan2(sgn * r(e.axis[2], e.axis[1]),
                                r(e.axis[2], e.axis[2]));
                            e.result = ER_UNIQUE;
                        }
                        else
                        {
                            e.angle[2] = (Real)0;
                            e.angle[1] = sgn * (Real)GTE_C_HALF_PI;
                            e.angle[0] = std::atan2(-sgn * r(e.axis[1], e.axis[2]),
                                r(e.axis[1], e.axis[1]));
                            e.result = ER_NOT_UNIQUE_DIF;
                        }
                    }
                    else
                    {
                        e.angle[2] = (Real)0;
                        e.angle[1] = -sgn * (Real)GTE_C_HALF_PI;
                        e.angle[0] = std::atan2(-sgn * r(e.axis[1], e.axis[2]),
                            r(e.axis[1], e.axis[1]));
                        e.result = ER_NOT_UNIQUE_SUM;
                    }
#else
                    // Map (0,1,2), (1,2,0), and (2,0,1) to +1.
                    // Map (0,2,1), (2,1,0), and (1,0,2) to -1.
                    int parity = (((e.axis[0] | (e.axis[1] << 2)) >> e.axis[2]) & 1);
                    Real const sgn = (parity & 1 ? (Real)+1 : (Real)-1);

                    if (r(e.axis[0], e.axis[2]) < (Real)1)
                    {
                        if (r(e.axis[0], e.axis[2]) > (Real)-1)
                        {
                            e.angle[0] = std::atan2(sgn * r(e.axis[1], e.axis[2]),
                                r(e.axis[2], e.axis[2]));
                            e.angle[1] = std::asin(-sgn * r(e.axis[0], e.axis[2]));
                            e.angle[2] = std::atan2(sgn * r(e.axis[0], e.axis[1]),
                                r(e.axis[0], e.axis[0]));
                            e.result = ER_UNIQUE;
                        }
                        else
                        {
                            e.angle[0] = (Real)0;
                            e.angle[1] = sgn * (Real)GTE_C_HALF_PI;
                            e.angle[2] = std::atan2(-sgn * r(e.axis[1], e.axis[0]),
                                r(e.axis[1], e.axis[1]));
                            e.result = ER_NOT_UNIQUE_DIF;
                        }
                    }
                    else
                    {
                        e.angle[0] = (Real)0;
                        e.angle[1] = -sgn * (Real)GTE_C_HALF_PI;
                        e.angle[2] = std::atan2(-sgn * r(e.axis[1], e.axis[0]),
                            r(e.axis[1], e.axis[1]));
                        e.result = ER_NOT_UNIQUE_SUM;
                    }
#endif
                }
                else
                {
#if defined(GTE_USE_MAT_VEC)
                    // Map (0,2,0), (1,0,1), and (2,1,2) to +1.
                    // Map (0,1,0), (1,2,1), and (2,0,2) to -1.
                    int b0 = 3 - e.axis[1] - e.axis[2];
                    int parity = (((b0 | (e.axis[1] << 2)) >> e.axis[2]) & 1);
                    Real const sgn = (parity & 1 ? (Real)+1 : (Real)-1);

                    if (r(e.axis[2], e.axis[2]) < (Real)1)
                    {
                        if (r(e.axis[2], e.axis[2]) > (Real)-1)
                        {
                            e.angle[2] = std::atan2(r(e.axis[1], e.axis[2]),
                                sgn * r(b0, e.axis[2]));
                            e.angle[1] = std::acos(r(e.axis[2], e.axis[2]));
                            e.angle[0] = std::atan2(r(e.axis[2], e.axis[1]),
                                -sgn * r(e.axis[2], b0));
                            e.result = ER_UNIQUE;
                        }
                        else
                        {
                            e.angle[2] = (Real)0;
                            e.angle[1] = (Real)GTE_C_PI;
                            e.angle[0] = std::atan2(sgn * r(e.axis[1], b0),
                                r(e.axis[1], e.axis[1]));
                            e.result = ER_NOT_UNIQUE_DIF;
                        }
                    }
                    else
                    {
                        e.angle[2] = (Real)0;
                        e.angle[1] = (Real)0;
                        e.angle[0] = std::atan2(sgn * r(e.axis[1], b0),
                            r(e.axis[1], e.axis[1]));
                        e.result = ER_NOT_UNIQUE_SUM;
                    }
#else
                    // Map (0,2,0), (1,0,1), and (2,1,2) to -1.
                    // Map (0,1,0), (1,2,1), and (2,0,2) to +1.
                    int b2 = 3 - e.axis[0] - e.axis[1];
                    int parity = (((b2 | (e.axis[1] << 2)) >> e.axis[0]) & 1);
                    Real const sgn = (parity & 1 ? (Real)-1 : (Real)+1);

                    if (r(e.axis[0], e.axis[0]) < (Real)1)
                    {
                        if (r(e.axis[0], e.axis[0]) > (Real)-1)
                        {
                            e.angle[0] = std::atan2(r(e.axis[1], e.axis[0]),
                                sgn * r(b2, e.axis[0]));
                            e.angle[1] = std::acos(r(e.axis[0], e.axis[0]));
                            e.angle[2] = std::atan2(r(e.axis[0], e.axis[1]),
                                -sgn * r(e.axis[0], b2));
                            e.result = ER_UNIQUE;
                        }
                        else
                        {
                            e.angle[0] = (Real)0;
                            e.angle[1] = (Real)GTE_C_PI;
                            e.angle[2] = std::atan2(sgn * r(e.axis[1], b2),
                                r(e.axis[1], e.axis[1]));
                            e.result = ER_NOT_UNIQUE_DIF;
                        }
                    }
                    else
                    {
                        e.angle[0] = (Real)0;
                        e.angle[1] = (Real)0;
                        e.angle[2] = std::atan2(sgn * r(e.axis[1], b2),
                            r(e.axis[1], e.axis[1]));
                        e.result = ER_NOT_UNIQUE_SUM;
                    }
#endif
                }
            }
            else
            {
                // Invalid angles.
                e.angle[0] = (Real)0;
                e.angle[1] = (Real)0;
                e.angle[2] = (Real)0;
                e.result = ER_INVALID;
            }
        }

        // Convert Euler angles to a rotation matrix.  The three integer
        // inputs are in {0,1,2} and correspond to world directions
        // unit(0) = (1,0,0), unit(1) = (0,1,0), or unit(2) = (0,0,1).  The
        // triples (N0,N1,N2) must be in the following set,
        //   {(0,1,2),(0,2,1),(1,0,2),(1,2,0),(2,0,1),(2,1,0),
        //    (0,1,0),(0,2,0),(1,0,1),(1,2,1),(2,0,2),(2,1,2)}
        // The rotation matrix is
        //   [GTE_USE_MAT_VEC]
        //     R(unit(N2),angleN2)*R(unit(N1),angleN1)*R(unit(N0),angleN0)
        // or
        //   [GTE_USE_VEC_MAT]
        //     R(unit(N0),angleN0)*R(unit(N1),angleN1)*R(unit(N2),angleN2)
        // The conventions of constructor Matrix3(axis,angle) apply here as
        // well.
        //
        // NOTE:  The reversal of order is chosen so that a rotation matrix
        // built with one multiplication convention is the transpose of the
        // rotation matrix built with the other multiplication convention.
        // Thus,
        // [GTE_USE_MAT_VEC]
        //   Matrix3x3<Real> R_mvconvention(N0,N1,N2,angleN0,angleN1,angleN2);
        //   Vector3<Real> V(...);
        //   Vector3<Real> U = R_mvconvention*V;  // (u0,u1,u2) = R2*R1*R0*V
        // [GTE_USE_VEC_MAT]
        //   Matrix3x3<Real> R_vmconvention(N0,N1,N2,angleN0,angleN1,angleN2);
        //   Vector3<Real> V(...);
        //   Vector3<Real> U = R_mvconvention*V;  // (u0,u1,u2) = V*R0*R1*R2
        // In either convention, you get the same 3-tuple U.
        static void Convert(EulerAngles<Real> const& e, Matrix<N, N, Real>& r)
        {
            static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

            if (0 <= e.axis[0] && e.axis[0] < 3
                && 0 <= e.axis[1] && e.axis[1] < 3
                && 0 <= e.axis[2] && e.axis[2] < 3
                && e.axis[1] != e.axis[0]
                && e.axis[1] != e.axis[2])
            {
                Matrix<N, N, Real> r0, r1, r2;
                Convert(AxisAngle<N, Real>(Vector<N, Real>::Unit(e.axis[0]),
                    e.angle[0]), r0);
                Convert(AxisAngle<N, Real>(Vector<N, Real>::Unit(e.axis[1]),
                    e.angle[1]), r1);
                Convert(AxisAngle<N, Real>(Vector<N, Real>::Unit(e.axis[2]),
                    e.angle[2]), r2);
#if defined(GTE_USE_MAT_VEC)
                r = r2 * r1 * r0;
#else
                r = r0 * r1 * r2;
#endif
            }
            else
            {
                // Invalid angles.
                r.MakeIdentity();
            }
        }

        // Convert a quaternion to an axis-angle pair, where
        //   q = sin(angle/2)*(axis[0]*i+axis[1]*j+axis[2]*k)+cos(angle/2)
        static void Convert(Quaternion<Real> const& q, AxisAngle<N, Real>& a)
        {
            static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

            a.axis.MakeZero();

            Real axisSqrLen = q[0] * q[0] + q[1] * q[1] + q[2] * q[2];
            if (axisSqrLen > (Real)0)
            {
#if defined(GTE_USE_MAT_VEC)
                Real adjust = ((Real)1) / std::sqrt(axisSqrLen);
#else
                Real adjust = ((Real)-1) / std::sqrt(axisSqrLen);
#endif
                a.axis[0] = q[0] * adjust;
                a.axis[1] = q[1] * adjust;
                a.axis[2] = q[2] * adjust;
                Real cs = std::max(std::min(q[3], (Real)1), (Real)-1);
                a.angle = (Real)2 * std::acos(cs);
            }
            else
            {
                // The angle is 0 (modulo 2*pi). Any axis will work, so choose
                // the Unit(0) axis.
                a.axis[0] = (Real)1;
                a.angle = (Real)0;
            }
        }

        // Convert an axis-angle pair to a quaternion, where
        //   q = sin(angle/2)*(axis[0]*i+axis[1]*j+axis[2]*k)+cos(angle/2)
        static void Convert(AxisAngle<N, Real> const& a, Quaternion<Real>& q)
        {
            static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

#if defined(GTE_USE_MAT_VEC)
            Real halfAngle = (Real)0.5 * a.angle;
#else
            Real halfAngle = (Real)-0.5 * a.angle;
#endif
            Real sn = std::sin(halfAngle);
            q[0] = sn * a.axis[0];
            q[1] = sn * a.axis[1];
            q[2] = sn * a.axis[2];
            q[3] = std::cos(halfAngle);
        }

        // Convert a quaternion to Euler angles.  The quaternion is converted
        // to a matrix which is then converted to Euler angles.
        static void Convert(Quaternion<Real> const& q, EulerAngles<Real>& e)
        {
            static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

            Matrix<N, N, Real> r;
            Convert(q, r);
            Convert(r, e);
        }

        // Convert Euler angles to a quaternion.  The Euler angles are
        // converted to a matrix which is then converted to a quaternion.
        static void Convert(EulerAngles<Real> const& e, Quaternion<Real>& q)
        {
            static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

            Matrix<N, N, Real> r;
            Convert(e, r);
            Convert(r, q);
        }

        // Convert an axis-angle pair to Euler angles.  The axis-angle pair
        // is converted to a quaternion which is then converted to Euler
        // angles.
        static void Convert(AxisAngle<N, Real> const& a, EulerAngles<Real>& e)
        {
            static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

            Quaternion<Real> q;
            Convert(a, q);
            Convert(q, e);
        }

        // Convert Euler angles to an axis-angle pair.  The Euler angles are
        // converted to a quaternion which is then converted to an axis-angle
        // pair.
        static void Convert(EulerAngles<Real> const& e, AxisAngle<N, Real>& a)
        {
            static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

            Quaternion<Real> q;
            Convert(e, q);
            Convert(q, a);
        }
    };
}