matrix.h

Go to the documentation of this file.

/*
 Copyright (C) 2001-2006, William Joseph.
 All Rights Reserved.

 This file is part of GtkRadiant.

 GtkRadiant is free software; you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation; either version 2 of the License, or
 (at your option) any later version.

 GtkRadiant is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with GtkRadiant; if not, write to the Free Software
 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

#if !defined(INCLUDED_MATH_MATRIX_H)
#define INCLUDED_MATH_MATRIX_H

#include "math/Vector3.h"
#include "math/Vector4.h"

class Matrix4
{
        float m_elements[16];
    public:

        // Default constructor
        Matrix4 ()
        {
        }

        /* NAMED CONSTRUCTORS FOR SPECIFIC MATRICES */

        /* Get a new identity matrix */

        static Matrix4 getIdentity ()
        {
            return Matrix4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
        }

        static Matrix4 getTranslation (const Vector3& translation)
        {
            return Matrix4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, translation.x(), translation.y(), translation.z(), 1);
        }

        // Custom constructor

        Matrix4 (float xx_, float xy_, float xz_, float xw_, float yx_, float yy_, float yz_, float yw_, float zx_,
                float zy_, float zz_, float zw_, float tx_, float ty_, float tz_, float tw_)
        {
            xx() = xx_;
            xy() = xy_;
            xz() = xz_;
            xw() = xw_;
            yx() = yx_;
            yy() = yy_;
            yz() = yz_;
            yw() = yw_;
            zx() = zx_;
            zy() = zy_;
            zz() = zz_;
            zw() = zw_;
            tx() = tx_;
            ty() = ty_;
            tz() = tz_;
            tw() = tw_;
        }

        float& xx ()
        {
            return m_elements[0];
        }
        const float& xx () const
        {
            return m_elements[0];
        }
        float& xy ()
        {
            return m_elements[1];
        }
        const float& xy () const
        {
            return m_elements[1];
        }
        float& xz ()
        {
            return m_elements[2];
        }
        const float& xz () const
        {
            return m_elements[2];
        }
        float& xw ()
        {
            return m_elements[3];
        }
        const float& xw () const
        {
            return m_elements[3];
        }
        float& yx ()
        {
            return m_elements[4];
        }
        const float& yx () const
        {
            return m_elements[4];
        }
        float& yy ()
        {
            return m_elements[5];
        }
        const float& yy () const
        {
            return m_elements[5];
        }
        float& yz ()
        {
            return m_elements[6];
        }
        const float& yz () const
        {
            return m_elements[6];
        }
        float& yw ()
        {
            return m_elements[7];
        }
        const float& yw () const
        {
            return m_elements[7];
        }
        float& zx ()
        {
            return m_elements[8];
        }
        const float& zx () const
        {
            return m_elements[8];
        }
        float& zy ()
        {
            return m_elements[9];
        }
        const float& zy () const
        {
            return m_elements[9];
        }
        float& zz ()
        {
            return m_elements[10];
        }
        const float& zz () const
        {
            return m_elements[10];
        }
        float& zw ()
        {
            return m_elements[11];
        }
        const float& zw () const
        {
            return m_elements[11];
        }
        float& tx ()
        {
            return m_elements[12];
        }
        const float& tx () const
        {
            return m_elements[12];
        }
        float& ty ()
        {
            return m_elements[13];
        }
        const float& ty () const
        {
            return m_elements[13];
        }
        float& tz ()
        {
            return m_elements[14];
        }
        const float& tz () const
        {
            return m_elements[14];
        }
        float& tw ()
        {
            return m_elements[15];
        }
        const float& tw () const
        {
            return m_elements[15];
        }

        Vector4& x ()
        {
            return reinterpret_cast<Vector4&> (xx());
        }
        const Vector4& x () const
        {
            return reinterpret_cast<const Vector4&> (xx());
        }
        Vector4& y ()
        {
            return reinterpret_cast<Vector4&> (yx());
        }
        const Vector4& y () const
        {
            return reinterpret_cast<const Vector4&> (yx());
        }
        Vector4& z ()
        {
            return reinterpret_cast<Vector4&> (zx());
        }
        const Vector4& z () const
        {
            return reinterpret_cast<const Vector4&> (zx());
        }
        Vector4& t ()
        {
            return reinterpret_cast<Vector4&> (tx());
        }
        const Vector4& t () const
        {
            return reinterpret_cast<const Vector4&> (tx());
        }

        const float& index (std::size_t i) const
        {
            return m_elements[i];
        }
        float& index (std::size_t i)
        {
            return m_elements[i];
        }
        const float& index (std::size_t r, std::size_t c) const
        {
            return m_elements[(r << 2) + c];
        }
        float& index (std::size_t r, std::size_t c)
        {
            return m_elements[(r << 2) + c];
        }

        operator float* ()
        {
            return m_elements;
        }

        operator const float* () const
        {
            return m_elements;
        }

        Vector4 transform (const Vector4& vector4) const
        {
            return Vector4(m_elements[0] * vector4[0] + m_elements[4] * vector4[1] + m_elements[8] * vector4[2]
                    + m_elements[12] * vector4[3], m_elements[1] * vector4[0] + m_elements[5] * vector4[1]
                    + m_elements[9] * vector4[2] + m_elements[13] * vector4[3], m_elements[2] * vector4[0]
                    + m_elements[6] * vector4[1] + m_elements[10] * vector4[2] + m_elements[14] * vector4[3],
                    m_elements[3] * vector4[0] + m_elements[7] * vector4[1] + m_elements[11] * vector4[2]
                            + m_elements[15] * vector4[3]);
        }

        Vector4 transform (const Vector3& vector3) const
        {
            return transform(Vector4(vector3, 1));
        }

};

