[SofaBaseTopology] add intersection methods

SofaKernel/modules/SofaBaseTopology/src/SofaBaseTopology/EdgeSetGeometryAlgorithms.h

@@ -157,6 +157,11 @@ class EdgeSetGeometryAlgorithms : public PointSetGeometryAlgorithms<DataTypes>
     /** return a pointer to the container of cubature points */
     NumericalIntegrationDescriptor<Real,1> &getEdgeNumericalIntegrationDescriptor();
 
+    bool computeEdgeSegmentIntersection(EdgeID edgeID,
+        const sofa::defaulttype::Vec<3,Real>& a,
+        const sofa::defaulttype::Vec<3, Real>& b,
+        Real &baryCoef);
+
 protected:
     Data<bool> showEdgeIndices; ///< Debug : view Edge indices.
     Data<bool>  d_drawEdges; ///< if true, draw the edges in the topology.

SofaKernel/modules/SofaBaseTopology/src/SofaBaseTopology/EdgeSetGeometryAlgorithms.inl

@@ -899,5 +899,50 @@ void EdgeSetGeometryAlgorithms<DataTypes>::initPointAdded(PointID index, const c
     }
 }
 
+template<class DataTypes>
+bool EdgeSetGeometryAlgorithms<DataTypes>::computeEdgeSegmentIntersection(EdgeID edgeID,
+    const sofa::defaulttype::Vec<3, Real>& a,
+    const sofa::defaulttype::Vec<3, Real>& b,
+    Real &baryCoef)
+{
+    const double EPS = 1e-05;
+    bool is_intersect = false;
+
+    const Edge& e = this->m_topology->getEdge(edgeID);
+    const VecCoord& p = (this->object->read(core::ConstVecCoordId::position())->getValue());
+    const typename DataTypes::Coord& c0 = p[e[0]];
+    const typename DataTypes::Coord& c1 = p[e[1]];
+
+    sofa::defaulttype::Vec<3, Real> p0{ c0[0],c0[1],c0[2] };
+    sofa::defaulttype::Vec<3, Real> p1{ c1[0],c1[1],c1[2] };
+    sofa::defaulttype::Vec<3, Real> pa{ a[0],a[1],a[2] };
+    sofa::defaulttype::Vec<3, Real> pb{ b[0],b[1],b[2] };
+
+    sofa::defaulttype::Vec<3, Real> v_0a = p0 - pa;
+    sofa::defaulttype::Vec<3, Real> v_ba = pb - pa;
+    sofa::defaulttype::Vec<3, Real> v_10 = p1 - p0;
+
+    Real d0aba, dba10, d0a10, dbaba, d1010;
+
+    d0aba = v_0a * v_ba;
+    dba10 = v_ba * v_ba;
+    d0a10 = v_0a * v_10;
+    dbaba = v_ba * v_ba;
+    d1010 = v_10 * v_10;
+
+    Real deno, num;
+    deno = d1010 * dbaba - dba10 * dba10;
+    if (!abs(deno) <= EPS)
+    {
+        num = d0aba * dba10 - d0a10 * dbaba;
+
+        //baryCoef= (d0aba + dba10 * (num / deno)) / dbaba;
+        baryCoef = num / deno;
+        //if (baryCoef >= 0.0 && baryCoef <= 1.0)
+            is_intersect = true;
+    }
+    return is_intersect;
+}
+
 
 } //namespace sofa::component::topology

SofaKernel/modules/SofaBaseTopology/src/SofaBaseTopology/TriangleSetGeometryAlgorithms.h

@@ -121,6 +121,11 @@ class TriangleSetGeometryAlgorithms : public EdgeSetGeometryAlgorithms<DataTypes
     */
     sofa::helper::vector< double > computeTriangleBarycoefs(const TriangleID ind_t, const sofa::defaulttype::Vec<3,double> &p) const;
 
+    /** \brief Computes barycentric coefficients of point p in initial triangle (a,b,c) indexed by ind_t
+    *
+    */
+    sofa::helper::vector< double > computeRestTriangleBarycoefs(const TriangleID ind_t, const sofa::defaulttype::Vec<3, double>& p) const;
+
     /** \brief Computes barycentric coefficients of point p in triangle whose vertices are indexed by (ind_p1,ind_p2,ind_p3)
      *
      */
@@ -231,6 +236,22 @@ class TriangleSetGeometryAlgorithms : public EdgeSetGeometryAlgorithms<DataTypes
             sofa::helper::vector<PointID> &indices,
             double &baryCoef, double& coord_kmin) const;
 
+    /** \brief Computes the intersections of the vector from point a to point b and the triangle indexed by t
+    *
+    * @param a : first input point
+    * @param b : last input point
+    * @param ind_t : triangle indice
+    * @param indices : indices of edges (belonging to ind_t) crossed by the vecteur AB
+    * @param baryCoef : barycoef of intersections points on the edge
+    */
+    bool computeIntersectionsLineTriangle(bool is_entered,
+        const sofa::defaulttype::Vec<3, Real>& a,
+        const sofa::defaulttype::Vec<3, Real>& b,
+        const TriangleID ind_t,
+        sofa::helper::vector<PointID>& indices,
+        sofa::helper::vector<Real>& vecBaryCoef,
+        sofa::helper::vector<Real>& vecCoordKmin) const;
+
     /** \brief Computes the list of points (ind_edge,coord) intersected by the segment from point a to point b and the triangular mesh
      *
      */