[operations] Robustify retraction detection check using tip velocity

Author: th-skam

src/CollisionAlgorithm/operations/NeedleOperations.cpp

@@ -6,7 +6,7 @@ namespace sofa::collisionalgorithm::Operations::Needle
 bool prunePointsUsingEdges(std::vector<BaseProximity::SPtr>& couplingPts,
                            const EdgeElement::SPtr& edge)
 {
-    if (!edge) 
+    if (!edge)
     {
         msg_warning("Needle::PrunePointsAheadOfTip")
             << "Null element pointer in prunePointsUsingEdges; returning false";
@@ -16,6 +16,12 @@ bool prunePointsUsingEdges(std::vector<BaseProximity::SPtr>& couplingPts,
     const type::Vec3 tip(edge->getP1()->getPosition());
     const type::Vec3 edgeDirection = tip - edgeBase;
 
+    // Only prune if the tip is retracting (moving against the insertion direction). 
+    // NOTE: This uses the needle tip velocity only. If retraction results from needle-tissue 
+    //       relative movement, retraction is not detected.
+    const type::Vec3 tipVelocity = edge->getP1()->getVelocity();
+    if (dot(tipVelocity, edgeDirection) >= 0_sreal) return true;
+
     const int initSize = couplingPts.size();
 
     while(!couplingPts.empty())