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;