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()); } } diff --git a/src/sofa/collisionAlgorithm/operations/NeedleOperations.cpp b/src/sofa/collisionAlgorithm/operations/NeedleOperations.cpp new file mode 100644 index 00000000..dbde50a2 --- /dev/null +++ b/src/sofa/collisionAlgorithm/operations/NeedleOperations.cpp @@ -0,0 +1,31 @@ +#include + +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 + 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 new file mode 100644 index 00000000..8b0b553c --- /dev/null +++ b/src/sofa/collisionAlgorithm/operations/NeedleOperations.h @@ -0,0 +1,34 @@ +#pragma once + +#include +#include +#include + +namespace sofa::collisionAlgorithm::Operations::Needle +{ + +class PrunePointsAheadOfTip + : public GenericOperation&, + const BaseElement::SPtr& // Parameters + > +{ + public: + bool defaultFunc(std::vector&, const BaseElement::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); + } +}; + +bool prunePointsUsingEdges(std::vector& couplingPts, + const EdgeElement::SPtr& edgeProx); + +} // namespace sofa::collisionAlgorithm::Operations::Needle