• Main Page
  • Related Pages
  • Classes
  • Files
  • File List
  • File Members

src/vmath.h

Go to the documentation of this file.
00001 /* -*- C++ -*- */
00113 // Modified 2011-06-12, Davide Bacchet (davide.bacchet at gmail dot com)
00114 //                      added () operators with standard matrix notation (opposite wrt at() ).
00115 #ifndef __vmath_Header_File__
00116 #define __vmath_Header_File__
00117 
00118 #include <cmath>
00119 #include <cstring>
00120 #include <iostream>
00121 #include <sstream>
00122 #include <string>
00123 #include <cassert>
00124 
00125 #ifdef VMATH_NAMESPACE
00126 namespace VMATH_NAMESPACE
00127 {
00128 #endif
00129 
00130 #ifndef M_PI
00131 #define M_PI           3.14159265358979323846  /* pi */
00132 #endif
00133 
00134 #define DEG2RAD(x) ((x * M_PI) / 180.0)
00135 //#define EPSILON (4.37114e-07)
00136 
00137 const double epsilon = 4.37114e-05;
00138 #define EPSILON epsilon
00139 
00150 template<class T>
00151 class Vector2
00152 {
00153 public:
00154     union
00155     {
00159         T x;
00160 
00165         T s;
00166     };
00167 
00168     union
00169     {
00173         T y;
00174 
00179         T t;
00180     };
00181 
00182     //----------------[ constructors ]--------------------------
00186     Vector2()
00187             : x(0), y(0)
00188     {
00189     }
00190 
00196     Vector2(T nx, T ny)
00197             : x(nx), y(ny)
00198     {
00199     }
00200 
00205     Vector2(const Vector2<T>& src)
00206             : x(src.x), y(src.y)
00207     {
00208     }
00209 
00214     template<class FromT>
00215     Vector2(const Vector2<FromT>& src)
00216             : x(static_cast<T>(src.x)), y(static_cast<T>(src.y))
00217     {
00218     }
00219 
00220     //----------------[ access operators ]-------------------
00225     template<class FromT>
00226     Vector2<T>& operator=(const Vector2<FromT>& rhs)
00227     {
00228         x = static_cast<T>(rhs.x);
00229         y = static_cast<T>(rhs.y);
00230         return *this;
00231     }
00232 
00237     Vector2<T>& operator=(const Vector2<T>& rhs)
00238     {
00239         x = rhs.x;
00240         y = rhs.y;
00241         return *this;
00242     }
00243 
00250     T& operator[](int n)
00251     {
00252         assert(n >= 0 && n <= 1);
00253         if (0 == n)
00254             return x;
00255         else
00256             return y;
00257     }
00258 
00265     const T& operator[](int n) const
00266     {
00267         assert(n >= 0 && n <= 1);
00268         if (0 == n)
00269             return x;
00270         else
00271             return y;
00272     }
00273 
00274     //---------------[ vector aritmetic operator ]--------------
00279     Vector2<T> operator+(const Vector2<T>& rhs) const
00280     {
00281         return Vector2<T>(x + rhs.x, y + rhs.y);
00282     }
00283 
00288     Vector2<T> operator-(const Vector2<T>& rhs) const
00289     {
00290         return Vector2<T>(x - rhs.x, y - rhs.y);
00291     }
00292 
00297     Vector2<T> operator*(const Vector2<T>& rhs) const
00298     {
00299         return Vector2<T>(x * rhs.x, y * rhs.y);
00300     }
00301 
00306     Vector2<T> operator/(const Vector2<T>& rhs) const
00307     {
00308         return Vector2<T>(x / rhs.x, y / rhs.y);
00309     }
00310 
00315     Vector2<T>& operator+=(const Vector2<T>& rhs)
00316     {
00317         x += rhs.x;
00318         y += rhs.y;
00319         return *this;
00320     }
00321 
00326     Vector2<T>& operator-=(const Vector2<T>& rhs)
00327     {
00328         x -= rhs.x;
00329         y -= rhs.y;
00330         return *this;
00331     }
00332 
00337     Vector2<T>& operator*=(const Vector2<T>& rhs)
00338     {
00339         x *= rhs.x;
00340         y *= rhs.y;
00341         return *this;
00342     }
00343 
00348     Vector2<T>& operator/=(const Vector2<T>& rhs)
00349     {
00350         x /= rhs.x;
00351         y /= rhs.y;
00352         return *this;
00353     }
00354 
00355     //--------------[ scalar vector operator ]--------------------
00360     Vector2<T> operator+(T rhs) const
00361     {
00362         return Vector2<T>(x + rhs, y + rhs);
00363     }
00364 
00369     Vector2<T> operator-(T rhs) const
00370     {
00371         return Vector2<T>(x - rhs, y - rhs);
00372     }
00373 
00378     Vector2<T> operator*(T rhs) const
00379     {
00380         return Vector2<T>(x * rhs, y * rhs);
00381     }
00382 
00387     Vector2<T> operator/(T rhs) const
00388     {
00389         return Vector2<T>(x / rhs, y / rhs);
00390     }
00391 
00396     Vector2<T>& operator+=(T rhs)
00397     {
00398         x += rhs;
00399         y += rhs;
00400         return *this;
00401     }
00402 
00407     Vector2<T>& operator-=(T rhs)
00408     {
00409         x -= rhs;
00410         y -= rhs;
00411         return *this;
00412     }
00413 
00418     Vector2<T>& operator*=(T rhs)
00419     {
00420         x *= rhs;
00421         y *= rhs;
00422         return *this;
00423     }
00424 
00429     Vector2<T>& operator/=(T rhs)
00430     {
00431         x /= rhs;
00432         y /= rhs;
00433         return *this;
00434     }
00435 
00436     //--------------[ equality operator ]------------------------
00444     bool operator==(const Vector2<T>& rhs) const
00445     {
00446         return (std::abs(x - rhs.x) < EPSILON) && (std::abs(y - rhs.y) < EPSILON);
00447     }
00448 
00454     bool operator!=(const Vector2<T>& rhs) const
00455     {
00456         return !(*this == rhs);
00457     }
00458 
00459     //-------------[ unary operations ]--------------------------
00464     Vector2<T> operator-() const
00465     {
00466         return Vector2<T>(-x, -y);
00467     }
00468 
00469     //-------------[ size operations ]---------------------------
00474     T length() const
00475     {
00476         return (T) std::sqrt(x * x + y * y);
00477     }
00478 
00482     void normalize()
00483     {
00484         T s = length();
00485         x /= s;
00486         y /= s;
00487     }
00488 
00496     T lengthSq() const
00497     {
00498         return x * x + y * y;
00499     }
00500 
00501     //--------------[ misc. operations ]-----------------------
00511     Vector2<T> lerp(T fact, const Vector2<T>& r) const
00512     {
00513         return (*this) + (r - (*this)) * fact;
00514     }
00515 
00516     //-------------[ conversion ]-----------------------------
00522     operator T*()
00523     {
00524         return (T*) this;
00525     }
00531     operator const T*() const
00532     {
00533         return (const T*) this;
00534     }
00535 
00536     //-------------[ output operator ]------------------------
00543     friend std::ostream& operator<<(std::ostream& lhs, const Vector2<T>& rhs)
00544     {
00545         lhs << "[" << rhs.x << "," << rhs.y << "]";
00546         return lhs;
00547     }
00548 
00552     std::string toString() const
00553     {
00554         std::ostringstream oss;
00555         oss << *this;
00556         return oss.str();
00557     }
00558 };
00559 
00560 //--------------------------------------
00561 // Typedef shortcuts for 2D vector
00562 //-------------------------------------
00564 typedef class Vector2<float> Vector2f;
00566 typedef class Vector2<double> Vector2d;
00568 typedef class Vector2<int> Vector2i;
00569 
00581 template<class T>
00582 class Vector3
00583 {
00584 public:
00585     //T x, y, z;
00586     union
00587     {
00591         T x;
00592 
00597         T s;
00598 
00603         T r;
00604     };
00605 
00606     union
00607     {
00611         T y;
00616         T t;
00621         T g;
00622     };
00623 
00624     union
00625     {
00629         T z;
00630 
00635         T u;
00640         T b;
00641     };
00642 
00643     //----------------[ constructors ]--------------------------
00647     Vector3()
00648             : x(0), y(0), z(0)
00649     {
00650     }
00651 
00658     Vector3(T nx, T ny, T nz)
00659             : x(nx), y(ny), z(nz)
00660     {
00661     }
00662 
00667     Vector3(const Vector3<T>& src)
00668             : x(src.x), y(src.y), z(src.z)
00669     {
00670     }
00671 
00676     template<class FromT>
00677     Vector3(const Vector3<FromT>& src)
00678             : x(static_cast<T>(src.x)), y(static_cast<T>(src.y)), z(static_cast<T>(src.z))
00679     {
00680     }
00681 
00682     //----------------[ access operators ]-------------------
00687     Vector3<T> operator=(const Vector3<T>& rhs)
00688     {
00689         x = rhs.x;
00690         y = rhs.y;
00691         z = rhs.z;
00692         return *this;
00693     }
00694 
00699     template<class FromT>
00700     Vector3<T> operator=(const Vector3<FromT>& rhs)
00701     {
00702         x = static_cast<T>(rhs.x);
00703         y = static_cast<T>(rhs.y);
00704         z = static_cast<T>(rhs.z);
00705         return *this;
00706     }
00707 
00715     T & operator[](int n)
00716     {
00717         assert(n >= 0 && n <= 2);
00718         if (0 == n)
00719             return x;
00720         else if (1 == n)
00721             return y;
00722         else
00723             return z;
00724     }
00725 
00733     const T & operator[](int n) const
00734     {
00735         assert(n >= 0 && n <= 2);
00736         if (0 == n)
00737             return x;
00738         else if (1 == n)
00739             return y;
00740         else
00741             return z;
00742     }
00743 
00744     //---------------[ vector arithmetic operator ]--------------
00749     Vector3<T> operator+(const Vector3<T>& rhs) const
00750     {
00751         return Vector3<T>(x + rhs.x, y + rhs.y, z + rhs.z);
00752     }
00753 
00758     Vector3<T> operator-(const Vector3<T>& rhs) const
00759     {
00760         return Vector3<T>(x - rhs.x, y - rhs.y, z - rhs.z);
00761     }
00762 
00767     Vector3<T> operator*(const Vector3<T>& rhs) const
00768     {
00769         return Vector3<T>(x * rhs.x, y * rhs.y, z * rhs.z);
00770     }
00771 
00776     Vector3<T> operator/(const Vector3<T>& rhs) const
00777     {
00778         return Vector3<T>(x / rhs.x, y / rhs.y, z / rhs.z);
00779     }
00780 
00785     Vector3<T>& operator+=(const Vector3<T>& rhs)
00786     {
00787         x += rhs.x;
00788         y += rhs.y;
00789         z += rhs.z;
00790         return *this;
00791     }
00792 
00797     Vector3<T>& operator-=(const Vector3<T>& rhs)
00798     {
00799         x -= rhs.x;
00800         y -= rhs.y;
00801         z -= rhs.z;
00802         return *this;
00803     }
00804 
00809     Vector3<T>& operator*=(const Vector3<T>& rhs)
00810     {
00811         x *= rhs.x;
00812         y *= rhs.y;
00813         z *= rhs.z;
00814         return *this;
00815     }
00816 
00821     Vector3<T>& operator/=(const Vector3<T>& rhs)
00822     {
00823         x /= rhs.x;
00824         y /= rhs.y;
00825         z /= rhs.z;
00826         return *this;
00827     }
00828 
00833     T dotProduct(const Vector3<T>& rhs) const
00834     {
00835         return x * rhs.x + y * rhs.y + z * rhs.z;
00836     }
00837 
00842     Vector3<T> crossProduct(const Vector3<T>& rhs) const
00843     {
00844         return Vector3<T>(y * rhs.z - rhs.y * z, z * rhs.x - rhs.z * x, x * rhs.y - rhs.x * y);
00845     }
00846 
00847     //--------------[ scalar vector operator ]--------------------
00852     Vector3<T> operator+(T rhs) const
00853     {
00854         return Vector3<T>(x + rhs, y + rhs, z + rhs);
00855     }
00856 
00861     Vector3<T> operator-(T rhs) const
00862     {
00863         return Vector3<T>(x - rhs, y - rhs, z - rhs);
00864     }
00865 
00870     Vector3<T> operator*(T rhs) const
00871     {
00872         return Vector3<T>(x * rhs, y * rhs, z * rhs);
00873     }
00874 
00879     Vector3<T> operator/(T rhs) const
00880     {
00881         return Vector3<T>(x / rhs, y / rhs, z / rhs);
00882     }
00883 
00888     Vector3<T>& operator+=(T rhs)
00889     {
00890         x += rhs;
00891         y += rhs;
00892         z += rhs;
00893         return *this;
00894     }
00895 
00900     Vector3<T>& operator-=(T rhs)
00901     {
00902         x -= rhs;
00903         y -= rhs;
00904         z -= rhs;
00905         return *this;
00906     }
00907 
00912     Vector3<T>& operator*=(T rhs)
00913     {
00914         x *= rhs;
00915         y *= rhs;
00916         z *= rhs;
00917         return *this;
00918     }
00919 
00924     Vector3<T>& operator/=(T rhs)
00925     {
00926         x /= rhs;
00927         y /= rhs;
00928         z /= rhs;
00929         return *this;
00930     }
00931 
00932     //--------------[ Equality operator ]------------------------
00940     bool operator==(const Vector3<T>& rhs) const
00941     {
00942         return std::fabs(x - rhs.x) < EPSILON && std::fabs(y - rhs.y) < EPSILON && std::fabs(z - rhs.z) < EPSILON;
00943     }
00944 
00950     bool operator!=(const Vector3<T>& rhs) const
00951     {
00952         return !(*this == rhs);
00953     }
00954 
00955     //-------------[ unary operations ]--------------------------
00960     Vector3<T> operator-() const
00961     {
00962         return Vector3<T>(-x, -y, -z);
00963     }
00964 
00965     //-------------[ size operations ]---------------------------
00970     T length() const
00971     {
00972         return (T) std::sqrt(x * x + y * y + z * z);
00973     }
00974 
00982     T lengthSq() const
00983     {
00984         return x * x + y * y + z * z;
00985     }
00986 
00990     void normalize()
00991     {
00992         T s = length();
00993         x /= s;
00994         y /= s;
00995         z /= s;
00996     }
00997 
00998     //------------[ other operations ]---------------------------
01005     void rotate(T ax, T ay, T az)
01006     {
01007         T a = cos(DEG2RAD(ax));
01008         T b = sin(DEG2RAD(ax));
01009         T c = cos(DEG2RAD(ay));
01010         T d = sin(DEG2RAD(ay));
01011         T e = cos(DEG2RAD(az));
01012         T f = sin(DEG2RAD(az));
01013         T nx = c * e * x - c * f * y + d * z;
01014         T ny = (a * f + b * d * e) * x + (a * e - b * d * f) * y - b * c * z;
01015         T nz = (b * f - a * d * e) * x + (a * d * f + b * e) * y + a * c * z;
01016         x = nx;
01017         y = ny;
01018         z = nz;
01019 
01020     }
01021 
01031     Vector3<T> lerp(T fact, const Vector3<T>& r) const
01032     {
01033         return (*this) + (r - (*this)) * fact;
01034     }
01035 
01036     //-------------[ conversion ]-----------------------------
01037 
01043     operator T*()
01044     {
01045         return (T*) this;
01046     }
01047 
01053     operator const T*() const
01054     {
01055         return (const T*) this;
01056     }
01057 
01058     //-------------[ output operator ]------------------------
01065     friend std::ostream& operator<<(std::ostream& lhs, const Vector3<T> rhs)
01066     {
01067         lhs << "[" << rhs.x << "," << rhs.y << "," << rhs.z << "]";
01068         return lhs;
01069     }
01070 
01074     std::string toString() const
01075     {
01076         std::ostringstream oss;
01077         oss << *this;
01078         return oss.str();
01079     }
01080 };
01081 
01083 typedef Vector3<float> Vector3f;
01085 typedef Vector3<double> Vector3d;
01087 typedef Vector3<int> Vector3i;
01088 
01100 template<class T>
01101 class Vector4
01102 {
01103 public:
01104 
01105     union
01106     {
01111         T r;
01115         T x;
01116     };
01117 
01118     union
01119     {
01124         T g;
01128         T y;
01129     };
01130 
01131     union
01132     {
01137         T b;
01141         T z;
01142     };
01143 
01144     union
01145     {
01150         T a;
01156         T w;
01157     };
01158 
01159     //----------------[ constructors ]--------------------------
01163     Vector4()
01164             : x(0), y(0), z(0), w(0)
01165     {
01166     }
01167 
01175     Vector4(T nx, T ny, T nz, T nw)
01176             : x(nx), y(ny), z(nz), w(nw)
01177     {
01178     }
01179 
01184     Vector4(const Vector4<T>& src)
01185             : x(src.x), y(src.y), z(src.z), w(src.w)
01186     {
01187     }
01188 
01193     template<class FromT>
01194     Vector4(const Vector4<FromT>& src)
01195             : x(static_cast<T>(src.x)), y(static_cast<T>(src.y)), z(static_cast<T>(src.z)), w(static_cast<T>(src.w))
01196     {
01197     }
01198 
01199     //----------------[ access operators ]-------------------
01204     Vector4<T> operator=(const Vector4<T>& rhs)
01205     {
01206         x = rhs.x;
01207         y = rhs.y;
01208         z = rhs.z;
01209         w = rhs.w;
01210         return *this;
01211     }
01212 
01217     template<class FromT>
01218     Vector4<T> operator=(const Vector4<FromT>& rhs)
01219     {
01220         x = static_cast<T>(rhs.x);
01221         y = static_cast<T>(rhs.y);
01222         z = static_cast<T>(rhs.z);
01223         w = static_cast<T>(rhs.w);
01224         return *this;
01225     }
01226 
01234     T & operator[](int n)
01235     {
01236         assert(n >= 0 && n <= 3);
01237         if (0 == n)
01238             return x;
01239         else if (1 == n)
01240             return y;
01241         else if (2 == n)
01242             return z;
01243         else
01244             return w;
01245     }
01246 
01254     const T & operator[](int n) const
01255     {
01256         assert(n >= 0 && n <= 3);
01257         if (0 == n)
01258             return x;
01259         else if (1 == n)
01260             return y;
01261         else if (2 == n)
01262             return z;
01263         else
01264             return w;
01265     }
01266 
01267     //---------------[ vector aritmetic operator ]--------------
01272     Vector4<T> operator+(const Vector4<T>& rhs) const
01273     {
01274         return Vector4<T>(x + rhs.x, y + rhs.y, z + rhs.z, w + rhs.w);
01275     }
01276 
01281     Vector4<T> operator-(const Vector4<T>& rhs) const
01282     {
01283         return Vector4<T>(x - rhs.x, y - rhs.y, z - rhs.z, w - rhs.w);
01284     }
01285 
01290     Vector4<T> operator*(const Vector4<T> rhs) const
01291     {
01292         return Vector4<T>(x * rhs.x, y * rhs.y, z * rhs.z, w * rhs.w);
01293     }
01294 
01299     Vector4<T> operator/(const Vector4<T>& rhs) const
01300     {
01301         return Vector4<T>(x / rhs.x, y / rhs.y, z / rhs.z, w / rhs.w);
01302     }
01303 
01308     Vector4<T>& operator+=(const Vector4<T>& rhs)
01309     {
01310         x += rhs.x;
01311         y += rhs.y;
01312         z += rhs.z;
01313         w += rhs.w;
01314         return *this;
01315     }
01316 
01321     Vector4<T>& operator-=(const Vector4<T>& rhs)
01322     {
01323         x -= rhs.x;
01324         y -= rhs.y;
01325         z -= rhs.z;
01326         w -= rhs.w;
01327         return *this;
01328     }
01329 
01334     Vector4<T>& operator*=(const Vector4<T>& rhs)
01335     {
01336         x *= rhs.x;
01337         y *= rhs.y;
01338         z *= rhs.z;
01339         w *= rhs.w;
01340         return *this;
01341     }
01342 
01347     Vector4<T>& operator/=(const Vector4<T>& rhs)
01348     {
01349         x /= rhs.x;
01350         y /= rhs.y;
01351         z /= rhs.z;
01352         w /= rhs.w;
01353         return *this;
01354     }
01355 
01356     //--------------[ equiality operator ]------------------------
01364     bool operator==(const Vector4<T>& rhs) const
01365     {
01366         return std::fabs(x - rhs.x) < EPSILON && std::fabs(y - rhs.y) < EPSILON && std::fabs(z - rhs.z) < EPSILON
01367                 && std::fabs(w - rhs.w) < EPSILON;
01368     }
01369 
01375     bool operator!=(const Vector4<T>& rhs) const
01376     {
01377         return !(*this == rhs);
01378     }
01379 
01380     //-------------[ unary operations ]--------------------------
01385     Vector4<T> operator-() const
01386     {
01387         return Vector4<T>(-x, -y, -z, -w);
01388     }
01389 
01390     //--------------[ scalar vector operator ]--------------------
01391 
01396     Vector4<T> operator+(T rhs) const
01397     {
01398         return Vector4<T>(x + rhs, y + rhs, z + rhs, w + rhs);
01399     }
01400 
01405     Vector4<T> operator-(T rhs) const
01406     {
01407         return Vector4<T>(x - rhs, y - rhs, z - rhs, w - rhs);
01408     }
01409 
01414     Vector4<T> operator*(T rhs) const
01415     {
01416         return Vector4<T>(x * rhs, y * rhs, z * rhs, w * rhs);
01417     }
01418 
01423     Vector4<T> operator/(T rhs) const
01424     {
01425         return Vector4<T>(x / rhs, y / rhs, z / rhs, w / rhs);
01426     }
01427 
01432     Vector4<T>& operator+=(T rhs)
01433     {
01434         x += rhs;
01435         y += rhs;
01436         z += rhs;
01437         w += rhs;
01438         return *this;
01439     }
01440 
01445     Vector4<T>& operator-=(T rhs)
01446     {
01447         x -= rhs;
01448         y -= rhs;
01449         z -= rhs;
01450         w -= rhs;
01451         return *this;
01452     }
01453 
01458     Vector4<T>& operator*=(T rhs)
01459     {
01460         x *= rhs;
01461         y *= rhs;
01462         z *= rhs;
01463         w *= rhs;
01464         return *this;
01465     }
01466 
01471     Vector4<T>& operator/=(T rhs)
01472     {
01473         x /= rhs;
01474         y /= rhs;
01475         z /= rhs;
01476         w /= rhs;
01477         return *this;
01478     }
01479 
01480     //-------------[ size operations ]---------------------------
01485     T length() const
01486     {
01487         return (T) std::sqrt(x * x + y * y + z * z + w * w);
01488     }
01489 
01493     void normalize()
01494     {
01495         T s = length();
01496         x /= s;
01497         y /= s;
01498         z /= s;
01499         w /= s;
01500     }
01501 
01509     T lengthSq() const
01510     {
01511         return x * x + y * y + z * z + w * w;
01512     }
01513 
01514     //--------------[ misc. operations ]-----------------------
01524     Vector4<T> lerp(T fact, const Vector4<T>& r) const
01525     {
01526         return (*this) + (r - (*this)) * fact;
01527     }
01528 
01529     //-------------[ conversion ]-----------------------------
01530 
01536     operator T*()
01537     {
01538         return (T*) this;
01539     }
01540 
01546     operator const T*() const
01547     {
01548         return (const T*) this;
01549     }
01550 
01551     //-------------[ output operator ]------------------------
01558     friend std::ostream& operator<<(std::ostream& lhs, const Vector4<T>& rhs)
01559     {
01560         lhs << "[" << rhs.x << "," << rhs.y << "," << rhs.z << "," << rhs.w << "]";
01561         return lhs;
01562     }
01563 
01567     std::string toString() const
01568     {
01569         std::ostringstream oss;
01570         oss << *this;
01571         return oss.str();
01572     }
01573 
01574 };
01575 
01577 typedef Vector4<float> Vector4f;
01579 typedef Vector4<double> Vector4d;
01581 typedef Vector4<int> Vector4i;
01582 
01589 template<class T>
01590 class Matrix3
01591 {
01592 public:
01594     T data[9];
01595 
01596     //--------------------------[ constructors ]-------------------------------
01600     Matrix3()
01601     {
01602         for (int i = 0; i < 9; i++)
01603             data[i] = (i % 4) ? 0 : 1;
01604     }
01605 
01610     Matrix3(const T * dt)
01611     {
01612         std::memcpy(data, dt, sizeof(T) * 9);
01613     }
01614 
01619     Matrix3(const Matrix3<T>& src)
01620     {
01621         std::memcpy(data, src.data, sizeof(T) * 9);
01622     }
01623 
01628     template<class FromT>
01629     Matrix3(const Matrix3<FromT>& src)
01630     {
01631         for (int i = 0; i < 9; i++)
01632         {
01633             data[i] = static_cast<T>(src.data[i]);
01634         }
01635     }
01636 
01640     void identity()
01641     {
01642         for (int i = 0; i < 9; i++)
01643             data[i] = (i % 4) ? 0 : 1;
01644     }
01645 
01652     static Matrix3<T> createRotationAroundAxis(T xDeg, T yDeg, T zDeg)
01653     {
01654         T xRads(DEG2RAD(xDeg));
01655         T yRads(DEG2RAD(yDeg));
01656         T zRads(DEG2RAD(zDeg));
01657 
01658         Matrix3<T> ma, mb, mc;
01659         float ac = cos(xRads);
01660         float as = sin(xRads);
01661         float bc = cos(yRads);
01662         float bs = sin(yRads);
01663         float cc = cos(zRads);
01664         float cs = sin(zRads);
01665 
01666         ma.at(1, 1) = ac;
01667         ma.at(2, 1) = as;
01668         ma.at(1, 2) = -as;
01669         ma.at(2, 2) = ac;
01670 
01671         mb.at(0, 0) = bc;
01672         mb.at(2, 0) = -bs;
01673         mb.at(0, 2) = bs;
01674         mb.at(2, 2) = bc;
01675 
01676         mc.at(0, 0) = cc;
01677         mc.at(1, 0) = cs;
01678         mc.at(0, 1) = -cs;
01679         mc.at(1, 1) = cc;
01680 
01681         Matrix3<T> ret = ma * mb * mc;
01682         return ret;
01683     }
01684 
01688     template<class It>
01689     static Matrix3<T> fromOde(const It* mat)
01690     {
01691         Matrix3<T> ret;
01692         for (int i = 0; i < 3; i++)
01693         {
01694             for (int j = 0; j < 3; j++)
01695             {
01696                 ret.at(i, j) = static_cast<T>(mat[j * 4 + i]);
01697             }
01698         }
01699         return ret;
01700     }
01701 
01708     template<class FromT>
01709     static Matrix3<T> fromRowMajorArray(const FromT* arr)
01710     {
01711         const T retData[] =
01712         { static_cast<T>(arr[0]), static_cast<T>(arr[3]), static_cast<T>(arr[6]), static_cast<T>(arr[1]),
01713                 static_cast<T>(arr[4]), static_cast<T>(arr[7]), static_cast<T>(arr[2]), static_cast<T>(arr[5]),
01714                 static_cast<T>(arr[8]) };
01715 
01716         return retData;
01717     }
01718 
01725     template<class FromT>
01726     static Matrix3<T> fromColumnMajorArray(const FromT* arr)
01727     {
01728         const T retData[] =
01729         { static_cast<T>(arr[0]), static_cast<T>(arr[1]), static_cast<T>(arr[2]), static_cast<T>(arr[3]),
01730                 static_cast<T>(arr[4]), static_cast<T>(arr[5]), static_cast<T>(arr[6]), static_cast<T>(arr[7]),
01731                 static_cast<T>(arr[8]) };
01732 
01733         return retData;
01734     }
01735 
01736     //---------------------[ equiality operators ]------------------------------
01745     bool operator==(const Matrix3<T>& rhs) const
01746     {
01747         for (int i = 0; i < 9; i++)
01748         {
01749             if (std::fabs(data[i] - rhs.data[i]) >= EPSILON)
01750                 return false;
01751         }
01752         return true;
01753     }
01754 
01760     bool operator!=(const Matrix3<T>& rhs) const
01761     {
01762         return !(*this == rhs);
01763     }
01764 
01765     //---------------------[ access operators ]---------------------------------
01771     T& at(int x, int y)
01772     {
01773         assert(x >= 0 && x < 3);
01774         assert(y >= 0 && y < 3);
01775         return data[x * 3 + y];
01776     }
01777 
01783     const T& at(int x, int y) const
01784     {
01785         assert(x >= 0 && x < 3);
01786         assert(y >= 0 && y < 3);
01787         return data[x * 3 + y];
01788     }
01789 
01795     T& operator()(int i, int j)
01796     {
01797         assert(i >= 1 && i <= 3);
01798         assert(j >= 1 && j <= 3);
01799         return data[(j - 1) * 3 + i - 1];
01800     }
01801 
01807     const T& operator()(int i, int j) const
01808     {
01809         assert(i >= 1 && i <= 3);
01810         assert(j >= 1 && j <= 3);
01811         return data[(j - 1) * 3 + i - 1];
01812     }
01813 
01818     Matrix3<T>& operator=(const Matrix3<T>& rhs)
01819     {
01820         std::memcpy(data, rhs.data, sizeof(T) * 9);
01821         return *this;
01822     }
01823 
01828     template<class FromT>
01829     Matrix3<T>& operator=(const Matrix3<FromT>& rhs)
01830     {
01831         for (int i = 0; i < 9; i++)
01832         {
01833             data[i] = static_cast<T>(rhs.data[i]);
01834         }
01835         return *this;
01836     }
01837 
01842     Matrix3<T>& operator=(const T* rhs)
01843     {
01844         std::memcpy(data, rhs, sizeof(T) * 9);
01845         return *this;
01846     }
01847 
01848     /*Matrix3<T> & operator=(const double* m)
01849      {
01850      for (int i = 0; i < 9; i++) data[i] = (T)m[i];
01851      return * this;
01852      }*/
01853 
01854     //--------------------[ matrix with matrix operations ]---------------------
01859     Matrix3<T> operator+(const Matrix3<T>& rhs) const
01860     {
01861         Matrix3<T> ret;
01862         for (int i = 0; i < 9; i++)
01863             ret.data[i] = data[i] + rhs.data[i];
01864         return ret;
01865     }
01866 
01871     Matrix3<T> operator-(const Matrix3<T>& rhs) const
01872     {
01873         Matrix3<T> ret;
01874         for (int i = 0; i < 9; i++)
01875             ret.data[i] = data[i] - rhs.data[i];
01876         return ret;
01877     }
01878 
01879     //--------------------[ matrix with scalar operations ]---------------------
01884     Matrix3<T> operator+(T rhs) const
01885     {
01886         Matrix3<T> ret;
01887         for (int i = 0; i < 9; i++)
01888             ret.data[i] = data[i] + rhs;
01889         return ret;
01890     }
01891 
01896     Matrix3<T> operator-(T rhs) const
01897     {
01898         Matrix3<T> ret;
01899         for (int i = 0; i < 9; i++)
01900             ret.data[i] = data[i] - rhs;
01901         return ret;
01902     }
01903 
01908     Matrix3<T> operator*(T rhs) const
01909     {
01910         Matrix3<T> ret;
01911         for (int i = 0; i < 9; i++)
01912             ret.data[i] = data[i] * rhs;
01913         return ret;
01914     }
01915 
01920     Matrix3<T> operator/(T rhs) const
01921     {
01922         Matrix3<T> ret;
01923         for (int i = 0; i < 9; i++)
01924             ret.data[i] = data[i] / rhs;
01925         return ret;
01926     }
01927 
01928     //--------------------[ multiply operators ]--------------------------------
01933     Vector3<T> operator*(const Vector3<T>& rhs) const
01934     {
01935         return Vector3<T>(data[0] * rhs.x + data[3] * rhs.y + data[6] * rhs.z,
01936                 data[1] * rhs.x + data[4] * rhs.y + data[7] * rhs.z,
01937                 data[2] * rhs.x + data[5] * rhs.y + data[8] * rhs.z);
01938     }
01939 
01944     Matrix3<T> operator*(Matrix3<T> rhs) const
01945     {
01946         static Matrix3<T> w;
01947         for (int i = 0; i < 3; i++)
01948         {
01949             for (int j = 0; j < 3; j++)
01950             {
01951                 T n = 0;
01952                 for (int k = 0; k < 3; k++)
01953                     n += rhs.at(i, k) * at(k, j);
01954                 w.at(i, j) = n;
01955             }
01956         }
01957         return w;
01958 
01959     }
01960 
01961     //---------------------------[ misc operations ]----------------------------
01965     Matrix3<T> transpose()
01966     {
01967         Matrix3<T> ret;
01968         for (int i = 0; i < 3; i++)
01969         {
01970             for (int j = 0; j < 3; j++)
01971             {
01972                 ret.at(i, j) = at(j, i);
01973             }
01974         }
01975         return ret;
01976     }
01977 
01987     Matrix3<T> lerp(T fact, const Matrix3<T>& rhs) const
01988     {
01989         Matrix3<T> ret = (*this) + (rhs - (*this)) * fact;
01990         return ret;
01991     }
01992 
01993     T det()
01994     {
01995         return +at(0, 0) * at(1, 1) * at(2, 2) + at(0, 1) * at(1, 2) * at(2, 0) + at(0, 2) * at(1, 0) * at(2, 1)
01996                 - at(0, 0) * at(1, 2) * at(2, 1) - at(0, 1) * at(1, 0) * at(2, 2) - at(0, 2) * at(1, 1) * at(2, 0);
01997     }
01998 
02003     Matrix3<T> inverse()
02004     {
02005         Matrix3<T> ret;
02006         ret.at(0, 0) = at(1, 1) * at(2, 2) - at(2, 1) * at(1, 2);
02007         ret.at(0, 1) = at(2, 1) * at(0, 2) - at(0, 1) * at(2, 2);
02008         ret.at(0, 2) = at(0, 1) * at(1, 2) - at(1, 1) * at(0, 2);
02009         ret.at(1, 0) = at(2, 0) * at(1, 2) - at(1, 0) * at(2, 2);
02010         ret.at(1, 1) = at(0, 0) * at(2, 2) - at(2, 0) * at(0, 2);
02011         ret.at(1, 2) = at(1, 0) * at(0, 2) - at(0, 0) * at(1, 2);
02012         ret.at(2, 0) = at(1, 0) * at(2, 1) - at(2, 0) * at(1, 1);
02013         ret.at(2, 1) = at(2, 0) * at(0, 1) - at(0, 0) * at(2, 1);
02014         ret.at(2, 2) = at(0, 0) * at(1, 1) - at(1, 0) * at(0, 1);
02015         return ret * (1.0f / det());
02016     }
02017 
02018     //-------------[ conversion ]-----------------------------
02019 
02025     operator T*()
02026     {
02027         return (T*) data;
02028     }
02029 
02035     operator const T*() const
02036     {
02037         return (const T*) data;
02038     }
02039 
02040     //----------[ output operator ]----------------------------
02047     friend std::ostream& operator <<(std::ostream& lhs, const Matrix3<T>& rhs)
02048     {
02049         for (int i = 0; i < 3; i++)
02050         {
02051             lhs << "|\t";
02052             for (int j = 0; j < 3; j++)
02053             {
02054                 lhs << rhs.at(j, i) << "\t";
02055             }
02056             lhs << "|" << std::endl;
02057         }
02058         return lhs;
02059     }
02060 
02064     std::string toString() const
02065     {
02066         std::ostringstream oss;
02067         oss << *this;
02068         return oss.str();
02069     }
02070 };
02071 
02073 typedef Matrix3<float> Matrix3f;
02075 typedef Matrix3<double> Matrix3d;
02077 typedef Matrix3<int> Matrix3i;
02078 
02085 template<class T>
02086 class Matrix4
02087 {
02088 public:
02090     T data[16];
02091 
02092     //--------------------------[ constructors ]-------------------------------
02096     Matrix4()
02097     {
02098         for (int i = 0; i < 16; i++)
02099             data[i] = (i % 5) ? 0 : 1;
02100     }
02101 
02106     Matrix4(const T * dt)
02107     {
02108         std::memcpy(data, dt, sizeof(T) * 16);
02109     }
02110 
02115     Matrix4(const Matrix4<T>& src)
02116     {
02117         std::memcpy(data, src.data, sizeof(T) * 16);
02118     }
02119 
02124     template<class FromT>
02125     Matrix4(const Matrix4<FromT>& src)
02126     {
02127         for (int i = 0; i < 16; i++)
02128         {
02129             data[i] = static_cast<T>(src.data[i]);
02130         }
02131     }
02132 
02136     void identity()
02137     {
02138         for (int i = 0; i < 16; i++)
02139             data[i] = (i % 5) ? 0 : 1;
02140     }
02141 
02148     static Matrix4<T> createRotationAroundAxis(T xDeg, T yDeg, T zDeg)
02149     {
02150         T xRads(DEG2RAD(xDeg));
02151         T yRads(DEG2RAD(yDeg));
02152         T zRads(DEG2RAD(zDeg));
02153 
02154         Matrix4<T> ma, mb, mc;
02155         float ac = cos(xRads);
02156         float as = sin(xRads);
02157         float bc = cos(yRads);
02158         float bs = sin(yRads);
02159         float cc = cos(zRads);
02160         float cs = sin(zRads);
02161 
02162         ma.at(1, 1) = ac;
02163         ma.at(2, 1) = as;
02164         ma.at(1, 2) = -as;
02165         ma.at(2, 2) = ac;
02166 
02167         mb.at(0, 0) = bc;
02168         mb.at(2, 0) = -bs;
02169         mb.at(0, 2) = bs;
02170         mb.at(2, 2) = bc;
02171 
02172         mc.at(0, 0) = cc;
02173         mc.at(1, 0) = cs;
02174         mc.at(0, 1) = -cs;
02175         mc.at(1, 1) = cc;
02176 
02177         /*std::cout << "RotVec = " << a << "," << b << "," << c << std::endl;
02178          std::cout << "Rx = " << std::endl << ma;
02179          std::cout << "Ry = " << std::endl << mb;
02180          std::cout << "Rz = " << std::endl << mc;*/
02181 
02182         Matrix4<T> ret = ma * mb * mc;
02183         //std::cout << "Result = " << std::endl << ma * (mb * mc);
02184 
02185         return ret;
02186     }
02187 
02189 
02196     static Matrix4<T> createTranslation(T x, T y, T z, T w = 1)
02197     {
02198         Matrix4 ret;
02199         ret.at(3, 0) = x;
02200         ret.at(3, 1) = y;
02201         ret.at(3, 2) = z;
02202         ret.at(3, 3) = w;
02203 
02204         return ret;
02205     }
02206 
02219     static Matrix4<T> createFrustum(T left, T right, T bottom, T top, T zNear, T zFar)
02220     {
02221         /*
02222          *
02223           2 zNear
02224         ------------       0              A              0
02225         right - left
02226 
02227                         2 zNear
02228             0         ------------        B              0
02229                       top - bottom
02230 
02231             0              0              C              D
02232 
02233             0              0              -1             0
02234 
02235                                                      A = (right + left) / (right - left)
02236 
02237                                                      B = (top + bottom) / (top - bottom)
02238 
02239                                                     C = - (zFar + zNear) / (zFar - zNear)
02240 
02241                                                     D = - (2 zFar zNear) / (zFar - zNear)
02242          *
02243          */
02244         Matrix4<T> ret;
02245 
02246         const T invWidth = 1.0 / (right - left);
02247         const T invHeight = 1.0 / (top - bottom);
02248         const T invDepth = 1.0 / (zFar - zNear);
02249 
02250         const T twoZNear = 2 * zNear;
02251 
02252         ret.at(0,0) = twoZNear * invWidth;
02253         ret.at(1,1) = twoZNear * invHeight;
02254 
02255         ret.at(2,0) = (right + left) * invWidth;
02256         ret.at(2,1) = (top + bottom) * invHeight;
02257         ret.at(2,2) = - (zFar + zNear) * invDepth;
02258         ret.at(2,3) = -1;
02259 
02260         ret.at(3,2) = - twoZNear * zFar * invDepth;
02261 
02262         return ret;
02263     }
02264 
02277     static Matrix4<T> createOrtho(T left, T right, T bottom, T top, T zNear, T zFar)
02278     {
02279         /*
02280                2
02281         ------------       0              0              tx
02282         right - left
02283 
02284                            2
02285             0         ------------        0              ty
02286                       top - bottom
02287 
02288                                           -2
02289             0              0         ------------        tz
02290                                       zFar-zNear
02291 
02292             0              0              0              1
02293 
02294        where
02295 
02296                                                     tx = - (right + left) / (right - left)
02297 
02298                                                     ty = - (top + bottom) / (top - bottom)
02299 
02300                                                     tz = - (zFar + zNear) / (zFar - zNear)
02301 
02302          */
02303 
02304         const T invWidth = 1.0 / (right  - left);
02305         const T invHeight = 1.0 / (top - bottom);
02306         const T invDepth = 1.0 / (zFar - zNear);
02307 
02308         Matrix4<T> ret;
02309 
02310         ret.at(0,0) = 2 * invWidth;
02311         ret.at(1,1) = 2 * invHeight;
02312         ret.at(2,2) = -2 * invDepth;
02313 
02314         ret.at(3,0) = -(right + left) * invWidth;
02315         ret.at(3,1) = -(top + bottom) * invHeight;
02316         ret.at(3,2) = -(zFar + zNear) * invDepth;
02317 
02318         return ret;
02319     }
02320 
02327     template<class FromT>
02328     static Matrix4<T> fromRowMajorArray(const FromT* arr)
02329     {
02330         const T retData[] =
02331         { static_cast<T>(arr[0]), static_cast<T>(arr[4]), static_cast<T>(arr[8]), static_cast<T>(arr[12]),
02332                 static_cast<T>(arr[1]), static_cast<T>(arr[5]), static_cast<T>(arr[9]), static_cast<T>(arr[13]),
02333                 static_cast<T>(arr[2]), static_cast<T>(arr[6]), static_cast<T>(arr[10]), static_cast<T>(arr[14]),
02334                 static_cast<T>(arr[3]), static_cast<T>(arr[7]), static_cast<T>(arr[11]), static_cast<T>(arr[15]) };
02335 
02336         return retData;
02337     }
02338 
02345     template<class FromT>
02346     static Matrix4<T> fromColumnMajorArray(const FromT* arr)
02347     {
02348         const T retData[] =
02349         { static_cast<T>(arr[0]), static_cast<T>(arr[1]), static_cast<T>(arr[2]), static_cast<T>(arr[3]),
02350                 static_cast<T>(arr[4]), static_cast<T>(arr[5]), static_cast<T>(arr[6]), static_cast<T>(arr[7]),
02351                 static_cast<T>(arr[8]), static_cast<T>(arr[9]), static_cast<T>(arr[10]), static_cast<T>(arr[11]),
02352                 static_cast<T>(arr[12]), static_cast<T>(arr[13]), static_cast<T>(arr[14]), static_cast<T>(arr[15]) };
02353 
02354         return retData;
02355     }
02356 
02357     //---------------------[ Equality operators ]------------------------------
02366     bool operator==(const Matrix4<T>& rhs) const
02367     {
02368         for (int i = 0; i < 16; i++)
02369         {
02370             if (std::fabs(data[i] - rhs.data[i]) >= EPSILON
02371                 )
02372                 return false;
02373         }
02374         return true;
02375     }
02376 
02382     bool operator!=(const Matrix4<T>& rhs) const
02383     {
02384         return !(*this == rhs);
02385     }
02386 
02387     //---------------------[ access operators ]---------------------------------
02393     T& at(int x, int y)
02394     {
02395         assert(x >= 0 && x < 4);
02396         assert(y >= 0 && y < 4);
02397         return data[x * 4 + y];
02398     }
02399 
02405     const T& at(int x, int y) const
02406     {
02407         assert(x >= 0 && x < 4);
02408         assert(y >= 0 && y < 4);
02409         return data[x * 4 + y];
02410     }
02411 
02417     T& operator()(int i, int j)
02418     {
02419         assert(i >= 1 && i <= 4);
02420         assert(j >= 1 && j <= 4);
02421         return data[(j - 1) * 4 + i - 1];
02422     }
02423 
02429     const T& operator()(int i, int j) const
02430     {
02431         assert(i >= 1 && i <= 4);
02432         assert(j >= 1 && j <= 4);
02433         return data[(j - 1) * 4 + i - 1];
02434     }
02435 
02441     void setTranslation(const Vector3<T>& v)
02442     {
02443         at(3, 0) = v.x;
02444         at(3, 1) = v.y;
02445         at(3, 2) = v.z;
02446         at(3, 3) = 1;
02447     }
02448 
02449     Vector3<T> getTranslation()
02450     {
02451         return Vector3<T>(at(3, 0), at(3, 1), at(3, 2));
02452     }
02453 
02459     void setRotation(const Matrix3<T>& m)
02460     {
02461         for (int i = 0; i < 3; i++)
02462         {
02463             for (int j = 0; j < 3; j++)
02464             {
02465                 at(i, j) = m.at(i, j);
02466             }
02467         }
02468     }
02469 
02474     Matrix4<T>& operator=(const Matrix4<T>& rhs)
02475     {
02476         std::memcpy(data, rhs.data, sizeof(T) * 16);
02477         return *this;
02478     }
02479 
02484     template<class FromT>
02485     Matrix4<T>& operator=(const Matrix4<FromT>& rhs)
02486     {
02487         for (int i = 0; i < 16; i++)
02488         {
02489             data[i] = static_cast<T>(rhs.data[i]);
02490         }
02491         return *this;
02492     }
02493 
02498     Matrix4<T>& operator=(const T* rhs)
02499     {
02500         std::memcpy(data, rhs, sizeof(T) * 16);
02501         return *this;
02502     }
02503 
02504     /*Matrix4<T> & operator=(const double* m)
02505      {
02506      for (int i = 0; i < 16; i++) data[i] = (T)m[i];
02507      return * this;
02508      }*/
02509 
02510     //--------------------[ matrix with matrix operations ]---------------------
02515     Matrix4<T> operator+(const Matrix4<T>& rhs) const
02516     {
02517         Matrix4<T> ret;
02518         for (int i = 0; i < 16; i++)
02519             ret.data[i] = data[i] + rhs.data[i];
02520         return ret;
02521     }
02522 
02527     Matrix4<T> operator-(const Matrix4<T>& rhs) const
02528     {
02529         Matrix4<T> ret;
02530         for (int i = 0; i < 16; i++)
02531             ret.data[i] = data[i] - rhs.data[i];
02532         return ret;
02533     }
02534 
02535     //--------------------[ matrix with scalar operations ]---------------------
02540     Matrix4<T> operator+(T rhs) const
02541     {
02542         Matrix4<T> ret;
02543         for (int i = 0; i < 16; i++)
02544             ret.data[i] = data[i] + rhs;
02545         return ret;
02546     }
02547 
02552     Matrix4<T> operator-(T rhs) const
02553     {
02554         Matrix4<T> ret;
02555         for (int i = 0; i < 16; i++)
02556             ret.data[i] = data[i] - rhs;
02557         return ret;
02558     }
02559 
02564     Matrix4<T> operator*(T rhs) const
02565     {
02566         Matrix4<T> ret;
02567         for (int i = 0; i < 16; i++)
02568             ret.data[i] = data[i] * rhs;
02569         return ret;
02570     }
02571 
02576     Matrix4<T> operator/(T rhs) const
02577     {
02578         Matrix4<T> ret;
02579         for (int i = 0; i < 16; i++)
02580             ret.data[i] = data[i] / rhs;
02581         return ret;
02582     }
02583 
02584     //--------------------[ multiply operators ]--------------------------------
02589     Vector4<T> operator*(const Vector4<T>& rhs) const
02590     {
02591         return Vector4<T>(data[0] * rhs.x + data[4] * rhs.y + data[8] * rhs.z + data[12] * rhs.w,
02592                 data[1] * rhs.x + data[5] * rhs.y + data[9] * rhs.z + data[13] * rhs.w,
02593                 data[2] * rhs.x + data[6] * rhs.y + data[10] * rhs.z + data[14] * rhs.w,
02594                 data[3] * rhs.x + data[7] * rhs.y + data[11] * rhs.z + data[15] * rhs.w);
02595 
02596     }
02597 
02602     Vector3<T> operator*(const Vector3<T>& rhs) const
02603     {
02604         return Vector3<T>(data[0] * rhs.x + data[4] * rhs.y + data[8] * rhs.z,
02605                 data[1] * rhs.x + data[5] * rhs.y + data[9] * rhs.z,
02606                 data[2] * rhs.x + data[6] * rhs.y + data[10] * rhs.z);
02607     }
02608 
02613     Matrix4<T> operator*(Matrix4<T> rhs) const
02614     {
02615         static Matrix4<T> w;
02616         for (int i = 0; i < 4; i++)
02617         {
02618             for (int j = 0; j < 4; j++)
02619             {
02620                 T n = 0;
02621                 for (int k = 0; k < 4; k++)
02622                     n += rhs.at(i, k) * at(k, j);
02623                 w.at(i, j) = n;
02624             }
02625         }
02626         return w;
02627 
02628     }
02629 
02630     //---------------------------[ misc operations ]----------------------------
02631 
02637     T det()
02638     {
02639 
02640         return +at(3, 0) * at(2, 1) * at(1, 2) * at(0, 3) - at(2, 0) * at(3, 1) * at(1, 2) * at(0, 3)
02641                 - at(3, 0) * at(1, 1) * at(2, 2) * at(0, 3) + at(1, 0) * at(3, 1) * at(2, 2) * at(0, 3)
02642 
02643         + at(2, 0) * at(1, 1) * at(3, 2) * at(0, 3) - at(1, 0) * at(2, 1) * at(3, 2) * at(0, 3)
02644                 - at(3, 0) * at(2, 1) * at(0, 2) * at(1, 3) + at(2, 0) * at(3, 1) * at(0, 2) * at(1, 3)
02645 
02646         + at(3, 0) * at(0, 1) * at(2, 2) * at(1, 3) - at(0, 0) * at(3, 1) * at(2, 2) * at(1, 3)
02647                 - at(2, 0) * at(0, 1) * at(3, 2) * at(1, 3) + at(0, 0) * at(2, 1) * at(3, 2) * at(1, 3)
02648 
02649         + at(3, 0) * at(1, 1) * at(0, 2) * at(2, 3) - at(1, 0) * at(3, 1) * at(0, 2) * at(2, 3)
02650                 - at(3, 0) * at(0, 1) * at(1, 2) * at(2, 3) + at(0, 0) * at(3, 1) * at(1, 2) * at(2, 3)
02651 
02652         + at(1, 0) * at(0, 1) * at(3, 2) * at(2, 3) - at(0, 0) * at(1, 1) * at(3, 2) * at(2, 3)
02653                 - at(2, 0) * at(1, 1) * at(0, 2) * at(3, 3) + at(1, 0) * at(2, 1) * at(0, 2) * at(3, 3)
02654 
02655         + at(2, 0) * at(0, 1) * at(1, 2) * at(3, 3) - at(0, 0) * at(2, 1) * at(1, 2) * at(3, 3)
02656                 - at(1, 0) * at(0, 1) * at(2, 2) * at(3, 3) + at(0, 0) * at(1, 1) * at(2, 2) * at(3, 3);
02657 
02658     }
02659 
02666     Matrix4<T> inverse()
02667     {
02668         Matrix4<T> ret;
02669 
02670         ret.at(0, 0) = +at(2, 1) * at(3, 2) * at(1, 3) - at(3, 1) * at(2, 2) * at(1, 3) + at(3, 1) * at(1, 2) * at(2, 3)
02671                 - at(1, 1) * at(3, 2) * at(2, 3) - at(2, 1) * at(1, 2) * at(3, 3) + at(1, 1) * at(2, 2) * at(3, 3);
02672 
02673         ret.at(1, 0) = +at(3, 0) * at(2, 2) * at(1, 3) - at(2, 0) * at(3, 2) * at(1, 3) - at(3, 0) * at(1, 2) * at(2, 3)
02674                 + at(1, 0) * at(3, 2) * at(2, 3) + at(2, 0) * at(1, 2) * at(3, 3) - at(1, 0) * at(2, 2) * at(3, 3);
02675 
02676         ret.at(2, 0) = +at(2, 0) * at(3, 1) * at(1, 3) - at(3, 0) * at(2, 1) * at(1, 3) + at(3, 0) * at(1, 1) * at(2, 3)
02677                 - at(1, 0) * at(3, 1) * at(2, 3) - at(2, 0) * at(1, 1) * at(3, 3) + at(1, 0) * at(2, 1) * at(3, 3);
02678 
02679         ret.at(3, 0) = +at(3, 0) * at(2, 1) * at(1, 2) - at(2, 0) * at(3, 1) * at(1, 2) - at(3, 0) * at(1, 1) * at(2, 2)
02680                 + at(1, 0) * at(3, 1) * at(2, 2) + at(2, 0) * at(1, 1) * at(3, 2) - at(1, 0) * at(2, 1) * at(3, 2);
02681 
02682         ret.at(0, 1) = +at(3, 1) * at(2, 2) * at(0, 3) - at(2, 1) * at(3, 2) * at(0, 3) - at(3, 1) * at(0, 2) * at(2, 3)
02683                 + at(0, 1) * at(3, 2) * at(2, 3) + at(2, 1) * at(0, 2) * at(3, 3) - at(0, 1) * at(2, 2) * at(3, 3);
02684 
02685         ret.at(1, 1) = +at(2, 0) * at(3, 2) * at(0, 3) - at(3, 0) * at(2, 2) * at(0, 3) + at(3, 0) * at(0, 2) * at(2, 3)
02686                 - at(0, 0) * at(3, 2) * at(2, 3) - at(2, 0) * at(0, 2) * at(3, 3) + at(0, 0) * at(2, 2) * at(3, 3);
02687 
02688         ret.at(2, 1) = +at(3, 0) * at(2, 1) * at(0, 3) - at(2, 0) * at(3, 1) * at(0, 3) - at(3, 0) * at(0, 1) * at(2, 3)
02689                 + at(0, 0) * at(3, 1) * at(2, 3) + at(2, 0) * at(0, 1) * at(3, 3) - at(0, 0) * at(2, 1) * at(3, 3);
02690 
02691         ret.at(3, 1) = +at(2, 0) * at(3, 1) * at(0, 2) - at(3, 0) * at(2, 1) * at(0, 2) + at(3, 0) * at(0, 1) * at(2, 2)
02692                 - at(0, 0) * at(3, 1) * at(2, 2) - at(2, 0) * at(0, 1) * at(3, 2) + at(0, 0) * at(2, 1) * at(3, 2);
02693 
02694         ret.at(0, 2) = +at(1, 1) * at(3, 2) * at(0, 3) - at(3, 1) * at(1, 2) * at(0, 3) + at(3, 1) * at(0, 2) * at(1, 3)
02695                 - at(0, 1) * at(3, 2) * at(1, 3) - at(1, 1) * at(0, 2) * at(3, 3) + at(0, 1) * at(1, 2) * at(3, 3);
02696 
02697         ret.at(1, 2) = +at(3, 0) * at(1, 2) * at(0, 3) - at(1, 0) * at(3, 2) * at(0, 3) - at(3, 0) * at(0, 2) * at(1, 3)
02698                 + at(0, 0) * at(3, 2) * at(1, 3) + at(1, 0) * at(0, 2) * at(3, 3) - at(0, 0) * at(1, 2) * at(3, 3);
02699 
02700         ret.at(2, 2) = +at(1, 0) * at(3, 1) * at(0, 3) - at(3, 0) * at(1, 1) * at(0, 3) + at(3, 0) * at(0, 1) * at(1, 3)
02701                 - at(0, 0) * at(3, 1) * at(1, 3) - at(1, 0) * at(0, 1) * at(3, 3) + at(0, 0) * at(1, 1) * at(3, 3);
02702 
02703         ret.at(3, 2) = +at(3, 0) * at(1, 1) * at(0, 2) - at(1, 0) * at(3, 1) * at(0, 2) - at(3, 0) * at(0, 1) * at(1, 2)
02704                 + at(0, 0) * at(3, 1) * at(1, 2) + at(1, 0) * at(0, 1) * at(3, 2) - at(0, 0) * at(1, 1) * at(3, 2);
02705 
02706         ret.at(0, 3) = +at(2, 1) * at(1, 2) * at(0, 3) - at(1, 1) * at(2, 2) * at(0, 3) - at(2, 1) * at(0, 2) * at(1, 3)
02707                 + at(0, 1) * at(2, 2) * at(1, 3) + at(1, 1) * at(0, 2) * at(2, 3) - at(0, 1) * at(1, 2) * at(2, 3);
02708 
02709         ret.at(1, 3) = +at(1, 0) * at(2, 2) * at(0, 3) - at(2, 0) * at(1, 2) * at(0, 3) + at(2, 0) * at(0, 2) * at(1, 3)
02710                 - at(0, 0) * at(2, 2) * at(1, 3) - at(1, 0) * at(0, 2) * at(2, 3) + at(0, 0) * at(1, 2) * at(2, 3);
02711 
02712         ret.at(2, 3) = +at(2, 0) * at(1, 1) * at(0, 3) - at(1, 0) * at(2, 1) * at(0, 3) - at(2, 0) * at(0, 1) * at(1, 3)
02713                 + at(0, 0) * at(2, 1) * at(1, 3) + at(1, 0) * at(0, 1) * at(2, 3) - at(0, 0) * at(1, 1) * at(2, 3);
02714 
02715         ret.at(3, 3) = +at(1, 0) * at(2, 1) * at(0, 2) - at(2, 0) * at(1, 1) * at(0, 2) + at(2, 0) * at(0, 1) * at(1, 2)
02716                 - at(0, 0) * at(2, 1) * at(1, 2) - at(1, 0) * at(0, 1) * at(2, 2) + at(0, 0) * at(1, 1) * at(2, 2);
02717 
02718         return ret / det();
02719     }
02720 
02724     Matrix4<T> transpose()
02725     {
02726         Matrix4<T> ret;
02727         for (int i = 0; i < 4; i++)
02728         {
02729             for (int j = 0; j < 4; j++)
02730             {
02731                 ret.at(i, j) = at(j, i);
02732             }
02733         }
02734         return ret;
02735     }
02736 
02746     Matrix4<T> lerp(T fact, const Matrix4<T>& rhs) const
02747     {
02748         Matrix4<T> ret = (*this) + (rhs - (*this)) * fact;
02749         return ret;
02750     }
02751 
02752     //-------------[ conversion ]-----------------------------
02758     operator T*()
02759     {
02760         return (T*) data;
02761     }
02762 
02768     operator const T*() const
02769     {
02770         return (const T*) data;
02771     }
02772 
02773     //----------[ output operator ]----------------------------
02780     friend std::ostream& operator <<(std::ostream& lhs, const Matrix4<T>& rhs)
02781     {
02782         for (int i = 0; i < 4; i++)
02783         {
02784             lhs << "|\t";
02785             for (int j = 0; j < 4; j++)
02786             {
02787                 lhs << rhs.at(j, i) << "\t";
02788             }
02789             lhs << "|" << std::endl;
02790         }
02791         return lhs;
02792     }
02793 
02797     std::string toString() const
02798     {
02799         std::ostringstream oss;
02800         oss << *this;
02801         return oss.str();
02802     }
02803 
02804 };
02805 
02807 typedef Matrix4<float> Matrix4f;
02809 typedef Matrix4<double> Matrix4d;
02811 typedef Matrix4<int> Matrix4i;
02812 
02820 template<class T>
02821 class Quaternion
02822 {
02823 public:
02827     T w;
02831     Vector3<T> v;
02832 
02836     Quaternion()
02837             : w(0), v(0, 0, 0)
02838     {
02839     }
02840 
02844     Quaternion(const Quaternion<T>& q)
02845             : w(q.w), v(q.v)
02846     {
02847     }
02848 
02852     template<class FromT>
02853     Quaternion(const Quaternion<FromT>& q)
02854             : w(static_cast<T>(q.w)), v(q.v)
02855     {
02856     }
02857 
02863     Quaternion(T w_, const Vector3<T>& v_)
02864             : w(w_), v(v_)
02865     {
02866     }
02867 
02875     Quaternion(T w_, T x, T y, T z)
02876             : w(w_), v(x, y, z)
02877     {
02878     }
02879 
02884     Quaternion<T>& operator=(const Quaternion<T>& rhs)
02885     {
02886         v = rhs.v;
02887         w = rhs.w;
02888         return *this;
02889     }
02890 
02895     template<class FromT>
02896     Quaternion<T>& operator=(const Quaternion<FromT>& rhs)
02897     {
02898         v = rhs.v;
02899         w = static_cast<T>(rhs.w);
02900         return *this;
02901     }
02902 
02907     Quaternion<T> operator+(const Quaternion<T>& rhs) const
02908     {
02909         const Quaternion<T>& lhs = *this;
02910         return Quaternion<T>(lhs.w + rhs.w, lhs.v + rhs.v);
02911     }
02912 
02917     Quaternion<T> operator*(const Quaternion<T>& rhs) const
02918     {
02919         const Quaternion<T>& lhs = *this;
02920         return Quaternion<T>(lhs.w * rhs.w - lhs.v.x * rhs.v.x - lhs.v.y * rhs.v.y - lhs.v.z * rhs.v.z,
02921                 lhs.w * rhs.v.x + lhs.v.x * rhs.w + lhs.v.y * rhs.v.z - lhs.v.z * rhs.v.y,
02922                 lhs.w * rhs.v.y - lhs.v.x * rhs.v.z + lhs.v.y * rhs.w + lhs.v.z * rhs.v.x,
02923                 lhs.w * rhs.v.z + lhs.v.x * rhs.v.y - lhs.v.y * rhs.v.x + lhs.v.z * rhs.w);
02924     }
02925 
02930     Quaternion<T> operator*(T rhs) const
02931     {
02932         return Quaternion<T>(w * rhs, v * rhs);
02933     }
02934 
02939     Quaternion<T> operator-(const Quaternion<T>& rhs) const
02940     {
02941         const Quaternion<T>& lhs = *this;
02942         return Quaternion<T>(lhs.w - rhs.w, lhs.v - rhs.v);
02943     }
02944 
02949     Quaternion<T>& operator+=(const Quaternion<T>& rhs)
02950     {
02951         w += rhs.w;
02952         v += rhs.v;
02953         return *this;
02954     }
02955 
02960     Quaternion<T>& operator-=(const Quaternion<T>& rhs)
02961     {
02962         w -= rhs.w;
02963         v -= rhs.v;
02964         return *this;
02965     }
02966 
02971     Quaternion<T>& operator*=(const Quaternion<T>& rhs)
02972     {
02973         Quaternion q = (*this) * rhs;
02974         v = q.v;
02975         w = q.w;
02976         return *this;
02977     }
02978 
02983     Quaternion<T>& operator*=(T rhs)
02984     {
02985         w *= rhs;
02986         v *= rhs;
02987         return *this;
02988     }
02989 
02997     bool operator==(const Quaternion<T>& rhs) const
02998     {
02999         const Quaternion<T>& lhs = *this;
03000         return (std::fabs(lhs.w - rhs.w) < EPSILON) && lhs.v == rhs.v;
03001     }
03002 
03008     bool operator!=(const Quaternion<T>& rhs) const
03009     {
03010         return !(*this == rhs);
03011     }
03012 
03013     //-------------[ unary operations ]--------------------------
03018     Quaternion<T> operator-() const
03019     {
03020         return Quaternion<T>(-w, -v);
03021     }
03022 
03027     Quaternion<T> operator~() const
03028     {
03029         return Quaternion<T>(w, -v);
03030     }
03031 
03036     T length() const
03037     {
03038         return (T) std::sqrt(w * w + v.lengthSq());
03039     }
03040 
03048     T lengthSq() const
03049     {
03050         return w * w + v.lengthSq();
03051     }
03052 
03056     void normalize()
03057     {
03058         T len = length();
03059         w /= len;
03060         v /= len;
03061     }
03062 
03070     static Quaternion<T> fromEulerAngles(T x, T y, T z)
03071     {
03072         Quaternion<T> ret = fromAxisRot(Vector3<T>(1, 0, 0), x) * fromAxisRot(Vector3<T>(0, 1, 0), y)
03073                 * fromAxisRot(Vector3<T>(0, 0, 1), z);
03074         return ret;
03075     }
03076 
03082     static Quaternion<T> fromAxisRot(Vector3<T> axis, float angleDeg)
03083     {
03084         double angleRad = DEG2RAD(angleDeg);
03085         double sa2 = std::sin(angleRad / 2);
03086         double ca2 = std::cos(angleRad / 2);
03087         return Quaternion<T>(ca2, axis * sa2);
03088     }
03089 
03094     Matrix3<T> rotMatrix()
03095     {
03096         Matrix3<T> ret;
03097 
03098         /*ret.at(0,0) = 1 - 2*v.y*v.y - 2*v.z*v.z;
03099          ret.at(1,0) = 2*v.x*v.y - 2*w*v.z;
03100          ret.at(2,0) = 2*v.x*v.z - 2*w*v.y;
03101          
03102          ret.at(0,1) = 2*v.x*v.y + 2*w*v.z;
03103          ret.at(1,1) = 1 - 2*v.x*v.x - 2*v.z*v.z;
03104          ret.at(2,1) = 2*v.y*v.z - 2*w*v.x;
03105          
03106          ret.at(0,2) = 2*v.x*v.z - 2*w*v.y;
03107          ret.at(1,2) = 2*v.y*v.z + 2*w*v.x;
03108          ret.at(2,2) = 1 - 2*v.x*v.x - 2*v.y*v.y;*/
03109 
03110         T xx = v.x * v.x;
03111         T xy = v.x * v.y;
03112         T xz = v.x * v.z;
03113         T xw = v.x * w;
03114 
03115         T yy = v.y * v.y;
03116         T yz = v.y * v.z;
03117         T yw = v.y * w;
03118 
03119         T zz = v.z * v.z;
03120         T zw = v.z * w;
03121 
03122         ret.at(0, 0) = 1 - 2 * (yy + zz);
03123         ret.at(1, 0) = 2 * (xy - zw);
03124         ret.at(2, 0) = 2 * (xz + yw);
03125 
03126         ret.at(0, 1) = 2 * (xy + zw);
03127         ret.at(1, 1) = 1 - 2 * (xx + zz);
03128         ret.at(2, 1) = 2 * (yz - xw);
03129 
03130         ret.at(0, 2) = 2 * (xz - yw);
03131         ret.at(1, 2) = 2 * (yz + xw);
03132         ret.at(2, 2) = 1 - 2 * (xx + yy);
03133 
03134         return ret;
03135     }
03136 
03143     Matrix4<T> transform() const
03144     {
03145         Matrix4<T> ret;
03146 
03147         T xx = v.x * v.x;
03148         T xy = v.x * v.y;
03149         T xz = v.x * v.z;
03150         T xw = v.x * w;
03151 
03152         T yy = v.y * v.y;
03153         T yz = v.y * v.z;
03154         T yw = v.y * w;
03155 
03156         T zz = v.z * v.z;
03157         T zw = v.z * w;
03158 
03159         ret.at(0, 0) = 1 - 2 * (yy + zz);
03160         ret.at(1, 0) = 2 * (xy - zw);
03161         ret.at(2, 0) = 2 * (xz + yw);
03162         ret.at(3, 0) = 0;
03163 
03164         ret.at(0, 1) = 2 * (xy + zw);
03165         ret.at(1, 1) = 1 - 2 * (xx + zz);
03166         ret.at(2, 1) = 2 * (yz - xw);
03167         ret.at(3, 1) = 0;
03168 
03169         ret.at(0, 2) = 2 * (xz - yw);
03170         ret.at(1, 2) = 2 * (yz + xw);
03171         ret.at(2, 2) = 1 - 2 * (xx + yy);
03172         ret.at(3, 2) = 0;
03173 
03174         ret.at(0, 3) = 0;
03175         ret.at(1, 3) = 0;
03176         ret.at(2, 3) = 0;
03177         ret.at(3, 3) = 1;
03178 
03179         return ret;
03180 
03181     }
03182 
03192     Quaternion<T> lerp(T fact, const Quaternion<T>& rhs) const
03193     {
03194         return Quaternion<T>((1 - fact) * w + fact * rhs.w, v.lerp(fact, rhs.v));
03195     }
03196 
03200     friend std::ostream& operator <<(std::ostream& oss, const Quaternion<T>& q)
03201     {
03202         oss << "Re: " << q.w << " Im: " << q.v;
03203         return oss;
03204     }
03205 
03209     std::string toString() const
03210     {
03211         std::ostringstream oss;
03212         oss << *this;
03213         return oss.str();
03214     }
03215 
03222     // 2011-07-02: Davide Bacchet: changed formula to fix degenerate cases
03223      static Quaternion<T> fromMatrix(const Matrix4<T>& m)
03224      {
03225         Quaternion<T> q;
03226 
03227         T tr, s;
03228         tr = m(1, 1) + m(2, 2) + m(3, 3);
03229         if (tr >= epsilon)
03230         {
03231             s = 0.5 / (T) sqrt(tr + 1.0);
03232             q.w = 0.25 / s;
03233             q.v.x = (m(3, 2) - m(2, 3)) * s;
03234             q.v.y = (m(1, 3) - m(3, 1)) * s;
03235             q.v.z = (m(2, 1) - m(1, 2)) * s;
03236         }
03237         else
03238         {
03239             T d0 = m(1, 1);
03240             T d1 = m(2, 2);
03241             T d2 = m(3, 3);
03242 
03243             char bigIdx = (d0 > d1) ? ((d0 > d2) ? 0 : 2):((d1 > d2) ? 1 : 2);
03244 
03245             if (bigIdx == 0)
03246             {
03247                 s = 2.0 * (T) sqrt(1.0 + m(1, 1) - m(2, 2) - m(3, 3));
03248                 q.w = (m(3, 2) - m(2, 3)) / s;
03249                 q.v.x = 0.25 * s;
03250                 q.v.y = (m(1, 2) + m(2, 1)) / s;
03251                 q.v.z = (m(1, 3) + m(3, 1)) / s;
03252             }
03253             else if (bigIdx == 1)
03254             {
03255                 s = 2.0 * (T) sqrt(1.0 + m(2, 2) - m(1, 1) - m(3, 3));
03256                 q.w = (m(1, 3) - m(3, 1)) / s;
03257                 q.v.x = (m(1, 2) + m(2, 1)) / s;
03258                 q.v.y = 0.25 * s;
03259                 q.v.z = (m(2, 3) + m(3, 2)) / s;
03260             }
03261             else
03262             {
03263                 s = 2.0 * (T) sqrt(1.0 + m(3, 3) - m(1, 1) - m(2, 2));
03264                 q.w = (m(2, 1) - m(1, 2)) / s;
03265                 q.v.x = (m(1, 3) + m(3, 1)) / s;
03266                 q.v.y = (m(2, 3) + m(3, 2)) / s;
03267                 q.v.z = 0.25 * s;
03268             }
03269         }
03270 
03271         return q;
03272     }
03273 
03281      // 2011-07-02: Davide Bacchet: changed formula to fix degenerate cases
03282      static Quaternion<T> fromMatrix(const Matrix3<T>& m)
03283      {
03284         Quaternion<T> q;
03285 
03286         T tr, s;
03287         tr = m(1, 1) + m(2, 2) + m(3, 3);
03288         if (tr >= epsilon)
03289         {
03290             s = 0.5 / (T) sqrt(tr + 1.0);
03291             q.w = 0.25 / s;
03292             q.v.x = (m(3, 2) - m(2, 3)) * s;
03293             q.v.y = (m(1, 3) - m(3, 1)) * s;
03294             q.v.z = (m(2, 1) - m(1, 2)) * s;
03295         }
03296         else
03297         {
03298             T d0 = m(1, 1);
03299             T d1 = m(2, 2);
03300             T d2 = m(3, 3);
03301 
03302             char bigIdx = (d0 > d1) ? ((d0 > d2) ? 0 : 2):((d1 > d2) ? 1 : 2);
03303 
03304             if (bigIdx == 0)
03305             {
03306                 s = 2.0 * (T) sqrt(1.0 + m(1, 1) - m(2, 2) - m(3, 3));
03307                 q.w = (m(3, 2) - m(2, 3)) / s;
03308                 q.v.x = 0.25 * s;
03309                 q.v.y = (m(1, 2) + m(2, 1)) / s;
03310                 q.v.z = (m(1, 3) + m(3, 1)) / s;
03311             }
03312             else if (bigIdx == 1)
03313             {
03314                 s = 2.0 * (T) sqrt(1.0 + m(2, 2) - m(1, 1) - m(3, 3));
03315                 q.w = (m(1, 3) - m(3, 1)) / s;
03316                 q.v.x = (m(1, 2) + m(2, 1)) / s;
03317                 q.v.y = 0.25 * s;
03318                 q.v.z = (m(2, 3) + m(3, 2)) / s;
03319             }
03320             else
03321             {
03322                 s = 2.0 * (T) sqrt(1.0 + m(3, 3) - m(1, 1) - m(2, 2));
03323                 q.w = (m(2, 1) - m(1, 2)) / s;
03324                 q.v.x = (m(1, 3) + m(3, 1)) / s;
03325                 q.v.y = (m(2, 3) + m(3, 2)) / s;
03326                 q.v.z = 0.25 * s;
03327             }
03328         }
03329 
03330         return q;
03331     }
03332 
03341     Quaternion<T> slerp(T r, const Quaternion<T>& q2) const
03342     {
03343         Quaternion<T> ret;
03344         T cosTheta = w * q2.w + v.x * q2.v.x + v.y * q2.v.y + v.z * q2.v.z;
03345         T theta = (T) acos(cosTheta);
03346         if (fabs(theta) < epsilon)
03347         {
03348             ret = *this;
03349         }
03350         else
03351         {
03352             T sinTheta = (T) sqrt(1.0 - cosTheta * cosTheta);
03353             if (fabs(sinTheta) < epsilon)
03354             {
03355                 ret.w = 0.5 * w + 0.5 * q2.w;
03356                 ret.v = v.lerp(0.5, q2.v);
03357             }
03358             else
03359             {
03360                 T rA = (T) sin((1.0 - r) * theta) / sinTheta;
03361                 T rB = (T) sin(r * theta) / sinTheta;
03362 
03363                 ret.w = w * rA + q2.w * rB;
03364                 ret.v.x = v.x * rA + q2.v.x * rB;
03365                 ret.v.y = v.y * rA + q2.v.y * rB;
03366                 ret.v.z = v.z * rA + q2.v.z * rB;
03367             }
03368         }
03369         return ret;
03370     }
03371 
03372 };
03373 
03374 typedef Quaternion<float> Quatf;
03375 typedef Quaternion<double> Quatd;
03376 
03377 #ifdef VMATH_NAMESPACE
03378 }
03379 #endif
03380 
03381 #endif // __vmath_Header_File__
03382 

Generated on Sun Jul 10 2011 22:47:47 for vmath by  doxygen 1.7.1