From 4d0eee0b6b69b7b8df1c40c78d7110ca3b8a7f31 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 11 Sep 2025 10:48:28 +0200 Subject: [PATCH 1/4] [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 2/4] [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 3/4] [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 4/4] [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