Skip to content

Commit

Permalink
[LinearAlgebra] Factorize value filtering (sofa-framework#4442)
Browse files Browse the repository at this point in the history 
* [LinearAlgebra] Factorize value filtering

No more template specialization

* Compatibility with all CompressedRowSparseMatrixGeneric

including CompressedRowSparseMatrixConstraint

---------

Co-authored-by: Paul Baksic <[email protected]>
  • Loading branch information
@alxbilger @bakpaul
alxbilger and bakpaul authored Jan 25, 2024
1 parent f655ab5 commit f39776f
Show file tree
Hide file tree
Showing 7 changed files with 198 additions and 124 deletions.
6 changes: 6 additions & 0 deletions Sofa/framework/DefaultType/src/sofa/defaulttype/RigidDeriv.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -606,6 +606,12 @@ namespace sofa::linearalgebra

static void transpose(BlockTranspose& res, const Block& b) { res = b; }

template<class TSubBlock, std::enable_if_t<std::is_scalar_v<TSubBlock>, 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<Real,IndexType>::getElementType(); }
static const char* Name() { return defaulttype::DataTypeName<defaulttype::RigidDeriv<N, T>>::name(); }
};
Expand Down
94 changes: 0 additions & 94 deletions Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/CompressedRowSparseMatrixMechanical.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,100 +63,6 @@ void CompressedRowSparseMatrixMechanical<type::Mat<3, 3, float> >::add(Index row
addBlockMat(*this, row, col, _M);
}


template<class RealDest, class RealSrc>
void filterValuesImpl(
CompressedRowSparseMatrixMechanical<RealDest>& dest,
CompressedRowSparseMatrixMechanical<type::Mat<3, 3, RealSrc> >& src,
typename CompressedRowSparseMatrixMechanical<RealDest>::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<RealDest>::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<std::size_t>(rowRange.begin()); xj < static_cast<std::size_t>(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<double>::filterValues(CompressedRowSparseMatrixMechanical<type::Mat<3, 3, double> >& M, filter_fn* filter, const Real ref, bool keepEmptyRows)
{
filterValuesImpl(*this, M, filter, ref, keepEmptyRows);
}

template <> template <>
void CompressedRowSparseMatrixMechanical<double>::filterValues(CompressedRowSparseMatrixMechanical<type::Mat<3, 3, float> >& M, filter_fn* filter, const Real ref, bool keepEmptyRows)
{
filterValuesImpl(*this, M, filter, ref, keepEmptyRows);
}

template <> template <>
void CompressedRowSparseMatrixMechanical<float>::filterValues(CompressedRowSparseMatrixMechanical<type::Mat<3, 3, float> >& M, filter_fn* filter, const Real ref, bool keepEmptyRows)
{
filterValuesImpl(*this, M, filter, ref, keepEmptyRows);
}

template <> template <>
void CompressedRowSparseMatrixMechanical<float>::filterValues(CompressedRowSparseMatrixMechanical<type::Mat<3, 3, double> >& 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<float>;
Expand Down
86 changes: 59 additions & 27 deletions Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/CompressedRowSparseMatrixMechanical.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<class TMatrix>
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<std::remove_const_t<TMatrix>*>(&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<Index>(M.rowIndex.size()); ++rowId)
for (Index srcRowId = 0; srcRowId < static_cast<Index>(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<std::size_t>(rowRange.begin()); xj < static_cast<std::size_t>(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<typename TMatrix::Block, sofa::SignedIndex>::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();
Expand Down Expand Up @@ -1366,11 +1403,6 @@ template<> void SOFA_LINEARALGEBRA_API CompressedRowSparseMatrixMechanical<type:
template<> void SOFA_LINEARALGEBRA_API CompressedRowSparseMatrixMechanical<type::Mat3x3f >::add(Index row, Index col, const type::Mat3x3d& _M);
template<> void SOFA_LINEARALGEBRA_API CompressedRowSparseMatrixMechanical<type::Mat3x3f >::add(Index row, Index col, const type::Mat3x3f& _M);

template<> template<> void SOFA_LINEARALGEBRA_API CompressedRowSparseMatrixMechanical<double>::filterValues<CompressedRowSparseMatrixMechanical<type::Mat3x3d > >(CompressedRowSparseMatrixMechanical<type::Mat<3, 3, double> >& M, filter_fn* filter, const Real ref, bool keepEmptyRows);
template<> template<> void SOFA_LINEARALGEBRA_API CompressedRowSparseMatrixMechanical<double>::filterValues<CompressedRowSparseMatrixMechanical<type::Mat3x3f > >(CompressedRowSparseMatrixMechanical<type::Mat<3, 3, float> >& M, filter_fn* filter, const Real ref, bool keepEmptyRows);
template<> template<> void SOFA_LINEARALGEBRA_API CompressedRowSparseMatrixMechanical<float>::filterValues<CompressedRowSparseMatrixMechanical<type::Mat3x3f > >(CompressedRowSparseMatrixMechanical<type::Mat<3, 3, float> >& M, filter_fn* filter, const Real ref, bool keepEmptyRows);
template<> template<> void SOFA_LINEARALGEBRA_API CompressedRowSparseMatrixMechanical<float>::filterValues<CompressedRowSparseMatrixMechanical<type::Mat3x3d > >(CompressedRowSparseMatrixMechanical<type::Mat<3, 3, double> >& M, filter_fn* filter, const Real ref, bool keepEmptyRows);

#if !defined(SOFA_COMPONENT_LINEARSOLVER_COMPRESSEDROWSPARSEMATRIXMECHANICAL_CPP)
extern template class SOFA_LINEARALGEBRA_API CompressedRowSparseMatrixMechanical<float>;
extern template class SOFA_LINEARALGEBRA_API CompressedRowSparseMatrixMechanical<type::Mat1x1f>;
Expand Down
36 changes: 36 additions & 0 deletions Sofa/framework/LinearAlgebra/src/sofa/linearalgebra/matrix_bloc_traits.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -95,6 +95,12 @@ class matrix_bloc_traits
static void split_row_index(IndexType& index, IndexType& modulo) { bloc_index_func<NL, IndexType>::split(index, modulo); }
static void split_col_index(IndexType& index, IndexType& modulo) { bloc_index_func<NC, IndexType>::split(index, modulo); }

template<class TSubBlock>
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<Real, IndexType>::getElementType(); }
};

Expand Down Expand Up @@ -130,6 +136,12 @@ class matrix_bloc_traits < type::Mat<L,C,real>, IndexType>
static void split_row_index(IndexType& index, IndexType& modulo) { bloc_index_func<NL, IndexType>::split(index, modulo); }
static void split_col_index(IndexType& index, IndexType& modulo) { bloc_index_func<NC, IndexType>::split(index, modulo); }

template<class TSubBlock>
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<Real, IndexType>::getElementType(); }
static const std::string Name()
{
Expand Down Expand Up @@ -179,6 +191,12 @@ class matrix_bloc_traits < sofa::type::Vec<N, T>, IndexType >

static void transpose(Block& res, const Block& b) { res = b; }

template<class TSubBlock, std::enable_if_t<std::is_scalar_v<TSubBlock>, 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<Real, IndexType>::getElementType(); }
static const std::string Name()
{
Expand Down Expand Up @@ -259,6 +277,12 @@ class matrix_bloc_traits < float, IndexType >
static void split_row_index(IndexType& index, IndexType& modulo) { bloc_index_func<NL, IndexType>::split(index, modulo); }
static void split_col_index(IndexType& index, IndexType& modulo) { bloc_index_func<NC, IndexType>::split(index, modulo); }

template<class TSubBlock, std::enable_if_t<std::is_scalar_v<TSubBlock>, 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); }
Expand All @@ -285,6 +309,12 @@ class matrix_bloc_traits < double, IndexType >
}
static void invert(Block& result, const Block& b) { result = 1.0/b; }

template<class TSubBlock, std::enable_if_t<std::is_scalar_v<TSubBlock>, 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<NL, IndexType>::split(index, modulo); }
static void split_col_index(IndexType& index, IndexType& modulo) { bloc_index_func<NC, IndexType>::split(index, modulo); }

