ARGoS  3
A parallel, multi-engine simulator for swarm robotics
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  for(UInt32 i = 0; i < ROWS * COLS; i++)
34  m_pfValues[i] = 0;
35  }
36 
37  CMatrix(const Real* pf_values) {
38  Set(pf_values);
39  }
40 
41  CMatrix(const CMatrix<ROWS,COLS>& c_matrix) {
42  Set(c_matrix.m_pfValues);
43  }
44 
46  if(this != &c_matrix) {
47  Set(c_matrix.m_pfValues);
48  }
49  return *this;
50  }
51 
52  inline Real& operator()(UInt32 un_row,
53  UInt32 un_col) {
54  ARGOS_ASSERT(un_row < ROWS && un_col < COLS,
55  "Matrix index out of bounds: un_row = " <<
56  un_row <<
57  ", un_col = " <<
58  un_col);
59  return m_pfValues[un_row * COLS + un_col];
60  }
61 
62  inline Real operator()(UInt32 un_row,
63  UInt32 un_col) const {
64  ARGOS_ASSERT(un_row < ROWS && un_col < COLS,
65  "Matrix index out of bounds: un_row = " <<
66  un_row <<
67  ", un_col = " <<
68  un_col);
69  return m_pfValues[un_row * COLS + un_col];
70  }
71 
72  inline Real operator()(UInt32 un_idx) const {
73  ARGOS_ASSERT(un_idx < ROWS * COLS,
74  "Matrix index out of bounds: un_idx = " <<
75  un_idx);
76  return m_pfValues[un_idx];
77  }
78 
79  inline Real& operator()(UInt32 un_idx) {
80  ARGOS_ASSERT(un_idx < ROWS * COLS,
81  "Matrix index out of bounds: un_idx = " <<
82  un_idx);
83  return m_pfValues[un_idx];
84  }
85 
86  void Set(const Real* f_values) {
87  for(UInt32 i = 0; i < ROWS * COLS; i++)
88  m_pfValues[i] = f_values[i];
89  }
90 
92  Real fNewValues[COLS * ROWS];
93  for(UInt32 i = 0; i < ROWS; i++)
94  for(UInt32 j = 0; j < COLS; j++)
95  fNewValues[j * ROWS + i] = m_pfValues[i * COLS + j];
96 
97  return CMatrix<COLS, ROWS>(fNewValues);
98  }
99 
100  template <UInt32 SMROWS, UInt32 SMCOLS>
102  UInt32 un_offset_row,
103  UInt32 un_offset_col) {
104  ARGOS_ASSERT((SMROWS - 1 + un_offset_row) < ROWS &&
105  (SMCOLS - 1 + un_offset_col) < COLS,
106  "Submatrix range is out of bounds: cannot extract a " <<
107  SMROWS << "x" << SMCOLS << " submatrix from a " <<
108  ROWS << "x" << COLS << " matrix with offsets " <<
109  " un_offset_row = " <<
110  un_offset_row <<
111  ", un_offset_col = " <<
112  un_offset_col);
113 
114  for(UInt32 i = 0; i < SMROWS; i++)
115  for(UInt32 j = 0; j < SMCOLS; j++)
116  c_matrix.m_pfValues[i * SMCOLS + j] = m_pfValues[(i + un_offset_row) * COLS + (j + un_offset_col)];
117 
118  return c_matrix;
119  }
120 
121  bool operator==(const CMatrix<ROWS, COLS>& c_matrix) const {
122  for(UInt32 i = 0; i < ROWS * COLS; i++) {
123  if(m_pfValues[i] != c_matrix.m_pfValues[i])
124  return false;
125  }
126  return true;
127  }
128 
130  for(UInt32 i = 0; i < ROWS * COLS; i++) {
131  m_pfValues[i] += c_matrix.m_pfValues[i];
132  }
133  return *this;
134  }
135 
137  for(UInt32 i = 0; i < ROWS * COLS; i++) {
138  m_pfValues[i] -= c_matrix.m_pfValues[i];
139  }
140  return *this;
141  }
142 
144  for(UInt32 i = 0; i < ROWS * COLS; i++) {
145  m_pfValues[i] *= f_scale;
146  }
147  return *this;
148  }
149 
151  CMatrix<ROWS, COLS> cResult = (*this);
152  cResult += c_matrix;
153  return cResult;
154  }
155 
157  CMatrix<ROWS, COLS> cResult = (*this);
158  cResult -= c_matrix;
159  return cResult;
160  }
161 
163  Real fNewValues[ROWS * COLS];
164  for(UInt32 i = 0; i < ROWS; i++) {
165  for(UInt32 j = 0; j < COLS; j++) {
166  fNewValues[i * COLS + j] = 0.0f;
167  for(UInt32 k = 0; k < COLS; k++) {
168  fNewValues[i * COLS + j] += m_pfValues[i * COLS + k] * c_matrix.m_pfValues[k * COLS + j];
169  }
170  }
171  }
172  Set(fNewValues);
173  return *this;
174  }
175 
176  template <UInt32 OTRCOLS>
178  Real fNewValues[ROWS * OTRCOLS];
179  for(UInt32 i = 0; i < ROWS; i++) {
180  for(UInt32 j = 0; j < OTRCOLS; j++) {
181  fNewValues[i * OTRCOLS + j] = 0.0f;
182  for(UInt32 k = 0; k < COLS; k++) {
183  fNewValues[i * OTRCOLS + j] += m_pfValues[i * COLS + k] * c_matrix.m_pfValues[k * OTRCOLS + j];
184  }
185  }
186  }
187  return CMatrix<ROWS, OTRCOLS>(fNewValues);
188  }
189 
190  friend std::ostream& operator<<(std::ostream& c_os,
191  const CMatrix& c_matrix) {
192 
193  std::ios_base::fmtflags unInitalFlags = c_os.flags();
194  std::streamsize nInitalPrec = c_os.precision();
195 
196  c_os.setf(std::ios::fixed);
197  c_os.precision(1);
198 
199  for(UInt32 i = 0; i < ROWS; i++) {
200  c_os << "| ";
201  for(UInt32 j = 0; j < COLS; j++) {
202  c_os << std::setw(6) << c_matrix(i, j) << " ";
203  }
204  c_os << "|" << std::endl;
205  }
206 
207  c_os.flags(unInitalFlags);
208  c_os.precision(nInitalPrec);
209  return c_os;
210  }
211 
212  protected:
213 
214  Real m_pfValues[ROWS * COLS];
215 
216  };
217 }
218 
219 #endif
argos::CTransformationMatrix2
Definition: transformationmatrix2.h:21
argos::CMatrix::operator()
Real & operator()(UInt32 un_row, UInt32 un_col)
Definition: matrix.h:52
argos::CRotationMatrix2
Definition: rotationmatrix2.h:21
argos::CMatrix::operator()
Real operator()(UInt32 un_row, UInt32 un_col) const
Definition: matrix.h:62
argos::CMatrix::GetTransposed
CMatrix< COLS, ROWS > GetTransposed()
Definition: matrix.h:91
argos
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
argos::CMatrix::operator=
CMatrix< ROWS, COLS > & operator=(const CMatrix< ROWS, COLS > &c_matrix)
Definition: matrix.h:45
argos::CMatrix::operator*=
CMatrix< ROWS, COLS > & operator*=(Real f_scale)
Definition: matrix.h:143
argos::CTransformationMatrix3
Definition: transformationmatrix3.h:21
argos::CMatrix::operator()
Real & operator()(UInt32 un_idx)
Definition: matrix.h:79
argos::CMatrix::operator<<
friend std::ostream & operator<<(std::ostream &c_os, const CMatrix &c_matrix)
Definition: matrix.h:190
argos::CMatrix::operator-=
CMatrix< ROWS, COLS > & operator-=(const CMatrix< ROWS, COLS > &c_matrix)
Definition: matrix.h:136
argos::CMatrix::CMatrix
CMatrix(const CMatrix< ROWS, COLS > &c_matrix)
Definition: matrix.h:41
argos::CMatrix::operator+=
CMatrix< ROWS, COLS > & operator+=(const CMatrix< ROWS, COLS > &c_matrix)
Definition: matrix.h:129
argos::CMatrix::CMatrix
CMatrix(const Real *pf_values)
Definition: matrix.h:37
argos::CMatrix::operator==
bool operator==(const CMatrix< ROWS, COLS > &c_matrix) const
Definition: matrix.h:121
argos::CMatrix::m_pfValues
Real m_pfValues[ROWS *COLS]
Definition: matrix.h:214
argos::CMatrix
Definition: matrix.h:20
ARGOS_ASSERT
#define ARGOS_ASSERT(condition, message)
When code is compiled in debug, this macro throws an ARGoS exception with the passed message if the s...
Definition: argos_exception.h:122
argos::CMatrix::operator*
CMatrix< ROWS, OTRCOLS > operator*(const CMatrix< COLS, OTRCOLS > &c_matrix) const
Definition: matrix.h:177
argos::CMatrix::operator()
Real operator()(UInt32 un_idx) const
Definition: matrix.h:72
UInt32
unsigned int UInt32
32-bit unsigned integer.
Definition: datatypes.h:97
argos::CMatrix::operator+
CMatrix< ROWS, COLS > operator+(const CMatrix< ROWS, COLS > &c_matrix) const
Definition: matrix.h:150
argos::CRotationMatrix3
Definition: rotationmatrix3.h:21
argos::CMatrix::operator*=
CMatrix< ROWS, COLS > & operator*=(const CMatrix< COLS, COLS > &c_matrix)
Definition: matrix.h:162
argos::CMatrix::operator-
CMatrix< ROWS, COLS > operator-(const CMatrix< ROWS, COLS > &c_matrix) const
Definition: matrix.h:156
argos::CMatrix::CMatrix
CMatrix()
Definition: matrix.h:32
Real
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
argos::CMatrix::Set
void Set(const Real *f_values)
Definition: matrix.h:86
argos::CMatrix::GetSubmatrix
CMatrix< SMROWS, SMCOLS > & GetSubmatrix(CMatrix< SMROWS, SMCOLS > &c_matrix, UInt32 un_offset_row, UInt32 un_offset_col)
Definition: matrix.h:101