[src] Merge and absorb modifications due to the addition of operation classes into the new architecture

Author: th-skam

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-5, 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
@@ -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)

src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h

@@ -6,8 +6,9 @@
 #include <CollisionAlgorithm/operations/CreateCenterProximity.h>
 #include <CollisionAlgorithm/operations/FindClosestProximity.h>
 #include <CollisionAlgorithm/operations/Project.h>
+#include <CollisionAlgorithm/operations/ContainsPoint.h>
+#include <CollisionAlgorithm/operations/NeedleOperations.h>
 #include <CollisionAlgorithm/proximity/EdgeProximity.h>
-#include <CollisionAlgorithm/proximity/TetrahedronProximity.h>
 #include <sofa/component/constraint/lagrangian/solver/ConstraintSolverImpl.h>
 #include <sofa/component/statecontainer/MechanicalObject.h>
 
@@ -16,7 +17,7 @@ namespace sofa::collisionalgorithm
 
 class InsertionAlgorithm : public BaseAlgorithm
 {
-    public:
+   public:
     SOFA_CLASS(InsertionAlgorithm, BaseAlgorithm);
 
     typedef core::behavior::MechanicalState<defaulttype::Vec3Types> MechStateTipType;
@@ -215,70 +216,87 @@ class InsertionAlgorithm : public BaseAlgorithm
             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())
+            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
+            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)
             {
+                // Prepare the operations before entering the loop
+                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)
+                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++)
                 {
-                    TetrahedronProximity::SPtr tetProx =
-                        dynamic_pointer_cast<TetrahedronProximity>(volProx);
-                    if (tetProx)
+                    BaseProximity::SPtr shaftProx = createShaftProximity(itShaft->element());
+                    if (!shaftProx) continue;
+
+                    const EdgeProximity::SPtr edgeProx =
+                        dynamic_pointer_cast<EdgeProximity>(shaftProx);
+                    if (!edgeProx) continue;
+
+                    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;
+
+                    // Skip if last CP lies after edge end point
+                    if (dot(shaftEdgeDir, lastCPToP1) < 0_sreal) continue;
+
+                    const int numCPs = floor(lastCPToP1.norm() / tipDistThreshold);
+
+                    for(int idCP = 0 ; idCP < numCPs ; idCP++)
                     {
-                        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)
+                        // 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 ... 
+                        shaftProx = projectOnShaft(candidateCP, itShaft->element()).prox;
+                        if (!shaftProx) continue;
+
+                        // ... then find nearest volume proximity
+                        const BaseProximity::SPtr volProx = findClosestProxOnVol(
+                            shaftProx, l_volGeom.get(), projectOnVol, getFilterFunc());
+                        if (!volProx) continue;
+
+                        // 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 (containsPointInVol(shaftProx->getPosition(), volProx))
                         {
                             volProx->normalize();
                             m_couplingPts.push_back(volProx);
+                            lastCP = volProx->getPosition();
                         }
                     }
                 }
             }
             else  // Don't bother with removing the point that was just added
             {
-                // 2.2. Check whether coupling point should be removed
+                // Remove coupling points that are ahead of the tip in the insertion direction
                 ElementIterator::SPtr itShaft = l_shaftGeom->begin(l_shaftGeom->getSize() - 2);
-                auto createShaftProximity =
-                    Operations::CreateCenterProximity::Operation::get(itShaft->getTypeInfo());
-                const BaseProximity::SPtr shaftProx = createShaftProximity(itShaft->element());
-                if (shaftProx)
-                {
-                    const EdgeProximity::SPtr edgeProx =
-                        dynamic_pointer_cast<EdgeProximity>(shaftProx);
-                    if (edgeProx)
-                    {
-                        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 (dot(tip2Pt, normal) > 0_sreal)
-                        {
-                            m_couplingPts.pop_back();
-                        }
-                    }
-                    else
-                    {
-                        msg_warning() << "shaftGeom: " << l_shaftGeom->getName()
-                                      << " is not an EdgeGeometry. Point removal is disabled";
-                    }
-                }
-                else
-                {
-                    msg_warning() << "Cannot create proximity from shaftGeom: "
-                                  << l_shaftGeom->getName() << " - point removal is disabled";
-                }
+                auto prunePointsAheadOfTip = 
+                    Operations::Needle::PrunePointsAheadOfTip::get(itShaft->getTypeInfo());
+                prunePointsAheadOfTip(m_couplingPts, itShaft->element());
             }
         }
 