const Matrix4 g_matrix4_identity(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);

inline bool operator== (const Matrix4& self, const Matrix4& other)
{
    return self.xx() == other.xx() && self.xy() == other.xy() && self.xz() == other.xz() && self.xw() == other.xw()
            && self.yx() == other.yx() && self.yy() == other.yy() && self.yz() == other.yz() && self.yw() == other.yw()
            && self.zx() == other.zx() && self.zy() == other.zy() && self.zz() == other.zz() && self.zw() == other.zw()
            && self.tx() == other.tx() && self.ty() == other.ty() && self.tz() == other.tz() && self.tw() == other.tw();
}

inline bool matrix4_equal (const Matrix4& self, const Matrix4& other)
{
    return self == other;
}

inline bool matrix4_equal_epsilon (const Matrix4& self, const Matrix4& other, float epsilon)
{
    return float_equal_epsilon(self.xx(), other.xx(), epsilon) && float_equal_epsilon(self.xy(), other.xy(), epsilon)
            && float_equal_epsilon(self.xz(), other.xz(), epsilon) && float_equal_epsilon(self.xw(), other.xw(),
            epsilon) && float_equal_epsilon(self.yx(), other.yx(), epsilon) && float_equal_epsilon(self.yy(),
            other.yy(), epsilon) && float_equal_epsilon(self.yz(), other.yz(), epsilon) && float_equal_epsilon(
            self.yw(), other.yw(), epsilon) && float_equal_epsilon(self.zx(), other.zx(), epsilon)
            && float_equal_epsilon(self.zy(), other.zy(), epsilon) && float_equal_epsilon(self.zz(), other.zz(),
            epsilon) && float_equal_epsilon(self.zw(), other.zw(), epsilon) && float_equal_epsilon(self.tx(),
            other.tx(), epsilon) && float_equal_epsilon(self.ty(), other.ty(), epsilon) && float_equal_epsilon(
            self.tz(), other.tz(), epsilon) && float_equal_epsilon(self.tw(), other.tw(), epsilon);
}

inline bool matrix4_affine_equal (const Matrix4& self, const Matrix4& other)
{
    return self[0] == other[0] && self[1] == other[1] && self[2] == other[2] && self[4] == other[4] && self[5]
            == other[5] && self[6] == other[6] && self[8] == other[8] && self[9] == other[9] && self[10] == other[10]
            && self[12] == other[12] && self[13] == other[13] && self[14] == other[14];
}

enum Matrix4Handedness
{
    MATRIX4_RIGHTHANDED = 0, MATRIX4_LEFTHANDED = 1,
};

inline Matrix4Handedness matrix4_handedness (const Matrix4& self)
{
    return (self.x().getVector3().crossProduct(self.y().getVector3()).dot(self.z().getVector3()) < 0.0) ? MATRIX4_LEFTHANDED
            : MATRIX4_RIGHTHANDED;
}

inline Matrix4 matrix4_multiplied_by_matrix4 (const Matrix4& self, const Matrix4& other)
{
    return Matrix4(other[0] * self[0] + other[1] * self[4] + other[2] * self[8] + other[3] * self[12], other[0]
            * self[1] + other[1] * self[5] + other[2] * self[9] + other[3] * self[13], other[0] * self[2] + other[1]
            * self[6] + other[2] * self[10] + other[3] * self[14], other[0] * self[3] + other[1] * self[7] + other[2]
            * self[11] + other[3] * self[15], other[4] * self[0] + other[5] * self[4] + other[6] * self[8] + other[7]
            * self[12], other[4] * self[1] + other[5] * self[5] + other[6] * self[9] + other[7] * self[13], other[4]
            * self[2] + other[5] * self[6] + other[6] * self[10] + other[7] * self[14], other[4] * self[3] + other[5]
            * self[7] + other[6] * self[11] + other[7] * self[15], other[8] * self[0] + other[9] * self[4] + other[10]
            * self[8] + other[11] * self[12], other[8] * self[1] + other[9] * self[5] + other[10] * self[9] + other[11]
            * self[13], other[8] * self[2] + other[9] * self[6] + other[10] * self[10] + other[11] * self[14], other[8]
            * self[3] + other[9] * self[7] + other[10] * self[11] + other[11] * self[15], other[12] * self[0]
            + other[13] * self[4] + other[14] * self[8] + other[15] * self[12], other[12] * self[1] + other[13]
            * self[5] + other[14] * self[9] + other[15] * self[13], other[12] * self[2] + other[13] * self[6]
            + other[14] * self[10] + other[15] * self[14], other[12] * self[3] + other[13] * self[7] + other[14]
            * self[11] + other[15] * self[15]);
}

