diff --git a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h index 694acd4b..ab95a71b 100644 --- a/src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/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()) diff --git a/src/CollisionAlgorithm/operations/NeedleOperations.cpp b/src/CollisionAlgorithm/operations/NeedleOperations.cpp index 1cf2cb54..a36a6869 100644 --- a/src/CollisionAlgorithm/operations/NeedleOperations.cpp +++ b/src/CollisionAlgorithm/operations/NeedleOperations.cpp @@ -14,16 +14,19 @@ bool prunePointsUsingEdges(std::vector& 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 =