tetrees/Plugins/Wwise/Source/AkAudio/Classes/GTE/Mathematics/ConvertCoordinates.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/Matrix.h>
#include <Mathematics/GaussianElimination.h>

// Convert points and transformations between two coordinate systems.
// The mathematics involves a change of basis.  See the document
//   https://www.geometrictools.com/Documentation/ConvertingBetweenCoordinateSystems.pdf
// for the details.  Typical usage for 3D conversion is shown next.
//
// // Linear change of basis.  The columns of U are the basis vectors for the
// // source coordinate system.  A vector X = { x0, x1, x2 } in the source
// // coordinate system is represented by
// //   X = x0*(1,0,0) + x1*(0,1,0) + x2*(0,0,1)
// // The Cartesian coordinates for the point are the combination of these
// // terms,
// //   X = (x0, x1, x2)
// // The columns of V are the basis vectors for the target coordinate system.
// // A vector Y = { y0, y1, y2 } in the target coordinate system is
// // represented by
// //   Y = y0*(1,0,0,0) + y1*(0,0,1) + y2*(0,1,0)
// // The Cartesian coordinates for the vector are the combination of these
// // terms,
// //   Y = (y0, y2, y1)
// // The call Y = convert.UToV(X) computes y0, y1 and y2 so that the Cartesian
// // coordinates for X and for Y are the same.  For example,
// //   X = { 1.0, 2.0, 3.0 }
// //     = 1.0*(1,0,0) + 2.0*(0,1,0) + 3.0*(0,0,1)
// //     = (1, 2, 3)
// //   Y = { 1.0, 3.0, 2.0 }
// //     = 1.0*(1,0,0) + 3.0*(0,0,1) + 2.0*(0,1,0)
// //     = (1, 2, 3)
// // X and Y represent the same vector (equal Cartesian coordinates) but have
// // different representations in the source and target coordinates.
//
// ConvertCoordinates<3, double> convert;
// Vector<3, double> X, Y, P0, P1, diff;
// Matrix<3, 3, double> U, V, A, B;
// bool isRHU, isRHV;
// U.SetCol(0, Vector3<double>{1.0, 0.0, 0.0});
// U.SetCol(1, Vector3<double>{0.0, 1.0, 0.0});
// U.SetCol(2, Vector3<double>{0.0, 0.0, 1.0});
// V.SetCol(0, Vector3<double>{1.0, 0.0, 0.0});
// V.SetCol(1, Vector3<double>{0.0, 0.0, 1.0});
// V.SetCol(2, Vector3<double>{0.0, 1.0, 0.0});
// convert(U, true, V, true);
// isRHU = convert.IsRightHandedU();  // true
// isRHV = convert.IsRightHandedV();  // false
// X = { 1.0, 2.0, 3.0 };
// Y = convert.UToV(X);  // { 1.0, 3.0, 2.0 }
// P0 = U*X;
// P1 = V*Y;
// diff = P0 - P1;  // { 0, 0, 0 }
// Y = { 0.0, 1.0, 2.0 };
// X = convert.VToU(Y);  // { 0.0, 2.0, 1.0 }
// P0 = U*X;
// P1 = V*Y;
// diff = P0 - P1;  // { 0, 0, 0 }
// double cs = 0.6, sn = 0.8;  // cs*cs + sn*sn = 1
// A.SetCol(0, Vector3<double>{  c,   s, 0.0});
// A.SetCol(1, Vector3<double>{ -s,   c, 0.0});
// A.SetCol(2, Vector3<double>{0.0, 0.0, 1.0});
// B = convert.UToV(A);
//   // B.GetCol(0) = { c, 0, s}
//   // B.GetCol(1) = { 0, 1, 0}
//   // B.GetCol(2) = {-s, 0, c}
// X = A*X;  // U is VOR
// Y = B*Y;  // V is VOR
// P0 = U*X;
// P1 = V*Y;
// diff = P0 - P1;  // { 0, 0, 0 }
//
// // Affine change of basis.  The first three columns of U are the basis
// // vectors for the source coordinate system and must have last components
// // set to 0.  The last column is the origin for that system and must have
// // last component set to 1.  A point X = { x0, x1, x2, 1 } in the source
// // coordinate system is represented by
// //   X = x0*(-1,0,0,0) + x1*(0,0,1,0) + x2*(0,-1,0,0) + 1*(1,2,3,1)
// // The Cartesian coordinates for the point are the combination of these
// // terms,
// //   X = (-x0 + 1, -x2 + 2, x1 + 3, 1)
// // The first three columns of V are the basis vectors for the target
// // coordinate system and must have last components set to 0.  The last
// // column is the origin for that system and must have last component set
// // to 1.  A point Y = { y0, y1, y2, 1 } in the target coordinate system is
// // represented by
// //   Y = y0*(0,1,0,0) + y1*(-1,0,0,0) + y2*(0,0,1,0) + 1*(4,5,6,1)
// // The Cartesian coordinates for the point are the combination of these
// // terms,
// //   Y = (-y1 + 4, y0 + 5, y2 + 6, 1)
// // The call Y = convert.UToV(X) computes y0, y1 and y2 so that the Cartesian
// // coordinates for X and for Y are the same.  For example,
// //   X = { -1.0, 4.0, -3.0, 1.0 }
// //     = -1.0*(-1,0,0,0) + 4.0*(0,0,1,0) - 3.0*(0,-1,0,0) + 1.0*(1,2,3,1)
// //     = (2, 5, 7, 1)
// //   Y = { 0.0, 2.0, 1.0, 1.0 }
// //     = 0.0*(0,1,0,0) + 2.0*(-1,0,0,0) + 1.0*(0,0,1,0) + 1.0*(4,5,6,1)
// //     = (2, 5, 7, 1)
// // X and Y represent the same point (equal Cartesian coordinates) but have
// // different representations in the source and target affine coordinates.
//
// ConvertCoordinates<4, double> convert;
// Vector<4, double> X, Y, P0, P1, diff;
// Matrix<4, 4, double> U, V, A, B;
// bool isRHU, isRHV;
// U.SetCol(0, Vector4<double>{-1.0, 0.0, 0.0, 0.0});
// U.SetCol(1, Vector4<double>{0.0, 0.0, 1.0, 0.0});
// U.SetCol(2, Vector4<double>{0.0, -1.0, 0.0, 0.0});
// U.SetCol(3, Vector4<double>{1.0, 2.0, 3.0, 1.0});
// V.SetCol(0, Vector4<double>{0.0, 1.0, 0.0, 0.0});
// V.SetCol(1, Vector4<double>{-1.0, 0.0, 0.0, 0.0});
// V.SetCol(2, Vector4<double>{0.0, 0.0, 1.0, 0.0});
// V.SetCol(3, Vector4<double>{4.0, 5.0, 6.0, 1.0});
// convert(U, true, V, false);
// isRHU = convert.IsRightHandedU();  // false
// isRHV = convert.IsRightHandedV();  // true
// X = { -1.0, 4.0, -3.0, 1.0 };
// Y = convert.UToV(X);  // { 0.0, 2.0, 1.0, 1.0 }
// P0 = U*X;
// P1 = V*Y;
// diff = P0 - P1;  // { 0, 0, 0, 0 }
// Y = { 1.0, 2.0, 3.0, 1.0 };
// X = convert.VToU(Y);  // { -1.0, 6.0, -4.0, 1.0 }
// P0 = U*X;
// P1 = V*Y;
// diff = P0 - P1;  // { 0, 0, 0, 0 }
// double c = 0.6, s = 0.8;  // c*c + s*s = 1
// A.SetCol(0, Vector4<double>{  c,  s,   0.0, 0.0});
// A.SetCol(1, Vector4<double>{ -s,  c,   0.0, 0.0});
// A.SetCol(2, Vector4<double>{0.0, 0.0,  1.0, 0.0});
// A.SetCol(3, Vector4<double>{0.3, 1.0, -2.0, 1.0});
// B = convert.UToV(A);
// // B.GetCol(0) = {   1,    0,    0, 0 }
// // B.GetCol(1) = {   0,    c,    s, 0 }
// // B.GetCol(2) = {   0,   -s,    c, 0 }
// // B.GetCol(3) = { 2.0, -0.9, -2.6, 1 }
// X = A*X;  // U is VOR
// Y = Y*B;  // V is VOL (not VOR)
// P0 = U*X;
// P1 = V*Y;
// diff = P0 - P1;  // { 0, 0, 0, 0 }