inline void matrix4_multiply_by_matrix4 (Matrix4& self, const Matrix4& other)
{
    self = matrix4_multiplied_by_matrix4(self, other);
}

inline Matrix4 matrix4_premultiplied_by_matrix4 (const Matrix4& self, const Matrix4& other)
{
#if 1
    return matrix4_multiplied_by_matrix4(other, self);
#else
    return Matrix4(
            self[0] * other[0] + self[1] * other[4] + self[2] * other[8] + self[3] * other[12],
            self[0] * other[1] + self[1] * other[5] + self[2] * other[9] + self[3] * other[13],
            self[0] * other[2] + self[1] * other[6] + self[2] * other[10]+ self[3] * other[14],
            self[0] * other[3] + self[1] * other[7] + self[2] * other[11]+ self[3] * other[15],
            self[4] * other[0] + self[5] * other[4] + self[6] * other[8] + self[7] * other[12],
            self[4] * other[1] + self[5] * other[5] + self[6] * other[9] + self[7] * other[13],
            self[4] * other[2] + self[5] * other[6] + self[6] * other[10]+ self[7] * other[14],
            self[4] * other[3] + self[5] * other[7] + self[6] * other[11]+ self[7] * other[15],
            self[8] * other[0] + self[9] * other[4] + self[10]* other[8] + self[11]* other[12],
            self[8] * other[1] + self[9] * other[5] + self[10]* other[9] + self[11]* other[13],
            self[8] * other[2] + self[9] * other[6] + self[10]* other[10]+ self[11]* other[14],
            self[8] * other[3] + self[9] * other[7] + self[10]* other[11]+ self[11]* other[15],
            self[12]* other[0] + self[13]* other[4] + self[14]* other[8] + self[15]* other[12],
            self[12]* other[1] + self[13]* other[5] + self[14]* other[9] + self[15]* other[13],
            self[12]* other[2] + self[13]* other[6] + self[14]* other[10]+ self[15]* other[14],
            self[12]* other[3] + self[13]* other[7] + self[14]* other[11]+ self[15]* other[15]
    );
#endif
}

inline void matrix4_premultiply_by_matrix4 (Matrix4& self, const Matrix4& other)
{
    self = matrix4_premultiplied_by_matrix4(self, other);
}

inline bool matrix4_is_affine (const Matrix4& transform)
{
    return transform[3] == 0 && transform[7] == 0 && transform[11] == 0 && transform[15] == 1;
}

inline Matrix4 matrix4_affine_multiplied_by_matrix4 (const Matrix4& self, const Matrix4& other)
{
    return Matrix4(other[0] * self[0] + other[1] * self[4] + other[2] * self[8], other[0] * self[1] + other[1]
            * self[5] + other[2] * self[9], other[0] * self[2] + other[1] * self[6] + other[2] * self[10], 0, other[4]
            * self[0] + other[5] * self[4] + other[6] * self[8], other[4] * self[1] + other[5] * self[5] + other[6]
            * self[9], other[4] * self[2] + other[5] * self[6] + other[6] * self[10], 0, other[8] * self[0] + other[9]
            * self[4] + other[10] * self[8], other[8] * self[1] + other[9] * self[5] + other[10] * self[9], other[8]
            * self[2] + other[9] * self[6] + other[10] * self[10], 0, other[12] * self[0] + other[13] * self[4]
            + other[14] * self[8] + self[12], other[12] * self[1] + other[13] * self[5] + other[14] * self[9]
            + self[13], other[12] * self[2] + other[13] * self[6] + other[14] * self[10] + self[14], 1);
}

inline void matrix4_affine_multiply_by_matrix4 (Matrix4& self, const Matrix4& other)
{
    self = matrix4_affine_multiplied_by_matrix4(self, other);
}

inline Matrix4 matrix4_affine_premultiplied_by_matrix4 (const Matrix4& self, const Matrix4& other)
{
#if 1
    return matrix4_affine_multiplied_by_matrix4(other, self);
#else
    return Matrix4(
            self[0] * other[0] + self[1] * other[4] + self[2] * other[8],
            self[0] * other[1] + self[1] * other[5] + self[2] * other[9],
            self[0] * other[2] + self[1] * other[6] + self[2] * other[10],
            0,
            self[4] * other[0] + self[5] * other[4] + self[6] * other[8],
            self[4] * other[1] + self[5] * other[5] + self[6] * other[9],
            self[4] * other[2] + self[5] * other[6] + self[6] * other[10],
            0,
            self[8] * other[0] + self[9] * other[4] + self[10]* other[8],
            self[8] * other[1] + self[9] * other[5] + self[10]* other[9],
            self[8] * other[2] + self[9] * other[6] + self[10]* other[10],
            0,
            self[12]* other[0] + self[13]* other[4] + self[14]* other[8] + other[12],
            self[12]* other[1] + self[13]* other[5] + self[14]* other[9] + other[13],
            self[12]* other[2] + self[13]* other[6] + self[14]* other[10]+ other[14],
            1
    )
    );
