[operations] Created a ContainsPoint operation

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

Author: th-skam

src/sofa/collisionAlgorithm/operations/ContainsPoint.cpp

@@ -0,0 +1,19 @@
+#include <sofa/collisionAlgorithm/operations/ContainsPoint.h>
+#include <sofa/collisionAlgorithm/proximity/TriangleProximity.h>
+#include <sofa/collisionAlgorithm/proximity/TetrahedronProximity.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::ContainsPointInProximity {
+
+int register_ContainsPointInProximity_Triangle = Operation::register_func<TriangleElement>(&containsPoint<TriangleProximity>);
+
+int register_ContainsPointInProximity_Tetrahedron = Operation::register_func<TetrahedronElement>(&containsPoint<TetrahedronProximity>);
+
+}

src/sofa/collisionAlgorithm/operations/ContainsPoint.h

@@ -0,0 +1,60 @@
+#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 {
+        std::cerr << "ERROR the operation ContainsPointOperation is not registered with for type = " << sofa::helper::NameDecoder::decodeFullName(id) << std::endl;
+    }
+
+};
+
+typedef Operation::FUNC FUNC;
+
+}
+
+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 {
+        std::cerr << "ERROR the operation ContainsPointProximityOperation is not registered with for type = " << sofa::helper::NameDecoder::decodeFullName(id) << std::endl;
+    }
+
+};
+
+template<typename PROX>
+Result containsPoint(const type::Vec3 & P, const typename std::shared_ptr<PROX> & prox)
+{
+    auto elem = prox->element();
+    auto containsPointInElem = sofa::collisionAlgorithm::Operations::ContainsPointInElement::Operation::get(elem);
+    return containsPointInElem(P, elem);
+}
+
+typedef Operation::FUNC FUNC;
+
+}

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

@@ -12,6 +12,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

@@ -12,6 +12,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);