src/INTERP_KERNEL/PolygonAlgorithms.txx

   1 //  Copyright (C) 2007-2008  CEA/DEN, EDF R&D
   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.
   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 #ifndef __POLYGONALGORITHMS_TXX__
  20 #define __POLYGONALGORITHMS_TXX__
  21
  22 #include "PolygonAlgorithms.hxx"
  23 #include "InterpolationUtils.hxx"
  24 #include <list>
  25 #include <map>
  26 #include <iostream>
  27
  28 namespace INTERP_KERNEL
  29 {
  30   template<int DIM>
  31   PolygonAlgorithms<DIM>::PolygonAlgorithms(double epsilon, double precision)//: (0)
  32   {
  33     _is_in_intersection = false;
  34     _epsilon = epsilon;
  35     _precision = precision;
  36   }
  37   /*************************************************************/
  38   /* Computes the 3D intersection between two COPLANAR         */
  39   /* Segments [A,B] and [C,D], stores the result in V.         */
  40   /* If A belongs to [CD] then the vertex E (preceeding A)     */
  41   /* is used to decide if the crossing is real. If A coincides */
  42   /* with C or D, a special treatment is performed             */
  43   /*************************************************************/
  44   template<int DIM>
  45   bool PolygonAlgorithms<DIM>::intersectSegmentSegment(const double * A,  const double * B, const double * C,
  46                                                        const double * D, const double * E, double * V)
  47   {
  48     double AB[DIM], DC[DIM], AC[DIM], det, t1, t2, inv_det;
  49
  50     /******* Initialisation of the linear system  t1*AB+t2*DC=AC ***********/
  51     for(int idim=0;idim<DIM;idim++)
  52       {
  53         AB[idim] = B[idim]-A[idim];//B-A
  54         DC[idim] = C[idim]-D[idim];//C-D
  55         AC[idim] = C[idim]-A[idim];//C-A
  56       }
  57
  58     /******* Resolution of the linear system  t1*AB+t2*DC=AC ***********/
  59     det = determinant(AB,DC);//determinant of the first two coordinates
  60     if(fabs(det) >_epsilon)
  61       {
  62         inv_det = 1/det;
  63         t1 = determinant(AC,DC)*inv_det;//solves the linear system t1*AB+t2*DC=AC
  64         t2 = determinant(AB,AC)*inv_det;//solves the linear system t1*AB+t2*DC=AC
  65       }
  66     else
  67       {
  68         switch(DIM)
  69           {
  70           case 2:
  71             {
  72               if(distance2<DIM>(A,D)<_epsilon)
  73                 crossprod<DIM>(A,C,E,_vdouble);//store the crossprod between vectors AC and AE (E=vertex preceding A)
  74               return false;//case of paralell segments
  75             }
  76           case 3://beware AB and CD may belong to a vertical plane
  77             det = determinant(&AB[1],&DC[1]);//determinant of the last two coefficients
  78             if(fabs(det) > _epsilon)
  79               {
  80                 inv_det = 1/det;
  81                 t1=(AC[1]*DC[2]-AC[2]*DC[1])*inv_det;
  82                 t2=(AB[1]*AC[2]-AB[2]*AC[1])*inv_det;
  83               }
  84             else //beware AB and CD may belong to a plane y = constant
  85               {
  86                 det = AB[0]*DC[2]-AB[2]*DC[0];
  87                 if(fabs(det) > _epsilon)
  88                   {
  89                     inv_det = 1/det;
  90                     t1=(AC[0]*DC[2]-AC[2]*DC[0])*inv_det;
  91                     t2=(AB[0]*AC[2]-AB[2]*AC[0])*inv_det;
  92                   }
  93                 else
  94                   {
  95                     if(distance2<DIM>(A,D)<_epsilon)
  96                       crossprod<DIM>(A,C,E,_vdouble);//store the crossprod between vectors AC and AE (E=vertex preceding A)
  97                     return false;//case of paralell segments
  98                   }
  99               }
 100           }
 101       }
 102
 103     if(t1>_precision && t1<1-_precision)
 104       {
 105         if( t2>_precision && t2<1-_precision)
 106           {
 107             for(int idim=0;idim<DIM;idim++) V[idim]=t1*AB[idim] + A[idim];
 108             return true;
 109           }
 110       }
 111     else if(fabs(t1) <= _precision)
 112       {
 113         if( t2>_precision && t2<1-_precision)//vertex on an edge
 114           {
 115             double V12[DIM];
 116             double V34[DIM];
 117             crossprod<DIM>(A,D,B,V12);
 118             crossprod<DIM>(A,D,E,V34);
 119             double same_side =dotprod<DIM>(V12, V34);
 120             if( same_side < -_epsilon ) // <= epsilon or 0 ?//crossing
 121               {
 122                 for(int idim=0;idim<DIM;idim++) V[idim]=A[idim];
 123                 return true;
 124               }
 125             else if( same_side > _epsilon ) _terminus= !_is_in_intersection;//reflexion
 126             else //separation of overlaping edges
 127               {
 128                 if(_Inter.empty() ) _terminus=true;
 129                 else if(!_is_in_intersection)
 130                   {
 131                     for(int idim=0;idim<DIM;idim++) V[idim]=A[idim];
 132                     return true;
 133                   }
 134               }
 135           }
 136         else if(fabs(t2-1) <= _precision)//vertex on a vertex (A=D), first run
 137           crossprod<DIM>(A,C,E,_vdouble);//store the angle between vectors AC and AE (E=vertex preceding A)
 138         else if(fabs(t2) <= _precision)//vertex on a vertex (A=C), second run
 139           {
 140             double Vdoublebis[DIM];
 141             //crossprod<DIM>(A,C,E,_vdouble);
 142             crossprod<DIM>(A,B,D,Vdoublebis);
 143             double in_between =dotprod<DIM>(Vdoublebis,_vdouble);
 144             if(in_between>_epsilon)//crossing
 145               {
 146                 for(int idim=0;idim<DIM;idim++) V[idim]=A[idim];
 147                 return true;
 148               }
 149             else if(fabs(in_between)<=_epsilon && dotprod<DIM>(Vdoublebis,Vdoublebis) > _epsilon)
 150               //ie _vdouble=0, separation of overlaping edges at a double point
 151               {
 152                 //crossprod<DIM>(A,E,B,_vdouble);
 153                 if(dotprod<DIM>(_vdouble,Vdoublebis) >=_epsilon )//crossing
 154                   {
 155                     if(_Inter.empty()) _terminus=true;
 156                     else if(!_is_in_intersection)
 157                       {
 158                         for(int idim=0;idim<DIM;idim++) V[idim]=A[idim];
 159                         return true;
 160                       }
 161                   }
 162               }
 163           }
 164       }
 165     return false;
 166   }
 167
 168   /*************************************************************/
 169   /* adds vertex i  to the list inter and updates _End_segments */
 170   /* i is the local index of the current vertex                */
 171   /*************************************************************/
 172   template<int DIM>
 173   inline void PolygonAlgorithms<DIM>::addNewVertex( int i, int i_glob, int i_next_glob, int i_prev_glob,
 174                                                     const double * P)
 175   {
 176     /* Question:Should we add vertex i to the front or back ? */
 177     if( _End_segments[1].second == i_glob)
 178       {
 179         for(int idim=0;idim<DIM;idim++) _Inter.push_back(P[DIM*i+idim]);
 180         _End_segments[1] = std::make_pair(i_glob, i_next_glob);
 181       }
 182     else
 183       {
 184         for(int idim=DIM-1;idim>-1;idim--) _Inter.push_front(P[DIM*i+idim]);
 185         _End_segments[0] = std::make_pair(i_glob, i_next_glob);
 186       }
 187   }
 188
 189   /************************************************************/
 190   /* adds a crossing between two segments starting at i and j */
 191   /* to the double ended list inter in the correct order      */
 192   /* according to endsegments, updates _End_segments           */
 193   /************************************************************/
 194   template<int DIM>
 195   inline void PolygonAlgorithms<DIM>::addCrossing( double * ABCD, std::pair< int,int > i_i_next,
 196                                                    std::pair< int,int > j_j_next)
 197   {
 198     if(!_Inter.empty() )
 199       {
 200         if(_End_segments[0] ==i_i_next)
 201           {
 202             for(int idim=DIM-1;idim>=0;idim--) _Inter.push_front(ABCD[idim]);
 203             _terminus= (_End_segments[1]== j_j_next);
 204             _End_segments[0] = j_j_next;
 205           }
 206         else
 207           {
 208             if( _End_segments[0]== j_j_next)
 209               {
 210                 for(int idim=DIM-1;idim>=0;idim--) _Inter.push_front(ABCD[idim]);
 211                 _terminus= (_End_segments[1]== i_i_next);
 212                 _End_segments[0] = i_i_next;
 213               }
 214             else
 215               {
 216                 for(int idim=0;idim<DIM;idim++) _Inter.push_back(ABCD[idim]);
 217                 _End_segments[1] = (_End_segments[1]== i_i_next) ? j_j_next : i_i_next;
 218               }
 219           }
 220       }
 221     else
 222       {
 223         for(int i=0;i<DIM;i++) _Inter.push_back(ABCD[i]);
 224         _End_segments.push_back(i_i_next);
 225         _End_segments.push_back(j_j_next);
 226       }
 227   }
 228
 229   /*******************************************************/
 230   /* checks the possible crossing between segments [A,B] */
 231   /* (with end-point global indices i and i_next)        */
 232   /* and  [C,D] (end-point global indices j and j_next). */
 233   /* If no intersection is detected, checks whether B is */
 234   /* inside the quadrangle AEDC.                         */
 235   /* Updates the lists inter and _End_segments            */
 236   /*******************************************************/
 237
 238   template<int DIM>
 239   void PolygonAlgorithms<DIM>::addCrossing0(const double * A, const double * B, int i, int i_next,
 240                                             const double * C, const double * D, int j, int j_next)
 241   {
 242     double ABCD[DIM];
 243     if(intersectSegmentSegment(A,B,C,D,ABCD, ABCD))
 244       //fifth and sixth arguments are useless here
 245       {
 246         /* Updating _End_segments */
 247         std::pair< int,int > i_i_next = std::make_pair(i, i_next);
 248         std::pair< int,int > j_j_next = std::make_pair(j, j_next);
 249         if( _End_segments[0] == i_i_next)
 250           {
 251             for(int idim=DIM-1;idim>-1;idim--) _Inter.push_front(ABCD[idim]);
 252             _End_segments[0] = j_j_next;
 253           }
 254         else
 255           {
 256             for(int idim=0;idim<DIM;idim++) _Inter.push_back(ABCD[idim]);
 257             _End_segments[1] = j_j_next;
 258             _terminus = _End_segments[0]== j_j_next;
 259           }
 260
 261         /* Updating _Status */
 262         _Status.insert(make_pair(i_next,std::make_pair(i, false)));
 263         std::multimap< int, std::pair< int,bool> >::iterator mi =_Status.find(j_next);
 264         ((* mi).second).second= !((* mi).second).second;
 265       }
 266     else        _Status.insert(std::make_pair(i_next,std::make_pair(i,true)));
 267   }
 268
 269   /*******************************************************/
 270   /* adds the possible crossings between segments [A,B] (with end-point global indices i and i_next) */
 271   /*and segments [C,D] and [E,F] to the list inter and updates _End_segments */
 272   /* In cases of ambiguity, the vertex G is used to decide wether the crossing should be accepted */
 273   /*******************************************************/
 274   template<int DIM>
 275   inline void PolygonAlgorithms<DIM>::addCrossings( const double * A, const double * B, int i , int i_next,
 276                                                     const double * C, const double * D, int j1, int j2,
 277                                                     const double * E, const double * F, int j3, int j4,
 278                                                     const double * G)
 279   {
 280     double ABCD[DIM];
 281     double ABEF[DIM];
 282     std::multimap< int, std::pair< int,bool> >::iterator mi;
 283
 284     if(intersectSegmentSegment(A,B,C,D,G,ABCD))
 285       {
 286         if(intersectSegmentSegment(A,B,E,F,G,ABEF))
 287           {
 288             VertexLess<DIM> vl;
 289             if (vl(ABCD,ABEF))
 290               {
 291                 addCrossing(ABCD,  std::make_pair(i, i_next),  std::make_pair(j1, j2));
 292                 addCrossing(ABEF,  std::make_pair(i, i_next),  std::make_pair(j3, j4));
 293               }
 294             else
 295               {
 296                 addCrossing(ABEF,  std::make_pair(i, i_next),  std::make_pair(j3, j4));
 297                 addCrossing(ABCD,  std::make_pair(i, i_next),  std::make_pair(j1, j2));
 298               }
 299             _Status.insert(std::make_pair(i_next,std::make_pair(i, _is_in_intersection)));
 300             mi=_Status.find(j2);
 301             ((* mi).second).second= !((* mi).second).second;
 302             mi=_Status.find(j4);
 303             ((* mi).second).second= !((* mi).second).second;
 304           }
 305         else
 306           {
 307             addCrossing(ABCD, std::make_pair( i, i_next),  std::make_pair(j1,j2));
 308             _Status.insert(std::make_pair(i_next,std::make_pair(i, !_is_in_intersection)));
 309             mi=_Status.find(j2);
 310             ((* mi).second).second= !((* mi).second).second;
 311           }
 312       }
 313     else
 314       {
 315         if(intersectSegmentSegment(A,B,E,F,G, ABEF))
 316           {
 317             addCrossing(ABEF, std::make_pair( i, i_next), std::make_pair( j3, j4));
 318             _Status.insert(std::make_pair(i_next,std::make_pair(i, !_is_in_intersection)));
 319             mi=_Status.find(j4);
 320             ((* mi).second).second= !((* mi).second).second;
 321           }
 322         else           _Status.insert(std::make_pair(i_next,std::make_pair(i, _is_in_intersection)));
 323       }
 324   }
 325
 326
 327   /* define various indices required in the function intersect_conv_polygon */
 328   /* vertices from the both polygons are supposed to be present in the status */
 329   template<int DIM>
 330   inline void PolygonAlgorithms<DIM>::defineIndices(int& i_loc, int& i_next, int& i_prev,
 331                                                     const double *& Poly1, const double *& Poly2,
 332                                                     int& j1, int& j1_glob, int& j2, int& j2_glob,
 333                                                     int& j3, int& j3_glob, int& j4,  int& j4_glob,
 334                                                     int& i_glob, int& i_next_glob, int& i_prev_glob,
 335                                                     const double * P_1, const double * P_2,
 336                                                     int N1, int N2, int sign)
 337   {
 338     int N0, shift;
 339     if(i_glob < N1)
 340       {
 341         N0 = N1;
 342         shift = 0;
 343         Poly1 = P_1;
 344         Poly2 = P_2;
 345
 346         std::multimap< int, std::pair< int,bool> >::reverse_iterator mi1=_Status.rbegin();
 347         j1_glob=((*mi1).second).first;
 348         j1=j1_glob-N1;
 349         j2_glob=(*mi1).first;
 350         j2=j2_glob-N1;
 351         mi1++;
 352         j3_glob=((*mi1).second).first;
 353         j3=j3_glob-N1;
 354         j4_glob=(*mi1).first;
 355         j4=j4_glob-N1;
 356       }
 357     else
 358       {
 359         N0 = N2;
 360         shift = N1;
 361         Poly1 = P_2;
 362         Poly2 = P_1;
 363
 364         std::multimap< int, std::pair< int,bool> >::iterator mi2= _Status.begin();
 365         j1_glob=((*mi2).second).first;
 366         j1=j1_glob;
 367         j2_glob=(*mi2).first;
 368         j2=j2_glob;
 369         mi2++;
 370         j3_glob=((*mi2).second).first;
 371         j3=j3_glob;
 372         j4_glob=(*mi2).first;
 373         j4=j4_glob;
 374       }
 375     i_loc = i_glob-shift;
 376     i_next = (i_next_glob-shift+N0)%N0;//end-point of segment starting at i
 377     i_prev = (i_prev_glob-shift+N0)%N0;
 378     i_next_glob = i_next+shift;
 379     i_prev_glob = i_prev+shift;
 380     //warning: sign is either 1 or -1;
 381     //To do: test and remove from Convex_intersecor.cxx
 382     //        while(distance2<DIM>(&Poly1[DIM*i_loc],&Poly1[DIM*i_next])< _epsilon && i_next != i_loc)
 383     //          i_next =(i_next+sign+N0)%N0;
 384     //        while(distance2<DIM>(&Poly1[DIM*i_loc],&Poly1[DIM*i_prev])< _epsilon && i_prev != i_loc)
 385     //          i_prev =(i_prev+sign+N0)%N0;
 386   }
 387   /*******************************************************/
 388   /* computes the vertices of the intersection of two COPLANAR */
 389   /* simple (no dble points)convex polygons using line sweep algorithm */
 390   /* P1 and P2 contain the 3D coordinates of the successive vertices */
 391   /*******************************************************/
 392   template<int DIM>
 393   std::deque< double > PolygonAlgorithms<DIM>::intersectConvexPolygons(const double* P_1,const double* P_2,
 394                                                                        int N1, int N2)
 395   {
 396     int i_loc, i_glob, j1, j1_glob, j2,j2_glob, j3, j3_glob, j4,j4_glob,
 397       i_prev, i_prev_glob, i_next, i_next_glob, nb_prev, sign, idim;
 398     const double * Poly1, * Poly2;
 399     bool four_neighbours=false;
 400     _terminus = N1 < 3 || N2<3;
 401
 402     /* list of future events ordered according to their coordinates (x,y,z) (lexicographical order) */
 403     std::multimap< const double *, int, VertexLess<DIM> > mmap_events;
 404     typename std::list< std::pair< const double *, int > >::iterator mi1,mi2;
 405
 406     std::multimap< int, std::pair< int,bool> >::iterator mi;
 407
 408     /********** Initalisation of events with P1 and P2 vertices ************/
 409     for(i_loc=0;i_loc<N1;i_loc++)
 410       mmap_events.insert(std::make_pair(&P_1[DIM*i_loc],i_loc));
 411     for(i_loc=0;i_loc<N2;i_loc++)
 412       mmap_events.insert(std::make_pair(&P_2[DIM*i_loc],i_loc+N1));
 413
 414     std::list< std::pair< const double *, int > > events(mmap_events.begin(),mmap_events.end());
 415
 416     if(!_terminus)
 417       {
 418         /******** Treatment of the first vertex ********/
 419         mi1=events.begin();
 420         i_glob = (* mi1).second;
 421         bool which_start = i_glob < N1;
 422         if(i_glob < N1){ i_next_glob = (i_glob   +1)%N1;     i_prev_glob = (i_glob   -1+N1)%N1;}
 423         else{            i_next_glob = (i_glob-N1+1)%N2 + N1;i_prev_glob = (i_glob-N1-1+N2)%N2 + N1;}
 424         _Status.insert(std::make_pair(i_next_glob,std::make_pair(i_glob, false)));
 425         _Status.insert(std::make_pair(i_prev_glob,std::make_pair(i_glob, false)));
 426         mi1++;
 427         //std::cout<< "nb_prev= "<< 0 << " i_glob= " << i_glob << std::endl;
 428
 429         /******* Loop until the second polygon is reached *******/
 430         while( !four_neighbours)
 431           {
 432             i_glob=(* mi1).second;//global index of vertex i
 433             nb_prev = _Status.count(i_glob);//counts the number of segments ending at i
 434
 435             //std::cout<< "nb_prev= "<< nb_prev << " i_glob= " << i_glob << std::endl;
 436             switch (nb_prev)
 437               {
 438               case 1 :
 439                 mi=_Status.find(i_glob);// pointer to the segment ending at i
 440                 i_prev_glob = ((*mi).second).first;//starting point of the segment ending at i
 441                 i_next= (i_prev_glob - i_glob > 0) == (abs(i_prev_glob - i_glob) == 1)  ? i_glob - 1 : i_glob + 1;
 442                 if(i_glob < N1) i_next_glob = (i_next   +N1)%N1;
 443                 else            i_next_glob = (i_next-N1+N2)%N2 + N1;
 444                 _Status.erase(mi);
 445                 _Status.insert(std::make_pair(i_next_glob,std::make_pair(i_glob, false)));
 446                 mi1++;
 447                 break;
 448               case 2 :
 449                 return _Inter;
 450               case 0 :
 451                 if( (i_glob < N1) != which_start)
 452                   {
 453                     mi2=mi1;
 454                     mi2++;
 455                     /* detection of double points */
 456                     if(distance2<DIM>((* mi1).first, (*mi2).first) > _epsilon)
 457                       four_neighbours = true;
 458                     else         /* Rare pothological case:  */
 459                       {
 460                         const std::pair< const double *, int > next_pt= *mi2;
 461                         events.erase(mi2);
 462                         mi1=events.insert(mi1,next_pt);
 463                       }
 464                   }
 465                 break;
 466               default:
 467                 throw Exception("intersectConvexPolygon: sequence of nodes does not describe a simple polygon (1)");
 468               }
 469           }
 470         /******** Loop until a terminal point or crossing is reached ************/
 471         while( !_terminus)
 472           {
 473             //std::cout<< "nb_prev= "<< nb_prev<< " nb_inter= " << _Inter.size()/DIM << std::endl;
 474             switch (nb_prev)
 475               {
 476               case 1 :
 477                 mi=_Status.find(i_glob);// pointer to the segment ending at i
 478                 i_prev_glob = ((*mi).second).first;//starting point of the segment ending at i
 479                 sign = (i_prev_glob - i_glob > 0) == (abs(i_prev_glob - i_glob) == 1)  ? - 1 : + 1;
 480                 i_next_glob = i_glob+sign;
 481                 _is_in_intersection = ((*mi).second).second;//boolean that tells if i is in the intersection
 482                 _Status.erase(mi);
 483                 defineIndices(i_loc,i_next,i_prev, Poly1,Poly2,
 484                               j1,j1_glob,j2,j2_glob,j3,j3_glob,j4,j4_glob,
 485                               i_glob,i_next_glob,i_prev_glob, P_1,P_2, N1, N2, sign);
 486                 if( _is_in_intersection ) addNewVertex(i_loc, i_glob, i_next_glob, i_prev_glob, Poly1);
 487                 addCrossings(&Poly1[DIM*i_loc], &Poly1[DIM*i_next], i_glob, i_next_glob,
 488                              &Poly2[DIM*j1]   , &Poly2[DIM*j2]    , j1_glob,j2_glob,
 489                              &Poly2[DIM*j3]   , &Poly2[DIM*j4]    , j3_glob,j4_glob, &Poly1[DIM*i_prev]);
 490                 break;
 491               case 2 :
 492                 if(!_Inter.empty())
 493                   if(i_glob < N1)  for(idim=0;idim<DIM;idim++) _Inter.push_back(P_1[DIM*i_glob+idim]);
 494                   else for(idim=0;idim<DIM;idim++) _Inter.push_back(P_2[DIM*(i_glob-N1)+idim]);
 495                 return _Inter;
 496               case 0 ://To do if possible : remove this case from here
 497                 if(_Inter.empty() && (i_glob < N1) != which_start){
 498                   i_next_glob = i_glob+1;
 499                   i_prev_glob = i_glob-1;
 500                   defineIndices(i_loc,i_next,i_prev, Poly1,Poly2,
 501                                 j1,j1_glob,j2,j2_glob,j3,j3_glob,j4,j4_glob,
 502                                 i_glob,i_next_glob,i_prev_glob, P_1,P_2, N1, N2, 1);
 503                   double V12[DIM], V34[DIM];
 504                   double inside = check_inside<DIM>(&Poly1[DIM*i_loc],&Poly2[DIM*j1],&Poly2[DIM*j2],
 505                                                     &Poly2[DIM*j3],   &Poly2[DIM*j4],V12, V34);
 506                   _is_in_intersection=( inside < _epsilon ); // <= epsilon or 0 ?
 507
 508                   if(fabs(inside) > _epsilon)//vertex clearly inside or outside
 509                     {
 510                       //std::cout<<"coucou1" << std::endl;
 511                       if( _is_in_intersection)
 512                         {
 513                           for(int idim=0;idim<DIM;idim++) _Inter.push_back(Poly1[DIM*i_loc+idim]);
 514                           _End_segments.push_back(std::make_pair(i_glob,i_next_glob));
 515                           _End_segments.push_back(std::make_pair(i_glob,i_prev_glob));
 516                         }
 517                       addCrossings(&Poly1[DIM*i_loc], &Poly1[DIM*i_next], i_glob, i_next_glob,
 518                                    &Poly2[DIM*j1]   , &Poly2[DIM*j2]    , j1_glob,j2_glob,
 519                                    &Poly2[DIM*j3]   , &Poly2[DIM*j4]    , j3_glob,j4_glob, &Poly1[DIM*i_prev]);
 520                       addCrossings(&Poly1[DIM*i_loc], &Poly1[DIM*i_prev], i_glob, i_prev_glob,
 521                                    &Poly2[DIM*j1]   , &Poly2[DIM*j2]    , j1_glob,j2_glob,
 522                                    &Poly2[DIM*j3]   , &Poly2[DIM*j4]    , j3_glob,j4_glob, &Poly1[DIM*i_next]);
 523                     }
 524                   else //vertex on an edge
 525                     {
 526                       //std::cout<<"coucou2" << std::endl;
 527                       bool is_inside_next, is_inside_prev;
 528                       double Vnext[DIM], Vprev[DIM];
 529                       for(idim=0;idim<DIM;idim++) _Inter.push_back(Poly1[DIM*i_loc+idim]);
 530
 531                       if(dotprod<DIM>(V34,V34) > _epsilon)//vertex i on edge (j1,j2), not on (j3,j4)
 532                         {
 533                           crossprod<DIM>(&Poly1[DIM*i_loc], &Poly2[DIM*j2], &Poly1[DIM*i_next],Vnext);
 534                           crossprod<DIM>(&Poly1[DIM*i_loc], &Poly2[DIM*j2], &Poly1[DIM*i_prev],Vprev);
 535                           is_inside_next= (dotprod<DIM>(Vnext,V34)<0);
 536                           is_inside_prev= (dotprod<DIM>(Vprev,V34)<0);
 537
 538                           if(!(is_inside_next || is_inside_prev)) return std::deque< double >();
 539
 540                           if(is_inside_next)
 541                             {
 542                               _End_segments.push_back(std::make_pair(i_glob,i_next_glob));
 543                               addCrossing0(&Poly1[DIM*i_loc], &Poly1[DIM*i_next], i_glob, i_next_glob,
 544                                            &Poly2[DIM*j3]   , &Poly2[DIM*j4]    , j3_glob,j4_glob);
 545                             }
 546                           else
 547                             {
 548                               _End_segments.push_back(std::make_pair(j1_glob,j2_glob));
 549                               _Status.insert(std::make_pair(i_next_glob,std::make_pair(i_glob, false)));
 550                               mi=_Status.find(j2_glob);
 551                               ((* mi).second).second= !((* mi).second).second;
 552                             }
 553                           if(is_inside_prev)
 554                             {
 555                               _End_segments.push_back(std::make_pair(i_glob,i_prev_glob));
 556                               addCrossing0(&Poly1[DIM*i_loc], &Poly1[DIM*i_prev], i_glob, i_prev_glob,
 557                                            &Poly2[DIM*j3]   , &Poly2[DIM*j4]    , j3_glob,j4_glob);
 558                             }
 559                           else
 560                             {
 561                               _End_segments.push_back(std::make_pair(j1_glob,j2_glob));
 562                               _Status.insert(std::make_pair(i_prev_glob,std::make_pair(i_glob, false)));
 563                               mi=_Status.find(j2_glob);
 564                               ((* mi).second).second= !((* mi).second).second;
 565                             }
 566                         }
 567                       else if(dotprod<DIM>(V12,V12) > _epsilon)//vertex i on a edge (j3,j4), not on (j1,j2)
 568                         {
 569                           crossprod<DIM>(&Poly1[DIM*i_loc], &Poly2[DIM*j4], &Poly1[DIM*i_next],Vnext);
 570                           crossprod<DIM>(&Poly1[DIM*i_loc], &Poly2[DIM*j4], &Poly1[DIM*i_prev],Vprev);
 571                           is_inside_next= dotprod<DIM>(Vnext,V12)<0;
 572                           is_inside_prev= dotprod<DIM>(Vprev,V12)<0;
 573
 574                           if(!(is_inside_next || is_inside_prev)) return std::deque< double >();
 575
 576                           if(is_inside_next)
 577                             {
 578                               _End_segments.push_back(std::make_pair(i_glob,i_next_glob));
 579                               addCrossing0(&Poly1[DIM*i_loc], &Poly1[DIM*i_next], i_glob, i_next_glob,
 580                                            &Poly2[DIM*j1]   , &Poly2[DIM*j2]    , j1_glob,j2_glob);
 581                             }
 582                           else
 583                             {
 584                               _End_segments.push_back(std::make_pair(j3_glob,j4_glob));
 585                               _Status.insert(std::make_pair(i_next_glob,std::make_pair(i_glob, false)));
 586                               mi=_Status.find(j4_glob);
 587                               ((* mi).second).second= ! ((* mi).second).second;
 588                             }
 589                           if(is_inside_prev)
 590                             {
 591                               _End_segments.push_back(std::make_pair(i_glob,i_prev_glob));
 592                               addCrossing0(&Poly1[DIM*i_loc], &Poly1[DIM*i_prev], i_glob, i_prev_glob,
 593                                            &Poly2[DIM*j1]   , &Poly2[DIM*j2]    , j1_glob,j2_glob);
 594                             }
 595                           else
 596                             {
 597                               _End_segments.push_back(std::make_pair(j3_glob,j4_glob));
 598                               _Status.insert(std::make_pair(i_prev_glob,std::make_pair(i_glob, false)));
 599                               mi=_Status.find(j4_glob);
 600                               ((* mi).second).second= !((* mi).second).second;
 601                             }
 602                         }
 603                       else //vertices i, j1 and j3 share the same coordinates
 604                         {
 605                           crossprod<DIM>(&Poly1[DIM*i_loc], &Poly2[DIM*j2], &Poly1[DIM*i_next],Vnext);
 606                           crossprod<DIM>(&Poly1[DIM*i_loc], &Poly2[DIM*j2], &Poly1[DIM*i_prev],Vprev);
 607                           crossprod<DIM>(&Poly1[DIM*i_loc], &Poly2[DIM*j4], &Poly1[DIM*i_next],V12);
 608                           crossprod<DIM>(&Poly1[DIM*i_loc], &Poly2[DIM*j4], &Poly1[DIM*i_prev],V34);
 609
 610                           double inside_next= dotprod<DIM>(Vnext,V12);
 611                           double inside_prev= dotprod<DIM>(Vprev,V34);
 612                           double inside_j2  = dotprod<DIM>(Vnext,Vprev);
 613                           double inside_j4  = dotprod<DIM>(V12,V34);
 614
 615                           std::map<double, std::pair<int,int> > which_is_inside;
 616                           which_is_inside[inside_next] = std::make_pair(i_glob,i_next_glob);
 617                           which_is_inside[inside_prev] = std::make_pair(i_glob,i_prev_glob);
 618                           which_is_inside[inside_j2] =  std::make_pair(j1_glob,j2_glob);
 619                           which_is_inside[inside_j4] =  std::make_pair(j3_glob,j4_glob);
 620
 621                           std::map<double, std::pair<int,int> >::iterator min = which_is_inside.begin();
 622                           std::map<double, std::pair<int,int> >::iterator minext = min;
 623                           minext++;
 624                           std::map<double, std::pair<int,int> >::reverse_iterator max = which_is_inside.rbegin();
 625                           std::multimap< int, std::pair< int,bool> >::iterator j2_in_status = _Status.find(((*min).second).second);
 626                           std::multimap< int, std::pair< int,bool> >::iterator j4_in_status = _Status.find(((*minext).second).second);
 627
 628                           if((*min).first < -_epsilon) //there is someone clearly inside
 629                             {
 630                               _End_segments.push_back( (*min).second );
 631                               _End_segments.push_back((* minext).second);
 632                               if(j2_in_status != _Status.end())
 633                                 ((*j2_in_status).second).second        = !        ((*j2_in_status).second).second;
 634                               if(j4_in_status != _Status.end())
 635                                 ((*j4_in_status).second).second        = !        ((*j4_in_status).second).second;
 636                               is_inside_next = ((*min).second).second == i_next_glob || ((*minext).second).second == i_next_glob;
 637                               is_inside_prev = ((*min).second).second == i_prev_glob || ((*minext).second).second == i_prev_glob;
 638                             }
 639                           else
 640                             if(fabs((*min).first) <= _epsilon) //nobody is clearly inside but two segments are superposed
 641                               {
 642                                 if(fabs((*max).first) > _epsilon)
 643                                   return std::deque< double >();
 644                                 else //all four segments are superposed
 645                                   {
 646                                     _End_segments.push_back(std::make_pair(i_glob,i_next_glob));
 647                                     _End_segments.push_back(std::make_pair(i_glob,i_prev_glob));
 648                                     is_inside_next= true;
 649                                     is_inside_prev= true;
 650                                   }
 651                               }
 652                             else                //there is nobody inside
 653                               return std::deque< double >();
 654
 655                           _Status.insert(std::make_pair(i_prev_glob,std::make_pair(i_glob,is_inside_prev)));
 656                           _Status.insert(std::make_pair(i_next_glob,std::make_pair(i_glob,is_inside_next)));
 657                         }
 658                     }
 659                 }
 660                 break;
 661               default:
 662                 std::cout << "Problem: nbprev= " << nb_prev << " ; i_glob = " << i_glob << std::endl;
 663                 throw Exception("intersectConvexPolygon: sequence of nodes does not describe a simple polygon (2)");
 664               }
 665             mi1++;
 666             i_glob=(* mi1).second;//global index of vertex i
 667             nb_prev = _Status.count(i_glob);
 668           }
 669       }
 670     return _Inter;
 671   }
 672
 673   /**************************************************************************/
 674   /* computes the convex hull of a polygon subP which is a sub polygon of P */
 675   /* P is the array of coordinates, subP is a map containing initially the indices of a subpolygon of P */
 676   /* in the end, subP contains only the elements belonging to the convex hull, and  not_in_hull the others */
 677   /**************************************************************************/
 678   template<int DIM>
 679   inline void PolygonAlgorithms<DIM>::convHull(const double *P, int N, double * normal,
 680                                                std::map< int,int >& subP, std::map< int,int >& not_in_hull,
 681                                                int& NsubP, const double epsilon)
 682   {
 683     if(NsubP>3)
 684       {
 685         std::map< int,int >::iterator mi_prev = subP.begin();
 686         std::map< int,int >::iterator  mi = mi_prev;
 687         mi++;
 688         std::map< int,int >::iterator  mi_next = mi;
 689         mi_next++;
 690         double directframe=0.;
 691
 692         /* Check if the polygon subP is positively oriented */
 693         std::map< int,int >::iterator mi1=mi;
 694         while(mi1 != subP.end() && distance2<DIM>(&P[DIM*(*subP.begin()).second],&P[DIM*(*mi1).second])< epsilon)
 695           mi1++;
 696         std::map< int,int >::iterator mi2=mi1;
 697         while(mi2 != subP.end() && fabs(directframe)<epsilon)
 698           {
 699             directframe =direct_frame<DIM>(&P[DIM* (*mi1).second],
 700                                            &P[DIM* (*subP.begin()).second],
 701                                            &P[DIM* (*mi2).second], normal);
 702             mi2++;
 703           }
 704         if(directframe < 0) for(int idim=0; idim< DIM; idim++) normal[idim] *= -1;
 705
 706         /* Core of the algorithm */
 707         while(mi_next != subP.end())
 708           {
 709             directframe = direct_frame<DIM>(&P[DIM* (*mi).second],
 710                                             &P[DIM* (*mi_prev).second],
 711                                             &P[DIM* (*mi_next).second], normal);
 712             if(directframe > -epsilon){
 713               mi ++;
 714               mi_prev++;
 715               mi_next++;
 716             }
 717             else
 718               {
 719                 not_in_hull.insert(*mi);
 720                 subP.erase(mi);
 721                 NsubP--;
 722                 mi--;
 723               }
 724           }
 725         directframe = direct_frame<DIM>(&P[DIM*(*mi).second],
 726                                         &P[DIM*(*mi_prev).second],
 727                                         &P[DIM*(*subP.begin()).second], normal);
 728         if(directframe < -epsilon)
 729           {
 730             not_in_hull.insert(*mi);
 731             subP.erase(mi);
 732             NsubP--;
 733           }
 734       }
 735   }
 736
 737   template<int DIM>
 738   void PolygonAlgorithms<DIM>::convexDecomposition(const double * P, int N, double *normal, std::vector< int > subP, int NsubP,
 739                                                    std::vector< std::map< int,int > >& components, std::vector< int >& components_index,
 740                                                    int& Ncomp, int sign, const double epsilon)
 741   {
 742     int i;
 743     std::map< int, int > hull;
 744     std::map< int, int > not_in_hull;
 745     std::map< int, int >::iterator mi, mj;
 746     std::vector< int > reflex_region;
 747     int Nreflex;
 748     int i_xmax=0;
 749     const double * xmax=&P[DIM*subP[0]];
 750     /* checking an extremal point of subP */
 751     for(i=0; i<NsubP; i++)
 752       {
 753         if(&P[DIM*subP[i]]> xmax)
 754           {
 755             i_xmax=i;
 756             xmax=&P[DIM*subP[i]];
 757           }
 758       }
 759     /* renumbering of SubP elements for the convex hull*/
 760     for(i=0; i<NsubP; i++) hull.insert(hull.end(),std::make_pair(i,subP[(i+i_xmax)%NsubP]));
 761     /* compute the convex hull */
 762     convHull(P, N, normal, hull, not_in_hull, NsubP,epsilon);
 763     /* convex hull is the next component */
 764     components.push_back(hull);
 765     components_index.push_back(sign*NsubP);
 766     Ncomp++;
 767     /* searching for reflex regions */
 768     for(mi=not_in_hull.begin(); mi!=not_in_hull.end(); mi++)
 769       {
 770         reflex_region.clear();
 771         reflex_region.push_back(hull[(*mi).first-1]);
 772         reflex_region.push_back( (*mi).second );
 773         Nreflex=2;
 774         mj=mi;
 775         mj++;
 776         while((mj != not_in_hull.end()) && ((*mj).first == (*mi).first+1))
 777           {
 778             reflex_region.push_back((*mj).second);
 779             Nreflex++;
 780             mi++;
 781             mj++;
 782           }
 783         reflex_region.push_back(hull[(*mi).first+1]);
 784         Nreflex++;
 785         convexDecomposition( P, N,normal, reflex_region, Nreflex, components, components_index, Ncomp, -sign, epsilon);
 786       }
 787   }
 788
 789   /**************************************************************************/
 790   /* decomposes a non convex polygon P with N vertices contained in a plane */
 791   /* into a sequence of convex polygons */
 792   /* the input vectors 'components' and 'components_index' should be empty */
 793   /* returns the number of convex components */
 794   /* if P is composed of a single point, then an empty polygon is returned */
 795   /**************************************************************************/
 796   template<int DIM>
 797   int PolygonAlgorithms<DIM>::convexDecomposition(const double * P, int N, std::vector< std::map< int,int > >& components,
 798                                                   std::vector< int >& components_index, const double epsilon)
 799   {
 800     int Ncomp=0;
 801     std::vector< int > subP(N);
 802     double normal[3]={0,0,0};
 803
 804     for(int i = 0; i<N; i++) subP[i]=i;
 805
 806     //Build the normal of polygon P
 807     int i1=1;
 808     while(i1<N && distance2<DIM>(&P[0],&P[i1])< epsilon) i1++;
 809     int i2=i1+1;
 810     while(i2<N && fabs(dotprod<DIM>(normal,normal))<epsilon)
 811       {
 812         crossprod<DIM>(&P[i1], &P[0], &P[i2],normal);
 813         i2++;
 814       }
 815
 816     convexDecomposition(P, N, normal, subP, N, components, components_index, Ncomp, 1, epsilon);
 817     return Ncomp;
 818   }
 819 }
 820
 821 #endif