From d3b153baf3988325e790ba89fcd727081bbfca0a Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Wed, 10 Jul 2024 17:06:54 +0200 Subject: [PATCH 01/16] Refactor --- Tests/Example.cpp | 10 - Tests/Example.h | 6 - tests/CMakeLists.txt | 2 + .../Cosserat.Binding.Tests}/CMakeLists.txt | 12 +- .../constraint/CMakeLists.txt | 0 .../constraint/ClassName.h | 0 .../constraint/Constraint.h | 0 ...ratUnilateralInteractionConstraintTest.cpp | 0 .../constraint/ExampleTest.cpp | 0 .../forcefield/BeamHookeLawForceFieldTest.cpp | 0 .../mapping/DiscretCosseratMappingTest.cpp | 0 .../mapping/POEMapping_test1.pyscn | 0 .../mapping/POEMapping_test2.pyscn | 0 tests/Cosserat.Core.Tests/CMakeLists.txt | 33 +++ .../constraint/CMakeLists.txt | 23 ++ .../constraint/ClassName.h | 12 + .../constraint/Constraint.h | 25 ++ ...ratUnilateralInteractionConstraintTest.cpp | 167 ++++++++++++ .../constraint/ExampleTest.cpp | 34 +++ .../forcefield/BeamHookeLawForceFieldTest.cpp | 201 ++++++++++++++ .../mapping/DiscretCosseratMappingTest.cpp | 247 ++++++++++++++++++ .../mapping/POEMapping_test1.pyscn | 240 +++++++++++++++++ .../mapping/POEMapping_test2.pyscn | 240 +++++++++++++++++ tests/Cosserat.Scene.Tests/CMakeLists.txt | 63 +++++ .../Components/BaseCosseratMapping.py | 12 + .../Components/DiscreteCosseratMapping.py | 46 ++++ .../Cosserat_Python_Tests.cpp | 68 +++++ .../Cosserat_Python_Tests.h | 2 + 28 files changed, 1418 insertions(+), 25 deletions(-) delete mode 100644 Tests/Example.cpp delete mode 100644 Tests/Example.h create mode 100644 tests/CMakeLists.txt rename {Tests => tests/Cosserat.Binding.Tests}/CMakeLists.txt (69%) rename {Tests => tests/Cosserat.Binding.Tests}/constraint/CMakeLists.txt (100%) rename {Tests => tests/Cosserat.Binding.Tests}/constraint/ClassName.h (100%) rename {Tests => tests/Cosserat.Binding.Tests}/constraint/Constraint.h (100%) rename {Tests => tests/Cosserat.Binding.Tests}/constraint/CosseratUnilateralInteractionConstraintTest.cpp (100%) rename {Tests => tests/Cosserat.Binding.Tests}/constraint/ExampleTest.cpp (100%) rename {Tests => tests/Cosserat.Binding.Tests}/forcefield/BeamHookeLawForceFieldTest.cpp (100%) rename {Tests => tests/Cosserat.Binding.Tests}/mapping/DiscretCosseratMappingTest.cpp (100%) rename {Tests => tests/Cosserat.Binding.Tests}/mapping/POEMapping_test1.pyscn (100%) rename {Tests => tests/Cosserat.Binding.Tests}/mapping/POEMapping_test2.pyscn (100%) create mode 100644 tests/Cosserat.Core.Tests/CMakeLists.txt create mode 100644 tests/Cosserat.Core.Tests/constraint/CMakeLists.txt create mode 100644 tests/Cosserat.Core.Tests/constraint/ClassName.h create mode 100644 tests/Cosserat.Core.Tests/constraint/Constraint.h create mode 100644 tests/Cosserat.Core.Tests/constraint/CosseratUnilateralInteractionConstraintTest.cpp create mode 100644 tests/Cosserat.Core.Tests/constraint/ExampleTest.cpp create mode 100644 tests/Cosserat.Core.Tests/forcefield/BeamHookeLawForceFieldTest.cpp create mode 100644 tests/Cosserat.Core.Tests/mapping/DiscretCosseratMappingTest.cpp create mode 100644 tests/Cosserat.Core.Tests/mapping/POEMapping_test1.pyscn create mode 100644 tests/Cosserat.Core.Tests/mapping/POEMapping_test2.pyscn create mode 100644 tests/Cosserat.Scene.Tests/CMakeLists.txt create mode 100644 tests/Cosserat.Scene.Tests/Components/BaseCosseratMapping.py create mode 100644 tests/Cosserat.Scene.Tests/Components/DiscreteCosseratMapping.py create mode 100644 tests/Cosserat.Scene.Tests/Cosserat_Python_Tests.cpp create mode 100644 tests/Cosserat.Scene.Tests/Cosserat_Python_Tests.h diff --git a/Tests/Example.cpp b/Tests/Example.cpp deleted file mode 100644 index 94bbcf05..00000000 --- a/Tests/Example.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include "Example.h" - -int MAC(int x, int y, int & sum){ - sum += x*y ; - return sum; -} - -int Square(int x){ - return x*x; -} \ No newline at end of file diff --git a/Tests/Example.h b/Tests/Example.h deleted file mode 100644 index 998cb741..00000000 --- a/Tests/Example.h +++ /dev/null @@ -1,6 +0,0 @@ -#pragma once - - -int MAC(int x, int y, int & sum); - -int Square(int x); diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt new file mode 100644 index 00000000..e5e2fc57 --- /dev/null +++ b/tests/CMakeLists.txt @@ -0,0 +1,2 @@ +add_subdirectory(Cosserat.Core.Tests) +add_subdirectory(Cosserat.Scene.Tests) diff --git a/Tests/CMakeLists.txt b/tests/Cosserat.Binding.Tests/CMakeLists.txt similarity index 69% rename from Tests/CMakeLists.txt rename to tests/Cosserat.Binding.Tests/CMakeLists.txt index d15db727..b686e957 100644 --- a/Tests/CMakeLists.txt +++ b/tests/Cosserat.Binding.Tests/CMakeLists.txt @@ -1,31 +1,25 @@ cmake_minimum_required(VERSION 3.12) +project(Cosserat_core_test C CXX) -set(This Cosserat_test) - -project(${This} C CXX) - -#find_package(Cosserat REQUIRED) find_package(Sofa.Testing REQUIRED) enable_testing() set(HEADER_FILES - Example.h constraint/Constraint.h ) set(SOURCE_FILES - Example.cpp constraint/ExampleTest.cpp -# constraint/CosseratUnilateralInteractionConstraintTest.cpp forcefield/BeamHookeLawForceFieldTest.cpp ) -add_executable(${This} ${SOURCE_FILES} ${HEADER_FILES}) +add_executable(${PROJECT_NAME} ${SOURCE_FILES} ${HEADER_FILES}) target_link_libraries(${PROJECT_NAME} Sofa.Testing Cosserat + ) target_include_directories(${This} diff --git a/Tests/constraint/CMakeLists.txt b/tests/Cosserat.Binding.Tests/constraint/CMakeLists.txt similarity index 100% rename from Tests/constraint/CMakeLists.txt rename to tests/Cosserat.Binding.Tests/constraint/CMakeLists.txt diff --git a/Tests/constraint/ClassName.h b/tests/Cosserat.Binding.Tests/constraint/ClassName.h similarity index 100% rename from Tests/constraint/ClassName.h rename to tests/Cosserat.Binding.Tests/constraint/ClassName.h diff --git a/Tests/constraint/Constraint.h b/tests/Cosserat.Binding.Tests/constraint/Constraint.h similarity index 100% rename from Tests/constraint/Constraint.h rename to tests/Cosserat.Binding.Tests/constraint/Constraint.h diff --git a/Tests/constraint/CosseratUnilateralInteractionConstraintTest.cpp b/tests/Cosserat.Binding.Tests/constraint/CosseratUnilateralInteractionConstraintTest.cpp similarity index 100% rename from Tests/constraint/CosseratUnilateralInteractionConstraintTest.cpp rename to tests/Cosserat.Binding.Tests/constraint/CosseratUnilateralInteractionConstraintTest.cpp diff --git a/Tests/constraint/ExampleTest.cpp b/tests/Cosserat.Binding.Tests/constraint/ExampleTest.cpp similarity index 100% rename from Tests/constraint/ExampleTest.cpp rename to tests/Cosserat.Binding.Tests/constraint/ExampleTest.cpp diff --git a/Tests/forcefield/BeamHookeLawForceFieldTest.cpp b/tests/Cosserat.Binding.Tests/forcefield/BeamHookeLawForceFieldTest.cpp similarity index 100% rename from Tests/forcefield/BeamHookeLawForceFieldTest.cpp rename to tests/Cosserat.Binding.Tests/forcefield/BeamHookeLawForceFieldTest.cpp diff --git a/Tests/mapping/DiscretCosseratMappingTest.cpp b/tests/Cosserat.Binding.Tests/mapping/DiscretCosseratMappingTest.cpp similarity index 100% rename from Tests/mapping/DiscretCosseratMappingTest.cpp rename to tests/Cosserat.Binding.Tests/mapping/DiscretCosseratMappingTest.cpp diff --git a/Tests/mapping/POEMapping_test1.pyscn b/tests/Cosserat.Binding.Tests/mapping/POEMapping_test1.pyscn similarity index 100% rename from Tests/mapping/POEMapping_test1.pyscn rename to tests/Cosserat.Binding.Tests/mapping/POEMapping_test1.pyscn diff --git a/Tests/mapping/POEMapping_test2.pyscn b/tests/Cosserat.Binding.Tests/mapping/POEMapping_test2.pyscn similarity index 100% rename from Tests/mapping/POEMapping_test2.pyscn rename to tests/Cosserat.Binding.Tests/mapping/POEMapping_test2.pyscn diff --git a/tests/Cosserat.Core.Tests/CMakeLists.txt b/tests/Cosserat.Core.Tests/CMakeLists.txt new file mode 100644 index 00000000..bb53fe82 --- /dev/null +++ b/tests/Cosserat.Core.Tests/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.12) +project(Cosserat.Core.Tests C CXX) + +find_package(Sofa.Testing REQUIRED) + +enable_testing() + +set(HEADER_FILES + constraint/Constraint.h + ) +set(SOURCE_FILES + forcefield/BeamHookeLawForceFieldTest.cpp + ) + + +add_executable(${PROJECT_NAME} ${SOURCE_FILES} ${HEADER_FILES}) + +target_link_libraries(${PROJECT_NAME} + Sofa.Testing + Cosserat +) + +target_include_directories(${PROJECT_NAME} + PUBLIC + "$" +) + + +add_test( + NAME ${PROJECT_NAME} + COMMAND ${PROJECT_NAME} +) + diff --git a/tests/Cosserat.Core.Tests/constraint/CMakeLists.txt b/tests/Cosserat.Core.Tests/constraint/CMakeLists.txt new file mode 100644 index 00000000..672f79b7 --- /dev/null +++ b/tests/Cosserat.Core.Tests/constraint/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.1) + +#[[project(Cosserat.test VERSION 1.0)]] +set(This ExampleTest) + +set(SOURCE_FILES + ExampleTest.cpp + ) + +add_executable(${This} ${SOURCE_FILES}) + +target_link_libraries(${This} PUBLIC + SofaTest + gtest + gtest_main + Cosserat + Example +) + +add_test( + NAME ${This} + COMMAND ${This} +) diff --git a/tests/Cosserat.Core.Tests/constraint/ClassName.h b/tests/Cosserat.Core.Tests/constraint/ClassName.h new file mode 100644 index 00000000..996a3693 --- /dev/null +++ b/tests/Cosserat.Core.Tests/constraint/ClassName.h @@ -0,0 +1,12 @@ +// +// Created by younes on 31/05/2021. +// + +#pragma once + + +class ClassName { + +}; + +#endif //SOFA_CLASSNAME_H diff --git a/tests/Cosserat.Core.Tests/constraint/Constraint.h b/tests/Cosserat.Core.Tests/constraint/Constraint.h new file mode 100644 index 00000000..df188d76 --- /dev/null +++ b/tests/Cosserat.Core.Tests/constraint/Constraint.h @@ -0,0 +1,25 @@ +// +// Created by younes on 02/06/2021. +// + +#pragma once + +#include +#include +#include + +#include "../../src/constraint/CosseratUnilateralInteractionConstraint.h" + + +using namespace sofa::defaulttype; + +bool testUpdateList(){ + + // + // + + /* @todo do the unitest code here for the updateList function + Check that the outout is a list of int + */ + return true; +} diff --git a/tests/Cosserat.Core.Tests/constraint/CosseratUnilateralInteractionConstraintTest.cpp b/tests/Cosserat.Core.Tests/constraint/CosseratUnilateralInteractionConstraintTest.cpp new file mode 100644 index 00000000..43d39519 --- /dev/null +++ b/tests/Cosserat.Core.Tests/constraint/CosseratUnilateralInteractionConstraintTest.cpp @@ -0,0 +1,167 @@ +// +// Created by younes on 02/06/2021. +// + +#include +using testing::Test; + +#include +using sofa::testing::BaseTest ; + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include "../../src/constraint/CosseratUnilateralInteractionConstraint.h" +#include "../../src/constraint/CosseratUnilateralInteractionConstraint.inl" + +#include "Constraint.h" +#include "../Example.h" + +using sofa::simulation::SceneLoaderXML ; + +namespace sofa { + +template +struct CosseratUnilateralInteractionConstraintTest : public NumericTest<> +{ + typedef _DataTypes DataTypes; + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename Coord::value_type Real; + typedef sofa::component::constraintset::CosseratUnilateralInteractionConstraint CosseratUnilateralInteractionConstraint; + + int* x; + int GetX() const{ + return *x; + } + + // Sets up the test fixture. + void SetUp() override { + // initialization or some code to run before each test + fprintf(stderr, "Starting up ! \n"); + x = new int(42); + + sofa::simpleapi::importPlugin("Sofa.Component"); + sofa::simpleapi::importPlugin("Cosserat"); + + //create the context for + if(simulation==nullptr) + sofa::simulation::setSimulation(simulation = new sofa::simulation::graph::DAGSimulation()); + } + + // Tears down the test fixture. + void TearDown() override { + // code to run after each test; + // can be used instead of a destructor, + // but exceptions can be handled in this function only + fprintf(stderr, "Starting down ! \n"); + delete x; + if(root) + simulation->unload(root); + } + + bool UpdateListTest() + { + /* @todo do the code here in order to test the updateList function */ + // m_constraint->UpdateList(); + // m_constraint.UpdateList(); + //typename CosseratConstraint::SPtr constraint = sofa::core::objectmodel::New(); + // simulation::Node::SPtr root = simulation->createNewGraph("root"); + // root->setGravity( defaulttype::Vector3(0,0,0) ); + CosseratUnilateralInteractionConstraint * m_constraint = new CosseratUnilateralInteractionConstraint; + + if (m_constraint == NULL) + return false; + else + return m_constraint->UpdateList(); + } + + /** + * + */ + void attributesTests(); + + protected: + ///< Root of the scene graph, created by the constructor an re-used in the tests + simulation::Node::SPtr root; + ///< created by the constructor an re-used in the tests + simulation::Simulation* simulation {nullptr}; +}; + + +template<> +void CosseratUnilateralInteractionConstraintTest::attributesTests(){ + /// I'm using '\n' so that the XML parser correctly report the line number + /// in case of problems. + std::stringstream scene; + scene << " \n" + " \n" + " \n" + " \n" + " \n" ; + + Node::SPtr root = SceneLoaderXML::loadFromMemory ("testscene", + scene.str().c_str(), + scene.str().size()) ; + root->init(sofa::core::execparams::defaultInstance()) ; + CosseratUnilateralInteractionConstraint * constraint = root->getTreeObject() ; + + EXPECT_TRUE( constraint != nullptr ) ; + EXPECT_TRUE( constraint->findData("value") != nullptr ) ; + EXPECT_TRUE( constraint->findData("force_dumping") != nullptr ) ; + EXPECT_TRUE( constraint->findData("valueIndex") != nullptr ) ; + EXPECT_TRUE( constraint->findData("entryPoint") != nullptr ) ; + EXPECT_TRUE( constraint->findData("vectorOfIndices") != nullptr ) ; + + return ; + } + + +/* an easy example */ +//TEST(ConstraintTest, Square){ +// int x = 5; +// int expectedSquare = x*x; +// EXPECT_EQ(expectedSquare, Square(x)); +//} + + +// Define the list of DataTypes to instantiation +using ::testing::Types; +typedef Types DataTypes; // the types to instantiate. + +// Test suite for Vec3Types instantiation +TYPED_TEST_SUITE(CosseratUnilateralInteractionConstraintTest, DataTypes); + +//TYPED_TEST(ConstraintTest, MAC) +//{ +// int y = 16; +// int sum = 100; +// auto val = this->GetX() ; +//// auto val = ConstraintTest->GetX() +// auto oldSum = sum + val * y; //sum + this->GetX() * y; +// int expectedNewSum = sum + val * y; //sum + this->GetX() * y; +// +// EXPECT_EQ(expectedNewSum,MAC(val,y,sum)); +// //EXPECT_EQ(expectedNewSum,oldSum); +//} + +// first test case, test the UpdateList function +TYPED_TEST( CosseratUnilateralInteractionConstraintTest, UpdateListTest ) +{ + EXPECT_TRUE(this->UpdateListTest()); +} + +TYPED_TEST( CosseratUnilateralInteractionConstraintTest , attributesTests) +{ + ASSERT_NO_THROW( this->attributesTests() ); +} + +} //sofa diff --git a/tests/Cosserat.Core.Tests/constraint/ExampleTest.cpp b/tests/Cosserat.Core.Tests/constraint/ExampleTest.cpp new file mode 100644 index 00000000..ea274c05 --- /dev/null +++ b/tests/Cosserat.Core.Tests/constraint/ExampleTest.cpp @@ -0,0 +1,34 @@ +// +// Created by younes on 31/05/2021. +// + +#include +#include "../Example.h" + +TEST(ExempleTests,MAC) +{ + int x = 42; + int y = 16; + int sum = 100; + int oldSum = sum; + int expectedNewSum = oldSum + x * y; + + EXPECT_EQ( + expectedNewSum, + MAC(x,y,sum) + ); + + EXPECT_EQ( + expectedNewSum, + sum + ); +} + +TEST(ExampleTests, Square){ + int x = 5; + int expectedSquare = x*x; + EXPECT_EQ( + expectedSquare, + Square(x) + ); +} \ No newline at end of file diff --git a/tests/Cosserat.Core.Tests/forcefield/BeamHookeLawForceFieldTest.cpp b/tests/Cosserat.Core.Tests/forcefield/BeamHookeLawForceFieldTest.cpp new file mode 100644 index 00000000..02a84f87 --- /dev/null +++ b/tests/Cosserat.Core.Tests/forcefield/BeamHookeLawForceFieldTest.cpp @@ -0,0 +1,201 @@ +// +// Created by younes on 07/06/2021. +// + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +using sofa::testing::BaseTest ; +using testing::Test; +using sofa::simulation::SceneLoaderXML ; +using namespace sofa::simpleapi; + +namespace sofa { + +template +struct BeamHookeLawForceFieldTest : public testing::NumericTest<> { + typedef _DataTypes DataTypes; + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename Coord::value_type Real; + + + + typedef sofa::component::forcefield::BeamHookeLawForceField TheBeamHookeLawForceField; + + // Sets up the test fixture. + void SetUp() override { + // initialization or some code to run before each test + fprintf(stderr, "Starting up ! \n"); + sofa::simpleapi::importPlugin("Sofa.Component"); + sofa::simpleapi::importPlugin("Cosserat"); + } + + // Tears down the test fixture. + void TearDown() override { + // code to run after each test; + // can be used instead of a destructor, + // but exceptions can be handled in this function only + if(root) { + sofa::simulation::node::unload(root); + } + fprintf(stderr, "Starting down ! \n"); + } + + void reinit() + { + /* @todo do the code here in order to test the updateList function */ + // m_constraint->UpdateList(); + // m_constraint.UpdateList(); + //typename CosseratConstraint::SPtr constraint = sofa::core::objectmodel::New(); + // simulation::Node::SPtr root = simulation->createNewGraph("root"); + // root->setGravity( defaulttype::Vector3(0,0,0) ); + auto m_forcefield = new TheBeamHookeLawForceField; + + if (m_forcefield == NULL) + return ; + else + m_forcefield->reinit(); + } + + /** + * + */ + void basicAttributesTest(); + void triangle(); + +// void addForceTest(const MechanicalParams* mparams, +// DataVecDeriv& f , +// const DataVecCoord& x , +// const DataVecDeriv& v) override; +// +// void addDForceTest(const MechanicalParams* mparams, +// DataVecDeriv& df , +// const DataVecDeriv& +// dx ) override; +// +// +// void addKToMatrixTest(const MechanicalParams* mparams, +// const MultiMatrixAccessor* matrix) override; +// +// double getPotentialEnergyTest(const MechanicalParams* mparams, +// const DataVecCoord& x) const override; + +protected: + ///< Root of the scene graph, created by the constructor an re-used in the tests + simulation::Node::SPtr root; + + void testFonctionnel(); +}; + + +template<> +void BeamHookeLawForceFieldTest::testFonctionnel() { + EXPECT_MSG_NOEMIT(Error, Warning) ; + createObject(root, "MechanicalObject", {{"position", "-1 0 1 1 0 1 -1 0 -1 1 0 -1 0 0 1 0 0 -1 -1 0 0 1 0 0 0 0 0"}}); + createObject(root, "TriangleSetTopologyContainer", {{"triangles", "7 5 8 8 2 6 4 6 0 1 8 4 7 3 5 8 5 2 4 8 6 1 7 8"}}); + + auto traction = dynamic_cast( + createObject(root, "BeamHookeLawForceField", {{"name", "beamHookeLaw"}, + {"crossSectionShape", "circular"}, + {"lengthY", "35e-5"}, + {"lengthZ", "1374e-5"}, + {"radius", "0.25"}, + {"varianteSections", "true"}}).get() + ); + + EXPECT_NE(traction, nullptr); + EXPECT_NE(root.get(), nullptr) ; + root->init(sofa::core::execparams::defaultInstance()) ; + + auto total_load = dynamic_cast *>(traction->findData("lengthY")); + for (unsigned int step = 1; step <= 5; ++step) { + sofa::simulation::node::animate(root.get(), 1); + EXPECT_DOUBLE_EQ(total_load->getValue(), 4*step) << "Total load at time step " << step << " is incorrect."; + } +} + + + +template<> +void BeamHookeLawForceFieldTest::basicAttributesTest(){ + EXPECT_MSG_NOEMIT(Error) ; + + std::stringstream scene ; + scene << "" + " \n" + " \n" + " \n" + " \n" + " \n" ; + + Node::SPtr root = SceneLoaderXML::loadFromMemory ("testscene", + scene.str().c_str()) ; + + EXPECT_NE(root.get(), nullptr) ; + root->init(sofa::core::execparams::defaultInstance()) ; + + TheBeamHookeLawForceField* forcefield ; + root->getTreeObject(forcefield) ; + + EXPECT_NE(nullptr, forcefield) ; + + /// List of the supported attributes the user expect to find + /// This list needs to be updated if you add an attribute. + sofa::type::vector attrnames = { + "crossSectionShape","youngModulus","poissonRatio","length", "radius", + "innerRadius", "lengthY", "lengthZ", "varianteSections", "youngModululsList", "poissonRatioList" + }; + + for(auto& attrname : attrnames) + EXPECT_NE( nullptr, forcefield->findData(attrname) ) + << "Missing attribute with name '" + << attrname << "'." ; + + for(int i=0; i<10; i++){ + sofa::simulation::node::animate(root.get(),(double)0.01); + } +} + + +/*** + * The test section + */ + +// Define the list of DataTypes to instanciate +using ::testing::Types; +typedef Types DataTypes; // the types to instantiate. + +// Test suite for all the instanciations +TYPED_TEST_SUITE(BeamHookeLawForceFieldTest, DataTypes);// first test case +TYPED_TEST( BeamHookeLawForceFieldTest , basicAttributesTest ) +{ + ASSERT_NO_THROW (this->basicAttributesTest()); +} + +TYPED_TEST(BeamHookeLawForceFieldTest, testFonctionnel) { + ASSERT_NO_THROW (this->testFonctionnel()); +} + + + + + +} diff --git a/tests/Cosserat.Core.Tests/mapping/DiscretCosseratMappingTest.cpp b/tests/Cosserat.Core.Tests/mapping/DiscretCosseratMappingTest.cpp new file mode 100644 index 00000000..1b2b1739 --- /dev/null +++ b/tests/Cosserat.Core.Tests/mapping/DiscretCosseratMappingTest.cpp @@ -0,0 +1,247 @@ +#include +using std::string ; +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "../../src/mapping/DiscreteCosseratMapping.inl" +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +using sofa::simulation::SceneLoaderXML ; +using sofa::simulation::Node ; +using sofa::simulation::SceneLoaderPY; +using sofa::component::container::MechanicalObject ; + +#include +using sofa::helper::system::SetDirectory; + +namespace sofa { +namespace { // anonymous namespace +using namespace core; +using namespace component; +using defaulttype::Vec; +using defaulttype::Mat; + + +/** Test suite for RigidMapping. +The test cases are defined in the #Test_Cases member group. + */ +template +struct DiscretCosseratMappingTest : public Multi2Mapping_test<_DiscretCosseratMapping> +{ + + typedef _DiscretCosseratMapping DiscretCosseratMapping; + typedef Multi2Mapping_test Inherit; + + typedef typename DiscretCosseratMapping::In1 In1Type; + typedef typename In1Type::VecCoord In1VecCoord; + typedef typename In1Type::VecDeriv In1VecDeriv; + typedef typename In1Type::Coord In1Coord; + typedef typename In1Type::Deriv In1Deriv; + typedef typename DiscretCosseratMapping::In1DataVecCoord In1DataVecCoord; + typedef typename DiscretCosseratMapping::In1DataVecDeriv In1DataVecDeriv; + typedef container::MechanicalObject In1DOFs; + typedef typename In1DOFs::ReadVecCoord ReadIn1VecCoord; + typedef typename In1DOFs::WriteVecCoord WriteIn1VecCoord; + typedef typename In1DOFs::WriteVecDeriv WriteIn1VecDeriv; + typedef typename In1Type::Real Real; + typedef Mat In1RotationMatrix; + + typedef typename DiscretCosseratMapping::In2 In2Type; + typedef typename In2Type::VecCoord In2VecCoord; + typedef typename In2Type::VecDeriv In2VecDeriv; + typedef typename In2Type::Coord In2Coord; + typedef typename In2Type::Deriv In2Deriv; + typedef typename DiscretCosseratMapping::In2DataVecCoord In2DataVecCoord; + typedef typename DiscretCosseratMapping::In2DataVecDeriv In2DataVecDeriv; + typedef container::MechanicalObject In2DOFs; + typedef typename In2DOFs::ReadVecCoord ReadIn2VecCoord; + typedef typename In2DOFs::WriteVecCoord WriteIn2VecCoord; + typedef typename In2DOFs::WriteVecDeriv WriteIn2VecDeriv; + typedef typename In1Type::Real Real2; + typedef Mat In2RotationMatrix; + + typedef typename DiscretCosseratMapping::Out OutType; + typedef typename OutType::VecCoord OutVecCoord; + typedef typename OutType::VecDeriv OutVecDeriv; + typedef typename OutType::Coord OutCoord; + typedef typename OutType::Deriv OutDeriv; + typedef typename DiscretCosseratMapping::OutDataVecCoord OutDataVecCoord; + typedef typename DiscretCosseratMapping::OutDataVecDeriv OutDataVecDeriv; + typedef container::MechanicalObject OutDOFs; + typedef typename OutDOFs::WriteVecCoord WriteOutVecCoord; + typedef typename OutDOFs::WriteVecDeriv WriteOutVecDeriv; + typedef typename OutDOFs::ReadVecCoord ReadOutVecCoord; + typedef typename OutDOFs::ReadVecDeriv ReadOutVecDeriv; + + typedef core::Multi2Mapping Multi2Mapping; + + typedef component::linearsolver::EigenSparseMatrix SparseJMatrixEigen1; + typedef component::linearsolver::EigenSparseMatrix SparseJMatrixEigen2; + typedef linearsolver::EigenSparseMatrix SparseKMatrixEigen1; + typedef linearsolver::EigenSparseMatrix SparseKMatrixEigen2; + + + + DiscretCosseratMapping* poeMapping; ///< the mapping to be tested + vector in1Dofs; ///< mapping input + vector in2Dofs; ///< mapping input + OutDOFs* outDofs; ///< mapping output + + std::string filepath; + + //simulation::SceneLoaderPY loader; + + /// Constructor + DiscretCosseratMappingTest() + :filepath("/home/younes/travail/plugin/plugin.Cosserat/tests/mapping/POEMapping_test1.pyscn") + { + // this->errorFactorDJ = 200; + + // poeMapping = static_cast( this->Multi2Mapping ); + + // // beamLengthMapping::getJs is not yet implemented + // this->flags &= ~Inherit::TEST_getJs; + + // // beamLengthMapping::getK is not yet implemented + // //this->flags &= ~Inherit::TEST_getK; + + // // beamLengthMapping::applyDJT is not yet implemented + // //this->flags &= ~Inherit::TEST_applyDJT; + + } + + /** @name Test_Cases + verify the computation of the beam lengths and the derivatives + */ + ///@{ + /** Two frames are placed + line topology + and the mapping is constructed + */ + + bool runTest1() + { + printf("================================> Inside the POEMapping test scene \n"); + + return true; + // const char *filename = filepath.c_str(); ; + // SetDirectory localDir(filename); + // std::string basename = SetDirectory::GetFileNameWithoutExtension(SetDirectory::GetFileName(filename).c_str()); + + // std::cout << "basename : "<< basename << std::endl; + + + // SceneLoaderPY loader; + // Node::SPtr root = loader.doLoad(filename); + + + // //std::cout<<"******************* Get the mapping "; + // DiscretCosseratMapping* poeMapping; + // this->root->getTreeObject(poeMapping); + // this->mapping = poeMapping; + // this->deltaRange.first= 1; + // this->deltaRange.second= 1000; + + + // MechanicalObject* FromModel1 = nullptr; + // this->root->getTreeObject(FromModel1); + // //this->in1Dofs = FromModel1; + // const Data& dataIn1X = *FromModel1->read(VecCoordId::position()); + // const In1VecCoord& x1in = dataIn1X.getValue(); + + // helper::vector vecIn1; + // vecIn1.push_back(x1in); + + // MechanicalObject* FromModel2 = nullptr; + // this->root->getTreeObject(FromModel2); + // //this->in2Dofs = FromModel2; + // const Data& dataIn2X = *FromModel2->read(VecCoordId::position()); + // const In2VecCoord& x2in = dataIn2X.getValue(); + + // helper::vector vecIn2; + // vecIn2.push_back(x2in); + + + // MechanicalObject* ToModel = nullptr; + // this->root->getTreeObject(ToModel); + // this->outDofs= ToModel; + // const Data& dataOutX = *ToModel->read(VecCoordId::position()); + // const OutVecCoord& xout = dataOutX.getValue(); + + // std::cout<<" x1 in = "<" +) + +add_test( + NAME ${PROJECT_NAME} + COMMAND $${PROJECT_NAME} +) + + +find_package(Sofa.Testing REQUIRED) + +# This will set rpaths relative to all SOFA core AND relocatable dependencies +sofa_auto_set_target_rpath( + TARGETS ${PROJECT_NAME} + RELOCATABLE "plugins" +) + + +set(DIR_BINDING_LIST + Components + ) +foreach(dir_binding ${DIR_BINDING_LIST}) + if (_isMultiConfig) # MSVC + foreach(config_type ${CMAKE_CONFIGURATION_TYPES}) + SP3_configure_directory(${CMAKE_CURRENT_SOURCE_DIR}/${dir_binding} ${CMAKE_BINARY_DIR}/${RUNTIME_OUTPUT_DIRECTORY}/${config_type}/${PROJECT_NAME}.d/${dir_binding}) + install(DIRECTORY ${CMAKE_BINARY_DIR}/${RUNTIME_OUTPUT_DIRECTORY}/${config_type}/${PROJECT_NAME}.d/${dir_binding} DESTINATION ${RUNTIME_OUTPUT_DIRECTORY}/${PROJECT_NAME}.d) + endforeach() + else() + SP3_configure_directory(${CMAKE_CURRENT_SOURCE_DIR}/${dir_binding} ${CMAKE_BINARY_DIR}/${RUNTIME_OUTPUT_DIRECTORY}/${PROJECT_NAME}.d/${dir_binding}) + install(DIRECTORY ${CMAKE_BINARY_DIR}/${RUNTIME_OUTPUT_DIRECTORY}/${PROJECT_NAME}.d/${dir_binding} DESTINATION ${RUNTIME_OUTPUT_DIRECTORY}/${PROJECT_NAME}.d) + endif() +endforeach() diff --git a/tests/Cosserat.Scene.Tests/Components/BaseCosseratMapping.py b/tests/Cosserat.Scene.Tests/Components/BaseCosseratMapping.py new file mode 100644 index 00000000..7294d393 --- /dev/null +++ b/tests/Cosserat.Scene.Tests/Components/BaseCosseratMapping.py @@ -0,0 +1,12 @@ +# coding: utf8 + +import Sofa +import Sofa.Testing +import Sofa.Simulation +import SofaRuntime +import unittest +import numpy + +class Test(unittest.TestCase): + def test_example_scene(self): + pass diff --git a/tests/Cosserat.Scene.Tests/Components/DiscreteCosseratMapping.py b/tests/Cosserat.Scene.Tests/Components/DiscreteCosseratMapping.py new file mode 100644 index 00000000..4a520378 --- /dev/null +++ b/tests/Cosserat.Scene.Tests/Components/DiscreteCosseratMapping.py @@ -0,0 +1,46 @@ +# coding: utf8 + +import Sofa +import Sofa.Simulation +import SofaRuntime +import unittest +import numpy + +class Test(unittest.TestCase): + def test_example_scene(self): + + scene = Sofa.import_sofa_python_scene("/home/dmarchal/projects/dev/sofa1/plugins/Cosserat/docs/testScene/tuto_1.py") + + # create a root node to store the scene + root = Sofa.Core.Node("rootNode") + + # fill the root node with the content of the scene + scene.createScene(root) + + # initialize everything + Sofa.Simulation.init(root) + + scenarios = [{ + "value" : [[0.0,0.0,0.0]] * 3, + "result" : [[-0., -0., -0., 0., 0., 0., 1.], + [10., -0., -0., 0., 0., 0., 1.], + [20., -0., -0., 0., 0., 0., 1.], + [30., -0., -0., 0., 0., 0., 1.]] + }, + { + "value" : [[0.0,0.1,0.0]] * 3, + "result" : [[ -0. , -0. , -0. , 0. , + 0. , 0. , 1. ], + [ 8.41470985, -0. , -4.59697694, 0. , + 0.47942554, 0. , 0.87758256], + [ 9.09297427, -0. , -14.16146837, 0. , + 0.84147098, 0. , 0.54030231], + [ 1.41120008, -0. , -19.89992497, 0. , + 0.99749499, 0. , 0.0707372 ]] + } + ] + for scenario in scenarios: + root.cosseratCoordinate.cosserat_state.position.value = scenario["value"] + Sofa.Simulation.animate(root, 0.01) + numpy.testing.assert_array_almost_equal(root.rigid_base.cosserat_in_Sofa_frame_node.FramesMO.position.value, scenario["result"]) + diff --git a/tests/Cosserat.Scene.Tests/Cosserat_Python_Tests.cpp b/tests/Cosserat.Scene.Tests/Cosserat_Python_Tests.cpp new file mode 100644 index 00000000..196c86f7 --- /dev/null +++ b/tests/Cosserat.Scene.Tests/Cosserat_Python_Tests.cpp @@ -0,0 +1,68 @@ +/****************************************************************************** +* Cosserat plugin * +* (c) 2024 CNRS, University of Lille, INRIA * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#include +#include +#include +#include "Cosserat_Python_Tests.h" + +using sofapython3::PythonTest ; +using sofapython3::PythonTestExtractor ; +using sofapython3::PrintTo ; +using std::string; + +#include +#include +using sofa::helper::logging::MessageDispatcher; +using sofa::helper::logging::MainPerComponentLoggingMessageHandler; + +namespace +{ + +bool init() +{ + MessageDispatcher::addHandler(&MainPerComponentLoggingMessageHandler::getInstance()) ; + return true; +} + +static int _inited_=init(); + +class Cosserat : public PythonTest {}; + +/// static build of the test list +static struct Cosserat_Scene_tests : public PythonTestExtractor +{ + Cosserat_Scene_tests() + { + std::string executable_directory = sofa::helper::Utils::getExecutableDirectory(); + executable_directory = "./"; + addTestDirectory(executable_directory+"/Cosserat.Scene.Tests/Components", "Cosserat_Scene_"); + } +} python_tests; + +/// run test list using the custom name function getTestName. +/// this allows to do gtest_filter=*FileName* +INSTANTIATE_TEST_SUITE_P(Cosserat, + Cosserat, + ::testing::ValuesIn(python_tests.extract()), + Cosserat::getTestName); + +TEST_P(Cosserat, all_tests) { run(GetParam()); } + +} diff --git a/tests/Cosserat.Scene.Tests/Cosserat_Python_Tests.h b/tests/Cosserat.Scene.Tests/Cosserat_Python_Tests.h new file mode 100644 index 00000000..3f59c932 --- /dev/null +++ b/tests/Cosserat.Scene.Tests/Cosserat_Python_Tests.h @@ -0,0 +1,2 @@ +#pragma once + From e1bf194087d11be60b7aea4e45e05c5eac1952b9 Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Wed, 10 Jul 2024 17:18:24 +0200 Subject: [PATCH 02/16] Clean tests. --- CMakeLists.txt | 2 +- tests/Cosserat.Core.Tests/CMakeLists.txt | 6 +- .../constraint/CMakeLists.txt | 23 -- .../constraint/ClassName.h | 12 - .../constraint/Constraint.h | 25 -- ...ratUnilateralInteractionConstraintTest.cpp | 167 ------------ .../constraint/ExampleTest.cpp | 34 --- .../mapping/DiscretCosseratMappingTest.cpp | 247 ------------------ .../mapping/POEMapping_test1.pyscn | 240 ----------------- .../mapping/POEMapping_test2.pyscn | 240 ----------------- 10 files changed, 3 insertions(+), 993 deletions(-) delete mode 100644 tests/Cosserat.Core.Tests/constraint/CMakeLists.txt delete mode 100644 tests/Cosserat.Core.Tests/constraint/ClassName.h delete mode 100644 tests/Cosserat.Core.Tests/constraint/Constraint.h delete mode 100644 tests/Cosserat.Core.Tests/constraint/CosseratUnilateralInteractionConstraintTest.cpp delete mode 100644 tests/Cosserat.Core.Tests/constraint/ExampleTest.cpp delete mode 100644 tests/Cosserat.Core.Tests/mapping/DiscretCosseratMappingTest.cpp delete mode 100644 tests/Cosserat.Core.Tests/mapping/POEMapping_test1.pyscn delete mode 100644 tests/Cosserat.Core.Tests/mapping/POEMapping_test2.pyscn diff --git a/CMakeLists.txt b/CMakeLists.txt index 9a698d4e..877ceda2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -147,7 +147,7 @@ sofa_create_package_with_targets( # If SOFA_BUILD_TESTS exists and is OFF, then these tests will be auto-disabled cmake_dependent_option(COSSERAT_BUILD_TESTS "Compile the tests" ON "SOFA_BUILD_TESTS OR NOT DEFINED SOFA_BUILD_TESTS" OFF) if(COSSERAT_BUILD_TESTS) - add_subdirectory(Tests) + add_subdirectory(tests) endif() # Config files and install rules for pythons scripts diff --git a/tests/Cosserat.Core.Tests/CMakeLists.txt b/tests/Cosserat.Core.Tests/CMakeLists.txt index bb53fe82..db86df0c 100644 --- a/tests/Cosserat.Core.Tests/CMakeLists.txt +++ b/tests/Cosserat.Core.Tests/CMakeLists.txt @@ -5,14 +5,12 @@ find_package(Sofa.Testing REQUIRED) enable_testing() -set(HEADER_FILES - constraint/Constraint.h - ) +set(HEADER_FILES) + set(SOURCE_FILES forcefield/BeamHookeLawForceFieldTest.cpp ) - add_executable(${PROJECT_NAME} ${SOURCE_FILES} ${HEADER_FILES}) target_link_libraries(${PROJECT_NAME} diff --git a/tests/Cosserat.Core.Tests/constraint/CMakeLists.txt b/tests/Cosserat.Core.Tests/constraint/CMakeLists.txt deleted file mode 100644 index 672f79b7..00000000 --- a/tests/Cosserat.Core.Tests/constraint/CMakeLists.txt +++ /dev/null @@ -1,23 +0,0 @@ -cmake_minimum_required(VERSION 3.1) - -#[[project(Cosserat.test VERSION 1.0)]] -set(This ExampleTest) - -set(SOURCE_FILES - ExampleTest.cpp - ) - -add_executable(${This} ${SOURCE_FILES}) - -target_link_libraries(${This} PUBLIC - SofaTest - gtest - gtest_main - Cosserat - Example -) - -add_test( - NAME ${This} - COMMAND ${This} -) diff --git a/tests/Cosserat.Core.Tests/constraint/ClassName.h b/tests/Cosserat.Core.Tests/constraint/ClassName.h deleted file mode 100644 index 996a3693..00000000 --- a/tests/Cosserat.Core.Tests/constraint/ClassName.h +++ /dev/null @@ -1,12 +0,0 @@ -// -// Created by younes on 31/05/2021. -// - -#pragma once - - -class ClassName { - -}; - -#endif //SOFA_CLASSNAME_H diff --git a/tests/Cosserat.Core.Tests/constraint/Constraint.h b/tests/Cosserat.Core.Tests/constraint/Constraint.h deleted file mode 100644 index df188d76..00000000 --- a/tests/Cosserat.Core.Tests/constraint/Constraint.h +++ /dev/null @@ -1,25 +0,0 @@ -// -// Created by younes on 02/06/2021. -// - -#pragma once - -#include -#include -#include - -#include "../../src/constraint/CosseratUnilateralInteractionConstraint.h" - - -using namespace sofa::defaulttype; - -bool testUpdateList(){ - - // - // - - /* @todo do the unitest code here for the updateList function - Check that the outout is a list of int - */ - return true; -} diff --git a/tests/Cosserat.Core.Tests/constraint/CosseratUnilateralInteractionConstraintTest.cpp b/tests/Cosserat.Core.Tests/constraint/CosseratUnilateralInteractionConstraintTest.cpp deleted file mode 100644 index 43d39519..00000000 --- a/tests/Cosserat.Core.Tests/constraint/CosseratUnilateralInteractionConstraintTest.cpp +++ /dev/null @@ -1,167 +0,0 @@ -// -// Created by younes on 02/06/2021. -// - -#include -using testing::Test; - -#include -using sofa::testing::BaseTest ; - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include "../../src/constraint/CosseratUnilateralInteractionConstraint.h" -#include "../../src/constraint/CosseratUnilateralInteractionConstraint.inl" - -#include "Constraint.h" -#include "../Example.h" - -using sofa::simulation::SceneLoaderXML ; - -namespace sofa { - -template -struct CosseratUnilateralInteractionConstraintTest : public NumericTest<> -{ - typedef _DataTypes DataTypes; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename Coord::value_type Real; - typedef sofa::component::constraintset::CosseratUnilateralInteractionConstraint CosseratUnilateralInteractionConstraint; - - int* x; - int GetX() const{ - return *x; - } - - // Sets up the test fixture. - void SetUp() override { - // initialization or some code to run before each test - fprintf(stderr, "Starting up ! \n"); - x = new int(42); - - sofa::simpleapi::importPlugin("Sofa.Component"); - sofa::simpleapi::importPlugin("Cosserat"); - - //create the context for - if(simulation==nullptr) - sofa::simulation::setSimulation(simulation = new sofa::simulation::graph::DAGSimulation()); - } - - // Tears down the test fixture. - void TearDown() override { - // code to run after each test; - // can be used instead of a destructor, - // but exceptions can be handled in this function only - fprintf(stderr, "Starting down ! \n"); - delete x; - if(root) - simulation->unload(root); - } - - bool UpdateListTest() - { - /* @todo do the code here in order to test the updateList function */ - // m_constraint->UpdateList(); - // m_constraint.UpdateList(); - //typename CosseratConstraint::SPtr constraint = sofa::core::objectmodel::New(); - // simulation::Node::SPtr root = simulation->createNewGraph("root"); - // root->setGravity( defaulttype::Vector3(0,0,0) ); - CosseratUnilateralInteractionConstraint * m_constraint = new CosseratUnilateralInteractionConstraint; - - if (m_constraint == NULL) - return false; - else - return m_constraint->UpdateList(); - } - - /** - * - */ - void attributesTests(); - - protected: - ///< Root of the scene graph, created by the constructor an re-used in the tests - simulation::Node::SPtr root; - ///< created by the constructor an re-used in the tests - simulation::Simulation* simulation {nullptr}; -}; - - -template<> -void CosseratUnilateralInteractionConstraintTest::attributesTests(){ - /// I'm using '\n' so that the XML parser correctly report the line number - /// in case of problems. - std::stringstream scene; - scene << " \n" - " \n" - " \n" - " \n" - " \n" ; - - Node::SPtr root = SceneLoaderXML::loadFromMemory ("testscene", - scene.str().c_str(), - scene.str().size()) ; - root->init(sofa::core::execparams::defaultInstance()) ; - CosseratUnilateralInteractionConstraint * constraint = root->getTreeObject() ; - - EXPECT_TRUE( constraint != nullptr ) ; - EXPECT_TRUE( constraint->findData("value") != nullptr ) ; - EXPECT_TRUE( constraint->findData("force_dumping") != nullptr ) ; - EXPECT_TRUE( constraint->findData("valueIndex") != nullptr ) ; - EXPECT_TRUE( constraint->findData("entryPoint") != nullptr ) ; - EXPECT_TRUE( constraint->findData("vectorOfIndices") != nullptr ) ; - - return ; - } - - -/* an easy example */ -//TEST(ConstraintTest, Square){ -// int x = 5; -// int expectedSquare = x*x; -// EXPECT_EQ(expectedSquare, Square(x)); -//} - - -// Define the list of DataTypes to instantiation -using ::testing::Types; -typedef Types DataTypes; // the types to instantiate. - -// Test suite for Vec3Types instantiation -TYPED_TEST_SUITE(CosseratUnilateralInteractionConstraintTest, DataTypes); - -//TYPED_TEST(ConstraintTest, MAC) -//{ -// int y = 16; -// int sum = 100; -// auto val = this->GetX() ; -//// auto val = ConstraintTest->GetX() -// auto oldSum = sum + val * y; //sum + this->GetX() * y; -// int expectedNewSum = sum + val * y; //sum + this->GetX() * y; -// -// EXPECT_EQ(expectedNewSum,MAC(val,y,sum)); -// //EXPECT_EQ(expectedNewSum,oldSum); -//} - -// first test case, test the UpdateList function -TYPED_TEST( CosseratUnilateralInteractionConstraintTest, UpdateListTest ) -{ - EXPECT_TRUE(this->UpdateListTest()); -} - -TYPED_TEST( CosseratUnilateralInteractionConstraintTest , attributesTests) -{ - ASSERT_NO_THROW( this->attributesTests() ); -} - -} //sofa diff --git a/tests/Cosserat.Core.Tests/constraint/ExampleTest.cpp b/tests/Cosserat.Core.Tests/constraint/ExampleTest.cpp deleted file mode 100644 index ea274c05..00000000 --- a/tests/Cosserat.Core.Tests/constraint/ExampleTest.cpp +++ /dev/null @@ -1,34 +0,0 @@ -// -// Created by younes on 31/05/2021. -// - -#include -#include "../Example.h" - -TEST(ExempleTests,MAC) -{ - int x = 42; - int y = 16; - int sum = 100; - int oldSum = sum; - int expectedNewSum = oldSum + x * y; - - EXPECT_EQ( - expectedNewSum, - MAC(x,y,sum) - ); - - EXPECT_EQ( - expectedNewSum, - sum - ); -} - -TEST(ExampleTests, Square){ - int x = 5; - int expectedSquare = x*x; - EXPECT_EQ( - expectedSquare, - Square(x) - ); -} \ No newline at end of file diff --git a/tests/Cosserat.Core.Tests/mapping/DiscretCosseratMappingTest.cpp b/tests/Cosserat.Core.Tests/mapping/DiscretCosseratMappingTest.cpp deleted file mode 100644 index 1b2b1739..00000000 --- a/tests/Cosserat.Core.Tests/mapping/DiscretCosseratMappingTest.cpp +++ /dev/null @@ -1,247 +0,0 @@ -#include -using std::string ; -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "../../src/mapping/DiscreteCosseratMapping.inl" -#include -#include -#include - -#include -#include -#include -#include - -#include -#include - -using sofa::simulation::SceneLoaderXML ; -using sofa::simulation::Node ; -using sofa::simulation::SceneLoaderPY; -using sofa::component::container::MechanicalObject ; - -#include -using sofa::helper::system::SetDirectory; - -namespace sofa { -namespace { // anonymous namespace -using namespace core; -using namespace component; -using defaulttype::Vec; -using defaulttype::Mat; - - -/** Test suite for RigidMapping. -The test cases are defined in the #Test_Cases member group. - */ -template -struct DiscretCosseratMappingTest : public Multi2Mapping_test<_DiscretCosseratMapping> -{ - - typedef _DiscretCosseratMapping DiscretCosseratMapping; - typedef Multi2Mapping_test Inherit; - - typedef typename DiscretCosseratMapping::In1 In1Type; - typedef typename In1Type::VecCoord In1VecCoord; - typedef typename In1Type::VecDeriv In1VecDeriv; - typedef typename In1Type::Coord In1Coord; - typedef typename In1Type::Deriv In1Deriv; - typedef typename DiscretCosseratMapping::In1DataVecCoord In1DataVecCoord; - typedef typename DiscretCosseratMapping::In1DataVecDeriv In1DataVecDeriv; - typedef container::MechanicalObject In1DOFs; - typedef typename In1DOFs::ReadVecCoord ReadIn1VecCoord; - typedef typename In1DOFs::WriteVecCoord WriteIn1VecCoord; - typedef typename In1DOFs::WriteVecDeriv WriteIn1VecDeriv; - typedef typename In1Type::Real Real; - typedef Mat In1RotationMatrix; - - typedef typename DiscretCosseratMapping::In2 In2Type; - typedef typename In2Type::VecCoord In2VecCoord; - typedef typename In2Type::VecDeriv In2VecDeriv; - typedef typename In2Type::Coord In2Coord; - typedef typename In2Type::Deriv In2Deriv; - typedef typename DiscretCosseratMapping::In2DataVecCoord In2DataVecCoord; - typedef typename DiscretCosseratMapping::In2DataVecDeriv In2DataVecDeriv; - typedef container::MechanicalObject In2DOFs; - typedef typename In2DOFs::ReadVecCoord ReadIn2VecCoord; - typedef typename In2DOFs::WriteVecCoord WriteIn2VecCoord; - typedef typename In2DOFs::WriteVecDeriv WriteIn2VecDeriv; - typedef typename In1Type::Real Real2; - typedef Mat In2RotationMatrix; - - typedef typename DiscretCosseratMapping::Out OutType; - typedef typename OutType::VecCoord OutVecCoord; - typedef typename OutType::VecDeriv OutVecDeriv; - typedef typename OutType::Coord OutCoord; - typedef typename OutType::Deriv OutDeriv; - typedef typename DiscretCosseratMapping::OutDataVecCoord OutDataVecCoord; - typedef typename DiscretCosseratMapping::OutDataVecDeriv OutDataVecDeriv; - typedef container::MechanicalObject OutDOFs; - typedef typename OutDOFs::WriteVecCoord WriteOutVecCoord; - typedef typename OutDOFs::WriteVecDeriv WriteOutVecDeriv; - typedef typename OutDOFs::ReadVecCoord ReadOutVecCoord; - typedef typename OutDOFs::ReadVecDeriv ReadOutVecDeriv; - - typedef core::Multi2Mapping Multi2Mapping; - - typedef component::linearsolver::EigenSparseMatrix SparseJMatrixEigen1; - typedef component::linearsolver::EigenSparseMatrix SparseJMatrixEigen2; - typedef linearsolver::EigenSparseMatrix SparseKMatrixEigen1; - typedef linearsolver::EigenSparseMatrix SparseKMatrixEigen2; - - - - DiscretCosseratMapping* poeMapping; ///< the mapping to be tested - vector in1Dofs; ///< mapping input - vector in2Dofs; ///< mapping input - OutDOFs* outDofs; ///< mapping output - - std::string filepath; - - //simulation::SceneLoaderPY loader; - - /// Constructor - DiscretCosseratMappingTest() - :filepath("/home/younes/travail/plugin/plugin.Cosserat/tests/mapping/POEMapping_test1.pyscn") - { - // this->errorFactorDJ = 200; - - // poeMapping = static_cast( this->Multi2Mapping ); - - // // beamLengthMapping::getJs is not yet implemented - // this->flags &= ~Inherit::TEST_getJs; - - // // beamLengthMapping::getK is not yet implemented - // //this->flags &= ~Inherit::TEST_getK; - - // // beamLengthMapping::applyDJT is not yet implemented - // //this->flags &= ~Inherit::TEST_applyDJT; - - } - - /** @name Test_Cases - verify the computation of the beam lengths and the derivatives - */ - ///@{ - /** Two frames are placed + line topology + and the mapping is constructed - */ - - bool runTest1() - { - printf("================================> Inside the POEMapping test scene \n"); - - return true; - // const char *filename = filepath.c_str(); ; - // SetDirectory localDir(filename); - // std::string basename = SetDirectory::GetFileNameWithoutExtension(SetDirectory::GetFileName(filename).c_str()); - - // std::cout << "basename : "<< basename << std::endl; - - - // SceneLoaderPY loader; - // Node::SPtr root = loader.doLoad(filename); - - - // //std::cout<<"******************* Get the mapping "; - // DiscretCosseratMapping* poeMapping; - // this->root->getTreeObject(poeMapping); - // this->mapping = poeMapping; - // this->deltaRange.first= 1; - // this->deltaRange.second= 1000; - - - // MechanicalObject* FromModel1 = nullptr; - // this->root->getTreeObject(FromModel1); - // //this->in1Dofs = FromModel1; - // const Data& dataIn1X = *FromModel1->read(VecCoordId::position()); - // const In1VecCoord& x1in = dataIn1X.getValue(); - - // helper::vector vecIn1; - // vecIn1.push_back(x1in); - - // MechanicalObject* FromModel2 = nullptr; - // this->root->getTreeObject(FromModel2); - // //this->in2Dofs = FromModel2; - // const Data& dataIn2X = *FromModel2->read(VecCoordId::position()); - // const In2VecCoord& x2in = dataIn2X.getValue(); - - // helper::vector vecIn2; - // vecIn2.push_back(x2in); - - - // MechanicalObject* ToModel = nullptr; - // this->root->getTreeObject(ToModel); - // this->outDofs= ToModel; - // const Data& dataOutX = *ToModel->read(VecCoordId::position()); - // const OutVecCoord& xout = dataOutX.getValue(); - - // std::cout<<" x1 in = "< Date: Wed, 10 Jul 2024 17:19:50 +0200 Subject: [PATCH 03/16] Clean --- tests/Cosserat.Binding.Tests/CMakeLists.txt | 36 --- .../constraint/CMakeLists.txt | 23 -- .../constraint/ClassName.h | 12 - .../constraint/Constraint.h | 25 -- ...ratUnilateralInteractionConstraintTest.cpp | 167 ------------ .../constraint/ExampleTest.cpp | 34 --- .../forcefield/BeamHookeLawForceFieldTest.cpp | 201 -------------- .../mapping/DiscretCosseratMappingTest.cpp | 247 ------------------ .../mapping/POEMapping_test1.pyscn | 240 ----------------- .../mapping/POEMapping_test2.pyscn | 240 ----------------- .../Cosserat_Python_Tests.cpp | 2 +- 11 files changed, 1 insertion(+), 1226 deletions(-) delete mode 100644 tests/Cosserat.Binding.Tests/CMakeLists.txt delete mode 100644 tests/Cosserat.Binding.Tests/constraint/CMakeLists.txt delete mode 100644 tests/Cosserat.Binding.Tests/constraint/ClassName.h delete mode 100644 tests/Cosserat.Binding.Tests/constraint/Constraint.h delete mode 100644 tests/Cosserat.Binding.Tests/constraint/CosseratUnilateralInteractionConstraintTest.cpp delete mode 100644 tests/Cosserat.Binding.Tests/constraint/ExampleTest.cpp delete mode 100644 tests/Cosserat.Binding.Tests/forcefield/BeamHookeLawForceFieldTest.cpp delete mode 100644 tests/Cosserat.Binding.Tests/mapping/DiscretCosseratMappingTest.cpp delete mode 100644 tests/Cosserat.Binding.Tests/mapping/POEMapping_test1.pyscn delete mode 100644 tests/Cosserat.Binding.Tests/mapping/POEMapping_test2.pyscn diff --git a/tests/Cosserat.Binding.Tests/CMakeLists.txt b/tests/Cosserat.Binding.Tests/CMakeLists.txt deleted file mode 100644 index b686e957..00000000 --- a/tests/Cosserat.Binding.Tests/CMakeLists.txt +++ /dev/null @@ -1,36 +0,0 @@ -cmake_minimum_required(VERSION 3.12) -project(Cosserat_core_test C CXX) - -find_package(Sofa.Testing REQUIRED) - -enable_testing() - -set(HEADER_FILES - constraint/Constraint.h - ) -set(SOURCE_FILES - constraint/ExampleTest.cpp - forcefield/BeamHookeLawForceFieldTest.cpp - ) - - -add_executable(${PROJECT_NAME} ${SOURCE_FILES} ${HEADER_FILES}) - -target_link_libraries(${PROJECT_NAME} - Sofa.Testing - Cosserat - -) - -target_include_directories(${This} - PUBLIC - "$" -) - - -add_test( - NAME ${This} - COMMAND ${This} -) - -#[[add_subdirectory(constraint)]] diff --git a/tests/Cosserat.Binding.Tests/constraint/CMakeLists.txt b/tests/Cosserat.Binding.Tests/constraint/CMakeLists.txt deleted file mode 100644 index 672f79b7..00000000 --- a/tests/Cosserat.Binding.Tests/constraint/CMakeLists.txt +++ /dev/null @@ -1,23 +0,0 @@ -cmake_minimum_required(VERSION 3.1) - -#[[project(Cosserat.test VERSION 1.0)]] -set(This ExampleTest) - -set(SOURCE_FILES - ExampleTest.cpp - ) - -add_executable(${This} ${SOURCE_FILES}) - -target_link_libraries(${This} PUBLIC - SofaTest - gtest - gtest_main - Cosserat - Example -) - -add_test( - NAME ${This} - COMMAND ${This} -) diff --git a/tests/Cosserat.Binding.Tests/constraint/ClassName.h b/tests/Cosserat.Binding.Tests/constraint/ClassName.h deleted file mode 100644 index 996a3693..00000000 --- a/tests/Cosserat.Binding.Tests/constraint/ClassName.h +++ /dev/null @@ -1,12 +0,0 @@ -// -// Created by younes on 31/05/2021. -// - -#pragma once - - -class ClassName { - -}; - -#endif //SOFA_CLASSNAME_H diff --git a/tests/Cosserat.Binding.Tests/constraint/Constraint.h b/tests/Cosserat.Binding.Tests/constraint/Constraint.h deleted file mode 100644 index df188d76..00000000 --- a/tests/Cosserat.Binding.Tests/constraint/Constraint.h +++ /dev/null @@ -1,25 +0,0 @@ -// -// Created by younes on 02/06/2021. -// - -#pragma once - -#include -#include -#include - -#include "../../src/constraint/CosseratUnilateralInteractionConstraint.h" - - -using namespace sofa::defaulttype; - -bool testUpdateList(){ - - // - // - - /* @todo do the unitest code here for the updateList function - Check that the outout is a list of int - */ - return true; -} diff --git a/tests/Cosserat.Binding.Tests/constraint/CosseratUnilateralInteractionConstraintTest.cpp b/tests/Cosserat.Binding.Tests/constraint/CosseratUnilateralInteractionConstraintTest.cpp deleted file mode 100644 index 43d39519..00000000 --- a/tests/Cosserat.Binding.Tests/constraint/CosseratUnilateralInteractionConstraintTest.cpp +++ /dev/null @@ -1,167 +0,0 @@ -// -// Created by younes on 02/06/2021. -// - -#include -using testing::Test; - -#include -using sofa::testing::BaseTest ; - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include "../../src/constraint/CosseratUnilateralInteractionConstraint.h" -#include "../../src/constraint/CosseratUnilateralInteractionConstraint.inl" - -#include "Constraint.h" -#include "../Example.h" - -using sofa::simulation::SceneLoaderXML ; - -namespace sofa { - -template -struct CosseratUnilateralInteractionConstraintTest : public NumericTest<> -{ - typedef _DataTypes DataTypes; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename Coord::value_type Real; - typedef sofa::component::constraintset::CosseratUnilateralInteractionConstraint CosseratUnilateralInteractionConstraint; - - int* x; - int GetX() const{ - return *x; - } - - // Sets up the test fixture. - void SetUp() override { - // initialization or some code to run before each test - fprintf(stderr, "Starting up ! \n"); - x = new int(42); - - sofa::simpleapi::importPlugin("Sofa.Component"); - sofa::simpleapi::importPlugin("Cosserat"); - - //create the context for - if(simulation==nullptr) - sofa::simulation::setSimulation(simulation = new sofa::simulation::graph::DAGSimulation()); - } - - // Tears down the test fixture. - void TearDown() override { - // code to run after each test; - // can be used instead of a destructor, - // but exceptions can be handled in this function only - fprintf(stderr, "Starting down ! \n"); - delete x; - if(root) - simulation->unload(root); - } - - bool UpdateListTest() - { - /* @todo do the code here in order to test the updateList function */ - // m_constraint->UpdateList(); - // m_constraint.UpdateList(); - //typename CosseratConstraint::SPtr constraint = sofa::core::objectmodel::New(); - // simulation::Node::SPtr root = simulation->createNewGraph("root"); - // root->setGravity( defaulttype::Vector3(0,0,0) ); - CosseratUnilateralInteractionConstraint * m_constraint = new CosseratUnilateralInteractionConstraint; - - if (m_constraint == NULL) - return false; - else - return m_constraint->UpdateList(); - } - - /** - * - */ - void attributesTests(); - - protected: - ///< Root of the scene graph, created by the constructor an re-used in the tests - simulation::Node::SPtr root; - ///< created by the constructor an re-used in the tests - simulation::Simulation* simulation {nullptr}; -}; - - -template<> -void CosseratUnilateralInteractionConstraintTest::attributesTests(){ - /// I'm using '\n' so that the XML parser correctly report the line number - /// in case of problems. - std::stringstream scene; - scene << " \n" - " \n" - " \n" - " \n" - " \n" ; - - Node::SPtr root = SceneLoaderXML::loadFromMemory ("testscene", - scene.str().c_str(), - scene.str().size()) ; - root->init(sofa::core::execparams::defaultInstance()) ; - CosseratUnilateralInteractionConstraint * constraint = root->getTreeObject() ; - - EXPECT_TRUE( constraint != nullptr ) ; - EXPECT_TRUE( constraint->findData("value") != nullptr ) ; - EXPECT_TRUE( constraint->findData("force_dumping") != nullptr ) ; - EXPECT_TRUE( constraint->findData("valueIndex") != nullptr ) ; - EXPECT_TRUE( constraint->findData("entryPoint") != nullptr ) ; - EXPECT_TRUE( constraint->findData("vectorOfIndices") != nullptr ) ; - - return ; - } - - -/* an easy example */ -//TEST(ConstraintTest, Square){ -// int x = 5; -// int expectedSquare = x*x; -// EXPECT_EQ(expectedSquare, Square(x)); -//} - - -// Define the list of DataTypes to instantiation -using ::testing::Types; -typedef Types DataTypes; // the types to instantiate. - -// Test suite for Vec3Types instantiation -TYPED_TEST_SUITE(CosseratUnilateralInteractionConstraintTest, DataTypes); - -//TYPED_TEST(ConstraintTest, MAC) -//{ -// int y = 16; -// int sum = 100; -// auto val = this->GetX() ; -//// auto val = ConstraintTest->GetX() -// auto oldSum = sum + val * y; //sum + this->GetX() * y; -// int expectedNewSum = sum + val * y; //sum + this->GetX() * y; -// -// EXPECT_EQ(expectedNewSum,MAC(val,y,sum)); -// //EXPECT_EQ(expectedNewSum,oldSum); -//} - -// first test case, test the UpdateList function -TYPED_TEST( CosseratUnilateralInteractionConstraintTest, UpdateListTest ) -{ - EXPECT_TRUE(this->UpdateListTest()); -} - -TYPED_TEST( CosseratUnilateralInteractionConstraintTest , attributesTests) -{ - ASSERT_NO_THROW( this->attributesTests() ); -} - -} //sofa diff --git a/tests/Cosserat.Binding.Tests/constraint/ExampleTest.cpp b/tests/Cosserat.Binding.Tests/constraint/ExampleTest.cpp deleted file mode 100644 index ea274c05..00000000 --- a/tests/Cosserat.Binding.Tests/constraint/ExampleTest.cpp +++ /dev/null @@ -1,34 +0,0 @@ -// -// Created by younes on 31/05/2021. -// - -#include -#include "../Example.h" - -TEST(ExempleTests,MAC) -{ - int x = 42; - int y = 16; - int sum = 100; - int oldSum = sum; - int expectedNewSum = oldSum + x * y; - - EXPECT_EQ( - expectedNewSum, - MAC(x,y,sum) - ); - - EXPECT_EQ( - expectedNewSum, - sum - ); -} - -TEST(ExampleTests, Square){ - int x = 5; - int expectedSquare = x*x; - EXPECT_EQ( - expectedSquare, - Square(x) - ); -} \ No newline at end of file diff --git a/tests/Cosserat.Binding.Tests/forcefield/BeamHookeLawForceFieldTest.cpp b/tests/Cosserat.Binding.Tests/forcefield/BeamHookeLawForceFieldTest.cpp deleted file mode 100644 index 02a84f87..00000000 --- a/tests/Cosserat.Binding.Tests/forcefield/BeamHookeLawForceFieldTest.cpp +++ /dev/null @@ -1,201 +0,0 @@ -// -// Created by younes on 07/06/2021. -// - -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include - -using sofa::testing::BaseTest ; -using testing::Test; -using sofa::simulation::SceneLoaderXML ; -using namespace sofa::simpleapi; - -namespace sofa { - -template -struct BeamHookeLawForceFieldTest : public testing::NumericTest<> { - typedef _DataTypes DataTypes; - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename Coord::value_type Real; - - - - typedef sofa::component::forcefield::BeamHookeLawForceField TheBeamHookeLawForceField; - - // Sets up the test fixture. - void SetUp() override { - // initialization or some code to run before each test - fprintf(stderr, "Starting up ! \n"); - sofa::simpleapi::importPlugin("Sofa.Component"); - sofa::simpleapi::importPlugin("Cosserat"); - } - - // Tears down the test fixture. - void TearDown() override { - // code to run after each test; - // can be used instead of a destructor, - // but exceptions can be handled in this function only - if(root) { - sofa::simulation::node::unload(root); - } - fprintf(stderr, "Starting down ! \n"); - } - - void reinit() - { - /* @todo do the code here in order to test the updateList function */ - // m_constraint->UpdateList(); - // m_constraint.UpdateList(); - //typename CosseratConstraint::SPtr constraint = sofa::core::objectmodel::New(); - // simulation::Node::SPtr root = simulation->createNewGraph("root"); - // root->setGravity( defaulttype::Vector3(0,0,0) ); - auto m_forcefield = new TheBeamHookeLawForceField; - - if (m_forcefield == NULL) - return ; - else - m_forcefield->reinit(); - } - - /** - * - */ - void basicAttributesTest(); - void triangle(); - -// void addForceTest(const MechanicalParams* mparams, -// DataVecDeriv& f , -// const DataVecCoord& x , -// const DataVecDeriv& v) override; -// -// void addDForceTest(const MechanicalParams* mparams, -// DataVecDeriv& df , -// const DataVecDeriv& -// dx ) override; -// -// -// void addKToMatrixTest(const MechanicalParams* mparams, -// const MultiMatrixAccessor* matrix) override; -// -// double getPotentialEnergyTest(const MechanicalParams* mparams, -// const DataVecCoord& x) const override; - -protected: - ///< Root of the scene graph, created by the constructor an re-used in the tests - simulation::Node::SPtr root; - - void testFonctionnel(); -}; - - -template<> -void BeamHookeLawForceFieldTest::testFonctionnel() { - EXPECT_MSG_NOEMIT(Error, Warning) ; - createObject(root, "MechanicalObject", {{"position", "-1 0 1 1 0 1 -1 0 -1 1 0 -1 0 0 1 0 0 -1 -1 0 0 1 0 0 0 0 0"}}); - createObject(root, "TriangleSetTopologyContainer", {{"triangles", "7 5 8 8 2 6 4 6 0 1 8 4 7 3 5 8 5 2 4 8 6 1 7 8"}}); - - auto traction = dynamic_cast( - createObject(root, "BeamHookeLawForceField", {{"name", "beamHookeLaw"}, - {"crossSectionShape", "circular"}, - {"lengthY", "35e-5"}, - {"lengthZ", "1374e-5"}, - {"radius", "0.25"}, - {"varianteSections", "true"}}).get() - ); - - EXPECT_NE(traction, nullptr); - EXPECT_NE(root.get(), nullptr) ; - root->init(sofa::core::execparams::defaultInstance()) ; - - auto total_load = dynamic_cast *>(traction->findData("lengthY")); - for (unsigned int step = 1; step <= 5; ++step) { - sofa::simulation::node::animate(root.get(), 1); - EXPECT_DOUBLE_EQ(total_load->getValue(), 4*step) << "Total load at time step " << step << " is incorrect."; - } -} - - - -template<> -void BeamHookeLawForceFieldTest::basicAttributesTest(){ - EXPECT_MSG_NOEMIT(Error) ; - - std::stringstream scene ; - scene << "" - " \n" - " \n" - " \n" - " \n" - " \n" ; - - Node::SPtr root = SceneLoaderXML::loadFromMemory ("testscene", - scene.str().c_str()) ; - - EXPECT_NE(root.get(), nullptr) ; - root->init(sofa::core::execparams::defaultInstance()) ; - - TheBeamHookeLawForceField* forcefield ; - root->getTreeObject(forcefield) ; - - EXPECT_NE(nullptr, forcefield) ; - - /// List of the supported attributes the user expect to find - /// This list needs to be updated if you add an attribute. - sofa::type::vector attrnames = { - "crossSectionShape","youngModulus","poissonRatio","length", "radius", - "innerRadius", "lengthY", "lengthZ", "varianteSections", "youngModululsList", "poissonRatioList" - }; - - for(auto& attrname : attrnames) - EXPECT_NE( nullptr, forcefield->findData(attrname) ) - << "Missing attribute with name '" - << attrname << "'." ; - - for(int i=0; i<10; i++){ - sofa::simulation::node::animate(root.get(),(double)0.01); - } -} - - -/*** - * The test section - */ - -// Define the list of DataTypes to instanciate -using ::testing::Types; -typedef Types DataTypes; // the types to instantiate. - -// Test suite for all the instanciations -TYPED_TEST_SUITE(BeamHookeLawForceFieldTest, DataTypes);// first test case -TYPED_TEST( BeamHookeLawForceFieldTest , basicAttributesTest ) -{ - ASSERT_NO_THROW (this->basicAttributesTest()); -} - -TYPED_TEST(BeamHookeLawForceFieldTest, testFonctionnel) { - ASSERT_NO_THROW (this->testFonctionnel()); -} - - - - - -} diff --git a/tests/Cosserat.Binding.Tests/mapping/DiscretCosseratMappingTest.cpp b/tests/Cosserat.Binding.Tests/mapping/DiscretCosseratMappingTest.cpp deleted file mode 100644 index 1b2b1739..00000000 --- a/tests/Cosserat.Binding.Tests/mapping/DiscretCosseratMappingTest.cpp +++ /dev/null @@ -1,247 +0,0 @@ -#include -using std::string ; -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "../../src/mapping/DiscreteCosseratMapping.inl" -#include -#include -#include - -#include -#include -#include -#include - -#include -#include - -using sofa::simulation::SceneLoaderXML ; -using sofa::simulation::Node ; -using sofa::simulation::SceneLoaderPY; -using sofa::component::container::MechanicalObject ; - -#include -using sofa::helper::system::SetDirectory; - -namespace sofa { -namespace { // anonymous namespace -using namespace core; -using namespace component; -using defaulttype::Vec; -using defaulttype::Mat; - - -/** Test suite for RigidMapping. -The test cases are defined in the #Test_Cases member group. - */ -template -struct DiscretCosseratMappingTest : public Multi2Mapping_test<_DiscretCosseratMapping> -{ - - typedef _DiscretCosseratMapping DiscretCosseratMapping; - typedef Multi2Mapping_test Inherit; - - typedef typename DiscretCosseratMapping::In1 In1Type; - typedef typename In1Type::VecCoord In1VecCoord; - typedef typename In1Type::VecDeriv In1VecDeriv; - typedef typename In1Type::Coord In1Coord; - typedef typename In1Type::Deriv In1Deriv; - typedef typename DiscretCosseratMapping::In1DataVecCoord In1DataVecCoord; - typedef typename DiscretCosseratMapping::In1DataVecDeriv In1DataVecDeriv; - typedef container::MechanicalObject In1DOFs; - typedef typename In1DOFs::ReadVecCoord ReadIn1VecCoord; - typedef typename In1DOFs::WriteVecCoord WriteIn1VecCoord; - typedef typename In1DOFs::WriteVecDeriv WriteIn1VecDeriv; - typedef typename In1Type::Real Real; - typedef Mat In1RotationMatrix; - - typedef typename DiscretCosseratMapping::In2 In2Type; - typedef typename In2Type::VecCoord In2VecCoord; - typedef typename In2Type::VecDeriv In2VecDeriv; - typedef typename In2Type::Coord In2Coord; - typedef typename In2Type::Deriv In2Deriv; - typedef typename DiscretCosseratMapping::In2DataVecCoord In2DataVecCoord; - typedef typename DiscretCosseratMapping::In2DataVecDeriv In2DataVecDeriv; - typedef container::MechanicalObject In2DOFs; - typedef typename In2DOFs::ReadVecCoord ReadIn2VecCoord; - typedef typename In2DOFs::WriteVecCoord WriteIn2VecCoord; - typedef typename In2DOFs::WriteVecDeriv WriteIn2VecDeriv; - typedef typename In1Type::Real Real2; - typedef Mat In2RotationMatrix; - - typedef typename DiscretCosseratMapping::Out OutType; - typedef typename OutType::VecCoord OutVecCoord; - typedef typename OutType::VecDeriv OutVecDeriv; - typedef typename OutType::Coord OutCoord; - typedef typename OutType::Deriv OutDeriv; - typedef typename DiscretCosseratMapping::OutDataVecCoord OutDataVecCoord; - typedef typename DiscretCosseratMapping::OutDataVecDeriv OutDataVecDeriv; - typedef container::MechanicalObject OutDOFs; - typedef typename OutDOFs::WriteVecCoord WriteOutVecCoord; - typedef typename OutDOFs::WriteVecDeriv WriteOutVecDeriv; - typedef typename OutDOFs::ReadVecCoord ReadOutVecCoord; - typedef typename OutDOFs::ReadVecDeriv ReadOutVecDeriv; - - typedef core::Multi2Mapping Multi2Mapping; - - typedef component::linearsolver::EigenSparseMatrix SparseJMatrixEigen1; - typedef component::linearsolver::EigenSparseMatrix SparseJMatrixEigen2; - typedef linearsolver::EigenSparseMatrix SparseKMatrixEigen1; - typedef linearsolver::EigenSparseMatrix SparseKMatrixEigen2; - - - - DiscretCosseratMapping* poeMapping; ///< the mapping to be tested - vector in1Dofs; ///< mapping input - vector in2Dofs; ///< mapping input - OutDOFs* outDofs; ///< mapping output - - std::string filepath; - - //simulation::SceneLoaderPY loader; - - /// Constructor - DiscretCosseratMappingTest() - :filepath("/home/younes/travail/plugin/plugin.Cosserat/tests/mapping/POEMapping_test1.pyscn") - { - // this->errorFactorDJ = 200; - - // poeMapping = static_cast( this->Multi2Mapping ); - - // // beamLengthMapping::getJs is not yet implemented - // this->flags &= ~Inherit::TEST_getJs; - - // // beamLengthMapping::getK is not yet implemented - // //this->flags &= ~Inherit::TEST_getK; - - // // beamLengthMapping::applyDJT is not yet implemented - // //this->flags &= ~Inherit::TEST_applyDJT; - - } - - /** @name Test_Cases - verify the computation of the beam lengths and the derivatives - */ - ///@{ - /** Two frames are placed + line topology + and the mapping is constructed - */ - - bool runTest1() - { - printf("================================> Inside the POEMapping test scene \n"); - - return true; - // const char *filename = filepath.c_str(); ; - // SetDirectory localDir(filename); - // std::string basename = SetDirectory::GetFileNameWithoutExtension(SetDirectory::GetFileName(filename).c_str()); - - // std::cout << "basename : "<< basename << std::endl; - - - // SceneLoaderPY loader; - // Node::SPtr root = loader.doLoad(filename); - - - // //std::cout<<"******************* Get the mapping "; - // DiscretCosseratMapping* poeMapping; - // this->root->getTreeObject(poeMapping); - // this->mapping = poeMapping; - // this->deltaRange.first= 1; - // this->deltaRange.second= 1000; - - - // MechanicalObject* FromModel1 = nullptr; - // this->root->getTreeObject(FromModel1); - // //this->in1Dofs = FromModel1; - // const Data& dataIn1X = *FromModel1->read(VecCoordId::position()); - // const In1VecCoord& x1in = dataIn1X.getValue(); - - // helper::vector vecIn1; - // vecIn1.push_back(x1in); - - // MechanicalObject* FromModel2 = nullptr; - // this->root->getTreeObject(FromModel2); - // //this->in2Dofs = FromModel2; - // const Data& dataIn2X = *FromModel2->read(VecCoordId::position()); - // const In2VecCoord& x2in = dataIn2X.getValue(); - - // helper::vector vecIn2; - // vecIn2.push_back(x2in); - - - // MechanicalObject* ToModel = nullptr; - // this->root->getTreeObject(ToModel); - // this->outDofs= ToModel; - // const Data& dataOutX = *ToModel->read(VecCoordId::position()); - // const OutVecCoord& xout = dataOutX.getValue(); - - // std::cout<<" x1 in = "< Date: Fri, 12 Jul 2024 17:06:25 +0200 Subject: [PATCH 04/16] Add empty BaseCosseratMapping.py --- .../Components/BaseCosseratMapping.py | 1 - tests/Cosserat.Scene.Tests/Cosserat_Python_Tests.cpp | 8 ++++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/tests/Cosserat.Scene.Tests/Components/BaseCosseratMapping.py b/tests/Cosserat.Scene.Tests/Components/BaseCosseratMapping.py index 7294d393..898d069f 100644 --- a/tests/Cosserat.Scene.Tests/Components/BaseCosseratMapping.py +++ b/tests/Cosserat.Scene.Tests/Components/BaseCosseratMapping.py @@ -1,7 +1,6 @@ # coding: utf8 import Sofa -import Sofa.Testing import Sofa.Simulation import SofaRuntime import unittest diff --git a/tests/Cosserat.Scene.Tests/Cosserat_Python_Tests.cpp b/tests/Cosserat.Scene.Tests/Cosserat_Python_Tests.cpp index f0de4820..4be8af96 100644 --- a/tests/Cosserat.Scene.Tests/Cosserat_Python_Tests.cpp +++ b/tests/Cosserat.Scene.Tests/Cosserat_Python_Tests.cpp @@ -19,6 +19,7 @@ ******************************************************************************/ #include #include +#include #include #include "Cosserat_Python_Tests.h" @@ -51,8 +52,11 @@ static struct Cosserat_Scene_tests : public PythonTestExtractor Cosserat_Scene_tests() { std::string executable_directory = sofa::helper::Utils::getExecutableDirectory(); - addTestDirectory(executable_directory+"/Cosserat.Scene.Tests/Components", "Cosserat_Scene_"); - addTestDirectory("./Cosserat.Scene.Tests/Components", "Cosserat_Scene_"); + if(sofa::helper::system::FileSystem::exists("./Cosserat.Scene.Tests/Components")){ + addTestDirectory("./Cosserat.Scene.Tests/Components", "Cosserat_Scene_"); + }else{ + addTestDirectory(executable_directory+"/Cosserat.Scene.Tests.d/Components", "Cosserat_Scene_"); + } } } python_tests; From a79ee6f033e7b947635cb6721c2b184f400306a6 Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Sat, 13 Jul 2024 00:52:15 +0200 Subject: [PATCH 05/16] Refactor PointManager It looks like it is a controller... (but in that cas why exposing it to python ?) Further refactoring should be added. --- .../Binding/Binding_PointsManager.cpp | 2 +- src/Cosserat/engine/PointsManager.cpp | 21 +- src/Cosserat/engine/PointsManager.h | 83 +++---- src/Cosserat/engine/PointsManager.inl | 214 +++++++++--------- 4 files changed, 152 insertions(+), 168 deletions(-) diff --git a/src/Cosserat/Binding/Binding_PointsManager.cpp b/src/Cosserat/Binding/Binding_PointsManager.cpp index 8040fd08..af470351 100644 --- a/src/Cosserat/Binding/Binding_PointsManager.cpp +++ b/src/Cosserat/Binding/Binding_PointsManager.cpp @@ -24,7 +24,7 @@ #include #include -typedef sofa::core::behavior::PointsManager PointsManager; +typedef cosserat::controller::PointsManager PointsManager; namespace py { using namespace pybind11; diff --git a/src/Cosserat/engine/PointsManager.cpp b/src/Cosserat/engine/PointsManager.cpp index 89e27686..cea8135f 100755 --- a/src/Cosserat/engine/PointsManager.cpp +++ b/src/Cosserat/engine/PointsManager.cpp @@ -1,19 +1,10 @@ -#include "PointsManager.inl" +#include #include -#include -#include -#include /* assert */ -namespace sofa::core::behavior +namespace cosserat::controller { - - using namespace sofa::defaulttype; - - SOFA_DECL_CLASS(PointsManager) - - int PointsManagerClass = core::RegisterObject("add and remove " - "points from the state will taking into account the changes inside the modifier and the container") - .add(); - -} // namespace sofa +int PointsManagerClass = sofa::core::RegisterObject( + R"doc(Add or remove controls point in a beam structure)doc") + .add(); +} // namespace cosserat::controller diff --git a/src/Cosserat/engine/PointsManager.h b/src/Cosserat/engine/PointsManager.h index 6d9f7353..d197899f 100755 --- a/src/Cosserat/engine/PointsManager.h +++ b/src/Cosserat/engine/PointsManager.h @@ -1,64 +1,55 @@ #pragma once #include -#include -#include -#include #include -#include +#include #include -#include -#include -#include -#include -#include -// #include -#include -typedef sofa::component::topology::container::dynamic::PointSetTopologyModifier PointSetTopologyModifier; +#include -using sofa::core::objectmodel::KeypressedEvent; -namespace sofa::core::behavior +namespace cosserat::controller { - class SOFA_COSSERAT_API PointsManager : public sofa::core::objectmodel::BaseObject - { +namespace { +using sofa::component::topology::container::dynamic::PointSetTopologyModifier; +using sofa::core::topology::TopologyContainer; +} - public: - SOFA_CLASS(PointsManager, sofa::core::objectmodel::BaseObject); +// TODO(dmarchal: 2024-07-12) This class looks like a Controller (not a BaseObject) +class SOFA_COSSERAT_API PointsManager : public sofa::core::behavior::BaseController +{ +public: + SOFA_CLASS(PointsManager, sofa::core::behavior::BaseController); - typedef sofa::defaulttype::Vec3dTypes DataTypes; - typedef DataTypes::VecCoord VecCoord; - typedef DataTypes::Coord Coord; - typedef DataTypes::Real Real; + PointsManager(); + ~PointsManager(); - public: - PointsManager(); - typedef type::Vec3 Vec3; + sofa::Data d_beamTip; + sofa::Data d_radius; + sofa::Data d_color; + sofa::Data d_beamPath; - Data d_beamTip; - Data d_radius; - Data d_color; - Data d_beamPath; + void init() override; + void handleEvent(sofa::core::objectmodel::Event *event) override; - PointSetTopologyModifier *m_modifier; - core::behavior::MechanicalState *m_beam; + void addNewPointToState(); + void removeLastPointfromState(); - void init() override; - void handleEvent(sofa::core::objectmodel::Event *event) override; - // void draw(const core::visual::VisualParams *vparams); +private: + using MState = sofa::core::behavior::MechanicalState; - void addNewPointToState(); - void removeLastPointfromState(); + PointSetTopologyModifier *m_modifier; + MState *m_beam; - topology::TopologyContainer *getTopology() - { - return dynamic_cast(getContext()->getTopology()); - } - sofa::core::behavior::MechanicalState *getMstate() - { - return dynamic_cast *>(getContext()->getMechanicalState()); - } - }; + auto getTopology() -> TopologyContainer* + { + return dynamic_cast(getContext()->getTopology()); + } + + auto getMstate() -> MState* + { + return dynamic_cast(getContext()->getMechanicalState()); + } +}; -} // namespace sofa +} // namespace cosserat::controller diff --git a/src/Cosserat/engine/PointsManager.inl b/src/Cosserat/engine/PointsManager.inl index be167a97..5792f28e 100755 --- a/src/Cosserat/engine/PointsManager.inl +++ b/src/Cosserat/engine/PointsManager.inl @@ -1,133 +1,135 @@ #pragma once -#include "PointsManager.h" +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include #include #include #include -namespace sofa::core::behavior +namespace cosserat::controller +{ +using sofa::core::objectmodel::KeypressedEvent; + +PointsManager::PointsManager() + : d_beamTip(initData(&d_beamTip, "beamTip", "The beam tip")), + d_radius(initData(&d_radius, double(1), "radius", "sphere radius")), + d_color(initData(&d_color, sofa::type::Vec4f(1, 0, 0, 1), "color", "Default color is (1,0,0,1)")), + d_beamPath(initData(&d_beamPath, "beamPath", "path to beam state")) { + this->f_listening.setValue(true); +} + +PointsManager::~PointsManager(){} + +void PointsManager::init() +{ + Inherit1::init(); + + if (getTopology() == NULL) + msg_error() << "Error cannot find the topology"; - PointsManager::PointsManager() - : d_beamTip(initData(&d_beamTip, "beamTip", "The beam tip")), - d_radius(initData(&d_radius, double(1), "radius", "sphere radius")), - d_color(initData(&d_color, type::Vec4f(1, 0, 0, 1), "color", "Default color is (1,0,0,1)")), - d_beamPath(initData(&d_beamPath, "beamPath", "path to beam state")) + if (getMstate() == NULL) + msg_error() << "Error cannot find the mechanical state"; + + this->getContext()->get(m_beam, d_beamPath.getValue()); + if (m_beam == nullptr) + msg_error() << "Cannot find the beam collision state : " << d_beamPath.getValue(); + + this->getContext()->get(m_modifier); + if (m_modifier == NULL) { - this->f_listening.setValue(true); + msg_error() << " Error cannot find the EdgeSetTopologyModifier"; + return; } +} - void PointsManager::init() - { - Inherit1::init(); +void PointsManager::addNewPointToState() +{ + auto x = sofa::helper::getWriteAccessor(*getMstate()->write(sofa::core::VecCoordId::position())); + auto xRest = sofa::helper::getWriteAccessor(*getMstate()->write(sofa::core::VecCoordId::restPosition())); + auto xfree = sofa::helper::getWriteAccessor(*getMstate()->write(sofa::core::VecCoordId::freePosition())); + auto xforce = sofa::helper::getWriteAccessor(*getMstate()->write(sofa::core::VecDerivId::force())); + const auto &beam = m_beam->readPositions(); - if (getTopology() == NULL) - msg_error() << "Error cannot find the topology"; + unsigned nbPoints = this->getTopology()->getNbPoints(); + // do not take the last point because there is a bug + // TODO(dmarchal 2024-07-12) fix the bug you are refering to in the previous line - if (getMstate() == NULL) - msg_error() << "Error cannot find the topology"; + size_t beamSz = beam.size(); - this->getContext()->get(m_beam, d_beamPath.getValue()); - if (m_beam == nullptr) - msg_error() << "Cannot find the beam collision state : " << d_beamPath.getValue(); + m_modifier->addPoints(1, true); + sofa::type::Vec3 pos = beam[beamSz - 1]; - this->getContext()->get(m_modifier); - if (m_modifier == NULL) - { - msg_error() << " Error cannot find the EdgeSetTopologyModifier"; - return; - } - } + x.resize(nbPoints + 1); + xRest.resize(nbPoints + 1); + xfree.resize(nbPoints + 1); + xforce.resize(nbPoints + 1); - void PointsManager::addNewPointToState() + x[nbPoints] = pos; + xRest[nbPoints] = pos; + xfree[nbPoints] = pos; + xforce[nbPoints] = sofa::type::Vec3(0, 0, 0); + + m_modifier->notifyEndingEvent(); +} + +void PointsManager::removeLastPointfromState() +{ + // do not take the last point because there is a bug + // TODO(dmarchal 2024-07-12) fix the bug you are refering to in the previous line + + unsigned nbPoints = getTopology()->getNbPoints(); + + if (nbPoints == 0) { - helper::WriteAccessor> x = *this->getMstate()->write(core::VecCoordId::position()); - helper::WriteAccessor> xRest = *this->getMstate()->write(core::VecCoordId::restPosition()); - helper::WriteAccessor> xfree = *this->getMstate()->write(core::VecCoordId::freePosition()); - helper::WriteAccessor> xforce = *this->getMstate()->write(core::VecDerivId::force()); - const helper::ReadAccessor> &beam = m_beam->readPositions(); - unsigned nbPoints = this->getTopology()->getNbPoints(); // do not take the last point because there is a bug - - size_t beamSz = beam.size(); - m_modifier->addPoints(1, true); - - Vec3 pos = beam[beamSz - 1]; -// std::cout << "beam tip is =-----> " << pos << std::endl; -// std::cout << "nbPoints is equal :" << nbPoints << std::endl; -// std::cout << "x.size is equal :" << x.size() << std::endl; - - x.resize(nbPoints + 1); - xRest.resize(nbPoints + 1); - xfree.resize(nbPoints + 1); - xforce.resize(nbPoints + 1); - - x[nbPoints] = pos; - xRest[nbPoints] = pos; - xfree[nbPoints] = pos; - xforce[nbPoints] = Vec3(0, 0, 0); - - m_modifier->notifyEndingEvent(); -// std::cout << "End addNewPointToState " << std::endl; -// std::cout << "End notifyEndingEvent " << std::endl; + msg_info() << "No more points"; + return; } - void PointsManager::removeLastPointfromState() - { - helper::WriteAccessor> x = *this->getMstate()->write(core::VecCoordId::position()); - helper::WriteAccessor> xfree = *this->getMstate()->write(core::VecCoordId::freePosition()); - unsigned nbPoints = this->getTopology()->getNbPoints(); // do not take the last point because there is a bug - sofa::type::vector Indices; + auto x = sofa::helper::getWriteAccessor(*getMstate()->write(sofa::core::VecCoordId::position())); + auto xfree = sofa::helper::getWriteAccessor(*getMstate()->write(sofa::core::VecCoordId::freePosition())); - if (nbPoints > 0) - { - Indices.push_back(nbPoints - 1); - m_modifier->removePoints(Indices, true); - x.resize(nbPoints - 1); - msg_info() << "the size is equal :" << nbPoints; - xfree.resize(nbPoints - 1); - } - else - { - msg_error() << "Error cannot remove the last point because there is no point in the state"; - } - m_modifier->notifyEndingEvent(); - } + sofa::type::vector indices = {nbPoints - 1}; + m_modifier->removePoints(indices, true); + + x.resize(nbPoints - 1); + msg_info() << "the size is equal :" << nbPoints; + xfree.resize(nbPoints - 1); + m_modifier->notifyEndingEvent(); +} - void PointsManager::handleEvent(sofa::core::objectmodel::Event *event) +void PointsManager::handleEvent(sofa::core::objectmodel::Event *event) +{ + if (KeypressedEvent::checkEventType(event)) { - if (KeypressedEvent::checkEventType(event)) + KeypressedEvent *ev = static_cast(event); + switch (ev->getKey()) { - KeypressedEvent *ev = static_cast(event); - switch (ev->getKey()) - { - case 'S': - case 's': - msg_info() << "A point is created ." ; - addNewPointToState(); - break; - case 'L': - case 'l': - msg_info() <<("Remove point from state \n"); - removeLastPointfromState(); - break; - } + case 'S': + case 's': + msg_info() << "A point is created ." ; + addNewPointToState(); + break; + case 'L': + case 'l': + msg_info() <<("Remove point from state \n"); + removeLastPointfromState(); + break; } } +} - // void PointsManager::draw(const core::visual::VisualParams *vparams) - // { - - // helper::ReadAccessor> x = *this->getMstate()->read(core::VecCoordId::position()); - // if (!x.size()) - // return; // if no points return - // glDisable(GL_LIGHTING); - // for (unsigned int j = 0; j < x.size(); j++) - // { - // glColor3f(1.0, 1.0, 0.0); - // vparams->drawTool()->drawSphere(x[j], d_radius.getValue() * 0.001); - // } - // glEnable(GL_LIGHTING); - // } - -} // Sofa +} // namespace cosserat::controller From 850eacbff106e7796c4023e13b622b3d80d873a1 Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Sat, 13 Jul 2024 09:40:20 +0200 Subject: [PATCH 06/16] Cleaning --- src/Cosserat/engine/PointsManager.h | 4 +- .../forcefield/BeamHookeLawForceFieldRigid.h | 2 + src/Cosserat/mapping/BaseCosseratMapping.h | 48 +++++++++++-------- .../mapping/DiscreteCosseratMapping.h | 2 +- 4 files changed, 33 insertions(+), 23 deletions(-) diff --git a/src/Cosserat/engine/PointsManager.h b/src/Cosserat/engine/PointsManager.h index d197899f..7bcfaaaa 100755 --- a/src/Cosserat/engine/PointsManager.h +++ b/src/Cosserat/engine/PointsManager.h @@ -2,9 +2,8 @@ #include #include -#include #include - +#include #include namespace cosserat::controller @@ -28,6 +27,7 @@ class SOFA_COSSERAT_API PointsManager : public sofa::core::behavior::BaseControl sofa::Data d_color; sofa::Data d_beamPath; + // Inherited from BaseObject void init() override; void handleEvent(sofa::core::objectmodel::Event *event) override; diff --git a/src/Cosserat/forcefield/BeamHookeLawForceFieldRigid.h b/src/Cosserat/forcefield/BeamHookeLawForceFieldRigid.h index 009abc4a..11ca863d 100644 --- a/src/Cosserat/forcefield/BeamHookeLawForceFieldRigid.h +++ b/src/Cosserat/forcefield/BeamHookeLawForceFieldRigid.h @@ -57,6 +57,8 @@ using sofa::core::behavior::MultiMatrixAccessor ; using sofa::helper::OptionsGroup; +// TODO(dmarchal: 2024-07-12) clarify why there is BeamHookeLawForceFieldRigid +// and BeamHookelawForceField. A diff on the two files shows a huge amount of duplicated code. /** * This component is used to compute the Hooke's law on a beam computed on strain / stress * Only bending and torsion strain / stress are considered here diff --git a/src/Cosserat/mapping/BaseCosseratMapping.h b/src/Cosserat/mapping/BaseCosseratMapping.h index fe937ef3..d1d31ac2 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.h +++ b/src/Cosserat/mapping/BaseCosseratMapping.h @@ -1,24 +1,32 @@ /****************************************************************************** - * SOFA, Simulation Open-Framework Architecture, development version * - * (c) 2006-2019 INRIA, USTL, UJF, CNRS, MGH * - * * - * This program is free software; you can redistribute it and/or modify it * - * under the terms of the GNU Lesser General Public License as published by * - * the Free Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. * - * * - * This program is distributed in the hope that it will be useful, but WITHOUT * - * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * - * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * - * for more details. * - * * - * You should have received a copy of the GNU Lesser General Public License * - * along with this program. If not, see . * - ******************************************************************************* - * Authors: The SOFA Team and external contributors (see Authors.txt) * - * * - * Contact information: contact@sofa-framework.org * - ******************************************************************************/ +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006-2018 INRIA, USTL, UJF, CNRS, MGH * +* * +* This library is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This library is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this library; if not, write to the Free Software Foundation, * +* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. * +******************************************************************************* +* Plugin Cosserat v1.0 * +* * +* This plugin is also distributed under the GNU LGPL (Lesser General * +* Public License) license with the same conditions than SOFA. * +* * +* Contributors: Defrost team (INRIA, University of Lille, CNRS, * +* Ecole Centrale de Lille) * +* * +* Contact information: https://www.inria.fr/fr/defrost * +* * +******************************************************************************/ #pragma once #include #include diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.h b/src/Cosserat/mapping/DiscreteCosseratMapping.h index bffa5d8a..81de7d8c 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.h +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.h @@ -95,7 +95,7 @@ class DiscreteCosseratMapping : public BaseCosseratMapping { public: ////////////////////////////////////////////////////////////////////// - /// The following methods are inherited from BaseObject + /// The following methods are inherited from BaseCosseratMapping /// @{ void doBaseCosseratInit() final override; void draw(const sofa::core::visual::VisualParams *vparams) override; From f0ac682487104d259cf4eb08d5bc30f145a175ba Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Sat, 13 Jul 2024 09:40:56 +0200 Subject: [PATCH 07/16] Clean: LegendrePolynomialsMapping, fix licences --- .../mapping/LegendrePolynomialsMapping.h | 41 +++++++++++++++---- .../mapping/LegendrePolynomialsMapping.inl | 32 +++++++++++++-- 2 files changed, 62 insertions(+), 11 deletions(-) diff --git a/src/Cosserat/mapping/LegendrePolynomialsMapping.h b/src/Cosserat/mapping/LegendrePolynomialsMapping.h index d47aa64c..0f05f81f 100644 --- a/src/Cosserat/mapping/LegendrePolynomialsMapping.h +++ b/src/Cosserat/mapping/LegendrePolynomialsMapping.h @@ -1,6 +1,32 @@ -// -// Created by younes on 17/11/2021. -// +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006-2018 INRIA, USTL, UJF, CNRS, MGH * +* * +* This library is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This library is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this library; if not, write to the Free Software Foundation, * +* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. * +******************************************************************************* +* Plugin Cosserat v1.0 * +* * +* This plugin is also distributed under the GNU LGPL (Lesser General * +* Public License) license with the same conditions than SOFA. * +* * +* Contributors: Defrost team (INRIA, University of Lille, CNRS, * +* Ecole Centrale de Lille) * +* * +* Contact information: https://www.inria.fr/fr/defrost * +* * +******************************************************************************/ #pragma once #include #include @@ -29,15 +55,14 @@ namespace sofa::component::mapping { * \class LegendrePolynomialsMapping * @brief Computes and map the length of the beams * - * This is a component: - * https://www.sofa-framework.org/community/doc/programming-with-sofa/create-your-component/ */ - template class LegendrePolynomialsMapping : public core::Mapping { public: - SOFA_CLASS(SOFA_TEMPLATE2(LegendrePolynomialsMapping,TIn,TOut), SOFA_TEMPLATE2(core::Mapping,TIn,TOut)); + SOFA_CLASS(SOFA_TEMPLATE2(LegendrePolynomialsMapping,TIn,TOut), + SOFA_TEMPLATE2(core::Mapping,TIn,TOut)); + typedef core::Mapping Inherit; typedef TIn In; typedef TOut Out; @@ -85,7 +110,7 @@ class LegendrePolynomialsMapping : public core::Mapping }; #if !defined(SOFA_COSSERAT_CPP_LegendrePolynomialsMapping) -extern template class SOFA_SOFARIGID_API LegendrePolynomialsMapping< sofa::defaulttype::Vec3Types, sofa::defaulttype::Vec3Types >; +extern template class SOFA_COSSERAT_API LegendrePolynomialsMapping< sofa::defaulttype::Vec3Types, sofa::defaulttype::Vec3Types >; #endif } diff --git a/src/Cosserat/mapping/LegendrePolynomialsMapping.inl b/src/Cosserat/mapping/LegendrePolynomialsMapping.inl index 276463db..9ac61e4b 100644 --- a/src/Cosserat/mapping/LegendrePolynomialsMapping.inl +++ b/src/Cosserat/mapping/LegendrePolynomialsMapping.inl @@ -1,6 +1,32 @@ -// -// Created by younes on 17/11/2021. -// +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006-2018 INRIA, USTL, UJF, CNRS, MGH * +* * +* This library is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This library is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this library; if not, write to the Free Software Foundation, * +* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. * +******************************************************************************* +* Plugin Cosserat v1.0 * +* * +* This plugin is also distributed under the GNU LGPL (Lesser General * +* Public License) license with the same conditions than SOFA. * +* * +* Contributors: Defrost team (INRIA, University of Lille, CNRS, * +* Ecole Centrale de Lille) * +* * +* Contact information: https://www.inria.fr/fr/defrost * +* * +******************************************************************************/ #pragma once #include "LegendrePolynomialsMapping.h" From 9d22b1e33c25d7d3664b5f188ef1cbb2f25108e2 Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Sat, 13 Jul 2024 09:42:13 +0200 Subject: [PATCH 08/16] Remove un-needed BaseMapping --- src/Cosserat/mapping/LegendrePolynomialsMapping.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/Cosserat/mapping/LegendrePolynomialsMapping.h b/src/Cosserat/mapping/LegendrePolynomialsMapping.h index 0f05f81f..f155f8ec 100644 --- a/src/Cosserat/mapping/LegendrePolynomialsMapping.h +++ b/src/Cosserat/mapping/LegendrePolynomialsMapping.h @@ -29,11 +29,8 @@ ******************************************************************************/ #pragma once #include -#include -#include #include -#include #include #include #include From 2c7f194e45d68ea07eb6e9adc06c05388150c7c0 Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Sat, 13 Jul 2024 09:43:59 +0200 Subject: [PATCH 09/16] Remove Un-needed includes. --- src/Cosserat/mapping/LegendrePolynomialsMapping.h | 10 +--------- src/Cosserat/mapping/LegendrePolynomialsMapping.inl | 2 ++ 2 files changed, 3 insertions(+), 9 deletions(-) diff --git a/src/Cosserat/mapping/LegendrePolynomialsMapping.h b/src/Cosserat/mapping/LegendrePolynomialsMapping.h index f155f8ec..0eaa9173 100644 --- a/src/Cosserat/mapping/LegendrePolynomialsMapping.h +++ b/src/Cosserat/mapping/LegendrePolynomialsMapping.h @@ -28,19 +28,11 @@ * * ******************************************************************************/ #pragma once -#include -#include -#include -#include +#include #include -#include - -#include - namespace sofa::component::mapping { - using sofa::defaulttype::SolidTypes; using sofa::core::objectmodel::BaseContext; using sofa::type::Matrix3; using sofa::type::Matrix4; diff --git a/src/Cosserat/mapping/LegendrePolynomialsMapping.inl b/src/Cosserat/mapping/LegendrePolynomialsMapping.inl index 9ac61e4b..0730d418 100644 --- a/src/Cosserat/mapping/LegendrePolynomialsMapping.inl +++ b/src/Cosserat/mapping/LegendrePolynomialsMapping.inl @@ -41,6 +41,8 @@ #include #include #include +#include + namespace sofa::component::mapping { From 6ec68efa5120e0a3d3c6a3e780344405b8e4e57d Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Sat, 13 Jul 2024 09:46:23 +0200 Subject: [PATCH 10/16] Remove un-needed class alias declaration in public namespace They are un-needed and bloat the namespace. --- src/Cosserat/mapping/LegendrePolynomialsMapping.h | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/src/Cosserat/mapping/LegendrePolynomialsMapping.h b/src/Cosserat/mapping/LegendrePolynomialsMapping.h index 0eaa9173..b8385a22 100644 --- a/src/Cosserat/mapping/LegendrePolynomialsMapping.h +++ b/src/Cosserat/mapping/LegendrePolynomialsMapping.h @@ -32,13 +32,8 @@ #include #include -namespace sofa::component::mapping { - using sofa::core::objectmodel::BaseContext; - using sofa::type::Matrix3; - using sofa::type::Matrix4; - using type::Vec3; - using type::Vec6; - using std::get; +namespace sofa::component::mapping +{ /*! * \class LegendrePolynomialsMapping From a4a530b72b065cb42d3bfac29cb26279ffa91ed1 Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Mon, 22 Jul 2024 13:30:00 +0200 Subject: [PATCH 11/16] Use type traites for template data types. --- .../mapping/LegendrePolynomialsMapping.h | 32 ++++----- .../mapping/LegendrePolynomialsMapping.inl | 72 +++++++++---------- 2 files changed, 44 insertions(+), 60 deletions(-) diff --git a/src/Cosserat/mapping/LegendrePolynomialsMapping.h b/src/Cosserat/mapping/LegendrePolynomialsMapping.h index b8385a22..102c2e73 100644 --- a/src/Cosserat/mapping/LegendrePolynomialsMapping.h +++ b/src/Cosserat/mapping/LegendrePolynomialsMapping.h @@ -31,9 +31,13 @@ #include #include +#include namespace sofa::component::mapping { + namespace{ + using namespace sofa::core::trait; + } /*! * \class LegendrePolynomialsMapping @@ -47,22 +51,6 @@ class LegendrePolynomialsMapping : public core::Mapping SOFA_CLASS(SOFA_TEMPLATE2(LegendrePolynomialsMapping,TIn,TOut), SOFA_TEMPLATE2(core::Mapping,TIn,TOut)); - typedef core::Mapping Inherit; - typedef TIn In; - typedef TOut Out; - typedef Out OutDataTypes; - typedef typename Out::VecCoord VecCoord; - typedef typename Out::VecDeriv VecDeriv; - typedef typename Out::Coord Coord; - typedef typename Out::Deriv OutDeriv; - typedef typename Out::MatrixDeriv OutMatrixDeriv; - typedef typename In::Real InReal; - typedef typename In::Deriv InDeriv; - typedef typename In::VecCoord InVecCoord; - typedef typename In::VecDeriv InVecDeriv; - typedef typename In::MatrixDeriv InMatrixDeriv; - typedef typename Coord::value_type Real; - Data index; ///< input DOF index Data d_order; ///< input DOF index Data> d_vectorOfCurvilinearAbscissa; @@ -81,13 +69,17 @@ class LegendrePolynomialsMapping : public core::Mapping void reinit() override; double legendrePoly(unsigned int n, const double x); - void apply(const core::MechanicalParams *mparams, Data& out, const Data& in) override; + void apply(const core::MechanicalParams *mparams, + DataVecCoord_t& out, const DataVecCoord_t& in) override; - void applyJ(const core::MechanicalParams *mparams, Data& out, const Data& in) override; + void applyJ(const core::MechanicalParams *mparams, + DataVecDeriv_t& out, const DataVecDeriv_t& in) override; - void applyJT(const core::MechanicalParams *mparams, Data& out, const Data& in) override; + void applyJT(const core::MechanicalParams *mparams, + DataVecDeriv_t& out, const DataVecDeriv_t& in) override; - void applyJT(const core::ConstraintParams *cparams, Data& out, const Data& in) override; + void applyJT(const core::ConstraintParams *cparams, + DataMatrixDeriv_t& out, const DataMatrixDeriv_t& in) override; void draw(const core::visual::VisualParams* vparams) override; diff --git a/src/Cosserat/mapping/LegendrePolynomialsMapping.inl b/src/Cosserat/mapping/LegendrePolynomialsMapping.inl index 0730d418..e6269907 100644 --- a/src/Cosserat/mapping/LegendrePolynomialsMapping.inl +++ b/src/Cosserat/mapping/LegendrePolynomialsMapping.inl @@ -48,7 +48,7 @@ namespace sofa::component::mapping { template LegendrePolynomialsMapping::LegendrePolynomialsMapping() - : Inherit() + : Inherit1() , index(initData(&index, (unsigned)0, "index", "input DOF index")) , d_order(initData(&d_order, (unsigned)3, "order", "The order of Legendre polynomials")) , d_vectorOfCurvilinearAbscissa(initData(&d_vectorOfCurvilinearAbscissa, "curvAbscissa", "Vector of curvilinear Abscissa element of [0, 1]")) @@ -94,65 +94,55 @@ namespace sofa::component::mapping { template - void LegendrePolynomialsMapping::apply(const core::MechanicalParams * /*mparams*/, Data& dOut, const Data& dIn) + void LegendrePolynomialsMapping::apply(const core::MechanicalParams * /*mparams*/, + DataVecCoord_t& dOut, const DataVecCoord_t& dIn) { - helper::ReadAccessor< Data > in = dIn; - helper::WriteOnlyAccessor< Data > out = dOut; + auto in = sofa::helper::getReadAccessor(dIn); + auto out = sofa::helper::getWriteAccessor(dOut); const auto sz = d_vectorOfCurvilinearAbscissa.getValue().size(); out.resize(sz-1); - // std::cout<< "Apply : in " << in[0] < - void LegendrePolynomialsMapping::applyJ(const core::MechanicalParams * /*mparams*/, Data& dOut, const Data& dIn) + void LegendrePolynomialsMapping::applyJ(const core::MechanicalParams * /*mparams*/, + DataVecDeriv_t& dOut, const DataVecDeriv_t& dIn) { - helper::WriteOnlyAccessor< Data > velOut = dOut; - helper::ReadAccessor< Data > velIn = dIn; - - helper::WriteOnlyAccessor< Data > out = dOut; + auto velOut = sofa::helper::getWriteAccessor(dOut); + auto velIn = sofa::helper::getReadAccessor(dIn); + auto out = sofa::helper::getWriteOnlyAccessor(dOut); const auto sz = d_vectorOfCurvilinearAbscissa.getValue().size(); out.resize(sz-1); - // std::cout<< "ApplyJ : "<< std::endl; for(sofa::Index i=0 ; i - void LegendrePolynomialsMapping::applyJT(const core::MechanicalParams * /*mparams*/, Data& dOut, const Data& dIn) + void LegendrePolynomialsMapping::applyJT(const core::MechanicalParams * /*mparams*/, + DataVecDeriv_t& dOut, const DataVecDeriv_t& dIn) { - helper::WriteAccessor< Data > out = dOut; - helper::ReadAccessor< Data > in = dIn; + auto out = sofa::helper::getWriteAccessor(dOut); + auto in = sofa::helper::getReadAccessor(dIn); + const unsigned int numDofs = this->getFromModel()->getSize(); out.resize(numDofs); for (unsigned int cI = 0; cI < out.size(); cI++){ for(sofa::Index i=0 ; i -void LegendrePolynomialsMapping::applyJT(const core::ConstraintParams * /*cparams*/, Data& dOut, const Data& dIn) +void LegendrePolynomialsMapping::applyJT(const core::ConstraintParams * /*cparams*/, DataMatrixDeriv_t& dOut, const DataMatrixDeriv_t& dIn) { - InMatrixDeriv& out = *dOut.beginEdit(); - const OutMatrixDeriv& in = dIn.getValue(); + auto out = sofa::helper::getWriteAccessor(dOut); + auto in = sofa::helper::getReadAccessor(dIn); const unsigned int numDofs = this->getFromModel()->getSize(); - typename Out::MatrixDeriv::RowConstIterator rowItEnd = in.end(); - type::vector tabF; tabF.resize(numDofs); + type::vector> tabF; + tabF.resize(numDofs); - for (typename Out::MatrixDeriv::RowConstIterator rowIt = in.begin(); rowIt != rowItEnd; ++rowIt) + auto rowIt = in->begin(); + auto rowEnd = in->end(); + + for (;rowIt!=rowEnd;++rowIt) { - typename OutMatrixDeriv::ColConstIterator colIt = rowIt.begin(); - typename OutMatrixDeriv::ColConstIterator colItEnd = rowIt.end(); + auto colIt = rowIt.begin(); + auto colItEnd = rowIt.end(); if (colIt == colItEnd) continue; - typename InMatrixDeriv::RowIterator o = out.writeLine(rowIt.index()); // we store the constraint number - while (colIt != colItEnd) { + auto o = out->writeLine(rowIt.index()); // we store the constraint number + + while (colIt != colItEnd) + { int childIndex = colIt.index(); - const OutDeriv f_It = colIt.val(); + auto f_It = colIt.val(); for (unsigned int order = 0; order < numDofs; order++){ - InDeriv f; + Deriv_t f; f = m_matOfCoeffs[childIndex][order] * f_It; tabF[order] += f; o.addCol(order, f); @@ -193,9 +188,6 @@ void LegendrePolynomialsMapping::applyJT(const core::ConstraintParams colIt++; } } - - // std::cout << "applyJT Constraint : new J on input DOFs = \n" << out << std::endl; - dOut.endEdit(); } From 0cd8be61c2aeb57959a928eece94703460a00414 Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Mon, 22 Jul 2024 13:33:37 +0200 Subject: [PATCH 12/16] Remove useless includes in LegendrePolynomials. --- .../mapping/LegendrePolynomialsMapping.cpp | 1 - .../mapping/LegendrePolynomialsMapping.inl | 18 +++--------------- 2 files changed, 3 insertions(+), 16 deletions(-) diff --git a/src/Cosserat/mapping/LegendrePolynomialsMapping.cpp b/src/Cosserat/mapping/LegendrePolynomialsMapping.cpp index 961daab4..e78a226f 100644 --- a/src/Cosserat/mapping/LegendrePolynomialsMapping.cpp +++ b/src/Cosserat/mapping/LegendrePolynomialsMapping.cpp @@ -5,7 +5,6 @@ #include "LegendrePolynomialsMapping.inl" #include -#include #include namespace sofa::component::mapping diff --git a/src/Cosserat/mapping/LegendrePolynomialsMapping.inl b/src/Cosserat/mapping/LegendrePolynomialsMapping.inl index e6269907..1b876301 100644 --- a/src/Cosserat/mapping/LegendrePolynomialsMapping.inl +++ b/src/Cosserat/mapping/LegendrePolynomialsMapping.inl @@ -28,24 +28,12 @@ * * ******************************************************************************/ #pragma once -#include "LegendrePolynomialsMapping.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "LegendrePolynomialsMapping.h" #include - -namespace sofa::component::mapping { - +namespace sofa::component::mapping +{ template LegendrePolynomialsMapping::LegendrePolynomialsMapping() : Inherit1() From 608aa7f635889de1c2757a273193c9d8c8e1056e Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Mon, 22 Jul 2024 13:54:27 +0200 Subject: [PATCH 13/16] Remove un-needed #includes in the mappings It is important to always keep only the neededs includes. Having un-needed includes: - slow down compilation time - increase (non linearly) the amount of file to recompile on change - make the code appears more complex and entangled that it is actually. --- src/Cosserat/mapping/BaseCosseratMapping.inl | 12 +++--------- src/Cosserat/mapping/DifferenceMultiMapping.h | 10 ---------- src/Cosserat/mapping/DifferenceMultiMapping.inl | 11 ----------- src/Cosserat/mapping/DiscreteCosseratMapping.inl | 13 ++----------- .../mapping/DiscreteDynamicCosseratMapping.inl | 11 ----------- src/Cosserat/mapping/RigidDistanceMapping.h | 5 ----- src/Cosserat/mapping/RigidDistanceMapping.inl | 10 ---------- 7 files changed, 5 insertions(+), 67 deletions(-) diff --git a/src/Cosserat/mapping/BaseCosseratMapping.inl b/src/Cosserat/mapping/BaseCosseratMapping.inl index 1a7ec6d8..4d4d8dcc 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.inl +++ b/src/Cosserat/mapping/BaseCosseratMapping.inl @@ -24,19 +24,13 @@ #include #include +// BaseCosserat inherit from Multi2Mapping. To properly instanciate the templated types +// in the current compilation unit it is need to have the definition (implementation) details +// of the Multi2Mapping class. #include -#include -#include -#include -#include -#include -#include - -#include // To go further => // https://www.mathworks.com/matlabcentral/fileexchange/83038-sorosim/ - namespace Cosserat::mapping { diff --git a/src/Cosserat/mapping/DifferenceMultiMapping.h b/src/Cosserat/mapping/DifferenceMultiMapping.h index 43dcec82..413a790d 100644 --- a/src/Cosserat/mapping/DifferenceMultiMapping.h +++ b/src/Cosserat/mapping/DifferenceMultiMapping.h @@ -21,18 +21,11 @@ ******************************************************************************/ #pragma once #include -#include -#include -#include #include -#include -#include - namespace Cosserat::mapping { -using sofa::defaulttype::SolidTypes ; using sofa::core::objectmodel::BaseContext ; using sofa::type::Matrix3; using sofa::type::Matrix4; @@ -41,7 +34,6 @@ using sofa::type::Vec6; using sofa::type::Quat; using std::get; using sofa::type::vector; -using Cosserat::mapping::BaseCosseratMapping; /*! * \class DifferenceMultiMapping @@ -92,8 +84,6 @@ class DifferenceMultiMapping : public sofa::core::Multi2Mapping OutDataVecDeriv; typedef sofa::Data OutDataMatrixDeriv; - typedef typename SolidTypes::Transform Transform ; - public: /********************** The component Data **************************/ //Input data diff --git a/src/Cosserat/mapping/DifferenceMultiMapping.inl b/src/Cosserat/mapping/DifferenceMultiMapping.inl index fec6b9d5..ee0bb715 100644 --- a/src/Cosserat/mapping/DifferenceMultiMapping.inl +++ b/src/Cosserat/mapping/DifferenceMultiMapping.inl @@ -21,22 +21,11 @@ ******************************************************************************/ #pragma once #include - -#include -#include -#include #include -#include -#include -#include -#include - -#include namespace Cosserat::mapping { using sofa::core::objectmodel::BaseContext; -using sofa::helper::AdvancedTimer; using sofa::helper::WriteAccessor; using sofa::type::RGBAColor; diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.inl b/src/Cosserat/mapping/DiscreteCosseratMapping.inl index d671f0f8..2f1e6498 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.inl +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.inl @@ -22,23 +22,14 @@ #pragma once #include -#include -#include -#include #include #include -#include -#include -#include -#include -#include - -namespace Cosserat::mapping { +namespace Cosserat::mapping +{ using sofa::core::objectmodel::BaseContext; using sofa::defaulttype::SolidTypes; -using sofa::helper::AdvancedTimer; using sofa::helper::WriteAccessor; using sofa::type::RGBAColor; diff --git a/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.inl b/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.inl index 3b6a8f4c..78f2a770 100644 --- a/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.inl +++ b/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.inl @@ -21,23 +21,12 @@ ******************************************************************************/ #pragma once #include - -#include -#include -#include #include -#include -#include -#include -#include - -#include namespace Cosserat::mapping { using sofa::core::objectmodel::BaseContext ; -using sofa::helper::AdvancedTimer; using sofa::helper::WriteAccessor; diff --git a/src/Cosserat/mapping/RigidDistanceMapping.h b/src/Cosserat/mapping/RigidDistanceMapping.h index 2fc14960..9b0e8f99 100644 --- a/src/Cosserat/mapping/RigidDistanceMapping.h +++ b/src/Cosserat/mapping/RigidDistanceMapping.h @@ -23,12 +23,7 @@ #include #include -#include -#include #include -#include -#include -#include namespace Cosserat::mapping { diff --git a/src/Cosserat/mapping/RigidDistanceMapping.inl b/src/Cosserat/mapping/RigidDistanceMapping.inl index 1d880873..965a3fd0 100644 --- a/src/Cosserat/mapping/RigidDistanceMapping.inl +++ b/src/Cosserat/mapping/RigidDistanceMapping.inl @@ -25,20 +25,10 @@ #include #include -#include -#include -#include -#include -#include -#include - -#include - namespace Cosserat::mapping { using sofa::core::objectmodel::BaseContext ; -using sofa::helper::AdvancedTimer; using sofa::helper::WriteAccessor; using sofa::defaulttype::SolidTypes ; using sofa::type::RGBAColor; From 2816a42fb5d2cc0489216461a8a45b9d4e0d2690 Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Mon, 22 Jul 2024 14:25:17 +0200 Subject: [PATCH 14/16] Use SReal and RGBA Color in RigidDistanceMapping data field. --- src/Cosserat/mapping/RigidDistanceMapping.h | 8 +-- src/Cosserat/mapping/RigidDistanceMapping.inl | 66 +++++++++++-------- 2 files changed, 42 insertions(+), 32 deletions(-) diff --git a/src/Cosserat/mapping/RigidDistanceMapping.h b/src/Cosserat/mapping/RigidDistanceMapping.h index 9b0e8f99..2e102828 100644 --- a/src/Cosserat/mapping/RigidDistanceMapping.h +++ b/src/Cosserat/mapping/RigidDistanceMapping.h @@ -97,10 +97,10 @@ class RigidDistanceMapping : public sofa::core::Multi2Mapping protected: Data > d_index1 ; Data > d_index2 ; - Data d_max ; - Data d_min ; - Data d_radius ; - Data d_color; + Data d_max ; + Data d_min ; + Data d_radius ; + Data d_color; Data > d_index; Data d_debug ; diff --git a/src/Cosserat/mapping/RigidDistanceMapping.inl b/src/Cosserat/mapping/RigidDistanceMapping.inl index 965a3fd0..26660bfd 100644 --- a/src/Cosserat/mapping/RigidDistanceMapping.inl +++ b/src/Cosserat/mapping/RigidDistanceMapping.inl @@ -22,7 +22,6 @@ #pragma once #include #include - #include #include @@ -35,15 +34,15 @@ using sofa::type::RGBAColor; template RigidDistanceMapping::RigidDistanceMapping() - : d_index1(initData(&d_index1, "first_point", "index of the first model \n")) - , d_index2(initData(&d_index2, "second_point", "index of the second model \n")) - , d_max(initData(&d_max, (Real)1.0e-2, "max", "the maximum of the deformation.\n")) - , d_min(initData(&d_min, (Real)0.0, "min", "the minimum of the deformation.\n")) - , d_radius(initData(&d_radius, (Real)3.0, "radius", "the axis in which we want to show the deformation.\n")) - , d_color(initData(&d_color, Vec4f (1, 0., 1., 0.8) ,"color", "The default beam color")) + : d_index1(initData(&d_index1, "first_point", "index of the first model")) + , d_index2(initData(&d_index2, "second_point", "index of the second model")) + , d_max(initData(&d_max, (SReal)1.0e-2, "max", "the maximum of the deformation")) + , d_min(initData(&d_min, (SReal)0.0, "min", "the minimum of the deformation")) + , d_radius(initData(&d_radius, (SReal)3.0, "radius", "the axis in which we want to show the deformation")) + , d_color(initData(&d_color, RGBAColor{1, 0., 1., 0.8} ,"color", "The default beam color")) , d_index(initData(&d_index, "index", "if this parameter is false, you draw the beam with color " - "according to the force apply to each beam")) - , d_debug(initData(&d_debug, false, "debug", "show debug output.\n")) + "according to the force apply to each beam")) + , d_debug(initData(&d_debug, false, "debug", "show debug output.")) , m_toModel(NULL) { d_debug.setValue(false); @@ -57,6 +56,7 @@ void RigidDistanceMapping::init() if(this->getFromModels1().empty() || this->getFromModels2().empty() || this->getToModels().empty()) { + // TODO(dmarchal: 2024-07-22): if this is an error, the componentState must be set to invalid msg_error() << "Error while initializing ; input getFromModels1/getFromModels2/output not found" ; return; } @@ -66,7 +66,9 @@ void RigidDistanceMapping::init() m_minInd = std::min(m1Indices.size(), m2Indices.size()); if (m_minInd == 0) { - msg_info("") << " The size of the indices must not be equal to zero" ; + // TODO(dmarchal: 2024-07-22): this looks like and error... but it is reported as an info... + // move it to a real error if this prevent the component to properly behave. + msg_info() << " The size of the indices must not be equal to zero" ; return; } } @@ -83,7 +85,6 @@ void RigidDistanceMapping::apply( if(dataVecOutPos.empty() || dataVecIn1Pos.empty() || dataVecIn2Pos.empty()) return; - ///Do Apply //We need only one input In model and input Root model (if present) const In1VecCoord& in1 = dataVecIn1Pos[0]->getValue(); const In2VecCoord& in2 = dataVecIn2Pos[0]->getValue(); @@ -94,7 +95,11 @@ void RigidDistanceMapping::apply( auto &m1Indices = d_index1.getValue(); auto &m2Indices = d_index2.getValue(); - for (sofa::Index pid=0; pidf_printLog.getValue(); + + for (sofa::Index pid=0; pid::apply( outOri.normalize(); out[pid] = OutCoord(outCenter,outOri); // This difference is in the word space - if (d_debug.getValue()){ - std::cout << " in1 :" << in1[tm1] << std::endl; - std::cout << " in2 :" << in2[tm2] << std::endl; - std::cout << " out :" << out[pid] << std::endl; + if (doPrintLog){ + tmp << " in1 :" << in1[tm1] << msgendl + << " in2 :" << in2[tm2] << msgendl + << " out :" << out[pid] << msgendl; } } + msg_info() << "Apply's dump " << msgendl + << tmp.str(); dataVecOutPos[0]->endEdit(); } @@ -136,9 +143,8 @@ void RigidDistanceMapping:: applyJ( getVOrientation(outVel[index]) = getVOrientation(in2Vel[m2Indices[index]]) - getVOrientation(in1Vel[m1Indices[index]]) ; } dataVecOutVel[0]->endEdit(); - if (d_debug.getValue()){ - std::cout << " =====> outVel[m1Indices[index]] : " << outVel << std::endl; - } + + msg_info() << " =====> outVel[m1Indices[index]] : " << outVel; } @@ -191,10 +197,10 @@ void RigidDistanceMapping::applyJT( const auto &m2Indices = d_index2.getValue(); typename OutMatrixDeriv::RowConstIterator rowItEnd = in.end(); + std::stringstream tmp; + bool doDump = this->f_printLog.getValue(); for (typename OutMatrixDeriv::RowConstIterator rowIt = in.begin(); rowIt != rowItEnd; ++rowIt) { typename OutMatrixDeriv::ColConstIterator colIt = rowIt.begin(); - // typename OutMatrixDeriv::ColConstIterator colItEnd = rowIt.end(); - typename In1MatrixDeriv::RowIterator o1 = out1.writeLine(rowIt.index()); // we store the constraint number typename In2MatrixDeriv::RowIterator o2 = out2.writeLine(rowIt.index()); @@ -215,18 +221,22 @@ void RigidDistanceMapping::applyJT( In2::setDPos(direction2,getVCenter(valueConst_)); In2::setDRot(direction2,getVOrientation(valueConst_)); - if (d_debug.getValue()){ - printf("1. ======================================================================================= \n"); - std::cout << "Constraint " << rowIt.index() << " ==> childIndex: "<< childIndex << std::endl; - std::cout << "parentIndex1 " << parentIndex1 << " ==> parentIndex2 "<< parentIndex2 << std::endl; - std::cout << "valueConst_: "<< valueConst_ << std::endl; - std::cout << "direction1: " << direction1 << std::endl; - std::cout << "direction2: " << direction2 << std::endl; + if (doDump) + { + tmp << "=======================================================================================" << msgendl + << "Constraint " << rowIt.index() << " ==> childIndex: "<< childIndex << msgendl + << "parentIndex1 " << parentIndex1 << " ==> parentIndex2 "<< parentIndex2 << msgendl + << "valueConst_: "<< valueConst_ << msgendl + << "direction1: " << direction1 << msgendl + << "direction2: " << direction2 << msgendl; } o1.addCol(parentIndex1, direction1); o2.addCol(parentIndex2, direction2); } + msg_info() << "Apply JT dump: " << msgendl + << tmp.str(); + dataMatOut1Const[0]->endEdit(); dataMatOut2Const[0]->endEdit(); } From 4e7713d9c4db42d3063976677d47c4d0274723c5 Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Mon, 22 Jul 2024 14:30:45 +0200 Subject: [PATCH 15/16] Clean data fields in DifferenceMultiMapping & RigidDistanceMapping --- src/Cosserat/mapping/DifferenceMultiMapping.h | 7 ++++--- src/Cosserat/mapping/DifferenceMultiMapping.inl | 4 ++-- src/Cosserat/mapping/RigidDistanceMapping.h | 1 - src/Cosserat/mapping/RigidDistanceMapping.inl | 2 -- 4 files changed, 6 insertions(+), 8 deletions(-) diff --git a/src/Cosserat/mapping/DifferenceMultiMapping.h b/src/Cosserat/mapping/DifferenceMultiMapping.h index 413a790d..b5fa2be9 100644 --- a/src/Cosserat/mapping/DifferenceMultiMapping.h +++ b/src/Cosserat/mapping/DifferenceMultiMapping.h @@ -89,11 +89,12 @@ class DifferenceMultiMapping : public sofa::core::Multi2Mapping> d_direction; sofa::Data> d_indices; - sofa::Data d_radius; - sofa::Data d_color; - sofa::Data d_drawArrows; + sofa::Data d_radius; + sofa::Data d_color; sofa::Data d_lastPointIsFixed; + sofa::Data d_drawArrows; + protected: sofa::core::State* m_fromModel1; sofa::core::State* m_fromModel2; diff --git a/src/Cosserat/mapping/DifferenceMultiMapping.inl b/src/Cosserat/mapping/DifferenceMultiMapping.inl index ee0bb715..4661da3d 100644 --- a/src/Cosserat/mapping/DifferenceMultiMapping.inl +++ b/src/Cosserat/mapping/DifferenceMultiMapping.inl @@ -33,8 +33,8 @@ template DifferenceMultiMapping::DifferenceMultiMapping() : d_direction(initData(&d_direction, "direction", "The list of directions of fix points .\n")), d_indices(initData(&d_indices, "indices", "Indices of fixe points of the cable")), - d_radius(initData(&d_radius, 2.0, "radius", "The size of the cable")), - d_color(initData(&d_color, sofa::type::Vec4f(1, 0, 0, 1), "color", "The color of the cable")), + d_radius(initData(&d_radius, (SReal)2.0, "radius", "The size of the cable")), + d_color(initData(&d_color, sofa::type::RGBAColor::red(), "color", "The color of the cable")), d_drawArrows(initData(&d_drawArrows, false, "drawArrows", "The color of the cable")), d_lastPointIsFixed(initData(&d_lastPointIsFixed, true, "lastPointIsFixed", "This select the last point as fixed of not," "one.")), diff --git a/src/Cosserat/mapping/RigidDistanceMapping.h b/src/Cosserat/mapping/RigidDistanceMapping.h index 2e102828..9cca58ed 100644 --- a/src/Cosserat/mapping/RigidDistanceMapping.h +++ b/src/Cosserat/mapping/RigidDistanceMapping.h @@ -101,7 +101,6 @@ class RigidDistanceMapping : public sofa::core::Multi2Mapping Data d_min ; Data d_radius ; Data d_color; - Data > d_index; Data d_debug ; sofa::core::State* m_toModel; diff --git a/src/Cosserat/mapping/RigidDistanceMapping.inl b/src/Cosserat/mapping/RigidDistanceMapping.inl index 26660bfd..2222fe17 100644 --- a/src/Cosserat/mapping/RigidDistanceMapping.inl +++ b/src/Cosserat/mapping/RigidDistanceMapping.inl @@ -40,8 +40,6 @@ RigidDistanceMapping::RigidDistanceMapping() , d_min(initData(&d_min, (SReal)0.0, "min", "the minimum of the deformation")) , d_radius(initData(&d_radius, (SReal)3.0, "radius", "the axis in which we want to show the deformation")) , d_color(initData(&d_color, RGBAColor{1, 0., 1., 0.8} ,"color", "The default beam color")) - , d_index(initData(&d_index, "index", "if this parameter is false, you draw the beam with color " - "according to the force apply to each beam")) , d_debug(initData(&d_debug, false, "debug", "show debug output.")) , m_toModel(NULL) { From 30dc2dd4e0674d314596de17cdbc363110659486 Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Mon, 22 Jul 2024 14:33:28 +0200 Subject: [PATCH 16/16] Remove the empty draw() method in some of the Cosserat&Legendre mapping. --- .../mapping/DiscreteCosseratMapping.inl | 1112 ++++++++--------- .../mapping/DiscreteDynamicCosseratMapping.h | 2 +- .../DiscreteDynamicCosseratMapping.inl | 7 - .../mapping/LegendrePolynomialsMapping.h | 2 - .../mapping/LegendrePolynomialsMapping.inl | 8 - 5 files changed, 557 insertions(+), 574 deletions(-) diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.inl b/src/Cosserat/mapping/DiscreteCosseratMapping.inl index 2f1e6498..1886fb6c 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.inl +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.inl @@ -50,9 +50,9 @@ DiscreteCosseratMapping::DiscreteCosseratMapping() "if this parameter is false, you draw the beam with " "color according to the force apply to each beam")), d_color(initData( - &d_color, - sofa::type::RGBAColor(40 / 255.0, 104 / 255.0, 137 / 255.0, 0.8), - "color", "The default beam color")), + &d_color, + sofa::type::RGBAColor(40 / 255.0, 104 / 255.0, 137 / 255.0, 0.8), + "color", "The default beam color")), d_index( initData(&d_index, "index", "if this parameter is false, you draw the beam with color " @@ -62,621 +62,621 @@ DiscreteCosseratMapping::DiscreteCosseratMapping() "base of Cosserat models, 0 by default this can" "take another value if the rigid base is given " "by another body.")) { - this->addUpdateCallback( - "updateFrames", {&d_curv_abs_section, &d_curv_abs_frames, &d_debug}, - [this](const sofa::core::DataTracker &t) { + this->addUpdateCallback( + "updateFrames", {&d_curv_abs_section, &d_curv_abs_frames, &d_debug}, + [this](const sofa::core::DataTracker &t) { SOFA_UNUSED(t); this->initializeFrames(); const In1VecCoord &inDeform = - m_fromModel1->read(sofa::core::ConstVecCoordId::position())->getValue(); + m_fromModel1->read(sofa::core::ConstVecCoordId::position())->getValue(); this->updateExponentialSE3(inDeform); return sofa::core::objectmodel::ComponentState::Valid; - }, - {}); + }, + {}); } template void DiscreteCosseratMapping::doBaseCosseratInit() { - m_colorMap.setColorScheme("Blue to Red"); - m_colorMap.reinit(); + m_colorMap.setColorScheme("Blue to Red"); + m_colorMap.reinit(); } template void DiscreteCosseratMapping::apply( - const sofa::core::MechanicalParams * /* mparams */, - const vector &dataVecOutPos, - const vector &dataVecIn1Pos, - const vector &dataVecIn2Pos) { - - if (dataVecOutPos.empty() || dataVecIn1Pos.empty() || dataVecIn2Pos.empty()) - return; - - // Checking the componentState, to trigger a callback if other data fields (specifically - // d_curv_abs_section and d_curv_abs_frames) were changed dynamically - if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) - return; - /// Do Apply - // We need only one input In model and input Root model (if present) - const In1VecCoord &in1 = dataVecIn1Pos[0]->getValue(); - const In2VecCoord &in2 = dataVecIn2Pos[0]->getValue(); - - const auto sz = d_curv_abs_frames.getValue().size(); - OutVecCoord &out = *dataVecOutPos[0]->beginEdit(); // frames states - out.resize(sz); - const auto baseIndex = d_baseIndex.getValue(); - - // update the Exponential matrices according to new deformation - // Here we update m_framesExponentialSE3Vectors & m_nodesExponentialSE3Vectors - // Which are the homogeneous matrices of the frames and the nodes in local - // coordinate. - this->updateExponentialSE3(in1); - - /* Apply the transformation to go from cossserat to SOFA frame*/ - Transform frame0 = - Transform(In2::getCPos(in2[baseIndex]), In2::getCRot(in2[baseIndex])); - - // Cache the printLog value out of the loop, otherwise it will trigger a graph - // update at every iteration. - bool doPrintLog = this->f_printLog.getValue(); - for (unsigned int i = 0; i < sz; i++) { - Transform frame = frame0; - for (unsigned int u = 0; u < m_indicesVectors[i]; u++) { - frame *= - m_nodesExponentialSE3Vectors[u]; // frame = gX(L_0)*...*gX(L_{n-1}) + const sofa::core::MechanicalParams * /* mparams */, + const vector &dataVecOutPos, + const vector &dataVecIn1Pos, + const vector &dataVecIn2Pos) { + + if (dataVecOutPos.empty() || dataVecIn1Pos.empty() || dataVecIn2Pos.empty()) + return; + + // Checking the componentState, to trigger a callback if other data fields (specifically + // d_curv_abs_section and d_curv_abs_frames) were changed dynamically + if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) + return; + /// Do Apply + // We need only one input In model and input Root model (if present) + const In1VecCoord &in1 = dataVecIn1Pos[0]->getValue(); + const In2VecCoord &in2 = dataVecIn2Pos[0]->getValue(); + + const auto sz = d_curv_abs_frames.getValue().size(); + OutVecCoord &out = *dataVecOutPos[0]->beginEdit(); // frames states + out.resize(sz); + const auto baseIndex = d_baseIndex.getValue(); + + // update the Exponential matrices according to new deformation + // Here we update m_framesExponentialSE3Vectors & m_nodesExponentialSE3Vectors + // Which are the homogeneous matrices of the frames and the nodes in local + // coordinate. + this->updateExponentialSE3(in1); + + /* Apply the transformation to go from cossserat to SOFA frame*/ + Transform frame0 = + Transform(In2::getCPos(in2[baseIndex]), In2::getCRot(in2[baseIndex])); + + // Cache the printLog value out of the loop, otherwise it will trigger a graph + // update at every iteration. + bool doPrintLog = this->f_printLog.getValue(); + for (unsigned int i = 0; i < sz; i++) { + Transform frame = frame0; + for (unsigned int u = 0; u < m_indicesVectors[i]; u++) { + frame *= + m_nodesExponentialSE3Vectors[u]; // frame = gX(L_0)*...*gX(L_{n-1}) + } + frame *= m_framesExponentialSE3Vectors[i]; // frame*gX(x) + + // This is a lazy printing approach, so there is no time consuming action in + // the core of the loop. + msg_info_when(doPrintLog) << "Frame : " << i << " = " << frame; + + Vec3 origin = frame.getOrigin(); + Quat orientation = frame.getOrientation(); + out[i] = OutCoord(origin, orientation); } - frame *= m_framesExponentialSE3Vectors[i]; // frame*gX(x) - - // This is a lazy printing approach, so there is no time consuming action in - // the core of the loop. - msg_info_when(doPrintLog) << "Frame : " << i << " = " << frame; - - Vec3 origin = frame.getOrigin(); - Quat orientation = frame.getOrientation(); - out[i] = OutCoord(origin, orientation); - } - - // If the printLog attribute is checked then print distance between out - // frames. - if (doPrintLog) { - std::stringstream tmp; - for (unsigned int i = 0; i < out.size() - 1; i++) { - Vec3 diff = out[i + 1].getCenter() - out[i].getCenter(); - tmp << "dist " << i << " : " << diff.norm() << msgendl; + + // If the printLog attribute is checked then print distance between out + // frames. + if (doPrintLog) { + std::stringstream tmp; + for (unsigned int i = 0; i < out.size() - 1; i++) { + Vec3 diff = out[i + 1].getCenter() - out[i].getCenter(); + tmp << "dist " << i << " : " << diff.norm() << msgendl; + } + msg_info() << tmp.str(); } - msg_info() << tmp.str(); - } - // TODO(dmarchal:2024/06/13): This looks a suspicious design pattern, - // elaborate more on the purpose of m_indexInput and how to use it. - m_indexInput = 0; + // TODO(dmarchal:2024/06/13): This looks a suspicious design pattern, + // elaborate more on the purpose of m_indexInput and how to use it. + m_indexInput = 0; } template void DiscreteCosseratMapping::computeLogarithm( - const double &x, const Mat4x4 &gX, Mat4x4 &log_gX) { - - // computes theta - double theta = computeTheta(x, gX); - - // if theta is very small we retun log_gX as the identity. - if (theta <= std::numeric_limits::epsilon()) { - log_gX = Mat4x4::Identity(); - return; - } - - // otherwise we compute it - double csc_theta = 1.0 / (sin(x * theta / 2.0)); - double sec_theta = 1.0 / (cos(x * theta / 2.0)); - double cst = (1.0 / 8) * (csc_theta * csc_theta * csc_theta) * sec_theta; - double x_theta = x * theta; - double cos_2Xtheta = cos(2.0 * x_theta); - double cos_Xtheta = cos(x_theta); - double sin_2Xtheta = sin(2.0 * x_theta); - double sin_Xtheta = sin(x_theta); - - log_gX.clear(); - log_gX = cst * ((x_theta * cos_2Xtheta - sin_Xtheta) * Mat4x4::Identity() - - (x_theta * cos_Xtheta + 2.0 * x_theta * cos_2Xtheta - - sin_Xtheta - sin_2Xtheta) * - gX + - (2.0 * x_theta * cos_Xtheta + x_theta * cos_2Xtheta - - sin_Xtheta - sin_2Xtheta) * - (gX * gX) - - (x_theta * cos_Xtheta - sin_Xtheta) * (gX * gX * gX)); + const double &x, const Mat4x4 &gX, Mat4x4 &log_gX) { + + // computes theta + double theta = computeTheta(x, gX); + + // if theta is very small we retun log_gX as the identity. + if (theta <= std::numeric_limits::epsilon()) { + log_gX = Mat4x4::Identity(); + return; + } + + // otherwise we compute it + double csc_theta = 1.0 / (sin(x * theta / 2.0)); + double sec_theta = 1.0 / (cos(x * theta / 2.0)); + double cst = (1.0 / 8) * (csc_theta * csc_theta * csc_theta) * sec_theta; + double x_theta = x * theta; + double cos_2Xtheta = cos(2.0 * x_theta); + double cos_Xtheta = cos(x_theta); + double sin_2Xtheta = sin(2.0 * x_theta); + double sin_Xtheta = sin(x_theta); + + log_gX.clear(); + log_gX = cst * ((x_theta * cos_2Xtheta - sin_Xtheta) * Mat4x4::Identity() - + (x_theta * cos_Xtheta + 2.0 * x_theta * cos_2Xtheta - + sin_Xtheta - sin_2Xtheta) * + gX + + (2.0 * x_theta * cos_Xtheta + x_theta * cos_2Xtheta - + sin_Xtheta - sin_2Xtheta) * + (gX * gX) - + (x_theta * cos_Xtheta - sin_Xtheta) * (gX * gX * gX)); } template void DiscreteCosseratMapping::applyJ( - const sofa::core::MechanicalParams * /* mparams */, - const vector &dataVecOutVel, - const vector &dataVecIn1Vel, - const vector &dataVecIn2Vel) { - - if (dataVecOutVel.empty() || dataVecIn1Vel.empty() || dataVecIn2Vel.empty()) - return; - - if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) - return; - if (d_debug.getValue()) - std::cout << " ########## ApplyJ Function ########" << std::endl; - const In1VecDeriv &in1_vel = dataVecIn1Vel[0]->getValue(); - const In2VecDeriv &in2_vel = dataVecIn2Vel[0]->getValue(); - OutVecDeriv &out_vel = *dataVecOutVel[0]->beginEdit(); - const auto baseIndex = d_baseIndex.getValue(); - - // Curv abscissa of nodes and frames - sofa::helper::ReadAccessor>> curv_abs_section = - d_curv_abs_section; - sofa::helper::ReadAccessor>> curv_abs_frames = - d_curv_abs_frames; - - const In1VecCoord &inDeform = - m_fromModel1->read(sofa::core::ConstVecCoordId::position()) - ->getValue(); // Strains - // Compute the tangent Exponential SE3 vectors - this->updateTangExpSE3(inDeform); - - // Get base velocity as input this is also called eta - m_nodesVelocityVectors.clear(); - - // Get base velocity and convert to Vec6, for the facility of computation - Vec6 baseVelocity; // - for (auto u = 0; u < 6; u++) - baseVelocity[u] = in2_vel[baseIndex][u]; - - // Apply the local transform i.e. from SOFA's frame to Cosserat's frame - const In2VecCoord &xfrom2Data = - m_fromModel2->read(sofa::core::ConstVecCoordId::position())->getValue(); - Transform TInverse = Transform(xfrom2Data[baseIndex].getCenter(), - xfrom2Data[baseIndex].getOrientation()) - .inversed(); - Mat6x6 P = this->buildProjector(TInverse); - Vec6 baseLocalVelocity = - P * baseVelocity; // This is the base velocity in Locale frame - m_nodesVelocityVectors.push_back(baseLocalVelocity); - if (d_debug.getValue()) - std::cout << "Base local Velocity :" << baseLocalVelocity << std::endl; - - // Compute velocity at nodes - for (unsigned int i = 1; i < curv_abs_section.size(); i++) { - Transform Trans = m_nodesExponentialSE3Vectors[i].inversed(); - TangentTransform Adjoint; - Adjoint.clear(); - this->computeAdjoint(Trans, Adjoint); - - /// The null vector is replace by the linear velocity in Vec6Type - Vec6 Xi_dot = Vec6(in1_vel[i - 1], Vec3(0.0, 0.0, 0.0)); - - Vec6 eta_node_i = Adjoint * (m_nodesVelocityVectors[i - 1] + - m_nodesTangExpVectors[i] * Xi_dot); - m_nodesVelocityVectors.push_back(eta_node_i); + const sofa::core::MechanicalParams * /* mparams */, + const vector &dataVecOutVel, + const vector &dataVecIn1Vel, + const vector &dataVecIn2Vel) { + + if (dataVecOutVel.empty() || dataVecIn1Vel.empty() || dataVecIn2Vel.empty()) + return; + + if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) + return; + if (d_debug.getValue()) + std::cout << " ########## ApplyJ Function ########" << std::endl; + const In1VecDeriv &in1_vel = dataVecIn1Vel[0]->getValue(); + const In2VecDeriv &in2_vel = dataVecIn2Vel[0]->getValue(); + OutVecDeriv &out_vel = *dataVecOutVel[0]->beginEdit(); + const auto baseIndex = d_baseIndex.getValue(); + + // Curv abscissa of nodes and frames + sofa::helper::ReadAccessor>> curv_abs_section = + d_curv_abs_section; + sofa::helper::ReadAccessor>> curv_abs_frames = + d_curv_abs_frames; + + const In1VecCoord &inDeform = + m_fromModel1->read(sofa::core::ConstVecCoordId::position()) + ->getValue(); // Strains + // Compute the tangent Exponential SE3 vectors + this->updateTangExpSE3(inDeform); + + // Get base velocity as input this is also called eta + m_nodesVelocityVectors.clear(); + + // Get base velocity and convert to Vec6, for the facility of computation + Vec6 baseVelocity; // + for (auto u = 0; u < 6; u++) + baseVelocity[u] = in2_vel[baseIndex][u]; + + // Apply the local transform i.e. from SOFA's frame to Cosserat's frame + const In2VecCoord &xfrom2Data = + m_fromModel2->read(sofa::core::ConstVecCoordId::position())->getValue(); + Transform TInverse = Transform(xfrom2Data[baseIndex].getCenter(), + xfrom2Data[baseIndex].getOrientation()) + .inversed(); + Mat6x6 P = this->buildProjector(TInverse); + Vec6 baseLocalVelocity = + P * baseVelocity; // This is the base velocity in Locale frame + m_nodesVelocityVectors.push_back(baseLocalVelocity); if (d_debug.getValue()) - std::cout << "Node velocity : " << i << " = " << eta_node_i << std::endl; - } - - const OutVecCoord &out = - m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); - auto sz = curv_abs_frames.size(); - out_vel.resize(sz); - for (unsigned int i = 0; i < sz; i++) { - Transform Trans = m_framesExponentialSE3Vectors[i].inversed(); - TangentTransform - Adjoint; ///< the class insure that the constructed adjoint is zeroed. - Adjoint.clear(); - this->computeAdjoint(Trans, Adjoint); - Vec6 frame_Xi_dot; - - for (auto u = 0; u < 3; u++) { - frame_Xi_dot(u) = in1_vel[m_indicesVectors[i] - 1][u]; - frame_Xi_dot(u + 3) = 0.; + std::cout << "Base local Velocity :" << baseLocalVelocity << std::endl; + + // Compute velocity at nodes + for (unsigned int i = 1; i < curv_abs_section.size(); i++) { + Transform Trans = m_nodesExponentialSE3Vectors[i].inversed(); + TangentTransform Adjoint; + Adjoint.clear(); + this->computeAdjoint(Trans, Adjoint); + + /// The null vector is replace by the linear velocity in Vec6Type + Vec6 Xi_dot = Vec6(in1_vel[i - 1], Vec3(0.0, 0.0, 0.0)); + + Vec6 eta_node_i = Adjoint * (m_nodesVelocityVectors[i - 1] + + m_nodesTangExpVectors[i] * Xi_dot); + m_nodesVelocityVectors.push_back(eta_node_i); + if (d_debug.getValue()) + std::cout << "Node velocity : " << i << " = " << eta_node_i << std::endl; } - Vec6 eta_frame_i = - Adjoint * (m_nodesVelocityVectors[m_indicesVectors[i] - 1] + - m_framesTangExpVectors[i] * frame_Xi_dot); // eta - auto T = Transform(out[i].getCenter(), out[i].getOrientation()); - Mat6x6 Proj = this->buildProjector(T); + const OutVecCoord &out = + m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); + auto sz = curv_abs_frames.size(); + out_vel.resize(sz); + for (unsigned int i = 0; i < sz; i++) { + Transform Trans = m_framesExponentialSE3Vectors[i].inversed(); + TangentTransform + Adjoint; ///< the class insure that the constructed adjoint is zeroed. + Adjoint.clear(); + this->computeAdjoint(Trans, Adjoint); + Vec6 frame_Xi_dot; + + for (auto u = 0; u < 3; u++) { + frame_Xi_dot(u) = in1_vel[m_indicesVectors[i] - 1][u]; + frame_Xi_dot(u + 3) = 0.; + } + Vec6 eta_frame_i = + Adjoint * (m_nodesVelocityVectors[m_indicesVectors[i] - 1] + + m_framesTangExpVectors[i] * frame_Xi_dot); // eta - out_vel[i] = Proj * eta_frame_i; + auto T = Transform(out[i].getCenter(), out[i].getOrientation()); + Mat6x6 Proj = this->buildProjector(T); - if (d_debug.getValue()) - std::cout << "Frame velocity : " << i << " = " << eta_frame_i - << std::endl; - } - dataVecOutVel[0]->endEdit(); - m_indexInput = 0; + out_vel[i] = Proj * eta_frame_i; + + if (d_debug.getValue()) + std::cout << "Frame velocity : " << i << " = " << eta_frame_i + << std::endl; + } + dataVecOutVel[0]->endEdit(); + m_indexInput = 0; } template void DiscreteCosseratMapping::applyJT( - const sofa::core::MechanicalParams * /*mparams*/, - const vector &dataVecOut1Force, - const vector &dataVecOut2Force, - const vector &dataVecInForce) { - - if (dataVecOut1Force.empty() || dataVecInForce.empty() || - dataVecOut2Force.empty()) - return; - - if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) - return; - - if (d_debug.getValue()) - std::cout << " ########## ApplyJT force Function ########" << std::endl; - const OutVecDeriv &in = dataVecInForce[0]->getValue(); - - In1VecDeriv &out1 = *dataVecOut1Force[0]->beginEdit(); - In2VecDeriv &out2 = *dataVecOut2Force[0]->beginEdit(); - const auto baseIndex = d_baseIndex.getValue(); - - const OutVecCoord &frame = - m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); - const In1DataVecCoord *x1fromData = - m_fromModel1->read(sofa::core::ConstVecCoordId::position()); - const In1VecCoord x1from = x1fromData->getValue(); - vector local_F_Vec; - local_F_Vec.clear(); - - out1.resize(x1from.size()); - - // convert the input from Deriv type to vec6 type, for the purpose of the - // matrix vector multiplication - for (unsigned int var = 0; var < in.size(); ++var) { - Vec6 vec; - for (unsigned j = 0; j < 6; j++) - vec[j] = in[var][j]; - // Convert input from global frame(SOFA) to local frame - Transform _T = - Transform(frame[var].getCenter(), frame[var].getOrientation()); - Mat6x6 P_trans = (this->buildProjector(_T)); - P_trans.transpose(); - Vec6 local_F = P_trans * vec; - local_F_Vec.push_back(local_F); - } - - // Compute output forces - auto sz = m_indicesVectors.size(); - auto index = m_indicesVectors[sz - 1]; - m_totalBeamForceVectors.clear(); - m_totalBeamForceVectors.resize(sz); - - Vec6 F_tot; - F_tot.clear(); - m_totalBeamForceVectors.push_back(F_tot); - - Mat3x6 matB_trans; - matB_trans.clear(); - for (unsigned int k = 0; k < 3; k++) - matB_trans[k][k] = 1.0; - - for (auto s = sz; s--;) { - Mat6x6 coAdjoint; - - this->computeCoAdjoint( - m_framesExponentialSE3Vectors[s], - coAdjoint); // m_framesExponentialSE3Vectors[s] computed in apply - Vec6 node_F_Vec = coAdjoint * local_F_Vec[s]; - Mat6x6 temp = - m_framesTangExpVectors[s]; // m_framesTangExpVectors[s] computed in - // applyJ (here we transpose) - temp.transpose(); - Vec3 f = matB_trans * temp * node_F_Vec; - - if (index != m_indicesVectors[s]) { - index--; - // bring F_tot to the reference of the new beam - this->computeCoAdjoint( - m_nodesExponentialSE3Vectors[index], - coAdjoint); // m_nodesExponentialSE3Vectors computed in apply - F_tot = coAdjoint * F_tot; - Mat6x6 temp = m_nodesTangExpVectors[index]; - temp.transpose(); - // apply F_tot to the new beam - Vec3 temp_f = matB_trans * temp * F_tot; - out1[index - 1] += temp_f; - } - if (d_debug.getValue()) - std::cout << "f at s =" << s << " and index" << index << " is : " << f - << std::endl; + const sofa::core::MechanicalParams * /*mparams*/, + const vector &dataVecOut1Force, + const vector &dataVecOut2Force, + const vector &dataVecInForce) { - // compute F_tot - F_tot += node_F_Vec; - out1[m_indicesVectors[s] - 1] += f; - } + if (dataVecOut1Force.empty() || dataVecInForce.empty() || + dataVecOut2Force.empty()) + return; - Transform frame0 = Transform(frame[0].getCenter(), frame[0].getOrientation()); - Mat6x6 M = this->buildProjector(frame0); - out2[baseIndex] += M * F_tot; + if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) + return; - if (d_debug.getValue()) { - std::cout << "Node forces " << out1 << std::endl; - std::cout << "base Force: " << out2[baseIndex] << std::endl; - } + if (d_debug.getValue()) + std::cout << " ########## ApplyJT force Function ########" << std::endl; + const OutVecDeriv &in = dataVecInForce[0]->getValue(); + + In1VecDeriv &out1 = *dataVecOut1Force[0]->beginEdit(); + In2VecDeriv &out2 = *dataVecOut2Force[0]->beginEdit(); + const auto baseIndex = d_baseIndex.getValue(); + + const OutVecCoord &frame = + m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); + const In1DataVecCoord *x1fromData = + m_fromModel1->read(sofa::core::ConstVecCoordId::position()); + const In1VecCoord x1from = x1fromData->getValue(); + vector local_F_Vec; + local_F_Vec.clear(); + + out1.resize(x1from.size()); + + // convert the input from Deriv type to vec6 type, for the purpose of the + // matrix vector multiplication + for (unsigned int var = 0; var < in.size(); ++var) { + Vec6 vec; + for (unsigned j = 0; j < 6; j++) + vec[j] = in[var][j]; + // Convert input from global frame(SOFA) to local frame + Transform _T = + Transform(frame[var].getCenter(), frame[var].getOrientation()); + Mat6x6 P_trans = (this->buildProjector(_T)); + P_trans.transpose(); + Vec6 local_F = P_trans * vec; + local_F_Vec.push_back(local_F); + } - dataVecOut1Force[0]->endEdit(); - dataVecOut2Force[0]->endEdit(); -} + // Compute output forces + auto sz = m_indicesVectors.size(); + auto index = m_indicesVectors[sz - 1]; + m_totalBeamForceVectors.clear(); + m_totalBeamForceVectors.resize(sz); -template -void DiscreteCosseratMapping::applyJT( - const sofa::core::ConstraintParams * /*cparams*/, - const vector &dataMatOut1Const, - const vector &dataMatOut2Const, - const vector &dataMatInConst) { - if (dataMatOut1Const.empty() || dataMatOut2Const.empty() || - dataMatInConst.empty()) - return; - - if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) - return; - - if (d_debug.getValue()) - std::cout << " ########## ApplyJT Constraint Function ########" - << std::endl; - // We need only one input In model and input Root model (if present) - In1MatrixDeriv &out1 = - *dataMatOut1Const[0]->beginEdit(); // constraints on the strain space - // (reduced coordinate) - In2MatrixDeriv &out2 = - *dataMatOut2Const[0] - ->beginEdit(); // constraints on the reference frame (base frame) - const OutMatrixDeriv &in = - dataMatInConst[0] - ->getValue(); // input constraints defined on the mapped frames - - const OutVecCoord &frame = - m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); - const In1DataVecCoord *x1fromData = - m_fromModel1->read(sofa::core::ConstVecCoordId::position()); - const In1VecCoord x1from = x1fromData->getValue(); - - Mat3x6 matB_trans; - matB_trans.clear(); - for (unsigned int k = 0; k < 3; k++) - matB_trans[k][k] = 1.0; - - vector> NodesInvolved; - vector> NodesInvolvedCompressed; - // helper::vector NodesConstraintDirection; - - typename OutMatrixDeriv::RowConstIterator rowItEnd = in.end(); - - for (typename OutMatrixDeriv::RowConstIterator rowIt = in.begin(); - rowIt != rowItEnd; ++rowIt) { - if (d_debug.getValue()) { - std::cout << "************* Apply JT (MatrixDeriv) iteration on line "; - std::cout << rowIt.index(); - std::cout << "************* " << std::endl; - } - typename OutMatrixDeriv::ColConstIterator colIt = rowIt.begin(); - typename OutMatrixDeriv::ColConstIterator colItEnd = rowIt.end(); - - // Creates a constraints if the input constraint is not empty. - if (colIt == colItEnd) { - if (d_debug.getValue()) { - std::cout << "no column for this constraint" << std::endl; - } - continue; - } - typename In1MatrixDeriv::RowIterator o1 = - out1.writeLine(rowIt.index()); // we store the constraint number - typename In2MatrixDeriv::RowIterator o2 = out2.writeLine(rowIt.index()); - - NodesInvolved.clear(); - while (colIt != colItEnd) { - int childIndex = colIt.index(); - - const OutDeriv valueConst_ = colIt.val(); - Vec6 valueConst; - for (unsigned j = 0; j < 6; j++) - valueConst[j] = valueConst_[j]; - - int indexBeam = m_indicesVectors[childIndex]; - - Transform _T = Transform(frame[childIndex].getCenter(), - frame[childIndex].getOrientation()); - Mat6x6 P_trans = (this->buildProjector(_T)); - P_trans.transpose(); - - Mat6x6 coAdjoint; - this->computeCoAdjoint( - m_framesExponentialSE3Vectors[childIndex], - coAdjoint); // m_framesExponentialSE3Vectors[s] computed in apply - Mat6x6 temp = - m_framesTangExpVectors[childIndex]; // m_framesTangExpVectors[s] - // computed in applyJ - // (here we transpose) - temp.transpose(); - - Vec6 local_F = - coAdjoint * P_trans * - valueConst; // constraint direction in local frame of the beam. - - Vec3 f = matB_trans * temp * - local_F; // constraint direction in the strain space. - - o1.addCol(indexBeam - 1, f); - std::tuple test = std::make_tuple(indexBeam, local_F); - - NodesInvolved.push_back(test); - colIt++; - } - if (d_debug.getValue()) { - std::cout << "==> NodesInvolved : " << std::endl; - for (size_t i = 0; i < NodesInvolved.size(); i++) - std::cout << "index :" << get<0>(NodesInvolved[i]) - << " force :" << get<1>(NodesInvolved[i]) << "\n "; - } + Vec6 F_tot; + F_tot.clear(); + m_totalBeamForceVectors.push_back(F_tot); - // sort the Nodes Invoved by decreasing order - std::sort( - begin(NodesInvolved), end(NodesInvolved), - [](std::tuple const &t1, std::tuple const &t2) { - return std::get<0>(t1) > std::get<0>(t2); // custom compare function - }); + Mat3x6 matB_trans; + matB_trans.clear(); + for (unsigned int k = 0; k < 3; k++) + matB_trans[k][k] = 1.0; - NodesInvolvedCompressed.clear(); - - for (unsigned n = 0; n < NodesInvolved.size(); n++) { - std::tuple test_i = NodesInvolved[n]; - int numNode_i = std::get<0>(test_i); - Vec6 cumulativeF = std::get<1>(test_i); - - if (n < NodesInvolved.size() - 1) { - std::tuple test_i1 = NodesInvolved[n + 1]; - int numNode_i1 = std::get<0>(test_i1); - - while (numNode_i == numNode_i1) { - cumulativeF += std::get<1>(test_i1); - //// This was if ((n!=NodesInvolved.size()-2)||(n==0)) before and I - /// change it to - /// if ((n!=NodesInvolved.size()-1)||(n==0)) since the code can't - /// leave the will loop - if ((n != NodesInvolved.size() - 1) || (n == 0)) { - n++; - break; - } - test_i1 = NodesInvolved[n + 1]; - numNode_i1 = std::get<0>(test_i1); + for (auto s = sz; s--;) { + Mat6x6 coAdjoint; + + this->computeCoAdjoint( + m_framesExponentialSE3Vectors[s], + coAdjoint); // m_framesExponentialSE3Vectors[s] computed in apply + Vec6 node_F_Vec = coAdjoint * local_F_Vec[s]; + Mat6x6 temp = + m_framesTangExpVectors[s]; // m_framesTangExpVectors[s] computed in + // applyJ (here we transpose) + temp.transpose(); + Vec3 f = matB_trans * temp * node_F_Vec; + + if (index != m_indicesVectors[s]) { + index--; + // bring F_tot to the reference of the new beam + this->computeCoAdjoint( + m_nodesExponentialSE3Vectors[index], + coAdjoint); // m_nodesExponentialSE3Vectors computed in apply + F_tot = coAdjoint * F_tot; + Mat6x6 temp = m_nodesTangExpVectors[index]; + temp.transpose(); + // apply F_tot to the new beam + Vec3 temp_f = matB_trans * temp * F_tot; + out1[index - 1] += temp_f; } - } - NodesInvolvedCompressed.push_back( - std::make_tuple(numNode_i, cumulativeF)); + if (d_debug.getValue()) + std::cout << "f at s =" << s << " and index" << index << " is : " << f + << std::endl; + + // compute F_tot + F_tot += node_F_Vec; + out1[m_indicesVectors[s] - 1] += f; } + Transform frame0 = Transform(frame[0].getCenter(), frame[0].getOrientation()); + Mat6x6 M = this->buildProjector(frame0); + out2[baseIndex] += M * F_tot; + if (d_debug.getValue()) { - std::cout << " NodesInvolved after sort and compress" << std::endl; - for (size_t i = 0; i < NodesInvolvedCompressed.size(); i++) - std::cout << "index :" << get<0>(NodesInvolvedCompressed[i]) - << " force :" << get<1>(NodesInvolvedCompressed[i]) << "\n "; + std::cout << "Node forces " << out1 << std::endl; + std::cout << "base Force: " << out2[baseIndex] << std::endl; } - for (unsigned n = 0; n < NodesInvolvedCompressed.size(); n++) { - std::tuple test = NodesInvolvedCompressed[n]; - int numNode = std::get<0>(test); - int i = numNode; - Vec6 CumulativeF = std::get<1>(test); + dataVecOut1Force[0]->endEdit(); + dataVecOut2Force[0]->endEdit(); +} - while (i > 0) { - // cumulate on beam frame - Mat6x6 coAdjoint; - this->computeCoAdjoint( - m_nodesExponentialSE3Vectors[i - 1], - coAdjoint); // m_nodesExponentialSE3Vectors computed in apply - CumulativeF = coAdjoint * CumulativeF; - // transfer to strain space (local coordinates) - Mat6x6 temp = m_nodesTangExpVectors[i - 1]; - temp.transpose(); - Vec3 temp_f = matB_trans * temp * CumulativeF; +template +void DiscreteCosseratMapping::applyJT( + const sofa::core::ConstraintParams * /*cparams*/, + const vector &dataMatOut1Const, + const vector &dataMatOut2Const, + const vector &dataMatInConst) { + if (dataMatOut1Const.empty() || dataMatOut2Const.empty() || + dataMatInConst.empty()) + return; - if (i > 1) - o1.addCol(i - 2, temp_f); - i--; - } + if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) + return; - Transform frame0 = - Transform(frame[0].getCenter(), frame[0].getOrientation()); - Mat6x6 M = this->buildProjector(frame0); + if (d_debug.getValue()) + std::cout << " ########## ApplyJT Constraint Function ########" + << std::endl; + // We need only one input In model and input Root model (if present) + In1MatrixDeriv &out1 = + *dataMatOut1Const[0]->beginEdit(); // constraints on the strain space + // (reduced coordinate) + In2MatrixDeriv &out2 = + *dataMatOut2Const[0] + ->beginEdit(); // constraints on the reference frame (base frame) + const OutMatrixDeriv &in = + dataMatInConst[0] + ->getValue(); // input constraints defined on the mapped frames + + const OutVecCoord &frame = + m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); + const In1DataVecCoord *x1fromData = + m_fromModel1->read(sofa::core::ConstVecCoordId::position()); + const In1VecCoord x1from = x1fromData->getValue(); + + Mat3x6 matB_trans; + matB_trans.clear(); + for (unsigned int k = 0; k < 3; k++) + matB_trans[k][k] = 1.0; + + vector> NodesInvolved; + vector> NodesInvolvedCompressed; + // helper::vector NodesConstraintDirection; + + typename OutMatrixDeriv::RowConstIterator rowItEnd = in.end(); + + for (typename OutMatrixDeriv::RowConstIterator rowIt = in.begin(); + rowIt != rowItEnd; ++rowIt) { + if (d_debug.getValue()) { + std::cout << "************* Apply JT (MatrixDeriv) iteration on line "; + std::cout << rowIt.index(); + std::cout << "************* " << std::endl; + } + typename OutMatrixDeriv::ColConstIterator colIt = rowIt.begin(); + typename OutMatrixDeriv::ColConstIterator colItEnd = rowIt.end(); + + // Creates a constraints if the input constraint is not empty. + if (colIt == colItEnd) { + if (d_debug.getValue()) { + std::cout << "no column for this constraint" << std::endl; + } + continue; + } + typename In1MatrixDeriv::RowIterator o1 = + out1.writeLine(rowIt.index()); // we store the constraint number + typename In2MatrixDeriv::RowIterator o2 = out2.writeLine(rowIt.index()); + + NodesInvolved.clear(); + while (colIt != colItEnd) { + int childIndex = colIt.index(); + + const OutDeriv valueConst_ = colIt.val(); + Vec6 valueConst; + for (unsigned j = 0; j < 6; j++) + valueConst[j] = valueConst_[j]; + + int indexBeam = m_indicesVectors[childIndex]; + + Transform _T = Transform(frame[childIndex].getCenter(), + frame[childIndex].getOrientation()); + Mat6x6 P_trans = (this->buildProjector(_T)); + P_trans.transpose(); + + Mat6x6 coAdjoint; + this->computeCoAdjoint( + m_framesExponentialSE3Vectors[childIndex], + coAdjoint); // m_framesExponentialSE3Vectors[s] computed in apply + Mat6x6 temp = + m_framesTangExpVectors[childIndex]; // m_framesTangExpVectors[s] + // computed in applyJ + // (here we transpose) + temp.transpose(); + + Vec6 local_F = + coAdjoint * P_trans * + valueConst; // constraint direction in local frame of the beam. + + Vec3 f = matB_trans * temp * + local_F; // constraint direction in the strain space. + + o1.addCol(indexBeam - 1, f); + std::tuple test = std::make_tuple(indexBeam, local_F); + + NodesInvolved.push_back(test); + colIt++; + } + if (d_debug.getValue()) { + std::cout << "==> NodesInvolved : " << std::endl; + for (size_t i = 0; i < NodesInvolved.size(); i++) + std::cout << "index :" << get<0>(NodesInvolved[i]) + << " force :" << get<1>(NodesInvolved[i]) << "\n "; + } + + // sort the Nodes Invoved by decreasing order + std::sort( + begin(NodesInvolved), end(NodesInvolved), + [](std::tuple const &t1, std::tuple const &t2) { + return std::get<0>(t1) > std::get<0>(t2); // custom compare function + }); + + NodesInvolvedCompressed.clear(); + + for (unsigned n = 0; n < NodesInvolved.size(); n++) { + std::tuple test_i = NodesInvolved[n]; + int numNode_i = std::get<0>(test_i); + Vec6 cumulativeF = std::get<1>(test_i); + + if (n < NodesInvolved.size() - 1) { + std::tuple test_i1 = NodesInvolved[n + 1]; + int numNode_i1 = std::get<0>(test_i1); + + while (numNode_i == numNode_i1) { + cumulativeF += std::get<1>(test_i1); + //// This was if ((n!=NodesInvolved.size()-2)||(n==0)) before and I + /// change it to + /// if ((n!=NodesInvolved.size()-1)||(n==0)) since the code can't + /// leave the will loop + if ((n != NodesInvolved.size() - 1) || (n == 0)) { + n++; + break; + } + test_i1 = NodesInvolved[n + 1]; + numNode_i1 = std::get<0>(test_i1); + } + } + NodesInvolvedCompressed.push_back( + std::make_tuple(numNode_i, cumulativeF)); + } + + if (d_debug.getValue()) { + std::cout << " NodesInvolved after sort and compress" << std::endl; + for (size_t i = 0; i < NodesInvolvedCompressed.size(); i++) + std::cout << "index :" << get<0>(NodesInvolvedCompressed[i]) + << " force :" << get<1>(NodesInvolvedCompressed[i]) << "\n "; + } - Vec6 base_force = M * CumulativeF; - o2.addCol(d_baseIndex.getValue(), base_force); + for (unsigned n = 0; n < NodesInvolvedCompressed.size(); n++) { + std::tuple test = NodesInvolvedCompressed[n]; + int numNode = std::get<0>(test); + int i = numNode; + Vec6 CumulativeF = std::get<1>(test); + + while (i > 0) { + // cumulate on beam frame + Mat6x6 coAdjoint; + this->computeCoAdjoint( + m_nodesExponentialSE3Vectors[i - 1], + coAdjoint); // m_nodesExponentialSE3Vectors computed in apply + CumulativeF = coAdjoint * CumulativeF; + // transfer to strain space (local coordinates) + Mat6x6 temp = m_nodesTangExpVectors[i - 1]; + temp.transpose(); + Vec3 temp_f = matB_trans * temp * CumulativeF; + + if (i > 1) + o1.addCol(i - 2, temp_f); + i--; + } + + Transform frame0 = + Transform(frame[0].getCenter(), frame[0].getOrientation()); + Mat6x6 M = this->buildProjector(frame0); + + Vec6 base_force = M * CumulativeF; + o2.addCol(d_baseIndex.getValue(), base_force); + } } - } - //"""END ARTICULATION SYSTEM MAPPING""" - dataMatOut1Const[0]->endEdit(); - dataMatOut2Const[0]->endEdit(); + //"""END ARTICULATION SYSTEM MAPPING""" + dataMatOut1Const[0]->endEdit(); + dataMatOut2Const[0]->endEdit(); } template void DiscreteCosseratMapping::computeBBox( - const sofa::core::ExecParams *, bool) { - const OutVecCoord &x = - m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); - - SReal minBBox[3] = {std::numeric_limits::max(), - std::numeric_limits::max(), - std::numeric_limits::max()}; - SReal maxBBox[3] = {-std::numeric_limits::max(), - -std::numeric_limits::max(), - -std::numeric_limits::max()}; - for (std::size_t i = 0; i < x.size(); i++) { - const OutCoord &p = x[i]; - for (int c = 0; c < 3; c++) { - if (p[c] > maxBBox[c]) - maxBBox[c] = p[c]; - if (p[c] < minBBox[c]) - minBBox[c] = p[c]; + const sofa::core::ExecParams *, bool) { + const OutVecCoord &x = + m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); + + SReal minBBox[3] = {std::numeric_limits::max(), + std::numeric_limits::max(), + std::numeric_limits::max()}; + SReal maxBBox[3] = {-std::numeric_limits::max(), + -std::numeric_limits::max(), + -std::numeric_limits::max()}; + for (std::size_t i = 0; i < x.size(); i++) { + const OutCoord &p = x[i]; + for (int c = 0; c < 3; c++) { + if (p[c] > maxBBox[c]) + maxBBox[c] = p[c]; + if (p[c] < minBBox[c]) + minBBox[c] = p[c]; + } } - } - this->f_bbox.setValue(sofa::type::TBoundingBox(minBBox, maxBBox)); + this->f_bbox.setValue(sofa::type::TBoundingBox(minBBox, maxBBox)); } template -void DiscreteCosseratMapping::draw( - const sofa::core::visual::VisualParams *vparams) { - if (!vparams->displayFlags().getShowMechanicalMappings()) - return; - - // draw cable - typedef RGBAColor RGBAColor; - - const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); - - const OutDataVecCoord *xfromData = - m_toModel->read(sofa::core::ConstVecCoordId::position()); - const OutVecCoord xData = xfromData->getValue(); - vector positions; - vector> Orientation; - positions.clear(); - Orientation.clear(); - unsigned int sz = xData.size(); - for (unsigned int i = 0; i < sz; i++) { - positions.push_back(xData[i].getCenter()); - Orientation.push_back(xData[i].getOrientation()); - } - - // Get access articulated - const In1DataVecCoord *artiData = - m_fromModel1->read(sofa::core::ConstVecCoordId::position()); - const In1VecCoord xPos = artiData->getValue(); - - RGBAColor drawColor = d_color.getValue(); - // draw each segment of the beam as a cylinder. - for (unsigned int i = 0; i < sz - 1; i++) - vparams->drawTool()->drawCylinder(positions[i], positions[i + 1], - d_radius.getValue(), drawColor); - - // Define color map - SReal min = d_min.getValue(); - SReal max = d_max.getValue(); - sofa::helper::ColorMap::evaluator _eval = - m_colorMap.getEvaluator(min, max); - - glLineWidth(d_radius.getValue()); - glBegin(GL_LINES); - if (d_drawMapBeam.getValue()) { - sofa::type::RGBAColor _color = d_color.getValue(); - RGBAColor colorL = RGBAColor(_color[0], _color[1], _color[2], _color[3]); - glColor4f(colorL[0], colorL[1], colorL[2], colorL[3]); - for (unsigned int i = 0; i < sz - 1; i++) { - vparams->drawTool()->drawLine(positions[i], positions[i + 1], colorL); +void DiscreteCosseratMapping::draw(const sofa::core::visual::VisualParams *vparams) +{ + if (!vparams->displayFlags().getShowMechanicalMappings()) + return; + + // draw cable + typedef RGBAColor RGBAColor; + + const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); + + const OutDataVecCoord *xfromData = + m_toModel->read(sofa::core::ConstVecCoordId::position()); + const OutVecCoord xData = xfromData->getValue(); + vector positions; + vector> Orientation; + positions.clear(); + Orientation.clear(); + unsigned int sz = xData.size(); + for (unsigned int i = 0; i < sz; i++) { + positions.push_back(xData[i].getCenter()); + Orientation.push_back(xData[i].getOrientation()); } - } else { - int j = 0; - vector index = d_index.getValue(); - for (unsigned int i = 0; i < sz - 1; i++) { - j = m_indicesVectorsDraw[i] - - 1; // to get the articulation on which the frame is related to - RGBAColor color = _eval(xPos[j][d_deformationAxis.getValue()]); - vparams->drawTool()->drawLine(positions[i], positions[i + 1], color); + + // Get access articulated + const In1DataVecCoord *artiData = + m_fromModel1->read(sofa::core::ConstVecCoordId::position()); + const In1VecCoord xPos = artiData->getValue(); + + RGBAColor drawColor = d_color.getValue(); + // draw each segment of the beam as a cylinder. + for (unsigned int i = 0; i < sz - 1; i++) + vparams->drawTool()->drawCylinder(positions[i], positions[i + 1], + d_radius.getValue(), drawColor); + + // Define color map + SReal min = d_min.getValue(); + SReal max = d_max.getValue(); + sofa::helper::ColorMap::evaluator _eval = + m_colorMap.getEvaluator(min, max); + + glLineWidth(d_radius.getValue()); + glBegin(GL_LINES); + if (d_drawMapBeam.getValue()) { + sofa::type::RGBAColor _color = d_color.getValue(); + RGBAColor colorL = RGBAColor(_color[0], _color[1], _color[2], _color[3]); + glColor4f(colorL[0], colorL[1], colorL[2], colorL[3]); + for (unsigned int i = 0; i < sz - 1; i++) { + vparams->drawTool()->drawLine(positions[i], positions[i + 1], colorL); + } + } else { + int j = 0; + vector index = d_index.getValue(); + for (unsigned int i = 0; i < sz - 1; i++) { + j = m_indicesVectorsDraw[i] - + 1; // to get the articulation on which the frame is related to + RGBAColor color = _eval(xPos[j][d_deformationAxis.getValue()]); + vparams->drawTool()->drawLine(positions[i], positions[i + 1], color); + } } - } - glLineWidth(1); - if (!vparams->displayFlags().getShowMappings()) - if (!d_debug.getValue()) - return; - glEnd(); + glLineWidth(1); + if (!vparams->displayFlags().getShowMappings()) + if (!d_debug.getValue()) + return; + glEnd(); } } // namespace Cosserat::mapping diff --git a/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h b/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h index 94f51f3b..272bdfb6 100644 --- a/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h +++ b/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h @@ -145,7 +145,7 @@ class DiscreteDynamicCosseratMapping : public BaseCosseratMapping::applyJT( dataMatOut2Const[0]->endEdit(); } -template -void DiscreteDynamicCosseratMapping::draw(const sofa::core::visual::VisualParams* vparams) -{ - if (!vparams->displayFlags().getShowMappings()) - return; -} - } // namespace sofa::component::mapping diff --git a/src/Cosserat/mapping/LegendrePolynomialsMapping.h b/src/Cosserat/mapping/LegendrePolynomialsMapping.h index 102c2e73..6edb32ff 100644 --- a/src/Cosserat/mapping/LegendrePolynomialsMapping.h +++ b/src/Cosserat/mapping/LegendrePolynomialsMapping.h @@ -81,8 +81,6 @@ class LegendrePolynomialsMapping : public core::Mapping void applyJT(const core::ConstraintParams *cparams, DataMatrixDeriv_t& out, const DataMatrixDeriv_t& in) override; - void draw(const core::visual::VisualParams* vparams) override; - }; #if !defined(SOFA_COSSERAT_CPP_LegendrePolynomialsMapping) diff --git a/src/Cosserat/mapping/LegendrePolynomialsMapping.inl b/src/Cosserat/mapping/LegendrePolynomialsMapping.inl index 1b876301..197928cc 100644 --- a/src/Cosserat/mapping/LegendrePolynomialsMapping.inl +++ b/src/Cosserat/mapping/LegendrePolynomialsMapping.inl @@ -178,12 +178,4 @@ void LegendrePolynomialsMapping::applyJT(const core::ConstraintParams } } - -template -void LegendrePolynomialsMapping::draw(const core::visual::VisualParams* /*vparams*/) -{ - // draw cable - -} - }