]> git.mxchange.org Git - simgear.git/blob - simgear/math/SGMatrix.hxx
Updates to remove unneeded and old version of zlib source.
[simgear.git] / simgear / math / SGMatrix.hxx
1 // Copyright (C) 2006  Mathias Froehlich - Mathias.Froehlich@web.de
2 //
3 // This library is free software; you can redistribute it and/or
4 // modify it under the terms of the GNU Library General Public
5 // License as published by the Free Software Foundation; either
6 // version 2 of the License, or (at your option) any later version.
7 //
8 // This library is distributed in the hope that it will be useful,
9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
11 // Library General Public License for more details.
12 //
13 // You should have received a copy of the GNU General Public License
14 // along with this program; if not, write to the Free Software
15 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
16 //
17
18 #ifndef SGMatrix_H
19 #define SGMatrix_H
20
21 /// Expression templates for poor programmers ... :)
22 template<typename T>
23 struct TransNegRef;
24
25 template<typename T>
26 class SGMatrix;
27
28 /// 3D Matrix Class
29 template<typename T>
30 class SGMatrix {
31 public:
32   enum { nCols = 4, nRows = 4, nEnts = 16 };
33   typedef T value_type;
34
35   /// Default constructor. Does not initialize at all.
36   /// If you need them zero initialized, use SGMatrix::zeros()
37   SGMatrix(void)
38   {
39     /// Initialize with nans in the debug build, that will guarantee to have
40     /// a fast uninitialized default constructor in the release but shows up
41     /// uninitialized values in the debug build very fast ...
42 #ifndef NDEBUG
43     for (unsigned i = 0; i < nEnts; ++i)
44       _data.flat[i] = SGLimits<T>::quiet_NaN();
45 #endif
46   }
47   /// Constructor. Initialize by the content of a plain array,
48   /// make sure it has at least 16 elements
49   explicit SGMatrix(const T* data)
50   { for (unsigned i = 0; i < nEnts; ++i) _data.flat[i] = data[i]; }
51
52   /// Constructor, build up a SGMatrix from given elements
53   SGMatrix(T m00, T m01, T m02, T m03,
54            T m10, T m11, T m12, T m13,
55            T m20, T m21, T m22, T m23,
56            T m30, T m31, T m32, T m33)
57   {
58     _data.flat[0] = m00; _data.flat[1] = m10;
59     _data.flat[2] = m20; _data.flat[3] = m30;
60     _data.flat[4] = m01; _data.flat[5] = m11;
61     _data.flat[6] = m21; _data.flat[7] = m31;
62     _data.flat[8] = m02; _data.flat[9] = m12;
63     _data.flat[10] = m22; _data.flat[11] = m32;
64     _data.flat[12] = m03; _data.flat[13] = m13;
65     _data.flat[14] = m23; _data.flat[15] = m33;
66   }
67
68   /// Constructor, build up a SGMatrix from a translation
69   SGMatrix(const SGVec3<T>& trans)
70   { set(trans); }
71
72   /// Constructor, build up a SGMatrix from a rotation and a translation
73   SGMatrix(const SGQuat<T>& quat, const SGVec3<T>& trans)
74   { set(quat, trans); }
75   /// Constructor, build up a SGMatrix from a rotation and a translation
76   SGMatrix(const SGQuat<T>& quat)
77   { set(quat); }
78
79   /// Copy constructor for a transposed negated matrix
80   SGMatrix(const TransNegRef<T>& tm)
81   { set(tm); }
82
83   /// Set from a tranlation
84   void set(const SGVec3<T>& trans)
85   {
86     _data.flat[0] = 1; _data.flat[4] = 0;
87     _data.flat[8] = 0; _data.flat[12] = -trans(0);
88     _data.flat[1] = 0; _data.flat[5] = 1;
89     _data.flat[9] = 0; _data.flat[13] = -trans(1);
90     _data.flat[2] = 0; _data.flat[6] = 0;
91     _data.flat[10] = 1; _data.flat[14] = -trans(2);
92     _data.flat[3] = 0; _data.flat[7] = 0;
93     _data.flat[11] = 0; _data.flat[15] = 1;
94   }
95
96   /// Set from a scale/rotation and tranlation
97   void set(const SGQuat<T>& quat, const SGVec3<T>& trans)
98   {
99     T w = quat.w(); T x = quat.x(); T y = quat.y(); T z = quat.z();
100     T xx = x*x; T yy = y*y; T zz = z*z;
101     T wx = w*x; T wy = w*y; T wz = w*z;
102     T xy = x*y; T xz = x*z; T yz = y*z;
103     _data.flat[0] = 1-2*(yy+zz); _data.flat[1] = 2*(xy-wz);
104     _data.flat[2] = 2*(xz+wy); _data.flat[3] = 0;
105     _data.flat[4] = 2*(xy+wz); _data.flat[5] = 1-2*(xx+zz);
106     _data.flat[6] = 2*(yz-wx); _data.flat[7] = 0;
107     _data.flat[8] = 2*(xz-wy); _data.flat[9] = 2*(yz+wx);
108     _data.flat[10] = 1-2*(xx+yy); _data.flat[11] = 0;
109     // Well, this one is ugly here, as that xform method on the current
110     // object needs the above data to be already set ...
111     SGVec3<T> t = xformVec(trans);
112     _data.flat[12] = -t(0); _data.flat[13] = -t(1);
113     _data.flat[14] = -t(2); _data.flat[15] = 1;
114   }
115   /// Set from a scale/rotation and tranlation
116   void set(const SGQuat<T>& quat)
117   {
118     T w = quat.w(); T x = quat.x(); T y = quat.y(); T z = quat.z();
119     T xx = x*x; T yy = y*y; T zz = z*z;
120     T wx = w*x; T wy = w*y; T wz = w*z;
121     T xy = x*y; T xz = x*z; T yz = y*z;
122     _data.flat[0] = 1-2*(yy+zz); _data.flat[1] = 2*(xy-wz);
123     _data.flat[2] = 2*(xz+wy); _data.flat[3] = 0;
124     _data.flat[4] = 2*(xy+wz); _data.flat[5] = 1-2*(xx+zz);
125     _data.flat[6] = 2*(yz-wx); _data.flat[7] = 0;
126     _data.flat[8] = 2*(xz-wy); _data.flat[9] = 2*(yz+wx);
127     _data.flat[10] = 1-2*(xx+yy); _data.flat[11] = 0;
128     _data.flat[12] = 0; _data.flat[13] = 0;
129     _data.flat[14] = 0; _data.flat[15] = 1;
130   }
131
132   /// set from a transposed negated matrix
133   void set(const TransNegRef<T>& tm)
134   {
135     const SGMatrix& m = tm.m;
136     _data.flat[0] = m(0,0);
137     _data.flat[1] = m(0,1);
138     _data.flat[2] = m(0,2);
139     _data.flat[3] = m(3,0);
140
141     _data.flat[4] = m(1,0);
142     _data.flat[5] = m(1,1);
143     _data.flat[6] = m(1,2);
144     _data.flat[7] = m(3,1);
145
146     _data.flat[8] = m(2,0);
147     _data.flat[9] = m(2,1);
148     _data.flat[10] = m(2,2);
149     _data.flat[11] = m(3,2);
150
151     // Well, this one is ugly here, as that xform method on the current
152     // object needs the above data to be already set ...
153     SGVec3<T> t = xformVec(SGVec3<T>(m(0,3), m(1,3), m(2,3)));
154     _data.flat[12] = -t(0);
155     _data.flat[13] = -t(1);
156     _data.flat[14] = -t(2);
157     _data.flat[15] = m(3,3);
158   }
159
160   /// Access by index, the index is unchecked
161   const T& operator()(unsigned i, unsigned j) const
162   { return _data.flat[i + 4*j]; }
163   /// Access by index, the index is unchecked
164   T& operator()(unsigned i, unsigned j)
165   { return _data.flat[i + 4*j]; }
166
167   /// Access raw data by index, the index is unchecked
168   const T& operator[](unsigned i) const
169   { return _data.flat[i]; }
170   /// Access by index, the index is unchecked
171   T& operator[](unsigned i)
172   { return _data.flat[i]; }
173
174   /// Get the data pointer
175   const T* data(void) const
176   { return _data.flat; }
177   /// Get the data pointer
178   T* data(void)
179   { return _data.flat; }
180
181   /// Readonly interface function to ssg's sgMat4/sgdMat4
182   const T (&sg(void) const)[4][4]
183   { return _data.carray; }
184   /// Interface function to ssg's sgMat4/sgdMat4
185   T (&sg(void))[4][4]
186   { return _data.carray; }
187
188   /// Inplace addition
189   SGMatrix& operator+=(const SGMatrix& m)
190   { for (unsigned i = 0; i < nEnts; ++i) _data.flat[i] += m._data.flat[i]; return *this; }
191   /// Inplace subtraction
192   SGMatrix& operator-=(const SGMatrix& m)
193   { for (unsigned i = 0; i < nEnts; ++i) _data.flat[i] -= m._data.flat[i]; return *this; }
194   /// Inplace scalar multiplication
195   template<typename S>
196   SGMatrix& operator*=(S s)
197   { for (unsigned i = 0; i < nEnts; ++i) _data.flat[i] *= s; return *this; }
198   /// Inplace scalar multiplication by 1/s
199   template<typename S>
200   SGMatrix& operator/=(S s)
201   { return operator*=(1/T(s)); }
202   /// Inplace matrix multiplication, post multiply
203   SGMatrix& operator*=(const SGMatrix<T>& m2);
204
205   SGVec3<T> xformPt(const SGVec3<T>& pt) const
206   {
207     SGVec3<T> tpt;
208     tpt(0) = (*this)(0,3);
209     tpt(1) = (*this)(1,3);
210     tpt(2) = (*this)(2,3);
211     for (unsigned i = 0; i < SGMatrix<T>::nCols-1; ++i) {
212       T tmp = pt(i);
213       tpt(0) += tmp*(*this)(0,i);
214       tpt(1) += tmp*(*this)(1,i);
215       tpt(2) += tmp*(*this)(2,i);
216     }
217     return tpt;
218   }
219   SGVec3<T> xformVec(const SGVec3<T>& v) const
220   {
221     SGVec3<T> tv;
222     T tmp = v(0);
223     tv(0) = tmp*(*this)(0,0);
224     tv(1) = tmp*(*this)(1,0);
225     tv(2) = tmp*(*this)(2,0);
226     for (unsigned i = 1; i < SGMatrix<T>::nCols-1; ++i) {
227       T tmp = v(i);
228       tv(0) += tmp*(*this)(0,i);
229       tv(1) += tmp*(*this)(1,i);
230       tv(2) += tmp*(*this)(2,i);
231     }
232     return tv;
233   }
234
235   /// Return an all zero matrix
236   static SGMatrix zeros(void)
237   { return SGMatrix(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); }
238
239   /// Return a unit matrix
240   static SGMatrix unit(void)
241   { return SGMatrix(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1); }
242
243 private:
244   /// Required to make that alias safe.
245   union Data {
246     T flat[16];
247     T carray[4][4];
248   };
249
250   /// The actual data, the matrix is stored in column major order,
251   /// that matches the storage format of OpenGL
252   Data _data;
253 };
254
255 /// Class to distinguish between a matrix and the matrix with a transposed
256 /// rotational part and a negated translational part
257 template<typename T>
258 struct TransNegRef {
259   TransNegRef(const SGMatrix<T>& _m) : m(_m) {}
260   const SGMatrix<T>& m;
261 };
262
263 /// Unary +, do nothing ...
264 template<typename T>
265 inline
266 const SGMatrix<T>&
267 operator+(const SGMatrix<T>& m)
268 { return m; }
269
270 /// Unary -, do nearly nothing
271 template<typename T>
272 inline
273 SGMatrix<T>
274 operator-(const SGMatrix<T>& m)
275 {
276   SGMatrix<T> ret;
277   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
278     ret[i] = -m[i];
279   return ret;
280 }
281
282 /// Binary +
283 template<typename T>
284 inline
285 SGMatrix<T>
286 operator+(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
287 {
288   SGMatrix<T> ret;
289   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
290     ret[i] = m1[i] + m2[i];
291   return ret;
292 }
293
294 /// Binary -
295 template<typename T>
296 inline
297 SGMatrix<T>
298 operator-(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
299 {
300   SGMatrix<T> ret;
301   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
302     ret[i] = m1[i] - m2[i];
303   return ret;
304 }
305
306 /// Scalar multiplication
307 template<typename S, typename T>
308 inline
309 SGMatrix<T>
310 operator*(S s, const SGMatrix<T>& m)
311 {
312   SGMatrix<T> ret;
313   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
314     ret[i] = s*m[i];
315   return ret;
316 }
317
318 /// Scalar multiplication
319 template<typename S, typename T>
320 inline
321 SGMatrix<T>
322 operator*(const SGMatrix<T>& m, S s)
323 {
324   SGMatrix<T> ret;
325   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
326     ret[i] = s*m[i];
327   return ret;
328 }
329
330 /// Vector multiplication
331 template<typename T>
332 inline
333 SGVec4<T>
334 operator*(const SGMatrix<T>& m, const SGVec4<T>& v)
335 {
336   SGVec4<T> mv;
337   T tmp = v(0);
338   mv(0) = tmp*m(0,0);
339   mv(1) = tmp*m(1,0);
340   mv(2) = tmp*m(2,0);
341   mv(3) = tmp*m(3,0);
342   for (unsigned i = 1; i < SGMatrix<T>::nCols; ++i) {
343     T tmp = v(i);
344     mv(0) += tmp*m(0,i);
345     mv(1) += tmp*m(1,i);
346     mv(2) += tmp*m(2,i);
347     mv(3) += tmp*m(3,i);
348   }
349   return mv;
350 }
351
352 /// Vector multiplication
353 template<typename T>
354 inline
355 SGVec4<T>
356 operator*(const TransNegRef<T>& tm, const SGVec4<T>& v)
357 {
358   const SGMatrix<T>& m = tm.m;
359   SGVec4<T> mv;
360   SGVec3<T> v2;
361   T tmp = v(3);
362   mv(0) = v2(0) = -tmp*m(0,3);
363   mv(1) = v2(1) = -tmp*m(1,3);
364   mv(2) = v2(2) = -tmp*m(2,3);
365   mv(3) = tmp*m(3,3);
366   for (unsigned i = 0; i < SGMatrix<T>::nCols - 1; ++i) {
367     T tmp = v(i) + v2(i);
368     mv(0) += tmp*m(i,0);
369     mv(1) += tmp*m(i,1);
370     mv(2) += tmp*m(i,2);
371     mv(3) += tmp*m(3,i);
372   }
373   return mv;
374 }
375
376 /// Matrix multiplication
377 template<typename T>
378 inline
379 SGMatrix<T>
380 operator*(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
381 {
382   SGMatrix<T> m;
383   for (unsigned j = 0; j < SGMatrix<T>::nCols; ++j) {
384     T tmp = m2(0,j);
385     m(0,j) = tmp*m1(0,0);
386     m(1,j) = tmp*m1(1,0);
387     m(2,j) = tmp*m1(2,0);
388     m(3,j) = tmp*m1(3,0);
389     for (unsigned i = 1; i < SGMatrix<T>::nCols; ++i) {
390       T tmp = m2(i,j);
391       m(0,j) += tmp*m1(0,i);
392       m(1,j) += tmp*m1(1,i);
393       m(2,j) += tmp*m1(2,i);
394       m(3,j) += tmp*m1(3,i);
395     }
396   }
397   return m;
398 }
399
400 /// Inplace matrix multiplication, post multiply
401 template<typename T>
402 inline
403 SGMatrix<T>&
404 SGMatrix<T>::operator*=(const SGMatrix<T>& m2)
405 { (*this) = operator*(*this, m2); return *this; }
406
407 /// Return a reference to the matrix typed to make sure we use the transposed
408 /// negated matrix
409 template<typename T>
410 inline
411 TransNegRef<T>
412 transNeg(const SGMatrix<T>& m)
413 { return TransNegRef<T>(m); }
414
415 /// Compute the inverse if the matrix src. Store the result in dst.
416 /// Return if the matrix is nonsingular. If it is singular dst contains
417 /// undefined values
418 template<typename T>
419 inline
420 bool
421 invert(SGMatrix<T>& dst, const SGMatrix<T>& src)
422 {
423   // Do a LU decomposition with row pivoting and solve into dst
424   SGMatrix<T> tmp = src;
425   dst = SGMatrix<T>::unit();
426
427   for (unsigned i = 0; i < 4; ++i) {
428     T val = tmp(i,i);
429     unsigned ind = i;
430
431     // Find the row with the maximum value in the i-th colum
432     for (unsigned j = i + 1; j < 4; ++j) {
433       if (fabs(tmp(j, i)) > fabs(val)) {
434         ind = j;
435         val = tmp(j, i);
436       }
437     }
438
439     // Do row pivoting
440     if (ind != i) {
441       for (unsigned j = 0; j < 4; ++j) {
442         T t;
443         t = dst(i,j); dst(i,j) = dst(ind,j); dst(ind,j) = t;
444         t = tmp(i,j); tmp(i,j) = tmp(ind,j); tmp(ind,j) = t;
445       }
446     }
447
448     // Check for singularity
449     if (fabs(val) <= SGLimits<T>::min())
450       return false;
451
452     T ival = 1/val;
453     for (unsigned j = 0; j < 4; ++j) {
454       tmp(i,j) *= ival;
455       dst(i,j) *= ival;
456     }
457
458     for (unsigned j = 0; j < 4; ++j) {
459       if (j == i)
460         continue;
461
462       val = tmp(j,i);
463       for (unsigned k = 0; k < 4; ++k) {
464         tmp(j,k) -= tmp(i,k) * val;
465         dst(j,k) -= dst(i,k) * val;
466       }
467     }
468   }
469   return true;
470 }
471
472 /// The 1-norm of the matrix, this is the largest column sum of
473 /// the absolute values of A.
474 template<typename T>
475 inline
476 T
477 norm1(const SGMatrix<T>& m)
478 {
479   T nrm = 0;
480   for (unsigned i = 0; i < SGMatrix<T>::nRows; ++i) {
481     T sum = fabs(m(i, 0)) + fabs(m(i, 1)) + fabs(m(i, 2)) + fabs(m(i, 3));
482     if (nrm < sum)
483       nrm = sum;
484   }
485   return nrm;
486 }
487
488 /// The inf-norm of the matrix, this is the largest row sum of
489 /// the absolute values of A.
490 template<typename T>
491 inline
492 T
493 normInf(const SGMatrix<T>& m)
494 {
495   T nrm = 0;
496   for (unsigned i = 0; i < SGMatrix<T>::nCols; ++i) {
497     T sum = fabs(m(0, i)) + fabs(m(1, i)) + fabs(m(2, i)) + fabs(m(3, i));
498     if (nrm < sum)
499       nrm = sum;
500   }
501   return nrm;
502 }
503
504 /// Return true if exactly the same
505 template<typename T>
506 inline
507 bool
508 operator==(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
509 {
510   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
511     if (m1[i] != m2[i])
512       return false;
513   return true;
514 }
515
516 /// Return true if not exactly the same
517 template<typename T>
518 inline
519 bool
520 operator!=(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
521 { return ! (m1 == m2); }
522
523 /// Return true if equal to the relative tolerance tol
524 template<typename T>
525 inline
526 bool
527 equivalent(const SGMatrix<T>& m1, const SGMatrix<T>& m2, T rtol, T atol)
528 { return norm1(m1 - m2) < rtol*(norm1(m1) + norm1(m2)) + atol; }
529
530 /// Return true if equal to the relative tolerance tol
531 template<typename T>
532 inline
533 bool
534 equivalent(const SGMatrix<T>& m1, const SGMatrix<T>& m2, T rtol)
535 { return norm1(m1 - m2) < rtol*(norm1(m1) + norm1(m2)); }
536
537 /// Return true if about equal to roundoff of the underlying type
538 template<typename T>
539 inline
540 bool
541 equivalent(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
542 {
543   T tol = 100*SGLimits<T>::epsilon();
544   return equivalent(m1, m2, tol, tol);
545 }
546
547 #ifndef NDEBUG
548 template<typename T>
549 inline
550 bool
551 isNaN(const SGMatrix<T>& m)
552 {
553   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i) {
554     if (SGMisc<T>::isNaN(m[i]))
555       return true;
556   }
557   return false;
558 }
559 #endif
560
561 /// Output to an ostream
562 template<typename char_type, typename traits_type, typename T> 
563 inline
564 std::basic_ostream<char_type, traits_type>&
565 operator<<(std::basic_ostream<char_type, traits_type>& s, const SGMatrix<T>& m)
566 {
567   s << "[ " << m(0,0) << ", " << m(0,1) << ", " << m(0,2) << ", " << m(0,3) << "\n";
568   s << "  " << m(1,0) << ", " << m(1,1) << ", " << m(1,2) << ", " << m(1,3) << "\n";
569   s << "  " << m(2,0) << ", " << m(2,1) << ", " << m(2,2) << ", " << m(2,3) << "\n";
570   s << "  " << m(3,0) << ", " << m(3,1) << ", " << m(3,2) << ", " << m(3,3) << " ]";
571   return s;
572 }
573
574 /// Two classes doing actually the same on different types
575 typedef SGMatrix<float> SGMatrixf;
576 typedef SGMatrix<double> SGMatrixd;
577
578 inline
579 SGMatrixf
580 toMatrixf(const SGMatrixd& m)
581 {
582   return SGMatrixf((float)m(0,0), (float)m(0,1), (float)m(0,2), (float)m(0,3),
583                    (float)m(1,0), (float)m(1,1), (float)m(1,2), (float)m(1,3),
584                    (float)m(3,0), (float)m(2,1), (float)m(2,2), (float)m(2,3),
585                    (float)m(4,0), (float)m(4,1), (float)m(4,2), (float)m(4,3));
586 }
587
588 inline
589 SGMatrixd
590 toMatrixd(const SGMatrixf& m)
591 {
592   return SGMatrixd(m(0,0), m(0,1), m(0,2), m(0,3),
593                    m(1,0), m(1,1), m(1,2), m(1,3),
594                    m(3,0), m(2,1), m(2,2), m(2,3),
595                    m(4,0), m(4,1), m(4,2), m(4,3));
596 }
597
598 #endif