From dde0315a7b4c121f8152a06368031e31947fa34f Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 18 Sep 2025 22:55:46 +0200 Subject: [PATCH 01/21] [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/21] [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/21] [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/21] [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/21] [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/21] [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/21] [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/21] [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/21] [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/21] [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/21] [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/21] [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/21] [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/21] [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/21] [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 = From 5a33a5cf7ac5f90e71cef46a6c6a79bc48a611cd Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Wed, 17 Sep 2025 14:32:03 +0200 Subject: [PATCH 16/21] [timer] Add the name of the AABBBroadPhase component during profiling --- src/CollisionAlgorithm/BaseAABBBroadPhase.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/CollisionAlgorithm/BaseAABBBroadPhase.h b/src/CollisionAlgorithm/BaseAABBBroadPhase.h index 3a65ba4d..e7cef83b 100644 --- a/src/CollisionAlgorithm/BaseAABBBroadPhase.h +++ b/src/CollisionAlgorithm/BaseAABBBroadPhase.h @@ -61,7 +61,7 @@ class SOFA_COLLISIONALGORITHM_API BaseAABBBroadPhase : public BaseGeometry::Broa } inline void doUpdate() { - sofa::helper::AdvancedTimer::stepBegin("========================= AABBBroadPhase do update ========================="); + sofa::helper::AdvancedTimer::stepBegin("AABBBroadPhase::doUpdate - "+this->getName()); m_Bmin = l_geometry->getPosition(0); m_Bmax = m_Bmin; @@ -116,7 +116,7 @@ class SOFA_COLLISIONALGORITHM_API BaseAABBBroadPhase : public BaseGeometry::Broa updateElemInBoxes(); // sofa::helper::AdvancedTimer::stepEnd("========================= Elements rangés dans boites in AABB doUpdate ========================="); - sofa::helper::AdvancedTimer::stepEnd("========================= AABBBroadPhase do update ========================="); + sofa::helper::AdvancedTimer::stepEnd("AABBBroadPhase::doUpdate - "+this->getName()); } From 1f78f088fcc74748d3073ea73c8651fc1328ab98 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Wed, 17 Sep 2025 13:35:15 +0200 Subject: [PATCH 17/21] [timer] Add a timer to the doDetection function --- src/CollisionAlgorithm/CollisionPipeline.h | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/CollisionAlgorithm/CollisionPipeline.h b/src/CollisionAlgorithm/CollisionPipeline.h index 75b3f65b..7dc7eb43 100644 --- a/src/CollisionAlgorithm/CollisionPipeline.h +++ b/src/CollisionAlgorithm/CollisionPipeline.h @@ -66,11 +66,9 @@ class SOFA_COLLISIONALGORITHM_API CollisionLoop : public core::objectmodel::Base void processObject(simulation::Node*, core::objectmodel::BaseObject* obj) { if (CollisionAlgorithm * component = dynamic_cast(obj)) { -// std::string timerName = std::string("-- Do detection : ") + obj->getName(); - -// sofa::helper::AdvancedTimer::stepBegin(timerName.c_str()); + sofa::helper::AdvancedTimer::stepBegin("doDetection - "+obj->getName()); component->doDetection(); -// sofa::helper::AdvancedTimer::stepEnd(timerName.c_str()); + sofa::helper::AdvancedTimer::stepEnd("doDetection - "+obj->getName()); } } From 9442d6164249dddb934385ee8fbf453dc80b3ec1 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 19 Sep 2025 16:42:02 +0200 Subject: [PATCH 18/21] [timer] Add a timer inside the doUpdate broadphase function --- src/CollisionAlgorithm/BaseAABBBroadPhase.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/CollisionAlgorithm/BaseAABBBroadPhase.h b/src/CollisionAlgorithm/BaseAABBBroadPhase.h index e7cef83b..f6ac3d15 100644 --- a/src/CollisionAlgorithm/BaseAABBBroadPhase.h +++ b/src/CollisionAlgorithm/BaseAABBBroadPhase.h @@ -66,7 +66,7 @@ class SOFA_COLLISIONALGORITHM_API BaseAABBBroadPhase : public BaseGeometry::Broa m_Bmax = m_Bmin; //updates bounding box area -// sofa::helper::AdvancedTimer::stepBegin("========================= BBox area update in AABBBroadPhase do update ========================="); + sofa::helper::AdvancedTimer::stepBegin("AABBBroadPhase BBox area update - "+this->getName()); for (unsigned j=1;jgetSize();j++) { type::Vec3 pos = l_geometry->getPosition(j); @@ -78,7 +78,7 @@ class SOFA_COLLISIONALGORITHM_API BaseAABBBroadPhase : public BaseGeometry::Broa } } -// sofa::helper::AdvancedTimer::stepEnd("========================= BBox area update in AABBBroadPhase do update ========================="); + sofa::helper::AdvancedTimer::stepEnd("AABBBroadPhase BBox area update - "+this->getName()); //fixes cell size for (int i = 0 ; i < 3 ; i++) { @@ -98,9 +98,9 @@ class SOFA_COLLISIONALGORITHM_API BaseAABBBroadPhase : public BaseGeometry::Broa } } -// sofa::helper::AdvancedTimer::stepBegin("========================= CLEAR MAP ========================="); + sofa::helper::AdvancedTimer::stepBegin("AABBBroadPhase clear map - "+this->getName()); newContainer(); -// sofa::helper::AdvancedTimer::stepEnd("========================= CLEAR MAP ========================="); + sofa::helper::AdvancedTimer::stepEnd("AABBBroadPhase clear map - "+this->getName()); // center in -0.5 cellwidth m_Bmin -= m_cellSize * 0.5; @@ -112,10 +112,10 @@ class SOFA_COLLISIONALGORITHM_API BaseAABBBroadPhase : public BaseGeometry::Broa return; } -// sofa::helper::AdvancedTimer::stepBegin("========================= Elements rangés dans boites in AABB doUpdate ========================="); + sofa::helper::AdvancedTimer::stepBegin("AABBBroadPhase updateElemInBoxes - " + this->getName()); updateElemInBoxes(); + sofa::helper::AdvancedTimer::stepEnd("AABBBroadPhase updateElemInBoxes - " + this->getName()); -// sofa::helper::AdvancedTimer::stepEnd("========================= Elements rangés dans boites in AABB doUpdate ========================="); sofa::helper::AdvancedTimer::stepEnd("AABBBroadPhase::doUpdate - "+this->getName()); } From 193b847d12cdd10de40521507401b0b1340eb609 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Wed, 17 Sep 2025 13:53:42 +0200 Subject: [PATCH 19/21] [timer] Add advanced timers in the InsertionAlgorithm --- src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h index 29ed4b3d..630a57e1 100644 --- a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -139,6 +139,8 @@ class SOFA_COLLISIONALGORITHM_API InsertionAlgorithm : public BaseAlgorithm if (m_couplingPts.empty() && l_surfGeom) { // Operations on surface geometry + sofa::helper::AdvancedTimer::stepBegin("Puncture detection - "+this->getName()); + auto findClosestProxOnSurf = Operations::FindClosestProximity::Operation::get(l_surfGeom); auto projectOnSurf = Operations::Project::Operation::get(l_surfGeom); @@ -185,8 +187,10 @@ class SOFA_COLLISIONALGORITHM_API InsertionAlgorithm : public BaseAlgorithm } } } + sofa::helper::AdvancedTimer::stepEnd("Puncture detection - "+this->getName()); // Shaft collision sequence - Disable if coupling points have been added + sofa::helper::AdvancedTimer::stepBegin("Shaft collision - "+this->getName()); if (d_enableShaftCollision.getValue() && m_couplingPts.empty() && l_shaftGeom) { auto createShaftProximity = @@ -220,10 +224,12 @@ class SOFA_COLLISIONALGORITHM_API InsertionAlgorithm : public BaseAlgorithm } } } + sofa::helper::AdvancedTimer::stepEnd("Shaft collision - "+this->getName()); } else { // Insertion sequence + sofa::helper::AdvancedTimer::stepBegin("Needle insertion - " + this->getName()); if (!d_enableInsertion.getValue() || !l_tipGeom || !l_volGeom || !l_shaftGeom) return; ElementIterator::SPtr itTip = l_tipGeom->begin(); @@ -322,8 +328,10 @@ class SOFA_COLLISIONALGORITHM_API InsertionAlgorithm : public BaseAlgorithm Operations::Needle::PrunePointsAheadOfTip::get(itShaft->getTypeInfo()); prunePointsAheadOfTip(m_couplingPts, itShaft->element()); } + sofa::helper::AdvancedTimer::stepEnd("Needle insertion - " + this->getName()); } + sofa::helper::AdvancedTimer::stepBegin("Reproject coupling points - "+this->getName()); if (d_enableInsertion.getValue() && !m_couplingPts.empty() && l_shaftGeom) { // Reprojection on shaft geometry sequence @@ -343,6 +351,7 @@ class SOFA_COLLISIONALGORITHM_API InsertionAlgorithm : public BaseAlgorithm // because the needle has exited abruptly. Thus, we clear the coupling points. if (insertionOutput.size() == 0) m_couplingPts.clear(); } + sofa::helper::AdvancedTimer::stepEnd("Reproject coupling points - "+this->getName()); d_collisionOutput.endEdit(); d_insertionOutput.endEdit(); From 0072225a24d88fb6e83d8f0fff18d4944db33709 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Sat, 20 Sep 2025 00:36:28 +0200 Subject: [PATCH 20/21] [algorithm] Modify loop to suppress warning --- 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 630a57e1..c9dd7f77 100644 --- a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -338,13 +338,13 @@ class SOFA_COLLISIONALGORITHM_API InsertionAlgorithm : public BaseAlgorithm auto findClosestProxOnShaft = Operations::FindClosestProximity::Operation::get(l_shaftGeom); auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom); - for (int i = 0; i < m_couplingPts.size(); i++) + for (const auto& cp : m_couplingPts) { const BaseProximity::SPtr shaftProx = findClosestProxOnShaft( - m_couplingPts[i], l_shaftGeom.get(), projectOnShaft, getFilterFunc()); + cp, l_shaftGeom.get(), projectOnShaft, getFilterFunc()); if (!shaftProx) continue; shaftProx->normalize(); - insertionOutput.add(shaftProx, m_couplingPts[i]); + insertionOutput.add(shaftProx, cp); } // 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 From dd3c4f2dfe546d49629056996eb584c73d78b06a Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Wed, 24 Sep 2025 10:40:05 +0200 Subject: [PATCH 21/21] [algorithm] Move function definitions in source file --- .../algorithm/InsertionAlgorithm.cpp | 317 +++++++++++++++++ .../algorithm/InsertionAlgorithm.h | 328 +----------------- 2 files changed, 325 insertions(+), 320 deletions(-) diff --git a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.cpp b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.cpp index 8b1736f1..4847bb6e 100644 --- a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.cpp +++ b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.cpp @@ -9,4 +9,321 @@ void registerInsertionAlgorithm(sofa::core::ObjectFactory* factory) "A class implementing a customized needle insertion algorithm") .add()); } + +InsertionAlgorithm::InsertionAlgorithm() + : l_tipGeom(initLink("tipGeom", "Link to the geometry structure of the needle tip.")), + l_surfGeom( + initLink("surfGeom", "Link to the geometry of the surface punctured by the needle.")), + l_shaftGeom(initLink("shaftGeom", "Link to the geometry structure of the needle shaft.")), + l_volGeom( + initLink("volGeom", "Link to the geometry of volume wherein the needle is inserted.")), + d_collisionOutput( + initData(&d_collisionOutput, "collisionOutput", "Detected proximities during puncture.")), + d_insertionOutput(initData(&d_insertionOutput, "insertionOutput", + "Detected proximities during insertion.")), + 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_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. " + "Once exceeded, puncture is initiated.")), + d_tipDistThreshold(initData(&d_tipDistThreshold, -1_sreal, "tipDistThreshold", + "Threshold for the distance advanced by the needle tip since " + "the last proximity detection. Once exceeded, a new " + "proximity pair is added for the needle-volume coupling.")), + m_constraintSolver(nullptr), + m_couplingPts(), + d_drawCollision(initData(&d_drawCollision, false, "drawcollision", "Draw collision.")), + d_drawPoints(initData(&d_drawPoints, false, "drawPoints", "Draw detection outputs.")), + d_drawPointsScale(initData(&d_drawPointsScale, 0.0005, "drawPointsScale", + "Scale the drawing of detection output points.")) +{ +} + +void InsertionAlgorithm::init() +{ + BaseAlgorithm::init(); + + if (d_enablePuncture.getValue()) + { + 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_enableInsertion.getValue()) + { + 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()); + } + } +} + +void InsertionAlgorithm::draw(const core::visual::VisualParams* vparams) +{ + if (!vparams->displayFlags().getShowCollisionModels() && !d_drawCollision.getValue()) return; + vparams->drawTool()->disableLighting(); + + const AlgorithmOutput& collisionOutput = d_collisionOutput.getValue(); + for (const auto& it : collisionOutput) + { + vparams->drawTool()->drawLine(it.first->getPosition(), it.second->getPosition(), + type::RGBAColor(0, 1, 0, 1)); + } + + const AlgorithmOutput& insertionOutput = d_insertionOutput.getValue(); + for (const auto& it : insertionOutput) + { + vparams->drawTool()->drawSphere(it.first->getPosition(), d_drawPointsScale.getValue(), + type::RGBAColor(1, 0, 1, 0.9)); + vparams->drawTool()->drawSphere(it.second->getPosition(), d_drawPointsScale.getValue(), + type::RGBAColor(0, 0, 1, 0.9)); + vparams->drawTool()->drawLine(it.first->getPosition(), it.second->getPosition(), + type::RGBAColor(1, 1, 0, 1)); + } +} + +void InsertionAlgorithm::doDetection() +{ + auto& collisionOutput = *d_collisionOutput.beginEdit(); + auto& insertionOutput = *d_insertionOutput.beginEdit(); + + insertionOutput.clear(); + collisionOutput.clear(); + + if (m_couplingPts.empty() && l_surfGeom) + { + // Operations on surface geometry + sofa::helper::AdvancedTimer::stepBegin("Puncture detection - " + this->getName()); + + auto findClosestProxOnSurf = Operations::FindClosestProximity::Operation::get(l_surfGeom); + auto projectOnSurf = Operations::Project::Operation::get(l_surfGeom); + + // Puncture sequence + if (d_enablePuncture.getValue() && l_tipGeom) + { + 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++) + { + BaseProximity::SPtr tipProx = createTipProximity(itTip->element()); + if (!tipProx) continue; + const BaseProximity::SPtr surfProx = findClosestProxOnSurf( + tipProx, l_surfGeom.get(), projectOnSurf, getFilterFunc()); + if (surfProx) + { + surfProx->normalize(); + + // 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_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); + } + } + } + sofa::helper::AdvancedTimer::stepEnd("Puncture detection - " + this->getName()); + + // Shaft collision sequence - Disable if coupling points have been added + sofa::helper::AdvancedTimer::stepBegin("Shaft collision - " + this->getName()); + if (d_enableShaftCollision.getValue() && m_couplingPts.empty() && 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()); + if (!shaftProx) continue; + const BaseProximity::SPtr surfProx = findClosestProxOnSurf( + shaftProx, l_surfGeom.get(), projectOnSurf, getFilterFunc()); + if (surfProx) + { + surfProx->normalize(); + + if (d_projective.getValue()) + { + // 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(); + } + collisionOutput.add(shaftProx, surfProx); + } + } + } + sofa::helper::AdvancedTimer::stepEnd("Shaft collision - " + this->getName()); + } + else + { + // Insertion sequence + sofa::helper::AdvancedTimer::stepBegin("Needle insertion - " + this->getName()); + if (!d_enableInsertion.getValue() || !l_tipGeom || !l_volGeom || !l_shaftGeom) return; + + ElementIterator::SPtr itTip = l_tipGeom->begin(); + auto createTipProximity = + Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo()); + const BaseProximity::SPtr tipProx = createTipProximity(itTip->element()); + if (!tipProx) return; + + // 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()) return; + + type::Vec3 lastCP = m_couplingPts.back()->getPosition(); + const SReal tipDistThreshold = this->d_tipDistThreshold.getValue(); + + // Vector from tip to last coupling point; used for distance and directional checks + const type::Vec3 tipToLastCP = lastCP - tipProx->getPosition(); + + // Only add a new coupling point if the needle tip has advanced far enough + if (tipToLastCP.norm() > tipDistThreshold) + { + // Prepare the operations before entering the loop + auto createShaftProximity = + Operations::CreateCenterProximity::Operation::get(l_shaftGeom->getTypeInfo()); + auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom); + auto findClosestProxOnVol = Operations::FindClosestProximity::Operation::get(l_volGeom); + auto projectOnVol = Operations::Project::Operation::get(l_volGeom); + auto containsPointInVol = + Operations::ContainsPointInProximity::Operation::get(l_volGeom); + + // Iterate over shaft segments to find which one contains the next candidate CP + for (auto itShaft = l_shaftGeom->begin(); itShaft != l_shaftGeom->end(); itShaft++) + { + BaseProximity::SPtr shaftProx = createShaftProximity(itShaft->element()); + if (!shaftProx) continue; + + const EdgeProximity::SPtr edgeProx = dynamic_pointer_cast(shaftProx); + if (!edgeProx) continue; + + const type::Vec3 p0 = edgeProx->element()->getP0()->getPosition(); + const type::Vec3 p1 = edgeProx->element()->getP1()->getPosition(); + const type::Vec3 shaftEdgeDir = (p1 - p0).normalized(); + const type::Vec3 lastCPToP1 = p1 - lastCP; + + // Skip if last CP lies after edge end point + if (dot(shaftEdgeDir, lastCPToP1) < 0_sreal) continue; + + const int numCPs = floor(lastCPToP1.norm() / tipDistThreshold); + + for (int idCP = 0; idCP < numCPs; idCP++) + { + // Candidate coupling point along shaft segment + const type::Vec3 candidateCP = lastCP + tipDistThreshold * shaftEdgeDir; + + // Project candidate CP onto the edge element and compute scalar coordinate + // along segment + const SReal edgeSegmentLength = (p1 - p0).norm(); + const type::Vec3 p0ToCandidateCP = candidateCP - p0; + const SReal projPtOnEdge = dot(p0ToCandidateCP, shaftEdgeDir); + + // Skip if candidate CP is outside current edge segment + if (projPtOnEdge < 0_sreal || projPtOnEdge > edgeSegmentLength) break; + + // Project candidate CP onto shaft geometry ... + shaftProx = projectOnShaft(candidateCP, itShaft->element()).prox; + if (!shaftProx) continue; + + // ... then find nearest volume proximity + const BaseProximity::SPtr volProx = findClosestProxOnVol( + 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 + // during insertion + if (containsPointInVol(shaftProx->getPosition(), volProx)) + { + volProx->normalize(); + m_couplingPts.push_back(volProx); + lastCP = volProx->getPosition(); + } + } + } + } + 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()); + } + sofa::helper::AdvancedTimer::stepEnd("Needle insertion - " + this->getName()); + } + + sofa::helper::AdvancedTimer::stepBegin("Reproject coupling points - " + this->getName()); + if (d_enableInsertion.getValue() && !m_couplingPts.empty() && l_shaftGeom) + { + // Reprojection on shaft geometry sequence + auto findClosestProxOnShaft = Operations::FindClosestProximity::Operation::get(l_shaftGeom); + auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom); + for (const auto& cp : m_couplingPts) + { + const BaseProximity::SPtr shaftProx = + findClosestProxOnShaft(cp, l_shaftGeom.get(), projectOnShaft, getFilterFunc()); + if (!shaftProx) continue; + shaftProx->normalize(); + insertionOutput.add(shaftProx, cp); + } + // 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(); + } + sofa::helper::AdvancedTimer::stepEnd("Reproject coupling points - " + this->getName()); + + d_collisionOutput.endEdit(); + d_insertionOutput.endEdit(); +} + } // namespace sofa::collisionalgorithm diff --git a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h index c9dd7f77..13ac70ca 100644 --- a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -36,326 +36,14 @@ class SOFA_COLLISIONALGORITHM_API InsertionAlgorithm : public BaseAlgorithm Data d_drawCollision, d_drawPoints; Data d_drawPointsScale; - InsertionAlgorithm() - : l_tipGeom(initLink("tipGeom", "Link to the geometry structure of the needle tip.")), - l_surfGeom( - initLink("surfGeom", "Link to the geometry of the surface punctured by the needle.")), - l_shaftGeom(initLink("shaftGeom", "Link to the geometry structure of the needle shaft.")), - l_volGeom(initLink("volGeom", - "Link to the geometry of volume wherein the needle is inserted.")), - d_collisionOutput(initData(&d_collisionOutput, "collisionOutput", - "Detected proximities during puncture.")), - d_insertionOutput(initData(&d_insertionOutput, "insertionOutput", - "Detected proximities during insertion.")), - 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_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. " - "Once exceeded, puncture is initiated.")), - d_tipDistThreshold(initData(&d_tipDistThreshold, -1_sreal, "tipDistThreshold", - "Threshold for the distance advanced by the needle tip since " - "the last proximity detection. Once exceeded, a new " - "proximity pair is added for the needle-volume coupling.")), - m_constraintSolver(nullptr), - m_couplingPts(), - d_drawCollision(initData(&d_drawCollision, false, "drawcollision", "Draw collision.")), - d_drawPoints(initData(&d_drawPoints, false, "drawPoints", "Draw detection outputs.")), - d_drawPointsScale(initData(&d_drawPointsScale, 0.0005, "drawPointsScale", - "Scale the drawing of detection output points.")) - { - } - - void init() override - { - BaseAlgorithm::init(); - - if (d_enablePuncture.getValue()) - { - 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_enableInsertion.getValue()) - { - 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()); - } - } - } - - void draw(const core::visual::VisualParams* vparams) - { - if (!vparams->displayFlags().getShowCollisionModels() && !d_drawCollision.getValue()) - return; - vparams->drawTool()->disableLighting(); - - const AlgorithmOutput& collisionOutput = d_collisionOutput.getValue(); - for (const auto& it : collisionOutput) - { - vparams->drawTool()->drawLine(it.first->getPosition(), it.second->getPosition(), - type::RGBAColor(0, 1, 0, 1)); - } - - const AlgorithmOutput& insertionOutput = d_insertionOutput.getValue(); - for (const auto& it : insertionOutput) - { - vparams->drawTool()->drawSphere(it.first->getPosition(), d_drawPointsScale.getValue(), - type::RGBAColor(1, 0, 1, 0.9)); - vparams->drawTool()->drawSphere(it.second->getPosition(), d_drawPointsScale.getValue(), - type::RGBAColor(0, 0, 1, 0.9)); - vparams->drawTool()->drawLine(it.first->getPosition(), it.second->getPosition(), - type::RGBAColor(1, 1, 0, 1)); - } - } - - void doDetection() - { - auto& collisionOutput = *d_collisionOutput.beginEdit(); - auto& insertionOutput = *d_insertionOutput.beginEdit(); - - insertionOutput.clear(); - collisionOutput.clear(); - - if (m_couplingPts.empty() && l_surfGeom) - { - // Operations on surface geometry - sofa::helper::AdvancedTimer::stepBegin("Puncture detection - "+this->getName()); - - auto findClosestProxOnSurf = - Operations::FindClosestProximity::Operation::get(l_surfGeom); - auto projectOnSurf = Operations::Project::Operation::get(l_surfGeom); - - // Puncture sequence - if (d_enablePuncture.getValue() && l_tipGeom) - { - 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++) - { - BaseProximity::SPtr tipProx = createTipProximity(itTip->element()); - if (!tipProx) continue; - const BaseProximity::SPtr surfProx = findClosestProxOnSurf( - tipProx, l_surfGeom.get(), projectOnSurf, getFilterFunc()); - if (surfProx) - { - surfProx->normalize(); - - // 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_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); - } - } - } - sofa::helper::AdvancedTimer::stepEnd("Puncture detection - "+this->getName()); - - // Shaft collision sequence - Disable if coupling points have been added - sofa::helper::AdvancedTimer::stepBegin("Shaft collision - "+this->getName()); - if (d_enableShaftCollision.getValue() && m_couplingPts.empty() && 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()); - if (!shaftProx) continue; - const BaseProximity::SPtr surfProx = findClosestProxOnSurf( - shaftProx, l_surfGeom.get(), projectOnSurf, getFilterFunc()); - if (surfProx) - { - surfProx->normalize(); - - if (d_projective.getValue()) - { - //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(); - } - collisionOutput.add(shaftProx, surfProx); - } - } - } - sofa::helper::AdvancedTimer::stepEnd("Shaft collision - "+this->getName()); - } - else - { - // Insertion sequence - sofa::helper::AdvancedTimer::stepBegin("Needle insertion - " + this->getName()); - if (!d_enableInsertion.getValue() || !l_tipGeom || !l_volGeom || !l_shaftGeom) return; - - ElementIterator::SPtr itTip = l_tipGeom->begin(); - auto createTipProximity = - Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo()); - const BaseProximity::SPtr tipProx = createTipProximity(itTip->element()); - if (!tipProx) return; - - // 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()) return; - - type::Vec3 lastCP = m_couplingPts.back()->getPosition(); - const SReal tipDistThreshold = this->d_tipDistThreshold.getValue(); - - // Vector from tip to last coupling point; used for distance and directional checks - const type::Vec3 tipToLastCP = lastCP - tipProx->getPosition(); - - // Only add a new coupling point if the needle tip has advanced far enough - if (tipToLastCP.norm() > tipDistThreshold) - { - // Prepare the operations before entering the loop - auto createShaftProximity = - Operations::CreateCenterProximity::Operation::get(l_shaftGeom->getTypeInfo()); - auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom); - auto findClosestProxOnVol = - Operations::FindClosestProximity::Operation::get(l_volGeom); - auto projectOnVol = Operations::Project::Operation::get(l_volGeom); - auto containsPointInVol = - Operations::ContainsPointInProximity::Operation::get(l_volGeom); - - // Iterate over shaft segments to find which one contains the next candidate CP - for (auto itShaft = l_shaftGeom->begin(); itShaft != l_shaftGeom->end(); itShaft++) - { - BaseProximity::SPtr shaftProx = createShaftProximity(itShaft->element()); - if (!shaftProx) continue; - - const EdgeProximity::SPtr edgeProx = - dynamic_pointer_cast(shaftProx); - if (!edgeProx) continue; - - const type::Vec3 p0 = edgeProx->element()->getP0()->getPosition(); - const type::Vec3 p1 = edgeProx->element()->getP1()->getPosition(); - const type::Vec3 shaftEdgeDir = (p1 - p0).normalized(); - const type::Vec3 lastCPToP1 = p1 - lastCP; - - // Skip if last CP lies after edge end point - if (dot(shaftEdgeDir, lastCPToP1) < 0_sreal) continue; - - const int numCPs = floor(lastCPToP1.norm() / tipDistThreshold); - - for (int idCP = 0; idCP < numCPs; idCP++) - { - // Candidate coupling point along shaft segment - const type::Vec3 candidateCP = lastCP + tipDistThreshold * shaftEdgeDir; - - // Project candidate CP onto the edge element and compute scalar coordinate - // along segment - const SReal edgeSegmentLength = (p1 - p0).norm(); - const type::Vec3 p0ToCandidateCP = candidateCP - p0; - const SReal projPtOnEdge = dot(p0ToCandidateCP, shaftEdgeDir); - - // Skip if candidate CP is outside current edge segment - if (projPtOnEdge < 0_sreal || projPtOnEdge > edgeSegmentLength) break; - - // Project candidate CP onto shaft geometry ... - shaftProx = projectOnShaft(candidateCP, itShaft->element()).prox; - if (!shaftProx) continue; - - // ... then find nearest volume proximity - const BaseProximity::SPtr volProx = findClosestProxOnVol( - 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 - // during insertion - if (containsPointInVol(shaftProx->getPosition(), volProx)) - { - volProx->normalize(); - m_couplingPts.push_back(volProx); - lastCP = volProx->getPosition(); - } - } - } - } - 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()); - } - sofa::helper::AdvancedTimer::stepEnd("Needle insertion - " + this->getName()); - } - - sofa::helper::AdvancedTimer::stepBegin("Reproject coupling points - "+this->getName()); - if (d_enableInsertion.getValue() && !m_couplingPts.empty() && l_shaftGeom) - { - // Reprojection on shaft geometry sequence - auto findClosestProxOnShaft = - Operations::FindClosestProximity::Operation::get(l_shaftGeom); - auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom); - for (const auto& cp : m_couplingPts) - { - const BaseProximity::SPtr shaftProx = findClosestProxOnShaft( - cp, l_shaftGeom.get(), projectOnShaft, getFilterFunc()); - if (!shaftProx) continue; - shaftProx->normalize(); - insertionOutput.add(shaftProx, cp); - } - // 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(); - } - sofa::helper::AdvancedTimer::stepEnd("Reproject coupling points - "+this->getName()); - - d_collisionOutput.endEdit(); - d_insertionOutput.endEdit(); - } + InsertionAlgorithm(); + void init() override; + void draw(const core::visual::VisualParams* vparams) override; + + /// Performs a proximity detection step during needle puncture and insertion. + /// Detection outputs are used to create collisions and needle-tissue coupling. + /// Handles puncture, shaft collisions and insertion into tissue phases. + void doDetection() override; }; } // namespace sofa::collisionalgorithm