//----------------------------------------------------------------------------
//  EgalTech 2014-2014
//----------------------------------------------------------------------------
// File : Triangulate.cpp       Data : 23.06.14          Versione : 1.5f6
// Contenuto : Implementazione della classe Triangulate.
//
//
//
// Modifiche : 08.04.14 DS Creazione modulo.
//             23.06.14 DS Corretto errore con contorni CW.
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include <algorithm>
#include "Triangulate.h"
#include "ProjPlane.h"
#include "\EgtDev\Include\EGkPolyLine.h"
#include "\EgtDev\Include\EGkPlane3d.h"

using namespace std ;

//----------------------------------------------------------------------------
static enum EarStatus{ EAS_NULL = -1, EAS_NO = 0, EAS_OK = 1} ;

//----------------------------------------------------------------------------
static bool ChangeStartPntVector( int nNewStart, PNTVECTOR& vPi) ;

//----------------------------------------------------------------------------
// In : PolyLine
// Out : PNTVECTOR (Point3d Vector) : points of the polyline 
//       INTVECTOR (int Vector) : 3*T indices of above points for T triangles   
//----------------------------------------------------------------------------
bool
Triangulate::Make( const PolyLine& PL, PNTVECTOR& vPt, INTVECTOR& vTr)
{
  // verifico che la polilinea sia chiusa e piana e calcolo il piano medio del poligono
   double dArea ;
   Plane3d plPlane ;
   if ( ! PL.IsClosedAndFlat( plPlane, dArea, 50 * EPS_SMALL))
      return false ;
  // determino il piano ottimale di proiezione e il relativo senso di rotazione
   bool bCCW ;
   if ( ! CalcProjPlane( plPlane.vtN, m_nPlane, bCCW))
      return false ;
  // riempio il vettore con i vertici del poligono da triangolare
   vPt.clear() ;
   vPt.reserve( PL.GetPointNbr() - 1) ;
   // salto il primo punto (coincide con l'ultimo)
   Point3d ptP ;
   if ( ! PL.GetFirstPoint( ptP))
      return false ;
   // inserisco i punti
   while ( PL.GetNextPoint( ptP))
      vPt.push_back( ptP) ;
  // se non CCW inverto il vettore dei punti
   if ( ! bCCW)
      reverse( vPt.begin(), vPt.end()) ;
  // creo il vettore degli indici del Poligono
   INTVECTOR vPol ;
   int n = int( vPt.size()) ;
   vPol.reserve( n) ;
   for ( int i = 0 ; i < n ; ++ i)
      vPol.push_back( i) ;

  // eseguo la triangolazione
   if ( ! MakeByEC2( vPt, vPol, vTr))
      return false ;

  // se era CW, devo invertire il senso dei triangoli
   if ( ! bCCW)
      reverse( vTr.begin(), vTr.end()) ;

   return true ;
}

