Skip to content

[Needle][algorithm] Extension of the insertion algorithm for proximity detection inside the volume #24

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
th-skam merged 6 commits into from
Jul 23, 2025
Merged

[Needle][algorithm] Extension of the insertion algorithm for proximity detection inside the volume #24

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
4d58d09
[Needle][algorithm] Enabled detection of coupling points inside the v…
th-skam Jul 15, 2025
d7fcff2
[Needle][algorithm] Draw output points as spheres
th-skam Jul 15, 2025
68d7efb
[Needle][algorithm] Each time a point is detected, the rest slide bac…
th-skam Jul 16, 2025
fdafff6
[Needle][algorithm] Removed the option to visualize crossed tetrahedra
th-skam Jul 16, 2025
8ea1d03
[Needle][algorithm] Changed algorithm to re-project volume proximitie…
th-skam Jul 21, 2025
bc791a8
[Needle][algorithm] Removed useless iterator increments - avoids usin…
th-skam Jul 23, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 9 additions & 1 deletion scenes/NeedleInsertion.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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")
Expand Down
193 changes: 95 additions & 98 deletions src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,28 +19,38 @@ class InsertionAlgorithm : public BaseAlgorithm {

core::objectmodel::SingleLink<InsertionAlgorithm,BaseGeometry,BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> l_from;
core::objectmodel::SingleLink<InsertionAlgorithm,BaseGeometry,BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> l_dest;
core::objectmodel::SingleLink<InsertionAlgorithm,BaseGeometry,BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> l_fromVol;
core::objectmodel::SingleLink<InsertionAlgorithm,BaseGeometry,BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> l_destVol;
Data<bool> d_drawCollision ;
Data<bool> d_drawPoints ;
Data<SReal> d_sphereRadius ;
Data<DetectionOutput<BaseProximity,BaseProximity> > d_output;
Data<DetectionOutput<BaseProximity,BaseProximity> > d_outputInside;
Data<DetectionOutput<BaseProximity,BaseProximity> > d_outputList;
Data<bool> d_projective ;
Data<SReal> d_punctureThreshold ;
Data<SReal> d_slideDistance ;
// Data<sofa::type::vector<double> > d_outputDist;
sofa::component::constraint::lagrangian::solver::ConstraintSolverImpl* m_constraintSolver;
std::vector<BaseProximity::SPtr> m_proximities;
std::vector<TetrahedronElement::SPtr> m_tetras;
std::vector<BaseProximity::SPtr> m_needlePts;
std::vector<BaseProximity::SPtr> 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<double>::max(), "punctureThreshold", "Threshold for puncture detection"))
, d_slideDistance(initData(&d_slideDistance, std::numeric_limits<double>::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 {
Expand All @@ -54,131 +64,118 @@ class InsertionAlgorithm : public BaseAlgorithm {
vparams->drawTool()->disableLighting();

DetectionOutput output = d_output.getValue() ;
for (unsigned i=0;i<output.size();i++) {
vparams->drawTool()->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;i<outputInside.size();i++) {
vparams->drawTool()->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<defaulttype::Vec3Types>* mstate
= l_from->getContext()->get<sofa::component::statecontainer::MechanicalObject<defaulttype::Vec3Types>>();
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<defaulttype::Vec3Types>* mstate
= l_from->getContext()->get<sofa::component::statecontainer::MechanicalObject<defaulttype::Vec3Types>>();
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<TetrahedronProximity>(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();
}

};
Expand Down