diff --git a/scenes/NeedleInsertion.py b/scenes/NeedleInsertion.py index 26b226fa..2cf1f06a 100644 --- a/scenes/NeedleInsertion.py +++ b/scenes/NeedleInsertion.py @@ -176,7 +176,15 @@ 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", + fromVol="@Needle/bodyCollision/geom", + destVol="@Volume/geom_tetra", + punctureThreshold=0.1, + slideDistance=0.005 + #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..e27f99f5 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -19,28 +19,38 @@ 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 ; + Data d_sphereRadius ; 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_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.0005, "sphereRadius", "radius for drawing detection outputs")) , 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) + , m_needlePts() + , m_couplingPts() {} void init() override { @@ -54,131 +64,118 @@ 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 outputInside = d_outputInside.getValue() ; - for (unsigned i=0;idrawTool()->drawLine( - outputInside[i].first->getPosition(), - outputInside[i].second->getPosition(), - sofa::type::RGBAColor(1, 1, 0, 1) - ); + DetectionOutput outputList = d_outputList.getValue() ; + 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) - ); - } - } 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& outputInside = *d_outputInside.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& itOutputPair : output) { - 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(); - outputInside.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(); - for (; itfrom != l_from->end(); itfrom++) { - auto pfrom = createProximityOp(itfrom->element()); - if (pfrom == nullptr) continue; + 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); - auto pdest = findClosestProxOp(pfrom, l_dest.get(), projectOp, getFilterFunc()); - if (pdest != nullptr) + for (; itfrom != l_from->end(); itfrom++) { - 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 pfrom = createProximityOp(itfrom->element()); + if (pfrom == nullptr) continue; + 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); + } } } + } + else + { + const SReal dist = (m_needlePts.back()->getPosition() - m_couplingPts.back()->getPosition()).norm(); - auto findClosestProxOpVol = Operations::FindClosestProximity::Operation::get(l_destVol); - auto pdestVol = findClosestProxOpVol(pfrom, l_destVol.get(), projectVolOp, getFilterFunc()); - if (pdestVol != nullptr) + if(dist > d_slideDistance.getValue()) { - 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] - ) - ) + 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) { - m_tetras.push_back(tetraProx->element()); - } + pdestVol->normalize(); + m_couplingPts.push_back(pdestVol); + m_needlePts.push_back(m_needlePts.back()); - 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 itfromVol = l_fromVol->begin(l_fromVol->getSize() - 1 - m_couplingPts.size()); + + 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 = findClosestProxOp_needle(m_couplingPts[i], l_fromVol.get(), projectOp_needle, getFilterFunc()); + m_needlePts[i] = pfromVol; + } + + for(int i = 0 ; i < m_needlePts.size(); i++) + outputList.add(m_needlePts[i], m_couplingPts[i]); } } } d_output.endEdit(); - d_outputInside.endEdit(); + d_outputList.endEdit(); } };