EEROS  0.4.1.0
API for the EEROS Real-Time Robotics Framework
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros
Matrix.hpp
Go to the documentation of this file.
1 #ifndef ORG_EEROS_MATH_MATRIX_HPP_
2 #define ORG_EEROS_MATH_MATRIX_HPP_
3 
6 
7 #include <utility>
8 #include <sstream>
9 #include <cstdlib>
10 #include <cmath>
11 
12 namespace eeros {
13  namespace math {
14  template < unsigned int M, unsigned int N = 1, typename T = double >
15  class Matrix {
16  public:
17 
18  static_assert((M > 1 && N >= 1) || (M >=1 && N > 1), "Matrix dimension must be greater or equal than 1x1!");
19 
20  using value_type = T;
21 
22  template < unsigned int IM, unsigned int IN, typename IT >
24  public:
25  MatrixInitializer(Matrix<IM, IN, IT>& mat, unsigned int index = 0) : mat(mat), index(index) { }
26 
28  mat(index / N, index % N) = right;
30  }
31 
33  unsigned int index;
34  }; // END class MatrixInitializer
35 
36  /********** Constructors **********/
37 
38  Matrix() { }
39 
40  Matrix(const T v) {
41  (*this) = v;
42  }
43 
44  template<typename... S>
45  Matrix(const S... v) : value{std::forward<const T>(v)...} {
46  static_assert(sizeof...(S) == M * N, "Invalid number of constructor arguments!");
47  }
48 
49  /********** Initializing the matrix **********/
50 
51  void zero() {
52  for(unsigned int i = 0; i < M * N; i++) {
53  value[i] = 0;
54  }
55  }
56 
57  void eye() {
58  zero();
59  unsigned int j = (M < N) ? M : N;
60  for(unsigned int i = 0; i < j; i++) {
61  (*this)(i, i) = 1;
62  }
63  }
64 
65  void fill(T v) {
66  (*this) = v;
67  }
68 
69  void rotx(double angle) {
70  Matrix<M, N, T>& m = *this;
71  if(M == 3 && N == 3) {
72  m(0,0) = 1;
73  m(1,0) = 0;
74  m(2,0) = 0;
75 
76  m(0,1) = 0;
77  m(1,1) = std::cos(angle);
78  m(2,1) = std::sin(angle);
79 
80  m(0,2) = 0;
81  m(1,2) = -std::sin(angle);
82  m(2,2) = std::cos(angle);
83  }
84  else {
85  throw EEROSException("rotx(double) is only implemented for 3x3 matrices");
86  }
87  }
88 
89  void roty(double angle) {
90  Matrix<M, N, T>& m = *this;
91  if(M == 3 && N == 3) {
92  m(0,0) = std::cos(angle);
93  m(1,0) = 0;
94  m(2,0) = -std::sin(angle);
95 
96  m(0,1) = 0;
97  m(1,1) = 1;
98  m(2,1) = 0;
99 
100  m(0,2) = std::sin(angle);
101  m(1,2) = 0;
102  m(2,2) = std::cos(angle);
103  }
104  else {
105  throw EEROSException("roty(double) is only implemented for 3x3 matrices");
106  }
107  }
108 
109  void rotz(double angle) {
110  Matrix<M, N, T>& m = *this;
111  if(M == 3 && N == 3) {
112  m(0,0) = std::cos(angle);
113  m(1,0) = std::sin(angle);
114  m(2,0) = 0;
115 
116  m(0,1) = -std::sin(angle);
117  m(1,1) = std::cos(angle);
118  m(2,1) = 0;
119 
120  m(0,2) = 0;
121  m(1,2) = 0;
122  m(2,2) = 1;
123  }
124  else {
125  throw EEROSException("rotz(double) is only implemented for 3x3 matrices");
126  }
127  }
128 
129  MatrixInitializer<M, N, T> operator<<(T right) {
130  (*this)[0] = right;
131  return MatrixInitializer<M, N, T>(*this, 1);
132  }
133 
134  /********** Element access **********/
135 
136  const T get(unsigned int m, unsigned int n) const {
137  return (*this)(m, n);
138  }
139 
140  Matrix<M, 1, T> getCol(unsigned int n) const {
141  Matrix<M, 1, T> col;
142  for(unsigned int m = 0; m < M; m++) {
143  col(m, 0) = (*this)(m, n);
144  }
145  return col;
146  }
147 
148  Matrix<1, N, T> getRow(unsigned int m) const {
149  Matrix<1, N, T> row;
150  for(unsigned int n = 0; n < N; n++) {
151  row(0, n) = (*this)(m, n);
152  }
153  return row;
154  }
155 
156  template<unsigned int U, unsigned int V>
157  Matrix<U, V, T> getSubMatrix(unsigned int m, unsigned int n) const {
158  static_assert(U <= M && V <= N, "Dimension of the sub matrix must be lower or equal than of the origin!");
159  if(m + U <= M && n + V <= N) {
160  Matrix<U, V, T> sub;
161  for(unsigned int u = 0; u < U; u++) {
162  for(unsigned int v = 0; v < V; v++) {
163  sub(u, v) = (*this)(m + u, n + v);
164  }
165  }
166  return sub;
167  }
168  else {
169  throw MatrixIndexOutOfBoundException(m + U - 1, M, n + V - 1, N);
170  }
171  }
172 
173  void set(unsigned int m, unsigned int n, T value) {
174  (*this)(m, n) = value;
175  }
176 
177  void setCol(unsigned int n, const Matrix<M, 1, T>& col) {
178  for(unsigned int m = 0; m < M; m++) {
179  (*this)(m, n) = col(m, 0);
180  }
181  }
182 
183  void setRow(unsigned int m, const Matrix<1, N, T>& row) {
184  for(unsigned int n = 0; n < N; n++) {
185  (*this)(m, n) = row(0, n);
186  }
187  }
188 
189  T& operator()(unsigned int m, unsigned int n) {
190  if(m >= 0 && m < M && n >= 0 && n < N) {
191  return value[M * n + m];
192  }
193  else {
194  throw MatrixIndexOutOfBoundException(m, M, n, N);
195  }
196  }
197 
198  const T operator()(unsigned int m, unsigned int n) const {
199  if(m >= 0 && m < M && n >= 0 && n < N) {
200  return value[M * n + m];
201  }
202  else {
203  throw MatrixIndexOutOfBoundException(m, M, n, N);
204  }
205  }
206 
207  T& operator()(unsigned int i) {
208  if(i >= 0 && i < M * N) {
209  return value[i];
210  }
211  else {
212  throw MatrixIndexOutOfBoundException(i, M * N);
213  }
214  }
215 
216  const T operator()(unsigned int i) const {
217  if(i >= 0 && i < M * N) {
218  return value[i];
219  }
220  else {
221  throw MatrixIndexOutOfBoundException(i, M * N);
222  }
223  }
224 
225  T& operator[](unsigned int i) {
226  if(i >= 0 && i < M * N) {
227  return value[i];
228  }
229  else {
230  throw MatrixIndexOutOfBoundException(i, M * N);
231  }
232  }
233 
234  const T operator[](unsigned int i) const {
235  if(i >= 0 && i < M * N) {
236  return value[i];
237  }
238  else {
239  throw MatrixIndexOutOfBoundException(i, M * N);
240  }
241  }
242 
243  /********** Matrix characteristics **********/
244 
245  constexpr bool isSquare() const {
246  return M == N;
247  }
248 
249  bool isOrthogonal() const {
250  Matrix<N, M, T> result, transposed, eye;
251  eye.eye();
252  transposed = this->transpose();
253  result = (*this) * transposed;
254  return result == eye;
255  }
256 
257  bool isSymmetric() const {
258  return (*this) == this->transpose();
259  }
260 
261  bool isDiagonal() const {
262  for(unsigned int m = 0; m < M; m++) {
263  for(unsigned int n = 0; n < N; n++) {
264  if(m != n){
265  if((*this)(m, n) != 0){
266  return false;
267  }
268  }
269  }
270  }
271  return true;
272  }
273 
274  bool isLowerTriangular() const {
275  for(unsigned int m = 0; m < M; m++) {
276  for(unsigned int n = 0; n < N; n++) {
277  if(m < n){
278  if((*this)(m, n) != 0){
279  return false;
280  }
281  }
282  }
283  }
284  return true;
285  }
286 
287  bool isUpperTriangular() const {
288  for(unsigned int m = 0; m < M; m++) {
289  for(unsigned int n = 0; n < N; n++) {
290  if(m > n){
291  if((*this)(m, n) != 0){
292  return false;
293  }
294  }
295  }
296  }
297  return true;
298  }
299 
300  bool isInvertible() const {
301  // non square matrices can't be inverted, if the determinat of a matrix is zero it is not invertible
302  return (M == N) && (this->det() != 0);
303  }
304 
305  constexpr unsigned int getNofRows() const {
306  return M;
307  }
308 
309  constexpr unsigned int getNofColums() const {
310  return N;
311  }
312 
313  constexpr unsigned int size() const {
314  return M * N;
315  }
316 
317  unsigned int rank() const {
318  unsigned int numberOfNonZeroRows = 0;
319  Matrix<M, N, T> temp = (*this);
320  temp.gaussRowElimination();
321  for(unsigned int m = 0; m < M; m++) {
322  for(unsigned int n = 0; n < N; n++) {
323  if(temp(m, n) != 0){
324  numberOfNonZeroRows++;
325  break;
326  }
327  }
328  }
329  return numberOfNonZeroRows;
330  }
331 
332  T det() const {
333  if(M == N) { // Determinat can only be calculated of a square matrix
334  if(M == 2) { // 2x2 matrix
335  return (*this)(0, 0) * (*this)(1, 1) - (*this)(0, 1) * (*this)(1,0);
336  }
337  else if(M == 3) { // 3x3 matrix
338  T det = 0;
339  det = (*this)(0, 0) * (*this)(1, 1) * (*this)(2, 2) +
340  (*this)(0, 1) * (*this)(1, 2) * (*this)(2, 0) +
341  (*this)(0, 2) * (*this)(1, 0) * (*this)(2, 1) -
342  (*this)(0, 2) * (*this)(1, 1) * (*this)(2, 0) -
343  (*this)(0, 0) * (*this)(1, 2) * (*this)(2, 1) -
344  (*this)(0, 1) * (*this)(1, 0) * (*this)(2, 2);
345  return det;
346  }
347  else { // 4x4 and bigger square matrices
348  // Use recurcive laplace formula to calculate the determinat.
349  // For big matrices this method needs a lot of time, this
350  // could be improved with another algorithm.
351  T det = 0;
352  unsigned int ignoredRow = 0;
353  for(unsigned int m = 0; m < M; m++) {
354  Matrix<M - 1, N - 1, T> subMatrix;
355  unsigned int x = 0, y = 0;
356  unsigned int a = 0, b = 1;
357  while(a < M) {
358  while(b < N) {
359  if(a != ignoredRow) {
360  subMatrix(y, x) = (*this)(a, b);
361  x++;
362  }
363  b++;
364  }
365  b = 1;
366  x = 0;
367  if(a != ignoredRow) {
368  y++;
369  }
370  a++;
371  }
372  ignoredRow++;
373  T detSubMatrix = subMatrix.det();
374  if(m % 2 == 0) { // even
375  det = det + (*this)(m, 0) * detSubMatrix;
376  }
377  else { // odd
378  det = det - (*this)(m, 0) * detSubMatrix;
379  }
380  }
381  return det;
382  }
383  }
384  else {
385  throw EEROSException("Calculating determinant failed: Matrix must be square");
386  }
387  return 0;
388  }
389 
390  T trace() const {
391  T result = 0;
392  unsigned int j = (M < N) ? M : N;
393  for(unsigned int i = 0; i < j; i++) {
394  result += (*this)(i, i);
395  }
396  return result;
397  }
398 
400  Matrix<N, M, T> result;
401  for(unsigned int m = 0; m < M; m++) {
402  for(unsigned int n = 0; n < N; n++) {
403  result(n, m) = (*this)(m, n);
404  }
405  }
406  return result;
407  }
408 
409  /********** Base operations **********/
410 
411  bool operator==(const Matrix<M, N, T>& right) const {
412  for(unsigned int m = 0; m < M; m++) {
413  for(unsigned int n = 0; n < N; n++) {
414  if((*this)(m, n) != right(m, n))
415  return false;
416  }
417  }
418  return true;
419  }
420 
421  bool operator!=(const Matrix<M, N, T>& right) const {
422  for(unsigned int m = 0; m < M; m++) {
423  for(unsigned int n = 0; n < N; n++) {
424  if((*this)(m, n) != right(m, n)) {
425  return true;
426  }
427  }
428  }
429  return false;
430  }
431 
432  bool operator<(const Matrix<M, N, T>& right) const {
433  for(unsigned int m = 0; m < M; m++) {
434  for(unsigned int n = 0; n < N; n++) {
435  if((*this)(m, n) >= right(m, n)) {
436  return false;
437  }
438  }
439  }
440  return true;
441  }
442 
443  bool operator<=(const Matrix<M, N, T>& right) const {
444  for(unsigned int m = 0; m < M; m++) {
445  for(unsigned int n = 0; n < N; n++) {
446  if((*this)(m, n) > right(m, n)) {
447  return false;
448  }
449  }
450  }
451  return true;
452  }
453 
454  bool operator>(const Matrix<M, N, T>& right) const {
455  for(unsigned int m = 0; m < M; m++) {
456  for(unsigned int n = 0; n < N; n++) {
457  if((*this)(m, n) <= right(m, n)) {
458  return false;
459  }
460  }
461  }
462  return true;
463  }
464 
465  bool operator>=(const Matrix<M, N, T>& right) const {
466  for(unsigned int m = 0; m < M; m++) {
467  for(unsigned int n = 0; n < N; n++) {
468  if((*this)(m, n) < right(m, n)) {
469  return false;
470  }
471  }
472  }
473  return true;
474  }
475 
477  for(unsigned int m = 0; m < M; m++) {
478  for(unsigned int n = 0; n < N; n++) {
479  (*this)(m, n) = right;
480  }
481  }
482  return *this;
483  }
484 
485  template < unsigned int K >
487  Matrix<M, K, T> result;
488  for(unsigned int m = 0; m < M; m++) {
489  for(unsigned int k = 0; k < K; k++) {
490  result(m, k) = 0;
491  for(unsigned int n = 0; n < N; n++) {
492  result(m, k) += (*this)(m, n) * right(n, k);
493  }
494  }
495  }
496  return result;
497  }
498 
499  Matrix<M, N, T> operator*(T right) const {
500  Matrix<M, N, T> result;
501  for(unsigned int m = 0; m < M; m++) {
502  for(unsigned int n = 0; n < N; n++) {
503  result(m,n) = (*this)(m,n) * right;
504  }
505  }
506  return result;
507  }
508 
510  Matrix<M, N, T> result;
511  for(unsigned int m = 0; m < M; m++) {
512  for(unsigned int n = 0; n < N; n++) {
513  result(m, n) = (*this)(m, n) * right(m, n);
514  }
515  }
516  return result;
517  }
518 
520  Matrix<M, N, T> result;
521  for(unsigned int m = 0; m < M; m++) {
522  for(unsigned int n = 0; n < N; n++) {
523  result(m, n) = (*this)(m, n) + right(m, n);
524  }
525  }
526  return result;
527  }
528 
529  Matrix<M, N, T> operator+(const T right) const {
530  Matrix<M, N, T> result;
531  for(unsigned int m = 0; m < M; m++) {
532  for(unsigned int n = 0; n < N; n++) {
533  result(m, n) = (*this)(m, n) + right;
534  }
535  }
536  return result;
537  }
538 
540  (*this) = (*this) + right;
541  return (*this);
542  }
543 
545  Matrix<M, N, T> result;
546  for(unsigned int m = 0; m < M; m++) {
547  for(unsigned int n = 0; n < N; n++) {
548  result(m, n) = (*this)(m, n) - right(m, n);
549  }
550  }
551  return result;
552  }
553 
554  Matrix<M, N, T> operator-(const T right) const {
555  Matrix<M, N, T> result;
556  for(unsigned int m = 0; m < M; m++) {
557  for(unsigned int n = 0; n < N; n++) {
558  result(m, n) = (*this)(m, n) - right;
559  }
560  }
561  return result;
562  }
563 
565  (*this) = (*this) - right;
566  return (*this);
567  }
568 
570  Matrix<M, N, T> result;
571  for(unsigned int m = 0; m < M; m++) {
572  for(unsigned int n = 0; n < N; n++) {
573  result(m, n) = -(*this)(m, n);
574  }
575  }
576  return result;
577  }
578 
579  Matrix<M, N, T> operator/(T right) const {
580  Matrix<M, N, T> result;
581  for(unsigned int m = 0; m < M; m++) {
582  for(unsigned int n = 0; n < N; n++) {
583  result(m, n) = (*this)(m, n) / right;
584  }
585  }
586  return result;
587  }
588 
590  T determinant = this->det();
591  if(N != M) {
592  throw EEROSException("Invert failed: matrix not square");
593  }
594  else if(determinant == 0) {
595  throw EEROSException("Invert failed: determinat of matrix is 0");
596  }
597  else if(this->isOrthogonal() == true) {
598  return transpose();
599  }
600  else if(M == 2) { // 2x2 matrix
601  Matrix<M, N, T> result;
602  result(0, 0) = (*this)(1, 1);
603  result(1, 0) = -(*this)(1, 0);
604  result(0, 1) = -(*this)(0, 1);
605  result(1, 1) = (*this)(0, 0);
606  return result / determinant;
607  }
608  else if(M == 3) { // 3x3 matrix
609  Matrix<M, N, T> result;
610  result(0, 0) = (*this)(1, 1) * (*this)(2, 2) - (*this)(1, 2) * (*this)(2, 1);
611  result(1, 0) = (*this)(1, 2) * (*this)(2, 0) - (*this)(1, 0) * (*this)(2, 2);
612  result(2, 0) = (*this)(1, 0) * (*this)(2, 1) - (*this)(1, 1) * (*this)(2, 0);
613  result(0, 1) = (*this)(0, 2) * (*this)(2, 1) - (*this)(0, 1) * (*this)(2, 2);
614  result(1, 1) = (*this)(0, 0) * (*this)(2, 2) - (*this)(0, 2) * (*this)(2, 0);
615  result(2, 1) = (*this)(0, 1) * (*this)(2, 0) - (*this)(0, 0) * (*this)(2, 1);
616  result(0, 2) = (*this)(0, 1) * (*this)(1, 2) - (*this)(0, 2) * (*this)(1, 1);
617  result(1, 2) = (*this)(0, 2) * (*this)(1, 0) - (*this)(0, 0) * (*this)(1, 2);
618  result(2, 2) = (*this)(0, 0) * (*this)(1, 1) - (*this)(0, 1) * (*this)(1, 0);
619  return result / determinant;
620  }
621  else {
622  // This algorithm needs a lot of time maybe there is a better one?
623  Matrix<M, N, T> result;
624  Matrix<M - 1, N - 1, T> smallerPart;
625  unsigned int ignoredRow = 0, ignoredColum = 0;
626  for(unsigned int m = 0; m < M; m++) {
627  for(unsigned int n = 0; n < N; n++) {
628  unsigned int a = 0, b = 0;
629  // 1. Create "matrix of minors"
630  for(unsigned int u = 0; u < M; u++) {
631  for(unsigned int w = 0; w < N; w++) {
632  if(u != m && w != n){
633  smallerPart(a, b) = (*this)(u, w);
634  b++;
635  b = b % (N - 1);
636  }
637  }
638  if(u != m) {
639  a++;
640  a = a&(M - 1);
641  }
642  }
643  // 2. Swapp signs
644  if(m % 2 == 0) {
645  if(n % 2 != 0) {
646  result(m,n) = -smallerPart.det();
647  }
648  else {
649  result(m, n) = smallerPart.det();
650  }
651  }
652  else {
653  if(n % 2 != 0){
654  result(m, n) = smallerPart.det();
655  }
656  else {
657  result(m, n) = -smallerPart.det();
658  }
659  }
660  // 3. Divide throu det of the original matrix
661  result(m, n) = result(m, n) / determinant;
662  }
663  }
664  // 4. transpose
665  return result.transpose();
666  }
667  }
668 
669  /********** Static functions **********/
670 
671  static Matrix<M, N, T> createRotX(double angle) {
672  Matrix<M, N, T> m;
673  m.rotx(angle);
674  return m;
675  }
676 
677  static Matrix<M, N, T> createRotY(double angle) {
678  Matrix<M, N, T> m;
679  m.roty(angle);
680  return m;
681  }
682 
683  static Matrix<M, N, T> createRotZ(double angle) {
684  Matrix<M, N, T> m;
685  m.rotz(angle);
686  return m;
687  }
688 
689  static Matrix<2, 1, T> createVector2(T x, T y) {
691  v(0) = x;
692  v(1) = y;
693  return v;
694  }
695 
696  static Matrix<3, 1, T> createVector3(T x, T y, T z) {
698  v(0) = x;
699  v(1) = y;
700  v(2) = z;
701  return v;
702  }
703 
705  Matrix<M, N, T> d;
706  d.eye();
707  return d * v;
708  }
709 
711  Matrix<3, 3, T> result;
712  result(0, 0) = 0;
713  result(0, 1) = -a(2);
714  result(0, 2) = a(1);
715  result(1, 0) = a(2);
716  result(1, 1) = 0;
717  result(1, 2) = -a(0);
718  result(2, 0) = -a(1);
719  result(2, 1) = a(0);
720  result(2, 2) = 0;
721  return result;
722  }
723 
725  Matrix<3, 1, T> result;
726  result(0, 0) = a(1, 0) * b(2, 0) - a(2, 0) * b(1, 0);
727  result(1, 0) = a(2, 0) * b(0, 0) - a(0, 0) * b(2, 0);
728  result(2, 0) = a(0, 0) * b(1, 0) - a(1, 0) * b(0, 0);
729  return result;
730  }
731 
732  /********** Helper functions **********/
733 
735  unsigned int completedColum = 0, completedRow = 0;
736  unsigned int checkingRow = 0, rootRow = 0;
737  T rowFactor = 0;
738 
740  while(completedColum < N) {
741  rootRow = completedRow;
742  checkingRow = rootRow + 1;
743  while(checkingRow < M && (*this)(rootRow, completedColum) != 0) {
744  if((*this)(checkingRow, completedColum) != 0) {
745  rowFactor = (*this)(checkingRow, completedColum) / (*this)(rootRow, completedColum);
746  for(unsigned int n = completedColum; n < N; n++) {
747  (*this)(checkingRow, n) = (*this)(checkingRow, n) - rowFactor * (*this)(rootRow, n);
748  }
749  }
750  checkingRow++;
751  }
752  completedRow++;
753  completedColum++;
754  }
755  }
756 
758  unsigned int completedColum = 0, completedRow = 0;
759  unsigned int swapRow = completedRow + 1;
760 
761  while(completedColum < N) {
762  while(completedRow < M) {
763  if((*this)(completedRow, completedColum) == 0 && swapRow < M && completedRow < M - 1) {
764  swapRows(completedRow, swapRow);
765  swapRow++;
766  }
767  else {
768  completedRow++;
769  }
770  }
771  completedColum++;
772  swapRow = completedRow + 1;
773  }
774  }
775 
776  void swapRows(unsigned int rowA, unsigned int rowB) {
777  for(unsigned int n = 0; n < N; n++) {
778  T t = (*this)(rowA, n);
779  (*this)(rowA, n) = (*this)(rowB, n);
780  (*this)(rowB, n) = t;
781  }
782  }
783 
784  protected:
785  T value[M * N];
786 
787  }; // END class Matrix
788 
789  /********** Operators **********/
790 
791  template < unsigned int M, unsigned int N = 1, typename T = double >
793  Matrix<M, N, T> result;
794  for(unsigned int m = 0; m < M; m++) {
795  for(unsigned int n = 0; n < N; n++) {
796  result(m, n) = left + right(m, n);
797  }
798  }
799  return result;
800  }
801 
802  template < unsigned int M, unsigned int N = 1, typename T = double >
804  Matrix<M, N, T> result;
805  for(unsigned int m = 0; m < M; m++) {
806  for(unsigned int n = 0; n < N; n++) {
807  result(m, n) = left - right(m, n);
808  }
809  }
810  return result;
811  }
812 
813  template < unsigned int M, unsigned int N = 1, typename T = double >
815  Matrix<M, N, T> result;
816  for(unsigned int m = 0; m < M; m++) {
817  for(unsigned int n = 0; n < N; n++) {
818  result(m, n) = -right(m, n);
819  }
820  }
821  return result;
822  }
823 
824  template < unsigned int M, unsigned int N = 1, typename T = double >
826  Matrix<M, N, T> result;
827  for(unsigned int m = 0; m < M; m++) {
828  for(unsigned int n = 0; n < N; n++) {
829  result(m, n) = left * right(m, n);
830  }
831  }
832  return result;
833  }
834 
835  template < unsigned int M, unsigned int N = 1, typename T = double >
837  Matrix<M, N, T> result;
838  for(unsigned int m = 0; m < M; m++) {
839  for(unsigned int n = 0; n < N; n++) {
840  result(m, n) = left / right(m, n);
841  }
842  }
843  return result;
844  }
845 
846  /********** Print functions **********/
847 
848  template < unsigned int M, unsigned int N = 1, typename T = double >
849  std::ostream& operator<<(std::ostream& os, const Matrix<M, N, T>& right) {
850  if(N > 1) os << "[ ";
851  for(unsigned int n = 0; n < N; n++) {
852  os << '[';
853  for(unsigned int m = 0; m < M; m++) {
854  os << right(m, n);
855  if(m < M - 1) os << ' ';
856  }
857  os << "]' ";
858  }
859  if(N > 1) os << "]";
860  return os;
861  }
862 
863  /********** Type definitions **********/
864 
868 
869  template < unsigned int M, typename T = double >
871 
872  /********** Specialization for a 1x1 matrix **********/
873 
874  template < typename T >
875  class Matrix<1, 1, T> {
876 
877  public:
878 
879  using value_type = T;
880 
881  Matrix() { }
882 
883  Matrix(const T v) { value = v; }
884 
885  void zero() { value = 0; }
886 
887  void eye() { value = 1; }
888 
889  void fill(T v) { value = v; }
890 
891  const T get(uint8_t m, uint8_t n) const { return (*this)(m, n); }
892 
893  Matrix<1, 1, T> getCol(uint8_t n) const { return (*this); }
894 
895  Matrix<1, 1, T> getRow(uint8_t m) const { return (*this); }
896 
897  void set(uint8_t m, uint8_t n, T value) { (*this)(m, n) = value; }
898 
899  T& operator()(uint8_t m, uint8_t n) { if(m == 0 && n == 0) return value; else throw MatrixIndexOutOfBoundException(m, 1, n, 1); }
900 
901  const T operator()(uint8_t m, uint8_t n) const { if(m == 0 && n == 0) return value; else throw MatrixIndexOutOfBoundException(m, 1, n, 1); }
902 
903  T& operator()(unsigned int i) { if(i == 0) return value; else throw MatrixIndexOutOfBoundException(i, 1); }
904 
905  const T operator()(unsigned int i) const { if(i == 0) return value; else throw MatrixIndexOutOfBoundException(i, 1); }
906 
907  T& operator[](unsigned int i) { if(i == 0) return value; else throw MatrixIndexOutOfBoundException(i, 1); }
908 
909  const T operator[](unsigned int i) const { if(i == 0) return value; else throw MatrixIndexOutOfBoundException(i, 1); }
910 
911  constexpr bool isSquare() const { return true; }
912 
913  bool isOrthogonal() const { return value == 1; }
914 
915  constexpr bool isSymmetric() const { return true; }
916 
917  constexpr bool isDiagonal() const { return true; }
918 
919  constexpr bool isLowerTriangular() const { return true; }
920 
921  constexpr bool isUpperTriangular() const { return true; }
922 
923  bool isInvertible() const { return value != 0; }
924 
925  constexpr unsigned int getNofRows() const { return 1; }
926 
927  constexpr unsigned int getNofColums() const { return 1; }
928 
929  constexpr unsigned int size() const { return 1; }
930 
931  unsigned int rank() const { return (value == 0) ? 1 : 0; }
932 
933  T det() const { return value; }
934 
935  T trace() const { return value; }
936 
937  Matrix<1, 1, T> operator!() const { Matrix<1, 1, T> inv(1/value); return inv; }
938 
939  operator T() const { return value; }
940 
941  protected:
942  T value;
943  };
944 
945  } // END namespace math
946 } // END namespache eeros
947 
948 #endif /* ORG_EEROS_MATH_MATRIX_HPP_ */
T & operator()(unsigned int i)
Definition: Matrix.hpp:207
bool isUpperTriangular() const
Definition: Matrix.hpp:287
bool isDiagonal() const
Definition: Matrix.hpp:261
MatrixInitializer< IM, IN, IT > operator,(IT right)
Definition: Matrix.hpp:27
Matrix< M, N, T > & operator+=(const Matrix< M, N, T > right)
Definition: Matrix.hpp:539
unsigned int rank() const
Definition: Matrix.hpp:317
Matrix< 1, 1, T > operator!() const
Definition: Matrix.hpp:937
Matrix< IM, IN, IT > & mat
Definition: Matrix.hpp:32
void set(uint8_t m, uint8_t n, T value)
Definition: Matrix.hpp:897
T trace() const
Definition: Matrix.hpp:935
static Matrix< 3, 1, T > createVector3(T x, T y, T z)
Definition: Matrix.hpp:696
constexpr bool isLowerTriangular() const
Definition: Matrix.hpp:919
Definition: MatrixIndexOutOfBoundException.hpp:9
constexpr unsigned int getNofRows() const
Definition: Matrix.hpp:305
Matrix< N, M, T > transpose() const
Definition: Matrix.hpp:399
void roty(double angle)
Definition: Matrix.hpp:89
bool operator>(const Matrix< M, N, T > &right) const
Definition: Matrix.hpp:454
Matrix< M, N, T > operator+(const Matrix< M, N, T > right) const
Definition: Matrix.hpp:519
const T operator[](unsigned int i) const
Definition: Matrix.hpp:909
constexpr bool isSquare() const
Definition: Matrix.hpp:245
bool isOrthogonal() const
Definition: Matrix.hpp:913
constexpr unsigned int getNofColums() const
Definition: Matrix.hpp:309
constexpr bool isSymmetric() const
Definition: Matrix.hpp:915
Matrix< M, N, T > & operator=(T right)
Definition: Matrix.hpp:476
Matrix< M, N, T > operator!() const
Definition: Matrix.hpp:589
void eye()
Definition: Matrix.hpp:887
Definition: EEROSException.hpp:9
static Matrix< 3, 3, T > createSkewSymmetric(Matrix< 3, 1, T > a)
Definition: Matrix.hpp:710
int v
Definition: RingBufferTest.cpp:14
Matrix< M, N, T > operator-()
Definition: Matrix.hpp:569
void swapRows(unsigned int rowA, unsigned int rowB)
Definition: Matrix.hpp:776
bool isInvertible() const
Definition: Matrix.hpp:300
bool operator>=(const Matrix< M, N, T > &right) const
Definition: Matrix.hpp:465
Matrix< M, N, T > operator-(const T right) const
Definition: Matrix.hpp:554
const T operator()(unsigned int m, unsigned int n) const
Definition: Matrix.hpp:198
constexpr bool isSquare() const
Definition: Matrix.hpp:911
T & operator()(unsigned int i)
Definition: Matrix.hpp:903
Matrix< M, 1, T > getCol(unsigned int n) const
Definition: Matrix.hpp:140
void setRow(unsigned int m, const Matrix< 1, N, T > &row)
Definition: Matrix.hpp:183
static Matrix< 3, 1, T > crossProduct(Matrix< 3, 1, T > a, Matrix< 3, 1, T > b)
Definition: Matrix.hpp:724
Matrix< M, N, T > operator*(T right) const
Definition: Matrix.hpp:499
T det() const
Definition: Matrix.hpp:933
T & operator[](unsigned int i)
Definition: Matrix.hpp:907
Matrix< 1, 1, T > getRow(uint8_t m) const
Definition: Matrix.hpp:895
const T operator()(unsigned int i) const
Definition: Matrix.hpp:216
Matrix< M, N, T > operator+(const T right) const
Definition: Matrix.hpp:529
T value_type
Definition: Matrix.hpp:20
T & operator[](unsigned int i)
Definition: Matrix.hpp:225
Matrix()
Definition: Matrix.hpp:881
bool isSymmetric() const
Definition: Matrix.hpp:257
unsigned int rank() const
Definition: Matrix.hpp:931
void zero()
Definition: Matrix.hpp:885
constexpr bool isDiagonal() const
Definition: Matrix.hpp:917
T & operator()(uint8_t m, uint8_t n)
Definition: Matrix.hpp:899
Matrix< 3, 1 > Vector3
Definition: Matrix.hpp:866
static Matrix< M, N, T > createRotZ(double angle)
Definition: Matrix.hpp:683
Matrix< M, K, T > operator*(const Matrix< N, K, T > right) const
Definition: Matrix.hpp:486
Definition: Matrix.hpp:875
bool isInvertible() const
Definition: Matrix.hpp:923
void rotx(double angle)
Definition: Matrix.hpp:69
constexpr unsigned int size() const
Definition: Matrix.hpp:313
Matrix< M, N, T > operator/(T right) const
Definition: Matrix.hpp:579
const T operator[](unsigned int i) const
Definition: Matrix.hpp:234
constexpr unsigned int getNofRows() const
Definition: Matrix.hpp:925
Matrix< 2, 1 > Vector2
Definition: Matrix.hpp:865
static Matrix< M, N, T > createRotX(double angle)
Definition: Matrix.hpp:671
Matrix< 1, N, T > getRow(unsigned int m) const
Definition: Matrix.hpp:148
void sortForGaussAlgorithm()
Definition: Matrix.hpp:757
T det() const
Definition: Matrix.hpp:332
MatrixInitializer(Matrix< IM, IN, IT > &mat, unsigned int index=0)
Definition: Matrix.hpp:25
Matrix< M, N, T > operator-(const Matrix< M, N, T > right) const
Definition: Matrix.hpp:544
void eye()
Definition: Matrix.hpp:57
Definition: Matrix.hpp:15
constexpr unsigned int size() const
Definition: Matrix.hpp:929
T value[M *N]
Definition: Matrix.hpp:785
int j
Definition: RingBufferTest.cpp:13
void gaussRowElimination()
Definition: Matrix.hpp:734
bool operator==(const Matrix< M, N, T > &right) const
Definition: Matrix.hpp:411
Matrix()
Definition: Matrix.hpp:38
void fill(T v)
Definition: Matrix.hpp:65
Matrix(const T v)
Definition: Matrix.hpp:40
T value_type
Definition: Matrix.hpp:879
bool isLowerTriangular() const
Definition: Matrix.hpp:274
void zero()
Definition: Matrix.hpp:51
unsigned int index
Definition: Matrix.hpp:33
T & operator()(unsigned int m, unsigned int n)
Definition: Matrix.hpp:189
static Matrix< M, N, T > createRotY(double angle)
Definition: Matrix.hpp:677
Matrix< 1, 1, T > getCol(uint8_t n) const
Definition: Matrix.hpp:893
Matrix< M, N, T > multiplyElementWise(const Matrix< M, N, T > right) const
Definition: Matrix.hpp:509
void setCol(unsigned int n, const Matrix< M, 1, T > &col)
Definition: Matrix.hpp:177
const T operator()(unsigned int i) const
Definition: Matrix.hpp:905
constexpr bool isUpperTriangular() const
Definition: Matrix.hpp:921
Matrix(const T v)
Definition: Matrix.hpp:883
Matrix< U, V, T > getSubMatrix(unsigned int m, unsigned int n) const
Definition: Matrix.hpp:157
const T operator()(uint8_t m, uint8_t n) const
Definition: Matrix.hpp:901
int i
Definition: RingBufferTest.cpp:12
void fill(T v)
Definition: Matrix.hpp:889
bool operator!=(const Matrix< M, N, T > &right) const
Definition: Matrix.hpp:421
void rotz(double angle)
Definition: Matrix.hpp:109
Matrix< M, N, T > & operator-=(const Matrix< M, N, T > right)
Definition: Matrix.hpp:564
static Matrix< M, N, T > createDiag(T v)
Definition: Matrix.hpp:704
void set(unsigned int m, unsigned int n, T value)
Definition: Matrix.hpp:173
Matrix< 4, 1 > Vector4
Definition: Matrix.hpp:867
T trace() const
Definition: Matrix.hpp:390
constexpr unsigned int getNofColums() const
Definition: Matrix.hpp:927
bool isOrthogonal() const
Definition: Matrix.hpp:249
MatrixInitializer< M, N, T > operator<<(T right)
Definition: Matrix.hpp:129
Matrix(const S...v)
Definition: Matrix.hpp:45
T value
Definition: Matrix.hpp:942
static Matrix< 2, 1, T > createVector2(T x, T y)
Definition: Matrix.hpp:689