Gurvan PRIEM
/
RaptorControl
An incomplete quadcopter control programme.
Diff: MPU6050/vector_math.h
- Revision:
- 0:9cb9445a11f0
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050/vector_math.h Wed Jul 17 15:58:25 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 + + +