[Needle][algorithm] Changed algorithm to re-project volume proximities onto the needle geometry

Author: th-skam

src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h

@@ -158,13 +158,17 @@ class InsertionAlgorithm : public BaseAlgorithm {
                     m_needlePts.push_back(m_needlePts.back());
 
                     auto itfromVol = l_fromVol->begin(l_fromVol->getSize() - 1 - m_couplingPts.size());
-                    auto createProximityOp_vol = Operations::CreateCenterProximity::Operation::get(itfromVol->getTypeInfo());
+                    //auto createProximityOp_vol = Operations::CreateCenterProximity::Operation::get(itfromVol->getTypeInfo());
+
+                    auto findClosestProxOp_needle = Operations::FindClosestProximity::Operation::get(l_fromVol);
+                    auto projectOp_needle = Operations::Project::Operation::get(l_fromVol);
 
                     for(int i = 0 ; i < m_needlePts.size() - 1; i++)
                     {
-                        auto pfromVol = createProximityOp_vol(itfromVol->element());
+                        //auto pfromVol = createProximityOp_vol(itfromVol->element());
+                        auto pfromVol = findClosestProxOp_needle(m_couplingPts[i], l_fromVol.get(), projectOp_needle, getFilterFunc());
                         if (d_projective.getValue()) {
-                            auto pfromVolProj = projectFromOp_vol(pdestVol->getPosition(), itfromVol->element()).prox;
+                            auto pfromVolProj = projectFromOp_vol(m_couplingPts[i]->getPosition(), itfromVol->element()).prox;
                             if (pfromVolProj == nullptr) continue;
                             pfromVolProj->normalize();
                             m_needlePts[i] = pfromVolProj;