Salome HOME
updated copyright message
[tools/medcoupling.git] / src / INTERP_KERNEL / CurveIntersector.txx
1 // Copyright (C) 2007-2023  CEA, EDF
2 //
3 // This library is free software; you can redistribute it and/or
4 // modify it under the terms of the GNU Lesser General Public
5 // License as published by the Free Software Foundation; either
6 // version 2.1 of the License, or (at your option) any later version.
7 //
8 // This library is distributed in the hope that it will be useful,
9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
11 // Lesser General Public License for more details.
12 //
13 // You should have received a copy of the GNU Lesser General Public
14 // License along with this library; if not, write to the Free Software
15 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
16 //
17 // See http://www.salome-platform.org/ or email : webmaster.salome@opencascade.com
18 //
19 // Author : Anthony Geay (CEA/DEN)
20
21 #pragma once
22
23 #include "CurveIntersector.hxx"
24 #include "InterpolationUtils.hxx"
25 #include "PointLocatorAlgos.txx"
26
27 #include <limits>
28
29 namespace INTERP_KERNEL
30 {
31   template<class MyMeshType, class MyMatrix>
32   CurveIntersector<MyMeshType,MyMatrix>
33   ::CurveIntersector(const MyMeshType& meshT, const MyMeshType& meshS,
34                      double precision, double tolerance, double medianLine, int printLevel):
35     _meshT(meshT),
36     _meshS(meshS),
37     _tolerance(tolerance),
38     _precision(precision),
39     _median_line(medianLine),
40     _print_level(printLevel)
41   {
42     if ( SPACEDIM != 1 && SPACEDIM != 2 )
43       throw Exception("CurveIntersector(): space dimension of mesh must be 1 or 2");
44     if ( MESHDIM != 1 )
45       throw Exception("CurveIntersector(): mesh dimension must be 1");
46
47     _connectT = meshT.getConnectivityPtr();
48     _connectS = meshS.getConnectivityPtr();
49     _connIndexT = meshT.getConnectivityIndexPtr();
50     _connIndexS = meshS.getConnectivityIndexPtr();
51     _coordsT = meshT.getCoordinatesPtr();
52     _coordsS = meshS.getCoordinatesPtr();
53   }
54
55   template<class MyMeshType, class MyMatrix>
56   CurveIntersector<MyMeshType,MyMatrix>::~CurveIntersector()
57   {
58   }
59
60   //================================================================================
61   /*!
62     \brief creates the bounding boxes for all the cells of mesh \a mesh
63
64     \param mesh structure pointing to the mesh
65     \param bbox vector containing the bounding boxes
66   */
67   //================================================================================
68
69   template<class MyMeshType, class MyMatrix>
70   void CurveIntersector<MyMeshType,MyMatrix>::createBoundingBoxes (const MyMeshType&    mesh,
71                                                                    std::vector<double>& bbox)
72   {
73     long nbelems = mesh.getNumberOfElements();
74     bbox.resize(2*SPACEDIM* nbelems);
75     const double* coords = mesh.getCoordinatesPtr();
76     const ConnType* conn = mesh.getConnectivityPtr();
77     const ConnType* conn_index = mesh.getConnectivityIndexPtr();  
78     int ibox=0;
79     for(long icell=0; icell<nbelems; icell++)
80       {
81         ConnType nb_nodes_per_elem = conn_index[icell+1]-conn_index[icell];
82         //initializing bounding box limits
83         for(int idim=0; idim<SPACEDIM; idim++)
84           {
85             bbox[2*SPACEDIM*ibox+2*idim]   =  std::numeric_limits<double>::max();
86             bbox[2*SPACEDIM*ibox+2*idim+1] = -std::numeric_limits<double>::max();
87           }
88         //updating the bounding box with each node of the element
89         for (ConnType j=0; j<nb_nodes_per_elem; j++)
90           {
91             const double* coord_node = coords + 
92               SPACEDIM*OTT<ConnType,numPol>
93               ::coo2C(conn[OTT<ConnType,numPol>::conn2C(conn_index[icell]+j)]);
94             for(int idim=0; idim<SPACEDIM; idim++)
95               {
96                 double x = *(coord_node+idim);
97                 bbox[ibox*2*SPACEDIM + 2*idim]   =
98                   ( bbox[ibox*2*SPACEDIM + 2*idim] < x ) ? bbox[ibox*2*SPACEDIM + 2*idim] : x;
99                 bbox[ibox*2*SPACEDIM + 2*idim+1] =
100                   ( bbox[ibox*2*SPACEDIM + 2*idim+1] > x ) ? bbox[ibox*2*SPACEDIM + 2*idim+1] : x;
101               }
102           }
103         ibox++;
104       }
105   }
106
107   /*!
108     Computes the bounding box of a given element. iP in numPol mode.
109   */
110   template<class MyMeshType, class MyMatrix>
111   void CurveIntersector<MyMeshType,MyMatrix>::getElemBB (double*           bb,
112                                                          const MyMeshType& mesh,
113                                                          ConnType          iP,
114                                                          ConnType          nb_nodes)
115   {
116     const double* coords = mesh.getCoordinatesPtr();
117     const ConnType* conn_index = mesh.getConnectivityIndexPtr();
118     const ConnType* conn = mesh.getConnectivityPtr();
119     //initializing bounding box limits
120     for(int idim=0; idim<SPACEDIM; idim++)
121       {
122         bb[2*idim  ] =  std::numeric_limits<double>::max();
123         bb[2*idim+1] = -std::numeric_limits<double>::max();
124       }
125
126     for (ConnType i=0; i<nb_nodes; i++)
127       {
128         //MN: iP= cell index, not node index, use of connectivity array ?
129         const double* coord_node = coords +
130           SPACEDIM*(OTT<ConnType,numPol>::coo2C(conn[OTT<ConnType,numPol>::conn2C(conn_index[OTT<ConnType,numPol>::ind2C(iP)]+i)]));
131         for(int idim=0; idim<SPACEDIM; idim++)
132           {
133             double x = *(coord_node+idim);
134             bb[2*idim  ] = (x<bb[2*idim  ]) ? x : bb[2*idim  ];
135             bb[2*idim+1] = (x>bb[2*idim+1]) ? x : bb[2*idim+1];
136           }
137       }
138   }
139   
140   /*!
141    * \param [in] startOfSeg - input coming from intersectSegments or intersectSegmentsInternal
142    * \param [in] endOfSeg - input coming from intersectSegments or intersectSegmentsInternal. NO Assume about sort 
143    * \param [in] pt - position of point that the method computes the bary coords for.
144    */
145   template<class MyMeshType, class MyMatrix>
146   void CurveIntersector<MyMeshType,MyMatrix>::ComputeBaryCoordsOf(double startOfSeg, double endOfSeg, double pt, double& startPos, double& endPos)
147   {
148     double deno(endOfSeg-startOfSeg);
149     startPos = (endOfSeg-pt)/deno;
150     startPos = std::max(startPos,0.); startPos = std::min(startPos,1.);
151     endPos=1.-startPos; 
152   }
153
154   /*!
155    * @param icellT id in target mesh in format of MyMeshType.
156    * @param coordsT output val that stores coordinates of the target cell
157    * automatically resized to the right length.
158    * @return true if segment is quadratic and in this case coordinates of medium node
159    * are placed in the middle of coordsT
160    */
161   template<class MyMeshType, class MyMatrix>
162   bool CurveIntersector<MyMeshType,MyMatrix>::getRealTargetCoordinates(ConnType icellT, std::vector<double>& coordsT) const
163   {
164     ConnType nbNodesT(_connIndexT[OTT<ConnType,numPol>::ind2C(icellT)+1] - _connIndexT[OTT<ConnType,numPol>::ind2C(icellT)]);
165     coordsT.resize(SPACEDIM*nbNodesT);
166     for (ConnType iT=0; iT<nbNodesT; iT++)
167       {
168         for(int idim=0; idim<SPACEDIM; idim++)
169           {
170             coordsT[SPACEDIM*iT+idim] =
171               _coordsT[SPACEDIM*OTT<ConnType,numPol>::coo2C(_connectT[OTT<ConnType,numPol>::conn2C(_connIndexT[OTT<ConnType,numPol>::ind2C(icellT)]+iT)])+idim];
172           }
173       }
174     if ( nbNodesT > 2 )
175       {
176         for(int idim=0; idim<SPACEDIM; idim++)
177           std::swap( coordsT[SPACEDIM*1+idim], coordsT[SPACEDIM*2+idim]);
178         return true;
179       }
180     return false;
181   }
182   
183   template<class MyMeshType, class MyMatrix>
184   typename MyMeshType::MyConnType CurveIntersector<MyMeshType,MyMatrix>::getNodeIdOfTargetCellAt(ConnType icellT, ConnType nodeIdInCellT) const
185   {
186     ConnType nbNodesT(_connIndexT[OTT<ConnType,numPol>::ind2C(icellT)+1] - _connIndexT[OTT<ConnType,numPol>::ind2C(icellT)]);
187     if(nodeIdInCellT>=0 && nodeIdInCellT<nbNodesT)
188       return OTT<ConnType,numPol>::coo2C(_connectT[OTT<ConnType,numPol>::conn2C(_connIndexT[OTT<ConnType,numPol>::ind2C(icellT)]+nodeIdInCellT)]);
189     else
190       throw Exception("getNodeIdOfTargetCellAt : error in nodeId in cell");
191   }
192
193   /*!
194    * @param icellS id in source mesh in format of MyMeshType.
195    * @param coordsS output val that stores coordinates of the source cell automatically resized to the right length.
196    * @return true if segment is quadratic and in this case coordinates of medium node
197    * are placed in the middle of coordsS
198    */
199   template<class MyMeshType, class MyMatrix>
200   bool CurveIntersector<MyMeshType,MyMatrix>::getRealSourceCoordinates(ConnType icellS, std::vector<double>& coordsS) const
201   {
202     ConnType nbNodesS = _connIndexS[OTT<ConnType,numPol>::ind2C(icellS)+1] - _connIndexS[OTT<ConnType,numPol>::ind2C(icellS)];
203     coordsS.resize(SPACEDIM*nbNodesS);
204     for(ConnType iS=0; iS<nbNodesS; iS++)
205       {
206         for(int idim=0; idim<SPACEDIM; idim++)
207           {
208             coordsS[SPACEDIM*iS+idim] =
209               _coordsS[SPACEDIM*OTT<ConnType,numPol>::coo2C(_connectS[OTT<ConnType,numPol>::conn2C(_connIndexS[OTT<ConnType,numPol>::ind2C(icellS)]+iS)])+idim];
210           }
211       }
212     if ( nbNodesS > 2 )
213       {
214         for(int idim=0; idim<SPACEDIM; idim++)
215           std::swap( coordsS[SPACEDIM*1+idim], coordsS[SPACEDIM*2+idim]);
216         return true;
217       }
218     return false;
219   }
220
221   template<class MyMeshType, class MyMatrix>
222   typename MyMeshType::MyConnType CurveIntersector<MyMeshType,MyMatrix>::getNodeIdOfSourceCellAt(ConnType icellS, ConnType nodeIdInCellS) const
223   {
224     ConnType nbNodesS(_connIndexS[OTT<ConnType,numPol>::ind2C(icellS)+1] - _connIndexS[OTT<ConnType,numPol>::ind2C(icellS)]);
225     if(nodeIdInCellS>=0 && nodeIdInCellS<nbNodesS)
226       return OTT<ConnType,numPol>::coo2C(_connectS[OTT<ConnType,numPol>::conn2C(_connIndexS[OTT<ConnType,numPol>::ind2C(icellS)]+nodeIdInCellS)]);
227     else
228       throw Exception("getNodeIdOfSourceCellAt : error in nodeId in cell");
229   }
230
231   /*!
232    * \brief Return dual segments of given segment
233    *  \param icell - given segment in C mode
234    *  \param mesh - mesh
235    *  \param segments - dual segments
236    */
237   template<class MyMeshType, class MyMatrix>
238   void CurveIntersector<MyMeshType,MyMatrix>::getDualSegments(ConnType                   icell,
239                                                               const MyMeshType&          mesh,
240                                                               std::vector<TDualSegment>& segments)
241   {
242     // get coordinates of cell nodes
243     ConnType nbNodes;
244     std::vector<double>   ncoords;
245     std::vector<ConnType> nodeIds;
246     {
247       const ConnType *connect   = mesh.getConnectivityPtr();
248       const ConnType *connIndex = mesh.getConnectivityIndexPtr();
249       const double *coords      = mesh.getCoordinatesPtr();
250
251       nbNodes = connIndex[icell+1] - connIndex[icell];
252
253       ncoords.resize(SPACEDIM*nbNodes);
254       nodeIds.resize(nbNodes);
255
256       for(ConnType i=0; i<nbNodes; i++)
257         for(int idim=0; idim<SPACEDIM; idim++)
258           {
259             nodeIds[i] = connect[OTT<ConnType,numPol>::conn2C(connIndex[OTT<ConnType,numPol>::ind2C(icell)]+i)];
260             ncoords[SPACEDIM*i+idim] = coords[SPACEDIM*OTT<ConnType,numPol>::coo2C(nodeIds[i])+idim];
261           }
262       if ( nbNodes > 2 ) // quadratic segment, put medium node in the middle
263         {
264           for(int idim=0; idim<SPACEDIM; idim++)
265             std::swap( ncoords[SPACEDIM*1+idim], ncoords[SPACEDIM*2+idim]);
266           std::swap( nodeIds[1], nodeIds[2] );
267         }
268     }
269
270     // fill segments
271     segments.clear();
272     segments.reserve( 2*nbNodes );
273     for(ConnType i=0; i<nbNodes-1; i++)
274       {
275         segments.push_back(TDualSegment());
276         TDualSegment& seg1 = segments.back();
277         segments.push_back(TDualSegment());
278         TDualSegment& seg2 = segments.back();
279
280         seg1._nodeId = nodeIds[i];
281         seg2._nodeId = nodeIds[i+1];
282
283         seg1._coords.resize( SPACEDIM * 2 );
284         seg2._coords.resize( SPACEDIM * 2 );
285
286         for(int idim=0; idim<SPACEDIM; idim++)
287           {
288             double c1 = ncoords[SPACEDIM*i+idim];
289             double c2 = ncoords[SPACEDIM*(i+1)+idim];
290             double m = 0.5 * ( c1 + c2 );
291             seg1._coords[ idim ] = c1;
292             seg1._coords[ SPACEDIM + idim ] = m;
293             seg2._coords[ idim ] = m;
294             seg2._coords[ SPACEDIM + idim ] = c2;
295           }
296       }
297   }
298   
299   template<class MyMeshType, class MyMatrix>
300   bool CurveIntersector<MyMeshType,MyMatrix>::projectionThis(const double *coordsT, const double *coordsS, double& xs0, double& xs1, double& xt) const
301   {    
302     enum { X=0, Y };
303     switch(SPACEDIM)
304       {
305       case 1:
306         {
307           xt  = coordsT[0];
308           xs0 = coordsS[0]; xs1 = coordsS[1];
309           return true;
310         }
311       case 2:
312         {
313           const double *s0(coordsS),*s1(coordsS + 2);
314           double s01[2] = { s1[X]-s0[X], s1[Y]-s0[Y] }; // src segment direction
315           double sSize = sqrt( s01[X]*s01[X] + s01[Y]*s01[Y] ); // src segment size
316           if( sSize < this->_precision )
317             return false;
318           s01[X] /= sSize; s01[Y] /= sSize; // normalize s01
319           double t[2] = { coordsT[X]-s0[X], coordsT[Y]-s0[Y] };
320           xs0 = 0. ; xs1 = sSize; xt = s01[X]*t[X] + s01[Y]*t[Y];
321           double proj_t_on_s[2] = { s0[X]+xt*s01[X], s0[Y]+xt*s01[Y] };
322           double dist_t_s_vect[2] = { coordsT[X]-proj_t_on_s[X], coordsT[Y]-proj_t_on_s[Y] };
323           double dist_t_s = sqrt( dist_t_s_vect[X]*dist_t_s_vect[X]+dist_t_s_vect[Y]*dist_t_s_vect[Y] );
324           return dist_t_s < this->_precision;
325         }
326       default:
327         throw Exception("CurveIntersector::projectionThis : space dimension of mesh must be 1 or 2");
328       }
329   }
330
331   template<class MyMeshType, class MyMatrix>
332   bool CurveIntersector<MyMeshType,MyMatrix>::projectionThis(const double *coordsT, const double *coordsS,
333                                                              double& xs0, double& xs1, double& xt0, double& xt1) const
334   {
335     xt0 = coordsT[0]; xt1 = coordsT[1];
336     xs0 = coordsS[0]; xs1 = coordsS[1];
337     if ( SPACEDIM == 2 )
338       {
339         // Pass 2D->1D
340
341         enum { X=0, Y };
342
343         // check if two segments overlap in 2D within tolerance
344
345         const double* t0 = coordsT;
346         const double* t1 = coordsT + 2;
347         double t01[2] = { t1[X]-t0[X], t1[Y]-t0[Y] }; // tgt segment direction
348         double tSize = sqrt( t01[X]*t01[X] + t01[Y]*t01[Y] ); // tgt segment size
349         if ( tSize < _precision )
350           return false; // degenerated segment
351         t01[X] /= tSize, t01[Y] /= tSize; // normalize t01
352
353         const double* s0 = coordsS;
354         const double* s1 = coordsS + 2;
355         double t0s0[2] = { s0[X]-t0[X], s0[Y]-t0[Y] };
356         double t0s1[2] = { s1[X]-t0[X], s1[Y]-t0[Y] };
357         double nt01_x_t0s0 = t0s0[X] * t01[Y] - t0s0[Y] * t01[X]; // t0s0 dot norm of t01
358         double nt01_x_t0s1 = t0s1[X] * t01[Y] - t0s1[Y] * t01[X]; // t0s1 dot norm of t01
359         double dist_ts0 = fabs( nt01_x_t0s0 ); // dist from tgt seg to s0
360         double dist_ts1 = fabs( nt01_x_t0s1 ); // dist from tgt seg to s1
361         bool s0_out_of_tol = ( dist_ts0 > _tolerance );
362         bool s1_out_of_tol = ( dist_ts1 > _tolerance );
363         if ( nt01_x_t0s0 * nt01_x_t0s1 > 0 && ( s0_out_of_tol || s1_out_of_tol ))
364           return false; // tgt segment is to far from src segment
365
366         double S0[2] = { s0[X], s0[Y] };
367         double S1[2] = { s1[X], s1[Y] };
368         if ( s0_out_of_tol ) // put s0 within tolerance
369           {
370             double t = _tolerance * nt01_x_t0s0 / dist_ts0; // signed tolerance
371             double r = ( nt01_x_t0s0 - t ) / ( nt01_x_t0s0 - nt01_x_t0s1 );
372             S0[X] = s0[X] * ( 1.-r ) + s1[X] * r;
373             S0[Y] = s0[Y] * ( 1.-r ) + s1[Y] * r;
374           }
375         if ( s1_out_of_tol ) // put s1 within tolerance
376           {
377             double t = _tolerance * nt01_x_t0s1 / dist_ts1; // signed tolerance
378             double r = ( nt01_x_t0s1 - t ) / ( nt01_x_t0s1 - nt01_x_t0s0 );
379             S1[X] = s1[X] * ( 1.-r ) + s0[X] * r;
380             S1[Y] = s1[Y] * ( 1.-r ) + s0[Y] * r;
381           }
382
383         // project tgt and src segments to median line
384
385         double s01[2] = { S1[X]-S0[X], S1[Y]-S0[Y] }; // src segment direction
386         double sSize = sqrt( s01[X]*s01[X] + s01[Y]*s01[Y] ); // src segment size
387         if ( sSize < _precision )
388           return false; // degenerated segment
389         s01[X] /= sSize, s01[Y] /= sSize; // normalize s01
390
391         // make t01 and s01 codirected
392         double t01_x_s01 = t01[X] * s01[X] + t01[Y] * s01[Y]; // t01 dot s01
393         if ( t01_x_s01 < 0 )
394           s01[X] = -s01[X], s01[Y] = -s01[Y];
395
396         double medianDir[2] = {
397           t01[X] * ( 1.-_median_line) + s01[X] * _median_line, 
398           t01[Y] * ( 1.-_median_line) + s01[Y] * _median_line
399         };
400         double medianSize = sqrt( medianDir[X]*medianDir[X] + medianDir[Y]*medianDir[Y] );
401         if ( medianSize < std::numeric_limits<double>::min() )
402           return false; // strange...
403         medianDir[X] /= medianSize, medianDir[Y] /= medianSize;
404
405         xt0 = t0[X] * medianDir[X] + t0[Y] * medianDir[Y];
406         xt1 = t1[X] * medianDir[X] + t1[Y] * medianDir[Y];
407         xs0 = S0[X] * medianDir[X] + S0[Y] * medianDir[Y];
408         xs1 = S1[X] * medianDir[X] + S1[Y] * medianDir[Y];
409
410       } // if ( SPACEDIM == 2 )
411     return true;
412   }
413   
414   /*!
415    * \brief Return length of intersection of two segments
416    */
417   template<class MyMeshType, class MyMatrix>
418   double CurveIntersector<MyMeshType,MyMatrix>::intersectSegmentsInternal(const double *coordsT, const double *coordsS, double& xs0, double& xs1, double& xt0, double& xt1) const
419   {
420     if(!projectionThis(coordsT,coordsS,xs0,xs1,xt0,xt1))
421       return 0.;
422     
423     if ( xt0 > xt1 ) std::swap( xt0, xt1 );
424     if ( xs0 > xs1 ) std::swap( xs0, xs1 );
425
426     double x0 = std::max( xt0, xs0 );
427     double x1 = std::min( xt1, xs1 );
428     return ( x0 < x1 ) ? ( x1 - x0 ) : 0.;
429   }
430
431   template<class MyMeshType>
432   class DummyMyMeshType1D
433   {
434   public:
435     static const int MY_SPACEDIM=1;
436     static const int MY_MESHDIM=8;
437     typedef mcIdType MyConnType;
438     static const INTERP_KERNEL::NumberingPolicy My_numPol=MyMeshType::My_numPol;
439     // begin
440     // useless, but for windows compilation ...
441     const double *getCoordinatesPtr() const { return nullptr; }
442     const MyConnType *getConnectivityPtr() const { return nullptr; }
443     const MyConnType *getConnectivityIndexPtr() const { return nullptr; }
444     INTERP_KERNEL::NormalizedCellType getTypeOfElement(MyConnType) const { return (INTERP_KERNEL::NormalizedCellType)0; }
445     // end
446   };
447
448   /*!
449    * This method determines if a target point ( \a coordsT ) is in source seg2 contained in \a coordsS. To do so _precision attribute is used.
450    * If target point is in, \a xs0, \a xs1 and \a xt are set to 1D referential for a further barycentric computation.
451    * This method deals with SPACEDIM == 2 (see projectionThis).
452    */
453   template<class MyMeshType, class MyMatrix>
454   bool CurveIntersector<MyMeshType,MyMatrix>::isPtIncludedInSeg(const double *coordsT, const double *coordsS, double& xs0, double& xs1, double& xt) const
455   {
456     if(!projectionThis(coordsT,coordsS,xs0,xs1,xt))
457       return false;
458     constexpr ConnType TAB[2]={0,1};
459     const double coordsS_1D[2]={xs0,xs1};
460     const double *coordsT_1D(&xt);
461     return PointLocatorAlgos<DummyMyMeshType1D<MyMeshType>>::isElementContainsPoint(coordsT_1D,NORM_SEG2,coordsS_1D,TAB,2,this->_precision);
462   }
463   
464   /*!
465    * \brief Return length of intersection of two segments
466    */
467   template<class MyMeshType, class MyMatrix>
468   double CurveIntersector<MyMeshType,MyMatrix>::intersectSegments(const double *coordsT, const double *coordsS) const
469   {
470     double xs0,xs1,xt0,xt1;
471     return intersectSegmentsInternal(coordsT,coordsS,xs0,xs1,xt0,xt1);
472   }
473 }