vmath.h

Go to the documentation of this file.

/* -*- C++ -*- */
// Modified 2011-06-12, Davide Bacchet (davide.bacchet at gmail dot com)
//                      added () operators with standard matrix notation (opposite wrt at() ).
#ifndef __vmath_Header_File__
#define __vmath_Header_File__

#include <cmath>
#include <cstring>
#include <iostream>
#include <sstream>
#include <string>
#include <cassert>

#ifdef VMATH_NAMESPACE
namespace VMATH_NAMESPACE
{
#endif

#ifndef M_PI
#define M_PI           3.14159265358979323846  /* pi */
#endif

#define DEG2RAD(x) ((x * M_PI) / 180.0)
//#define EPSILON (4.37114e-07)

const double epsilon = 4.37114e-05;
#define EPSILON epsilon

template<class T>
class Vector2
{
public:
    union
    {
        T x;

        T s;
    };

    union
    {
        T y;

        T t;
    };

    //----------------[ constructors ]--------------------------
    Vector2()
            : x(0), y(0)
    {
    }

    Vector2(T nx, T ny)
            : x(nx), y(ny)
    {
    }

    Vector2(const Vector2<T>& src)
            : x(src.x), y(src.y)
    {
    }

    template<class FromT>
    Vector2(const Vector2<FromT>& src)
            : x(static_cast<T>(src.x)), y(static_cast<T>(src.y))
    {
    }

    //----------------[ access operators ]-------------------
    template<class FromT>
    Vector2<T>& operator=(const Vector2<FromT>& rhs)
    {
        x = static_cast<T>(rhs.x);
        y = static_cast<T>(rhs.y);
        return *this;
    }

    Vector2<T>& operator=(const Vector2<T>& rhs)
    {
        x = rhs.x;
        y = rhs.y;
        return *this;
    }

    T& operator[](int n)
    {
        assert(n >= 0 && n <= 1);
        if (0 == n)
            return x;
        else
            return y;
    }

    const T& operator[](int n) const
    {
        assert(n >= 0 && n <= 1);
        if (0 == n)
            return x;
        else
            return y;
    }

    //---------------[ vector aritmetic operator ]--------------
    Vector2<T> operator+(const Vector2<T>& rhs) const
    {
        return Vector2<T>(x + rhs.x, y + rhs.y);
    }

    Vector2<T> operator-(const Vector2<T>& rhs) const
    {
        return Vector2<T>(x - rhs.x, y - rhs.y);
    }

    Vector2<T> operator*(const Vector2<T>& rhs) const
    {
        return Vector2<T>(x * rhs.x, y * rhs.y);
    }

    Vector2<T> operator/(const Vector2<T>& rhs) const
    {
        return Vector2<T>(x / rhs.x, y / rhs.y);
    }

    Vector2<T>& operator+=(const Vector2<T>& rhs)
    {
        x += rhs.x;
        y += rhs.y;
        return *this;
    }

    Vector2<T>& operator-=(const Vector2<T>& rhs)
    {
        x -= rhs.x;
        y -= rhs.y;
        return *this;
    }

    Vector2<T>& operator*=(const Vector2<T>& rhs)
    {
        x *= rhs.x;
        y *= rhs.y;
        return *this;
    }

    Vector2<T>& operator/=(const Vector2<T>& rhs)
    {
        x /= rhs.x;
        y /= rhs.y;
        return *this;
    }

    //--------------[ scalar vector operator ]--------------------
    Vector2<T> operator+(T rhs) const
    {
        return Vector2<T>(x + rhs, y + rhs);
    }

    Vector2<T> operator-(T rhs) const
    {
        return Vector2<T>(x - rhs, y - rhs);
    }

    Vector2<T> operator*(T rhs) const
    {
        return Vector2<T>(x * rhs, y * rhs);
    }

    Vector2<T> operator/(T rhs) const
    {
        return Vector2<T>(x / rhs, y / rhs);
    }

    Vector2<T>& operator+=(T rhs)
    {
        x += rhs;
        y += rhs;
        return *this;
    }

    Vector2<T>& operator-=(T rhs)
    {
        x -= rhs;
        y -= rhs;
        return *this;
    }

    Vector2<T>& operator*=(T rhs)
    {
        x *= rhs;
        y *= rhs;
        return *this;
    }

    Vector2<T>& operator/=(T rhs)
    {
        x /= rhs;
        y /= rhs;
        return *this;
    }

    //--------------[ equality operator ]------------------------
    bool operator==(const Vector2<T>& rhs) const
    {
        return (std::abs(x - rhs.x) < EPSILON) && (std::abs(y - rhs.y) < EPSILON);
    }

    bool operator!=(const Vector2<T>& rhs) const
    {
        return !(*this == rhs);
    }

    //-------------[ unary operations ]--------------------------
    Vector2<T> operator-() const
    {
        return Vector2<T>(-x, -y);
    }

    //-------------[ size operations ]---------------------------
    T length() const
    {
        return (T) std::sqrt(x * x + y * y);
    }

    void normalize()
    {
        T s = length();
        x /= s;
        y /= s;
    }

    T lengthSq() const
    {
        return x * x + y * y;
    }

    //--------------[ misc. operations ]-----------------------
    Vector2<T> lerp(T fact, const Vector2<T>& r) const
    {
        return (*this) + (r - (*this)) * fact;
    }

    //-------------[ conversion ]-----------------------------
    operator T*()
    {
        return (T*) this;
    }
    operator const T*() const
    {
        return (const T*) this;
    }

    //-------------[ output operator ]------------------------
    friend std::ostream& operator<<(std::ostream& lhs, const Vector2<T>& rhs)
    {
        lhs << "[" << rhs.x << "," << rhs.y << "]";
        return lhs;
    }

    std::string toString() const
    {
        std::ostringstream oss;
        oss << *this;
        return oss.str();
    }
};

//--------------------------------------
// Typedef shortcuts for 2D vector
//-------------------------------------
typedef class Vector2<float> Vector2f;
typedef class Vector2<double> Vector2d;
typedef class Vector2<int> Vector2i;

template<class T>
class Vector3
{
public:
    //T x, y, z;
    union
    {
        T x;

        T s;

        T r;
    };

    union
    {
        T y;
        T t;
        T g;
    };

    union
    {
        T z;

        T u;
        T b;
    };

    //----------------[ constructors ]--------------------------
    Vector3()
            : x(0), y(0), z(0)
    {
    }

    Vector3(T nx, T ny, T nz)
            : x(nx), y(ny), z(nz)
    {
    }

    Vector3(const Vector3<T>& src)
            : x(src.x), y(src.y), z(src.z)
    {
    }

    template<class FromT>
    Vector3(const Vector3<FromT>& src)
            : x(static_cast<T>(src.x)), y(static_cast<T>(src.y)), z(static_cast<T>(src.z))
    {
    }

    //----------------[ access operators ]-------------------
    Vector3<T> operator=(const Vector3<T>& rhs)
    {
        x = rhs.x;
        y = rhs.y;
        z = rhs.z;
        return *this;
    }

    template<class FromT>
    Vector3<T> operator=(const Vector3<FromT>& rhs)
    {
        x = static_cast<T>(rhs.x);
        y = static_cast<T>(rhs.y);
        z = static_cast<T>(rhs.z);
        return *this;
    }

    T & operator[](int n)
    {
        assert(n >= 0 && n <= 2);
        if (0 == n)
            return x;
        else if (1 == n)
            return y;
        else
            return z;
    }

    const T & operator[](int n) const
    {
        assert(n >= 0 && n <= 2);
        if (0 == n)
            return x;
        else if (1 == n)
            return y;
        else
            return z;
    }

    //---------------[ vector arithmetic operator ]--------------
    Vector3<T> operator+(const Vector3<T>& rhs) const
    {
        return Vector3<T>(x + rhs.x, y + rhs.y, z + rhs.z);
    }

    Vector3<T> operator-(const Vector3<T>& rhs) const
    {
        return Vector3<T>(x - rhs.x, y - rhs.y, z - rhs.z);
    }

    Vector3<T> operator*(const Vector3<T>& rhs) const
    {
        return Vector3<T>(x * rhs.x, y * rhs.y, z * rhs.z);
    }

    Vector3<T> operator/(const Vector3<T>& rhs) const
    {
        return Vector3<T>(x / rhs.x, y / rhs.y, z / rhs.z);
    }

    Vector3<T>& operator+=(const Vector3<T>& rhs)
    {
        x += rhs.x;
        y += rhs.y;
        z += rhs.z;
        return *this;
    }

    Vector3<T>& operator-=(const Vector3<T>& rhs)
    {
        x -= rhs.x;
        y -= rhs.y;
        z -= rhs.z;
        return *this;
    }

    Vector3<T>& operator*=(const Vector3<T>& rhs)
    {
        x *= rhs.x;
        y *= rhs.y;
        z *= rhs.z;
        return *this;
    }

    Vector3<T>& operator/=(const Vector3<T>& rhs)
    {
        x /= rhs.x;
        y /= rhs.y;
        z /= rhs.z;
        return *this;
    }

    T dotProduct(const Vector3<T>& rhs) const
    {
        return x * rhs.x + y * rhs.y + z * rhs.z;
    }

    Vector3<T> crossProduct(const Vector3<T>& rhs) const
    {
        return Vector3<T>(y * rhs.z - rhs.y * z, z * rhs.x - rhs.z * x, x * rhs.y - rhs.x * y);
    }

