[algorithm] Remove duplicated code for coupling point pruning - merge leftover

Author: th-skam

src/CollisionAlgorithm/algorithm/InsertionAlgorithm.cpp

@@ -276,82 +276,73 @@ void InsertionAlgorithm::insertionPhase()
     const type::Vec3 tipToLastCP = lastCP - tipProx->getPosition();
 
     // Only add a new coupling point if the needle tip has advanced far enough
-    if (tipToLastCP.norm() > tipDistThreshold)
+    if (tipToLastCP.norm() < tipDistThreshold) return;
+    
+    // Prepare operations before entering loop
+    auto createShaftProximity =
+        Operations::CreateCenterProximity::Operation::get(l_shaftGeom->getTypeInfo());
+    auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom);
+    auto findClosestProxOnVol = Operations::FindClosestProximity::Operation::get(l_volGeom);
+    auto projectOnVol = Operations::Project::Operation::get(l_volGeom);
+    auto containsPointInVol = Operations::ContainsPointInProximity::Operation::get(l_volGeom);
+
+    // Iterate over shaft segments to find which one contains the next candidate CP
+    for (auto itShaft = l_shaftGeom->begin(); itShaft != l_shaftGeom->end(); itShaft++)
     {
-        // Prepare operations before entering loop
-        auto createShaftProximity =
-            Operations::CreateCenterProximity::Operation::get(l_shaftGeom->getTypeInfo());
-        auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom);
-        auto findClosestProxOnVol = Operations::FindClosestProximity::Operation::get(l_volGeom);
-        auto projectOnVol = Operations::Project::Operation::get(l_volGeom);
-        auto containsPointInVol = Operations::ContainsPointInProximity::Operation::get(l_volGeom);
-
-        // Iterate over shaft segments to find which one contains the next candidate CP
-        for (auto itShaft = l_shaftGeom->begin(); itShaft != l_shaftGeom->end(); itShaft++)
-        {
-            BaseProximity::SPtr shaftProx = createShaftProximity(itShaft->element());
+        BaseProximity::SPtr shaftProx = createShaftProximity(itShaft->element());
 
-            if (!shaftProx) continue;
+        if (!shaftProx) continue;
 
-            const EdgeProximity::SPtr edgeProx = dynamic_pointer_cast<EdgeProximity>(shaftProx);
+        const EdgeProximity::SPtr edgeProx = dynamic_pointer_cast<EdgeProximity>(shaftProx);
 
-            if (!edgeProx) continue;
+        if (!edgeProx) continue;
 
-            const type::Vec3 p0 = edgeProx->element()->getP0()->getPosition();
-            const type::Vec3 p1 = edgeProx->element()->getP1()->getPosition();
-            const type::Vec3 shaftEdgeDir = (p1 - p0).normalized();
-            const type::Vec3 lastCPToP1 = p1 - lastCP;
+        const type::Vec3 p0 = edgeProx->element()->getP0()->getPosition();
+        const type::Vec3 p1 = edgeProx->element()->getP1()->getPosition();
+        const type::Vec3 shaftEdgeDir = (p1 - p0).normalized();
+        const type::Vec3 lastCPToP1 = p1 - lastCP;
 
-            // Skip if last CP lies after edge end point
-            if (dot(shaftEdgeDir, lastCPToP1) < 0_sreal) continue;
+        // Skip if last CP lies after edge end point
+        if (dot(shaftEdgeDir, lastCPToP1) < 0_sreal) continue;
 
-            const int numCPs = floor(lastCPToP1.norm() / tipDistThreshold);
+        const int numCPs = floor(lastCPToP1.norm() / tipDistThreshold);
 
-            for (int idCP = 0; idCP < numCPs; idCP++)
-            {
-                // Candidate coupling point along shaft segment
-                const type::Vec3 candidateCP = lastCP + tipDistThreshold * shaftEdgeDir;
+        for (int idCP = 0; idCP < numCPs; idCP++)
+        {
+            // Candidate coupling point along shaft segment
+            const type::Vec3 candidateCP = lastCP + tipDistThreshold * shaftEdgeDir;
 
-                // Project candidate CP onto the edge element and compute scalar coordinate
-                // along segment
-                const SReal edgeSegmentLength = (p1 - p0).norm();
-                const type::Vec3 p0ToCandidateCP = candidateCP - p0;
-                const SReal projPtOnEdge = dot(p0ToCandidateCP, shaftEdgeDir);
+            // Project candidate CP onto the edge element and compute scalar coordinate
+            // along segment
+            const SReal edgeSegmentLength = (p1 - p0).norm();
+            const type::Vec3 p0ToCandidateCP = candidateCP - p0;
+            const SReal projPtOnEdge = dot(p0ToCandidateCP, shaftEdgeDir);
 
-                // Skip if candidate CP is outside current edge segment
-                if (projPtOnEdge < 0_sreal || projPtOnEdge > edgeSegmentLength) break;
+            // Skip if candidate CP is outside current edge segment
+            if (projPtOnEdge < 0_sreal || projPtOnEdge > edgeSegmentLength) break;
 
-                // Project candidate CP onto shaft geometry ...
-                shaftProx = projectOnShaft(candidateCP, itShaft->element()).prox;
+            // Project candidate CP onto shaft geometry ...
+            shaftProx = projectOnShaft(candidateCP, itShaft->element()).prox;
 
-                if (!shaftProx) continue;
+            if (!shaftProx) continue;
 
-                // ... then find nearest volume proximity
-                const BaseProximity::SPtr volProx =
-                    findClosestProxOnVol(shaftProx, l_volGeom.get(), projectOnVol, getFilterFunc());
+            // ... then find nearest volume proximity
+            const BaseProximity::SPtr volProx =
+                findClosestProxOnVol(shaftProx, l_volGeom.get(), projectOnVol, getFilterFunc());
 
-                if (!volProx) continue;
+            if (!volProx) continue;
 
-                // Proximity can be detected before the tip enters the tetra (e.g. near a
-                // boundary face) Only accept proximities if the tip is inside the tetra
-                // during insertion
-                if (containsPointInVol(shaftProx->getPosition(), volProx))
-                {
-                    volProx->normalize();
-                    m_couplingPts.push_back(volProx);
-                    lastCP = volProx->getPosition();
-                }
+            // Proximity can be detected before the tip enters the tetra (e.g. near a
+            // boundary face) Only accept proximities if the tip is inside the tetra
+            // during insertion
+            if (containsPointInVol(shaftProx->getPosition(), volProx))
+            {
+                volProx->normalize();
+                m_couplingPts.push_back(volProx);
+                lastCP = volProx->getPosition();
             }
         }
     }
-    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());
-    }
 }
 
 InsertionAlgorithm::AlgorithmOutput InsertionAlgorithm::reprojectCouplingPoints()