[Wire Shape] Fix the confusion between the nbEdgesCollision and the number of mechanical Beams (#177)

* make link for topo

* compat for nbbeams

Author: fredroy

src/BeamAdapter/component/BaseBeamInterpolation.h

@@ -114,6 +114,8 @@ class BaseBeamInterpolation : public virtual sofa::core::objectmodel::BaseObject
     Real getLength(const EdgeID edgeInList);
     void setLength(const EdgeID edgeInList, Real& length);
 
+    virtual void getMechanicalSampling(Real& dx, const Real x_localcurv_abs) = 0;
+    
     /// Collision information using @sa d_beamCollision
     virtual void getCollisionSampling(Real& dx, const Real x_localcurv_abs) = 0;
     void addCollisionOnBeam(const sofa::Index beam);

src/BeamAdapter/component/BeamInterpolation.h

@@ -199,6 +199,7 @@ class BeamInterpolation : public BaseBeamInterpolation<DataTypes>
     virtual void getSamplingParameters(type::vector<Real>& xP_noticeable,
                                        type::vector<sofa::Size>& nbP_density) override;
     Real getRestTotalLength() override;
+    void getMechanicalSampling(Real& dx, const Real x_localcurv_abs) override;
     void getCollisionSampling(Real &dx, const Real x_localcurv_abs) override;
     void getNumberOfCollisionSegment(Real &dx, unsigned int &numLines) override;
 

src/BeamAdapter/component/BeamInterpolation.inl

@@ -420,17 +420,29 @@ typename BeamInterpolation<DataTypes>::Real BeamInterpolation<DataTypes>::getRes
     return le;
 }
 
+
+template <class DataTypes>
+void BeamInterpolation<DataTypes>::getMechanicalSampling(Real& dx, const Real x_localcurv_abs)
+{
+    SOFA_UNUSED(x_localcurv_abs);
+    
+    const auto numLines = this->m_topologyEdges->size();
+    dx = getRestTotalLength()/numLines;
+}
+
 template <class DataTypes>
-void BeamInterpolation<DataTypes>::getCollisionSampling(Real &dx, const Real /*x_localcurv_abs*/)
+void BeamInterpolation<DataTypes>::getCollisionSampling(Real &dx, const Real x_localcurv_abs)
 {
-    unsigned int numLines = 30;
+    SOFA_UNUSED(x_localcurv_abs);
+    
+    const auto numLines = this->m_topologyEdges->size();
     dx = getRestTotalLength()/numLines;
 }
 
 template <class DataTypes>
 void BeamInterpolation<DataTypes>::getNumberOfCollisionSegment(Real &dx, unsigned int &numLines)
 {
-    numLines = 30;
+    numLines = static_cast<unsigned int>(this->m_topologyEdges->size());
     dx = getRestTotalLength()/numLines;
 }
 

src/BeamAdapter/component/WireBeamInterpolation.h

@@ -117,6 +117,11 @@ class WireBeamInterpolation : public BaseBeamInterpolation<DataTypes>
         return this->m_restShape->getLength();
     }
 