    //--------------[ scalar vector operator ]--------------------
    Vector3<T> operator+(T rhs) const
    {
        return Vector3<T>(x + rhs, y + rhs, z + rhs);
    }

    Vector3<T> operator-(T rhs) const
    {
        return Vector3<T>(x - rhs, y - rhs, z - rhs);
    }

    Vector3<T> operator*(T rhs) const
    {
        return Vector3<T>(x * rhs, y * rhs, z * rhs);
    }

    Vector3<T> operator/(T rhs) const
    {
        return Vector3<T>(x / rhs, y / rhs, z / rhs);
    }

    Vector3<T>& operator+=(T rhs)
    {
        x += rhs;
        y += rhs;
        z += rhs;
        return *this;
    }

    Vector3<T>& operator-=(T rhs)
    {
        x -= rhs;
        y -= rhs;
        z -= rhs;
        return *this;
    }

    Vector3<T>& operator*=(T rhs)
    {
        x *= rhs;
        y *= rhs;
        z *= rhs;
        return *this;
    }

    Vector3<T>& operator/=(T rhs)
    {
        x /= rhs;
        y /= rhs;
        z /= rhs;
        return *this;
    }

    //--------------[ Equality operator ]------------------------
    bool operator==(const Vector3<T>& rhs) const
    {
        return std::fabs(x - rhs.x) < EPSILON && std::fabs(y - rhs.y) < EPSILON && std::fabs(z - rhs.z) < EPSILON;
    }

    bool operator!=(const Vector3<T>& rhs) const
    {
        return !(*this == rhs);
    }

    //-------------[ unary operations ]--------------------------
    Vector3<T> operator-() const
    {
        return Vector3<T>(-x, -y, -z);
    }

    //-------------[ size operations ]---------------------------
    T length() const
    {
        return (T) std::sqrt(x * x + y * y + z * z);
    }

    T lengthSq() const
    {
        return x * x + y * y + z * z;
    }

    void normalize()
    {
        T s = length();
        x /= s;
        y /= s;
        z /= s;
    }

    //------------[ other operations ]---------------------------
    void rotate(T ax, T ay, T az)
    {
        T a = cos(DEG2RAD(ax));
        T b = sin(DEG2RAD(ax));
        T c = cos(DEG2RAD(ay));
        T d = sin(DEG2RAD(ay));
        T e = cos(DEG2RAD(az));
        T f = sin(DEG2RAD(az));
        T nx = c * e * x - c * f * y + d * z;
        T ny = (a * f + b * d * e) * x + (a * e - b * d * f) * y - b * c * z;
        T nz = (b * f - a * d * e) * x + (a * d * f + b * e) * y + a * c * z;
        x = nx;
        y = ny;
        z = nz;

    }

    Vector3<T> lerp(T fact, const Vector3<T>& r) const
    {
        return (*this) + (r - (*this)) * fact;
    }

    //-------------[ conversion ]-----------------------------

    operator T*()
    {
        return (T*) this;
    }

    operator const T*() const
    {
        return (const T*) this;
    }

    //-------------[ output operator ]------------------------
    friend std::ostream& operator<<(std::ostream& lhs, const Vector3<T> rhs)
    {
        lhs << "[" << rhs.x << "," << rhs.y << "," << rhs.z << "]";
        return lhs;
    }

    std::string toString() const
    {
        std::ostringstream oss;
        oss << *this;
        return oss.str();
    }
};

typedef Vector3<float> Vector3f;
typedef Vector3<double> Vector3d;
typedef Vector3<int> Vector3i;

template<class T>
class Vector4
{
public:

    union
    {
        T r;
        T x;
    };

    union
    {
        T g;
        T y;
    };

    union
    {
        T b;
        T z;
    };

    union
    {
        T a;
        T w;
    };

    //----------------[ constructors ]--------------------------
    Vector4()
            : x(0), y(0), z(0), w(0)
    {
    }

    Vector4(T nx, T ny, T nz, T nw)
            : x(nx), y(ny), z(nz), w(nw)
    {
    }

    Vector4(const Vector4<T>& src)
            : x(src.x), y(src.y), z(src.z), w(src.w)
    {
    }

    template<class FromT>
    Vector4(const Vector4<FromT>& src)
            : x(static_cast<T>(src.x)), y(static_cast<T>(src.y)), z(static_cast<T>(src.z)), w(static_cast<T>(src.w))
    {
    }

    Vector4(const Vector3<T>& src, T w)
        : x(src.x), y(src.y), z(src.z), w(w)
    {}

    template <typename FromT>
    Vector4(const Vector3<FromT>& src, FromT w)
        : x(static_cast<T>(src.x)), y(static_cast<T>(src.y)), z(static_cast<T>(src.z)), w(static_cast<T>(w))
    {}

    //----------------[ access operators ]-------------------
    Vector4<T> operator=(const Vector4<T>& rhs)
    {
        x = rhs.x;
        y = rhs.y;
        z = rhs.z;
        w = rhs.w;
        return *this;
    }

    template<class FromT>
    Vector4<T> operator=(const Vector4<FromT>& rhs)
    {
        x = static_cast<T>(rhs.x);
        y = static_cast<T>(rhs.y);
        z = static_cast<T>(rhs.z);
        w = static_cast<T>(rhs.w);
        return *this;
    }

    T & operator[](int n)
    {
        assert(n >= 0 && n <= 3);
        if (0 == n)
            return x;
        else if (1 == n)
            return y;
        else if (2 == n)
            return z;
        else
            return w;
    }

    const T & operator[](int n) const
    {
        assert(n >= 0 && n <= 3);
        if (0 == n)
            return x;
        else if (1 == n)
            return y;
        else if (2 == n)
            return z;
        else
            return w;
    }

    //---------------[ vector aritmetic operator ]--------------
    Vector4<T> operator+(const Vector4<T>& rhs) const
    {
        return Vector4<T>(x + rhs.x, y + rhs.y, z + rhs.z, w + rhs.w);
    }

    Vector4<T> operator-(const Vector4<T>& rhs) const
    {
        return Vector4<T>(x - rhs.x, y - rhs.y, z - rhs.z, w - rhs.w);
    }

    Vector4<T> operator*(const Vector4<T> rhs) const
    {
        return Vector4<T>(x * rhs.x, y * rhs.y, z * rhs.z, w * rhs.w);
    }

    Vector4<T> operator/(const Vector4<T>& rhs) const
    {
        return Vector4<T>(x / rhs.x, y / rhs.y, z / rhs.z, w / rhs.w);
    }

    Vector4<T>& operator+=(const Vector4<T>& rhs)
    {
        x += rhs.x;
        y += rhs.y;
        z += rhs.z;
        w += rhs.w;
        return *this;
    }

    Vector4<T>& operator-=(const Vector4<T>& rhs)
    {
        x -= rhs.x;
        y -= rhs.y;
        z -= rhs.z;
        w -= rhs.w;
        return *this;
    }

    Vector4<T>& operator*=(const Vector4<T>& rhs)
    {
        x *= rhs.x;
        y *= rhs.y;
        z *= rhs.z;
        w *= rhs.w;
        return *this;
    }

    Vector4<T>& operator/=(const Vector4<T>& rhs)
    {
        x /= rhs.x;
        y /= rhs.y;
        z /= rhs.z;
        w /= rhs.w;
        return *this;
    }

    //--------------[ equiality operator ]------------------------
    bool operator==(const Vector4<T>& rhs) const
    {
        return std::fabs(x - rhs.x) < EPSILON && std::fabs(y - rhs.y) < EPSILON && std::fabs(z - rhs.z) < EPSILON
                && std::fabs(w - rhs.w) < EPSILON;
    }

    bool operator!=(const Vector4<T>& rhs) const
    {
        return !(*this == rhs);
    }

    //-------------[ unary operations ]--------------------------
    Vector4<T> operator-() const
    {
        return Vector4<T>(-x, -y, -z, -w);
    }

    //--------------[ scalar vector operator ]--------------------

    Vector4<T> operator+(T rhs) const
    {
        return Vector4<T>(x + rhs, y + rhs, z + rhs, w + rhs);
    }

    Vector4<T> operator-(T rhs) const
    {
        return Vector4<T>(x - rhs, y - rhs, z - rhs, w - rhs);
    }

    Vector4<T> operator*(T rhs) const
    {
        return Vector4<T>(x * rhs, y * rhs, z * rhs, w * rhs);
    }

    Vector4<T> operator/(T rhs) const
    {
        return Vector4<T>(x / rhs, y / rhs, z / rhs, w / rhs);
    }

    Vector4<T>& operator+=(T rhs)
    {
        x += rhs;
        y += rhs;
        z += rhs;
        w += rhs;
        return *this;
    }

    Vector4<T>& operator-=(T rhs)
    {
        x -= rhs;
        y -= rhs;
        z -= rhs;
        w -= rhs;
        return *this;
    }

    Vector4<T>& operator*=(T rhs)
    {
        x *= rhs;
        y *= rhs;
        z *= rhs;
        w *= rhs;
        return *this;
    }

    Vector4<T>& operator/=(T rhs)
    {
        x /= rhs;
        y /= rhs;
        z /= rhs;
        w /= rhs;
        return *this;
    }

