Aloïs Wolff / HK10DOF

mbed implementation of the FreeIMU IMU for HobbyKing's 10DOF board

Dependents:   testHK10DOF

vector_math.h@1:85fcfcb7b137, 2013-07-17 (annotated)

Committer:
pommzorz
Date:
Wed Jul 17 18:53:37 2013 +0000
Revision:
1:85fcfcb7b137
oops forgot one file...

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pommzorz 1:85fcfcb7b137 1 /*
pommzorz 1:85fcfcb7b137 2 Copyright (c) 2007, Markus Trenkwalder
pommzorz 1:85fcfcb7b137 3
pommzorz 1:85fcfcb7b137 4 All rights reserved.
pommzorz 1:85fcfcb7b137 5
pommzorz 1:85fcfcb7b137 6 Redistribution and use in source and binary forms, with or without
pommzorz 1:85fcfcb7b137 7 modification, are permitted provided that the following conditions are met:
pommzorz 1:85fcfcb7b137 8
pommzorz 1:85fcfcb7b137 9 * Redistributions of source code must retain the above copyright notice,
pommzorz 1:85fcfcb7b137 10 this list of conditions and the following disclaimer.
pommzorz 1:85fcfcb7b137 11
pommzorz 1:85fcfcb7b137 12 * Redistributions in binary form must reproduce the above copyright notice,
pommzorz 1:85fcfcb7b137 13 this list of conditions and the following disclaimer in the documentation
pommzorz 1:85fcfcb7b137 14 and/or other materials provided with the distribution.
pommzorz 1:85fcfcb7b137 15
pommzorz 1:85fcfcb7b137 16 * Neither the name of the library's copyright owner nor the names of its
pommzorz 1:85fcfcb7b137 17 contributors may be used to endorse or promote products derived from this
pommzorz 1:85fcfcb7b137 18 software without specific prior written permission.
pommzorz 1:85fcfcb7b137 19
pommzorz 1:85fcfcb7b137 20 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
pommzorz 1:85fcfcb7b137 21 "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
pommzorz 1:85fcfcb7b137 22 LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
pommzorz 1:85fcfcb7b137 23 A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
pommzorz 1:85fcfcb7b137 24 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
pommzorz 1:85fcfcb7b137 25 EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
pommzorz 1:85fcfcb7b137 26 PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
pommzorz 1:85fcfcb7b137 27 PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
pommzorz 1:85fcfcb7b137 28 LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
pommzorz 1:85fcfcb7b137 29 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
pommzorz 1:85fcfcb7b137 30 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
pommzorz 1:85fcfcb7b137 31 */
pommzorz 1:85fcfcb7b137 32
pommzorz 1:85fcfcb7b137 33 #ifndef VECTOR_MATH_H
pommzorz 1:85fcfcb7b137 34 #define VECTOR_MATH_H
pommzorz 1:85fcfcb7b137 35
pommzorz 1:85fcfcb7b137 36 //#include <cmath>
pommzorz 1:85fcfcb7b137 37
pommzorz 1:85fcfcb7b137 38 // "minor" can be defined from GCC and can cause problems
pommzorz 1:85fcfcb7b137 39 #undef minor
pommzorz 1:85fcfcb7b137 40
pommzorz 1:85fcfcb7b137 41 #ifndef M_PI
pommzorz 1:85fcfcb7b137 42 #define M_PI 3.14159265358979323846
pommzorz 1:85fcfcb7b137 43 #endif
pommzorz 1:85fcfcb7b137 44
pommzorz 1:85fcfcb7b137 45 namespace vmath {
pommzorz 1:85fcfcb7b137 46
pommzorz 1:85fcfcb7b137 47 //using std::sin;
pommzorz 1:85fcfcb7b137 48 //using std::cos;
pommzorz 1:85fcfcb7b137 49 //using std::acos;
pommzorz 1:85fcfcb7b137 50 //using std::sqrt;
pommzorz 1:85fcfcb7b137 51
pommzorz 1:85fcfcb7b137 52 template <typename T>
pommzorz 1:85fcfcb7b137 53 inline T rsqrt(T x)
pommzorz 1:85fcfcb7b137 54 {
pommzorz 1:85fcfcb7b137 55 return T(1) / sqrt(x);
pommzorz 1:85fcfcb7b137 56 }
pommzorz 1:85fcfcb7b137 57
pommzorz 1:85fcfcb7b137 58 template <typename T>
pommzorz 1:85fcfcb7b137 59 inline T inv(T x)
pommzorz 1:85fcfcb7b137 60 {
pommzorz 1:85fcfcb7b137 61 return T(1) / x;
pommzorz 1:85fcfcb7b137 62 }
pommzorz 1:85fcfcb7b137 63
pommzorz 1:85fcfcb7b137 64 namespace detail {
pommzorz 1:85fcfcb7b137 65 // This function is used heavily in this library. Here is a generic
pommzorz 1:85fcfcb7b137 66 // implementation for it. If you can provide a faster one for your specific
pommzorz 1:85fcfcb7b137 67 // types this can speed up things considerably.
pommzorz 1:85fcfcb7b137 68 template <typename T>
pommzorz 1:85fcfcb7b137 69 inline T multiply_accumulate(int count, const T *a, const T *b)
pommzorz 1:85fcfcb7b137 70 {
pommzorz 1:85fcfcb7b137 71 T result = T(0);
pommzorz 1:85fcfcb7b137 72 for (int i = 0; i < count; ++i)
pommzorz 1:85fcfcb7b137 73 result += a[i] * b[i];
pommzorz 1:85fcfcb7b137 74 return result;
pommzorz 1:85fcfcb7b137 75 }
pommzorz 1:85fcfcb7b137 76 }
pommzorz 1:85fcfcb7b137 77
pommzorz 1:85fcfcb7b137 78 #define MOP_M_CLASS_TEMPLATE(CLASS, OP, COUNT) \
pommzorz 1:85fcfcb7b137 79 CLASS & operator OP (const CLASS& rhs) \
pommzorz 1:85fcfcb7b137 80 { \
pommzorz 1:85fcfcb7b137 81 for (int i = 0; i < (COUNT); ++i ) \
pommzorz 1:85fcfcb7b137 82 (*this)[i] OP rhs[i]; \
pommzorz 1:85fcfcb7b137 83 return *this; \
pommzorz 1:85fcfcb7b137 84 }
pommzorz 1:85fcfcb7b137 85
pommzorz 1:85fcfcb7b137 86 #define MOP_M_TYPE_TEMPLATE(CLASS, OP, COUNT) \
pommzorz 1:85fcfcb7b137 87 CLASS & operator OP (const T & rhs) \
pommzorz 1:85fcfcb7b137 88 { \
pommzorz 1:85fcfcb7b137 89 for (int i = 0; i < (COUNT); ++i ) \
pommzorz 1:85fcfcb7b137 90 (*this)[i] OP rhs; \
pommzorz 1:85fcfcb7b137 91 return *this; \
pommzorz 1:85fcfcb7b137 92 }
pommzorz 1:85fcfcb7b137 93
pommzorz 1:85fcfcb7b137 94 #define MOP_COMP_TEMPLATE(CLASS, COUNT) \
pommzorz 1:85fcfcb7b137 95 bool operator == (const CLASS & rhs) \
pommzorz 1:85fcfcb7b137 96 { \
pommzorz 1:85fcfcb7b137 97 bool result = true; \
pommzorz 1:85fcfcb7b137 98 for (int i = 0; i < (COUNT); ++i) \
pommzorz 1:85fcfcb7b137 99 result = result && (*this)[i] == rhs[i]; \
pommzorz 1:85fcfcb7b137 100 return result; \
pommzorz 1:85fcfcb7b137 101 } \
pommzorz 1:85fcfcb7b137 102 bool operator != (const CLASS & rhs) \
pommzorz 1:85fcfcb7b137 103 { return !((*this) == rhs); }
pommzorz 1:85fcfcb7b137 104
pommzorz 1:85fcfcb7b137 105 #define MOP_G_UMINUS_TEMPLATE(CLASS, COUNT) \
pommzorz 1:85fcfcb7b137 106 CLASS operator - () const \
pommzorz 1:85fcfcb7b137 107 { \
pommzorz 1:85fcfcb7b137 108 CLASS result; \
pommzorz 1:85fcfcb7b137 109 for (int i = 0; i < (COUNT); ++i) \
pommzorz 1:85fcfcb7b137 110 result[i] = -(*this)[i]; \
pommzorz 1:85fcfcb7b137 111 return result; \
pommzorz 1:85fcfcb7b137 112 }
pommzorz 1:85fcfcb7b137 113
pommzorz 1:85fcfcb7b137 114 #define COMMON_OPERATORS(CLASS, COUNT) \
pommzorz 1:85fcfcb7b137 115 MOP_M_CLASS_TEMPLATE(CLASS, +=, COUNT) \
pommzorz 1:85fcfcb7b137 116 MOP_M_CLASS_TEMPLATE(CLASS, -=, COUNT) \
pommzorz 1:85fcfcb7b137 117 /*no *= as this is not the same for vectors and matrices */ \
pommzorz 1:85fcfcb7b137 118 MOP_M_CLASS_TEMPLATE(CLASS, /=, COUNT) \
pommzorz 1:85fcfcb7b137 119 MOP_M_TYPE_TEMPLATE(CLASS, +=, COUNT) \
pommzorz 1:85fcfcb7b137 120 MOP_M_TYPE_TEMPLATE(CLASS, -=, COUNT) \
pommzorz 1:85fcfcb7b137 121 MOP_M_TYPE_TEMPLATE(CLASS, *=, COUNT) \
pommzorz 1:85fcfcb7b137 122 MOP_M_TYPE_TEMPLATE(CLASS, /=, COUNT) \
pommzorz 1:85fcfcb7b137 123 MOP_G_UMINUS_TEMPLATE(CLASS, COUNT) \
pommzorz 1:85fcfcb7b137 124 MOP_COMP_TEMPLATE(CLASS, COUNT)
pommzorz 1:85fcfcb7b137 125
pommzorz 1:85fcfcb7b137 126 #define VECTOR_COMMON(CLASS, COUNT) \
pommzorz 1:85fcfcb7b137 127 COMMON_OPERATORS(CLASS, COUNT) \
pommzorz 1:85fcfcb7b137 128 MOP_M_CLASS_TEMPLATE(CLASS, *=, COUNT) \
pommzorz 1:85fcfcb7b137 129 operator const T* () const { return &x; } \
pommzorz 1:85fcfcb7b137 130 operator T* () { return &x; }
pommzorz 1:85fcfcb7b137 131
pommzorz 1:85fcfcb7b137 132 #define FOP_G_SOURCE_TEMPLATE(OP, CLASS) \
pommzorz 1:85fcfcb7b137 133 { CLASS<T> r = lhs; r OP##= rhs; return r; }
pommzorz 1:85fcfcb7b137 134
pommzorz 1:85fcfcb7b137 135 #define FOP_G_CLASS_TEMPLATE(OP, CLASS) \
pommzorz 1:85fcfcb7b137 136 template <typename T> \
pommzorz 1:85fcfcb7b137 137 inline CLASS<T> operator OP (const CLASS<T> &lhs, const CLASS<T> &rhs) \
pommzorz 1:85fcfcb7b137 138 FOP_G_SOURCE_TEMPLATE(OP, CLASS)
pommzorz 1:85fcfcb7b137 139
pommzorz 1:85fcfcb7b137 140 #define FOP_G_TYPE_TEMPLATE(OP, CLASS) \
pommzorz 1:85fcfcb7b137 141 template <typename T> \
pommzorz 1:85fcfcb7b137 142 inline CLASS<T> operator OP (const CLASS<T> &lhs, const T &rhs) \
pommzorz 1:85fcfcb7b137 143 FOP_G_SOURCE_TEMPLATE(OP, CLASS)
pommzorz 1:85fcfcb7b137 144
pommzorz 1:85fcfcb7b137 145 // forward declarations
pommzorz 1:85fcfcb7b137 146 template <typename T> struct vec2;
pommzorz 1:85fcfcb7b137 147 template <typename T> struct vec3;
pommzorz 1:85fcfcb7b137 148 template <typename T> struct vec4;
pommzorz 1:85fcfcb7b137 149 template <typename T> struct mat2;
pommzorz 1:85fcfcb7b137 150 template <typename T> struct mat3;
pommzorz 1:85fcfcb7b137 151 template <typename T> struct mat4;
pommzorz 1:85fcfcb7b137 152 template <typename T> struct quat;
pommzorz 1:85fcfcb7b137 153
pommzorz 1:85fcfcb7b137 154 #define FREE_MODIFYING_OPERATORS(CLASS) \
pommzorz 1:85fcfcb7b137 155 FOP_G_CLASS_TEMPLATE(+, CLASS) \
pommzorz 1:85fcfcb7b137 156 FOP_G_CLASS_TEMPLATE(-, CLASS) \
pommzorz 1:85fcfcb7b137 157 FOP_G_CLASS_TEMPLATE(*, CLASS) \
pommzorz 1:85fcfcb7b137 158 FOP_G_CLASS_TEMPLATE(/, CLASS) \
pommzorz 1:85fcfcb7b137 159 FOP_G_TYPE_TEMPLATE(+, CLASS) \
pommzorz 1:85fcfcb7b137 160 FOP_G_TYPE_TEMPLATE(-, CLASS) \
pommzorz 1:85fcfcb7b137 161 FOP_G_TYPE_TEMPLATE(*, CLASS) \
pommzorz 1:85fcfcb7b137 162 FOP_G_TYPE_TEMPLATE(/, CLASS)
pommzorz 1:85fcfcb7b137 163
pommzorz 1:85fcfcb7b137 164 FREE_MODIFYING_OPERATORS(vec2)
pommzorz 1:85fcfcb7b137 165 FREE_MODIFYING_OPERATORS(vec3)
pommzorz 1:85fcfcb7b137 166 FREE_MODIFYING_OPERATORS(vec4)
pommzorz 1:85fcfcb7b137 167 FREE_MODIFYING_OPERATORS(mat2)
pommzorz 1:85fcfcb7b137 168 FREE_MODIFYING_OPERATORS(mat3)
pommzorz 1:85fcfcb7b137 169 FREE_MODIFYING_OPERATORS(mat4)
pommzorz 1:85fcfcb7b137 170 FREE_MODIFYING_OPERATORS(quat)
pommzorz 1:85fcfcb7b137 171
pommzorz 1:85fcfcb7b137 172 #define FREE_OPERATORS(CLASS) \
pommzorz 1:85fcfcb7b137 173 template <typename T> \
pommzorz 1:85fcfcb7b137 174 inline CLASS<T> operator + (const T& a, const CLASS<T>& b) \
pommzorz 1:85fcfcb7b137 175 { CLASS<T> r = b; r += a; return r; } \
pommzorz 1:85fcfcb7b137 176 \
pommzorz 1:85fcfcb7b137 177 template <typename T> \
pommzorz 1:85fcfcb7b137 178 inline CLASS<T> operator * (const T& a, const CLASS<T>& b) \
pommzorz 1:85fcfcb7b137 179 { CLASS<T> r = b; r *= a; return r; } \
pommzorz 1:85fcfcb7b137 180 \
pommzorz 1:85fcfcb7b137 181 template <typename T> \
pommzorz 1:85fcfcb7b137 182 inline CLASS<T> operator - (const T& a, const CLASS<T>& b) \
pommzorz 1:85fcfcb7b137 183 { return -b + a; } \
pommzorz 1:85fcfcb7b137 184 \
pommzorz 1:85fcfcb7b137 185 template <typename T> \
pommzorz 1:85fcfcb7b137 186 inline CLASS<T> operator / (const T& a, const CLASS<T>& b) \
pommzorz 1:85fcfcb7b137 187 { CLASS<T> r(a); r /= b; return r; }
pommzorz 1:85fcfcb7b137 188
pommzorz 1:85fcfcb7b137 189 FREE_OPERATORS(vec2)
pommzorz 1:85fcfcb7b137 190 FREE_OPERATORS(vec3)
pommzorz 1:85fcfcb7b137 191 FREE_OPERATORS(vec4)
pommzorz 1:85fcfcb7b137 192 FREE_OPERATORS(mat2)
pommzorz 1:85fcfcb7b137 193 FREE_OPERATORS(mat3)
pommzorz 1:85fcfcb7b137 194 FREE_OPERATORS(mat4)
pommzorz 1:85fcfcb7b137 195 FREE_OPERATORS(quat)
pommzorz 1:85fcfcb7b137 196
pommzorz 1:85fcfcb7b137 197 template <typename T>
pommzorz 1:85fcfcb7b137 198 struct vec2 {
pommzorz 1:85fcfcb7b137 199 T x, y;
pommzorz 1:85fcfcb7b137 200
pommzorz 1:85fcfcb7b137 201 vec2() {};
pommzorz 1:85fcfcb7b137 202 explicit vec2(const T i) : x(i), y(i) {}
pommzorz 1:85fcfcb7b137 203 explicit vec2(const T ix, const T iy) : x(ix), y(iy) {}
pommzorz 1:85fcfcb7b137 204 explicit vec2(const vec3<T>& v);
pommzorz 1:85fcfcb7b137 205 explicit vec2(const vec4<T>& v);
pommzorz 1:85fcfcb7b137 206
pommzorz 1:85fcfcb7b137 207 VECTOR_COMMON(vec2, 2)
pommzorz 1:85fcfcb7b137 208 };
pommzorz 1:85fcfcb7b137 209
pommzorz 1:85fcfcb7b137 210 template <typename T>
pommzorz 1:85fcfcb7b137 211 struct vec3 {
pommzorz 1:85fcfcb7b137 212 T x, y, z;
pommzorz 1:85fcfcb7b137 213
pommzorz 1:85fcfcb7b137 214 vec3() {};
pommzorz 1:85fcfcb7b137 215 explicit vec3(const T i) : x(i), y(i), z(i) {}
pommzorz 1:85fcfcb7b137 216 explicit vec3(const T ix, const T iy, const T iz) : x(ix), y(iy), z(iz) {}
pommzorz 1:85fcfcb7b137 217 explicit vec3(const vec2<T>& xy, const T iz) : x(xy.x), y(xy.y), z(iz) {}
pommzorz 1:85fcfcb7b137 218 explicit vec3(const T ix, const vec2<T>& yz) : x(ix), y(yz.y), z(yz.z) {}
pommzorz 1:85fcfcb7b137 219 explicit vec3(const vec4<T>& v);
pommzorz 1:85fcfcb7b137 220
pommzorz 1:85fcfcb7b137 221 VECTOR_COMMON(vec3, 3)
pommzorz 1:85fcfcb7b137 222 };
pommzorz 1:85fcfcb7b137 223
pommzorz 1:85fcfcb7b137 224 template <typename T>
pommzorz 1:85fcfcb7b137 225 struct vec4 {
pommzorz 1:85fcfcb7b137 226 T x, y, z, w;
pommzorz 1:85fcfcb7b137 227
pommzorz 1:85fcfcb7b137 228 vec4() {};
pommzorz 1:85fcfcb7b137 229 explicit vec4(const T i) : x(i), y(i), z(i), w(i) {}
pommzorz 1:85fcfcb7b137 230 explicit vec4(const T ix, const T iy, const T iz, const T iw) : x(ix), y(iy), z(iz), w(iw) {}
pommzorz 1:85fcfcb7b137 231 explicit vec4(const vec3<T>& xyz,const T iw) : x(xyz.x), y(xyz.y), z(xyz.z), w(iw) {}
pommzorz 1:85fcfcb7b137 232 explicit vec4(const T ix, const vec3<T>& yzw) : x(ix), y(yzw.x), z(yzw.y), w(yzw.z) {}
pommzorz 1:85fcfcb7b137 233 explicit vec4(const vec2<T>& xy, const vec2<T>& zw) : x(xy.x), y(xy.y), z(zw.x), w(zw.y) {}
pommzorz 1:85fcfcb7b137 234
pommzorz 1:85fcfcb7b137 235 VECTOR_COMMON(vec4, 4)
pommzorz 1:85fcfcb7b137 236 };
pommzorz 1:85fcfcb7b137 237
pommzorz 1:85fcfcb7b137 238 // additional constructors that omit the last element
pommzorz 1:85fcfcb7b137 239 template <typename T> inline vec2<T>::vec2(const vec3<T>& v) : x(v.x), y(v.y) {}
pommzorz 1:85fcfcb7b137 240 template <typename T> inline vec2<T>::vec2(const vec4<T>& v) : x(v.x), y(v.y) {}
pommzorz 1:85fcfcb7b137 241 template <typename T> inline vec3<T>::vec3(const vec4<T>& v) : x(v.x), y(v.y), z(v.z) {}
pommzorz 1:85fcfcb7b137 242
pommzorz 1:85fcfcb7b137 243 #define VEC_QUAT_FUNC_TEMPLATE(CLASS, COUNT) \
pommzorz 1:85fcfcb7b137 244 template <typename T> \
pommzorz 1:85fcfcb7b137 245 inline T dot(const CLASS & u, const CLASS & v) \
pommzorz 1:85fcfcb7b137 246 { \
pommzorz 1:85fcfcb7b137 247 const T *a = u; \
pommzorz 1:85fcfcb7b137 248 const T *b = v; \
pommzorz 1:85fcfcb7b137 249 using namespace detail; \
pommzorz 1:85fcfcb7b137 250 return multiply_accumulate(COUNT, a, b); \
pommzorz 1:85fcfcb7b137 251 } \
pommzorz 1:85fcfcb7b137 252 template <typename T> \
pommzorz 1:85fcfcb7b137 253 inline T length(const CLASS & v) \
pommzorz 1:85fcfcb7b137 254 { \
pommzorz 1:85fcfcb7b137 255 return sqrt(dot(v, v)); \
pommzorz 1:85fcfcb7b137 256 } \
pommzorz 1:85fcfcb7b137 257 template <typename T> inline CLASS normalize(const CLASS & v) \
pommzorz 1:85fcfcb7b137 258 { \
pommzorz 1:85fcfcb7b137 259 return v * rsqrt(dot(v, v)); \
pommzorz 1:85fcfcb7b137 260 } \
pommzorz 1:85fcfcb7b137 261 template <typename T> inline CLASS lerp(const CLASS & u, const CLASS & v, const T x) \
pommzorz 1:85fcfcb7b137 262 { \
pommzorz 1:85fcfcb7b137 263 return u * (T(1) - x) + v * x; \
pommzorz 1:85fcfcb7b137 264 }
pommzorz 1:85fcfcb7b137 265
pommzorz 1:85fcfcb7b137 266 VEC_QUAT_FUNC_TEMPLATE(vec2<T>, 2)
pommzorz 1:85fcfcb7b137 267 VEC_QUAT_FUNC_TEMPLATE(vec3<T>, 3)
pommzorz 1:85fcfcb7b137 268 VEC_QUAT_FUNC_TEMPLATE(vec4<T>, 4)
pommzorz 1:85fcfcb7b137 269 VEC_QUAT_FUNC_TEMPLATE(quat<T>, 4)
pommzorz 1:85fcfcb7b137 270
pommzorz 1:85fcfcb7b137 271 #define VEC_FUNC_TEMPLATE(CLASS) \
pommzorz 1:85fcfcb7b137 272 template <typename T> inline CLASS reflect(const CLASS & I, const CLASS & N) \
pommzorz 1:85fcfcb7b137 273 { \
pommzorz 1:85fcfcb7b137 274 return I - T(2) * dot(N, I) * N; \
pommzorz 1:85fcfcb7b137 275 } \
pommzorz 1:85fcfcb7b137 276 template <typename T> inline CLASS refract(const CLASS & I, const CLASS & N, T eta) \
pommzorz 1:85fcfcb7b137 277 { \
pommzorz 1:85fcfcb7b137 278 const T d = dot(N, I); \
pommzorz 1:85fcfcb7b137 279 const T k = T(1) - eta * eta * (T(1) - d * d); \
pommzorz 1:85fcfcb7b137 280 if ( k < T(0) ) \
pommzorz 1:85fcfcb7b137 281 return CLASS(T(0)); \
pommzorz 1:85fcfcb7b137 282 else \
pommzorz 1:85fcfcb7b137 283 return eta * I - (eta * d + static_cast<T>(sqrt(k))) * N; \
pommzorz 1:85fcfcb7b137 284 }
pommzorz 1:85fcfcb7b137 285
pommzorz 1:85fcfcb7b137 286 VEC_FUNC_TEMPLATE(vec2<T>)
pommzorz 1:85fcfcb7b137 287 VEC_FUNC_TEMPLATE(vec3<T>)
pommzorz 1:85fcfcb7b137 288 VEC_FUNC_TEMPLATE(vec4<T>)
pommzorz 1:85fcfcb7b137 289
pommzorz 1:85fcfcb7b137 290 template <typename T> inline T lerp(const T & u, const T & v, const T x)
pommzorz 1:85fcfcb7b137 291 {
pommzorz 1:85fcfcb7b137 292 return dot(vec2<T>(u, v), vec2<T>((T(1) - x), x));
pommzorz 1:85fcfcb7b137 293 }
pommzorz 1:85fcfcb7b137 294
pommzorz 1:85fcfcb7b137 295 template <typename T> inline vec3<T> cross(const vec3<T>& u, const vec3<T>& v)
pommzorz 1:85fcfcb7b137 296 {
pommzorz 1:85fcfcb7b137 297 return vec3<T>(
pommzorz 1:85fcfcb7b137 298 dot(vec2<T>(u.y, -v.y), vec2<T>(v.z, u.z)),
pommzorz 1:85fcfcb7b137 299 dot(vec2<T>(u.z, -v.z), vec2<T>(v.x, u.x)),
pommzorz 1:85fcfcb7b137 300 dot(vec2<T>(u.x, -v.x), vec2<T>(v.y, u.y)));
pommzorz 1:85fcfcb7b137 301 }
pommzorz 1:85fcfcb7b137 302
pommzorz 1:85fcfcb7b137 303
pommzorz 1:85fcfcb7b137 304 #define MATRIX_COL4(SRC, C) \
pommzorz 1:85fcfcb7b137 305 vec4<T>(SRC.elem[0][C], SRC.elem[1][C], SRC.elem[2][C], SRC.elem[3][C])
pommzorz 1:85fcfcb7b137 306
pommzorz 1:85fcfcb7b137 307 #define MATRIX_ROW4(SRC, R) \
pommzorz 1:85fcfcb7b137 308 vec4<T>(SRC.elem[R][0], SRC.elem[R][1], SRC.elem[R][2], SRC.elem[R][3])
pommzorz 1:85fcfcb7b137 309
pommzorz 1:85fcfcb7b137 310 #define MATRIX_COL3(SRC, C) \
pommzorz 1:85fcfcb7b137 311 vec3<T>(SRC.elem[0][C], SRC.elem[1][C], SRC.elem[2][C])
pommzorz 1:85fcfcb7b137 312
pommzorz 1:85fcfcb7b137 313 #define MATRIX_ROW3(SRC, R) \
pommzorz 1:85fcfcb7b137 314 vec3<T>(SRC.elem[R][0], SRC.elem[R][1], SRC.elem[R][2])
pommzorz 1:85fcfcb7b137 315
pommzorz 1:85fcfcb7b137 316 #define MATRIX_COL2(SRC, C) \
pommzorz 1:85fcfcb7b137 317 vec2<T>(SRC.elem[0][C], SRC.elem[1][C])
pommzorz 1:85fcfcb7b137 318
pommzorz 1:85fcfcb7b137 319 #define MATRIX_ROW2(SRC, R) \
pommzorz 1:85fcfcb7b137 320 vec2<T>(SRC.elem[R][0], SRC.elem[R][1])
pommzorz 1:85fcfcb7b137 321
pommzorz 1:85fcfcb7b137 322 #define MOP_M_MATRIX_MULTIPLY(CLASS, SIZE) \
pommzorz 1:85fcfcb7b137 323 CLASS & operator *= (const CLASS & rhs) \
pommzorz 1:85fcfcb7b137 324 { \
pommzorz 1:85fcfcb7b137 325 CLASS result; \
pommzorz 1:85fcfcb7b137 326 for (int r = 0; r < SIZE; ++r) \
pommzorz 1:85fcfcb7b137 327 for (int c = 0; c < SIZE; ++c) \
pommzorz 1:85fcfcb7b137 328 result.elem[r][c] = dot( \
pommzorz 1:85fcfcb7b137 329 MATRIX_ROW ## SIZE((*this), r), \
pommzorz 1:85fcfcb7b137 330 MATRIX_COL ## SIZE(rhs, c)); \
pommzorz 1:85fcfcb7b137 331 return (*this) = result; \
pommzorz 1:85fcfcb7b137 332 }
pommzorz 1:85fcfcb7b137 333
pommzorz 1:85fcfcb7b137 334 #define MATRIX_CONSTRUCTOR_FROM_T(CLASS, SIZE) \
pommzorz 1:85fcfcb7b137 335 explicit CLASS(const T v) \
pommzorz 1:85fcfcb7b137 336 { \
pommzorz 1:85fcfcb7b137 337 for (int r = 0; r < SIZE; ++r) \
pommzorz 1:85fcfcb7b137 338 for (int c = 0; c < SIZE; ++c) \
pommzorz 1:85fcfcb7b137 339 if (r == c) elem[r][c] = v; \
pommzorz 1:85fcfcb7b137 340 else elem[r][c] = T(0); \
pommzorz 1:85fcfcb7b137 341 }
pommzorz 1:85fcfcb7b137 342
pommzorz 1:85fcfcb7b137 343 #define MATRIX_CONSTRUCTOR_FROM_LOWER(CLASS1, CLASS2, SIZE1, SIZE2) \
pommzorz 1:85fcfcb7b137 344 explicit CLASS1(const CLASS2<T>& m) \
pommzorz 1:85fcfcb7b137 345 { \
pommzorz 1:85fcfcb7b137 346 for (int r = 0; r < SIZE1; ++r) \
pommzorz 1:85fcfcb7b137 347 for (int c = 0; c < SIZE1; ++c) \
pommzorz 1:85fcfcb7b137 348 if (r < SIZE2 && c < SIZE2) elem[r][c] = m.elem[r][c]; \
pommzorz 1:85fcfcb7b137 349 else elem[r][c] = r == c ? T(1) : T(0); \
pommzorz 1:85fcfcb7b137 350 }
pommzorz 1:85fcfcb7b137 351
pommzorz 1:85fcfcb7b137 352 #define MATRIX_COMMON(CLASS, SIZE) \
pommzorz 1:85fcfcb7b137 353 COMMON_OPERATORS(CLASS, SIZE*SIZE) \
pommzorz 1:85fcfcb7b137 354 MOP_M_MATRIX_MULTIPLY(CLASS, SIZE) \
pommzorz 1:85fcfcb7b137 355 MATRIX_CONSTRUCTOR_FROM_T(CLASS, SIZE) \
pommzorz 1:85fcfcb7b137 356 operator const T* () const { return (const T*) elem; } \
pommzorz 1:85fcfcb7b137 357 operator T* () { return (T*) elem; }
pommzorz 1:85fcfcb7b137 358
pommzorz 1:85fcfcb7b137 359 template <typename T> struct mat2;
pommzorz 1:85fcfcb7b137 360 template <typename T> struct mat3;
pommzorz 1:85fcfcb7b137 361 template <typename T> struct mat4;
pommzorz 1:85fcfcb7b137 362
pommzorz 1:85fcfcb7b137 363 template <typename T>
pommzorz 1:85fcfcb7b137 364 struct mat2 {
pommzorz 1:85fcfcb7b137 365 T elem[2][2];
pommzorz 1:85fcfcb7b137 366
pommzorz 1:85fcfcb7b137 367 mat2() {}
pommzorz 1:85fcfcb7b137 368
pommzorz 1:85fcfcb7b137 369 explicit mat2(
pommzorz 1:85fcfcb7b137 370 const T m00, const T m01,
pommzorz 1:85fcfcb7b137 371 const T m10, const T m11)
pommzorz 1:85fcfcb7b137 372 {
pommzorz 1:85fcfcb7b137 373 elem[0][0] = m00; elem[0][1] = m01;
pommzorz 1:85fcfcb7b137 374 elem[1][0] = m10; elem[1][1] = m11;
pommzorz 1:85fcfcb7b137 375 }
pommzorz 1:85fcfcb7b137 376
pommzorz 1:85fcfcb7b137 377 explicit mat2(const vec2<T>& v0, const vec2<T>& v1)
pommzorz 1:85fcfcb7b137 378 {
pommzorz 1:85fcfcb7b137 379 elem[0][0] = v0[0];
pommzorz 1:85fcfcb7b137 380 elem[1][0] = v0[1];
pommzorz 1:85fcfcb7b137 381 elem[0][1] = v1[0];
pommzorz 1:85fcfcb7b137 382 elem[1][1] = v1[1];
pommzorz 1:85fcfcb7b137 383 }
pommzorz 1:85fcfcb7b137 384
pommzorz 1:85fcfcb7b137 385 explicit mat2(const mat3<T>& m);
pommzorz 1:85fcfcb7b137 386
pommzorz 1:85fcfcb7b137 387 MATRIX_COMMON(mat2, 2)
pommzorz 1:85fcfcb7b137 388 };
pommzorz 1:85fcfcb7b137 389
pommzorz 1:85fcfcb7b137 390 template <typename T>
pommzorz 1:85fcfcb7b137 391 struct mat3 {
pommzorz 1:85fcfcb7b137 392 T elem[3][3];
pommzorz 1:85fcfcb7b137 393
pommzorz 1:85fcfcb7b137 394 mat3() {}
pommzorz 1:85fcfcb7b137 395
pommzorz 1:85fcfcb7b137 396 explicit mat3(
pommzorz 1:85fcfcb7b137 397 const T m00, const T m01, const T m02,
pommzorz 1:85fcfcb7b137 398 const T m10, const T m11, const T m12,
pommzorz 1:85fcfcb7b137 399 const T m20, const T m21, const T m22)
pommzorz 1:85fcfcb7b137 400 {
pommzorz 1:85fcfcb7b137 401 elem[0][0] = m00; elem[0][1] = m01; elem[0][2] = m02;
pommzorz 1:85fcfcb7b137 402 elem[1][0] = m10; elem[1][1] = m11; elem[1][2] = m12;
pommzorz 1:85fcfcb7b137 403 elem[2][0] = m20; elem[2][1] = m21; elem[2][2] = m22;
pommzorz 1:85fcfcb7b137 404 }
pommzorz 1:85fcfcb7b137 405
pommzorz 1:85fcfcb7b137 406 explicit mat3(const vec3<T>& v0, const vec3<T>& v1, const vec3<T>& v2)
pommzorz 1:85fcfcb7b137 407 {
pommzorz 1:85fcfcb7b137 408 elem[0][0] = v0[0];
pommzorz 1:85fcfcb7b137 409 elem[1][0] = v0[1];
pommzorz 1:85fcfcb7b137 410 elem[2][0] = v0[2];
pommzorz 1:85fcfcb7b137 411 elem[0][1] = v1[0];
pommzorz 1:85fcfcb7b137 412 elem[1][1] = v1[1];
pommzorz 1:85fcfcb7b137 413 elem[2][1] = v1[2];
pommzorz 1:85fcfcb7b137 414 elem[0][2] = v2[0];
pommzorz 1:85fcfcb7b137 415 elem[1][2] = v2[1];
pommzorz 1:85fcfcb7b137 416 elem[2][2] = v2[2];
pommzorz 1:85fcfcb7b137 417 }
pommzorz 1:85fcfcb7b137 418
pommzorz 1:85fcfcb7b137 419 explicit mat3(const mat4<T>& m);
pommzorz 1:85fcfcb7b137 420
pommzorz 1:85fcfcb7b137 421 MATRIX_CONSTRUCTOR_FROM_LOWER(mat3, mat2, 3, 2)
pommzorz 1:85fcfcb7b137 422 MATRIX_COMMON(mat3, 3)
pommzorz 1:85fcfcb7b137 423 };
pommzorz 1:85fcfcb7b137 424
pommzorz 1:85fcfcb7b137 425 template <typename T>
pommzorz 1:85fcfcb7b137 426 struct mat4 {
pommzorz 1:85fcfcb7b137 427 T elem[4][4];
pommzorz 1:85fcfcb7b137 428
pommzorz 1:85fcfcb7b137 429 mat4() {}
pommzorz 1:85fcfcb7b137 430
pommzorz 1:85fcfcb7b137 431 explicit mat4(
pommzorz 1:85fcfcb7b137 432 const T m00, const T m01, const T m02, const T m03,
pommzorz 1:85fcfcb7b137 433 const T m10, const T m11, const T m12, const T m13,
pommzorz 1:85fcfcb7b137 434 const T m20, const T m21, const T m22, const T m23,
pommzorz 1:85fcfcb7b137 435 const T m30, const T m31, const T m32, const T m33)
pommzorz 1:85fcfcb7b137 436 {
pommzorz 1:85fcfcb7b137 437 elem[0][0] = m00; elem[0][1] = m01; elem[0][2] = m02; elem[0][3] = m03;
pommzorz 1:85fcfcb7b137 438 elem[1][0] = m10; elem[1][1] = m11; elem[1][2] = m12; elem[1][3] = m13;
pommzorz 1:85fcfcb7b137 439 elem[2][0] = m20; elem[2][1] = m21; elem[2][2] = m22; elem[2][3] = m23;
pommzorz 1:85fcfcb7b137 440 elem[3][0] = m30; elem[3][1] = m31; elem[3][2] = m32; elem[3][3] = m33;
pommzorz 1:85fcfcb7b137 441 }
pommzorz 1:85fcfcb7b137 442
pommzorz 1:85fcfcb7b137 443 explicit mat4(const vec4<T>& v0, const vec4<T>& v1, const vec4<T>& v2, const vec4<T>& v3)
pommzorz 1:85fcfcb7b137 444 {
pommzorz 1:85fcfcb7b137 445 elem[0][0] = v0[0];
pommzorz 1:85fcfcb7b137 446 elem[1][0] = v0[1];
pommzorz 1:85fcfcb7b137 447 elem[2][0] = v0[2];
pommzorz 1:85fcfcb7b137 448 elem[3][0] = v0[3];
pommzorz 1:85fcfcb7b137 449 elem[0][1] = v1[0];
pommzorz 1:85fcfcb7b137 450 elem[1][1] = v1[1];
pommzorz 1:85fcfcb7b137 451 elem[2][1] = v1[2];
pommzorz 1:85fcfcb7b137 452 elem[3][1] = v1[3];
pommzorz 1:85fcfcb7b137 453 elem[0][2] = v2[0];
pommzorz 1:85fcfcb7b137 454 elem[1][2] = v2[1];
pommzorz 1:85fcfcb7b137 455 elem[2][2] = v2[2];
pommzorz 1:85fcfcb7b137 456 elem[3][2] = v2[3];
pommzorz 1:85fcfcb7b137 457 elem[0][3] = v3[0];
pommzorz 1:85fcfcb7b137 458 elem[1][3] = v3[1];
pommzorz 1:85fcfcb7b137 459 elem[2][3] = v3[2];
pommzorz 1:85fcfcb7b137 460 elem[3][3] = v3[3];
pommzorz 1:85fcfcb7b137 461 }
pommzorz 1:85fcfcb7b137 462
pommzorz 1:85fcfcb7b137 463 MATRIX_CONSTRUCTOR_FROM_LOWER(mat4, mat3, 4, 3)
pommzorz 1:85fcfcb7b137 464 MATRIX_COMMON(mat4, 4)
pommzorz 1:85fcfcb7b137 465 };
pommzorz 1:85fcfcb7b137 466
pommzorz 1:85fcfcb7b137 467 #define MATRIX_CONSTRUCTOR_FROM_HIGHER(CLASS1, CLASS2, SIZE) \
pommzorz 1:85fcfcb7b137 468 template <typename T> \
pommzorz 1:85fcfcb7b137 469 inline CLASS1<T>::CLASS1(const CLASS2<T>& m) \
pommzorz 1:85fcfcb7b137 470 { \
pommzorz 1:85fcfcb7b137 471 for (int r = 0; r < SIZE; ++r) \
pommzorz 1:85fcfcb7b137 472 for (int c = 0; c < SIZE; ++c) \
pommzorz 1:85fcfcb7b137 473 elem[r][c] = m.elem[r][c]; \
pommzorz 1:85fcfcb7b137 474 }
pommzorz 1:85fcfcb7b137 475
pommzorz 1:85fcfcb7b137 476 MATRIX_CONSTRUCTOR_FROM_HIGHER(mat2, mat3, 2)
pommzorz 1:85fcfcb7b137 477 MATRIX_CONSTRUCTOR_FROM_HIGHER(mat3, mat4, 3)
pommzorz 1:85fcfcb7b137 478
pommzorz 1:85fcfcb7b137 479 #define MAT_FUNC_TEMPLATE(CLASS, SIZE) \
pommzorz 1:85fcfcb7b137 480 template <typename T> \
pommzorz 1:85fcfcb7b137 481 inline CLASS transpose(const CLASS & m) \
pommzorz 1:85fcfcb7b137 482 { \
pommzorz 1:85fcfcb7b137 483 CLASS result; \
pommzorz 1:85fcfcb7b137 484 for (int r = 0; r < SIZE; ++r) \
pommzorz 1:85fcfcb7b137 485 for (int c = 0; c < SIZE; ++c) \
pommzorz 1:85fcfcb7b137 486 result.elem[r][c] = m.elem[c][r]; \
pommzorz 1:85fcfcb7b137 487 return result; \
pommzorz 1:85fcfcb7b137 488 } \
pommzorz 1:85fcfcb7b137 489 template <typename T> \
pommzorz 1:85fcfcb7b137 490 inline CLASS identity ## SIZE() \
pommzorz 1:85fcfcb7b137 491 { \
pommzorz 1:85fcfcb7b137 492 CLASS result; \
pommzorz 1:85fcfcb7b137 493 for (int r = 0; r < SIZE; ++r) \
pommzorz 1:85fcfcb7b137 494 for (int c = 0; c < SIZE; ++c) \
pommzorz 1:85fcfcb7b137 495 result.elem[r][c] = r == c ? T(1) : T(0); \
pommzorz 1:85fcfcb7b137 496 return result; \
pommzorz 1:85fcfcb7b137 497 } \
pommzorz 1:85fcfcb7b137 498 template <typename T> \
pommzorz 1:85fcfcb7b137 499 inline T trace(const CLASS & m) \
pommzorz 1:85fcfcb7b137 500 { \
pommzorz 1:85fcfcb7b137 501 T result = T(0); \
pommzorz 1:85fcfcb7b137 502 for (int i = 0; i < SIZE; ++i) \
pommzorz 1:85fcfcb7b137 503 result += m.elem[i][i]; \
pommzorz 1:85fcfcb7b137 504 return result; \
pommzorz 1:85fcfcb7b137 505 }
pommzorz 1:85fcfcb7b137 506
pommzorz 1:85fcfcb7b137 507 MAT_FUNC_TEMPLATE(mat2<T>, 2)
pommzorz 1:85fcfcb7b137 508 MAT_FUNC_TEMPLATE(mat3<T>, 3)
pommzorz 1:85fcfcb7b137 509 MAT_FUNC_TEMPLATE(mat4<T>, 4)
pommzorz 1:85fcfcb7b137 510
pommzorz 1:85fcfcb7b137 511 #define MAT_FUNC_MINOR_TEMPLATE(CLASS1, CLASS2, SIZE) \
pommzorz 1:85fcfcb7b137 512 template <typename T> \
pommzorz 1:85fcfcb7b137 513 inline CLASS2 minor(const CLASS1 & m, int _r = SIZE, int _c = SIZE) { \
pommzorz 1:85fcfcb7b137 514 CLASS2 result; \
pommzorz 1:85fcfcb7b137 515 for (int r = 0; r < SIZE - 1; ++r) \
pommzorz 1:85fcfcb7b137 516 for (int c = 0; c < SIZE - 1; ++c) { \
pommzorz 1:85fcfcb7b137 517 int rs = r >= _r ? 1 : 0; \
pommzorz 1:85fcfcb7b137 518 int cs = c >= _c ? 1 : 0; \
pommzorz 1:85fcfcb7b137 519 result.elem[r][c] = m.elem[r + rs][c + cs]; \
pommzorz 1:85fcfcb7b137 520 } \
pommzorz 1:85fcfcb7b137 521 return result; \
pommzorz 1:85fcfcb7b137 522 }
pommzorz 1:85fcfcb7b137 523
pommzorz 1:85fcfcb7b137 524 MAT_FUNC_MINOR_TEMPLATE(mat3<T>, mat2<T>, 3)
pommzorz 1:85fcfcb7b137 525 MAT_FUNC_MINOR_TEMPLATE(mat4<T>, mat3<T>, 4)
pommzorz 1:85fcfcb7b137 526
pommzorz 1:85fcfcb7b137 527 template <typename T>
pommzorz 1:85fcfcb7b137 528 inline T det(const mat2<T>& m)
pommzorz 1:85fcfcb7b137 529 {
pommzorz 1:85fcfcb7b137 530 return dot(
pommzorz 1:85fcfcb7b137 531 vec2<T>(m.elem[0][0], -m.elem[0][1]),
pommzorz 1:85fcfcb7b137 532 vec2<T>(m.elem[1][1], m.elem[1][0]));
pommzorz 1:85fcfcb7b137 533 }
pommzorz 1:85fcfcb7b137 534
pommzorz 1:85fcfcb7b137 535 template <typename T>
pommzorz 1:85fcfcb7b137 536 inline T det(const mat3<T>& m)
pommzorz 1:85fcfcb7b137 537 {
pommzorz 1:85fcfcb7b137 538 return dot(cross(MATRIX_COL3(m, 0), MATRIX_COL3(m, 1)), MATRIX_COL3(m, 2));
pommzorz 1:85fcfcb7b137 539 }
pommzorz 1:85fcfcb7b137 540
pommzorz 1:85fcfcb7b137 541 template <typename T>
pommzorz 1:85fcfcb7b137 542 inline T det(const mat4<T>& m)
pommzorz 1:85fcfcb7b137 543 {
pommzorz 1:85fcfcb7b137 544 vec4<T> b;
pommzorz 1:85fcfcb7b137 545 for (int i = 0; i < 4; ++i)
pommzorz 1:85fcfcb7b137 546 b[i] = (i & 1 ? -1 : 1) * det(minor(m, 0, i));
pommzorz 1:85fcfcb7b137 547 return dot(MATRIX_ROW4(m, 0), b);
pommzorz 1:85fcfcb7b137 548 }
pommzorz 1:85fcfcb7b137 549
pommzorz 1:85fcfcb7b137 550 #define MAT_ADJOINT_TEMPLATE(CLASS, SIZE) \
pommzorz 1:85fcfcb7b137 551 template <typename T> \
pommzorz 1:85fcfcb7b137 552 inline CLASS adjoint(const CLASS & m) \
pommzorz 1:85fcfcb7b137 553 { \
pommzorz 1:85fcfcb7b137 554 CLASS result; \
pommzorz 1:85fcfcb7b137 555 for (int r = 0; r < SIZE; ++r) \
pommzorz 1:85fcfcb7b137 556 for (int c = 0; c < SIZE; ++c) \
pommzorz 1:85fcfcb7b137 557 result.elem[r][c] = ((r + c) & 1 ? -1 : 1) * det(minor(m, c, r)); \
pommzorz 1:85fcfcb7b137 558 return result; \
pommzorz 1:85fcfcb7b137 559 }
pommzorz 1:85fcfcb7b137 560
pommzorz 1:85fcfcb7b137 561 MAT_ADJOINT_TEMPLATE(mat3<T>, 3)
pommzorz 1:85fcfcb7b137 562 MAT_ADJOINT_TEMPLATE(mat4<T>, 4)
pommzorz 1:85fcfcb7b137 563
pommzorz 1:85fcfcb7b137 564 template <typename T>
pommzorz 1:85fcfcb7b137 565 inline mat2<T> adjoint(const mat2<T> & m)
pommzorz 1:85fcfcb7b137 566 {
pommzorz 1:85fcfcb7b137 567 return mat2<T>(
pommzorz 1:85fcfcb7b137 568 m.elem[1][1], -m.elem[0][1],
pommzorz 1:85fcfcb7b137 569 -m.elem[1][0], m.elem[0][0]
pommzorz 1:85fcfcb7b137 570 );
pommzorz 1:85fcfcb7b137 571 }
pommzorz 1:85fcfcb7b137 572
pommzorz 1:85fcfcb7b137 573 #define MAT_INVERSE_TEMPLATE(CLASS) \
pommzorz 1:85fcfcb7b137 574 template <typename T> \
pommzorz 1:85fcfcb7b137 575 inline CLASS inverse(const CLASS & m) \
pommzorz 1:85fcfcb7b137 576 { \
pommzorz 1:85fcfcb7b137 577 return adjoint(m) * inv(det(m)); \
pommzorz 1:85fcfcb7b137 578 }
pommzorz 1:85fcfcb7b137 579
pommzorz 1:85fcfcb7b137 580 MAT_INVERSE_TEMPLATE(mat2<T>)
pommzorz 1:85fcfcb7b137 581 MAT_INVERSE_TEMPLATE(mat3<T>)
pommzorz 1:85fcfcb7b137 582 MAT_INVERSE_TEMPLATE(mat4<T>)
pommzorz 1:85fcfcb7b137 583
pommzorz 1:85fcfcb7b137 584 #define MAT_VEC_FUNCS_TEMPLATE(MATCLASS, VECCLASS, SIZE) \
pommzorz 1:85fcfcb7b137 585 template <typename T> \
pommzorz 1:85fcfcb7b137 586 inline VECCLASS operator * (const MATCLASS & m, const VECCLASS & v) \
pommzorz 1:85fcfcb7b137 587 { \
pommzorz 1:85fcfcb7b137 588 VECCLASS result; \
pommzorz 1:85fcfcb7b137 589 for (int i = 0; i < SIZE; ++i) {\
pommzorz 1:85fcfcb7b137 590 result[i] = dot(MATRIX_ROW ## SIZE(m, i), v); \
pommzorz 1:85fcfcb7b137 591 } \
pommzorz 1:85fcfcb7b137 592 return result; \
pommzorz 1:85fcfcb7b137 593 } \
pommzorz 1:85fcfcb7b137 594 template <typename T> \
pommzorz 1:85fcfcb7b137 595 inline VECCLASS operator * (const VECCLASS & v, const MATCLASS & m) \
pommzorz 1:85fcfcb7b137 596 { \
pommzorz 1:85fcfcb7b137 597 VECCLASS result; \
pommzorz 1:85fcfcb7b137 598 for (int i = 0; i < SIZE; ++i) \
pommzorz 1:85fcfcb7b137 599 result[i] = dot(v, MATRIX_COL ## SIZE(m, i)); \
pommzorz 1:85fcfcb7b137 600 return result; \
pommzorz 1:85fcfcb7b137 601 }
pommzorz 1:85fcfcb7b137 602
pommzorz 1:85fcfcb7b137 603 MAT_VEC_FUNCS_TEMPLATE(mat2<T>, vec2<T>, 2)
pommzorz 1:85fcfcb7b137 604 MAT_VEC_FUNCS_TEMPLATE(mat3<T>, vec3<T>, 3)
pommzorz 1:85fcfcb7b137 605 MAT_VEC_FUNCS_TEMPLATE(mat4<T>, vec4<T>, 4)
pommzorz 1:85fcfcb7b137 606
pommzorz 1:85fcfcb7b137 607 // Returns the inverse of a 4x4 matrix. It is assumed that the matrix passed
pommzorz 1:85fcfcb7b137 608 // as argument describes a rigid-body transformation.
pommzorz 1:85fcfcb7b137 609 template <typename T>
pommzorz 1:85fcfcb7b137 610 inline mat4<T> fast_inverse(const mat4<T>& m)
pommzorz 1:85fcfcb7b137 611 {
pommzorz 1:85fcfcb7b137 612 const vec3<T> t = MATRIX_COL3(m, 3);
pommzorz 1:85fcfcb7b137 613 const T tx = -dot(MATRIX_COL3(m, 0), t);
pommzorz 1:85fcfcb7b137 614 const T ty = -dot(MATRIX_COL3(m, 1), t);
pommzorz 1:85fcfcb7b137 615 const T tz = -dot(MATRIX_COL3(m, 2), t);
pommzorz 1:85fcfcb7b137 616
pommzorz 1:85fcfcb7b137 617 return mat4<T>(
pommzorz 1:85fcfcb7b137 618 m.elem[0][0], m.elem[1][0], m.elem[2][0], tx,
pommzorz 1:85fcfcb7b137 619 m.elem[0][1], m.elem[1][1], m.elem[2][1], ty,
pommzorz 1:85fcfcb7b137 620 m.elem[0][2], m.elem[1][2], m.elem[2][2], tz,
pommzorz 1:85fcfcb7b137 621 T(0), T(0), T(0), T(1)
pommzorz 1:85fcfcb7b137 622 );
pommzorz 1:85fcfcb7b137 623 }
pommzorz 1:85fcfcb7b137 624
pommzorz 1:85fcfcb7b137 625 // Transformations for points and vectors. Potentially faster than a full
pommzorz 1:85fcfcb7b137 626 // matrix * vector multiplication
pommzorz 1:85fcfcb7b137 627
pommzorz 1:85fcfcb7b137 628 #define MAT_TRANFORMS_TEMPLATE(MATCLASS, VECCLASS, VECSIZE) \
pommzorz 1:85fcfcb7b137 629 /* computes vec3<T>(m * vec4<T>(v, 0.0)) */ \
pommzorz 1:85fcfcb7b137 630 template <typename T> \
pommzorz 1:85fcfcb7b137 631 inline VECCLASS transform_vector(const MATCLASS & m, const VECCLASS & v) \
pommzorz 1:85fcfcb7b137 632 { \
pommzorz 1:85fcfcb7b137 633 VECCLASS result; \
pommzorz 1:85fcfcb7b137 634 for (int i = 0; i < VECSIZE; ++i) \
pommzorz 1:85fcfcb7b137 635 result[i] = dot(MATRIX_ROW ## VECSIZE(m, i), v); \
pommzorz 1:85fcfcb7b137 636 return result;\
pommzorz 1:85fcfcb7b137 637 } \
pommzorz 1:85fcfcb7b137 638 /* computes vec3(m * vec4(v, 1.0)) */ \
pommzorz 1:85fcfcb7b137 639 template <typename T> \
pommzorz 1:85fcfcb7b137 640 inline VECCLASS transform_point(const MATCLASS & m, const VECCLASS & v) \
pommzorz 1:85fcfcb7b137 641 { \
pommzorz 1:85fcfcb7b137 642 /*return transform_vector(m, v) + MATRIX_ROW ## VECSIZE(m, VECSIZE); */\
pommzorz 1:85fcfcb7b137 643 VECCLASS result; \
pommzorz 1:85fcfcb7b137 644 for (int i = 0; i < VECSIZE; ++i) \
pommzorz 1:85fcfcb7b137 645 result[i] = dot(MATRIX_ROW ## VECSIZE(m, i), v) + m.elem[i][VECSIZE]; \
pommzorz 1:85fcfcb7b137 646 return result; \
pommzorz 1:85fcfcb7b137 647 } \
pommzorz 1:85fcfcb7b137 648 /* computes VECCLASS(transpose(m) * vec4<T>(v, 0.0)) */ \
pommzorz 1:85fcfcb7b137 649 template <typename T> \
pommzorz 1:85fcfcb7b137 650 inline VECCLASS transform_vector_transpose(const MATCLASS & m, const VECCLASS& v) \
pommzorz 1:85fcfcb7b137 651 { \
pommzorz 1:85fcfcb7b137 652 VECCLASS result; \
pommzorz 1:85fcfcb7b137 653 for (int i = 0; i < VECSIZE; ++i) \
pommzorz 1:85fcfcb7b137 654 result[i] = dot(MATRIX_COL ## VECSIZE(m, i), v); \
pommzorz 1:85fcfcb7b137 655 return result; \
pommzorz 1:85fcfcb7b137 656 } \
pommzorz 1:85fcfcb7b137 657 /* computes VECCLASS(transpose(m) * vec4<T>(v, 1.0)) */ \
pommzorz 1:85fcfcb7b137 658 template <typename T> \
pommzorz 1:85fcfcb7b137 659 inline VECCLASS transform_point_transpose(const MATCLASS & m, const VECCLASS& v) \
pommzorz 1:85fcfcb7b137 660 { \
pommzorz 1:85fcfcb7b137 661 /*return transform_vector_transpose(m, v) + MATRIX_COL ## VECSIZE(m, VECSIZE); */\
pommzorz 1:85fcfcb7b137 662 VECCLASS result; \
pommzorz 1:85fcfcb7b137 663 for (int i = 0; i < VECSIZE; ++i) \
pommzorz 1:85fcfcb7b137 664 result[i] = dot(MATRIX_COL ## VECSIZE(m, i), v) + m.elem[VECSIZE][i]; \
pommzorz 1:85fcfcb7b137 665 return result; \
pommzorz 1:85fcfcb7b137 666 }
pommzorz 1:85fcfcb7b137 667
pommzorz 1:85fcfcb7b137 668 MAT_TRANFORMS_TEMPLATE(mat4<T>, vec3<T>, 3)
pommzorz 1:85fcfcb7b137 669 MAT_TRANFORMS_TEMPLATE(mat3<T>, vec2<T>, 2)
pommzorz 1:85fcfcb7b137 670
pommzorz 1:85fcfcb7b137 671 #define MAT_OUTERPRODUCT_TEMPLATE(MATCLASS, VECCLASS, MATSIZE) \
pommzorz 1:85fcfcb7b137 672 template <typename T> \
pommzorz 1:85fcfcb7b137 673 inline MATCLASS outer_product(const VECCLASS & v1, const VECCLASS & v2) \
pommzorz 1:85fcfcb7b137 674 { \
pommzorz 1:85fcfcb7b137 675 MATCLASS r; \
pommzorz 1:85fcfcb7b137 676 for ( int j = 0; j < MATSIZE; ++j ) \
pommzorz 1:85fcfcb7b137 677 for ( int k = 0; k < MATSIZE; ++k ) \
pommzorz 1:85fcfcb7b137 678 r.elem[j][k] = v1[j] * v2[k]; \
pommzorz 1:85fcfcb7b137 679 return r; \
pommzorz 1:85fcfcb7b137 680 }
pommzorz 1:85fcfcb7b137 681
pommzorz 1:85fcfcb7b137 682 MAT_OUTERPRODUCT_TEMPLATE(mat4<T>, vec4<T>, 4)
pommzorz 1:85fcfcb7b137 683 MAT_OUTERPRODUCT_TEMPLATE(mat3<T>, vec3<T>, 3)
pommzorz 1:85fcfcb7b137 684 MAT_OUTERPRODUCT_TEMPLATE(mat2<T>, vec2<T>, 2)
pommzorz 1:85fcfcb7b137 685
pommzorz 1:85fcfcb7b137 686 template <typename T>
pommzorz 1:85fcfcb7b137 687 inline mat4<T> translation_matrix(const T x, const T y, const T z)
pommzorz 1:85fcfcb7b137 688 {
pommzorz 1:85fcfcb7b137 689 mat4<T> r(T(1));
pommzorz 1:85fcfcb7b137 690 r.elem[0][3] = x;
pommzorz 1:85fcfcb7b137 691 r.elem[1][3] = y;
pommzorz 1:85fcfcb7b137 692 r.elem[2][3] = z;
pommzorz 1:85fcfcb7b137 693 return r;
pommzorz 1:85fcfcb7b137 694 }
pommzorz 1:85fcfcb7b137 695
pommzorz 1:85fcfcb7b137 696 template <typename T>
pommzorz 1:85fcfcb7b137 697 inline mat4<T> translation_matrix(const vec3<T>& v)
pommzorz 1:85fcfcb7b137 698 {
pommzorz 1:85fcfcb7b137 699 return translation_matrix(v.x, v.y, v.z);
pommzorz 1:85fcfcb7b137 700 }
pommzorz 1:85fcfcb7b137 701
pommzorz 1:85fcfcb7b137 702 template <typename T>
pommzorz 1:85fcfcb7b137 703 inline mat4<T> scaling_matrix(const T x, const T y, const T z)
pommzorz 1:85fcfcb7b137 704 {
pommzorz 1:85fcfcb7b137 705 mat4<T> r(T(0));
pommzorz 1:85fcfcb7b137 706 r.elem[0][0] = x;
pommzorz 1:85fcfcb7b137 707 r.elem[1][1] = y;
pommzorz 1:85fcfcb7b137 708 r.elem[2][2] = z;
pommzorz 1:85fcfcb7b137 709 r.elem[3][3] = T(1);
pommzorz 1:85fcfcb7b137 710 return r;
pommzorz 1:85fcfcb7b137 711 }
pommzorz 1:85fcfcb7b137 712
pommzorz 1:85fcfcb7b137 713 template <typename T>
pommzorz 1:85fcfcb7b137 714 inline mat4<T> scaling_matrix(const vec3<T>& v)
pommzorz 1:85fcfcb7b137 715 {
pommzorz 1:85fcfcb7b137 716 return scaling_matrix(v.x, v.y, v.z);
pommzorz 1:85fcfcb7b137 717 }
pommzorz 1:85fcfcb7b137 718
pommzorz 1:85fcfcb7b137 719 template <typename T>
pommzorz 1:85fcfcb7b137 720 inline mat4<T> rotation_matrix(const T angle, const vec3<T>& v)
pommzorz 1:85fcfcb7b137 721 {
pommzorz 1:85fcfcb7b137 722 const T a = angle * T(M_PI/180) ;
pommzorz 1:85fcfcb7b137 723 const vec3<T> u = normalize(v);
pommzorz 1:85fcfcb7b137 724
pommzorz 1:85fcfcb7b137 725 const mat3<T> S(
pommzorz 1:85fcfcb7b137 726 T(0), -u[2], u[1],
pommzorz 1:85fcfcb7b137 727 u[2], T(0), -u[0],
pommzorz 1:85fcfcb7b137 728 -u[1], u[0], T(0)
pommzorz 1:85fcfcb7b137 729 );
pommzorz 1:85fcfcb7b137 730
pommzorz 1:85fcfcb7b137 731 const mat3<T> uut = outer_product(u, u);
pommzorz 1:85fcfcb7b137 732 const mat3<T> R = uut + T(cos(a)) * (identity3<T>() - uut) + T(sin(a)) * S;
pommzorz 1:85fcfcb7b137 733
pommzorz 1:85fcfcb7b137 734 return mat4<T>(R);
pommzorz 1:85fcfcb7b137 735 }
pommzorz 1:85fcfcb7b137 736
pommzorz 1:85fcfcb7b137 737
pommzorz 1:85fcfcb7b137 738 template <typename T>
pommzorz 1:85fcfcb7b137 739 inline mat4<T> rotation_matrix(const T angle, const T x, const T y, const T z)
pommzorz 1:85fcfcb7b137 740 {
pommzorz 1:85fcfcb7b137 741 return rotation_matrix(angle, vec3<T>(x, y, z));
pommzorz 1:85fcfcb7b137 742 }
pommzorz 1:85fcfcb7b137 743
pommzorz 1:85fcfcb7b137 744 // Constructs a shear-matrix that shears component i by factor with
pommzorz 1:85fcfcb7b137 745 // Respect to component j.
pommzorz 1:85fcfcb7b137 746 template <typename T>
pommzorz 1:85fcfcb7b137 747 inline mat4<T> shear_matrix(const int i, const int j, const T factor)
pommzorz 1:85fcfcb7b137 748 {
pommzorz 1:85fcfcb7b137 749 mat4<T> m = identity4<T>();
pommzorz 1:85fcfcb7b137 750 m.elem[i][j] = factor;
pommzorz 1:85fcfcb7b137 751 return m;
pommzorz 1:85fcfcb7b137 752 }
pommzorz 1:85fcfcb7b137 753
pommzorz 1:85fcfcb7b137 754 template <typename T>
pommzorz 1:85fcfcb7b137 755 inline mat4<T> euler(const T head, const T pitch, const T roll)
pommzorz 1:85fcfcb7b137 756 {
pommzorz 1:85fcfcb7b137 757 return rotation_matrix(roll, T(0), T(0), T(1)) *
pommzorz 1:85fcfcb7b137 758 rotation_matrix(pitch, T(1), T(0), T(0)) *
pommzorz 1:85fcfcb7b137 759 rotation_matrix(head, T(0), T(1), T(0));
pommzorz 1:85fcfcb7b137 760 }
pommzorz 1:85fcfcb7b137 761
pommzorz 1:85fcfcb7b137 762 template <typename T>
pommzorz 1:85fcfcb7b137 763 inline mat4<T> frustum_matrix(const T l, const T r, const T b, const T t, const T n, const T f)
pommzorz 1:85fcfcb7b137 764 {
pommzorz 1:85fcfcb7b137 765 return mat4<T>(
pommzorz 1:85fcfcb7b137 766 (2 * n)/(r - l), T(0), (r + l)/(r - l), T(0),
pommzorz 1:85fcfcb7b137 767 T(0), (2 * n)/(t - b), (t + b)/(t - b), T(0),
pommzorz 1:85fcfcb7b137 768 T(0), T(0), -(f + n)/(f - n), -(2 * f * n)/(f - n),
pommzorz 1:85fcfcb7b137 769 T(0), T(0), -T(1), T(0)
pommzorz 1:85fcfcb7b137 770 );
pommzorz 1:85fcfcb7b137 771 }
pommzorz 1:85fcfcb7b137 772
pommzorz 1:85fcfcb7b137 773 template <typename T>
pommzorz 1:85fcfcb7b137 774 inline mat4<T> perspective_matrix(const T fovy, const T aspect, const T zNear, const T zFar)
pommzorz 1:85fcfcb7b137 775 {
pommzorz 1:85fcfcb7b137 776 const T dz = zFar - zNear;
pommzorz 1:85fcfcb7b137 777 const T rad = fovy / T(2) * T(M_PI/180);
pommzorz 1:85fcfcb7b137 778 const T s = sin(rad);
pommzorz 1:85fcfcb7b137 779
pommzorz 1:85fcfcb7b137 780 if ( ( dz == T(0) ) || ( s == T(0) ) || ( aspect == T(0) ) ) {
pommzorz 1:85fcfcb7b137 781 return identity4<T>();
pommzorz 1:85fcfcb7b137 782 }
pommzorz 1:85fcfcb7b137 783
pommzorz 1:85fcfcb7b137 784 const T cot = cos(rad) / s;
pommzorz 1:85fcfcb7b137 785
pommzorz 1:85fcfcb7b137 786 mat4<T> m = identity4<T>();
pommzorz 1:85fcfcb7b137 787 m[0] = cot / aspect;
pommzorz 1:85fcfcb7b137 788 m[5] = cot;
pommzorz 1:85fcfcb7b137 789 m[10] = -(zFar + zNear) / dz;
pommzorz 1:85fcfcb7b137 790 m[14] = T(-1);
pommzorz 1:85fcfcb7b137 791 m[11] = -2 * zNear * zFar / dz;
pommzorz 1:85fcfcb7b137 792 m[15] = T(0);
pommzorz 1:85fcfcb7b137 793
pommzorz 1:85fcfcb7b137 794 return m;
pommzorz 1:85fcfcb7b137 795 }
pommzorz 1:85fcfcb7b137 796
pommzorz 1:85fcfcb7b137 797 template <typename T>
pommzorz 1:85fcfcb7b137 798 inline mat4<T> ortho_matrix(const T l, const T r, const T b, const T t, const T n, const T f)
pommzorz 1:85fcfcb7b137 799 {
pommzorz 1:85fcfcb7b137 800 return mat4<T>(
pommzorz 1:85fcfcb7b137 801 T(2)/(r - l), T(0), T(0), -(r + l)/(r - l),
pommzorz 1:85fcfcb7b137 802 T(0), T(2)/(t - b), T(0), -(t + b)/(t - b),
pommzorz 1:85fcfcb7b137 803 T(0), T(0), -T(2)/(f - n), -(f + n)/(f - n),
pommzorz 1:85fcfcb7b137 804 T(0), T(0), T(0), T(1)
pommzorz 1:85fcfcb7b137 805 );
pommzorz 1:85fcfcb7b137 806 }
pommzorz 1:85fcfcb7b137 807
pommzorz 1:85fcfcb7b137 808 template <typename T>
pommzorz 1:85fcfcb7b137 809 inline mat4<T> lookat_matrix(const vec3<T>& eye, const vec3<T>& center, const vec3<T>& up) {
pommzorz 1:85fcfcb7b137 810 const vec3<T> forward = normalize(center - eye);
pommzorz 1:85fcfcb7b137 811 const vec3<T> side = normalize(cross(forward, up));
pommzorz 1:85fcfcb7b137 812
pommzorz 1:85fcfcb7b137 813 const vec3<T> up2 = cross(side, forward);
pommzorz 1:85fcfcb7b137 814
pommzorz 1:85fcfcb7b137 815 mat4<T> m = identity4<T>();
pommzorz 1:85fcfcb7b137 816
pommzorz 1:85fcfcb7b137 817 m.elem[0][0] = side[0];
pommzorz 1:85fcfcb7b137 818 m.elem[0][1] = side[1];
pommzorz 1:85fcfcb7b137 819 m.elem[0][2] = side[2];
pommzorz 1:85fcfcb7b137 820
pommzorz 1:85fcfcb7b137 821 m.elem[1][0] = up2[0];
pommzorz 1:85fcfcb7b137 822 m.elem[1][1] = up2[1];
pommzorz 1:85fcfcb7b137 823 m.elem[1][2] = up2[2];
pommzorz 1:85fcfcb7b137 824
pommzorz 1:85fcfcb7b137 825 m.elem[2][0] = -forward[0];
pommzorz 1:85fcfcb7b137 826 m.elem[2][1] = -forward[1];
pommzorz 1:85fcfcb7b137 827 m.elem[2][2] = -forward[2];
pommzorz 1:85fcfcb7b137 828
pommzorz 1:85fcfcb7b137 829 return m * translation_matrix(-eye);
pommzorz 1:85fcfcb7b137 830 }
pommzorz 1:85fcfcb7b137 831
pommzorz 1:85fcfcb7b137 832 template <typename T>
pommzorz 1:85fcfcb7b137 833 inline mat4<T> picking_matrix(const T x, const T y, const T dx, const T dy, int viewport[4]) {
pommzorz 1:85fcfcb7b137 834 if (dx <= 0 || dy <= 0) {
pommzorz 1:85fcfcb7b137 835 return identity4<T>();
pommzorz 1:85fcfcb7b137 836 }
pommzorz 1:85fcfcb7b137 837
pommzorz 1:85fcfcb7b137 838 mat4<T> r = translation_matrix((viewport[2] - 2 * (x - viewport[0])) / dx,
pommzorz 1:85fcfcb7b137 839 (viewport[3] - 2 * (y - viewport[1])) / dy, 0);
pommzorz 1:85fcfcb7b137 840 r *= scaling_matrix(viewport[2] / dx, viewport[2] / dy, 1);
pommzorz 1:85fcfcb7b137 841 return r;
pommzorz 1:85fcfcb7b137 842 }
pommzorz 1:85fcfcb7b137 843
pommzorz 1:85fcfcb7b137 844 // Constructs a shadow matrix. q is the light source and p is the plane.
pommzorz 1:85fcfcb7b137 845 template <typename T> inline mat4<T> shadow_matrix(const vec4<T>& q, const vec4<T>& p) {
pommzorz 1:85fcfcb7b137 846 mat4<T> m;
pommzorz 1:85fcfcb7b137 847
pommzorz 1:85fcfcb7b137 848 m.elem[0][0] = p.y * q[1] + p.z * q[2] + p.w * q[3];
pommzorz 1:85fcfcb7b137 849 m.elem[0][1] = -p.y * q[0];
pommzorz 1:85fcfcb7b137 850 m.elem[0][2] = -p.z * q[0];
pommzorz 1:85fcfcb7b137 851 m.elem[0][3] = -p.w * q[0];
pommzorz 1:85fcfcb7b137 852
pommzorz 1:85fcfcb7b137 853 m.elem[1][0] = -p.x * q[1];
pommzorz 1:85fcfcb7b137 854 m.elem[1][1] = p.x * q[0] + p.z * q[2] + p.w * q[3];
pommzorz 1:85fcfcb7b137 855 m.elem[1][2] = -p.z * q[1];
pommzorz 1:85fcfcb7b137 856 m.elem[1][3] = -p.w * q[1];
pommzorz 1:85fcfcb7b137 857
pommzorz 1:85fcfcb7b137 858
pommzorz 1:85fcfcb7b137 859 m.elem[2][0] = -p.x * q[2];
pommzorz 1:85fcfcb7b137 860 m.elem[2][1] = -p.y * q[2];
pommzorz 1:85fcfcb7b137 861 m.elem[2][2] = p.x * q[0] + p.y * q[1] + p.w * q[3];
pommzorz 1:85fcfcb7b137 862 m.elem[2][3] = -p.w * q[2];
pommzorz 1:85fcfcb7b137 863
pommzorz 1:85fcfcb7b137 864 m.elem[3][1] = -p.x * q[3];
pommzorz 1:85fcfcb7b137 865 m.elem[3][2] = -p.y * q[3];
pommzorz 1:85fcfcb7b137 866 m.elem[3][3] = -p.z * q[3];
pommzorz 1:85fcfcb7b137 867 m.elem[3][0] = p.x * q[0] + p.y * q[1] + p.z * q[2];
pommzorz 1:85fcfcb7b137 868
pommzorz 1:85fcfcb7b137 869 return m;
pommzorz 1:85fcfcb7b137 870 }
pommzorz 1:85fcfcb7b137 871
pommzorz 1:85fcfcb7b137 872 // Quaternion class
pommzorz 1:85fcfcb7b137 873 template <typename T>
pommzorz 1:85fcfcb7b137 874 struct quat {
pommzorz 1:85fcfcb7b137 875 vec3<T> v;
pommzorz 1:85fcfcb7b137 876 T w;
pommzorz 1:85fcfcb7b137 877
pommzorz 1:85fcfcb7b137 878 quat() {}
pommzorz 1:85fcfcb7b137 879 quat(const vec3<T>& iv, const T iw) : v(iv), w(iw) {}
pommzorz 1:85fcfcb7b137 880 quat(const T vx, const T vy, const T vz, const T iw) : v(vx, vy, vz), w(iw) {}
pommzorz 1:85fcfcb7b137 881 quat(const vec4<T>& i) : v(i.x, i.y, i.z), w(i.w) {}
pommzorz 1:85fcfcb7b137 882
pommzorz 1:85fcfcb7b137 883 operator const T* () const { return &(v[0]); }
pommzorz 1:85fcfcb7b137 884 operator T* () { return &(v[0]); }
pommzorz 1:85fcfcb7b137 885
pommzorz 1:85fcfcb7b137 886 quat& operator += (const quat& q) { v += q.v; w += q.w; return *this; }
pommzorz 1:85fcfcb7b137 887 quat& operator -= (const quat& q) { v -= q.v; w -= q.w; return *this; }
pommzorz 1:85fcfcb7b137 888
pommzorz 1:85fcfcb7b137 889 quat& operator *= (const T& s) { v *= s; w *= s; return *this; }
pommzorz 1:85fcfcb7b137 890 quat& operator /= (const T& s) { v /= s; w /= s; return *this; }
pommzorz 1:85fcfcb7b137 891
pommzorz 1:85fcfcb7b137 892 quat& operator *= (const quat& r)
pommzorz 1:85fcfcb7b137 893 {
pommzorz 1:85fcfcb7b137 894 //q1 x q2 = [s1,v1] x [s2,v2] = [(s1*s2 - v1*v2),(s1*v2 + s2*v1 + v1xv2)].
pommzorz 1:85fcfcb7b137 895 quat q;
pommzorz 1:85fcfcb7b137 896 q.v = cross(v, r.v) + r.w * v + w * r.v;
pommzorz 1:85fcfcb7b137 897 q.w = w * r.w - dot(v, r.v);
pommzorz 1:85fcfcb7b137 898 return *this = q;
pommzorz 1:85fcfcb7b137 899 }
pommzorz 1:85fcfcb7b137 900
pommzorz 1:85fcfcb7b137 901 quat& operator /= (const quat& q) { return (*this) *= inverse(q); }
pommzorz 1:85fcfcb7b137 902 };
pommzorz 1:85fcfcb7b137 903
pommzorz 1:85fcfcb7b137 904 // Quaternion functions
pommzorz 1:85fcfcb7b137 905
pommzorz 1:85fcfcb7b137 906 template <typename T>
pommzorz 1:85fcfcb7b137 907 inline quat<T> identityq()
pommzorz 1:85fcfcb7b137 908 {
pommzorz 1:85fcfcb7b137 909 return quat<T>(T(0), T(0), T(0), T(1));
pommzorz 1:85fcfcb7b137 910 }
pommzorz 1:85fcfcb7b137 911
pommzorz 1:85fcfcb7b137 912 template <typename T>
pommzorz 1:85fcfcb7b137 913 inline quat<T> conjugate(const quat<T>& q)
pommzorz 1:85fcfcb7b137 914 {
pommzorz 1:85fcfcb7b137 915 return quat<T>(-q.v, q.w);
pommzorz 1:85fcfcb7b137 916 }
pommzorz 1:85fcfcb7b137 917
pommzorz 1:85fcfcb7b137 918 template <typename T>
pommzorz 1:85fcfcb7b137 919 inline quat<T> inverse(const quat<T>& q)
pommzorz 1:85fcfcb7b137 920 {
pommzorz 1:85fcfcb7b137 921 const T l = dot(q, q);
pommzorz 1:85fcfcb7b137 922 if ( l > T(0) ) return conjugate(q) * inv(l);
pommzorz 1:85fcfcb7b137 923 else return identityq<T>();
pommzorz 1:85fcfcb7b137 924 }
pommzorz 1:85fcfcb7b137 925
pommzorz 1:85fcfcb7b137 926 // quaternion utility functions
pommzorz 1:85fcfcb7b137 927
pommzorz 1:85fcfcb7b137 928 // the input quaternion is assumed to be normalized
pommzorz 1:85fcfcb7b137 929 template <typename T>
pommzorz 1:85fcfcb7b137 930 inline mat3<T> quat_to_mat3(const quat<T>& q)
pommzorz 1:85fcfcb7b137 931 {
pommzorz 1:85fcfcb7b137 932 // const quat<T> q = normalize(qq);
pommzorz 1:85fcfcb7b137 933
pommzorz 1:85fcfcb7b137 934 const T xx = q[0] * q[0];
pommzorz 1:85fcfcb7b137 935 const T xy = q[0] * q[1];
pommzorz 1:85fcfcb7b137 936 const T xz = q[0] * q[2];
pommzorz 1:85fcfcb7b137 937 const T xw = q[0] * q[3];
pommzorz 1:85fcfcb7b137 938
pommzorz 1:85fcfcb7b137 939 const T yy = q[1] * q[1];
pommzorz 1:85fcfcb7b137 940 const T yz = q[1] * q[2];
pommzorz 1:85fcfcb7b137 941 const T yw = q[1] * q[3];
pommzorz 1:85fcfcb7b137 942
pommzorz 1:85fcfcb7b137 943 const T zz = q[2] * q[2];
pommzorz 1:85fcfcb7b137 944 const T zw = q[2] * q[3];
pommzorz 1:85fcfcb7b137 945
pommzorz 1:85fcfcb7b137 946 return mat3<T>(
pommzorz 1:85fcfcb7b137 947 1 - 2*(yy + zz), 2*(xy - zw), 2*(xz + yw),
pommzorz 1:85fcfcb7b137 948 2*(xy + zw), 1 - 2*(xx + zz), 2*(yz - xw),
pommzorz 1:85fcfcb7b137 949 2*(xz - yw), 2*(yz + xw), 1 - 2*(xx + yy)
pommzorz 1:85fcfcb7b137 950 );
pommzorz 1:85fcfcb7b137 951 }
pommzorz 1:85fcfcb7b137 952
pommzorz 1:85fcfcb7b137 953 // the input quat<T>ernion is assumed to be normalized
pommzorz 1:85fcfcb7b137 954 template <typename T>
pommzorz 1:85fcfcb7b137 955 inline mat4<T> quat_to_mat4(const quat<T>& q)
pommzorz 1:85fcfcb7b137 956 {
pommzorz 1:85fcfcb7b137 957 // const quat<T> q = normalize(qq);
pommzorz 1:85fcfcb7b137 958
pommzorz 1:85fcfcb7b137 959 return mat4<T>(quat_to_mat3(q));
pommzorz 1:85fcfcb7b137 960 }
pommzorz 1:85fcfcb7b137 961
pommzorz 1:85fcfcb7b137 962 template <typename T>
pommzorz 1:85fcfcb7b137 963 inline quat<T> mat_to_quat(const mat4<T>& m)
pommzorz 1:85fcfcb7b137 964 {
pommzorz 1:85fcfcb7b137 965 const T t = m.elem[0][0] + m.elem[1][1] + m.elem[2][2] + T(1);
pommzorz 1:85fcfcb7b137 966 quat<T> q;
pommzorz 1:85fcfcb7b137 967
pommzorz 1:85fcfcb7b137 968 if ( t > 0 ) {
pommzorz 1:85fcfcb7b137 969 const T s = T(0.5) / sqrt(t);
pommzorz 1:85fcfcb7b137 970 q[3] = T(0.25) * inv(s);
pommzorz 1:85fcfcb7b137 971 q[0] = (m.elem[2][1] - m.elem[1][2]) * s;
pommzorz 1:85fcfcb7b137 972 q[1] = (m.elem[0][2] - m.elem[2][0]) * s;
pommzorz 1:85fcfcb7b137 973 q[2] = (m.elem[1][0] - m.elem[0][1]) * s;
pommzorz 1:85fcfcb7b137 974 } else {
pommzorz 1:85fcfcb7b137 975 if ( m.elem[0][0] > m.elem[1][1] && m.elem[0][0] > m.elem[2][2] ) {
pommzorz 1:85fcfcb7b137 976 const T s = T(2) * sqrt( T(1) + m.elem[0][0] - m.elem[1][1] - m.elem[2][2]);
pommzorz 1:85fcfcb7b137 977 const T invs = inv(s);
pommzorz 1:85fcfcb7b137 978 q[0] = T(0.25) * s;
pommzorz 1:85fcfcb7b137 979 q[1] = (m.elem[0][1] + m.elem[1][0] ) * invs;
pommzorz 1:85fcfcb7b137 980 q[2] = (m.elem[0][2] + m.elem[2][0] ) * invs;
pommzorz 1:85fcfcb7b137 981 q[3] = (m.elem[1][2] - m.elem[2][1] ) * invs;
pommzorz 1:85fcfcb7b137 982 } else if (m.elem[1][1] > m.elem[2][2]) {
pommzorz 1:85fcfcb7b137 983 const T s = T(2) * sqrt( T(1) + m.elem[1][1] - m.elem[0][0] - m.elem[2][2]);
pommzorz 1:85fcfcb7b137 984 const T invs = inv(s);
pommzorz 1:85fcfcb7b137 985 q[0] = (m.elem[0][1] + m.elem[1][0] ) * invs;
pommzorz 1:85fcfcb7b137 986 q[1] = T(0.25) * s;
pommzorz 1:85fcfcb7b137 987 q[2] = (m.elem[1][2] + m.elem[2][1] ) * invs;
pommzorz 1:85fcfcb7b137 988 q[3] = (m.elem[0][2] - m.elem[2][0] ) * invs;
pommzorz 1:85fcfcb7b137 989 } else {
pommzorz 1:85fcfcb7b137 990 const T s = T(2) * sqrt( T(1) + m.elem[2][2] - m.elem[0][0] - m.elem[1][1] );
pommzorz 1:85fcfcb7b137 991 const T invs = inv(s);
pommzorz 1:85fcfcb7b137 992 q[0] = (m.elem[0][2] + m.elem[2][0] ) * invs;
pommzorz 1:85fcfcb7b137 993 q[1] = (m.elem[1][2] + m.elem[2][1] ) * invs;
pommzorz 1:85fcfcb7b137 994 q[2] = T(0.25) * s;
pommzorz 1:85fcfcb7b137 995 q[3] = (m.elem[0][1] - m.elem[1][0] ) * invs;
pommzorz 1:85fcfcb7b137 996 }
pommzorz 1:85fcfcb7b137 997 }
pommzorz 1:85fcfcb7b137 998
pommzorz 1:85fcfcb7b137 999 return q;
pommzorz 1:85fcfcb7b137 1000 }
pommzorz 1:85fcfcb7b137 1001
pommzorz 1:85fcfcb7b137 1002 template <typename T>
pommzorz 1:85fcfcb7b137 1003 inline quat<T> mat_to_quat(const mat3<T>& m)
pommzorz 1:85fcfcb7b137 1004 {
pommzorz 1:85fcfcb7b137 1005 return mat_to_quat(mat4<T>(m));
pommzorz 1:85fcfcb7b137 1006 }
pommzorz 1:85fcfcb7b137 1007
pommzorz 1:85fcfcb7b137 1008 // the angle is in radians
pommzorz 1:85fcfcb7b137 1009 template <typename T>
pommzorz 1:85fcfcb7b137 1010 inline quat<T> quat_from_axis_angle(const vec3<T>& axis, const T a)
pommzorz 1:85fcfcb7b137 1011 {
pommzorz 1:85fcfcb7b137 1012 quat<T> r;
pommzorz 1:85fcfcb7b137 1013 const T inv2 = inv(T(2));
pommzorz 1:85fcfcb7b137 1014 r.v = sin(a * inv2) * normalize(axis);
pommzorz 1:85fcfcb7b137 1015 r.w = cos(a * inv2);
pommzorz 1:85fcfcb7b137 1016
pommzorz 1:85fcfcb7b137 1017 return r;
pommzorz 1:85fcfcb7b137 1018 }
pommzorz 1:85fcfcb7b137 1019
pommzorz 1:85fcfcb7b137 1020 // the angle is in radians
pommzorz 1:85fcfcb7b137 1021 template <typename T>
pommzorz 1:85fcfcb7b137 1022 inline quat<T> quat_from_axis_angle(const T x, const T y, const T z, const T angle)
pommzorz 1:85fcfcb7b137 1023 {
pommzorz 1:85fcfcb7b137 1024 return quat_from_axis_angle<T>(vec3<T>(x, y, z), angle);
pommzorz 1:85fcfcb7b137 1025 }
pommzorz 1:85fcfcb7b137 1026
pommzorz 1:85fcfcb7b137 1027 // the angle is stored in radians
pommzorz 1:85fcfcb7b137 1028 template <typename T>
pommzorz 1:85fcfcb7b137 1029 inline void quat_to_axis_angle(const quat<T>& qq, vec3<T>* axis, T *angle)
pommzorz 1:85fcfcb7b137 1030 {
pommzorz 1:85fcfcb7b137 1031 quat<T> q = normalize(qq);
pommzorz 1:85fcfcb7b137 1032
pommzorz 1:85fcfcb7b137 1033 *angle = 2 * acos(q.w);
pommzorz 1:85fcfcb7b137 1034
pommzorz 1:85fcfcb7b137 1035 const T s = sin((*angle) * inv(T(2)));
pommzorz 1:85fcfcb7b137 1036 if ( s != T(0) )
pommzorz 1:85fcfcb7b137 1037 *axis = q.v * inv(s);
pommzorz 1:85fcfcb7b137 1038 else
pommzorz 1:85fcfcb7b137 1039 * axis = vec3<T>(T(0), T(0), T(0));
pommzorz 1:85fcfcb7b137 1040 }
pommzorz 1:85fcfcb7b137 1041
pommzorz 1:85fcfcb7b137 1042 // Spherical linear interpolation
pommzorz 1:85fcfcb7b137 1043 template <typename T>
pommzorz 1:85fcfcb7b137 1044 inline quat<T> slerp(const quat<T>& qq1, const quat<T>& qq2, const T t)
pommzorz 1:85fcfcb7b137 1045 {
pommzorz 1:85fcfcb7b137 1046 // slerp(q1,q2) = sin((1-t)*a)/sin(a) * q1 + sin(t*a)/sin(a) * q2
pommzorz 1:85fcfcb7b137 1047 const quat<T> q1 = normalize(qq1);
pommzorz 1:85fcfcb7b137 1048 const quat<T> q2 = normalize(qq2);
pommzorz 1:85fcfcb7b137 1049
pommzorz 1:85fcfcb7b137 1050 const T a = acos(dot(q1, q2));
pommzorz 1:85fcfcb7b137 1051 const T s = sin(a);
pommzorz 1:85fcfcb7b137 1052
pommzorz 1:85fcfcb7b137 1053 #define EPS T(1e-5)
pommzorz 1:85fcfcb7b137 1054
pommzorz 1:85fcfcb7b137 1055 if ( !(-EPS <= s && s <= EPS) ) {
pommzorz 1:85fcfcb7b137 1056 return sin((T(1)-t)*a)/s * q1 + sin(t*a)/s * q2;
pommzorz 1:85fcfcb7b137 1057 } else {
pommzorz 1:85fcfcb7b137 1058 // if the angle is to small use a linear interpolation
pommzorz 1:85fcfcb7b137 1059 return lerp(q1, q2, t);
pommzorz 1:85fcfcb7b137 1060 }
pommzorz 1:85fcfcb7b137 1061
pommzorz 1:85fcfcb7b137 1062 #undef EPS
pommzorz 1:85fcfcb7b137 1063 }
pommzorz 1:85fcfcb7b137 1064
pommzorz 1:85fcfcb7b137 1065 // Sperical quadtratic interpolation using a smooth cubic spline
pommzorz 1:85fcfcb7b137 1066 // The parameters a and b are the control points.
pommzorz 1:85fcfcb7b137 1067 template <typename T>
pommzorz 1:85fcfcb7b137 1068 inline quat<T> squad(
pommzorz 1:85fcfcb7b137 1069 const quat<T>& q0,
pommzorz 1:85fcfcb7b137 1070 const quat<T>& a,
pommzorz 1:85fcfcb7b137 1071 const quat<T>& b,
pommzorz 1:85fcfcb7b137 1072 const quat<T>& q1,
pommzorz 1:85fcfcb7b137 1073 const T t)
pommzorz 1:85fcfcb7b137 1074 {
pommzorz 1:85fcfcb7b137 1075 return slerp(slerp(q0, q1, t),slerp(a, b, t), 2 * t * (1 - t));
pommzorz 1:85fcfcb7b137 1076 }
pommzorz 1:85fcfcb7b137 1077
pommzorz 1:85fcfcb7b137 1078 #undef MOP_M_CLASS_TEMPLATE
pommzorz 1:85fcfcb7b137 1079 #undef MOP_M_TYPE_TEMPLATE
pommzorz 1:85fcfcb7b137 1080 #undef MOP_COMP_TEMPLATE
pommzorz 1:85fcfcb7b137 1081 #undef MOP_G_UMINUS_TEMPLATE
pommzorz 1:85fcfcb7b137 1082 #undef COMMON_OPERATORS
pommzorz 1:85fcfcb7b137 1083 #undef VECTOR_COMMON
pommzorz 1:85fcfcb7b137 1084 #undef FOP_G_SOURCE_TEMPLATE
pommzorz 1:85fcfcb7b137 1085 #undef FOP_G_CLASS_TEMPLATE
pommzorz 1:85fcfcb7b137 1086 #undef FOP_G_TYPE_TEMPLATE
pommzorz 1:85fcfcb7b137 1087 #undef VEC_QUAT_FUNC_TEMPLATE
pommzorz 1:85fcfcb7b137 1088 #undef VEC_FUNC_TEMPLATE
pommzorz 1:85fcfcb7b137 1089 #undef MATRIX_COL4
pommzorz 1:85fcfcb7b137 1090 #undef MATRIX_ROW4
pommzorz 1:85fcfcb7b137 1091 #undef MATRIX_COL3
pommzorz 1:85fcfcb7b137 1092 #undef MATRIX_ROW3
pommzorz 1:85fcfcb7b137 1093 #undef MATRIX_COL2
pommzorz 1:85fcfcb7b137 1094 #undef MATRIX_ROW2
pommzorz 1:85fcfcb7b137 1095 #undef MOP_M_MATRIX_MULTIPLY
pommzorz 1:85fcfcb7b137 1096 #undef MATRIX_CONSTRUCTOR_FROM_T
pommzorz 1:85fcfcb7b137 1097 #undef MATRIX_CONSTRUCTOR_FROM_LOWER
pommzorz 1:85fcfcb7b137 1098 #undef MATRIX_COMMON
pommzorz 1:85fcfcb7b137 1099 #undef MATRIX_CONSTRUCTOR_FROM_HIGHER
pommzorz 1:85fcfcb7b137 1100 #undef MAT_FUNC_TEMPLATE
pommzorz 1:85fcfcb7b137 1101 #undef MAT_FUNC_MINOR_TEMPLATE
pommzorz 1:85fcfcb7b137 1102 #undef MAT_ADJOINT_TEMPLATE
pommzorz 1:85fcfcb7b137 1103 #undef MAT_INVERSE_TEMPLATE
pommzorz 1:85fcfcb7b137 1104 #undef MAT_VEC_FUNCS_TEMPLATE
pommzorz 1:85fcfcb7b137 1105 #undef MAT_TRANFORMS_TEMPLATE
pommzorz 1:85fcfcb7b137 1106 #undef MAT_OUTERPRODUCT_TEMPLATE
pommzorz 1:85fcfcb7b137 1107 #undef FREE_MODIFYING_OPERATORS
pommzorz 1:85fcfcb7b137 1108 #undef FREE_OPERATORS
pommzorz 1:85fcfcb7b137 1109
pommzorz 1:85fcfcb7b137 1110 } // end namespace vmath
pommzorz 1:85fcfcb7b137 1111
pommzorz 1:85fcfcb7b137 1112 #endif
pommzorz 1:85fcfcb7b137 1113
pommzorz 1:85fcfcb7b137 1114
pommzorz 1:85fcfcb7b137 1115
pommzorz 1:85fcfcb7b137 1116