Expand Down Expand Up @@ -313,6 +343,12 @@ class matrix_bloc_traits < int, IndexType >
}
static void invert(Block& result, const Block& b) { result = 1.0f/b; }

template<class TSubBlock, std::enable_if_t<std::is_scalar_v<TSubBlock>, 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<NL, IndexType>::split(index, modulo); }
static void split_col_index(int& index, int& modulo) { bloc_index_func<NC, IndexType>::split(index, modulo); }

Expand Down
72 changes: 69 additions & 3 deletions Sofa/framework/LinearAlgebra/test/CompressedRowSparseMatrix_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,44 @@
#include <sofa/testing/NumericTest.h>

#include <sofa/helper/RandomGenerator.h>
#include <sofa/testing/LinearCongruentialRandomGenerator.h>


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<std::pair<sofa::Index, sofa::Index>>{{0, 0}, {0, 3}, {3, 0}, {3, 3}, {1, 2}})
{
sofa::type::Mat<3, 3, SReal> mat3x3;
sofa::linearalgebra::matrix_bloc_traits<sofa::type::Mat<6, 6, SReal>, 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<std::pair<sofa::Index, sofa::Index>>{{0, 0}, {0, 3}, {3, 0}, {3, 3}, {1, 2}})
{
SReal real;
sofa::linearalgebra::matrix_bloc_traits<sofa::type::Mat<6, 6, SReal>, sofa::Index>::subBlock(mat6x6, r, c, real);
EXPECT_EQ(real, mat6x6(r, c));
}

}

template<typename TBlock>
void generateMatrix(sofa::linearalgebra::CompressedRowSparseMatrix<TBlock>& matrix,
Expand Down Expand Up @@ -249,7 +287,7 @@ TEST(CompressedRowSparseMatrix, copyNonZeros)
EXPECT_EQ(numberNonZeroValues1, numberNonZeroValues3);
}

TEST(CompressedRowSparseMatrix, copyNonZerosFromBlocks)
TEST(CompressedRowSparseMatrix, copyNonZerosFrom3x3Blocks)
{
sofa::linearalgebra::CompressedRowSparseMatrix<sofa::type::Mat<3, 3, SReal>> A;
generateMatrix(A, 1321, 3556, 0.0003, 12);
Expand All @@ -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<sofa::type::Vec<3, SReal>> 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<SReal> 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;
}
}
}
Expand Down
Loading

0 comments on commit f39776f

Please sign in to comment.