    //-------------[ size operations ]---------------------------
    T length() const
    {
        return (T) std::sqrt(x * x + y * y + z * z + w * w);
    }

    void normalize()
    {
        T s = length();
        x /= s;
        y /= s;
        z /= s;
        w /= s;
    }

    T lengthSq() const
    {
        return x * x + y * y + z * z + w * w;
    }

    //--------------[ misc. operations ]-----------------------
    Vector4<T> lerp(T fact, const Vector4<T>& r) const
    {
        return (*this) + (r - (*this)) * fact;
    }

    //-------------[ conversion ]-----------------------------

    operator T*()
    {
        return (T*) this;
    }

    operator const T*() const
    {
        return (const T*) this;
    }

    Vector3<T> xyz() const
    {
        if (w == 0 || w == 1)
            return Vector3<T>(x,y,z);

        const T invW = 1.0 / w;
        return Vector3<T>(x * invW, y * invW, z * invW);
    }

    //-------------[ output operator ]------------------------
    friend std::ostream& operator<<(std::ostream& lhs, const Vector4<T>& rhs)
    {
        lhs << "[" << rhs.x << "," << rhs.y << "," << rhs.z << "," << rhs.w << "]";
        return lhs;
    }

    std::string toString() const
    {
        std::ostringstream oss;
        oss << *this;
        return oss.str();
    }

};

typedef Vector4<float> Vector4f;
typedef Vector4<double> Vector4d;
typedef Vector4<int> Vector4i;

template<class T>
class Matrix3
{
public:
    T data[9];

    //--------------------------[ constructors ]-------------------------------
    Matrix3()
    {
        for (int i = 0; i < 9; i++)
            data[i] = (i % 4) ? 0 : 1;
    }

    Matrix3(const T * dt)
    {
        std::memcpy(data, dt, sizeof(T) * 9);
    }

    Matrix3(const Matrix3<T>& src)
    {
        std::memcpy(data, src.data, sizeof(T) * 9);
    }

    template<class FromT>
    Matrix3(const Matrix3<FromT>& src)
    {
        for (int i = 0; i < 9; i++)
        {
            data[i] = static_cast<T>(src.data[i]);
        }
    }

    void identity()
    {
        for (int i = 0; i < 9; i++)
            data[i] = (i % 4) ? 0 : 1;
    }

    static Matrix3<T> createRotationAroundAxis(T xDeg, T yDeg, T zDeg)
    {
        T xRads(DEG2RAD(xDeg));
        T yRads(DEG2RAD(yDeg));
        T zRads(DEG2RAD(zDeg));

        Matrix3<T> ma, mb, mc;
        float ac = cos(xRads);
        float as = sin(xRads);
        float bc = cos(yRads);
        float bs = sin(yRads);
        float cc = cos(zRads);
        float cs = sin(zRads);

        ma.at(1, 1) = ac;
        ma.at(2, 1) = as;
        ma.at(1, 2) = -as;
        ma.at(2, 2) = ac;

        mb.at(0, 0) = bc;
        mb.at(2, 0) = -bs;
        mb.at(0, 2) = bs;
        mb.at(2, 2) = bc;

        mc.at(0, 0) = cc;
        mc.at(1, 0) = cs;
        mc.at(0, 1) = -cs;
        mc.at(1, 1) = cc;

        Matrix3<T> ret = ma * mb * mc;
        return ret;
    }

    template<class It>
    static Matrix3<T> fromOde(const It* mat)
    {
        Matrix3<T> ret;
        for (int i = 0; i < 3; i++)
        {
            for (int j = 0; j < 3; j++)
            {
                ret.at(i, j) = static_cast<T>(mat[j * 4 + i]);
            }
        }
        return ret;
    }

    template<class FromT>
    static Matrix3<T> fromRowMajorArray(const FromT* arr)
    {
        const T retData[] =
        { static_cast<T>(arr[0]), static_cast<T>(arr[3]), static_cast<T>(arr[6]), static_cast<T>(arr[1]),
                static_cast<T>(arr[4]), static_cast<T>(arr[7]), static_cast<T>(arr[2]), static_cast<T>(arr[5]),
                static_cast<T>(arr[8]) };

        return retData;
    }

    template<class FromT>
    static Matrix3<T> fromColumnMajorArray(const FromT* arr)
    {
        const T retData[] =
        { static_cast<T>(arr[0]), static_cast<T>(arr[1]), static_cast<T>(arr[2]), static_cast<T>(arr[3]),
                static_cast<T>(arr[4]), static_cast<T>(arr[5]), static_cast<T>(arr[6]), static_cast<T>(arr[7]),
                static_cast<T>(arr[8]) };

        return retData;
    }

    //---------------------[ equiality operators ]------------------------------
    bool operator==(const Matrix3<T>& rhs) const
    {
        for (int i = 0; i < 9; i++)
        {
            if (std::fabs(data[i] - rhs.data[i]) >= EPSILON)
                return false;
        }
        return true;
    }

    bool operator!=(const Matrix3<T>& rhs) const
    {
        return !(*this == rhs);
    }

    //---------------------[ access operators ]---------------------------------
    T& at(int x, int y)
    {
        assert(x >= 0 && x < 3);
        assert(y >= 0 && y < 3);
        return data[x * 3 + y];
    }

    const T& at(int x, int y) const
    {
        assert(x >= 0 && x < 3);
        assert(y >= 0 && y < 3);
        return data[x * 3 + y];
    }

    T& operator()(int i, int j)
    {
        assert(i >= 1 && i <= 3);
        assert(j >= 1 && j <= 3);
        return data[(j - 1) * 3 + i - 1];
    }

    const T& operator()(int i, int j) const
    {
        assert(i >= 1 && i <= 3);
        assert(j >= 1 && j <= 3);
        return data[(j - 1) * 3 + i - 1];
    }

    Matrix3<T>& operator=(const Matrix3<T>& rhs)
    {
        std::memcpy(data, rhs.data, sizeof(T) * 9);
        return *this;
    }

    template<class FromT>
    Matrix3<T>& operator=(const Matrix3<FromT>& rhs)
    {
        for (int i = 0; i < 9; i++)
        {
            data[i] = static_cast<T>(rhs.data[i]);
        }
        return *this;
    }

    Matrix3<T>& operator=(const T* rhs)
    {
        std::memcpy(data, rhs, sizeof(T) * 9);
        return *this;
    }

    /*Matrix3<T> & operator=(const double* m)
     {
     for (int i = 0; i < 9; i++) data[i] = (T)m[i];
     return * this;
     }*/

    //--------------------[ matrix with matrix operations ]---------------------
    Matrix3<T> operator+(const Matrix3<T>& rhs) const
    {
        Matrix3<T> ret;
        for (int i = 0; i < 9; i++)
            ret.data[i] = data[i] + rhs.data[i];
        return ret;
    }

    Matrix3<T> operator-(const Matrix3<T>& rhs) const
    {
        Matrix3<T> ret;
        for (int i = 0; i < 9; i++)
            ret.data[i] = data[i] - rhs.data[i];
        return ret;
    }

    //--------------------[ matrix with scalar operations ]---------------------
    Matrix3<T> operator+(T rhs) const
    {
        Matrix3<T> ret;
        for (int i = 0; i < 9; i++)
            ret.data[i] = data[i] + rhs;
        return ret;
    }

    Matrix3<T> operator-(T rhs) const
    {
        Matrix3<T> ret;
        for (int i = 0; i < 9; i++)
            ret.data[i] = data[i] - rhs;
        return ret;
    }

    Matrix3<T> operator*(T rhs) const
    {
        Matrix3<T> ret;
        for (int i = 0; i < 9; i++)
            ret.data[i] = data[i] * rhs;
        return ret;
    }

    Matrix3<T> operator/(T rhs) const
    {
        Matrix3<T> ret;
        for (int i = 0; i < 9; i++)
            ret.data[i] = data[i] / rhs;
        return ret;
    }

    //--------------------[ multiply operators ]--------------------------------
    Vector3<T> operator*(const Vector3<T>& rhs) const
    {
        return Vector3<T>(data[0] * rhs.x + data[3] * rhs.y + data[6] * rhs.z,
                data[1] * rhs.x + data[4] * rhs.y + data[7] * rhs.z,
                data[2] * rhs.x + data[5] * rhs.y + data[8] * rhs.z);
    }

    Matrix3<T> operator*(Matrix3<T> rhs) const
    {
        Matrix3<T> w;
        for (int i = 0; i < 3; i++)
        {
            for (int j = 0; j < 3; j++)
            {
                T n = 0;
                for (int k = 0; k < 3; k++)
                    n += rhs.at(i, k) * at(k, j);
                w.at(i, j) = n;
            }
        }
        return w;

    }

    //---------------------------[ misc operations ]----------------------------
    Matrix3<T> transpose()
    {
        Matrix3<T> ret;
        for (int i = 0; i < 3; i++)
        {
            for (int j = 0; j < 3; j++)
            {
                ret.at(i, j) = at(j, i);
            }
        }
        return ret;
    }

    Matrix3<T> lerp(T fact, const Matrix3<T>& rhs) const
    {
        Matrix3<T> ret = (*this) + (rhs - (*this)) * fact;
        return ret;
    }