//----------------------------------------------------------------------------
// In : POLYLINEVECTOR : vector of polylines, the first outer, the others inner
// Out : PNTVECTOR (Point3d Vector) : points of the polyline 
//       INTVECTOR (int Vector) : 3*T indices of above points for T triangles   
//----------------------------------------------------------------------------
bool
Triangulate::Make( const POLYLINEVECTOR& vPL, PNTVECTOR& vPt, INTVECTOR& vTr)
{
  // pulisco i vettori di ritorno
   vPt.clear() ;
   vTr.clear() ;
  // ci devono essere delle polilinee nel vettore
   if ( &vPL == nullptr  ||  vPL.empty())
      return false ;
  // se una sola polilinea mi riconduco al caso precedente
   if ( vPL.size() == 1)
      return Make( vPL[0], vPt, vTr) ;
  // verifico che la polilinea esterna sia chiusa e piana e calcolo il piano medio del poligono
   double dArea ;
   Plane3d plExtPlane ;
   if ( ! vPL[0].IsClosedAndFlat( plExtPlane, dArea, 50 * EPS_SMALL))
      return false ;
  // determino il piano ottimale di proiezione e il relativo senso di rotazione
   bool bCCW ;
   if ( ! CalcProjPlane( plExtPlane.vtN, m_nPlane, bCCW))
      return false ;
  // verifico le altre polilinee
   for ( int i = 1 ; i < int( vPL.size()) ; ++i) {
     // deve essere chiusa, giacere nello stesso piano ed essere orientata al contrario della esterna
      double dArea ;
      Plane3d plPlane ;
      if ( ! vPL[i].IsClosedAndFlat( plPlane, dArea, 50 * EPS_SMALL)  ||
           ! AreOppositeVectorApprox( plExtPlane.vtN, plPlane.vtN))
         return false ;
   }
  // se non CCW inverto tutte le polilinee
   if ( ! bCCW) {
      for ( int i = 0 ; i < int( vPL.size()) ; ++i)
         const_cast<PolyLine&>(vPL[i]).Invert( true) ;
   }
  // calcolo ordine decrescente su Xmax o Ymax o Zmax secondo m_nPlane per le curve interne
   INTVECTOR vOrd ;
   if ( ! SortInternalLoops( vPL, vOrd))
      return false ;
  // riempio il vettore con i punti dei poligoni da triangolare
   // calcolo spazio totale e spazio massimo per un percorso interno
   int nTot = vPL[0].GetPointNbr() - 1 ;
   int nMax = 0 ;
   for ( int i = 1 ; i < int( vPL.size()) ; ++i) {
      nTot += vPL[i].GetPointNbr() + 1 ;
      if ( vPL[i].GetPointNbr() > nMax)
         nMax = vPL[i].GetPointNbr() ;
   }
   // riservo spazio pari al totale dei punti (anche quelli ripetuti di chiusura che servono per i link)
   vPt.reserve( nTot) ;
   // inserisco i punti del contorno esterno (tranne il primo che coincide con l'ultimo)
   if ( ! GetPntVectorFromPolyline( vPL[0], false, vPt))
      return false ;
   // ciclo sui percorsi interni ordinati secondo Xmax decrescente
   PNTVECTOR vPi ;
   vPi.reserve( nMax) ;
   for ( int i = 1 ; i < int( vPL.size()) ; ++ i) {
      // riordino il percorso per avere punto iniziale con Xmax
      if ( ! GetPntVectorFromPolyline( vPL[vOrd[i]], true, vPi))
         return false ;
      // cerco un punto del percorso esterno visibile dal punto iniziale del precedente interno
      int nI ;
      if ( ! GetOuterPntToJoin( vPt, vPi[0], nI))
         return false ;
      // riordino percorso esterno per avere questo punto all'inizio
      if ( ! ChangeStartPntVector( nI, vPt))
         return false ;
      // ripeto punto iniziale
      vPt.push_back( vPt[0]) ;
      // accodo percorso interno
      for ( int j = 0 ; j < int( vPi.size()) ; ++j)
         vPt.push_back( vPi[j]) ;
      // ripeto punto iniziale di percorso interno
      vPt.push_back( vPi[0]) ;
      // cancello percorso interno
      vPi.clear() ;
   }
  // ripristino l'orientamento delle polilinee originali per il caso non CCW
   if ( ! bCCW) {
      for ( int i = 0 ; i < int( vPL.size()) ; ++i)
         const_cast<PolyLine&>(vPL[i]).Invert( true) ;
   }

  // creo il vettore degli indici del Poligono
   INTVECTOR vPol ;
   int n = int( vPt.size()) ;
   vPol.reserve( n) ;
   // non devo gestire separatamente CCW perchè ho già invertito i punti
   for ( int i = 0 ; i < n ; ++ i)
      vPol.push_back( i) ;

  // eseguo la triangolazione
   if ( ! MakeByEC2( vPt, vPol, vTr))
      return false ;

  // se era CW, devo invertire il senso dei triangoli
   if ( ! bCCW)
      reverse( vTr.begin(), vTr.end()) ;

   return true ;
}

//----------------------------------------------------------------------------
bool
Triangulate::PrepareGrid( const PNTVECTOR& vPt, const INTVECTOR& vPol,
                          const INTVECTOR& vPrev, const INTVECTOR& vNext)
{
  // points number
   int n = int( vPol.size()) ;
  // overall box
   BBox3d b3All ;
   for ( int j = 0 ; j < n ; ++ j)
      b3All.Add( vPt[vPol[j]]) ;
  // grid cell dimension
   double dCellDim ;
   b3All.GetRadius( dCellDim) ;
   dCellDim *=  2 / sqrt( n) ;
  // grid 
   if ( ! m_VertGrid.Init( 2 * n, dCellDim))
      return false ;
   for ( int j = 0 ; j < n ; ++ j) {
     // insert only reflex vertex
      if ( ! TriangleIsCCW( vPt[vPol[vPrev[j]]], vPt[vPol[j]], vPt[vPol[vNext[j]]])) {
         if ( ! m_VertGrid.InsertPoint( vPt[vPol[j]], j))
            return false ;
      }
   }
   m_vVert.reserve( n / 5) ;

   return true ;
}

//----------------------------------------------------------------------------
// Triangulate the CCW n-gon specified by the vertices vPt (Pt[n] != Pt[0])
// Ear Clipping algorithm
//----------------------------------------------------------------------------
bool
Triangulate::MakeByEC( const PNTVECTOR& vPt, const INTVECTOR& vPol, INTVECTOR& vTr)
{
  // Clear triangle vector
   vTr.clear() ;

  // At least 3 points
   int n = int( vPol.size()) ;
   if ( n < 3)
      return false ;

  // Preallocate triangle vector ( #triangles = n - 2)
   vTr.reserve( 3 * ( n - 2)) ;

  // Set up previous and next links to effectively form a double-linked vertex list
   INTVECTOR vPrev( n) ;
   INTVECTOR vNext( n) ;
   for ( int j = 0 ; j < n ; ++ j) {
      vPrev[j] = j - 1 ;
      vNext[j] = j + 1 ;
   }
   vPrev[0] = n - 1 ;
   vNext[n-1] = 0 ;

  // Start at vertex 0
   int i = 0 ;
   int nCount = n ;
  // Keep removing vertices until just a triangle left
   while ( n >= 3) {
     // To avoid infinite loop
      if ( nCount <= 0)
         return false ;
      -- nCount ;
     // Test if current vertex, v[i], is an ear
      bool bIsEar = TestTriangle( vPt, vPol, vPrev, vNext, i) ;
      bool bSave = bIsEar ;
     // Verify if triangle is null (accept to discard)
      if ( ! bIsEar) {
         if ( Collinear( vPt[vPol[vPrev[i]]], vPt[vPol[i]], vPt[vPol[vNext[i]]]))
            bIsEar = true ;
      }
     // If current vertex v[i] is an ear, delete it and visit the previous vertex
      if ( bIsEar) {
         // Triangle (v[prev[i]], v[i], v[next[i]]) is an ear
          if ( bSave) {
             vTr.push_back( vPol[vPrev[i]]) ;
             vTr.push_back( vPol[i]) ;
             vTr.push_back( vPol[vNext[i]]) ;
          }
         // ‘Delete’ vertex v[i] by redirecting next and previous links
         // of neighboring verts past it. Decrement vertex count
          vNext[vPrev[i]] = vNext[i] ;
          vPrev[vNext[i]] = vPrev[i] ;
          n--;
         // Visit the previous vertex next
          i = vPrev[i] ;
         // Reset Count
          nCount = n ;
      }
      else {
         // Current vertex is not an ear; visit the next vertex
          i = vNext[i] ;
      }
   }

   return true ;
}

