diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 8100084c..ee2dbac5 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_sreal, "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_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.")), @@ -72,6 +70,24 @@ class InsertionAlgorithm : public BaseAlgorithm { BaseAlgorithm::init(); this->getContext()->get(m_constraintSolver); + 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) @@ -106,14 +122,18 @@ 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) { const auto& lambda = m_constraintSolver->getLambda()[mstate.get()].read()->getValue(); - if (lambda[0].norm() > d_punctureForceThreshold.getValue()) + SReal norm{0_sreal}; + for (const auto& l : lambda) { + norm += l.norm(); + } + if (norm > d_punctureForceThreshold.getValue()) { auto findClosestProxOnShaft = Operations::FindClosestProximity::Operation::get(l_shaftGeom);