Skip to content

[algorithm] Check whether the needle tip is inside the detected tetrahedron proximity before adding it #64

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
epernod merged 3 commits into from
Aug 13, 2025
Merged

[algorithm] Check whether the needle tip is inside the detected tetrahedron proximity before adding it #64

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
33995a6
[algorithm] check whether the tip is inside tetra before adding the d…
th-skam Aug 11, 2025
6cf9e7c
[algorithm] Check tetProx after downcasting to TetrahedronProximity
th-skam Aug 11, 2025
97ad9dc
[algorithm] Added comments
th-skam Aug 13, 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
45 changes: 33 additions & 12 deletions src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
#include <sofa/collisionAlgorithm/operations/FindClosestProximity.h>
#include <sofa/collisionAlgorithm/operations/Project.h>
#include <sofa/collisionAlgorithm/proximity/EdgeProximity.h>
#include <sofa/collisionAlgorithm/proximity/TetrahedronProximity.h>
#include <sofa/component/constraint/lagrangian/solver/ConstraintSolverImpl.h>
#include <sofa/component/statecontainer/MechanicalObject.h>

Expand All @@ -15,7 +16,7 @@ namespace sofa::collisionAlgorithm

class InsertionAlgorithm : public BaseAlgorithm
{
public:
public:
SOFA_CLASS(InsertionAlgorithm, BaseAlgorithm);

typedef core::behavior::MechanicalState<defaulttype::Vec3Types> MechStateTipType;
Expand Down Expand Up @@ -154,7 +155,8 @@ class InsertionAlgorithm : public BaseAlgorithm
const auto& lambda =
m_constraintSolver->getLambda()[mstate.get()].read()->getValue();
SReal norm{0_sreal};
for (const auto& l : lambda) {
for (const auto& l : lambda)
{
norm += l.norm();
}
if (norm > punctureForceThreshold)
Expand All @@ -178,9 +180,9 @@ class InsertionAlgorithm : public BaseAlgorithm
// 1.3 Collision with the shaft geometry
if (collisionOutput.size())
{
auto createShaftProximity =
Operations::CreateCenterProximity::Operation::get(l_shaftGeom->getTypeInfo());
auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom);
auto createShaftProximity =
Operations::CreateCenterProximity::Operation::get(l_shaftGeom->getTypeInfo());
auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom);
for (auto itShaft = l_shaftGeom->begin(); itShaft != l_shaftGeom->end(); itShaft++)
{
BaseProximity::SPtr shaftProx = createShaftProximity(itShaft->element());
Expand All @@ -190,11 +192,12 @@ class InsertionAlgorithm : public BaseAlgorithm
if (surfProx)
{
surfProx->normalize();

// 1.2 If not, create a proximity pair for the tip-surface collision
if (d_projective.getValue())
{
shaftProx = projectOnShaft(surfProx->getPosition(), itShaft->element()).prox;
shaftProx =
projectOnShaft(surfProx->getPosition(), itShaft->element()).prox;
if (!shaftProx) continue;
shaftProx->normalize();
}
Expand All @@ -220,15 +223,30 @@ class InsertionAlgorithm : public BaseAlgorithm
auto projectOnVol = Operations::Project::Operation::get(l_volGeom);
const BaseProximity::SPtr volProx =
findClosestProxOnVol(tipProx, l_volGeom.get(), projectOnVol, getFilterFunc());
// Proximity can be detected before the tip enters the tetra (e.g. near a boundary face)
// Only accept proximities if the tip is inside the tetra during insertion
if (volProx)
{
volProx->normalize();
m_couplingPts.push_back(volProx);
TetrahedronProximity::SPtr tetProx =
dynamic_pointer_cast<TetrahedronProximity>(volProx);
if (tetProx)
{
double f0(tetProx->f0()), f1(tetProx->f1()), f2(tetProx->f2()),
f3(tetProx->f3());
bool isInTetra = toolbox::TetrahedronToolBox::isInTetra(
tipProx->getPosition(), tetProx->element()->getTetrahedronInfo(), f0,
f1, f2, f3);
if (isInTetra)
{
volProx->normalize();
m_couplingPts.push_back(volProx);
}
}
}
}
else // Don't bother with removing the point that was just added
else // Don't bother with removing the point that was just added
{
// 2.2. Check whether coupling point should be removed
// 2.2. Check whether coupling point should be removed
ElementIterator::SPtr itShaft = l_shaftGeom->begin(l_shaftGeom->getSize() - 2);
auto createShaftProximity =
Operations::CreateCenterProximity::Operation::get(itShaft->getTypeInfo());
Expand All @@ -237,7 +255,10 @@ class InsertionAlgorithm : public BaseAlgorithm
const type::Vec3 normal = (edgeProx->element()->getP1()->getPosition() -
edgeProx->element()->getP0()->getPosition())
.normalized();
if (dot(tip2Pt, normal) > 0_sreal) {
// If the coupling point lies ahead of the tip (positive dot product),
// the needle is retreating. The last coupling point is removed
if (dot(tip2Pt, normal) > 0_sreal)
{
m_couplingPts.pop_back();
}
}
Expand Down
Loading