//----------------------------------------------------------------------------
// Triangulate the CCW n-gon specified by the vertices vPt (Pt[n] != Pt[0])
// Ear Clipping algorithm enhanced, choose smaller diagonal
//----------------------------------------------------------------------------
bool
Triangulate::MakeByEC2( const PNTVECTOR& vPt, const INTVECTOR& vPol, INTVECTOR& vTr)
{
  // Clear triangle vector
   vTr.clear() ;

  // At least 3 points
   int n = int( vPol.size()) ;
   if ( n < 3)
      return false ;

  // Preallocate triangle vector ( #triangles = n - 2)
   vTr.reserve( 3 * ( n - 2)) ;

  // Set up previous and next links to effectively form a double-linked vertex list
   INTVECTOR vPrev( n) ;
   INTVECTOR vNext( n) ;
   for ( int j = 0 ; j < n ; ++ j) {
      vPrev[j] = j - 1 ;
      vNext[j] = j + 1 ;
   }
   vPrev[0] = n - 1 ;
   vNext[n-1] = 0 ;

  // Prepare Ear status vector
   INTVECTOR vEar( n, EAS_NULL) ;

  // Prepare PointGrid
   if ( ! PrepareGrid( vPt, vPol, vPrev, vNext))
      return false ;

  // Start at vertex 0
   int i = 0 ;
   int nCount = n ;
  // Keep removing vertices until just a triangle left
   while ( n >= 3) {
     // To avoid infinite loop
      if ( nCount <= 0)
         return false ;
      -- nCount ;
     // Test if current vertex, v[i], is an ear
      if ( vEar[i] == EAS_NULL)
         vEar[i] = ( TestTriangle( vPt, vPol, vPrev, vNext, i) ? EAS_OK : EAS_NO) ;
      bool bIsEar = ( vEar[i] == EAS_OK) ;
      bool bSave = bIsEar ;
      if ( bIsEar) {
        // Save square distance of diagonal
         double dSqDist = SqDist(vPt[vPol[vPrev[i]]], vPt[vPol[vNext[i]]]) ;
        // Try with 3 next
         int j = i ;
         double dSqDist1 = INFINITO ;
         for ( int h = 0 ; h < 3 ; ++ h) {
            j = vNext[j] ;
            if ( vEar[j] == EAS_NULL)
               vEar[j] = ( TestTriangle( vPt, vPol, vPrev, vNext, j) ? EAS_OK : EAS_NO) ;
            if ( vEar[j] == EAS_OK) {
               dSqDist1 = SqDist( vPt[vPol[vPrev[j]]], vPt[vPol[vNext[j]]]) ;
               break ;
            }
         }
        // Try with 3 prev
         int k = i ;
         double dSqDist2 = INFINITO ;
         for ( int h = 0 ; h < 3 ; ++ h) {
            k = vPrev[k] ;
            if ( vEar[k] == EAS_NULL)
               vEar[k] = ( TestTriangle( vPt, vPol, vPrev, vNext, k) ? EAS_OK : EAS_NO) ;
            if ( vEar[k] == EAS_OK) {
               dSqDist2 = SqDist( vPt[vPol[vPrev[k]]], vPt[vPol[vNext[k]]]) ;
               break ;
            }
         }
        // Choose the better (EPS_ZERO to have a difference)
         if ( dSqDist < dSqDist1 + EPS_ZERO  &&  dSqDist < dSqDist2 + EPS_ZERO)
            ;  // i is the better
         else if ( dSqDist1 < dSqDist2 + EPS_ZERO)
            i = j ;
         else
            i = k ;
      }
     // Verify if triangle is null (accept to discard)
      else {
         if ( Collinear( vPt[vPol[vPrev[i]]], vPt[vPol[i]], vPt[vPol[vNext[i]]])  &&
              ! Aligned( vPt[vPol[vPrev[i]]], vPt[vPol[i]], vPt[vPol[vNext[i]]]))
            bIsEar = true ;
      }
     // If current vertex v[i] is an ear, delete it and visit the previous vertex
      if ( bIsEar) {
        // Triangle (v[prev[i]], v[i], v[next[i]]) is an ear
         if ( bSave) {
            vTr.push_back( vPol[vPrev[i]]) ;
            vTr.push_back( vPol[i]) ;
            vTr.push_back( vPol[vNext[i]]) ;
         }
        // Reset earity of diagonal endpoints
         vEar[vPrev[i]] = EAS_NULL ;
         vEar[vNext[i]] = EAS_NULL ;
        // ‘Delete’ vertex v[i] by redirecting next and previous links
        // of neighboring verts past it. Decrement vertex count
         vNext[vPrev[i]] = vNext[i] ;
         vPrev[vNext[i]] = vPrev[i] ;
         -- n ;
        // Visit the previous vertex next
         i = vPrev[i] ;
        // Reset Count
         nCount = n ;
      }
      else {
         // Current vertex is not an ear; visit the next vertex
          i = vNext[i] ;
      }
   }

   return true ;
}

