[algorithm] Fix bug concerning coupling point removal during retraction (#90)

* [operations] Added a namespace for NeedleOperations and a function to remove points ahead of the tip

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

* [algorithm] Use custom operation for point removal  - avoid dynamic casts to edge element types

* [operations] Message warning for empty element pointer or empty coupling points vector

* [operations] Wrongly defined return statements in point removal operation

* [operations] Fix operation removing only one coupling point per time step

* [algorithm] update algorithm logic

* [algorithm] Fix crash in case coupling points are all removed and vector is emptied

* [operations] Fail-proof while loop with safety counter

* [operations] Remove safety counter

Author: th-skam

src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h

@@ -216,6 +216,14 @@ class InsertionAlgorithm : public BaseAlgorithm
             const BaseProximity::SPtr tipProx = createTipProximity(itTip->element());
             if (!tipProx) return;
 
+            // Remove coupling points that are ahead of the tip in the insertion direction
+            ElementIterator::SPtr itShaft = l_shaftGeom->begin(l_shaftGeom->getSize() - 2);
+            auto prunePointsAheadOfTip = 
+                Operations::Needle::PrunePointsAheadOfTip::get(itShaft->getTypeInfo());
+            prunePointsAheadOfTip(m_couplingPts, itShaft->element());
+
+            if (m_couplingPts.empty()) return;
+
             type::Vec3 lastCP = m_couplingPts.back()->getPosition();
             const SReal tipDistThreshold = this->d_tipDistThreshold.getValue();
 
@@ -290,14 +298,6 @@ class InsertionAlgorithm : public BaseAlgorithm
                     }
                 }
             }
-            else  // Don't bother with removing the point that was just added
-            {
-                // Remove coupling points that are ahead of the tip in the insertion direction
-                ElementIterator::SPtr itShaft = l_shaftGeom->begin(l_shaftGeom->getSize() - 2);
-                auto prunePointsAheadOfTip = 
-                    Operations::Needle::PrunePointsAheadOfTip::get(itShaft->getTypeInfo());
-                prunePointsAheadOfTip(m_couplingPts, itShaft->element());
-            }
         }
 
         if (!m_couplingPts.empty())

src/CollisionAlgorithm/operations/NeedleOperations.cpp

@@ -14,16 +14,19 @@ bool prunePointsUsingEdges(std::vector<BaseProximity::SPtr>& couplingPts,
     }
     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;
+    const int initSize = couplingPts.size();
 
-    // Positive dot product means the point is ahead of the tip
-    if (dot(tip2Pt, edgeDirection) > 0_sreal) couplingPts.pop_back();
+    while(!couplingPts.empty())
+    {
+        const type::Vec3 tip2Pt = couplingPts.back()->getPosition() - tip;
 
-    return true;
+        // Negative dot product means the point is behind the tip
+        if(dot(tip2Pt, edgeDirection) < 0_sreal) break;
+        couplingPts.pop_back();
+    }
+    return (initSize == couplingPts.size());
 }
 
 int register_PrunePointsAheadOfTip_Edge =