namespace WwiseGTE
{
    template <int N, typename Real>
    class ConvertCoordinates
    {
    public:
        // Construction of the change of basis matrix.  The implementation
        // supports both linear change of basis and affine change of basis.
        ConvertCoordinates()
            :
            mIsVectorOnRightU(true),
            mIsVectorOnRightV(true),
            mIsRightHandedU(true),
            mIsRightHandedV(true)
        {
            mC.MakeIdentity();
            mInverseC.MakeIdentity();
        }

        // Compute a change of basis between two coordinate systems.  The
        // return value is 'true' iff U and V are invertible.  The
        // matrix-vector multiplication conventions affect the conversion of
        // matrix transformations.  The Boolean inputs indicate how you want
        // the matrices to be interpreted when applied as transformations of
        // a vector.
        bool operator()(
            Matrix<N, N, Real> const& U, bool vectorOnRightU,
            Matrix<N, N, Real> const& V, bool vectorOnRightV)
        {
            // Initialize in case of early exit.
            mC.MakeIdentity();
            mInverseC.MakeIdentity();
            mIsVectorOnRightU = true;
            mIsVectorOnRightV = true;
            mIsRightHandedU = true;
            mIsRightHandedV = true;

            Matrix<N, N, Real> inverseU;
            Real determinantU;
            bool invertibleU = GaussianElimination<Real>()(N, &U[0], &inverseU[0],
                determinantU, nullptr, nullptr, nullptr, 0, nullptr);
            if (!invertibleU)
            {
                return false;
            }

            Matrix<N, N, Real> inverseV;
            Real determinantV;
            bool invertibleV = GaussianElimination<Real>()(N, &V[0], &inverseV[0],
                determinantV, nullptr, nullptr, nullptr, 0, nullptr);
            if (!invertibleV)
            {
                return false;
            }

            mC = inverseU * V;
            mInverseC = inverseV * U;
            mIsVectorOnRightU = vectorOnRightU;
            mIsVectorOnRightV = vectorOnRightV;
            mIsRightHandedU = (determinantU > (Real)0);
            mIsRightHandedV = (determinantV > (Real)0);
            return true;
        }