//----------------------------------------------------------------------------
// Triangulate the CCW n-gon specified by the vertices vPt (Pt[n] != Pt[0])
// Ear Clipping algorithm enhanced, choose smaller triangle aspect ratio
// AR = ( Lmax * Lmax) / ( 2 * Area) = SqLenMax / L1 * L2
//----------------------------------------------------------------------------
bool
Triangulate::MakeByEC3( const PNTVECTOR& vPt, const INTVECTOR& vPol, INTVECTOR& vTr)
{
  // Clear triangle vector
   vTr.clear() ;

  // At least 3 points
   int n = int( vPol.size()) ;
   if ( n < 3)
      return false ;

  // Preallocate triangle vector ( #triangles = n - 2)
   vTr.reserve( 3 * ( n - 2)) ;

  // Set up previous and next links to effectively form a double-linked vertex list
   INTVECTOR vPrev( n) ;
   INTVECTOR vNext( n) ;
   for ( int j = 0 ; j < n ; ++ j) {
      vPrev[j] = j - 1 ;
      vNext[j] = j + 1 ;
   }
   vPrev[0] = n - 1 ;
   vNext[n-1] = 0 ;

  // Prepare Ear status vector
   INTVECTOR vEar( n, EAS_NULL) ;

  // Prepare PointGrid
   if ( ! PrepareGrid( vPt, vPol, vPrev, vNext))
      return false ;

  // Start at vertex 0
   int i = 0 ;
   int nCount = n ;
  // Keep removing vertices until just a triangle left
   while ( n >= 3) {
     // To avoid infinite loop
      if ( nCount <= 0)
         return false ;
      -- nCount ;
     // Test if current vertex, v[i], is an ear
      if ( vEar[i] == EAS_NULL)
         vEar[i] = ( TestTriangle( vPt, vPol, vPrev, vNext, i) ? EAS_OK : EAS_NO) ;
      bool bIsEar = ( vEar[i] == EAS_OK) ;
      bool bSave = bIsEar ;
      if ( bIsEar) {
        // Square Length & Aspect Ratio
         double dSqLen = SqDist(vPt[vPol[vPrev[i]]], vPt[vPol[vNext[i]]]) ;
         double dAR = CalcTriangleAspectRatio( vPt[vPol[vPrev[i]]], vPt[vPol[i]], vPt[vPol[vNext[i]]]) ;
        // Try with all other verticis
         int ii = i ;
         int j = vNext[i] ;
         while ( j != ii) {
           // New Square Length
            double dNewSqLen = SqDist(vPt[vPol[vPrev[j]]], vPt[vPol[vNext[j]]]) ;
            if ( dNewSqLen < 0.99 * dSqLen) {
              // New Aspect Ratio
               double dNewAR = CalcTriangleAspectRatio( vPt[vPol[vPrev[j]]], vPt[vPol[j]], vPt[vPol[vNext[j]]]) ;
              // if better, test if triangle ok
               if ( dNewAR < 30  ||  dNewAR < 1.2 * dAR) {
                  if ( vEar[j] == EAS_NULL)
                     vEar[j] = ( TestTriangle( vPt, vPol, vPrev, vNext, j) ? EAS_OK : EAS_NO) ;
                  if ( vEar[j] == EAS_OK) {
                     dSqLen = dNewSqLen ;
                     dAR = min( dAR, dNewAR) ;
                     i = j ;
                  }
               }
            }
            j = vNext[j] ;
         } 
      }
     // Verify if triangle is null (accept to discard)
      else {
         if ( Collinear( vPt[vPol[vPrev[i]]], vPt[vPol[i]], vPt[vPol[vNext[i]]])  &&
              ! Aligned( vPt[vPol[vPrev[i]]], vPt[vPol[i]], vPt[vPol[vNext[i]]]))
            bIsEar = true ;
      }
     // If current vertex v[i] is an ear, delete it and visit the previous vertex
      if ( bIsEar) {
        // Triangle (v[prev[i]], v[i], v[next[i]]) is an ear to save
         if ( bSave) {
          vTr.push_back( vPol[vPrev[i]]) ;
          vTr.push_back( vPol[i]) ;
          vTr.push_back( vPol[vNext[i]]) ;
         }
        // Reset earity of diagonal endpoints
         vEar[vPrev[i]] = EAS_NULL ;
         vEar[vNext[i]] = EAS_NULL ;
        // ‘Delete’ vertex v[i] by redirecting next and previous links
        // of neighboring verts past it. Decrement vertex count
         vNext[vPrev[i]] = vNext[i] ;
         vPrev[vNext[i]] = vPrev[i] ;
         -- n ;
        // Visit the previous vertex next
         i = vPrev[i] ;
        // Reset Count
         nCount = n ;
      }
      else {
        // Current vertex is not an ear; visit the next vertex
         i = vNext[i] ;
      }
   }

   return true ;
}

