From a5d6c2d54c534a25e6864cb62c457b025d871293 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 8 Aug 2025 15:58:35 +0200 Subject: [PATCH 1/6] [algorithm] shaftProx in if check and normalize --- src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index a344dc73..36c76cfc 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -253,7 +253,10 @@ class InsertionAlgorithm : public BaseAlgorithm { const BaseProximity::SPtr shaftProx = findClosestProxOnShaft( m_couplingPts[i], l_shaftGeom.get(), projectOnShaft, getFilterFunc()); - insertionOutput.add(shaftProx, m_couplingPts[i]); + if(shaftProx) { + shaftProx->normalize(); + insertionOutput.add(shaftProx, m_couplingPts[i]); + } } } From fb4e2c70b176bd0a9237380c3e7e124c7c541dd3 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 8 Aug 2025 15:54:12 +0200 Subject: [PATCH 2/6] [algorithm] check for tipProx etc. --- src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 36c76cfc..66a12f08 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -210,6 +210,7 @@ class InsertionAlgorithm : public BaseAlgorithm auto createTipProximity = Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo()); const BaseProximity::SPtr tipProx = createTipProximity(itTip->element()); + if(!tipProx) return; // 2.1 Check whether coupling point should be added const type::Vec3 tip2Pt = m_couplingPts.back()->getPosition() - tipProx->getPosition(); From c8bd748e7560b4af5898488892cd716f7ea45a2f Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 8 Aug 2025 15:55:53 +0200 Subject: [PATCH 3/6] [algorithm] edgeProx in if check --- .../algorithm/InsertionAlgorithm.h | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 66a12f08..c922ca7e 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -235,11 +235,14 @@ class InsertionAlgorithm : public BaseAlgorithm Operations::CreateCenterProximity::Operation::get(itShaft->getTypeInfo()); const BaseProximity::SPtr shaftProx = createShaftProximity(itShaft->element()); const EdgeProximity::SPtr edgeProx = dynamic_pointer_cast(shaftProx); - const type::Vec3 normal = (edgeProx->element()->getP1()->getPosition() - - edgeProx->element()->getP0()->getPosition()) - .normalized(); - if (dot(tip2Pt, normal) > 0_sreal) { - m_couplingPts.pop_back(); + if(edgeProx) + { + const type::Vec3 normal = (edgeProx->element()->getP1()->getPosition() - + edgeProx->element()->getP0()->getPosition()) + .normalized(); + if (dot(tip2Pt, normal) > 0_sreal) { + m_couplingPts.pop_back(); + } } } } From 04a5255858274784a85af6c4b1ffcd21498e4039 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Wed, 13 Aug 2025 14:30:25 +0200 Subject: [PATCH 4/6] [algorithm] Added comments --- .../algorithm/InsertionAlgorithm.h | 52 +++++++++++++------ 1 file changed, 35 insertions(+), 17 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 3d3c097e..a02aad3c 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -154,7 +154,8 @@ class InsertionAlgorithm : public BaseAlgorithm const auto& lambda = m_constraintSolver->getLambda()[mstate.get()].read()->getValue(); SReal norm{0_sreal}; - for (const auto& l : lambda) { + for (const auto& l : lambda) + { norm += l.norm(); } if (norm > punctureForceThreshold) @@ -178,9 +179,9 @@ class InsertionAlgorithm : public BaseAlgorithm // 1.3 Collision with the shaft geometry if (collisionOutput.size()) { - auto createShaftProximity = - Operations::CreateCenterProximity::Operation::get(l_shaftGeom->getTypeInfo()); - auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom); + auto createShaftProximity = + Operations::CreateCenterProximity::Operation::get(l_shaftGeom->getTypeInfo()); + auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom); for (auto itShaft = l_shaftGeom->begin(); itShaft != l_shaftGeom->end(); itShaft++) { BaseProximity::SPtr shaftProx = createShaftProximity(itShaft->element()); @@ -190,11 +191,12 @@ class InsertionAlgorithm : public BaseAlgorithm if (surfProx) { surfProx->normalize(); - + // 1.2 If not, create a proximity pair for the tip-surface collision if (d_projective.getValue()) { - shaftProx = projectOnShaft(surfProx->getPosition(), itShaft->element()).prox; + shaftProx = + projectOnShaft(surfProx->getPosition(), itShaft->element()).prox; if (!shaftProx) continue; shaftProx->normalize(); } @@ -210,7 +212,7 @@ class InsertionAlgorithm : public BaseAlgorithm auto createTipProximity = Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo()); const BaseProximity::SPtr tipProx = createTipProximity(itTip->element()); - if(!tipProx) return; + if (!tipProx) return; // 2.1 Check whether coupling point should be added const type::Vec3 tip2Pt = m_couplingPts.back()->getPosition() - tipProx->getPosition(); @@ -227,23 +229,38 @@ class InsertionAlgorithm : public BaseAlgorithm m_couplingPts.push_back(volProx); } } - else // Don't bother with removing the point that was just added + else // Don't bother with removing the point that was just added { - // 2.2. Check whether coupling point should be removed + // 2.2. Check whether coupling point should be removed ElementIterator::SPtr itShaft = l_shaftGeom->begin(l_shaftGeom->getSize() - 2); auto createShaftProximity = Operations::CreateCenterProximity::Operation::get(itShaft->getTypeInfo()); const BaseProximity::SPtr shaftProx = createShaftProximity(itShaft->element()); - const EdgeProximity::SPtr edgeProx = dynamic_pointer_cast(shaftProx); - if(edgeProx) + if (shaftProx) { - const type::Vec3 normal = (edgeProx->element()->getP1()->getPosition() - - edgeProx->element()->getP0()->getPosition()) - .normalized(); - if (dot(tip2Pt, normal) > 0_sreal) { - m_couplingPts.pop_back(); + const EdgeProximity::SPtr edgeProx = + dynamic_pointer_cast(shaftProx); + if (edgeProx) + { + const type::Vec3 normal = (edgeProx->element()->getP1()->getPosition() - + edgeProx->element()->getP0()->getPosition()) + .normalized(); + if (dot(tip2Pt, normal) > 0_sreal) + { + m_couplingPts.pop_back(); + } + } + else + { + msg_warning() << "shaftGeom: " << l_shaftGeom->getName() + << " is not an EdgeGeometry. Point removal is disabled"; } } + else + { + msg_warning() << "Cannot create proximity from shaftGeom: " + << l_shaftGeom->getName() << " - point removal is disabled"; + } } } @@ -257,7 +274,8 @@ class InsertionAlgorithm : public BaseAlgorithm { const BaseProximity::SPtr shaftProx = findClosestProxOnShaft( m_couplingPts[i], l_shaftGeom.get(), projectOnShaft, getFilterFunc()); - if(shaftProx) { + if (shaftProx) + { shaftProx->normalize(); insertionOutput.add(shaftProx, m_couplingPts[i]); } From 68e2a6c2956583f91b3df3cb7b135723d3495e71 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis <70031729+th-skam@users.noreply.github.com> Date: Thu, 14 Aug 2025 09:17:00 +0200 Subject: [PATCH 5/6] [algorithm] More compact syntax for shaftProx check in loop --- .../collisionAlgorithm/algorithm/InsertionAlgorithm.h | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index fefc9798..5b7c0392 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -292,11 +292,9 @@ class InsertionAlgorithm : public BaseAlgorithm { const BaseProximity::SPtr shaftProx = findClosestProxOnShaft( m_couplingPts[i], l_shaftGeom.get(), projectOnShaft, getFilterFunc()); - if (shaftProx) - { - shaftProx->normalize(); - insertionOutput.add(shaftProx, m_couplingPts[i]); - } + if (!shaftProx) continue; + shaftProx->normalize(); + insertionOutput.add(shaftProx, m_couplingPts[i]); } } From 2b562051e6ff59a642cb8cad267ffeef1c38becc Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Wed, 13 Aug 2025 22:23:59 +0200 Subject: [PATCH 6/6] [algorithm] Added a final check for coupling points removal --- src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 5b7c0392..01ece77d 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -296,6 +296,11 @@ class InsertionAlgorithm : public BaseAlgorithm shaftProx->normalize(); insertionOutput.add(shaftProx, m_couplingPts[i]); } + // This is a final-frontier check: If there are coupling points stored, but the + // findClosestProxOnShaf operation yields no proximities on the shaft, it could be + // because the needle has exited abruptly. Thus, we clear the coupling points. + if (insertionOutput.size() == 0) + m_couplingPts.clear(); } d_collisionOutput.endEdit();