KSG-Framework/Mat4.h

#pragma once

#include "Vec3.h"
#include "Mat3.h"
#include <iostream>

namespace Framework
{
    template< typename T >
    //! Eine 4x4 Matrix
    class Mat4
    {
    public:
        T elements[ 4 ][ 4 ]; //! Die Elemente der Matrix
        //! Kopiert alle Werte einer anderen Matrix
        //! \param r Die andere Matrix
        Mat4 &operator=( const Mat4 &r )
        {
            memcpy( elements, r.elements, sizeof( elements ) );
            return *this;
        }
        //! Skalliert die Matrix
        //! \param r der Faktor
        Mat4 &operator*=( const T r )
        {
            for( T &e : elements )
                e *= r;
            return *this;
        }
        //! Multipliziert die MAtrix mit einer anderen
        //! \param r Die andere Matrix
        Mat4 &operator*=( const Mat4 &r )
        {
            return *this = *this * r;
        }
        //! Skalliert die Matrix ohne sie zu verändern
        //! \param r der Faktor
        Mat4 operator*( const T r ) const
        {
            Mat4 result = *this;
            return result *= r;
        }
        //! Multipliziert zwei Matrizen
        //! \param r Die andere 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;
        }
        //! Multiplziert die Matrix mit einem Vektor
        //! \param r Der Vektor
        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;
        }
        //! Berechnet die 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 )
            {
                std::cout << "Fehler beim erstellen der Inversen 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;
        }
        //! Erzeugt eine Matrix, die einen Vektor um die Z Achse dreht, wenn sie mit ihm multipliziert wird
        //! \param radian Der Winkel in Bogenmas
        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;
        }
        //! Erzeugt eine Matrix, die einen Vektor um die X Achse dreht, wenn sie mit ihm multipliziert wird
        //! \param radian Der Winkel in Bogenmas
        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;
        }
        //! Erzeugt eine Matrix, die einen Vektor um die Y Achse dreht, wenn sie mit ihm multipliziert wird
        //! \param radian Der Winkel in Bogenmas
        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;
        }
        //! Erzeugt eine Matrix, die einen Vektor Skalliert, wenn sie mit ihm multipliziert wird
        //! \param faktor Der Faktor
        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;
        }
        //! Erzeugt eine Matrix, die einen Vektor Skalliert, wenn sie mit ihm multipliziert wird
        //! \param faktorX Der Faktor für die X Komponente des Vektors
        //! \param faktorY Der Faktor für die Y Komponente des Vektors
        //! \param faktorZ Der Faktor für die Z Komponente des Vektors
        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;
        }
        //! Erzeugt eine Matrix, die einen Vektor verchiebt, wenn sie mit ihm multipliziert wird
        //! \param offset Die Koordinaten, um die der Vektor verschoben werden soll
        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;
        }
        //! Erzeugt eine Matrix, die einen Vektor auf den Bildschirm Projeziert
        //! \param openingAngle Der Öffnungswinkel der Kamera im Bogenmas
        //! \param bildschirmXY Das Seitenverhältnis des Rechtecks auf dem Bildschirm, in dem gezeichnet werden soll. (Breite / Höhe)
        //! \param minz Der Mindestabstand zur Kamera, ab dem gezeichnet wird
        //! \param maxZ Der Maximalabstand zur Kamera, ab dem nicht mehr gezeichnet wird
        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;
        }
        //! Erzeugt eine Matrix, die mit allem Multipliziert werden kann ohne es zu ändern
        static Mat4 identity()
        {
            Mat4 i = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 };
            return i;
        }

        //! Gibt eine Rotationsmatrix zurück, so dass vector a wenn man ihn damit dreht in richtung des vectors b zeigt
        //! \param a der vector der gedreht werden soll
        //! \param b der vector zu dem gedreht werden soll
        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 } );
        }
    };
}