Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
49 changes: 43 additions & 6 deletions Sofa/framework/Type/src/sofa/type/Mat.h
Original file line number Diff line number Diff line change
Expand Up @@ -708,17 +708,54 @@ class Mat
/// Addition of the transposed of m
constexpr void addTransposed(const Mat<C,L,real>& m) noexcept
{
for(Size i=0; i<L; i++)
for(Size j=0; j<C; j++)
(*this)(i,j) += m(j,i);
if (canSelfTranspose(*this, m))
{
// m aliases *this: process each off-diagonal pair once, from values read
// before either of the two positions is written.
for(Size i=0; i<L; i++)
{
(*this)(i,i) += (*this)(i,i);
for(Size j=i+1; j<C; j++)
{
const real sum = (*this)(i,j) + (*this)(j,i);
(*this)(i,j) = sum;
(*this)(j,i) = sum;
}
}
}
else
{
for(Size i=0; i<L; i++)
for(Size j=0; j<C; j++)
(*this)(i,j) += m(j,i);
}
}

/// Subtraction of the transposed of m
constexpr void subTransposed(const Mat<C,L,real>& m) noexcept
{
for(Size i=0; i<L; i++)
for(Size j=0; j<C; j++)
(*this)(i,j) -= m(j,i);
if (canSelfTranspose(*this, m))
{
// m aliases *this: process each off-diagonal pair once, from values read
// before either of the two positions is written.
for(Size i=0; i<L; i++)
{
(*this)(i,i) = real{};
for(Size j=i+1; j<C; j++)
{
const real mij = (*this)(i,j);
const real mji = (*this)(j,i);
(*this)(i,j) = mij - mji;
(*this)(j,i) = mji - mij;
}
}
}
else
{
for(Size i=0; i<L; i++)
for(Size j=0; j<C; j++)
(*this)(i,j) -= m(j,i);
}
}

/// Subtraction assignment operator.
Expand Down
22 changes: 22 additions & 0 deletions Sofa/framework/Type/test/MatTypes_test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -699,6 +699,28 @@ TEST(MatTypesTest, addSubTransposed)
EXPECT_EQ(M, Matrix3::Identity() + A.transposed());
}

TEST(MatTypesTest, addTransposedSelfAliasing)
{
Matrix3 M(Matrix3::Line(1., 2., 3.), Matrix3::Line(4., 5., 6.), Matrix3::Line(7., 8., 9.));
const Matrix3 expected(Matrix3::Line(2., 6., 10.),
Matrix3::Line(6., 10., 14.),
Matrix3::Line(10., 14., 18.));

M.addTransposed(M);
EXPECT_EQ(M, expected);
}

TEST(MatTypesTest, subTransposedSelfAliasing)
{
Matrix3 M(Matrix3::Line(1., 2., 3.), Matrix3::Line(4., 5., 6.), Matrix3::Line(7., 8., 9.));
const Matrix3 expected(Matrix3::Line(0., -2., -4.),
Matrix3::Line(2., 0., -2.),
Matrix3::Line(4., 2., 0.));

M.subTransposed(M);
EXPECT_EQ(M, expected);
}

TEST(MatTypesTest, symmetrize)
{
Matrix3 A(Matrix3::Line(1., 2., 3.), Matrix3::Line(4., 5., 6.), Matrix3::Line(7., 8., 9.));
Expand Down
Loading