#endif
}

inline void matrix4_affine_premultiply_by_matrix4 (Matrix4& self, const Matrix4& other)
{
    self = matrix4_affine_premultiplied_by_matrix4(self, other);
}

template<typename Element>
inline BasicVector3<Element> matrix4_transformed_point (const Matrix4& self, const BasicVector3<Element>& point)
{
    return BasicVector3<Element> (static_cast<Element> (self[0] * point[0] + self[4] * point[1] + self[8] * point[2]
            + self[12]),
            static_cast<Element> (self[1] * point[0] + self[5] * point[1] + self[9] * point[2] + self[13]),
            static_cast<Element> (self[2] * point[0] + self[6] * point[1] + self[10] * point[2] + self[14]));
}

template<typename Element>
inline void matrix4_transform_point (const Matrix4& self, BasicVector3<Element>& point)
{
    point = matrix4_transformed_point(self, point);
}

template<typename Element>
inline BasicVector3<Element> matrix4_transformed_direction (const Matrix4& self, const BasicVector3<Element>& direction)
{
    return BasicVector3<Element> (static_cast<Element> (self[0] * direction[0] + self[4] * direction[1] + self[8]
            * direction[2]), static_cast<Element> (self[1] * direction[0] + self[5] * direction[1] + self[9]
            * direction[2]), static_cast<Element> (self[2] * direction[0] + self[6] * direction[1] + self[10]
            * direction[2]));
}

template<typename Element>
inline void matrix4_transform_direction (const Matrix4& self, BasicVector3<Element>& normal)
{
    normal = matrix4_transformed_direction(self, normal);
}

inline Vector4 matrix4_transformed_vector4 (const Matrix4& self, const Vector4& vector4)
{
    return Vector4(self[0] * vector4[0] + self[4] * vector4[1] + self[8] * vector4[2] + self[12] * vector4[3], self[1]
            * vector4[0] + self[5] * vector4[1] + self[9] * vector4[2] + self[13] * vector4[3], self[2] * vector4[0]
            + self[6] * vector4[1] + self[10] * vector4[2] + self[14] * vector4[3], self[3] * vector4[0] + self[7]
            * vector4[1] + self[11] * vector4[2] + self[15] * vector4[3]);
}

inline void matrix4_transform_vector4 (const Matrix4& self, Vector4& vector4)
{
    vector4 = matrix4_transformed_vector4(self, vector4);
}

inline void matrix4_transpose (Matrix4& self)
{
    std::swap(self.xy(), self.yx());
    std::swap(self.xz(), self.zx());
    std::swap(self.xw(), self.tx());
    std::swap(self.yz(), self.zy());
    std::swap(self.yw(), self.ty());
    std::swap(self.zw(), self.tz());
}

inline Matrix4 matrix4_transposed (const Matrix4& self)
{
    return Matrix4(self.xx(), self.yx(), self.zx(), self.tx(), self.xy(), self.yy(), self.zy(), self.ty(), self.xz(),
            self.yz(), self.zz(), self.tz(), self.xw(), self.yw(), self.zw(), self.tw());
}

inline Matrix4 matrix4_affine_inverse (const Matrix4& self)
{
    Matrix4 result;

    // determinant of rotation submatrix
    double det = self[0] * (self[5] * self[10] - self[9] * self[6]) - self[1]
            * (self[4] * self[10] - self[8] * self[6]) + self[2] * (self[4] * self[9] - self[8] * self[5]);

    // throw exception here if (det*det < 1e-25)

    // invert rotation submatrix
    det = 1.0 / det;

    result[0] = ((self[5] * self[10] - self[6] * self[9]) * det);
    result[1] = (-(self[1] * self[10] - self[2] * self[9]) * det);
    result[2] = ((self[1] * self[6] - self[2] * self[5]) * det);
    result[3] = 0;
    result[4] = (-(self[4] * self[10] - self[6] * self[8]) * det);
    result[5] = ((self[0] * self[10] - self[2] * self[8]) * det);
    result[6] = (-(self[0] * self[6] - self[2] * self[4]) * det);
    result[7] = 0;
    result[8] = ((self[4] * self[9] - self[5] * self[8]) * det);
    result[9] = (-(self[0] * self[9] - self[1] * self[8]) * det);
    result[10] = ((self[0] * self[5] - self[1] * self[4]) * det);
    result[11] = 0;

    // multiply translation part by rotation
    result[12] = -(self[12] * result[0] + self[13] * result[4] + self[14] * result[8]);
    result[13] = -(self[12] * result[1] + self[13] * result[5] + self[14] * result[9]);
    result[14] = -(self[12] * result[2] + self[13] * result[6] + self[14] * result[10]);
    result[15] = 1;

    return result;
}

inline void matrix4_affine_invert (Matrix4& self)
{
    self = matrix4_affine_inverse(self);
}

template<int VALUE_>
struct IntegralConstant
{
        enum unnamed_
        {
            VALUE = VALUE_
        };
};

template<typename Row, typename Col>
class Matrix4Index
{
    public:
        typedef IntegralConstant<Row::VALUE> r;
        typedef IntegralConstant<Col::VALUE> c;
        typedef IntegralConstant<(r::VALUE * 4) + c::VALUE> i;
};

