From 5e31a8850ddc2cf0da1dc85a501cf925b0d5d366 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 1 Aug 2025 17:31:18 +0200 Subject: [PATCH 1/2] [algorithm] switched the order of coupling point removal and addition * no reason to simultaneously add and remove coupling points --- .../algorithm/InsertionAlgorithm.h | 31 ++++++++++--------- 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 29ddcdd4..363436e6 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -188,22 +188,8 @@ class InsertionAlgorithm : public BaseAlgorithm Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo()); const BaseProximity::SPtr tipProx = createTipProximity(itTip->element()); - 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); - const type::Vec3 normal = (edgeProx->element()->getP1()->getPosition() - - edgeProx->element()->getP0()->getPosition()) - .normalized(); const type::Vec3 ab = m_couplingPts.back()->getPosition() - tipProx->getPosition(); - const SReal dotProd = dot(ab, normal); - if (dotProd > 0.0) { - m_couplingPts.pop_back(); - } - - const SReal dist = ab.norm(); - if (dist > d_tipDistThreshold.getValue()) + if (ab.norm() > d_tipDistThreshold.getValue()) { auto findClosestProxOnVol = Operations::FindClosestProximity::Operation::get(l_volGeom); @@ -216,6 +202,21 @@ class InsertionAlgorithm : public BaseAlgorithm m_couplingPts.push_back(volProx); } } + else // Don't bother with removing the point that was just added + { + 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); + const type::Vec3 normal = (edgeProx->element()->getP1()->getPosition() - + edgeProx->element()->getP0()->getPosition()) + .normalized(); + const SReal dotProd = dot(ab, normal); + if (dotProd > 0.0) { + m_couplingPts.pop_back(); + } + } auto findClosestProxOnShaft = Operations::FindClosestProximity::Operation::get(l_shaftGeom); From 871e747e4b045977857862c8fdcde998d2e3ed2e Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 1 Aug 2025 17:35:32 +0200 Subject: [PATCH 2/2] [algorithm] rename the vector from the tip to the last coupling point --- src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 363436e6..77f39136 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -188,8 +188,8 @@ class InsertionAlgorithm : public BaseAlgorithm Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo()); const BaseProximity::SPtr tipProx = createTipProximity(itTip->element()); - const type::Vec3 ab = m_couplingPts.back()->getPosition() - tipProx->getPosition(); - if (ab.norm() > d_tipDistThreshold.getValue()) + const type::Vec3 tip2Pt = m_couplingPts.back()->getPosition() - tipProx->getPosition(); + if (tip2Pt.norm() > d_tipDistThreshold.getValue()) { auto findClosestProxOnVol = Operations::FindClosestProximity::Operation::get(l_volGeom); @@ -212,8 +212,7 @@ class InsertionAlgorithm : public BaseAlgorithm const type::Vec3 normal = (edgeProx->element()->getP1()->getPosition() - edgeProx->element()->getP0()->getPosition()) .normalized(); - const SReal dotProd = dot(ab, normal); - if (dotProd > 0.0) { + if (dot(tip2Pt, normal) > 0_sreal) { m_couplingPts.pop_back(); } }