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

Author: th-skam

src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h

@@ -216,11 +216,13 @@ 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());
+            // 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;
 
             // 2.1 Check whether coupling point should be added
             const type::Vec3 tip2Pt = m_couplingPts.back()->getPosition() - tipProx->getPosition();
@@ -252,9 +254,6 @@ class InsertionAlgorithm : public BaseAlgorithm
                     }
                 }
             }
-            else  // Don't bother with removing the point that was just added
-            {
-            }
         }
 
         if (!m_couplingPts.empty())