    T det()
    {
        return +at(0, 0) * at(1, 1) * at(2, 2) + at(0, 1) * at(1, 2) * at(2, 0) + at(0, 2) * at(1, 0) * at(2, 1)
                - at(0, 0) * at(1, 2) * at(2, 1) - at(0, 1) * at(1, 0) * at(2, 2) - at(0, 2) * at(1, 1) * at(2, 0);
    }

    Matrix3<T> inverse()
    {
        Matrix3<T> ret;
        ret.at(0, 0) = at(1, 1) * at(2, 2) - at(2, 1) * at(1, 2);
        ret.at(0, 1) = at(2, 1) * at(0, 2) - at(0, 1) * at(2, 2);
        ret.at(0, 2) = at(0, 1) * at(1, 2) - at(1, 1) * at(0, 2);
        ret.at(1, 0) = at(2, 0) * at(1, 2) - at(1, 0) * at(2, 2);
        ret.at(1, 1) = at(0, 0) * at(2, 2) - at(2, 0) * at(0, 2);
        ret.at(1, 2) = at(1, 0) * at(0, 2) - at(0, 0) * at(1, 2);
        ret.at(2, 0) = at(1, 0) * at(2, 1) - at(2, 0) * at(1, 1);
        ret.at(2, 1) = at(2, 0) * at(0, 1) - at(0, 0) * at(2, 1);
        ret.at(2, 2) = at(0, 0) * at(1, 1) - at(1, 0) * at(0, 1);
        return ret * (1.0f / det());
    }

    //-------------[ conversion ]-----------------------------

    operator T*()
    {
        return (T*) data;
    }

    operator const T*() const
    {
        return (const T*) data;
    }

    //----------[ output operator ]----------------------------
    friend std::ostream& operator <<(std::ostream& lhs, const Matrix3<T>& rhs)
    {
        for (int i = 0; i < 3; i++)
        {
            lhs << "|\t";
            for (int j = 0; j < 3; j++)
            {
                lhs << rhs.at(j, i) << "\t";
            }
            lhs << "|" << std::endl;
        }
        return lhs;
    }

    std::string toString() const
    {
        std::ostringstream oss;
        oss << *this;
        return oss.str();
    }
};

typedef Matrix3<float> Matrix3f;
typedef Matrix3<double> Matrix3d;
typedef Matrix3<int> Matrix3i;

template<class T>
class Matrix4
{
public:
    T data[16];

    //--------------------------[ constructors ]-------------------------------
    Matrix4()
    {
        for (int i = 0; i < 16; i++)
            data[i] = (i % 5) ? 0 : 1;
    }

    Matrix4(const T * dt)
    {
        std::memcpy(data, dt, sizeof(T) * 16);
    }

    Matrix4(const Matrix4<T>& src)
    {
        std::memcpy(data, src.data, sizeof(T) * 16);
    }

    template<class FromT>
    Matrix4(const Matrix4<FromT>& src)
    {
        for (int i = 0; i < 16; i++)
        {
            data[i] = static_cast<T>(src.data[i]);
        }
    }

    void identity()
    {
        for (int i = 0; i < 16; i++)
            data[i] = (i % 5) ? 0 : 1;
    }

    static Matrix4<T> createRotationAroundAxis(T xDeg, T yDeg, T zDeg)
    {
        T xRads(DEG2RAD(xDeg));
        T yRads(DEG2RAD(yDeg));
        T zRads(DEG2RAD(zDeg));

        Matrix4<T> ma, mb, mc;
        float ac = cos(xRads);
        float as = sin(xRads);
        float bc = cos(yRads);
        float bs = sin(yRads);
        float cc = cos(zRads);
        float cs = sin(zRads);

        ma.at(1, 1) = ac;
        ma.at(2, 1) = as;
        ma.at(1, 2) = -as;
        ma.at(2, 2) = ac;

        mb.at(0, 0) = bc;
        mb.at(2, 0) = -bs;
        mb.at(0, 2) = bs;
        mb.at(2, 2) = bc;

        mc.at(0, 0) = cc;
        mc.at(1, 0) = cs;
        mc.at(0, 1) = -cs;
        mc.at(1, 1) = cc;

        /*std::cout << "RotVec = " << a << "," << b << "," << c << std::endl;
         std::cout << "Rx = " << std::endl << ma;
         std::cout << "Ry = " << std::endl << mb;
         std::cout << "Rz = " << std::endl << mc;*/

        Matrix4<T> ret = ma * mb * mc;
        //std::cout << "Result = " << std::endl << ma * (mb * mc);

        return ret;
    }


    static Matrix4<T> createTranslation(T x, T y, T z, T w = 1)
    {
        Matrix4 ret;
        ret.at(3, 0) = x;
        ret.at(3, 1) = y;
        ret.at(3, 2) = z;
        ret.at(3, 3) = w;

        return ret;
    }
    
    static Matrix4<T> createScale(T sx, T sy, T sz)
    {
        Matrix4<T> ret;
        ret.at(0,0) = sx;
        ret.at(1,1) = sy;
        ret.at(2,2) = sz;

        return ret;
    }

    static Matrix4<T> createLookAt(const Vector3<T>& eyePos, const Vector3<T>& centerPos, const Vector3<T>& upDir)
    {
        Vector3<T> forward, side, up;
        Matrix4<T> m;

        forward = centerPos - eyePos;
        up = upDir;

        forward.normalize();

        // Side = forward x up
        side = forward.crossProduct(up);
        side.normalize();

        // Recompute up as: up = side x forward
        up = side.crossProduct(forward);

        m.at(0, 0) = side.x;
        m.at(1, 0) = side.y;
        m.at(2, 0) = side.z;

        m.at(0, 1) = up.x;
        m.at(1, 1) = up.y;
        m.at(2, 1) = up.z;

        m.at(0, 2) = -forward.x;
        m.at(1, 2) = -forward.y;
        m.at(2, 2) = -forward.z;

        m = m * Matrix4<T>::createTranslation(-eyePos.x, -eyePos.y, -eyePos.z);
        return m;
    }


    static Matrix4<T> createFrustum(T left, T right, T bottom, T top, T zNear, T zFar)
    {
        /*
         *
          2 zNear
        ------------       0              A              0
        right - left

                        2 zNear
            0         ------------        B              0
                      top - bottom

            0              0              C              D

            0              0              -1             0

                                                     A = (right + left) / (right - left)

                                                     B = (top + bottom) / (top - bottom)

                                                    C = - (zFar + zNear) / (zFar - zNear)

                                                    D = - (2 zFar zNear) / (zFar - zNear)
         *
         */
        Matrix4<T> ret;

        const T invWidth = 1.0 / (right - left);
        const T invHeight = 1.0 / (top - bottom);
        const T invDepth = 1.0 / (zFar - zNear);

        const T twoZNear = 2 * zNear;

        ret.at(0,0) = twoZNear * invWidth;
        ret.at(1,1) = twoZNear * invHeight;

        ret.at(2,0) = (right + left) * invWidth;
        ret.at(2,1) = (top + bottom) * invHeight;
        ret.at(2,2) = - (zFar + zNear) * invDepth;
        ret.at(2,3) = -1;

        ret.at(3,2) = - twoZNear * zFar * invDepth;
        ret.at(3,3) = 0;

        return ret;
    }

    static Matrix4<T> createOrtho(T left, T right, T bottom, T top, T zNear, T zFar)
    {
        /*
               2
        ------------       0              0              tx
        right - left

                           2
            0         ------------        0              ty
                      top - bottom

                                          -2
            0              0         ------------        tz
                                      zFar-zNear

            0              0              0              1

       where

                                                    tx = - (right + left) / (right - left)

                                                    ty = - (top + bottom) / (top - bottom)

                                                    tz = - (zFar + zNear) / (zFar - zNear)

         */

        const T invWidth = 1.0 / (right  - left);
        const T invHeight = 1.0 / (top - bottom);
        const T invDepth = 1.0 / (zFar - zNear);

        Matrix4<T> ret;

        ret.at(0,0) = 2 * invWidth;
        ret.at(1,1) = 2 * invHeight;
        ret.at(2,2) = -2 * invDepth;

        ret.at(3,0) = -(right + left) * invWidth;
        ret.at(3,1) = -(top + bottom) * invHeight;
        ret.at(3,2) = -(zFar + zNear) * invDepth;

        return ret;
    }

    template<class FromT>
    static Matrix4<T> fromRowMajorArray(const FromT* arr)
    {
        const T retData[] =
        { static_cast<T>(arr[0]), static_cast<T>(arr[4]), static_cast<T>(arr[8]), static_cast<T>(arr[12]),
                static_cast<T>(arr[1]), static_cast<T>(arr[5]), static_cast<T>(arr[9]), static_cast<T>(arr[13]),
                static_cast<T>(arr[2]), static_cast<T>(arr[6]), static_cast<T>(arr[10]), static_cast<T>(arr[14]),
                static_cast<T>(arr[3]), static_cast<T>(arr[7]), static_cast<T>(arr[11]), static_cast<T>(arr[15]) };

        return retData;
    }

    template<class FromT>
    static Matrix4<T> fromColumnMajorArray(const FromT* arr)
    {
        const T retData[] =
        { static_cast<T>(arr[0]), static_cast<T>(arr[1]), static_cast<T>(arr[2]), static_cast<T>(arr[3]),
                static_cast<T>(arr[4]), static_cast<T>(arr[5]), static_cast<T>(arr[6]), static_cast<T>(arr[7]),
                static_cast<T>(arr[8]), static_cast<T>(arr[9]), static_cast<T>(arr[10]), static_cast<T>(arr[11]),
                static_cast<T>(arr[12]), static_cast<T>(arr[13]), static_cast<T>(arr[14]), static_cast<T>(arr[15]) };

        return retData;
    }

