Stellarium  0.16.1
VecMath.hpp
1 /*
2  *
3  * Copyright (C) 2003 Fabien Chereau
4  *
5  * This program is free software; you can redistribute it and/or
6  * modify it under the terms of the GNU General Public License
7  * as published by the Free Software Foundation; either version 2
8  * of the License, or (at your option) any later version.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software
17  * Foundation, Inc., 51 Franklin Street, Suite 500, Boston, MA 02110-1335, USA.
18  */
19 
20 // Template vector and matrix library.
21 // Use OpenGL compatible ordering ie. you can pass a matrix or vector to
22 // openGL functions without changes in the ordering
23 
24 #ifndef _VECMATH_H_
25 #define _VECMATH_H_
26 
27 #include <cmath>
28 #include <limits>
29 #include <QString>
30 #include <QMatrix4x4>
31 
32 template<class T> class Vector2;
33 template<class T> class Vector3;
34 template<class T> class Vector4;
35 template<class T> class Matrix4;
36 template<class T> class Matrix3;
37 
38 typedef Vector2<double> Vec2d;
39 typedef Vector2<float> Vec2f;
40 typedef Vector2<int> Vec2i;
41 
44 typedef Vector3<double> Vec3d;
45 
48 typedef Vector3<float> Vec3f;
49 
52 typedef Vector3<int> Vec3i;
53 
56 typedef Vector4<double> Vec4d;
57 
60 typedef Vector4<float> Vec4f;
61 
64 typedef Vector4<int> Vec4i;
65 
68 typedef Matrix4<double> Mat4d;
69 
72 typedef Matrix4<float> Mat4f;
73 
76 typedef Matrix3<double> Mat3d;
77 
80 typedef Matrix3<float> Mat3f;
81 
82 
86 template<class T> class Vector2
87 {
88 public:
90  inline Vector2();
92  inline Vector2(T);
95  inline explicit Vector2(const T*);
98  template <class T2> inline explicit Vector2(const Vector2<T2>&);
99  inline Vector2(T, T);
100 
103  inline Vector2<T>& operator=(const T*);
104  inline void set(T, T);
105 
106  inline bool operator==(const Vector2<T>&) const;
107  inline bool operator!=(const Vector2<T>&) const;
108 
109  inline const T& operator[](int x) const;
110  inline T& operator[](int);
111  inline operator const T*() const;
112  inline operator T*();
113 
114  inline Vector2<T>& operator+=(const Vector2<T>&);
115  inline Vector2<T>& operator-=(const Vector2<T>&);
117  inline Vector2<T>& operator*=(T);
119  inline Vector2<T>& operator*=(const Vector2<T>&);
121  inline Vector2<T>& operator/=(T);
123  inline Vector2<T>& operator/=(const Vector2<T>&);
124 
125  inline Vector2<T> operator-(const Vector2<T>&) const;
126  inline Vector2<T> operator+(const Vector2<T>&) const;
127 
128  inline Vector2<T> operator-() const;
129  inline Vector2<T> operator+() const;
130 
132  inline Vector2<T> operator*(T) const;
134  inline Vector2<T> operator*(const Vector2<T>&) const;
136  inline Vector2<T> operator/(T) const;
138  inline Vector2<T> operator/(const Vector2<T>&) const;
139 
141  inline Vector2<T> min(const Vector2<T>&) const;
143  inline Vector2<T> max(const Vector2<T>&) const;
145  inline Vector2<T> clamp(const Vector2<T>& low, const Vector2<T>& high) const;
146 
147  inline T dot(const Vector2<T>&) const;
148 
149  inline T length() const;
150  inline T lengthSquared() const;
151  inline void normalize();
152 
153  T v[2];
154 
155  QString toString() const {return QString("[%1, %2]").arg(v[0]).arg(v[1]);}
156 };
157 
158 
162 template<class T> class Vector3
163 {
164 public:
166  inline Vector3();
168  inline Vector3(T);
171  inline explicit Vector3(const T*);
172  inline Vector3(T, T, T);
173 
174  //inline Vector3& operator=(const Vector3&);
175 
178  inline Vector3& operator=(const T*);
179  //template <class T2> inline Vector3& operator=(const Vector3<T2>&);
180  inline void set(T, T, T);
181 
182  inline bool operator==(const Vector3<T>&) const;
183  inline bool operator!=(const Vector3<T>&) const;
185  inline bool fuzzyEquals(const Vector3<T>&, T epsilon = std::numeric_limits<T>::epsilon()) const;
186 
187  inline T& operator[](int);
188  inline const T& operator[](int) const;
189  inline operator const T*() const;
190  inline operator T*();
191  inline const T* data() const {return v;}
192  inline T* data() {return v;}
193 
194  inline void operator+=(const Vector3<T>&);
195  inline void operator-=(const Vector3<T>&);
196  inline void operator*=(T);
197  inline void operator/=(T);
198 
199  inline Vector3 operator-(const Vector3<T>&) const;
200  inline Vector3 operator+(const Vector3<T>&) const;
201 
202  inline Vector3 operator-() const;
203  inline Vector3 operator+() const;
204 
205  inline Vector3 operator*(T) const;
206  inline Vector3 operator/(T) const;
207 
208 
209  inline T dot(const Vector3<T>&) const;
210  inline Vector3 operator^(const Vector3<T>&) const;
211 
212  // Return latitude in rad
213  inline T latitude() const;
214  // Return longitude in rad
215  inline T longitude() const;
216 
217  // Distance in radian between two
218  inline T angle(const Vector3<T>&) const;
219  inline T angleNormalized(const Vector3<T>&) const;
220 
221  inline T length() const;
222  inline T lengthSquared() const;
223  inline void normalize();
224 
225  inline void transfo4d(const Mat4d&);
226  inline void transfo4d(const Mat4f&);
227 
228  //used for explicit conversion to other precision type
229  inline Vec3f toVec3f() const;
230  inline Vec3d toVec3d() const;
231 
232  T v[3]; // The 3 values
233 
234  QString toString() const {return QString("[%1, %2, %3]").arg(v[0]).arg(v[1]).arg(v[2]);}
235  QString toStringLonLat() const {return QString("[") + QString::number(longitude()*180./M_PI, 'g', 12) + "," + QString::number(latitude()*180./M_PI, 'g', 12)+"]";}
236 };
237 
238 
242 template<class T> class Vector4
243 {
244 public:
246  inline Vector4();
249  inline explicit Vector4(const T*);
251  inline Vector4(const Vector3<T>&);
253  inline Vector4(T, T, T);
254  inline Vector4(T, T, T, T);
255 
256  inline Vector4& operator=(const Vector3<T>&);
257  inline Vector4& operator=(const T*);
258  inline void set(T, T, T, T);
259 
260  inline bool operator==(const Vector4<T>&) const;
261  inline bool operator!=(const Vector4<T>&) const;
262 
263  inline T& operator[](int);
264  inline const T& operator[](int) const;
265  inline operator T*();
266  inline operator const T*() const;
267 
268  inline void operator+=(const Vector4<T>&);
269  inline void operator-=(const Vector4<T>&);
270  inline void operator*=(T);
271  inline void operator/=(T);
272 
273  inline Vector4 operator-(const Vector4<T>&) const;
274  inline Vector4 operator+(const Vector4<T>&) const;
275 
276  inline Vector4 operator-() const;
277  inline Vector4 operator+() const;
278 
279  inline Vector4 operator*(T) const;
280  inline Vector4 operator/(T) const;
281 
282 
283  inline T dot(const Vector4<T>&) const;
284 
285  inline T length() const;
286  inline T lengthSquared() const;
287  inline void normalize();
288 
289  inline void transfo4d(const Mat4d&);
290  QString toString() const {return QString("[%1, %2, %3, %4]").arg(v[0]).arg(v[1]).arg(v[2]).arg(v[3]);}
291 
292  T v[4]; // The 4 values
293 };
294 
298 template<class T> class Matrix3
299 {
300 public:
301  Matrix3();
302  Matrix3(T,T,T,T,T,T,T,T,T);
303  Matrix3(const T*);
304  Matrix3(const Vector3<T>&, const Vector3<T>&, const Vector3<T>&);
305 
306  inline Matrix3& operator=(const T*);
307  inline void set(T,T,T,T,T,T,T,T,T);
308 
309  inline T& operator[](int);
310  inline operator T*();
311  inline operator const T*() const;
312 
313  inline Matrix3 operator-(const Matrix3<T>&) const;
314  inline Matrix3 operator+(const Matrix3<T>&) const;
315  inline Matrix3 operator*(const Matrix3<T>&) const;
316 
317  inline Vector3<T> operator*(const Vector3<T>&) const;
318 
319  static Matrix3<T> identity();
320 
321  Matrix3<T> transpose() const;
322  Matrix3<T> inverse() const;
324  inline T trace() const {return r[0]+r[4]+r[8];}
326  inline T angle() const {return acos(0.5*(this->trace()-1.0));}
327 
328  inline void print(void) const;
329  QString toString(int fieldWidth=0, char format='g', int precision=-1) const {return QString("[[%1, %2, %3], [%4, %5, %6], [%7, %8, %9]]")
330  .arg(r[0], fieldWidth, format, precision)
331  .arg(r[1], fieldWidth, format, precision)
332  .arg(r[2], fieldWidth, format, precision)
333  .arg(r[3], fieldWidth, format, precision)
334  .arg(r[4], fieldWidth, format, precision)
335  .arg(r[5], fieldWidth, format, precision)
336  .arg(r[6], fieldWidth, format, precision)
337  .arg(r[7], fieldWidth, format, precision)
338  .arg(r[8], fieldWidth, format, precision);}
339 
340  T r[9];
341 };
342 
346 template<class T> class Matrix4
347 {
348 public:
349  Matrix4();
350  Matrix4(T,T,T,T,T,T,T,T,T,T,T,T,T,T,T,T);
351  Matrix4(const T*);
352  Matrix4(const Vector4<T>&, const Vector4<T>&,
353  const Vector4<T>&, const Vector4<T>&);
354 
355  inline Matrix4& operator=(const T*);
356  inline void set(T,T,T,T,T,T,T,T,T,T,T,T,T,T,T,T);
357 
358  inline T& operator[](int);
359  inline operator T*();
360  inline operator const T*() const;
361 
362  inline Matrix4 operator-(const Matrix4<T>&) const;
363  inline Matrix4 operator+(const Matrix4<T>&) const;
364  inline Matrix4 operator*(const Matrix4<T>&) const;
365 
366  inline Vector3<T> operator*(const Vector3<T>&) const;
367  inline Vector3<T> multiplyWithoutTranslation(const Vector3<T>& a) const;
368  inline Vector4<T> operator*(const Vector4<T>&) const;
369 
370  inline void transfo(Vector3<T>&) const;
371 
372  static Matrix4<T> identity();
373  static Matrix4<T> translation(const Vector3<T>&);
374 
375  // static Matrix4<T> rotation(const Vector3<T>&);
376  static Matrix4<T> rotation(const Vector3<T>&, T);
377  static Matrix4<T> xrotation(T);
378  static Matrix4<T> yrotation(T);
379  static Matrix4<T> zrotation(T);
380  static Matrix4<T> scaling(const Vector3<T>&);
381  static Matrix4<T> scaling(T);
382 
383  Matrix4<T> transpose() const;
384  Matrix4<T> inverse() const;
385 
386  //Returns the upper 3x3 Matrix of the current 4x4 Matrix
387  Matrix3<T> upper3x3() const;
388  Matrix3<T> upper3x3Transposed() const;
389 
390  inline Vector4<T> getRow(const int row) const;
391  inline Vector4<T> getColumn(const int column) const;
392 
393  //Converts to QMatix4x4 (for use in OpenGL or other Qt classes)
394  inline QMatrix4x4 convertToQMatrix() const;
395 
396  inline void print(void) const;
397  QString toString(int fieldWidth=0, char format='g', int precision=-1) const {return QString("[[%1, %2, %3, %4], [%5, %6, %7, %8], [%9, %10, %11, %12], [%13, %14, %15, %16]]")
398  .arg(r[0], fieldWidth, format, precision)
399  .arg(r[1], fieldWidth, format, precision)
400  .arg(r[2], fieldWidth, format, precision)
401  .arg(r[3], fieldWidth, format, precision)
402  .arg(r[4], fieldWidth, format, precision)
403  .arg(r[5], fieldWidth, format, precision)
404  .arg(r[6], fieldWidth, format, precision)
405  .arg(r[7], fieldWidth, format, precision)
406  .arg(r[8], fieldWidth, format, precision)
407  .arg(r[9], fieldWidth, format, precision)
408  .arg(r[10], fieldWidth, format, precision)
409  .arg(r[11], fieldWidth, format, precision)
410  .arg(r[12], fieldWidth, format, precision)
411  .arg(r[13], fieldWidth, format, precision)
412  .arg(r[14], fieldWidth, format, precision)
413  .arg(r[15], fieldWidth, format, precision);}
414 
415  T r[16];
416 };
417 
419 template<class T> QDataStream& operator<<(QDataStream& out, const Vector2<T>& v) {out << v[0] << v[1]; return out;}
420 template<class T> QDataStream& operator<<(QDataStream& out, const Vector3<T>& v) {out << v[0] << v[1] << v[2]; return out;}
421 template<class T> QDataStream& operator<<(QDataStream& out, const Vector4<T>& v) {out << v[0] << v[1] << v[2] << v[3]; return out;}
422 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;}
423 template<class T> QDataStream& operator<<(QDataStream& out, const Matrix3<T>& m) {out << m[0] << m[1] << m[2] << m[3] << m[4] << m[5] << m[6] << m[7] << m[8]; return out;}
424 
425 template<class T> QDataStream& operator>>(QDataStream& in, Vector2<T>& v) {in >> v[0] >> v[1]; return in;}
426 template<class T> QDataStream& operator>>(QDataStream& in, Vector3<T>& v) {in >> v[0] >> v[1] >> v[2]; return in;}
427 template<class T> QDataStream& operator>>(QDataStream& in, Vector4<T>& v) {in >> v[0] >> v[1] >> v[2] >> v[3]; return in;}
428 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;}
429 template<class T> QDataStream& operator>>(QDataStream& in, Matrix3<T>& m) {in >> m[0] >> m[1] >> m[2] >> m[3] >> m[4] >> m[5] >> m[6] >> m[7] >> m[8]; return in;}
430 
432 template <class T> inline bool fuzzyEquals(T a, T b, T eps = std::numeric_limits<T>::epsilon())
433 {
434  if(a == b) return true;
435  if(((a+eps) < b) || ((a-eps) > b)) return false;
436  return true;
437 }
438 
440 
441 template<class T> Vector2<T>::Vector2() {}
442 
443 template<class T> Vector2<T>::Vector2(T x)
444 {
445  v[0]=x; v[1]=x;
446 }
447 
448 template<class T> Vector2<T>::Vector2(const T* x)
449 {
450  v[0]=x[0]; v[1]=x[1];
451 }
452 
453 template <class T> template <class T2> Vector2<T>::Vector2(const Vector2<T2>& other)
454 {
455  v[0]=other[0]; v[1]=other[1];
456 }
457 
458 template<class T> Vector2<T>::Vector2(T x, T y)
459 {
460  v[0]=x; v[1]=y;
461 }
462 
463 
464 template<class T> Vector2<T>& Vector2<T>::operator=(const T* a)
465 {
466  v[0]=a[0]; v[1]=a[1];
467  return *this;
468 }
469 
470 template<class T> void Vector2<T>::set(T x, T y)
471 {
472  v[0]=x; v[1]=y;
473 }
474 
475 
476 template<class T> bool Vector2<T>::operator==(const Vector2<T>& a) const
477 {
478  return (v[0] == a.v[0] && v[1] == a.v[1]);
479 }
480 
481 template<class T> bool Vector2<T>::operator!=(const Vector2<T>& a) const
482 {
483  return (v[0] != a.v[0] || v[1] != a.v[1]);
484 }
485 
486 template<class T> const T& Vector2<T>::operator[](int x) const
487 {
488  return v[x];
489 }
490 
491 template<class T> T& Vector2<T>::operator[](int x)
492 {
493  return v[x];
494 }
495 
496 template<class T> Vector2<T>::operator const T*() const
497 {
498  return v;
499 }
500 
501 template<class T> Vector2<T>::operator T*()
502 {
503  return v;
504 }
505 
506 
507 template<class T> Vector2<T>& Vector2<T>::operator+=(const Vector2<T>& a)
508 {
509  v[0] += a.v[0]; v[1] += a.v[1];
510  return *this;
511 }
512 
513 template<class T> Vector2<T>& Vector2<T>::operator-=(const Vector2<T>& a)
514 {
515  v[0] -= a.v[0]; v[1] -= a.v[1];
516  return *this;
517 }
518 
519 template<class T> Vector2<T>& Vector2<T>::operator*=(T s)
520 {
521  v[0] *= s; v[1] *= s;
522  return *this;
523 }
524 
525 template<class T> Vector2<T>& Vector2<T>::operator*=(const Vector2<T>& b)
526 {
527  v[0] *= b.v[0]; v[1] *= b.v[1];
528  return *this;
529 }
530 
531 template<class T> Vector2<T>& Vector2<T>::operator/=(T s)
532 {
533  v[0] /= s; v[1] /= s;
534  return *this;
535 }
536 
537 template<class T> Vector2<T>& Vector2<T>::operator/=(const Vector2<T>& b)
538 {
539  v[0] /= b.v[0]; v[1] /= b.v[1];
540  return *this;
541 }
542 
543 template<class T> Vector2<T> Vector2<T>::operator-() const
544 {
545  return Vector2<T>(-v[0], -v[1]);
546 }
547 
548 template<class T> Vector2<T> Vector2<T>::operator+() const
549 {
550  return *this;
551 }
552 
553 template<class T> Vector2<T> Vector2<T>::operator+(const Vector2<T>& b) const
554 {
555  return Vector2<T>(v[0] + b.v[0], v[1] + b.v[1]);
556 }
557 
558 template<class T> Vector2<T> Vector2<T>::operator-(const Vector2<T>& b) const
559 {
560  return Vector2<T>(v[0] - b.v[0], v[1] - b.v[1]);
561 }
562 
563 template<class T> Vector2<T> Vector2<T>::operator*(T s) const
564 {
565  return Vector2<T>(s * v[0], s * v[1]);
566 }
567 
568 template<class T> Vector2<T> Vector2<T>::operator*(const Vector2<T>& b) const
569 {
570  return Vector2<T>(v[0] * b.v[0], v[1] * b.v[1]);
571 }
572 
573 template<class T> Vector2<T> Vector2<T>::operator/(T s) const
574 {
575  return Vector2<T>(v[0]/s, v[1]/s);
576 }
577 
578 template<class T> Vector2<T> Vector2<T>::operator/(const Vector2<T>& b) const
579 {
580  return Vector2<T>(v[0]/b.v[0], v[1]/b.v[1]);
581 }
582 
583 template<class T> Vector2<T> Vector2<T>::min(const Vector2<T>& b) const
584 {
585  return Vector2<T>(std::min(v[0],b.v[0]), std::min(v[1],b.v[1]));
586 }
587 
588 template<class T> Vector2<T> Vector2<T>::max(const Vector2<T>& b) const
589 {
590  return Vector2<T>(std::max(v[0],b.v[0]), std::max(v[1],b.v[1]));
591 }
592 
593 template<class T> Vector2<T> Vector2<T>::clamp(const Vector2<T>& low, const Vector2<T>& high) const
594 {
595  return this->max(low).min(high);
596 }
597 
598 template<class T> T Vector2<T>::dot(const Vector2<T>& b) const
599 {
600  return v[0] * b.v[0] + v[1] * b.v[1];
601 }
602 
603 
604 template<class T> T Vector2<T>::length() const
605 {
606  return (T) std::sqrt(v[0] * v[0] + v[1] * v[1]);
607 }
608 
609 template<class T> T Vector2<T>::lengthSquared() const
610 {
611  return v[0] * v[0] + v[1] * v[1];
612 }
613 
614 template<class T> void Vector2<T>::normalize()
615 {
616  T s = (T) 1 / std::sqrt(v[0] * v[0] + v[1] * v[1]);
617  v[0] *= s;
618  v[1] *= s;
619 }
620 
621 // template<class T>
622 // std::ostream& operator<<(std::ostream &o,const Vector2<T> &v) {
623 // return o << '[' << v[0] << ',' << v[1] << ']';
624 // }
625 
627 
628 template<class T> Vector3<T>::Vector3() {}
629 
630 //template<class T> Vector3<T>::Vector3(const Vector3& a)
631 //{
632 // v[0]=a.v[0]; v[1]=a.v[1]; v[2]=a.v[2];
633 //}
634 
635 //template<class T> template<class T2> Vector3<T>::Vector3(const Vector3<T2>& a)
636 //{
637 // v[0]=a.v[0]; v[1]=a.v[1]; v[2]=a.v[2];
638 //}
639 
640 template<class T> Vector3<T>::Vector3(T x)
641 {
642  v[0]=x; v[1]=x; v[2]=x;
643 }
644 
645 template<class T> Vector3<T>::Vector3(const T* x)
646 {
647  v[0]=x[0]; v[1]=x[1]; v[2]=x[2];
648 }
649 
650 template<class T> Vector3<T>::Vector3(T x, T y, T z)
651 {
652  v[0]=x; v[1]=y; v[2]=z;
653 }
654 
655 //template<class T> Vector3<T>& Vector3<T>::operator=(const Vector3& a)
656 //{
657 // v[0]=a.v[0]; v[1]=a.v[1]; v[2]=a.v[2];
658 // return *this;
659 //}
660 
661 //template<class T> template <class T2> Vector3<T>& Vector3<T>::operator=(const Vector3<T2>& a)
662 //{
663 // v[0]=a.v[0]; v[1]=a.v[1]; v[2]=a.v[2];
664 // return *this;
665 //}
666 
667 template<class T> Vector3<T>& Vector3<T>::operator=(const T* a)
668 {
669  v[0]=a[0]; v[1]=a[1]; v[2]=a[2];
670  return *this;
671 }
672 
673 template<class T> void Vector3<T>::set(T x, T y, T z)
674 {
675  v[0]=x; v[1]=y; v[2]=z;
676 }
677 
678 
679 template<class T> bool Vector3<T>::operator==(const Vector3<T>& a) const
680 {
681  return (v[0] == a.v[0] && v[1] == a.v[1] && v[2] == a.v[2]);
682 }
683 
684 template<class T> bool Vector3<T>::operator!=(const Vector3<T>& a) const
685 {
686  return (v[0] != a.v[0] || v[1] != a.v[1] || v[2] != a.v[2]);
687 }
688 
689 template<class T> bool Vector3<T>::fuzzyEquals(const Vector3<T>& a, const T eps) const
690 {
691  return ::fuzzyEquals(v[0], a.v[0], eps) && ::fuzzyEquals(v[1], a.v[1], eps) && ::fuzzyEquals(v[2], a.v[2], eps);
692 }
693 
694 template<class T> T& Vector3<T>::operator[](int x)
695 {
696  return v[x];
697 }
698 
699 template<class T> const T& Vector3<T>::operator[](int x) const
700 {
701  return v[x];
702 }
703 
704 template<class T> Vector3<T>::operator const T*() const
705 {
706  return v;
707 }
708 
709 template<class T> Vector3<T>::operator T*()
710 {
711  return v;
712 }
713 
714 template<class T> void Vector3<T>::operator+=(const Vector3<T>& a)
715 {
716  v[0] += a.v[0]; v[1] += a.v[1]; v[2] += a.v[2];
717 }
718 
719 template<class T> void Vector3<T>::operator-=(const Vector3<T>& a)
720 {
721  v[0] -= a.v[0]; v[1] -= a.v[1]; v[2] -= a.v[2];
722 }
723 
724 template<class T> void Vector3<T>::operator*=(T s)
725 {
726  v[0] *= s; v[1] *= s; v[2] *= s;
727 }
728 
729 template<class T> void Vector3<T>::operator/=(T s)
730 {
731  v[0] /= s; v[1] /= s; v[2] /= s;
732 }
733 
734 template<class T> Vector3<T> Vector3<T>::operator-() const
735 {
736  return Vector3<T>(-v[0], -v[1], -v[2]);
737 }
738 
739 template<class T> Vector3<T> Vector3<T>::operator+() const
740 {
741  return *this;
742 }
743 
744 template<class T> Vector3<T> Vector3<T>::operator+(const Vector3<T>& b) const
745 {
746  return Vector3<T>(v[0] + b.v[0], v[1] + b.v[1], v[2] + b.v[2]);
747 }
748 
749 template<class T> Vector3<T> Vector3<T>::operator-(const Vector3<T>& b) const
750 {
751  return Vector3<T>(v[0] - b.v[0], v[1] - b.v[1], v[2] - b.v[2]);
752 }
753 
754 template<class T> Vector3<T> Vector3<T>::operator*(T s) const
755 {
756  return Vector3<T>(s * v[0], s * v[1], s * v[2]);
757 }
758 
759 template<class T> Vector3<T> Vector3<T>::operator/(T s) const
760 {
761  return Vector3<T>(v[0]/s, v[1]/s, v[2]/s);
762 }
763 
764 
765 template<class T> T Vector3<T>::dot(const Vector3<T>& b) const
766 {
767  return v[0] * b.v[0] + v[1] * b.v[1] + v[2] * b.v[2];
768 }
769 
770 
771 // cross product
772 template<class T> Vector3<T> Vector3<T>::operator^(const Vector3<T>& b) const
773 {
774  return Vector3<T>(v[1] * b.v[2] - v[2] * b.v[1],
775  v[2] * b.v[0] - v[0] * b.v[2],
776  v[0] * b.v[1] - v[1] * b.v[0]);
777 }
778 
779 // Angle in radian between two vectors
780 template<class T> T Vector3<T>::angle(const Vector3<T>& b) const
781 {
782  const T cosAngle = dot(b)/std::sqrt(lengthSquared()*b.lengthSquared());
783  return cosAngle>=1 ? 0 : (cosAngle<=-1 ? M_PI : std::acos(cosAngle));
784 }
785 
786 // Angle in radian between two normalized vectors
787 template<class T> T Vector3<T>::angleNormalized(const Vector3<T>& b) const
788 {
789  const T cosAngle = dot(b);
790  return cosAngle>=1 ? 0 : (cosAngle<=-1 ? M_PI : std::acos(cosAngle));
791 }
792 
793 template<class T> T Vector3<T>::length() const
794 {
795  return (T) std::sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
796 }
797 
798 template<class T> T Vector3<T>::lengthSquared() const
799 {
800  return v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
801 }
802 
803 template<class T> void Vector3<T>::normalize()
804 {
805  T s = (T) (1. / std::sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]));
806  v[0] *= s;
807  v[1] *= s;
808  v[2] *= s;
809 }
810 
811 template<class T> void Vector3<T>::transfo4d(const Mat4d& m)
812 {
813  const T v0 = v[0];
814  const T v1 = v[1];
815  v[0]=m.r[0]*v0 + m.r[4]*v1 + m.r[8]*v[2] + m.r[12];
816  v[1]=m.r[1]*v0 + m.r[5]*v1 + m.r[9]*v[2] + m.r[13];
817  v[2]=m.r[2]*v0 + m.r[6]*v1 + m.r[10]*v[2] + m.r[14];
818 }
819 
820 template<class T> void Vector3<T>::transfo4d(const Mat4f& m)
821 {
822  const T v0 = v[0];
823  const T v1 = v[1];
824  v[0]=m.r[0]*v0 + m.r[4]*v1 + m.r[8]*v[2] + m.r[12];
825  v[1]=m.r[1]*v0 + m.r[5]*v1 + m.r[9]*v[2] + m.r[13];
826  v[2]=m.r[2]*v0 + m.r[6]*v1 + m.r[10]*v[2] + m.r[14];
827 }
828 
829 template<class T> Vec3f Vector3<T>::toVec3f() const
830 {
831  return Vec3f(v[0],v[1],v[2]);
832 }
833 
834 template<class T> Vec3d Vector3<T>::toVec3d() const
835 {
836  return Vec3d(v[0],v[1],v[2]);
837 }
838 
839 // Return latitude in rad
840 template<class T> T Vector3<T>::latitude() const
841 {
842  return std::asin(v[2]/length());
843 }
844 
845 // Return longitude in rad
846 template<class T> T Vector3<T>::longitude() const
847 {
848  return std::atan2(v[1],v[0]);
849 }
850 
851 
853 
854 template<class T> Vector4<T>::Vector4() {}
855 
856 template<class T> Vector4<T>::Vector4(const T* x)
857 {
858  v[0]=x[0]; v[1]=x[1]; v[2]=x[2]; v[3]=x[3];
859 }
860 
861 template<class T> Vector4<T>::Vector4(const Vector3<T>& a)
862 {
863  v[0]=a.v[0]; v[1]=a.v[1]; v[2]=a.v[2]; v[3]=1;
864 }
865 
866 template<class T> Vector4<T>::Vector4(T x, T y, T z)
867 {
868  v[0]=x; v[1]=y; v[2]=z; v[3]=1;
869 }
870 
871 template<class T> Vector4<T>::Vector4(T x, T y, T z, T a)
872 {
873  v[0]=x; v[1]=y; v[2]=z; v[3]=a;
874 }
875 
876 template<class T> Vector4<T>& Vector4<T>::operator=(const Vector3<T>& a)
877 {
878  v[0]=a.v[0]; v[1]=a.v[1]; v[2]=a.v[2]; v[3]=1;
879  return *this;
880 }
881 
882 template<class T> Vector4<T>& Vector4<T>::operator=(const T* a)
883 {
884  v[0]=a[0]; v[1]=a[1]; v[2]=a[2]; v[3]=a[3];
885  return *this;
886 }
887 
888 template<class T> void Vector4<T>::set(T x, T y, T z, T a)
889 {
890  v[0]=x; v[1]=y; v[2]=z; v[3]=a;
891 }
892 
893 template<class T> bool Vector4<T>::operator==(const Vector4<T>& a) const
894 {
895  return (v[0] == a.v[0] && v[1] == a.v[1] && v[2] == a.v[2] && v[3] == a.v[3]);
896 }
897 
898 template<class T> bool Vector4<T>::operator!=(const Vector4<T>& a) const
899 {
900  return (v[0] != a.v[0] || v[1] != a.v[1] || v[2] != a.v[2] || v[3] != a.v[3]);
901 }
902 
903 template<class T> T& Vector4<T>::operator[](int x)
904 {
905  return v[x];
906 }
907 
908 template<class T> const T& Vector4<T>::operator[](int x) const
909 {
910  return v[x];
911 }
912 
913 template<class T> Vector4<T>::operator T*()
914 {
915  return v;
916 }
917 
918 template<class T> Vector4<T>::operator const T*() const
919 {
920  return v;
921 }
922 
923 template<class T> void Vector4<T>::operator+=(const Vector4<T>& a)
924 {
925  v[0] += a.v[0]; v[1] += a.v[1]; v[2] += a.v[2]; v[3] += a.v[3];
926 }
927 
928 template<class T> void Vector4<T>::operator-=(const Vector4<T>& a)
929 {
930  v[0] -= a.v[0]; v[1] -= a.v[1]; v[2] -= a.v[2]; v[3] -= a.v[3];
931 }
932 
933 template<class T> void Vector4<T>::operator*=(T s)
934 {
935  v[0] *= s; v[1] *= s; v[2] *= s; v[3] *= s;
936 }
937 
938 template<class T> void Vector4<T>::operator/=(T s)
939 {
940  v[0] /= s; v[1] /= s; v[2] /= s; v[3] /= s;
941 }
942 
943 template<class T> Vector4<T> Vector4<T>::operator-() const
944 {
945  return Vector4<T>(-v[0], -v[1], -v[2], -v[3]);
946 }
947 
948 template<class T> Vector4<T> Vector4<T>::operator+() const
949 {
950  return *this;
951 }
952 
953 template<class T> Vector4<T> Vector4<T>::operator+(const Vector4<T>& b) const
954 {
955  return Vector4<T>(v[0] + b.v[0], v[1] + b.v[1], v[2] + b.v[2], v[3] + b.v[3]);
956 }
957 
958 template<class T> Vector4<T> Vector4<T>::operator-(const Vector4<T>& b) const
959 {
960  return Vector4<T>(v[0] - b.v[0], v[1] - b.v[1], v[2] - b.v[2], v[3] - b.v[3]);
961 }
962 
963 template<class T> Vector4<T> Vector4<T>::operator*(T s) const
964 {
965  return Vector4<T>(s * v[0], s * v[1], s * v[2], s * v[3]);
966 }
967 
968 template<class T> Vector4<T> Vector4<T>::operator/(T s) const
969 {
970  return Vector4<T>(v[0]/s, v[1]/s, v[2]/s, v[3]/s);
971 }
972 
973 template<class T> T Vector4<T>::dot(const Vector4<T>& b) const
974 {
975  return v[0] * b.v[0] + v[1] * b.v[1] + v[2] * b.v[2] + v[3] * b.v[3];
976 }
977 
978 template<class T> T Vector4<T>::length() const
979 {
980  return (T) std::sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3]);
981 }
982 
983 template<class T> T Vector4<T>::lengthSquared() const
984 {
985  return v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3];
986 }
987 
988 template<class T> void Vector4<T>::normalize()
989 {
990  T s = (T) (1. / std::sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3]));
991  v[0] *= s;
992  v[1] *= s;
993  v[2] *= s;
994  v[3] *= s;
995 }
996 
997 template<class T> void Vector4<T>::transfo4d(const Mat4d& m)
998 {
999  (*this)=m*(*this);
1000 }
1001 /*
1002 template<class T>
1003 std::ostream& operator<<(std::ostream &o,const Vector4<T> &v) {
1004  return o << '[' << v[0] << ',' << v[1] << ',' << v[2] << ',' << v[3] << ']';
1005 }*/
1006 
1007 
1009 
1010 template<class T> Matrix3<T>::Matrix3() {}
1011 
1012 template<class T> Matrix3<T>::Matrix3(const T* m)
1013 {
1014  memcpy(r,m,sizeof(T)*9);
1015 }
1016 
1017 template<class T> Matrix3<T>::Matrix3(const Vector3<T>& v0, const Vector3<T>& v1, const Vector3<T>& v2)
1018 {
1019  r[0] = v0.v[0]; r[3] = v1.v[0]; r[6] = v2.v[0];
1020  r[1] = v0.v[1]; r[4] = v1.v[1]; r[7] = v2.v[1];
1021  r[2] = v0.v[2]; r[5] = v1.v[2]; r[8] = v2.v[2];
1022 }
1023 
1024 template<class T> Matrix3<T>::Matrix3(T a, T b, T c, T d, T e, T f, T g, T h, T i)
1025 {
1026  r[0] = a; r[3] = d; r[6] = g;
1027  r[1] = b; r[4] = e; r[7] = h;
1028  r[2] = c; r[5] = f; r[8] = i;
1029 }
1030 
1031 template<class T> void Matrix3<T>::set(T a, T b, T c, T d, T e, T f, T g, T h, T i)
1032 {
1033  r[0] = a; r[3] = d; r[6] = g;
1034  r[1] = b; r[4] = e; r[7] = h;
1035  r[2] = c; r[5] = f; r[8] = i;
1036 }
1037 
1038 template<class T> T& Matrix3<T>::operator[](int n)
1039 {
1040  return r[n];
1041 }
1042 
1043 template<class T> Matrix3<T>::operator T*()
1044 {
1045  return r;
1046 }
1047 
1048 template<class T> Matrix3<T>::operator const T*() const
1049 {
1050  return r;
1051 }
1052 
1053 template<class T> Matrix3<T> Matrix3<T>::identity()
1054 {
1055  return Matrix3<T>(1, 0, 0,
1056  0, 1, 0,
1057  0, 0, 1);
1058 }
1059 
1060 // multiply column vector by a 3x3 matrix
1061 template<class T> Vector3<T> Matrix3<T>::operator*(const Vector3<T>& a) const
1062 {
1063  return Vector3<T>(r[0]*a.v[0] + r[3]*a.v[1] + r[6]*a.v[2],
1064  r[1]*a.v[0] + r[4]*a.v[1] + r[7]*a.v[2],
1065  r[2]*a.v[0] + r[5]*a.v[1] + r[8]*a.v[2]);
1066 }
1067 
1068 template<class T> Matrix3<T> Matrix3<T>::transpose() const
1069 {
1070  return Matrix3<T>(r[0], r[3], r[6],
1071  r[1], r[4], r[7],
1072  r[2], r[5], r[8]);
1073 }
1074 
1075 template<class T> Matrix3<T> Matrix3<T>::operator*(const Matrix3<T>& a) const
1076 {
1077 #define MATMUL(R, C) (r[R] * a.r[C] + r[R+3] * a.r[C+1] + r[R+6] * a.r[C+2])
1078  return Matrix3<T>(MATMUL(0,0), MATMUL(1,0), MATMUL(2,0),
1079  MATMUL(0,3), MATMUL(1,3), MATMUL(2,3),
1080  MATMUL(0,6), MATMUL(1,6), MATMUL(2,6));
1081 #undef MATMUL
1082 }
1083 
1084 
1085 template<class T> Matrix3<T> Matrix3<T>::operator+(const Matrix3<T>& a) const
1086 {
1087  return Matrix3<T>(r[0]+a.r[0], r[1]+a.r[1], r[2]+a.r[2],
1088  r[3]+a.r[3], r[4]+a.r[4], r[5]+a.r[5],
1089  r[6]+a.r[6], r[7]+a.r[7], r[8]+a.r[8]);
1090 }
1091 
1092 template<class T> Matrix3<T> Matrix3<T>::operator-(const Matrix3<T>& a) const
1093 {
1094  return Matrix3<T>(r[0]-a.r[0], r[1]-a.r[1], r[2]-a.r[2],
1095  r[3]-a.r[3], r[4]-a.r[4], r[5]-a.r[5],
1096  r[6]-a.r[6], r[7]-a.r[7], r[8]-a.r[8]);
1097 }
1098 
1099 /*
1100  * Code taken from 4x4 version below
1101  * Added by Andrei Borza
1102  */
1103 template<class T> Matrix3<T> Matrix3<T>::inverse() const
1104 {
1105  const T * m = r;
1106  T out[9];
1107 
1108  /* NB. OpenGL Matrices are COLUMN major. */
1109  #define SWAP_ROWS(a, b) { T *_tmp = a; (a)=(b); (b)=_tmp; }
1110  #define MAT(m,r,c) (m)[(c)*3+(r)]
1111 
1112  T wtmp[3][6];
1113  T m0, m1, m2, s;
1114  T *r0, *r1, *r2;
1115 
1116  r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2];
1117 
1118  r0[0] = MAT(m, 0, 0), r0[1] = MAT(m, 0, 1), r0[2] = MAT(m, 0, 2),
1119  r0[3] = 1.0, r0[4] = r0[5] = 0.0,
1120  r1[0] = MAT(m, 1, 0), r1[1] = MAT(m, 1, 1), r1[2] = MAT(m, 1, 2),
1121  r1[4] = 1.0, r1[3] = r1[5] = 0.0,
1122  r2[0] = MAT(m, 2, 0), r2[1] = MAT(m, 2, 1), r2[2] = MAT(m, 2, 2),
1123  r2[5] = 1.0, r2[3] = r2[4] = 0.0;
1124 
1125  /* choose pivot - or die */
1126  if (fabs(r2[0]) > fabs(r1[0]))
1127  SWAP_ROWS(r2, r1);
1128  if (fabs(r1[0]) > fabs(r0[0]))
1129  SWAP_ROWS(r1, r0);
1130  if (0.0 == r0[0])
1131  return Matrix3<T>();
1132 
1133  /* eliminate first variable */
1134  m1 = r1[0] / r0[0];
1135  m2 = r2[0] / r0[0];
1136  s = r0[1];
1137  r1[1] -= m1 * s;
1138  r2[1] -= m2 * s;
1139  s = r0[2];
1140  r1[2] -= m1 * s;
1141  r2[2] -= m2 * s;
1142  s = r0[3];
1143  if (s != 0.0)
1144  {
1145  r1[3] -= m1 * s;
1146  r2[3] -= m2 * s;
1147  }
1148  s = r0[4];
1149  if (s != 0.0)
1150  {
1151  r1[4] -= m1 * s;
1152  r2[4] -= m2 * s;
1153  }
1154  s = r0[5];
1155  if (s != 0.0)
1156  {
1157  r1[5] -= m1 * s;
1158  r2[5] -= m2 * s;
1159  }
1160 
1161  /* choose pivot - or die */
1162  if (fabs(r2[1]) > fabs(r1[1]))
1163  SWAP_ROWS(r2, r1);
1164  if (0.0 == r1[1])
1165  return Matrix3<T>();
1166 
1167  /* eliminate second variable */
1168  m2 = r2[1] / r1[1];
1169  r2[2] -= m2 * r1[2];
1170  s = r1[3];
1171  if (0.0 != s)
1172  {
1173  r2[3] -= m2 * s;
1174  }
1175  s = r1[4];
1176  if (0.0 != s)
1177  {
1178  r2[4] -= m2 * s;
1179  }
1180  s = r1[5];
1181  if (0.0 != s)
1182  {
1183  r2[5] -= m2 * s;
1184  }
1185 
1186  s = 1.0 / r2[2]; /* now back substitute row 2 */
1187  r2[3] *= s;
1188  r2[4] *= s;
1189  r2[5] *= s;
1190 
1191  m1 = r1[2]; /* now back substitute row 1 */
1192  s = 1.0 / r1[1];
1193  r1[3] = s * (r1[3] - r2[3] * m1), r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1);
1194  m0 = r0[2];
1195  r0[3] -= r2[3] * m0, r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0;
1196 
1197  m0 = r0[1]; /* now back substitute row 0 */
1198  s = 1.0 / r0[0];
1199  r0[3] = s * (r0[3] - r1[3] * m0), r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0);
1200 
1201  MAT(out, 0, 0) = r0[3]; MAT(out, 0, 1) = r0[4]; MAT(out, 0, 2) = r0[5];
1202  MAT(out, 1, 0) = r1[3]; MAT(out, 1, 1) = r1[4]; MAT(out, 1, 2) = r1[5];
1203  MAT(out, 2, 0) = r2[3]; MAT(out, 2, 1) = r2[4]; MAT(out, 2, 2) = r2[5];
1204 
1205  return Matrix3<T>(out);
1206 
1207  #undef MAT
1208  #undef SWAP_ROWS
1209 }
1210 
1211 template<class T> void Matrix3<T>::print(void) const
1212 {
1213  printf("[%5.2lf %5.2lf %5.2lf]\n"
1214  "[%5.2lf %5.2lf %5.2lf]\n"
1215  "[%5.2lf %5.2lf %5.2lf]\n",
1216  r[0],r[3],r[6],
1217  r[1],r[4],r[7],
1218  r[2],r[5],r[8]);
1219 }
1220 
1222 
1223 template<class T> Matrix4<T>::Matrix4() {}
1224 
1225 template<class T> Matrix4<T>::Matrix4(const T* m)
1226 {
1227  memcpy(r,m,sizeof(T)*16);
1228 }
1229 
1230 template<class T> Matrix4<T>::Matrix4(const Vector4<T>& v0,
1231  const Vector4<T>& v1,
1232  const Vector4<T>& v2,
1233  const Vector4<T>& v3)
1234 {
1235  r[0] = v0.v[0];
1236  r[1] = v0.v[1];
1237  r[2] = v0.v[2];
1238  r[3] = v0.v[3];
1239  r[4] = v1.v[0];
1240  r[5] = v1.v[1];
1241  r[6] = v1.v[2];
1242  r[7] = v1.v[3];
1243  r[8] = v2.v[0];
1244  r[9] = v2.v[1];
1245  r[10] = v2.v[2];
1246  r[11] = v2.v[3];
1247  r[12] = v3.v[0];
1248  r[13] = v3.v[1];
1249  r[14] = v3.v[2];
1250  r[15] = v3.v[3];
1251 }
1252 
1253 
1254 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)
1255 {
1256  r[0]=a; r[1]=b; r[2]=c; r[3]=d; r[4]=e; r[5]=f; r[6]=g; r[7]=h;
1257  r[8]=i; r[9]=j; r[10]=k; r[11]=l; r[12]=m; r[13]=n; r[14]=o; r[15]=p;
1258 }
1259 
1260 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)
1261 {
1262  r[0]=a; r[1]=b; r[2]=c; r[3]=d; r[4]=e; r[5]=f; r[6]=g; r[7]=h;
1263  r[8]=i; r[9]=j; r[10]=k; r[11]=l; r[12]=m; r[13]=n; r[14]=o; r[15]=p;
1264 }
1265 
1266 template<class T> T& Matrix4<T>::operator[](int n)
1267 {
1268  return r[n];
1269 }
1270 
1271 template<class T> Matrix4<T>::operator T*()
1272 {
1273  return r;
1274 }
1275 
1276 template<class T> Matrix4<T>::operator const T*() const
1277 {
1278  return r;
1279 }
1280 
1281 template<class T> Matrix4<T> Matrix4<T>::identity()
1282 {
1283  return Matrix4<T>(1, 0, 0, 0,
1284  0, 1, 0, 0,
1285  0, 0, 1, 0,
1286  0, 0, 0, 1);
1287 }
1288 
1289 
1290 template<class T> Matrix4<T> Matrix4<T>::translation(const Vector3<T>& a)
1291 {
1292  return Matrix4<T>(1 , 0 , 0 , 0,
1293  0 , 1 , 0 , 0,
1294  0 , 0 , 1 , 0,
1295  a.v[0], a.v[1], a.v[2], 1);
1296 }
1297 
1298 
1299 template<class T> Matrix4<T> Matrix4<T>::rotation(const Vector3<T>& axis, T angle)
1300 {
1301  Vector3<T> a(axis);
1302  a.normalize();
1303  const T c = (T) cos(angle);
1304  const T s = (T) sin(angle);
1305  const T d = 1-c;
1306  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,
1307  a[0]*a[1]*d-a[2]*s, a[1]*a[1]*d+c , a[1]*a[2]*d+a[0]*s, 0,
1308  a[0]*a[2]*d+a[1]*s, a[1]*a[2]*d-a[0]*s, a[2]*a[2]*d+c , 0,
1309  0 , 0, 0, 1);
1310 }
1311 
1312 template<class T> Matrix4<T> Matrix4<T>::xrotation(T angle)
1313 {
1314  T c = (T) cos(angle);
1315  T s = (T) sin(angle);
1316 
1317  return Matrix4<T>(1, 0, 0, 0,
1318  0, c, s, 0,
1319  0,-s, c, 0,
1320  0, 0, 0, 1);
1321 }
1322 
1323 
1324 template<class T> Matrix4<T> Matrix4<T>::yrotation(T angle)
1325 {
1326  T c = (T) cos(angle);
1327  T s = (T) sin(angle);
1328 
1329  return Matrix4<T>(c, 0,-s, 0,
1330  0, 1, 0, 0,
1331  s, 0, c, 0,
1332  0, 0, 0, 1);
1333 }
1334 
1335 
1336 template<class T> Matrix4<T> Matrix4<T>::zrotation(T angle)
1337 {
1338  T c = (T) cos(angle);
1339  T s = (T) sin(angle);
1340 
1341  return Matrix4<T>(c , s, 0, 0,
1342  -s, c, 0, 0,
1343  0 , 0, 1, 0,
1344  0 , 0, 0, 1);
1345 }
1346 
1347 
1348 template<class T> Matrix4<T> Matrix4<T>::scaling(const Vector3<T>& s)
1349 {
1350  return Matrix4<T>(s[0], 0 , 0 , 0,
1351  0 ,s[1], 0 , 0,
1352  0 , 0 ,s[2], 0,
1353  0 , 0 , 0 , 1);
1354 }
1355 
1356 
1357 template<class T> Matrix4<T> Matrix4<T>::scaling(T scale)
1358 {
1359  return scaling(Vector3<T>(scale, scale, scale));
1360 }
1361 
1362 // multiply column vector by a 4x4 matrix in homogeneous coordinate (use a[3]=1)
1363 template<class T> Vector3<T> Matrix4<T>::operator*(const Vector3<T>& a) const
1364 {
1365  return Vector3<T>(r[0]*a.v[0] + r[4]*a.v[1] + r[8]*a.v[2] + r[12],
1366  r[1]*a.v[0] + r[5]*a.v[1] + r[9]*a.v[2] + r[13],
1367  r[2]*a.v[0] + r[6]*a.v[1] + r[10]*a.v[2] + r[14]);
1368 }
1369 
1370 template<class T> Vector3<T> Matrix4<T>::multiplyWithoutTranslation(const Vector3<T>& a) const
1371 {
1372  return Vector3<T>(r[0]*a.v[0] + r[4]*a.v[1] + r[8]*a.v[2],
1373  r[1]*a.v[0] + r[5]*a.v[1] + r[9]*a.v[2],
1374  r[2]*a.v[0] + r[6]*a.v[1] + r[10]*a.v[2]);
1375 }
1376 
1377 // multiply column vector by a 4x4 matrix in homogeneous coordinate (considere a[3]=1)
1378 template<class T> Vector4<T> Matrix4<T>::operator*(const Vector4<T>& a) const
1379 {
1380  return Vector4<T>(r[0]*a.v[0] + r[4]*a.v[1] + r[8]*a.v[2] + r[12]*a.v[3],
1381  r[1]*a.v[0] + r[5]*a.v[1] + r[9]*a.v[2] + r[13]*a.v[3],
1382  r[2]*a.v[0] + r[6]*a.v[1] + r[10]*a.v[2] + r[14]*a.v[3]);
1383 }
1384 
1385 template<class T> void Matrix4<T>::transfo(Vector3<T>& a) const
1386 {
1387  a.set(r[0]*a.v[0] + r[4]*a.v[1] + r[8]*a.v[2] + r[12],
1388  r[1]*a.v[0] + r[5]*a.v[1] + r[9]*a.v[2] + r[13],
1389  r[2]*a.v[0] + r[6]*a.v[1] + r[10]*a.v[2] + r[14]);
1390 }
1391 
1392 template<class T> Matrix4<T> Matrix4<T>::transpose() const
1393 {
1394  return Matrix4<T>(r[0], r[4], r[8], r[12],
1395  r[1], r[5], r[9], r[13],
1396  r[2], r[6], r[10], r[14],
1397  r[3], r[7], r[11], r[15]);
1398 }
1399 
1400 template<class T> Matrix4<T> Matrix4<T>::operator*(const Matrix4<T>& a) const
1401 {
1402 #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])
1403  return Matrix4<T>(MATMUL(0,0) , MATMUL(1,0) , MATMUL(2,0) , MATMUL(3,0) ,
1404  MATMUL(0,4) , MATMUL(1,4) , MATMUL(2,4) , MATMUL(3,4) ,
1405  MATMUL(0,8) , MATMUL(1,8) , MATMUL(2,8) , MATMUL(3,8) ,
1406  MATMUL(0,12), MATMUL(1,12), MATMUL(2,12), MATMUL(3,12));
1407 #undef MATMUL
1408 }
1409 
1410 
1411 template<class T> Matrix4<T> Matrix4<T>::operator+(const Matrix4<T>& a) const
1412 {
1413  return Matrix4<T>(r[0]+a.r[0] , r[1]+a.r[1] , r[2]+a.r[2] , r[3]+a.r[3] ,
1414  r[4]+a.r[4] , r[5]+a.r[5] , r[6]+a.r[6] , r[7]+a.r[7] ,
1415  r[8]+a.r[8] , r[9]+a.r[9] , r[10]+a.r[10], r[11]+a.r[11],
1416  r[12]+a.r[12], r[13]+a.r[13], r[14]+a.r[14], r[15]+a.r[15]);
1417 }
1418 
1419 template<class T> Matrix4<T> Matrix4<T>::operator-(const Matrix4<T>& a) const
1420 {
1421  return Matrix4<T>(r[0]-a.r[0] , r[1]-a.r[1] , r[2]-a.r[2] , r[3]-a.r[3] ,
1422  r[4]-a.r[4] , r[5]-a.r[5] , r[6]-a.r[6] , r[7]-a.r[7] ,
1423  r[8]-a.r[8] , r[9]-a.r[9] , r[10]-a.r[10], r[11]-a.r[11],
1424  r[12]-a.r[12], r[13]-a.r[13], r[14]-a.r[14], r[15]-a.r[15]);
1425 }
1426 
1427 /*
1428  * Code ripped from the GLU library
1429  * Compute inverse of 4x4 transformation matrix.
1430  * Code contributed by Jacques Leroy jle@star.be
1431  * Return zero matrix on failure (singular matrix)
1432  */
1433 template<class T> Matrix4<T> Matrix4<T>::inverse() const
1434 {
1435  const T * m = r;
1436  T out[16];
1437 
1438  /* NB. OpenGL Matrices are COLUMN major. */
1439  #define SWAP_ROWS(a, b) { T *_tmp = a; (a)=(b); (b)=_tmp; }
1440  #define MAT(m,r,c) (m)[(c)*4+(r)]
1441 
1442  T wtmp[4][8];
1443  T m0, m1, m2, m3, s;
1444  T *r0, *r1, *r2, *r3;
1445 
1446  r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3];
1447 
1448  r0[0] = MAT(m, 0, 0), r0[1] = MAT(m, 0, 1),
1449  r0[2] = MAT(m, 0, 2), r0[3] = MAT(m, 0, 3),
1450  r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0,
1451  r1[0] = MAT(m, 1, 0), r1[1] = MAT(m, 1, 1),
1452  r1[2] = MAT(m, 1, 2), r1[3] = MAT(m, 1, 3),
1453  r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0,
1454  r2[0] = MAT(m, 2, 0), r2[1] = MAT(m, 2, 1),
1455  r2[2] = MAT(m, 2, 2), r2[3] = MAT(m, 2, 3),
1456  r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0,
1457  r3[0] = MAT(m, 3, 0), r3[1] = MAT(m, 3, 1),
1458  r3[2] = MAT(m, 3, 2), r3[3] = MAT(m, 3, 3),
1459  r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0;
1460 
1461  /* choose pivot - or die */
1462  if (fabs(r3[0]) > fabs(r2[0]))
1463  SWAP_ROWS(r3, r2);
1464  if (fabs(r2[0]) > fabs(r1[0]))
1465  SWAP_ROWS(r2, r1);
1466  if (fabs(r1[0]) > fabs(r0[0]))
1467  SWAP_ROWS(r1, r0);
1468  if (0.0 == r0[0])
1469  return Matrix4<T>();
1470 
1471  /* eliminate first variable */
1472  m1 = r1[0] / r0[0];
1473  m2 = r2[0] / r0[0];
1474  m3 = r3[0] / r0[0];
1475  s = r0[1];
1476  r1[1] -= m1 * s;
1477  r2[1] -= m2 * s;
1478  r3[1] -= m3 * s;
1479  s = r0[2];
1480  r1[2] -= m1 * s;
1481  r2[2] -= m2 * s;
1482  r3[2] -= m3 * s;
1483  s = r0[3];
1484  r1[3] -= m1 * s;
1485  r2[3] -= m2 * s;
1486  r3[3] -= m3 * s;
1487  s = r0[4];
1488  if (s != 0.0) {
1489  r1[4] -= m1 * s;
1490  r2[4] -= m2 * s;
1491  r3[4] -= m3 * s;
1492  }
1493  s = r0[5];
1494  if (s != 0.0) {
1495  r1[5] -= m1 * s;
1496  r2[5] -= m2 * s;
1497  r3[5] -= m3 * s;
1498  }
1499  s = r0[6];
1500  if (s != 0.0) {
1501  r1[6] -= m1 * s;
1502  r2[6] -= m2 * s;
1503  r3[6] -= m3 * s;
1504  }
1505  s = r0[7];
1506  if (s != 0.0) {
1507  r1[7] -= m1 * s;
1508  r2[7] -= m2 * s;
1509  r3[7] -= m3 * s;
1510  }
1511 
1512  /* choose pivot - or die */
1513  if (fabs(r3[1]) > fabs(r2[1]))
1514  SWAP_ROWS(r3, r2);
1515  if (fabs(r2[1]) > fabs(r1[1]))
1516  SWAP_ROWS(r2, r1);
1517  if (0.0 == r1[1])
1518  return Matrix4<T>();
1519 
1520  /* eliminate second variable */
1521  m2 = r2[1] / r1[1];
1522  m3 = r3[1] / r1[1];
1523  r2[2] -= m2 * r1[2];
1524  r3[2] -= m3 * r1[2];
1525  r2[3] -= m2 * r1[3];
1526  r3[3] -= m3 * r1[3];
1527  s = r1[4];
1528  if (0.0 != s) {
1529  r2[4] -= m2 * s;
1530  r3[4] -= m3 * s;
1531  }
1532  s = r1[5];
1533  if (0.0 != s) {
1534  r2[5] -= m2 * s;
1535  r3[5] -= m3 * s;
1536  }
1537  s = r1[6];
1538  if (0.0 != s) {
1539  r2[6] -= m2 * s;
1540  r3[6] -= m3 * s;
1541  }
1542  s = r1[7];
1543  if (0.0 != s) {
1544  r2[7] -= m2 * s;
1545  r3[7] -= m3 * s;
1546  }
1547 
1548  /* choose pivot - or die */
1549  if (fabs(r3[2]) > fabs(r2[2]))
1550  SWAP_ROWS(r3, r2);
1551  if (0.0 == r2[2])
1552  return Matrix4<T>();
1553 
1554  /* eliminate third variable */
1555  m3 = r3[2] / r2[2];
1556  r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4],
1557  r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6], r3[7] -= m3 * r2[7];
1558 
1559  /* last check */
1560  if (0.0 == r3[3])
1561  return Matrix4<T>();
1562 
1563  s = 1.0 / r3[3]; /* now back substitute row 3 */
1564  r3[4] *= s;
1565  r3[5] *= s;
1566  r3[6] *= s;
1567  r3[7] *= s;
1568 
1569  m2 = r2[3]; /* now back substitute row 2 */
1570  s = 1.0 / r2[2];
1571  r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2),
1572  r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2);
1573  m1 = r1[3];
1574  r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1,
1575  r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1;
1576  m0 = r0[3];
1577  r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0,
1578  r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0;
1579 
1580  m1 = r1[2]; /* now back substitute row 1 */
1581  s = 1.0 / r1[1];
1582  r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1),
1583  r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1);
1584  m0 = r0[2];
1585  r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0,
1586  r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0;
1587 
1588  m0 = r0[1]; /* now back substitute row 0 */
1589  s = 1.0 / r0[0];
1590  r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0),
1591  r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0);
1592 
1593  MAT(out, 0, 0) = r0[4];
1594  MAT(out, 0, 1) = r0[5], MAT(out, 0, 2) = r0[6];
1595  MAT(out, 0, 3) = r0[7], MAT(out, 1, 0) = r1[4];
1596  MAT(out, 1, 1) = r1[5], MAT(out, 1, 2) = r1[6];
1597  MAT(out, 1, 3) = r1[7], MAT(out, 2, 0) = r2[4];
1598  MAT(out, 2, 1) = r2[5], MAT(out, 2, 2) = r2[6];
1599  MAT(out, 2, 3) = r2[7], MAT(out, 3, 0) = r3[4];
1600  MAT(out, 3, 1) = r3[5], MAT(out, 3, 2) = r3[6];
1601  MAT(out, 3, 3) = r3[7];
1602 
1603  return Matrix4<T>(out);
1604 
1605  #undef MAT
1606  #undef SWAP_ROWS
1607 }
1608 
1609 
1610 template<class T> Matrix3<T> Matrix4<T>::upper3x3() const
1611 {
1612  return Matrix3<T>(r[0], r[1], r[2],
1613  r[4], r[5], r[6],
1614  r[8], r[9], r[10]);
1615 }
1616 template<class T> Matrix3<T> Matrix4<T>::upper3x3Transposed() const
1617 {
1618  return Matrix3<T>(r[0], r[4], r[8],
1619  r[1], r[5], r[9],
1620  r[2], r[6], r[10]);
1621 }
1622 template<class T> Vector4<T> Matrix4<T>::getRow(const int row) const
1623 {
1624  return Vector4<T>(r[0 + row], r[4 + row], r[8 + row], r[12 + row]);
1625 }
1626 
1627 template<class T> Vector4<T> Matrix4<T>::getColumn(const int column) const
1628 {
1629  return Vector4<T>(r[0 + column * 4], r[1 + column * 4], r[2 + column * 4], r[3 + column * 4]);
1630 }
1631 
1632 template<class T> QMatrix4x4 Matrix4<T>::convertToQMatrix() const
1633 {
1634  return QMatrix4x4( r[0], r[4], r[8],r[12],
1635  r[1], r[5], r[9],r[13],
1636  r[2], r[6],r[10],r[14],
1637  r[3], r[7],r[11],r[15] );
1638 }
1639 
1640 template<class T> void Matrix4<T>::print(void) const
1641 {
1642  printf("[%5.2lf %5.2lf %5.2lf %17.12le]\n"
1643  "[%5.2lf %5.2lf %5.2lf %17.12le]\n"
1644  "[%5.2lf %5.2lf %5.2lf %17.12le]\n"
1645  "[%5.2lf %5.2lf %5.2lf %17.12le]\n\n",
1646  r[0],r[4],r[8],r[12],
1647  r[1],r[5],r[9],r[13],
1648  r[2],r[6],r[10],r[14],
1649  r[3],r[7],r[11],r[15]);
1650 }
1651 
1652 
1653 template<class T> inline
1654 T operator*(const Vector2<T>&a,const Vector2<T>&b)
1655 {
1656  return a.v[0] * b.v[0] + a.v[1] * b.v[1];
1657 }
1658 
1659 template<class T> inline
1660 T operator*(const Vector3<T>&a,const Vector3<T>&b)
1661 {
1662  return a.v[0] * b.v[0] + a.v[1] * b.v[1] + a.v[2] * b.v[2];
1663 }
1664 
1665 template<class T> inline
1666 T operator*(const Vector4<T>&a,const Vector4<T>&b)
1667 {
1668  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];
1669 }
1670 
1671 template<class T> inline
1672 Vector2<T> operator*(T s,const Vector2<T>&v)
1673 {
1674  return Vector2<T>(s*v[0],s*v[1]);
1675 }
1676 
1677 template<class T> inline
1678 Vector3<T> operator*(T s,const Vector3<T>&v)
1679 {
1680  return Vector3<T>(s*v[0],s*v[1],s*v[2]);
1681 }
1682 
1683 template<class T> inline
1684 Vector4<T> operator*(T s,const Vector4<T>&v)
1685 {
1686  return Vector4<T>(s*v[0],s*v[1],s*v[2],s*v[3]);
1687 }
1688 
1689 //Make Qt handle the classes as primitive type. This optimizes performance with Qt's container classes
1690 Q_DECLARE_TYPEINFO(Vec2d, Q_PRIMITIVE_TYPE);
1691 Q_DECLARE_TYPEINFO(Vec2f, Q_PRIMITIVE_TYPE);
1692 Q_DECLARE_TYPEINFO(Vec2i, Q_PRIMITIVE_TYPE);
1693 Q_DECLARE_TYPEINFO(Vec3d, Q_PRIMITIVE_TYPE);
1694 Q_DECLARE_TYPEINFO(Vec3f, Q_PRIMITIVE_TYPE);
1695 Q_DECLARE_TYPEINFO(Vec3i, Q_PRIMITIVE_TYPE);
1696 Q_DECLARE_TYPEINFO(Vec4d, Q_PRIMITIVE_TYPE);
1697 Q_DECLARE_TYPEINFO(Vec4f, Q_PRIMITIVE_TYPE);
1698 Q_DECLARE_TYPEINFO(Vec4i, Q_PRIMITIVE_TYPE);
1699 Q_DECLARE_TYPEINFO(Mat4d, Q_PRIMITIVE_TYPE);
1700 Q_DECLARE_TYPEINFO(Mat4f, Q_PRIMITIVE_TYPE);
1701 Q_DECLARE_TYPEINFO(Mat3d, Q_PRIMITIVE_TYPE);
1702 Q_DECLARE_TYPEINFO(Mat3f, Q_PRIMITIVE_TYPE);
1703 
1704 //Declare meta-type information so that Vec/Mat classes can be used in QVariant
1705 //They are registered (qRegisterMetaType) in StelCore::registerMathMetaTypes, called in constructor
1706 Q_DECLARE_METATYPE(Vec2d)
1707 Q_DECLARE_METATYPE(Vec2f)
1708 Q_DECLARE_METATYPE(Vec2i)
1709 Q_DECLARE_METATYPE(Vec3d)
1710 Q_DECLARE_METATYPE(Vec3f)
1711 Q_DECLARE_METATYPE(Vec3i)
1712 Q_DECLARE_METATYPE(Vec4d)
1713 Q_DECLARE_METATYPE(Vec4f)
1714 Q_DECLARE_METATYPE(Vec4i)
1715 Q_DECLARE_METATYPE(Mat4d)
1716 Q_DECLARE_METATYPE(Mat4f)
1717 Q_DECLARE_METATYPE(Mat3d)
1718 Q_DECLARE_METATYPE(Mat3f)
1719 
1720 template <class T>
1721 QDebug operator<<(QDebug debug, const Vector2<T> &c)
1722 {
1723  QDebugStateSaver saver(debug);
1724  debug.nospace() << '[' << c[0] << ", " << c[1] << ']';
1725 
1726  return debug;
1727 }
1728 
1729 template <class T>
1730 QDebug operator<<(QDebug debug, const Vector3<T> &c)
1731 {
1732  QDebugStateSaver saver(debug);
1733  debug.nospace() << '[' << c[0] << ", " << c[1] << ", " << c[2] << ']';
1734 
1735  return debug;
1736 }
1737 
1738 template <class T>
1739 QDebug operator<<(QDebug debug, const Vector4<T> &c)
1740 {
1741  QDebugStateSaver saver(debug);
1742  debug.nospace() << '[' << c[0] << ", " << c[1] << ", " << c[2] << ", " << c[3] << ']';
1743 
1744  return debug;
1745 }
1746 
1747 
1749 inline QVector3D operator*(const QMatrix3x3& mat, const QVector3D& vec)
1750 {
1751  float x,y,z;
1752  x = vec.x() * mat(0,0) +
1753  vec.y() * mat(0,1) +
1754  vec.z() * mat(0,2);
1755  y = vec.x() * mat(1,0) +
1756  vec.y() * mat(1,1) +
1757  vec.z() * mat(1,2);
1758  z = vec.x() * mat(2,0) +
1759  vec.y() * mat(2,1) +
1760  vec.z() * mat(2,2);
1761  return QVector3D(x,y,z);
1762 }
1763 
1764 #endif // _VECMATH_H_
Vector2< T > clamp(const Vector2< T > &low, const Vector2< T > &high) const
Component-wise clamping to the specified upper and lower bounds.
Definition: VecMath.hpp:593
Vector4()
The vector is not initialized!
Definition: VecMath.hpp:854
Vector3 & operator=(const T *)
Assignment from array.
Definition: VecMath.hpp:667
bool fuzzyEquals(const Vector3< T > &, T epsilon=std::numeric_limits< T >::epsilon()) const
allows for a fuzzy comparison using some epsilon value
Definition: VecMath.hpp:689
Vector2< T > max(const Vector2< T > &) const
Component-wise maximum determination.
Definition: VecMath.hpp:588
Vector2< T > operator/(T) const
Scalar division.
Definition: VecMath.hpp:573
T trace() const
return trace (sum of diagonal elements).
Definition: VecMath.hpp:324
A templatized column-major 3x3 matrix compatible with openGL (mostly for NormalMatrix calculation)...
Definition: VecMath.hpp:36
T angle() const
return rotational angle
Definition: VecMath.hpp:326
A templatized 2d vector compatible with openGL.
Definition: VecMath.hpp:32
A templatized column-major 4x4 matrix compatible with openGL.
Definition: VecMath.hpp:35
Vector2< T > & operator*=(T)
Scalar multiplication.
Definition: VecMath.hpp:519
A templatized 4d vector compatible with openGL.
Definition: VecMath.hpp:34
Vector2< T > & operator=(const T *)
Assignment from array.
Definition: VecMath.hpp:464
Vector2()
The vector is not initialized!
Definition: VecMath.hpp:441
Vector2< T > & operator/=(T)
Scalar division.
Definition: VecMath.hpp:531
QDataStream & operator>>(QDataStream &in, SphericalRegionP &region)
Load the SphericalRegionP from a binary blob.
A templatized 3d vector compatible with openGL.
Definition: VecMath.hpp:33
Vector3()
The vector is not initialized!
Definition: VecMath.hpp:628
Vector2< T > operator*(T) const
Scalar multiplication.
Definition: VecMath.hpp:563
const QCPRange operator-(const QCPRange &range, double value)
Definition: qcustomplot.h:562
Vector2< T > min(const Vector2< T > &) const
Component-wise minimum determination.
Definition: VecMath.hpp:583
const QCPRange operator+(const QCPRange &range, double value)
Definition: qcustomplot.h:542