From 380ed1a74baebca07e854d812eb6a5d149dcc38a Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Mon, 4 Aug 2025 10:44:47 +0200 Subject: [PATCH 1/3] [algorithm] Add a proximity and then check for puncture + small clean & rework --- .../algorithm/InsertionAlgorithm.h | 59 +++++++++---------- 1 file changed, 29 insertions(+), 30 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 77f39136..1c8bab10 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -122,39 +122,11 @@ class InsertionAlgorithm : public BaseAlgorithm auto& insertionOutput = *d_insertionOutput.beginEdit(); insertionOutput.clear(); + collisionOutput.clear(); 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(); - 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); - auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom); - for (const auto& dpair : collisionOutput) - { - // Reproject onto the needle to create an EdgeProximity - the - // EdgeNormalHandler in the Constraint classes will need this - const BaseProximity::SPtr shaftProx = findClosestProxOnShaft( - dpair.second, l_shaftGeom.get(), projectOnShaft, getFilterFunc()); - m_couplingPts.push_back(dpair.second); - insertionOutput.add(shaftProx, dpair.second); - } - collisionOutput.clear(); - return; - } - } - - collisionOutput.clear(); - + // 1. Puncture algorithm auto createTipProximity = Operations::CreateCenterProximity::Operation::get(l_tipGeom->getTypeInfo()); auto findClosestProxOnSurf = @@ -171,6 +143,26 @@ class InsertionAlgorithm : public BaseAlgorithm if (surfProx) { surfProx->normalize(); + + // 1.1 Check whether puncture is happening - if so, create coupling point + if (m_constraintSolver) + { + const MechStateTipType::SPtr mstate = + l_tipGeom->getContext()->get(); + const auto& lambda = + m_constraintSolver->getLambda()[mstate.get()].read()->getValue(); + SReal norm{0.}; + for (const auto& l : lambda) { + norm += l.norm(); + } + if (norm > d_punctureForceThreshold.getValue()) + { + m_couplingPts.push_back(surfProx); + continue; + } + } + + // 1.2 If not, create a proximity pair for the tip-surface collision if (d_projective.getValue()) { tipProx = projectOnTip(surfProx->getPosition(), itTip.element()).prox; @@ -183,11 +175,13 @@ class InsertionAlgorithm : public BaseAlgorithm } else { + // 2. Needle insertion algorithm ElementIterator::SPtr itTip = l_tipGeom->begin(); auto createTipProximity = Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo()); const BaseProximity::SPtr tipProx = createTipProximity(itTip->element()); + // 2.1 Check whether coupling point should be added const type::Vec3 tip2Pt = m_couplingPts.back()->getPosition() - tipProx->getPosition(); if (tip2Pt.norm() > d_tipDistThreshold.getValue()) { @@ -204,6 +198,7 @@ class InsertionAlgorithm : public BaseAlgorithm } else // Don't bother with removing the point that was just added { + // 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()); @@ -216,7 +211,11 @@ class InsertionAlgorithm : public BaseAlgorithm m_couplingPts.pop_back(); } } + } + if (!m_couplingPts.empty()) + { + // 3. Re-project proximities on the shaft geometry auto findClosestProxOnShaft = Operations::FindClosestProximity::Operation::get(l_shaftGeom); auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom); From 128d306e77f3dbb596e935b149dc31ada66b3f1b Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 8 Aug 2025 13:03:21 +0200 Subject: [PATCH 2/3] [algorithm] Store variables retrieved with getValue() before entering a loop, whenever possible --- src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 1c8bab10..09c5c7e7 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -134,6 +134,8 @@ class InsertionAlgorithm : public BaseAlgorithm auto projectOnSurf = Operations::Project::Operation::get(l_surfGeom); auto projectOnTip = Operations::Project::Operation::get(l_tipGeom); + const bool isProjective = d_projective.getValue(); + const SReal punctureForceThreshold = d_punctureForceThreshold.getValue(); for (const auto& itTip : *l_tipGeom) { BaseProximity::SPtr tipProx = createTipProximity(itTip.element()); @@ -155,7 +157,7 @@ class InsertionAlgorithm : public BaseAlgorithm for (const auto& l : lambda) { norm += l.norm(); } - if (norm > d_punctureForceThreshold.getValue()) + if (norm > punctureForceThreshold) { m_couplingPts.push_back(surfProx); continue; @@ -163,7 +165,7 @@ class InsertionAlgorithm : public BaseAlgorithm } // 1.2 If not, create a proximity pair for the tip-surface collision - if (d_projective.getValue()) + if (isProjective) { tipProx = projectOnTip(surfProx->getPosition(), itTip.element()).prox; if (!tipProx) continue; From 2c3de44f3c678bab4a8d1984a36f922c42ce899b Mon Sep 17 00:00:00 2001 From: Themis Skamagkis <70031729+th-skam@users.noreply.github.com> Date: Fri, 8 Aug 2025 14:52:55 +0200 Subject: [PATCH 3/3] Adopt 0_sreal 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 09c5c7e7..5d8d0882 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -153,7 +153,7 @@ class InsertionAlgorithm : public BaseAlgorithm l_tipGeom->getContext()->get(); 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(); }