Quadrirotor

Dependencies:   CommonTypes ESC Matrix PID Servo kalman mbed-rtos mbed

Fork of Nucleo_MPU_9250 by Alan Huchin Herrera

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers vector_math.h Source File

vector_math.h

00001 /*
00002 Copyright (c) 2007, Markus Trenkwalder
00003 
00004 All rights reserved.
00005 
00006 Redistribution and use in source and binary forms, with or without 
00007 modification, are permitted provided that the following conditions are met:
00008 
00009 * Redistributions of source code must retain the above copyright notice, 
00010   this list of conditions and the following disclaimer.
00011 
00012 * Redistributions in binary form must reproduce the above copyright notice,
00013   this list of conditions and the following disclaimer in the documentation 
00014   and/or other materials provided with the distribution.
00015 
00016 * Neither the name of the library's copyright owner nor the names of its 
00017   contributors may be used to endorse or promote products derived from this 
00018   software without specific prior written permission.
00019 
00020 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
00023 A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
00024 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
00025 EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
00026 PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
00027 PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00028 LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
00029 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00030 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00031 */
00032 
00033 #ifndef VECTOR_MATH_H
00034 #define VECTOR_MATH_H
00035 
00036 //#include <cmath>
00037 
00038 // "minor" can be defined from GCC and can cause problems
00039 #undef minor
00040 
00041 #ifndef M_PI
00042 #define M_PI 3.14159265358979323846
00043 #endif
00044 
00045 namespace vmath {
00046 
00047 //using std::sin;
00048 //using std::cos;
00049 //using std::acos;
00050 //using std::sqrt;
00051 
00052 template <typename T>
00053 inline T rsqrt(T x)
00054 {
00055     return T(1) / sqrt(x);
00056 }
00057 
00058 template <typename T>
00059 inline T inv(T x)
00060 {
00061     return T(1) / x;
00062 }
00063 
00064 namespace detail {
00065     // This function is used heavily in this library. Here is a generic 
00066     // implementation for it. If you can provide a faster one for your specific
00067     // types this can speed up things considerably.
00068     template <typename T>
00069     inline T multiply_accumulate(int count, const T *a, const T *b)
00070     {
00071         T result = T(0);
00072         for (int i = 0; i < count; ++i)
00073             result += a[i] * b[i];
00074         return result;
00075     }
00076 }
00077 
00078 #define MOP_M_CLASS_TEMPLATE(CLASS, OP, COUNT) \
00079     CLASS & operator OP (const CLASS& rhs)  \
00080     { \
00081         for (int i = 0; i < (COUNT); ++i )  \
00082             (*this)[i] OP rhs[i]; \
00083         return *this; \
00084     }
00085 
00086 #define MOP_M_TYPE_TEMPLATE(CLASS, OP, COUNT) \
00087     CLASS & operator OP (const T & rhs) \
00088     { \
00089         for (int i = 0; i < (COUNT); ++i ) \
00090             (*this)[i] OP rhs; \
00091         return *this; \
00092     }
00093 
00094 #define MOP_COMP_TEMPLATE(CLASS, COUNT) \
00095     bool operator == (const CLASS & rhs) \
00096     { \
00097         bool result = true; \
00098         for (int i = 0; i < (COUNT); ++i) \
00099             result = result && (*this)[i] == rhs[i]; \
00100         return result; \
00101     } \
00102     bool operator != (const CLASS & rhs) \
00103     { return !((*this) == rhs); }
00104 
00105 #define MOP_G_UMINUS_TEMPLATE(CLASS, COUNT) \
00106     CLASS operator - () const \
00107     { \
00108         CLASS result; \
00109         for (int i = 0; i < (COUNT); ++i) \
00110             result[i] = -(*this)[i]; \
00111         return result; \
00112     }
00113 
00114 #define COMMON_OPERATORS(CLASS, COUNT) \
00115     MOP_M_CLASS_TEMPLATE(CLASS, +=, COUNT) \
00116     MOP_M_CLASS_TEMPLATE(CLASS, -=, COUNT) \
00117     /*no *= as this is not the same for vectors and matrices */ \
00118     MOP_M_CLASS_TEMPLATE(CLASS, /=, COUNT) \
00119     MOP_M_TYPE_TEMPLATE(CLASS, +=, COUNT) \
00120     MOP_M_TYPE_TEMPLATE(CLASS, -=, COUNT) \
00121     MOP_M_TYPE_TEMPLATE(CLASS, *=, COUNT) \
00122     MOP_M_TYPE_TEMPLATE(CLASS, /=, COUNT) \
00123     MOP_G_UMINUS_TEMPLATE(CLASS, COUNT) \
00124     MOP_COMP_TEMPLATE(CLASS, COUNT)
00125 
00126 #define VECTOR_COMMON(CLASS, COUNT) \
00127     COMMON_OPERATORS(CLASS, COUNT) \
00128     MOP_M_CLASS_TEMPLATE(CLASS, *=, COUNT) \
00129     operator const T* () const { return &x; } \
00130     operator T* () { return &x; }
00131 
00132 #define FOP_G_SOURCE_TEMPLATE(OP, CLASS) \
00133     { CLASS<T> r = lhs; r OP##= rhs; return r; }
00134 
00135 #define FOP_G_CLASS_TEMPLATE(OP, CLASS) \
00136     template <typename T> \
00137     inline CLASS<T> operator OP (const CLASS<T> &lhs, const CLASS<T> &rhs) \
00138     FOP_G_SOURCE_TEMPLATE(OP, CLASS)
00139 
00140 #define FOP_G_TYPE_TEMPLATE(OP, CLASS) \
00141     template <typename T> \
00142     inline CLASS<T> operator OP (const CLASS<T> &lhs, const T &rhs) \
00143     FOP_G_SOURCE_TEMPLATE(OP, CLASS)
00144 
00145 // forward declarations
00146 template <typename T> struct vec2;
00147 template <typename T> struct vec3;
00148 template <typename T> struct vec4;
00149 template <typename T> struct mat2;
00150 template <typename T> struct mat3;
00151 template <typename T> struct mat4;
00152 template <typename T> struct quat;
00153 
00154 #define FREE_MODIFYING_OPERATORS(CLASS) \
00155     FOP_G_CLASS_TEMPLATE(+, CLASS) \
00156     FOP_G_CLASS_TEMPLATE(-, CLASS) \
00157     FOP_G_CLASS_TEMPLATE(*, CLASS) \
00158     FOP_G_CLASS_TEMPLATE(/, CLASS) \
00159     FOP_G_TYPE_TEMPLATE(+, CLASS) \
00160     FOP_G_TYPE_TEMPLATE(-, CLASS) \
00161     FOP_G_TYPE_TEMPLATE(*, CLASS) \
00162     FOP_G_TYPE_TEMPLATE(/, CLASS)
00163 
00164 FREE_MODIFYING_OPERATORS(vec2)
00165 FREE_MODIFYING_OPERATORS(vec3)
00166 FREE_MODIFYING_OPERATORS(vec4)
00167 FREE_MODIFYING_OPERATORS(mat2)
00168 FREE_MODIFYING_OPERATORS(mat3)
00169 FREE_MODIFYING_OPERATORS(mat4)
00170 FREE_MODIFYING_OPERATORS(quat)
00171 
00172 #define FREE_OPERATORS(CLASS) \
00173     template <typename T> \
00174     inline CLASS<T> operator + (const T& a, const CLASS<T>& b)  \
00175     { CLASS<T> r = b; r += a; return r; } \
00176     \
00177     template <typename T> \
00178     inline CLASS<T> operator * (const T& a, const CLASS<T>& b)  \
00179     { CLASS<T> r = b; r *= a; return r; } \
00180     \
00181     template <typename T> \
00182     inline CLASS<T> operator - (const T& a, const CLASS<T>& b)  \
00183     { return -b + a; } \
00184     \
00185     template <typename T> \
00186     inline CLASS<T> operator / (const T& a, const CLASS<T>& b)  \
00187     { CLASS<T> r(a); r /= b; return r; }
00188 
00189 FREE_OPERATORS(vec2)
00190 FREE_OPERATORS(vec3)
00191 FREE_OPERATORS(vec4)
00192 FREE_OPERATORS(mat2)
00193 FREE_OPERATORS(mat3)
00194 FREE_OPERATORS(mat4)
00195 FREE_OPERATORS(quat)
00196 
00197 template <typename T>
00198 struct vec2 {
00199     T x, y;
00200     
00201     vec2() {};
00202     explicit vec2(const T i) : x(i), y(i) {}
00203     explicit vec2(const T ix, const T iy) : x(ix), y(iy) {}
00204     explicit vec2(const vec3<T>& v);
00205     explicit vec2(const vec4<T>& v);
00206 
00207     VECTOR_COMMON(vec2, 2)
00208 };
00209 
00210 template <typename T> 
00211 struct vec3 {
00212     T x, y, z;
00213     
00214     vec3() {};
00215     explicit vec3(const T i) : x(i), y(i), z(i) {}
00216     explicit vec3(const T ix, const T iy, const T iz) : x(ix), y(iy), z(iz) {}
00217     explicit vec3(const vec2<T>& xy, const T iz) : x(xy.x), y(xy.y), z(iz) {}
00218     explicit vec3(const T ix, const vec2<T>& yz) : x(ix), y(yz.y), z(yz.z) {}
00219     explicit vec3(const vec4<T>& v);
00220 
00221     VECTOR_COMMON(vec3, 3)
00222 };
00223 
00224 template <typename T> 
00225 struct vec4 {
00226     T x, y, z, w;
00227     
00228     vec4() {};
00229     explicit vec4(const T i) : x(i), y(i), z(i), w(i) {}
00230     explicit vec4(const T ix, const T iy, const T iz, const T iw) : x(ix), y(iy), z(iz), w(iw) {}
00231     explicit vec4(const vec3<T>& xyz,const T iw) : x(xyz.x), y(xyz.y), z(xyz.z), w(iw) {}
00232     explicit vec4(const T ix, const vec3<T>& yzw) : x(ix), y(yzw.x), z(yzw.y), w(yzw.z) {}
00233     explicit vec4(const vec2<T>& xy, const vec2<T>& zw) : x(xy.x), y(xy.y), z(zw.x), w(zw.y) {}
00234 
00235     VECTOR_COMMON(vec4, 4)
00236 };
00237 
00238 // additional constructors that omit the last element
00239 template <typename T> inline vec2<T>::vec2(const vec3<T>& v) : x(v.x), y(v.y) {}
00240 template <typename T> inline vec2<T>::vec2(const vec4<T>& v) : x(v.x), y(v.y) {}
00241 template <typename T> inline vec3<T>::vec3(const vec4<T>& v) : x(v.x), y(v.y), z(v.z) {}
00242 
00243 #define VEC_QUAT_FUNC_TEMPLATE(CLASS, COUNT) \
00244     template <typename T> \
00245     inline T dot(const CLASS & u, const CLASS & v) \
00246     { \
00247         const T *a = u; \
00248         const T *b = v; \
00249         using namespace detail; \
00250         return multiply_accumulate(COUNT, a, b); \
00251     } \
00252     template <typename T> \
00253     inline T length(const CLASS & v) \
00254     { \
00255         return sqrt(dot(v, v)); \
00256     } \
00257     template <typename T> inline CLASS normalize(const CLASS & v) \
00258     { \
00259         return v * rsqrt(dot(v, v)); \
00260     } \
00261     template <typename T> inline CLASS lerp(const CLASS & u, const CLASS & v, const T x) \
00262     { \
00263         return u * (T(1) - x) + v * x; \
00264     }
00265 
00266 VEC_QUAT_FUNC_TEMPLATE(vec2<T>, 2)
00267 VEC_QUAT_FUNC_TEMPLATE(vec3<T>, 3)
00268 VEC_QUAT_FUNC_TEMPLATE(vec4<T>, 4)
00269 VEC_QUAT_FUNC_TEMPLATE(quat<T>, 4)
00270 
00271 #define VEC_FUNC_TEMPLATE(CLASS) \
00272     template <typename T> inline CLASS reflect(const CLASS & I, const CLASS & N) \
00273     { \
00274         return I - T(2) * dot(N, I) * N; \
00275     } \
00276     template <typename T> inline CLASS refract(const CLASS & I, const CLASS & N, T eta) \
00277     { \
00278         const T d = dot(N, I); \
00279         const T k = T(1) - eta * eta * (T(1) - d * d); \
00280         if ( k < T(0) ) \
00281             return CLASS(T(0)); \
00282         else \
00283             return eta * I - (eta * d + static_cast<T>(sqrt(k))) * N; \
00284     } 
00285 
00286 VEC_FUNC_TEMPLATE(vec2<T>)
00287 VEC_FUNC_TEMPLATE(vec3<T>)
00288 VEC_FUNC_TEMPLATE(vec4<T>)
00289 
00290 template <typename T> inline T lerp(const T & u, const T & v, const T x) 
00291 { 
00292     return dot(vec2<T>(u, v), vec2<T>((T(1) - x), x));
00293 }
00294 
00295 template <typename T> inline vec3<T> cross(const vec3<T>& u, const vec3<T>& v)
00296 {
00297     return vec3<T>(
00298         dot(vec2<T>(u.y, -v.y), vec2<T>(v.z, u.z)),
00299         dot(vec2<T>(u.z, -v.z), vec2<T>(v.x, u.x)),
00300         dot(vec2<T>(u.x, -v.x), vec2<T>(v.y, u.y)));
00301 }
00302 
00303 
00304 #define MATRIX_COL4(SRC, C) \
00305     vec4<T>(SRC.elem[0][C], SRC.elem[1][C], SRC.elem[2][C], SRC.elem[3][C])
00306 
00307 #define MATRIX_ROW4(SRC, R) \
00308     vec4<T>(SRC.elem[R][0], SRC.elem[R][1], SRC.elem[R][2], SRC.elem[R][3])
00309 
00310 #define MATRIX_COL3(SRC, C) \
00311     vec3<T>(SRC.elem[0][C], SRC.elem[1][C], SRC.elem[2][C])
00312 
00313 #define MATRIX_ROW3(SRC, R) \
00314     vec3<T>(SRC.elem[R][0], SRC.elem[R][1], SRC.elem[R][2])
00315 
00316 #define MATRIX_COL2(SRC, C) \
00317     vec2<T>(SRC.elem[0][C], SRC.elem[1][C])
00318 
00319 #define MATRIX_ROW2(SRC, R) \
00320     vec2<T>(SRC.elem[R][0], SRC.elem[R][1])
00321 
00322 #define MOP_M_MATRIX_MULTIPLY(CLASS, SIZE) \
00323     CLASS & operator *= (const CLASS & rhs) \
00324     { \
00325         CLASS result; \
00326         for (int r = 0; r < SIZE; ++r) \
00327         for (int c = 0; c < SIZE; ++c) \
00328             result.elem[r][c] = dot( \
00329                 MATRIX_ROW ## SIZE((*this), r), \
00330                 MATRIX_COL ## SIZE(rhs, c)); \
00331         return (*this) = result; \
00332     }
00333 
00334 #define MATRIX_CONSTRUCTOR_FROM_T(CLASS, SIZE) \
00335     explicit CLASS(const T v) \
00336     { \
00337         for (int r = 0; r < SIZE; ++r) \
00338         for (int c = 0; c < SIZE; ++c) \
00339             if (r == c) elem[r][c] = v; \
00340             else elem[r][c] = T(0); \
00341     }
00342 
00343 #define MATRIX_CONSTRUCTOR_FROM_LOWER(CLASS1, CLASS2, SIZE1, SIZE2) \
00344     explicit CLASS1(const CLASS2<T>& m) \
00345     { \
00346         for (int r = 0; r < SIZE1; ++r) \
00347         for (int c = 0; c < SIZE1; ++c) \
00348             if (r < SIZE2 && c < SIZE2) elem[r][c] = m.elem[r][c]; \
00349             else elem[r][c] = r == c ? T(1) : T(0); \
00350     }
00351 
00352 #define MATRIX_COMMON(CLASS, SIZE) \
00353     COMMON_OPERATORS(CLASS, SIZE*SIZE) \
00354     MOP_M_MATRIX_MULTIPLY(CLASS, SIZE) \
00355     MATRIX_CONSTRUCTOR_FROM_T(CLASS, SIZE) \
00356     operator const T* () const { return (const T*) elem; } \
00357     operator T* () { return (T*) elem; }
00358 
00359 template <typename T> struct mat2;
00360 template <typename T> struct mat3;
00361 template <typename T> struct mat4;
00362 
00363 template <typename T> 
00364 struct mat2  {
00365     T elem[2][2];
00366     
00367     mat2() {}
00368     
00369     explicit mat2(
00370         const T m00, const T m01,
00371         const T m10, const T m11)
00372     {
00373         elem[0][0] = m00; elem[0][1] = m01;
00374         elem[1][0] = m10; elem[1][1] = m11;
00375     }
00376 
00377     explicit mat2(const vec2<T>& v0, const vec2<T>& v1)
00378     {
00379         elem[0][0] = v0[0];
00380         elem[1][0] = v0[1];
00381         elem[0][1] = v1[0];
00382         elem[1][1] = v1[1];
00383     }
00384 
00385     explicit mat2(const mat3<T>& m);
00386 
00387     MATRIX_COMMON(mat2, 2)
00388 };
00389 
00390 template <typename T> 
00391 struct mat3 {
00392     T elem[3][3];
00393     
00394     mat3() {}
00395     
00396     explicit mat3(
00397         const T m00, const T m01, const T m02,
00398         const T m10, const T m11, const T m12,
00399         const T m20, const T m21, const T m22)
00400     {
00401         elem[0][0] = m00; elem[0][1] = m01; elem[0][2] = m02;
00402         elem[1][0] = m10; elem[1][1] = m11; elem[1][2] = m12;
00403         elem[2][0] = m20; elem[2][1] = m21; elem[2][2] = m22;
00404     }
00405     
00406     explicit mat3(const vec3<T>& v0, const vec3<T>& v1, const vec3<T>& v2)
00407     {
00408         elem[0][0] = v0[0];
00409         elem[1][0] = v0[1];
00410         elem[2][0] = v0[2];
00411         elem[0][1] = v1[0];
00412         elem[1][1] = v1[1];
00413         elem[2][1] = v1[2];
00414         elem[0][2] = v2[0];
00415         elem[1][2] = v2[1];
00416         elem[2][2] = v2[2];
00417     }
00418 
00419     explicit mat3(const mat4<T>& m);
00420 
00421     MATRIX_CONSTRUCTOR_FROM_LOWER(mat3, mat2, 3, 2)
00422     MATRIX_COMMON(mat3, 3)
00423 };
00424 
00425 template <typename T> 
00426 struct mat4 {
00427     T elem[4][4];
00428     
00429     mat4() {}
00430 
00431     explicit mat4(
00432         const T m00, const T m01, const T m02, const T m03,
00433         const T m10, const T m11, const T m12, const T m13,
00434         const T m20, const T m21, const T m22, const T m23,
00435         const T m30, const T m31, const T m32, const T m33)
00436     {
00437         elem[0][0] = m00; elem[0][1] = m01; elem[0][2] = m02; elem[0][3] = m03;
00438         elem[1][0] = m10; elem[1][1] = m11; elem[1][2] = m12; elem[1][3] = m13;
00439         elem[2][0] = m20; elem[2][1] = m21; elem[2][2] = m22; elem[2][3] = m23;
00440         elem[3][0] = m30; elem[3][1] = m31; elem[3][2] = m32; elem[3][3] = m33;
00441     }
00442 
00443     explicit mat4(const vec4<T>& v0, const vec4<T>& v1, const vec4<T>& v2, const vec4<T>& v3)
00444     {
00445         elem[0][0] = v0[0];
00446         elem[1][0] = v0[1];
00447         elem[2][0] = v0[2];
00448         elem[3][0] = v0[3];
00449         elem[0][1] = v1[0];
00450         elem[1][1] = v1[1];
00451         elem[2][1] = v1[2];
00452         elem[3][1] = v1[3];
00453         elem[0][2] = v2[0];
00454         elem[1][2] = v2[1];
00455         elem[2][2] = v2[2];
00456         elem[3][2] = v2[3];
00457         elem[0][3] = v3[0];
00458         elem[1][3] = v3[1];
00459         elem[2][3] = v3[2];
00460         elem[3][3] = v3[3];
00461     }
00462 
00463     MATRIX_CONSTRUCTOR_FROM_LOWER(mat4, mat3, 4, 3)
00464     MATRIX_COMMON(mat4, 4)
00465 };
00466 
00467 #define MATRIX_CONSTRUCTOR_FROM_HIGHER(CLASS1, CLASS2, SIZE) \
00468     template <typename T> \
00469     inline CLASS1<T>::CLASS1(const CLASS2<T>& m) \
00470     { \
00471         for (int r = 0; r < SIZE; ++r) \
00472         for (int c = 0; c < SIZE; ++c) \
00473             elem[r][c] = m.elem[r][c]; \
00474     }
00475 
00476 MATRIX_CONSTRUCTOR_FROM_HIGHER(mat2, mat3, 2)
00477 MATRIX_CONSTRUCTOR_FROM_HIGHER(mat3, mat4, 3)
00478 
00479 #define MAT_FUNC_TEMPLATE(CLASS, SIZE) \
00480     template <typename T>  \
00481     inline CLASS transpose(const CLASS & m) \
00482     { \
00483         CLASS result; \
00484         for (int r = 0; r < SIZE; ++r) \
00485         for (int c = 0; c < SIZE; ++c) \
00486             result.elem[r][c] = m.elem[c][r]; \
00487         return result; \
00488     } \
00489     template <typename T>  \
00490     inline CLASS identity ## SIZE() \
00491     { \
00492         CLASS result; \
00493         for (int r = 0; r < SIZE; ++r) \
00494         for (int c = 0; c < SIZE; ++c) \
00495             result.elem[r][c] = r == c ? T(1) : T(0); \
00496         return result; \
00497     } \
00498     template <typename T> \
00499     inline T trace(const CLASS & m) \
00500     { \
00501         T result = T(0); \
00502         for (int i = 0; i < SIZE; ++i) \
00503             result += m.elem[i][i]; \
00504         return result; \
00505     }
00506 
00507 MAT_FUNC_TEMPLATE(mat2<T>, 2)
00508 MAT_FUNC_TEMPLATE(mat3<T>, 3)
00509 MAT_FUNC_TEMPLATE(mat4<T>, 4)
00510 
00511 #define MAT_FUNC_MINOR_TEMPLATE(CLASS1, CLASS2, SIZE) \
00512     template <typename T>  \
00513     inline CLASS2 minor(const CLASS1 & m, int _r = SIZE, int _c = SIZE) { \
00514         CLASS2 result; \
00515         for (int r = 0; r < SIZE - 1; ++r) \
00516         for (int c = 0; c < SIZE - 1; ++c) { \
00517             int rs = r >= _r ? 1 : 0; \
00518             int cs = c >= _c ? 1 : 0; \
00519             result.elem[r][c] = m.elem[r + rs][c + cs]; \
00520         } \
00521         return result; \
00522     }
00523 
00524 MAT_FUNC_MINOR_TEMPLATE(mat3<T>, mat2<T>, 3)
00525 MAT_FUNC_MINOR_TEMPLATE(mat4<T>, mat3<T>, 4)
00526 
00527 template <typename T>
00528 inline T det(const mat2<T>& m)
00529 {
00530     return dot(
00531         vec2<T>(m.elem[0][0], -m.elem[0][1]), 
00532         vec2<T>(m.elem[1][1], m.elem[1][0]));
00533 }
00534 
00535 template <typename T>
00536 inline T det(const mat3<T>& m)
00537 {
00538     return dot(cross(MATRIX_COL3(m, 0), MATRIX_COL3(m, 1)), MATRIX_COL3(m, 2));
00539 }
00540 
00541 template <typename T>
00542 inline T det(const mat4<T>& m)
00543 {
00544     vec4<T> b;
00545     for (int i = 0; i < 4; ++i)
00546         b[i] = (i & 1 ? -1 : 1) * det(minor(m, 0, i));
00547     return dot(MATRIX_ROW4(m, 0), b);
00548 }
00549 
00550 #define MAT_ADJOINT_TEMPLATE(CLASS, SIZE) \
00551     template <typename T> \
00552     inline CLASS adjoint(const CLASS & m) \
00553     { \
00554         CLASS result; \
00555         for (int r = 0; r < SIZE; ++r) \
00556         for (int c = 0; c < SIZE; ++c) \
00557             result.elem[r][c] = ((r + c) & 1 ? -1 : 1) * det(minor(m, c, r)); \
00558         return result; \
00559     }
00560 
00561 MAT_ADJOINT_TEMPLATE(mat3<T>, 3)
00562 MAT_ADJOINT_TEMPLATE(mat4<T>, 4)
00563 
00564 template <typename T>
00565 inline mat2<T> adjoint(const mat2<T> & m)
00566 {
00567     return mat2<T>(
00568          m.elem[1][1], -m.elem[0][1],
00569         -m.elem[1][0],  m.elem[0][0]
00570     );
00571 }
00572 
00573 #define MAT_INVERSE_TEMPLATE(CLASS) \
00574     template <typename T> \
00575     inline CLASS inverse(const CLASS & m) \
00576     { \
00577         return adjoint(m) * inv(det(m)); \
00578     }
00579 
00580 MAT_INVERSE_TEMPLATE(mat2<T>)
00581 MAT_INVERSE_TEMPLATE(mat3<T>)
00582 MAT_INVERSE_TEMPLATE(mat4<T>)
00583 
00584 #define MAT_VEC_FUNCS_TEMPLATE(MATCLASS, VECCLASS, SIZE) \
00585     template <typename T>  \
00586     inline VECCLASS operator * (const MATCLASS & m, const VECCLASS & v) \
00587     { \
00588         VECCLASS result; \
00589         for (int i = 0; i < SIZE; ++i) {\
00590             result[i] = dot(MATRIX_ROW ## SIZE(m, i), v); \
00591         } \
00592         return result; \
00593     } \
00594     template <typename T>  \
00595     inline VECCLASS operator * (const VECCLASS & v, const MATCLASS & m) \
00596     { \
00597         VECCLASS result; \
00598         for (int i = 0; i < SIZE; ++i) \
00599             result[i] = dot(v, MATRIX_COL ## SIZE(m, i)); \
00600         return result; \
00601     }
00602 
00603 MAT_VEC_FUNCS_TEMPLATE(mat2<T>, vec2<T>, 2)
00604 MAT_VEC_FUNCS_TEMPLATE(mat3<T>, vec3<T>, 3)
00605 MAT_VEC_FUNCS_TEMPLATE(mat4<T>, vec4<T>, 4)
00606 
00607 // Returns the inverse of a 4x4 matrix. It is assumed that the matrix passed
00608 // as argument describes a rigid-body transformation.
00609 template <typename T> 
00610 inline mat4<T> fast_inverse(const mat4<T>& m)
00611 {
00612     const vec3<T> t = MATRIX_COL3(m, 3);
00613     const T tx = -dot(MATRIX_COL3(m, 0), t);
00614     const T ty = -dot(MATRIX_COL3(m, 1), t);
00615     const T tz = -dot(MATRIX_COL3(m, 2), t);
00616 
00617     return mat4<T>(
00618         m.elem[0][0], m.elem[1][0], m.elem[2][0], tx,
00619         m.elem[0][1], m.elem[1][1], m.elem[2][1], ty,
00620         m.elem[0][2], m.elem[1][2], m.elem[2][2], tz,
00621         T(0), T(0), T(0), T(1)
00622     );
00623 }
00624 
00625 // Transformations for points and vectors. Potentially faster than a full 
00626 // matrix * vector multiplication
00627 
00628 #define MAT_TRANFORMS_TEMPLATE(MATCLASS, VECCLASS, VECSIZE) \
00629     /* computes vec3<T>(m * vec4<T>(v, 0.0)) */ \
00630     template <typename T>  \
00631     inline VECCLASS transform_vector(const MATCLASS & m, const VECCLASS & v) \
00632     { \
00633         VECCLASS result; \
00634         for (int i = 0; i < VECSIZE; ++i) \
00635             result[i] = dot(MATRIX_ROW ## VECSIZE(m, i), v); \
00636         return result;\
00637     } \
00638     /* computes vec3(m * vec4(v, 1.0)) */ \
00639     template <typename T>  \
00640     inline VECCLASS transform_point(const MATCLASS & m, const VECCLASS & v) \
00641     { \
00642         /*return transform_vector(m, v) + MATRIX_ROW ## VECSIZE(m, VECSIZE); */\
00643         VECCLASS result; \
00644         for (int i = 0; i < VECSIZE; ++i) \
00645             result[i] = dot(MATRIX_ROW ## VECSIZE(m, i), v) + m.elem[i][VECSIZE]; \
00646         return result; \
00647     } \
00648     /* computes VECCLASS(transpose(m) * vec4<T>(v, 0.0)) */ \
00649     template <typename T>  \
00650     inline VECCLASS transform_vector_transpose(const MATCLASS & m, const VECCLASS& v) \
00651     { \
00652         VECCLASS result; \
00653         for (int i = 0; i < VECSIZE; ++i) \
00654             result[i] = dot(MATRIX_COL ## VECSIZE(m, i), v); \
00655         return result; \
00656     } \
00657     /* computes VECCLASS(transpose(m) * vec4<T>(v, 1.0)) */ \
00658     template <typename T>  \
00659     inline VECCLASS transform_point_transpose(const MATCLASS & m, const VECCLASS& v) \
00660     { \
00661         /*return transform_vector_transpose(m, v) + MATRIX_COL ## VECSIZE(m, VECSIZE); */\
00662         VECCLASS result; \
00663         for (int i = 0; i < VECSIZE; ++i) \
00664             result[i] = dot(MATRIX_COL ## VECSIZE(m, i), v) + m.elem[VECSIZE][i]; \
00665         return result; \
00666     }
00667 
00668 MAT_TRANFORMS_TEMPLATE(mat4<T>, vec3<T>, 3)
00669 MAT_TRANFORMS_TEMPLATE(mat3<T>, vec2<T>, 2)
00670 
00671 #define MAT_OUTERPRODUCT_TEMPLATE(MATCLASS, VECCLASS, MATSIZE) \
00672     template <typename T> \
00673     inline MATCLASS outer_product(const VECCLASS & v1, const VECCLASS & v2) \
00674     { \
00675         MATCLASS r; \
00676         for ( int j = 0; j < MATSIZE; ++j ) \
00677         for ( int k = 0; k < MATSIZE; ++k ) \
00678             r.elem[j][k] = v1[j] * v2[k]; \
00679         return r; \
00680     }
00681 
00682 MAT_OUTERPRODUCT_TEMPLATE(mat4<T>, vec4<T>, 4)
00683 MAT_OUTERPRODUCT_TEMPLATE(mat3<T>, vec3<T>, 3)
00684 MAT_OUTERPRODUCT_TEMPLATE(mat2<T>, vec2<T>, 2)
00685 
00686 template <typename T>
00687 inline mat4<T> translation_matrix(const T x, const T y, const T z)
00688 {
00689     mat4<T> r(T(1));
00690     r.elem[0][3] = x;
00691     r.elem[1][3] = y;
00692     r.elem[2][3] = z;
00693     return r;
00694 }
00695 
00696 template <typename T> 
00697 inline mat4<T> translation_matrix(const vec3<T>& v)
00698 {
00699     return translation_matrix(v.x, v.y, v.z);
00700 }
00701 
00702 template <typename T> 
00703 inline mat4<T> scaling_matrix(const T x, const T y, const T z)
00704 {
00705     mat4<T> r(T(0));
00706     r.elem[0][0] = x;
00707     r.elem[1][1] = y;
00708     r.elem[2][2] = z;
00709     r.elem[3][3] = T(1);
00710     return r;
00711 }
00712 
00713 template <typename T>
00714 inline mat4<T> scaling_matrix(const vec3<T>& v)
00715 {
00716     return scaling_matrix(v.x, v.y, v.z);
00717 }
00718 
00719 template <typename T>
00720 inline mat4<T> rotation_matrix(const T angle, const vec3<T>& v)
00721 {
00722     const T a = angle * T(M_PI/180) ;
00723     const vec3<T> u = normalize(v);
00724     
00725     const mat3<T> S(
00726          T(0), -u[2],  u[1],
00727          u[2],  T(0), -u[0],
00728         -u[1],  u[0],  T(0) 
00729     );
00730     
00731     const mat3<T> uut = outer_product(u, u);
00732     const mat3<T> R = uut + T(cos(a)) * (identity3<T>() - uut) + T(sin(a)) * S;
00733     
00734     return mat4<T>(R);
00735 }
00736 
00737 
00738 template <typename T> 
00739 inline mat4<T> rotation_matrix(const T angle, const T x, const T y, const T z)
00740 {
00741     return rotation_matrix(angle, vec3<T>(x, y, z));
00742 }
00743 
00744 // Constructs a shear-matrix that shears component i by factor with
00745 // Respect to component j.
00746 template <typename T> 
00747 inline mat4<T> shear_matrix(const int i, const int j, const T factor)
00748 {
00749     mat4<T> m = identity4<T>();
00750     m.elem[i][j] = factor;
00751     return m;
00752 }
00753 
00754 template <typename T> 
00755 inline mat4<T> euler(const T head, const T pitch, const T roll)
00756 {
00757     return rotation_matrix(roll, T(0), T(0), T(1)) * 
00758         rotation_matrix(pitch, T(1), T(0), T(0)) * 
00759         rotation_matrix(head, T(0), T(1), T(0));
00760 }
00761 
00762 template <typename T> 
00763 inline mat4<T> frustum_matrix(const T l, const T r, const T b, const T t, const T n, const T f)
00764 {
00765     return mat4<T>(
00766         (2 * n)/(r - l), T(0), (r + l)/(r - l), T(0),
00767         T(0), (2 * n)/(t - b), (t + b)/(t - b), T(0),
00768         T(0), T(0), -(f + n)/(f - n), -(2 * f * n)/(f - n),
00769         T(0), T(0), -T(1), T(0)
00770     );
00771 }
00772 
00773 template <typename T>
00774 inline mat4<T> perspective_matrix(const T fovy, const T aspect, const T zNear, const T zFar)
00775 {
00776     const T dz = zFar - zNear;
00777     const T rad = fovy / T(2) * T(M_PI/180);
00778     const T s = sin(rad);
00779     
00780     if ( ( dz == T(0) ) || ( s == T(0) ) || ( aspect == T(0) ) ) {
00781         return identity4<T>();
00782     }
00783     
00784     const T cot = cos(rad) / s;
00785     
00786     mat4<T> m = identity4<T>();
00787     m[0]  = cot / aspect;
00788     m[5]  = cot;
00789     m[10] = -(zFar + zNear) / dz;
00790     m[14] = T(-1);
00791     m[11] = -2 * zNear * zFar / dz;
00792     m[15] = T(0);
00793     
00794     return m;
00795 }
00796 
00797 template <typename T>
00798 inline mat4<T> ortho_matrix(const T l, const T r, const T b, const T t, const T n, const T f)
00799 {
00800     return mat4<T>(
00801         T(2)/(r - l), T(0), T(0), -(r + l)/(r - l),
00802         T(0), T(2)/(t - b), T(0), -(t + b)/(t - b),
00803         T(0), T(0), -T(2)/(f - n), -(f + n)/(f - n),
00804         T(0), T(0), T(0), T(1)
00805     );
00806 }
00807 
00808 template <typename T>
00809 inline mat4<T> lookat_matrix(const vec3<T>& eye, const vec3<T>& center, const vec3<T>& up) {
00810     const vec3<T> forward = normalize(center - eye);
00811     const vec3<T> side = normalize(cross(forward, up));
00812     
00813     const vec3<T> up2 = cross(side, forward);
00814     
00815     mat4<T> m = identity4<T>();
00816     
00817     m.elem[0][0] = side[0];
00818     m.elem[0][1] = side[1];
00819     m.elem[0][2] = side[2];
00820     
00821     m.elem[1][0] = up2[0];
00822     m.elem[1][1] = up2[1];
00823     m.elem[1][2] = up2[2];
00824     
00825     m.elem[2][0] = -forward[0];
00826     m.elem[2][1] = -forward[1];
00827     m.elem[2][2] = -forward[2];
00828     
00829     return m * translation_matrix(-eye);
00830 }
00831 
00832 template <typename T> 
00833 inline mat4<T> picking_matrix(const T x, const T y, const T dx, const T dy, int viewport[4]) {
00834     if (dx <= 0 || dy <= 0) { 
00835         return identity4<T>();
00836     }
00837 
00838     mat4<T> r = translation_matrix((viewport[2] - 2 * (x - viewport[0])) / dx,
00839         (viewport[3] - 2 * (y - viewport[1])) / dy,    0);
00840     r *= scaling_matrix(viewport[2] / dx, viewport[2] / dy, 1);
00841     return r;
00842 }
00843 
00844 // Constructs a shadow matrix. q is the light source and p is the plane.
00845 template <typename T> inline mat4<T> shadow_matrix(const vec4<T>& q, const vec4<T>& p) {
00846     mat4<T> m;
00847     
00848     m.elem[0][0] =  p.y * q[1] + p.z * q[2] + p.w * q[3];
00849     m.elem[0][1] = -p.y * q[0];
00850     m.elem[0][2] = -p.z * q[0];
00851     m.elem[0][3] = -p.w * q[0];
00852     
00853     m.elem[1][0] = -p.x * q[1];
00854     m.elem[1][1] =  p.x * q[0] + p.z * q[2] + p.w * q[3];
00855     m.elem[1][2] = -p.z * q[1];
00856     m.elem[1][3] = -p.w * q[1];
00857     
00858 
00859     m.elem[2][0] = -p.x * q[2];
00860     m.elem[2][1] = -p.y * q[2];
00861     m.elem[2][2] =  p.x * q[0] + p.y * q[1] + p.w * q[3];
00862     m.elem[2][3] = -p.w * q[2];
00863 
00864     m.elem[3][1] = -p.x * q[3];
00865     m.elem[3][2] = -p.y * q[3];
00866     m.elem[3][3] = -p.z * q[3];
00867     m.elem[3][0] =  p.x * q[0] + p.y * q[1] + p.z * q[2];
00868 
00869     return m;
00870 }
00871 
00872 // Quaternion class
00873 template <typename T> 
00874 struct quat {
00875     vec3<T>    v;
00876     T w;
00877 
00878     quat() {}
00879     quat(const vec3<T>& iv, const T iw) : v(iv), w(iw) {}
00880     quat(const T vx, const T vy, const T vz, const T iw) : v(vx, vy, vz), w(iw)    {}
00881     quat(const vec4<T>& i) : v(i.x, i.y, i.z), w(i.w) {}
00882 
00883     operator const T* () const { return &(v[0]); }
00884     operator T* () { return &(v[0]); }    
00885 
00886     quat& operator += (const quat& q) { v += q.v; w += q.w; return *this; }
00887     quat& operator -= (const quat& q) { v -= q.v; w -= q.w; return *this; }
00888 
00889     quat& operator *= (const T& s) { v *= s; w *= s; return *this; }
00890     quat& operator /= (const T& s) { v /= s; w /= s; return *this; }
00891 
00892     quat& operator *= (const quat& r)
00893     {
00894         //q1 x q2 = [s1,v1] x [s2,v2] = [(s1*s2 - v1*v2),(s1*v2 + s2*v1 + v1xv2)].
00895         quat q;
00896         q.v = cross(v, r.v) + r.w * v + w * r.v;
00897         q.w = w * r.w - dot(v, r.v);
00898         return *this = q;
00899     }
00900 
00901     quat& operator /= (const quat& q) { return (*this) *= inverse(q); }
00902 };
00903 
00904 // Quaternion functions
00905 
00906 template <typename T> 
00907 inline quat<T> identityq()
00908 {
00909     return quat<T>(T(0), T(0), T(0), T(1));
00910 }
00911 
00912 template <typename T> 
00913 inline quat<T> conjugate(const quat<T>& q)
00914 {
00915     return quat<T>(-q.v, q.w);
00916 }
00917 
00918 template <typename T> 
00919 inline quat<T> inverse(const quat<T>& q)
00920 {
00921     const T l = dot(q, q);
00922     if ( l > T(0) ) return conjugate(q) * inv(l);
00923     else return identityq<T>();
00924 }
00925 
00926 // quaternion utility functions
00927 
00928 // the input quaternion is assumed to be normalized
00929 template <typename T> 
00930 inline mat3<T> quat_to_mat3(const quat<T>& q)
00931 {
00932     // const quat<T> q = normalize(qq);
00933 
00934     const T xx = q[0] * q[0];
00935     const T xy = q[0] * q[1];
00936     const T xz = q[0] * q[2];
00937     const T xw = q[0] * q[3];
00938     
00939     const T yy = q[1] * q[1];
00940     const T yz = q[1] * q[2];
00941     const T yw = q[1] * q[3];
00942     
00943     const T zz = q[2] * q[2];
00944     const T zw = q[2] * q[3];
00945     
00946     return mat3<T>(
00947         1 - 2*(yy + zz), 2*(xy - zw), 2*(xz + yw),
00948         2*(xy + zw), 1 - 2*(xx + zz), 2*(yz - xw),
00949         2*(xz - yw), 2*(yz + xw), 1 - 2*(xx + yy)
00950     );
00951 }
00952 
00953 // the input quat<T>ernion is assumed to be normalized
00954 template <typename T> 
00955 inline mat4<T> quat_to_mat4(const quat<T>& q)
00956 {
00957     // const quat<T> q = normalize(qq);
00958 
00959     return mat4<T>(quat_to_mat3(q));
00960 }
00961 
00962 template <typename T> 
00963 inline quat<T> mat_to_quat(const mat4<T>& m)
00964 {
00965     const T t = m.elem[0][0] + m.elem[1][1] + m.elem[2][2] + T(1);
00966     quat<T> q;
00967 
00968     if ( t > 0 ) {
00969         const T s = T(0.5) / sqrt(t);
00970         q[3] = T(0.25) * inv(s);
00971         q[0] = (m.elem[2][1] - m.elem[1][2]) * s;
00972         q[1] = (m.elem[0][2] - m.elem[2][0]) * s;
00973         q[2] = (m.elem[1][0] - m.elem[0][1]) * s;
00974     } else {
00975         if ( m.elem[0][0] > m.elem[1][1] && m.elem[0][0] > m.elem[2][2] ) {
00976             const T s = T(2) * sqrt( T(1) + m.elem[0][0] - m.elem[1][1] - m.elem[2][2]);
00977             const T invs = inv(s);
00978             q[0] = T(0.25) * s;
00979             q[1] = (m.elem[0][1] + m.elem[1][0] ) * invs;
00980             q[2] = (m.elem[0][2] + m.elem[2][0] ) * invs;
00981             q[3] = (m.elem[1][2] - m.elem[2][1] ) * invs;
00982         } else if (m.elem[1][1] > m.elem[2][2]) {
00983             const T s = T(2) * sqrt( T(1) + m.elem[1][1] - m.elem[0][0] - m.elem[2][2]);
00984             const T invs = inv(s);
00985             q[0] = (m.elem[0][1] + m.elem[1][0] ) * invs;
00986             q[1] = T(0.25) * s;
00987             q[2] = (m.elem[1][2] + m.elem[2][1] ) * invs;
00988             q[3] = (m.elem[0][2] - m.elem[2][0] ) * invs;
00989         } else {
00990             const T s = T(2) * sqrt( T(1) + m.elem[2][2] - m.elem[0][0] - m.elem[1][1] );
00991             const T invs = inv(s);
00992             q[0] = (m.elem[0][2] + m.elem[2][0] ) * invs;
00993             q[1] = (m.elem[1][2] + m.elem[2][1] ) * invs;
00994             q[2] = T(0.25) * s;
00995             q[3] = (m.elem[0][1] - m.elem[1][0] ) * invs;
00996         }
00997     }
00998     
00999     return q;
01000 }
01001 
01002 template <typename T> 
01003 inline quat<T> mat_to_quat(const mat3<T>& m)
01004 {
01005     return mat_to_quat(mat4<T>(m));
01006 }
01007 
01008 // the angle is in radians
01009 template <typename T> 
01010 inline quat<T> quat_from_axis_angle(const vec3<T>& axis, const T a)
01011 {
01012     quat<T> r;
01013     const T inv2 = inv(T(2));
01014     r.v = sin(a * inv2) * normalize(axis);
01015     r.w = cos(a * inv2);
01016     
01017     return r;
01018 }
01019 
01020 // the angle is in radians
01021 template <typename T> 
01022 inline quat<T> quat_from_axis_angle(const T x, const T y, const T z, const T angle)
01023 {
01024     return quat_from_axis_angle<T>(vec3<T>(x, y, z), angle);
01025 }
01026 
01027 // the angle is stored in radians
01028 template <typename T> 
01029 inline void quat_to_axis_angle(const quat<T>& qq, vec3<T>* axis, T *angle)
01030 {
01031     quat<T> q = normalize(qq);
01032     
01033     *angle = 2 * acos(q.w);
01034     
01035     const T s = sin((*angle) * inv(T(2)));
01036     if ( s != T(0) )
01037         *axis = q.v * inv(s);
01038     else 
01039         * axis = vec3<T>(T(0), T(0), T(0));
01040 }
01041 
01042 // Spherical linear interpolation
01043 template <typename T> 
01044 inline quat<T> slerp(const quat<T>& qq1, const quat<T>& qq2, const T t) 
01045 {
01046     // slerp(q1,q2) = sin((1-t)*a)/sin(a) * q1  +  sin(t*a)/sin(a) * q2
01047     const quat<T> q1 = normalize(qq1);
01048     const quat<T> q2 = normalize(qq2);
01049     
01050     const T a = acos(dot(q1, q2));
01051     const T s = sin(a);
01052     
01053     #define EPS T(1e-5)
01054 
01055     if ( !(-EPS <= s && s <= EPS) ) {
01056         return sin((T(1)-t)*a)/s * q1  +  sin(t*a)/s * q2;
01057     } else {
01058         // if the angle is to small use a linear interpolation
01059         return lerp(q1, q2, t);
01060     }
01061 
01062     #undef EPS
01063 }
01064 
01065 // Sperical quadtratic interpolation using a smooth cubic spline
01066 // The parameters a and b are the control points.
01067 template <typename T> 
01068 inline quat<T> squad(
01069     const quat<T>& q0, 
01070     const quat<T>& a, 
01071     const quat<T>& b, 
01072     const quat<T>& q1, 
01073     const T t) 
01074 {
01075     return slerp(slerp(q0, q1, t),slerp(a, b, t), 2 * t * (1 - t));
01076 }
01077 
01078 #undef MOP_M_CLASS_TEMPLATE
01079 #undef MOP_M_TYPE_TEMPLATE
01080 #undef MOP_COMP_TEMPLATE
01081 #undef MOP_G_UMINUS_TEMPLATE
01082 #undef COMMON_OPERATORS
01083 #undef VECTOR_COMMON
01084 #undef FOP_G_SOURCE_TEMPLATE
01085 #undef FOP_G_CLASS_TEMPLATE
01086 #undef FOP_G_TYPE_TEMPLATE
01087 #undef VEC_QUAT_FUNC_TEMPLATE
01088 #undef VEC_FUNC_TEMPLATE
01089 #undef MATRIX_COL4
01090 #undef MATRIX_ROW4
01091 #undef MATRIX_COL3
01092 #undef MATRIX_ROW3
01093 #undef MATRIX_COL2
01094 #undef MATRIX_ROW2
01095 #undef MOP_M_MATRIX_MULTIPLY
01096 #undef MATRIX_CONSTRUCTOR_FROM_T
01097 #undef MATRIX_CONSTRUCTOR_FROM_LOWER
01098 #undef MATRIX_COMMON
01099 #undef MATRIX_CONSTRUCTOR_FROM_HIGHER
01100 #undef MAT_FUNC_TEMPLATE 
01101 #undef MAT_FUNC_MINOR_TEMPLATE
01102 #undef MAT_ADJOINT_TEMPLATE
01103 #undef MAT_INVERSE_TEMPLATE
01104 #undef MAT_VEC_FUNCS_TEMPLATE
01105 #undef MAT_TRANFORMS_TEMPLATE
01106 #undef MAT_OUTERPRODUCT_TEMPLATE
01107 #undef FREE_MODIFYING_OPERATORS
01108 #undef FREE_OPERATORS
01109 
01110 } // end namespace vmath
01111 
01112 #endif
01113 
01114 
01115 
01116