From 33995a614c4b90cae5365badb6db46113ab2819b Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Mon, 11 Aug 2025 14:48:41 +0200 Subject: [PATCH 1/3] [algorithm] check whether the tip is inside tetra before adding the detected tetrahedron to the insertionOutput --- .../algorithm/InsertionAlgorithm.h | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index b8e89f6c..babb8216 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -7,6 +7,7 @@ #include #include #include +#include #include #include @@ -222,8 +223,18 @@ class InsertionAlgorithm : public BaseAlgorithm findClosestProxOnVol(tipProx, l_volGeom.get(), projectOnVol, getFilterFunc()); if (volProx) { - volProx->normalize(); - m_couplingPts.push_back(volProx); + TetrahedronProximity::SPtr tetProx = + dynamic_pointer_cast(volProx); + double f0(tetProx->f0()), f1(tetProx->f1()), f2(tetProx->f2()), + f3(tetProx->f3()); + bool isInTetra = toolbox::TetrahedronToolBox::isInTetra( + tipProx->getPosition(), tetProx->element()->getTetrahedronInfo(), f0, f1, + f2, f3); + if(isInTetra) + { + volProx->normalize(); + m_couplingPts.push_back(volProx); + } } } else // Don't bother with removing the point that was just added From 6cf9e7ca8e605850e6d40cefa8e43a570da2496c Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Mon, 11 Aug 2025 22:09:01 +0200 Subject: [PATCH 2/3] [algorithm] Check tetProx after downcasting to TetrahedronProximity --- .../algorithm/InsertionAlgorithm.h | 40 +++++++++++-------- 1 file changed, 23 insertions(+), 17 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index babb8216..4b66f693 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -155,7 +155,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) @@ -179,9 +180,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()); @@ -191,11 +192,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(); } @@ -225,21 +227,24 @@ class InsertionAlgorithm : public BaseAlgorithm { TetrahedronProximity::SPtr tetProx = dynamic_pointer_cast(volProx); - double f0(tetProx->f0()), f1(tetProx->f1()), f2(tetProx->f2()), - f3(tetProx->f3()); - bool isInTetra = toolbox::TetrahedronToolBox::isInTetra( - tipProx->getPosition(), tetProx->element()->getTetrahedronInfo(), f0, f1, - f2, f3); - if(isInTetra) + if (tetProx) { - volProx->normalize(); - m_couplingPts.push_back(volProx); + double f0(tetProx->f0()), f1(tetProx->f1()), f2(tetProx->f2()), + f3(tetProx->f3()); + bool isInTetra = toolbox::TetrahedronToolBox::isInTetra( + tipProx->getPosition(), tetProx->element()->getTetrahedronInfo(), f0, + f1, f2, f3); + if (isInTetra) + { + volProx->normalize(); + 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()); @@ -248,7 +253,8 @@ class InsertionAlgorithm : public BaseAlgorithm const type::Vec3 normal = (edgeProx->element()->getP1()->getPosition() - edgeProx->element()->getP0()->getPosition()) .normalized(); - if (dot(tip2Pt, normal) > 0_sreal) { + if (dot(tip2Pt, normal) > 0_sreal) + { m_couplingPts.pop_back(); } } From 97ad9dc2bac53544ed76b1aadaee6da636881106 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Wed, 13 Aug 2025 13:44:13 +0200 Subject: [PATCH 3/3] [algorithm] Added comments --- src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 4b66f693..48de1634 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -16,7 +16,7 @@ namespace sofa::collisionAlgorithm class InsertionAlgorithm : public BaseAlgorithm { - public: + public: SOFA_CLASS(InsertionAlgorithm, BaseAlgorithm); typedef core::behavior::MechanicalState MechStateTipType; @@ -223,6 +223,8 @@ class InsertionAlgorithm : public BaseAlgorithm auto projectOnVol = Operations::Project::Operation::get(l_volGeom); const BaseProximity::SPtr volProx = findClosestProxOnVol(tipProx, l_volGeom.get(), projectOnVol, getFilterFunc()); + // Proximity can be detected before the tip enters the tetra (e.g. near a boundary face) + // Only accept proximities if the tip is inside the tetra during insertion if (volProx) { TetrahedronProximity::SPtr tetProx = @@ -253,6 +255,8 @@ class InsertionAlgorithm : public BaseAlgorithm const type::Vec3 normal = (edgeProx->element()->getP1()->getPosition() - edgeProx->element()->getP0()->getPosition()) .normalized(); + // If the coupling point lies ahead of the tip (positive dot product), + // the needle is retreating. The last coupling point is removed if (dot(tip2Pt, normal) > 0_sreal) { m_couplingPts.pop_back();