From 4d0eee0b6b69b7b8df1c40c78d7110ca3b8a7f31 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 11 Sep 2025 10:48:28 +0200 Subject: [PATCH 01/10] [operations] Added a namespace for NeedleOperations and a function to remove points ahead of the tip --- .../operations/NeedleOperations.cpp | 8 +++++ .../operations/NeedleOperations.h | 30 +++++++++++++++++++ 2 files changed, 38 insertions(+) create mode 100644 src/sofa/collisionAlgorithm/operations/NeedleOperations.cpp create mode 100644 src/sofa/collisionAlgorithm/operations/NeedleOperations.h diff --git a/src/sofa/collisionAlgorithm/operations/NeedleOperations.cpp b/src/sofa/collisionAlgorithm/operations/NeedleOperations.cpp new file mode 100644 index 00000000..128990da --- /dev/null +++ b/src/sofa/collisionAlgorithm/operations/NeedleOperations.cpp @@ -0,0 +1,8 @@ +#include + +namespace sofa::collisionAlgorithm::Operations::Needle { + +//int register_Project_Edge = Operation::register_func(&toolbox::EdgeToolBox::project); + +} + diff --git a/src/sofa/collisionAlgorithm/operations/NeedleOperations.h b/src/sofa/collisionAlgorithm/operations/NeedleOperations.h new file mode 100644 index 00000000..7c80a23a --- /dev/null +++ b/src/sofa/collisionAlgorithm/operations/NeedleOperations.h @@ -0,0 +1,30 @@ +#pragma once + +#include +#include + +namespace sofa::collisionAlgorithm::Operations::Needle +{ + +class PrunePointsAheadOfTip + : public GenericOperation&, + const BaseProximity::SPtr& // Parameters + > +{ + public: + bool defaultFunc(std::vector&, const BaseProximity::SPtr&) const override + { + return false; + } + + void notFound(const std::type_info& id) const override + { + msg_error("Needle::PrunePointsAheadOfTip") + << "The operation PrunePointsAheadOfTipOperation is not registered with for type = " + << sofa::helper::NameDecoder::decodeFullName(id); + } +}; + +} // namespace sofa::collisionAlgorithm::Operations::Needle From 30c2cd3a5a5ba47c784dc2d83886728516050ad5 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 11 Sep 2025 11:11:23 +0200 Subject: [PATCH 02/10] [operations] Implemented a custom function for removing points based on edge direction --- .../operations/NeedleOperations.cpp | 19 +++++++++++++++++-- .../operations/NeedleOperations.h | 8 ++++++-- 2 files changed, 23 insertions(+), 4 deletions(-) diff --git a/src/sofa/collisionAlgorithm/operations/NeedleOperations.cpp b/src/sofa/collisionAlgorithm/operations/NeedleOperations.cpp index 128990da..dfd4fb4f 100644 --- a/src/sofa/collisionAlgorithm/operations/NeedleOperations.cpp +++ b/src/sofa/collisionAlgorithm/operations/NeedleOperations.cpp @@ -1,8 +1,23 @@ #include -namespace sofa::collisionAlgorithm::Operations::Needle { +namespace sofa::collisionAlgorithm::Operations::Needle +{ -//int register_Project_Edge = Operation::register_func(&toolbox::EdgeToolBox::project); +bool prunePointsUsingEdges(std::vector& couplingPts, + const EdgeElement::SPtr& edge) +{ + const type::Vec3 edgeBase(edge->getP0()->getPosition()); + const type::Vec3 tip(edge->getP1()->getPosition()); + const type::Vec3 edgeDirection = tip - edgeBase; + const type::Vec3 tip2Pt = couplingPts.back()->getPosition() - tip; + + // Positive dot product means the point is ahead of the tip + if (dot(tip2Pt, edgeDirection) > 0_sreal) couplingPts.pop_back(); + + return true; } +int register_PrunePointsAheadOfTip_Edge = + PrunePointsAheadOfTip::register_func(&prunePointsUsingEdges); +} // namespace sofa::collisionAlgorithm::Operations::Needle diff --git a/src/sofa/collisionAlgorithm/operations/NeedleOperations.h b/src/sofa/collisionAlgorithm/operations/NeedleOperations.h index 7c80a23a..8b0b553c 100644 --- a/src/sofa/collisionAlgorithm/operations/NeedleOperations.h +++ b/src/sofa/collisionAlgorithm/operations/NeedleOperations.h @@ -2,6 +2,7 @@ #include #include +#include namespace sofa::collisionAlgorithm::Operations::Needle { @@ -10,11 +11,11 @@ class PrunePointsAheadOfTip : public GenericOperation&, - const BaseProximity::SPtr& // Parameters + const BaseElement::SPtr& // Parameters > { public: - bool defaultFunc(std::vector&, const BaseProximity::SPtr&) const override + bool defaultFunc(std::vector&, const BaseElement::SPtr&) const override { return false; } @@ -27,4 +28,7 @@ class PrunePointsAheadOfTip } }; +bool prunePointsUsingEdges(std::vector& couplingPts, + const EdgeElement::SPtr& edgeProx); + } // namespace sofa::collisionAlgorithm::Operations::Needle From afda3a8fbec8c8bb75db7d35d1c65631f76ba811 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 11 Sep 2025 12:00:01 +0200 Subject: [PATCH 03/10] [algorithm] Use custom operation for point removal - avoid dynamic casts to edge element types --- .../algorithm/InsertionAlgorithm.h | 36 +++---------------- 1 file changed, 5 insertions(+), 31 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 2ab650fb..1a455834 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -6,6 +6,7 @@ #include #include #include +#include #include #include #include @@ -247,38 +248,11 @@ class InsertionAlgorithm : public BaseAlgorithm } else // Don't bother with removing the point that was just added { - // 2.2. Check whether coupling point should be removed + // Remove coupling points that are ahead of the tip in the insertion direction ElementIterator::SPtr itShaft = l_shaftGeom->begin(l_shaftGeom->getSize() - 2); - auto createShaftProximity = - Operations::CreateCenterProximity::Operation::get(itShaft->getTypeInfo()); - const BaseProximity::SPtr shaftProx = createShaftProximity(itShaft->element()); - if (shaftProx) - { - const EdgeProximity::SPtr edgeProx = - dynamic_pointer_cast(shaftProx); - if (edgeProx) - { - const type::Vec3 normal = (edgeProx->element()->getP1()->getPosition() - - edgeProx->element()->getP0()->getPosition()) - .normalized(); - // If the (last) coupling point lies ahead of the tip (positive dot product), - // the needle is retreating. Thus, that point is removed. - if (dot(tip2Pt, normal) > 0_sreal) - { - m_couplingPts.pop_back(); - } - } - else - { - msg_warning() << "shaftGeom: " << l_shaftGeom->getName() - << " is not an EdgeGeometry. Point removal is disabled"; - } - } - else - { - msg_warning() << "Cannot create proximity from shaftGeom: " - << l_shaftGeom->getName() << " - point removal is disabled"; - } + auto prunePointsAheadOfTip = + Operations::Needle::PrunePointsAheadOfTip::get(itShaft->getTypeInfo()); + prunePointsAheadOfTip(m_couplingPts, itShaft->element()); } } From 2fbd7ad30b73e7cce9d17178f1f3158a4644644c Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 11 Sep 2025 12:07:17 +0200 Subject: [PATCH 04/10] [operations] Message warning for empty element pointer or empty coupling points vector --- .../collisionAlgorithm/operations/NeedleOperations.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/sofa/collisionAlgorithm/operations/NeedleOperations.cpp b/src/sofa/collisionAlgorithm/operations/NeedleOperations.cpp index dfd4fb4f..dbde50a2 100644 --- a/src/sofa/collisionAlgorithm/operations/NeedleOperations.cpp +++ b/src/sofa/collisionAlgorithm/operations/NeedleOperations.cpp @@ -6,10 +6,18 @@ namespace sofa::collisionAlgorithm::Operations::Needle bool prunePointsUsingEdges(std::vector& couplingPts, const EdgeElement::SPtr& edge) { + if (!edge) + { + msg_warning("Needle::PrunePointsAheadOfTip") + << "Null element pointer in prunePointsUsingEdges; returning false"; + return false; + } const type::Vec3 edgeBase(edge->getP0()->getPosition()); const type::Vec3 tip(edge->getP1()->getPosition()); const type::Vec3 edgeDirection = tip - edgeBase; + + if (couplingPts.empty()) return true; const type::Vec3 tip2Pt = couplingPts.back()->getPosition() - tip; // Positive dot product means the point is ahead of the tip From 3e295737b141cab1f74fb4d16a980b0a6f1ac22d Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 11 Sep 2025 13:34:03 +0200 Subject: [PATCH 05/10] [operations] Wrongly defined return statements in point removal operation --- .../collisionAlgorithm/operations/NeedleOperations.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/sofa/collisionAlgorithm/operations/NeedleOperations.cpp b/src/sofa/collisionAlgorithm/operations/NeedleOperations.cpp index dbde50a2..7a8c49a7 100644 --- a/src/sofa/collisionAlgorithm/operations/NeedleOperations.cpp +++ b/src/sofa/collisionAlgorithm/operations/NeedleOperations.cpp @@ -17,13 +17,17 @@ bool prunePointsUsingEdges(std::vector& couplingPts, const type::Vec3 edgeDirection = tip - edgeBase; - if (couplingPts.empty()) return true; + if (couplingPts.empty()) return false; const type::Vec3 tip2Pt = couplingPts.back()->getPosition() - tip; // Positive dot product means the point is ahead of the tip - if (dot(tip2Pt, edgeDirection) > 0_sreal) couplingPts.pop_back(); + if (dot(tip2Pt, edgeDirection) > 0_sreal) + { + couplingPts.pop_back(); + return true; + } - return true; + return false; } int register_PrunePointsAheadOfTip_Edge = From ee940a400c886d7852d15cbeea7183390e9dfb05 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 11 Sep 2025 13:32:03 +0200 Subject: [PATCH 06/10] [operations] Fix operation removing only one coupling point per time step --- .../operations/NeedleOperations.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/sofa/collisionAlgorithm/operations/NeedleOperations.cpp b/src/sofa/collisionAlgorithm/operations/NeedleOperations.cpp index 7a8c49a7..02354972 100644 --- a/src/sofa/collisionAlgorithm/operations/NeedleOperations.cpp +++ b/src/sofa/collisionAlgorithm/operations/NeedleOperations.cpp @@ -14,20 +14,19 @@ bool prunePointsUsingEdges(std::vector& couplingPts, } const type::Vec3 edgeBase(edge->getP0()->getPosition()); const type::Vec3 tip(edge->getP1()->getPosition()); - const type::Vec3 edgeDirection = tip - edgeBase; - if (couplingPts.empty()) return false; - const type::Vec3 tip2Pt = couplingPts.back()->getPosition() - tip; + const int initSize = couplingPts.size(); - // Positive dot product means the point is ahead of the tip - if (dot(tip2Pt, edgeDirection) > 0_sreal) + while(!couplingPts.empty()) { + const type::Vec3 tip2Pt = couplingPts.back()->getPosition() - tip; + + // Negative dot product means the point is behind the tip + if(dot(tip2Pt, edgeDirection) < 0_sreal) break; couplingPts.pop_back(); - return true; } - - return false; + return (initSize == couplingPts.size()); } int register_PrunePointsAheadOfTip_Edge = From 4e64e683bf31952c31f79c6e8128fc6670cb1435 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 11 Sep 2025 13:43:34 +0200 Subject: [PATCH 07/10] [algorithm] update algorithm logic --- .../collisionAlgorithm/algorithm/InsertionAlgorithm.h | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 1a455834..abb4ad08 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -216,6 +216,12 @@ class InsertionAlgorithm : public BaseAlgorithm 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()); + // 2.1 Check whether coupling point should be added const type::Vec3 tip2Pt = m_couplingPts.back()->getPosition() - tipProx->getPosition(); if (tip2Pt.norm() > d_tipDistThreshold.getValue()) @@ -248,11 +254,6 @@ 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()); } } From c3a179b46ea72a03b6e17e7ad397191af5ace3cf Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 11 Sep 2025 13:59:47 +0200 Subject: [PATCH 08/10] [algorithm] Fix crash in case coupling points are all removed and vector is emptied --- .../algorithm/InsertionAlgorithm.h | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index abb4ad08..dddf55ab 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -216,11 +216,13 @@ class InsertionAlgorithm : public BaseAlgorithm 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()); + // 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; // 2.1 Check whether coupling point should be added const type::Vec3 tip2Pt = m_couplingPts.back()->getPosition() - tipProx->getPosition(); @@ -252,9 +254,6 @@ class InsertionAlgorithm : public BaseAlgorithm } } } - else // Don't bother with removing the point that was just added - { - } } if (!m_couplingPts.empty()) From 9a4f6d42085b9670d22cc49770e2222d444df2b4 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 19 Sep 2025 17:57:52 +0200 Subject: [PATCH 09/10] [operations] Fail-proof while loop with safety counter --- src/CollisionAlgorithm/operations/NeedleOperations.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/CollisionAlgorithm/operations/NeedleOperations.cpp b/src/CollisionAlgorithm/operations/NeedleOperations.cpp index a36a6869..c9a6dcef 100644 --- a/src/CollisionAlgorithm/operations/NeedleOperations.cpp +++ b/src/CollisionAlgorithm/operations/NeedleOperations.cpp @@ -18,6 +18,7 @@ bool prunePointsUsingEdges(std::vector& couplingPts, const int initSize = couplingPts.size(); + int safeCounter = initSize; while(!couplingPts.empty()) { const type::Vec3 tip2Pt = couplingPts.back()->getPosition() - tip; @@ -25,6 +26,14 @@ bool prunePointsUsingEdges(std::vector& couplingPts, // Negative dot product means the point is behind the tip if(dot(tip2Pt, edgeDirection) < 0_sreal) break; couplingPts.pop_back(); + + // Temporary safety to avoid infinite loops - remove when confident + safeCounter--; + if (safeCounter < 0) + { + msg_warning("Needle::PrunePointsAheadOfTip") << "Counter expired; breaking loop"; + break; + } } return (initSize == couplingPts.size()); } From 0b5ebb530a7ee1733d548463620b8cd1f3a82d1b Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 19 Sep 2025 18:17:11 +0200 Subject: [PATCH 10/10] [operations] Remove safety counter --- src/CollisionAlgorithm/operations/NeedleOperations.cpp | 9 --------- 1 file changed, 9 deletions(-) diff --git a/src/CollisionAlgorithm/operations/NeedleOperations.cpp b/src/CollisionAlgorithm/operations/NeedleOperations.cpp index c9a6dcef..a36a6869 100644 --- a/src/CollisionAlgorithm/operations/NeedleOperations.cpp +++ b/src/CollisionAlgorithm/operations/NeedleOperations.cpp @@ -18,7 +18,6 @@ bool prunePointsUsingEdges(std::vector& couplingPts, const int initSize = couplingPts.size(); - int safeCounter = initSize; while(!couplingPts.empty()) { const type::Vec3 tip2Pt = couplingPts.back()->getPosition() - tip; @@ -26,14 +25,6 @@ bool prunePointsUsingEdges(std::vector& couplingPts, // Negative dot product means the point is behind the tip if(dot(tip2Pt, edgeDirection) < 0_sreal) break; couplingPts.pop_back(); - - // Temporary safety to avoid infinite loops - remove when confident - safeCounter--; - if (safeCounter < 0) - { - msg_warning("Needle::PrunePointsAheadOfTip") << "Counter expired; breaking loop"; - break; - } } return (initSize == couplingPts.size()); }