[operations] Implemented a custom function for removing points based on edge direction

Author: th-skam

src/sofa/collisionAlgorithm/operations/NeedleOperations.cpp

@@ -1,8 +1,23 @@
 #include <sofa/collisionAlgorithm/operations/NeedleOperations.h>
 
-namespace sofa::collisionAlgorithm::Operations::Needle {
+namespace sofa::collisionAlgorithm::Operations::Needle
+{
 
-//int register_Project_Edge = Operation::register_func<EdgeProximity>(&toolbox::EdgeToolBox::project);
+bool prunePointsUsingEdges(std::vector<BaseProximity::SPtr>& 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<EdgeElement>(&prunePointsUsingEdges);
+}  // namespace sofa::collisionAlgorithm::Operations::Needle

src/sofa/collisionAlgorithm/operations/NeedleOperations.h

@@ -2,6 +2,7 @@
 
 #include <sofa/collisionAlgorithm/BaseOperation.h>
 #include <sofa/collisionAlgorithm/BaseProximity.h>
+#include <sofa/collisionAlgorithm/elements/EdgeElement.h>
 
 namespace sofa::collisionAlgorithm::Operations::Needle
 {
@@ -10,11 +11,11 @@ class PrunePointsAheadOfTip
     : public GenericOperation<PrunePointsAheadOfTip,  // Type of the operation
                               bool,                   // Default return type
                               std::vector<BaseProximity::SPtr>&,
-                              const BaseProximity::SPtr&  // Parameters
+                              const BaseElement::SPtr&  // Parameters
                               >
 {
    public:
-    bool defaultFunc(std::vector<BaseProximity::SPtr>&, const BaseProximity::SPtr&) const override
+    bool defaultFunc(std::vector<BaseProximity::SPtr>&, const BaseElement::SPtr&) const override
     {
         return false;
     }
@@ -27,4 +28,7 @@ class PrunePointsAheadOfTip
     }
 };
 
+bool prunePointsUsingEdges(std::vector<BaseProximity::SPtr>& couplingPts,
+                           const EdgeElement::SPtr& edgeProx);
+
 }  // namespace sofa::collisionAlgorithm::Operations::Needle