From 4d58d09598c95cea376641eba0de4ec8804cacd8 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Tue, 15 Jul 2025 14:04:23 +0200 Subject: [PATCH 1/6] [Needle][algorithm] Enabled detection of coupling points inside the volume by the needle tip right after puncture. The slideDistance variable determins how often coupling points are added --- scenes/NeedleInsertion.py | 9 +- .../algorithm/InsertionAlgorithm.h | 150 ++++++++++-------- 2 files changed, 93 insertions(+), 66 deletions(-) diff --git a/scenes/NeedleInsertion.py b/scenes/NeedleInsertion.py index 26b226fa..7e3fa0b6 100644 --- a/scenes/NeedleInsertion.py +++ b/scenes/NeedleInsertion.py @@ -176,7 +176,14 @@ def createScene(root): volumeVisuWire.addObject("IdentityMapping") - root.addObject("InsertionAlgorithm", name="InsertionAlgo", fromGeom="@Needle/tipCollision/geom", destGeom="@Volume/collision/geom_tri", destVol="@Volume/geom_tetra", punctureThreshold=0.1) + root.addObject("InsertionAlgorithm", name="InsertionAlgo", + fromGeom="@Needle/tipCollision/geom", + destGeom="@Volume/collision/geom_tri", + destVol="@Volume/geom_tetra", + punctureThreshold=0.1, + slideDistance=0.012 + #projective=True + ) root.addObject("DistanceFilter",algo="@InsertionAlgo",distance=0.01) root.addObject("SecondDirection",name="punctureDirection",handler="@Volume/collision/SurfaceTriangles") root.addObject("ConstraintUnilateral",input="@InsertionAlgo.output",directions="@punctureDirection",draw_scale="0.001")#, mu="0.001") diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index fa81c8fc..34bc8e13 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -22,13 +22,14 @@ class InsertionAlgorithm : public BaseAlgorithm { core::objectmodel::SingleLink l_destVol; Data d_drawCollision ; Data > d_output; - Data > d_outputInside; + Data > d_outputList; Data d_projective ; Data d_punctureThreshold ; + Data d_slideDistance ; // Data > d_outputDist; sofa::component::constraint::lagrangian::solver::ConstraintSolverImpl* m_constraintSolver; std::vector m_proximities; - std::vector m_tetras; + //std::vector m_tetras; InsertionAlgorithm() : l_from(initLink("fromGeom", "link to from geometry")) @@ -36,9 +37,10 @@ class InsertionAlgorithm : public BaseAlgorithm { , l_destVol(initLink("destVol", "link to dest geometry (volume)")) , d_drawCollision (initData(&d_drawCollision, true, "drawcollision", "draw collision")) , d_output(initData(&d_output,"output", "output of the collision detection")) - , d_outputInside(initData(&d_outputInside,"outputInside", "output of the detection inside the volume")) + , d_outputList(initData(&d_outputList,"outputList", "output of the detection inside the volume")) , d_projective(initData(&d_projective, false,"projective", "projection of closest prox onto from element")) , d_punctureThreshold(initData(&d_punctureThreshold, std::numeric_limits::max(), "punctureThreshold", "Threshold for puncture detection")) + , d_slideDistance(initData(&d_slideDistance, std::numeric_limits::min(), "slideDistance", "Distance along the insertion trajectory after which the proximities slide backwards along the needle shaft")) // , d_outputDist(initData(&d_outputDist,"outputDist", "Distance of the outpu pair of detections")) , m_constraintSolver(nullptr) {} @@ -62,24 +64,30 @@ class InsertionAlgorithm : public BaseAlgorithm { ); } - DetectionOutput outputInside = d_outputInside.getValue() ; - for (unsigned i=0;idrawTool()->drawLine( - outputInside[i].first->getPosition(), - outputInside[i].second->getPosition(), + DetectionOutput outputList = d_outputList.getValue() ; + //for (unsigned i=0;idrawTool()->drawLine( + // outputList[i].first->getPosition(), + // outputList[i].second->getPosition(), + // sofa::type::RGBAColor(1, 1, 0, 1) + // ); + //} + for (const auto& output : outputList) { + vparams->drawTool()->drawPoint( + output.second->getPosition(), sofa::type::RGBAColor(1, 1, 0, 1) ); } - for(unsigned i = 0; i < m_tetras.size(); ++i) { - vparams->drawTool()->drawTetrahedron( - m_tetras[i]->getP0()->getPosition(), - m_tetras[i]->getP1()->getPosition(), - m_tetras[i]->getP2()->getPosition(), - m_tetras[i]->getP3()->getPosition(), - sofa::type::RGBAColor(1, 1, 0, 1) - ); - } + //for(unsigned i = 0; i < m_tetras.size(); ++i) { + // vparams->drawTool()->drawTetrahedron( + // m_tetras[i]->getP0()->getPosition(), + // m_tetras[i]->getP1()->getPosition(), + // m_tetras[i]->getP2()->getPosition(), + // m_tetras[i]->getP3()->getPosition(), + // sofa::type::RGBAColor(1, 1, 0, 1) + // ); + //} } @@ -88,7 +96,7 @@ class InsertionAlgorithm : public BaseAlgorithm { if (l_dest == NULL) return; auto& output = *d_output.beginEdit(); - auto& outputInside = *d_outputInside.beginEdit(); + auto& outputList = *d_outputList.beginEdit(); const sofa::component::statecontainer::MechanicalObject* mstate = l_from->getContext()->get>(); @@ -103,8 +111,9 @@ class InsertionAlgorithm : public BaseAlgorithm { m_constraintSolver->getLambda()[mstate].read()->getValue(); if (lambda[0].norm() > d_punctureThreshold.getValue()) { - for (const auto& itOutputPair : output) { - m_proximities.push_back(itOutputPair.second->copy()); + for (const auto& dpair : output) { + outputList.add(dpair.first->copy(), dpair.second->copy()); + //m_proximities.push_back(itOutputPair.second->copy()); } output.clear(); return; @@ -112,7 +121,7 @@ class InsertionAlgorithm : public BaseAlgorithm { } output.clear(); - outputInside.clear(); + //outputInside.clear(); auto itfrom = l_from->begin(); @@ -122,63 +131,74 @@ class InsertionAlgorithm : public BaseAlgorithm { auto projectVolOp = Operations::Project::Operation::get(l_destVol); auto projectFromOp = Operations::Project::Operation::get(l_from); - for (; itfrom != l_from->end(); itfrom++) { + auto findClosestProxOpVol = Operations::FindClosestProximity::Operation::get(l_destVol); + + for (; itfrom != l_from->end(); itfrom++) + { auto pfrom = createProximityOp(itfrom->element()); if (pfrom == nullptr) continue; - auto pdest = findClosestProxOp(pfrom, l_dest.get(), projectOp, getFilterFunc()); - if (pdest != nullptr) + if (outputList.size() == 0) { - pdest->normalize(); - - if (d_projective.getValue()) { - auto pfromProj = projectFromOp(pdest->getPosition(), itfrom->element()).prox; - if (pfromProj == nullptr) continue; - pfromProj->normalize(); - - output.add(pfromProj, pdest); - } - else { - output.add(pfrom, pdest); + auto pdest = findClosestProxOp(pfrom, l_dest.get(), projectOp, getFilterFunc()); + if (pdest != nullptr) + { + pdest->normalize(); + + if (d_projective.getValue()) { + auto pfromProj = projectFromOp(pdest->getPosition(), itfrom->element()).prox; + if (pfromProj == nullptr) continue; + pfromProj->normalize(); + + output.add(pfromProj, pdest); + } + else { + output.add(pfrom, pdest); + } } } - - auto findClosestProxOpVol = Operations::FindClosestProximity::Operation::get(l_destVol); - auto pdestVol = findClosestProxOpVol(pfrom, l_destVol.get(), projectVolOp, getFilterFunc()); - if (pdestVol != nullptr) + else { - TetrahedronProximity::SPtr tetraProx = std::dynamic_pointer_cast(pdestVol); - double* baryCoords = tetraProx->getBaryCoord(); - if (toolbox::TetrahedronToolBox::isInTetra( - pfrom->getPosition(), - tetraProx->element()->getTetrahedronInfo(), - baryCoords[0], - baryCoords[1], - baryCoords[2], - baryCoords[3] - ) - ) + const SReal dist = (pfrom->getPosition() - outputList.back().second->getPosition()).norm(); + if(dist > d_slideDistance.getValue()) { - m_tetras.push_back(tetraProx->element()); - } - - pdestVol->normalize(); - - if (d_projective.getValue()) { - auto pfromProj = projectFromOp(pdestVol->getPosition(), itfrom->element()).prox; - if (pfromProj == nullptr) continue; - pfromProj->normalize(); - - outputInside.add(pfromProj, pdestVol); - } - else { - outputInside.add(pfrom, pdestVol); + auto pdestVol = findClosestProxOpVol(pfrom, l_destVol.get(), projectVolOp, getFilterFunc()); + if (pdestVol != nullptr) + { + //TetrahedronProximity::SPtr tetraProx = std::dynamic_pointer_cast(pdestVol); + //double* baryCoords = tetraProx->getBaryCoord(); + //if (toolbox::TetrahedronToolBox::isInTetra( + // pfrom->getPosition(), + // tetraProx->element()->getTetrahedronInfo(), + // baryCoords[0], + // baryCoords[1], + // baryCoords[2], + // baryCoords[3] + // ) + //) + //{ + // //m_tetras.push_back(tetraProx->element()); + //} + + pdestVol->normalize(); + + if (d_projective.getValue()) { + auto pfromProj = projectFromOp(pdestVol->getPosition(), itfrom->element()).prox; + if (pfromProj == nullptr) continue; + pfromProj->normalize(); + + outputList.add(pfromProj, pdestVol); + } + else { + outputList.add(pfrom, pdestVol); + } + } } } } d_output.endEdit(); - d_outputInside.endEdit(); + d_outputList.endEdit(); } }; From d7fcff2eec28a342420eddbacfd695b91d5057d4 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Tue, 15 Jul 2025 16:45:37 +0200 Subject: [PATCH 2/6] [Needle][algorithm] Draw output points as spheres --- scenes/NeedleInsertion.py | 2 +- .../algorithm/InsertionAlgorithm.h | 78 +++++++++---------- 2 files changed, 36 insertions(+), 44 deletions(-) diff --git a/scenes/NeedleInsertion.py b/scenes/NeedleInsertion.py index 7e3fa0b6..ddb34593 100644 --- a/scenes/NeedleInsertion.py +++ b/scenes/NeedleInsertion.py @@ -181,7 +181,7 @@ def createScene(root): destGeom="@Volume/collision/geom_tri", destVol="@Volume/geom_tetra", punctureThreshold=0.1, - slideDistance=0.012 + slideDistance=0.015 #projective=True ) root.addObject("DistanceFilter",algo="@InsertionAlgo",distance=0.01) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 34bc8e13..3e0a6358 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -21,6 +21,8 @@ class InsertionAlgorithm : public BaseAlgorithm { core::objectmodel::SingleLink l_dest; core::objectmodel::SingleLink l_destVol; Data d_drawCollision ; + Data d_drawPoints ; + Data d_sphereRadius ; Data > d_output; Data > d_outputList; Data d_projective ; @@ -29,13 +31,15 @@ class InsertionAlgorithm : public BaseAlgorithm { // Data > d_outputDist; sofa::component::constraint::lagrangian::solver::ConstraintSolverImpl* m_constraintSolver; std::vector m_proximities; - //std::vector m_tetras; + std::vector m_tetras; InsertionAlgorithm() : l_from(initLink("fromGeom", "link to from geometry")) , l_dest(initLink("destGeom", "link to dest geometry")) , l_destVol(initLink("destVol", "link to dest geometry (volume)")) , d_drawCollision (initData(&d_drawCollision, true, "drawcollision", "draw collision")) + , d_drawPoints(initData(&d_drawPoints, true, "drawPoints", "draw detection outputs")) + , d_sphereRadius(initData(&d_sphereRadius, 0.001, "sphereRadius", "radius for drawing detection outputs")) , d_output(initData(&d_output,"output", "output of the collision detection")) , d_outputList(initData(&d_outputList,"outputList", "output of the detection inside the volume")) , d_projective(initData(&d_projective, false,"projective", "projection of closest prox onto from element")) @@ -43,6 +47,7 @@ class InsertionAlgorithm : public BaseAlgorithm { , d_slideDistance(initData(&d_slideDistance, std::numeric_limits::min(), "slideDistance", "Distance along the insertion trajectory after which the proximities slide backwards along the needle shaft")) // , d_outputDist(initData(&d_outputDist,"outputDist", "Distance of the outpu pair of detections")) , m_constraintSolver(nullptr) + , m_tetras() {} void init() override { @@ -56,38 +61,26 @@ class InsertionAlgorithm : public BaseAlgorithm { vparams->drawTool()->disableLighting(); DetectionOutput output = d_output.getValue() ; - for (unsigned i=0;idrawTool()->drawLine( - output[i].first->getPosition(), - output[i].second->getPosition(), - sofa::type::RGBAColor(0, 1, 0, 1) - ); + for (const auto& it : output) { + vparams->drawTool()->drawLine(it.first->getPosition(), it.second->getPosition(), sofa::type::RGBAColor(0, 1, 0, 1)); } DetectionOutput outputList = d_outputList.getValue() ; - //for (unsigned i=0;idrawTool()->drawLine( - // outputList[i].first->getPosition(), - // outputList[i].second->getPosition(), - // sofa::type::RGBAColor(1, 1, 0, 1) - // ); - //} - for (const auto& output : outputList) { - vparams->drawTool()->drawPoint( - output.second->getPosition(), - sofa::type::RGBAColor(1, 1, 0, 1) - ); + for (const auto& it : outputList) { + vparams->drawTool()->drawSphere(it.first->getPosition(), d_sphereRadius.getValue(), sofa::type::RGBAColor(1, 0, 0, 1)); + vparams->drawTool()->drawSphere(it.second->getPosition(), d_sphereRadius.getValue(), sofa::type::RGBAColor(0, 0, 1, 1)); + vparams->drawTool()->drawLine(it.first->getPosition(), it.second->getPosition(), sofa::type::RGBAColor(1, 1, 0, 1)); } - //for(unsigned i = 0; i < m_tetras.size(); ++i) { - // vparams->drawTool()->drawTetrahedron( - // m_tetras[i]->getP0()->getPosition(), - // m_tetras[i]->getP1()->getPosition(), - // m_tetras[i]->getP2()->getPosition(), - // m_tetras[i]->getP3()->getPosition(), - // sofa::type::RGBAColor(1, 1, 0, 1) - // ); - //} + for(const auto& it : m_tetras) { + vparams->drawTool()->drawTetrahedron( + it->getP0()->getPosition(), + it->getP1()->getPosition(), + it->getP2()->getPosition(), + it->getP3()->getPosition(), + sofa::type::RGBAColor(1, 1, 0, 0.3) + ); + } } @@ -121,7 +114,7 @@ class InsertionAlgorithm : public BaseAlgorithm { } output.clear(); - //outputInside.clear(); + //outputList.clear(); auto itfrom = l_from->begin(); @@ -165,20 +158,19 @@ class InsertionAlgorithm : public BaseAlgorithm { auto pdestVol = findClosestProxOpVol(pfrom, l_destVol.get(), projectVolOp, getFilterFunc()); if (pdestVol != nullptr) { - //TetrahedronProximity::SPtr tetraProx = std::dynamic_pointer_cast(pdestVol); - //double* baryCoords = tetraProx->getBaryCoord(); - //if (toolbox::TetrahedronToolBox::isInTetra( - // pfrom->getPosition(), - // tetraProx->element()->getTetrahedronInfo(), - // baryCoords[0], - // baryCoords[1], - // baryCoords[2], - // baryCoords[3] - // ) - //) - //{ - // //m_tetras.push_back(tetraProx->element()); - //} + /* + TetrahedronProximity::SPtr tetraProx = std::dynamic_pointer_cast(pdestVol); + double* baryCoords = tetraProx->getBaryCoord(); + if (toolbox::TetrahedronToolBox::isInTetra( + pfrom->getPosition(), + tetraProx->element()->getTetrahedronInfo(), + baryCoords[0], + baryCoords[1], + baryCoords[2], + baryCoords[3] + ) + ) m_tetras.push_back(tetraProx->element()); + */ pdestVol->normalize(); From 68d7efb99008bbee980159ef311b32bdfc452950 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Wed, 16 Jul 2025 12:03:20 +0200 Subject: [PATCH 3/6] [Needle][algorithm] Each time a point is detected, the rest slide backwards along the needle shaft Some assumptions are made: 1. The slide distance is set equal to the length of the needle edges 2. A second geometry is used (l_fromVol) to couple points detected inside the volume with points on the needle edges 3. The second geometry is assumed to be an EdgeGeometry --- scenes/NeedleInsertion.py | 3 +- .../algorithm/InsertionAlgorithm.h | 148 ++++++++++-------- 2 files changed, 85 insertions(+), 66 deletions(-) diff --git a/scenes/NeedleInsertion.py b/scenes/NeedleInsertion.py index ddb34593..2cf1f06a 100644 --- a/scenes/NeedleInsertion.py +++ b/scenes/NeedleInsertion.py @@ -179,9 +179,10 @@ def createScene(root): root.addObject("InsertionAlgorithm", name="InsertionAlgo", fromGeom="@Needle/tipCollision/geom", destGeom="@Volume/collision/geom_tri", + fromVol="@Needle/bodyCollision/geom", destVol="@Volume/geom_tetra", punctureThreshold=0.1, - slideDistance=0.015 + slideDistance=0.005 #projective=True ) root.addObject("DistanceFilter",algo="@InsertionAlgo",distance=0.01) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 3e0a6358..692f964c 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -19,6 +19,7 @@ class InsertionAlgorithm : public BaseAlgorithm { core::objectmodel::SingleLink l_from; core::objectmodel::SingleLink l_dest; + core::objectmodel::SingleLink l_fromVol; core::objectmodel::SingleLink l_destVol; Data d_drawCollision ; Data d_drawPoints ; @@ -30,16 +31,18 @@ class InsertionAlgorithm : public BaseAlgorithm { Data d_slideDistance ; // Data > d_outputDist; sofa::component::constraint::lagrangian::solver::ConstraintSolverImpl* m_constraintSolver; - std::vector m_proximities; std::vector m_tetras; + std::vector m_needlePts; + std::vector m_couplingPts; InsertionAlgorithm() : l_from(initLink("fromGeom", "link to from geometry")) , l_dest(initLink("destGeom", "link to dest geometry")) + , l_fromVol(initLink("fromVol", "link to from geometry (volume)")) , l_destVol(initLink("destVol", "link to dest geometry (volume)")) , d_drawCollision (initData(&d_drawCollision, true, "drawcollision", "draw collision")) , d_drawPoints(initData(&d_drawPoints, true, "drawPoints", "draw detection outputs")) - , d_sphereRadius(initData(&d_sphereRadius, 0.001, "sphereRadius", "radius for drawing detection outputs")) + , d_sphereRadius(initData(&d_sphereRadius, 0.0005, "sphereRadius", "radius for drawing detection outputs")) , d_output(initData(&d_output,"output", "output of the collision detection")) , d_outputList(initData(&d_outputList,"outputList", "output of the detection inside the volume")) , d_projective(initData(&d_projective, false,"projective", "projection of closest prox onto from element")) @@ -48,6 +51,8 @@ class InsertionAlgorithm : public BaseAlgorithm { // , d_outputDist(initData(&d_outputDist,"outputDist", "Distance of the outpu pair of detections")) , m_constraintSolver(nullptr) , m_tetras() + , m_needlePts() + , m_couplingPts() {} void init() override { @@ -81,61 +86,57 @@ class InsertionAlgorithm : public BaseAlgorithm { sofa::type::RGBAColor(1, 1, 0, 0.3) ); } - } void doDetection() { if (l_from == NULL) return; if (l_dest == NULL) return; + if (l_fromVol == NULL) return; + if (l_destVol == NULL) return; auto& output = *d_output.beginEdit(); auto& outputList = *d_outputList.beginEdit(); - const sofa::component::statecontainer::MechanicalObject* mstate - = l_from->getContext()->get>(); - if (mstate->getSize() > 1) - { - msg_warning() << "Requested MechanicalObject, corresponding to the tip of the needle in the InsertionAlgorithm, has a size greater than 1. " - << "The algorithm is designed to work with a single point. Only the first element will be used."; - } - if (m_constraintSolver) + if (outputList.size() == 0) { - defaulttype::Vec3Types::VecCoord lambda = - m_constraintSolver->getLambda()[mstate].read()->getValue(); - if (lambda[0].norm() > d_punctureThreshold.getValue()) + const sofa::component::statecontainer::MechanicalObject* mstate + = l_from->getContext()->get>(); + if (mstate->getSize() > 1) { + msg_warning() << "Requested MechanicalObject, corresponding to the tip of the needle in the InsertionAlgorithm, has a size greater than 1. " + << "The algorithm is designed to work with a single point. Only the first element will be used."; + } + if (m_constraintSolver) { - for (const auto& dpair : output) { - outputList.add(dpair.first->copy(), dpair.second->copy()); - //m_proximities.push_back(itOutputPair.second->copy()); + defaulttype::Vec3Types::VecCoord lambda = + m_constraintSolver->getLambda()[mstate].read()->getValue(); + if (lambda[0].norm() > d_punctureThreshold.getValue()) + { + for (const auto& dpair : output) + { + outputList.add(dpair.first->copy(), dpair.second->copy()); + m_needlePts.push_back(dpair.first->copy()); + m_couplingPts.push_back(dpair.second->copy()); + } + output.clear(); + return; } - output.clear(); - return; } - } - - output.clear(); - //outputList.clear(); - auto itfrom = l_from->begin(); + output.clear(); - auto createProximityOp = Operations::CreateCenterProximity::Operation::get(itfrom->getTypeInfo()); - auto findClosestProxOp = Operations::FindClosestProximity::Operation::get(l_dest); - auto projectOp = Operations::Project::Operation::get(l_dest); - auto projectVolOp = Operations::Project::Operation::get(l_destVol); - auto projectFromOp = Operations::Project::Operation::get(l_from); + auto itfrom = l_from->begin(); - auto findClosestProxOpVol = Operations::FindClosestProximity::Operation::get(l_destVol); + auto createProximityOp = Operations::CreateCenterProximity::Operation::get(itfrom->getTypeInfo()); + auto findClosestProxOp = Operations::FindClosestProximity::Operation::get(l_dest); + auto projectOp = Operations::Project::Operation::get(l_dest); + auto projectFromOp = Operations::Project::Operation::get(l_from); - for (; itfrom != l_from->end(); itfrom++) - { - auto pfrom = createProximityOp(itfrom->element()); - if (pfrom == nullptr) continue; - - if (outputList.size() == 0) + for (; itfrom != l_from->end(); itfrom++) { + auto pfrom = createProximityOp(itfrom->element()); + if (pfrom == nullptr) continue; auto pdest = findClosestProxOp(pfrom, l_dest.get(), projectOp, getFilterFunc()); - if (pdest != nullptr) - { + if (pdest != nullptr) { pdest->normalize(); if (d_projective.getValue()) { @@ -150,41 +151,58 @@ class InsertionAlgorithm : public BaseAlgorithm { } } } - else + } + else + { + const SReal dist = (m_needlePts.back()->getPosition() - m_couplingPts.back()->getPosition()).norm(); + + if(dist > d_slideDistance.getValue()) { - const SReal dist = (pfrom->getPosition() - outputList.back().second->getPosition()).norm(); - if(dist > d_slideDistance.getValue()) + outputList.clear(); + auto findClosestProxOp_vol = Operations::FindClosestProximity::Operation::get(l_destVol); + auto projectOp_vol = Operations::Project::Operation::get(l_destVol); + auto projectFromOp_vol = Operations::Project::Operation::get(l_fromVol); + auto pdestVol = findClosestProxOp_vol(m_needlePts.back(), l_destVol.get(), projectOp_vol, getFilterFunc()); + if (pdestVol != nullptr) { - auto pdestVol = findClosestProxOpVol(pfrom, l_destVol.get(), projectVolOp, getFilterFunc()); - if (pdestVol != nullptr) + /* + TetrahedronProximity::SPtr tetraProx = std::dynamic_pointer_cast(pdestVol); + double* baryCoords = tetraProx->getBaryCoord(); + if (toolbox::TetrahedronToolBox::isInTetra( + pfrom->getPosition(), + tetraProx->element()->getTetrahedronInfo(), + baryCoords[0], + baryCoords[1], + baryCoords[2], + baryCoords[3] + ) + ) m_tetras.push_back(tetraProx->element()); + */ + + pdestVol->normalize(); + m_couplingPts.push_back(pdestVol); + 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()); + + for(int i = 0 ; i < m_needlePts.size() - 1; i++) { - /* - TetrahedronProximity::SPtr tetraProx = std::dynamic_pointer_cast(pdestVol); - double* baryCoords = tetraProx->getBaryCoord(); - if (toolbox::TetrahedronToolBox::isInTetra( - pfrom->getPosition(), - tetraProx->element()->getTetrahedronInfo(), - baryCoords[0], - baryCoords[1], - baryCoords[2], - baryCoords[3] - ) - ) m_tetras.push_back(tetraProx->element()); - */ - - pdestVol->normalize(); - + auto pfromVol = createProximityOp_vol(itfromVol->element()); if (d_projective.getValue()) { - auto pfromProj = projectFromOp(pdestVol->getPosition(), itfrom->element()).prox; - if (pfromProj == nullptr) continue; - pfromProj->normalize(); - - outputList.add(pfromProj, pdestVol); + auto pfromVolProj = projectFromOp_vol(pdestVol->getPosition(), itfromVol->element()).prox; + if (pfromVolProj == nullptr) continue; + pfromVolProj->normalize(); + m_needlePts[i] = pfromVolProj; } else { - outputList.add(pfrom, pdestVol); + m_needlePts[i] = pfromVol; } + itfromVol++; } + + for(int i = 0 ; i < m_needlePts.size(); i++) + outputList.add(m_needlePts[i], m_couplingPts[i]); } } } From fdafff6beb87c63dc4e5a574b39a2629ca901a13 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Wed, 16 Jul 2025 16:57:16 +0200 Subject: [PATCH 4/6] [Needle][algorithm] Removed the option to visualize crossed tetrahedra --- .../algorithm/InsertionAlgorithm.h | 26 ------------------- 1 file changed, 26 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 692f964c..8e6d8ce6 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -31,7 +31,6 @@ class InsertionAlgorithm : public BaseAlgorithm { Data d_slideDistance ; // Data > d_outputDist; sofa::component::constraint::lagrangian::solver::ConstraintSolverImpl* m_constraintSolver; - std::vector m_tetras; std::vector m_needlePts; std::vector m_couplingPts; @@ -50,7 +49,6 @@ class InsertionAlgorithm : public BaseAlgorithm { , d_slideDistance(initData(&d_slideDistance, std::numeric_limits::min(), "slideDistance", "Distance along the insertion trajectory after which the proximities slide backwards along the needle shaft")) // , d_outputDist(initData(&d_outputDist,"outputDist", "Distance of the outpu pair of detections")) , m_constraintSolver(nullptr) - , m_tetras() , m_needlePts() , m_couplingPts() {} @@ -76,16 +74,6 @@ class InsertionAlgorithm : public BaseAlgorithm { vparams->drawTool()->drawSphere(it.second->getPosition(), d_sphereRadius.getValue(), sofa::type::RGBAColor(0, 0, 1, 1)); vparams->drawTool()->drawLine(it.first->getPosition(), it.second->getPosition(), sofa::type::RGBAColor(1, 1, 0, 1)); } - - for(const auto& it : m_tetras) { - vparams->drawTool()->drawTetrahedron( - it->getP0()->getPosition(), - it->getP1()->getPosition(), - it->getP2()->getPosition(), - it->getP3()->getPosition(), - sofa::type::RGBAColor(1, 1, 0, 0.3) - ); - } } void doDetection() { @@ -165,20 +153,6 @@ class InsertionAlgorithm : public BaseAlgorithm { auto pdestVol = findClosestProxOp_vol(m_needlePts.back(), l_destVol.get(), projectOp_vol, getFilterFunc()); if (pdestVol != nullptr) { - /* - TetrahedronProximity::SPtr tetraProx = std::dynamic_pointer_cast(pdestVol); - double* baryCoords = tetraProx->getBaryCoord(); - if (toolbox::TetrahedronToolBox::isInTetra( - pfrom->getPosition(), - tetraProx->element()->getTetrahedronInfo(), - baryCoords[0], - baryCoords[1], - baryCoords[2], - baryCoords[3] - ) - ) m_tetras.push_back(tetraProx->element()); - */ - pdestVol->normalize(); m_couplingPts.push_back(pdestVol); m_needlePts.push_back(m_needlePts.back()); From 8ea1d039f526d568e36565496c59ed83c59fa873 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Mon, 21 Jul 2025 14:32:45 +0200 Subject: [PATCH 5/6] [Needle][algorithm] Changed algorithm to re-project volume proximities onto the needle geometry --- .../collisionAlgorithm/algorithm/InsertionAlgorithm.h | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 8e6d8ce6..676218d2 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/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; From bc791a8a98b6bdb6cc277b07f2658d9ddbfdb7cc Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Wed, 23 Jul 2025 15:11:33 +0200 Subject: [PATCH 6/6] [Needle][algorithm] Removed useless iterator increments - avoids using the end pointer --- .../algorithm/InsertionAlgorithm.h | 13 +------------ 1 file changed, 1 insertion(+), 12 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 676218d2..e27f99f5 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -158,25 +158,14 @@ 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 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 = findClosestProxOp_needle(m_couplingPts[i], l_fromVol.get(), projectOp_needle, getFilterFunc()); - if (d_projective.getValue()) { - auto pfromVolProj = projectFromOp_vol(m_couplingPts[i]->getPosition(), itfromVol->element()).prox; - if (pfromVolProj == nullptr) continue; - pfromVolProj->normalize(); - m_needlePts[i] = pfromVolProj; - } - else { - m_needlePts[i] = pfromVol; - } - itfromVol++; + m_needlePts[i] = pfromVol; } for(int i = 0 ; i < m_needlePts.size(); i++)