diff --git a/BeamAdapter_test/component/controller/InterventionalRadiologyController_test.cpp b/BeamAdapter_test/component/controller/InterventionalRadiologyController_test.cpp index 0d78f9027..3fa2898ee 100644 --- a/BeamAdapter_test/component/controller/InterventionalRadiologyController_test.cpp +++ b/BeamAdapter_test/component/controller/InterventionalRadiologyController_test.cpp @@ -122,7 +122,8 @@ void InterventionalRadiologyController_test::testDefaultInit() " " " " " " - " " + " " + " " " " " " " " diff --git a/BeamAdapter_test/component/model/WireRestShape_test.cpp b/BeamAdapter_test/component/model/WireRestShape_test.cpp index 0e81d0a0b..c41d9f054 100644 --- a/BeamAdapter_test/component/model/WireRestShape_test.cpp +++ b/BeamAdapter_test/component/model/WireRestShape_test.cpp @@ -119,13 +119,14 @@ void WireRestShape_test::testDefaultInit() { std::string scene = "" - " " - " " - " " - " " - " " - " " - " "; + " " + " " + " " + " " + " " + " " + " " + " "; loadScene(scene); @@ -141,15 +142,17 @@ void WireRestShape_test::testParameterInit() { std::string scene = "" - " " - " " - " " - " " - " " - " " - " "; - + " " + " " + " " + " " + " " + " " + " " + " " + " "; + loadScene(scene); WireRestShapeRig3::SPtr wireRShape = this->m_root->get< WireRestShapeRig3 >(sofa::core::objectmodel::BaseContext::SearchDown); @@ -157,12 +160,13 @@ void WireRestShape_test::testParameterInit() EXPECT_NE(wire, nullptr); Real fullLength = wire->getLength(); + Real straightLength = 95.0; EXPECT_EQ(fullLength, 100.0); - Real straightLength = 95.0; + int nbrE0 = 50; + int nbrE1 = 10; vector keysPoints, keysPoints_ref = { 0, straightLength, fullLength }; - Real ratio = straightLength / fullLength; - vector nbP_density, nbP_density_ref = { int(floor(5.0 * ratio)), int(floor(20.0 * (1 - ratio))) }; + vector nbP_density, nbP_density_ref = { nbrE0, nbrE1 }; wire->getSamplingParameters(keysPoints, nbP_density); EXPECT_EQ(keysPoints.size(), 3); @@ -175,9 +179,9 @@ void WireRestShape_test::testParameterInit() wire->getCollisionSampling(dx1, 0.0); wire->getCollisionSampling(dx2, fullLength); wire->getCollisionSampling(dx3, 90.0); - EXPECT_EQ(dx1, straightLength / nbEdgesCol_ref); - EXPECT_EQ(dx2, (fullLength - straightLength) / nbEdgesCol_ref); - EXPECT_EQ(dx3, straightLength / nbEdgesCol_ref); + EXPECT_EQ(dx1, straightLength / nbrE0); + EXPECT_EQ(dx2, (fullLength - straightLength) / nbrE1); + EXPECT_EQ(dx3, straightLength / nbrE0); } @@ -187,8 +191,10 @@ void WireRestShape_test::testTopologyInit() "" " " " " - " " + " " + " " + " " " " " " " " @@ -205,11 +211,12 @@ void WireRestShape_test::testTopologyInit() EXPECT_NE(topo, nullptr); // checking topo created by WireRestShape - int numbEdgesVisu = 30; - EXPECT_EQ(topo->getNbPoints(), numbEdgesVisu + 1); - EXPECT_EQ(topo->getNbEdges(), numbEdgesVisu); + int numbEdgesVisu0 = 20; + int numbEdgesVisu1 = 10; + EXPECT_EQ(topo->getNbPoints(), numbEdgesVisu0 + numbEdgesVisu1 + 1); + EXPECT_EQ(topo->getNbEdges(), numbEdgesVisu0 + numbEdgesVisu1); - Real dx = 100.0 / Real(numbEdgesVisu); + Real dx = 95.0 / Real(numbEdgesVisu0); EXPECT_EQ(topo->getPX(0), 0.0); EXPECT_EQ(topo->getPX(1), dx); @@ -227,8 +234,10 @@ void WireRestShape_test::testTransformMethods() "" " " " " - " " + " " + " " + " " " " " " " " diff --git a/examples/3instruments.html b/examples/3instruments.html index 1361ea6ba..f11bc3233 100644 --- a/examples/3instruments.html +++ b/examples/3instruments.html @@ -9,7 +9,10 @@
Instruments control

- You can use the keyboard to control the instrument. + Recorded motion has been recorded to demonstrate the 3 intruments behavior. +
+
To manually test it, set the component BeamAdapterActionController writeMode to true. +
Then you can use the keyboard to control the instrument:
  • To change instrument
      @@ -18,16 +21,16 @@
    • ctrl + 2
    • -
  • To progress instrument +
  • To move the instrument forward/backward
      -
    • ctrl + + -
    • ctrl + - +
    • ctrl + UP key +
    • ctrl + DOWN key
  • To turn instrument
      -
    • ctrl + A -
    • ctrl + E +
    • ctrl + LEFT key +
    • ctrl + RIGHT key
diff --git a/examples/3instruments.scn b/examples/3instruments.scn index 2e1843149..e9e438d18 100644 --- a/examples/3instruments.scn +++ b/examples/3instruments.scn @@ -15,34 +15,41 @@ - - - + + + + + - + + + + + - - + + + + + + @@ -53,10 +60,10 @@ @@ -69,11 +76,16 @@ - + + + - + diff --git a/examples/3instruments_collis.html b/examples/3instruments_collis.html index 51a7b1b11..f11bc3233 100644 --- a/examples/3instruments_collis.html +++ b/examples/3instruments_collis.html @@ -1,12 +1,42 @@ + + + -

Scene controls

-After launching the simulation ("Animate" button), clic on the viewport to give it the focus.
-Ctrl+[UP/DOWN key] move the tool forward/backward.
-Ctrl+[LEFT/RIGHT key] rotate the tool.
-Ctrl+[0/1/2] switch tool.
- +
+

Beam Adapter 3 instruments interventional radiology

+
+
Instruments control
+
+
+ Recorded motion has been recorded to demonstrate the 3 intruments behavior. +
+
To manually test it, set the component BeamAdapterActionController writeMode to true. +
Then you can use the keyboard to control the instrument: +
    +
  • To change instrument +
      +
    • ctrl + 0 +
    • ctrl + 1 +
    • ctrl + 2 +
    +
    • +
  • To move the instrument forward/backward +
      +
    • ctrl + UP key +
    • ctrl + DOWN key +
    +
    • +
  • To turn instrument +
      +
    • ctrl + LEFT key +
    • ctrl + RIGHT key +
    +
    • +
+
+ +
+
- - diff --git a/examples/3instruments_collis.scn b/examples/3instruments_collis.scn index 4a5511faa..0b37c8b56 100644 --- a/examples/3instruments_collis.scn +++ b/examples/3instruments_collis.scn @@ -1,6 +1,6 @@ - - - + + + @@ -23,138 +23,146 @@ - + - + - + - - + + - - - - - - + + + + + + + + + + - - - - - - + + + + + + + + + + - - - - - - + + + + + + + + + + - - - - + + + - + - - + + - - + + - - + + - + + - + - - + + - - - - - - - + + + + + + + - - - - - - + + + + + + - + - - - + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + - - - diff --git a/examples/SingleBeamDeployment.scn b/examples/SingleBeamDeployment.scn index ca9739617..f29e61142 100644 --- a/examples/SingleBeamDeployment.scn +++ b/examples/SingleBeamDeployment.scn @@ -15,11 +15,10 @@ - + + + + diff --git a/examples/SingleBeamDeploymentCollision.scn b/examples/SingleBeamDeploymentCollision.scn index b455d0f44..ed3a1b887 100644 --- a/examples/SingleBeamDeploymentCollision.scn +++ b/examples/SingleBeamDeploymentCollision.scn @@ -23,20 +23,20 @@ - - - + + + - - + + - + + + + + @@ -57,24 +57,24 @@ startingPos="0 0 0 0 0 0 1" xtip="0 0 0" printLog="1" rotationInstrument="0 0 0" step="5" speed="5" listening="1" controlledInstrument="0"/> - + - - - - + + + + - - - + + + diff --git a/examples/Tool_from_loader.scn b/examples/Tool_from_loader.scn index 544bcba57..e76e8fd76 100644 --- a/examples/Tool_from_loader.scn +++ b/examples/Tool_from_loader.scn @@ -23,11 +23,12 @@ - - + + + + + + diff --git a/regression/references/3instruments_collis.scn.reference_0_DOFs_mstate.txt.gz b/regression/references/3instruments_collis.scn.reference_0_DOFs_mstate.txt.gz index 5ae65366a..25287bcb6 100644 Binary files a/regression/references/3instruments_collis.scn.reference_0_DOFs_mstate.txt.gz and b/regression/references/3instruments_collis.scn.reference_0_DOFs_mstate.txt.gz differ diff --git a/regression/references/3instruments_collis.scn.reference_1_CollisionDOFs_mstate.txt.gz b/regression/references/3instruments_collis.scn.reference_1_CollisionDOFs_mstate.txt.gz index ad60ce246..84512cf85 100644 Binary files a/regression/references/3instruments_collis.scn.reference_1_CollisionDOFs_mstate.txt.gz and b/regression/references/3instruments_collis.scn.reference_1_CollisionDOFs_mstate.txt.gz differ diff --git a/regression/references/3instruments_collis.scn.reference_2_Quads_mstate.txt.gz b/regression/references/3instruments_collis.scn.reference_2_Quads_mstate.txt.gz index be948c7c2..acd07db10 100644 Binary files a/regression/references/3instruments_collis.scn.reference_2_Quads_mstate.txt.gz and b/regression/references/3instruments_collis.scn.reference_2_Quads_mstate.txt.gz differ diff --git a/regression/references/3instruments_collis.scn.reference_3_Quads_mstate.txt.gz b/regression/references/3instruments_collis.scn.reference_3_Quads_mstate.txt.gz index 1c5c8831a..10e5de405 100644 Binary files a/regression/references/3instruments_collis.scn.reference_3_Quads_mstate.txt.gz and b/regression/references/3instruments_collis.scn.reference_3_Quads_mstate.txt.gz differ diff --git a/regression/references/3instruments_collis.scn.reference_4_Quads_mstate.txt.gz b/regression/references/3instruments_collis.scn.reference_4_Quads_mstate.txt.gz index e78fdc8ce..e18d1a110 100644 Binary files a/regression/references/3instruments_collis.scn.reference_4_Quads_mstate.txt.gz and b/regression/references/3instruments_collis.scn.reference_4_Quads_mstate.txt.gz differ diff --git a/regression/references/SingleBeamDeploymentCollision.scn.reference_0_DOFs Container_mstate.txt.gz b/regression/references/SingleBeamDeploymentCollision.scn.reference_0_DOFs Container_mstate.txt.gz index c02ffd128..397f4ad15 100644 Binary files a/regression/references/SingleBeamDeploymentCollision.scn.reference_0_DOFs Container_mstate.txt.gz and b/regression/references/SingleBeamDeploymentCollision.scn.reference_0_DOFs Container_mstate.txt.gz differ diff --git a/regression/references/SingleBeamDeploymentCollision.scn.reference_1_CollisionDOFs_mstate.txt.gz b/regression/references/SingleBeamDeploymentCollision.scn.reference_1_CollisionDOFs_mstate.txt.gz index a5227ee08..2886309b2 100644 Binary files a/regression/references/SingleBeamDeploymentCollision.scn.reference_1_CollisionDOFs_mstate.txt.gz and b/regression/references/SingleBeamDeploymentCollision.scn.reference_1_CollisionDOFs_mstate.txt.gz differ diff --git a/src/BeamAdapter/component/engine/SteerableCatheter.h b/src/BeamAdapter/component/engine/SteerableCatheter.h index 17ceebb98..27819a66a 100644 --- a/src/BeamAdapter/component/engine/SteerableCatheter.h +++ b/src/BeamAdapter/component/engine/SteerableCatheter.h @@ -104,9 +104,10 @@ class SteerableCatheter : public WireRestShape /// Bring inherited attributes and function in the current lookup context. /// otherwise any access to the base::attribute would require /// the "this->" approach. - using Inherit1::d_spireDiameter; - using Inherit1::d_length; - using Inherit1::d_straightLength; + Data d_length; + Data d_straightLength; + Data d_spireDiameter; + Data d_spireHeight; /////////////////////////////////////////////////////////////////////////// }; diff --git a/src/BeamAdapter/component/engine/WireRestShape.cpp b/src/BeamAdapter/component/engine/WireRestShape.cpp index dacba394e..34572378a 100644 --- a/src/BeamAdapter/component/engine/WireRestShape.cpp +++ b/src/BeamAdapter/component/engine/WireRestShape.cpp @@ -52,7 +52,7 @@ using namespace sofa::defaulttype; /// //////////////////////////////////////////////////////////////////////////////////////////////////// -static int WireRestShapeClass = core::RegisterObject("Describe the shape functions on multiple segments using curvilinear abscissa") +const int WireRestShapeClass = core::RegisterObject("Describe the shape functions on multiple segments using curvilinear abscissa") .add< WireRestShape >(true) ; diff --git a/src/BeamAdapter/component/engine/WireRestShape.h b/src/BeamAdapter/component/engine/WireRestShape.h index d027656c5..386c40af2 100644 --- a/src/BeamAdapter/component/engine/WireRestShape.h +++ b/src/BeamAdapter/component/engine/WireRestShape.h @@ -33,7 +33,8 @@ #include -#include +#include + #include #include #include @@ -48,11 +49,15 @@ namespace _wirerestshape_ using sofa::core::topology::TopologyContainer; using sofa::core::loader::MeshLoader; +using namespace sofa::beamadapter; + /** * \class WireRestShape * \brief Describe the shape functions on multiple segments - * - * Describe the shape functions on multiple segments using curvilinear abscissa + * + * Describe the full shape of a Wire with a given set of @sa BaseRodSectionMaterial. The wire is discretized by a set of beams (given by the keyPoints and the relatives Beam density) + * @sa d_keyPoints and @d_density are computed by method @sa initLengths using the set of rod sections description. + * This component compute the beam discretization and the shape functions on multiple segments using curvilinear abscissa. */ template class WireRestShape : public core::objectmodel::BaseObject @@ -66,8 +71,6 @@ class WireRestShape : public core::objectmodel::BaseObject using Vec3 = sofa::type::Vec<3, Real>; using Quat = sofa::type::Quat; - using BeamSection = sofa::beamadapter::BeamSection; - /** * @brief Default Constructor. */ @@ -79,13 +82,10 @@ class WireRestShape : public core::objectmodel::BaseObject virtual ~WireRestShape() = default; /////////////////////////// Inherited from BaseObject ////////////////////////////////////////// - void parse(core::objectmodel::BaseObjectDescription* arg) override; void init() override ; - - void draw(const core::visual::VisualParams * vparams) override ; - /////////////////////////// Methods of WireRestShape ////////////////////////////////////////// + /////////////////////////// Methods of WireRestShape ////////////////////////////////////////// /// This function is called by the force field to evaluate the rest position of each beam void getRestTransformOnX(Transform &global_H_local, const Real &x); @@ -104,18 +104,13 @@ class WireRestShape : public core::objectmodel::BaseObject /// Functions enabling to load and use a geometry given from OBJ external file - void initRestConfig(); - void getRestPosNonProcedural(Real& abs, Coord &p); void computeOrientation(const Vec3& AB, const Quat& Q, Quat &result); - void initFromLoader(); - bool checkTopology(); - - [[nodiscard]] bool fillTopology(); + + Real getLength() ; - void getCollisionSampling(Real &dx, const Real &x_curv) ; + void getCollisionSampling(Real &dx, const Real &x_curv); void getNumberOfCollisionSegment(Real &dx, unsigned int &numLines) ; - void rotateFrameForAlignX(const Quat &input, Vec3 &x, Quat &output); /////////////////////////// Deprecated Methods ////////////////////////////////////////// @@ -132,55 +127,25 @@ class WireRestShape : public core::objectmodel::BaseObject msg_warning() << "Releasing catheter or brokenIn2 mode is not anymore supported. Feature has been removed after release v23.06"; } +protected: + /// Internal method to init Lengths vector @sa d_keyPoints using the length of each materials @sa l_sectionMaterials. + void initLengths(); + /// Internal method to init Edge Topology @sa _topology using the list of materials @sa l_sectionMaterials. Returns false if init can't be performed. + bool initTopology(); + + public: - /// Analitical creation of wire shape... - Data d_isAProceduralShape; - Data d_nonProceduralScale; - Data d_length; - Data d_straightLength; - Data d_spireDiameter; - Data d_spireHeight; Data > d_density; Data > d_keyPoints; - Data< int > d_numEdges; - Data > d_numEdgesCollis; - - /// User Data about the Young modulus - Data d_poissonRatio; - Data d_youngModulus1; - Data d_youngModulus2; - - /// Radius - Data d_radius1; - Data d_radius2; - Data d_innerRadius1; - Data d_innerRadius2; - - Data d_massDensity1; - Data d_massDensity2; - - /// broken in 2 case - Data d_drawRestShape; - -private: - /// Data required for the File loading - type::vector m_localRestPositions; - type::vector m_localRestTransforms; - type::vector m_curvAbs ; - double m_absOfGeometry {0}; - BeamSection beamSection1; - BeamSection beamSection2; + /// Vector or links to the Wire section material. The order of the linked material will define the WireShape structure. + MultiLink, BaseRodSectionMaterial, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_sectionMaterials; +private: /// Link to be set to the topology container in the component graph. SingleLink, TopologyContainer, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology; /// Pointer to the topology container, should be set using @sa l_topology, otherwise will search for one in current Node. TopologyContainer* _topology{ nullptr }; - - /// Link to be set to the topology container in the component graph. - SingleLink, MeshLoader, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_loader; - /// Pointer to the MeshLoader, should be set using @sa l_loader, otherwise will search for one in current Node. - MeshLoader* loader{ nullptr }; }; diff --git a/src/BeamAdapter/component/engine/WireRestShape.inl b/src/BeamAdapter/component/engine/WireRestShape.inl index bd3f5706d..7a2a759fd 100644 --- a/src/BeamAdapter/component/engine/WireRestShape.inl +++ b/src/BeamAdapter/component/engine/WireRestShape.inl @@ -55,82 +55,15 @@ using sofa::core::objectmodel::BaseContext ; * @brief Default Constructor. */ template -WireRestShape::WireRestShape() : - //TODO(dmarchal 2017-05-17) not sure that procedural & nonProceduralScale are very understandable name...are they exclusives ? - //if so have look in my comment in the init section. - d_isAProceduralShape( initData(&d_isAProceduralShape,(bool)true,"isAProceduralShape","is the guidewire shape mathemetically defined ?") ) - , d_nonProceduralScale( initData ( &d_nonProceduralScale, (Real)1.0, "nonProceduralScale", "scale of the model defined by file" ) ) - , d_length(initData(&d_length, (Real)1.0, "length", "total length of the wire instrument")) - , d_straightLength(initData(&d_straightLength, (Real)0.0, "straightLength", "length of the initial straight shape")) - , d_spireDiameter(initData(&d_spireDiameter, (Real)0.1, "spireDiameter", "diameter of the spire")) - , d_spireHeight(initData(&d_spireHeight, (Real)0.01, "spireHeight", "height between each spire")) - , d_density(initData(&d_density, "densityOfBeams", "density of beams between key points")) - , d_keyPoints(initData(&d_keyPoints,"keyPoints","key points of the shape (curv absc)")) - , d_numEdges(initData(&d_numEdges, 10, "numEdges","number of Edges for the visual model")) - , d_numEdgesCollis(initData(&d_numEdgesCollis,"numEdgesCollis", "number of Edges for the collision model" )) - , d_poissonRatio(initData(&d_poissonRatio,(Real)0.49,"poissonRatio","Poisson Ratio")) - , d_youngModulus1(initData(&d_youngModulus1,(Real)5000,"youngModulus","Young Modulus")) - , d_youngModulus2(initData(&d_youngModulus2,(Real)3000,"youngModulusExtremity","youngModulus for beams at the extremity\nonly if not straight")) - , d_radius1(initData(&d_radius1,(Real)1.0f,"radius","radius")) - , d_radius2(initData(&d_radius2,(Real)1.0f,"radiusExtremity","radius for beams at the extremity\nonly if not straight")) - , d_innerRadius1(initData(&d_innerRadius1,(Real)0.0f,"innerRadius","inner radius if it applies")) - , d_innerRadius2(initData(&d_innerRadius2,(Real)0.0f,"innerRadiusExtremity","inner radius for beams at the extremity\nonly if not straight")) - , d_massDensity1(initData(&d_massDensity1,(Real)1.0,"massDensity", "Density of the mass (usually in kg/m^3)" )) - , d_massDensity2(initData(&d_massDensity2,(Real)1.0,"massDensityExtremity", "Density of the mass at the extremity\nonly if not straight" )) - , d_drawRestShape(initData(&d_drawRestShape, (bool)false, "draw", "draw rest shape")) - , l_topology(initLink("topology", "link to the topology container")) - , l_loader(initLink("loader", "link to the MeshLoader")) +WireRestShape::WireRestShape() + : d_density(initData(&d_density, "densityOfBeams", "density of beams between key points")) + , d_keyPoints(initData(&d_keyPoints,"keyPoints","key points of the shape (curv absc)")) + , l_sectionMaterials(initLink("wireMaterials", "link to Wire Section Materials (to be ordered according to the instrument, from handle to tip)")) + , l_topology(initLink("topology", "link to the topology container")) { - d_spireDiameter.setGroup("Procedural"); - d_spireHeight.setGroup("Procedural"); -} - -template -void WireRestShape::rotateFrameForAlignX(const Quat &input, Vec3 &x, Quat &output) -{ - x.normalize(); - Vec3 x0=input.inverseRotate(x); - Real cTheta=x0[0]; - Real theta; - if (cTheta>(1-EPSILON)) - { - output = input; - } - else - { - theta=acos(cTheta); - // axis of rotation - Vec3 dw(0,-x0[2],x0[1]); - dw.normalize(); - - // computation of the rotation - Quat inputRoutput; - inputRoutput.axisToQuat(dw, theta); - - output=input*inputRoutput; - } } -template -void WireRestShape::parse(core::objectmodel::BaseObjectDescription* args) -{ - const char* arg = args->getAttribute("procedural") ; - if(arg) - { - msg_warning() << "The attribute 'procedural' has been renamed into 'isAProceduralShape'. " << msgendl - << "To remove this warning you need to update your scene and replace 'procedural' with 'isAProceduralShape'" ; - - /// As arg is owned by the "procedural" attribute it cannot be removed before - /// being copied in the "isAProceduralShape". So please keep the ordering of the - /// two following functions. - args->setAttribute("isAProceduralShape", arg) ; - args->removeAttribute("procedural") ; - - } - - Inherit1::parse(args) ; -} template void WireRestShape::init() @@ -158,572 +91,229 @@ void WireRestShape::init() return; } - if (!d_isAProceduralShape.getValue()) + if (l_sectionMaterials.empty()) { - // Get meshLoader, check first if loader has been set using link. Otherwise will search in current context. - loader = l_loader.get(); - - if (!loader) - this->getContext()->get(loader); - - if (!loader) { - msg_error() << "Cannot find a mesh loader. Please insert a MeshObjLoader in the same node or use l_loader to specify the path in the scene graph."; - this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); - return; - } - else - { - msg_info() << "Found a mesh with " << loader->d_edges.getValue().size() << " edges"; - initFromLoader(); - } - } - else - { - if (!fillTopology()) - { - msg_error() << "Error while trying to fill the associated topology, setting the state to Invalid"; - this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); - return; - } + msg_error() << "No BaseRodSectionMaterial set. At least one material should be set and link using wireMaterials."; + this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); + return; } - + //////////////////////////////////////////////////////// ////////// keyPoint list and Density Assignement /////// //////////////////////////////////////////////////////// - auto keyPointList = sofa::helper::getWriteOnlyAccessor(d_keyPoints); - if(!keyPointList.size()) - { - keyPointList.push_back(0.0); - if(d_straightLength.getValue()>= 0.001*this->d_length.getValue() && d_straightLength.getValue() <= 0.999*d_length.getValue()) - keyPointList.push_back(d_straightLength.getValue()); - keyPointList.push_back(d_length.getValue()); - } - if( d_density.getValue().size() != keyPointList.size()-1) - { - auto densityList = sofa::helper::getWriteOnlyAccessor(d_density); - - if(densityList.size() > keyPointList.size()-1 ) - densityList.resize(keyPointList.size()-1); - else - { - densityList.clear(); - - if(d_straightLength.getValue()>= 0.001*this->d_length.getValue() ) - { - int numNodes = (int) floor(5.0*d_straightLength.getValue() / d_length.getValue() ); - densityList.push_back(numNodes); - } - if( d_straightLength.getValue() <= 0.999*d_length.getValue()) - { - int numNodes = (int) floor(20.0*(1.0 - d_straightLength.getValue() / d_length.getValue()) ); - densityList.push_back(numNodes); - } - } - } - - if(!d_numEdgesCollis.getValue().size()) - { - auto densityCol = sofa::helper::getWriteOnlyAccessor(d_numEdgesCollis); - densityCol.resize(keyPointList.size()-1); - for (unsigned int i=0; id_radius1.getValue(); - double rInner = this->d_innerRadius1.getValue(); - this->beamSection1._r = r; - this->beamSection1._rInner = rInner; - this->beamSection1._Iz = M_PI*(r*r*r*r - rInner*rInner*rInner*rInner)/4.0; - this->beamSection1._Iy = this->beamSection1._Iz ; - this->beamSection1._J = this->beamSection1._Iz + this->beamSection1._Iy; - this->beamSection1._A = M_PI*(r*r - rInner*rInner); - this->beamSection1._Asy = 0.0; - this->beamSection1._Asz = 0.0; - - r = this->d_radius2.getValue(); - rInner = this->d_innerRadius2.getValue(); - this->beamSection2._r = r; - this->beamSection2._rInner = rInner; - this->beamSection2._Iz = M_PI*(r*r*r*r - rInner*rInner*rInner*rInner)/4.0; - this->beamSection2._Iy = this->beamSection2._Iz ; - this->beamSection2._J = this->beamSection2._Iz + this->beamSection2._Iy; - this->beamSection2._A = M_PI*(r*r - rInner*rInner); - this->beamSection2._Asy = 0.0; - this->beamSection2._Asz = 0.0; + + initTopology(); + this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); + msg_info() << "WireRestShape end init"; } template -void WireRestShape::getSamplingParameters(type::vector& xP_noticeable, - type::vector& nbP_density) const +void WireRestShape::initLengths() { - xP_noticeable = d_keyPoints.getValue(); - nbP_density = d_density.getValue(); -} - -template -void WireRestShape::getCollisionSampling(Real &dx, const Real &x_curv) -{ - unsigned int numLines; - Real x_used = x_curv - EPSILON; - if(x_used>d_length.getValue()) - x_used=d_length.getValue(); - - if(x_used<0.0) - x_used=0.0; - - // verify that size of numEdgesCollis = size of keyPoints-1 - if( d_numEdgesCollis.getValue().size() != d_keyPoints.getValue().size()-1) + auto keyPointList = sofa::helper::getWriteOnlyAccessor(d_keyPoints); + auto densityList = sofa::helper::getWriteOnlyAccessor(d_density); + keyPointList.resize(l_sectionMaterials.size() + 1); + keyPointList[0] = Real(0.0); + densityList.resize(l_sectionMaterials.size()); + + for (unsigned int i = 0; i < l_sectionMaterials.size(); ++i) { - msg_error() << "Problem size of numEdgesCollis ()" << d_numEdgesCollis.getValue().size() << " != size of keyPoints-1 " << d_keyPoints.getValue().size()-1 ; - numLines = (unsigned int)d_numEdgesCollis.getValue()[0]; - dx=d_length.getValue()/numLines; - return; + auto rodSection = l_sectionMaterials.get(i); + keyPointList[i+1] = keyPointList[i] + rodSection->getLength(); + densityList[i] = rodSection->getNbCollisionEdges(); } - - - for (unsigned int i=1; id_keyPoints.getValue().size(); i++) - { - if( x_used < this->d_keyPoints.getValue()[i] ) - { - numLines = (unsigned int)d_numEdgesCollis.getValue()[i-1]; - dx=(this->d_keyPoints.getValue()[i] - this->d_keyPoints.getValue()[i-1])/numLines; - return; - } - } - - dx=d_length.getValue()/20; - msg_error() << " problem is getCollisionSampling : x_curv "< -void WireRestShape::getRestTransformOnX(Transform &global_H_local, const Real &x) +bool WireRestShape::initTopology() { - Real x_used = x - EPSILON; - - if(x_used>d_length.getValue()) - x_used=d_length.getValue(); - - if(x_used<0.0) - x_used=0.0; + /// fill topology : + _topology->clear(); + _topology->cleanup(); - if( x_used < d_straightLength.getValue()) + const type::vector& keyPts = d_keyPoints.getValue(); + if (l_sectionMaterials.size() != keyPts.size() - 1) { - global_H_local.set(Vec3(x_used, 0.0, 0.0 ), Quat()); - return; + msg_error() << "Wrong number of inputs. Component can't be init. Number of input materials: " << l_sectionMaterials.size() << ", should be equal to keyPointList.size()-1. keyPointList.size() is equal to: " << keyPts.size(); + return false; } + + Real prev_length = 0.0; + int prev_edges = 0; + int startPtId = 0; + for (auto i = 0; i < l_sectionMaterials.size(); ++i) + { + // Add topology of the material + int nbrVisuEdges = l_sectionMaterials.get(i)->getNbVisualEdges(); + Real length = fabs(keyPts[i + 1] - keyPts[i]); + Real dx = length / nbrVisuEdges; + + // add points from the material + for (int i = startPtId; i < nbrVisuEdges + 1; i++) { + _topology->addPoint(prev_length + i * dx, 0, 0); + } - if(d_isAProceduralShape.getValue()) - { - Real projetedLength = d_spireDiameter.getValue()*M_PI; - Real lengthSpire=sqrt(d_spireHeight.getValue()*d_spireHeight.getValue() + projetedLength*projetedLength ); - // angle in the z direction - Real phi= atan(d_spireHeight.getValue()/projetedLength); - - Quat Qphi; - Qphi.axisToQuat(Vec3(0,0,1),phi); - - // spire angle (if theta=2*PI, there is a complete spire between startx and x_used) - Real lengthCurve= x_used-d_straightLength.getValue(); - Real numSpire=lengthCurve/lengthSpire; - Real theta= 2*M_PI*numSpire; - - // computation of the Quat - Quat Qtheta; - Qtheta.axisToQuat(Vec3(0,1,0),theta); - Quat newSpireQuat = Qtheta*Qphi; - - - // computation of the position - Real radius=d_spireDiameter.getValue()/2.0; - Vec3 PosEndCurve(radius*sin(theta), numSpire*d_spireHeight.getValue(), radius*(cos(theta)-1) ); - Vec3 SpirePos=PosEndCurve + Vec3(d_straightLength.getValue(),0,0); + // add segments from the material + for (int i = prev_edges; i < prev_edges + nbrVisuEdges; i++) { + _topology->addEdge(i, i + 1); + } - global_H_local.set(SpirePos,newSpireQuat); + prev_length = length; + prev_edges = nbrVisuEdges; + startPtId = 1; // Assume the last point of mat[n] == first point of mat[n+1] } - else - { - x_used = x_used - d_straightLength.getValue(); - x_used = x_used/(d_length.getValue()-d_straightLength.getValue()) * m_absOfGeometry; - Coord p; - this->getRestPosNonProcedural(x_used,p); - Vec3 PosEndCurve = p.getCenter(); - Quat ExtremityQuat = p.getOrientation(); - Vec3 ExtremityPos = PosEndCurve + Vec3(d_straightLength.getValue(),0,0); - - global_H_local.set(ExtremityPos,ExtremityQuat); - } + return true; } template -void WireRestShape::getYoungModulusAtX(const Real& x_curv, Real& youngModulus, Real& cPoisson) const +void WireRestShape::getSamplingParameters(type::vector& xP_noticeable, + type::vector& nbP_density) const { - //Initialization - Real _E1, _E2; - youngModulus = 0.0; - cPoisson = 0.0; - - //Get the two possible values of the Young modulus - _E1 = this->d_youngModulus1.getValue(); - _E2 = this->d_youngModulus2.getValue(); - - //Get User data - cPoisson = this->d_poissonRatio.getValue(); - - //Depending on the position of the beam, determine the Young modulus - if(x_curv <= this->d_straightLength.getValue()) - { - youngModulus = _E1; - } - else - { - if(_E2 == 0.0) - youngModulus = _E1; - else - youngModulus = _E2; - } - return; + xP_noticeable = d_keyPoints.getValue(); + nbP_density = d_density.getValue(); } template -void WireRestShape::getInterpolationParam(const Real& x_curv, Real &_rho, Real &_A, Real &_Iy , Real &_Iz, Real &_Asy, Real &_Asz, Real &_J) const -{ - if(x_curv <= this->d_straightLength.getValue()) - { - if(d_massDensity1.isSet()) - _rho = d_massDensity1.getValue(); - - if(d_radius1.isSet()) - { - _A =beamSection1._A; - _Iy =beamSection1._Iy; - _Iz =beamSection1._Iz; - _Asy =beamSection1._Asy; - _Asz =beamSection1._Asz; - _J =beamSection1._J; - } - } - else - { - if(d_massDensity2.isSet()) - _rho = d_massDensity2.getValue(); - else if(d_massDensity1.isSet()) - _rho = d_massDensity1.getValue(); - - if(d_radius2.isSet()) - { - _A =beamSection2._A; - _Iy =beamSection2._Iy; - _Iz =beamSection2._Iz; - _Asy =beamSection2._Asy; - _Asz =beamSection2._Asz; - _J =beamSection2._J; - } - else if(d_radius1.isSet()) - { - _A =beamSection1._A; - _Iy =beamSection1._Iy; - _Iz =beamSection1._Iz; - _Asy =beamSection1._Asy; - _Asz =beamSection1._Asz; - _J =beamSection1._J; - } - } -} - -template -bool WireRestShape::checkTopology() +void WireRestShape::getCollisionSampling(Real &dx, const Real &x_curv) { - if (!loader->d_edges.getValue().size()) - { - msg_error() << "There is no edges in the topology loaded by " << loader->getName() ; - return false; - } - - if (loader->d_triangles.getValue().size()) - { - msg_error() << "There are triangles in the topology loaded by " << loader->getName() ; - return false; - } + unsigned int numLines; + Real x_used = x_curv - EPSILON; - if (loader->d_quads.getValue().size()) - { - msg_error() << "There are quads in the topology loaded by " << loader->getName() ; - return false; + const Real totalLength = this->getLength(); + if (x_used > totalLength) { + x_used = totalLength; } - - if (loader->d_polygons.getValue().size()) - { - msg_error() << "There are polygons in the topology loaded by " << loader->getName() ; - return false; + else if (x_used < 0.0) { + x_used = 0.0; } - //TODO(dmarchal 2017-05-17) when writing a TODO please specify: - // who will do that - // when it will be done - /// \todo check if the topology is like a wire - - - return true; -} - - -template -bool WireRestShape::fillTopology() -{ - if (!_topology) - { - msg_error() << "Topology is null"; - return false; + const type::vector& keyPts = d_keyPoints.getValue(); + + // verify that size of number of materials == size of keyPoints-1 + if (l_sectionMaterials.size() != keyPts.size() - 1) + { + msg_error() << "Problem size of number of materials: " << l_sectionMaterials.size() + << " != size of keyPoints-1 " << keyPts.size()-1 + << ". Returning default values."; + numLines = 20; + dx = totalLength / numLines; + return; } - - const auto length = this->d_length.getValue(); - if (length <= Real(0.0)) + + // Check in which section x_used belongs to and get access to this section material + for (auto i = 1; i< keyPts.size(); ++i) { - msg_error() << "Length is 0 (or negative), check if d_length has been given or computed."; - return false; - } + if (x_used <= keyPts[i]) + { + numLines = l_sectionMaterials.get(i-1)->getNbCollisionEdges(); - int nbrEdges = d_numEdges.getValue(); - if (nbrEdges <= 0) - { - msg_warning() << "Number of edges has been set to an invalid value: " << nbrEdges << ". Value should be a positive integer. Setting to default value: 10"; - nbrEdges = 10; + Real length = fabs(keyPts[i] - keyPts[i-1]); + dx = length / numLines; + return; + } } - /// fill topology : - _topology->clear(); - _topology->cleanup(); - - Real dx = this->d_length.getValue() / nbrEdges; - - /// add points - for (int i = 0; i < d_numEdges.getValue() + 1; i++) - _topology->addPoint(i * dx, 0, 0); - - /// add segments - for (int i = 0; i < d_numEdges.getValue(); i++) - _topology->addEdge(i, i + 1); - - return true; + // If x_used is out of bounds. Warn user and returns default value. + numLines = 20; + dx = totalLength / numLines; + msg_error() << " problem in getCollisionSampling : x_curv " << x_used << " is not between keyPoints" << d_keyPoints.getValue(); } - template -void WireRestShape::initFromLoader() +void WireRestShape::getRestTransformOnX(Transform &global_H_local, const Real &x) { - if (!checkTopology()) - { - this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); - return; - } - - // this is... dirty but d_straightLength itself is not relevant for loader-created wire - // but the code uses it actively and expects it to not be zero. - if (d_straightLength.getValue() <= 0.0) - { - msg_warning() << "straightLength cannot be 0 (or negative...). Setting a minimum value."; - d_straightLength.setValue(0.0001); - } - - type::vector vertices; - sofa::core::topology::BaseMeshTopology::SeqEdges edges; - - //get the topology position - auto topoVertices = sofa::helper::getReadAccessor(loader->d_positions); - - //copy the topology edges in a local vector - auto topoEdges = sofa::helper::getReadAccessor(loader->d_edges); - edges = topoEdges.ref(); - - /** renumber the vertices **/ - type::vector verticesConnexion; //gives the number of edges connected to a vertex - for(unsigned int i =0; i < topoVertices.size(); i++) - verticesConnexion.push_back(2); + Real x_used = x - EPSILON; - for(const auto& ed : edges) - { - verticesConnexion[ed[0]]--; - verticesConnexion[ed[1]]--; + const Real totalLength = this->getLength(); + if (x_used > totalLength) { + x_used = totalLength; } - - msg_info() << "Successfully compute the vertex connexion" ; - - // check for the first corner of the edge - unsigned int firstIndex = 0; - bool found = false; - while((firstIndex < verticesConnexion.size()) && !found) - { - if(verticesConnexion[firstIndex] == 1) - found = true; - else - firstIndex++; + else if (x_used < 0.0) { + x_used = 0.0; } - - if(firstIndex == verticesConnexion.size()) - { - msg_error() << "The first vertex of the beam structure is not found, probably because of a closed structure" ; - this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); - return; - } - - vertices.push_back(topoVertices[firstIndex]); - - while(edges.size() > 0) + + const type::vector& keyPts = d_keyPoints.getValue(); + for (auto i = 1; i < keyPts.size(); ++i) { - auto it = edges.begin(); - auto end = edges.end(); - - bool notFound = true; - while (notFound && (it != end)) + if (x_used <= keyPts[i]) { - const auto& ed = (*it); - auto toDel = it; - it++; - if(ed[0] == firstIndex) - { - vertices.push_back(topoVertices[ed[1]]); - firstIndex = ed[1]; - edges.erase(toDel); - notFound = false; - - } - else if(ed[1] == firstIndex) - { - vertices.push_back(topoVertices[ed[0]]); - firstIndex = ed[0]; - edges.erase(toDel); - notFound = false; - } + return l_sectionMaterials.get(i - 1)->getRestTransformOnX(global_H_local, x_used, keyPts[i - 1]); } } - msg_info() << "Successfully computed the topology" ; - - m_localRestPositions = vertices; - - for(unsigned int i = 0; i < m_localRestPositions.size() - 1; i++) - m_localRestPositions[i] *= d_nonProceduralScale.getValue(); - - initRestConfig(); - // TODO epernod 2022-08-05: Init from loader seems quite buggy, need to check if this is still needed and working - this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); + msg_error() << " problem in getRestTransformOnX : x_curv " << x_used << " is not between keyPoints" << d_keyPoints.getValue(); } template -void WireRestShape::initRestConfig() +void WireRestShape::getYoungModulusAtX(const Real& x_curv, Real& youngModulus, Real& cPoisson) const { - m_curvAbs.clear(); - double tot = 0; - m_curvAbs.push_back(0); - Quat input, output; - input.identity(); - m_localRestTransforms.resize(m_localRestPositions.size()); - m_localRestTransforms[0].setOrigin(Vec3(0,0,0)); - m_localRestTransforms[0].setOrientation(input); - - for(unsigned int i = 0; i < m_localRestPositions.size() - 1; i++) - { - Vec3 vec = m_localRestPositions[i+1] - m_localRestPositions[i]; - double norm = vec.norm(); - tot += norm; - - this->rotateFrameForAlignX(input, vec, output); - - input = output; - - m_localRestTransforms[i+1].setOrientation(output); - - Vec3 localPos = m_localRestPositions[i+1] - m_localRestPositions[0]; - - m_localRestTransforms[i+1].setOrigin(localPos); - - m_curvAbs.push_back(tot); - } - m_absOfGeometry = tot; - - Real newLength = d_straightLength.getValue() + m_absOfGeometry; - d_length.setValue(newLength); + const Real x_used = x_curv - Real(EPSILON); + const type::vector& keyPts = d_keyPoints.getValue(); - msg_info() <<"Length of the loaded shape = "<< m_absOfGeometry << ", total length with straight length = " << newLength ; - - if (!fillTopology()) + // Depending on the position of the beam, determine the corresponding section material and returning its Young modulus + for (auto i = 1; i < keyPts.size(); ++i) { - msg_error() << "Error while trying to fill the associated topology, setting the state to Invalid"; - this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid); - return; + if (x_used <= keyPts[i]) + { + return l_sectionMaterials.get(i - 1)->getYoungModulusAtX(youngModulus, cPoisson); + } } + + msg_error() << " problem in getYoungModulusAtX : x_curv " << x_curv << " is not between keyPoints" << keyPts; } template -void WireRestShape::getRestPosNonProcedural(Real& abs, Coord &p) +void WireRestShape::getInterpolationParam(const Real& x_curv, Real &_rho, Real &_A, Real &_Iy , Real &_Iz, Real &_Asy, Real &_Asz, Real &_J) const { - /*** find the range which includes the "requested" abs ***/ - double startingAbs = 0; unsigned int index = 0; + const Real x_used = x_curv - Real(EPSILON); + const type::vector& keyPts = d_keyPoints.getValue(); - while ((startingAbs < abs) && (index < m_localRestPositions.size())) + // Check in which section x_used belongs to and get access to this section material + for (auto i = 1; i < keyPts.size(); ++i) { - index++; - startingAbs = m_curvAbs[index]; - } - - /*** OOB ***/ - if(abs > startingAbs) - { - msg_error() << "abs = "<getInterpolationParam(_rho, _A, _Iy, _Iz, _Asy, _Asz, _J); + } } - else /*** Expected case ***/ - { - Real alpha, one_minus_alpha; - Vec3 result; - - alpha = (abs - m_curvAbs[index-1] ) / (m_curvAbs[index] - m_curvAbs[index-1]); - one_minus_alpha = 1 - alpha; - result = m_localRestTransforms[index - 1].getOrigin() * one_minus_alpha + m_localRestTransforms[index].getOrigin() * alpha; - Quat slerp; - slerp.slerp( m_localRestTransforms[index - 1].getOrientation(), m_localRestTransforms[index].getOrientation(), alpha, true ); - - slerp.normalize(); - p.getCenter() = result; - - p.getOrientation() = slerp; - } + msg_error() << " problem in getInterpolationParam : x_curv " << x_curv << " is not between keyPoints" << keyPts; } + template typename WireRestShape::Real WireRestShape::getLength() { - return d_length.getValue(); + return d_keyPoints.getValue().back(); } + template void WireRestShape::getNumberOfCollisionSegment(Real &dx, unsigned int &numLines) { numLines = 0; - for (unsigned i=0; igetNbCollisionEdges(); } - dx=d_length.getValue()/numLines; + dx = getLength() / numLines; } + template void WireRestShape::computeOrientation(const Vec3& AB, const Quat& Q, Quat &result) { @@ -756,26 +346,6 @@ void WireRestShape::computeOrientation(const Vec3& AB, const Quat& Q, } -template -void WireRestShape::draw(const core::visual::VisualParams* vparams) -{ - if (!d_drawRestShape.getValue()) - return; - - vparams->drawTool()->saveLastState(); - vparams->drawTool()->setLightingEnabled(false); - - std::vector< sofa::type::Vec3 > points; - points.reserve(m_localRestPositions.size()); - - for (unsigned int i = 0; i < m_localRestPositions.size(); i++) - { - points.emplace_back(m_localRestPositions[i][0], m_localRestPositions[i][1], m_localRestPositions[i][2]); - } - - vparams->drawTool()->drawPoints(points, 10, sofa::type::RGBAColor(1, 0.5, 0.5, 1)); - vparams->drawTool()->restoreLastState(); -} } // namespace _wirerestshape_ using _wirerestshape_::WireRestShape;