j3ml/include/J3ML/LinearAlgebra/Matrix3x3.h

#pragma once

#include <J3ML/LinearAlgebra/Common.h>
#include <J3ML/LinearAlgebra/Vector2.h>
#include <J3ML/LinearAlgebra/Vector3.h>
#include <J3ML/LinearAlgebra/Quaternion.h>

namespace J3ML::LinearAlgebra {



    /** Sets the top-left 3x3 area of the matrix to the rotation matrix about the X-axis. Elements
        outside the top-left 3x3 area are ignored. This matrix rotates counterclockwise if multiplied
        in the order M*v, and clockwise if rotated in the order v*M.
        @param m The matrix to store the result.
        @param angle the rotation angle in radians. */
    template <typename Matrix>
    void Set3x3PartRotateX(Matrix &m, float angle)
    {
        float sinz, cosz;
        sinz = std::sin(angle);
        cosz = std::cos(angle);

        m[0][0] = 1.f; m[0][1] =  0.f; m[0][2] =   0.f;
        m[1][0] = 0.f; m[1][1] = cosz; m[1][2] = -sinz;
        m[2][0] = 0.f; m[2][1] = sinz; m[2][2] =  cosz;
    }

    /** Sets the top-left 3x3 area of the matrix to the rotation matrix about the Y-axis. Elements
        outside the top-left 3x3 area are ignored. This matrix rotates counterclockwise if multiplied
        in the order M*v, and clockwise if rotated in the order v*M.
        @param m The matrix to store the result
        @param angle The rotation angle in radians. */
    template <typename Matrix>
    void Set3x3PartRotateY(Matrix &m, float angle)
    {
        float sinz, cosz;
        sinz = std::sin(angle);
        cosz = std::cos(angle);

        m[0][0] = cosz;  m[0][1] = 0.f; m[0][2] = sinz;
        m[1][0] = 0.f;   m[1][1] = 1.f; m[1][2] = 0.f;
        m[2][0] = -sinz; m[2][1] = 0.f; m[2][2] = cosz;
    }

    /** Sets the top-left 3x3 area of the matrix to the rotation matrix about the Z-axis. Elements
        outside the top-left 3x3 area are ignored. This matrix rotates counterclockwise if multiplied
        in the order of M*v, and clockwise if rotated in the order v*M.
        @param m The matrix to store the result.
        @param angle The rotation angle in radians. */
    template <typename Matrix>
    void Set3x3RotatePartZ(Matrix &m, float angle)
    {
        float sinz, cosz;
        sinz = std::sin(angle);
        cosz = std::cos(angle);

        m[0][0] = cosz; m[0][1] = -sinz; m[0][2] = 0.f;
        m[1][0] = sinz; m[1][1] =  cosz; m[1][2] = 0.f;
        m[2][0] =  0.f; m[2][1] =   0.f; m[2][2] = 1.f;
    }


    /** Computes the matrix M = R_x * R_y * R_z, where R_d is the cardinal rotation matrix
        about the axis +d, rotating counterclockwise.
        This function was adapted from https://www.geometrictools.com/Documentation/EulerAngles.pdf .
        Parameters x y and z are the angles of rotation, in radians. */
    template <typename Matrix>
    void Set3x3PartRotateEulerXYZ(Matrix &m, float x, float y, float z)
    {
        // TODO: vectorize to compute 4 sines + cosines at one time;
        float cx = std::cos(x);
        float sx = std::sin(x);
        float cy = std::cos(y);
        float sy = std::sin(y);
        float cz = std::cos(z);
        float sz = std::sin(z);

        m[0][0] = cy * cz; m[0][1] = -cy * sz; m[0][2] = sy;
        m[1][0] = cz*sx*sy + cx*sz; m[1][1] = cx*cz - sx*sy*sz; m[1][2] = -cy*sx;
        m[2][0] = -cx*cz*sy + sx*sz; m[2][1] = cz*sx + cx*sy*sz; m[2][2] = cx*cy;
    }

    class Quaternion;