//----------------------------------------------------------------------------
bool
Triangulate::TestTriangle( const PNTVECTOR& vPt, const INTVECTOR& vPol,
                           const INTVECTOR& vPrev, INTVECTOR& vNext, int i)
{
  // Test if current vertex, v[i], is an ear
   bool bIsEar = true ;
  // An ear must be convex (here counterclockwise)
   if ( TriangleIsCCW( vPt[vPol[vPrev[i]]], vPt[vPol[i]], vPt[vPol[vNext[i]]])) {
     // Loop over all vertices not part of the tentative ear
      BBox3d b3Tria ;
      b3Tria.Add( vPt[vPol[vPrev[i]]]) ;
      b3Tria.Add( vPt[vPol[i]]) ;
      b3Tria.Add( vPt[vPol[vNext[i]]]) ;
      m_VertGrid.Find( b3Tria, m_vVert) ;
      for ( int j = 0 ; j < int( m_vVert.size()) ; ++ j) {
         int k = m_vVert[j] ;
         if ( k == i  ||  k == vPrev[i]  ||  k == vNext[i] )
            continue ;
        // If vertex k is on a triangle vertex
         if ( AreSamePoint( vPt[vPol[k]], vPt[vPol[vPrev[i]]])  ||  
              AreSamePoint( vPt[vPol[k]], vPt[vPol[i]])  ||
              AreSamePoint( vPt[vPol[k]], vPt[vPol[vNext[i]]])) {
           // Test previous segment with triangle diagonal
            if ( TestIntersection( vPt[vPol[vPrev[k]]], vPt[vPol[k]], vPt[vPol[vPrev[i]]], vPt[vPol[vNext[i]]])) {
               bIsEar = false ;
               break ;
            }
           // Test next segment with triangle diagonal
            if ( TestIntersection( vPt[vPol[k]], vPt[vPol[vNext[k]]], vPt[vPol[vPrev[i]]], vPt[vPol[vNext[i]]])) {
               bIsEar = false ;
               break ;
            }
         }
        // If vertex k is inside the ear triangle, then this is not an ear
         else if ( TestPointInTriangle( vPt[vPol[k]], vPt[vPol[vPrev[i]]], vPt[vPol[i]], vPt[vPol[vNext[i]]])) {
            bIsEar = false ;
            break ;
         }
      }
   }
   else {
     // The ‘ear’ triangle is clockwise so v[i] is not an ear
      bIsEar = false ;
   }

   return bIsEar ;
}

//----------------------------------------------------------------------------
// Ratio between max side and opposite height
double
Triangulate::CalcTriangleAspectRatio( const Point3d& ptPa, const Point3d& ptPb, const Point3d& ptPc)
{
   double dSqDistA = SquareDist( ptPa, ptPb) ;
   double dSqDistB = SquareDist( ptPb, ptPc) ;
   double dSqDistC = SquareDist( ptPc, ptPa) ;
   double dTwoArea = fabs( TwoArea( ptPa, ptPb, ptPc)) ;
   if ( dTwoArea < EPS_SMALL * EPS_SMALL)
      return INFINITO ;
   else
      return ( (std::max)( dSqDistA, (std::max)( dSqDistB, dSqDistC)) / dTwoArea) ;
}

//----------------------------------------------------------------------------
double
Triangulate::SquareDist( const Point3d& ptA, const Point3d& ptB)
{
   switch ( m_nPlane) {
   default : // PL_XY
      return (( ptB.x - ptA.x) * ( ptB.x - ptA.x) + ( ptB.y - ptA.y) * ( ptB.y - ptA.y)) ;
   case PL_YZ :
      return (( ptB.y - ptA.y) * ( ptB.y - ptA.y) + ( ptB.z - ptA.z) * ( ptB.z - ptA.z)) ;
   case PL_ZX :
      return (( ptB.z - ptA.z) * ( ptB.z - ptA.z) + ( ptB.x - ptA.x) * ( ptB.x - ptA.x)) ;
   }
}

//----------------------------------------------------------------------------
// Vector product is double of the area (positive if CCW)
double
Triangulate::TwoArea( const Point3d& ptA, const Point3d& ptB, const Point3d& ptC)
{
   switch ( m_nPlane) {
   default : // PL_XY
      return ( ptB.x - ptA.x) * ( ptC.y - ptB.y) - ( ptB.y - ptA.y) * ( ptC.x - ptB.x) ;
   case PL_YZ :
      return ( ptB.y - ptA.y) * ( ptC.z - ptB.z) - ( ptB.z - ptA.z) * ( ptC.y - ptB.y) ;
   case PL_ZX :
      return ( ptB.z - ptA.z) * ( ptC.x - ptB.x) - ( ptB.x - ptA.x) * ( ptC.z - ptB.z) ;
   }
}

//----------------------------------------------------------------------------
// Distance less than tolerance
bool
Triangulate::AreSamePoint( const Point3d& ptA, const Point3d& ptB, double dToler)
{
   return ( SquareDist( ptA, ptB) < dToler * dToler) ;
}