template<typename Row, typename Col>
class Matrix4Cofactor
{
    public:
        typedef typename Matrix4Index<typename Row::x, typename Col::x>::i xx;
        typedef typename Matrix4Index<typename Row::x, typename Col::y>::i xy;
        typedef typename Matrix4Index<typename Row::x, typename Col::z>::i xz;
        typedef typename Matrix4Index<typename Row::y, typename Col::x>::i yx;
        typedef typename Matrix4Index<typename Row::y, typename Col::y>::i yy;
        typedef typename Matrix4Index<typename Row::y, typename Col::z>::i yz;
        typedef typename Matrix4Index<typename Row::z, typename Col::x>::i zx;
        typedef typename Matrix4Index<typename Row::z, typename Col::y>::i zy;
        typedef typename Matrix4Index<typename Row::z, typename Col::z>::i zz;
        static double apply (const Matrix4& self)
        {
            return self[xx::VALUE] * (self[yy::VALUE] * self[zz::VALUE] - self[zy::VALUE] * self[yz::VALUE])
                    - self[xy::VALUE] * (self[yx::VALUE] * self[zz::VALUE] - self[zx::VALUE] * self[yz::VALUE])
                    + self[xz::VALUE] * (self[yx::VALUE] * self[zy::VALUE] - self[zx::VALUE] * self[yy::VALUE]);
        }
};

template<int Element>
class Cofactor4
{
    public:
        typedef IntegralConstant<(Element <= 0) ? 1 : 0> x;
        typedef IntegralConstant<(Element <= 1) ? 2 : 1> y;
        typedef IntegralConstant<(Element <= 2) ? 3 : 2> z;
};

inline double matrix4_determinant (const Matrix4& self)
{
    return self.xx() * Matrix4Cofactor<Cofactor4<0> , Cofactor4<0> >::apply(self) - self.xy() * Matrix4Cofactor<
            Cofactor4<0> , Cofactor4<1> >::apply(self) + self.xz()
            * Matrix4Cofactor<Cofactor4<0> , Cofactor4<2> >::apply(self) - self.xw() * Matrix4Cofactor<Cofactor4<0> ,
            Cofactor4<3> >::apply(self);
}

inline Matrix4 matrix4_full_inverse (const Matrix4& self)
{
    double determinant = 1.0 / matrix4_determinant(self);

    return Matrix4(static_cast<float> (Matrix4Cofactor<Cofactor4<0> , Cofactor4<0> >::apply(self) * determinant),
            static_cast<float> (-Matrix4Cofactor<Cofactor4<1> , Cofactor4<0> >::apply(self) * determinant),
            static_cast<float> (Matrix4Cofactor<Cofactor4<2> , Cofactor4<0> >::apply(self) * determinant),
            static_cast<float> (-Matrix4Cofactor<Cofactor4<3> , Cofactor4<0> >::apply(self) * determinant),
            static_cast<float> (-Matrix4Cofactor<Cofactor4<0> , Cofactor4<1> >::apply(self) * determinant),
            static_cast<float> (Matrix4Cofactor<Cofactor4<1> , Cofactor4<1> >::apply(self) * determinant),
            static_cast<float> (-Matrix4Cofactor<Cofactor4<2> , Cofactor4<1> >::apply(self) * determinant),
            static_cast<float> (Matrix4Cofactor<Cofactor4<3> , Cofactor4<1> >::apply(self) * determinant),
            static_cast<float> (Matrix4Cofactor<Cofactor4<0> , Cofactor4<2> >::apply(self) * determinant),
            static_cast<float> (-Matrix4Cofactor<Cofactor4<1> , Cofactor4<2> >::apply(self) * determinant),
            static_cast<float> (Matrix4Cofactor<Cofactor4<2> , Cofactor4<2> >::apply(self) * determinant),
            static_cast<float> (-Matrix4Cofactor<Cofactor4<3> , Cofactor4<2> >::apply(self) * determinant),
            static_cast<float> (-Matrix4Cofactor<Cofactor4<0> , Cofactor4<3> >::apply(self) * determinant),
            static_cast<float> (Matrix4Cofactor<Cofactor4<1> , Cofactor4<3> >::apply(self) * determinant),
            static_cast<float> (-Matrix4Cofactor<Cofactor4<2> , Cofactor4<3> >::apply(self) * determinant),
            static_cast<float> (Matrix4Cofactor<Cofactor4<3> , Cofactor4<3> >::apply(self) * determinant));
}

inline void matrix4_full_invert (Matrix4& self)
{
    self = matrix4_full_inverse(self);
}

inline Vector3 matrix4_get_translation_vec3 (const Matrix4& self)
{
    return self.t().getVector3();
}

inline void matrix4_translate_by_vec3 (Matrix4& self, const Vector3& translation)
{
    matrix4_multiply_by_matrix4(self, Matrix4::getTranslation(translation));
}

inline Matrix4 matrix4_translated_by_vec3 (const Matrix4& self, const Vector3& translation)
{
    return matrix4_multiplied_by_matrix4(self, Matrix4::getTranslation(translation));
}

