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