watchrobot/libraries/AP_Math/matrix4.h

#pragma once
#include "vector4.h"


// 4x4 matrix with elements of type T
template <typename T>
class Matrix4 {
public:

    // Vectors comprising the rows of the matrix
    Vector4<T>        a, b, c,d;

    // trivial ctor
    // note that the Vector3 ctor will zero the vector elements
    constexpr Matrix4<T>() {}

    // setting ctor
    constexpr Matrix4<T>(const Vector4<T> &a0, const Vector4<T> &b0, const Vector4<T> &c0, const Vector4<T> &d0)
        : a(a0)
        , b(b0)
        , c(c0)
		, d(d0){}

    // setting ctor
    // M00, M01, M02, M03
    // M10, M11, M12, M13
    // M20, M21, M22, M23
    // M30, M31, M32, M33
    constexpr Matrix4<T>(const T M00, const T M01, const T M02,const T M03,
                         const T M10, const T M11, const T M12,const T M13,
                         const T M20, const T M21, const T M22,const T M23,
                         const T M30, const T M31, const T M32,const T M33)
        : a(M00, M01, M02, M03)
        , b(M10, M11, M12, M13)
        , c(M20, M21, M22, M23) 
        , d(M30, M31, M32, M33) 
			{}

    // function call operator
    void operator        () (const Vector4<T> &a0, const Vector4<T> &b0, const Vector4<T> &c0, const Vector4<T> &d0)
    {
        a = a0; b = b0; c = c0; d = d0;
    }

    // test for equality
    bool operator        == (const Matrix4<T> &m)
    {
        return (a==m.a && b==m.b && c==m.c && d==m.d);
    }

    // test for inequality
    bool operator        != (const Matrix4<T> &m)
    {
        return (a!=m.a || b!=m.b || c!=m.c|| d!=m.d);
    }

    // negation
    Matrix4<T> operator        - (void) const
    {
        return Matrix4<T>(-a,-b,-c,-d);
    }

    // addition
    Matrix4<T> operator        + (const Matrix4<T> &m) const
    {
        return Matrix4<T>(a+m.a, b+m.b, c+m.c, d+m.d);
    }
    Matrix4<T> &operator        += (const Matrix4<T> &m)
    {
        return *this = *this + m;
    }

    // subtraction
    Matrix4<T> operator        - (const Matrix4<T> &m) const
    {
        return Matrix3<T>(a-m.a, b-m.b, c-m.c,d-m.d);
    }
    Matrix4<T> &operator        -= (const Matrix4<T> &m)
    {
        return *this = *this - m;
    }

    // uniform scaling
    Matrix4<T> operator        * (const T num) const
    {
        return Matrix4<T>(a*num, b*num, c*num, d*num);
    }
    Matrix4<T> &operator        *= (const T num)
    {
        return *this = *this * num;
    }
    Matrix4<T> operator        / (const T num) const
    {
        return Matrix4<T>(a/num, b/num, c/num, d/num);
    }
    Matrix4<T> &operator        /= (const T num)
    {
        return *this = *this / num;
    }

    Vector4<T> operator *(const Vector4<T> &v) const;

  

    /**
     * Calculate the determinant of this matrix.
     *
     * @return The value of the determinant.
     */
    T det() const;

    /**
     * Calculate the inverse of this matrix.
     *
     * @param inv[in] Where to store the result.
     *
     * @return If this matrix is invertible, then true is returned. Otherwise,
     * \p inv is unmodified and false is returned.
     */
    bool inverse(Matrix4<T>& inv) const;

  

    // check if any elements are NAN
    bool        is_nan(void) WARN_IF_UNUSED
    {
        return a.is_nan() || b.is_nan() || c.is_nan();
    }

    void zero(void);
    void        normalize(void);
};

typedef Matrix4<int16_t>                Matrix4i;
typedef Matrix4<uint16_t>               Matrix4ui;
typedef Matrix4<int32_t>                Matrix4l;
typedef Matrix4<uint32_t>               Matrix4ul;
typedef Matrix4<float>                  Matrix4f;