[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.

Author: th-skam

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")
 

src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h

@@ -87,8 +87,8 @@ class InsertionAlgorithm : public BaseAlgorithm {
 
         if (outputList.size() == 0) 
         {
-            const sofa::component::statecontainer::MechanicalObject<defaulttype::Vec3Types>* mstate
-                = l_from->getContext()->get<sofa::component::statecontainer::MechanicalObject<defaulttype::Vec3Types>>();
+            const sofa::core::behavior::MechanicalState<defaulttype::Vec3Types>* mstate
+                = l_from->getContext()->get<sofa::core::behavior::MechanicalState<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.";
@@ -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;