+    void getMechanicalSampling(Real &dx, const Real x_localcurv_abs) override
+    {
+        this->m_restShape->getMechanicalSampling(dx,x_localcurv_abs);
+    }
+    
     void getCollisionSampling(Real &dx, const Real x_localcurv_abs) override
     {
         this->m_restShape->getCollisionSampling(dx,x_localcurv_abs);

src/BeamAdapter/component/controller/InterventionalRadiologyController.h

@@ -180,6 +180,10 @@ class InterventionalRadiologyController : public sofa::component::controller::Me
     SingleLink<
         InterventionalRadiologyController, FixedProjectiveConstraint<DataTypes>,
         BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_fixedConstraint;
+    SingleLink<
+        InterventionalRadiologyController, sofa::core::topology::BaseMeshTopology,
+        BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_mechanicalTopology;
+    
     DeprecatedAndRemoved m_fixedConstraint;
 
     type::vector<Vec3d>                   m_sensorMotionData;

src/BeamAdapter/component/controller/InterventionalRadiologyController.inl

@@ -70,6 +70,7 @@ InterventionalRadiologyController<DataTypes>::InterventionalRadiologyController(
 , d_motionFilename(initData(&d_motionFilename, "motionFilename", "text file that includes tracked motion from optical sensor"))
 , d_indexFirstNode(initData(&d_indexFirstNode, (unsigned int) 0, "indexFirstNode", "first node (should be fixed with restshape)"))
 , l_fixedConstraint(initLink("fixedConstraint", "Path to the FixedProjectiveConstraint"))
+, l_mechanicalTopology(initLink("topology", "Path to the mechanical topology"))
 {
     m_sensored =false;
 }
@@ -80,6 +81,23 @@ void InterventionalRadiologyController<DataTypes>::init()
 {
     BaseContext* context = getContext();
     this->mState = nullptr;
+    
+    if(!l_mechanicalTopology)
+    {
+        msg_info() << "topology (path to the mechanical topology) has not been set, searching for one in the context.";
+        l_mechanicalTopology.set(this->getContext()->getMeshTopologyLink());
+    }
+    
+    if (l_mechanicalTopology)
+    {
+        msg_info() << "Found topology named "<< l_mechanicalTopology->getName() ;
+    }
+    else
+    {
+        msg_error() << "Cannot find topology container. Please specify the link to the topology or insert one in the same node.";
+        this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid);
+        return;
+    }
 
     //get the pointers of the WireBeamInterpolations
     const type::vector<std::string>& instrumentPathList = d_instrumentsPath.getValue();
@@ -100,7 +118,8 @@ void InterventionalRadiologyController<DataTypes>::init()
         }
     }
 
