From 08ce046b64a79322f27de2e98bce66e8213cb0ea Mon Sep 17 00:00:00 2001 From: Paul Baksic Date: Thu, 22 Aug 2024 17:49:31 +0200 Subject: [PATCH 1/6] Add unit test to test rotation of inertia matrix for rigid objects --- .../Mass/tests/DiagonalMass_test.cpp | 100 ++++++++++++++++++ 1 file changed, 100 insertions(+) diff --git a/Sofa/Component/Mass/tests/DiagonalMass_test.cpp b/Sofa/Component/Mass/tests/DiagonalMass_test.cpp index 882614ea793..1e35671e956 100644 --- a/Sofa/Component/Mass/tests/DiagonalMass_test.cpp +++ b/Sofa/Component/Mass/tests/DiagonalMass_test.cpp @@ -39,6 +39,7 @@ using sofa::core::ExecParams ; #include #include #include +#include #include using sofa::simulation::Node ; @@ -984,6 +985,97 @@ class DiagonalMass_test : public BaseTest EXPECT_EQ(vMasses.size(), 0); EXPECT_NEAR(mass->getTotalMass(), 0, 1e-4); } + + static Node::SPtr generateRigidScene() + { + static const string scene = + "" + "" + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + ""; + + + + Node::SPtr root = SceneLoaderXML::loadFromMemory("loadWithNoParam", scene.c_str()); + sofa::simulation::node::initRoot(root.get()); + + return root; + } + + void nonIdentityIntertiaMatrix_DifferentRotationDirection() + { + Node::SPtr root = generateRigidScene(); + Rigid3Types::VecDeriv* CF1_force = reinterpret_cast(root->getChild("Aligned")->getObject("ConstantForceField1")->findData("forces")->beginEditVoidPtr()); + Rigid3Types::VecDeriv* CF2_force = reinterpret_cast(root->getChild("Rotated")->getObject("ConstantForceField2")->findData("forces")->beginEditVoidPtr()); + + (*CF1_force)[0][5] = 1.0; + (*CF2_force)[0][3] = 1.0; + + root->getChild("Aligned")->getObject("ConstantForceField1")->findData("forces")->endEditVoidPtr(); + root->getChild("Rotated")->getObject("ConstantForceField2")->findData("forces")->endEditVoidPtr(); + + auto mstate1 = root->getChild("Aligned")->getNodeObject>(); + auto mstate2 = root->getChild("Rotated")->getNodeObject>(); + + sofa::simulation::node::animate(root.get(),50); + + //Because the inertia is smaller along z, we expect different velocity after some times with a ratio equivalent to the inverse ratio between the inertia + EXPECT_GT(mstate2->v.getValue()[0][3],0.0); + EXPECT_NEAR(mstate1->v.getValue()[0][5] / mstate2->v.getValue()[0][3], 0.0427/0.00375, 1.0e-5 ); + + + } + + void nonIdentityIntertiaMatrix_RotationOfOneRigid() + { + Node::SPtr root = generateRigidScene(); + + sofa::simulation::node::animate(root.get(),1); + + + auto mstate1 = root->getChild("Aligned")->getNodeObject>(); + auto mstate2 = root->getChild("Rotated")->getNodeObject>(); + + //Rotate the rigid + mstate2->x.setValue(Rigid3Types::VecCoord{Rigid3Types::Coord(Rigid3Types::Coord::Pos(1,0,0),Rigid3Types::Coord::Rot (0.707,0,0.707,0))}); + + Rigid3Types::VecDeriv* CF1_force = reinterpret_cast(root->getChild("Aligned")->getObject("ConstantForceField1")->findData("forces")->beginEditVoidPtr()); + Rigid3Types::VecDeriv* CF2_force = reinterpret_cast(root->getChild("Rotated")->getObject("ConstantForceField2")->findData("forces")->beginEditVoidPtr()); + + //With rotated state, we now apply rotation along the Z axis of both rigids, this should result in the same acceleration if the inertia matrix is also rotated + (*CF1_force)[0][5] = 1.0; + (*CF2_force)[0][3] = 1.0; + + root->getChild("Aligned")->getObject("ConstantForceField1")->findData("forces")->endEditVoidPtr(); + root->getChild("Rotated")->getObject("ConstantForceField2")->findData("forces")->endEditVoidPtr(); + + sofa::simulation::node::animate(root.get(),50); + EXPECT_GT(mstate1->v.getValue()[0][5],0.0); + EXPECT_GT(mstate2->v.getValue()[0][3],0.0); + EXPECT_NEAR(mstate1->v.getValue()[0][5] / mstate2->v.getValue()[0][3], 1.0, 1.0e-5 ); + } }; @@ -1203,4 +1295,12 @@ TEST_F(DiagonalMass3_test, checkAttributeLoadFromXpsMassSpring){ } +TEST_F(DiagonalMass3_test, nonIdentityIntertiaMatrix_DifferentRotationVector){ + EXPECT_NO_THROW(nonIdentityIntertiaMatrix_DifferentRotationDirection()); +} + +TEST_F(DiagonalMass3_test, nonIdentityIntertiaMatrix_RotationOfTheRigid){ + EXPECT_NO_THROW(nonIdentityIntertiaMatrix_RotationOfOneRigid()); +} + } // namespace sofa From ab7333a6d826ee783eda1949999d8809eb14730b Mon Sep 17 00:00:00 2001 From: Paul Baksic <30337881+bakpaul@users.noreply.github.com> Date: Mon, 26 Aug 2024 09:57:52 +0200 Subject: [PATCH 2/6] Apply suggestions from code review Co-authored-by: Frederick Roy --- Sofa/Component/Mass/tests/DiagonalMass_test.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/Sofa/Component/Mass/tests/DiagonalMass_test.cpp b/Sofa/Component/Mass/tests/DiagonalMass_test.cpp index 1e35671e956..2471ab98e30 100644 --- a/Sofa/Component/Mass/tests/DiagonalMass_test.cpp +++ b/Sofa/Component/Mass/tests/DiagonalMass_test.cpp @@ -1003,7 +1003,7 @@ class DiagonalMass_test : public BaseTest " " " " " " - " " + " " " " " " " " @@ -1024,7 +1024,7 @@ class DiagonalMass_test : public BaseTest return root; } - void nonIdentityIntertiaMatrix_DifferentRotationDirection() + void nonIdentityInertiaMatrix_DifferentRotationDirection() { Node::SPtr root = generateRigidScene(); Rigid3Types::VecDeriv* CF1_force = reinterpret_cast(root->getChild("Aligned")->getObject("ConstantForceField1")->findData("forces")->beginEditVoidPtr()); @@ -1048,7 +1048,7 @@ class DiagonalMass_test : public BaseTest } - void nonIdentityIntertiaMatrix_RotationOfOneRigid() + void nonIdentityInertiaMatrix_RotationOfOneRigid() { Node::SPtr root = generateRigidScene(); @@ -1295,12 +1295,12 @@ TEST_F(DiagonalMass3_test, checkAttributeLoadFromXpsMassSpring){ } -TEST_F(DiagonalMass3_test, nonIdentityIntertiaMatrix_DifferentRotationVector){ - EXPECT_NO_THROW(nonIdentityIntertiaMatrix_DifferentRotationDirection()); +TEST_F(DiagonalMass3_test, nonIdentityInertiaMatrix_DifferentRotationVector){ + EXPECT_NO_THROW(nonIdentityInertiaMatrix_DifferentRotationDirection()); } -TEST_F(DiagonalMass3_test, nonIdentityIntertiaMatrix_RotationOfTheRigid){ - EXPECT_NO_THROW(nonIdentityIntertiaMatrix_RotationOfOneRigid()); +TEST_F(DiagonalMass3_test, nonIdentityInertiaMatrix_RotationOfTheRigid){ + EXPECT_NO_THROW(nonIdentityInertiaMatrix_RotationOfOneRigid()); } } // namespace sofa From 016890bb807bd84570e29b2d7c76eef95fc816ee Mon Sep 17 00:00:00 2001 From: Paul Baksic Date: Mon, 26 Aug 2024 16:17:01 +0200 Subject: [PATCH 3/6] Wrong test set... --- .../Mass/tests/DiagonalMass_test.cpp | 100 ----------------- .../Component/Mass/tests/UniformMass_test.cpp | 102 ++++++++++++++++++ 2 files changed, 102 insertions(+), 100 deletions(-) diff --git a/Sofa/Component/Mass/tests/DiagonalMass_test.cpp b/Sofa/Component/Mass/tests/DiagonalMass_test.cpp index 2471ab98e30..4c4097b6301 100644 --- a/Sofa/Component/Mass/tests/DiagonalMass_test.cpp +++ b/Sofa/Component/Mass/tests/DiagonalMass_test.cpp @@ -985,97 +985,6 @@ class DiagonalMass_test : public BaseTest EXPECT_EQ(vMasses.size(), 0); EXPECT_NEAR(mass->getTotalMass(), 0, 1e-4); } - - static Node::SPtr generateRigidScene() - { - static const string scene = - "" - "" - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - ""; - - - - Node::SPtr root = SceneLoaderXML::loadFromMemory("loadWithNoParam", scene.c_str()); - sofa::simulation::node::initRoot(root.get()); - - return root; - } - - void nonIdentityInertiaMatrix_DifferentRotationDirection() - { - Node::SPtr root = generateRigidScene(); - Rigid3Types::VecDeriv* CF1_force = reinterpret_cast(root->getChild("Aligned")->getObject("ConstantForceField1")->findData("forces")->beginEditVoidPtr()); - Rigid3Types::VecDeriv* CF2_force = reinterpret_cast(root->getChild("Rotated")->getObject("ConstantForceField2")->findData("forces")->beginEditVoidPtr()); - - (*CF1_force)[0][5] = 1.0; - (*CF2_force)[0][3] = 1.0; - - root->getChild("Aligned")->getObject("ConstantForceField1")->findData("forces")->endEditVoidPtr(); - root->getChild("Rotated")->getObject("ConstantForceField2")->findData("forces")->endEditVoidPtr(); - - auto mstate1 = root->getChild("Aligned")->getNodeObject>(); - auto mstate2 = root->getChild("Rotated")->getNodeObject>(); - - sofa::simulation::node::animate(root.get(),50); - - //Because the inertia is smaller along z, we expect different velocity after some times with a ratio equivalent to the inverse ratio between the inertia - EXPECT_GT(mstate2->v.getValue()[0][3],0.0); - EXPECT_NEAR(mstate1->v.getValue()[0][5] / mstate2->v.getValue()[0][3], 0.0427/0.00375, 1.0e-5 ); - - - } - - void nonIdentityInertiaMatrix_RotationOfOneRigid() - { - Node::SPtr root = generateRigidScene(); - - sofa::simulation::node::animate(root.get(),1); - - - auto mstate1 = root->getChild("Aligned")->getNodeObject>(); - auto mstate2 = root->getChild("Rotated")->getNodeObject>(); - - //Rotate the rigid - mstate2->x.setValue(Rigid3Types::VecCoord{Rigid3Types::Coord(Rigid3Types::Coord::Pos(1,0,0),Rigid3Types::Coord::Rot (0.707,0,0.707,0))}); - - Rigid3Types::VecDeriv* CF1_force = reinterpret_cast(root->getChild("Aligned")->getObject("ConstantForceField1")->findData("forces")->beginEditVoidPtr()); - Rigid3Types::VecDeriv* CF2_force = reinterpret_cast(root->getChild("Rotated")->getObject("ConstantForceField2")->findData("forces")->beginEditVoidPtr()); - - //With rotated state, we now apply rotation along the Z axis of both rigids, this should result in the same acceleration if the inertia matrix is also rotated - (*CF1_force)[0][5] = 1.0; - (*CF2_force)[0][3] = 1.0; - - root->getChild("Aligned")->getObject("ConstantForceField1")->findData("forces")->endEditVoidPtr(); - root->getChild("Rotated")->getObject("ConstantForceField2")->findData("forces")->endEditVoidPtr(); - - sofa::simulation::node::animate(root.get(),50); - EXPECT_GT(mstate1->v.getValue()[0][5],0.0); - EXPECT_GT(mstate2->v.getValue()[0][3],0.0); - EXPECT_NEAR(mstate1->v.getValue()[0][5] / mstate2->v.getValue()[0][3], 1.0, 1.0e-5 ); - } }; @@ -1294,13 +1203,4 @@ TEST_F(DiagonalMass3_test, checkAttributeLoadFromXpsMassSpring){ checkAttributeLoadFromFile("BehaviorModels/chain.xs3", 6, 0.6, false); } - -TEST_F(DiagonalMass3_test, nonIdentityInertiaMatrix_DifferentRotationVector){ - EXPECT_NO_THROW(nonIdentityInertiaMatrix_DifferentRotationDirection()); -} - -TEST_F(DiagonalMass3_test, nonIdentityInertiaMatrix_RotationOfTheRigid){ - EXPECT_NO_THROW(nonIdentityInertiaMatrix_RotationOfOneRigid()); -} - } // namespace sofa diff --git a/Sofa/Component/Mass/tests/UniformMass_test.cpp b/Sofa/Component/Mass/tests/UniformMass_test.cpp index 312765db17e..84fa2c97520 100644 --- a/Sofa/Component/Mass/tests/UniformMass_test.cpp +++ b/Sofa/Component/Mass/tests/UniformMass_test.cpp @@ -412,6 +412,98 @@ struct UniformMassTest : public BaseTest EXPECT_TRUE(todo == false) ; } + + static Node::SPtr generateRigidScene() + { + static const string scene = + "" + "" + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + ""; + + + + Node::SPtr root = SceneLoaderXML::loadFromMemory("loadWithNoParam", scene.c_str()); + sofa::simulation::node::initRoot(root.get()); + + return root; + } + + void nonIdentityInertiaMatrix_DifferentRotationDirection() + { + Node::SPtr root = generateRigidScene(); + Rigid3Types::VecDeriv* CF1_force = reinterpret_cast(root->getChild("Aligned")->getObject("ConstantForceField1")->findData("forces")->beginEditVoidPtr()); + Rigid3Types::VecDeriv* CF2_force = reinterpret_cast(root->getChild("Rotated")->getObject("ConstantForceField2")->findData("forces")->beginEditVoidPtr()); + + (*CF1_force)[0][5] = 1.0; + (*CF2_force)[0][3] = 1.0; + + root->getChild("Aligned")->getObject("ConstantForceField1")->findData("forces")->endEditVoidPtr(); + root->getChild("Rotated")->getObject("ConstantForceField2")->findData("forces")->endEditVoidPtr(); + + auto mstate1 = root->getChild("Aligned")->getNodeObject>(); + auto mstate2 = root->getChild("Rotated")->getNodeObject>(); + + sofa::simulation::node::animate(root.get(),50); + + //Because the inertia is smaller along z, we expect different velocity after some times with a ratio equivalent to the inverse ratio between the inertia + EXPECT_GT(mstate2->v.getValue()[0][3],0.0); + EXPECT_NEAR(mstate1->v.getValue()[0][5] / mstate2->v.getValue()[0][3], 0.0427/0.00375, 1.0e-5 ); + + + } + + void nonIdentityInertiaMatrix_RotationOfOneRigid() + { + Node::SPtr root = generateRigidScene(); + + sofa::simulation::node::animate(root.get(),1); + + + auto mstate1 = root->getChild("Aligned")->getNodeObject>(); + auto mstate2 = root->getChild("Rotated")->getNodeObject>(); + + //Rotate the rigid + mstate2->x.setValue(Rigid3Types::VecCoord{Rigid3Types::Coord(Rigid3Types::Coord::Pos(1,0,0),Rigid3Types::Coord::Rot (0.707,0,0.707,0))}); + + Rigid3Types::VecDeriv* CF1_force = reinterpret_cast(root->getChild("Aligned")->getObject("ConstantForceField1")->findData("forces")->beginEditVoidPtr()); + Rigid3Types::VecDeriv* CF2_force = reinterpret_cast(root->getChild("Rotated")->getObject("ConstantForceField2")->findData("forces")->beginEditVoidPtr()); + + //With rotated state, we now apply rotation along the Z axis of both rigids, this should result in the same acceleration if the inertia matrix is also rotated + (*CF1_force)[0][5] = 1.0; + (*CF2_force)[0][3] = 1.0; + + root->getChild("Aligned")->getObject("ConstantForceField1")->findData("forces")->endEditVoidPtr(); + root->getChild("Rotated")->getObject("ConstantForceField2")->findData("forces")->endEditVoidPtr(); + + sofa::simulation::node::animate(root.get(),50); + EXPECT_GT(mstate1->v.getValue()[0][5],0.0); + EXPECT_GT(mstate2->v.getValue()[0][3],0.0); + EXPECT_NEAR(mstate1->v.getValue()[0][5] / mstate2->v.getValue()[0][3], 1.0, 1.0e-5 ); + } + }; @@ -493,3 +585,13 @@ TYPED_TEST(UniformMassTest, checkRigidAttribute) { TYPED_TEST(UniformMassTest, reinitTest) { //ASSERT_NO_THROW(this->reinitTest()) ; } + +TYPED_TEST(UniformMassTest, nonIdentityInertiaMatrix_DifferentRotationDirection){ + EXPECT_NO_THROW(this->nonIdentityInertiaMatrix_DifferentRotationDirection()); +} + +TYPED_TEST(UniformMassTest, nonIdentityInertiaMatrix_RotationOfOneRigid){ + EXPECT_NO_THROW(this->nonIdentityInertiaMatrix_RotationOfOneRigid()); +} + + From ddb3a6444ebc56f2a6b5daf1486037efa82d6494 Mon Sep 17 00:00:00 2001 From: Paul Baksic Date: Mon, 26 Aug 2024 17:14:10 +0200 Subject: [PATCH 4/6] Use accessors instead of data --- .../Mass/tests/DiagonalMass_test.cpp | 2 +- .../Component/Mass/tests/UniformMass_test.cpp | 19 ++++++++++++------- 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/Sofa/Component/Mass/tests/DiagonalMass_test.cpp b/Sofa/Component/Mass/tests/DiagonalMass_test.cpp index 4c4097b6301..882614ea793 100644 --- a/Sofa/Component/Mass/tests/DiagonalMass_test.cpp +++ b/Sofa/Component/Mass/tests/DiagonalMass_test.cpp @@ -39,7 +39,6 @@ using sofa::core::ExecParams ; #include #include #include -#include #include using sofa::simulation::Node ; @@ -1203,4 +1202,5 @@ TEST_F(DiagonalMass3_test, checkAttributeLoadFromXpsMassSpring){ checkAttributeLoadFromFile("BehaviorModels/chain.xs3", 6, 0.6, false); } + } // namespace sofa diff --git a/Sofa/Component/Mass/tests/UniformMass_test.cpp b/Sofa/Component/Mass/tests/UniformMass_test.cpp index 84fa2c97520..5cf3921a725 100644 --- a/Sofa/Component/Mass/tests/UniformMass_test.cpp +++ b/Sofa/Component/Mass/tests/UniformMass_test.cpp @@ -51,6 +51,7 @@ using sofa::testing::BaseTest; using testing::Types; #include +#include template struct TemplateTypes @@ -430,7 +431,7 @@ struct UniformMassTest : public BaseTest " " " " " " - " " + " " " " " " " " @@ -469,8 +470,10 @@ struct UniformMassTest : public BaseTest sofa::simulation::node::animate(root.get(),50); //Because the inertia is smaller along z, we expect different velocity after some times with a ratio equivalent to the inverse ratio between the inertia - EXPECT_GT(mstate2->v.getValue()[0][3],0.0); - EXPECT_NEAR(mstate1->v.getValue()[0][5] / mstate2->v.getValue()[0][3], 0.0427/0.00375, 1.0e-5 ); + EXPECT_GT(mstate2->read(sofa::core::ConstVecDerivId::velocity())->getValue()[0][3],0.0); + EXPECT_NEAR(mstate1->read(sofa::core::ConstVecDerivId::velocity())->getValue()[0][5] / + mstate2->read(sofa::core::ConstVecDerivId::velocity())->getValue()[0][3], + 0.0427/0.00375, 1.0e-5 ); } @@ -486,7 +489,7 @@ struct UniformMassTest : public BaseTest auto mstate2 = root->getChild("Rotated")->getNodeObject>(); //Rotate the rigid - mstate2->x.setValue(Rigid3Types::VecCoord{Rigid3Types::Coord(Rigid3Types::Coord::Pos(1,0,0),Rigid3Types::Coord::Rot (0.707,0,0.707,0))}); + mstate2->write(sofa::core::VecCoordId::position())->setValue({Rigid3Types::Coord(Rigid3Types::Coord::Pos(1,0,0),Rigid3Types::Coord::Rot (0.707106781,0,0.707106781,0))}); Rigid3Types::VecDeriv* CF1_force = reinterpret_cast(root->getChild("Aligned")->getObject("ConstantForceField1")->findData("forces")->beginEditVoidPtr()); Rigid3Types::VecDeriv* CF2_force = reinterpret_cast(root->getChild("Rotated")->getObject("ConstantForceField2")->findData("forces")->beginEditVoidPtr()); @@ -499,9 +502,11 @@ struct UniformMassTest : public BaseTest root->getChild("Rotated")->getObject("ConstantForceField2")->findData("forces")->endEditVoidPtr(); sofa::simulation::node::animate(root.get(),50); - EXPECT_GT(mstate1->v.getValue()[0][5],0.0); - EXPECT_GT(mstate2->v.getValue()[0][3],0.0); - EXPECT_NEAR(mstate1->v.getValue()[0][5] / mstate2->v.getValue()[0][3], 1.0, 1.0e-5 ); + EXPECT_GT(mstate1->read(sofa::core::ConstVecDerivId::velocity())->getValue()[0][5],0.0); + EXPECT_GT(mstate2->read(sofa::core::ConstVecDerivId::velocity())->getValue()[0][3],0.0); + EXPECT_NEAR(mstate1->read(sofa::core::ConstVecDerivId::velocity())->getValue()[0][5] / + mstate2->read(sofa::core::ConstVecDerivId::velocity())->getValue()[0][3], + 1.0, 1.0e-5 ); } }; From e53483ccb3178d3450f82ff3949f4f702cca17eb Mon Sep 17 00:00:00 2001 From: Paul Baksic Date: Mon, 26 Aug 2024 17:17:37 +0200 Subject: [PATCH 5/6] Fix inertia matrix rotation for rigid by rotating the inertia matrix before using it --- .../src/sofa/component/mass/UniformMass.cpp | 4 +- .../src/sofa/component/mass/UniformMass.inl | 62 +++++++++++++++---- .../src/sofa/defaulttype/RigidMass.h | 16 +++++ 3 files changed, 68 insertions(+), 14 deletions(-) diff --git a/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.cpp b/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.cpp index d4cc864b3d1..32f73b6f562 100644 --- a/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.cpp +++ b/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.cpp @@ -330,9 +330,9 @@ Vec6 UniformMass::getMomentumRigid3DImpl( const MechanicalParams*, ReadAccessor v = d_v; ReadAccessor x = d_x; const ReadAccessor > indices = d_indices; + const auto & pos = this->getMState()->read(core::ConstVecCoordId::position())->getValue(); Real m = d_vertexMass.getValue().mass; - const typename MassType::Mat3x3& I = d_vertexMass.getValue().inertiaMassMatrix; type::Vec6d momentum; @@ -341,7 +341,7 @@ Vec6 UniformMass::getMomentumRigid3DImpl( const MechanicalParams*, typename RigidTypes::Vec3 linearMomentum = m*v[index].getLinear(); for( int j=0 ; j< 3 ; ++j ) momentum[j] += linearMomentum[j]; - typename RigidTypes::Vec3 angularMomentum = cross( x[index].getCenter(), linearMomentum ) + ( I * v[index].getAngular() ); + typename RigidTypes::Vec3 angularMomentum = cross( x[index].getCenter(), linearMomentum ) + ( d_vertexMass.getValue().rotate(pos[index].getOrientation()).inertiaMassMatrix * v[index].getAngular() ); for( int j=0 ; j< 3 ; ++j ) momentum[3+j] += angularMomentum[j]; } diff --git a/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.inl b/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.inl index f94c2381f6c..76a5e5b42e8 100644 --- a/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.inl +++ b/Sofa/Component/Mass/src/sofa/component/mass/UniformMass.inl @@ -400,6 +400,7 @@ void UniformMass::addMDx ( const core::MechanicalParams*, { helper::WriteAccessor res = vres; helper::ReadAccessor dx = vdx; + const auto & pos = this->getMState()->read(core::ConstVecCoordId::position())->getValue(); WriteAccessor > indices = d_indices; @@ -408,7 +409,13 @@ void UniformMass::addMDx ( const core::MechanicalParams*, m *= typename DataTypes::Real(factor); for (const auto i : indices) - res[i] += dx[i] * m; + { + if constexpr (std::is_same::value) + res[i] += dx[i] * m.rotate(pos[i].getOrientation()); + else + res[i] += dx[i] * m; + + } } @@ -419,12 +426,18 @@ void UniformMass::accFromF ( const core::MechanicalParams*, { WriteOnlyAccessor a = va; ReadAccessor f = vf; + const auto & pos = this->getMState()->read(core::ConstVecCoordId::position())->getValue(); const ReadAccessor > indices = d_indices; MassType m = d_vertexMass.getValue(); for (const auto i : indices) - a[i] = f[i] / m; + { + if constexpr (std::is_same::value) + a[i] = f[i] / m.rotate(pos[i].getOrientation()); + else + a[i] = f[i] / m; + } } @@ -473,6 +486,7 @@ void UniformMass::addForce ( const core::MechanicalParams*, DataVecDe return; helper::WriteAccessor f = vf; + const auto & pos = this->getMState()->read(core::ConstVecCoordId::position())->getValue(); // weight const SReal* g = getContext()->getGravity().ptr(); @@ -480,16 +494,16 @@ void UniformMass::addForce ( const core::MechanicalParams*, DataVecDe DataTypes::set ( theGravity, g[0], g[1], g[2] ); const MassType& m = d_vertexMass.getValue(); - Deriv mg = theGravity * m; - - dmsg_info() <<" addForce, mg = "<getNodeObject>(); + const auto * DataPos = mstate1->read(sofa::core::ConstVecId::position()); + const auto * DataVel = mstate1->read(sofa::core::ConstVecDerivId::velocity()); + + Rigid3Types::VecDeriv* CF1_force = reinterpret_cast(root->getChild("Aligned")->getObject("ConstantForceField1")->findData("forces")->beginEditVoidPtr()); + + //We apply two different rotation, one exactly normal to z and one slightly along z + (*CF1_force)[0][3] = 1.0; + (*CF1_force)[0][5] = 0.1; + + root->getChild("Aligned")->getObject("ConstantForceField1")->findData("forces")->endEditVoidPtr(); + + sofa::simulation::node::animate(root.get(),100); + + //After rotating for some time, the centrifugal forces should have made the Z axis (along which most of the mass is located) normal to the axis of rotation + sofa::type::Vec3 Vel1 = DataVel->getValue()[0].getVOrientation(); + sofa::type::Vec3 ori1_z = DataPos->getValue()[0].getOrientation().rotate(zAxis); + EXPECT_GT(norm(Vel1),0.0); + EXPECT_NEAR(dot(Vel1/norm(Vel1),ori1_z), 0.0, 1.0e-5 ); + + //To make sure the first try wasn't a fluke, we continue the rotation a bit and we check again + sofa::simulation::node::animate(root.get(),5); + Vel1 = DataVel->getValue()[0].getVOrientation(); + ori1_z = DataPos->getValue()[0].getOrientation().rotate(zAxis); + EXPECT_GT(norm(Vel1),0.0); + EXPECT_NEAR(dot(Vel1/norm(Vel1),ori1_z), 0.0, 1.0e-5 ); + + //and again + sofa::simulation::node::animate(root.get(),5); + Vel1 = DataVel->getValue()[0].getVOrientation(); + ori1_z = DataPos->getValue()[0].getOrientation().rotate(zAxis); + EXPECT_GT(norm(Vel1),0.0); + EXPECT_NEAR(dot(Vel1/norm(Vel1),ori1_z), 0.0, 1.0e-5 ); + + //and one final time + sofa::simulation::node::animate(root.get(),5); + Vel1 = DataVel->getValue()[0].getVOrientation(); + ori1_z = DataPos->getValue()[0].getOrientation().rotate(zAxis); + EXPECT_GT(norm(Vel1),0.0); + EXPECT_NEAR(dot(Vel1/norm(Vel1),ori1_z), 0.0, 1.0e-5 ); + } + }; @@ -599,4 +648,8 @@ TYPED_TEST(UniformMassTest, nonIdentityInertiaMatrix_RotationOfOneRigid){ EXPECT_NO_THROW(this->nonIdentityInertiaMatrix_RotationOfOneRigid()); } +TYPED_TEST(UniformMassTest, nonIdentityInertiaMatrix_CentrifugalForces){ + EXPECT_NO_THROW(this->nonIdentityInertiaMatrix_CentrifugalForces()); +} +