From 203125439281c96acf252ccb394132abf6061f67 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Tue, 15 Jul 2025 11:19:18 +0200 Subject: [PATCH 1/2] [Needle][algorithm] Fix incorrect index of needle mstate in InsertionAlrogithm after changes in RigidMapping --- 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 cf290454..29d62574 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -95,7 +95,7 @@ class InsertionAlgorithm : public BaseAlgorithm { if (m_constraintSolver) { defaulttype::RigidTypes::Vec3 lambda = - m_constraintSolver->getLambda()[mstate].read()->getValue()[0].getVCenter(); + m_constraintSolver->getLambda()[mstate].read()->getValue()[mstate->getSize()-1].getVCenter(); if (lambda.norm() > d_punctureThreshold.getValue()) { for (const auto& itOutputPair : output) { From fed85676b1346a95f45d844aaf4df02f2f24c8a0 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Tue, 15 Jul 2025 11:41:43 +0200 Subject: [PATCH 2/2] [Needle][algorithm] Use the MechanicalState of the needle tip node to initiate puncture instead of the one in the master needle node --- .../algorithm/InsertionAlgorithm.h | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 29d62574..fa81c8fc 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -90,13 +90,18 @@ class InsertionAlgorithm : public BaseAlgorithm { auto& output = *d_output.beginEdit(); auto& outputInside = *d_outputInside.beginEdit(); - const sofa::component::statecontainer::MechanicalObject* mstate - = l_from->getContext()->get>(); + const sofa::component::statecontainer::MechanicalObject* mstate + = l_from->getContext()->get>(); + if (mstate->getSize() > 1) + { + msg_warning() << "Requested MechanicalObject, corresponding to the tip of the needle in the InsertionAlgorithm, has a size greater than 1. " + << "The algorithm is designed to work with a single point. Only the first element will be used."; + } if (m_constraintSolver) { - defaulttype::RigidTypes::Vec3 lambda = - m_constraintSolver->getLambda()[mstate].read()->getValue()[mstate->getSize()-1].getVCenter(); - if (lambda.norm() > d_punctureThreshold.getValue()) + defaulttype::Vec3Types::VecCoord lambda = + m_constraintSolver->getLambda()[mstate].read()->getValue(); + if (lambda[0].norm() > d_punctureThreshold.getValue()) { for (const auto& itOutputPair : output) { m_proximities.push_back(itOutputPair.second->copy());