[algorithm] Refactor: Rework order of removal/addition of coupling points (#52)

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

  * no reason to simultaneously add and remove coupling points

* [algorithm] rename the vector from the tip to the last coupling point

Author: th-skam

src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h

@@ -188,22 +188,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())
+            const type::Vec3 tip2Pt = m_couplingPts.back()->getPosition() - tipProx->getPosition();
+            if (tip2Pt.norm() > d_tipDistThreshold.getValue())
             {
                 auto findClosestProxOnVol =
                     Operations::FindClosestProximity::Operation::get(l_volGeom);
@@ -216,6 +202,20 @@ 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();
+                if (dot(tip2Pt, normal) > 0_sreal) {
+                    m_couplingPts.pop_back();
+                }
+            }
 
             auto findClosestProxOnShaft =
                 Operations::FindClosestProximity::Operation::get(l_shaftGeom);