diff --git a/CHANGELOG.md b/CHANGELOG.md index 108f956f3c8..ffe4fbba400 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added - Add the possibility to plot and update frames in the Matlab visualizer. +- Add the possibility to extract submatrix with MatrixView (https://github.com/robotology/idyntree/pull/800) ## [2.0.2] - 2020-12-04 diff --git a/src/core/include/iDynTree/Core/MatrixView.h b/src/core/include/iDynTree/Core/MatrixView.h index 3a7eadc5878..a2e1aa1b722 100644 --- a/src/core/include/iDynTree/Core/MatrixView.h +++ b/src/core/include/iDynTree/Core/MatrixView.h @@ -77,14 +77,17 @@ namespace iDynTree MatrixStorageOrdering m_storageOrder; + index_type m_innerStride; + index_type m_outerStride; + index_type rawIndex(index_type row, index_type col) const { if (m_storageOrder == MatrixStorageOrdering::RowMajor) { - return (col + this->m_cols * row); + return (col + this->m_outerStride * row); } else { - return (this->m_rows * col + row); + return (this->m_outerStride * col + row); } } @@ -174,6 +177,8 @@ namespace iDynTree , m_rows(in_rows) , m_cols(in_cols) , m_storageOrder(order) + , m_innerStride(1) + , m_outerStride(order == MatrixStorageOrdering::RowMajor ? in_cols : in_rows) { } @@ -210,6 +215,25 @@ namespace iDynTree return this->m_cols; } ///@} + + MatrixView block(index_type startingRow, index_type startingColumn, + index_type rows, index_type cols) const + { + assert(rows <= this->rows()); + assert(cols <= this->cols()); + + const index_type offset = rawIndex(startingRow, startingColumn); + assert(offset < this->rows() * this->cols()); + + MatrixView block; + block = *this; + block.m_rows = rows; + block.m_cols = cols; + block.m_storage = this->m_storage + offset; + + return block; + } + }; template diff --git a/src/core/tests/MatrixViewUnitTest.cpp b/src/core/tests/MatrixViewUnitTest.cpp index 61e200e18d9..23a3d31f01e 100644 --- a/src/core/tests/MatrixViewUnitTest.cpp +++ b/src/core/tests/MatrixViewUnitTest.cpp @@ -29,27 +29,41 @@ int main() // test with iDynTree matrix MatrixDynSize mat1(27, 41); getRandomMatrix(mat1); - MatrixView view1(mat1); + MatrixDynSize subMat1(10, 23); + toEigen(subMat1) = toEigen(mat1).block(3, 5, 10, 23); + MatrixView view1(mat1); areMatricesEqual(view1, mat1); + auto subView1 = view1.block(3, 5, 10, 23); + areMatricesEqual(subView1, subMat1); + // Test with eigen column major matrix Eigen::MatrixXd mat2(27, 41); mat2.setRandom(); - MatrixView view2(mat2); + Eigen::MatrixXd subMat2; + subMat2 = mat2.block(3, 5, 10, 23); + MatrixView view2(mat2); areMatricesEqual(view2, mat2); + auto subView2 = view2.block(3, 5, 10, 23); + areMatricesEqual(subView2, subMat2); + // Test with eigen row major matrix Eigen::Matrix mat3(27, 41); mat3.setRandom(); - MatrixView view3(mat3); + Eigen::Matrix subMat3; + subMat3 = mat3.block(3, 5, 10, 23); + MatrixView view3(mat3); areMatricesEqual(view3, mat3); auto view4 = make_matrix_view(mat3); areMatricesEqual(view4, mat3); + auto subView3 = view3.block(3, 5, 10, 23); + areMatricesEqual(subView3, subMat3); return 0; }