Diff: vector_math.h

Revision:

1:2daa36bfe56b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/vector_math.h	Wed Feb 20 15:52:50 2013 +0000
@@ -0,0 +1,1115 @@
+/*
+Copyright (c) 2007, Markus Trenkwalder
+
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without 
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, 
+  this list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation 
+  and/or other materials provided with the distribution.
+
+* Neither the name of the library's copyright owner nor the names of its 
+  contributors may be used to endorse or promote products derived from this 
+  software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef VECTOR_MATH_H
+#define VECTOR_MATH_H
+
+//#include <cmath>
+
+// "minor" can be defined from GCC and can cause problems
+#undef minor
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+namespace vmath {
+
+//using std::sin;
+//using std::cos;
+//using std::acos;
+//using std::sqrt;
+
+template <typename T>
+inline T rsqrt(T x)
+{
+    return T(1) / sqrt(x);
+}
+
+template <typename T>
+inline T inv(T x)
+{
+    return T(1) / x;
+}
+
+namespace detail {
+    // This function is used heavily in this library. Here is a generic 
+    // implementation for it. If you can provide a faster one for your specific
+    // types this can speed up things considerably.
+    template <typename T>
+    inline T multiply_accumulate(int count, const T *a, const T *b)
+    {
+        T result = T(0);
+        for (int i = 0; i < count; ++i)
+            result += a[i] * b[i];
+        return result;
+    }
+}
+
+#define MOP_M_CLASS_TEMPLATE(CLASS, OP, COUNT) \
+    CLASS & operator OP (const CLASS& rhs)  \
+    { \
+        for (int i = 0; i < (COUNT); ++i )  \
+            (*this)[i] OP rhs[i]; \
+        return *this; \
+    }
+
+#define MOP_M_TYPE_TEMPLATE(CLASS, OP, COUNT) \
+    CLASS & operator OP (const T & rhs) \
+    { \
+        for (int i = 0; i < (COUNT); ++i ) \
+            (*this)[i] OP rhs; \
+        return *this; \
+    }
+
+#define MOP_COMP_TEMPLATE(CLASS, COUNT) \
+    bool operator == (const CLASS & rhs) \
+    { \
+        bool result = true; \
+        for (int i = 0; i < (COUNT); ++i) \
+            result = result && (*this)[i] == rhs[i]; \
+        return result; \
+    } \
+    bool operator != (const CLASS & rhs) \
+    { return !((*this) == rhs); }
+
+#define MOP_G_UMINUS_TEMPLATE(CLASS, COUNT) \
+    CLASS operator - () const \
+    { \
+        CLASS result; \
+        for (int i = 0; i < (COUNT); ++i) \
+            result[i] = -(*this)[i]; \
+        return result; \
+    }
+
+#define COMMON_OPERATORS(CLASS, COUNT) \
+    MOP_M_CLASS_TEMPLATE(CLASS, +=, COUNT) \
+    MOP_M_CLASS_TEMPLATE(CLASS, -=, COUNT) \
+    /*no *= as this is not the same for vectors and matrices */ \
+    MOP_M_CLASS_TEMPLATE(CLASS, /=, COUNT) \
+    MOP_M_TYPE_TEMPLATE(CLASS, +=, COUNT) \
+    MOP_M_TYPE_TEMPLATE(CLASS, -=, COUNT) \
+    MOP_M_TYPE_TEMPLATE(CLASS, *=, COUNT) \
+    MOP_M_TYPE_TEMPLATE(CLASS, /=, COUNT) \
+    MOP_G_UMINUS_TEMPLATE(CLASS, COUNT) \
+    MOP_COMP_TEMPLATE(CLASS, COUNT)
+
+#define VECTOR_COMMON(CLASS, COUNT) \
+    COMMON_OPERATORS(CLASS, COUNT) \
+    MOP_M_CLASS_TEMPLATE(CLASS, *=, COUNT) \
+    operator const T* () const { return &x; } \
+    operator T* () { return &x; }
+
+#define FOP_G_SOURCE_TEMPLATE(OP, CLASS) \
+    { CLASS<T> r = lhs; r OP##= rhs; return r; }
+
+#define FOP_G_CLASS_TEMPLATE(OP, CLASS) \
+    template <typename T> \
+    inline CLASS<T> operator OP (const CLASS<T> &lhs, const CLASS<T> &rhs) \
+    FOP_G_SOURCE_TEMPLATE(OP, CLASS)
+
+#define FOP_G_TYPE_TEMPLATE(OP, CLASS) \
+    template <typename T> \
+    inline CLASS<T> operator OP (const CLASS<T> &lhs, const T &rhs) \
+    FOP_G_SOURCE_TEMPLATE(OP, CLASS)
+
+// forward declarations
+template <typename T> struct vec2;
+template <typename T> struct vec3;
+template <typename T> struct vec4;
+template <typename T> struct mat2;
+template <typename T> struct mat3;
+template <typename T> struct mat4;
+template <typename T> struct quat;
+
+#define FREE_MODIFYING_OPERATORS(CLASS) \
+    FOP_G_CLASS_TEMPLATE(+, CLASS) \
+    FOP_G_CLASS_TEMPLATE(-, CLASS) \
+    FOP_G_CLASS_TEMPLATE(*, CLASS) \
+    FOP_G_CLASS_TEMPLATE(/, CLASS) \
+    FOP_G_TYPE_TEMPLATE(+, CLASS) \
+    FOP_G_TYPE_TEMPLATE(-, CLASS) \
+    FOP_G_TYPE_TEMPLATE(*, CLASS) \
+    FOP_G_TYPE_TEMPLATE(/, CLASS)
+
+FREE_MODIFYING_OPERATORS(vec2)
+FREE_MODIFYING_OPERATORS(vec3)
+FREE_MODIFYING_OPERATORS(vec4)
+FREE_MODIFYING_OPERATORS(mat2)
+FREE_MODIFYING_OPERATORS(mat3)
+FREE_MODIFYING_OPERATORS(mat4)
+FREE_MODIFYING_OPERATORS(quat)
+
+#define FREE_OPERATORS(CLASS) \
+    template <typename T> \
+    inline CLASS<T> operator + (const T& a, const CLASS<T>& b)  \
+    { CLASS<T> r = b; r += a; return r; } \
+    \
+    template <typename T> \
+    inline CLASS<T> operator * (const T& a, const CLASS<T>& b)  \
+    { CLASS<T> r = b; r *= a; return r; } \
+    \
+    template <typename T> \
+    inline CLASS<T> operator - (const T& a, const CLASS<T>& b)  \
+    { return -b + a; } \
+    \
+    template <typename T> \
+    inline CLASS<T> operator / (const T& a, const CLASS<T>& b)  \
+    { CLASS<T> r(a); r /= b; return r; }
+
+FREE_OPERATORS(vec2)
+FREE_OPERATORS(vec3)
+FREE_OPERATORS(vec4)
+FREE_OPERATORS(mat2)
+FREE_OPERATORS(mat3)
+FREE_OPERATORS(mat4)
+FREE_OPERATORS(quat)
+
+template <typename T>
+struct vec2 {
+    T x, y;
+    
+    vec2() {};
+    explicit vec2(const T i) : x(i), y(i) {}
+    explicit vec2(const T ix, const T iy) : x(ix), y(iy) {}
+    explicit vec2(const vec3<T>& v);
+    explicit vec2(const vec4<T>& v);
+
+    VECTOR_COMMON(vec2, 2)
+};
+
+template <typename T> 
+struct vec3 {
+    T x, y, z;
+    
+    vec3() {};
+    explicit vec3(const T i) : x(i), y(i), z(i) {}
+    explicit vec3(const T ix, const T iy, const T iz) : x(ix), y(iy), z(iz) {}
+    explicit vec3(const vec2<T>& xy, const T iz) : x(xy.x), y(xy.y), z(iz) {}
+    explicit vec3(const T ix, const vec2<T>& yz) : x(ix), y(yz.y), z(yz.z) {}
+    explicit vec3(const vec4<T>& v);
+
+    VECTOR_COMMON(vec3, 3)
+};
+
+template <typename T> 
+struct vec4 {
+    T x, y, z, w;
+    
+    vec4() {};
+    explicit vec4(const T i) : x(i), y(i), z(i), w(i) {}
+    explicit vec4(const T ix, const T iy, const T iz, const T iw) : x(ix), y(iy), z(iz), w(iw) {}
+    explicit vec4(const vec3<T>& xyz,const T iw) : x(xyz.x), y(xyz.y), z(xyz.z), w(iw) {}
+    explicit vec4(const T ix, const vec3<T>& yzw) : x(ix), y(yzw.x), z(yzw.y), w(yzw.z) {}
+    explicit vec4(const vec2<T>& xy, const vec2<T>& zw) : x(xy.x), y(xy.y), z(zw.x), w(zw.y) {}
+
+    VECTOR_COMMON(vec4, 4)
+};
+
+// additional constructors that omit the last element
+template <typename T> inline vec2<T>::vec2(const vec3<T>& v) : x(v.x), y(v.y) {}
+template <typename T> inline vec2<T>::vec2(const vec4<T>& v) : x(v.x), y(v.y) {}
+template <typename T> inline vec3<T>::vec3(const vec4<T>& v) : x(v.x), y(v.y), z(v.z) {}
+
+#define VEC_QUAT_FUNC_TEMPLATE(CLASS, COUNT) \
+    template <typename T> \
+    inline T dot(const CLASS & u, const CLASS & v) \
+    { \
+        const T *a = u; \
+        const T *b = v; \
+        using namespace detail; \
+        return multiply_accumulate(COUNT, a, b); \
+    } \
+    template <typename T> \
+    inline T length(const CLASS & v) \
+    { \
+        return sqrt(dot(v, v)); \
+    } \
+    template <typename T> inline CLASS normalize(const CLASS & v) \
+    { \
+        return v * rsqrt(dot(v, v)); \
+    } \
+    template <typename T> inline CLASS lerp(const CLASS & u, const CLASS & v, const T x) \
+    { \
+        return u * (T(1) - x) + v * x; \
+    }
+
+VEC_QUAT_FUNC_TEMPLATE(vec2<T>, 2)
+VEC_QUAT_FUNC_TEMPLATE(vec3<T>, 3)
+VEC_QUAT_FUNC_TEMPLATE(vec4<T>, 4)
+VEC_QUAT_FUNC_TEMPLATE(quat<T>, 4)
+
+#define VEC_FUNC_TEMPLATE(CLASS) \
+    template <typename T> inline CLASS reflect(const CLASS & I, const CLASS & N) \
+    { \
+        return I - T(2) * dot(N, I) * N; \
+    } \
+    template <typename T> inline CLASS refract(const CLASS & I, const CLASS & N, T eta) \
+    { \
+        const T d = dot(N, I); \
+        const T k = T(1) - eta * eta * (T(1) - d * d); \
+        if ( k < T(0) ) \
+            return CLASS(T(0)); \
+        else \
+            return eta * I - (eta * d + static_cast<T>(sqrt(k))) * N; \
+    } 
+
+VEC_FUNC_TEMPLATE(vec2<T>)
+VEC_FUNC_TEMPLATE(vec3<T>)
+VEC_FUNC_TEMPLATE(vec4<T>)
+
+template <typename T> inline T lerp(const T & u, const T & v, const T x) 
+{ 
+    return dot(vec2<T>(u, v), vec2<T>((T(1) - x), x));
+}
+
+template <typename T> inline vec3<T> cross(const vec3<T>& u, const vec3<T>& v)
+{
+    return vec3<T>(
+        dot(vec2<T>(u.y, -v.y), vec2<T>(v.z, u.z)),
+        dot(vec2<T>(u.z, -v.z), vec2<T>(v.x, u.x)),
+        dot(vec2<T>(u.x, -v.x), vec2<T>(v.y, u.y)));
+}
+
+
+#define MATRIX_COL4(SRC, C) \
+    vec4<T>(SRC.elem[0][C], SRC.elem[1][C], SRC.elem[2][C], SRC.elem[3][C])
+
+#define MATRIX_ROW4(SRC, R) \
+    vec4<T>(SRC.elem[R][0], SRC.elem[R][1], SRC.elem[R][2], SRC.elem[R][3])
+
+#define MATRIX_COL3(SRC, C) \
+    vec3<T>(SRC.elem[0][C], SRC.elem[1][C], SRC.elem[2][C])
+
+#define MATRIX_ROW3(SRC, R) \
+    vec3<T>(SRC.elem[R][0], SRC.elem[R][1], SRC.elem[R][2])
+
+#define MATRIX_COL2(SRC, C) \
+    vec2<T>(SRC.elem[0][C], SRC.elem[1][C])
+
+#define MATRIX_ROW2(SRC, R) \
+    vec2<T>(SRC.elem[R][0], SRC.elem[R][1])
+
+#define MOP_M_MATRIX_MULTIPLY(CLASS, SIZE) \
+    CLASS & operator *= (const CLASS & rhs) \
+    { \
+        CLASS result; \
+        for (int r = 0; r < SIZE; ++r) \
+        for (int c = 0; c < SIZE; ++c) \
+            result.elem[r][c] = dot( \
+                MATRIX_ROW ## SIZE((*this), r), \
+                MATRIX_COL ## SIZE(rhs, c)); \
+        return (*this) = result; \
+    }
+
+#define MATRIX_CONSTRUCTOR_FROM_T(CLASS, SIZE) \
+    explicit CLASS(const T v) \
+    { \
+        for (int r = 0; r < SIZE; ++r) \
+        for (int c = 0; c < SIZE; ++c) \
+            if (r == c) elem[r][c] = v; \
+            else elem[r][c] = T(0); \
+    }
+
+#define MATRIX_CONSTRUCTOR_FROM_LOWER(CLASS1, CLASS2, SIZE1, SIZE2) \
+    explicit CLASS1(const CLASS2<T>& m) \
+    { \
+        for (int r = 0; r < SIZE1; ++r) \
+        for (int c = 0; c < SIZE1; ++c) \
+            if (r < SIZE2 && c < SIZE2) elem[r][c] = m.elem[r][c]; \
+            else elem[r][c] = r == c ? T(1) : T(0); \
+    }
+
+#define MATRIX_COMMON(CLASS, SIZE) \
+    COMMON_OPERATORS(CLASS, SIZE*SIZE) \
+    MOP_M_MATRIX_MULTIPLY(CLASS, SIZE) \
+    MATRIX_CONSTRUCTOR_FROM_T(CLASS, SIZE) \
+    operator const T* () const { return (const T*) elem; } \
+    operator T* () { return (T*) elem; }
+
+template <typename T> struct mat2;
+template <typename T> struct mat3;
+template <typename T> struct mat4;
+
+template <typename T> 
+struct mat2  {
+    T elem[2][2];
+    
+    mat2() {}
+    
+    explicit mat2(
+        const T m00, const T m01,
+        const T m10, const T m11)
+    {
+        elem[0][0] = m00; elem[0][1] = m01;
+        elem[1][0] = m10; elem[1][1] = m11;
+    }
+
+    explicit mat2(const vec2<T>& v0, const vec2<T>& v1)
+    {
+        elem[0][0] = v0[0];
+        elem[1][0] = v0[1];
+        elem[0][1] = v1[0];
+        elem[1][1] = v1[1];
+    }
+
+    explicit mat2(const mat3<T>& m);
+
+    MATRIX_COMMON(mat2, 2)
+};
+
+template <typename T> 
+struct mat3 {
+    T elem[3][3];
+    
+    mat3() {}
+    
+    explicit mat3(
+        const T m00, const T m01, const T m02,
+        const T m10, const T m11, const T m12,
+        const T m20, const T m21, const T m22)
+    {
+        elem[0][0] = m00; elem[0][1] = m01; elem[0][2] = m02;
+        elem[1][0] = m10; elem[1][1] = m11; elem[1][2] = m12;
+        elem[2][0] = m20; elem[2][1] = m21; elem[2][2] = m22;
+    }
+    
+    explicit mat3(const vec3<T>& v0, const vec3<T>& v1, const vec3<T>& v2)
+    {
+        elem[0][0] = v0[0];
+        elem[1][0] = v0[1];
+        elem[2][0] = v0[2];
+        elem[0][1] = v1[0];
+        elem[1][1] = v1[1];
+        elem[2][1] = v1[2];
+        elem[0][2] = v2[0];
+        elem[1][2] = v2[1];
+        elem[2][2] = v2[2];
+    }
+
+    explicit mat3(const mat4<T>& m);
+
+    MATRIX_CONSTRUCTOR_FROM_LOWER(mat3, mat2, 3, 2)
+    MATRIX_COMMON(mat3, 3)
+};
+
+template <typename T> 
+struct mat4 {
+    T elem[4][4];
+    
+    mat4() {}
+
+    explicit mat4(
+        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)
+    {
+        elem[0][0] = m00; elem[0][1] = m01; elem[0][2] = m02; elem[0][3] = m03;
+        elem[1][0] = m10; elem[1][1] = m11; elem[1][2] = m12; elem[1][3] = m13;
+        elem[2][0] = m20; elem[2][1] = m21; elem[2][2] = m22; elem[2][3] = m23;
+        elem[3][0] = m30; elem[3][1] = m31; elem[3][2] = m32; elem[3][3] = m33;
+    }
+
+    explicit mat4(const vec4<T>& v0, const vec4<T>& v1, const vec4<T>& v2, const vec4<T>& v3)
+    {
+        elem[0][0] = v0[0];
+        elem[1][0] = v0[1];
+        elem[2][0] = v0[2];
+        elem[3][0] = v0[3];
+        elem[0][1] = v1[0];
+        elem[1][1] = v1[1];
+        elem[2][1] = v1[2];
+        elem[3][1] = v1[3];
+        elem[0][2] = v2[0];
+        elem[1][2] = v2[1];
+        elem[2][2] = v2[2];
+        elem[3][2] = v2[3];
+        elem[0][3] = v3[0];
+        elem[1][3] = v3[1];
+        elem[2][3] = v3[2];
+        elem[3][3] = v3[3];
+    }
+
+    MATRIX_CONSTRUCTOR_FROM_LOWER(mat4, mat3, 4, 3)
+    MATRIX_COMMON(mat4, 4)
+};
+
+#define MATRIX_CONSTRUCTOR_FROM_HIGHER(CLASS1, CLASS2, SIZE) \
+    template <typename T> \
+    inline CLASS1<T>::CLASS1(const CLASS2<T>& m) \
+    { \
+        for (int r = 0; r < SIZE; ++r) \
+        for (int c = 0; c < SIZE; ++c) \
+            elem[r][c] = m.elem[r][c]; \
+    }
+
+MATRIX_CONSTRUCTOR_FROM_HIGHER(mat2, mat3, 2)
+MATRIX_CONSTRUCTOR_FROM_HIGHER(mat3, mat4, 3)
+
+#define MAT_FUNC_TEMPLATE(CLASS, SIZE) \
+    template <typename T>  \
+    inline CLASS transpose(const CLASS & m) \
+    { \
+        CLASS result; \
+        for (int r = 0; r < SIZE; ++r) \
+        for (int c = 0; c < SIZE; ++c) \
+            result.elem[r][c] = m.elem[c][r]; \
+        return result; \
+    } \
+    template <typename T>  \
+    inline CLASS identity ## SIZE() \
+    { \
+        CLASS result; \
+        for (int r = 0; r < SIZE; ++r) \
+        for (int c = 0; c < SIZE; ++c) \
+            result.elem[r][c] = r == c ? T(1) : T(0); \
+        return result; \
+    } \
+    template <typename T> \
+    inline T trace(const CLASS & m) \
+    { \
+        T result = T(0); \
+        for (int i = 0; i < SIZE; ++i) \
+            result += m.elem[i][i]; \
+        return result; \
+    }
+
+MAT_FUNC_TEMPLATE(mat2<T>, 2)
+MAT_FUNC_TEMPLATE(mat3<T>, 3)
+MAT_FUNC_TEMPLATE(mat4<T>, 4)
+
+#define MAT_FUNC_MINOR_TEMPLATE(CLASS1, CLASS2, SIZE) \
+    template <typename T>  \
+    inline CLASS2 minor(const CLASS1 & m, int _r = SIZE, int _c = SIZE) { \
+        CLASS2 result; \
+        for (int r = 0; r < SIZE - 1; ++r) \
+        for (int c = 0; c < SIZE - 1; ++c) { \
+            int rs = r >= _r ? 1 : 0; \
+            int cs = c >= _c ? 1 : 0; \
+            result.elem[r][c] = m.elem[r + rs][c + cs]; \
+        } \
+        return result; \
+    }
+
+MAT_FUNC_MINOR_TEMPLATE(mat3<T>, mat2<T>, 3)
+MAT_FUNC_MINOR_TEMPLATE(mat4<T>, mat3<T>, 4)
+
+template <typename T>
+inline T det(const mat2<T>& m)
+{
+    return dot(
+        vec2<T>(m.elem[0][0], -m.elem[0][1]), 
+        vec2<T>(m.elem[1][1], m.elem[1][0]));
+}
+
+template <typename T>
+inline T det(const mat3<T>& m)
+{
+    return dot(cross(MATRIX_COL3(m, 0), MATRIX_COL3(m, 1)), MATRIX_COL3(m, 2));
+}
+
+template <typename T>
+inline T det(const mat4<T>& m)
+{
+    vec4<T> b;
+    for (int i = 0; i < 4; ++i)
+        b[i] = (i & 1 ? -1 : 1) * det(minor(m, 0, i));
+    return dot(MATRIX_ROW4(m, 0), b);
+}
+
+#define MAT_ADJOINT_TEMPLATE(CLASS, SIZE) \
+    template <typename T> \
+    inline CLASS adjoint(const CLASS & m) \
+    { \
+        CLASS result; \
+        for (int r = 0; r < SIZE; ++r) \
+        for (int c = 0; c < SIZE; ++c) \
+            result.elem[r][c] = ((r + c) & 1 ? -1 : 1) * det(minor(m, c, r)); \
+        return result; \
+    }
+
+MAT_ADJOINT_TEMPLATE(mat3<T>, 3)
+MAT_ADJOINT_TEMPLATE(mat4<T>, 4)
+
+template <typename T>
+inline mat2<T> adjoint(const mat2<T> & m)
+{
+    return mat2<T>(
+         m.elem[1][1], -m.elem[0][1],
+        -m.elem[1][0],  m.elem[0][0]
+    );
+}
+
+#define MAT_INVERSE_TEMPLATE(CLASS) \
+    template <typename T> \
+    inline CLASS inverse(const CLASS & m) \
+    { \
+        return adjoint(m) * inv(det(m)); \
+    }
+
+MAT_INVERSE_TEMPLATE(mat2<T>)
+MAT_INVERSE_TEMPLATE(mat3<T>)
+MAT_INVERSE_TEMPLATE(mat4<T>)
+
+#define MAT_VEC_FUNCS_TEMPLATE(MATCLASS, VECCLASS, SIZE) \
+    template <typename T>  \
+    inline VECCLASS operator * (const MATCLASS & m, const VECCLASS & v) \
+    { \
+        VECCLASS result; \
+        for (int i = 0; i < SIZE; ++i) {\
+            result[i] = dot(MATRIX_ROW ## SIZE(m, i), v); \
+        } \
+        return result; \
+    } \
+    template <typename T>  \
+    inline VECCLASS operator * (const VECCLASS & v, const MATCLASS & m) \
+    { \
+        VECCLASS result; \
+        for (int i = 0; i < SIZE; ++i) \
+            result[i] = dot(v, MATRIX_COL ## SIZE(m, i)); \
+        return result; \
+    }
+
+MAT_VEC_FUNCS_TEMPLATE(mat2<T>, vec2<T>, 2)
+MAT_VEC_FUNCS_TEMPLATE(mat3<T>, vec3<T>, 3)
+MAT_VEC_FUNCS_TEMPLATE(mat4<T>, vec4<T>, 4)
+
+// Returns the inverse of a 4x4 matrix. It is assumed that the matrix passed
+// as argument describes a rigid-body transformation.
+template <typename T> 
+inline mat4<T> fast_inverse(const mat4<T>& m)
+{
+    const vec3<T> t = MATRIX_COL3(m, 3);
+    const T tx = -dot(MATRIX_COL3(m, 0), t);
+    const T ty = -dot(MATRIX_COL3(m, 1), t);
+    const T tz = -dot(MATRIX_COL3(m, 2), t);
+
+    return mat4<T>(
+        m.elem[0][0], m.elem[1][0], m.elem[2][0], tx,
+        m.elem[0][1], m.elem[1][1], m.elem[2][1], ty,
+        m.elem[0][2], m.elem[1][2], m.elem[2][2], tz,
+        T(0), T(0), T(0), T(1)
+    );
+}
+
+// Transformations for points and vectors. Potentially faster than a full 
+// matrix * vector multiplication
+
+#define MAT_TRANFORMS_TEMPLATE(MATCLASS, VECCLASS, VECSIZE) \
+    /* computes vec3<T>(m * vec4<T>(v, 0.0)) */ \
+    template <typename T>  \
+    inline VECCLASS transform_vector(const MATCLASS & m, const VECCLASS & v) \
+    { \
+        VECCLASS result; \
+        for (int i = 0; i < VECSIZE; ++i) \
+            result[i] = dot(MATRIX_ROW ## VECSIZE(m, i), v); \
+        return result;\
+    } \
+    /* computes vec3(m * vec4(v, 1.0)) */ \
+    template <typename T>  \
+    inline VECCLASS transform_point(const MATCLASS & m, const VECCLASS & v) \
+    { \
+        /*return transform_vector(m, v) + MATRIX_ROW ## VECSIZE(m, VECSIZE); */\
+        VECCLASS result; \
+        for (int i = 0; i < VECSIZE; ++i) \
+            result[i] = dot(MATRIX_ROW ## VECSIZE(m, i), v) + m.elem[i][VECSIZE]; \
+        return result; \
+    } \
+    /* computes VECCLASS(transpose(m) * vec4<T>(v, 0.0)) */ \
+    template <typename T>  \
+    inline VECCLASS transform_vector_transpose(const MATCLASS & m, const VECCLASS& v) \
+    { \
+        VECCLASS result; \
+        for (int i = 0; i < VECSIZE; ++i) \
+            result[i] = dot(MATRIX_COL ## VECSIZE(m, i), v); \
+        return result; \
+    } \
+    /* computes VECCLASS(transpose(m) * vec4<T>(v, 1.0)) */ \
+    template <typename T>  \
+    inline VECCLASS transform_point_transpose(const MATCLASS & m, const VECCLASS& v) \
+    { \
+        /*return transform_vector_transpose(m, v) + MATRIX_COL ## VECSIZE(m, VECSIZE); */\
+        VECCLASS result; \
+        for (int i = 0; i < VECSIZE; ++i) \
+            result[i] = dot(MATRIX_COL ## VECSIZE(m, i), v) + m.elem[VECSIZE][i]; \
+        return result; \
+    }
+
+MAT_TRANFORMS_TEMPLATE(mat4<T>, vec3<T>, 3)
+MAT_TRANFORMS_TEMPLATE(mat3<T>, vec2<T>, 2)
+
+#define MAT_OUTERPRODUCT_TEMPLATE(MATCLASS, VECCLASS, MATSIZE) \
+    template <typename T> \
+    inline MATCLASS outer_product(const VECCLASS & v1, const VECCLASS & v2) \
+    { \
+        MATCLASS r; \
+        for ( int j = 0; j < MATSIZE; ++j ) \
+        for ( int k = 0; k < MATSIZE; ++k ) \
+            r.elem[j][k] = v1[j] * v2[k]; \
+        return r; \
+    }
+
+MAT_OUTERPRODUCT_TEMPLATE(mat4<T>, vec4<T>, 4)
+MAT_OUTERPRODUCT_TEMPLATE(mat3<T>, vec3<T>, 3)
+MAT_OUTERPRODUCT_TEMPLATE(mat2<T>, vec2<T>, 2)
+
+template <typename T>
+inline mat4<T> translation_matrix(const T x, const T y, const T z)
+{
+    mat4<T> r(T(1));
+    r.elem[0][3] = x;
+    r.elem[1][3] = y;
+    r.elem[2][3] = z;
+    return r;
+}
+
+template <typename T> 
+inline mat4<T> translation_matrix(const vec3<T>& v)
+{
+    return translation_matrix(v.x, v.y, v.z);
+}
+
+template <typename T> 
+inline mat4<T> scaling_matrix(const T x, const T y, const T z)
+{
+    mat4<T> r(T(0));
+    r.elem[0][0] = x;
+    r.elem[1][1] = y;
+    r.elem[2][2] = z;
+    r.elem[3][3] = T(1);
+    return r;
+}
+
+template <typename T>
+inline mat4<T> scaling_matrix(const vec3<T>& v)
+{
+    return scaling_matrix(v.x, v.y, v.z);
+}
+
+template <typename T>
+inline mat4<T> rotation_matrix(const T angle, const vec3<T>& v)
+{
+    const T a = angle * T(M_PI/180) ;
+    const vec3<T> u = normalize(v);
+    
+    const mat3<T> S(
+         T(0), -u[2],  u[1],
+         u[2],  T(0), -u[0],
+        -u[1],  u[0],  T(0) 
+    );
+    
+    const mat3<T> uut = outer_product(u, u);
+    const mat3<T> R = uut + T(cos(a)) * (identity3<T>() - uut) + T(sin(a)) * S;
+    
+    return mat4<T>(R);
+}
+
+
+template <typename T> 
+inline mat4<T> rotation_matrix(const T angle, const T x, const T y, const T z)
+{
+    return rotation_matrix(angle, vec3<T>(x, y, z));
+}
+
+// Constructs a shear-matrix that shears component i by factor with
+// Respect to component j.
+template <typename T> 
+inline mat4<T> shear_matrix(const int i, const int j, const T factor)
+{
+    mat4<T> m = identity4<T>();
+    m.elem[i][j] = factor;
+    return m;
+}
+
+template <typename T> 
+inline mat4<T> euler(const T head, const T pitch, const T roll)
+{
+    return rotation_matrix(roll, T(0), T(0), T(1)) * 
+        rotation_matrix(pitch, T(1), T(0), T(0)) * 
+        rotation_matrix(head, T(0), T(1), T(0));
+}
+
+template <typename T> 
+inline mat4<T> frustum_matrix(const T l, const T r, const T b, const T t, const T n, const T f)
+{
+    return mat4<T>(
+        (2 * n)/(r - l), T(0), (r + l)/(r - l), T(0),
+        T(0), (2 * n)/(t - b), (t + b)/(t - b), T(0),
+        T(0), T(0), -(f + n)/(f - n), -(2 * f * n)/(f - n),
+        T(0), T(0), -T(1), T(0)
+    );
+}
+
+template <typename T>
+inline mat4<T> perspective_matrix(const T fovy, const T aspect, const T zNear, const T zFar)
+{
+    const T dz = zFar - zNear;
+    const T rad = fovy / T(2) * T(M_PI/180);
+    const T s = sin(rad);
+    
+    if ( ( dz == T(0) ) || ( s == T(0) ) || ( aspect == T(0) ) ) {
+        return identity4<T>();
+    }
+    
+    const T cot = cos(rad) / s;
+    
+    mat4<T> m = identity4<T>();
+    m[0]  = cot / aspect;
+    m[5]  = cot;
+    m[10] = -(zFar + zNear) / dz;
+    m[14] = T(-1);
+    m[11] = -2 * zNear * zFar / dz;
+    m[15] = T(0);
+    
+    return m;
+}
+
+template <typename T>
+inline mat4<T> ortho_matrix(const T l, const T r, const T b, const T t, const T n, const T f)
+{
+    return mat4<T>(
+        T(2)/(r - l), T(0), T(0), -(r + l)/(r - l),
+        T(0), T(2)/(t - b), T(0), -(t + b)/(t - b),
+        T(0), T(0), -T(2)/(f - n), -(f + n)/(f - n),
+        T(0), T(0), T(0), T(1)
+    );
+}
+
+template <typename T>
+inline mat4<T> lookat_matrix(const vec3<T>& eye, const vec3<T>& center, const vec3<T>& up) {
+    const vec3<T> forward = normalize(center - eye);
+    const vec3<T> side = normalize(cross(forward, up));
+    
+    const vec3<T> up2 = cross(side, forward);
+    
+    mat4<T> m = identity4<T>();
+    
+    m.elem[0][0] = side[0];
+    m.elem[0][1] = side[1];
+    m.elem[0][2] = side[2];
+    
+    m.elem[1][0] = up2[0];
+    m.elem[1][1] = up2[1];
+    m.elem[1][2] = up2[2];
+    
+    m.elem[2][0] = -forward[0];
+    m.elem[2][1] = -forward[1];
+    m.elem[2][2] = -forward[2];
+    
+    return m * translation_matrix(-eye);
+}
+
+template <typename T> 
+inline mat4<T> picking_matrix(const T x, const T y, const T dx, const T dy, int viewport[4]) {
+    if (dx <= 0 || dy <= 0) { 
+        return identity4<T>();
+    }
+
+    mat4<T> r = translation_matrix((viewport[2] - 2 * (x - viewport[0])) / dx,
+        (viewport[3] - 2 * (y - viewport[1])) / dy,    0);
+    r *= scaling_matrix(viewport[2] / dx, viewport[2] / dy, 1);
+    return r;
+}
+
+// Constructs a shadow matrix. q is the light source and p is the plane.
+template <typename T> inline mat4<T> shadow_matrix(const vec4<T>& q, const vec4<T>& p) {
+    mat4<T> m;
+    
+    m.elem[0][0] =  p.y * q[1] + p.z * q[2] + p.w * q[3];
+    m.elem[0][1] = -p.y * q[0];
+    m.elem[0][2] = -p.z * q[0];
+    m.elem[0][3] = -p.w * q[0];
+    
+    m.elem[1][0] = -p.x * q[1];
+    m.elem[1][1] =  p.x * q[0] + p.z * q[2] + p.w * q[3];
+    m.elem[1][2] = -p.z * q[1];
+    m.elem[1][3] = -p.w * q[1];
+    
+
+    m.elem[2][0] = -p.x * q[2];
+    m.elem[2][1] = -p.y * q[2];
+    m.elem[2][2] =  p.x * q[0] + p.y * q[1] + p.w * q[3];
+    m.elem[2][3] = -p.w * q[2];
+
+    m.elem[3][1] = -p.x * q[3];
+    m.elem[3][2] = -p.y * q[3];
+    m.elem[3][3] = -p.z * q[3];
+    m.elem[3][0] =  p.x * q[0] + p.y * q[1] + p.z * q[2];
+
+    return m;
+}
+
+// Quaternion class
+template <typename T> 
+struct quat {
+    vec3<T>    v;
+    T w;
+
+    quat() {}
+    quat(const vec3<T>& iv, const T iw) : v(iv), w(iw) {}
+    quat(const T vx, const T vy, const T vz, const T iw) : v(vx, vy, vz), w(iw)    {}
+    quat(const vec4<T>& i) : v(i.x, i.y, i.z), w(i.w) {}
+
+    operator const T* () const { return &(v[0]); }
+    operator T* () { return &(v[0]); }    
+
+    quat& operator += (const quat& q) { v += q.v; w += q.w; return *this; }
+    quat& operator -= (const quat& q) { v -= q.v; w -= q.w; return *this; }
+
+    quat& operator *= (const T& s) { v *= s; w *= s; return *this; }
+    quat& operator /= (const T& s) { v /= s; w /= s; return *this; }
+
+    quat& operator *= (const quat& r)
+    {
+        //q1 x q2 = [s1,v1] x [s2,v2] = [(s1*s2 - v1*v2),(s1*v2 + s2*v1 + v1xv2)].
+        quat q;
+        q.v = cross(v, r.v) + r.w * v + w * r.v;
+        q.w = w * r.w - dot(v, r.v);
+        return *this = q;
+    }
+
+    quat& operator /= (const quat& q) { return (*this) *= inverse(q); }
+};
+
+// Quaternion functions
+
+template <typename T> 
+inline quat<T> identityq()
+{
+    return quat<T>(T(0), T(0), T(0), T(1));
+}
+
+template <typename T> 
+inline quat<T> conjugate(const quat<T>& q)
+{
+    return quat<T>(-q.v, q.w);
+}
+
+template <typename T> 
+inline quat<T> inverse(const quat<T>& q)
+{
+    const T l = dot(q, q);
+    if ( l > T(0) ) return conjugate(q) * inv(l);
+    else return identityq<T>();
+}
+
+// quaternion utility functions
+
+// the input quaternion is assumed to be normalized
+template <typename T> 
+inline mat3<T> quat_to_mat3(const quat<T>& q)
+{
+    // const quat<T> q = normalize(qq);
+
+    const T xx = q[0] * q[0];
+    const T xy = q[0] * q[1];
+    const T xz = q[0] * q[2];
+    const T xw = q[0] * q[3];
+    
+    const T yy = q[1] * q[1];
+    const T yz = q[1] * q[2];
+    const T yw = q[1] * q[3];
+    
+    const T zz = q[2] * q[2];
+    const T zw = q[2] * q[3];
+    
+    return mat3<T>(
+        1 - 2*(yy + zz), 2*(xy - zw), 2*(xz + yw),
+        2*(xy + zw), 1 - 2*(xx + zz), 2*(yz - xw),
+        2*(xz - yw), 2*(yz + xw), 1 - 2*(xx + yy)
+    );
+}
+
+// the input quat<T>ernion is assumed to be normalized
+template <typename T> 
+inline mat4<T> quat_to_mat4(const quat<T>& q)
+{
+    // const quat<T> q = normalize(qq);
+
+    return mat4<T>(quat_to_mat3(q));
+}
+
+template <typename T> 
+inline quat<T> mat_to_quat(const mat4<T>& m)
+{
+    const T t = m.elem[0][0] + m.elem[1][1] + m.elem[2][2] + T(1);
+    quat<T> q;
+
+    if ( t > 0 ) {
+        const T s = T(0.5) / sqrt(t);
+        q[3] = T(0.25) * inv(s);
+        q[0] = (m.elem[2][1] - m.elem[1][2]) * s;
+        q[1] = (m.elem[0][2] - m.elem[2][0]) * s;
+        q[2] = (m.elem[1][0] - m.elem[0][1]) * s;
+    } else {
+        if ( m.elem[0][0] > m.elem[1][1] && m.elem[0][0] > m.elem[2][2] ) {
+            const T s = T(2) * sqrt( T(1) + m.elem[0][0] - m.elem[1][1] - m.elem[2][2]);
+            const T invs = inv(s);
+            q[0] = T(0.25) * s;
+            q[1] = (m.elem[0][1] + m.elem[1][0] ) * invs;
+            q[2] = (m.elem[0][2] + m.elem[2][0] ) * invs;
+            q[3] = (m.elem[1][2] - m.elem[2][1] ) * invs;
+        } else if (m.elem[1][1] > m.elem[2][2]) {
+            const T s = T(2) * sqrt( T(1) + m.elem[1][1] - m.elem[0][0] - m.elem[2][2]);
+            const T invs = inv(s);
+            q[0] = (m.elem[0][1] + m.elem[1][0] ) * invs;
+            q[1] = T(0.25) * s;
+            q[2] = (m.elem[1][2] + m.elem[2][1] ) * invs;
+            q[3] = (m.elem[0][2] - m.elem[2][0] ) * invs;
+        } else {
+            const T s = T(2) * sqrt( T(1) + m.elem[2][2] - m.elem[0][0] - m.elem[1][1] );
+            const T invs = inv(s);
+            q[0] = (m.elem[0][2] + m.elem[2][0] ) * invs;
+            q[1] = (m.elem[1][2] + m.elem[2][1] ) * invs;
+            q[2] = T(0.25) * s;
+            q[3] = (m.elem[0][1] - m.elem[1][0] ) * invs;
+        }
+    }
+    
+    return q;
+}
+
+template <typename T> 
+inline quat<T> mat_to_quat(const mat3<T>& m)
+{
+    return mat_to_quat(mat4<T>(m));
+}
+
+// the angle is in radians
+template <typename T> 
+inline quat<T> quat_from_axis_angle(const vec3<T>& axis, const T a)
+{
+    quat<T> r;
+    const T inv2 = inv(T(2));
+    r.v = sin(a * inv2) * normalize(axis);
+    r.w = cos(a * inv2);
+    
+    return r;
+}
+
+// the angle is in radians
+template <typename T> 
+inline quat<T> quat_from_axis_angle(const T x, const T y, const T z, const T angle)
+{
+    return quat_from_axis_angle<T>(vec3<T>(x, y, z), angle);
+}
+
+// the angle is stored in radians
+template <typename T> 
+inline void quat_to_axis_angle(const quat<T>& qq, vec3<T>* axis, T *angle)
+{
+    quat<T> q = normalize(qq);
+    
+    *angle = 2 * acos(q.w);
+    
+    const T s = sin((*angle) * inv(T(2)));
+    if ( s != T(0) )
+        *axis = q.v * inv(s);
+    else 
+        * axis = vec3<T>(T(0), T(0), T(0));
+}
+
+// Spherical linear interpolation
+template <typename T> 
+inline quat<T> slerp(const quat<T>& qq1, const quat<T>& qq2, const T t) 
+{
+    // slerp(q1,q2) = sin((1-t)*a)/sin(a) * q1  +  sin(t*a)/sin(a) * q2
+    const quat<T> q1 = normalize(qq1);
+    const quat<T> q2 = normalize(qq2);
+    
+    const T a = acos(dot(q1, q2));
+    const T s = sin(a);
+    
+    #define EPS T(1e-5)
+
+    if ( !(-EPS <= s && s <= EPS) ) {
+        return sin((T(1)-t)*a)/s * q1  +  sin(t*a)/s * q2;
+    } else {
+        // if the angle is to small use a linear interpolation
+        return lerp(q1, q2, t);
+    }
+
+    #undef EPS
+}
+
+// Sperical quadtratic interpolation using a smooth cubic spline
+// The parameters a and b are the control points.
+template <typename T> 
+inline quat<T> squad(
+    const quat<T>& q0, 
+    const quat<T>& a, 
+    const quat<T>& b, 
+    const quat<T>& q1, 
+    const T t) 
+{
+    return slerp(slerp(q0, q1, t),slerp(a, b, t), 2 * t * (1 - t));
+}
+
+#undef MOP_M_CLASS_TEMPLATE
+#undef MOP_M_TYPE_TEMPLATE
+#undef MOP_COMP_TEMPLATE
+#undef MOP_G_UMINUS_TEMPLATE
+#undef COMMON_OPERATORS
+#undef VECTOR_COMMON
+#undef FOP_G_SOURCE_TEMPLATE
+#undef FOP_G_CLASS_TEMPLATE
+#undef FOP_G_TYPE_TEMPLATE
+#undef VEC_QUAT_FUNC_TEMPLATE
+#undef VEC_FUNC_TEMPLATE
+#undef MATRIX_COL4
+#undef MATRIX_ROW4
+#undef MATRIX_COL3
+#undef MATRIX_ROW3
+#undef MATRIX_COL2
+#undef MATRIX_ROW2
+#undef MOP_M_MATRIX_MULTIPLY
+#undef MATRIX_CONSTRUCTOR_FROM_T
+#undef MATRIX_CONSTRUCTOR_FROM_LOWER
+#undef MATRIX_COMMON
+#undef MATRIX_CONSTRUCTOR_FROM_HIGHER
+#undef MAT_FUNC_TEMPLATE 
+#undef MAT_FUNC_MINOR_TEMPLATE
+#undef MAT_ADJOINT_TEMPLATE
+#undef MAT_INVERSE_TEMPLATE
+#undef MAT_VEC_FUNCS_TEMPLATE
+#undef MAT_TRANFORMS_TEMPLATE
+#undef MAT_OUTERPRODUCT_TEMPLATE
+#undef FREE_MODIFYING_OPERATORS
+#undef FREE_OPERATORS
+
+} // end namespace vmath
+
+#endif
+
+
+