        // Member access.
        inline Matrix<N, N, Real> const& GetC() const
        {
            return mC;
        }

        inline Matrix<N, N, Real> const& GetInverseC() const
        {
            return mInverseC;
        }

        inline bool IsVectorOnRightU() const
        {
            return mIsVectorOnRightU;
        }

        inline bool IsVectorOnRightV() const
        {
            return mIsVectorOnRightV;
        }

        inline bool IsRightHandedU() const
        {
            return mIsRightHandedU;
        }

        inline bool IsRightHandedV() const
        {
            return mIsRightHandedV;
        }

        // Convert points between coordinate systems.  The names of the
        // systems are U and V to make it clear which inputs of operator()
        // they are associated with.  The X vector stores coordinates for the
        // U-system and the Y vector stores coordinates for the V-system.

        // Y = C^{-1}*X
        inline Vector<N, Real> UToV(Vector<N, Real> const& X) const
        {
            return mInverseC * X;
        }

        // X = C*Y
        inline Vector<N, Real> VToU(Vector<N, Real> const& Y) const
        {
            return mC * Y;
        }

        // Convert transformations between coordinate systems.  The outputs
        // are computed according to the tables shown before the function
        // declarations. The superscript T denotes the transpose operator.
        // vectorOnRightU = true:  transformation is X' = A*X
        // vectorOnRightU = false: transformation is (X')^T = X^T*A
        // vectorOnRightV = true:  transformation is Y' = B*Y
        // vectorOnRightV = false: transformation is (Y')^T = Y^T*B

        // vectorOnRightU  | vectorOnRightV  | output
        // ----------------+-----------------+---------------------
        // true            | true            | C^{-1} * A * C
        // true            | false           | (C^{-1} * A * C)^T 
        // false           | true            | C^{-1} * A^T * C
        // false           | false           | (C^{-1} * A^T * C)^T
        Matrix<N, N, Real> UToV(Matrix<N, N, Real> const& A) const
        {
            Matrix<N, N, Real> product;

            if (mIsVectorOnRightU)
            {
                product = mInverseC * A * mC;
                if (mIsVectorOnRightV)
                {
                    return product;
                }
                else
                {
                    return Transpose(product);
                }
            }
            else
            {
                product = mInverseC * MultiplyATB(A, mC);
                if (mIsVectorOnRightV)
                {
                    return product;
                }
                else
                {
                    return Transpose(product);
                }
            }
        }

        // vectorOnRightU  | vectorOnRightV  | output
        // ----------------+-----------------+---------------------
        // true            | true            | C * B * C^{-1}
        // true            | false           | C * B^T * C^{-1}
        // false           | true            | (C * B * C^{-1})^T
        // false           | false           | (C * B^T * C^{-1})^T
        Matrix<N, N, Real> VToU(Matrix<N, N, Real> const& B) const
        {
            // vectorOnRightU  | vectorOnRightV  | output
            // ----------------+-----------------+---------------------
            // true            | true            | C * B * C^{-1}
            // true            | false           | C * B^T * C^{-1}
            // false           | true            | (C * B * C^{-1})^T
            // false           | false           | (C * B^T * C^{-1})^T
            Matrix<N, N, Real> product;

            if (mIsVectorOnRightV)
            {
                product = mC * B * mInverseC;
                if (mIsVectorOnRightU)
                {
                    return product;
                }
                else
                {
                    return Transpose(product);
                }
            }
            else
            {
                product = mC * MultiplyATB(B, mInverseC);
                if (mIsVectorOnRightU)
                {
                    return product;
                }
                else
                {
                    return Transpose(product);
                }
            }
        }

    private:
        // C = U^{-1}*V, C^{-1} = V^{-1}*U
        Matrix<N, N, Real> mC, mInverseC;
        bool mIsVectorOnRightU, mIsVectorOnRightV;
        bool mIsRightHandedU, mIsRightHandedV;
    };
}