4343
4444
4545#include < BeamAdapter/component/controller/InterventionalRadiologyController.h>
46+ #include < ranges>
4647
4748
4849namespace beamadapter
@@ -128,6 +129,7 @@ void InterventionalRadiologyController<DataTypes>::init()
128129 {
129130 msg_info () << m_instrumentsList.size () << " instrument(s) found (WireBeamInterpolation)"
130131 }
132+ const auto numberOfInstruments = m_instrumentsList.size ();
131133
132134 m_activatedPointsBuf.clear ();
133135
@@ -143,15 +145,44 @@ void InterventionalRadiologyController<DataTypes>::init()
143145 loadMotionData (d_motionFilename.getValue ());
144146 }
145147
146- auto x_instr_tip = sofa::helper::getWriteOnlyAccessor (d_xTip);
147- x_instr_tip.resize (m_instrumentsList.size ());
148-
149- auto angle_Instrument = sofa::helper::getWriteOnlyAccessor (d_rotationInstrument);
150- angle_Instrument.resize (m_instrumentsList.size ());
151-
152- for (unsigned int i=0 ; i<m_instrumentsList.size (); i++)
153- m_instrumentsList[i]->setControlled (true );
148+ auto checkData = [&](auto & data)
149+ {
150+ if (data.isSet ())
151+ {
152+ const auto & values = data.getValue ();
153+ const auto valueSize = values.size ();
154+
155+ if (valueSize != numberOfInstruments)
156+ {
157+ msg_warning () << " Discrepancy for " getName () << " value: it manages " " tools, but there are " " defined in instrumentPathList."
158+ if (valueSize > numberOfInstruments)
159+ {
160+ msg_warning () << " The superfluous values will be ignored."
161+ }
162+ else
163+ {
164+ msg_warning () << " The missing values will be set as zero."
165+ }
166+ }
167+ }
168+ };
169+
170+ {
171+ checkData (d_xTip);
172+ auto xTip = sofa::helper::getWriteOnlyAccessor (d_xTip);
173+ xTip.resize (numberOfInstruments);
174+ }
175+ {
176+ checkData (d_rotationInstrument);
177+ auto angle_Instrument = sofa::helper::getWriteOnlyAccessor (d_rotationInstrument);
178+ angle_Instrument.resize (numberOfInstruments);
179+ }
154180
181+ for (auto * instrument : m_instrumentsList)
182+ {
183+ instrument->setControlled (true );
184+ }
185+
155186 if (!l_fixedConstraint)
156187 {
157188 typename FixedProjectiveConstraint<DataTypes>::SPtr fixedConstraint{};
@@ -176,49 +207,49 @@ void InterventionalRadiologyController<DataTypes>::init()
176207
177208 m_nodeCurvAbs.clear ();
178209 m_idInstrumentCurvAbsTable.clear ();
210+
211+ // initiliaze current curvAbs
212+ // if there are to be deployed at start (xtip set) then it should reflect that.
213+ // it should always start with zero (origin)
179214 m_nodeCurvAbs.push_back (0.0 );
180- type::vector<int > listInit;
181-
182- for (unsigned int i=0 ; i<m_instrumentsList.size (); i++)
183- listInit.push_back (int (i));
184-
185- m_idInstrumentCurvAbsTable.push_back (listInit);
186-
187- Inherit::init ();
188-
189- sofa::core::objectmodel::BaseObject::d_componentState.setValue (sofa::core::objectmodel::ComponentState::Valid);
190- }
191-
192- template <class DataTypes >
193- void InterventionalRadiologyController<DataTypes>::loadMotionData(std::string filename)
194- {
195- if (!helper::system::DataRepository.findFile (filename))
215+ const auto & xTips = d_xTip.getValue ();
216+ for (const auto xTip : xTips | std::views::reverse)
196217 {
197- msg_error () << " File " " not found."
198- return ;
218+ if (xTip > 0.0 )
219+ {
220+ m_nodeCurvAbs.push_back (xTip);
221+ }
199222 }
200- std::ifstream file (filename.c_str ());
201-
202- std::string line;
203- Vec3 result;
204- while ( std::getline (file,line) )
223+
224+ // initialize curvAbsNode <-> instrument ID table
225+ // i,e identify for each "simulated" node the corresponding tool(s)
226+ // if xtip not initialized then only the 0 (origin) node is considered, and has all the tools
227+ // and as before, if they are deployed at start (xtip set), then we need to take it into account
228+ for (sofa::Index i = 0 ; i < m_nodeCurvAbs.size (); i++)
205229 {
206- if (line.empty ())
207- continue ;
208- std::istringstream values (line);
209- values >> result[0 ] >> result[1 ] >> result[2 ];
210- result[0 ] /= 1000 ;
211- m_sensorMotionData.push_back (result);
230+ type::vector<int > listTool;
231+ for (sofa::Index id = 0 ; id < numberOfInstruments ; id++)
232+ {
233+ const auto xTip = xTips[id];
234+
235+ if (xTip >= m_nodeCurvAbs[i])
236+ {
237+ listTool.push_back (id);
238+ }
239+ }
240+
241+ m_idInstrumentCurvAbsTable.push_back (listTool);
212242 }
213243
214- file.close ();
215- }
244+
245+ type::vector<int > listInit;
246+ for (unsigned int i=0 ; i<numberOfInstruments; i++)
247+ listInit.push_back (int (i));
216248
249+ m_idInstrumentCurvAbsTable.push_back (listInit);
217250
218- template <class DataTypes >
219- void InterventionalRadiologyController<DataTypes>::bwdInit()
220- {
221- // assign the starting pos to each point of the Mechanical State
251+ Inherit::init ();
252+
222253 Coord stPos =d_startingPos.getValue ();
223254 stPos.getOrientation ().normalize ();
224255 d_startingPos.setValue (stPos);
@@ -257,6 +288,30 @@ void InterventionalRadiologyController<DataTypes>::bwdInit()
257288 sofa::core::objectmodel::BaseObject::d_componentState.setValue (sofa::core::objectmodel::ComponentState::Valid);
258289}
259290
291+ template <class DataTypes >
292+ void InterventionalRadiologyController<DataTypes>::loadMotionData(std::string filename)
293+ {
294+ if (!helper::system::DataRepository.findFile (filename))
295+ {
296+ msg_error () << " File " " not found."
297+ return ;
298+ }
299+ std::ifstream file (filename.c_str ());
300+
301+ std::string line;
302+ Vec3 result;
303+ while ( std::getline (file,line) )
304+ {
305+ if (line.empty ())
306+ continue ;
307+ std::istringstream values (line);
308+ values >> result[0 ] >> result[1 ] >> result[2 ];
309+ result[0 ] /= 1000 ;
310+ m_sensorMotionData.push_back (result);
311+ }
312+
313+ file.close ();
314+ }
260315
261316/* !
262317 * \todo fix the mouse event with better controls
0 commit comments