00001
00113
00114
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
00132 #endif
00133
00134 #define DEG2RAD(x) ((x * M_PI) / 180.0)
00135
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
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
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
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
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
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
00464 Vector2<T> operator-() const
00465 {
00466 return Vector2<T>(-x, -y);
00467 }
00468
00469
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
00511 Vector2<T> lerp(T fact, const Vector2<T>& r) const
00512 {
00513 return (*this) + (r - (*this)) * fact;
00514 }
00515
00516
00522 operator T*()
00523 {
00524 return (T*) this;
00525 }
00531 operator const T*() const
00532 {
00533 return (const T*) this;
00534 }
00535
00536
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
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
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
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
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
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
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
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
00960 Vector3<T> operator-() const
00961 {
00962 return Vector3<T>(-x, -y, -z);
00963 }
00964
00965
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
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
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
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
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
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
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
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
01385 Vector4<T> operator-() const
01386 {
01387 return Vector4<T>(-x, -y, -z, -w);
01388 }
01389
01390
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
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
01524 Vector4<T> lerp(T fact, const Vector4<T>& r) const
01525 {
01526 return (*this) + (r - (*this)) * fact;
01527 }
01528
01529
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
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
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
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
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
01849
01850
01851
01852
01853
01854
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
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
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
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
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
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
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
02178
02179
02180
02181
02182 Matrix4<T> ret = ma * mb * mc;
02183
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
02224
02225
02226
02227
02228
02229
02230
02231
02232
02233
02234
02235
02236
02237
02238
02239
02240
02241
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
02281
02282
02283
02284
02285
02286
02287
02288
02289
02290
02291
02292
02293
02294
02295
02296
02297
02298
02299
02300
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
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
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
02505
02506
02507
02508
02509
02510
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
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
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
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
02758 operator T*()
02759 {
02760 return (T*) data;
02761 }
02762
02768 operator const T*() const
02769 {
02770 return (const T*) data;
02771 }
02772
02773
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
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
03099
03100
03101
03102
03103
03104
03105
03106
03107
03108
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
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
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