KSG-Framework/Mat4.h

#pragma once

#include <iostream>

#include "Logging.h"
#include "Mat3.h"
#include "Vec3.h"

namespace Framework
{
    template<typename T>
    //! A 4x4 Matrix
    class Mat4
    {
    public:
        T elements[4][4]; //! The elements of the matrix

        //! Copies all values from another matrix
        //! \param r The other matrix
        Mat4& operator=(const Mat4& r)
        {
            memcpy(elements, r.elements, sizeof(elements));
            return *this;
        }

        //! Scales the matrix
        //! \param r The factor
        Mat4& operator*=(const T r)
        {
            for (T& e : elements)
                e *= r;
            return *this;
        }

        //! Multiplies the matrix with another
        //! \param r The other matrix
        Mat4& operator*=(const Mat4& r)
        {
            return *this = *this * r;
        }

        //! Scales the matrix without modifying it
        //! \param r The factor
        Mat4 operator*(const T r) const
        {
            Mat4 result = *this;
            return result *= r;
        }

        //! Multiplies two matrices
        //! \param r The other matrix
        Mat4 operator*(const Mat4& r) const
        {
            Mat4 result;
            for (int j = 0; j < 4; j++)
            {
                for (int k = 0; k < 4; k++)
                {
                    T sum = 0;
                    for (int i = 0; i < 4; i++)
                        sum += elements[j][i] * r.elements[i][k];
                    result.elements[j][k] = sum;
                }
            }
            return result;
        }

        //! Multiplies the matrix with a vector
        //! \param r The vector
        Vec3<T> operator*(const Vec3<T>& r) const
        {
            Vec3<T> result;
            result.x = elements[0][0] * r.x + elements[0][1] * r.y
                     + elements[0][2] * r.z + elements[0][3];
            result.y = elements[1][0] * r.x + elements[1][1] * r.y
                     + elements[1][2] * r.z + elements[1][3];
            result.z = elements[2][0] * r.x + elements[2][1] * r.y
                     + elements[2][2] * r.z + elements[2][3];
            return result;
        }

        Vec3<T> mult0(const Vec3<T>& r) const
        {
            Vec3<T> result;
            result.x = elements[0][0] * r.x + elements[0][1] * r.y
                     + elements[0][2] * r.z;
            result.y = elements[1][0] * r.x + elements[1][1] * r.y
                     + elements[1][2] * r.z;
            result.z = elements[2][0] * r.x + elements[2][1] * r.y
                     + elements[2][2] * r.z;
            return result;
        }

