00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00068 #ifndef __vmath_Header_File__
00069 #define __vmath_Header_File__
00070
00071 #include <cmath>
00072 #include <cstring>
00073 #include <iostream>
00074 #include <cassert>
00075
00076
00077
00078 #ifdef VMATH_NAMESPACE
00079 namespace VMATH_NAMESPACE
00080 {
00081 #endif
00082
00083 #ifndef M_PI
00084 #define M_PI 3.14159265358979323846
00085 #endif
00086
00087 #define DEG2RAD(x) ((x * M_PI) / 180.0)
00088
00089
00090 const double epsilon = 4.37114e-05;
00091 #define EPSILON epsilon
00092
00093
00097 template <class T>
00098 class Vector2
00099 {
00100 public:
00101 union
00102 {
00106 T x;
00107
00112 T s;
00113 };
00114
00115 union
00116 {
00120 T y;
00121
00126 T t;
00127 };
00128
00129
00133 Vector2() : x(0),y(0)
00134 { }
00135
00136
00137
00143 Vector2(T nx, T ny) : x(nx), y(ny)
00144 { }
00145
00146
00151 Vector2(const Vector2<T>& src)
00152 : x(src.x), y(src.y)
00153 { }
00154
00155
00160 template <class FromT>
00161 Vector2(const Vector2<FromT>& src)
00162 : x(static_cast<T>(src.x)),
00163 y(static_cast<T>(src.y))
00164 { }
00165
00166
00167
00172 template <class FromT>
00173 Vector2<T>& operator=(const Vector2<FromT>& rhs)
00174 {
00175 x = static_cast<T>(rhs.x);
00176 y = static_cast<T>(rhs.y);
00177 return * this;
00178 }
00179
00184 Vector2<T>& operator=(const Vector2<T>& rhs)
00185 {
00186 x = rhs.x;
00187 y = rhs.y;
00188 return * this;
00189 }
00190
00191
00198 T& operator[](int n)
00199 {
00200 assert(n >= 0 && n <= 1);
00201 if (0 == n) return x;
00202 else
00203 return y;
00204 }
00205
00206
00207
00212 Vector2<T> operator+(const Vector2<T>& rhs) const
00213 {
00214 return Vector2<T> (x + rhs.x, y + rhs.y);
00215 }
00216
00221 Vector2<T> operator-(const Vector2<T>& rhs) const
00222 {
00223 return Vector2<T> (x - rhs.x, y - rhs.y);
00224 }
00225
00230 Vector2<T> operator*(const Vector2<T>& rhs) const
00231 {
00232 return Vector2<T> (x * rhs.x, y * rhs.y);
00233 }
00234
00239 Vector2<T> operator/(const Vector2<T>& rhs) const
00240 {
00241 return Vector2<T> (x / rhs.x, y / rhs.y);
00242 }
00243
00248 Vector2<T>& operator+=(const Vector2<T>& rhs)
00249 {
00250 x += rhs.x;
00251 y += rhs.y;
00252 return * this;
00253 }
00254
00259 Vector2<T>& operator-=(const Vector2<T>& rhs)
00260 {
00261 x -= rhs.x;
00262 y -= rhs.y;
00263 return * this;
00264 }
00265
00270 Vector2<T>& operator*=(const Vector2<T>& rhs)
00271 {
00272 x *= rhs.x;
00273 y *= rhs.y;
00274 return * this;
00275 }
00276
00281 Vector2<T>& operator/=(const Vector2<T>& rhs)
00282 {
00283 x /= rhs.x;
00284 y /= rhs.y;
00285 return * this;
00286 }
00287
00288
00293 Vector2<T> operator+(T rhs) const
00294 {
00295 return Vector2<T> (x + rhs, y + rhs);
00296 }
00297
00302 Vector2<T> operator-(T rhs) const
00303 {
00304 return Vector2<T> (x - rhs, y - rhs);
00305 }
00306
00311 Vector2<T> operator*(T rhs) const
00312 {
00313 return Vector2<T> (x * rhs, y * rhs);
00314 }
00315
00320 Vector2<T> operator/(T rhs) const
00321 {
00322 return Vector2<T> (x / rhs, y / rhs);
00323 }
00324
00329 Vector2<T>& operator+=(T rhs)
00330 {
00331 x += rhs;
00332 y += rhs;
00333 return * this;
00334 }
00335
00340 Vector2<T>& operator-=(T rhs)
00341 {
00342 x -= rhs;
00343 y -= rhs;
00344 return * this;
00345 }
00346
00351 Vector2<T>& operator*=(T rhs)
00352 {
00353 x *= rhs;
00354 y *= rhs;
00355 return * this;
00356 }
00357
00362 Vector2<T>& operator/=(T rhs)
00363 {
00364 x /= rhs;
00365 y /= rhs;
00366 return * this;
00367 }
00368
00369
00377 bool operator==(const Vector2<T>& rhs) const
00378 {
00379 return (std::abs(x - rhs.x) < EPSILON) && (std::abs(y - rhs.y) < EPSILON);
00380 }
00381
00382
00388 bool operator!=(const Vector2<T>& rhs) const { return ! (*this == rhs); }
00389
00390
00395 Vector2<T> operator-() const
00396 {
00397 return Vector2<T>(-x, -y);
00398 }
00399
00400
00405 T length() const
00406 {
00407 return (T)std::sqrt(x * x + y * y);
00408 }
00409
00413 void normalize()
00414 {
00415 T s = length();
00416 x /= s;
00417 y /= s;
00418 }
00419
00427 T lengthSq() const
00428 {
00429 return x * x + y * y;
00430 }
00431
00432
00442 Vector2<T> lerp(T fact, const Vector2<T>& r) const
00443 {
00444 return (*this) + (r - (*this)) * fact;
00445 }
00446
00447
00448
00454 operator T*(){ return (T*) this; }
00460 operator const T*() const { return (const T*) this; }
00461
00462
00469 friend std::ostream& operator<<(std::ostream& lhs, const Vector2<T>& rhs)
00470 {
00471 lhs << "[" << rhs.x << "," << rhs.y << "]";
00472 return lhs;
00473 }
00474
00475
00476 };
00477
00478
00479
00480
00481
00483 typedef class Vector2 <float> Vector2f;
00485 typedef class Vector2 <double> Vector2d;
00486
00487
00491 template <class T>
00492 class Vector3
00493 {
00494 public:
00495
00496 union
00497 {
00501 T x;
00502
00507 T s;
00508
00513 T r;
00514 };
00515
00516 union
00517 {
00521 T y;
00526 T t;
00531 T g;
00532 };
00533
00534 union
00535 {
00539 T z;
00540
00545 T u;
00550 T b;
00551 };
00552
00553
00557 Vector3() : x(0),y(0),z(0)
00558 { }
00559
00566 Vector3(T nx, T ny, T nz) : x(nx),y(ny),z(nz)
00567 { }
00568
00573 Vector3(const Vector3<T>& src)
00574 : x(src.x), y(src.y), z(src.z)
00575 {}
00576
00581 template <class FromT>
00582 Vector3(const Vector3<FromT>& src)
00583 : x(static_cast<T>(src.x)),
00584 y(static_cast<T>(src.y)),
00585 z(static_cast<T>(src.z))
00586 {}
00587
00588
00589
00594 Vector3<T> operator=(const Vector3<T>& rhs)
00595 {
00596 x = rhs.x;
00597 y = rhs.y;
00598 z = rhs.z;
00599 return * this;
00600 }
00601
00606 template <class FromT>
00607 Vector3<T> operator=(const Vector3<FromT>& rhs)
00608 {
00609 x = static_cast<T>(rhs.x);
00610 y = static_cast<T>(rhs.y);
00611 z = static_cast<T>(rhs.z);
00612 return * this;
00613 }
00614
00622 T & operator[](int n)
00623 {
00624 assert(n >= 0 && n <= 2);
00625 if (0 == n) return x;
00626 else if (1 == n) return y;
00627 else
00628 return z;
00629 }
00630
00631
00632
00637 Vector3<T> operator+(const Vector3<T>& rhs) const
00638 {
00639 return Vector3<T> (x + rhs.x, y + rhs.y, z + rhs.z);
00640 }
00641
00646 Vector3<T> operator-(const Vector3<T>& rhs) const
00647 {
00648 return Vector3<T> (x - rhs.x, y - rhs.y, z - rhs.z);
00649 }
00650
00655 Vector3<T> operator*(const Vector3<T>& rhs) const
00656 {
00657 return Vector3<T> (x * rhs.x, y * rhs.y, z * rhs.z);
00658 }
00659
00664 Vector3<T> operator/(const Vector3<T>& rhs) const
00665 {
00666 return Vector3<T> (x / rhs.x, y / rhs.y, z / rhs.z);
00667 }
00668
00673 Vector3<T>& operator+=(const Vector3<T>& rhs)
00674 {
00675 x += rhs.x;
00676 y += rhs.y;
00677 z += rhs.z;
00678 return * this;
00679 }
00680
00685 Vector3<T>& operator-=(const Vector3<T>& rhs)
00686 {
00687 x -= rhs.x;
00688 y -= rhs.y;
00689 z -= rhs.z;
00690 return * this;
00691 }
00692
00697 Vector3<T>& operator*=(const Vector3<T>& rhs)
00698 {
00699 x *= rhs.x;
00700 y *= rhs.y;
00701 z *= rhs.z;
00702 return * this;
00703 }
00704
00709 Vector3<T>& operator/=(const Vector3<T>& rhs)
00710 {
00711 x /= rhs.x;
00712 y /= rhs.y;
00713 z /= rhs.z;
00714 return * this;
00715 }
00716
00721 T dotProduct(const Vector3<T>& rhs) const
00722 {
00723 return x * rhs.x + y * rhs.y + z * rhs.z;
00724 }
00725
00730 Vector3<T> crossProduct(const Vector3<T>& rhs) const
00731 {
00732 return Vector3<T> (y * rhs.z - rhs.y * z, z * rhs.x - rhs.z * x, x * rhs.y - rhs.x * y);
00733 }
00734
00735
00736
00741 Vector3<T> operator+(T rhs) const
00742 {
00743 return Vector3<T> (x + rhs, y + rhs, z + rhs);
00744 }
00745
00750 Vector3<T> operator-(T rhs) const
00751 {
00752 return Vector3<T> (x - rhs, y - rhs, z - rhs);
00753 }
00754
00759 Vector3<T> operator*(T rhs) const
00760 {
00761 return Vector3<T> (x * rhs, y * rhs, z * rhs);
00762 }
00763
00768 Vector3<T> operator/(T rhs) const
00769 {
00770 return Vector3<T> (x / rhs, y / rhs, z / rhs);
00771 }
00772
00777 Vector3<T>& operator+=(T rhs)
00778 {
00779 x += rhs;
00780 y += rhs;
00781 z += rhs;
00782 return * this;
00783 }
00784
00789 Vector3<T>& operator-=(T rhs)
00790 {
00791 x -= rhs;
00792 y -= rhs;
00793 z -= rhs;
00794 return * this;
00795 }
00796
00801 Vector3<T>& operator*=(T rhs)
00802 {
00803 x *= rhs;
00804 y *= rhs;
00805 z *= rhs;
00806 return * this;
00807 }
00808
00813 Vector3<T>& operator/=(T rhs)
00814 {
00815 x /= rhs;
00816 y /= rhs;
00817 z /= rhs;
00818 return * this;
00819 }
00820
00821
00829 bool operator==(const Vector3<T>& rhs) const
00830 {
00831 return std::fabs(x - rhs.x) < EPSILON
00832 && std::fabs(y - rhs.y) < EPSILON
00833 && std::fabs(z - rhs.z) < EPSILON;
00834 }
00835
00841 bool operator!=(const Vector3<T>& rhs) const { return !(*this == rhs); }
00842
00843
00848 Vector3<T> operator-() const
00849 {
00850 return Vector3<T>(-x, -y, -z);
00851 }
00852
00853
00858 T length() const
00859 {
00860 return (T)std::sqrt(x * x + y * y + z * z);
00861 }
00862
00870 T lengthSq() const
00871 {
00872 return x * x + y * y + z * z;
00873 }
00874
00878 void normalize()
00879 {
00880 T s = length();
00881 x /= s;
00882 y /= s;
00883 z /= s;
00884 }
00885
00886
00893 void rotate(T ax, T ay, T az)
00894 {
00895 T a = cos(DEG2RAD(ax));
00896 T b = sin(DEG2RAD(ax));
00897 T c = cos(DEG2RAD(ay));
00898 T d = sin(DEG2RAD(ay));
00899 T e = cos(DEG2RAD(az));
00900 T f = sin(DEG2RAD(az));
00901 T nx = c * e * x - c * f * y + d * z;
00902 T ny = (a * f + b * d * e) * x + (a * e - b * d * f) * y - b * c * z;
00903 T nz = (b * f - a * d * e) * x + (a * d * f + b * e) * y + a * c * z;
00904 x = nx; y = ny; z = nz;
00905
00906
00907 }
00908
00918 Vector3<T> lerp(T fact, const Vector3<T>& r) const
00919 {
00920 return (*this) + (r - (*this)) * fact;
00921 }
00922
00923
00924
00925
00926
00927
00928
00934 operator T*(){ return (T*) this; }
00935
00941 operator const T*() const { return (const T*) this; }
00942
00943
00950 friend std::ostream& operator<<(std::ostream& lhs, const Vector3<T> rhs)
00951 {
00952 lhs << "[" << rhs.x << "," << rhs.y << "," << rhs.z << "]";
00953 return lhs;
00954 }
00955
00956 };
00957
00958
00960 typedef Vector3 <float> Vector3f;
00962 typedef Vector3 <double> Vector3d;
00963
00967 template <class T>
00968 class Vector4{
00969 public:
00970
00971 union
00972 {
00977 T r;
00981 T x;
00982 };
00983
00984 union
00985 {
00990 T g;
00994 T y;
00995 };
00996
00997 union
00998 {
01003 T b;
01007 T z;
01008 };
01009
01010 union
01011 {
01016 T a;
01022 T w;
01023 };
01024
01025
01029 Vector4() : x(0),y(0),z(0),w(0)
01030 { }
01031
01032
01040 Vector4(T nx, T ny, T nz, T nw) : x(nx), y(ny), z(nz), w(nw)
01041 { }
01042
01047 Vector4(const Vector4<T>& src)
01048 : x(src.x), y(src.y), z(src.z), w(src.w)
01049 {}
01050
01055 template <class FromT>
01056 Vector4(const Vector4<FromT>& src)
01057 : x(static_cast<T>(src.x)),
01058 y(static_cast<T>(src.y)),
01059 z(static_cast<T>(src.z)),
01060 w(static_cast<T>(src.w))
01061 {}
01062
01063
01064
01069 Vector4<T> operator=(const Vector4<T>& rhs)
01070 {
01071 x = rhs.x;
01072 y = rhs.y;
01073 z = rhs.z;
01074 w = rhs.w;
01075 return * this;
01076 }
01077
01082 template<class FromT>
01083 Vector4<T> operator=(const Vector4<FromT>& rhs)
01084 {
01085 x = static_cast<T>(rhs.x);
01086 y = static_cast<T>(rhs.y);
01087 z = static_cast<T>(rhs.z);
01088 w = static_cast<T>(rhs.w);
01089 return * this;
01090 }
01091
01092
01100 T & operator[](int n)
01101 {
01102 assert(n >= 0 && n <= 3);
01103 if (0 == n) return x;
01104 else if (1 == n) return y;
01105 else if (2 == n) return z;
01106 else return w;
01107 }
01108
01109
01110
01115 Vector4<T> operator+(const Vector4<T>& rhs) const
01116 {
01117 return Vector4<T> (x + rhs.x, y + rhs.y, z + rhs.z, w + rhs.w);
01118 }
01119
01124 Vector4<T> operator-(const Vector4<T>& rhs) const
01125 {
01126 return Vector4<T> (x - rhs.x, y - rhs.y, z - rhs.z, w - rhs.w);
01127 }
01128
01133 Vector4<T> operator*(const Vector4<T> rhs) const
01134 {
01135 return Vector4<T> (x * rhs.x, y * rhs.y, z * rhs.z, w * rhs.w);
01136 }
01137
01142 Vector4<T> operator/(const Vector4<T>& rhs) const
01143 {
01144 return Vector4<T> (x / rhs.x, y / rhs.y, z / rhs.z, w / rhs.w);
01145 }
01146
01151 Vector4<T>& operator+=(const Vector4<T>& rhs)
01152 {
01153 x += rhs.x;
01154 y += rhs.y;
01155 z += rhs.z;
01156 w += rhs.w;
01157 return * this;
01158 }
01159
01164 Vector4<T>& operator-=(const Vector4<T>& rhs)
01165 {
01166 x -= rhs.x;
01167 y -= rhs.y;
01168 z -= rhs.z;
01169 w -= rhs.w;
01170 return * this;
01171 }
01172
01177 Vector4<T>& operator*=(const Vector4<T>& rhs)
01178 {
01179 x *= rhs.x;
01180 y *= rhs.y;
01181 z *= rhs.z;
01182 w *= rhs.w;
01183 return * this;
01184 }
01185
01190 Vector4<T>& operator/=(const Vector4<T>& rhs)
01191 {
01192 x /= rhs.x;
01193 y /= rhs.y;
01194 z /= rhs.z;
01195 w /= rhs.w;
01196 return * this;
01197 }
01198
01199
01207 bool operator==(const Vector4<T>& rhs) const
01208 {
01209 return std::fabs(x - rhs.x) < EPSILON
01210 && std::fabs(y - rhs.y) < EPSILON
01211 && std::fabs(z - rhs.z) < EPSILON
01212 && std::fabs(w - rhs.w) < EPSILON;
01213 }
01214
01215
01221 bool operator!=(const Vector4<T>& rhs) const { return ! (*this == rhs); }
01222
01223
01228 Vector4<T> operator-() const
01229 {
01230 return Vector4<T>(-x, -y, -z, -w);
01231 }
01232
01233
01234
01239 Vector4<T> operator+(T rhs) const
01240 {
01241 return Vector4<T> (x + rhs, y + rhs, z + rhs, w + rhs);
01242 }
01243
01248 Vector4<T> operator-(T rhs) const
01249 {
01250 return Vector4<T> (x - rhs, y - rhs, z - rhs, w - rhs);
01251 }
01252
01257 Vector4<T> operator*(T rhs) const
01258 {
01259 return Vector4<T> (x * rhs, y * rhs, z * rhs, w * rhs);
01260 }
01261
01266 Vector4<T> operator/(T rhs) const
01267 {
01268 return Vector4<T> (x / rhs, y / rhs, z / rhs, w / rhs);
01269 }
01270
01275 Vector4<T>& operator+=(T rhs)
01276 {
01277 x += rhs;
01278 y += rhs;
01279 z += rhs;
01280 w += rhs;
01281 return * this;
01282 }
01283
01288 Vector4<T>& operator-=(T rhs)
01289 {
01290 x -= rhs;
01291 y -= rhs;
01292 z -= rhs;
01293 w -= rhs;
01294 return * this;
01295 }
01296
01301 Vector4<T>& operator*=(T rhs)
01302 {
01303 x *= rhs;
01304 y *= rhs;
01305 z *= rhs;
01306 w *= rhs;
01307 return * this;
01308 }
01309
01314 Vector4<T>& operator/=(T rhs)
01315 {
01316 x /= rhs;
01317 y /= rhs;
01318 z /= rhs;
01319 w /= rhs;
01320 return * this;
01321 }
01322
01323
01328 T length() const
01329 {
01330 return (T)std::sqrt(x * x + y * y + z * z + w * w);
01331 }
01332
01336 void normalize()
01337 {
01338 T s = length();
01339 x /= s;
01340 y /= s;
01341 z /= s;
01342 w /= s;
01343 }
01344
01352 T lengthSq() const
01353 {
01354 return x * x + y * y + z * z + w * w;
01355 }
01356
01357
01367 Vector4<T> lerp(T fact, const Vector4<T>& r) const
01368 {
01369 return (*this) + (r - (*this)) * fact;
01370 }
01371
01372
01373
01374
01375
01376
01382 operator T*(){ return (T*) this; }
01383
01384
01390 operator const T*() const { return (const T*) this; }
01391
01392
01399 friend std::ostream& operator<<(std::ostream& lhs, const Vector4<T>& rhs)
01400 {
01401 lhs << "[" << rhs.x << "," << rhs.y << "," << rhs.z << "," << rhs.w << "]";
01402 return lhs;
01403 }
01404 };
01405
01407 typedef Vector4<float> Vector4f;
01409 typedef Vector4<double> Vector4d;
01410
01411
01412
01413
01414
01419 template <class T>
01420 class Matrix3
01421 {
01422 public:
01424 T data[9];
01425
01426
01430 Matrix3()
01431 {
01432 for (int i = 0; i < 9; i++)
01433 data[i] = (i % 4) ? 0 : 1;
01434 }
01435
01440 Matrix3(const T * dt)
01441 {
01442 std::memcpy(data, dt, sizeof(T) * 9);
01443 }
01444
01449 Matrix3(const Matrix3<T>& src)
01450 {
01451 std::memcpy(data, src.data, sizeof(T) * 9);
01452 }
01453
01458 template<class FromT>
01459 Matrix3(const Matrix3<FromT>& src)
01460 {
01461 for (int i = 0; i < 9; i++)
01462 {
01463 data[i] = static_cast<T>(src.data[i]);
01464 }
01465 }
01466
01470 void identity()
01471 {
01472 for (int i = 0; i < 9; i++)
01473 data[i] = (i % 4) ? 0 : 1;
01474 }
01475
01476
01477
01484 static Matrix3<T> createRotationAroundAxis(T a, T b, T c)
01485 {
01486 Matrix3<T> ma, mb, mc;
01487 float ac = cos(a);
01488 float as = sin(a);
01489 float bc = cos(b);
01490 float bs = sin(b);
01491 float cc = cos(c);
01492 float cs = sin(c);
01493
01494 ma.at(1,1) = ac;
01495 ma.at(2,1) = as;
01496 ma.at(1,2) = -as;
01497 ma.at(2,2) = ac;
01498
01499 mb.at(0,0) = bc;
01500 mb.at(2,0) = -bs;
01501 mb.at(0,2) = bs;
01502 mb.at(2,2) = bc;
01503
01504 mc.at(0,0) = cc;
01505 mc.at(1,0) = cs;
01506 mc.at(0,1) = -cs;
01507 mc.at(1,1) = cc;
01508
01509 Matrix3<T> ret = ma * mb * mc;
01510 return ret;
01511 }
01512
01516 template <class It>
01517 static Matrix3<T> fromOde(const It* mat)
01518 {
01519 Matrix3<T> ret;
01520 for (int i = 0; i < 3; i++)
01521 {
01522 for (int j = 0; j < 3; j++)
01523 {
01524 ret.at(i,j) = static_cast<T>(mat[j * 4 + i]);
01525 }
01526 }
01527 return ret;
01528 }
01529
01530
01531
01540 bool operator==(const Matrix3<T>& rhs) const
01541 {
01542 for (int i = 0; i < 9; i++)
01543 {
01544 if (std::fabs(data[i] - rhs.data[i]) >= EPSILON)
01545 return false;
01546 }
01547 return true;
01548 }
01549
01550
01556 bool operator!=(const Matrix3<T>& rhs) const
01557 {
01558 return !(*this == rhs);
01559 }
01560
01561
01562
01568 T& at(int x, int y)
01569 {
01570 assert(x >= 0 && x < 3);
01571 assert(y >= 0 && y < 3);
01572 return data[x * 3 + y];
01573 }
01574
01580 const T& at(int x, int y) const
01581 {
01582 assert(x >= 0 && x < 3);
01583 assert(y >= 0 && y < 3);
01584 return data[x * 3 + y];
01585 }
01586
01591 Matrix3<T>& operator=(const Matrix3<T>& rhs)
01592 {
01593 std::memcpy(data, rhs.data, sizeof(T) * 9);
01594 return * this;
01595 }
01596
01601 template<class FromT>
01602 Matrix3<T>& operator=(const Matrix3<FromT>& rhs)
01603 {
01604 for (int i = 0; i < 9; i++)
01605 {
01606 data[i] = static_cast<T>(rhs.data[i]);
01607 }
01608 return * this;
01609 }
01610
01615 Matrix3<T>& operator=(const T* rhs)
01616 {
01617 std::memcpy(data, rhs, sizeof(T) * 9);
01618 return * this;
01619 }
01620
01621
01622
01623
01624
01625
01626
01627
01628
01633 Matrix3<T> operator+(const Matrix3<T>& rhs) const
01634 {
01635 Matrix3<T> ret;
01636 for (int i = 0; i < 9; i++)
01637 ret.data[i] = data[i] + rhs.data[i];
01638 return ret;
01639 }
01640
01645 Matrix3<T> operator-(const Matrix3<T>& rhs) const
01646 {
01647 Matrix3<T> ret;
01648 for (int i = 0; i < 9; i++)
01649 ret.data[i] = data[i] - rhs.data[i];
01650 return ret;
01651 }
01652
01653
01658 Matrix3<T> operator+(T rhs) const
01659 {
01660 Matrix3<T> ret;
01661 for (int i = 0; i < 9; i++)
01662 ret.data[i] = data[i] + rhs;
01663 return ret;
01664 }
01665
01670 Matrix3<T> operator-(T rhs) const
01671 {
01672 Matrix3<T> ret;
01673 for (int i = 0; i < 9; i++)
01674 ret.data[i] = data[i] - rhs;
01675 return ret;
01676 }
01677
01682 Matrix3<T> operator*(T rhs) const
01683 {
01684 Matrix3<T> ret;
01685 for (int i = 0; i < 9; i++)
01686 ret.data[i] = data[i] * rhs;
01687 return ret;
01688 }
01689
01694 Matrix3<T> operator/(T rhs) const
01695 {
01696 Matrix3<T> ret;
01697 for (int i = 0; i < 9; i++)
01698 ret.data[i] = data[i] / rhs;
01699 return ret;
01700 }
01701
01702
01703
01708 Vector3<T> operator*(const Vector3<T>& rhs) const
01709 {
01710 return Vector3<T>(
01711 data[0] * rhs.x + data[3] * rhs.y + data[6] * rhs.z,
01712 data[1] * rhs.x + data[4] * rhs.y + data[7] * rhs.z,
01713 data[2] * rhs.x + data[5] * rhs.y + data[8] * rhs.z
01714 );
01715 }
01716
01721 Matrix3<T> operator*(Matrix3<T> rhs) const
01722 {
01723 static Matrix3<T> w;
01724 for (int i = 0; i < 3; i++)
01725 {
01726 for (int j = 0; j < 3; j++)
01727 {
01728 T n = 0;
01729 for (int k = 0; k < 3; k++) n += rhs.at(i, k) * at(k, j);
01730 w.at(i, j) = n;
01731 }
01732 }
01733 return w;
01734
01735 }
01736
01737
01738
01742 Matrix3<T> transpose()
01743 {
01744 Matrix3<T> ret;
01745 for (int i = 0; i < 3; i++)
01746 {
01747 for (int j = 0; j < 3; j++)
01748 {
01749 ret.at(i,j) = at(j,i);
01750 }
01751 }
01752 return ret;
01753 }
01754
01764 Matrix3<T> lerp(T fact, const Matrix3<T>& rhs) const
01765 {
01766 Matrix3<T> ret = (*this) + (rhs - (*this)) * fact;
01767 return ret;
01768 }
01769
01770
01771
01772
01778 operator T*(){ return (T*) data; }
01779
01785 operator const T*() const { return (const T*) data; }
01786
01787
01794 friend std::ostream& operator << (std::ostream& lhs, const Matrix3<T>& rhs)
01795 {
01796 for (int i = 0; i < 3; i++)
01797 {
01798 lhs << "|\t";
01799 for (int j = 0; j < 3; j++)
01800 {
01801 lhs << rhs.at(j,i) << "\t";
01802 }
01803 lhs << "|" << std::endl;
01804 }
01805 return lhs;
01806 }
01807
01808 };
01809
01811 typedef Matrix3<float> Matrix3f;
01813 typedef Matrix3<double> Matrix3d;
01814
01815
01820 template <class T>
01821 class Matrix4
01822 {
01823 public:
01825 T data[16];
01826
01827
01831 Matrix4()
01832 {
01833 for (int i = 0; i < 16; i++)
01834 data[i] = (i % 5) ? 0 : 1;
01835 }
01836
01841 Matrix4(const T * dt)
01842 {
01843 std::memcpy(data, dt, sizeof(T) * 16);
01844 }
01845
01850 Matrix4(const Matrix4<T>& src)
01851 {
01852 std::memcpy(data, src.data, sizeof(T) * 16);
01853 }
01854
01859 template <class FromT>
01860 Matrix4(const Matrix4<FromT>& src)
01861 {
01862 for (int i = 0; i < 16; i++)
01863 {
01864 data[i] = static_cast<T>(src.data[i]);
01865 }
01866 }
01867
01871 void identity()
01872 {
01873 for (int i = 0; i < 16; i++)
01874 data[i] = (i % 5) ? 0 : 1;
01875 }
01876
01877
01878
01885 static Matrix4<T> createRotationAroundAxis(T a, T b, T c)
01886 {
01887 Matrix4<T> ma, mb, mc;
01888 float ac = cos(a);
01889 float as = sin(a);
01890 float bc = cos(b);
01891 float bs = sin(b);
01892 float cc = cos(c);
01893 float cs = sin(c);
01894
01895 ma.at(1,1) = ac;
01896 ma.at(2,1) = as;
01897 ma.at(1,2) = -as;
01898 ma.at(2,2) = ac;
01899
01900 mb.at(0,0) = bc;
01901 mb.at(2,0) = -bs;
01902 mb.at(0,2) = bs;
01903 mb.at(2,2) = bc;
01904
01905 mc.at(0,0) = cc;
01906 mc.at(1,0) = cs;
01907 mc.at(0,1) = -cs;
01908 mc.at(1,1) = cc;
01909
01910
01911
01912
01913
01914
01915 Matrix4<T> ret = ma * mb * mc;
01916
01917
01918 return ret;
01919 }
01920
01922
01929 static Matrix4<T> createTranslation(T x, T y, T z, T w = 1)
01930 {
01931 Matrix4 ret;
01932 ret.at(3,0) = x;
01933 ret.at(3,1) = y;
01934 ret.at(3,2) = z;
01935 ret.at(3,3) = w;
01936
01937 return ret;
01938 }
01939
01940
01941
01950 bool operator==(const Matrix4<T>& rhs) const
01951 {
01952 for (int i = 0; i < 16; i++)
01953 {
01954 if (std::fabs(data[i] - rhs.data[i]) >= EPSILON)
01955 return false;
01956 }
01957 return true;
01958 }
01959
01965 bool operator!=(const Matrix4<T>& rhs) const
01966 {
01967 return !(*this == rhs);
01968 }
01969
01970
01971
01977 T& at(int x, int y)
01978 {
01979 assert(x >= 0 && x < 4);
01980 assert(y >= 0 && y < 4);
01981 return data[x * 4 + y];
01982 }
01983
01989 const T& at(int x, int y) const
01990 {
01991 assert(x >= 0 && x < 4);
01992 assert(y >= 0 && y < 4);
01993 return data[x * 4 + y];
01994 }
01995
01996
02002 void setTranslation(const Vector3<T>& v)
02003 {
02004 at(3,0) = v.x;
02005 at(3,1) = v.y;
02006 at(3,2) = v.z;
02007 at(3,3) = 1;
02008 }
02009
02010 Vector3<T> getTranslation()
02011 { return Vector3<T>(at(3,0),at(3,1),at(3,2)); }
02012
02018 void setRotation(const Matrix3<T>& m)
02019 {
02020 for (int i = 0; i < 3; i++)
02021 {
02022 for (int j = 0; j < 3; j++)
02023 {
02024 at(i,j) = m.at(i,j);
02025 }
02026 }
02027 }
02028
02029
02034 Matrix4<T>& operator=(const Matrix4<T>& rhs)
02035 {
02036 std::memcpy(data, rhs.data, sizeof(T) * 16);
02037 return * this;
02038 }
02039
02044 template <class FromT>
02045 Matrix4<T>& operator=(const Matrix4<FromT>& rhs)
02046 {
02047 for (int i = 0; i < 16; i++)
02048 {
02049 data[i] = static_cast<T>(rhs.data[i]);
02050 }
02051 return * this;
02052 }
02053
02058 Matrix4<T>& operator=(const T* rhs)
02059 {
02060 std::memcpy(data, rhs, sizeof(T) * 16);
02061 return * this;
02062 }
02063
02064
02065
02066
02067
02068
02069
02070
02075 Matrix4<T> operator+(const Matrix4<T>& rhs) const
02076 {
02077 Matrix4<T> ret;
02078 for (int i = 0; i < 16; i++)
02079 ret.data[i] = data[i] + rhs.data[i];
02080 return ret;
02081 }
02082
02087 Matrix4<T> operator-(const Matrix4<T>& rhs) const
02088 {
02089 Matrix4<T> ret;
02090 for (int i = 0; i < 16; i++)
02091 ret.data[i] = data[i] - rhs.data[i];
02092 return ret;
02093 }
02094
02095
02100 Matrix4<T> operator+(T rhs) const
02101 {
02102 Matrix4<T> ret;
02103 for (int i = 0; i < 16; i++)
02104 ret.data[i] = data[i] + rhs;
02105 return ret;
02106 }
02107
02112 Matrix4<T> operator-(T rhs) const
02113 {
02114 Matrix4<T> ret;
02115 for (int i = 0; i < 16; i++)
02116 ret.data[i] = data[i] - rhs;
02117 return ret;
02118 }
02119
02124 Matrix4<T> operator*(T rhs) const
02125 {
02126 Matrix4<T> ret;
02127 for (int i = 0; i < 16; i++)
02128 ret.data[i] = data[i] * rhs;
02129 return ret;
02130 }
02131
02136 Matrix4<T> operator/(T rhs) const
02137 {
02138 Matrix4<T> ret;
02139 for (int i = 0; i < 16; i++)
02140 ret.data[i] = data[i] / rhs;
02141 return ret;
02142 }
02143
02144
02145
02150 Vector4<T> operator*(const Vector4<T>& rhs) const
02151 {
02152 return Vector4<T>(
02153 data[0] * rhs.x + data[4] * rhs.y + data[8] * rhs.z + data[12] * rhs.w,
02154 data[1] * rhs.x + data[5] * rhs.y + data[9] * rhs.z + data[13] * rhs.w,
02155 data[2] * rhs.x + data[6] * rhs.y + data[10] * rhs.z + data[14] * rhs.w,
02156 data[3] * rhs.x + data[7] * rhs.y + data[11] * rhs.z + data[15] * rhs.w
02157 );
02158
02159 }
02160
02165 Vector3<T> operator*(const Vector3<T>& rhs) const
02166 {
02167 return Vector3<T>(
02168 data[0] * rhs.x + data[4] * rhs.y + data[8] * rhs.z,
02169 data[1] * rhs.x + data[5] * rhs.y + data[9] * rhs.z,
02170 data[2] * rhs.x + data[6] * rhs.y + data[10] * rhs.z
02171 );
02172 }
02173
02178 Matrix4<T> operator*(Matrix4<T> rhs) const
02179 {
02180 static Matrix4<T> w;
02181 for (int i = 0; i < 4; i++)
02182 {
02183 for (int j = 0; j < 4; j++)
02184 {
02185 T n = 0;
02186 for (int k = 0; k < 4; k++) n += rhs.at(i, k) * at(k, j);
02187 w.at(i, j) = n;
02188 }
02189 }
02190 return w;
02191
02192 }
02193
02194
02195
02196
02202 T det()
02203 {
02204
02205 return
02206 + at(3,0) * at(2,1) * at(1,2) * at(0,3)
02207 - at(2,0) * at(3,1) * at(1,2) * at(0,3)
02208 - at(3,0) * at(1,1) * at(2,2) * at(0,3)
02209 + at(1,0) * at(3,1) * at(2,2) * at(0,3)
02210
02211 + at(2,0) * at(1,1) * at(3,2) * at(0,3)
02212 - at(1,0) * at(2,1) * at(3,2) * at(0,3)
02213 - at(3,0) * at(2,1) * at(0,2) * at(1,3)
02214 + at(2,0) * at(3,1) * at(0,2) * at(1,3)
02215
02216 + at(3,0) * at(0,1) * at(2,2) * at(1,3)
02217 - at(0,0) * at(3,1) * at(2,2) * at(1,3)
02218 - at(2,0) * at(0,1) * at(3,2) * at(1,3)
02219 + at(0,0) * at(2,1) * at(3,2) * at(1,3)
02220
02221 + at(3,0) * at(1,1) * at(0,2) * at(2,3)
02222 - at(1,0) * at(3,1) * at(0,2) * at(2,3)
02223 - at(3,0) * at(0,1) * at(1,2) * at(2,3)
02224 + at(0,0) * at(3,1) * at(1,2) * at(2,3)
02225
02226 + at(1,0) * at(0,1) * at(3,2) * at(2,3)
02227 - at(0,0) * at(1,1) * at(3,2) * at(2,3)
02228 - at(2,0) * at(1,1) * at(0,2) * at(3,3)
02229 + at(1,0) * at(2,1) * at(0,2) * at(3,3)
02230
02231 + at(2,0) * at(0,1) * at(1,2) * at(3,3)
02232 - at(0,0) * at(2,1) * at(1,2) * at(3,3)
02233 - at(1,0) * at(0,1) * at(2,2) * at(3,3)
02234 + at(0,0) * at(1,1) * at(2,2) * at(3,3);
02235
02236
02237 }
02238
02239
02246 Matrix4<T> inverse()
02247 {
02248 Matrix4<T> ret;
02249
02250 ret.at(0,0) =
02251 + at(2,1) * at(3,2) * at(1,3)
02252 - at(3,1) * at(2,2) * at(1,3)
02253 + at(3,1) * at(1,2) * at(2,3)
02254 - at(1,1) * at(3,2) * at(2,3)
02255 - at(2,1) * at(1,2) * at(3,3)
02256 + at(1,1) * at(2,2) * at(3,3);
02257
02258 ret.at(1,0) =
02259 + at(3,0) * at(2,2) * at(1,3)
02260 - at(2,0) * at(3,2) * at(1,3)
02261 - at(3,0) * at(1,2) * at(2,3)
02262 + at(1,0) * at(3,2) * at(2,3)
02263 + at(2,0) * at(1,2) * at(3,3)
02264 - at(1,0) * at(2,2) * at(3,3);
02265
02266 ret.at(2,0) =
02267 + at(2,0) * at(3,1) * at(1,3)
02268 - at(3,0) * at(2,1) * at(1,3)
02269 + at(3,0) * at(1,1) * at(2,3)
02270 - at(1,0) * at(3,1) * at(2,3)
02271 - at(2,0) * at(1,1) * at(3,3)
02272 + at(1,0) * at(2,1) * at(3,3);
02273
02274 ret.at(3,0) =
02275 + at(3,0) * at(2,1) * at(1,2)
02276 - at(2,0) * at(3,1) * at(1,2)
02277 - at(3,0) * at(1,1) * at(2,2)
02278 + at(1,0) * at(3,1) * at(2,2)
02279 + at(2,0) * at(1,1) * at(3,2)
02280 - at(1,0) * at(2,1) * at(3,2);
02281
02282 ret.at(0,1) =
02283 + at(3,1) * at(2,2) * at(0,3)
02284 - at(2,1) * at(3,2) * at(0,3)
02285 - at(3,1) * at(0,2) * at(2,3)
02286 + at(0,1) * at(3,2) * at(2,3)
02287 + at(2,1) * at(0,2) * at(3,3)
02288 - at(0,1) * at(2,2) * at(3,3);
02289
02290 ret.at(1,1) =
02291 + at(2,0) * at(3,2) * at(0,3)
02292 - at(3,0) * at(2,2) * at(0,3)
02293 + at(3,0) * at(0,2) * at(2,3)
02294 - at(0,0) * at(3,2) * at(2,3)
02295 - at(2,0) * at(0,2) * at(3,3)
02296 + at(0,0) * at(2,2) * at(3,3);
02297
02298 ret.at(2,1) =
02299 + at(3,0) * at(2,1) * at(0,3)
02300 - at(2,0) * at(3,1) * at(0,3)
02301 - at(3,0) * at(0,1) * at(2,3)
02302 + at(0,0) * at(3,1) * at(2,3)
02303 + at(2,0) * at(0,1) * at(3,3)
02304 - at(0,0) * at(2,1) * at(3,3);
02305
02306 ret.at(3,1) =
02307 + at(2,0) * at(3,1) * at(0,2)
02308 - at(3,0) * at(2,1) * at(0,2)
02309 + at(3,0) * at(0,1) * at(2,2)
02310 - at(0,0) * at(3,1) * at(2,2)
02311 - at(2,0) * at(0,1) * at(3,2)
02312 + at(0,0) * at(2,1) * at(3,2);
02313
02314 ret.at(0,2) =
02315 + at(1,1) * at(3,2) * at(0,3)
02316 - at(3,1) * at(1,2) * at(0,3)
02317 + at(3,1) * at(0,2) * at(1,3)
02318 - at(0,1) * at(3,2) * at(1,3)
02319 - at(1,1) * at(0,2) * at(3,3)
02320 + at(0,1) * at(1,2) * at(3,3);
02321
02322 ret.at(1,2) =
02323 + at(3,0) * at(1,2) * at(0,3)
02324 - at(1,0) * at(3,2) * at(0,3)
02325 - at(3,0) * at(0,2) * at(1,3)
02326 + at(0,0) * at(3,2) * at(1,3)
02327 + at(1,0) * at(0,2) * at(3,3)
02328 - at(0,0) * at(1,2) * at(3,3);
02329
02330 ret.at(2,2) =
02331 + at(1,0) * at(3,1) * at(0,3)
02332 - at(3,0) * at(1,1) * at(0,3)
02333 + at(3,0) * at(0,1) * at(1,3)
02334 - at(0,0) * at(3,1) * at(1,3)
02335 - at(1,0) * at(0,1) * at(3,3)
02336 + at(0,0) * at(1,1) * at(3,3);
02337
02338 ret.at(3,2) =
02339 + at(3,0) * at(1,1) * at(0,2)
02340 - at(1,0) * at(3,1) * at(0,2)
02341 - at(3,0) * at(0,1) * at(1,2)
02342 + at(0,0) * at(3,1) * at(1,2)
02343 + at(1,0) * at(0,1) * at(3,2)
02344 - at(0,0) * at(1,1) * at(3,2);
02345
02346 ret.at(0,3) =
02347 + at(2,1) * at(1,2) * at(0,3)
02348 - at(1,1) * at(2,2) * at(0,3)
02349 - at(2,1) * at(0,2) * at(1,3)
02350 + at(0,1) * at(2,2) * at(1,3)
02351 + at(1,1) * at(0,2) * at(2,3)
02352 - at(0,1) * at(1,2) * at(2,3);
02353
02354 ret.at(1,3) =
02355 + at(1,0) * at(2,2) * at(0,3)
02356 - at(2,0) * at(1,2) * at(0,3)
02357 + at(2,0) * at(0,2) * at(1,3)
02358 - at(0,0) * at(2,2) * at(1,3)
02359 - at(1,0) * at(0,2) * at(2,3)
02360 + at(0,0) * at(1,2) * at(2,3);
02361
02362 ret.at(2,3) =
02363 + at(2,0) * at(1,1) * at(0,3)
02364 - at(1,0) * at(2,1) * at(0,3)
02365 - at(2,0) * at(0,1) * at(1,3)
02366 + at(0,0) * at(2,1) * at(1,3)
02367 + at(1,0) * at(0,1) * at(2,3)
02368 - at(0,0) * at(1,1) * at(2,3);
02369
02370 ret.at(3,3) =
02371 + at(1,0) * at(2,1) * at(0,2)
02372 - at(2,0) * at(1,1) * at(0,2)
02373 + at(2,0) * at(0,1) * at(1,2)
02374 - at(0,0) * at(2,1) * at(1,2)
02375 - at(1,0) * at(0,1) * at(2,2)
02376 + at(0,0) * at(1,1) * at(2,2);
02377
02378 return ret * det();
02379 }
02380
02381
02382
02386 Matrix4<T> transpose()
02387 {
02388 Matrix4<T> ret;
02389 for (int i = 0; i < 4; i++)
02390 {
02391 for (int j = 0; j < 4; j++)
02392 {
02393 ret.at(i,j) = at(j,i);
02394 }
02395 }
02396 return ret;
02397 }
02398
02408 Matrix4<T> lerp(T fact, const Matrix4<T>& rhs) const
02409 {
02410 Matrix4<T> ret = (*this) + (rhs - (*this)) * fact;
02411 return ret;
02412 }
02413
02414
02415
02421 operator T*(){ return (T*) data; }
02422
02428 operator const T*() const { return (const T*) data; }
02429
02430
02437 friend std::ostream& operator << (std::ostream& lhs, const Matrix4<T>& rhs)
02438 {
02439 for (int i = 0; i < 4; i++)
02440 {
02441 lhs << "|\t";
02442 for (int j = 0; j < 4; j++)
02443 {
02444 lhs << rhs.at(j,i) << "\t";
02445 }
02446 lhs << "|" << std::endl;
02447 }
02448 return lhs;
02449 }
02450
02451 };
02452
02453
02455 typedef Matrix4<float> Matrix4f;
02457 typedef Matrix4<double> Matrix4d;
02458
02459
02467 template <class T>
02468 class Quaternion
02469 {
02470 public:
02474 T w;
02478 Vector3<T> v;
02479
02483 Quaternion(): w(0), v(0,0,0){}
02484
02488 Quaternion(const Quaternion<T>& q): w(q.w), v(q.v){ }
02489
02493 template <class FromT>
02494 Quaternion(const Quaternion<FromT>& q)
02495 : w(static_cast<T>(q.w)),
02496 v(q.v){ }
02497
02498
02504 Quaternion(T w_, const Vector3<T>& v_): w(w_), v(v_){}
02505
02513 Quaternion(T w_, T x, T y, T z): w(w_), v(x,y,z){}
02514
02519 Quaternion<T>& operator= (const Quaternion<T>& rhs)
02520 {
02521 v = rhs.v;
02522 w = rhs.w;
02523 return *this;
02524 }
02525
02530 template<class FromT>
02531 Quaternion<T>& operator= (const Quaternion<FromT>& rhs)
02532 {
02533 v = rhs.v;
02534 w = static_cast<T>(rhs.w);
02535 return *this;
02536 }
02537
02542 Quaternion<T> operator+ (const Quaternion<T>& rhs) const
02543 {
02544 const Quaternion<T>& lhs = *this;
02545 return Quaternion<T>(lhs.w + rhs.w, lhs.v + rhs.v);
02546 }
02547
02552 Quaternion<T> operator* (const Quaternion<T>& rhs) const
02553 {
02554 const Quaternion<T>& lhs = *this;
02555 return Quaternion<T>(
02556 lhs.w * rhs.w - lhs.v.x * rhs.v.x - lhs.v.y * rhs.v.y - lhs.v.z * rhs.v.z,
02557 lhs.w * rhs.v.x + lhs.v.x * rhs.w + lhs.v.y * rhs.v.z - lhs.v.z * rhs.v.y,
02558 lhs.w * rhs.v.y - lhs.v.x * rhs.v.z + lhs.v.y * rhs.w + lhs.v.z * rhs.v.x,
02559 lhs.w * rhs.v.z + lhs.v.x * rhs.v.y - lhs.v.y * rhs.v.x + lhs.v.z * rhs.w
02560 );
02561 }
02562
02567 Quaternion<T> operator* (T rhs) const
02568 {
02569 return Quaternion<T>(w*rhs, v*rhs);
02570 }
02571
02576 Quaternion<T> operator- (const Quaternion<T>& rhs) const
02577 {
02578 const Quaternion<T>& lhs = *this;
02579 return Quaternion<T>(lhs.w - rhs.w, lhs.v - rhs.v);
02580 }
02581
02586 Quaternion<T>& operator+= (const Quaternion<T>& rhs)
02587 {
02588 w += rhs.w;
02589 v += rhs.v;
02590 return *this;
02591 }
02592
02597 Quaternion<T>& operator-= (const Quaternion<T>& rhs)
02598 {
02599 w -= rhs.w;
02600 v -= rhs.v;
02601 return *this;
02602 }
02603
02608 Quaternion<T>& operator*= (const Quaternion<T>& rhs)
02609 {
02610 Quaternion q = (*this) * rhs;
02611 v = q.v;
02612 w = q.w;
02613 return *this;
02614 }
02615
02620 Quaternion<T>& operator*= (T rhs)
02621 {
02622 w *= rhs;
02623 v *= rhs;
02624 return *this;
02625 }
02626
02634 bool operator==(const Quaternion<T>& rhs) const
02635 {
02636 const Quaternion<T>& lhs = *this;
02637 return (std::fabs(lhs.w - rhs.w) < EPSILON) && lhs.v == rhs.v;
02638 }
02639
02645 bool operator!=(const Quaternion<T>& rhs) const { return ! (*this == rhs); }
02646
02647
02652 Quaternion<T> operator-() const
02653 {
02654 return Quaternion<T>(-w, -v);
02655 }
02656
02661 Quaternion<T> operator~() const
02662 {
02663 return Quaternion<T>(w, -v);
02664 }
02665
02670 T length() const
02671 {
02672 return (T)std::sqrt(w*w + v.lengthSq());
02673 }
02674
02682 T lengthSq() const
02683 {
02684 return w * w + v.lengthSq();
02685 }
02686
02687
02691 void normalize()
02692 {
02693 T len = length();
02694 w /= len;
02695 v /= len;
02696 }
02697
02705 static Quaternion<T> fromEulerAngles(T x, T y, T z)
02706 {
02707 Quaternion<T> ret = fromAxisRot(Vector3<T>(1,0,0), x)
02708 * fromAxisRot(Vector3<T>(0,1,0),y) * fromAxisRot(Vector3<T>(0,0,1),z);
02709 return ret;
02710 }
02711
02717 static Quaternion<T> fromAxisRot(Vector3<T> axis, float angleDeg)
02718 {
02719 double angleRad = DEG2RAD(angleDeg);
02720 double sa2 = std::sin(angleRad/2);
02721 double ca2 = std::cos(angleRad/2);
02722 return Quaternion<T>( ca2, axis * sa2);
02723 }
02724
02729 Matrix3<T> rotMatrix()
02730 {
02731 Matrix3<T> ret;
02732
02733
02734
02735
02736
02737
02738
02739
02740
02741
02742
02743
02744
02745 T xx = v.x * v.x;
02746 T xy = v.x * v.y;
02747 T xz = v.x * v.z;
02748 T xw = v.x * w;
02749
02750 T yy = v.y * v.y;
02751 T yz = v.y * v.z;
02752 T yw = v.y * w;
02753
02754 T zz = v.z * v.z;
02755 T zw = v.z * w;
02756
02757 ret.at(0,0) = 1 - 2 * (yy + zz);
02758 ret.at(1,0) = 2 * (xy - zw);
02759 ret.at(2,0) = 2 * (xz + yw);
02760
02761 ret.at(0,1) = 2 * (xy + zw);
02762 ret.at(1,1) = 1 - 2 * (xx + zz);
02763 ret.at(2,1) = 2 * (yz - xw);
02764
02765 ret.at(0,2) = 2 * (xz - yw);
02766 ret.at(1,2) = 2 * (yz + xw);
02767 ret.at(2,2) = 1 - 2 * (xx + yy);
02768
02769
02770 return ret;
02771 }
02772
02779 Matrix4<T> transform()
02780 {
02781 Matrix4<T> ret;
02782
02783 T xx = v.x * v.x;
02784 T xy = v.x * v.y;
02785 T xz = v.x * v.z;
02786 T xw = v.x * w;
02787
02788 T yy = v.y * v.y;
02789 T yz = v.y * v.z;
02790 T yw = v.y * w;
02791
02792 T zz = v.z * v.z;
02793 T zw = v.z * w;
02794
02795 ret.at(0,0) = 1 - 2 * (yy + zz);
02796 ret.at(1,0) = 2 * (xy - zw);
02797 ret.at(2,0) = 2 * (xz + yw);
02798 ret.at(3,0) = 0;
02799
02800 ret.at(0,1) = 2 * (xy + zw);
02801 ret.at(1,1) = 1 - 2 * (xx + zz);
02802 ret.at(2,1) = 2 * (yz - xw);
02803 ret.at(3,1) = 0;
02804
02805 ret.at(0,2) = 2 * (xz - yw);
02806 ret.at(1,2) = 2 * (yz + xw);
02807 ret.at(2,2) = 1 - 2 * (xx + yy);
02808 ret.at(3,2) = 0;
02809
02810 ret.at(0,3) = 0;
02811 ret.at(1,3) = 0;
02812 ret.at(2,3) = 0;
02813 ret.at(3,3) = 1;
02814
02815 return ret;
02816
02817 }
02818
02828 Quaternion<T> lerp(T fact, const Quaternion<T>& rhs) const
02829 {
02830 return Quaternion<T>((1-fact) * w + fact * rhs.w, v.lerp(fact, rhs.v));
02831 }
02832
02836 friend std::ostream& operator << (std::ostream& oss, const Quaternion<T>& q)
02837 {
02838 oss << "Re: " << q.w << " Im: " << q.v;
02839 return oss;
02840 }
02841
02848 static Quaternion<T> fromMatrix(const Matrix4<T>& m)
02849 {
02850 Quaternion<T> q;
02851
02852 T tr,s;
02853 tr = m.at(0,0) + m.at(1,1) + m.at(2,2);
02854 if (tr >= epsilon)
02855 {
02856 s = (T)sqrt(tr + 1);
02857 q.w = 0.5 * s;
02858 s = 0.5 / s;
02859
02860 q.v.x = (m.at(1,2) - m.at(2,1)) * s;
02861 q.v.y = (m.at(2,0) - m.at(0,2)) * s;
02862 q.v.z = (m.at(0,1) - m.at(1,0)) * s;
02863 }
02864 else
02865 {
02866 T d0 = m.at(0,0);
02867 T d1 = m.at(1,1);
02868 T d2 = m.at(2,2);
02869
02870 char bigIdx = (d0 > d1) ? ((d0 > d2)? 0 : 2) : ((d1 > d2) ? 1 : 2);
02871
02872 if (bigIdx == 0)
02873 {
02874 s = (T)sqrt((d0 - (d1 + d2)) + 1);
02875
02876 q.v.x = 0.5 * s;
02877 s = 0.5 / s;
02878 q.v.y = (m.at(1,0) + m.at(0,1)) * s;
02879 q.v.z = (m.at(2,0) + m.at(0,2)) * s;
02880 q.w = (m.at(1,2) - m.at(2,1)) * s;
02881 }
02882 else if (bigIdx == 1)
02883 {
02884 s = (T)sqrt(1 + d1 - (d0 + d2));
02885 q.v.y = 0.5 * s;
02886 s = 0.5 / s;
02887 q.v.z = (m.at(2,1) + m.at(1,2)) / s;
02888 q.w = (m.at(2,0) - m.at(0,2)) / s;
02889 q.v.x = (m.at(1,0) + m.at(0,1)) / s;
02890 }
02891 else
02892 {
02893 s = (T)sqrt(1 + d2 - (d0 + d1));
02894 q.v.z = 0.5 * s;
02895 s = 0.5 / s;
02896 q.w = (m.at(0,1) - m.at(1,0)) / s;
02897 q.v.x = (m.at(2,0) + m.at(0,2)) / s;
02898 q.v.y = (m.at(2,1) + m.at(1,2)) / s;
02899 }
02900 }
02901
02902 return q;
02903 }
02904
02911 static Quaternion<T> fromMatrix(const Matrix3<T>& m)
02912 {
02913 Quaternion<T> q;
02914
02915 T tr,s;
02916 tr = m.at(0,0) + m.at(1,1) + m.at(2,2);
02917
02918 if (tr >= epsilon)
02919 {
02920 s = (T)sqrt(tr + 1);
02921 q.w = 0.5 * s;
02922 s = 0.5 / s;
02923
02924 q.v.x = (m.at(1,2) - m.at(2,1)) * s;
02925 q.v.y = (m.at(2,0) - m.at(0,2)) * s;
02926 q.v.z = (m.at(0,1) - m.at(1,0)) * s;
02927 }
02928 else
02929 {
02930 T d0 = m.at(0,0);
02931 T d1 = m.at(1,1);
02932 T d2 = m.at(2,2);
02933
02934
02935 char bigIdx = (d0 > d1) ? ((d0 > d2)? 0 : 2) : ((d1 > d2) ? 1 : 2);
02936
02937 if (bigIdx == 0)
02938 {
02939 s = (T)sqrt((d0 - (d1 + d2)) + 1);
02940
02941 q.v.x = 0.5 * s;
02942 s = 0.5 / s;
02943 q.v.y = (m.at(1,0) + m.at(0,1)) * s;
02944 q.v.z = (m.at(2,0) + m.at(0,2)) * s;
02945 q.w = (m.at(1,2) - m.at(2,1)) * s;
02946 }
02947 else if (bigIdx == 1)
02948 {
02949 s = (T)sqrt(1 + d1 - (d0 + d2));
02950 q.v.y = 0.5 * s;
02951 s = 0.5 / s;
02952 q.v.z = (m.at(2,1) + m.at(1,2)) / s;
02953 q.w = (m.at(2,0) - m.at(0,2)) / s;
02954 q.v.x = (m.at(1,0) + m.at(0,1)) / s;
02955 }
02956 else
02957 {
02958 s = (T)sqrt(1 + d2 - (d0 + d1));
02959 q.v.z = 0.5 * s;
02960 s = 0.5 / s;
02961 q.w = (m.at(0,1) - m.at(1,0)) / s;
02962 q.v.x = (m.at(2,0) + m.at(0,2)) / s;
02963 q.v.y = (m.at(2,1) + m.at(1,2)) / s;
02964 }
02965 }
02966
02967 return q;
02968 }
02969
02978 Quaternion<T> slerp(T r, const Quaternion<T>& q2) const
02979 {
02980 Quaternion<T> ret;
02981 T cosTheta = w * q2.w + v.x * q2.v.x + v.y *q2.v.y + v.z * q2.v.z;
02982 T theta = (T) acos(cosTheta);
02983 if (fabs(theta) < epsilon)
02984 {
02985 ret = *this;
02986 }
02987 else
02988 {
02989 T sinTheta = (T)sqrt(1.0 - cosTheta * cosTheta);
02990 if (fabs(sinTheta) < epsilon)
02991 {
02992 ret.w = 0.5 * w + 0.5 * q2.w;
02993 ret.v = v.lerp(0.5, q2.v);
02994 }
02995 else
02996 {
02997 T rA = (T)sin((1.0 - r) * theta) / sinTheta;
02998 T rB = (T)sin(r * theta) / sinTheta;
02999
03000 ret.w = w * rA + q2.w * rB;
03001 ret.v.x = v.x * rA + q2.v.x * rB;
03002 ret.v.y = v.y * rA + q2.v.y * rB;
03003 ret.v.z = v.z * rA + q2.v.z * rB;
03004 }
03005 }
03006 return ret;
03007 }
03008
03009
03010
03011 };
03012
03013 typedef Quaternion<float> Quatf;
03014 typedef Quaternion<double> Quatd;
03015
03016
03017
03018 #ifdef VMATH_NAMESPACE
03019 }
03020 #endif
03021
03022 #endif // __vmath_Header_File__
03023
03024