Merge branch 'master' into operations-containsPoint

Author: th-skam

src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h

@@ -7,6 +7,7 @@
 #include <sofa/collisionAlgorithm/operations/FindClosestProximity.h>
 #include <sofa/collisionAlgorithm/operations/Project.h>
 #include <sofa/collisionAlgorithm/operations/ContainsPoint.h>
+#include <sofa/collisionAlgorithm/operations/NeedleOperations.h>
 #include <sofa/collisionAlgorithm/proximity/EdgeProximity.h>
 #include <sofa/collisionAlgorithm/proximity/TetrahedronProximity.h>
 #include <sofa/component/constraint/lagrangian/solver/ConstraintSolverImpl.h>
@@ -241,38 +242,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<EdgeProximity>(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());
             }
         }
 

src/sofa/collisionAlgorithm/operations/NeedleOperations.cpp

@@ -0,0 +1,31 @@
+#include <sofa/collisionAlgorithm/operations/NeedleOperations.h>
+
+namespace sofa::collisionAlgorithm::Operations::Needle
+{
+
+bool prunePointsUsingEdges(std::vector<BaseProximity::SPtr>& 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<EdgeElement>(&prunePointsUsingEdges);
+}  // namespace sofa::collisionAlgorithm::Operations::Needle

src/sofa/collisionAlgorithm/operations/NeedleOperations.h

@@ -0,0 +1,34 @@
+#pragma once
+
+#include <sofa/collisionAlgorithm/BaseOperation.h>
+#include <sofa/collisionAlgorithm/BaseProximity.h>
+#include <sofa/collisionAlgorithm/elements/EdgeElement.h>
+
+namespace sofa::collisionAlgorithm::Operations::Needle
+{
+
+class PrunePointsAheadOfTip
+    : public GenericOperation<PrunePointsAheadOfTip,  // Type of the operation
+                              bool,                   // Default return type
+                              std::vector<BaseProximity::SPtr>&,
+                              const BaseElement::SPtr&  // Parameters
+                              >
+{
+   public:
+    bool defaultFunc(std::vector<BaseProximity::SPtr>&, 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<BaseProximity::SPtr>& couplingPts,
+                           const EdgeElement::SPtr& edgeProx);
+
+}  // namespace sofa::collisionAlgorithm::Operations::Needle