@@ -299,8 +317,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();

src/CollisionAlgorithm/operations/ContainsPoint.cpp

@@ -0,0 +1,25 @@
+#include <CollisionAlgorithm/operations/ContainsPoint.h>
+#include <CollisionAlgorithm/proximity/TetrahedronProximity.h>
+#include <CollisionAlgorithm/proximity/TriangleProximity.h>
+
+namespace sofa::collisionalgorithm::Operations::ContainsPointInElement
+{
+
+int register_ContainsPoint_Triangle =
+    Operation::register_func<TriangleElement>(&toolbox::TriangleToolBox::containsPoint);
+
+int register_ContainsPoint_Tetrahedron =
+    Operation::register_func<TetrahedronElement>(&toolbox::TetrahedronToolBox::containsPoint);
+
+}  // namespace sofa::collisionalgorithm::Operations::ContainsPointInElement
+
+namespace sofa::collisionalgorithm::Operations::ContainsPointInProximity
+{
+
+int register_ContainsPointInProximity_Triangle =
+    Operation::register_func<TriangleElement>(&containsPoint<TriangleProximity>);
+
+int register_ContainsPointInProximity_Tetrahedron =
+    Operation::register_func<TetrahedronElement>(&containsPoint<TetrahedronProximity>);
+
+}  // namespace sofa::collisionalgorithm::Operations::ContainsPointInProximity

src/CollisionAlgorithm/operations/ContainsPoint.h

@@ -0,0 +1,74 @@
+#pragma once
+
+#include <CollisionAlgorithm/BaseElement.h>
+#include <CollisionAlgorithm/BaseOperation.h>
+
+namespace sofa::collisionalgorithm::Operations::ContainsPointInElement
+{
+
+typedef bool Result;
+
+class Operation : public GenericOperation<Operation,  // Type of the operation
+                                          Result,     // Default return type
+                                          const type::Vec3&, const BaseElement::SPtr&  // Parameters
+                                          >
+{
+   public:
+    Result defaultFunc(const type::Vec3&, const BaseElement::SPtr&) const override { return false; }
+
+    void notFound(const std::type_info& id) const override
+    {
+        msg_error("ContainsPointInElement")
+            << "The operation ContainsPointInElementOperation is not registered with for type = "
+            << sofa::helper::NameDecoder::decodeFullName(id);
+    }
+};
+
+typedef Operation::FUNC FUNC;
+
+}  // namespace sofa::collisionalgorithm::Operations::ContainsPointInElement
+
+namespace sofa::collisionalgorithm::Operations::ContainsPointInProximity
+{
+
+typedef bool Result;
+
+class Operation
+    : public GenericOperation<Operation,  // Type of the operation
+                              Result,     // Default return type
+                              const type::Vec3&, const BaseProximity::SPtr&  // Parameters
+                              >
+{
+   public:
+    Result defaultFunc(const type::Vec3&, const BaseProximity::SPtr&) const override
+    {
+        return false;
+    }
+
+    void notFound(const std::type_info& id) const override
+    {
+        msg_error("ContainsPointInProximity")
+            << "The operation ContainsPointInProximityOperation is not registered with for type = "
+            << sofa::helper::NameDecoder::decodeFullName(id);
+    }
+};
+
+template <typename PROX>
+Result containsPoint(const type::Vec3& P, const typename std::shared_ptr<PROX>& prox)
+{
+    if (!prox)
+    {
+        msg_warning("ContainsPointInProximity") << "Null proximity pointer in containsPoint"
+            << "Operation is disabled; returning false"; 
+        return false;
+    }
+
+    auto elem = prox->element();
+    auto containsPointInElem =
+        sofa::collisionalgorithm::Operations::ContainsPointInElement::Operation::get(elem);
+    return containsPointInElem(P, elem);
+}
+
+typedef Operation::FUNC FUNC;
+
+}  // namespace sofa::collisionalgorithm::Operations::ContainsPointInProximity