    //---------------------[ Equality operators ]------------------------------
    bool operator==(const Matrix4<T>& rhs) const
    {
        for (int i = 0; i < 16; i++)
        {
            if (std::fabs(data[i] - rhs.data[i]) >= EPSILON
                )
                return false;
        }
        return true;
    }

    bool operator!=(const Matrix4<T>& rhs) const
    {
        return !(*this == rhs);
    }

    //---------------------[ access operators ]---------------------------------
    T& at(int x, int y)
    {
        assert(x >= 0 && x < 4);
        assert(y >= 0 && y < 4);
        return data[x * 4 + y];
    }

    const T& at(int x, int y) const
    {
        assert(x >= 0 && x < 4);
        assert(y >= 0 && y < 4);
        return data[x * 4 + y];
    }

    T& operator()(int i, int j)
    {
        assert(i >= 1 && i <= 4);
        assert(j >= 1 && j <= 4);
        return data[(j - 1) * 4 + i - 1];
    }

    const T& operator()(int i, int j) const
    {
        assert(i >= 1 && i <= 4);
        assert(j >= 1 && j <= 4);
        return data[(j - 1) * 4 + i - 1];
    }

    void setTranslation(const Vector3<T>& v)
    {
        at(3, 0) = v.x;
        at(3, 1) = v.y;
        at(3, 2) = v.z;
        at(3, 3) = 1;
    }

    Vector3<T> getTranslation() const
    {
        return Vector3<T>(at(3, 0), at(3, 1), at(3, 2));
    }

    void setRotation(const Matrix3<T>& m)
    {
        for (int i = 0; i < 3; i++)
        {
            for (int j = 0; j < 3; j++)
            {
                at(i, j) = m.at(i, j);
            }
        }
    }

    Vector3<T> getScale() const
    { return Vector3<T>(at(0,0), at(1,1), at(2,2)); }

    void setScale(T s)
    {
        at(0,0) = at(1,1) = at(2,2) = s;
    }

    void setScale(T sx, T sy, T sz)
    {
        at(0,0) = sx;
        at(1,1) = sy;
        at(2,2) = sz;
    }

    void setScale(const Vector3<T>& s)
    {
        at(0,0) = s.x;
        at(1,1) = s.y;
        at(2,2) = s.z;
    }

    Matrix4<T>& operator=(const Matrix4<T>& rhs)
    {
        std::memcpy(data, rhs.data, sizeof(T) * 16);
        return *this;
    }

    template<class FromT>
    Matrix4<T>& operator=(const Matrix4<FromT>& rhs)
    {
        for (int i = 0; i < 16; i++)
        {
            data[i] = static_cast<T>(rhs.data[i]);
        }
        return *this;
    }

    Matrix4<T>& operator=(const T* rhs)
    {
        std::memcpy(data, rhs, sizeof(T) * 16);
        return *this;
    }

    /*Matrix4<T> & operator=(const double* m)
     {
     for (int i = 0; i < 16; i++) data[i] = (T)m[i];
     return * this;
     }*/

    //--------------------[ matrix with matrix operations ]---------------------
    Matrix4<T> operator+(const Matrix4<T>& rhs) const
    {
        Matrix4<T> ret;
        for (int i = 0; i < 16; i++)
            ret.data[i] = data[i] + rhs.data[i];
        return ret;
    }

    Matrix4<T> operator-(const Matrix4<T>& rhs) const
    {
        Matrix4<T> ret;
        for (int i = 0; i < 16; i++)
            ret.data[i] = data[i] - rhs.data[i];
        return ret;
    }

    //--------------------[ matrix with scalar operations ]---------------------
    Matrix4<T> operator+(T rhs) const
    {
        Matrix4<T> ret;
        for (int i = 0; i < 16; i++)
            ret.data[i] = data[i] + rhs;
        return ret;
    }

    Matrix4<T> operator-(T rhs) const
    {
        Matrix4<T> ret;
        for (int i = 0; i < 16; i++)
            ret.data[i] = data[i] - rhs;
        return ret;
    }

    Matrix4<T> operator*(T rhs) const
    {
        Matrix4<T> ret;
        for (int i = 0; i < 16; i++)
            ret.data[i] = data[i] * rhs;
        return ret;
    }

    Matrix4<T> operator/(T rhs) const
    {
        Matrix4<T> ret;
        for (int i = 0; i < 16; i++)
            ret.data[i] = data[i] / rhs;
        return ret;
    }

    //--------------------[ multiply operators ]--------------------------------
    Vector4<T> operator*(const Vector4<T>& rhs) const
    {
        return Vector4<T>(data[0] * rhs.x + data[4] * rhs.y + data[8] * rhs.z + data[12] * rhs.w,
                data[1] * rhs.x + data[5] * rhs.y + data[9] * rhs.z + data[13] * rhs.w,
                data[2] * rhs.x + data[6] * rhs.y + data[10] * rhs.z + data[14] * rhs.w,
                data[3] * rhs.x + data[7] * rhs.y + data[11] * rhs.z + data[15] * rhs.w);

    }

    Vector3<T> operator*(const Vector3<T>& rhs) const
    {
        return Vector3<T>(data[0] * rhs.x + data[4] * rhs.y + data[8] * rhs.z,
                data[1] * rhs.x + data[5] * rhs.y + data[9] * rhs.z,
                data[2] * rhs.x + data[6] * rhs.y + data[10] * rhs.z);
    }

    Matrix4<T> operator*(Matrix4<T> rhs) const
    {
        Matrix4<T> w;
        for (int i = 0; i < 4; i++)
        {
            for (int j = 0; j < 4; j++)
            {
                T n = 0;
                for (int k = 0; k < 4; k++)
                    n += rhs.at(i, k) * at(k, j);
                w.at(i, j) = n;
            }
        }
        return w;

    }

    //---------------------------[ misc operations ]----------------------------

    T det()
    {

        return +at(3, 0) * at(2, 1) * at(1, 2) * at(0, 3) - at(2, 0) * at(3, 1) * at(1, 2) * at(0, 3)
                - at(3, 0) * at(1, 1) * at(2, 2) * at(0, 3) + at(1, 0) * at(3, 1) * at(2, 2) * at(0, 3)

        + at(2, 0) * at(1, 1) * at(3, 2) * at(0, 3) - at(1, 0) * at(2, 1) * at(3, 2) * at(0, 3)
                - at(3, 0) * at(2, 1) * at(0, 2) * at(1, 3) + at(2, 0) * at(3, 1) * at(0, 2) * at(1, 3)

        + at(3, 0) * at(0, 1) * at(2, 2) * at(1, 3) - at(0, 0) * at(3, 1) * at(2, 2) * at(1, 3)
                - at(2, 0) * at(0, 1) * at(3, 2) * at(1, 3) + at(0, 0) * at(2, 1) * at(3, 2) * at(1, 3)

        + at(3, 0) * at(1, 1) * at(0, 2) * at(2, 3) - at(1, 0) * at(3, 1) * at(0, 2) * at(2, 3)
                - at(3, 0) * at(0, 1) * at(1, 2) * at(2, 3) + at(0, 0) * at(3, 1) * at(1, 2) * at(2, 3)

        + at(1, 0) * at(0, 1) * at(3, 2) * at(2, 3) - at(0, 0) * at(1, 1) * at(3, 2) * at(2, 3)
                - at(2, 0) * at(1, 1) * at(0, 2) * at(3, 3) + at(1, 0) * at(2, 1) * at(0, 2) * at(3, 3)

        + at(2, 0) * at(0, 1) * at(1, 2) * at(3, 3) - at(0, 0) * at(2, 1) * at(1, 2) * at(3, 3)
                - at(1, 0) * at(0, 1) * at(2, 2) * at(3, 3) + at(0, 0) * at(1, 1) * at(2, 2) * at(3, 3);

    }