#include "math/pi.h"

inline float angle_modulate_degrees_range (float angle)
{
    return static_cast<float> (float_mod_range(angle, 360.0));
}

inline Vector3 euler_radians_to_degrees (const Vector3& euler)
{
    return Vector3(static_cast<float> (radians_to_degrees(euler.x())),
            static_cast<float> (radians_to_degrees(euler.y())), static_cast<float> (radians_to_degrees(euler.z())));
}

inline Vector3 euler_degrees_to_radians (const Vector3& euler)
{
    return Vector3(static_cast<float> (degrees_to_radians(euler.x())),
            static_cast<float> (degrees_to_radians(euler.y())), static_cast<float> (degrees_to_radians(euler.z())));
}

inline Matrix4 matrix4_rotation_for_sincos_x (float s, float c)
{
    return Matrix4(1, 0, 0, 0, 0, c, s, 0, 0, -s, c, 0, 0, 0, 0, 1);
}

inline Matrix4 matrix4_rotation_for_x (double x)
{
    return matrix4_rotation_for_sincos_x(static_cast<float> (sin(x)), static_cast<float> (cos(x)));
}

inline Matrix4 matrix4_rotation_for_x_degrees (float x)
{
    return matrix4_rotation_for_x(degrees_to_radians(x));
}

inline Matrix4 matrix4_rotation_for_sincos_y (float s, float c)
{
    return Matrix4(c, 0, -s, 0, 0, 1, 0, 0, s, 0, c, 0, 0, 0, 0, 1);
}

inline Matrix4 matrix4_rotation_for_y (double y)
{
    return matrix4_rotation_for_sincos_y(static_cast<float> (sin(y)), static_cast<float> (cos(y)));
}

inline Matrix4 matrix4_rotation_for_y_degrees (float y)
{
    return matrix4_rotation_for_y(degrees_to_radians(y));
}

