diff --git a/src/CollisionAlgorithm/operations/NeedleOperations.cpp b/src/CollisionAlgorithm/operations/NeedleOperations.cpp index a36a6869..7c51c08c 100644 --- a/src/CollisionAlgorithm/operations/NeedleOperations.cpp +++ b/src/CollisionAlgorithm/operations/NeedleOperations.cpp @@ -6,7 +6,7 @@ namespace sofa::collisionalgorithm::Operations::Needle bool prunePointsUsingEdges(std::vector& 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& 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& 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(&prunePointsUsingEdges); +int register_PrunePointsAheadOfTip_Edge = PrunePointsAheadOfTip::register_func(&prunePointsUsingEdges); + } // namespace sofa::collisionalgorithm::Operations::Needle diff --git a/src/CollisionAlgorithm/operations/NeedleOperations.h b/src/CollisionAlgorithm/operations/NeedleOperations.h index 184677a4..1c39f1c8 100644 --- a/src/CollisionAlgorithm/operations/NeedleOperations.h +++ b/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& couplingPts, const EdgeElement::SPtr& edgeProx);