From f39776f745df976a555ebf3acc7405bf26843454 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Thu, 25 Jan 2024 05:12:06 +0100 Subject: [PATCH] [LinearAlgebra] Factorize value filtering (#4442) * [LinearAlgebra] Factorize value filtering No more template specialization * Compatibility with all CompressedRowSparseMatrixGeneric including CompressedRowSparseMatrixConstraint --------- Co-authored-by: Paul Baksic <30337881+bakpaul@users.noreply.github.com> --- .../src/sofa/defaulttype/RigidDeriv.h | 6 ++ .../CompressedRowSparseMatrixMechanical.cpp | 94 ------------------- .../CompressedRowSparseMatrixMechanical.h | 86 +++++++++++------ .../sofa/linearalgebra/matrix_bloc_traits.h | 36 +++++++ .../test/CompressedRowSparseMatrix_test.cpp | 72 +++++++++++++- Sofa/framework/Type/src/sofa/type/Mat.h | 14 +++ Sofa/framework/Type/src/sofa/type/Vec.h | 14 +++ 7 files changed, 198 insertions(+), 124 deletions(-) diff --git a/Sofa/framework/DefaultType/src/sofa/defaulttype/RigidDeriv.h b/Sofa/framework/DefaultType/src/sofa/defaulttype/RigidDeriv.h index ff3ff6edcf6..a8175cda15a 100644 --- a/Sofa/framework/DefaultType/src/sofa/defaulttype/RigidDeriv.h +++ b/Sofa/framework/DefaultType/src/sofa/defaulttype/RigidDeriv.h @@ -606,6 +606,12 @@ namespace sofa::linearalgebra static void transpose(BlockTranspose& res, const Block& b) { res = b; } + template, bool> = true> + static void subBlock(const Block& b, IndexType row, IndexType col, TSubBlock& subBlock) + { + subBlock = b[col]; + } + static sofa::linearalgebra::BaseMatrix::ElementType getElementType() { return matrix_bloc_traits::getElementType(); } static const char* Name() { return defaulttype::DataTypeName>::name(); } }; diff --git a/Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/CompressedRowSparseMatrixMechanical.cpp b/Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/CompressedRowSparseMatrixMechanical.cpp index b73b3662a99..b3de05353fb 100644 --- a/Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/CompressedRowSparseMatrixMechanical.cpp +++ b/Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/CompressedRowSparseMatrixMechanical.cpp @@ -63,100 +63,6 @@ void CompressedRowSparseMatrixMechanical >::add(Index row addBlockMat(*this, row, col, _M); } - -template -void filterValuesImpl( - CompressedRowSparseMatrixMechanical& dest, - CompressedRowSparseMatrixMechanical >& src, - typename CompressedRowSparseMatrixMechanical::filter_fn* filter, - const RealDest ref, const bool keepEmptyRows) -{ - src.compress(); - dest.nRow = src.rowSize(); - dest.nCol = src.colSize(); - dest.nBlockRow = src.rowSize(); - dest.nBlockCol = src.colSize(); - dest.rowIndex.clear(); - dest.rowBegin.clear(); - dest.colsIndex.clear(); - dest.colsValue.clear(); - dest.btemp.clear(); - dest.skipCompressZero = true; - dest.rowIndex.reserve(src.rowIndex.size() * 3); - dest.rowBegin.reserve(src.rowBegin.size() * 3); - dest.colsIndex.reserve(src.colsIndex.size() * 9); - dest.colsValue.reserve(src.colsValue.size() * 9); - - Index vid = 0; - for (std::size_t rowId = 0; rowId < src.rowIndex.size(); ++rowId) - { - const Index i = src.rowIndex[rowId] * 3; - - typename CompressedRowSparseMatrixMechanical::Range rowRange(src.rowBegin[rowId], src.rowBegin[rowId + 1]); - - for (Index lb = 0; lb < 3; lb++) - { - dest.rowIndex.push_back(i + lb); - dest.rowBegin.push_back(vid); - - for (std::size_t xj = static_cast(rowRange.begin()); xj < static_cast(rowRange.end()); ++xj) - { - const Index j = src.colsIndex[xj] * 3; - type::Mat<3, 3, RealDest> b = src.colsValue[xj]; - if ((*filter)(i + lb, j + 0, b[lb][0], ref)) - { - dest.colsIndex.push_back(j + 0); - dest.colsValue.push_back(b[lb][0]); - ++vid; - } - if ((*filter)(i + lb, j + 1, b[lb][1], ref)) - { - dest.colsIndex.push_back(j + 1); - dest.colsValue.push_back(b[lb][1]); - ++vid; - } - if ((*filter)(i + lb, j + 2, b[lb][2], ref)) - { - dest.colsIndex.push_back(j + 2); - dest.colsValue.push_back(b[lb][2]); - ++vid; - } - } - - if (!keepEmptyRows && dest.rowBegin.back() == vid) // row was empty - { - dest.rowIndex.pop_back(); - dest.rowBegin.pop_back(); - } - } - } - dest.rowBegin.push_back(vid); // end of last row -} - -template <> template <> -void CompressedRowSparseMatrixMechanical::filterValues(CompressedRowSparseMatrixMechanical >& M, filter_fn* filter, const Real ref, bool keepEmptyRows) -{ - filterValuesImpl(*this, M, filter, ref, keepEmptyRows); -} - -template <> template <> -void CompressedRowSparseMatrixMechanical::filterValues(CompressedRowSparseMatrixMechanical >& M, filter_fn* filter, const Real ref, bool keepEmptyRows) -{ - filterValuesImpl(*this, M, filter, ref, keepEmptyRows); -} - -template <> template <> -void CompressedRowSparseMatrixMechanical::filterValues(CompressedRowSparseMatrixMechanical >& M, filter_fn* filter, const Real ref, bool keepEmptyRows) -{ - filterValuesImpl(*this, M, filter, ref, keepEmptyRows); -} - -template <> template <> -void CompressedRowSparseMatrixMechanical::filterValues(CompressedRowSparseMatrixMechanical >& M, filter_fn* filter, const Real ref, bool keepEmptyRows) -{ - filterValuesImpl(*this, M, filter, ref, keepEmptyRows); -} - using namespace sofa::type; template class SOFA_LINEARALGEBRA_API CompressedRowSparseMatrixMechanical; diff --git a/Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/CompressedRowSparseMatrixMechanical.h b/Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/CompressedRowSparseMatrixMechanical.h index 93f6df95d12..e53298da336 100644 --- a/Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/CompressedRowSparseMatrixMechanical.h +++ b/Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/CompressedRowSparseMatrixMechanical.h @@ -506,42 +506,79 @@ class CompressedRowSparseMatrixMechanical final // final is used to allow the co static bool lower_nonsmall(Index i , Index j , Block& val, const Real ref ) { return lower(i,j,val,ref) && nonsmall(i,j,val,ref); } template - void filterValues(TMatrix& M, filter_fn* filter = &nonzeros, const Real ref = Real(), bool keepEmptyRows=false) + void filterValues(TMatrix& srcMatrix, filter_fn* filter = &nonzeros, const Real ref = Real(), bool keepEmptyRows=false) { - M.compress(); - this->nBlockRow = M.rowBSize(); - this->nBlockCol = M.colBSize(); - this->nRow = M.rowSize(); - this->nCol = M.colSize(); + static constexpr auto SrcBlockRows = TMatrix::NL; + static constexpr auto SrcBlockColumns = TMatrix::NC; + + static constexpr auto DstBlockRows = NL; + static constexpr auto DstBlockColumns = NC; + + static_assert(SrcBlockRows % DstBlockRows == 0); + static_assert(SrcBlockColumns % DstBlockColumns == 0); + + static constexpr auto NbBlocksRows = SrcBlockRows / DstBlockRows; + static constexpr auto NbBlocksColumns = SrcBlockColumns / DstBlockColumns; + + if constexpr (TMatrix::Policy::AutoCompress) + { + /// \warning this violates the const-ness of TMatrix ! + const_cast*>(&srcMatrix)->compress(); + } + + this->nRow = srcMatrix.nBlockRow * SrcBlockRows; + this->nCol = srcMatrix.nBlockCol * SrcBlockColumns; + this->nBlockRow = srcMatrix.nBlockRow * NbBlocksRows; + this->nBlockCol = srcMatrix.nBlockCol * NbBlocksColumns; this->rowIndex.clear(); this->rowBegin.clear(); this->colsIndex.clear(); this->colsValue.clear(); this->skipCompressZero = true; this->btemp.clear(); - this->rowIndex.reserve(M.rowIndex.size()); - this->rowBegin.reserve(M.rowBegin.size()); - this->colsIndex.reserve(M.colsIndex.size()); - this->colsValue.reserve(M.colsValue.size()); + this->rowIndex.reserve(srcMatrix.rowIndex.size() * NbBlocksRows); + this->rowBegin.reserve(srcMatrix.rowBegin.size() * NbBlocksRows); + this->colsIndex.reserve(srcMatrix.colsIndex.size() * NbBlocksRows * NbBlocksColumns); + this->colsValue.reserve(srcMatrix.colsValue.size() * NbBlocksRows * NbBlocksColumns); Index vid = 0; - for (Index rowId = 0; rowId < static_cast(M.rowIndex.size()); ++rowId) + for (Index srcRowId = 0; srcRowId < static_cast(srcMatrix.rowIndex.size()); ++srcRowId) { - Index i = M.rowIndex[rowId]; - this->rowIndex.push_back(i); - this->rowBegin.push_back(vid); - Range rowRange(M.rowBegin[rowId], M.rowBegin[rowId+1]); - for (Index xj = rowRange.begin(); xj < rowRange.end(); ++xj) + // row id if blocks were scalars + const Index scalarRowId = srcMatrix.rowIndex[srcRowId] * SrcBlockRows; + + Range rowRange(srcMatrix.rowBegin[srcRowId], srcMatrix.rowBegin[srcRowId+1]); + for (Index subRow = 0; subRow < NbBlocksRows; ++subRow) { - Index j = M.colsIndex[xj]; - Block b = M.colsValue[xj]; - if ((*filter)(i,j,b,ref)) + const auto oldVid = vid; + + for (std::size_t xj = static_cast(rowRange.begin()); xj < static_cast(rowRange.end()); ++xj) { - this->colsIndex.push_back(j); - this->colsValue.push_back(b); - ++vid; + // col id if blocks were scalars + const Index scalarColId = srcMatrix.colsIndex[xj] * SrcBlockColumns; + const typename TMatrix::Block& srcBlock = srcMatrix.colsValue[xj]; + + for (Index subCol = 0; subCol < NbBlocksColumns; ++subCol) + { + Block subBlock; + matrix_bloc_traits::subBlock(srcBlock, subRow * DstBlockRows, subCol * DstBlockColumns, subBlock); + + if ((*filter)(scalarRowId / DstBlockRows + subRow, scalarColId / DstBlockColumns + subCol, subBlock, ref)) + { + this->colsIndex.push_back(scalarColId / DstBlockColumns + subCol); + this->colsValue.push_back(subBlock); + ++vid; + } + } + } + + if (oldVid != vid) //check in case all sub-blocks have been filtered out + { + this->rowIndex.push_back(scalarRowId / DstBlockRows + subRow); + this->rowBegin.push_back(oldVid); } } + if (!keepEmptyRows && this->rowBegin.back() == vid) // row was empty { this->rowIndex.pop_back(); @@ -1366,11 +1403,6 @@ template<> void SOFA_LINEARALGEBRA_API CompressedRowSparseMatrixMechanical void SOFA_LINEARALGEBRA_API CompressedRowSparseMatrixMechanical::add(Index row, Index col, const type::Mat3x3d& _M); template<> void SOFA_LINEARALGEBRA_API CompressedRowSparseMatrixMechanical::add(Index row, Index col, const type::Mat3x3f& _M); -template<> template<> void SOFA_LINEARALGEBRA_API CompressedRowSparseMatrixMechanical::filterValues >(CompressedRowSparseMatrixMechanical >& M, filter_fn* filter, const Real ref, bool keepEmptyRows); -template<> template<> void SOFA_LINEARALGEBRA_API CompressedRowSparseMatrixMechanical::filterValues >(CompressedRowSparseMatrixMechanical >& M, filter_fn* filter, const Real ref, bool keepEmptyRows); -template<> template<> void SOFA_LINEARALGEBRA_API CompressedRowSparseMatrixMechanical::filterValues >(CompressedRowSparseMatrixMechanical >& M, filter_fn* filter, const Real ref, bool keepEmptyRows); -template<> template<> void SOFA_LINEARALGEBRA_API CompressedRowSparseMatrixMechanical::filterValues >(CompressedRowSparseMatrixMechanical >& M, filter_fn* filter, const Real ref, bool keepEmptyRows); - #if !defined(SOFA_COMPONENT_LINEARSOLVER_COMPRESSEDROWSPARSEMATRIXMECHANICAL_CPP) extern template class SOFA_LINEARALGEBRA_API CompressedRowSparseMatrixMechanical; extern template class SOFA_LINEARALGEBRA_API CompressedRowSparseMatrixMechanical; diff --git a/Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/matrix_bloc_traits.h b/Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/matrix_bloc_traits.h index 75faf1f72e1..e023637e90f 100644 --- a/Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/matrix_bloc_traits.h +++ b/Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/matrix_bloc_traits.h @@ -95,6 +95,12 @@ class matrix_bloc_traits static void split_row_index(IndexType& index, IndexType& modulo) { bloc_index_func::split(index, modulo); } static void split_col_index(IndexType& index, IndexType& modulo) { bloc_index_func::split(index, modulo); } + template + static void subBlock(const Block& b, IndexType row, IndexType col, TSubBlock& subBlock) + { + b.getsub(row, col, subBlock); + } + static sofa::linearalgebra::BaseMatrix::ElementType getElementType() { return matrix_bloc_traits::getElementType(); } }; @@ -130,6 +136,12 @@ class matrix_bloc_traits < type::Mat, IndexType> static void split_row_index(IndexType& index, IndexType& modulo) { bloc_index_func::split(index, modulo); } static void split_col_index(IndexType& index, IndexType& modulo) { bloc_index_func::split(index, modulo); } + template + static void subBlock(const Block& b, IndexType row, IndexType col, TSubBlock& subBlock) + { + b.getsub(row, col, subBlock); + } + static sofa::linearalgebra::BaseMatrix::ElementType getElementType() { return matrix_bloc_traits::getElementType(); } static const std::string Name() { @@ -179,6 +191,12 @@ class matrix_bloc_traits < sofa::type::Vec, IndexType > static void transpose(Block& res, const Block& b) { res = b; } + template, bool> = true> + static void subBlock(const Block& b, IndexType row, IndexType col, TSubBlock& subBlock) + { + b.getsub(col, subBlock); + } + static sofa::linearalgebra::BaseMatrix::ElementType getElementType() { return matrix_bloc_traits::getElementType(); } static const std::string Name() { @@ -259,6 +277,12 @@ class matrix_bloc_traits < float, IndexType > static void split_row_index(IndexType& index, IndexType& modulo) { bloc_index_func::split(index, modulo); } static void split_col_index(IndexType& index, IndexType& modulo) { bloc_index_func::split(index, modulo); } + template, bool> = true> + static void subBlock(const Block& b, IndexType row, IndexType col, TSubBlock& subBlock) + { + subBlock = v(b, row, col); + } + static const std::string Name() { return "f"; } static sofa::linearalgebra::BaseMatrix::ElementType getElementType() { return sofa::linearalgebra::BaseMatrix::ELEMENT_FLOAT; } static IndexType getElementSize() { return sizeof(Real); } @@ -285,6 +309,12 @@ class matrix_bloc_traits < double, IndexType > } static void invert(Block& result, const Block& b) { result = 1.0/b; } + template, bool> = true> + static void subBlock(const Block& b, IndexType row, IndexType col, TSubBlock& subBlock) + { + subBlock = v(b, row, col); + } + static void split_row_index(IndexType& index, IndexType& modulo) { bloc_index_func::split(index, modulo); } static void split_col_index(IndexType& index, IndexType& modulo) { bloc_index_func::split(index, modulo); } @@ -313,6 +343,12 @@ class matrix_bloc_traits < int, IndexType > } static void invert(Block& result, const Block& b) { result = 1.0f/b; } + template, bool> = true> + static void subBlock(const Block& b, IndexType row, IndexType col, TSubBlock& subBlock) + { + subBlock = v(b, row, col); + } + static void split_row_index(int& index, int& modulo) { bloc_index_func::split(index, modulo); } static void split_col_index(int& index, int& modulo) { bloc_index_func::split(index, modulo); } diff --git a/Sofa/framework/LinearAlgebra/test/CompressedRowSparseMatrix_test.cpp b/Sofa/framework/LinearAlgebra/test/CompressedRowSparseMatrix_test.cpp index b1082b8d17f..33d312bafc3 100644 --- a/Sofa/framework/LinearAlgebra/test/CompressedRowSparseMatrix_test.cpp +++ b/Sofa/framework/LinearAlgebra/test/CompressedRowSparseMatrix_test.cpp @@ -26,6 +26,44 @@ #include #include +#include + + +TEST(matrix_bloc_traits, subBlock) +{ + sofa::type::Mat<6, 6, SReal> mat6x6; + + sofa::testing::LinearCongruentialRandomGenerator lcg(46515387); + for (sofa::Size i = 0; i < mat6x6.nbLines; i++) + { + for (sofa::Size j = 0; j < mat6x6.nbCols; j++) + { + mat6x6(i, j) = lcg.generateInRange(0., 1.); + } + } + + for (const auto& [r, c] : sofa::type::vector>{{0, 0}, {0, 3}, {3, 0}, {3, 3}, {1, 2}}) + { + sofa::type::Mat<3, 3, SReal> mat3x3; + sofa::linearalgebra::matrix_bloc_traits, sofa::Index>::subBlock(mat6x6, r, c, mat3x3); + + for (sofa::Size i = 0; i < mat3x3.nbLines; i++) + { + for (sofa::Size j = 0; j < mat3x3.nbCols; j++) + { + EXPECT_EQ(mat6x6(i + r, j + c), mat3x3(i, j)); + } + } + } + + for (const auto& [r, c] : sofa::type::vector>{{0, 0}, {0, 3}, {3, 0}, {3, 3}, {1, 2}}) + { + SReal real; + sofa::linearalgebra::matrix_bloc_traits, sofa::Index>::subBlock(mat6x6, r, c, real); + EXPECT_EQ(real, mat6x6(r, c)); + } + +} template void generateMatrix(sofa::linearalgebra::CompressedRowSparseMatrix& matrix, @@ -249,7 +287,7 @@ TEST(CompressedRowSparseMatrix, copyNonZeros) EXPECT_EQ(numberNonZeroValues1, numberNonZeroValues3); } -TEST(CompressedRowSparseMatrix, copyNonZerosFromBlocks) +TEST(CompressedRowSparseMatrix, copyNonZerosFrom3x3Blocks) { sofa::linearalgebra::CompressedRowSparseMatrix> A; generateMatrix(A, 1321, 3556, 0.0003, 12); @@ -270,9 +308,37 @@ TEST(CompressedRowSparseMatrix, copyNonZerosFromBlocks) for (unsigned int r = 0; r < A.rowSize(); ++r) { - for (unsigned int c = 0; c < A.rowSize(); ++c) + for (unsigned int c = 0; c < A.colSize(); ++c) + { + EXPECT_NEAR(A(r, c), B(r, c), 1e-12_sreal) << "r = " << r << ", c = " << c; + } + } +} + +TEST(CompressedRowSparseMatrix, copyNonZerosFrom1x3Blocks) +{ + sofa::linearalgebra::CompressedRowSparseMatrix> A; + generateMatrix(A, 1321, 3556, 0.0003, 12); + + const auto numberNonZeroValues1 = A.colsValue.size(); + + A.add(23, 569, 0); + A.add(874, 326, 0); + A.add(769, 1789, 0); + A.compress(); + + const auto numberNonZeroValues2 = A.colsValue.size(); + EXPECT_GT(numberNonZeroValues2, numberNonZeroValues1); + + sofa::linearalgebra::CompressedRowSparseMatrix B; + + B.copyNonZeros(A); + + for (unsigned int r = 0; r < A.rowSize(); ++r) + { + for (unsigned int c = 0; c < A.colSize(); ++c) { - EXPECT_NEAR(A(r, c), B(r, c), 1e-12_sreal); + EXPECT_NEAR(A(r, c), B(r, c), 1e-12_sreal) << "r = " << r << ", c = " << c; } } } diff --git a/Sofa/framework/Type/src/sofa/type/Mat.h b/Sofa/framework/Type/src/sofa/type/Mat.h index cf1ca2a5ead..260b4f1553d 100644 --- a/Sofa/framework/Type/src/sofa/type/Mat.h +++ b/Sofa/framework/Type/src/sofa/type/Mat.h @@ -236,6 +236,20 @@ class Mat m[i][j] = this->elems[i+L0][j+C0]; } + template + constexpr void getsub(const Size L0, const Size C0, Vec& m) const noexcept + { + for (Size j = 0; j < C2; j++) + { + m[j] = this->elems[L0][j + C0]; + } + } + + constexpr void getsub(Size L0, Size C0, real& m) const noexcept + { + m = this->elems[L0][C0]; + } + template constexpr void setsub(Size L0, Size C0, const Mat& m) noexcept { diff --git a/Sofa/framework/Type/src/sofa/type/Vec.h b/Sofa/framework/Type/src/sofa/type/Vec.h index 7c1bd70928e..229f30645a9 100644 --- a/Sofa/framework/Type/src/sofa/type/Vec.h +++ b/Sofa/framework/Type/src/sofa/type/Vec.h @@ -290,6 +290,20 @@ class Vec return this->elems.data(); } + template = true> + constexpr void getsub(const Size i, Vec& m) const noexcept + { + for (Size j = 0; j < N2; j++) + { + m[j] = this->elems[j + i]; + } + } + + constexpr void getsub(const Size i, ValueType& m) const noexcept + { + m = this->elems[i]; + } + // LINEAR ALGEBRA constexpr Vec mulscalar(const ValueType f) const noexcept {