    /// A 3-by-3 matrix for linear transformations of 3D geometry.
    /* This can represent any kind of linear transformations of 3D geometry, which include
     * rotation, scale, shear, mirroring, and orthographic projection.
     * A 3x3 matrix cannot represent translation, which requires a 3x4, or perspective projection (4x4).
     * The elements of this matrix are
     *  m_00, m_01, m_02
     *  m_10, m_11, m_12
     *  m_20, m_21, m_22
     *
     *  The element m_yx is the value on the row y and column x.
     *  You can access m_yx using the double-bracket notation m[y][x], or using the member function At.
     *
     *  @note The member functions in this class use the convention that transforms are applied to
     *  vectors in the form M * v. This means that "Matrix3x3 M, M1, M2; M = M1 * M2;" gives a transformation M
     *  that applies M2 first, followed by M1 second
     */

    class Matrix3x3 {
    public: /// Constant Values
        enum { Rows = 3 };
        enum { Cols = 3 };
    public: /// Constant Members
        static const Matrix3x3 Zero;
        static const Matrix3x3 Identity;
        static const Matrix3x3 NaN;
    public: /// Constructors
        /// Creates a new Matrix3x3 with uninitalized member values.
        Matrix3x3() {}

        Matrix3x3(const Matrix3x3& rhs) { Set(rhs); }
        Matrix3x3(float val);
        /// Creates a new Matrix3x3 by explicitly specifying all the matrix elements.
        /// The elements are specified in row-major format, i.e. the first row first followed by the second and third row.
        /// E.g. The element m10 denotes the scalar at second (idx 1) row, first (idx 0) column.
        Matrix3x3(float m00, float m01, float m02,
                  float m10, float m11, float m12,
                  float m20, float m21, float m22);
        /// Constructs the matrix by explicitly specifying the three column vectors.
        /** @param col0 The first column. If this matrix represents a change-of-basis transformation, this parameter is the world-space
            direction of the local X axis.
            @param col1 The second column. If this matrix represents a change-of-basis transformation, this parameter is the world-space
            direction of the local Y axis.
            @param col2 The third column. If this matrix represents a change-of-basis transformation, this parameter is the world-space
            direction of the local Z axis. */
        Matrix3x3(const Vector3& col0, const Vector3& col1, const Vector3& col2);
        /// Constructs this matrix3x3 from the given quaternion.
        explicit Matrix3x3(const Quaternion& orientation);
        /// Constructs this Matrix3x3 from a pointer to an array of floats.
        explicit Matrix3x3(const float *data);
        /// Creates a new Matrix3x3 that rotates about one of the principal axes by the given angle.
        /// Calling RotateX, RotateY, or RotateZ is slightly faster than calling the more generic RotateAxisAngle function.
        static Matrix3x3 RotateX(float radians);
        /// [similarOverload: RotateX] [hideIndex]
        static Matrix3x3 RotateY(float radians);
        /// [similarOverload: RotateX] [hideIndex]
        static Matrix3x3 RotateZ(float radians);
        /// Creates a new M3x3 that rotates about the given axis by the given angle
        static Matrix3x3 RotateAxisAngle(const Vector3& axis, float angleRadians);
        /// Creates a matrix that rotates the sourceDirection vector to coincide with the targetDirection vector.]
        /** Both input direction vectors must be normalized.
            @note There are infinite such rotations - this function returns the rotation that has the shortest angle
            (when decomposed to axis-angle notation)
            @return An orthonormal matrix M with a determinant of +1. For the matrix M it holds that
            M * sourceDirection = targetDirection */
        static Matrix3x3 RotateFromTo(const Vector3& source, const Vector3& direction);
        /// Creates a LookAt matrix.
        /** A LookAt matrix is a rotation matrix that orients an object to face towards a specified target direction.
         * @param forward Specifies the forward direction in the local space of the object. This is the direction
                the model is facing at in its own local/object space, often +X (1,0,0), +Y (0,1,0), or +Z (0,0,1). The
                vector to pass in here depends on the conventions you or your modeling software is using, and it is best
                pick one convention for all your objects, and be consistent.
         * @param target Specifies the desired world space direction the object should look at. This function
                will compute a rotation matrix which will rotate the localForward vector to orient towards this targetDirection
                vector. This input parameter must be a normalized vector.
         * @param localUp Specifies the up direction in the local space of the object. This is the up direction the model
                was authored in, often +Y (0,1,0) or +Z (0,0,1). The vector to pass in here depends on the conventions you
                or your modeling software is using, and it is best to pick one convention for all your objects, and be
                consistent. This input parameter must be a normalized vector. This vector must be perpendicular to the
                vector localForward, i.e. localForward.Dot(localUp) == 0.
         * @param worldUp Specifies the global up direction of the scene in world space. Simply rotating one vector to
                coincide with another (localForward->targetDirection) would cause the up direction of the resulting
                orientation to drift (e.g. the model could be looking at its target its head slanted sideways). To keep
                the up direction straight, this function orients the localUp direction of the model to point towards the
                specified worldUp direction (as closely as possible). The worldUp and targetDirection vectors cannot be
                collinear, but they do not need to be perpendicular either.
         * @return A matrix that maps the given local space forward direction vector to point towards the given target
                direction, and the given local up direction towards the given target world up direction. This matrix can be
                used as the 'world transform' of an object. THe returned matrix M is orthogonal with a determinant of +1.
                For the matrix M it holds that M * localForward = targetDirection, and M * localUp lies in the plane spanned by
                the vectors targetDirection and worldUp.
         * @see RotateFromTo()
         * @note Be aware that the convention of a 'LookAt' matrix in J3ML differs from e.g. GLM. In J3ML, the returned
                matrix is a mapping from local space to world space, meaning that the returned matrix can be used as the 'world transform'
                for any 3D object (camera or not). The view space is the local space of the camera, so this function returns the mapping
                view->world. In GLM, the LookAt function is tied to cameras only, and it returns the inverse mapping world->view.
         */
        static Matrix3x3 LookAt(const Vector3& forward, const Vector3& target, const Vector3& localUp, const Vector3& worldUp);
        /// Creates a new Matrix3x3 that performs the rotation expressed by the given quaternion.
        static Matrix3x3 FromQuat(const Quaternion& orientation);
        /// Creates a new Matrix3x3 as a combination of rotation and scale.
        // This function creates a new matrix M in the form M = R * S
        // where R is a rotation matrix and S is a scale matrix.
        // Transforming a vector v using this matrix computes the vector
        // v' == M * v == R*S*v == (R * (S * v)) which means the scale operation
        // is applied to the vector first, followed by rotation, and finally translation
        static Matrix3x3 FromRS(const Quaternion& rotate, const Vector3& scale);
        static Matrix3x3 FromRS(const Matrix3x3 &rotate, const Vector3& scale);
        /// Creates a new transformation matrix that scales by the given factors.
        // This matrix scales with respect to origin.
        static Matrix3x3 FromScale(float sx, float sy, float sz);
        static Matrix3x3 FromScale(const Vector3& scale);
    public: /// Member Methods
        /// Sets this matrix to perform rotation about the positive X axis which passes through the origin
        /// [similarOverload: SetRotatePart] [hideIndex]
        void SetRotatePartX(float angle);
        /// Sets this matrix to perform rotation about the positive Y axis.
        void SetRotatePartY(float angle);
        /// Sets this matrix to perform rotation about the positive Z axis.
        void SetRotatePartZ(float angle);

