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/=(T s)
00518 {
00519 v[0] /= s; v[1] /= s; v[2] /= s;
00520 return *this;
00521 }
00522
00523 template<class T> Vector3<T> Vector3<T>::operator-() const
00524 {
00525 return Vector3<T>(-v[0], -v[1], -v[2]);
00526 }
00527
00528 template<class T> Vector3<T> Vector3<T>::operator+() const
00529 {
00530 return *this;
00531 }
00532
00533 template<class T> Vector3<T> Vector3<T>::operator+(const Vector3<T>& b) const
00534 {
00535 return Vector3<T>(v[0] + b.v[0], v[1] + b.v[1], v[2] + b.v[2]);
00536 }
00537
00538 template<class T> Vector3<T> Vector3<T>::operator-(const Vector3<T>& b) const
00539 {
00540 return Vector3<T>(v[0] - b.v[0], v[1] - b.v[1], v[2] - b.v[2]);
00541 }
00542
00543 template<class T> Vector3<T> Vector3<T>::operator*(T s) const
00544 {
00545 return Vector3<T>(s * v[0], s * v[1], s * v[2]);
00546 }
00547
00548 template<class T> Vector3<T> Vector3<T>::operator/(T s) const
00549 {
00550 return Vector3<T>(v[0]/s, v[1]/s, v[2]/s);
00551 }
00552
00553
00554 template<class T> T Vector3<T>::dot(const Vector3<T>& b) const
00555 {
00556 return v[0] * b.v[0] + v[1] * b.v[1] + v[2] * b.v[2];
00557 }
00558
00559
00560
00561 template<class T> Vector3<T> Vector3<T>::operator^(const Vector3<T>& b) const
00562 {
00563 return Vector3<T>(v[1] * b.v[2] - v[2] * b.v[1],
00564 v[2] * b.v[0] - v[0] * b.v[2],
00565 v[0] * b.v[1] - v[1] * b.v[0]);
00566 }
00567
00568
00569 template<class T> T Vector3<T>::angle(const Vector3<T>& b) const
00570 {
00571 return std::acos(dot(b)/sqrt(lengthSquared()*b.lengthSquared()));
00572 }
00573
00574 template<class T> T Vector3<T>::length() const
00575 {
00576 return (T) sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
00577 }
00578
00579 template<class T> T Vector3<T>::lengthSquared() const
00580 {
00581 return v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
00582 }
00583
00584 template<class T> void Vector3<T>::normalize()
00585 {
00586 T s = (T) (1. / std::sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]));
00587 v[0] *= s;
00588 v[1] *= s;
00589 v[2] *= s;
00590 }
00591
00592 template<class T> void Vector3<T>::transfo4d(const Mat4d& m)
00593 {
00594 (*this)=m*(*this);
00595 }
00596
00597 template<class T> void Vector3<T>::transfo4d(const Mat4f& m)
00598 {
00599 (*this)=m*(*this);
00600 }
00601
00602
00603 template<class T> T Vector3<T>::latitude() const
00604 {
00605 return std::asin(v[2]/length());
00606 }
00607
00608
00609 template<class T> T Vector3<T>::longitude() const
00610 {
00611 return std::atan2(v[1],v[0]);
00612 }
00613
00614
00615
00616
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631
00632
00634
00635 template<class T> Vector4<T>::Vector4() {}
00636
00637 template<class T> Vector4<T>::Vector4(const Vector4<T>& a)
00638 {
00639 v[0]=a.v[0]; v[1]=a.v[1]; v[2]=a.v[2]; v[3]=a.v[3];
00640 }
00641
00642 template<class T> Vector4<T>::Vector4(const Vector3<T>& a)
00643 {
00644 v[0]=a.v[0]; v[1]=a.v[1]; v[2]=a.v[2]; v[3]=1;
00645 }
00646
00647 template<class T> Vector4<T>::Vector4(T x, T y, T z)
00648 {
00649 v[0]=x; v[1]=y; v[2]=z; v[3]=1;
00650 }
00651
00652 template<class T> Vector4<T>::Vector4(T x, T y, T z, T a = 1)
00653 {
00654 v[0]=x; v[1]=y; v[2]=z; v[3]=a;
00655 }
00656
00657 template<class T> Vector4<T>& Vector4<T>::operator=(const Vector4<T>& a)
00658 {
00659 v[0]=a.v[0]; v[1]=a.v[1]; v[2]=a.v[2]; v[3]=a.v[3];
00660 return *this;
00661 }
00662
00663 template<class T> Vector4<T>& Vector4<T>::operator=(const Vector3<T>& a)
00664 {
00665 v[0]=a.v[0]; v[1]=a.v[1]; v[2]=a.v[2]; v[3]=1;
00666 return *this;
00667 }
00668
00669 template<class T> Vector4<T>& Vector4<T>::operator=(const T* a)
00670 {
00671 v[0]=a[0]; v[1]=a[1]; v[2]=a[2]; v[3]=a[3];
00672 return *this;
00673 }
00674
00675 template<class T> void Vector4<T>::set(T x, T y, T z, T a)
00676 {
00677 v[0]=x; v[1]=y; v[2]=z; v[3]=a;
00678 }
00679
00680 template<class T> bool Vector4<T>::operator==(const Vector4<T>& a) const
00681 {
00682 return (v[0] == a.v[0] && v[1] == a.v[1] && v[2] == a.v[2] && v[3] == a.v[3]);
00683 }
00684
00685 template<class T> bool Vector4<T>::operator!=(const Vector4<T>& a) const
00686 {
00687 return (v[0] != a.v[0] || v[1] != a.v[1] || v[2] != a.v[2] || v[3] != a.v[3]);
00688 }
00689
00690 template<class T> T& Vector4<T>::operator[](int x)
00691 {
00692 return v[x];
00693 }
00694
00695 template<class T> const T& Vector4<T>::operator[](int x) const
00696 {
00697 return v[x];
00698 }
00699
00700 template<class T> Vector4<T>::operator T*()
00701 {
00702 return v;
00703 }
00704
00705 template<class T> Vector4<T>::operator const T*() const
00706 {
00707 return v;
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-=(const Vector4<T>& a)
00717 {
00718 v[0] -= a.v[0]; v[1] -= a.v[1]; v[2] -= a.v[2]; v[3] -= a/v[3];
00719 return *this;
00720 }
00721
00722 template<class T> Vector4<T>& Vector4<T>::operator*=(T s)
00723 {
00724 v[0] *= s; v[1] *= s; v[2] *= s; v[3] *= s;
00725 return *this;
00726 }
00727
00728 template<class T> Vector4<T> Vector4<T>::operator-() const
00729 {
00730 return Vector4<T>(-v[0], -v[1], -v[2], -v[3]);
00731 }
00732
00733 template<class T> Vector4<T> Vector4<T>::operator+() const
00734 {
00735 return *this;
00736 }
00737
00738 template<class T> Vector4<T> Vector4<T>::operator+(const Vector4<T>& b) const
00739 {
00740 return Vector4<T>(v[0] + b.v[0], v[1] + b.v[1], v[2] + b.v[2], v[3] + b.v[3]);
00741 }
00742
00743 template<class T> Vector4<T> Vector4<T>::operator-(const Vector4<T>& b) const
00744 {
00745 return Vector4<T>(v[0] - b.v[0], v[1] - b.v[1], v[2] - b.v[2], v[3] - b.v[3]);
00746 }
00747
00748 template<class T> Vector4<T> Vector4<T>::operator*(T s) const
00749 {
00750 return Vector4<T>(s * v[0], s * v[1], s * v[2], s * v[3]);
00751 }
00752
00753 template<class T> Vector4<T> Vector4<T>::operator/(T s) const
00754 {
00755 return Vector4<T>(v[0]/s, v[1]/s, v[2]/s, v[3]/s);
00756 }
00757
00758 template<class T> T Vector4<T>::dot(const Vector4<T>& b) const
00759 {
00760 return v[0] * b.v[0] + v[1] * b.v[1] + v[2] * b.v[2] + v[3] * b.v[3];
00761 }
00762
00763 template<class T> T Vector4<T>::length() const
00764 {
00765 return (T) sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3]);
00766 }
00767
00768 template<class T> T Vector4<T>::lengthSquared() const
00769 {
00770 return v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3];
00771 }
00772
00773 template<class T> void Vector4<T>::normalize()
00774 {
00775 T s = (T) (1. / sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3]));
00776 v[0] *= s;
00777 v[1] *= s;
00778 v[2] *= s;
00779 v[3] *= s;
00780 }
00781
00782 template<class T> void Vector4<T>::transfo4d(const Mat4d& m)
00783 {
00784 (*this)=m*(*this);
00785 }
00786
00787
00788
00789
00790
00791
00792
00794
00795 template<class T> Matrix4<T>::Matrix4() {}
00796
00797 template<class T> Matrix4<T>::Matrix4(const Matrix4<T>& m)
00798 {
00799 memcpy(r,m.r,sizeof(T)*16);
00800 }
00801
00802 template<class T> Matrix4<T>::Matrix4(const T* m)
00803 {
00804 memcpy(r,m,sizeof(T)*16);
00805 }
00806
00807 template<class T> Matrix4<T>& Matrix4<T>::operator=(const Matrix4<T>& m)
00808 {
00809 memcpy(r,m.r,sizeof(T)*16);
00810 return (*this);
00811 }
00812
00813 template<class T> Matrix4<T>::Matrix4(const Vector4<T>& v0,
00814 const Vector4<T>& v1,
00815 const Vector4<T>& v2,
00816 const Vector4<T>& v3)
00817 {
00818 r[0] = v0.v[0];
00819 r[1] = v0.v[1];
00820 r[2] = v0.v[2];
00821 r[3] = v0.v[3];
00822 r[4] = v1.v[0];
00823 r[5] = v1.v[1];
00824 r[6] = v1.v[2];
00825 r[7] = v1.v[3];
00826 r[8] = v2.v[0];
00827 r[9] = v2.v[1];
00828 r[10] = v2.v[2];
00829 r[11] = v2.v[3];
00830 r[12] = v3.v[0];
00831 r[13] = v3.v[1];
00832 r[14] = v3.v[2];
00833 r[15] = v3.v[3];
00834 }
00835
00836
00837 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)
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> 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)
00844 {
00845 r[0]=a; r[1]=b; r[2]=c; r[3]=d; r[4]=e; r[5]=f; r[6]=g; r[7]=h;
00846 r[8]=i; r[9]=j; r[10]=k; r[11]=l; r[12]=m; r[13]=n; r[14]=o; r[15]=p;
00847 }
00848
00849 template<class T> T* Matrix4<T>::operator[](int n)
00850 {
00851 return &(r[n*4]);
00852 }
00853
00854 template<class T> Matrix4<T>::operator T*()
00855 {
00856 return r;
00857 }
00858
00859 template<class T> Matrix4<T>::operator const T*() const
00860 {
00861 return r;
00862 }
00863
00864 template<class T> Matrix4<T> Matrix4<T>::identity()
00865 {
00866 return Matrix4<T>( 1, 0, 0, 0,
00867 0, 1, 0, 0,
00868 0, 0, 1, 0,
00869 0, 0, 0, 1 );
00870 }
00871
00872
00873 template<class T> Matrix4<T> Matrix4<T>::translation(const Vector3<T>& a)
00874 {
00875 return Matrix4<T>( 1, 0, 0, 0,
00876 0, 1, 0, 0,
00877 0, 0, 1, 0,
00878 a.v[0], a.v[1], a.v[2], 1);
00879 }
00880
00881 template<class T> Matrix4<T> Matrix4<T>::rotation(const Vector3<T>& a,
00882 T angle)
00883 {
00884 Vec3d axis(a);
00885 axis.normalize();
00886
00887
00888
00889
00890
00891
00892
00893
00894
00895
00896
00897
00898
00899
00900
00901
00902
00903
00904 T sin_a = std::sin( angle / 2 );
00905 T cos_a = std::cos( angle / 2 );
00906 T X = axis[0] * sin_a;
00907 T Y = axis[1] * sin_a;
00908 T Z = axis[2] * sin_a;
00909 T W = cos_a;
00910
00911 T xx = X * X;
00912 T xy = X * Y;
00913 T xz = X * Z;
00914 T xw = X * W;
00915
00916 T yy = Y * Y;
00917 T yz = Y * Z;
00918 T yw = Y * W;
00919
00920 T zz = Z * Z;
00921 T zw = Z * W;
00922
00923 return Matrix4<T>(
00924 1. - 2. * ( yy + zz ), 2. * ( xy + zw ), 2. * ( xz - yw ), 0.,
00925 2. * ( xy - zw ), 1. - 2. * ( xx + zz ), 2. * ( yz + xw ), 0.,
00926 2. * ( xz + yw ), 2. * ( yz - xw ), 1. - 2. * ( xx + yy ), 0.,
00927 0., 0., 0., 1.);
00928 }
00929
00930
00931
00932
00933
00934
00935
00936
00937
00938
00939
00940
00941
00942
00943
00944 template<class T> Matrix4<T> Matrix4<T>::xrotation(T angle)
00945 {
00946 T c = (T) cos(angle);
00947 T s = (T) sin(angle);
00948
00949 return Matrix4<T>(1, 0, 0, 0,
00950 0, c, s, 0,
00951 0,-s, c, 0,
00952 0, 0, 0, 1 );
00953 }
00954
00955
00956 template<class T> Matrix4<T> Matrix4<T>::yrotation(T angle)
00957 {
00958 T c = (T) cos(angle);
00959 T s = (T) sin(angle);
00960
00961 return Matrix4<T>( c, 0,-s, 0,
00962 0, 1, 0, 0,
00963 s, 0, c, 0,
00964 0, 0, 0, 1 );
00965 }
00966
00967
00968 template<class T> Matrix4<T> Matrix4<T>::zrotation(T angle)
00969 {
00970 T c = (T) cos(angle);
00971 T s = (T) sin(angle);
00972
00973 return Matrix4<T>(c, s, 0, 0,
00974 -s, c, 0, 0,
00975 0, 0, 1, 0,
00976 0, 0, 0, 1 );
00977 }
00978
00979
00980 template<class T> Matrix4<T> Matrix4<T>::scaling(const Vector3<T>& s)
00981 {
00982 return Matrix4<T>(s[0], 0 , 0 , 0,
00983 0 ,s[1], 0 , 0,
00984 0 , 0 ,s[2], 0,
00985 0 , 0 , 0 , 1);
00986 }
00987
00988
00989 template<class T> Matrix4<T> Matrix4<T>::scaling(T scale)
00990 {
00991 return scaling(Vector3<T>(scale, scale, scale));
00992 }
00993
00994
00995 template<class T> Vector3<T> Matrix4<T>::operator*(const Vector3<T>& a) const
00996 {
00997 return Vector3<T>( r[0]*a.v[0] + r[4]*a.v[1] + r[8]*a.v[2] + r[12],
00998 r[1]*a.v[0] + r[5]*a.v[1] + r[9]*a.v[2] + r[13],
00999 r[2]*a.v[0] + r[6]*a.v[1] + r[10]*a.v[2] + r[14] );
01000 }
01001
01002 template<class T> Vector3<T> Matrix4<T>::multiplyWithoutTranslation(const Vector3<T>& a) const
01003 {
01004 return Vector3<T>( r[0]*a.v[0] + r[4]*a.v[1] + r[8]*a.v[2],
01005 r[1]*a.v[0] + r[5]*a.v[1] + r[9]*a.v[2],
01006 r[2]*a.v[0] + r[6]*a.v[1] + r[10]*a.v[2] );
01007 }
01008
01009
01010 template<class T> Vector4<T> Matrix4<T>::operator*(const Vector4<T>& a) const
01011 {
01012 return Vector4<T>( r[0]*a.v[0] + r[4]*a.v[1] + r[8]*a.v[2] + r[12]*a.v[3],
01013 r[1]*a.v[0] + r[5]*a.v[1] + r[9]*a.v[2] + r[13]*a.v[3],
01014 r[2]*a.v[0] + r[6]*a.v[1] + r[10]*a.v[2] + r[14]*a.v[3] );
01015 }
01016
01017 template<class T> void Matrix4<T>::transfo(Vector3<T>& a) const
01018 {
01019 a.set( r[0]*a.v[0] + r[4]*a.v[1] + r[8]*a.v[2] + r[12],
01020 r[1]*a.v[0] + r[5]*a.v[1] + r[9]*a.v[2] + r[13],
01021 r[2]*a.v[0] + r[6]*a.v[1] + r[10]*a.v[2] + r[14]);
01022 }
01023
01024 template<class T> Matrix4<T> Matrix4<T>::transpose() const
01025 {
01026 return Matrix4<T>( r[0], r[4], r[8], r[12],
01027 r[1], r[5], r[9], r[13],
01028 r[2], r[6], r[10], r[14],
01029 r[3], r[7], r[11], r[15]);
01030 }
01031
01032 template<class T> Matrix4<T> Matrix4<T>::operator*(const Matrix4<T>& a) const
01033 {
01034 #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])
01035 return Matrix4<T>( MATMUL(0,0), MATMUL(1,0), MATMUL(2,0), MATMUL(3,0),
01036 MATMUL(0,4), MATMUL(1,4), MATMUL(2,4), MATMUL(3,4),
01037 MATMUL(0,8), MATMUL(1,8), MATMUL(2,8), MATMUL(3,8),
01038 MATMUL(0,12), MATMUL(1,12), MATMUL(2,12), MATMUL(3,12) );
01039 #undef MATMUL
01040 }
01041
01042
01043 template<class T> Matrix4<T> Matrix4<T>::operator+(const Matrix4<T>& a) const
01044 {
01045 return Matrix4<T>( r[0]+a.r[0], r[1]+a.r[1], r[2]+a.r[2], r[3]+a.r[3],
01046 r[4]+a.r[4], r[5]+a.r[5], r[6]+a.r[6], r[7]+a.r[7],
01047 r[8]+a.r[8], r[9]+a.r[9], r[10]+a.r[10], r[11]+a.r[11],
01048 r[12]+a.r[12], r[13]+a.r[13], r[14]+a.r[14], r[15]+a.r[15] );
01049 }
01050
01051 template<class T> Matrix4<T> Matrix4<T>::operator-(const Matrix4<T>& a) const
01052 {
01053 return Matrix4<T>( r[0]-a.r[0], r[1]-a.r[1], r[2]-a.r[2], r[3]-a.r[3],
01054 r[4]-a.r[4], r[5]-a.r[5], r[6]-a.r[6], r[7]-a.r[7],
01055 r[8]-a.r[8], r[9]-a.r[9], r[10]-a.r[10], r[11]-a.r[11],
01056 r[12]-a.r[12], r[13]-a.r[13], r[14]-a.r[14], r[15]-a.r[15] );
01057 }
01058
01059
01060
01061
01062
01063
01064
01065 template<class T> Matrix4<T> Matrix4<T>::inverse() const
01066 {
01067 const T * m = r;
01068 T out[16];
01069
01070
01071 #define SWAP_ROWS(a, b) { T *_tmp = a; (a)=(b); (b)=_tmp; }
01072 #define MAT(m,r,c) (m)[(c)*4+(r)]
01073
01074 T wtmp[4][8];
01075 T m0, m1, m2, m3, s;
01076 T *r0, *r1, *r2, *r3;
01077
01078 r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3];
01079
01080 r0[0] = MAT(m, 0, 0), r0[1] = MAT(m, 0, 1),
01081 r0[2] = MAT(m, 0, 2), r0[3] = MAT(m, 0, 3),
01082 r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0,
01083 r1[0] = MAT(m, 1, 0), r1[1] = MAT(m, 1, 1),
01084 r1[2] = MAT(m, 1, 2), r1[3] = MAT(m, 1, 3),
01085 r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0,
01086 r2[0] = MAT(m, 2, 0), r2[1] = MAT(m, 2, 1),
01087 r2[2] = MAT(m, 2, 2), r2[3] = MAT(m, 2, 3),
01088 r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0,
01089 r3[0] = MAT(m, 3, 0), r3[1] = MAT(m, 3, 1),
01090 r3[2] = MAT(m, 3, 2), r3[3] = MAT(m, 3, 3),
01091 r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0;
01092
01093
01094 if (fabs(r3[0]) > fabs(r2[0]))
01095 SWAP_ROWS(r3, r2);
01096 if (fabs(r2[0]) > fabs(r1[0]))
01097 SWAP_ROWS(r2, r1);
01098 if (fabs(r1[0]) > fabs(r0[0]))
01099 SWAP_ROWS(r1, r0);
01100 if (0.0 == r0[0])
01101 return Matrix4<T>();
01102
01103
01104 m1 = r1[0] / r0[0];
01105 m2 = r2[0] / r0[0];
01106 m3 = r3[0] / r0[0];
01107 s = r0[1];
01108 r1[1] -= m1 * s;
01109 r2[1] -= m2 * s;
01110 r3[1] -= m3 * s;
01111 s = r0[2];
01112 r1[2] -= m1 * s;
01113 r2[2] -= m2 * s;
01114 r3[2] -= m3 * s;
01115 s = r0[3];
01116 r1[3] -= m1 * s;
01117 r2[3] -= m2 * s;
01118 r3[3] -= m3 * s;
01119 s = r0[4];
01120 if (s != 0.0) {
01121 r1[4] -= m1 * s;
01122 r2[4] -= m2 * s;
01123 r3[4] -= m3 * s;
01124 }
01125 s = r0[5];
01126 if (s != 0.0) {
01127 r1[5] -= m1 * s;
01128 r2[5] -= m2 * s;
01129 r3[5] -= m3 * s;
01130 }
01131 s = r0[6];
01132 if (s != 0.0) {
01133 r1[6] -= m1 * s;
01134 r2[6] -= m2 * s;
01135 r3[6] -= m3 * s;
01136 }
01137 s = r0[7];
01138 if (s != 0.0) {
01139 r1[7] -= m1 * s;
01140 r2[7] -= m2 * s;
01141 r3[7] -= m3 * s;
01142 }
01143
01144
01145 if (fabs(r3[1]) > fabs(r2[1]))
01146 SWAP_ROWS(r3, r2);
01147 if (fabs(r2[1]) > fabs(r1[1]))
01148 SWAP_ROWS(r2, r1);
01149 if (0.0 == r1[1])
01150 return Matrix4<T>();
01151
01152
01153 m2 = r2[1] / r1[1];
01154 m3 = r3[1] / r1[1];
01155 r2[2] -= m2 * r1[2];
01156 r3[2] -= m3 * r1[2];
01157 r2[3] -= m2 * r1[3];
01158 r3[3] -= m3 * r1[3];
01159 s = r1[4];
01160 if (0.0 != s) {
01161 r2[4] -= m2 * s;
01162 r3[4] -= m3 * s;
01163 }
01164 s = r1[5];
01165 if (0.0 != s) {
01166 r2[5] -= m2 * s;
01167 r3[5] -= m3 * s;
01168 }
01169 s = r1[6];
01170 if (0.0 != s) {
01171 r2[6] -= m2 * s;
01172 r3[6] -= m3 * s;
01173 }
01174 s = r1[7];
01175 if (0.0 != s) {
01176 r2[7] -= m2 * s;
01177 r3[7] -= m3 * s;
01178 }
01179
01180
01181 if (fabs(r3[2]) > fabs(r2[2]))
01182 SWAP_ROWS(r3, r2);
01183 if (0.0 == r2[2])
01184 return Matrix4<T>();
01185
01186
01187 m3 = r3[2] / r2[2];
01188 r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4],
01189 r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6], r3[7] -= m3 * r2[7];
01190
01191
01192 if (0.0 == r3[3])
01193 return Matrix4<T>();
01194
01195 s = 1.0 / r3[3];
01196 r3[4] *= s;
01197 r3[5] *= s;
01198 r3[6] *= s;
01199 r3[7] *= s;
01200
01201 m2 = r2[3];
01202 s = 1.0 / r2[2];
01203 r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2),
01204 r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2);
01205 m1 = r1[3];
01206 r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1,
01207 r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1;
01208 m0 = r0[3];
01209 r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0,
01210 r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0;
01211
01212 m1 = r1[2];
01213 s = 1.0 / r1[1];
01214 r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1),
01215 r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1);
01216 m0 = r0[2];
01217 r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0,
01218 r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0;
01219
01220 m0 = r0[1];
01221 s = 1.0 / r0[0];
01222 r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0),
01223 r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0);
01224
01225 MAT(out, 0, 0) = r0[4];
01226 MAT(out, 0, 1) = r0[5], MAT(out, 0, 2) = r0[6];
01227 MAT(out, 0, 3) = r0[7], MAT(out, 1, 0) = r1[4];
01228 MAT(out, 1, 1) = r1[5], MAT(out, 1, 2) = r1[6];
01229 MAT(out, 1, 3) = r1[7], MAT(out, 2, 0) = r2[4];
01230 MAT(out, 2, 1) = r2[5], MAT(out, 2, 2) = r2[6];
01231 MAT(out, 2, 3) = r2[7], MAT(out, 3, 0) = r3[4];
01232 MAT(out, 3, 1) = r3[5], MAT(out, 3, 2) = r3[6];
01233 MAT(out, 3, 3) = r3[7];
01234
01235 return Matrix4<T>(out);
01236
01237 #undef MAT
01238 #undef SWAP_ROWS
01239 }
01240
01241
01242 template<class T> void Matrix4<T>::print(void) const
01243 {
01244 printf("[%5.2lf %5.2lf %5.2lf %17.12le]\n"
01245 "[%5.2lf %5.2lf %5.2lf %17.12le]\n"
01246 "[%5.2lf %5.2lf %5.2lf %17.12le]\n"
01247 "[%5.2lf %5.2lf %5.2lf %17.12le]\n\n",
01248 r[0],r[4],r[8],r[12],
01249 r[1],r[5],r[9],r[13],
01250 r[2],r[6],r[10],r[14],
01251 r[3],r[7],r[11],r[15]);
01252 }
01253
01254
01255 template<class T> inline
01256 T operator*(const Vector2<T>&a,const Vector2<T>&b) {
01257 return a.v[0] * b.v[0] + a.v[1] * b.v[1];
01258 }
01259
01260 template<class T> inline
01261 T operator*(const Vector3<T>&a,const Vector3<T>&b) {
01262 return a.v[0] * b.v[0] + a.v[1] * b.v[1] + a.v[2] * b.v[2];
01263 }
01264
01265 template<class T> inline
01266 T operator*(const Vector4<T>&a,const Vector4<T>&b) {
01267 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];
01268 }
01269
01270 template<class T> inline
01271 Vector2<T> operator*(T s,const Vector2<T>&v) {
01272 return Vector2<T>(s*v[0],s*v[1]);
01273 }
01274
01275 template<class T> inline
01276 Vector3<T> operator*(T s,const Vector3<T>&v) {
01277 return Vector3<T>(s*v[0],s*v[1],s*v[2]);
01278 }
01279
01280 template<class T> inline
01281 Vector4<T> operator*(T s,const Vector4<T>&v) {
01282 return Vector4<T>(s*v[0],s*v[1],s*v[2],s*v[3]);
01283 }
01284
01285
01286 #endif // _VECMATH_H_