Merge branch 'master' into add-checks

Author: th-skam

src/sofa/collisionAlgorithm/CollisionPipeline.cpp

@@ -1,9 +1,13 @@
 #include <sofa/collisionAlgorithm/CollisionPipeline.h>
 #include <sofa/core/ObjectFactory.h>
 
-namespace sofa::collisionAlgorithm {
-
-int CollisionLoopClass = core::RegisterObject("CollisionLoop")
-.add< CollisionLoop >();
-
+namespace sofa::collisionAlgorithm
+{
+void registerCollisionLoop(sofa::core::ObjectFactory* factory)
+{
+    factory->registerObjects(
+        sofa::core::ObjectRegistrationData(
+            "A collision pipeline customized for proximity detection during needle insertion")
+            .add<CollisionLoop>());
 }
+}  // namespace sofa::collisionAlgorithm

src/sofa/collisionAlgorithm/algorithm/Find2DClosestProximityAlgorithm.cpp

@@ -1,13 +1,13 @@
 #include <sofa/collisionAlgorithm/algorithm/Find2DClosestProximityAlgorithm.h>
 #include <sofa/core/ObjectFactory.h>
 
-namespace sofa::collisionAlgorithm {
-
-int Find2DClosestProximityAlgorithmClass = core::RegisterObject("Find2DClosestProximityAlgorithm")
-.add< Find2DClosestProximityAlgorithm >();
-
-
+namespace sofa::collisionAlgorithm
+{
+void registerFind2DClosestProximityAlgorithm(sofa::core::ObjectFactory* factory)
+{
+    factory->registerObjects(
+        sofa::core::ObjectRegistrationData(
+            "An algorithm to find the closest proximity between two BaseGeometry types in 2D")
+            .add<Find2DClosestProximityAlgorithm>());
 }
-
-
-
+}  // namespace sofa::collisionAlgorithm

src/sofa/collisionAlgorithm/algorithm/FindClosestProximityAlgorithm.cpp

@@ -1,13 +1,13 @@
 #include <sofa/collisionAlgorithm/algorithm/FindClosestProximityAlgorithm.h>
 #include <sofa/core/ObjectFactory.h>
 
-namespace sofa::collisionAlgorithm {
-
-int FindClosestProximityAlgorithmClass = core::RegisterObject("FindClosestProximityAlgorithm")
-.add< FindClosestProximityAlgorithm >();
-
-
+namespace sofa::collisionAlgorithm
+{
+void registerFindClosestProximityAlgorithm(sofa::core::ObjectFactory* factory)
+{
+    factory->registerObjects(
+        sofa::core::ObjectRegistrationData(
+            "An algorithm to find the closest proximity between two BaseGeometry types")
+            .add<FindClosestProximityAlgorithm>());
 }
-
-
-
+}  // namespace sofa::collisionAlgorithm

src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.cpp

@@ -1,13 +1,12 @@
 #include <sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h>
 #include <sofa/core/ObjectFactory.h>
 
-namespace sofa::collisionAlgorithm {
-
-int InsertionAlgorithmClass = core::RegisterObject("InsertionAlgorithm")
-.add< InsertionAlgorithm >();
-
-
+namespace sofa::collisionAlgorithm
+{
+void registerInsertionAlgorithm(sofa::core::ObjectFactory* factory)
+{
+    factory->registerObjects(sofa::core::ObjectRegistrationData(
+                                 "A class implementing a customized needle insertion algorithm")
+                                 .add<InsertionAlgorithm>());
 }
-
-
-
+}  // namespace sofa::collisionAlgorithm

src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h

@@ -7,6 +7,7 @@
 #include <sofa/collisionAlgorithm/operations/FindClosestProximity.h>
 #include <sofa/collisionAlgorithm/operations/Project.h>
 #include <sofa/collisionAlgorithm/proximity/EdgeProximity.h>
+#include <sofa/collisionAlgorithm/proximity/TetrahedronProximity.h>
 #include <sofa/component/constraint/lagrangian/solver/ConstraintSolverImpl.h>
 #include <sofa/component/statecontainer/MechanicalObject.h>
 
@@ -15,7 +16,7 @@ namespace sofa::collisionAlgorithm
 
 class InsertionAlgorithm : public BaseAlgorithm
 {
-   public:
+    public:
     SOFA_CLASS(InsertionAlgorithm, BaseAlgorithm);
 
     typedef core::behavior::MechanicalState<defaulttype::Vec3Types> MechStateTipType;
@@ -223,10 +224,25 @@ class InsertionAlgorithm : public BaseAlgorithm
                 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)
                 {
-                    volProx->normalize();
-                    m_couplingPts.push_back(volProx);
+                    TetrahedronProximity::SPtr tetProx =
+                        dynamic_pointer_cast<TetrahedronProximity>(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
@@ -245,6 +261,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 (dot(tip2Pt, normal) > 0_sreal)
                         {
                             m_couplingPts.pop_back();

src/sofa/collisionAlgorithm/broadphase/AABBBroadPhase.cpp

@@ -1,13 +1,18 @@
 #include <sofa/collisionAlgorithm/broadphase/AABBBroadPhase.h>
 #include <sofa/core/ObjectFactory.h>
 
-namespace sofa::collisionAlgorithm {
-
-int AABBBroadPhaseClass = core::RegisterObject(R"(
-**AABBBroadPhase** is a real-time collision detection component that uses the Axis-Aligned Bounding Box (AABB) approach for broad-phase collision handling.
-
-It organizes elements into a 3D spatial grid, efficiently indexing and retrieving them to identify potential collisions, optimizing the process of narrowing down collision checks for more precise detection in 3D simulations.
-)")
-.add< AABBBroadPhase >();
+namespace sofa::collisionAlgorithm
+{
+void registerAABBBroadPhase(sofa::core::ObjectFactory* factory)
+{
+    factory->registerObjects(sofa::core::ObjectRegistrationData(R"(
+            **AABBBroadPhase** is a real-time collision detection component that uses the 
+            Axis-Aligned Bounding Box (AABB) approach for broad-phase collision handling.
 
+            It organizes elements into a 3D spatial grid, efficiently indexing and retrieving them 
+            to identify potential collisions, optimizing the process of narrowing down collision 
+            checks for more precise detection in 3D simulations.
+        )")
+                                 .add<AABBBroadPhase>());
 }
+}  // namespace sofa::collisionAlgorithm

src/sofa/collisionAlgorithm/broadphase/FullAABBBroadPhase.cpp

@@ -1,9 +1,10 @@
 #include <sofa/collisionAlgorithm/broadphase/FullAABBBroadPhase.h>
 #include <sofa/core/ObjectFactory.h>
 
-namespace sofa::collisionAlgorithm {
-
-int FullAABBBroadPhaseClass = core::RegisterObject("FullAABBBroadPhase")
-.add< FullAABBBroadPhase >();
-
+namespace sofa::collisionAlgorithm
+{
+void registerFullAABBBroadPhase(sofa::core::ObjectFactory* factory)
+{
+    factory->registerObjects(sofa::core::ObjectRegistrationData("").add<FullAABBBroadPhase>());
 }
+}  // namespace sofa::collisionAlgorithm

src/sofa/collisionAlgorithm/filters/DistanceFilter.cpp

@@ -1,9 +1,13 @@
 #include <sofa/collisionAlgorithm/filters/DistanceFilter.h>
 #include <sofa/core/ObjectFactory.h>
 
-namespace sofa::collisionAlgorithm {
-
-int DistanceFilterClass = core::RegisterObject("DistanceFilter")
-.add< DistanceFilter >();
-
+namespace sofa::collisionAlgorithm
+{
+void registerDistanceFilter(sofa::core::ObjectFactory* factory)
+{
+    factory->registerObjects(
+        sofa::core::ObjectRegistrationData(
+            "This class filters detected proximities based on their distance from source")
+            .add<DistanceFilter>());
 }
+}  // namespace sofa::collisionAlgorithm

src/sofa/collisionAlgorithm/geometry/EdgeGeometry.cpp

@@ -1,13 +1,13 @@
 #include <sofa/collisionAlgorithm/geometry/EdgeGeometry.h>
 #include <sofa/core/ObjectFactory.h>
 
-namespace sofa {
-
-namespace collisionAlgorithm {
-
-int EdgeGeometryClass = core::RegisterObject("EdgeGeometry")
-.add< EdgeGeometry<sofa::defaulttype::Vec3dTypes> >();
-
-}
-
+namespace sofa::collisionAlgorithm
+{
+void registerEdgeGeometry(sofa::core::ObjectFactory* factory)
+{
+    factory->registerObjects(
+        sofa::core::ObjectRegistrationData(
+            "A class bridging edge topological information with the proximity detection algorithm")
+            .add<EdgeGeometry<sofa::defaulttype::Vec3dTypes> >());
 }
+}  // namespace sofa::collisionAlgorithm

src/sofa/collisionAlgorithm/geometry/PointGeometry.cpp

@@ -1,15 +1,13 @@
 #include <sofa/collisionAlgorithm/geometry/PointGeometry.h>
 #include <sofa/core/ObjectFactory.h>
 
-namespace sofa {
-
-namespace collisionAlgorithm {
-
-int PointGeometryClass = core::RegisterObject("PointGeometry")
-.add< PointGeometry<sofa::defaulttype::Vec3dTypes> >();
-
+namespace sofa::collisionAlgorithm
+{
+void registerPointGeometry(sofa::core::ObjectFactory* factory)
+{
+    factory->registerObjects(
+        sofa::core::ObjectRegistrationData(
+            "A class bridging point topological information with the proximity detection algorithm")
+            .add<PointGeometry<sofa::defaulttype::Vec3dTypes> >());
 }
-
-}
-
-
+}  // namespace sofa::collisionAlgorithm