//----------------------------------------------------------------------------
// Collinear <--> Null Area
bool
Triangulate::Aligned( const Point3d& ptA, const Point3d& ptB, const Point3d& ptC)
{
   switch ( m_nPlane) {
   default : // PL_XY
      return (( ptB.x - ptA.x) * ( ptC.x - ptB.x) + ( ptB.y - ptA.y) * ( ptC.y - ptB.y)) > 0 ;
   case PL_YZ :
      return (( ptB.y - ptA.y) * ( ptC.y - ptB.y) + ( ptB.z - ptA.z) * ( ptC.z - ptB.z)) > 0 ;
   case PL_ZX :
      return (( ptB.z - ptA.z) * ( ptC.z - ptB.z) + ( ptB.x - ptA.x) * ( ptC.x - ptB.x)) > 0 ;
   }
}

//----------------------------------------------------------------------------
// Collinear <--> Null Area
bool
Triangulate::Collinear( const Point3d& ptA, const Point3d& ptB, const Point3d& ptC, double dToler)
{
   return ( fabs( TwoArea( ptA, ptB, ptC)) < dToler * dToler) ;
}

//----------------------------------------------------------------------------
// Positive Area means A -> B -> C counterclockwise
bool
Triangulate::TriangleIsCCW( const Point3d& ptA, const Point3d& ptB, const Point3d& ptC, double dToler)
{
   return ( TwoArea( ptA, ptB, ptC) > dToler * dToler) ;
}

//----------------------------------------------------------------------------
// Proper intersection between line segments
bool
Triangulate::TestIntersection( const Point3d& ptA1, const Point3d& ptA2,
                               const Point3d& ptB1, const Point3d& ptB2)
{
  // Collinearity is not considered intersection
   if ( Collinear( ptA1, ptA2, ptB1)  ||
        Collinear( ptA1, ptA2, ptB2)  ||
        Collinear( ptB1, ptB2, ptA1)  ||
        Collinear( ptB1, ptB2, ptA2))
      return false ;
  // To intersect, the endpoints of a line segment must be on opposite side of the other line segment
   return ( TriangleIsCCW( ptA1, ptA2, ptB1) != TriangleIsCCW( ptA1, ptA2, ptB2)  &&
            TriangleIsCCW( ptB1, ptB2, ptA1) != TriangleIsCCW( ptB1, ptB2, ptA2)) ;
}

//----------------------------------------------------------------------------
// Test if point p is contained in triangle (a, b, c)
bool
Triangulate::TestPointInTriangle( const Point3d& ptP, const Point3d& ptA, const Point3d& ptB, const Point3d& ptC)
{
  // If P is on a vertex is considered outside
   if ( AreSamePoint( ptP, ptA))
      return false ;
   if ( AreSamePoint( ptP, ptB))
      return false ;
   if ( AreSamePoint( ptP, ptC))
      return false ;
  // If P is on the right of at least one edge is outside
   if ( TriangleIsCCW( ptA, ptP, ptB))
      return false ;
   if ( TriangleIsCCW( ptB, ptP, ptC))
      return false ;
   if ( TriangleIsCCW( ptC, ptP, ptA))
      return false ;
  // P is in triangle
   return true ;
}

//----------------------------------------------------------------------------
bool
Triangulate::SortInternalLoops( const POLYLINEVECTOR& vPL, INTVECTOR& vOrd)
{
  // riempio vettore con indice e massimo specifico per ogni loop interno
   typedef std::pair<int,double> INDMAX ;         // coppia indice, massimo
   typedef std::vector<INDMAX>   INDMAXVECTOR ;   // vettore di coppie indice, massimo
   INDMAXVECTOR vMax ;
   vMax.reserve( vPL.size()) ;
   vMax.emplace_back( 0, 0.0) ;
   for ( int i = 1 ; i < int( vPL.size()) ; ++ i) {
      BBox3d b3Loc ;
      vPL[i].GetLocalBBox( b3Loc) ;
      switch ( m_nPlane) {
      default : // PL_XY
         vMax.emplace_back( i, b3Loc.GetMax().x) ;
         break ;
      case PL_YZ :
         vMax.emplace_back( i, b3Loc.GetMax().y) ;
         break ;
      case PL_ZX :
         vMax.emplace_back( i, b3Loc.GetMax().z) ;
         break ;
      }
   }
  // ordino vettore in senso decrescente rispetto al massimo
   sort( vMax.begin() + 1, vMax.end(), 
         []( const INDMAX& a, const INDMAX&b) { return a.second > b.second ; }) ;
  // copio indice nel vettore di ordine
   vOrd.reserve( vPL.size()) ;
   for ( int i = 0 ; i < int( vPL.size()) ; ++ i)
      vOrd.push_back( vMax[i].first) ;

   return true ;
}

//----------------------------------------------------------------------------
bool
Triangulate::GetPntVectorFromPolyline( const PolyLine& PL, bool bXmaxStart, PNTVECTOR& vPi)
{
  // copio i punti nel vettore (tranne il primo che coincide con l'ultimo)
   Point3d ptP ;
   if ( ! PL.GetFirstPoint( ptP))
      return false ;
   while ( PL.GetNextPoint( ptP)) {
      vPi.push_back( ptP) ;
   }
  // se non richiesto riordino per Xmax o Ymax o Zmax, ho finito
   if ( ! bXmaxStart)
      return true ;
  // determino l'indice del punto con Xmax
   int nI = - 1 ;
   double dXmax = - INFINITO ;
   for ( int i = 0 ; i < int( vPi.size()) ; ++ i) {
      switch ( m_nPlane) {
      default : // PL_XY
         if ( vPi[i].x > dXmax) {
            dXmax = vPi[i].x ;
            nI = i ;
         }
         break ;
      case PL_YZ :
         if ( vPi[i].y > dXmax) {
            dXmax = vPi[i].y ;
            nI = i ;
         }
         break ;
      case PL_ZX :
         if ( vPi[i].z > dXmax) {
            dXmax = vPi[i].z ;
            nI = i ;
         }
         break ;
      }
   }
   if ( nI == - 1)
      return false ;
  // riordino il vettore, per avere il punto con Xmax in prima posizione
   return ChangeStartPntVector( nI, vPi) ;
}