    Matrix4<T> inverse()
    {
        Matrix4<T> ret;

        ret.at(0, 0) = +at(2, 1) * at(3, 2) * at(1, 3) - at(3, 1) * at(2, 2) * at(1, 3) + at(3, 1) * at(1, 2) * at(2, 3)
                - at(1, 1) * at(3, 2) * at(2, 3) - at(2, 1) * at(1, 2) * at(3, 3) + at(1, 1) * at(2, 2) * at(3, 3);

        ret.at(1, 0) = +at(3, 0) * at(2, 2) * at(1, 3) - at(2, 0) * at(3, 2) * at(1, 3) - at(3, 0) * at(1, 2) * at(2, 3)
                + at(1, 0) * at(3, 2) * at(2, 3) + at(2, 0) * at(1, 2) * at(3, 3) - at(1, 0) * at(2, 2) * at(3, 3);

        ret.at(2, 0) = +at(2, 0) * at(3, 1) * at(1, 3) - at(3, 0) * at(2, 1) * at(1, 3) + at(3, 0) * at(1, 1) * at(2, 3)
                - at(1, 0) * at(3, 1) * at(2, 3) - at(2, 0) * at(1, 1) * at(3, 3) + at(1, 0) * at(2, 1) * at(3, 3);

        ret.at(3, 0) = +at(3, 0) * at(2, 1) * at(1, 2) - at(2, 0) * at(3, 1) * at(1, 2) - at(3, 0) * at(1, 1) * at(2, 2)
                + at(1, 0) * at(3, 1) * at(2, 2) + at(2, 0) * at(1, 1) * at(3, 2) - at(1, 0) * at(2, 1) * at(3, 2);

        ret.at(0, 1) = +at(3, 1) * at(2, 2) * at(0, 3) - at(2, 1) * at(3, 2) * at(0, 3) - at(3, 1) * at(0, 2) * at(2, 3)
                + at(0, 1) * at(3, 2) * at(2, 3) + at(2, 1) * at(0, 2) * at(3, 3) - at(0, 1) * at(2, 2) * at(3, 3);

        ret.at(1, 1) = +at(2, 0) * at(3, 2) * at(0, 3) - at(3, 0) * at(2, 2) * at(0, 3) + at(3, 0) * at(0, 2) * at(2, 3)
                - at(0, 0) * at(3, 2) * at(2, 3) - at(2, 0) * at(0, 2) * at(3, 3) + at(0, 0) * at(2, 2) * at(3, 3);

        ret.at(2, 1) = +at(3, 0) * at(2, 1) * at(0, 3) - at(2, 0) * at(3, 1) * at(0, 3) - at(3, 0) * at(0, 1) * at(2, 3)
                + at(0, 0) * at(3, 1) * at(2, 3) + at(2, 0) * at(0, 1) * at(3, 3) - at(0, 0) * at(2, 1) * at(3, 3);

        ret.at(3, 1) = +at(2, 0) * at(3, 1) * at(0, 2) - at(3, 0) * at(2, 1) * at(0, 2) + at(3, 0) * at(0, 1) * at(2, 2)
                - at(0, 0) * at(3, 1) * at(2, 2) - at(2, 0) * at(0, 1) * at(3, 2) + at(0, 0) * at(2, 1) * at(3, 2);

        ret.at(0, 2) = +at(1, 1) * at(3, 2) * at(0, 3) - at(3, 1) * at(1, 2) * at(0, 3) + at(3, 1) * at(0, 2) * at(1, 3)
                - at(0, 1) * at(3, 2) * at(1, 3) - at(1, 1) * at(0, 2) * at(3, 3) + at(0, 1) * at(1, 2) * at(3, 3);

        ret.at(1, 2) = +at(3, 0) * at(1, 2) * at(0, 3) - at(1, 0) * at(3, 2) * at(0, 3) - at(3, 0) * at(0, 2) * at(1, 3)
                + at(0, 0) * at(3, 2) * at(1, 3) + at(1, 0) * at(0, 2) * at(3, 3) - at(0, 0) * at(1, 2) * at(3, 3);

        ret.at(2, 2) = +at(1, 0) * at(3, 1) * at(0, 3) - at(3, 0) * at(1, 1) * at(0, 3) + at(3, 0) * at(0, 1) * at(1, 3)
                - at(0, 0) * at(3, 1) * at(1, 3) - at(1, 0) * at(0, 1) * at(3, 3) + at(0, 0) * at(1, 1) * at(3, 3);

        ret.at(3, 2) = +at(3, 0) * at(1, 1) * at(0, 2) - at(1, 0) * at(3, 1) * at(0, 2) - at(3, 0) * at(0, 1) * at(1, 2)
                + at(0, 0) * at(3, 1) * at(1, 2) + at(1, 0) * at(0, 1) * at(3, 2) - at(0, 0) * at(1, 1) * at(3, 2);

        ret.at(0, 3) = +at(2, 1) * at(1, 2) * at(0, 3) - at(1, 1) * at(2, 2) * at(0, 3) - at(2, 1) * at(0, 2) * at(1, 3)
                + at(0, 1) * at(2, 2) * at(1, 3) + at(1, 1) * at(0, 2) * at(2, 3) - at(0, 1) * at(1, 2) * at(2, 3);

        ret.at(1, 3) = +at(1, 0) * at(2, 2) * at(0, 3) - at(2, 0) * at(1, 2) * at(0, 3) + at(2, 0) * at(0, 2) * at(1, 3)
                - at(0, 0) * at(2, 2) * at(1, 3) - at(1, 0) * at(0, 2) * at(2, 3) + at(0, 0) * at(1, 2) * at(2, 3);

        ret.at(2, 3) = +at(2, 0) * at(1, 1) * at(0, 3) - at(1, 0) * at(2, 1) * at(0, 3) - at(2, 0) * at(0, 1) * at(1, 3)
                + at(0, 0) * at(2, 1) * at(1, 3) + at(1, 0) * at(0, 1) * at(2, 3) - at(0, 0) * at(1, 1) * at(2, 3);

        ret.at(3, 3) = +at(1, 0) * at(2, 1) * at(0, 2) - at(2, 0) * at(1, 1) * at(0, 2) + at(2, 0) * at(0, 1) * at(1, 2)
                - at(0, 0) * at(2, 1) * at(1, 2) - at(1, 0) * at(0, 1) * at(2, 2) + at(0, 0) * at(1, 1) * at(2, 2);

        return ret / det();
    }

    Matrix4<T> transpose()
    {
        Matrix4<T> ret;
        for (int i = 0; i < 4; i++)
        {
            for (int j = 0; j < 4; j++)
            {
                ret.at(i, j) = at(j, i);
            }
        }
        return ret;
    }

    Matrix4<T> lerp(T fact, const Matrix4<T>& rhs) const
    {
        Matrix4<T> ret = (*this) + (rhs - (*this)) * fact;
        return ret;
    }

    //-------------[ conversion ]-----------------------------
    operator T*()
    {
        return (T*) data;
    }

    operator const T*() const
    {
        return (const T*) data;
    }

    //----------[ output operator ]----------------------------
    friend std::ostream& operator <<(std::ostream& lhs, const Matrix4<T>& rhs)
    {
        for (int i = 0; i < 4; i++)
        {
            lhs << "|\t";
            for (int j = 0; j < 4; j++)
            {
                lhs << rhs.at(j, i) << "\t";
            }
            lhs << "|" << std::endl;
        }
        return lhs;
    }

    std::string toString() const
    {
        std::ostringstream oss;
        oss << *this;
        return oss.str();
    }

};

typedef Matrix4<float> Matrix4f;
typedef Matrix4<double> Matrix4d;
typedef Matrix4<int> Matrix4i;

template<class T>
class Quaternion
{
public:
    T w;
    Vector3<T> v;

    Quaternion()
            : w(0), v(0, 0, 0)
    {
    }

    Quaternion(const Quaternion<T>& q)
            : w(q.w), v(q.v)
    {
    }

    template<class FromT>
    Quaternion(const Quaternion<FromT>& q)
            : w(static_cast<T>(q.w)), v(q.v)
    {
    }

    Quaternion(T w_, const Vector3<T>& v_)
            : w(w_), v(v_)
    {
    }

    Quaternion(T w_, T x, T y, T z)
            : w(w_), v(x, y, z)
    {
    }

    Quaternion<T>& operator=(const Quaternion<T>& rhs)
    {
        v = rhs.v;
        w = rhs.w;
        return *this;
    }

    template<class FromT>
    Quaternion<T>& operator=(const Quaternion<FromT>& rhs)
    {
        v = rhs.v;
        w = static_cast<T>(rhs.w);
        return *this;
    }

    Quaternion<T> operator+(const Quaternion<T>& rhs) const
    {
        const Quaternion<T>& lhs = *this;
        return Quaternion<T>(lhs.w + rhs.w, lhs.v + rhs.v);
    }

    Quaternion<T> operator*(const Quaternion<T>& rhs) const
    {
        const Quaternion<T>& lhs = *this;
        return Quaternion<T>(lhs.w * rhs.w - lhs.v.x * rhs.v.x - lhs.v.y * rhs.v.y - lhs.v.z * rhs.v.z,
                lhs.w * rhs.v.x + lhs.v.x * rhs.w + lhs.v.y * rhs.v.z - lhs.v.z * rhs.v.y,
                lhs.w * rhs.v.y - lhs.v.x * rhs.v.z + lhs.v.y * rhs.w + lhs.v.z * rhs.v.x,
                lhs.w * rhs.v.z + lhs.v.x * rhs.v.y - lhs.v.y * rhs.v.x + lhs.v.z * rhs.w);
    }

    Quaternion<T> operator*(T rhs) const
    {
        return Quaternion<T>(w * rhs, v * rhs);
    }

    Quaternion<T> operator-(const Quaternion<T>& rhs) const
    {
        const Quaternion<T>& lhs = *this;
        return Quaternion<T>(lhs.w - rhs.w, lhs.v - rhs.v);
    }

    Quaternion<T>& operator+=(const Quaternion<T>& rhs)
    {
        w += rhs.w;
        v += rhs.v;
        return *this;
    }

    Quaternion<T>& operator-=(const Quaternion<T>& rhs)
    {
        w -= rhs.w;
        v -= rhs.v;
        return *this;
    }

    Quaternion<T>& operator*=(const Quaternion<T>& rhs)
    {
        Quaternion q = (*this) * rhs;
        v = q.v;
        w = q.w;
        return *this;
    }