        //! Calculates the inverse matrix
        Mat4 getInverse() const
        {
            Mat4 ret;
            ret.elements[0][0]
                = elements[1][1] * elements[2][2] * elements[3][3]
                - elements[1][1] * elements[2][3] * elements[3][2]
                - elements[2][1] * elements[1][2] * elements[3][3]
                + elements[2][1] * elements[1][3] * elements[3][2]
                + elements[3][1] * elements[1][2] * elements[2][3]
                - elements[3][1] * elements[1][3] * elements[2][2];

            ret.elements[1][0]
                = -elements[1][0] * elements[2][2] * elements[3][3]
                + elements[1][0] * elements[2][3] * elements[3][2]
                + elements[2][0] * elements[1][2] * elements[3][3]
                - elements[2][0] * elements[1][3] * elements[3][2]
                - elements[3][0] * elements[1][2] * elements[2][3]
                + elements[3][0] * elements[1][3] * elements[2][2];

            ret.elements[2][0]
                = elements[1][0] * elements[2][1] * elements[3][3]
                - elements[1][0] * elements[2][3] * elements[3][1]
                - elements[2][0] * elements[1][1] * elements[3][3]
                + elements[2][0] * elements[1][3] * elements[3][1]
                + elements[3][0] * elements[1][1] * elements[2][3]
                - elements[3][0] * elements[1][3] * elements[2][1];

            ret.elements[3][0]
                = -elements[1][0] * elements[2][1] * elements[3][2]
                + elements[1][0] * elements[2][2] * elements[3][1]
                + elements[2][0] * elements[1][1] * elements[3][2]
                - elements[2][0] * elements[1][2] * elements[3][1]
                - elements[3][0] * elements[1][1] * elements[2][2]
                + elements[3][0] * elements[1][2] * elements[2][1];

            ret.elements[0][1]
                = -elements[0][1] * elements[2][2] * elements[3][3]
                + elements[0][1] * elements[2][3] * elements[3][2]
                + elements[2][1] * elements[0][2] * elements[3][3]
                - elements[2][1] * elements[0][3] * elements[3][2]
                - elements[3][1] * elements[0][2] * elements[2][3]
                + elements[3][1] * elements[0][3] * elements[2][2];

            ret.elements[1][1]
                = elements[0][0] * elements[2][2] * elements[3][3]
                - elements[0][0] * elements[2][3] * elements[3][2]
                - elements[2][0] * elements[0][2] * elements[3][3]
                + elements[2][0] * elements[0][3] * elements[3][2]
                + elements[3][0] * elements[0][2] * elements[2][3]
                - elements[3][0] * elements[0][3] * elements[2][2];

            ret.elements[2][1]
                = -elements[0][0] * elements[2][1] * elements[3][3]
                + elements[0][0] * elements[2][3] * elements[3][1]
                + elements[2][0] * elements[0][1] * elements[3][3]
                - elements[2][0] * elements[0][3] * elements[3][1]
                - elements[3][0] * elements[0][1] * elements[2][3]
                + elements[3][0] * elements[0][3] * elements[2][1];

            ret.elements[3][1]
                = elements[0][0] * elements[2][1] * elements[3][2]
                - elements[0][0] * elements[2][2] * elements[3][1]
                - elements[2][0] * elements[0][1] * elements[3][2]
                + elements[2][0] * elements[0][2] * elements[3][1]
                + elements[3][0] * elements[0][1] * elements[2][2]
                - elements[3][0] * elements[0][2] * elements[2][1];

            ret.elements[0][2]
                = elements[0][1] * elements[1][2] * elements[3][3]
                - elements[0][1] * elements[1][3] * elements[3][2]
                - elements[1][1] * elements[0][2] * elements[3][3]
                + elements[1][1] * elements[0][3] * elements[3][2]
                + elements[3][1] * elements[0][2] * elements[1][3]
                - elements[3][1] * elements[0][3] * elements[1][2];

            ret.elements[1][2]
                = -elements[0][0] * elements[1][2] * elements[3][3]
                + elements[0][0] * elements[1][3] * elements[3][2]
                + elements[1][0] * elements[0][2] * elements[3][3]
                - elements[1][0] * elements[0][3] * elements[3][2]
                - elements[3][0] * elements[0][2] * elements[1][3]
                + elements[3][0] * elements[0][3] * elements[1][2];

            ret.elements[2][2]
                = elements[0][0] * elements[1][1] * elements[3][3]
                - elements[0][0] * elements[1][3] * elements[3][1]
                - elements[1][0] * elements[0][1] * elements[3][3]
                + elements[1][0] * elements[0][3] * elements[3][1]
                + elements[3][0] * elements[0][1] * elements[1][3]
                - elements[3][0] * elements[0][3] * elements[1][1];

            ret.elements[3][2]
                = -elements[0][0] * elements[1][1] * elements[3][2]
                + elements[0][0] * elements[1][2] * elements[3][1]
                + elements[1][0] * elements[0][1] * elements[3][2]
                - elements[1][0] * elements[0][2] * elements[3][1]
                - elements[3][0] * elements[0][1] * elements[1][2]
                + elements[3][0] * elements[0][2] * elements[1][1];

            ret.elements[0][3]
                = -elements[0][1] * elements[1][2] * elements[2][3]
                + elements[0][1] * elements[1][3] * elements[2][2]
                + elements[1][1] * elements[0][2] * elements[2][3]
                - elements[1][1] * elements[0][3] * elements[2][2]
                - elements[2][1] * elements[0][2] * elements[1][3]
                + elements[2][1] * elements[0][3] * elements[1][2];

            ret.elements[1][3]
                = elements[0][0] * elements[1][2] * elements[2][3]
                - elements[0][0] * elements[1][3] * elements[2][2]
                - elements[1][0] * elements[0][2] * elements[2][3]
                + elements[1][0] * elements[0][3] * elements[2][2]
                + elements[2][0] * elements[0][2] * elements[1][3]
                - elements[2][0] * elements[0][3] * elements[1][2];

            ret.elements[2][3]
                = -elements[0][0] * elements[1][1] * elements[2][3]
                + elements[0][0] * elements[1][3] * elements[2][1]
                + elements[1][0] * elements[0][1] * elements[2][3]
                - elements[1][0] * elements[0][3] * elements[2][1]
                - elements[2][0] * elements[0][1] * elements[1][3]
                + elements[2][0] * elements[0][3] * elements[1][1];

            ret.elements[3][3]
                = elements[0][0] * elements[1][1] * elements[2][2]
                - elements[0][0] * elements[1][2] * elements[2][1]
                - elements[1][0] * elements[0][1] * elements[2][2]
                + elements[1][0] * elements[0][2] * elements[2][1]
                + elements[2][0] * elements[0][1] * elements[1][2]
                - elements[2][0] * elements[0][2] * elements[1][1];

            T det = elements[0][0] * ret.elements[0][0]
                  + elements[0][1] * ret.elements[1][0]
                  + elements[0][2] * ret.elements[2][0]
                  + elements[0][3] * ret.elements[3][0];

            if (det == 0)
            {
                Logging::error() << "Error creating the inverse matrix";
                return ret;
            }
            det = 1.0f / det;

            for (int i = 0; i < 16; i++)
                ret.elements[i / 4][i % 4] = ret.elements[i / 4][i % 4] * det;

            return ret;
        }