        /// Sets this matrix to perform a rotation about the given axis and angle.
        void SetRotatePart(const Vector3& a, float angle);
        void SetRotatePart(const AxisAngle& axisAngle);
        /// Sets this matrix to perform the rotation expressed by the given quaternion.
        void SetRotatePart(const Quaternion& quat);

        /// Returns the given row.
        /** @param row The zero-based index [0, 2] of the row to get. */
        Vector3 GetRow(int index) const;
        Vector3 Row(int index) const { return GetRow(index);}
        /// This method also allows assignment to the retrieved row.
        Vector3& Row(int row);

        /// Returns only the first-three elements of the given row.
        Vector3 GetRow3(int index) const;
        Vector3 Row3(int index) const;
        /// This method also allows assignment to the retrieved row.
        Vector3& Row3(int index);

        /// Returns the given column.
        /** @param col The zero-based index [0, 2] of the column to get. */
        Vector3 GetColumn(int index) const;
        Vector3 Column(int index) const;
        Vector3 Col(int index) const;
        /// This method also allows assignment to the retrieved column.
        Vector3& Col(int index);

        /// Returns only the first three elements of the given column.
        Vector3 GetColumn3(int index) const;
        Vector3 Column3(int index) const;
        Vector3 Col3(int index) const;
        /// This method also allows assignment to the retrieved column.
        Vector3& Col3(int index);