-    if (m_instrumentsList.empty()) {
+    if (m_instrumentsList.empty())
+    {
         msg_error() << "No instrument found (no WireBeamInterpolation)! the component can not work and will be set to Invalid.";
         sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid);
         return;
@@ -567,7 +586,7 @@ void InterventionalRadiologyController<DataTypes>::interventionalRadiologyCollis
     for (int i = static_cast<int>(xPointList.size()) - 1; i>=0; i--)
     {
         //1.  we determin if the poin ument
-        int instrumentId = idInstrumentList[i];
+        const int instrumentId = idInstrumentList[i];
 
         // x_max for the instrument that is controlled (not dropped part)
         Real xMaxControlled = m_instrumentsList[instrumentId]->getRestTotalLength();
@@ -585,9 +604,9 @@ void InterventionalRadiologyController<DataTypes>::interventionalRadiologyCollis
         xPointList[i] = xAbsCurv - xBegin; // provides the "local" curv absc of the point (on the instrument reference)
         idInstrumentList[i] = firstInstruOnx;
 
-        // 3. we look for the collision sampling of the current instrument in order to "place" the following point
+        // 3. we look for the mechanical sampling of the current instrument in order to "place" the following point
         Real xIncr;
-        m_instrumentsList[firstInstruOnx]->getCollisionSampling(xIncr, xPointList[i]);
+        m_instrumentsList[firstInstruOnx]->getMechanicalSampling(xIncr, xPointList[i]);
         xAbsCurv -= xIncr;
 
         // the following point could not have x_abs_curv<0;
@@ -626,7 +645,7 @@ void InterventionalRadiologyController<DataTypes>::interventionalRadiologyCollis
 
     for (unsigned int i=0; i<xPointList.size(); i++)
     {
-        int instrumentId = idInstrumentList[i];
+        const int instrumentId = idInstrumentList[i];
 
         // x_max for the instrument that is controlled (not dropped part)
         Real xMaxInstrument = m_instrumentsList[instrumentId]->getRestTotalLength();
@@ -709,7 +728,7 @@ void InterventionalRadiologyController<DataTypes>::applyInterventionalRadiologyC
 
     // Create vectors with the CurvAbs of the noticiable points and the id of the corresponding instrument
     type::vector<Real> newCurvAbs;
-    type::vector<type::vector<int>> idInstrumentTable;
+    type::vector<type::vector<int>> idInstrumentTable; // i.e for each node -> [instID0, instID1...]
 
     // ## STEP 1: Find the total length of the COMBINED INSTRUMENTS and the one for which xtip > 0 (so the one which are simulated)
     helper::AdvancedTimer::stepBegin("step1");
@@ -767,10 +786,11 @@ void InterventionalRadiologyController<DataTypes>::applyInterventionalRadiologyC
     //    => Get write access to current nodes/dofs
     Data<VecCoord>* datax = this->getMechanicalState()->write(sofa::core::vec_id::write_access::position);
     auto x = sofa::helper::getWriteOnlyAccessor(*datax);
-    VecCoord xbuf = x.ref();
+    const VecCoord xbuf = x.ref(); // make a copy of the positions, as it will changed meanwhile
 
     const sofa::Size numberOfNodes = x.size();
     sofa::Size numberOfSimulatedNodes = newCurvAbs.size(); // number of simulated nodes
+
     if (numberOfSimulatedNodes > numberOfNodes)
     {
         msg_warning() << "Parameters missmatch. There are more curv abscisses '" << numberOfSimulatedNodes << "' than the number of dof: " << numberOfNodes;
@@ -1012,7 +1032,7 @@ void InterventionalRadiologyController<DataTypes>::fillInstrumentCurvAbsTable(co
         xBegin -= threshold;
         xEnd += threshold;
 
-        // check curvAbs sorted value, if value is inside [xBegin, xBegin] of the tool add it to instrumentList. 
+        // check curvAbs sorted value, if value is inside [xBegin, xEnd] of the tool add it to instrumentList.
         for (unsigned int i = 0; i < curvAbs.size(); i++)
         {
             if (curvAbs[i] < xBegin) // still not inside range
@@ -1066,7 +1086,7 @@ const type::vector< type::vector<int> >& InterventionalRadiologyController<DataT
 template <class DataTypes>
 int InterventionalRadiologyController<DataTypes>::getTotalNbEdges() const
 {
-    return getContext()->getMeshTopology()->getNbEdges();
+    return l_mechanicalTopology->getNbEdges();
 }
 
 

src/BeamAdapter/component/engine/WireRestShape.h

@@ -107,10 +107,10 @@ class WireRestShape : public core::objectmodel::BaseObject
 
 
      Real getLength() ;
-     void getCollisionSampling(Real &dx, const Real x_curv);
-     void getNumberOfCollisionSegment(Real &dx, sofa::Size& numLines) ;
-
-
+    
+    void getMechanicalSampling(Real& dx, const Real x_localcurv_abs);
+    void getCollisionSampling(Real &dx, const Real x_curv);
+    void getNumberOfCollisionSegment(Real &dx, sofa::Size& numLines);
 
      /////////////////////////// Deprecated Methods  ////////////////////////////////////////// 
 
@@ -142,9 +142,7 @@ class WireRestShape : public core::objectmodel::BaseObject
 
 private:
      /// Link to be set to the topology container in the component graph.
-     SingleLink<WireRestShape<DataTypes>, 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 }; 
+     SingleLink<WireRestShape<DataTypes>, sofa::core::topology::BaseMeshTopology, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_topology;
 };
 
 

src/BeamAdapter/component/engine/WireRestShape.inl

@@ -58,7 +58,8 @@ WireRestShape<DataTypes>::WireRestShape()
     , 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_density.setReadOnly(true); // density is supposed to be filled using the section materials
+    d_keyPoints.setReadOnly(true); // key points are supposed to be filled using the section materials
 }
 
 
@@ -70,16 +71,15 @@ void WireRestShape<DataTypes>::init()
     //////////////////////////////////////////////
     ////////// get and fill local topology ///////
     //////////////////////////////////////////////
-    
-    // Get pointer to given topology using the link. If not found will search in current context.
-    _topology = l_topology.get();
 
-    if (!_topology)
-        this->getContext()->get(_topology);
+    if (!l_topology)
+    {
+        l_topology.set(this->getContext()->getMeshTopologyLink());
+    }
 
-    if(_topology != nullptr)
+    if (l_topology)
     {
-        msg_info() << "found topology named "<< _topology->getName() ;
+        msg_info() << "Found topology named "<< l_topology->getName() ;
     }
     else
     {
@@ -94,6 +94,16 @@ void WireRestShape<DataTypes>::init()
         this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid);
         return;
     }
+    
+    // Workaround around the fact that d_density and d_keyPoints are supposed to be output only
+    if(d_density.isSet())
+    {
+        msg_warning() << "The density field will be ignored (output only Data).";
+    }
+    if(d_keyPoints.isSet())
+    {
+        msg_warning() << "The keyPoints field will be ignored (output only Data).";
+    }
 
 
     ////////////////////////////////////////////////////////
@@ -124,7 +134,7 @@ void WireRestShape<DataTypes>::initLengths()
     {
         auto rodSection = l_sectionMaterials.get(i);
         keyPointList[i+1] = keyPointList[i] + rodSection->getLength();
-        densityList[i] = rodSection->getNbCollisionEdges();
+        densityList[i] = rodSection->getNbBeams();
     }
 }
 
@@ -133,8 +143,8 @@ template <class DataTypes>
 bool WireRestShape<DataTypes>::initTopology()
 {
     /// fill topology :
-    _topology->clear();
-    _topology->cleanup();
+    l_topology->clear();
+    l_topology->cleanup();
 
     const type::vector<Real>& keyPts = d_keyPoints.getValue();
     if (l_sectionMaterials.size() != keyPts.size() - 1)
@@ -155,12 +165,12 @@ bool WireRestShape<DataTypes>::initTopology()
 
         // add points from the material
         for (int i = startPtId; i < nbrVisuEdges + 1; i++) {
-            _topology->addPoint(prev_length + i * dx, 0, 0);
+            l_topology->addPoint(prev_length + i * dx, 0, 0);
         }
 
         // add segments from the material
         for (int i = prev_edges; i < prev_edges + nbrVisuEdges; i++) {
-            _topology->addEdge(i, i + 1);
+            l_topology->addEdge(i, i + 1);
         }
 
         prev_length = length;
@@ -181,10 +191,51 @@ void WireRestShape<DataTypes>::getSamplingParameters(type::vector<Real>& xP_noti
 }
 
 
+template <class DataTypes>
+void WireRestShape<DataTypes>::getMechanicalSampling(Real &dx, const Real x_curv)
+{
+    unsigned int numLines = 0;
+    Real x_used = x_curv - EPSILON;
+
+    const Real totalLength = this->getLength();
+    x_used = std::clamp(x_used, 0.0, totalLength);
+
+    const type::vector<Real>& 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;
+    }
+    
+    // Check in which section x_used belongs to and get access to this section material
+    for (sofa::Size i = 1; i< keyPts.size(); ++i)
+    {
+        if (x_used <= keyPts[i])
+        {
+            numLines = l_sectionMaterials.get(i-1)->getNbBeams();
+
+            Real length = fabs(keyPts[i] - keyPts[i-1]);
+            dx = length / numLines;
+            return;
+        }
+    }
+
+    // If x_used is out of bounds. Warn user and returns default value.
+    numLines = 20;
+    dx = totalLength / numLines;
+    msg_error() << " problem in getMechanicalSampling : x_curv " << x_used << " is not between keyPoints" << d_keyPoints.getValue();
+}
+
 template <class DataTypes>
 void WireRestShape<DataTypes>::getCollisionSampling(Real &dx, const Real x_curv)
 {
-    unsigned int numLines;
+    unsigned int numLines = 0;
     Real x_used = x_curv - EPSILON;
 
     const Real totalLength = this->getLength();
@@ -227,6 +278,17 @@ void WireRestShape<DataTypes>::getCollisionSampling(Real &dx, const Real x_curv)
     msg_error() << " problem in getCollisionSampling : x_curv " << x_used << " is not between keyPoints" << d_keyPoints.getValue();
 }
 
+template <class DataTypes>
+void WireRestShape<DataTypes>::getNumberOfCollisionSegment(Real &dx, sofa::Size& numLines)
+{
+    numLines = 0;
+    for (sofa::Size i = 0; i < l_sectionMaterials.size(); ++i)
+    {
+        numLines += l_sectionMaterials.get(i)->getNbCollisionEdges();
+    }
+    dx = getLength() / numLines;
+}
+
 
 template <class DataTypes>
 void WireRestShape<DataTypes>::getRestTransformOnX(Transform &global_H_local, const Real x)
@@ -319,19 +381,6 @@ typename WireRestShape<DataTypes>::Real WireRestShape<DataTypes>::getLength()
     return d_keyPoints.getValue().back();
 }
 
-
-template <class DataTypes>
-void WireRestShape<DataTypes>::getNumberOfCollisionSegment(Real &dx, sofa::Size& numLines)
-{
-    numLines = 0;
-    for (sofa::Size i = 0; i < l_sectionMaterials.size(); ++i)
-    {
-        numLines += l_sectionMaterials.get(i)->getNbCollisionEdges();
-    }
-    dx = getLength() / numLines;
-}
-
-
 template <class DataTypes>
 void WireRestShape<DataTypes>::computeOrientation(const Vec3& AB, const Quat& Q, Quat &result)
 {