matrix.h
Go to the documentation of this file.
1 
9 #ifndef MATRIX_H
10 #define MATRIX_H
11 
12 #include <argos3/core/utility/math/general.h>
13 #include <argos3/core/utility/math/angles.h>
14 #include <cmath>
15 #include <iomanip>
16 
17 namespace argos {
18 
19  template <UInt32 ROWS, UInt32 COLS>
20  class CMatrix {
21 
22  /* Matrices of different dimensions can access each others data */
23  template <UInt32 SMROWS, UInt32 SMCOLS> friend class CMatrix;
24 
25  /* The specified derived classes are allowed to access members of the base class */
26  friend class CRotationMatrix2;
27  friend class CTransformationMatrix2;
28  friend class CRotationMatrix3;
29  friend class CTransformationMatrix3;
30 
31  public:
32  CMatrix() {
33  SetZero();
34  }
35 
36  CMatrix(const Real* pf_values) {
37  Set(pf_values);
38  }
39 
40  CMatrix(const CMatrix<ROWS,COLS>& c_matrix) {
41  Set(c_matrix.m_pfValues);
42  }
43 
45  if(this != &c_matrix) {
46  Set(c_matrix.m_pfValues);
47  }
48  return *this;
49  }
50 
51  inline Real& operator()(UInt32 un_row,
52  UInt32 un_col) {
53  ARGOS_ASSERT(un_row < ROWS && un_col < COLS,
54  "Matrix index out of bounds: un_row = " <<
55  un_row <<
56  ", un_col = " <<
57  un_col);
58  return m_pfValues[un_row * COLS + un_col];
59  }
60 
61  inline Real operator()(UInt32 un_row,
62  UInt32 un_col) const {
63  ARGOS_ASSERT(un_row < ROWS && un_col < COLS,
64  "Matrix index out of bounds: un_row = " <<
65  un_row <<
66  ", un_col = " <<
67  un_col);
68  return m_pfValues[un_row * COLS + un_col];
69  }
70 
71  inline Real operator()(UInt32 un_idx) const {
72  ARGOS_ASSERT(un_idx < ROWS * COLS,
73  "Matrix index out of bounds: un_idx = " <<
74  un_idx);
75  return m_pfValues[un_idx];
76  }
77 
78  inline Real& operator()(UInt32 un_idx) {
79  ARGOS_ASSERT(un_idx < ROWS * COLS,
80  "Matrix index out of bounds: un_idx = " <<
81  un_idx);
82  return m_pfValues[un_idx];
83  }
84 
85  void Set(const Real* f_values) {
86  for(UInt32 i = 0; i < ROWS * COLS; i++)
87  m_pfValues[i] = f_values[i];
88  }
89 
90  void SetZero() {
91  for(UInt32 i = 0; i < ROWS * COLS; i++)
92  m_pfValues[i] = 0.0;
93  }
94 
96  Real fNewValues[COLS * ROWS];
97  for(UInt32 i = 0; i < ROWS; i++)
98  for(UInt32 j = 0; j < COLS; j++)
99  fNewValues[j * ROWS + i] = m_pfValues[i * COLS + j];
100 
101  return CMatrix<COLS, ROWS>(fNewValues);
102  }
103 
104  template <UInt32 SMROWS, UInt32 SMCOLS>
106  UInt32 un_offset_row,
107  UInt32 un_offset_col) const {
108  ARGOS_ASSERT((SMROWS - 1 + un_offset_row) < ROWS &&
109  (SMCOLS - 1 + un_offset_col) < COLS,
110  "Submatrix range is out of bounds: cannot extract a " <<
111  SMROWS << "x" << SMCOLS << " submatrix from a " <<
112  ROWS << "x" << COLS << " matrix with offsets " <<
113  " un_offset_row = " <<
114  un_offset_row <<
115  ", un_offset_col = " <<
116  un_offset_col);
117 
118  for(UInt32 i = 0; i < SMROWS; i++)
119  for(UInt32 j = 0; j < SMCOLS; j++)
120  c_sub_matrix.m_pfValues[i * SMCOLS + j] = m_pfValues[(i + un_offset_row) * COLS + (j + un_offset_col)];
121  }
122 
123  bool operator==(const CMatrix<ROWS, COLS>& c_matrix) const {
124  for(UInt32 i = 0; i < ROWS * COLS; i++) {
125  if(m_pfValues[i] != c_matrix.m_pfValues[i])
126  return false;
127  }
128  return true;
129  }
130 
132  for(UInt32 i = 0; i < ROWS * COLS; i++) {
133  m_pfValues[i] += c_matrix.m_pfValues[i];
134  }
135  return *this;
136  }
137 
139  for(UInt32 i = 0; i < ROWS * COLS; i++) {
140  m_pfValues[i] -= c_matrix.m_pfValues[i];
141  }
142  return *this;
143  }
144 
146  for(UInt32 i = 0; i < ROWS * COLS; i++) {
147  m_pfValues[i] *= f_scale;
148  }
149  return *this;
150  }
151 
153  CMatrix<ROWS, COLS> cResult = (*this);
154  cResult += c_matrix;
155  return cResult;
156  }
157 
159  CMatrix<ROWS, COLS> cResult = (*this);
160  cResult -= c_matrix;
161  return cResult;
162  }
163 
165  CMatrix<ROWS, COLS> cResult;
166  cResult -= (*this);
167  return cResult;
168  }
169 
171  Real fNewValues[ROWS * COLS];
172  for(UInt32 i = 0; i < ROWS; i++) {
173  for(UInt32 j = 0; j < COLS; j++) {
174  fNewValues[i * COLS + j] = 0.0f;
175  for(UInt32 k = 0; k < COLS; k++) {
176  fNewValues[i * COLS + j] += m_pfValues[i * COLS + k] * c_matrix.m_pfValues[k * COLS + j];
177  }
178  }
179  }
180  Set(fNewValues);
181  return *this;
182  }
183 
184  template <UInt32 OTRCOLS>
186  Real fNewValues[ROWS * OTRCOLS];
187  for(UInt32 i = 0; i < ROWS; i++) {
188  for(UInt32 j = 0; j < OTRCOLS; j++) {
189  fNewValues[i * OTRCOLS + j] = 0.0f;
190  for(UInt32 k = 0; k < COLS; k++) {
191  fNewValues[i * OTRCOLS + j] += m_pfValues[i * COLS + k] * c_matrix.m_pfValues[k * OTRCOLS + j];
192  }
193  }
194  }
195  return CMatrix<ROWS, OTRCOLS>(fNewValues);
196  }
197 
198  friend std::ostream& operator<<(std::ostream& c_os,
199  const CMatrix& c_matrix) {
200 
201  std::ios_base::fmtflags unInitalFlags = c_os.flags();
202  std::streamsize nInitalPrec = c_os.precision();
203 
204  c_os.setf(std::ios::fixed);
205  c_os.precision(2);
206 
207  for(UInt32 i = 0; i < ROWS; i++) {
208  c_os << "| ";
209  for(UInt32 j = 0; j < COLS; j++) {
210  c_os << std::setw(6) << c_matrix(i, j) << " ";
211  }
212  c_os << "|" << std::endl;
213  }
214 
215  c_os.flags(unInitalFlags);
216  c_os.precision(nInitalPrec);
217  return c_os;
218  }
219 
220  protected:
221 
222  Real m_pfValues[ROWS * COLS];
223 
224  };
225 }
226 
227 #endif
CMatrix< ROWS, COLS > operator-() const
Definition: matrix.h:164
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
CMatrix< COLS, ROWS > GetTransposed() const
Definition: matrix.h:95
void SetZero()
Definition: matrix.h:90
CMatrix< ROWS, COLS > & operator+=(const CMatrix< ROWS, COLS > &c_matrix)
Definition: matrix.h:131
Real & operator()(UInt32 un_row, UInt32 un_col)
Definition: matrix.h:51
CMatrix< ROWS, COLS > & operator*=(const CMatrix< COLS, COLS > &c_matrix)
Definition: matrix.h:170
bool operator==(const CMatrix< ROWS, COLS > &c_matrix) const
Definition: matrix.h:123
CMatrix< ROWS, COLS > & operator-=(const CMatrix< ROWS, COLS > &c_matrix)
Definition: matrix.h:138
#define ARGOS_ASSERT(condition, message)
When code is compiled in debug, this macro throws an ARGoS exception with the passed message if the s...
unsigned int UInt32
32-bit unsigned integer.
Definition: datatypes.h:97
CMatrix< ROWS, COLS > operator+(const CMatrix< ROWS, COLS > &c_matrix) const
Definition: matrix.h:152
Real operator()(UInt32 un_idx) const
Definition: matrix.h:71
void Set(const Real *f_values)
Definition: matrix.h:85
Real m_pfValues[ROWS *COLS]
Definition: matrix.h:222
Real & operator()(UInt32 un_idx)
Definition: matrix.h:78
CMatrix(const Real *pf_values)
Definition: matrix.h:36
void GetSubMatrix(CMatrix< SMROWS, SMCOLS > &c_sub_matrix, UInt32 un_offset_row, UInt32 un_offset_col) const
Definition: matrix.h:105
CMatrix< ROWS, COLS > & operator=(const CMatrix< ROWS, COLS > &c_matrix)
Definition: matrix.h:44
Real operator()(UInt32 un_row, UInt32 un_col) const
Definition: matrix.h:61
CMatrix(const CMatrix< ROWS, COLS > &c_matrix)
Definition: matrix.h:40
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
CMatrix< ROWS, OTRCOLS > operator*(const CMatrix< COLS, OTRCOLS > &c_matrix) const
Definition: matrix.h:185
CMatrix< ROWS, COLS > operator-(const CMatrix< ROWS, COLS > &c_matrix) const
Definition: matrix.h:158
CMatrix< ROWS, COLS > & operator*=(Real f_scale)
Definition: matrix.h:145
friend std::ostream & operator<<(std::ostream &c_os, const CMatrix &c_matrix)
Definition: matrix.h:198