//----------------------------------------------------------------------------
bool
Triangulate::GetOuterPntToJoin( const PNTVECTOR& vPt, const Point3d& ptP, int& nI)
{
  // cerco prima intersezione del raggio dal punto con direzione X+ al contorno esterno
   nI = - 1 ;
   double dMinDist = INFINITO ;
   Point3d ptInt ;
   int nNumPt = int( vPt.size()) ;
   for ( int i = 0 ; i < nNumPt ; ++ i) {
     // indice punto precedente
      int h = ( i - 1 >= 0 ) ? i - 1 : nNumPt - 1 ;
     // indice punto successivo
      int j = ( i + 1 < nNumPt) ? i + 1 : 0 ;
     // mi metto nel piano principale
      switch (m_nPlane) {
      default :  // PL_XY
        // se i punti coincidono
         if ( fabs( vPt[i].x - ptP.x) < EPS_SMALL  &&  fabs( vPt[i].y - ptP.y) < EPS_SMALL) {
            dMinDist = 0 ;
            nI = i ;
            ptInt = vPt[i] ;
         }
        // se punto esattamente sul raggio e raggio interno al settore
         else if ( vPt[i].x > ptP.x  &&  fabs( vPt[i].y - ptP.y) < EPS_SMALL  &&
              PointInSector( ptP, vPt[h], vPt[i], vPt[j])) {
           // se distanza minore della minima, nuovo minimo
            if ( ( vPt[i].x - ptP.x) < dMinDist) {
               dMinDist = vPt[i].x - ptP.x ;
               nI = i ;
               ptInt = vPt[i] ;
            }
         }
        // se segmento al punto successivo che attraversa il raggio e raggio interno ovvero a sinistra ( segmento crescente in Y)
         else if ( vPt[i].y < ptP.y  &&  vPt[j].y > ptP.y) {
           // calcolo l'ascissa di intersezione
            double dCoeff = ( ptP.y - vPt[i].y) / ( vPt[j].y - vPt[i].y) ;
            double dX = vPt[i].x + ( vPt[j].x - vPt[i].x) * dCoeff ;
           // se sta sul raggio e distanza minore della minima
            if ( dX > ptP.x - EPS_SMALL  &&  ( dX - ptP.x) < dMinDist) {
               dMinDist = dX - ptP.x ;
               nI = ( vPt[i].x >= vPt[j].x) ? i : j ;
               double dZ = vPt[i].z + ( vPt[j].z - vPt[i].z) * dCoeff ;
               ptInt.Set( dX, ptP.y, dZ) ;
            }
         }
         break ;
      case PL_YZ :
        // se i punti coincidono
         if ( fabs( vPt[i].y - ptP.y) < EPS_SMALL  &&  fabs( vPt[i].z - ptP.z) < EPS_SMALL) {
            dMinDist = 0 ;
            nI = i ;
            ptInt = vPt[i] ;
         }
        // se punto esattamente sul raggio e raggio interno al settore
         else if ( vPt[i].y > ptP.y  &&  fabs( vPt[i].z - ptP.z) < EPS_SMALL  &&
              PointInSector( ptP, vPt[h], vPt[i], vPt[j])) {
           // se distanza minore della minima, nuovo minimo
            if ( ( vPt[i].y - ptP.y) < dMinDist) {
               dMinDist = vPt[i].y - ptP.y ;
               nI = i ;
               ptInt = vPt[i] ;
            }
         }
        // se segmento al punto successivo che attraversa il raggio e raggio interno ovvero a sinistra ( segmento crescente in Y)
         else if ( vPt[i].z < ptP.z  &&  vPt[j].z > ptP.z) {
           // calcolo l'ascissa di intersezione
            double dCoeff = ( ptP.z - vPt[i].z) / ( vPt[j].z - vPt[i].z) ;
            double dY = vPt[i].y + ( vPt[j].y - vPt[i].y) * dCoeff ;
           // se sta sul raggio e distanza minore della minima
            if ( dY > ptP.y - EPS_SMALL  &&  ( dY - ptP.y) < dMinDist) {
               dMinDist = dY - ptP.y ;
               nI = ( vPt[i].y >= vPt[j].y) ? i : j ;
               double dX = vPt[i].x + ( vPt[j].x - vPt[i].x) * dCoeff ;
               ptInt.Set( dX, dY, ptP.z) ;
            }
         }
         break ;
      case PL_ZX :
        // se i punti coincidono
         if ( fabs( vPt[i].z - ptP.z) < EPS_SMALL  &&  fabs( vPt[i].x - ptP.x) < EPS_SMALL) {
            dMinDist = 0 ;
            nI = i ;
            ptInt = vPt[i] ;
         }
        // se punto esattamente sul raggio e raggio interno al settore
         else if ( vPt[i].z > ptP.z  &&  fabs( vPt[i].x - ptP.x) < EPS_SMALL  &&
              PointInSector( ptP, vPt[h], vPt[i], vPt[j])) {
           // se distanza minore della minima, nuovo minimo
            if ( ( vPt[i].z - ptP.z) < dMinDist) {
               dMinDist = vPt[i].z - ptP.z ;
               nI = i ;
               ptInt = vPt[i] ;
            }
         }
        // se segmento al punto successivo che attraversa il raggio e raggio interno ovvero a sinistra ( segmento crescente in Y)
         else if ( vPt[i].x < ptP.x  &&  vPt[j].x > ptP.x) {
           // calcolo l'ascissa di intersezione
            double dCoeff = ( ptP.x - vPt[i].x) / ( vPt[j].x - vPt[i].x) ;
            double dZ = vPt[i].z + ( vPt[j].z - vPt[i].z) * dCoeff ;
           // se sta sul raggio e distanza minore della minima
            if ( dZ > ptP.z - EPS_SMALL  &&  ( dZ - ptP.z) < dMinDist) {
               dMinDist = dZ - ptP.z ;
               nI = ( vPt[i].z >= vPt[j].z) ? i : j ;
               double dY = vPt[i].y + ( vPt[j].y - vPt[i].y) * dCoeff ;
               ptInt.Set( ptP.y, dY, dZ) ;
            }
         }
         break ;
      }
   }
  // non ho trovato alcunché, errore
   if ( nI == - 1)
      return false ;
  // se ho trovato un punto esatto del contorno, non devo fare altri controlli
   if ( AreSamePointApprox( ptInt, vPt[nI]))
      return true ;
  // devo ora verificare che il segmento che unisce i punti non intersechi altri lati del contorno esterno
  // altrimenti tengo il punto con raggio più vicino a X_AX o Y_AX o Z_AX secondo m_nPlane
   int nJ = nI ;
   Point3d ptPa = ptP ;
   Point3d ptPb = vPt[nI] ;
   Point3d ptPc = ptInt ;
   bool bSwap = false ;
   switch ( m_nPlane) {
   default : /* PL_XY */ bSwap = ( ptPb.y > ptPc.y) ; break ;
   case PL_YZ : bSwap = ( ptPb.z > ptPc.z) ; break ;
   case PL_ZX : bSwap = ( ptPb.x > ptPc.x) ; break ;
   }
   if ( bSwap)
      swap( ptPb, ptPc) ;
   double dMinTan = INFINITO ;
   double dMinSqDist = INFINITO * INFINITO ;
   for ( int i = 0 ; i < nNumPt ; ++ i) {
     // salto il punto già trovato
      if ( i == nJ)
         continue ;
     // verifico se sta nel triangolo
      if ( TestPointInTriangle( vPt[i], ptPa, ptPb, ptPc)) {
         double dTan = INFINITO ;
         switch ( m_nPlane) {
         default : /* PL_XY */ dTan = fabs(vPt[i].y - ptP.y) / (vPt[i].x - ptP.x) ; break ;
         case PL_YZ : dTan = fabs(vPt[i].z - ptP.z) / (vPt[i].y - ptP.y) ; break ;
         case PL_ZX : dTan = fabs(vPt[i].x - ptP.x) / (vPt[i].z - ptP.z) ; break ;
         }
         if ( dTan < dMinTan + EPS_ZERO) {
           // indice punto precedente
            int h = ( i - 1 >= 0) ? i - 1 : nNumPt - 1 ;
           // indice punto successivo
            int j = ( i + 1 < nNumPt) ? i + 1 : 0 ;
           // verifico che il raggio che unisce i punti stia nel settore
            if ( PointInSector( ptP, vPt[h], vPt[i], vPt[j])) {
               double dSqDist = SqDist( vPt[i], ptP) ;
               if ( dTan < dMinTan - EPS_ZERO  ||  dSqDist < dMinSqDist) {
                  dMinTan = dTan ;
                  dMinSqDist = dSqDist ;
                  nI = i ;
               }
            }
         }
      }  
   }

   return true ;
}