inline Matrix4 matrix4_rotation_for_sincos_z (float s, float c)
{
    return Matrix4(c, s, 0, 0, -s, c, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
}

inline Matrix4 matrix4_rotation_for_z (double z)
{
    return matrix4_rotation_for_sincos_z(static_cast<float> (sin(z)), static_cast<float> (cos(z)));
}

inline Matrix4 matrix4_rotation_for_z_degrees (float z)
{
    return matrix4_rotation_for_z(degrees_to_radians(z));
}


inline Matrix4 matrix4_rotation_for_euler_xyz (const Vector3& euler)
{
#if 1

    double cx = cos(euler[0]);
    double sx = sin(euler[0]);
    double cy = cos(euler[1]);
    double sy = sin(euler[1]);
    double cz = cos(euler[2]);
    double sz = sin(euler[2]);

    return Matrix4(static_cast<float> (cy * cz), static_cast<float> (cy * sz), static_cast<float> (-sy), 0,
            static_cast<float> (sx * sy * cz + cx * -sz), static_cast<float> (sx * sy * sz + cx * cz),
            static_cast<float> (sx * cy), 0, static_cast<float> (cx * sy * cz + sx * sz), static_cast<float> (cx * sy
                    * sz + -sx * cz), static_cast<float> (cx * cy), 0, 0, 0, 0, 1);

#else

    return matrix4_premultiply_by_matrix4(
            matrix4_premultiply_by_matrix4(
                    matrix4_rotation_for_x(euler[0]),
                    matrix4_rotation_for_y(euler[1])
            ),
            matrix4_rotation_for_z(euler[2])
    );

#endif
}

inline Matrix4 matrix4_rotation_for_euler_xyz_degrees (const Vector3& euler)
{
    return matrix4_rotation_for_euler_xyz(euler_degrees_to_radians(euler));
}

inline void matrix4_rotate_by_euler_xyz_degrees (Matrix4& self, const Vector3& euler)
{
    matrix4_multiply_by_matrix4(self, matrix4_rotation_for_euler_xyz_degrees(euler));
}

inline Matrix4 matrix4_rotation_for_euler_yzx (const Vector3& euler)
{
    return matrix4_premultiplied_by_matrix4(matrix4_premultiplied_by_matrix4(matrix4_rotation_for_y(euler[1]),
            matrix4_rotation_for_z(euler[2])), matrix4_rotation_for_x(euler[0]));
}

inline Matrix4 matrix4_rotation_for_euler_yzx_degrees (const Vector3& euler)
{
    return matrix4_rotation_for_euler_yzx(euler_degrees_to_radians(euler));
}

inline Matrix4 matrix4_rotation_for_euler_xzy (const Vector3& euler)
{
    return matrix4_premultiplied_by_matrix4(matrix4_premultiplied_by_matrix4(matrix4_rotation_for_x(euler[0]),
            matrix4_rotation_for_z(euler[2])), matrix4_rotation_for_y(euler[1]));
}

inline Matrix4 matrix4_rotation_for_euler_xzy_degrees (const Vector3& euler)
{
    return matrix4_rotation_for_euler_xzy(euler_degrees_to_radians(euler));
}


inline Matrix4 matrix4_rotation_for_euler_yxz (const Vector3& euler)
{
#if 1

    double cx = cos(euler[0]);
    double sx = sin(euler[0]);
    double cy = cos(euler[1]);
    double sy = sin(euler[1]);
    double cz = cos(euler[2]);
    double sz = sin(euler[2]);

    return Matrix4(static_cast<float> (cy * cz + sx * sy * -sz), static_cast<float> (cy * sz + sx * sy * cz),
            static_cast<float> (-cx * sy), 0, static_cast<float> (cx * -sz), static_cast<float> (cx * cz),
            static_cast<float> (sx), 0, static_cast<float> (sy * cz + -sx * cy * -sz), static_cast<float> (sy * sz
                    + -sx * cy * cz), static_cast<float> (cx * cy), 0, 0, 0, 0, 1);

#else

    return matrix4_premultiply_by_matrix4(
            matrix4_premultiply_by_matrix4(
                    matrix4_rotation_for_y(euler[1]),
                    matrix4_rotation_for_x(euler[0])
            ),
            matrix4_rotation_for_z(euler[2])
    );

#endif
}

inline Matrix4 matrix4_rotation_for_euler_yxz_degrees (const Vector3& euler)
{
    return matrix4_rotation_for_euler_yxz(euler_degrees_to_radians(euler));
}

inline Matrix4 matrix4_rotated_by_euler_yxz_degrees (const Matrix4& self, const Vector3& euler)
{
    return matrix4_multiplied_by_matrix4(self, matrix4_rotation_for_euler_yxz_degrees(euler));
}

inline void matrix4_rotate_by_euler_yxz_degrees (Matrix4& self, const Vector3& euler)
{
    self = matrix4_rotated_by_euler_yxz_degrees(self, euler);
}

inline Matrix4 matrix4_rotation_for_euler_zxy (const Vector3& euler)
{
#if 1
    return matrix4_premultiplied_by_matrix4(matrix4_premultiplied_by_matrix4(matrix4_rotation_for_z(euler[2]),
            matrix4_rotation_for_x(euler[0])), matrix4_rotation_for_y(euler[1]));
#else
    double cx = cos(euler[0]);
    double sx = sin(euler[0]);
    double cy = cos(euler[1]);
    double sy = sin(euler[1]);
    double cz = cos(euler[2]);
    double sz = sin(euler[2]);

    return Matrix4(
            static_cast<float>(cz * cy + sz * sx * sy),
            static_cast<float>(sz * cx),
            static_cast<float>(cz * -sy + sz * sx * cy),
            0,
            static_cast<float>(-sz * cy + cz * sx * sy),
            static_cast<float>(cz * cx),
            static_cast<float>(-sz * -sy + cz * cx * cy),
            0,
            static_cast<float>(cx* sy),
            static_cast<float>(-sx),
            static_cast<float>(cx* cy),
            0,
            0,
            0,
            0,
            1
    );
#endif
}

inline Matrix4 matrix4_rotation_for_euler_zxy_degrees (const Vector3& euler)
{
    return matrix4_rotation_for_euler_zxy(euler_degrees_to_radians(euler));
}

inline Matrix4 matrix4_rotated_by_euler_zxy_degrees (const Matrix4& self, const Vector3& euler)
{
    return matrix4_multiplied_by_matrix4(self, matrix4_rotation_for_euler_zxy_degrees(euler));
}

inline void matrix4_rotate_by_euler_zxy_degrees (Matrix4& self, const Vector3& euler)
{
    self = matrix4_rotated_by_euler_zxy_degrees(self, euler);
}

inline Matrix4 matrix4_rotation_for_euler_zyx (const Vector3& euler)
{
#if 1

    double cx = cos(euler[0]);
    double sx = sin(euler[0]);
    double cy = cos(euler[1]);
    double sy = sin(euler[1]);
    double cz = cos(euler[2]);
    double sz = sin(euler[2]);

    return Matrix4(static_cast<float> (cy * cz), static_cast<float> (sx * sy * cz + cx * sz), static_cast<float> (cx
            * -sy * cz + sx * sz), 0, static_cast<float> (cy * -sz), static_cast<float> (sx * sy * -sz + cx * cz),
            static_cast<float> (cx * -sy * -sz + sx * cz), 0, static_cast<float> (sy), static_cast<float> (-sx * cy),
            static_cast<float> (cx * cy), 0, 0, 0, 0, 1);

#else

    return matrix4_premultiply_by_matrix4(
            matrix4_premultiply_by_matrix4(
                    matrix4_rotation_for_z(euler[2]),
                    matrix4_rotation_for_y(euler[1])
            ),
            matrix4_rotation_for_x(euler[0])
    );

#endif
}

inline Matrix4 matrix4_rotation_for_euler_zyx_degrees (const Vector3& euler)
{
    return matrix4_rotation_for_euler_zyx(euler_degrees_to_radians(euler));
}

inline Vector3 matrix4_get_rotation_euler_xyz (const Matrix4& self)
{
    double a = asin(-self[2]);
    double ca = cos(a);

    if (fabs(ca) > 0.005) // Gimbal lock?
    {
        return Vector3(static_cast<float> (atan2(self[6] / ca, self[10] / ca)), static_cast<float> (a),
                static_cast<float> (atan2(self[1] / ca, self[0] / ca)));
    } else // Gimbal lock has occurred
    {
        return Vector3(static_cast<float> (atan2(-self[9], self[5])), static_cast<float> (a), 0);
    }
}

inline Vector3 matrix4_get_rotation_euler_xyz_degrees (const Matrix4& self)
{
    return euler_radians_to_degrees(matrix4_get_rotation_euler_xyz(self));
}

inline Vector3 matrix4_get_rotation_euler_yxz (const Matrix4& self)
{
    double a = asin(self[6]);
    double ca = cos(a);

    if (fabs(ca) > 0.005) // Gimbal lock?
    {
        return Vector3(static_cast<float> (a), static_cast<float> (atan2(-self[2] / ca, self[10] / ca)),
                static_cast<float> (atan2(-self[4] / ca, self[5] / ca)));
    } else // Gimbal lock has occurred
    {
        return Vector3(static_cast<float> (a), static_cast<float> (atan2(self[8], self[0])), 0);
    }
}

inline Vector3 matrix4_get_rotation_euler_yxz_degrees (const Matrix4& self)
{
    return euler_radians_to_degrees(matrix4_get_rotation_euler_yxz(self));
}

inline Vector3 matrix4_get_rotation_euler_zxy (const Matrix4& self)
{
    double a = asin(-self[9]);
    double ca = cos(a);

    if (fabs(ca) > 0.005) // Gimbal lock?
    {
        return Vector3(static_cast<float> (a), static_cast<float> (atan2(self[8] / ca, self[10] / ca)),
                static_cast<float> (atan2(self[1] / ca, self[5] / ca)));
    } else // Gimbal lock has occurred
    {
        return Vector3(static_cast<float> (a), 0, static_cast<float> (atan2(-self[4], self[0])));
    }
}

inline Vector3 matrix4_get_rotation_euler_zxy_degrees (const Matrix4& self)
{
    return euler_radians_to_degrees(matrix4_get_rotation_euler_zxy(self));
}

inline Vector3 matrix4_get_rotation_euler_zyx (const Matrix4& self)
{
    double a = asin(self[8]);
    double ca = cos(a);

    if (fabs(ca) > 0.005) // Gimbal lock?
    {
        return Vector3(static_cast<float> (atan2(-self[9] / ca, self[10] / ca)), static_cast<float> (a),
                static_cast<float> (atan2(-self[4] / ca, self[0] / ca)));
    } else // Gimbal lock has occurred
    {
        return Vector3(0, static_cast<float> (a), static_cast<float> (atan2(self[1], self[5])));
    }
}

inline Vector3 matrix4_get_rotation_euler_zyx_degrees (const Matrix4& self)
{
    return euler_radians_to_degrees(matrix4_get_rotation_euler_zyx(self));
}

inline void matrix4_pivoted_rotate_by_euler_xyz_degrees (Matrix4& self, const Vector3& euler, const Vector3& pivotpoint)
{
    matrix4_translate_by_vec3(self, pivotpoint);
    matrix4_rotate_by_euler_xyz_degrees(self, euler);
    matrix4_translate_by_vec3(self, -pivotpoint);
}

inline Matrix4 matrix4_scale_for_vec3 (const Vector3& scale)
{
    return Matrix4(scale[0], 0, 0, 0, 0, scale[1], 0, 0, 0, 0, scale[2], 0, 0, 0, 0, 1);
}

inline Vector3 matrix4_get_scale_vec3 (const Matrix4& self)
{
    return Vector3(static_cast<float> (self.x().getVector3().getLength()),
            static_cast<float> (self.y().getVector3().getLength()),
            static_cast<float> (self.z().getVector3().getLength()));
}

inline void matrix4_scale_by_vec3 (Matrix4& self, const Vector3& scale)
{
    matrix4_multiply_by_matrix4(self, matrix4_scale_for_vec3(scale));
}

inline void matrix4_pivoted_scale_by_vec3 (Matrix4& self, const Vector3& scale, const Vector3& pivotpoint)
{
    matrix4_translate_by_vec3(self, pivotpoint);
    matrix4_scale_by_vec3(self, scale);
    matrix4_translate_by_vec3(self, -pivotpoint);
}

inline void matrix4_transform_by_euler_xyz_degrees (Matrix4& self, const Vector3& translation, const Vector3& euler,
        const Vector3& scale)
{
    matrix4_translate_by_vec3(self, translation);
    matrix4_rotate_by_euler_xyz_degrees(self, euler);
    matrix4_scale_by_vec3(self, scale);
}

inline void matrix4_pivoted_transform_by_euler_xyz_degrees (Matrix4& self, const Vector3& translation,
        const Vector3& euler, const Vector3& scale, const Vector3& pivotpoint)
{
    matrix4_translate_by_vec3(self, pivotpoint + translation);
    matrix4_rotate_by_euler_xyz_degrees(self, euler);
    matrix4_scale_by_vec3(self, scale);
    matrix4_translate_by_vec3(self, -pivotpoint);
}

#endif