[InterventionalRadiologyController] Rewrite method sortCurvAbs to use…

… std sort + unique and update instrumentTable fill.
sofa-framework · Sep 26, 2022 · 72374bc · 72374bc
1 parent 17df60b
commit 72374bc
Show file tree

Hide file tree

Showing 2 changed files with 33 additions and 50 deletions.
diff --git a/src/BeamAdapter/component/controller/InterventionalRadiologyController.h b/src/BeamAdapter/component/controller/InterventionalRadiologyController.h
@@ -131,7 +131,7 @@ class InterventionalRadiologyController : public MechanicalStateController<DataT
     void processDrop(unsigned int &previousNumControlledNodes,  unsigned int &seg_remove);
     void interventionalRadiologyComputeSampling(type::vector<Real> &newCurvAbs, type::vector< type::vector<int> > &id_instrument_table, const type::vector<Real> &xBegin, const Real& xEnd);
     /// Sort the curv Abs in the ascending order and avoid doubloon
-    void sortCurvAbs(type::vector<Real> &CurvAbs,  type::vector< type::vector<int> >& id_instrument_table);
+    void sortCurvAbs(type::vector<Real> &CurvAbs, type::vector< type::vector<int> >& id_instrument_table);
     void totalLengthIsChanging(const type::vector<Real>& newNodeCurvAbs, type::vector<Real>& modifiedNodeCurvAbs, const type::vector< type::vector<int> >& newTable);
     void fixFirstNodesWithUntil(unsigned int first_simulated_Node);
     void activateBeamListForCollision( type::vector<Real> &curv_abs, type::vector< type::vector<int> > &id_instrument_table);
@@ -159,6 +159,10 @@ class InterventionalRadiologyController : public MechanicalStateController<DataT
     type::vector< type::vector<int> >           m_idInstrumentCurvAbsTable;
     unsigned int                    m_numControlledNodes; // Excluding the nodes that are "dropped"
     bool                            m_dropCall;
+
+private:
+    /// Copy of @sa d_threshold for direct access in lambda unique function to avoid numerous getValue calls
+    Real m_threshold = 0.0;
 };
 
 #if !defined(SOFA_PLUGIN_BEAMADAPTER_INTERVENTIONALRADIOCONTROLLER_CPP)

diff --git a/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl b/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl
@@ -996,66 +996,45 @@ template <class DataTypes>
 void InterventionalRadiologyController<DataTypes>::sortCurvAbs(type::vector<Real> &curvAbs,
                                                                type::vector<type::vector<int> >& idInstrumentTable)
 {
-    auto newCurvAbs = sofa::helper::getWriteOnlyAccessor(d_curvAbs);
-    Real eps = d_threshold.getValue();
+    // here we sort CurvAbs   
+    std::sort(curvAbs.begin(), curvAbs.end());
 
-   // here we sort CurvAbs
-   // a buffer verctor: newCurvAbs will be filled by iteratively removing the minimal values found in CurvAbs and push_back them in newCurvAbs
-   // a threshod is used to remove the values that are "too" similars...
-   // TODO: could be improve by function "sort" ???
+    // copy threshold in variable member for std::unique
+    m_threshold = d_threshold.getValue();
 
-    newCurvAbs.clear();
-    while(curvAbs.size()>0)
-    {
-        Real xMin = 1.0e30;
-        unsigned int index_min=0;
-        for(unsigned int j=0; j<curvAbs.size(); j++)
-        {
-            if(xMin > curvAbs[j] )
-            {
-                xMin = curvAbs[j];
-               index_min=j;
-            }
-        }
-
-        type::removeIndex( curvAbs, index_min);
-
-        // verify using a eps that the same node does not already exist
-        Real xLast;
-        if (newCurvAbs.size()>0 )
-            xLast= newCurvAbs[newCurvAbs.size()-1];
-        else
-            xLast=-1.0e30;
-
-        if( fabs(xMin - xLast) > eps)
-        {
-            newCurvAbs.push_back(xMin);
-        }
-    }
+    // a threshod is used to remove the values that are "too" similars...
+    auto it = std::unique(curvAbs.begin(), curvAbs.end(), [this](const Real v1, const Real v2) {
+        return fabs(v1 - v2) < this->m_threshold;
+    });
+    curvAbs.erase(it, curvAbs.end());
 
-    curvAbs = newCurvAbs;
-
-    // here we build a table that provides the list of each instrument for each dof in the list of newCurvAbs
+    // here we build a table that provides the list of each instrument for each dof in the list of curvAbs
     // dofs can be shared by several instruments
     idInstrumentTable.clear();
+    idInstrumentTable.resize(curvAbs.size());
 
-    for (unsigned int i=0; i<newCurvAbs.size(); i++)
+    for (unsigned int id = 0; id < m_instrumentsList.size(); id++)
     {
-        type::vector<int> listNewNode;
-        for (unsigned int id=0; id<m_instrumentsList.size(); id++)
-        {
-            Real xEnd= d_xTip.getValue()[id];
-            Real xBegin = xEnd - m_instrumentsList[id]->getRestTotalLength();
+        // Get instrument absciss range
+        Real xEnd = d_xTip.getValue()[id];
+        Real xBegin = xEnd - m_instrumentsList[id]->getRestTotalLength();
 
-            if ( xBegin<(newCurvAbs[i]+eps) && xEnd >(newCurvAbs[i]-eps) )
-                listNewNode.push_back(id);
-        }
+        // enlarge range to ensure to considere borders in absisses comparisons
+        xBegin -= m_threshold;
+        xEnd += m_threshold;
 
-        if(listNewNode.size() ==0)
-            msg_error()<<"No instrument find for curvAbs"<<newCurvAbs[i];
+        // check curvAbs sorted value, if value is inside [xBegin, xBegin] of the tool add it to instrumentList. 
+        for (unsigned int i = 0; i < curvAbs.size(); i++)
+        {
+            auto xCurv = curvAbs[i];
+            if (xCurv < xBegin) // still not inside range
+                continue;
 
-        idInstrumentTable.push_back(listNewNode);
+            if (xCurv > xEnd) // exit range
+                break;
 
+            idInstrumentTable[i].push_back(id);
+        }
     }
 }