    Quaternion<T>& operator*=(T rhs)
    {
        w *= rhs;
        v *= rhs;
        return *this;
    }

    bool operator==(const Quaternion<T>& rhs) const
    {
        const Quaternion<T>& lhs = *this;
        return (std::fabs(lhs.w - rhs.w) < EPSILON) && lhs.v == rhs.v;
    }

    bool operator!=(const Quaternion<T>& rhs) const
    {
        return !(*this == rhs);
    }

    //-------------[ unary operations ]--------------------------
    Quaternion<T> operator-() const
    {
        return Quaternion<T>(-w, -v);
    }

    Quaternion<T> operator~() const
    {
        return Quaternion<T>(w, -v);
    }

    T length() const
    {
        return (T) std::sqrt(w * w + v.lengthSq());
    }

    T lengthSq() const
    {
        return w * w + v.lengthSq();
    }

    void normalize()
    {
        T len = length();
        w /= len;
        v /= len;
    }

    static Quaternion<T> fromEulerAngles(T x, T y, T z)
    {
        Quaternion<T> ret = fromAxisRot(Vector3<T>(1, 0, 0), x) * fromAxisRot(Vector3<T>(0, 1, 0), y)
                * fromAxisRot(Vector3<T>(0, 0, 1), z);
        return ret;
    }

    static Quaternion<T> fromAxisRot(Vector3<T> axis, float angleDeg)
    {
        double angleRad = DEG2RAD(angleDeg);
        double sa2 = std::sin(angleRad / 2);
        double ca2 = std::cos(angleRad / 2);
        return Quaternion<T>(ca2, axis * sa2);
    }

    Matrix3<T> rotMatrix()
    {
        Matrix3<T> ret;

        /*ret.at(0,0) = 1 - 2*v.y*v.y - 2*v.z*v.z;
         ret.at(1,0) = 2*v.x*v.y - 2*w*v.z;
         ret.at(2,0) = 2*v.x*v.z - 2*w*v.y;
         
         ret.at(0,1) = 2*v.x*v.y + 2*w*v.z;
         ret.at(1,1) = 1 - 2*v.x*v.x - 2*v.z*v.z;
         ret.at(2,1) = 2*v.y*v.z - 2*w*v.x;
         
         ret.at(0,2) = 2*v.x*v.z - 2*w*v.y;
         ret.at(1,2) = 2*v.y*v.z + 2*w*v.x;
         ret.at(2,2) = 1 - 2*v.x*v.x - 2*v.y*v.y;*/

        T xx = v.x * v.x;
        T xy = v.x * v.y;
        T xz = v.x * v.z;
        T xw = v.x * w;

        T yy = v.y * v.y;
        T yz = v.y * v.z;
        T yw = v.y * w;

        T zz = v.z * v.z;
        T zw = v.z * w;

        ret.at(0, 0) = 1 - 2 * (yy + zz);
        ret.at(1, 0) = 2 * (xy - zw);
        ret.at(2, 0) = 2 * (xz + yw);

        ret.at(0, 1) = 2 * (xy + zw);
        ret.at(1, 1) = 1 - 2 * (xx + zz);
        ret.at(2, 1) = 2 * (yz - xw);

        ret.at(0, 2) = 2 * (xz - yw);
        ret.at(1, 2) = 2 * (yz + xw);
        ret.at(2, 2) = 1 - 2 * (xx + yy);

        return ret;
    }

    Matrix4<T> transform() const
    {
        Matrix4<T> ret;

        T xx = v.x * v.x;
        T xy = v.x * v.y;
        T xz = v.x * v.z;
        T xw = v.x * w;

        T yy = v.y * v.y;
        T yz = v.y * v.z;
        T yw = v.y * w;

        T zz = v.z * v.z;
        T zw = v.z * w;

        ret.at(0, 0) = 1 - 2 * (yy + zz);
        ret.at(1, 0) = 2 * (xy - zw);
        ret.at(2, 0) = 2 * (xz + yw);
        ret.at(3, 0) = 0;

        ret.at(0, 1) = 2 * (xy + zw);
        ret.at(1, 1) = 1 - 2 * (xx + zz);
        ret.at(2, 1) = 2 * (yz - xw);
        ret.at(3, 1) = 0;

        ret.at(0, 2) = 2 * (xz - yw);
        ret.at(1, 2) = 2 * (yz + xw);
        ret.at(2, 2) = 1 - 2 * (xx + yy);
        ret.at(3, 2) = 0;

        ret.at(0, 3) = 0;
        ret.at(1, 3) = 0;
        ret.at(2, 3) = 0;
        ret.at(3, 3) = 1;

        return ret;

    }

    Quaternion<T> lerp(T fact, const Quaternion<T>& rhs) const
    {
        return Quaternion<T>((1 - fact) * w + fact * rhs.w, v.lerp(fact, rhs.v));
    }

    friend std::ostream& operator <<(std::ostream& oss, const Quaternion<T>& q)
    {
        oss << "Re: " << q.w << " Im: " << q.v;
        return oss;
    }

    std::string toString() const
    {
        std::ostringstream oss;
        oss << *this;
        return oss.str();
    }

    // 2011-07-02: Davide Bacchet: changed formula to fix degenerate cases
     static Quaternion<T> fromMatrix(const Matrix4<T>& m)
     {
        Quaternion<T> q;

        T tr, s;
        tr = m(1, 1) + m(2, 2) + m(3, 3);
        if (tr >= epsilon)
        {
            s = 0.5 / (T) sqrt(tr + 1.0);
            q.w = 0.25 / s;
            q.v.x = (m(3, 2) - m(2, 3)) * s;
            q.v.y = (m(1, 3) - m(3, 1)) * s;
            q.v.z = (m(2, 1) - m(1, 2)) * s;
        }
        else
        {
            T d0 = m(1, 1);
            T d1 = m(2, 2);
            T d2 = m(3, 3);

            char bigIdx = (d0 > d1) ? ((d0 > d2) ? 0 : 2):((d1 > d2) ? 1 : 2);

            if (bigIdx == 0)
            {
                s = 2.0 * (T) sqrt(1.0 + m(1, 1) - m(2, 2) - m(3, 3));
                q.w = (m(3, 2) - m(2, 3)) / s;
                q.v.x = 0.25 * s;
                q.v.y = (m(1, 2) + m(2, 1)) / s;
                q.v.z = (m(1, 3) + m(3, 1)) / s;
            }
            else if (bigIdx == 1)
            {
                s = 2.0 * (T) sqrt(1.0 + m(2, 2) - m(1, 1) - m(3, 3));
                q.w = (m(1, 3) - m(3, 1)) / s;
                q.v.x = (m(1, 2) + m(2, 1)) / s;
                q.v.y = 0.25 * s;
                q.v.z = (m(2, 3) + m(3, 2)) / s;
            }
            else
            {
                s = 2.0 * (T) sqrt(1.0 + m(3, 3) - m(1, 1) - m(2, 2));
                q.w = (m(2, 1) - m(1, 2)) / s;
                q.v.x = (m(1, 3) + m(3, 1)) / s;
                q.v.y = (m(2, 3) + m(3, 2)) / s;
                q.v.z = 0.25 * s;
            }
        }

        return q;
    }

     // 2011-07-02: Davide Bacchet: changed formula to fix degenerate cases
     static Quaternion<T> fromMatrix(const Matrix3<T>& m)
     {
        Quaternion<T> q;

        T tr, s;
        tr = m(1, 1) + m(2, 2) + m(3, 3);
        if (tr >= epsilon)
        {
            s = 0.5 / (T) sqrt(tr + 1.0);
            q.w = 0.25 / s;
            q.v.x = (m(3, 2) - m(2, 3)) * s;
            q.v.y = (m(1, 3) - m(3, 1)) * s;
            q.v.z = (m(2, 1) - m(1, 2)) * s;
        }
        else
        {
            T d0 = m(1, 1);
            T d1 = m(2, 2);
            T d2 = m(3, 3);

            char bigIdx = (d0 > d1) ? ((d0 > d2) ? 0 : 2):((d1 > d2) ? 1 : 2);

            if (bigIdx == 0)
            {
                s = 2.0 * (T) sqrt(1.0 + m(1, 1) - m(2, 2) - m(3, 3));
                q.w = (m(3, 2) - m(2, 3)) / s;
                q.v.x = 0.25 * s;
                q.v.y = (m(1, 2) + m(2, 1)) / s;
                q.v.z = (m(1, 3) + m(3, 1)) / s;
            }
            else if (bigIdx == 1)
            {
                s = 2.0 * (T) sqrt(1.0 + m(2, 2) - m(1, 1) - m(3, 3));
                q.w = (m(1, 3) - m(3, 1)) / s;
                q.v.x = (m(1, 2) + m(2, 1)) / s;
                q.v.y = 0.25 * s;
                q.v.z = (m(2, 3) + m(3, 2)) / s;
            }
            else
            {
                s = 2.0 * (T) sqrt(1.0 + m(3, 3) - m(1, 1) - m(2, 2));
                q.w = (m(2, 1) - m(1, 2)) / s;
                q.v.x = (m(1, 3) + m(3, 1)) / s;
                q.v.y = (m(2, 3) + m(3, 2)) / s;
                q.v.z = 0.25 * s;
            }
        }

        return q;
    }