        //! Creates a matrix that rotates a vector around the Z axis when
        //! multiplied with it \param radian The angle in radians
        static Mat4 rotationZ(T radian)
        {
            const T cosTheta = (T)lowPrecisionCos(radian);
            const T sinTheta = (T)lowPrecisionSin(radian);
            Mat4 r = {cosTheta,
                -sinTheta,
                0,
                0,
                sinTheta,
                cosTheta,
                0,
                0,
                0,
                0,
                1,
                0,
                0,
                0,
                0,
                1};
            return r;
        }

        //! Creates a matrix that rotates a vector around the X axis when
        //! multiplied with it \param radian The angle in radians
        static Mat4 rotationX(T radian)
        {
            const T cosTheta = (T)lowPrecisionCos(radian);
            const T sinTheta = (T)lowPrecisionSin(radian);
            Mat4 r = {1,
                0,
                0,
                0,
                0,
                cosTheta,
                -sinTheta,
                0,
                0,
                sinTheta,
                cosTheta,
                0,
                0,
                0,
                0,
                1};
            return r;
        }

        //! Creates a matrix that rotates a vector around the Y axis when
        //! multiplied with it \param radian The angle in radians
        static Mat4 rotationY(T radian)
        {
            const T cosTheta = (T)lowPrecisionCos(radian);
            const T sinTheta = (T)lowPrecisionSin(radian);
            Mat4 r = {cosTheta,
                0,
                sinTheta,
                0,
                0,
                1,
                0,
                0,
                -sinTheta,
                0,
                cosTheta,
                0,
                0,
                0,
                0,
                1};
            return r;
        }

        //! Creates a matrix that scales a vector when multiplied with it
        //! \param faktor The factor
        static Mat4 scaling(T faktor)
        {
            Mat4 s = {
                faktor, 0, 0, 0, 0, faktor, 0, 0, 0, 0, faktor, 0, 0, 0, 0, 1};
            return s;
        }

        //! Creates a matrix that scales a vector when multiplied with it
        //! \param faktorX The factor for the X component of the vector
        //! \param faktorY The factor for the Y component of the vector
        //! \param faktorZ The factor for the Z component of the vector
        static Mat4 scaling(T faktorX, T faktorY, T faktorZ)
        {
            Mat4 s = {faktorX,
                0,
                0,
                0,
                0,
                faktorY,
                0,
                0,
                0,
                0,
                faktorZ,
                0,
                0,
                0,
                0,
                1};
            return s;
        }

        //! Creates a matrix that translates a vector when multiplied with it
        //! \param offset The coordinates by which the vector should be shifted
        static Mat4 translation(const Vec3<T> offset)
        {
            Mat4 t = {1,
                0,
                0,
                offset.x,
                0,
                1,
                0,
                offset.y,
                0,
                0,
                1,
                offset.z,
                0,
                0,
                0,
                1};
            return t;
        }

        //! Creates a matrix that projects a vector onto the screen
        //! \param openingAngle The opening angle of the camera in radians
        //! \param bildschirmXY The aspect ratio of the rectangle on the
        //! screen to draw in. (Width / Height) \param
        //! minz The minimum distance to the camera from which drawing begins \param
        //! maxZ The maximum distance to the camera beyond which drawing stops
        static Mat4 projektion(
            float openingAngle, float bildschirmXY, float minZ, float maxZ)
        {
            Mat4 p = {(float)(1 / tan(openingAngle / 2)) / bildschirmXY,
                0,
                0,
                0,
                0,
                (float)(1 / tan(openingAngle / 2)),
                0,
                0,
                0,
                0,
                maxZ / (maxZ - minZ),
                -(minZ * maxZ) / (maxZ - minZ),
                0,
                0,
                1,
                0};
            return p;
        }

        //! Creates an identity matrix that can be multiplied with anything
        //! without changing it
        static Mat4 identity()
        {
            Mat4 i = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
            return i;
        }

        //! Returns a rotation matrix such that vector a, when rotated by it,
        //! points in the direction of vector b \param a The vector to rotate
        //! \param b The vector to rotate towards
        static Mat4 rotationTo(Vec3<T>& a, Vec3<T>& b)
        {
            Vec3<T> aNorm = Vec3<T>(a).normalize();
            Vec3<T> bNorm = Vec3<T>(b).normalize();
            Vec3<T> v = aNorm.crossProduct(bNorm);
            T s = v.getLengthSq();
            T c = aNorm * bNorm;
            T m = (1 - c) / s;
            Mat3<T> cpm({0, -v.z, v.y, v.z, 0, -v.x, -v.y, v.x, 0});
            Mat3<T> cpm2 = cpm * cpm;
            Mat3<T> res = Mat3<T>::identity() + cpm + cpm2 * m;
            return Mat4({res.elements[0][0],
                res.elements[0][1],
                res.elements[0][2],
                0,
                res.elements[1][0],
                res.elements[1][1],
                res.elements[1][2],
                0,
                res.elements[2][0],
                res.elements[2][1],
                res.elements[2][2],
                0,
                0,
                0,
                0,
                1});
        }
    };
} // namespace Framework