From a4cd7e6cf010cadf3d3b75109561e7efe3cbe4da Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Wed, 30 Jul 2025 23:07:23 +0200 Subject: [PATCH 1/8] [algorithm] check whether a constraint solver is found in the context at init --- src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 8100084c..7c3fc25c 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -72,6 +72,8 @@ class InsertionAlgorithm : public BaseAlgorithm { BaseAlgorithm::init(); this->getContext()->get(m_constraintSolver); + if (!m_constraintSolver) + msg_warning("No constraint solver found in context. Insertion algorithm is disabled."); } void draw(const core::visual::VisualParams* vparams) From 406ce5d42bd34aef36bcd572410458bc6349aef9 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Wed, 30 Jul 2025 23:35:20 +0200 Subject: [PATCH 2/8] [algorithm] check whether user provided acceptable values for thresholds used by the algorithm --- .../algorithm/InsertionAlgorithm.h | 26 ++++++++++++++----- 1 file changed, 20 insertions(+), 6 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 7c3fc25c..df1767c9 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -48,13 +48,11 @@ class InsertionAlgorithm : public BaseAlgorithm d_projective(initData( &d_projective, false, "projective", "Projection of closest detected proximity back onto the needle tip element.")), - d_punctureForceThreshold(initData(&d_punctureForceThreshold, - std::numeric_limits::max(), + d_punctureForceThreshold(initData(&d_punctureForceThreshold, -1., "punctureForceThreshold", "Threshold for the force applied to the needle tip. " "Once exceeded, puncture is initiated.")), - d_tipDistThreshold(initData(&d_tipDistThreshold, std::numeric_limits::min(), - "tipDistThreshold", + d_tipDistThreshold(initData(&d_tipDistThreshold, -1., "tipDistThreshold", "Threshold for the distance advanced by the needle tip since " "the last proximity detection. Once exceeded, a new " "proximity pair is added for the needle-volume coupling.")), @@ -72,8 +70,24 @@ class InsertionAlgorithm : public BaseAlgorithm { BaseAlgorithm::init(); this->getContext()->get(m_constraintSolver); - if (!m_constraintSolver) - msg_warning("No constraint solver found in context. Insertion algorithm is disabled."); + msg_warning_when(!m_constraintSolver) + << "No constraint solver found in context. Insertion algorithm is disabled."; + + if (d_punctureForceThreshold.getValue() < 0) + { + msg_warning() << d_punctureForceThreshold.getName() + + " parameter not defined or set to negative value." msgendl + << "Puncture will not function properly; provide a positive value"; + d_punctureForceThreshold.setValue(std::numeric_limits::max()); + } + + if (d_tipDistThreshold.getValue() < 0) + { + msg_warning() << d_tipDistThreshold.getName() + + " parameter not defined or set to negative value." msgendl + << "Needle-volume coupling is disabled; provide a positive value"; + d_tipDistThreshold.setValue(std::numeric_limits::max()); + } } void draw(const core::visual::VisualParams* vparams) From 3559b9d2736ea1d3cbf35da83155ca20665a7315 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Wed, 30 Jul 2025 23:41:46 +0200 Subject: [PATCH 3/8] [algorithm] sum all lambda values applied on the tipGeom mstate Still better to iterate and sum than accessing the first value (->[0]). It also makes sense in case one defines the tip as a collection of points (in case the needle has thickness). --- src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index df1767c9..276d2e9a 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -129,7 +129,9 @@ class InsertionAlgorithm : public BaseAlgorithm { const auto& lambda = m_constraintSolver->getLambda()[mstate.get()].read()->getValue(); - if (lambda[0].norm() > d_punctureForceThreshold.getValue()) + SReal norm{0.}; + for (const auto& l : lambda) norm += l.norm(); + if (norm > d_punctureForceThreshold.getValue()) { auto findClosestProxOnShaft = Operations::FindClosestProximity::Operation::get(l_shaftGeom); From 269e72cac99f7b21ae436a0fd6d18fc70cb72f5a Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 31 Jul 2025 10:27:45 +0200 Subject: [PATCH 4/8] [algorithm] Distinguish execution flow between puncture and insertion based on m_couplingPts.empty --- src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 276d2e9a..867cac1c 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -122,7 +122,7 @@ class InsertionAlgorithm : public BaseAlgorithm auto& collisionOutput = *d_collisionOutput.beginEdit(); auto& insertionOutput = *d_insertionOutput.beginEdit(); - if (insertionOutput.size() == 0) + if (m_couplingPts.empty()) { const MechStateTipType::SPtr mstate = l_tipGeom->getContext()->get(); if (m_constraintSolver) From 5ed212dcc60c1c0efb5cea537abfe461d2fd2ca3 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis <70031729+th-skam@users.noreply.github.com> Date: Thu, 7 Aug 2025 15:46:48 +0200 Subject: [PATCH 5/8] Apply changes #1 Co-authored-by: erik pernod --- src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 867cac1c..e388811f 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -48,7 +48,7 @@ class InsertionAlgorithm : public BaseAlgorithm d_projective(initData( &d_projective, false, "projective", "Projection of closest detected proximity back onto the needle tip element.")), - d_punctureForceThreshold(initData(&d_punctureForceThreshold, -1., + d_punctureForceThreshold(initData(&d_punctureForceThreshold, -1_sreal, "punctureForceThreshold", "Threshold for the force applied to the needle tip. " "Once exceeded, puncture is initiated.")), From 40354704e9dce5522f42ae0c64f3eb1284575036 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis <70031729+th-skam@users.noreply.github.com> Date: Thu, 7 Aug 2025 15:47:18 +0200 Subject: [PATCH 6/8] Apply changes #2 Co-authored-by: erik pernod --- src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index e388811f..276a284d 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -52,7 +52,7 @@ class InsertionAlgorithm : public BaseAlgorithm "punctureForceThreshold", "Threshold for the force applied to the needle tip. " "Once exceeded, puncture is initiated.")), - d_tipDistThreshold(initData(&d_tipDistThreshold, -1., "tipDistThreshold", + d_tipDistThreshold(initData(&d_tipDistThreshold, -1_sreal, "tipDistThreshold", "Threshold for the distance advanced by the needle tip since " "the last proximity detection. Once exceeded, a new " "proximity pair is added for the needle-volume coupling.")), From 103ce9e90e36ae4f401f9eff646218affb5f934e Mon Sep 17 00:00:00 2001 From: Themis Skamagkis <70031729+th-skam@users.noreply.github.com> Date: Thu, 7 Aug 2025 15:47:45 +0200 Subject: [PATCH 7/8] Apply changes #3 Co-authored-by: erik pernod --- src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 276a284d..c7609db1 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -129,7 +129,7 @@ class InsertionAlgorithm : public BaseAlgorithm { const auto& lambda = m_constraintSolver->getLambda()[mstate.get()].read()->getValue(); - SReal norm{0.}; + SReal norm{0_sreal}; for (const auto& l : lambda) norm += l.norm(); if (norm > d_punctureForceThreshold.getValue()) { From 3fab4a64f3cc7339d274751510f6a083a6cc20ab Mon Sep 17 00:00:00 2001 From: Themis Skamagkis <70031729+th-skam@users.noreply.github.com> Date: Thu, 7 Aug 2025 15:48:01 +0200 Subject: [PATCH 8/8] Apply change #4 - Style Co-authored-by: erik pernod --- src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index c7609db1..ee2dbac5 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -130,7 +130,9 @@ class InsertionAlgorithm : public BaseAlgorithm const auto& lambda = m_constraintSolver->getLambda()[mstate.get()].read()->getValue(); SReal norm{0_sreal}; - for (const auto& l : lambda) norm += l.norm(); + for (const auto& l : lambda) { + norm += l.norm(); + } if (norm > d_punctureForceThreshold.getValue()) { auto findClosestProxOnShaft =