From b950468d08140294fe62690363ede8a3ecac85cc Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 17 Jul 2025 09:19:35 +0200 Subject: [PATCH] [Needle][algorithm] Modified scene and algorithm to allow for the EdgeNormalHandler to operate The moment the first coupling point is created (right after the puncture), it is important to reproject the first proximity back on the needle. This will create an EdgeProximity type and will allow the EdgeNormalHandler to operate. Otherwise, a MechanicalProximity is used for which there are no specialized functions. --- scenes/NeedleInsertion.py | 21 ++++++++++--------- .../algorithm/InsertionAlgorithm.h | 12 +++++++---- 2 files changed, 19 insertions(+), 14 deletions(-) diff --git a/scenes/NeedleInsertion.py b/scenes/NeedleInsertion.py index 2cf1f06a..7671bf92 100644 --- a/scenes/NeedleInsertion.py +++ b/scenes/NeedleInsertion.py @@ -94,14 +94,15 @@ def createScene(root): needleBodyCollision = needle.addChild("bodyCollision") needleBodyCollision.addObject("EdgeSetTopologyContainer", name="Container_body", src="@../Container") needleBodyCollision.addObject("MechanicalObject",name="mstate_body", template="Vec3d",) - needleBodyCollision.addObject("EdgeGeometry",name="geom",mstate="@mstate_body", topology="@Container_body") + needleBodyCollision.addObject("EdgeGeometry",name="geom_body",mstate="@mstate_body", topology="@Container_body") + needleBodyCollision.addObject("EdgeNormalHandler", name="NeedleBeams", geometry="@geom_body") needleBodyCollision.addObject("IdentityMapping") needleTipCollision = needle.addChild("tipCollision") needleTipCollision.addObject("MechanicalObject",name="mstate_tip",position=[g_needleLength+g_needleBaseOffset[0], g_needleBaseOffset[1], g_needleBaseOffset[2]],template="Vec3d",) - needleTipCollision.addObject("PointGeometry",name="geom",mstate="@mstate_tip") + needleTipCollision.addObject("PointGeometry",name="geom_tip",mstate="@mstate_tip") needleTipCollision.addObject("RigidMapping",globalToLocalCoords=True,index=g_needleNumberOfElems) @@ -177,21 +178,21 @@ def createScene(root): root.addObject("InsertionAlgorithm", name="InsertionAlgo", - fromGeom="@Needle/tipCollision/geom", + fromGeom="@Needle/tipCollision/geom_tip", destGeom="@Volume/collision/geom_tri", - fromVol="@Needle/bodyCollision/geom", + fromVol="@Needle/bodyCollision/geom_body", destVol="@Volume/geom_tetra", - punctureThreshold=0.1, - slideDistance=0.005 + punctureThreshold=0.05, + slideDistance=0.005, + drawcollision=True, + sphereRadius=0.0001 #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") + root.addObject("FirstDirection",name="bindDirection", handler="@Needle/bodyCollision/NeedleBeams") + root.addObject("ConstraintInsertion",input="@InsertionAlgo.outputList", directions="@bindDirection",draw_scale="0.005")#, mu="0.001") - #root.addObject("InsertionAlgorithm",name="InsertionAlgo",fromGeom="@Needle/tipCollision/geom", destGeom="@Volume/tri_geom") - #root.addObject("DistanceFilter",algo="@InsertionAlgo",distance=0.005) - #root.addObject("BindDirection",name="insertionDirection")#,handler="@Volume/InternalTriangles") - #root.addObject("ConstraintBilateral",input="@InsertionAlgo.output",directions="@punctureDirection",draw_scale="0.001") diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index e27f99f5..e17d880e 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -87,8 +87,8 @@ class InsertionAlgorithm : public BaseAlgorithm { if (outputList.size() == 0) { - const sofa::component::statecontainer::MechanicalObject* mstate - = l_from->getContext()->get>(); + const sofa::core::behavior::MechanicalState* 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."; @@ -99,11 +99,15 @@ class InsertionAlgorithm : public BaseAlgorithm { m_constraintSolver->getLambda()[mstate].read()->getValue(); if (lambda[0].norm() > d_punctureThreshold.getValue()) { + auto findClosestProxOp_needle = Operations::FindClosestProximity::Operation::get(l_fromVol); + auto projectOp_needle = Operations::Project::Operation::get(l_fromVol); for (const auto& dpair : output) { - outputList.add(dpair.first->copy(), dpair.second->copy()); - m_needlePts.push_back(dpair.first->copy()); + // Reproject onto the needle to create an EdgeProximity - The EdgeHandler requires this + auto pfromVol = findClosestProxOp_needle(dpair.second, l_fromVol.get(), projectOp_needle, getFilterFunc()); + m_needlePts.push_back(pfromVol); m_couplingPts.push_back(dpair.second->copy()); + outputList.add(pfromVol, dpair.second->copy()); } output.clear(); return;