    Quaternion<T> slerp(T r, const Quaternion<T>& q2) const
    {
        Quaternion<T> ret;
        T cosTheta = w * q2.w + v.x * q2.v.x + v.y * q2.v.y + v.z * q2.v.z;
        T theta = (T) acos(cosTheta);
        if (fabs(theta) < epsilon)
        {
            ret = *this;
        }
        else
        {
            T sinTheta = (T) sqrt(1.0 - cosTheta * cosTheta);
            if (fabs(sinTheta) < epsilon)
            {
                ret.w = 0.5 * w + 0.5 * q2.w;
                ret.v = v.lerp(0.5, q2.v);
            }
            else
            {
                T rA = (T) sin((1.0 - r) * theta) / sinTheta;
                T rB = (T) sin(r * theta) / sinTheta;

                ret.w = w * rA + q2.w * rB;
                ret.v.x = v.x * rA + q2.v.x * rB;
                ret.v.y = v.y * rA + q2.v.y * rB;
                ret.v.z = v.z * rA + q2.v.z * rB;
            }
        }
        return ret;
    }

};

typedef Quaternion<float> Quatf;
typedef Quaternion<double> Quatd;

#ifdef VMATH_NAMESPACE
}
#endif



//
// Standard C++ library extensions
//


// Shortcut defines
#ifdef VMATH_NAMESPACE
#define VEC2 VMATH_NAMESPACE::Vector2
#define VEC3 VMATH_NAMESPACE::Vector3
#define VEC4 VMATH_NAMESPACE::Vector4
#else
#define VEC2 Vector2
#define VEC3 Vector3
#define VEC4 Vector4
#endif

namespace std
{

    template <typename T>
    VEC2<T> min(const VEC2<T>& a, const VEC2<T>& b)
    {
        return VEC2<T>(::std::min(a.x, b.x), ::std::min(a.y, b.y));
    }

    template <typename T>
    VEC3<T> min(const VEC3<T>& a, const VEC3<T>& b)
    {
        return VEC3<T>(::std::min(a.x, b.x), ::std::min(a.y, b.y), ::std::min(a.z, b.z));
    }

    template <typename T>
    VEC4<T> min(const VEC4<T>& a, const VEC4<T>& b)
    {
        return VEC4<T>(::std::min(a.x, b.x), ::std::min(a.y, b.y), ::std::min(a.z, b.z), ::std::min(a.w, b.w));
    }

    template <typename T>
    VEC2<T> max(const VEC2<T>& a, const VEC2<T>& b)
    {
        return VEC2<T>(::std::max(a.x, b.x), ::std::max(a.y, b.y));
    }

    template <typename T>
    VEC3<T> max(const VEC3<T>& a, const VEC3<T>& b)
    {
        return VEC3<T>(::std::max(a.x, b.x), ::std::max(a.y, b.y), ::std::max(a.z, b.z));
    }

    template <typename T>
    VEC4<T> max(const VEC4<T>& a, const VEC4<T>& b)
    {
        return VEC4<T>(::std::max(a.x, b.x), ::std::max(a.y, b.y), ::std::max(a.z, b.z), ::std::max(a.w, b.w));
    }
}

// cleanup shortcut defines
#undef VEC2
#undef VEC3
#undef VEC4

#ifdef VMATH_NAMESPACE
namespace VMATH_NAMESPACE
{
#endif //VMATH_NAMESPACE

    template <typename T>
    class Aabb3
    {
    public:
        Vector3<T> min;

        Vector3<T> max;

        Aabb3()
            : min(1,1,1), max(-1,-1,-1)
        {}

        template <typename SrcT>
        Aabb3(const Vector3<SrcT>& point)
            : min(point), max(point)
        {}

        template <typename SrcT>
        Aabb3(SrcT x0, SrcT y0, SrcT z0, SrcT x1, SrcT y1, SrcT z1)
            : min(std::min(x0,x1), std::min(y0,y1), std::min(z0,z1)),
              max(std::max(x0,x1), std::max(y0,y1), std::max(z0,z1))
        {}

        template <typename SrcT>
        Aabb3(SrcT x, SrcT y, SrcT z)
            : min(x,y,z), max(x,y,z)
        {}

        template <typename SrcT>
        Aabb3(const Aabb3<SrcT>& src)
            : min(src.min), max(src.max)
        {}

        template <typename SrcT>
        Aabb3<T>& operator=(const Aabb3<SrcT>& rhs)
        {
            min = rhs.min;
            max = rhs.max;
            return *this;
        }


        bool valid() const
        { return min.x <= max.x && min.y <= max.y && min.z <= max.z; }

        void invalidate()
        { min = Vector3<T>(1,1,1); max = Vector3<T>(-1,-1,-1); }

        template <typename SrcT>
        void extend(const Vector3<SrcT>& point)
        {
            if (!valid())
            {
                min = max = point;
            }
            else
            {
                min = std::min(min, point);
                max = std::max(max, point);
            }
        }

        template <typename SrcT>
        void extend(const Aabb3<SrcT>& box)
        {
            if (!valid())
            {
                min = box.min;
                max = box.max;
            }
            else
            {
                min = std::min(min, box.min);
                max = std::max(max, box.max);
            }
        }

        template <typename SrcT>
        Aabb3<T> extended(const Vector3<SrcT>& point) const
        {
            Aabb3<T> ret(*this);
            ret.extend(point);
            return ret;
        }

        template <typename SrcT>
        Aabb3<T> extended(const Aabb3<SrcT>& box) const
        {
            Aabb3<T> ret(*this);
            ret.extend(box);
            return ret;
        }

        template <typename SrcT>
        bool intersects(const Vector3<SrcT>& point) const
        {
            /*
            for (size_t i = 0; i < 3; i++)
            {
                if (min[i] > point[i] || point[i] > max[i])
                    return false;
            }
            */
            // unrolled loop above
            if (min.x > point.x || point.x > max.x)
                return false;
            if (min.y > point.y || point.y > max.y)
                return false;
            if (min.z > point.z || point.z > max.z)
                return false;

            return true;
        }

        template <typename SrcT>
        bool intersects(const Aabb3<SrcT>& box) const
        {
            /*
            for (size_t i = 0; i < 3; i++)
            {
                if (max[i] < box.min[i] || min[i] > box.max[i])
                    return false;
            }
            */
            // unrolled loop above
            if (max.x < box.min.x || min.x > box.max.x)
                return false;
            if (max.y < box.min.y || min.y > box.max.y)
                return false;
            if (max.z < box.min.z || min.z > box.max.z)
                return false;

            return true;
        }

        template <typename SrcT>
        Aabb3<T> intersection(const Aabb3<SrcT>& other) const
        {
            Aabb3<T> ret;
            if (max.x < other.min.x || min.x > other.max.x)
                return ret;
            if (max.y < other.min.y || min.y > other.max.y)
                return ret;
            if (max.z < other.min.z || min.z > other.max.z)
                return ret;

            ret.min = std::max(min, other.min);
            ret.max = std::min(max, other.max);

            return ret;
        }

        Vector3<T> center() const
        { return (min + max) * 0.5f; }

        Vector3<T> extent() const
        { return (max - min) * 0.5f; }

        Vector3<T> size() const
        { return max - min; }

        Vector3<T> point(size_t i) const
        {
            assert(i < 8);
            return Vector3<T>(i & 1 ? min.x : max.x, i & 2 ? min.y : max.y, i & 4 ? min.z : max.z);
        }

        Aabb3<T> transformed(const Matrix4<T>& t) const
        {
            Aabb3<T> ret;
            for (size_t i = 0; i < 8; i++)
            {
                const Vector4<T> p(point(i), 1);
                ret.extend((t * p).xyz());
            }

            return ret;
        }

        //-------------------------------------------------------------------------------------------------------------
        // operators
        //-------------------------------------------------------------------------------------------------------------
        template <typename RhsT>
        bool operator==(const Aabb3<RhsT>& rhs) const
        {
            return min == rhs.min && max == rhs.max;
        }

        template <typename RhsT>
        bool operator!=(const Aabb3<RhsT>& rhs) const
        {
            return min != rhs.min || max != rhs.max;
        }

        Aabb3<T> operator*(const Matrix4<T>& rhs) const
        {
            return transformed(rhs);
        }

        Aabb3<T>& operator*=(const Matrix4<T>& rhs)
        {
            *this = transformed(rhs);
            return *this;
        }


        template <typename SrcT>
        Aabb3<T>& operator<<(const Vector3<SrcT>& rhs)
        {
            extend(rhs);
            return *this;
        }

        template <typename SrcT>
        Aabb3<T>& operator<<(const Aabb3<SrcT>& rhs)
        {
            extend(rhs);
            return *this;
        }

        template <typename RhsT>
        Aabb3<T> operator|(const Aabb3<RhsT>& rhs) const
        {
            return extended(rhs);
        }

        template <typename RhsT>
        Aabb3<T> operator&(const Aabb3<RhsT>& rhs) const
        {
            return intersection(rhs);
        }

        friend std::ostream& operator<<(std::ostream& lhs, const Aabb3<T>& rhs)
        {
            lhs << rhs.min << " x " << rhs.max;
            return lhs;
        }

    };

    typedef Aabb3<float> Aabb3f;
    typedef Aabb3<double> Aabb3d;

#ifdef VMATH_NAMESPACE
}
#endif //VMATH_NAMESPACE


#endif // __vmath_Header_File__