src/CollisionAlgorithm/operations/NeedleOperations.cpp

@@ -0,0 +1,31 @@
+#include <CollisionAlgorithm/operations/NeedleOperations.h>
+
+namespace sofa::collisionalgorithm::Operations::Needle
+{
+
+bool prunePointsUsingEdges(std::vector<BaseProximity::SPtr>& couplingPts,
+                           const EdgeElement::SPtr& edge)
+{
+    if (!edge) 
+    {
+        msg_warning("Needle::PrunePointsAheadOfTip")
+            << "Null element pointer in prunePointsUsingEdges; returning false";
+        return false;
+    }
+    const type::Vec3 edgeBase(edge->getP0()->getPosition());
+    const type::Vec3 tip(edge->getP1()->getPosition());
+
+    const type::Vec3 edgeDirection = tip - edgeBase;
+
+    if (couplingPts.empty()) return true;
+    const type::Vec3 tip2Pt = couplingPts.back()->getPosition() - tip;
+
+    // Positive dot product means the point is ahead of the tip
+    if (dot(tip2Pt, edgeDirection) > 0_sreal) couplingPts.pop_back();
+
+    return true;
+}
+
+int register_PrunePointsAheadOfTip_Edge =
+    PrunePointsAheadOfTip::register_func<EdgeElement>(&prunePointsUsingEdges);
+}  // namespace sofa::collisionalgorithm::Operations::Needle

src/CollisionAlgorithm/operations/NeedleOperations.h

@@ -0,0 +1,34 @@
+#pragma once
+
+#include <CollisionAlgorithm/BaseOperation.h>
+#include <CollisionAlgorithm/BaseProximity.h>
+#include <CollisionAlgorithm/elements/EdgeElement.h>
+
+namespace sofa::collisionalgorithm::Operations::Needle
+{
+
+class PrunePointsAheadOfTip
+    : public GenericOperation<PrunePointsAheadOfTip,  // Type of the operation
+                              bool,                   // Default return type
+                              std::vector<BaseProximity::SPtr>&,
+                              const BaseElement::SPtr&  // Parameters
+                              >
+{
+   public:
+    bool defaultFunc(std::vector<BaseProximity::SPtr>&, const BaseElement::SPtr&) const override
+    {
+        return false;
+    }
+
+    void notFound(const std::type_info& id) const override
+    {
+        msg_error("Needle::PrunePointsAheadOfTip")
+            << "The operation PrunePointsAheadOfTipOperation is not registered with for type = "
+            << sofa::helper::NameDecoder::decodeFullName(id);
+    }
+};
+
+bool prunePointsUsingEdges(std::vector<BaseProximity::SPtr>& couplingPts,
+                           const EdgeElement::SPtr& edgeProx);
+
+}  // namespace sofa::collisionalgorithm::Operations::Needle

src/CollisionAlgorithm/toolbox/TetrahedronToolBox.cpp

@@ -10,6 +10,12 @@ Operations::CreateCenterProximity::Result TetrahedronToolBox::createCenterProxim
     return TetrahedronProximity::create(tetra, 1.0/4.0,1.0/4.0,1.0/4.0,1.0/4.0);
 }
 
+Operations::ContainsPointInElement::Result TetrahedronToolBox::containsPoint(const type::Vec3 & P, const TetrahedronElement::SPtr & tetra) {
+    TetrahedronProximity::SPtr prox = TetrahedronProximity::create(tetra, 1.0/4.0,1.0/4.0,1.0/4.0,1.0/4.0);
+    double f0(prox->f0()), f1(prox->f1()), f2(prox->f2()), f3(prox->f3());
+    return isInTetra(P,tetra->getTetrahedronInfo(),f0,f1,f2,f3);
+}
+
 Operations::Project::Result TetrahedronToolBox::project(const type::Vec3 & P, const TetrahedronElement::SPtr & tetra) {
     double fact[4];
     projectOnTetra(P, tetra->getTetrahedronInfo(),fact[0],fact[1],fact[2],fact[3]);