[operations] Robustify retraction detection check using tip velocity (#117)

* [operations] Robustify retraction detection check using tip velocity

* [operations] Invert bool return from pruning operations

* Put method description into header file

---------

Co-authored-by: epernod <erik.pernod@gmail.com>

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 false;
+
     const int initSize = couplingPts.size();
 
     while(!couplingPts.empty())
@@ -26,9 +32,9 @@ bool prunePointsUsingEdges(std::vector<BaseProximity::SPtr>& couplingPts,
         if(dot(tip2Pt, edgeDirection) < 0_sreal) break;
         couplingPts.pop_back();
     }
-    return (initSize == couplingPts.size());
+    return (couplingPts.size() < initSize);
 }
 
-int register_PrunePointsAheadOfTip_Edge =
-    PrunePointsAheadOfTip::register_func<EdgeElement>(&prunePointsUsingEdges);
+int register_PrunePointsAheadOfTip_Edge = PrunePointsAheadOfTip::register_func<EdgeElement>(&prunePointsUsingEdges);
+
 }  // namespace sofa::collisionalgorithm::Operations::Needle

src/CollisionAlgorithm/operations/NeedleOperations.h

@@ -29,6 +29,11 @@ class SOFA_COLLISIONALGORITHM_API PrunePointsAheadOfTip
     }
 };
 
+/**
+* Returns true when at least one coupling point was popped from the back, 
+* false when the set was left unchanged (including the null-edge error path
+* and the non-retracting early-exit).
+*/
 bool prunePointsUsingEdges(std::vector<BaseProximity::SPtr>& couplingPts,
                            const EdgeElement::SPtr& edgeProx);