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