00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #ifndef _VECMATH_H_
00025 #define _VECMATH_H_
00026
00027 #include <cmath>
00028 #include <QString>
00029
00030 template<class T> class Vector2;
00031 template<class T> class Vector3;
00032 template<class T> class Vector4;
00033 template<class T> class Matrix4;
00034
00035 typedef Vector2<double> Vec2d;
00036 typedef Vector2<float> Vec2f;
00037 typedef Vector2<int> Vec2i;
00038
00041 typedef Vector3<double> Vec3d;
00042
00045 typedef Vector3<float> Vec3f;
00046
00049 typedef Vector4<double> Vec4d;
00050
00053 typedef Vector4<float> Vec4f;
00054
00057 typedef Vector4<int> Vec4i;
00058
00061 typedef Matrix4<double> Mat4d;
00062
00065 typedef Matrix4<float> Mat4f;
00066
00067
00071 template<class T> class Vector2
00072 {
00073 public:
00074 inline Vector2();
00075 inline Vector2(const Vector2<T>&);
00076 inline Vector2(T, T);
00077
00078 inline Vector2& operator=(const Vector2<T>&);
00079 inline Vector2& operator=(const T*);
00080 inline void set(T, T);
00081
00082 inline bool operator==(const Vector2<T>&) const;
00083 inline bool operator!=(const Vector2<T>&) const;
00084
00085 inline const T& operator[](int x) const;
00086 inline T& operator[](int);
00087 inline operator const T*() const;
00088 inline operator T*();
00089
00090 inline Vector2& operator+=(const Vector2<T>&);
00091 inline Vector2& operator-=(const Vector2<T>&);
00092 inline Vector2& operator*=(T);
00093 inline Vector2& operator/=(T);
00094
00095 inline Vector2 operator-(const Vector2<T>&) const;
00096 inline Vector2 operator+(const Vector2<T>&) const;
00097
00098 inline Vector2 operator-() const;
00099 inline Vector2 operator+() const;
00100
00101 inline Vector2 operator*(T) const;
00102 inline Vector2 operator/(T) const;
00103
00104
00105 inline T dot(const Vector2<T>&) const;
00106
00107 inline T length() const;
00108 inline T lengthSquared() const;
00109 inline void normalize();
00110
00111 T v[2];
00112 };
00113
00117 template<class T> class Vector3
00118 {
00119 public:
00120 inline Vector3();
00121 inline Vector3(const Vector3&);
00122 template <class T2> inline Vector3(const Vector3<T2>&);
00123 inline Vector3(T, T, T);
00124 inline Vector3(T);
00125
00126 inline Vector3& operator=(const Vector3&);
00127 inline Vector3& operator=(const T*);
00128 template <class T2> inline Vector3& operator=(const Vector3<T2>&);
00129 inline void set(T, T, T);
00130
00131 inline bool operator==(const Vector3<T>&) const;
00132 inline bool operator!=(const Vector3<T>&) const;
00133
00134 inline T& operator[](int);
00135 inline const T& operator[](int) const;
00136 inline operator const T*() const;
00137 inline operator T*();
00138
00139 inline Vector3& operator+=(const Vector3<T>&);
00140 inline Vector3& operator-=(const Vector3<T>&);
00141 inline Vector3& operator*=(T);
00142 inline Vector3& operator/=(T);
00143
00144 inline Vector3 operator-(const Vector3<T>&) const;
00145 inline Vector3 operator+(const Vector3<T>&) const;
00146
00147 inline Vector3 operator-() const;
00148 inline Vector3 operator+() const;
00149
00150 inline Vector3 operator*(T) const;
00151 inline Vector3 operator/(T) const;
00152
00153
00154 inline T dot(const Vector3<T>&) const;
00155 inline Vector3 operator^(const Vector3<T>&) const;
00156
00157
00158 inline T latitude() const;
00159
00160 inline T longitude() const;
00161
00162
00163 inline T angle(const Vector3<T>&) const;
00164
00165 inline T length() const;
00166 inline T lengthSquared() const;
00167 inline void normalize();
00168
00169 inline void transfo4d(const Mat4d&);
00170 inline void transfo4d(const Mat4f&);
00171 T v[3];
00172
00173 QString toString() const {return QString("[%1, %2, %3]").arg(v[0]).arg(v[1]).arg(v[2]);}
00174 };
00175
00176
00180 template<class T> class Vector4
00181 {
00182 public:
00183 inline Vector4();
00184 inline Vector4(const Vector4<T>&);
00185 inline Vector4(const Vector3<T>&);
00186 inline Vector4(T, T, T);
00187 inline Vector4(T, T, T, T);
00188
00189 inline Vector4& operator=(const Vector4<T>&);
00190 inline Vector4& operator=(const Vector3<T>&);
00191 inline Vector4& operator=(const T*);
00192 inline void set(T, T, T, T);
00193
00194 inline bool operator==(const Vector4<T>&) const;
00195 inline bool operator!=(const Vector4<T>&) const;
00196
00197 inline T& operator[](int);
00198 inline const T& operator[](int) const;
00199 inline operator T*();
00200 inline operator const T*() const;
00201
00202 inline Vector4& operator+=(const Vector4<T>&);
00203 inline Vector4& operator-=(const Vector4<T>&);
00204 inline Vector4& operator*=(T);
00205 inline Vector4& operator/=(T);
00206
00207 inline Vector4 operator-(const Vector4<T>&) const;
00208 inline Vector4 operator+(const Vector4<T>&) const;
00209
00210 inline Vector4 operator-() const;
00211 inline Vector4 operator+() const;
00212
00213 inline Vector4 operator*(T) const;
00214 inline Vector4 operator/(T) const;
00215
00216
00217 inline T dot(const Vector4<T>&) const;
00218
00219 inline T length() const;
00220 inline T lengthSquared() const;
00221 inline void normalize();
00222
00223 inline void transfo4d(const Mat4d&);
00224
00225 T v[4];
00226 };
00227
00231 template<class T> class Matrix4
00232 {
00233 public:
00234 Matrix4();
00235 Matrix4(const Matrix4<T>& m);
00236 Matrix4(T,T,T,T,T,T,T,T,T,T,T,T,T,T,T,T);
00237 Matrix4(const T*);
00238 Matrix4(const Vector4<T>&, const Vector4<T>&,
00239 const Vector4<T>&, const Vector4<T>&);
00240
00241 inline Matrix4& operator=(const Matrix4<T>&);
00242 inline Matrix4& operator=(const T*);
00243 inline void set(T,T,T,T,T,T,T,T,T,T,T,T,T,T,T,T);
00244
00245 inline T* operator[](int);
00246 inline operator T*();
00247 inline operator const T*() const;
00248
00249 inline Matrix4 operator-(const Matrix4<T>&) const;
00250 inline Matrix4 operator+(const Matrix4<T>&) const;
00251 inline Matrix4 operator*(const Matrix4<T>&) const;
00252
00253 inline Vector3<T> operator*(const Vector3<T>&) const;
00254 inline Vector3<T> multiplyWithoutTranslation(const Vector3<T>& a) const;
00255 inline Vector4<T> operator*(const Vector4<T>&) const;
00256
00257 inline void transfo(Vector3<T>&) const;
00258
00259 static Matrix4<T> identity();
00260 static Matrix4<T> translation(const Vector3<T>&);
00261
00262
00263 static Matrix4<T> rotation(const Vector3<T>&, T);
00264 static Matrix4<T> xrotation(T);
00265 static Matrix4<T> yrotation(T);
00266 static Matrix4<T> zrotation(T);
00267 static Matrix4<T> scaling(const Vector3<T>&);
00268 static Matrix4<T> scaling(T);
00269
00270 Matrix4<T> transpose() const;
00271 Matrix4<T> inverse() const;
00272
00273 inline void print(void) const;
00274
00275 T r[16];
00276 };
00277
00278
00279
00281
00282 template<class T> Vector2<T>::Vector2() {}
00283
00284 template<class T> Vector2<T>::Vector2(const Vector2<T>& a)
00285 {
00286 v[0]=a.v[0]; v[1]=a.v[1];
00287 }
00288
00289 template<class T> Vector2<T>::Vector2(T x, T y)
00290 {
00291 v[0]=x; v[1]=y;
00292 }
00293
00294 template<class T> Vector2<T>& Vector2<T>::operator=(const Vector2<T>& a)
00295 {
00296 v[0]=a.v[0]; v[1]=a.v[1];
00297 return *this;
00298 }
00299
00300 template<class T> Vector2<T>& Vector2<T>::operator=(const T* a)
00301 {
00302 v[0]=a[0]; v[1]=a[1];
00303 return *this;
00304 }
00305
00306 template<class T> void Vector2<T>::set(T x, T y)
00307 {
00308 v[0]=x; v[1]=y;
00309 }
00310
00311
00312 template<class T> bool Vector2<T>::operator==(const Vector2<T>& a) const
00313 {
00314 return (v[0] == a.v[0] && v[1] == a.v[1]);
00315 }
00316
00317 template<class T> bool Vector2<T>::operator!=(const Vector2<T>& a) const
00318 {
00319 return (v[0] != a.v[0] || v[1] != a.v[1]);
00320 }
00321
00322 template<class T> const T& Vector2<T>::operator[](int x) const
00323 {
00324 return v[x];
00325 }
00326
00327 template<class T> T& Vector2<T>::operator[](int x)
00328 {
00329 return v[x];
00330 }
00331
00332 template<class T> Vector2<T>::operator const T*() const
00333 {
00334 return v;
00335 }
00336
00337 template<class T> Vector2<T>::operator T*()
00338 {
00339 return v;
00340 }
00341
00342
00343 template<class T> Vector2<T>& Vector2<T>::operator+=(const Vector2<T>& a)
00344 {
00345 v[0] += a.v[0]; v[1] += a.v[1];
00346 return *this;
00347 }
00348
00349 template<class T> Vector2<T>& Vector2<T>::operator-=(const Vector2<T>& a)
00350 {
00351 v[0] -= a.v[0]; v[1] -= a.v[1];
00352 return *this;
00353 }
00354
00355 template<class T> Vector2<T>& Vector2<T>::operator*=(T s)
00356 {
00357 v[0] *= s; v[1] *= s;
00358 return *this;
00359 }
00360
00361 template<class T> Vector2<T> Vector2<T>::operator-() const
00362 {
00363 return Vector2<T>(-v[0], -v[1]);
00364 }
00365
00366 template<class T> Vector2<T> Vector2<T>::operator+() const
00367 {
00368 return *this;
00369 }
00370
00371 template<class T> Vector2<T> Vector2<T>::operator+(const Vector2<T>& b) const
00372 {
00373 return Vector2<T>(v[0] + b.v[0], v[1] + b.v[1]);
00374 }
00375
00376 template<class T> Vector2<T> Vector2<T>::operator-(const Vector2<T>& b) const
00377 {
00378 return Vector2<T>(v[0] - b.v[0], v[1] - b.v[1]);
00379 }
00380
00381 template<class T> Vector2<T> Vector2<T>::operator*(T s) const
00382 {
00383 return Vector2<T>(s * v[0], s * v[1]);
00384 }
00385
00386 template<class T> Vector2<T> Vector2<T>::operator/(T s) const
00387 {
00388 return Vector2<T>(v[0]/s, v[1]/s);
00389 }
00390
00391
00392 template<class T> T Vector2<T>::dot(const Vector2<T>& b) const
00393 {
00394 return v[0] * b.v[0] + v[1] * b.v[1];
00395 }
00396
00397
00398 template<class T> T Vector2<T>::length() const
00399 {
00400 return (T) std::sqrt(v[0] * v[0] + v[1] * v[1]);
00401 }
00402
00403 template<class T> T Vector2<T>::lengthSquared() const
00404 {
00405 return v[0] * v[0] + v[1] * v[1];
00406 }
00407
00408 template<class T> void Vector2<T>::normalize()
00409 {
00410 T s = (T) 1 / std::sqrt(v[0] * v[0] + v[1] * v[1]);
00411 v[0] *= s;
00412 v[1] *= s;
00413 }
00414
00415
00416
00417
00418
00419
00421
00422 template<class T> Vector3<T>::Vector3() {}
00423
00424 template<class T> Vector3<T>::Vector3(const Vector3& a)
00425 {
00426 v[0]=a.v[0]; v[1]=a.v[1]; v[2]=a.v[2];
00427 }
00428
00429 template<class T> template<class T2> Vector3<T>::Vector3(const Vector3<T2>& a)
00430 {
00431 v[0]=a.v[0]; v[1]=a.v[1]; v[2]=a.v[2];
00432 }
00433
00434 template<class T> Vector3<T>::Vector3(T x)
00435 {
00436 v[0]=x; v[1]=x; v[2]=x;
00437 }
00438
00439 template<class T> Vector3<T>::Vector3(T x, T y, T z)
00440 {
00441 v[0]=x; v[1]=y; v[2]=z;
00442 }
00443
00444 template<class T> Vector3<T>& Vector3<T>::operator=(const Vector3& a)
00445 {
00446 v[0]=a.v[0]; v[1]=a.v[1]; v[2]=a.v[2];
00447 return *this;
00448 }
00449
00450 template<class T> template <class T2> Vector3<T>& Vector3<T>::operator=(const Vector3<T2>& a)
00451 {
00452 v[0]=a.v[0]; v[1]=a.v[1]; v[2]=a.v[2];
00453 return *this;
00454 }
00455
00456 template<class T> Vector3<T>& Vector3<T>::operator=(const T* a)
00457 {
00458 v[0]=a[0]; v[1]=a[1]; v[2]=a[2];
00459 return *this;
00460 }
00461
00462 template<class T> void Vector3<T>::set(T x, T y, T z)
00463 {
00464 v[0]=x; v[1]=y; v[2]=z;
00465 }
00466
00467
00468 template<class T> bool Vector3<T>::operator==(const Vector3<T>& a) const
00469 {
00470 return (v[0] == a.v[0] && v[1] == a.v[1] && v[2] == a.v[2]);
00471 }
00472
00473 template<class T> bool Vector3<T>::operator!=(const Vector3<T>& a) const
00474 {
00475 return (v[0] != a.v[0] || v[1] != a.v[1] || v[2] != a.v[2]);
00476 }
00477
00478
00479 template<class T> T& Vector3<T>::operator[](int x)
00480 {
00481 return v[x];
00482 }
00483
00484 template<class T> const T& Vector3<T>::operator[](int x) const
00485 {
00486 return v[x];
00487 }
00488
00489 template<class T> Vector3<T>::operator const T*() const
00490 {
00491 return v;
00492 }
00493
00494 template<class T> Vector3<T>::operator T*()
00495 {
00496 return v;
00497 }
00498
00499 template<class T> Vector3<T>& Vector3<T>::operator+=(const Vector3<T>& a)
00500 {
00501 v[0] += a.v[0]; v[1] += a.v[1]; v[2] += a.v[2];
00502 return *this;
00503 }
00504
00505 template<class T> Vector3<T>& Vector3<T>::operator-=(const Vector3<T>& a)
00506 {
00507 v[0] -= a.v[0]; v[1] -= a.v[1]; v[2] -= a.v[2];
00508 return *this;
00509 }
00510
00511 template<class T> Vector3<T>& Vector3<T>::operator*=(T s)
00512 {
00513 v[0] *= s; v[1] *= s; v[2] *= s;
00514 return *this;
00515 }
00516
00517 template<class T> Vector3<T> Vector3<T>::operator-() const
00518 {
00519 return Vector3<T>(-v[0], -v[1], -v[2]);
00520 }
00521
00522 template<class T> Vector3<T> Vector3<T>::operator+() const
00523 {
00524 return *this;
00525 }
00526
00527 template<class T> Vector3<T> Vector3<T>::operator+(const Vector3<T>& b) const
00528 {
00529 return Vector3<T>(v[0] + b.v[0], v[1] + b.v[1], v[2] + b.v[2]);
00530 }
00531
00532 template<class T> Vector3<T> Vector3<T>::operator-(const Vector3<T>& b) const
00533 {
00534 return Vector3<T>(v[0] - b.v[0], v[1] - b.v[1], v[2] - b.v[2]);
00535 }
00536
00537 template<class T> Vector3<T> Vector3<T>::operator*(T s) const
00538 {
00539 return Vector3<T>(s * v[0], s * v[1], s * v[2]);
00540 }
00541
00542 template<class T> Vector3<T> Vector3<T>::operator/(T s) const
00543 {
00544 return Vector3<T>(v[0]/s, v[1]/s, v[2]/s);
00545 }
00546
00547
00548 template<class T> T Vector3<T>::dot(const Vector3<T>& b) const
00549 {
00550 return v[0] * b.v[0] + v[1] * b.v[1] + v[2] * b.v[2];
00551 }
00552
00553
00554
00555 template<class T> Vector3<T> Vector3<T>::operator^(const Vector3<T>& b) const
00556 {
00557 return Vector3<T>(v[1] * b.v[2] - v[2] * b.v[1],
00558 v[2] * b.v[0] - v[0] * b.v[2],
00559 v[0] * b.v[1] - v[1] * b.v[0]);
00560 }
00561
00562
00563 template<class T> T Vector3<T>::angle(const Vector3<T>& b) const
00564 {
00565 return std::acos(dot(b)/sqrt(lengthSquared()*b.lengthSquared()));
00566 }
00567
00568 template<class T> T Vector3<T>::length() const
00569 {
00570 return (T) sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
00571 }
00572
00573 template<class T> T Vector3<T>::lengthSquared() const
00574 {
00575 return v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
00576 }
00577
00578 template<class T> void Vector3<T>::normalize()
00579 {
00580 T s = (T) (1. / std::sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]));
00581 v[0] *= s;
00582 v[1] *= s;
00583 v[2] *= s;
00584 }
00585
00586 template<class T> void Vector3<T>::transfo4d(const Mat4d& m)
00587 {
00588 (*this)=m*(*this);
00589 }
00590
00591 template<class T> void Vector3<T>::transfo4d(const Mat4f& m)
00592 {
00593 (*this)=m*(*this);
00594 }
00595
00596
00597 template<class T> T Vector3<T>::latitude() const
00598 {
00599 return std::asin(v[2]/length());
00600 }
00601
00602
00603 template<class T> T Vector3<T>::longitude() const
00604 {
00605 return std::atan2(v[1],v[0]);
00606 }
00607
00608
00609
00610
00611
00612
00613
00614
00615
00616
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626
00628
00629 template<class T> Vector4<T>::Vector4() {}
00630
00631 template<class T> Vector4<T>::Vector4(const Vector4<T>& a)
00632 {
00633 v[0]=a.v[0]; v[1]=a.v[1]; v[2]=a.v[2]; v[3]=a.v[3];
00634 }
00635
00636 template<class T> Vector4<T>::Vector4(const Vector3<T>& a)
00637 {
00638 v[0]=a.v[0]; v[1]=a.v[1]; v[2]=a.v[2]; v[3]=1;
00639 }
00640
00641 template<class T> Vector4<T>::Vector4(T x, T y, T z)
00642 {
00643 v[0]=x; v[1]=y; v[2]=z; v[3]=1;
00644 }
00645
00646 template<class T> Vector4<T>::Vector4(T x, T y, T z, T a = 1)
00647 {
00648 v[0]=x; v[1]=y; v[2]=z; v[3]=a;
00649 }
00650
00651 template<class T> Vector4<T>& Vector4<T>::operator=(const Vector4<T>& a)
00652 {
00653 v[0]=a.v[0]; v[1]=a.v[1]; v[2]=a.v[2]; v[3]=a.v[3];
00654 return *this;
00655 }
00656
00657 template<class T> Vector4<T>& Vector4<T>::operator=(const Vector3<T>& a)
00658 {
00659 v[0]=a.v[0]; v[1]=a.v[1]; v[2]=a.v[2]; v[3]=1;
00660 return *this;
00661 }
00662
00663 template<class T> Vector4<T>& Vector4<T>::operator=(const T* a)
00664 {
00665 v[0]=a[0]; v[1]=a[1]; v[2]=a[2]; v[3]=a[3];
00666 return *this;
00667 }
00668
00669 template<class T> void Vector4<T>::set(T x, T y, T z, T a)
00670 {
00671 v[0]=x; v[1]=y; v[2]=z; v[3]=a;
00672 }
00673
00674 template<class T> bool Vector4<T>::operator==(const Vector4<T>& a) const
00675 {
00676 return (v[0] == a.v[0] && v[1] == a.v[1] && v[2] == a.v[2] && v[3] == a.v[3]);
00677 }
00678
00679 template<class T> bool Vector4<T>::operator!=(const Vector4<T>& a) const
00680 {
00681 return (v[0] != a.v[0] || v[1] != a.v[1] || v[2] != a.v[2] || v[3] != a.v[3]);
00682 }
00683
00684 template<class T> T& Vector4<T>::operator[](int x)
00685 {
00686 return v[x];
00687 }
00688
00689 template<class T> const T& Vector4<T>::operator[](int x) const
00690 {
00691 return v[x];
00692 }
00693
00694 template<class T> Vector4<T>::operator T*()
00695 {
00696 return v;
00697 }
00698
00699 template<class T> Vector4<T>::operator const T*() const
00700 {
00701 return v;
00702 }
00703
00704 template<class T> Vector4<T>& Vector4<T>::operator+=(const Vector4<T>& a)
00705 {
00706 v[0] += a.v[0]; v[1] += a.v[1]; v[2] += a.v[2]; v[3] += a.v[3];
00707 return *this;
00708 }
00709
00710 template<class T> Vector4<T>& Vector4<T>::operator-=(const Vector4<T>& a)
00711 {
00712 v[0] -= a.v[0]; v[1] -= a.v[1]; v[2] -= a.v[2]; v[3] -= a/v[3];
00713 return *this;
00714 }
00715
00716 template<class T> Vector4<T>& Vector4<T>::operator*=(T s)
00717 {
00718 v[0] *= s; v[1] *= s; v[2] *= s; v[3] *= s;
00719 return *this;
00720 }
00721
00722 template<class T> Vector4<T> Vector4<T>::operator-() const
00723 {
00724 return Vector4<T>(-v[0], -v[1], -v[2], -v[3]);
00725 }
00726
00727 template<class T> Vector4<T> Vector4<T>::operator+() const
00728 {
00729 return *this;
00730 }
00731
00732 template<class T> Vector4<T> Vector4<T>::operator+(const Vector4<T>& b) const
00733 {
00734 return Vector4<T>(v[0] + b.v[0], v[1] + b.v[1], v[2] + b.v[2], v[3] + b.v[3]);
00735 }
00736
00737 template<class T> Vector4<T> Vector4<T>::operator-(const Vector4<T>& b) const
00738 {
00739 return Vector4<T>(v[0] - b.v[0], v[1] - b.v[1], v[2] - b.v[2], v[3] - b.v[3]);
00740 }
00741
00742 template<class T> Vector4<T> Vector4<T>::operator*(T s) const
00743 {
00744 return Vector4<T>(s * v[0], s * v[1], s * v[2], s * v[3]);
00745 }
00746
00747 template<class T> Vector4<T> Vector4<T>::operator/(T s) const
00748 {
00749 return Vector4<T>(v[0]/s, v[1]/s, v[2]/s, v[3]/s);
00750 }
00751
00752 template<class T> T Vector4<T>::dot(const Vector4<T>& b) const
00753 {
00754 return v[0] * b.v[0] + v[1] * b.v[1] + v[2] * b.v[2] + v[3] * b.v[3];
00755 }
00756
00757 template<class T> T Vector4<T>::length() const
00758 {
00759 return (T) sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3]);
00760 }
00761
00762 template<class T> T Vector4<T>::lengthSquared() const
00763 {
00764 return v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3];
00765 }
00766
00767 template<class T> void Vector4<T>::normalize()
00768 {
00769 T s = (T) (1. / sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3]));
00770 v[0] *= s;
00771 v[1] *= s;
00772 v[2] *= s;
00773 v[3] *= s;
00774 }
00775
00776 template<class T> void Vector4<T>::transfo4d(const Mat4d& m)
00777 {
00778 (*this)=m*(*this);
00779 }
00780
00781
00782
00783
00784
00785
00786
00788
00789 template<class T> Matrix4<T>::Matrix4() {}
00790
00791 template<class T> Matrix4<T>::Matrix4(const Matrix4<T>& m)
00792 {
00793 memcpy(r,m.r,sizeof(T)*16);
00794 }
00795
00796 template<class T> Matrix4<T>::Matrix4(const T* m)
00797 {
00798 memcpy(r,m,sizeof(T)*16);
00799 }
00800
00801 template<class T> Matrix4<T>& Matrix4<T>::operator=(const Matrix4<T>& m)
00802 {
00803 memcpy(r,m.r,sizeof(T)*16);
00804 return (*this);
00805 }
00806
00807 template<class T> Matrix4<T>::Matrix4(const Vector4<T>& v0,
00808 const Vector4<T>& v1,
00809 const Vector4<T>& v2,
00810 const Vector4<T>& v3)
00811 {
00812 r[0] = v0.v[0];
00813 r[1] = v0.v[1];
00814 r[2] = v0.v[2];
00815 r[3] = v0.v[3];
00816 r[4] = v1.v[0];
00817 r[5] = v1.v[1];
00818 r[6] = v1.v[2];
00819 r[7] = v1.v[3];
00820 r[8] = v2.v[0];
00821 r[9] = v2.v[1];
00822 r[10] = v2.v[2];
00823 r[11] = v2.v[3];
00824 r[12] = v3.v[0];
00825 r[13] = v3.v[1];
00826 r[14] = v3.v[2];
00827 r[15] = v3.v[3];
00828 }
00829
00830
00831 template<class T> Matrix4<T>::Matrix4(T a, T b, T c, T d, T e, T f, T g, T h, T i, T j, T k, T l, T m, T n, T o, T p)
00832 {
00833 r[0]=a; r[1]=b; r[2]=c; r[3]=d; r[4]=e; r[5]=f; r[6]=g; r[7]=h;
00834 r[8]=i; r[9]=j; r[10]=k; r[11]=l; r[12]=m; r[13]=n; r[14]=o; r[15]=p;
00835 }
00836
00837 template<class T> void Matrix4<T>::set(T a, T b, T c, T d, T e, T f, T g, T h, T i, T j, T k, T l, T m, T n, T o, T p)
00838 {
00839 r[0]=a; r[1]=b; r[2]=c; r[3]=d; r[4]=e; r[5]=f; r[6]=g; r[7]=h;
00840 r[8]=i; r[9]=j; r[10]=k; r[11]=l; r[12]=m; r[13]=n; r[14]=o; r[15]=p;
00841 }
00842
00843 template<class T> T* Matrix4<T>::operator[](int n)
00844 {
00845 return &(r[n*4]);
00846 }
00847
00848 template<class T> Matrix4<T>::operator T*()
00849 {
00850 return r;
00851 }
00852
00853 template<class T> Matrix4<T>::operator const T*() const
00854 {
00855 return r;
00856 }
00857
00858 template<class T> Matrix4<T> Matrix4<T>::identity()
00859 {
00860 return Matrix4<T>( 1, 0, 0, 0,
00861 0, 1, 0, 0,
00862 0, 0, 1, 0,
00863 0, 0, 0, 1 );
00864 }
00865
00866
00867 template<class T> Matrix4<T> Matrix4<T>::translation(const Vector3<T>& a)
00868 {
00869 return Matrix4<T>( 1, 0, 0, 0,
00870 0, 1, 0, 0,
00871 0, 0, 1, 0,
00872 a.v[0], a.v[1], a.v[2], 1);
00873 }
00874
00875 template<class T> Matrix4<T> Matrix4<T>::rotation(const Vector3<T>& a,
00876 T angle)
00877 {
00878 Vec3d axis(a);
00879 axis.normalize();
00880
00881
00882
00883
00884
00885
00886
00887
00888
00889
00890
00891
00892
00893
00894
00895
00896
00897
00898 T sin_a = std::sin( angle / 2 );
00899 T cos_a = std::cos( angle / 2 );
00900 T X = axis[0] * sin_a;
00901 T Y = axis[1] * sin_a;
00902 T Z = axis[2] * sin_a;
00903 T W = cos_a;
00904
00905 T xx = X * X;
00906 T xy = X * Y;
00907 T xz = X * Z;
00908 T xw = X * W;
00909
00910 T yy = Y * Y;
00911 T yz = Y * Z;
00912 T yw = Y * W;
00913
00914 T zz = Z * Z;
00915 T zw = Z * W;
00916
00917 return Matrix4<T>(
00918 1. - 2. * ( yy + zz ), 2. * ( xy + zw ), 2. * ( xz - yw ), 0.,
00919 2. * ( xy - zw ), 1. - 2. * ( xx + zz ), 2. * ( yz + xw ), 0.,
00920 2. * ( xz + yw ), 2. * ( yz - xw ), 1. - 2. * ( xx + yy ), 0.,
00921 0., 0., 0., 1.);
00922 }
00923
00924
00925
00926
00927
00928
00929
00930
00931
00932
00933
00934
00935
00936
00937
00938 template<class T> Matrix4<T> Matrix4<T>::xrotation(T angle)
00939 {
00940 T c = (T) cos(angle);
00941 T s = (T) sin(angle);
00942
00943 return Matrix4<T>(1, 0, 0, 0,
00944 0, c, s, 0,
00945 0,-s, c, 0,
00946 0, 0, 0, 1 );
00947 }
00948
00949
00950 template<class T> Matrix4<T> Matrix4<T>::yrotation(T angle)
00951 {
00952 T c = (T) cos(angle);
00953 T s = (T) sin(angle);
00954
00955 return Matrix4<T>( c, 0,-s, 0,
00956 0, 1, 0, 0,
00957 s, 0, c, 0,
00958 0, 0, 0, 1 );
00959 }
00960
00961
00962 template<class T> Matrix4<T> Matrix4<T>::zrotation(T angle)
00963 {
00964 T c = (T) cos(angle);
00965 T s = (T) sin(angle);
00966
00967 return Matrix4<T>(c, s, 0, 0,
00968 -s, c, 0, 0,
00969 0, 0, 1, 0,
00970 0, 0, 0, 1 );
00971 }
00972
00973
00974 template<class T> Matrix4<T> Matrix4<T>::scaling(const Vector3<T>& s)
00975 {
00976 return Matrix4<T>(s[0], 0 , 0 , 0,
00977 0 ,s[1], 0 , 0,
00978 0 , 0 ,s[2], 0,
00979 0 , 0 , 0 , 1);
00980 }
00981
00982
00983 template<class T> Matrix4<T> Matrix4<T>::scaling(T scale)
00984 {
00985 return scaling(Vector3<T>(scale, scale, scale));
00986 }
00987
00988
00989 template<class T> Vector3<T> Matrix4<T>::operator*(const Vector3<T>& a) const
00990 {
00991 return Vector3<T>( r[0]*a.v[0] + r[4]*a.v[1] + r[8]*a.v[2] + r[12],
00992 r[1]*a.v[0] + r[5]*a.v[1] + r[9]*a.v[2] + r[13],
00993 r[2]*a.v[0] + r[6]*a.v[1] + r[10]*a.v[2] + r[14] );
00994 }
00995
00996 template<class T> Vector3<T> Matrix4<T>::multiplyWithoutTranslation(const Vector3<T>& a) const
00997 {
00998 return Vector3<T>( r[0]*a.v[0] + r[4]*a.v[1] + r[8]*a.v[2],
00999 r[1]*a.v[0] + r[5]*a.v[1] + r[9]*a.v[2],
01000 r[2]*a.v[0] + r[6]*a.v[1] + r[10]*a.v[2] );
01001 }
01002
01003
01004 template<class T> Vector4<T> Matrix4<T>::operator*(const Vector4<T>& a) const
01005 {
01006 return Vector4<T>( r[0]*a.v[0] + r[4]*a.v[1] + r[8]*a.v[2] + r[12]*a.v[3],
01007 r[1]*a.v[0] + r[5]*a.v[1] + r[9]*a.v[2] + r[13]*a.v[3],
01008 r[2]*a.v[0] + r[6]*a.v[1] + r[10]*a.v[2] + r[14]*a.v[3] );
01009 }
01010
01011 template<class T> void Matrix4<T>::transfo(Vector3<T>& a) const
01012 {
01013 a.set( r[0]*a.v[0] + r[4]*a.v[1] + r[8]*a.v[2] + r[12],
01014 r[1]*a.v[0] + r[5]*a.v[1] + r[9]*a.v[2] + r[13],
01015 r[2]*a.v[0] + r[6]*a.v[1] + r[10]*a.v[2] + r[14]);
01016 }
01017
01018 template<class T> Matrix4<T> Matrix4<T>::transpose() const
01019 {
01020 return Matrix4<T>( r[0], r[4], r[8], r[12],
01021 r[1], r[5], r[9], r[13],
01022 r[2], r[6], r[10], r[14],
01023 r[3], r[7], r[11], r[15]);
01024 }
01025
01026 template<class T> Matrix4<T> Matrix4<T>::operator*(const Matrix4<T>& a) const
01027 {
01028 #define MATMUL(R, C) (r[R] * a.r[C] + r[R+4] * a.r[C+1] + r[R+8] * a.r[C+2] + r[R+12] * a.r[C+3])
01029 return Matrix4<T>( MATMUL(0,0), MATMUL(1,0), MATMUL(2,0), MATMUL(3,0),
01030 MATMUL(0,4), MATMUL(1,4), MATMUL(2,4), MATMUL(3,4),
01031 MATMUL(0,8), MATMUL(1,8), MATMUL(2,8), MATMUL(3,8),
01032 MATMUL(0,12), MATMUL(1,12), MATMUL(2,12), MATMUL(3,12) );
01033 #undef MATMUL
01034 }
01035
01036
01037 template<class T> Matrix4<T> Matrix4<T>::operator+(const Matrix4<T>& a) const
01038 {
01039 return Matrix4<T>( r[0]+a.r[0], r[1]+a.r[1], r[2]+a.r[2], r[3]+a.r[3],
01040 r[4]+a.r[4], r[5]+a.r[5], r[6]+a.r[6], r[7]+a.r[7],
01041 r[8]+a.r[8], r[9]+a.r[9], r[10]+a.r[10], r[11]+a.r[11],
01042 r[12]+a.r[12], r[13]+a.r[13], r[14]+a.r[14], r[15]+a.r[15] );
01043 }
01044
01045 template<class T> Matrix4<T> Matrix4<T>::operator-(const Matrix4<T>& a) const
01046 {
01047 return Matrix4<T>( r[0]-a.r[0], r[1]-a.r[1], r[2]-a.r[2], r[3]-a.r[3],
01048 r[4]-a.r[4], r[5]-a.r[5], r[6]-a.r[6], r[7]-a.r[7],
01049 r[8]-a.r[8], r[9]-a.r[9], r[10]-a.r[10], r[11]-a.r[11],
01050 r[12]-a.r[12], r[13]-a.r[13], r[14]-a.r[14], r[15]-a.r[15] );
01051 }
01052
01053
01054
01055
01056
01057
01058
01059 template<class T> Matrix4<T> Matrix4<T>::inverse() const
01060 {
01061 const T * m = r;
01062 T out[16];
01063
01064
01065 #define SWAP_ROWS(a, b) { T *_tmp = a; (a)=(b); (b)=_tmp; }
01066 #define MAT(m,r,c) (m)[(c)*4+(r)]
01067
01068 T wtmp[4][8];
01069 T m0, m1, m2, m3, s;
01070 T *r0, *r1, *r2, *r3;
01071
01072 r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3];
01073
01074 r0[0] = MAT(m, 0, 0), r0[1] = MAT(m, 0, 1),
01075 r0[2] = MAT(m, 0, 2), r0[3] = MAT(m, 0, 3),
01076 r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0,
01077 r1[0] = MAT(m, 1, 0), r1[1] = MAT(m, 1, 1),
01078 r1[2] = MAT(m, 1, 2), r1[3] = MAT(m, 1, 3),
01079 r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0,
01080 r2[0] = MAT(m, 2, 0), r2[1] = MAT(m, 2, 1),
01081 r2[2] = MAT(m, 2, 2), r2[3] = MAT(m, 2, 3),
01082 r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0,
01083 r3[0] = MAT(m, 3, 0), r3[1] = MAT(m, 3, 1),
01084 r3[2] = MAT(m, 3, 2), r3[3] = MAT(m, 3, 3),
01085 r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0;
01086
01087
01088 if (fabs(r3[0]) > fabs(r2[0]))
01089 SWAP_ROWS(r3, r2);
01090 if (fabs(r2[0]) > fabs(r1[0]))
01091 SWAP_ROWS(r2, r1);
01092 if (fabs(r1[0]) > fabs(r0[0]))
01093 SWAP_ROWS(r1, r0);
01094 if (0.0 == r0[0])
01095 return Matrix4<T>();
01096
01097
01098 m1 = r1[0] / r0[0];
01099 m2 = r2[0] / r0[0];
01100 m3 = r3[0] / r0[0];
01101 s = r0[1];
01102 r1[1] -= m1 * s;
01103 r2[1] -= m2 * s;
01104 r3[1] -= m3 * s;
01105 s = r0[2];
01106 r1[2] -= m1 * s;
01107 r2[2] -= m2 * s;
01108 r3[2] -= m3 * s;
01109 s = r0[3];
01110 r1[3] -= m1 * s;
01111 r2[3] -= m2 * s;
01112 r3[3] -= m3 * s;
01113 s = r0[4];
01114 if (s != 0.0) {
01115 r1[4] -= m1 * s;
01116 r2[4] -= m2 * s;
01117 r3[4] -= m3 * s;
01118 }
01119 s = r0[5];
01120 if (s != 0.0) {
01121 r1[5] -= m1 * s;
01122 r2[5] -= m2 * s;
01123 r3[5] -= m3 * s;
01124 }
01125 s = r0[6];
01126 if (s != 0.0) {
01127 r1[6] -= m1 * s;
01128 r2[6] -= m2 * s;
01129 r3[6] -= m3 * s;
01130 }
01131 s = r0[7];
01132 if (s != 0.0) {
01133 r1[7] -= m1 * s;
01134 r2[7] -= m2 * s;
01135 r3[7] -= m3 * s;
01136 }
01137
01138
01139 if (fabs(r3[1]) > fabs(r2[1]))
01140 SWAP_ROWS(r3, r2);
01141 if (fabs(r2[1]) > fabs(r1[1]))
01142 SWAP_ROWS(r2, r1);
01143 if (0.0 == r1[1])
01144 return Matrix4<T>();
01145
01146
01147 m2 = r2[1] / r1[1];
01148 m3 = r3[1] / r1[1];
01149 r2[2] -= m2 * r1[2];
01150 r3[2] -= m3 * r1[2];
01151 r2[3] -= m2 * r1[3];
01152 r3[3] -= m3 * r1[3];
01153 s = r1[4];
01154 if (0.0 != s) {
01155 r2[4] -= m2 * s;
01156 r3[4] -= m3 * s;
01157 }
01158 s = r1[5];
01159 if (0.0 != s) {
01160 r2[5] -= m2 * s;
01161 r3[5] -= m3 * s;
01162 }
01163 s = r1[6];
01164 if (0.0 != s) {
01165 r2[6] -= m2 * s;
01166 r3[6] -= m3 * s;
01167 }
01168 s = r1[7];
01169 if (0.0 != s) {
01170 r2[7] -= m2 * s;
01171 r3[7] -= m3 * s;
01172 }
01173
01174
01175 if (fabs(r3[2]) > fabs(r2[2]))
01176 SWAP_ROWS(r3, r2);
01177 if (0.0 == r2[2])
01178 return Matrix4<T>();
01179
01180
01181 m3 = r3[2] / r2[2];
01182 r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4],
01183 r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6], r3[7] -= m3 * r2[7];
01184
01185
01186 if (0.0 == r3[3])
01187 return Matrix4<T>();
01188
01189 s = 1.0 / r3[3];
01190 r3[4] *= s;
01191 r3[5] *= s;
01192 r3[6] *= s;
01193 r3[7] *= s;
01194
01195 m2 = r2[3];
01196 s = 1.0 / r2[2];
01197 r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2),
01198 r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2);
01199 m1 = r1[3];
01200 r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1,
01201 r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1;
01202 m0 = r0[3];
01203 r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0,
01204 r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0;
01205
01206 m1 = r1[2];
01207 s = 1.0 / r1[1];
01208 r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1),
01209 r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1);
01210 m0 = r0[2];
01211 r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0,
01212 r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0;
01213
01214 m0 = r0[1];
01215 s = 1.0 / r0[0];
01216 r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0),
01217 r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0);
01218
01219 MAT(out, 0, 0) = r0[4];
01220 MAT(out, 0, 1) = r0[5], MAT(out, 0, 2) = r0[6];
01221 MAT(out, 0, 3) = r0[7], MAT(out, 1, 0) = r1[4];
01222 MAT(out, 1, 1) = r1[5], MAT(out, 1, 2) = r1[6];
01223 MAT(out, 1, 3) = r1[7], MAT(out, 2, 0) = r2[4];
01224 MAT(out, 2, 1) = r2[5], MAT(out, 2, 2) = r2[6];
01225 MAT(out, 2, 3) = r2[7], MAT(out, 3, 0) = r3[4];
01226 MAT(out, 3, 1) = r3[5], MAT(out, 3, 2) = r3[6];
01227 MAT(out, 3, 3) = r3[7];
01228
01229 return Matrix4<T>(out);
01230
01231 #undef MAT
01232 #undef SWAP_ROWS
01233 }
01234
01235
01236 template<class T> void Matrix4<T>::print(void) const
01237 {
01238 printf("[%5.2lf %5.2lf %5.2lf %17.12le]\n"
01239 "[%5.2lf %5.2lf %5.2lf %17.12le]\n"
01240 "[%5.2lf %5.2lf %5.2lf %17.12le]\n"
01241 "[%5.2lf %5.2lf %5.2lf %17.12le]\n\n",
01242 r[0],r[4],r[8],r[12],
01243 r[1],r[5],r[9],r[13],
01244 r[2],r[6],r[10],r[14],
01245 r[3],r[7],r[11],r[15]);
01246 }
01247
01248
01249 template<class T> inline
01250 T operator*(const Vector2<T>&a,const Vector2<T>&b) {
01251 return a.v[0] * b.v[0] + a.v[1] * b.v[1];
01252 }
01253
01254 template<class T> inline
01255 T operator*(const Vector3<T>&a,const Vector3<T>&b) {
01256 return a.v[0] * b.v[0] + a.v[1] * b.v[1] + a.v[2] * b.v[2];
01257 }
01258
01259 template<class T> inline
01260 T operator*(const Vector4<T>&a,const Vector4<T>&b) {
01261 return a.v[0]*b.v[0] + a.v[1]*b.v[1] + a.v[2]*b.v[2] + a.v[3]*b.v[3];
01262 }
01263
01264 template<class T> inline
01265 Vector2<T> operator*(T s,const Vector2<T>&v) {
01266 return Vector2<T>(s*v[0],s*v[1]);
01267 }
01268
01269 template<class T> inline
01270 Vector3<T> operator*(T s,const Vector3<T>&v) {
01271 return Vector3<T>(s*v[0],s*v[1],s*v[2]);
01272 }
01273
01274 template<class T> inline
01275 Vector4<T> operator*(T s,const Vector4<T>&v) {
01276 return Vector4<T>(s*v[0],s*v[1],s*v[2],s*v[3]);
01277 }
01278
01279
01280 #endif // _VECMATH_H_