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