From 08879e260816d1e5d9b3bcbc4b9a8b0b14b3bc3d Mon Sep 17 00:00:00 2001 From: Frederick Roy Date: Thu, 16 Jul 2026 09:17:51 +0900 Subject: [PATCH 1/8] remove motion-related code --- .../InterventionalRadiologyController.h | 10 +--- .../InterventionalRadiologyController.inl | 55 +------------------ 2 files changed, 4 insertions(+), 61 deletions(-) diff --git a/src/BeamAdapter/component/controller/InterventionalRadiologyController.h b/src/BeamAdapter/component/controller/InterventionalRadiologyController.h index 7cbb48b29..5e1518da8 100644 --- a/src/BeamAdapter/component/controller/InterventionalRadiologyController.h +++ b/src/BeamAdapter/component/controller/InterventionalRadiologyController.h @@ -156,7 +156,6 @@ class InterventionalRadiologyController : public sofa::component::controller::Me void totalLengthIsChanging(const type::vector& newNodeCurvAbs, type::vector& modifiedNodeCurvAbs, const type::vector< type::vector >& newTable); void fixFirstNodesWithUntil(unsigned int first_simulated_Node); void activateBeamListForCollision( type::vector &curv_abs, type::vector< type::vector > &id_instrument_table); - void loadMotionData(std::string filename); Data d_controlledInstrument; Data> d_xTip; @@ -167,12 +166,11 @@ class InterventionalRadiologyController : public sofa::component::controller::Me Data d_startingPos; Data d_threshold; Data> d_rigidCurvAbs; // Pairs (start - end) - Data d_motionFilename; Data d_indexFirstNode; // First Node simulated - - + + bool m_useBeamActions = false; - bool m_FF, m_RW, m_sensored; + bool m_FF = false, m_RW = false; SingleLink< InterventionalRadiologyController, FixedProjectiveConstraint, @@ -183,8 +181,6 @@ class InterventionalRadiologyController : public sofa::component::controller::Me DeprecatedAndRemoved m_fixedConstraint; - type::vector m_sensorMotionData; - unsigned int m_currentSensorData; type::vector m_nodeCurvAbs; type::vector< type::vector > m_idInstrumentCurvAbsTable; }; diff --git a/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl b/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl index 812c2983f..affde0f77 100644 --- a/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl +++ b/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl @@ -38,7 +38,6 @@ #include #include #include -#include #include @@ -68,12 +67,10 @@ InterventionalRadiologyController::InterventionalRadiologyController( , d_startingPos(initData(&d_startingPos,Coord(),"startingPos","starting pos for inserting the instrument")) , d_threshold(initData(&d_threshold, (Real)0.01, "threshold", "threshold for controller precision which is homogeneous to the unit of length used in the simulation")) , d_rigidCurvAbs(initData(&d_rigidCurvAbs, "rigidCurvAbs", "pairs of curv abs for beams we want to rigidify")) -, 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; } @@ -137,13 +134,7 @@ void InterventionalRadiologyController::init() { m_FF=true; m_RW=false; - m_sensored = false; } - if (!d_motionFilename.getValue().empty()) - { - m_FF = true; m_sensored = true; m_currentSensorData = 0; - loadMotionData(d_motionFilename.getValue()); - } auto checkData = [&](auto& data) { @@ -296,31 +287,6 @@ void InterventionalRadiologyController::init() sofa::core::objectmodel::BaseComponent::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); } -template -void InterventionalRadiologyController::loadMotionData(std::string filename) -{ - if (!helper::system::DataRepository.findFile(filename)) - { - msg_error() << "File " << filename << " not found."; - return; - } - std::ifstream file(filename.c_str()); - - std::string line; - Vec3 result; - while( std::getline(file,line) ) - { - if (line.empty()) - continue; - std::istringstream values(line); - values >> result[0] >> result[1] >> result[2]; - result[0] /= 1000; - m_sensorMotionData.push_back(result); - } - - file.close(); -} - /*! * \todo fix the mouse event with better controls */ @@ -413,26 +379,7 @@ void InterventionalRadiologyController::onBeginAnimationStep(const do } if (m_FF) { - if (!m_sensored) - xInstrTip[id] += d_speed.getValue() * context->getDt(); - else - { - unsigned int newSensorData = m_currentSensorData + 1; - - while( m_sensorMotionData[newSensorData][0] < context->getTime() ) - { - m_currentSensorData = newSensorData; - newSensorData++; - } - if(newSensorData >= m_sensorMotionData.size()) - { - xInstrTip[id] = 0; - } - else - { - xInstrTip[id] += m_sensorMotionData[m_currentSensorData][1]; - } - } + xInstrTip[id] += d_speed.getValue() * context->getDt(); } if (m_RW) { From 0c1338217cbf52a4b0170d4d20d4e6476ef5ccf4 Mon Sep 17 00:00:00 2001 From: Frederick Roy Date: Thu, 16 Jul 2026 09:44:44 +0900 Subject: [PATCH 2/8] add more unit tests for InterventionalRadiologyController --- ...InterventionalRadiologyController_test.cpp | 576 +++++++++++++++--- 1 file changed, 507 insertions(+), 69 deletions(-) diff --git a/BeamAdapter_test/component/controller/InterventionalRadiologyController_test.cpp b/BeamAdapter_test/component/controller/InterventionalRadiologyController_test.cpp index 61cd1e80b..580342fb2 100644 --- a/BeamAdapter_test/component/controller/InterventionalRadiologyController_test.cpp +++ b/BeamAdapter_test/component/controller/InterventionalRadiologyController_test.cpp @@ -27,8 +27,12 @@ #include #include +#include +#include #include +#include + namespace beamadapter_test { using namespace sofa::testing; @@ -40,11 +44,9 @@ using namespace beamadapter; class InterventionalRadiologyController_test : public BaseTest { public: - using InterventionalRadioCtrlRig3 = InterventionalRadiologyController; - typedef typename Rigid3Types::Coord Coord; - typedef typename Rigid3Types::Deriv Deriv; - typedef typename Coord::value_type Real; - //typedef typename sofa::defaulttype::SolidTypes::Transform Transform; + using IRController = InterventionalRadiologyController; + using Coord = Rigid3Types::Coord; + using Real = Coord::value_type; void doSetUp() override { @@ -53,108 +55,544 @@ class InterventionalRadiologyController_test : public BaseTest sofa::simpleapi::importPlugin(Sofa.Component.Topology.Container.Grid); sofa::simpleapi::importPlugin(Sofa.Component.Constraint.Projective); sofa::simpleapi::importPlugin(Sofa.Component.StateContainer); + sofa::simpleapi::importPlugin(Sofa.Component.ODESolver.Backward); + sofa::simpleapi::importPlugin(Sofa.Component.LinearSolver.Direct); + sofa::simpleapi::importPlugin(Sofa.Component.SolidMechanics.Spring); + sofa::simpleapi::importPlugin(Sofa.Component.AnimationLoop); m_root = sofa::simpleapi::createRootNode(sofa::simulation::getSimulation(), "root"); } - /// Unload the scene void doTearDown() override { - if (m_root != nullptr) { + if (m_root != nullptr) sofa::simulation::node::unload(m_root); - } } + /// Load + init only the components (no animation loop needed). void loadScene(const std::string& scene) { m_root = sofa::simulation::SceneLoaderXML::loadFromMemory("testscene", scene.c_str()); - - EXPECT_NE(m_root.get(), nullptr); - + ASSERT_NE(m_root.get(), nullptr); m_root->init(sofa::core::execparams::defaultInstance()); } + /// Load + fully init the graph through the simulation, so the scene can be animated. + void loadAndInitRoot(const std::string& scene) + { + m_root = sofa::simulation::SceneLoaderXML::loadFromMemory("testscene", scene.c_str()); + ASSERT_NE(m_root.get(), nullptr); + sofa::simulation::node::initRoot(m_root.get()); + } - /// Test creation of InterventionalRadiologyController in an empty scene without parameters - void testEmptyInit(); + IRController* getController() + { + return m_root->get(BaseContext::SearchDown); + } - /// Test creation of InterventionalRadiologyController in a default scene with needed components - void testDefaultInit(); + /// A single-instrument scene without solver (for init / action / getter tests). + /// `ctrlAttrs` are extra attributes injected on the controller tag. + static std::string singleInstrumentScene(const std::string& ctrlAttrs = "") + { + return + "" + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " "; + } - ///// Test creation of WireRestShape in a default scene and check parameters cohesion - void testParameterInit(); + /// A two-instrument scene without solver. + static std::string twoInstrumentScene() + { + return + "" + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " "; + } - /** TODO epernod 2023-05-03: - - Test creation of WireRestShape in a default scene and check created topology - - Test WireRestShape topology init from MeshLoader - - Test WireRestShape transform methods - */ + /// A full deployment scene with solver, mass and rest-shape springs (for stepping tests). + static std::string deploymentScene(const std::string& ctrlAttrs) + { + return + "" + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " "; + } -private: - /// Pointer to root Node +protected: sofa::simulation::Node::SPtr m_root = nullptr; - }; +///////////////////////////////////// Init / validation /////////////////////////////////////////// -void InterventionalRadiologyController_test::testEmptyInit() +/// No topology, no instrument, no mechanical state => Invalid. +TEST_F(InterventionalRadiologyController_test, init_empty_invalid) { - std::string scene = - " " - " " - " " - " " - " " - " "; + EXPECT_MSG_EMIT(Error); + loadScene( + "" + "" + " " + " " + " " + ""); + + auto* ctrl = getController(); + ASSERT_NE(ctrl, nullptr); + EXPECT_EQ(ctrl->getComponentState(), ComponentState::Invalid); +} + +/// Fully wired single-instrument scene => Valid, one instrument registered. +TEST_F(InterventionalRadiologyController_test, init_default_valid) +{ + EXPECT_MSG_NOEMIT(Error); + loadScene(singleInstrumentScene()); + auto* ctrl = getController(); + ASSERT_NE(ctrl, nullptr); + EXPECT_EQ(ctrl->getComponentState(), ComponentState::Valid); + EXPECT_EQ(ctrl->m_instrumentsList.size(), 1u); +} + +/// A topology is present but there is no WireBeamInterpolation => Invalid. +TEST_F(InterventionalRadiologyController_test, init_noInstrument_invalid) +{ EXPECT_MSG_EMIT(Error); - loadScene(scene); - - InterventionalRadioCtrlRig3::SPtr IRCtrlPtr = m_root->get< InterventionalRadioCtrlRig3 >(sofa::core::objectmodel::BaseContext::SearchDown); - EXPECT_NE(IRCtrlPtr.get(), nullptr); - EXPECT_EQ(IRCtrlPtr->getComponentState(), ComponentState::Invalid); -} - - -void InterventionalRadiologyController_test::testDefaultInit() -{ - std::string scene = - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " "; + loadScene( + "" + "" + " " + " " + " " + " " + " " + " " + ""); + + auto* ctrl = getController(); + ASSERT_NE(ctrl, nullptr); + EXPECT_EQ(ctrl->getComponentState(), ComponentState::Invalid); +} +/// An instrument path that does not resolve => error and Invalid. +TEST_F(InterventionalRadiologyController_test, init_instrumentPathNotFound_invalid) +{ + EXPECT_MSG_EMIT(Error); + loadScene( + "" + "" + " " + " " + " " + " " + " " + " " + ""); + + auto* ctrl = getController(); + ASSERT_NE(ctrl, nullptr); + EXPECT_EQ(ctrl->getComponentState(), ComponentState::Invalid); +} + +/// Without a FixedProjectiveConstraint the controller should become Invalid. +/// DISABLED: currently CRASHES. init() sets the state to Invalid on a missing +/// FixedProjectiveConstraint (IRC.inl ~L196) but does not return, so it goes on to call +/// applyInterventionalRadiologyController() at the end of init(), which dereferences the +/// null l_fixedConstraint in fixFirstNodesWithUntil() -> segfault. Re-enable once init() +/// returns early (or applyInterventionalRadiologyController() guards the null link). +TEST_F(InterventionalRadiologyController_test, DISABLED_init_noFixedConstraint_invalid) +{ + EXPECT_MSG_EMIT(Error); + loadScene( + "" + "" + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + ""); + + auto* ctrl = getController(); + ASSERT_NE(ctrl, nullptr); + EXPECT_EQ(ctrl->getComponentState(), ComponentState::Invalid); +} + +/// Two instruments => both registered, and the per-instrument Data are sized accordingly. +TEST_F(InterventionalRadiologyController_test, init_twoInstruments) +{ + EXPECT_MSG_NOEMIT(Error); + loadScene(twoInstrumentScene()); + + auto* ctrl = getController(); + ASSERT_NE(ctrl, nullptr); + EXPECT_EQ(ctrl->getComponentState(), ComponentState::Valid); + EXPECT_EQ(ctrl->m_instrumentsList.size(), 2u); + EXPECT_EQ(ctrl->d_xTip.getValue().size(), 2u); + EXPECT_EQ(ctrl->d_rotationInstrument.getValue().size(), 2u); +} + + +///////////////////////////////////// Data initialization ///////////////////////////////////////// + +/// xtip / rotationInstrument are resized to the number of instruments even if left unset. +TEST_F(InterventionalRadiologyController_test, init_dataResizedToInstrumentCount) +{ + EXPECT_MSG_NOEMIT(Error); + loadScene(singleInstrumentScene()); + + auto* ctrl = getController(); + ASSERT_NE(ctrl, nullptr); + EXPECT_EQ(ctrl->d_xTip.getValue().size(), 1u); + EXPECT_EQ(ctrl->d_rotationInstrument.getValue().size(), 1u); +} + +/// startingPos orientation is normalized during init, and all dofs are initialized to it. +TEST_F(InterventionalRadiologyController_test, init_startingPosNormalized) +{ + EXPECT_MSG_NOEMIT(Error); + // deliberately non-unit quaternion (0 0 0 5) + loadScene(singleInstrumentScene("startingPos='1 2 3 0 0 0 5'")); + + auto* ctrl = getController(); + ASSERT_NE(ctrl, nullptr); + + const auto q = ctrl->d_startingPos.getValue().getOrientation(); + const double qNorm = std::sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); + EXPECT_NEAR(qNorm, 1.0, 1e-6); + + // every dof of the mechanical state has been set to startingPos + auto* beamModel = m_root->getChild("BeamModel"); + ASSERT_NE(beamModel, nullptr); + auto* mstate = beamModel->getMechanicalState(); + ASSERT_NE(mstate, nullptr); + EXPECT_GT(mstate->getSize(), 0u); +} + +/// The current curvilinear abscissa list always starts at the origin (0). +TEST_F(InterventionalRadiologyController_test, init_curvAbsStartsAtOrigin) +{ EXPECT_MSG_NOEMIT(Error); - loadScene(scene); + loadScene(singleInstrumentScene()); - InterventionalRadioCtrlRig3::SPtr IRCtrlPtr = m_root->get< InterventionalRadioCtrlRig3 >(sofa::core::objectmodel::BaseContext::SearchDown); - EXPECT_NE(IRCtrlPtr.get(), nullptr); - EXPECT_EQ(IRCtrlPtr->getComponentState(), ComponentState::Valid); + auto* ctrl = getController(); + ASSERT_NE(ctrl, nullptr); + const auto& curvAbs = ctrl->getCurrentCurvAbscisses(); + ASSERT_FALSE(curvAbs.empty()); + EXPECT_NEAR(curvAbs.front(), 0.0, 1e-9); +} - EXPECT_EQ(IRCtrlPtr.get()->m_instrumentsList.size(), 1); +/// An instrument deployed at start (xtip>0) creates a node at that abscissa. +TEST_F(InterventionalRadiologyController_test, init_deployedTipCreatesNode) +{ + EXPECT_MSG_NOEMIT(Error); + loadScene(singleInstrumentScene("xtip='10'")); + + auto* ctrl = getController(); + ASSERT_NE(ctrl, nullptr); + const auto& curvAbs = ctrl->getCurrentCurvAbscisses(); + // origin + deployed length => tip reaches ~10 + EXPECT_NEAR(curvAbs.back(), 10.0, ctrl->d_threshold.getValue()); + EXPECT_GT(curvAbs.size(), 1u); } +///////////////////////////////////// applyAction ///////////////////////////////////////////////// + +TEST_F(InterventionalRadiologyController_test, action_moveForwardBackward) +{ + loadScene(singleInstrumentScene()); + auto* ctrl = getController(); + ASSERT_NE(ctrl, nullptr); + + const Real step = ctrl->d_step.getValue(); + const Real x0 = ctrl->d_xTip.getValue()[0]; + + ctrl->applyAction(BeamAdapterAction::MOVE_FORWARD); + EXPECT_NEAR(ctrl->d_xTip.getValue()[0], x0 + step, 1e-9); + + ctrl->applyAction(BeamAdapterAction::MOVE_BACKWARD); + EXPECT_NEAR(ctrl->d_xTip.getValue()[0], x0, 1e-9); +} + +TEST_F(InterventionalRadiologyController_test, action_spinRightLeft) +{ + loadScene(singleInstrumentScene()); + auto* ctrl = getController(); + ASSERT_NE(ctrl, nullptr); + + const Real astep = ctrl->d_angularStep.getValue(); + const Real r0 = ctrl->d_rotationInstrument.getValue()[0]; + + ctrl->applyAction(BeamAdapterAction::SPIN_RIGHT); + EXPECT_NEAR(ctrl->d_rotationInstrument.getValue()[0], r0 + astep, 1e-9); -TEST_F(InterventionalRadiologyController_test, test_init_empty) { - testEmptyInit(); + ctrl->applyAction(BeamAdapterAction::SPIN_LEFT); + EXPECT_NEAR(ctrl->d_rotationInstrument.getValue()[0], r0, 1e-9); } -TEST_F(InterventionalRadiologyController_test, test_init_default) { - testDefaultInit(); +/// With a single instrument, switching tools is bounded and keeps the controlled index at 0. +TEST_F(InterventionalRadiologyController_test, action_switchTools_singleInstrumentBounded) +{ + loadScene(singleInstrumentScene()); + auto* ctrl = getController(); + ASSERT_NE(ctrl, nullptr); + ASSERT_EQ(ctrl->d_controlledInstrument.getValue(), 0); + + ctrl->applyAction(BeamAdapterAction::SWITCH_NEXT_TOOL); // no next tool + EXPECT_EQ(ctrl->d_controlledInstrument.getValue(), 0); + + ctrl->applyAction(BeamAdapterAction::SWITCH_PREVIOUS_TOOL); // already first + EXPECT_EQ(ctrl->d_controlledInstrument.getValue(), 0); + + ctrl->applyAction(BeamAdapterAction::USE_TOOL_1); // does not exist + EXPECT_EQ(ctrl->d_controlledInstrument.getValue(), 0); + + ctrl->applyAction(BeamAdapterAction::USE_TOOL_2); // does not exist + EXPECT_EQ(ctrl->d_controlledInstrument.getValue(), 0); +} + +/// With two instruments, switching moves the controlled index within [0, 1]. +TEST_F(InterventionalRadiologyController_test, action_switchTools_multiInstrument) +{ + loadScene(twoInstrumentScene()); + auto* ctrl = getController(); + ASSERT_NE(ctrl, nullptr); + ASSERT_EQ(ctrl->d_controlledInstrument.getValue(), 0); + + ctrl->applyAction(BeamAdapterAction::SWITCH_NEXT_TOOL); + EXPECT_EQ(ctrl->d_controlledInstrument.getValue(), 1); + + ctrl->applyAction(BeamAdapterAction::SWITCH_NEXT_TOOL); // no third tool, stays at 1 + EXPECT_EQ(ctrl->d_controlledInstrument.getValue(), 1); + + ctrl->applyAction(BeamAdapterAction::SWITCH_PREVIOUS_TOOL); + EXPECT_EQ(ctrl->d_controlledInstrument.getValue(), 0); + + ctrl->applyAction(BeamAdapterAction::USE_TOOL_1); + EXPECT_EQ(ctrl->d_controlledInstrument.getValue(), 1); + + ctrl->applyAction(BeamAdapterAction::USE_TOOL_0); + EXPECT_EQ(ctrl->d_controlledInstrument.getValue(), 0); +} + +/// MOVE_FORWARD acts on the currently controlled instrument. +TEST_F(InterventionalRadiologyController_test, action_movesControlledInstrument) +{ + loadScene(twoInstrumentScene()); + auto* ctrl = getController(); + ASSERT_NE(ctrl, nullptr); + + const Real step = ctrl->d_step.getValue(); + + // NB: we snapshot instrument 0's tip rather than assuming it is exactly 0. + // When no instrument is deployed at init (all xtip == 0), applyInterventionalRadiologyController() + // refuses to build a zero-length wire and instead nudges the first instrument's tip to + // std::numeric_limits::epsilon() (~1.19e-7), then recomputes. + // See InterventionalRadiologyController.inl:784-790 ("if the totalLength is 0, move the first instrument"). + // Since init() calls that method at its end, d_xTip[0] already reads ~1e-7 here, not 0. + const Real x0Before = ctrl->d_xTip.getValue()[0]; + + ctrl->applyAction(BeamAdapterAction::USE_TOOL_1); + ctrl->applyAction(BeamAdapterAction::MOVE_FORWARD); + + // only the controlled instrument (1) advances by one step; instrument 0 is left untouched + EXPECT_NEAR(ctrl->d_xTip.getValue()[1], step, 1e-9); + EXPECT_NEAR(ctrl->d_xTip.getValue()[0], x0Before, 1e-9); +} + + +///////////////////////////////////// Getters / helpers /////////////////////////////////////////// + +TEST_F(InterventionalRadiologyController_test, getTotalNbEdges) +{ + EXPECT_MSG_NOEMIT(Error); + loadScene(singleInstrumentScene()); + auto* ctrl = getController(); + ASSERT_NE(ctrl, nullptr); + // RegularGridTopology nx=40 => 39 edges + EXPECT_EQ(ctrl->getTotalNbEdges(), 39); +} + +TEST_F(InterventionalRadiologyController_test, getInstrumentList) +{ + loadScene(twoInstrumentScene()); + auto* ctrl = getController(); + ASSERT_NE(ctrl, nullptr); + + type::vector*> list; + ctrl->getInstrumentList(list); + EXPECT_EQ(list.size(), 2u); + for (auto* instr : list) + EXPECT_NE(instr, nullptr); +} + +/// fixFirstNodesWithUntil(0) is rejected with a warning and resets indexFirstNode to 0. +TEST_F(InterventionalRadiologyController_test, fixFirstNodesWithUntil_zeroIsRejected) +{ + loadScene(singleInstrumentScene()); + auto* ctrl = getController(); + ASSERT_NE(ctrl, nullptr); + + EXPECT_MSG_EMIT(Warning); + ctrl->fixFirstNodesWithUntil(0); + EXPECT_EQ(ctrl->d_indexFirstNode.getValue(), 0u); +} + +/// fixFirstNodesWithUntil(k) fixes the first k-1 nodes and sets indexFirstNode to k-1. +TEST_F(InterventionalRadiologyController_test, fixFirstNodesWithUntil_setsIndexFirstNode) +{ + loadScene(singleInstrumentScene()); + auto* ctrl = getController(); + ASSERT_NE(ctrl, nullptr); + + ctrl->fixFirstNodesWithUntil(5); + EXPECT_EQ(ctrl->d_indexFirstNode.getValue(), 4u); +} + + +///////////////////////////////////// Deployment (stepping) /////////////////////////////////////// + +/// With a positive speed the tip advances by speed*dt each step and the tip node follows. +TEST_F(InterventionalRadiologyController_test, deploy_speedAdvancesTip) +{ + EXPECT_MSG_NOEMIT(Error); + loadAndInitRoot(deploymentScene("xtip='0' step='0.5' speed='10' controlledInstrument='0'")); + + auto* ctrl = getController(); + ASSERT_NE(ctrl, nullptr); + ASSERT_EQ(ctrl->getComponentState(), ComponentState::Valid); + + // The scene starts undeployed (xtip=0). Rather than build a zero-length wire, init() -> + // applyInterventionalRadiologyController() nudges the first tip to std::numeric_limits::epsilon() + // (~1.19e-7) and recomputes. See InterventionalRadiologyController.inl:784-790. + // So the tip starts at ~1e-7, not exactly 0 -- hence the tolerance instead of an == 0 check. + ASSERT_LT(ctrl->d_xTip.getValue()[0], 1e-6); + + const int nbSteps = 20; + for (int i = 0; i < nbSteps; ++i) + sofa::simulation::node::animate(m_root.get(), 0.01); + + // Each step advances the tip by speed*dt (see onBeginAnimationStep). After 20 steps: + // speed * dt * nbSteps = 10 * 0.01 * 20 = 2.0, plus the negligible initial epsilon nudge above. + const Real expectedTip = 10.0 * 0.01 * nbSteps; + EXPECT_NEAR(ctrl->d_xTip.getValue()[0], expectedTip, 1e-5); + + // the deployed length is reflected in the last curvilinear abscissa + const auto& curvAbs = ctrl->getCurrentCurvAbscisses(); + EXPECT_NEAR(curvAbs.back(), expectedTip, ctrl->d_threshold.getValue()); +} + +/// The tip is clamped to the instrument's total rest length. +TEST_F(InterventionalRadiologyController_test, deploy_tipClampedToRestLength) +{ + EXPECT_MSG_NOEMIT(Error); + // start already fully deployed, keep pushing forward + loadAndInitRoot(deploymentScene("xtip='100' step='0.5' speed='10' controlledInstrument='0'")); + + auto* ctrl = getController(); + ASSERT_NE(ctrl, nullptr); + + for (int i = 0; i < 5; ++i) + sofa::simulation::node::animate(m_root.get(), 0.01); + + // instrument rest length is 100 => xtip cannot exceed it + EXPECT_NEAR(ctrl->d_xTip.getValue()[0], 100.0, 1e-6); +} + +/// As the instrument deploys, the index of the first simulated node decreases. +TEST_F(InterventionalRadiologyController_test, deploy_indexFirstNodeDecreases) +{ + EXPECT_MSG_NOEMIT(Error); + loadAndInitRoot(deploymentScene("xtip='0' step='0.5' speed='10' controlledInstrument='0'")); + + auto* ctrl = getController(); + ASSERT_NE(ctrl, nullptr); + const unsigned int idxStart = ctrl->d_indexFirstNode.getValue(); + + for (int i = 0; i < 20; ++i) + sofa::simulation::node::animate(m_root.get(), 0.01); + + EXPECT_LT(ctrl->d_indexFirstNode.getValue(), idxStart); +} + +/// A longer run remains stable (no error) and keeps the component Valid. +TEST_F(InterventionalRadiologyController_test, deploy_runsStable) +{ + EXPECT_MSG_NOEMIT(Error); + loadAndInitRoot(deploymentScene("xtip='0' step='0.5' speed='5' controlledInstrument='0'")); + + auto* ctrl = getController(); + ASSERT_NE(ctrl, nullptr); + + for (int i = 0; i < 100; ++i) + sofa::simulation::node::animate(m_root.get(), 0.01); + + EXPECT_EQ(ctrl->getComponentState(), ComponentState::Valid); } } // namespace beamadapter_test From 01dee5faecf239245afb9a993b70ad759639e9be Mon Sep 17 00:00:00 2001 From: Frederick Roy Date: Thu, 16 Jul 2026 09:57:01 +0900 Subject: [PATCH 3/8] fix out-of-bounds when interpolating a node beyond the previous tip In applyInterventionalRadiologyController() STEP 3, the search for the previous node could leave prev_xId == m_nodeCurvAbs.size() (the "Case 1" path), leading to out-of-bounds reads of m_nodeCurvAbs[prev_xId] and m_idInstrumentCurvAbsTable[prev_xId]. Clamp prev_xId to the last node. --- .../controller/InterventionalRadiologyController.inl | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl b/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl index affde0f77..2e563cba2 100644 --- a/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl +++ b/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl @@ -850,6 +850,11 @@ void InterventionalRadiologyController::applyInterventionalRadiologyC break; } + // If no previous node was found beyond xCurvAbs (see the "Case 1" warning above), the loop + // ends with prev_xId == m_nodeCurvAbs.size(): clamp to the last node to avoid out-of-bounds access. + if (prev_xId >= m_nodeCurvAbs.size()) + prev_xId = m_nodeCurvAbs.size() - 1; + sofa::Index prev_globalNodeId = prev_numberOfUnactiveNodes + prev_xId; const Real prev_xCurvAbs = m_nodeCurvAbs[prev_xId]; From b70fffb5967fb567287221159acbb6071d6c4948 Mon Sep 17 00:00:00 2001 From: Frederick Roy Date: Thu, 16 Jul 2026 09:59:53 +0900 Subject: [PATCH 4/8] guard against a negative controlledInstrument --- ...InterventionalRadiologyController_test.cpp | 30 +++++++++++++++++++ .../InterventionalRadiologyController.inl | 4 +-- 2 files changed, 32 insertions(+), 2 deletions(-) diff --git a/BeamAdapter_test/component/controller/InterventionalRadiologyController_test.cpp b/BeamAdapter_test/component/controller/InterventionalRadiologyController_test.cpp index 580342fb2..1728e42a5 100644 --- a/BeamAdapter_test/component/controller/InterventionalRadiologyController_test.cpp +++ b/BeamAdapter_test/component/controller/InterventionalRadiologyController_test.cpp @@ -465,6 +465,20 @@ TEST_F(InterventionalRadiologyController_test, action_movesControlledInstrument) EXPECT_NEAR(ctrl->d_xTip.getValue()[0], x0Before, 1e-9); } +/// A negative controlledInstrument must be rejected (bounds check), not used as a negative index. +TEST_F(InterventionalRadiologyController_test, action_negativeControlledInstrument_isRejected) +{ + loadScene(singleInstrumentScene("controlledInstrument='-1'")); + auto* ctrl = getController(); + ASSERT_NE(ctrl, nullptr); + + const Real x0Before = ctrl->d_xTip.getValue()[0]; + + // applyAction returns early for an out-of-range (negative) instrument: no crash, no change. + ctrl->applyAction(BeamAdapterAction::MOVE_FORWARD); + EXPECT_NEAR(ctrl->d_xTip.getValue()[0], x0Before, 1e-9); +} + ///////////////////////////////////// Getters / helpers /////////////////////////////////////////// @@ -595,4 +609,20 @@ TEST_F(InterventionalRadiologyController_test, deploy_runsStable) EXPECT_EQ(ctrl->getComponentState(), ComponentState::Valid); } +/// A negative controlledInstrument during stepping must fall back to instrument 0 (no negative index). +TEST_F(InterventionalRadiologyController_test, deploy_negativeControlledInstrument_fallsBackToZero) +{ + loadAndInitRoot(deploymentScene("xtip='0' step='0.5' speed='10' controlledInstrument='-1'")); + + auto* ctrl = getController(); + ASSERT_NE(ctrl, nullptr); + ASSERT_EQ(ctrl->getComponentState(), ComponentState::Valid); + + for (int i = 0; i < 10; ++i) + sofa::simulation::node::animate(m_root.get(), 0.01); + + // the step is applied to the fallback instrument 0 instead of dereferencing index -1 + EXPECT_GT(ctrl->d_xTip.getValue()[0], 0.0); +} + } // namespace beamadapter_test diff --git a/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl b/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl index 2e563cba2..e8c1ee6a7 100644 --- a/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl +++ b/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl @@ -372,7 +372,7 @@ void InterventionalRadiologyController::onBeginAnimationStep(const do if(m_FF || m_RW) { int id = d_controlledInstrument.getValue(); - if (id >= (int)xInstrTip.size()) + if (id < 0 || id >= (int)xInstrTip.size()) { msg_warning()<<"Controlled Instument num "< void InterventionalRadiologyController::applyAction(BeamAdapterAction action) { int id = d_controlledInstrument.getValue(); - if (id >= int(m_instrumentsList.size())) + if (id < 0 || id >= int(m_instrumentsList.size())) { msg_warning() << "Controlled Instrument num " << id << " does not exist (size =" << m_instrumentsList.size() << ")."; return; From 3371907af0383484331a48239fcff44d348849b7 Mon Sep 17 00:00:00 2001 From: Frederick Roy Date: Thu, 16 Jul 2026 10:12:15 +0900 Subject: [PATCH 5/8] guard against an instrument with empty sampling parameters --- .../controller/InterventionalRadiologyController.inl | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl b/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl index e8c1ee6a7..95b9878a1 100644 --- a/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl +++ b/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl @@ -500,6 +500,14 @@ void InterventionalRadiologyController::computeInstrumentsCurvAbs(typ type::vector density_I; m_instrumentsList[i]->getSamplingParameters(xP_noticeable_I, density_I); // sampling of the different section of this instrument + // an instrument must provide at least one noticeable point; otherwise the loop below and the + // final key point access (xP_noticeable_I.size()-1) would read out of bounds. + if (xP_noticeable_I.empty()) + { + msg_error() << "Instrument " << i << " provides no sampling parameters (no noticeable point). Skipping it."; + continue; + } + // check each interval of noticeable point to see if they go out (>0) and use corresponding density to sample the interval. for (int j=0; j<(int)(xP_noticeable_I.size()-1); j++) { From cd33ab9ad472f0fd755bf36cebc22ec4a041ba5d Mon Sep 17 00:00:00 2001 From: Frederick Roy Date: Thu, 16 Jul 2026 10:19:11 +0900 Subject: [PATCH 6/8] fix segRemove sentinel dropping edge index 0 --- .../controller/InterventionalRadiologyController.inl | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl b/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl index 95b9878a1..0e1dc9fdf 100644 --- a/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl +++ b/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl @@ -611,10 +611,8 @@ void InterventionalRadiologyController::interventionalRadiologyCollis Real xAbsCurv = m_nodeCurvAbs[node]; int firstInstruOnx = m_idInstrumentCurvAbsTable[node][0]; - type::vector segRemove; - - for (unsigned int it=0; it segRemove(m_instrumentsList.size(), -1); for (int i = static_cast(xPointList.size()) - 1; i>=0; i--) { @@ -667,7 +665,7 @@ void InterventionalRadiologyController::interventionalRadiologyCollis for (unsigned int it=0; it Date: Thu, 16 Jul 2026 10:21:18 +0900 Subject: [PATCH 7/8] clamp node 1 in totalLengthIsChanging The monotonicity clamp on modifiedNodeCurvAbs started at i > 1, skipping node 1, which could then dip below node 0 (the origin) and feed unsorted curv abscissa into the STEP 3 interpolation. Start the clamp at i > 0. --- .../controller/InterventionalRadiologyController.inl | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl b/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl index 0e1dc9fdf..050e11c85 100644 --- a/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl +++ b/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl @@ -1022,7 +1022,9 @@ void InterventionalRadiologyController::totalLengthIsChanging(const t if (newTable[i].size() == 1) { modifiedNodeCurvAbs[i] -= dLength; - if (i > 1 && modifiedNodeCurvAbs[i] < modifiedNodeCurvAbs[i - 1]) + // keep the curv abscissa monotonic: clamp against the previous node (including node 0, the origin), + // otherwise applyInterventionalRadiologyController's interpolation would receive unsorted abscissa. + if (i > 0 && modifiedNodeCurvAbs[i] < modifiedNodeCurvAbs[i - 1]) { modifiedNodeCurvAbs[i] = modifiedNodeCurvAbs[i - 1]; } From dbdcfc03b8797f6b5dcc2f237d7fbd083d438c6d Mon Sep 17 00:00:00 2001 From: Frederick Roy Date: Thu, 16 Jul 2026 10:25:12 +0900 Subject: [PATCH 8/8] bound the STEP 5 rigid-segment loop The loop fixing rigid-segment border nodes could compute firstSimulatedNode + i == numberOfNodes (one past the last dof) and pass it to FixedProjectiveConstraint::addConstraint. Stop the loop before the index leaves the valid dof range. --- .../component/controller/InterventionalRadiologyController.inl | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl b/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl index 050e11c85..efba3afbe 100644 --- a/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl +++ b/src/BeamAdapter/component/controller/InterventionalRadiologyController.inl @@ -961,7 +961,8 @@ void InterventionalRadiologyController::applyInterventionalRadiologyC { RealConstIterator it = rigidCurvAbs->begin(); - for (unsigned int i=0; i::epsilon()) && newCurvAbs[i] > ((*it)- std::numeric_limits::epsilon())) // node= border of the rigid segment {