//----------------------------------------------------------------------------
bool
Triangulate::PointInSector( const Point3d& ptTest, const Point3d& ptPrev, const Point3d& ptCorn, const Point3d& ptNext)
{
  // la parte valida del settore è a sinistra dei segmenti ptPrev --> ptCorn --> ptNext
  // se corner convesso
   if ( TriangleIsCCW( ptPrev, ptCorn, ptNext, 0))
      return ( TriangleIsCCW( ptPrev, ptCorn, ptTest)  &&
               TriangleIsCCW( ptTest, ptCorn, ptNext)) ;
  // altrimenti corner concavo ( reflex)
   else
      return ! ( TriangleIsCCW( ptTest, ptCorn, ptPrev, 0)  &&
                 TriangleIsCCW( ptNext, ptCorn, ptTest, 0)) ;
}

//----------------------------------------------------------------------------
bool
ChangeStartPntVector( int nNewStart, PNTVECTOR& vPi)
{
  // se il nuovo inizio coincide col vecchio, non devo fare alcunché
   if ( nNewStart == 0)
      return true ;
  // se il nuovo indice è oltre la dimensione del vettore, errore
   if ( nNewStart >= int( vPi.size()))
      return false ;
  // ciclo di aggiustamento
   rotate( vPi.begin(), vPi.begin() + nNewStart, vPi.end()) ;
   return true ;
}