From dde0315a7b4c121f8152a06368031e31947fa34f Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 18 Sep 2025 22:55:46 +0200 Subject: [PATCH 01/15] [algorithm] Remove projection operation on tip in the InsertionAlgorithm- completely unnecessary --- src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h index ab95a71b..c9060676 100644 --- a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -136,7 +136,6 @@ 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 (auto itTip = l_tipGeom->begin(); itTip != l_tipGeom->end(); itTip++) { @@ -167,13 +166,7 @@ class InsertionAlgorithm : public BaseAlgorithm } } - // 1.2 If not, create a proximity pair for the tip-surface collision - if (isProjective) - { - tipProx = projectOnTip(surfProx->getPosition(), itTip->element()).prox; - if (!tipProx) continue; - tipProx->normalize(); - } + // ... if not, create a proximity pair for the tip-surface collision collisionOutput.add(tipProx, surfProx); } } From 71782d1b3f3d1998764c3da967bb2b80f9404d0e Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 4 Sep 2025 11:46:02 +0200 Subject: [PATCH 02/15] [algorithm] Changed FindClosestProximityAlgorithm to find the closest proximity back on the from geometry --- .../algorithm/FindClosestProximityAlgorithm.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/CollisionAlgorithm/algorithm/FindClosestProximityAlgorithm.h b/src/CollisionAlgorithm/algorithm/FindClosestProximityAlgorithm.h index c776cf7b..06cdf782 100644 --- a/src/CollisionAlgorithm/algorithm/FindClosestProximityAlgorithm.h +++ b/src/CollisionAlgorithm/algorithm/FindClosestProximityAlgorithm.h @@ -67,7 +67,10 @@ class FindClosestProximityAlgorithm : public BaseAlgorithm { if (d_projective.getValue()) { - auto pfromProj = projectFromOp(pdest->getPosition(),itfrom->element()).prox; + //auto pfromProj = projectFromOp(pdest->getPosition(),itfrom->element()).prox; + auto projectOnShaft = Operations::Project::Operation::get(l_from); + auto findClosestProxOnShaft = Operations::FindClosestProximity::Operation::get(l_from); + auto pfromProj = findClosestProxOnShaft(pdest, l_from, projectOnShaft, getFilterFunc()); if (pfromProj == nullptr) continue; pfromProj->normalize(); From 69ded0461b8971bdacfe3c0b591eca191ff7c562 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 18 Sep 2025 22:42:19 +0200 Subject: [PATCH 03/15] [algorithm] Auto-format code --- .../algorithm/InsertionAlgorithm.h | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h index c9060676..b6914efd 100644 --- a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -256,7 +256,7 @@ class InsertionAlgorithm : public BaseAlgorithm const int numCPs = floor(lastCPToP1.norm() / tipDistThreshold); - for(int idCP = 0 ; idCP < numCPs ; idCP++) + for (int idCP = 0; idCP < numCPs; idCP++) { // Candidate coupling point along shaft segment const type::Vec3 candidateCP = lastCP + tipDistThreshold * shaftEdgeDir; @@ -270,7 +270,7 @@ class InsertionAlgorithm : public BaseAlgorithm // Skip if candidate CP is outside current edge segment if (projPtOnEdge < 0_sreal || projPtOnEdge > edgeSegmentLength) break; - // Project candidate CP onto shaft geometry ... + // Project candidate CP onto shaft geometry ... shaftProx = projectOnShaft(candidateCP, itShaft->element()).prox; if (!shaftProx) continue; @@ -279,8 +279,8 @@ class InsertionAlgorithm : public BaseAlgorithm shaftProx, l_volGeom.get(), projectOnVol, getFilterFunc()); if (!volProx) continue; - // 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 + // 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 (containsPointInVol(shaftProx->getPosition(), volProx)) { @@ -291,6 +291,14 @@ class InsertionAlgorithm : public BaseAlgorithm } } } + else // Don't bother with removing the point that was just added + { + // Remove coupling points that are ahead of the tip in the insertion direction + ElementIterator::SPtr itShaft = l_shaftGeom->begin(l_shaftGeom->getSize() - 2); + auto prunePointsAheadOfTip = + Operations::Needle::PrunePointsAheadOfTip::get(itShaft->getTypeInfo()); + prunePointsAheadOfTip(m_couplingPts, itShaft->element()); + } } if (!m_couplingPts.empty()) From 38608bbce05a2aba0e815f514281b5dd5167c4f4 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 18 Sep 2025 22:45:45 +0200 Subject: [PATCH 04/15] [algorithm] Rewrite some comments --- .../algorithm/InsertionAlgorithm.h | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h index b6914efd..431d7a8b 100644 --- a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -3,11 +3,11 @@ #include #include #include +#include #include #include -#include -#include #include +#include #include #include #include @@ -128,7 +128,7 @@ class InsertionAlgorithm : public BaseAlgorithm if (m_couplingPts.empty()) { - // 1. Puncture algorithm + // Puncture sequence auto createTipProximity = Operations::CreateCenterProximity::Operation::get(l_tipGeom->getTypeInfo()); auto findClosestProxOnSurf = @@ -147,7 +147,7 @@ class InsertionAlgorithm : public BaseAlgorithm { surfProx->normalize(); - // 1.1 Check whether puncture is happening - if so, create coupling point + // Check whether puncture is happening - if so, create coupling point ... if (m_constraintSolver) { const MechStateTipType::SPtr mstate = @@ -171,7 +171,7 @@ class InsertionAlgorithm : public BaseAlgorithm } } - // 1.3 Collision with the shaft geometry + // Shaft collision sequence - Disable if coupling points have been added if (m_couplingPts.empty()) { auto createShaftProximity = @@ -187,7 +187,6 @@ class InsertionAlgorithm : public BaseAlgorithm { surfProx->normalize(); - // 1.2 If not, create a proximity pair for the tip-surface collision if (d_projective.getValue()) { shaftProx = @@ -202,7 +201,7 @@ class InsertionAlgorithm : public BaseAlgorithm } else { - // 2. Needle insertion algorithm + // Insertion sequence ElementIterator::SPtr itTip = l_tipGeom->begin(); auto createTipProximity = Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo()); @@ -303,7 +302,7 @@ class InsertionAlgorithm : public BaseAlgorithm if (!m_couplingPts.empty()) { - // 3. Re-project proximities on the shaft geometry + // Reprojection on shaft geometry sequence auto findClosestProxOnShaft = Operations::FindClosestProximity::Operation::get(l_shaftGeom); auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom); From 7241d3994f120d4028a90bbe605a52e4491efa88 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 18 Sep 2025 22:50:23 +0200 Subject: [PATCH 05/15] [algorithm] Place shared operations at the top --- src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h index 431d7a8b..325f91fe 100644 --- a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -128,12 +128,14 @@ class InsertionAlgorithm : public BaseAlgorithm if (m_couplingPts.empty()) { - // Puncture sequence - auto createTipProximity = - Operations::CreateCenterProximity::Operation::get(l_tipGeom->getTypeInfo()); + // Operations on surface geometry auto findClosestProxOnSurf = Operations::FindClosestProximity::Operation::get(l_surfGeom); auto projectOnSurf = Operations::Project::Operation::get(l_surfGeom); + + // Puncture sequence + auto createTipProximity = + Operations::CreateCenterProximity::Operation::get(l_tipGeom->getTypeInfo()); auto projectOnTip = Operations::Project::Operation::get(l_tipGeom); const SReal punctureForceThreshold = d_punctureForceThreshold.getValue(); From adf8d79b27f047cc39cdc3774c465c21bd0efa4f Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 18 Sep 2025 22:47:03 +0200 Subject: [PATCH 06/15] [algorithm] Add a bool to enable/disable the puncture sequence --- .../algorithm/InsertionAlgorithm.h | 67 ++++++++++--------- 1 file changed, 36 insertions(+), 31 deletions(-) diff --git a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h index 325f91fe..d7091feb 100644 --- a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -29,7 +29,7 @@ class InsertionAlgorithm : public BaseAlgorithm GeomLink l_tipGeom, l_surfGeom, l_shaftGeom, l_volGeom; Data d_collisionOutput, d_insertionOutput; - Data d_projective; + Data d_projective, d_enablePuncture; Data d_punctureForceThreshold, d_tipDistThreshold; ConstraintSolver::SPtr m_constraintSolver; std::vector m_couplingPts; @@ -50,6 +50,8 @@ class InsertionAlgorithm : public BaseAlgorithm d_projective(initData( &d_projective, false, "projective", "Projection of closest detected proximity back onto the needle tip element.")), + d_enablePuncture( + initData(&d_enablePuncture, true, "enablePuncture", "Enable puncture algorithm.")), d_punctureForceThreshold(initData(&d_punctureForceThreshold, -1_sreal, "punctureForceThreshold", "Threshold for the force applied to the needle tip. " @@ -134,42 +136,45 @@ class InsertionAlgorithm : public BaseAlgorithm auto projectOnSurf = Operations::Project::Operation::get(l_surfGeom); // Puncture sequence - auto createTipProximity = - Operations::CreateCenterProximity::Operation::get(l_tipGeom->getTypeInfo()); - auto projectOnTip = Operations::Project::Operation::get(l_tipGeom); - - const SReal punctureForceThreshold = d_punctureForceThreshold.getValue(); - for (auto itTip = l_tipGeom->begin(); itTip != l_tipGeom->end(); itTip++) + if (d_enablePuncture.getValue()) { - BaseProximity::SPtr tipProx = createTipProximity(itTip->element()); - if (!tipProx) continue; - const BaseProximity::SPtr surfProx = findClosestProxOnSurf( - tipProx, l_surfGeom.get(), projectOnSurf, getFilterFunc()); - if (surfProx) + auto createTipProximity = + Operations::CreateCenterProximity::Operation::get(l_tipGeom->getTypeInfo()); + auto projectOnTip = Operations::Project::Operation::get(l_tipGeom); + + const SReal punctureForceThreshold = d_punctureForceThreshold.getValue(); + for (auto itTip = l_tipGeom->begin(); itTip != l_tipGeom->end(); itTip++) { - surfProx->normalize(); - - // Check whether puncture is happening - if so, create coupling point ... - if (m_constraintSolver) + BaseProximity::SPtr tipProx = createTipProximity(itTip->element()); + if (!tipProx) continue; + const BaseProximity::SPtr surfProx = findClosestProxOnSurf( + tipProx, l_surfGeom.get(), projectOnSurf, getFilterFunc()); + if (surfProx) { - const MechStateTipType::SPtr mstate = - l_tipGeom->getContext()->get(); - const auto& lambda = - m_constraintSolver->getLambda()[mstate.get()].read()->getValue(); - SReal norm{0_sreal}; - for (const auto& l : lambda) - { - norm += l.norm(); - } - if (norm > punctureForceThreshold) + surfProx->normalize(); + + // Check whether puncture is happening - if so, create coupling point ... + if (m_constraintSolver) { - m_couplingPts.push_back(surfProx); - continue; + const MechStateTipType::SPtr mstate = + l_tipGeom->getContext()->get(); + const auto& lambda = + m_constraintSolver->getLambda()[mstate.get()].read()->getValue(); + SReal norm{0_sreal}; + for (const auto& l : lambda) + { + norm += l.norm(); + } + if (norm > punctureForceThreshold) + { + m_couplingPts.push_back(surfProx); + continue; + } } + + // ... if not, create a proximity pair for the tip-surface collision + collisionOutput.add(tipProx, surfProx); } - - // ... if not, create a proximity pair for the tip-surface collision - collisionOutput.add(tipProx, surfProx); } } From 62b077aaa8d56db5d1120bbe00f269ce2c2c1786 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 18 Sep 2025 23:09:59 +0200 Subject: [PATCH 07/15] [algorithm] Add a bool to enable/disable insertion sequence --- src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h index d7091feb..2a32eb24 100644 --- a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -29,7 +29,7 @@ class InsertionAlgorithm : public BaseAlgorithm GeomLink l_tipGeom, l_surfGeom, l_shaftGeom, l_volGeom; Data d_collisionOutput, d_insertionOutput; - Data d_projective, d_enablePuncture; + Data d_projective, d_enablePuncture, d_enableInsertion; Data d_punctureForceThreshold, d_tipDistThreshold; ConstraintSolver::SPtr m_constraintSolver; std::vector m_couplingPts; @@ -52,6 +52,8 @@ class InsertionAlgorithm : public BaseAlgorithm "Projection of closest detected proximity back onto the needle tip element.")), d_enablePuncture( initData(&d_enablePuncture, true, "enablePuncture", "Enable puncture algorithm.")), + d_enableInsertion( + initData(&d_enableInsertion, true, "enableInsertion", "Enable insertion algorithm.")), d_punctureForceThreshold(initData(&d_punctureForceThreshold, -1_sreal, "punctureForceThreshold", "Threshold for the force applied to the needle tip. " @@ -209,6 +211,8 @@ class InsertionAlgorithm : public BaseAlgorithm else { // Insertion sequence + if (!d_enableInsertion.getValue()) return; + ElementIterator::SPtr itTip = l_tipGeom->begin(); auto createTipProximity = Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo()); @@ -307,7 +311,7 @@ class InsertionAlgorithm : public BaseAlgorithm } } - if (!m_couplingPts.empty()) + if (d_enableInsertion.getValue() && !m_couplingPts.empty()) { // Reprojection on shaft geometry sequence auto findClosestProxOnShaft = From 4400991b4fbe118fbd76f55ae636d68d7eb0f473 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 18 Sep 2025 23:12:12 +0200 Subject: [PATCH 08/15] [algorithm] Add a bool to enable/disable shaft collision sequence --- src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h index 2a32eb24..b57ad8d3 100644 --- a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -29,7 +29,7 @@ class InsertionAlgorithm : public BaseAlgorithm GeomLink l_tipGeom, l_surfGeom, l_shaftGeom, l_volGeom; Data d_collisionOutput, d_insertionOutput; - Data d_projective, d_enablePuncture, d_enableInsertion; + Data d_projective, d_enablePuncture, d_enableInsertion, d_enableShaftCollision; Data d_punctureForceThreshold, d_tipDistThreshold; ConstraintSolver::SPtr m_constraintSolver; std::vector m_couplingPts; @@ -54,6 +54,8 @@ class InsertionAlgorithm : public BaseAlgorithm initData(&d_enablePuncture, true, "enablePuncture", "Enable puncture algorithm.")), d_enableInsertion( initData(&d_enableInsertion, true, "enableInsertion", "Enable insertion algorithm.")), + d_enableShaftCollision(initData(&d_enableShaftCollision, true, "enableShaftCollision", + "Enable shaft-surface collision.")), d_punctureForceThreshold(initData(&d_punctureForceThreshold, -1_sreal, "punctureForceThreshold", "Threshold for the force applied to the needle tip. " @@ -181,7 +183,7 @@ class InsertionAlgorithm : public BaseAlgorithm } // Shaft collision sequence - Disable if coupling points have been added - if (m_couplingPts.empty()) + if (d_enableShaftCollision.getValue() && m_couplingPts.empty()) { auto createShaftProximity = Operations::CreateCenterProximity::Operation::get(l_shaftGeom->getTypeInfo()); From ec35524c68c5de90b2766873495a8516e6247ab7 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 18 Sep 2025 23:23:04 +0200 Subject: [PATCH 09/15] [algorithm] auto format code --- src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h index b57ad8d3..b63fc5c4 100644 --- a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -145,7 +145,7 @@ class InsertionAlgorithm : public BaseAlgorithm auto createTipProximity = Operations::CreateCenterProximity::Operation::get(l_tipGeom->getTypeInfo()); auto projectOnTip = Operations::Project::Operation::get(l_tipGeom); - + const SReal punctureForceThreshold = d_punctureForceThreshold.getValue(); for (auto itTip = l_tipGeom->begin(); itTip != l_tipGeom->end(); itTip++) { @@ -156,7 +156,7 @@ class InsertionAlgorithm : public BaseAlgorithm if (surfProx) { surfProx->normalize(); - + // Check whether puncture is happening - if so, create coupling point ... if (m_constraintSolver) { @@ -175,7 +175,7 @@ class InsertionAlgorithm : public BaseAlgorithm continue; } } - + // ... if not, create a proximity pair for the tip-surface collision collisionOutput.add(tipProx, surfProx); } From 91ad07d8e3e9883bb373bc6c5f344a195541e161 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 18 Sep 2025 23:23:25 +0200 Subject: [PATCH 10/15] [algorithm] Absorb the FindClosestProximity for shaft collision --- src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h index b63fc5c4..3900d68a 100644 --- a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -205,6 +205,16 @@ class InsertionAlgorithm : public BaseAlgorithm if (!shaftProx) continue; shaftProx->normalize(); } + // Experimental + //else + //{ + // auto findClosestProxOnShaft = + // Operations::FindClosestProximity::Operation::get(l_shaftGeom); + // shaftProx = findClosestProxOnShaft(surfProx, l_shaftGeom, + // projectOnShaft, getFilterFunc()); + // if (!shaftProx) continue; + // shaftProx->normalize(); + //} collisionOutput.add(shaftProx, surfProx); } } From f57b8982cd35fef7e4a98d7b69a19660cba4f673 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Tue, 23 Sep 2025 12:24:47 +0200 Subject: [PATCH 11/15] [algorithm] Add an experimental component in the algorithm --- .../algorithm/InsertionAlgorithm.h | 21 ++++++++----------- 1 file changed, 9 insertions(+), 12 deletions(-) diff --git a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h index 3900d68a..b409d219 100644 --- a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -200,21 +200,18 @@ class InsertionAlgorithm : public BaseAlgorithm if (d_projective.getValue()) { - shaftProx = - projectOnShaft(surfProx->getPosition(), itShaft->element()).prox; + //shaftProx = + // projectOnShaft(surfProx->getPosition(), itShaft->element()).prox; + //if (!shaftProx) continue; + //shaftProx->normalize(); + // Experimental - This enables projection anywhere on the edge + auto findClosestProxOnShaft = + Operations::FindClosestProximity::Operation::get(l_shaftGeom); + shaftProx = findClosestProxOnShaft(surfProx, l_shaftGeom, + projectOnShaft, getFilterFunc()); if (!shaftProx) continue; shaftProx->normalize(); } - // Experimental - //else - //{ - // auto findClosestProxOnShaft = - // Operations::FindClosestProximity::Operation::get(l_shaftGeom); - // shaftProx = findClosestProxOnShaft(surfProx, l_shaftGeom, - // projectOnShaft, getFilterFunc()); - // if (!shaftProx) continue; - // shaftProx->normalize(); - //} collisionOutput.add(shaftProx, surfProx); } } From 181dc5409df0253971a2dbcb6fe5298caf7049b1 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Tue, 23 Sep 2025 12:50:20 +0200 Subject: [PATCH 12/15] [algorithm] Remove FindClosestProximityAlgorithm --- .../FindClosestProximityAlgorithm.cpp | 13 --- .../algorithm/FindClosestProximityAlgorithm.h | 90 ------------------- 2 files changed, 103 deletions(-) delete mode 100644 src/CollisionAlgorithm/algorithm/FindClosestProximityAlgorithm.cpp delete mode 100644 src/CollisionAlgorithm/algorithm/FindClosestProximityAlgorithm.h diff --git a/src/CollisionAlgorithm/algorithm/FindClosestProximityAlgorithm.cpp b/src/CollisionAlgorithm/algorithm/FindClosestProximityAlgorithm.cpp deleted file mode 100644 index 1761699b..00000000 --- a/src/CollisionAlgorithm/algorithm/FindClosestProximityAlgorithm.cpp +++ /dev/null @@ -1,13 +0,0 @@ -#include -#include - -namespace sofa::collisionalgorithm -{ -void registerFindClosestProximityAlgorithm(sofa::core::ObjectFactory* factory) -{ - factory->registerObjects( - sofa::core::ObjectRegistrationData( - "An algorithm to find the closest proximity between two BaseGeometry types") - .add()); -} -} // namespace sofa::collisionalgorithm diff --git a/src/CollisionAlgorithm/algorithm/FindClosestProximityAlgorithm.h b/src/CollisionAlgorithm/algorithm/FindClosestProximityAlgorithm.h deleted file mode 100644 index 06cdf782..00000000 --- a/src/CollisionAlgorithm/algorithm/FindClosestProximityAlgorithm.h +++ /dev/null @@ -1,90 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace sofa::collisionalgorithm { - -class FindClosestProximityAlgorithm : public BaseAlgorithm { -public: - SOFA_CLASS(FindClosestProximityAlgorithm, BaseAlgorithm); - - core::objectmodel::SingleLink l_from; - core::objectmodel::SingleLink l_dest; - Data d_drawCollision ; - Data > d_output; - Data d_projective ; -// Data > d_outputDist; - - FindClosestProximityAlgorithm() - : l_from(initLink("fromGeom", "link to from geometry")) - , l_dest(initLink("destGeom", "link to dest geometry")) - , d_drawCollision (initData(&d_drawCollision, true, "drawcollision", "draw collision")) - , d_output(initData(&d_output,"output", "output of the collision detection")) - , d_projective(initData(&d_projective, false,"projective", "projection of closest prox onto from element")) -// , d_outputDist(initData(&d_outputDist,"outputDist", "Distance of the outpu pair of detections")) - {} - - void draw(const core::visual::VisualParams* vparams) { - if (! vparams->displayFlags().getShowCollisionModels() && ! d_drawCollision.getValue()) return; - glDisable(GL_LIGHTING); - glColor4f(0,1,0,1); - - glBegin(GL_LINES); - DetectionOutput output = d_output.getValue() ; - for (unsigned i=0;igetPosition().data()); - glVertex3dv(output[i].second->getPosition().data()); - } - glEnd(); - } - - void doDetection() { - if (l_from == NULL) return; - if (l_dest == NULL) return; - - auto & output = *d_output.beginEdit(); - output.clear(); - - auto itfrom = l_from->begin(); - - auto createProximityOp = Operations::CreateCenterProximity::Operation::get(itfrom->getTypeInfo()); - auto findClosestProxOp = Operations::FindClosestProximity::Operation::get(l_dest); - auto projectOp = Operations::Project::Operation::get(l_dest); - auto projectFromOp = Operations::Project::Operation::get(l_from); - - for (;itfrom!=l_from->end();itfrom++) { - auto pfrom = createProximityOp(itfrom->element()); - if (pfrom == nullptr) continue; - - auto pdest = findClosestProxOp(pfrom, l_dest.get(), projectOp, getFilterFunc()); - if (pdest == nullptr) continue; - pdest->normalize(); - - - if (d_projective.getValue()) { - //auto pfromProj = projectFromOp(pdest->getPosition(),itfrom->element()).prox; - auto projectOnShaft = Operations::Project::Operation::get(l_from); - auto findClosestProxOnShaft = Operations::FindClosestProximity::Operation::get(l_from); - auto pfromProj = findClosestProxOnShaft(pdest, l_from, projectOnShaft, getFilterFunc()); - if (pfromProj == nullptr) continue; - pfromProj->normalize(); - - output.add(pfromProj,pdest); - } else { - output.add(pfrom,pdest); - } - - } - - d_output.endEdit(); - } - -}; - -} - From 6a25270ea0eff2ccb6d0672ba9c61a5f0c1052ef Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Tue, 23 Sep 2025 12:59:12 +0200 Subject: [PATCH 13/15] [cmake] Remove FindClosestProximityAlgorithm from CMakeLists --- CMakeLists.txt | 2 -- src/CollisionAlgorithm/initCollisionAlgorithm.cpp | 2 -- 2 files changed, 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 37ba1ed4..881390d4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -24,7 +24,6 @@ set(HEADER_FILES ${COLLISIONALGORITHM_SRC}/InternalData.h ${COLLISIONALGORITHM_SRC}/algorithm/Find2DClosestProximityAlgorithm.h - ${COLLISIONALGORITHM_SRC}/algorithm/FindClosestProximityAlgorithm.h ${COLLISIONALGORITHM_SRC}/algorithm/InsertionAlgorithm.h ${COLLISIONALGORITHM_SRC}/broadphase/AABBBroadPhase.h @@ -69,7 +68,6 @@ set(SOURCE_FILES ${COLLISIONALGORITHM_SRC}/CollisionPipeline.cpp ${COLLISIONALGORITHM_SRC}/algorithm/Find2DClosestProximityAlgorithm.cpp - ${COLLISIONALGORITHM_SRC}/algorithm/FindClosestProximityAlgorithm.cpp ${COLLISIONALGORITHM_SRC}/algorithm/InsertionAlgorithm.cpp ${COLLISIONALGORITHM_SRC}/broadphase/AABBBroadPhase.cpp diff --git a/src/CollisionAlgorithm/initCollisionAlgorithm.cpp b/src/CollisionAlgorithm/initCollisionAlgorithm.cpp index ba5b0849..539882a7 100644 --- a/src/CollisionAlgorithm/initCollisionAlgorithm.cpp +++ b/src/CollisionAlgorithm/initCollisionAlgorithm.cpp @@ -20,7 +20,6 @@ extern void registerCollisionLoop(sofa::core::ObjectFactory* factory); // Algorithms extern void registerFind2DClosestProximityAlgorithm(sofa::core::ObjectFactory* factory); -extern void registerFindClosestProximityAlgorithm(sofa::core::ObjectFactory* factory); extern void registerInsertionAlgorithm(sofa::core::ObjectFactory* factory); // BroadPhase @@ -90,7 +89,6 @@ void registerObjects(sofa::core::ObjectFactory* factory) registerCollisionLoop(factory); // Register Algorithms registerFind2DClosestProximityAlgorithm(factory); - registerFindClosestProximityAlgorithm(factory); registerInsertionAlgorithm(factory); // Register BroadPhase registerAABBBroadPhase(factory); From 7e138a6eacaa7bad7a7f1603a4f3b24993c0652b Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Tue, 23 Sep 2025 13:24:40 +0200 Subject: [PATCH 14/15] [algorithm] Make sanity checks happen only when appropriate for some data --- .../algorithm/InsertionAlgorithm.h | 32 +++++++++++-------- 1 file changed, 19 insertions(+), 13 deletions(-) diff --git a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h index b409d219..f0a5b865 100644 --- a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -76,24 +76,30 @@ class InsertionAlgorithm : public BaseAlgorithm void init() override { BaseAlgorithm::init(); - this->getContext()->get(m_constraintSolver); - msg_warning_when(!m_constraintSolver) - << "No constraint solver found in context. Insertion algorithm is disabled."; - if (d_punctureForceThreshold.getValue() < 0) + if (d_enablePuncture.getValue()) { - msg_warning() << d_punctureForceThreshold.getName() + - " parameter not defined or set to negative value." msgendl - << "Puncture will not function properly; provide a positive value"; - d_punctureForceThreshold.setValue(std::numeric_limits::max()); + this->getContext()->get(m_constraintSolver); + msg_warning_when(!m_constraintSolver) + << "No constraint solver found in context. Insertion algorithm is disabled."; + if (d_punctureForceThreshold.getValue() < 0) + { + msg_warning() << d_punctureForceThreshold.getName() + + " parameter not defined or set to negative value." msgendl + << "Puncture will not function properly; provide a positive value"; + d_punctureForceThreshold.setValue(std::numeric_limits::max()); + } } - if (d_tipDistThreshold.getValue() < 0) + if (d_enableInsertion.getValue()) { - msg_warning() << d_tipDistThreshold.getName() + - " parameter not defined or set to negative value." msgendl - << "Needle-volume coupling is disabled; provide a positive value"; - d_tipDistThreshold.setValue(std::numeric_limits::max()); + if (d_tipDistThreshold.getValue() < 0) + { + msg_warning() << d_tipDistThreshold.getName() + + " parameter not defined or set to negative value." msgendl + << "Needle-volume coupling is disabled; provide a positive value"; + d_tipDistThreshold.setValue(std::numeric_limits::max()); + } } } From c221011f42a59db9bf1ce866dd2e8c9c651cef97 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Tue, 23 Sep 2025 13:32:51 +0200 Subject: [PATCH 15/15] [algorithm] Place geometry checks in InsertionAlgorithm when they make sense --- .../algorithm/InsertionAlgorithm.h | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h index f0a5b865..7d78a0df 100644 --- a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -130,15 +130,13 @@ class InsertionAlgorithm : public BaseAlgorithm void doDetection() { - if (!l_tipGeom || !l_surfGeom || !l_shaftGeom || !l_volGeom) return; - auto& collisionOutput = *d_collisionOutput.beginEdit(); auto& insertionOutput = *d_insertionOutput.beginEdit(); insertionOutput.clear(); collisionOutput.clear(); - if (m_couplingPts.empty()) + if (m_couplingPts.empty() && l_surfGeom) { // Operations on surface geometry auto findClosestProxOnSurf = @@ -146,7 +144,7 @@ class InsertionAlgorithm : public BaseAlgorithm auto projectOnSurf = Operations::Project::Operation::get(l_surfGeom); // Puncture sequence - if (d_enablePuncture.getValue()) + if (d_enablePuncture.getValue() && l_tipGeom) { auto createTipProximity = Operations::CreateCenterProximity::Operation::get(l_tipGeom->getTypeInfo()); @@ -189,7 +187,7 @@ class InsertionAlgorithm : public BaseAlgorithm } // Shaft collision sequence - Disable if coupling points have been added - if (d_enableShaftCollision.getValue() && m_couplingPts.empty()) + if (d_enableShaftCollision.getValue() && m_couplingPts.empty() && l_shaftGeom) { auto createShaftProximity = Operations::CreateCenterProximity::Operation::get(l_shaftGeom->getTypeInfo()); @@ -226,7 +224,7 @@ class InsertionAlgorithm : public BaseAlgorithm else { // Insertion sequence - if (!d_enableInsertion.getValue()) return; + if (!d_enableInsertion.getValue() || !l_tipGeom || !l_volGeom || !l_shaftGeom) return; ElementIterator::SPtr itTip = l_tipGeom->begin(); auto createTipProximity = @@ -326,7 +324,7 @@ class InsertionAlgorithm : public BaseAlgorithm } } - if (d_enableInsertion.getValue() && !m_couplingPts.empty()) + if (d_enableInsertion.getValue() && !m_couplingPts.empty() && l_shaftGeom) { // Reprojection on shaft geometry sequence auto findClosestProxOnShaft =