        void SetRow(int i, const Vector3 &vector3);
        void SetColumn(int i, const Vector3& vector);
        void SetAt(int x, int y, float value);

        float &At(int row, int col);
        float At(int x, int y) const;

        void Set(const Matrix3x3 &x3);


        void Orthonormalize(int c0, int c1, int c2);



        /// Convers this rotation matrix to a quaternion.
        /// This function assumes that the matrix is orthonormal (no shear or scaling) and does not perform any mirroring (determinant > 0)
        Quaternion ToQuat() const;
        /// Attempts to convert this matrix to a quaternion. Returns false if the conversion cannot succeed (this matrix was not a rotation
        /// matrix, and there is scaling ,shearing, or mirroring in this matrix)
        bool TryConvertToQuat(Quaternion& q) const;






        /// Returns the main diagonal.
        Vector3 Diagonal() const;
        /// Returns the local +X/+Y/+Z axis in world space.
        /// This is the same as transforming the vector{1,0,0} by this matrix.
        Vector3 WorldX() const;
        /// Returns the local +Y axis in world space.
        /// This is the same as transforming the vector{0,1,0} by this matrix.
        Vector3 WorldY() const;
        /// Returns the local +Z axis in world space.
        /// This is the same as transforming the vector{0,0,1} by this matrix.
        Vector3 WorldZ() const;

        /// Computes the determinant of this matrix.
        // If the determinant is nonzero, this matrix is invertible.
        // If the determinant is negative, this matrix performs reflection about some axis.
        // From http://msdn.microsoft.com/en-us/library/bb204853(VS.85).aspx :
        // "If the determinant is positive, the basis is said to be "positively" oriented (or right-handed).
        // If the determinant is negative, the basis is said to be "negatively" oriented (or left-handed)."
        // @note This function computes 9 LOADs, 9 MULs and 5 ADDs. */
        float Determinant() const;

        // Returns an inverted copy of this matrix. This
        Matrix3x3 Inverse() const;

        // Returns a transposed copy of this matrix.
        Matrix3x3 Transpose() const;

        // Transforms the given vectors by this matrix M, i.e. returns M * (x,y,z)
        Vector2 Transform(const Vector2& rhs) const;
        Vector3 Transform(const Vector3& rhs) const;


        Matrix3x3 ScaleBy(const Vector3& rhs);
        Vector3 GetScale() const;

        Vector3 operator[](int row) const;

        Vector2 operator * (const Vector2& rhs) const;
        Vector3 operator * (const Vector3& rhs) const;
        Matrix3x3 operator * (const Matrix3x3& rhs) const;
        Matrix4x4 operator * (const Matrix4x4& rhs) const;
        Matrix3x3 Mul(const Matrix3x3& rhs) const;
        Matrix4x4 Mul(const Matrix4x4& rhs) const;
        Vector2 Mul(const Vector2& rhs) const;
        Vector3 Mul(const Vector3& rhs) const;
        Vector4 Mul(const Vector4& rhs) const;
        Quaternion Mul(const Quaternion& rhs) const;

        // Returns true if the column vectors of this matrix are all perpendicular to each other.
        bool IsColOrthogonal(float epsilon = 1e-3f) const;
        // Returns true if the row vectors of this matrix are all perpendicular to each other.
        bool IsRowOrthogonal(float epsilon = 1e-3f) const;


        bool HasUniformScale(float epsilon  = 1e-3f) const;

        Vector3 ExtractScale() const;

    protected: /// Member values
        float elems[3][3];
    };
}