[algorithm] switched the order of coupling point removal and addition

  * no reason to simultaneously add and remove coupling points

Author: th-skam

src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h

@@ -186,20 +186,8 @@ class InsertionAlgorithm : public BaseAlgorithm
                 Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo());
             const BaseProximity::SPtr tipProx = createTipProximity(itTip->element());
 
-            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());
-            const EdgeProximity::SPtr edgeProx = dynamic_pointer_cast<EdgeProximity>(shaftProx);
-            const type::Vec3 normal = (edgeProx->element()->getP1()->getPosition() -
-                                       edgeProx->element()->getP0()->getPosition())
-                                          .normalized();
             const type::Vec3 ab = m_couplingPts.back()->getPosition() - tipProx->getPosition();
-            const SReal dotProd = dot(ab, normal);
-            if (dotProd > 0.0) m_couplingPts.pop_back();
-
-            const SReal dist = ab.norm();
-            if (dist > d_tipDistThreshold.getValue())
+            if (ab.norm() > d_tipDistThreshold.getValue())
             {
                 auto findClosestProxOnVol =
                     Operations::FindClosestProximity::Operation::get(l_volGeom);
@@ -212,6 +200,19 @@ class InsertionAlgorithm : public BaseAlgorithm
                     m_couplingPts.push_back(volProx);
                 }
             }
+            else // Don't bother with removing the point that was just added
+            {
+                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());
+                const EdgeProximity::SPtr edgeProx = dynamic_pointer_cast<EdgeProximity>(shaftProx);
+                const type::Vec3 normal = (edgeProx->element()->getP1()->getPosition() -
+                                           edgeProx->element()->getP0()->getPosition())
+                                              .normalized();
+                const SReal dotProd = dot(ab, normal);
+                if (dotProd > 0.0) m_couplingPts.pop_back();
+            }
 
             auto findClosestProxOnShaft =
                 Operations::FindClosestProximity::Operation::get(l_shaftGeom);