12 #include <argos3/core/utility/math/general.h>
13 #include <argos3/core/utility/math/angles.h>
19 template <UInt32 ROWS, UInt32 COLS>
23 template <UInt32 SMROWS, UInt32 SMCOLS>
friend class CMatrix;
33 for(
UInt32 i = 0; i < ROWS * COLS; i++)
46 if(
this != &c_matrix) {
55 "Matrix index out of bounds: un_row = " <<
65 "Matrix index out of bounds: un_row = " <<
74 "Matrix index out of bounds: un_idx = " <<
81 "Matrix index out of bounds: un_idx = " <<
87 for(
UInt32 i = 0; i < ROWS * COLS; i++)
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];
100 template <UInt32 SMROWS, UInt32 SMCOLS>
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 = " <<
111 ", un_offset_col = " <<
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)];
122 for(
UInt32 i = 0; i < ROWS * COLS; i++) {
130 for(
UInt32 i = 0; i < ROWS * COLS; i++) {
137 for(
UInt32 i = 0; i < ROWS * COLS; i++) {
144 for(
UInt32 i = 0; i < ROWS * COLS; i++) {
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++) {
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++) {
193 std::ios_base::fmtflags unInitalFlags = c_os.flags();
194 std::streamsize nInitalPrec = c_os.precision();
196 c_os.setf(std::ios::fixed);
199 for(
UInt32 i = 0; i < ROWS; i++) {
201 for(
UInt32 j = 0; j < COLS; j++) {
202 c_os << std::setw(6) << c_matrix(i, j) <<
" ";
204 c_os <<
"|" << std::endl;
207 c_os.flags(unInitalFlags);
208 c_os.precision(nInitalPrec);