VectorNav C++ Library
matrix.h
Go to the documentation of this file.
1 #ifndef _VN_MATH_MAT_H_
7 #define _VN_MATH_MAT_H_
8 
9 #include <cassert>
10 #include <iostream>
11 
12 #include "vector.h"
13 #include "exceptions.h"
14 
15 namespace vn {
16 namespace math {
17 
19 template <size_t m, size_t n = m, typename T = float>
20 struct mat
21 {
22  // Public Members /////////////////////////////////////////////////////////
23 
24 public:
25 
27  union
28  {
29  T e[m * n];
30  };
31 
32  // Constructors ///////////////////////////////////////////////////////////
33 
34 public:
35 
37  mat() { }
38 
42  explicit mat(T val)
43  {
44  std::fill_n(e, m * n, val);
45  }
46 
47  // Helper Methods /////////////////////////////////////////////////////////
48 
49 public:
50 
54  static mat zero()
55  {
56  return mat<m, n, T>(0);
57  }
58 
62  static mat one()
63  {
64  return mat<m, n, T>(1);
65  }
66 
71  {
72  assert(m == n);
73 
74  mat<m, m, T> nm(0);
75 
76  for (size_t i = 0; i < m; i++)
77  nm.e[i * m + i] = 1;
78 
79  return nm;
80  }
81 
82  // Operator Overloads /////////////////////////////////////////////////////
83 
84 public:
85 
91  T& operator()(size_t row, size_t col)
92  {
93  return const_cast<T&>(static_cast<const mat&>(*this)(row, col));
94  }
95 
101  const T& operator()(size_t row, size_t col) const
102  {
103  assert(row < m && col < n);
104 
105  //return e[col * m + row];
106  return e[col + row * m];
107  }
108 
112  mat operator-() const
113  {
114  return neg();
115  }
116 
121  template<typename S>
122  mat& operator*=(const S& rhs)
123  {
124  for (size_t i = 0; i < m*n; i++)
125  e[i] *= rhs;
126 
127  return *this;
128  }
129 
134  template<size_t r, size_t s, typename S>
136  {
137  // columns from the matrix must match rows of the input matrix
138  if (m != s)
139  {
140  return *this;
141  }
142 
143  size_t row = 0;
144  size_t col = 0;
145  size_t cell = 0;
146 
147  mat<r, n, T> return_mat = zero();
148 
149  // Remember, this matrix is stored in column order
150  for (size_t row_index = 0; row_index < r; row_index++)
151  {
152  for (size_t col_index = 0; col_index < n; col_index++)
153  {
154 
155  cell = row_index + (col_index * r);
156 
157  for (size_t cell_index = 0; cell_index < m; cell_index++)
158  {
159  // calculated for the cell in the current row
160  row = (col_index * m) + cell_index;
161  // calculated for the cell in the current column
162  col = (cell_index * r) + row_index;
163  return_mat.e[cell] += this->e[row] * rhs.e[col];
164  }
165  }
166  }
167 
168  return return_mat;
169  }
170 
175  template<typename S>
176  mat& operator/=(const T & rhs)
177  {
178  for (size_t i = 0; i < m*n; i++)
179  e[i] /= rhs;
180 
181  return *this;
182  }
183 
188  template<typename S>
190  {
191  for (size_t i = 0; i < m*n; i++)
192  e[i] += rhs.e[i];
193 
194  return *this;
195  }
196 
201  template<typename S>
203  {
204  for (size_t i = 0; i < m*n; i++)
205  e[i] -= rhs.e[i];
206 
207  return *this;
208  }
209 
210  // Public Methods /////////////////////////////////////////////////////////
211 
212 public:
213 
217  size_t dimRow() const { return m; }
218 
222  size_t dimCol() const { return n; }
223 
227  mat neg() const
228  {
229  mat nm;
230 
231  for (size_t i = 0; i < m * n; i++)
232  nm.e[i] = -e[i];
233 
234  return nm;
235  }
236 
241  mat mult(const double& scalar) const
242  {
243  mat nm;
244 
245  for (size_t i = 0; i < m*n; i++)
246  nm.e[i] = e[i] * scalar;
247 
248  return nm;
249  }
250 
255  mat div(const double& scalar) const
256  {
257  mat nm;
258 
259  for (size_t i = 0; i < m*n; i++)
260  nm.e[i] = e[i] / scalar;
261 
262  return nm;
263  }
264 
269  mat add(const mat& toAdd) const
270  {
271  mat nm;
272 
273  for (size_t i = 0; i < m*n; i++)
274  nm.e[i] = e[i] + toAdd.e[i];
275 
276  return nm;
277  }
278 
283  mat sub(const mat& toSub) const
284  {
285  mat nm;
286 
287  for (size_t i = 0; i < m*n; i++)
288  nm.e[i] = e[i] - toSub.e[i];
289 
290  return nm;
291  }
292 
297  {
298  mat<n, m, T> nm;
299 
300  for (size_t row = 0; row < m; row++)
301  {
302  for (size_t col = 0; col < n; col++)
303  {
304  nm.e[row * n + col] = e[col * m + row];
305  }
306  }
307 
308  return nm;
309  }
310 
311 };
312 
313 // Specializations ////////////////////////////////////////////////////////////
314 
315 #if defined(_MSC_VER)
316  #pragma warning(push)
317 
318  // Disable warning about 'nonstandard extension used : nameless struct/union'.
319  #pragma warning(disable:4201)
320 #endif
321 
323 template <typename T>
324 struct mat<2, 2, T>
325 {
326 
327  // Public Members /////////////////////////////////////////////////////////
328 
329 public:
330 
331  union
332  {
333  struct
334  {
336  T e00;
337 
339  T e10;
340 
342  T e01;
343 
345  T e11;
346  };
347 
349  T e[2 * 2];
350  };
351 
352  // Constructors ///////////////////////////////////////////////////////////
353 
354 public:
355 
357  mat() { }
358 
362  explicit mat(T val) :
363  e00(val), e10(val),
364  e01(val), e11(val)
365  { }
366 
374  mat(T e00v, T e01v,
375  T e10v, T e11v) :
376  e00(e00v), e10(e10v),
377  e01(e01v), e11(e11v)
378  { }
379 
386  mat(vec<2, T> col0, vec<2, T> col1) :
387  e00(col0.x), e10(col1.x),
388  e01(col0.y), e11(col1.y)
389  {}
390 
391  // Helper Methods /////////////////////////////////////////////////////////
392 
393 public:
394 
398  static mat zero()
399  {
400  return mat<2, 2, T>(0);
401  }
402 
406  static mat one()
407  {
408  return mat<2, 2, T>(1);
409  }
410 
415  {
416  return mat<2, 2, T>(
417  1, 0,
418  0, 1);
419  }
420 
421  // Operator Overloads /////////////////////////////////////////////////////
422 
423 public:
424 
430  T& operator()(size_t row, size_t col)
431  {
432  return const_cast<T&>(static_cast<const mat&>(*this)(row, col));
433  }
434 
440  const T& operator()(size_t row, size_t col) const
441  {
442  assert(row < 2 && col < 2);
443 
444  return e[col * 2 + row];
445  }
446 
450  mat operator-() const
451  {
452  return neg();
453  }
454 
459  template<typename S>
460  mat& operator*=(const S& rhs)
461  {
462  #if defined(_MSC_VER)
463  #pragma warning(push)
464 
465  // The operator*= throws a warning when a mat*f is multiplied by a mat*d.
466  #pragma warning(disable:4244)
467  #endif
468 
469  for (size_t i = 0; i < 2 * 2; i++)
470  e[i] *= rhs;
471 
472  #if defined (_MSC_VER)
473  #pragma warning(pop)
474  #endif
475 
476  return *this;
477  }
478 
483  template<typename S>
485  {
486  size_t row = 0;
487  size_t col = 0;
488  size_t cell = 0;
489 
490  mat<2, 2, T> return_mat = zero();
491 
492  // Remember, this matrix is stored in column order
493  for (size_t row_index = 0; row_index < 2; row_index++)
494  {
495  for (size_t col_index = 0; col_index < 2; col_index++)
496  {
497  cell = row_index + (col_index * 2);
498 
499  for (size_t cell_index = 0; cell_index < 2; cell_index++)
500  {
501  // calculated for the cell in the current row
502  row = row_index + (cell_index * 2);
503  // calculated for the cell in the current column
504  col = cell_index + (col_index * 2);
505 
506  return_mat.e[cell] += this->e[row] * rhs.e[col];
507  }
508  }
509  }
510 
511  *this = return_mat;
512 
513  return *this;
514  }
515 
520  template<typename S>
521  mat& operator/=(const S& rhs)
522  {
523  #if defined(_MSC_VER)
524  #pragma warning(push)
525 
526  // The operator/= throws a warning when a mat*f is divided by a double.
527  #pragma warning(disable:4244)
528  #endif
529 
530  for (size_t i = 0; i < 2 * 2; i++)
531  e[i] /= rhs;
532 
533  #if defined (_MSC_VER)
534  #pragma warning(pop)
535  #endif
536 
537  return *this;
538  }
539 
544  template<typename S>
546  {
547  for (size_t i = 0; i < 2 * 2; i++)
548  e[i] += rhs.e[i];
549 
550  return *this;
551  }
552 
557  template<typename S>
559  {
560  for (size_t i = 0; i < 2 * 2; i++)
561  e[i] -= rhs.e[i];
562 
563  return *this;
564  }
565 
566  // Public Methods /////////////////////////////////////////////////////////
567 
568 public:
569 
573  size_t dimRow() const { return 2; }
574 
578 
579  size_t dimCol() const { return 2; }
580 
581  template<typename S>
582  void copy(const mat<2, 2, S> rhs)
583  {
584  *this = rhs;
585  }
586 
590  mat neg() const
591  {
592  mat nm;
593 
594  for (size_t i = 0; i < 2 * 2; i++)
595  nm.e[i] = -e[i];
596 
597  return nm;
598  }
599 
604  mat mult(const double& scalar) const
605  {
606  mat nm;
607 
608  for (size_t i = 0; i < 2 * 2; i++)
609  nm.e[i] = e[i] * scalar;
610 
611  return nm;
612  }
613 
618  mat div(const double& scalar) const
619  {
620  mat nm;
621 
622  for (size_t i = 0; i < 2 * 2; i++)
623  nm.e[i] = e[i] / scalar;
624 
625  return nm;
626  }
627 
632  mat add(const mat& toAdd) const
633  {
634  mat nm;
635 
636  for (size_t i = 0; i < 2 * 2; i++)
637  nm.e[i] = e[i] + toAdd.e[i];
638 
639  return nm;
640  }
641 
646  mat sub(const mat& toSub) const
647  {
648  mat nm;
649 
650  for (size_t i = 0; i < 2 * 2; i++)
651  nm.e[i] = e[i] - toSub.e[i];
652 
653  return nm;
654  }
655 
660  {
661  mat<2, 2, T> nm;
662 
663  for (size_t row = 0; row < 2; row++)
664  {
665  for (size_t col = 0; col < 2; col++)
666  {
667  nm.e[row * 2 + col] = e[col * 2 + row];
668  }
669  }
670 
671  return nm;
672  }
673 };
674 
677 template <typename T>
678 struct mat<3, 3, T>
679 {
680 
681  // Public Members /////////////////////////////////////////////////////////
682 
683 public:
684 
685  union
686  {
687  struct
688  {
690  T e00;
691 
693  T e10;
694 
696  T e20;
697 
699  T e01;
700 
702  T e11;
703 
705  T e21;
706 
708  T e02;
709 
711  T e12;
712 
714  T e22;
715  };
716 
718  T e[3 * 3];
719  };
720 
721  // Constructors ///////////////////////////////////////////////////////////
722 
723 public:
724 
726  mat() { }
727 
731  explicit mat(T val) :
732  e00(val), e10(val), e20(val),
733  e01(val), e11(val), e21(val),
734  e02(val), e12(val), e22(val)
735  { }
736 
749  mat(T e00v, T e01v, T e02v,
750  T e10v, T e11v, T e12v,
751  T e20v, T e21v, T e22v) :
752  e00(e00v), e10(e10v), e20(e20v),
753  e01(e01v), e11(e11v), e21(e21v),
754  e02(e02v), e12(e12v), e22(e22v)
755 
756  { }
757 
763  mat(vec<3, T> col0, vec<3, T> col1, vec<3, T> col2) :
764  e00(col0.x), e10(col0.y), e20(col0.z),
765  e01(col1.x), e11(col1.y), e21(col1.z),
766  e02(col2.x), e12(col2.y), e22(col2.z)
767  {
768 
769  }
770 
771  // Helper Methods /////////////////////////////////////////////////////////
772 
773 public:
774 
778  static mat zero()
779  {
780  return mat<3, 3, T>(0);
781  }
782 
786  static mat one()
787  {
788  return mat<3, 3, T>(1);
789  }
790 
795  {
796  return mat<3, 3, T>(
797  1, 0, 0,
798  0, 1, 0,
799  0, 0, 1);
800  }
801 
802  // Operator Overloads /////////////////////////////////////////////////////
803 
804 public:
805 
811  T& operator()(size_t row, size_t col)
812  {
813  return const_cast<T&>(static_cast<const mat&>(*this)(row, col));
814  }
815 
821  const T& operator()(size_t row, size_t col) const
822  {
823  assert(row < 3 && col < 3);
824 
825  return e[col * 3 + row];
826  }
827 
831  mat operator-() const
832  {
833  return neg();
834  }
835 
840  template<typename S>
841  mat& operator*=(const S& rhs)
842  {
843  #if defined(_MSC_VER)
844  #pragma warning(push)
845 
846  // The operator*= throws a warning when a mat*f is multiplied by a mat*d.
847  #pragma warning(disable:4244)
848  #endif
849 
850  for (size_t i = 0; i < 3 * 3; i++)
851  e[i] *= rhs;
852 
853  #if defined (_MSC_VER)
854  #pragma warning(pop)
855  #endif
856 
857  return *this;
858  }
859 
864  template<typename S>
866  {
867  size_t counter = 0;
868  size_t row = 0;
869  size_t col = 0;
870  size_t cell = 0;
871  mat<3, 3, T> return_mat = zero();
872 
873  // Remember, this matrix is stored in column order
874  for(size_t row_index = 0; row_index < 3; row_index++)
875  {
876  for(size_t col_index = 0; col_index < 3; col_index++)
877  {
878  cell = row_index + (col_index * 3);
879 
880  for (size_t cell_index = 0; cell_index < 3; cell_index++)
881  {
882  // calculated for the cell in the current row
883  row = row_index + (cell_index * 3);
884  // calculated for the cell in the current column
885  col = cell_index + (col_index * 3);
886 
887  return_mat.e[cell] += this->e[row] * rhs.e[col];
888  }
889  }
890  }
891 
892  // std::copy(return_mat.e[0], return_mat.e[8], this->e[0]);
893  // std::copy(return_mat.e, return_mat.e + 7, this->e);
894  *this = return_mat;
895 
896  return *this;
897  }
898 
903  template<typename S>
904  mat& operator/=(const S & rhs)
905  {
906  #if defined(_MSC_VER)
907  #pragma warning(push)
908 
909  // The operator/= throws a warning when a mat*f is divided by a double.
910  #pragma warning(disable:4244)
911  #endif
912 
913  for (size_t i = 0; i < 3 * 3; i++)
914  e[i] /= rhs;
915 
916  #if defined (_MSC_VER)
917  #pragma warning(pop)
918  #endif
919 
920  return *this;
921  }
922 
927  template<typename S>
929  {
930  for (size_t i = 0; i < 3 * 3; i++)
931  e[i] += rhs.e[i];
932 
933  return *this;
934  }
935 
940  template<typename S>
942  {
943  for (size_t i = 0; i < 3 * 3; i++)
944  e[i] -= rhs.e[i];
945 
946  return *this;
947  }
948 
949  // Public Methods /////////////////////////////////////////////////////////
950 
951 public:
952 
956  size_t dimRow() const { return 3; }
957 
961  size_t dimCol() const { return 3; }
962  size_t dimCols() const { return 3; }
963 
964  template<typename S>
965  void copy(const mat<3, 3, S>& rhs)
966  {
967  *this = rhs;
968  }
969 
973  mat neg() const
974  {
975  mat nm;
976 
977  for (size_t i = 0; i < 3 * 3; i++)
978  nm.e[i] = -e[i];
979 
980  return nm;
981  }
982 
987  mat mult(const double& scalar) const
988  {
989  mat nm;
990 
991  for (size_t i = 0; i < 3 * 3; i++)
992  nm.e[i] = e[i] * scalar;
993 
994  return nm;
995  }
996 
1001  mat div(const double& scalar) const
1002  {
1003  mat nm;
1004 
1005  for (size_t i = 0; i < 3 * 3; i++)
1006  nm.e[i] = e[i] / scalar;
1007 
1008  return nm;
1009  }
1010 
1015  mat add(const mat& toAdd) const
1016  {
1017  mat nm;
1018 
1019  for (size_t i = 0; i < 3 * 3; i++)
1020  nm.e[i] = e[i] + toAdd.e[i];
1021 
1022  return nm;
1023  }
1024 
1029  mat sub(const mat& toSub) const
1030  {
1031  mat nm;
1032 
1033  for (size_t i = 0; i < 3 * 3; i++)
1034  nm.e[i] = e[i] - toSub.e[i];
1035 
1036  return nm;
1037  }
1038 
1043  {
1044  mat<3, 3, T> nm;
1045 
1046  for (size_t row = 0; row < 3; row++)
1047  {
1048  for (size_t col = 0; col < 3; col++)
1049  {
1050  nm.e[row * 3 + col] = e[col * 3 + row];
1051  }
1052  }
1053 
1054  return nm;
1055  }
1056 };
1057 
1059 template <typename T>
1060 struct mat<4, 4, T>
1061 {
1062 
1063  // Public Members /////////////////////////////////////////////////////////
1064 
1065 public:
1066 
1067  union
1068  {
1069  struct
1070  {
1072  T e00;
1073 
1075  T e10;
1076 
1078  T e20;
1079 
1081  T e30;
1082 
1084  T e01;
1085 
1087  T e11;
1088 
1090  T e21;
1091 
1093  T e31;
1094 
1096  T e02;
1097 
1099  T e12;
1100 
1102  T e22;
1103 
1105  T e32;
1106 
1108  T e03;
1109 
1111  T e13;
1112 
1114  T e23;
1115 
1117  T e33;
1118  };
1119 
1121  T e[4*4];
1122  };
1123 
1124  // Constructors ///////////////////////////////////////////////////////////
1125 
1126 public:
1127 
1129  mat() { }
1130 
1134  explicit mat(T val) :
1135  e00(val), e01(val), e02(val), e03(val),
1136  e10(val), e11(val), e12(val), e13(val),
1137  e20(val), e21(val), e22(val), e23(val),
1138  e30(val), e31(val), e32(val), e33(val)
1139  { }
1140 
1160  mat(T e00v, T e01v, T e02v, T e03v,
1161  T e10v, T e11v, T e12v, T e13v,
1162  T e20v, T e21v, T e22v, T e23v,
1163  T e30v, T e31v, T e32v, T e33v) :
1164  e00(e00v), e01(e01v), e02(e02v), e03(e03v),
1165  e10(e10v), e11(e11v), e12(e12v), e13(e13v),
1166  e20(e20v), e21(e21v), e22(e22v), e23(e23v),
1167  e30(e30v), e31(e31v), e32(e32v), e33(e33v)
1168  { }
1169 
1176  mat(vec<4, T> col0, vec<4, T> col1, vec<4, T> col2, vec<4, T> col3) :
1177  e00(col0.x), e10(col1.x), e20(col2.x), e30(col3.x),
1178  e01(col0.y), e11(col1.y), e21(col2.y), e31(col3.y),
1179  e02(col0.z), e12(col1.z), e22(col2.z), e32(col3.z),
1180  e03(col0.w), e13(col1.w), e23(col2.w), e33(col3.w)
1181  {
1182 
1183  }
1184 
1185  // Helper Methods /////////////////////////////////////////////////////////
1186 
1187 public:
1188 
1192  static mat zero()
1193  {
1194  return mat<4, 4, T>(0);
1195  }
1196 
1200  static mat one()
1201  {
1202  return mat<4, 4, T>(1);
1203  }
1204 
1209  {
1210  return mat<4, 4, T>(1, 0, 0, 0,
1211  0, 1, 0, 0,
1212  0, 0, 1, 0,
1213  0, 0, 0, 1);
1214  }
1215 
1216  // Operator Overloads /////////////////////////////////////////////////////
1217 
1218 public:
1219 
1225  T& operator()(size_t row, size_t col)
1226  {
1227  return const_cast<T&>(static_cast<const mat&>(*this)(row, col));
1228  }
1229 
1235  const T& operator()(size_t row, size_t col) const
1236  {
1237  assert(row < 4 && col < 4);
1238 
1239  return e[col * 4 + row];
1240  }
1241 
1245  mat operator-() const
1246  {
1247  return neg();
1248  }
1249 
1254  template<typename S>
1255  mat& operator*=(const S& rhs)
1256  {
1257  #if defined(_MSC_VER)
1258  #pragma warning(push)
1259 
1260  // The operator*= throws a warning when a mat*f is multiplied by a mat*d.
1261  #pragma warning(disable:4244)
1262  #endif
1263 
1264  for (size_t i = 0; i < 4 * 4; i++)
1265  e[i] *= rhs;
1266 
1267  #if defined (_MSC_VER)
1268  #pragma warning(pop)
1269  #endif
1270 
1271  return *this;
1272  }
1273 
1278  template<typename S>
1280  {
1281  size_t row = 0;
1282  size_t col = 0;
1283  size_t cell = 0;
1284 
1285  mat<4, 4, T> return_mat = zero();
1286 
1287  // Remember, this matrix is stored in column order
1288  for (size_t row_index = 0; row_index < 4; row_index++)
1289  {
1290  for (size_t col_index = 0; col_index < 4; col_index++)
1291  {
1292  cell = row_index + (col_index * 4);
1293 
1294  for (size_t cell_index = 0; cell_index < 4; cell_index++)
1295  {
1296  // calculated for the cell in the current row
1297  row = row_index + (cell_index * 4);
1298  // calculated for the cell in the current column
1299  col = cell_index + (col_index * 4);
1300 
1301  return_mat.e[cell] += this->e[row] * rhs.e[col];
1302  }
1303  }
1304  }
1305 
1306  *this = return_mat;
1307 
1308  return *this;
1309  }
1310 
1315  template<typename S>
1316  mat& operator/=(const S& rhs)
1317  {
1318  #if defined(_MSC_VER)
1319  #pragma warning(push)
1320 
1321  // The operator/= throws a warning when a mat*f is divided by a double.
1322  #pragma warning(disable:4244)
1323  #endif
1324 
1325  for (size_t i = 0; i < 4 * 4; i++)
1326  e[i] /= rhs;
1327 
1328  #if defined (_MSC_VER)
1329  #pragma warning(pop)
1330  #endif
1331 
1332  return *this;
1333  }
1334 
1339  template<typename S>
1341  {
1342  for (size_t i = 0; i < 4*4; i++)
1343  e[i] += rhs.e[i];
1344 
1345  return *this;
1346  }
1347 
1352  template<typename S>
1354  {
1355  for (size_t i = 0; i < 4*4; i++)
1356  e[i] -= rhs.e[i];
1357 
1358  return *this;
1359  }
1360 
1361  // Public Methods /////////////////////////////////////////////////////////
1362 
1363 public:
1364 
1368  size_t dimRow() const { return 4; }
1369 
1373  size_t dimCol() const { return 4; }
1374 
1375  template<typename S>
1376  void copy(const mat<4, 4, S>& rhs)
1377  {
1378  *this = rhs;
1379  }
1380 
1384  mat neg() const
1385  {
1386  mat nm;
1387 
1388  for (size_t i = 0; i < 4 * 4; i++)
1389  nm.e[i] = -e[i];
1390 
1391  return nm;
1392  }
1393 
1398  mat mult(const double& scalar) const
1399  {
1400  mat nm;
1401 
1402  for (size_t i = 0; i < 4*4; i++)
1403  nm.e[i] = e[i] * scalar;
1404 
1405  return nm;
1406  }
1407 
1408  //mat mult(const mat& rhs)
1409 
1414  mat div(const double& scalar) const
1415  {
1416  mat nm;
1417 
1418  for (size_t i = 0; i < 4*4; i++)
1419  nm.e[i] = e[i] / scalar;
1420 
1421  return nm;
1422  }
1423 
1428  mat add(const mat& toAdd) const
1429  {
1430  mat nm;
1431 
1432  for (size_t i = 0; i < 4*4; i++)
1433  nm.e[i] = e[i] + toAdd.e[i];
1434 
1435  return nm;
1436  }
1437 
1442  mat sub(const mat& toSub) const
1443  {
1444  mat nm;
1445 
1446  for (size_t i = 0; i < 4*4; i++)
1447  nm.e[i] = e[i] - toSub.e[i];
1448 
1449  return nm;
1450  }
1451 
1456  {
1457  mat<4, 4, T> nm;
1458 
1459  for (size_t row = 0; row < 4; row++)
1460  {
1461  for (size_t col = 0; col < 4; col++)
1462  {
1463  nm.e[row * 4 + col] = e[col * 4 + row];
1464  }
1465  }
1466 
1467  return nm;
1468  }
1469 };
1470 
1471 #if defined (_MSC_VER)
1472  #pragma warning(pop)
1473 #endif
1474 
1475 // Operator Overloads /////////////////////////////////////////////////////////
1476 
1482 template <size_t m, size_t n, typename T, typename S>
1483 mat<m, n, T> operator*(mat<m, n, T> lhs, const S& rhs)
1484 {
1485  lhs *= rhs;
1486 
1487  return lhs;
1488 }
1489 
1495 template <size_t m, size_t n, typename T, typename S>
1496 mat<m, n, T> operator*(const S& lhs, mat<m, n, T> rhs)
1497 {
1498  rhs *= lhs;
1499 
1500  return rhs;
1501 }
1502 
1508 template <size_t m, size_t n, typename T, size_t r, size_t s, typename S>
1509 mat<m, n, T> operator*(mat<m, n, T>&lhs, const mat<r, s, S>& rhs)
1510 {
1511  mat<m, n, T> tmp = lhs;
1512  tmp *= rhs;
1513 
1514  return tmp;
1515 }
1516 
1522 template <size_t m, size_t n, typename T, typename S>
1523 mat<m, n, T> operator/(mat<m, n, T> lhs, const S& rhs)
1524 {
1525  lhs /= rhs;
1526 
1527  return lhs;
1528 }
1529 
1535 template <size_t m, size_t n, typename T, typename S>
1536 mat<m, n, T> operator+(mat<m, n, T> lhs, const mat<m, n, S>& rhs)
1537 {
1538  lhs += rhs;
1539 
1540  return lhs;
1541 }
1542 
1548 template <size_t m, size_t n, typename T, typename S>
1549 mat<m, n, T> operator-(mat<m, n, T> lhs, const mat<m, n, S>& rhs)
1550 {
1551  lhs -= rhs;
1552 
1553  return lhs;
1554 }
1555 
1556 // Specific Typedefs //////////////////////////////////////////////////////////
1557 
1559 typedef mat<2> mat2;
1560 
1562 typedef mat<3> mat3;
1563 
1565 typedef mat<4> mat4;
1566 
1568 typedef mat<2> mat22;
1569 
1571 typedef mat<3> mat33;
1572 
1574 typedef mat<4> mat44;
1575 
1577 typedef mat<2, 2, float> mat2f;
1578 
1580 typedef mat<3, 3, float> mat3f;
1581 
1583 typedef mat<4, 4, float> mat4f;
1584 
1586 typedef mat<2, 2, double> mat2d;
1587 
1589 typedef mat<3, 3, double> mat3d;
1590 
1592 typedef mat<4, 4, double> mat4d;
1593 
1595 typedef mat<2, 2, long double> mat2ld;
1596 
1598 typedef mat<3, 3, long double> mat3ld;
1599 
1601 typedef mat<4, 4, long double> mat4ld;
1602 
1604 typedef mat<2, 2, float> mat22f;
1605 
1607 typedef mat<3, 3, float> mat33f;
1608 
1610 typedef mat<4, 3, float> mat44f;
1611 
1613 typedef mat<2, 2, double> mat22d;
1614 
1616 typedef mat<3, 3, double> mat33d;
1617 
1619 typedef mat<4, 4, double> mat44d;
1620 
1622 typedef mat<2, 2, long double> mat22ld;
1623 
1625 typedef mat<3, 3, long double> mat33ld;
1626 
1628 typedef mat<4, 4, long double> mat44ld;
1629 
1630 // Common functions for working with matrices.
1631 
1637 template <size_t mDim, size_t nDim, typename T> std::string str(mat<mDim, nDim, T> m)
1638 {
1639  std::stringstream ss;
1640  ss << "[";
1641 
1642  for (size_t row_index = 0; row_index < m.dimRow(); row_index++)
1643  {
1644  ss << "(";
1645 
1646  for (size_t col_index = 0; col_index < m.dimCol(); col_index++)
1647  {
1648  ss << m(row_index, col_index);
1649 
1650  if (col_index + 1 < m.dimCol())
1651  ss << "; ";
1652  }
1653 
1654  ss << ")";
1655  }
1656 
1657  ss << "]";
1658 
1659  return ss.str();
1660 }
1661 
1668 template <size_t mDim, size_t nDim, typename T> std::ostream& operator<<(std::ostream& out, mat<mDim, nDim, T> m)
1669 {
1670  out << str(m);
1671  return out;
1672 }
1673 
1674 }
1675 }
1676 
1677 #endif
mat()
Creates a new matrix with uninitialized elements.
Definition: matrix.h:357
T e12
Element 1,2.
Definition: matrix.h:1099
static mat one()
Matrix with all of its elements set to 1.
Definition: matrix.h:406
mat & operator*=(const mat< 4, 4, S > &rhs)
Multiplies the matrix by another matrix.
Definition: matrix.h:1279
T & operator()(size_t row, size_t col)
Indexing into the matrix's elements.
Definition: matrix.h:91
const T & operator()(size_t row, size_t col) const
Indexing into the matrix's elements.
Definition: matrix.h:1235
static mat< 4, 4, T > identity()
Identity matrix with its diagonal elements set to 1.
Definition: matrix.h:1208
mat(vec< 3, T > col0, vec< 3, T > col1, vec< 3, T > col2)
Constructs a matrix from 3 column vectors. Vectors are stored in column order.
Definition: matrix.h:763
T e02
Element 0,2.
Definition: matrix.h:1096
mat add(const mat &toAdd) const
Adds a matrix to this matrix.
Definition: matrix.h:1428
mat(T e00v, T e01v, T e10v, T e11v)
Creates a new matrix with its components initialized to the provided values.
Definition: matrix.h:374
const T & operator()(size_t row, size_t col) const
Indexing into the matrix's elements.
Definition: matrix.h:440
T e33
Element 3,3.
Definition: matrix.h:1117
T & operator()(size_t row, size_t col)
Indexing into the matrix's elements.
Definition: matrix.h:811
mat & operator-=(const mat< m, n, S > &rhs)
Subtracts a matrix from this matrix.
Definition: matrix.h:202
Vector with 4 component specialization.
Definition: vector.h:869
mat neg() const
Negates the matrix.
Definition: matrix.h:973
mat & operator+=(const mat< m, n, S > &rhs)
Adds a matrix to this matrix.
Definition: matrix.h:189
mat add(const mat &toAdd) const
Adds a matrix to this matrix.
Definition: matrix.h:632
T e21
Element 2,1.
Definition: matrix.h:1090
T e12
Element 1,2.
Definition: matrix.h:711
mat div(const double &scalar) const
Divides the matrix by a scalar.
Definition: matrix.h:1414
mat div(const double &scalar) const
Divides the matrix by a scalar.
Definition: matrix.h:1001
mat & operator/=(const S &rhs)
Divides the matrix by a scalar.
Definition: matrix.h:521
3x3 matrix specialization.
Definition: matrix.h:678
T e21
Element 2,1.
Definition: matrix.h:705
T e02
Element 0,2.
Definition: matrix.h:708
T e01
Element 0,1.
Definition: matrix.h:699
mat & operator*=(const mat< 3, 3, S > &rhs)
Multiplies the matrix by another matrix.
Definition: matrix.h:865
T e01
Element 0,1.
Definition: matrix.h:1084
mat mult(const double &scalar) const
Multiplies the matrix by a scalar.
Definition: matrix.h:1398
mat operator-() const
Negates the matrix.
Definition: matrix.h:450
mat & operator*=(const S &rhs)
Multiplies the matrix by a scalar.
Definition: matrix.h:841
mat< 2, 2, T > transpose() const
Transposes the matrix.
Definition: matrix.h:659
2x2 matrix specialization.
Definition: matrix.h:324
mat add(const mat &toAdd) const
Adds a matrix to this matrix.
Definition: matrix.h:1015
size_t dimRow() const
The matrix's row dimension.
Definition: matrix.h:956
mat & operator+=(const mat< 3, 3, S > &rhs)
Adds a matrix to this matrix.
Definition: matrix.h:928
T & operator()(size_t row, size_t col)
Indexing into the matrix's elements.
Definition: matrix.h:430
mat & operator*=(const S &rhs)
Multiplies the matrix by a scalar.
Definition: matrix.h:460
size_t dimCol() const
The matrix's column dimension.
Definition: matrix.h:961
mat< 3, 3, T > transpose() const
Transposes the matrix.
Definition: matrix.h:1042
T e01
Element 0,1.
Definition: matrix.h:342
static mat zero()
Matrix with all of its elements set to 0.
Definition: matrix.h:778
mat(vec< 2, T > col0, vec< 2, T > col1)
Constructs a matrix from 4 column vectors.
Definition: matrix.h:386
size_t dimCol() const
The matrix's column dimension.
Definition: matrix.h:222
static mat one()
Matrix with all of its elements set to 1.
Definition: matrix.h:1200
T e11
Element 1,1.
Definition: matrix.h:1087
T e10
Element 1,0.
Definition: matrix.h:693
mat< n, m, T > transpose() const
Transposes the matrix.
Definition: matrix.h:296
mat(T e00v, T e01v, T e02v, T e10v, T e11v, T e12v, T e20v, T e21v, T e22v)
Creates a new matrix with its components intialized to the provided values.
Definition: matrix.h:749
T e20
Element 2,0.
Definition: matrix.h:696
mat & operator-=(const mat< 4, 4, S > &rhs)
Subtracts a matrix from this matrix.
Definition: matrix.h:1353
T e[4 *4]
The matrix's elements.
Definition: matrix.h:1121
mat & operator*=(const S &rhs)
Multiplies the matrix by a scalar.
Definition: matrix.h:122
Template for a matrix.
Definition: matrix.h:20
T e22
Element 2,2.
Definition: matrix.h:714
size_t dimCol() const
The matrix's column dimension.
Definition: matrix.h:579
mat sub(const mat &toSub) const
Subtracts a matrix from this matrix.
Definition: matrix.h:283
mat mult(const double &scalar) const
Multiplies the matrix by a scalar.
Definition: matrix.h:604
Vector with 3 component specialization.
Definition: vector.h:551
T e10
Element 1,0.
Definition: matrix.h:1075
mat(T val)
Creates a new matrix with its elements initialized to val.
Definition: matrix.h:362
mat & operator/=(const S &rhs)
Divides the matrix by a scalar.
Definition: matrix.h:904
T e00
Element 0,0.
Definition: matrix.h:690
mat()
Creates a new matrix with uninitialized elements.
Definition: matrix.h:726
size_t dimRow() const
The matrix's row dimension.
Definition: matrix.h:573
T e30
Element 3,0.
Definition: matrix.h:1081
size_t dimRow() const
The matrix's row dimension.
Definition: matrix.h:1368
mat add(const mat &toAdd) const
Adds a matrix to this matrix.
Definition: matrix.h:269
mat sub(const mat &toSub) const
Subtracts a matrix from this matrix.
Definition: matrix.h:646
const T & operator()(size_t row, size_t col) const
Indexing into the matrix's elements.
Definition: matrix.h:821
mat & operator+=(const mat< 2, 2, S > &rhs)
Adds a matrix to this matrix.
Definition: matrix.h:545
static mat one()
Matrix with all of its elements set to 1.
Definition: matrix.h:786
mat & operator*=(const mat< 2, 2, S > &rhs)
Multiplies the matrix by another matrix.
Definition: matrix.h:484
mat operator-() const
Negates the matrix.
Definition: matrix.h:112
T e20
Element 2,0.
Definition: matrix.h:1078
mat div(const double &scalar) const
Divides the matrix by a scalar.
Definition: matrix.h:618
mat & operator*=(const S &rhs)
Multiplies the matrix by a scalar.
Definition: matrix.h:1255
size_t dimCol() const
The matrix's column dimension.
Definition: matrix.h:1373
mat neg() const
Negates the matrix.
Definition: matrix.h:590
mat div(const double &scalar) const
Divides the matrix by a scalar.
Definition: matrix.h:255
T e22
Element 2,2.
Definition: matrix.h:1102
T e[3 *3]
The matrix's elements.
Definition: matrix.h:718
T e13
Element 1,3.
Definition: matrix.h:1111
T e00
Element 0,0.
Definition: matrix.h:336
mat & operator+=(const mat< 4, 4, S > &rhs)
Adds a matrix to this matrix.
Definition: matrix.h:1340
T e31
Element 3,1.
Definition: matrix.h:1093
static mat one()
Matrix with all of its elements set to 1.
Definition: matrix.h:62
mat< 4, 4, T > transpose() const
Transposes the matrix.
Definition: matrix.h:1455
T e00
Element 0,0.
Definition: matrix.h:1072
mat sub(const mat &toSub) const
Subtracts a matrix from this matrix.
Definition: matrix.h:1029
T e11
Element 1,1.
Definition: matrix.h:702
mat()
Creates a new matrix with uninitialized elements.
Definition: matrix.h:37
mat(vec< 4, T > col0, vec< 4, T > col1, vec< 4, T > col2, vec< 4, T > col3)
Constructs a matrix from 4 column vectors.
Definition: matrix.h:1176
mat operator-() const
Negates the matrix.
Definition: matrix.h:831
mat & operator-=(const mat< 3, 3, S > &rhs)
Subtracts a matrix from this matrix.
Definition: matrix.h:941
mat(T val)
Creates a new matrix with ints elements initialized to val.
Definition: matrix.h:42
mat & operator*(const mat< r, s, S > &rhs)
Multiplies the matrix by another matrix.
Definition: matrix.h:135
T e10
Element 1,0.
Definition: matrix.h:339
static mat zero()
Matrix with all of its elements set to 0.
Definition: matrix.h:398
mat neg() const
Negates the matrix.
Definition: matrix.h:227
mat & operator-=(const mat< 2, 2, S > &rhs)
Subtracts a matrix from this matrix.
Definition: matrix.h:558
T e[2 *2]
The matrix's elements.
Definition: matrix.h:349
Definition: attitude.h:8
T & operator()(size_t row, size_t col)
Indexing into the matrix's elements.
Definition: matrix.h:1225
const T & operator()(size_t row, size_t col) const
Indexing into the matrix's elements.
Definition: matrix.h:101
mat(T val)
Creates a new matrix with its elements initialized to val.
Definition: matrix.h:731
static mat< m, m, T > identity()
Identity matrix with its diagonal elements set to 1.
Definition: matrix.h:70
Vector with 2 component specialization.
Definition: vector.h:279
T e23
Element 2,3.
Definition: matrix.h:1114
4x4 matrix specialization.
Definition: matrix.h:1060
mat()
Creates a new matrix with uninitialized elements.
Definition: matrix.h:1129
mat(T e00v, T e01v, T e02v, T e03v, T e10v, T e11v, T e12v, T e13v, T e20v, T e21v, T e22v, T e23v, T e30v, T e31v, T e32v, T e33v)
Creates a new matrix with its components intialized to the provided values.
Definition: matrix.h:1160
size_t dimRow() const
The matrix's row dimension.
Definition: matrix.h:217
T e32
Element 3,2.
Definition: matrix.h:1105
mat(T val)
Creates a new matrix with its elements initialized to val.
Definition: matrix.h:1134
mat mult(const double &scalar) const
Multiplies the matrix by a scalar.
Definition: matrix.h:987
mat & operator/=(const S &rhs)
Divides the matrix by a scalar.
Definition: matrix.h:1316
mat mult(const double &scalar) const
Multiplies the matrix by a scalar.
Definition: matrix.h:241
mat neg() const
Negates the matrix.
Definition: matrix.h:1384
static mat zero()
Matrix with all of its elements set to 0.
Definition: matrix.h:1192
static mat zero()
Matrix with all of its elements set to 0.
Definition: matrix.h:54
static mat< 3, 3, T > identity()
Identity matrix with its diagonal elements set to 1.
Definition: matrix.h:794
T e03
Element 0,3.
Definition: matrix.h:1108
static mat< 2, 2, T > identity()
Identity matrix with its diagonal elements set to 1.
Definition: matrix.h:414
mat sub(const mat &toSub) const
Subtracts a matrix from this matrix.
Definition: matrix.h:1442
mat operator-() const
Negates the matrix.
Definition: matrix.h:1245
mat & operator/=(const T &rhs)
Divides the matrix by a scalar.
Definition: matrix.h:176
T e11
Element 1,1.
Definition: matrix.h:345