From 0f07cab192d59cdaf619f2cf627e53a1266edacd Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 4 Sep 2025 11:44:45 +0200 Subject: [PATCH 01/18] [algorithm] Modified scene to illustrate tight puncture bug --- scenes/NeedleInsertion.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/scenes/NeedleInsertion.py b/scenes/NeedleInsertion.py index cea29819..95b401d1 100644 --- a/scenes/NeedleInsertion.py +++ b/scenes/NeedleInsertion.py @@ -6,7 +6,7 @@ g_needleRadius = 0.001 #(m) g_needleMechanicalParameters = { "radius":g_needleRadius, - "youngModulus":1e11, + "youngModulus":1e12, "poissonRatio":0.3 } g_needleTotalMass=0.01 @@ -17,7 +17,7 @@ "max":[0.125, 0.125, -0.100] } #Again all in mm g_gelMechanicalParameters = { - "youngModulus":8e5, + "youngModulus":1e6, "poissonRatio":0.45, "method":"large" } @@ -60,7 +60,7 @@ def createScene(root): root.addObject("ConstraintAttachButtonSetting") root.addObject("VisualStyle", displayFlags="showVisualModels hideBehaviorModels showCollisionModels hideMappings hideForceFields showWireframe showInteractionForceFields" ) root.addObject("FreeMotionAnimationLoop") - root.addObject("GenericConstraintSolver", tolerance=0.00001, maxIt=5000) + root.addObject("GenericConstraintSolver", tolerance=1e-3, maxIt=5000) root.addObject("CollisionLoop") needleBaseMaster = root.addChild("NeedleBaseMaster") @@ -189,7 +189,7 @@ def createScene(root): surfGeom="@Volume/collision/geom_tri", shaftGeom="@Needle/bodyCollision/geom_body", volGeom="@Volume/geom_tetra", - punctureForceThreshold=2., + punctureForceThreshold=20, tipDistThreshold=0.003, drawcollision=True, drawPointsScale=0.0001 From aaffc91eec9be27357acae8083bfad798c5c506e Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 4 Sep 2025 17:17:34 +0200 Subject: [PATCH 02/18] [algorithm] Fix works only for tip distance equal to the line segments --- scenes/NeedleInsertion.py | 2 +- .../algorithm/InsertionAlgorithm.h | 111 ++++++++++++++---- 2 files changed, 89 insertions(+), 24 deletions(-) diff --git a/scenes/NeedleInsertion.py b/scenes/NeedleInsertion.py index 95b401d1..98e4de9f 100644 --- a/scenes/NeedleInsertion.py +++ b/scenes/NeedleInsertion.py @@ -190,7 +190,7 @@ def createScene(root): shaftGeom="@Needle/bodyCollision/geom_body", volGeom="@Volume/geom_tetra", punctureForceThreshold=20, - tipDistThreshold=0.003, + tipDistThreshold=0.005, drawcollision=True, drawPointsScale=0.0001 ) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 2ab650fb..f88814ac 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -16,7 +16,7 @@ namespace sofa::collisionAlgorithm class InsertionAlgorithm : public BaseAlgorithm { - public: + public: SOFA_CLASS(InsertionAlgorithm, BaseAlgorithm); typedef core::behavior::MechanicalState MechStateTipType; @@ -216,35 +216,101 @@ class InsertionAlgorithm : public BaseAlgorithm if (!tipProx) return; // 2.1 Check whether coupling point should be added - const type::Vec3 tip2Pt = m_couplingPts.back()->getPosition() - tipProx->getPosition(); - if (tip2Pt.norm() > d_tipDistThreshold.getValue()) + type::Vec3 lastCp = m_couplingPts.back()->getPosition(); + const SReal tipDistThreshold = this->d_tipDistThreshold.getValue(); + const type::Vec3 tip2Pt = lastCp - tipProx->getPosition(); + if (tip2Pt.norm() > tipDistThreshold) { + // find our current segment: + auto createShaftProximity = + Operations::CreateCenterProximity::Operation::get(l_shaftGeom->getTypeInfo()); + auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom); auto findClosestProxOnVol = Operations::FindClosestProximity::Operation::get(l_volGeom); 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) + for (auto itShaft = l_shaftGeom->begin(); itShaft != l_shaftGeom->end(); itShaft++) { - TetrahedronProximity::SPtr tetProx = - dynamic_pointer_cast(volProx); - if (tetProx) + BaseProximity::SPtr shaftProx = createShaftProximity(itShaft->element()); + if (shaftProx) { - 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) + const EdgeProximity::SPtr edgeProx = + dynamic_pointer_cast(shaftProx); + if (edgeProx) { - volProx->normalize(); - m_couplingPts.push_back(volProx); + const type::Vec3 p0 = edgeProx->element()->getP0()->getPosition(); + const type::Vec3 p1 = edgeProx->element()->getP1()->getPosition(); + const type::Vec3 newCp = lastCp + tipDistThreshold * (p1 - lastCp).normalized(); + if(dot(tip2Pt, (newCp - lastCp)) > 0_sreal) continue; + const type::Vec3 edgeNormal = (p1 - p0).normalized(); + const SReal edgeSegmentLength = (p1 - p0).norm(); + const type::Vec3 p0toCp = newCp - p0; + const SReal dotProd = dot(edgeNormal, p0toCp); + if (dotProd < 0_sreal || dotProd > edgeSegmentLength) continue; + + shaftProx = projectOnShaft(newCp, itShaft->element()).prox; + const BaseProximity::SPtr volProx = + findClosestProxOnVol(shaftProx, l_volGeom.get(), projectOnVol, getFilterFunc()); + if (volProx) + { + TetrahedronProximity::SPtr tetProx = + dynamic_pointer_cast(volProx); + if (tetProx) + { + double f0(tetProx->f0()), f1(tetProx->f1()), f2(tetProx->f2()), + f3(tetProx->f3()); + bool isInTetra = toolbox::TetrahedronToolBox::isInTetra( + shaftProx->getPosition(), tetProx->element()->getTetrahedronInfo(), f0, + f1, f2, f3); + if (isInTetra) + { + volProx->normalize(); + m_couplingPts.push_back(volProx); + lastCp = volProx->getPosition(); + } + } + } } } } } + + + //ElementIterator::SPtr itTip = l_tipGeom->begin(); + //auto createTipProximity = + // Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo()); + //const BaseProximity::SPtr tipProx = createTipProximity(itTip->element()); + //if (!tipProx) return; + // + //// 2.1 Check whether coupling point should be added + //const type::Vec3 tip2Pt = m_couplingPts.back()->getPosition() - tipProx->getPosition(); + //if (tip2Pt.norm() > d_tipDistThreshold.getValue()) + //{ + // auto findClosestProxOnVol = + // Operations::FindClosestProximity::Operation::get(l_volGeom); + // 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) + // { + // TetrahedronProximity::SPtr tetProx = + // dynamic_pointer_cast(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 { // 2.2. Check whether coupling point should be removed @@ -261,8 +327,8 @@ class InsertionAlgorithm : public BaseAlgorithm const type::Vec3 normal = (edgeProx->element()->getP1()->getPosition() - edgeProx->element()->getP0()->getPosition()) .normalized(); - // If the (last) coupling point lies ahead of the tip (positive dot product), - // the needle is retreating. Thus, that point is removed. + // If the (last) coupling point lies ahead of the tip (positive dot + // product), the needle is retreating. Thus, that point is removed. if (dot(tip2Pt, normal) > 0_sreal) { m_couplingPts.pop_back(); @@ -299,8 +365,7 @@ class InsertionAlgorithm : public BaseAlgorithm // This is a final-frontier check: If there are coupling points stored, but the // findClosestProxOnShaf operation yields no proximities on the shaft, it could be // because the needle has exited abruptly. Thus, we clear the coupling points. - if (insertionOutput.size() == 0) - m_couplingPts.clear(); + if (insertionOutput.size() == 0) m_couplingPts.clear(); } d_collisionOutput.endEdit(); From 3af4c9c3c2e56c0c13549c60f72c90575fc1e3d9 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 5 Sep 2025 11:38:42 +0200 Subject: [PATCH 03/18] [algorithm] Renamed lastCp --- .../collisionAlgorithm/algorithm/InsertionAlgorithm.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index f88814ac..d6a70953 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -216,9 +216,9 @@ class InsertionAlgorithm : public BaseAlgorithm if (!tipProx) return; // 2.1 Check whether coupling point should be added - type::Vec3 lastCp = m_couplingPts.back()->getPosition(); + type::Vec3 lastCouplingPt = m_couplingPts.back()->getPosition(); const SReal tipDistThreshold = this->d_tipDistThreshold.getValue(); - const type::Vec3 tip2Pt = lastCp - tipProx->getPosition(); + const type::Vec3 tip2Pt = lastCouplingPt - tipProx->getPosition(); if (tip2Pt.norm() > tipDistThreshold) { // find our current segment: @@ -239,8 +239,8 @@ class InsertionAlgorithm : public BaseAlgorithm { const type::Vec3 p0 = edgeProx->element()->getP0()->getPosition(); const type::Vec3 p1 = edgeProx->element()->getP1()->getPosition(); - const type::Vec3 newCp = lastCp + tipDistThreshold * (p1 - lastCp).normalized(); - if(dot(tip2Pt, (newCp - lastCp)) > 0_sreal) continue; + const type::Vec3 newCp = lastCouplingPt + tipDistThreshold * (p1 - lastCouplingPt).normalized(); + if(dot(tip2Pt, (newCp - lastCouplingPt)) > 0_sreal) continue; const type::Vec3 edgeNormal = (p1 - p0).normalized(); const SReal edgeSegmentLength = (p1 - p0).norm(); const type::Vec3 p0toCp = newCp - p0; @@ -265,7 +265,7 @@ class InsertionAlgorithm : public BaseAlgorithm { volProx->normalize(); m_couplingPts.push_back(volProx); - lastCp = volProx->getPosition(); + lastCouplingPt = volProx->getPosition(); } } } From 24f7e7abc02203dac77255c256f1a3fda84c50f5 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 5 Sep 2025 11:39:10 +0200 Subject: [PATCH 04/18] [algorithm] Renamed tip2Pt --- .../algorithm/InsertionAlgorithm.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index d6a70953..cf62cf13 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -218,8 +218,8 @@ class InsertionAlgorithm : public BaseAlgorithm // 2.1 Check whether coupling point should be added type::Vec3 lastCouplingPt = m_couplingPts.back()->getPosition(); const SReal tipDistThreshold = this->d_tipDistThreshold.getValue(); - const type::Vec3 tip2Pt = lastCouplingPt - tipProx->getPosition(); - if (tip2Pt.norm() > tipDistThreshold) + const type::Vec3 tipToLastCouplingPt = lastCouplingPt - tipProx->getPosition(); + if (tipToLastCouplingPt.norm() > tipDistThreshold) { // find our current segment: auto createShaftProximity = @@ -240,7 +240,7 @@ class InsertionAlgorithm : public BaseAlgorithm const type::Vec3 p0 = edgeProx->element()->getP0()->getPosition(); const type::Vec3 p1 = edgeProx->element()->getP1()->getPosition(); const type::Vec3 newCp = lastCouplingPt + tipDistThreshold * (p1 - lastCouplingPt).normalized(); - if(dot(tip2Pt, (newCp - lastCouplingPt)) > 0_sreal) continue; + if(dot(tipToLastCouplingPt, (newCp - lastCouplingPt)) > 0_sreal) continue; const type::Vec3 edgeNormal = (p1 - p0).normalized(); const SReal edgeSegmentLength = (p1 - p0).norm(); const type::Vec3 p0toCp = newCp - p0; @@ -282,8 +282,8 @@ class InsertionAlgorithm : public BaseAlgorithm //if (!tipProx) return; // //// 2.1 Check whether coupling point should be added - //const type::Vec3 tip2Pt = m_couplingPts.back()->getPosition() - tipProx->getPosition(); - //if (tip2Pt.norm() > d_tipDistThreshold.getValue()) + //const type::Vec3 tipToLastCouplingPt = m_couplingPts.back()->getPosition() - tipProx->getPosition(); + //if (tipToLastCouplingPt.norm() > d_tipDistThreshold.getValue()) //{ // auto findClosestProxOnVol = // Operations::FindClosestProximity::Operation::get(l_volGeom); @@ -329,7 +329,7 @@ class InsertionAlgorithm : public BaseAlgorithm .normalized(); // If the (last) coupling point lies ahead of the tip (positive dot // product), the needle is retreating. Thus, that point is removed. - if (dot(tip2Pt, normal) > 0_sreal) + if (dot(tipToLastCouplingPt, normal) > 0_sreal) { m_couplingPts.pop_back(); } From ee4889e5ded5f4623b211311e4709c4434871c3b Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 5 Sep 2025 11:40:08 +0200 Subject: [PATCH 05/18] [algorithm] Renamed newCp --- .../collisionAlgorithm/algorithm/InsertionAlgorithm.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index cf62cf13..7b7961d9 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -239,15 +239,15 @@ class InsertionAlgorithm : public BaseAlgorithm { const type::Vec3 p0 = edgeProx->element()->getP0()->getPosition(); const type::Vec3 p1 = edgeProx->element()->getP1()->getPosition(); - const type::Vec3 newCp = lastCouplingPt + tipDistThreshold * (p1 - lastCouplingPt).normalized(); - if(dot(tipToLastCouplingPt, (newCp - lastCouplingPt)) > 0_sreal) continue; + const type::Vec3 candidateCouplingPt = lastCouplingPt + tipDistThreshold * (p1 - lastCouplingPt).normalized(); + if(dot(tipToLastCouplingPt, (candidateCouplingPt - lastCouplingPt)) > 0_sreal) continue; const type::Vec3 edgeNormal = (p1 - p0).normalized(); const SReal edgeSegmentLength = (p1 - p0).norm(); - const type::Vec3 p0toCp = newCp - p0; + const type::Vec3 p0toCp = candidateCouplingPt - p0; const SReal dotProd = dot(edgeNormal, p0toCp); if (dotProd < 0_sreal || dotProd > edgeSegmentLength) continue; - shaftProx = projectOnShaft(newCp, itShaft->element()).prox; + shaftProx = projectOnShaft(candidateCouplingPt, itShaft->element()).prox; const BaseProximity::SPtr volProx = findClosestProxOnVol(shaftProx, l_volGeom.get(), projectOnVol, getFilterFunc()); if (volProx) From e56f7bf4f3faeca41f0bebe5b19c5b4c69ef9c04 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 5 Sep 2025 11:45:16 +0200 Subject: [PATCH 06/18] [algorithm] Renamed p0toCp --- src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 7b7961d9..c7573d05 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -243,8 +243,8 @@ class InsertionAlgorithm : public BaseAlgorithm if(dot(tipToLastCouplingPt, (candidateCouplingPt - lastCouplingPt)) > 0_sreal) continue; const type::Vec3 edgeNormal = (p1 - p0).normalized(); const SReal edgeSegmentLength = (p1 - p0).norm(); - const type::Vec3 p0toCp = candidateCouplingPt - p0; - const SReal dotProd = dot(edgeNormal, p0toCp); + const type::Vec3 p0ToCandidatePt = candidateCouplingPt - p0; + const SReal dotProd = dot(edgeNormal, p0ToCandidatePt); if (dotProd < 0_sreal || dotProd > edgeSegmentLength) continue; shaftProx = projectOnShaft(candidateCouplingPt, itShaft->element()).prox; From e6e67355c6cae605b89f846f4208f6ed0253a1aa Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 5 Sep 2025 11:46:34 +0200 Subject: [PATCH 07/18] [algorithm] Adopted CP abbreviation for coupling point --- .../algorithm/InsertionAlgorithm.h | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index c7573d05..d324d481 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -216,10 +216,10 @@ class InsertionAlgorithm : public BaseAlgorithm if (!tipProx) return; // 2.1 Check whether coupling point should be added - type::Vec3 lastCouplingPt = m_couplingPts.back()->getPosition(); + type::Vec3 lastCP = m_couplingPts.back()->getPosition(); const SReal tipDistThreshold = this->d_tipDistThreshold.getValue(); - const type::Vec3 tipToLastCouplingPt = lastCouplingPt - tipProx->getPosition(); - if (tipToLastCouplingPt.norm() > tipDistThreshold) + const type::Vec3 tipToLastCP = lastCP - tipProx->getPosition(); + if (tipToLastCP.norm() > tipDistThreshold) { // find our current segment: auto createShaftProximity = @@ -239,15 +239,15 @@ class InsertionAlgorithm : public BaseAlgorithm { const type::Vec3 p0 = edgeProx->element()->getP0()->getPosition(); const type::Vec3 p1 = edgeProx->element()->getP1()->getPosition(); - const type::Vec3 candidateCouplingPt = lastCouplingPt + tipDistThreshold * (p1 - lastCouplingPt).normalized(); - if(dot(tipToLastCouplingPt, (candidateCouplingPt - lastCouplingPt)) > 0_sreal) continue; + const type::Vec3 candidateCP = lastCP + tipDistThreshold * (p1 - lastCP).normalized(); + if(dot(tipToLastCP, (candidateCP - lastCP)) > 0_sreal) continue; const type::Vec3 edgeNormal = (p1 - p0).normalized(); const SReal edgeSegmentLength = (p1 - p0).norm(); - const type::Vec3 p0ToCandidatePt = candidateCouplingPt - p0; - const SReal dotProd = dot(edgeNormal, p0ToCandidatePt); + const type::Vec3 p0ToCandidateCP = candidateCP - p0; + const SReal dotProd = dot(edgeNormal, p0ToCandidateCP); if (dotProd < 0_sreal || dotProd > edgeSegmentLength) continue; - shaftProx = projectOnShaft(candidateCouplingPt, itShaft->element()).prox; + shaftProx = projectOnShaft(candidateCP, itShaft->element()).prox; const BaseProximity::SPtr volProx = findClosestProxOnVol(shaftProx, l_volGeom.get(), projectOnVol, getFilterFunc()); if (volProx) @@ -265,7 +265,7 @@ class InsertionAlgorithm : public BaseAlgorithm { volProx->normalize(); m_couplingPts.push_back(volProx); - lastCouplingPt = volProx->getPosition(); + lastCP = volProx->getPosition(); } } } @@ -282,8 +282,8 @@ class InsertionAlgorithm : public BaseAlgorithm //if (!tipProx) return; // //// 2.1 Check whether coupling point should be added - //const type::Vec3 tipToLastCouplingPt = m_couplingPts.back()->getPosition() - tipProx->getPosition(); - //if (tipToLastCouplingPt.norm() > d_tipDistThreshold.getValue()) + //const type::Vec3 tipToLastCP = m_couplingPts.back()->getPosition() - tipProx->getPosition(); + //if (tipToLastCP.norm() > d_tipDistThreshold.getValue()) //{ // auto findClosestProxOnVol = // Operations::FindClosestProximity::Operation::get(l_volGeom); @@ -329,7 +329,7 @@ class InsertionAlgorithm : public BaseAlgorithm .normalized(); // If the (last) coupling point lies ahead of the tip (positive dot // product), the needle is retreating. Thus, that point is removed. - if (dot(tipToLastCouplingPt, normal) > 0_sreal) + if (dot(tipToLastCP, normal) > 0_sreal) { m_couplingPts.pop_back(); } From d4431b8feb4ed01fbe4db0da6f53ce7e284c6164 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 5 Sep 2025 11:47:41 +0200 Subject: [PATCH 08/18] [algorithm] Renamed m_couplingPts to m_CPs --- .../algorithm/InsertionAlgorithm.h | 30 +++++++++---------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index d324d481..32f7361a 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -31,7 +31,7 @@ class InsertionAlgorithm : public BaseAlgorithm Data d_projective; Data d_punctureForceThreshold, d_tipDistThreshold; ConstraintSolver::SPtr m_constraintSolver; - std::vector m_couplingPts; + std::vector m_CPs; Data d_drawCollision, d_drawPoints; Data d_drawPointsScale; @@ -58,7 +58,7 @@ class InsertionAlgorithm : public BaseAlgorithm "the last proximity detection. Once exceeded, a new " "proximity pair is added for the needle-volume coupling.")), m_constraintSolver(nullptr), - m_couplingPts(), + m_CPs(), d_drawCollision(initData(&d_drawCollision, false, "drawcollision", "Draw collision.")), d_drawPoints(initData(&d_drawPoints, false, "drawPoints", "Draw detection outputs.")), d_drawPointsScale(initData(&d_drawPointsScale, 0.0005, "drawPointsScale", @@ -125,7 +125,7 @@ class InsertionAlgorithm : public BaseAlgorithm insertionOutput.clear(); collisionOutput.clear(); - if (m_couplingPts.empty()) + if (m_CPs.empty()) { // 1. Puncture algorithm auto createTipProximity = @@ -161,7 +161,7 @@ class InsertionAlgorithm : public BaseAlgorithm } if (norm > punctureForceThreshold) { - m_couplingPts.push_back(surfProx); + m_CPs.push_back(surfProx); continue; } } @@ -178,7 +178,7 @@ class InsertionAlgorithm : public BaseAlgorithm } // 1.3 Collision with the shaft geometry - if (m_couplingPts.empty()) + if (m_CPs.empty()) { auto createShaftProximity = Operations::CreateCenterProximity::Operation::get(l_shaftGeom->getTypeInfo()); @@ -216,7 +216,7 @@ class InsertionAlgorithm : public BaseAlgorithm if (!tipProx) return; // 2.1 Check whether coupling point should be added - type::Vec3 lastCP = m_couplingPts.back()->getPosition(); + type::Vec3 lastCP = m_CPs.back()->getPosition(); const SReal tipDistThreshold = this->d_tipDistThreshold.getValue(); const type::Vec3 tipToLastCP = lastCP - tipProx->getPosition(); if (tipToLastCP.norm() > tipDistThreshold) @@ -264,7 +264,7 @@ class InsertionAlgorithm : public BaseAlgorithm if (isInTetra) { volProx->normalize(); - m_couplingPts.push_back(volProx); + m_CPs.push_back(volProx); lastCP = volProx->getPosition(); } } @@ -282,7 +282,7 @@ class InsertionAlgorithm : public BaseAlgorithm //if (!tipProx) return; // //// 2.1 Check whether coupling point should be added - //const type::Vec3 tipToLastCP = m_couplingPts.back()->getPosition() - tipProx->getPosition(); + //const type::Vec3 tipToLastCP = m_CPs.back()->getPosition() - tipProx->getPosition(); //if (tipToLastCP.norm() > d_tipDistThreshold.getValue()) //{ // auto findClosestProxOnVol = @@ -306,7 +306,7 @@ class InsertionAlgorithm : public BaseAlgorithm // if (isInTetra) // { // volProx->normalize(); - // m_couplingPts.push_back(volProx); + // m_CPs.push_back(volProx); // } // } // } @@ -331,7 +331,7 @@ class InsertionAlgorithm : public BaseAlgorithm // product), the needle is retreating. Thus, that point is removed. if (dot(tipToLastCP, normal) > 0_sreal) { - m_couplingPts.pop_back(); + m_CPs.pop_back(); } } else @@ -348,24 +348,24 @@ class InsertionAlgorithm : public BaseAlgorithm } } - if (!m_couplingPts.empty()) + if (!m_CPs.empty()) { // 3. Re-project proximities on the shaft geometry auto findClosestProxOnShaft = Operations::FindClosestProximity::Operation::get(l_shaftGeom); auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom); - for (int i = 0; i < m_couplingPts.size(); i++) + for (int i = 0; i < m_CPs.size(); i++) { const BaseProximity::SPtr shaftProx = findClosestProxOnShaft( - m_couplingPts[i], l_shaftGeom.get(), projectOnShaft, getFilterFunc()); + m_CPs[i], l_shaftGeom.get(), projectOnShaft, getFilterFunc()); if (!shaftProx) continue; shaftProx->normalize(); - insertionOutput.add(shaftProx, m_couplingPts[i]); + insertionOutput.add(shaftProx, m_CPs[i]); } // This is a final-frontier check: If there are coupling points stored, but the // findClosestProxOnShaf operation yields no proximities on the shaft, it could be // because the needle has exited abruptly. Thus, we clear the coupling points. - if (insertionOutput.size() == 0) m_couplingPts.clear(); + if (insertionOutput.size() == 0) m_CPs.clear(); } d_collisionOutput.endEdit(); From 62c5447206fd53b2da443f310702441a1689331d Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 5 Sep 2025 11:49:31 +0200 Subject: [PATCH 09/18] [algorithm] Renamed edgeNormal --- src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 32f7361a..4199f187 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -241,10 +241,10 @@ class InsertionAlgorithm : public BaseAlgorithm const type::Vec3 p1 = edgeProx->element()->getP1()->getPosition(); const type::Vec3 candidateCP = lastCP + tipDistThreshold * (p1 - lastCP).normalized(); if(dot(tipToLastCP, (candidateCP - lastCP)) > 0_sreal) continue; - const type::Vec3 edgeNormal = (p1 - p0).normalized(); + const type::Vec3 shaftEdgeDirection = (p1 - p0).normalized(); const SReal edgeSegmentLength = (p1 - p0).norm(); const type::Vec3 p0ToCandidateCP = candidateCP - p0; - const SReal dotProd = dot(edgeNormal, p0ToCandidateCP); + const SReal dotProd = dot(shaftEdgeDirection, p0ToCandidateCP); if (dotProd < 0_sreal || dotProd > edgeSegmentLength) continue; shaftProx = projectOnShaft(candidateCP, itShaft->element()).prox; From dab1bb2084685030329c76520097adde83e3b6b3 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 5 Sep 2025 12:08:07 +0200 Subject: [PATCH 10/18] [algorithm] Renamed dotProd --- src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 4199f187..7050bb8b 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -241,11 +241,11 @@ class InsertionAlgorithm : public BaseAlgorithm const type::Vec3 p1 = edgeProx->element()->getP1()->getPosition(); const type::Vec3 candidateCP = lastCP + tipDistThreshold * (p1 - lastCP).normalized(); if(dot(tipToLastCP, (candidateCP - lastCP)) > 0_sreal) continue; - const type::Vec3 shaftEdgeDirection = (p1 - p0).normalized(); + const type::Vec3 shaftEdgeDir = (p1 - p0).normalized(); const SReal edgeSegmentLength = (p1 - p0).norm(); const type::Vec3 p0ToCandidateCP = candidateCP - p0; - const SReal dotProd = dot(shaftEdgeDirection, p0ToCandidateCP); - if (dotProd < 0_sreal || dotProd > edgeSegmentLength) continue; + const SReal projPtOnEdge = dot(p0ToCandidateCP, shaftEdgeDir); + if (projPtOnEdge < 0_sreal || projPtOnEdge > edgeSegmentLength) continue; shaftProx = projectOnShaft(candidateCP, itShaft->element()).prox; const BaseProximity::SPtr volProx = From 0a68bf628c94947c2390a70e0e5cbec62cfdf184 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 5 Sep 2025 14:18:15 +0200 Subject: [PATCH 11/18] [algorithm] Commented code --- .../algorithm/InsertionAlgorithm.h | 22 ++++++++++++++++--- 1 file changed, 19 insertions(+), 3 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 7050bb8b..3faf7465 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -215,19 +215,23 @@ class InsertionAlgorithm : public BaseAlgorithm const BaseProximity::SPtr tipProx = createTipProximity(itTip->element()); if (!tipProx) return; - // 2.1 Check whether coupling point should be added type::Vec3 lastCP = m_CPs.back()->getPosition(); const SReal tipDistThreshold = this->d_tipDistThreshold.getValue(); - const type::Vec3 tipToLastCP = lastCP - tipProx->getPosition(); + + // Vector from tip to last coupling point; used for distance and directional checks + const type::Vec3 tipToLastCP = lastCP - tipProx->getPosition(); + + // Only add a new coupling point if the needle tip has advanced far enough if (tipToLastCP.norm() > tipDistThreshold) { - // find our current segment: auto createShaftProximity = Operations::CreateCenterProximity::Operation::get(l_shaftGeom->getTypeInfo()); auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom); auto findClosestProxOnVol = Operations::FindClosestProximity::Operation::get(l_volGeom); auto projectOnVol = Operations::Project::Operation::get(l_volGeom); + + // Iterate over shaft segments to find which one contains the next candidate CP for (auto itShaft = l_shaftGeom->begin(); itShaft != l_shaftGeom->end(); itShaft++) { BaseProximity::SPtr shaftProx = createShaftProximity(itShaft->element()); @@ -239,17 +243,27 @@ class InsertionAlgorithm : public BaseAlgorithm { const type::Vec3 p0 = edgeProx->element()->getP0()->getPosition(); const type::Vec3 p1 = edgeProx->element()->getP1()->getPosition(); + + // Candidate coupling point along shaft segment const type::Vec3 candidateCP = lastCP + tipDistThreshold * (p1 - lastCP).normalized(); + + // Skip if candidate CP lies before the last CP if(dot(tipToLastCP, (candidateCP - lastCP)) > 0_sreal) continue; + + // Project candidate CP onto the edge element and compute scalar coordinate along segment const type::Vec3 shaftEdgeDir = (p1 - p0).normalized(); const SReal edgeSegmentLength = (p1 - p0).norm(); const type::Vec3 p0ToCandidateCP = candidateCP - p0; const SReal projPtOnEdge = dot(p0ToCandidateCP, shaftEdgeDir); + + // Skip if candidate CP is outside current edge segment if (projPtOnEdge < 0_sreal || projPtOnEdge > edgeSegmentLength) continue; + // Project candidate CP onto shaft geometry and then find nearest volume proximity shaftProx = projectOnShaft(candidateCP, itShaft->element()).prox; const BaseProximity::SPtr volProx = findClosestProxOnVol(shaftProx, l_volGeom.get(), projectOnVol, getFilterFunc()); + if (volProx) { TetrahedronProximity::SPtr tetProx = @@ -261,6 +275,8 @@ class InsertionAlgorithm : public BaseAlgorithm bool isInTetra = toolbox::TetrahedronToolBox::isInTetra( shaftProx->getPosition(), tetProx->element()->getTetrahedronInfo(), f0, f1, f2, f3); + + // Ensure candidate CP lies inside tetrahedron if (isInTetra) { volProx->normalize(); From cc5cf76fb781bc3385d82a6495b962a51f7f6e7e Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 5 Sep 2025 14:29:30 +0200 Subject: [PATCH 12/18] [algorithm] Format code --- .../algorithm/InsertionAlgorithm.h | 169 +++++++++--------- 1 file changed, 84 insertions(+), 85 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 3faf7465..467a79fb 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -219,7 +219,7 @@ class InsertionAlgorithm : public BaseAlgorithm const SReal tipDistThreshold = this->d_tipDistThreshold.getValue(); // Vector from tip to last coupling point; used for distance and directional checks - const type::Vec3 tipToLastCP = lastCP - tipProx->getPosition(); + const type::Vec3 tipToLastCP = lastCP - tipProx->getPosition(); // Only add a new coupling point if the needle tip has advanced far enough if (tipToLastCP.norm() > tipDistThreshold) @@ -235,98 +235,97 @@ class InsertionAlgorithm : public BaseAlgorithm for (auto itShaft = l_shaftGeom->begin(); itShaft != l_shaftGeom->end(); itShaft++) { BaseProximity::SPtr shaftProx = createShaftProximity(itShaft->element()); - if (shaftProx) + if (!shaftProx) continue; + + const EdgeProximity::SPtr edgeProx = + dynamic_pointer_cast(shaftProx); + if (!edgeProx) continue; + + const type::Vec3 p0 = edgeProx->element()->getP0()->getPosition(); + const type::Vec3 p1 = edgeProx->element()->getP1()->getPosition(); + + // Candidate coupling point along shaft segment + const type::Vec3 candidateCP = + lastCP + tipDistThreshold * (p1 - lastCP).normalized(); + + // Skip if candidate CP lies before the last CP + if (dot(tipToLastCP, (candidateCP - lastCP)) > 0_sreal) continue; + + // Project candidate CP onto the edge element and compute scalar coordinate + // along segment + const type::Vec3 shaftEdgeDir = (p1 - p0).normalized(); + const SReal edgeSegmentLength = (p1 - p0).norm(); + const type::Vec3 p0ToCandidateCP = candidateCP - p0; + const SReal projPtOnEdge = dot(p0ToCandidateCP, shaftEdgeDir); + + // Skip if candidate CP is outside current edge segment + if (projPtOnEdge < 0_sreal || projPtOnEdge > edgeSegmentLength) continue; + + // Project candidate CP onto shaft geometry and then find nearest volume + // proximity + shaftProx = projectOnShaft(candidateCP, itShaft->element()).prox; + const BaseProximity::SPtr volProx = findClosestProxOnVol( + shaftProx, l_volGeom.get(), projectOnVol, getFilterFunc()); + if (!volProx) continue; + + TetrahedronProximity::SPtr tetProx = + dynamic_pointer_cast(volProx); + if (!tetProx) continue; + + double f0(tetProx->f0()), f1(tetProx->f1()), f2(tetProx->f2()), + f3(tetProx->f3()); + bool isInTetra = toolbox::TetrahedronToolBox::isInTetra( + shaftProx->getPosition(), tetProx->element()->getTetrahedronInfo(), f0, f1, + f2, f3); + + // Ensure candidate CP lies inside tetrahedron + if (isInTetra) { - const EdgeProximity::SPtr edgeProx = - dynamic_pointer_cast(shaftProx); - if (edgeProx) - { - const type::Vec3 p0 = edgeProx->element()->getP0()->getPosition(); - const type::Vec3 p1 = edgeProx->element()->getP1()->getPosition(); - - // Candidate coupling point along shaft segment - const type::Vec3 candidateCP = lastCP + tipDistThreshold * (p1 - lastCP).normalized(); - - // Skip if candidate CP lies before the last CP - if(dot(tipToLastCP, (candidateCP - lastCP)) > 0_sreal) continue; - - // Project candidate CP onto the edge element and compute scalar coordinate along segment - const type::Vec3 shaftEdgeDir = (p1 - p0).normalized(); - const SReal edgeSegmentLength = (p1 - p0).norm(); - const type::Vec3 p0ToCandidateCP = candidateCP - p0; - const SReal projPtOnEdge = dot(p0ToCandidateCP, shaftEdgeDir); - - // Skip if candidate CP is outside current edge segment - if (projPtOnEdge < 0_sreal || projPtOnEdge > edgeSegmentLength) continue; - - // Project candidate CP onto shaft geometry and then find nearest volume proximity - shaftProx = projectOnShaft(candidateCP, itShaft->element()).prox; - const BaseProximity::SPtr volProx = - findClosestProxOnVol(shaftProx, l_volGeom.get(), projectOnVol, getFilterFunc()); - - if (volProx) - { - TetrahedronProximity::SPtr tetProx = - dynamic_pointer_cast(volProx); - if (tetProx) - { - double f0(tetProx->f0()), f1(tetProx->f1()), f2(tetProx->f2()), - f3(tetProx->f3()); - bool isInTetra = toolbox::TetrahedronToolBox::isInTetra( - shaftProx->getPosition(), tetProx->element()->getTetrahedronInfo(), f0, - f1, f2, f3); - - // Ensure candidate CP lies inside tetrahedron - if (isInTetra) - { - volProx->normalize(); - m_CPs.push_back(volProx); - lastCP = volProx->getPosition(); - } - } - } - } + volProx->normalize(); + m_CPs.push_back(volProx); + lastCP = volProx->getPosition(); } } } - - //ElementIterator::SPtr itTip = l_tipGeom->begin(); - //auto createTipProximity = - // Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo()); - //const BaseProximity::SPtr tipProx = createTipProximity(itTip->element()); - //if (!tipProx) return; + // ElementIterator::SPtr itTip = l_tipGeom->begin(); + // auto createTipProximity = + // Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo()); + // const BaseProximity::SPtr tipProx = createTipProximity(itTip->element()); + // if (!tipProx) return; // //// 2.1 Check whether coupling point should be added - //const type::Vec3 tipToLastCP = m_CPs.back()->getPosition() - tipProx->getPosition(); - //if (tipToLastCP.norm() > d_tipDistThreshold.getValue()) + // const type::Vec3 tipToLastCP = m_CPs.back()->getPosition() - tipProx->getPosition(); + // if (tipToLastCP.norm() > d_tipDistThreshold.getValue()) //{ - // auto findClosestProxOnVol = - // Operations::FindClosestProximity::Operation::get(l_volGeom); - // 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) - // { - // TetrahedronProximity::SPtr tetProx = - // dynamic_pointer_cast(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_CPs.push_back(volProx); - // } - // } - // } - //} + // auto findClosestProxOnVol = + // Operations::FindClosestProximity::Operation::get(l_volGeom); + // 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) + // { + // TetrahedronProximity::SPtr tetProx = + // dynamic_pointer_cast(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_CPs.push_back(volProx); + // } + // } + // } + // } else // Don't bother with removing the point that was just added { // 2.2. Check whether coupling point should be removed From 5dc642715213fd463c0a15b2d43a0cc624e9eada Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 5 Sep 2025 14:30:07 +0200 Subject: [PATCH 13/18] [algorithm] Remove old part of the code - tip-based detection --- .../algorithm/InsertionAlgorithm.h | 39 ------------------- 1 file changed, 39 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 467a79fb..3f321582 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -287,45 +287,6 @@ class InsertionAlgorithm : public BaseAlgorithm } } } - - // ElementIterator::SPtr itTip = l_tipGeom->begin(); - // auto createTipProximity = - // Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo()); - // const BaseProximity::SPtr tipProx = createTipProximity(itTip->element()); - // if (!tipProx) return; - // - //// 2.1 Check whether coupling point should be added - // const type::Vec3 tipToLastCP = m_CPs.back()->getPosition() - tipProx->getPosition(); - // if (tipToLastCP.norm() > d_tipDistThreshold.getValue()) - //{ - // auto findClosestProxOnVol = - // Operations::FindClosestProximity::Operation::get(l_volGeom); - // 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) - // { - // TetrahedronProximity::SPtr tetProx = - // dynamic_pointer_cast(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_CPs.push_back(volProx); - // } - // } - // } - // } else // Don't bother with removing the point that was just added { // 2.2. Check whether coupling point should be removed From 3104014326dabb9a3a601df56495b09e8b7c6dd3 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 5 Sep 2025 15:22:29 +0200 Subject: [PATCH 14/18] [algorithm] Finding location of last CP relies on shaft edge direction instead of tipToLastCP --- .../algorithm/InsertionAlgorithm.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 3f321582..eb36f2d5 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -243,17 +243,17 @@ class InsertionAlgorithm : public BaseAlgorithm const type::Vec3 p0 = edgeProx->element()->getP0()->getPosition(); const type::Vec3 p1 = edgeProx->element()->getP1()->getPosition(); + const type::Vec3 shaftEdgeDir = (p1 - p0).normalized(); + const type::Vec3 lastCPToP1 = p1 - lastCP; - // Candidate coupling point along shaft segment - const type::Vec3 candidateCP = - lastCP + tipDistThreshold * (p1 - lastCP).normalized(); + // Skip if last CP lies after edge end point + if (dot(shaftEdgeDir, lastCPToP1) < 0_sreal) continue; - // Skip if candidate CP lies before the last CP - if (dot(tipToLastCP, (candidateCP - lastCP)) > 0_sreal) continue; + // Candidate coupling point along shaft segment + const type::Vec3 candidateCP = lastCP + tipDistThreshold * shaftEdgeDir; // Project candidate CP onto the edge element and compute scalar coordinate // along segment - const type::Vec3 shaftEdgeDir = (p1 - p0).normalized(); const SReal edgeSegmentLength = (p1 - p0).norm(); const type::Vec3 p0ToCandidateCP = candidateCP - p0; const SReal projPtOnEdge = dot(p0ToCandidateCP, shaftEdgeDir); From 6b827241e6c28de5dbf710ccf86edae2cbf42b49 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 5 Sep 2025 15:39:15 +0200 Subject: [PATCH 15/18] [algorithm] Enabled addition of coupling points even if the spacing is smaller than the edge length --- scenes/NeedleInsertion.py | 2 +- .../algorithm/InsertionAlgorithm.h | 74 ++++++++++--------- 2 files changed, 41 insertions(+), 35 deletions(-) diff --git a/scenes/NeedleInsertion.py b/scenes/NeedleInsertion.py index 98e4de9f..95b401d1 100644 --- a/scenes/NeedleInsertion.py +++ b/scenes/NeedleInsertion.py @@ -190,7 +190,7 @@ def createScene(root): shaftGeom="@Needle/bodyCollision/geom_body", volGeom="@Volume/geom_tetra", punctureForceThreshold=20, - tipDistThreshold=0.005, + tipDistThreshold=0.003, drawcollision=True, drawPointsScale=0.0001 ) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index eb36f2d5..3d0944ce 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -249,41 +249,47 @@ class InsertionAlgorithm : public BaseAlgorithm // Skip if last CP lies after edge end point if (dot(shaftEdgeDir, lastCPToP1) < 0_sreal) continue; - // Candidate coupling point along shaft segment - const type::Vec3 candidateCP = lastCP + tipDistThreshold * shaftEdgeDir; - - // Project candidate CP onto the edge element and compute scalar coordinate - // along segment - const SReal edgeSegmentLength = (p1 - p0).norm(); - const type::Vec3 p0ToCandidateCP = candidateCP - p0; - const SReal projPtOnEdge = dot(p0ToCandidateCP, shaftEdgeDir); - - // Skip if candidate CP is outside current edge segment - if (projPtOnEdge < 0_sreal || projPtOnEdge > edgeSegmentLength) continue; - - // Project candidate CP onto shaft geometry and then find nearest volume - // proximity - shaftProx = projectOnShaft(candidateCP, itShaft->element()).prox; - const BaseProximity::SPtr volProx = findClosestProxOnVol( - shaftProx, l_volGeom.get(), projectOnVol, getFilterFunc()); - if (!volProx) continue; - - TetrahedronProximity::SPtr tetProx = - dynamic_pointer_cast(volProx); - if (!tetProx) continue; - - double f0(tetProx->f0()), f1(tetProx->f1()), f2(tetProx->f2()), - f3(tetProx->f3()); - bool isInTetra = toolbox::TetrahedronToolBox::isInTetra( - shaftProx->getPosition(), tetProx->element()->getTetrahedronInfo(), f0, f1, - f2, f3); - - // Ensure candidate CP lies inside tetrahedron - if (isInTetra) + const int numCPs = floor(lastCPToP1.norm() / tipDistThreshold); + + for(int idCP = 0 ; idCP < numCPs ; idCP++) { - volProx->normalize(); - m_CPs.push_back(volProx); - lastCP = volProx->getPosition(); + // Candidate coupling point along shaft segment + const type::Vec3 candidateCP = lastCP + tipDistThreshold * shaftEdgeDir; + + // Project candidate CP onto the edge element and compute scalar coordinate + // along segment + const SReal edgeSegmentLength = (p1 - p0).norm(); + const type::Vec3 p0ToCandidateCP = candidateCP - p0; + const SReal projPtOnEdge = dot(p0ToCandidateCP, shaftEdgeDir); + + // Skip if candidate CP is outside current edge segment + if (projPtOnEdge < 0_sreal || projPtOnEdge > edgeSegmentLength) break; + + // Project candidate CP onto shaft geometry and then find nearest volume + // proximity + shaftProx = projectOnShaft(candidateCP, itShaft->element()).prox; + if (!shaftProx) continue; + const BaseProximity::SPtr volProx = findClosestProxOnVol( + shaftProx, l_volGeom.get(), projectOnVol, getFilterFunc()); + if (!volProx) continue; + + TetrahedronProximity::SPtr tetProx = + dynamic_pointer_cast(volProx); + if (!tetProx) continue; + + double f0(tetProx->f0()), f1(tetProx->f1()), f2(tetProx->f2()), + f3(tetProx->f3()); + bool isInTetra = toolbox::TetrahedronToolBox::isInTetra( + shaftProx->getPosition(), tetProx->element()->getTetrahedronInfo(), f0, + f1, f2, f3); + + // Ensure candidate CP lies inside tetrahedron + if (isInTetra) + { + volProx->normalize(); + m_CPs.push_back(volProx); + lastCP = volProx->getPosition(); + } } } } From 9b5865206edea7017287c3b5e15f393222badd11 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 5 Sep 2025 16:12:59 +0200 Subject: [PATCH 16/18] [scene] Adjusted parameters in the scene --- scenes/NeedleInsertion.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/scenes/NeedleInsertion.py b/scenes/NeedleInsertion.py index 95b401d1..0a744d8a 100644 --- a/scenes/NeedleInsertion.py +++ b/scenes/NeedleInsertion.py @@ -60,7 +60,7 @@ def createScene(root): root.addObject("ConstraintAttachButtonSetting") root.addObject("VisualStyle", displayFlags="showVisualModels hideBehaviorModels showCollisionModels hideMappings hideForceFields showWireframe showInteractionForceFields" ) root.addObject("FreeMotionAnimationLoop") - root.addObject("GenericConstraintSolver", tolerance=1e-3, maxIt=5000) + root.addObject("GenericConstraintSolver", tolerance=1e-5, maxIt=5000) root.addObject("CollisionLoop") needleBaseMaster = root.addChild("NeedleBaseMaster") @@ -199,4 +199,4 @@ def createScene(root): root.addObject("ConstraintUnilateral",input="@InsertionAlgo.collisionOutput",directions="@punctureDirection",draw_scale=0.001) root.addObject("FirstDirection",name="bindDirection", handler="@Needle/bodyCollision/NeedleBeams") - root.addObject("ConstraintInsertion",input="@InsertionAlgo.insertionOutput", directions="@bindDirection",draw_scale=0.002, frictionCoeff=0.05) + root.addObject("ConstraintInsertion",input="@InsertionAlgo.insertionOutput", directions="@bindDirection",draw_scale=0.002, frictionCoeff=0.01) From aee4031fabfb7971ee104cb91a2f8f379f3bc8b9 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Fri, 5 Sep 2025 16:28:44 +0200 Subject: [PATCH 17/18] [algorithm] Revert back to m_couplingPts naming --- .../algorithm/InsertionAlgorithm.h | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 3d0944ce..4a6464c3 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -31,7 +31,7 @@ class InsertionAlgorithm : public BaseAlgorithm Data d_projective; Data d_punctureForceThreshold, d_tipDistThreshold; ConstraintSolver::SPtr m_constraintSolver; - std::vector m_CPs; + std::vector m_couplingPts; Data d_drawCollision, d_drawPoints; Data d_drawPointsScale; @@ -58,7 +58,7 @@ class InsertionAlgorithm : public BaseAlgorithm "the last proximity detection. Once exceeded, a new " "proximity pair is added for the needle-volume coupling.")), m_constraintSolver(nullptr), - m_CPs(), + m_couplingPts(), d_drawCollision(initData(&d_drawCollision, false, "drawcollision", "Draw collision.")), d_drawPoints(initData(&d_drawPoints, false, "drawPoints", "Draw detection outputs.")), d_drawPointsScale(initData(&d_drawPointsScale, 0.0005, "drawPointsScale", @@ -125,7 +125,7 @@ class InsertionAlgorithm : public BaseAlgorithm insertionOutput.clear(); collisionOutput.clear(); - if (m_CPs.empty()) + if (m_couplingPts.empty()) { // 1. Puncture algorithm auto createTipProximity = @@ -161,7 +161,7 @@ class InsertionAlgorithm : public BaseAlgorithm } if (norm > punctureForceThreshold) { - m_CPs.push_back(surfProx); + m_couplingPts.push_back(surfProx); continue; } } @@ -178,7 +178,7 @@ class InsertionAlgorithm : public BaseAlgorithm } // 1.3 Collision with the shaft geometry - if (m_CPs.empty()) + if (m_couplingPts.empty()) { auto createShaftProximity = Operations::CreateCenterProximity::Operation::get(l_shaftGeom->getTypeInfo()); @@ -215,7 +215,7 @@ class InsertionAlgorithm : public BaseAlgorithm const BaseProximity::SPtr tipProx = createTipProximity(itTip->element()); if (!tipProx) return; - type::Vec3 lastCP = m_CPs.back()->getPosition(); + type::Vec3 lastCP = m_couplingPts.back()->getPosition(); const SReal tipDistThreshold = this->d_tipDistThreshold.getValue(); // Vector from tip to last coupling point; used for distance and directional checks @@ -287,7 +287,7 @@ class InsertionAlgorithm : public BaseAlgorithm if (isInTetra) { volProx->normalize(); - m_CPs.push_back(volProx); + m_couplingPts.push_back(volProx); lastCP = volProx->getPosition(); } } @@ -313,7 +313,7 @@ class InsertionAlgorithm : public BaseAlgorithm // product), the needle is retreating. Thus, that point is removed. if (dot(tipToLastCP, normal) > 0_sreal) { - m_CPs.pop_back(); + m_couplingPts.pop_back(); } } else @@ -330,24 +330,24 @@ class InsertionAlgorithm : public BaseAlgorithm } } - if (!m_CPs.empty()) + if (!m_couplingPts.empty()) { // 3. Re-project proximities on the shaft geometry auto findClosestProxOnShaft = Operations::FindClosestProximity::Operation::get(l_shaftGeom); auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom); - for (int i = 0; i < m_CPs.size(); i++) + for (int i = 0; i < m_couplingPts.size(); i++) { const BaseProximity::SPtr shaftProx = findClosestProxOnShaft( - m_CPs[i], l_shaftGeom.get(), projectOnShaft, getFilterFunc()); + m_couplingPts[i], l_shaftGeom.get(), projectOnShaft, getFilterFunc()); if (!shaftProx) continue; shaftProx->normalize(); - insertionOutput.add(shaftProx, m_CPs[i]); + insertionOutput.add(shaftProx, m_couplingPts[i]); } // This is a final-frontier check: If there are coupling points stored, but the // findClosestProxOnShaf operation yields no proximities on the shaft, it could be // because the needle has exited abruptly. Thus, we clear the coupling points. - if (insertionOutput.size() == 0) m_CPs.clear(); + if (insertionOutput.size() == 0) m_couplingPts.clear(); } d_collisionOutput.endEdit(); From 37c2a59f4869b04b2404bc43d6feaa3640577586 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Tue, 9 Sep 2025 14:02:47 +0200 Subject: [PATCH 18/18] [algorithm] Using the ContainsInPoint operation --- .../algorithm/InsertionAlgorithm.h | 17 +++++------------ 1 file changed, 5 insertions(+), 12 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 4a6464c3..6ad6fa49 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -6,6 +6,7 @@ #include #include #include +#include #include #include #include @@ -230,6 +231,8 @@ class InsertionAlgorithm : public BaseAlgorithm auto findClosestProxOnVol = Operations::FindClosestProximity::Operation::get(l_volGeom); auto projectOnVol = Operations::Project::Operation::get(l_volGeom); + auto containsPointInVol = + Operations::ContainsPointInProximity::Operation::get(l_volGeom); // Iterate over shaft segments to find which one contains the next candidate CP for (auto itShaft = l_shaftGeom->begin(); itShaft != l_shaftGeom->end(); itShaft++) @@ -269,22 +272,12 @@ class InsertionAlgorithm : public BaseAlgorithm // proximity shaftProx = projectOnShaft(candidateCP, itShaft->element()).prox; if (!shaftProx) continue; + const BaseProximity::SPtr volProx = findClosestProxOnVol( shaftProx, l_volGeom.get(), projectOnVol, getFilterFunc()); if (!volProx) continue; - TetrahedronProximity::SPtr tetProx = - dynamic_pointer_cast(volProx); - if (!tetProx) continue; - - double f0(tetProx->f0()), f1(tetProx->f1()), f2(tetProx->f2()), - f3(tetProx->f3()); - bool isInTetra = toolbox::TetrahedronToolBox::isInTetra( - shaftProx->getPosition(), tetProx->element()->getTetrahedronInfo(), f0, - f1, f2, f3); - - // Ensure candidate CP lies inside tetrahedron - if (isInTetra) + if (containsPointInVol(shaftProx->getPosition(), volProx)) { volProx->normalize(); m_couplingPts.push_back(volProx);