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