[operations] A new Operations class to enable point-in-element checks from the top level insertion algorithm (#85)

* [operations] Created a ContainsPoint operation

    * Checks whether a point is inside an element
    * Works with an element or a proximity type as input

* [toolbox] Added a containsPoint function for point-in-tetra operation

* [algorithm] Using containsPoint operations in the insertion algorithm class - no dynamic casts to elements

* [operations] Use SOFA-like error messaging

* [operations] Message warning in case input proximity is null in ContainsPointInProximity operation

Author: th-skam

src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h

@@ -6,6 +6,7 @@
 #include <sofa/collisionAlgorithm/operations/CreateCenterProximity.h>
 #include <sofa/collisionAlgorithm/operations/FindClosestProximity.h>
 #include <sofa/collisionAlgorithm/operations/Project.h>
+#include <sofa/collisionAlgorithm/operations/ContainsPoint.h>
 #include <sofa/collisionAlgorithm/operations/NeedleOperations.h>
 #include <sofa/collisionAlgorithm/proximity/EdgeProximity.h>
 #include <sofa/collisionAlgorithm/proximity/TetrahedronProximity.h>
@@ -225,24 +226,17 @@ 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)
                 {
-                    TetrahedronProximity::SPtr tetProx =
-                        dynamic_pointer_cast<TetrahedronProximity>(volProx);
-                    if (tetProx)
+                    auto containsPointInVol = Operations::ContainsPointInProximity::Operation::get(
+                        l_volGeom->getTypeInfo());
+                    if(containsPointInVol(tipProx->getPosition(), volProx))
                     {
-                        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);
-                        }
+                        volProx->normalize();
+                        m_couplingPts.push_back(volProx);
                     }
                 }
             }

src/sofa/collisionAlgorithm/operations/ContainsPoint.cpp

@@ -0,0 +1,25 @@
+#include <sofa/collisionAlgorithm/operations/ContainsPoint.h>
+#include <sofa/collisionAlgorithm/proximity/TetrahedronProximity.h>
+#include <sofa/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/sofa/collisionAlgorithm/operations/ContainsPoint.h

@@ -0,0 +1,74 @@
+#pragma once
+
+#include <sofa/collisionAlgorithm/BaseElement.h>
+#include <sofa/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/sofa/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]);

src/sofa/collisionAlgorithm/toolbox/TetrahedronToolBox.h

@@ -4,6 +4,7 @@
 #include <sofa/collisionAlgorithm/elements/TetrahedronElement.h>
 #include <sofa/collisionAlgorithm/operations/Project.h>
 #include <sofa/collisionAlgorithm/operations/CreateCenterProximity.h>
+#include <sofa/collisionAlgorithm/operations/ContainsPoint.h>
 
 namespace sofa::collisionAlgorithm::toolbox {
 
@@ -12,6 +13,8 @@ class TetrahedronToolBox {
 
     static Operations::CreateCenterProximity::Result createCenterProximity(const TetrahedronElement::SPtr & tetra);
 
+    static Operations::ContainsPointInElement::Result containsPoint(const type::Vec3 & P, const TetrahedronElement::SPtr & tetra);
+
     static Operations::Project::Result project(const type::Vec3 & P, const TetrahedronElement::SPtr & tetra);
 
     static void projectOnTetra(const type::Vec3d projectP, const TetrahedronElement::TetraInfo & teinfo, double & fact_u, double & fact_v, double & fact_w,double & fact_x);

src/sofa/collisionAlgorithm/toolbox/TriangleToolBox.cpp

@@ -8,6 +8,12 @@ Operations::CreateCenterProximity::Result TriangleToolBox::createCenterProximity
     return TriangleProximity::create(tri, 1.0/3.0,1.0/3.0,1.0/3.0);
 }
 
+Operations::ContainsPointInElement::Result TriangleToolBox::containsPoint(const type::Vec3 & P, const TriangleElement::SPtr & tri) {
+    TriangleProximity::SPtr prox = TriangleProximity::create(tri, 1.0 / 3.0, 1.0 / 3.0, 1.0 / 3.0);
+    double f0(prox->f0()), f1(prox->f1()), f2(prox->f2());
+    return isInTriangle(P,tri->getTriangleInfo(),f0,f1,f1);
+}
+
 //Barycentric coordinates are computed according to
 //http://gamedev.stackexchange.com/questions/23743/whats-the-most-efficient-way-to-find-barycentric-coordinates
 Operations::Project::Result TriangleToolBox::project(const type::Vec3 & P, const TriangleElement::SPtr & tri) {

src/sofa/collisionAlgorithm/toolbox/TriangleToolBox.h

@@ -4,6 +4,7 @@
 #include <sofa/collisionAlgorithm/elements/TriangleElement.h>
 #include <sofa/collisionAlgorithm/operations/Project.h>
 #include <sofa/collisionAlgorithm/operations/CreateCenterProximity.h>
+#include <sofa/collisionAlgorithm/operations/ContainsPoint.h>
 
 namespace sofa::collisionAlgorithm::toolbox {
 
@@ -12,6 +13,8 @@ class TriangleToolBox {
 
     static Operations::CreateCenterProximity::Result createCenterProximity(const TriangleElement::SPtr & tri);
 
+    static Operations::ContainsPointInElement::Result containsPoint(const type::Vec3 & P, const TriangleElement::SPtr & tri);
+
     static Operations::Project::Result project(const type::Vec3 & P, const TriangleElement::SPtr & tri);
 
     static void computeTriangleBaryCoords(const type::Vec3d & proj_P, const TriangleElement::TriangleInfo & tinfo, double & fact_u, double & fact_v, double & fact_w);