//----------------------------------------------------------------------------
//  EgalTech 2018-2018
//----------------------------------------------------------------------------
// File : IntersTriaTria.cpp       Data : 27.08.18         Versione : 1.9h3
// Contenuto : Implementazione della intersezione triangolo/triangolo.
//
//
//
// Modifiche : 27.08.18 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "ProjPlane.h"
#include "IntersLineTria.h"
#include "CurveComposite.h"
#include "SurfFlatRegion.h"
#include "Triangulate.h"
#include "GeoConst.h"
#include "/EgtDev/Include/EGkIntersTriaTria.h"
#include "/EgtDev/Include/EGkIntersPlanePlane.h"
#include <array>

using namespace std ;

//----------------------------------------------------------------------------
static int IntersCoplanarTriaTria( const Triangle3d& trTria1, const Triangle3d& trTria2, TRIA3DVECTOR& vTria) ;

//----------------------------------------------------------------------------
static int FindTriaTriaIntersType( const Triangle3d& trTria1, const Triangle3d& trTria2, const Point3d& ptLineSt, const Vector3d& vtLineDir,
                                   double dStU1, double dEnU1, double dStU2, double dEnU2, int nRes1, int nRes2, double& dIntStU, double& dIntEnU) ;

//----------------------------------------------------------------------------
int
IntersTriaTria( const Triangle3d& trTria1, const Triangle3d& trTria2, Point3d& ptInt, Point3d& ptInt2, TRIA3DVECTOR& vTria)
{
  // piano del secondo triangolo
   Plane3d plTria2 ;
   plTria2.Set( trTria2.GetCentroid(), trTria2.GetN()) ;
  // calcolo le distanze dei vertici del primo triangolo dal piano del secondo
   array< double, 3> vDist1 ;
   for ( int i = 0 ; i < 3 ; ++i)
      vDist1[i] = DistPointPlane( trTria1.GetP( i), plTria2) ;
  // verifico posizione del primo triangolo rispetto al piano del secondo
   int nVertPos1 = 0 ; int nVertNeg1 = 0 ;
   for ( const auto& dDist : vDist1) {
      if ( dDist > EPS_SMALL)
         ++ nVertPos1 ;
      else if ( dDist < -EPS_SMALL)
         ++ nVertNeg1 ;
   }
  // se il triangolo giace tutto da una parte del piano, nessuna intersezione
   if ( nVertPos1 == 3  ||  nVertNeg1 == 3)
      return ITTT_NO ;

  // piano del primo triangolo
   Plane3d plTria1 ;
   plTria1.Set( trTria1.GetCentroid(), trTria1.GetN()) ;
  // calcolo le distanze dei vertici del secondo triangolo dal piano del primo
   array< double, 3> vDist2 ;
   for ( int i = 0 ; i < 3 ; ++i)
      vDist2[i] = DistPointPlane( trTria2.GetP( i), plTria1) ;
  // verifico posizione del secondo triangolo rispetto al piano del primo
   int nVertPos2 = 0 ; int nVertNeg2 = 0 ;
   for ( const auto& dDist : vDist2) {
      if ( dDist > EPS_SMALL)
         ++ nVertPos2 ;
      else if ( dDist < -EPS_SMALL)
         ++ nVertNeg2 ;
   }
  // se il triangolo giace tutto da una parte del piano, nessuna intersezione
   if ( nVertPos2 == 3  ||  nVertNeg2 == 3)
      return ITTT_NO ;

  // intersezione tra i piani dei due triangoli
   Point3d ptL ; Vector3d vtL ;
   int nResPP = IntersPlanePlane( plTria1, plTria2, ptL, vtL) ;
   if ( nResPP == IPPT_NO)
      return ITTT_NO ;

  // se i triangoli sono complanari
   if ( nResPP == IPPT_OVERLAPS  ||
        ((( nVertPos1 == 0  &&  nVertNeg1 == 0)  ||  ( nVertPos2 == 0  &&  nVertNeg2 == 0)) &&
         ( trTria1.GetN() ^ trTria2.GetN()).SqLen() < 25 * SIN_EPS_ANG_SMALL * SIN_EPS_ANG_SMALL)) {
     // verifica per triangoli con normali controverse
      bool bCounter = false ;
      Triangle3d trMyTria2 = trTria2 ;
      if ( trTria1.GetN() * trTria2.GetN() < 0) {
         Point3d ptV0 = trMyTria2.GetP( 0) ;
         trMyTria2.SetP( 0, trMyTria2.GetP( 1)) ;
         trMyTria2.SetP( 1, ptV0) ;
         trMyTria2.Validate() ;
         bCounter = true ;
      }
     // verifica per triangoli coincidenti
      bool bAreSameTria = ( ( AreSamePointExact( trTria1.GetP( 0), trMyTria2.GetP( 0))  &&
                              AreSamePointExact( trTria1.GetP( 1), trMyTria2.GetP( 1))  &&
                              AreSamePointExact( trTria1.GetP( 2), trMyTria2.GetP( 2)))  ||
                            ( AreSamePointExact( trTria1.GetP( 0), trMyTria2.GetP( 1))  &&
                              AreSamePointExact( trTria1.GetP( 1), trMyTria2.GetP( 2))  &&
                              AreSamePointExact( trTria1.GetP( 2), trMyTria2.GetP( 0)))  ||
                            ( AreSamePointExact( trTria1.GetP( 0), trMyTria2.GetP( 2))  &&
                              AreSamePointExact( trTria1.GetP( 1), trMyTria2.GetP( 0))  &&
                              AreSamePointExact( trTria1.GetP( 2), trMyTria2.GetP( 1))))  &&
                          ( AreSameVectorExact( trTria1.GetN(), trMyTria2.GetN())) ;
      if ( bAreSameTria) {
         vTria.emplace_back( trTria1) ;
         return ( bCounter ? ITTT_COUNTER_OVERLAPS : ITTT_OVERLAPS) ;
      }
      if ( ITTT_OVERLAPS == IntersCoplanarTriaTria( trTria1, trMyTria2, vTria)) {
         return ( bCounter ? ITTT_COUNTER_OVERLAPS : ITTT_OVERLAPS) ;
      }
      return ITTT_NO ;
   }

  // limito la linea di intersezione con il primo triangolo
   Point3d ptSt1, ptEn1 ;
   int nRes1 = IntersCoplanarLineTria( ptL, vtL, 100.0, trTria1, ptSt1, ptEn1, false) ;

  // limito la linea di intersezione con il secondo triangolo
   Point3d ptSt2, ptEn2 ;
   int nRes2 = IntersCoplanarLineTria( ptL, vtL, 100.0, trTria2, ptSt2, ptEn2, false) ;

  // eseguo classificazione
   double dIntStU, dIntEnU ;
   int nIntType = FindTriaTriaIntersType( trTria1, trTria2, ptL, vtL,
                                          ( ptSt1 - ptL) * vtL, ( ptEn1 - ptL) * vtL,
                                          ( ptSt2 - ptL) * vtL, ( ptEn2 - ptL) * vtL,
                                          nRes1, nRes2, dIntStU, dIntEnU) ;
   if ( nIntType != ITTT_NO) {
      ptInt = ptL + dIntStU * vtL ;
      ptInt2 = ptL + dIntEnU * vtL ;
      return nIntType ;
   }
   return ITTT_NO ;
}

//----------------------------------------------------------------------------
int
IntersCoplanarTriaTria( const Triangle3d& trTria1, const Triangle3d& trTria2, TRIA3DVECTOR& vTria)
{
  // Se i tre vertici del secondo triangolo stanno tutti a destra di almeno un lato del primo, non c'� intersezione
   for ( int i = 0 ; i < 3 ; ++ i) {
      Vector3d vtSide = trTria1.GetP( ( i + 1) % 3) - trTria1.GetP( i) ;
      Vector3d vtSN = vtSide ^ trTria1.GetN() ;
      vtSN.Normalize() ;
      if ( ( trTria2.GetP( 0) - trTria1.GetP( i)) * vtSN > - EPS_TRIA_H  &&
           ( trTria2.GetP( 1) - trTria1.GetP( i)) * vtSN > - EPS_TRIA_H  &&
           ( trTria2.GetP( 2) - trTria1.GetP( i)) * vtSN > - EPS_TRIA_H)
         return ITTT_NO ;
   }
  // Se i tre vertici del primo triangolo stanno tutti a destra di almeno un lato del secondo, non c'� intersezione
   for ( int i = 0 ; i < 3 ; ++ i) {
      Vector3d vtSide = trTria2.GetP( ( i + 1) % 3) - trTria2.GetP( i) ;
      Vector3d vtSN = vtSide ^ trTria2.GetN() ;
      vtSN.Normalize() ;
      if ( ( trTria1.GetP( 0) - trTria2.GetP( i)) * vtSN > - EPS_TRIA_H  &&
           ( trTria1.GetP( 1) - trTria2.GetP( i)) * vtSN > - EPS_TRIA_H  &&
           ( trTria1.GetP( 2) - trTria2.GetP( i)) * vtSN > - EPS_TRIA_H)
         return ITTT_NO ;
   }

  // determino il piano medio dei due triangoli
   Plane3d plMed ;
   plMed.Set( ( trTria2.GetCentroid() + trTria2.GetCentroid()) / 2, trTria1.GetN() + trTria2.GetN()) ;

  // creo la regione equivalente al primo triangolo
   SurfFlatRegion sfrTria1 ;
   PtrOwner<CurveComposite> pCcTria1( CreateBasicCurveComposite()) ;
   if ( IsNull( pCcTria1))
      return ITTT_NO ;
   pCcTria1->AddPoint( ProjectPointOnPlane( trTria1.GetP( 0), plMed)) ;
   pCcTria1->AddLine( ProjectPointOnPlane( trTria1.GetP( 1), plMed)) ;
   pCcTria1->AddLine( ProjectPointOnPlane( trTria1.GetP( 2), plMed)) ;
   pCcTria1->Close() ;
   if ( ! sfrTria1.AddExtLoop( Release( pCcTria1)))
      return ITTT_NO ;

  // creo la regione equivalente al secondo triangolo
   SurfFlatRegion sfrTria2 ;
   PtrOwner<CurveComposite> pCcTria2( CreateBasicCurveComposite()) ;
   if ( IsNull( pCcTria2))
      return ITTT_NO ;
   pCcTria2->AddPoint( ProjectPointOnPlane( trTria2.GetP( 0), plMed)) ;
   pCcTria2->AddLine( ProjectPointOnPlane( trTria2.GetP( 1), plMed)) ;
   pCcTria2->AddLine( ProjectPointOnPlane( trTria2.GetP( 2), plMed)) ;
   pCcTria2->Close() ;
   if ( ! sfrTria2.AddExtLoop( Release( pCcTria2)))
      return ITTT_NO ;
   if ( sfrTria1.GetNormVersor() * sfrTria2.GetNormVersor() < 0)
      sfrTria2.Invert() ;

  // calcolo l'intersezione tra le due regioni
   if ( ! sfrTria1.Intersect( sfrTria2)  ||  ! sfrTria1.IsValid())
      return ITTT_NO ;

  // recupero il contorno esterno del risultato come polilinea
   PolyLine PL ;
   PtrOwner<ICurve> pLoop( sfrTria1.GetLoop( 0, 0)) ;
   if ( IsNull( pLoop)  ||  pLoop->GetType() != CRV_COMPO)
      return ITTT_NO ;
   ICurveComposite* pCoLoop = GetBasicCurveComposite( pLoop) ;
   for ( int i = 0 ; i < pCoLoop->GetCurveCount() ; ++ i) {
      const ICurve* pCrv = pCoLoop->GetCurve( i) ;
      if ( i == 0) {
         Point3d ptS ; pCrv->GetStartPoint( ptS) ;
         PL.AddUPoint( i, ptS) ;
      }
      Point3d ptE ; pCrv->GetEndPoint( ptE) ;
      PL.AddUPoint( i+1, ptE) ;
   }

  // eseguo una triangolazione del contorno chiuso
   PNTVECTOR vPnt ;
   INTVECTOR vTrVert ;
   Triangulate Tri ;
   if ( ! Tri.Make( PL, vPnt, vTrVert))
      return ITTT_NO ;
   int nTrVert = int( vTrVert.size()) / 3 ;
   for ( int i = 0 ; i < nTrVert ; ++i) {
      Triangle3d Tria ;
      Tria.Set( vPnt[vTrVert[3*i]], vPnt[vTrVert[3*i+1]], vPnt[vTrVert[3*i+2]]) ;
      if ( Tria.Validate( true))
         vTria.emplace_back( Tria) ;
   }

   return ITTT_OVERLAPS ;
}

//----------------------------------------------------------------------------
int
FindTriaTriaIntersType( const Triangle3d& trTria1, const Triangle3d& trTria2, const Point3d& ptLineSt, const Vector3d& vtLineDir,
                        double dStU1, double dEnU1, double dStU2, double dEnU2, int nRes1, int nRes2, double& dIntStU, double& dIntEnU)
{
  // Controllo su validità input
   if ( ! ( trTria1.IsValid()  &&  trTria2.IsValid()  &&  vtLineDir.IsNormalized()))
      return ITTT_NO ;
  // Casi
   switch ( nRes1) {
   case ILTT_SEGM :
      if ( nRes2 == ILTT_SEGM) {
         if ( dEnU2 < dStU1 - EPS_SMALL)
            return ITTT_NO ;
         else if ( dEnU2 < dStU1 + EPS_SMALL) {
            dIntStU = ( dStU1 + dEnU2) / 2 ;
            dIntEnU = dIntStU ;
            Point3d ptInt = ptLineSt + dIntStU * vtLineDir ;
            bool bOnVert1 = IsPointOnTriangleVertex( ptInt, trTria1) ;
            bool bOnVert2 = IsPointOnTriangleVertex( ptInt, trTria2) ;
            if ( bOnVert1  &&  bOnVert2)
               return ITTT_VERT_VERT ;
            else if ( bOnVert1)
               return ITTT_VERT_EDGE ;
            else if ( bOnVert2)
               return ITTT_EDGE_VERT ;
            else 
               return ITTT_EDGE_EDGE_PNT ;
         }
         else if ( dStU2 < dEnU1 - EPS_SMALL) {
            dIntStU = max( dStU1, dStU2) ;
            dIntEnU = min( dEnU1, dEnU2) ;
            return ITTT_INT_INT_SEG ;
         }
         else if ( dStU2 < dEnU1 + EPS_SMALL) {
            dIntStU = ( dStU2 + dEnU1) / 2 ;
            dIntEnU = dIntStU ;
            Point3d ptInt = ptLineSt + dIntStU * vtLineDir ;
            bool bOnVert1 = IsPointOnTriangleVertex( ptInt, trTria1) ;
            bool bOnVert2 = IsPointOnTriangleVertex( ptInt, trTria2) ;
            if ( bOnVert1  &&  bOnVert2)
               return ITTT_VERT_VERT ;
            else if ( bOnVert1)
               return ITTT_VERT_EDGE ;
            else if ( bOnVert2)
               return ITTT_EDGE_VERT ;
            else
               return ITTT_EDGE_EDGE_PNT ;
         }
         else
            return ITTT_NO ;
      }
      else if ( nRes2 == ILTT_SEGM_ON_EDGE) {
         if ( dEnU2 < dStU1 - EPS_SMALL)
            return ITTT_NO ;
         else if ( dEnU2 < dStU1 + EPS_SMALL) {
            dIntStU = ( dStU1 + dEnU2) / 2 ;
            dIntEnU = dIntStU ;
            Point3d ptInt = ptLineSt + dIntStU * vtLineDir ;
            bool bOnVert1 = IsPointOnTriangleVertex( ptInt, trTria1) ;
            return ( bOnVert1 ? ITTT_VERT_VERT : ITTT_EDGE_VERT) ;
         }
         else if ( dStU2 < dEnU1 - EPS_SMALL) {
            dIntStU = max( dStU1, dStU2) ;
            dIntEnU = min( dEnU1, dEnU2) ;
            return ITTT_INT_EDGE ;
         }
         else if ( dStU2 < dEnU1 + EPS_SMALL) {
            dIntStU = ( dStU2 + dEnU1) / 2 ;
            dIntEnU = dIntStU ;
            Point3d ptInt = ptLineSt + dIntStU * vtLineDir ;
            bool bOnVert1 = IsPointOnTriangleVertex( ptInt, trTria1) ;
            return ( bOnVert1 ? ITTT_VERT_VERT : ITTT_EDGE_VERT) ;
         }
         else
            return ITTT_NO ;
      }
      else if ( nRes2 == ILTT_VERT) {
         dEnU2 = dStU2 ;
         if ( dStU2 < dStU1 - EPS_SMALL)
            return ITTT_NO ;
         else if ( dStU2 < dStU1 + EPS_SMALL) {
            dIntStU = ( dStU1 + dStU2) / 2 ;
            dIntEnU = dIntStU ;
            Point3d ptInt = ptLineSt + dIntStU * vtLineDir ;
            bool bOnVert1 = IsPointOnTriangleVertex( ptInt, trTria1) ;
            return ( bOnVert1 ? ITTT_VERT_VERT : ITTT_EDGE_VERT) ;
         }
         else if ( dStU2 < dEnU1 - EPS_SMALL) {
            dIntStU = max( dStU1, dStU2) ;
            dIntEnU = min( dEnU1, dEnU2) ;
            return ITTT_INT_VERT ;
         }
         else if ( dStU2 < dEnU1 + EPS_SMALL) {
            dIntStU = ( dStU2 + dEnU1) / 2 ;
            dIntEnU = dIntStU ;
            Point3d ptInt = ptLineSt + dIntStU * vtLineDir ;
            bool bOnVert1 = IsPointOnTriangleVertex( ptInt, trTria1) ;
            return ( bOnVert1 ? ITTT_VERT_VERT : ITTT_EDGE_VERT) ;
         }
         else
            return ITTT_NO ;
      }
      else
         return ITTT_NO ;
      break ;

   case ILTT_SEGM_ON_EDGE :
      if ( nRes2 == ILTT_SEGM) {
         if ( dEnU2 < dStU1 - EPS_SMALL)
            return ITTT_NO ;
         else if ( dEnU2 < dStU1 + EPS_SMALL) {
            dIntStU = ( dStU1 + dEnU2) /2 ;
            dIntEnU = dIntStU ;
            Point3d ptInt = ptLineSt + dIntStU * vtLineDir ;
            bool bOnVert2 = IsPointOnTriangleVertex( ptInt, trTria2) ;
            return ( bOnVert2 ? ITTT_VERT_VERT : ITTT_VERT_EDGE) ;
         }
         else if ( dStU2 < dEnU1 - EPS_SMALL) {
            dIntStU = max( dStU1, dStU2) ;
            dIntEnU = min( dEnU1, dEnU2) ;
            return ITTT_EDGE_INT ;
         }
         else if ( dStU2 < dEnU1 + EPS_SMALL) {
            dIntStU = ( dStU2 + dEnU1) / 2 ;
            dIntEnU = dIntStU ;
            Point3d ptInt = ptLineSt + dIntStU * vtLineDir ;
            bool bOnVert2 = IsPointOnTriangleVertex( ptInt, trTria2) ;
            return ( bOnVert2 ? ITTT_VERT_VERT : ITTT_VERT_EDGE) ;
         }
         else
            return ITTT_NO ;    
      }
      else if ( nRes2 == ILTT_SEGM_ON_EDGE) {
         if ( dEnU2 < dStU1 - EPS_SMALL)
            return ITTT_NO ;
         else if ( dEnU2 < dStU1 + EPS_SMALL) {
            dIntStU = ( dStU1 + dEnU2) / 2 ;
            dIntEnU = dIntStU ;
            return ITTT_VERT_VERT ;
         }
         else if ( dStU2 < dEnU1 - EPS_SMALL) {
            dIntStU = max( dStU1, dStU2) ;
            dIntEnU = min( dEnU1, dEnU2) ;
            return ITTT_EDGE_EDGE_SEG ;
         }
         else if ( dStU2 < dEnU1 + EPS_SMALL) {
            dIntStU = ( dStU2 + dEnU1) / 2 ;
            dIntEnU = dIntStU ;
            return ITTT_VERT_VERT ;
         }
         else
            return ITTT_NO ;
      }
      else if ( nRes2 == ILTT_VERT) {
         dEnU2 = dStU2 ;
         if ( dEnU2 < dStU1 - EPS_SMALL)
            return ITTT_NO ;
         else if ( dEnU2 < dStU1 + EPS_SMALL) {
            dIntStU = ( dStU1 + dEnU2) / 2 ;
            dIntEnU = dIntStU ;
            return ITTT_VERT_VERT ;
         }
         else if ( dStU2 < dEnU1 - EPS_SMALL) {
            dIntStU = max( dStU1, dStU2) ;
            dIntEnU = min( dEnU1, dEnU2) ;
            return ITTT_EDGE_VERT ;
         }
         else if ( dStU2 < dEnU1 + EPS_SMALL) {
            dIntStU = ( dStU2 + dEnU1) / 2 ;
            dIntEnU = dIntStU ;
            return ITTT_VERT_VERT ;
         }
         else
            return ITTT_NO ;  
      }
      else
         return ITTT_NO ;
      break ;

   case ILTT_VERT :
      dEnU1 = dStU1 ;
      if ( nRes2 == ILTT_SEGM) {
         if ( dEnU2 < dStU1 - EPS_SMALL)
            return ITTT_NO ;
         else if ( dEnU2 < dStU1 + EPS_SMALL) {
            dIntStU = ( dStU1 + dEnU2) / 2 ;
            dIntEnU = dIntStU ;
            Point3d ptInt = ptLineSt + dIntStU * vtLineDir ;
            bool bOnVert2 = IsPointOnTriangleVertex( ptInt, trTria2) ;
            return ( bOnVert2 ? ITTT_VERT_VERT : ITTT_VERT_EDGE) ;
         }
         else if ( dStU2 < dEnU1 - EPS_SMALL) {
            dIntStU = max( dStU1, dStU2) ;
            dIntEnU = min( dEnU1, dEnU2) ;
            return ITTT_VERT_INT ;
         }
         else if ( dStU2 < dEnU1 + EPS_SMALL) {
            dIntStU = ( dStU2 + dEnU1) / 2 ;
            dIntEnU = dIntStU ;
            Point3d ptInt = ptLineSt + dIntStU * vtLineDir ;
            bool bOnVert2 = IsPointOnTriangleVertex( ptInt, trTria2) ;
            return ( bOnVert2 ? ITTT_VERT_VERT : ITTT_VERT_EDGE) ;
         }
         else
            return ITTT_NO ;
      }
      else if ( nRes2 == ILTT_SEGM_ON_EDGE) {
         if ( dEnU2 < dStU1 - EPS_SMALL)
            return ITTT_NO ;
         else if ( dEnU2 < dStU1 + EPS_SMALL) {
            dIntStU = ( dStU1 + dEnU2) / 2 ;
            dIntEnU = dIntStU ;
            return ITTT_VERT_VERT ;
         }
         else if ( dStU2 < dEnU1 - EPS_SMALL) {
            dIntStU = max( dStU1, dStU2) ;
            dIntEnU = min( dEnU1, dEnU2) ;
            return ITTT_VERT_EDGE ;
         }
         else if ( dStU2 < dEnU1 + EPS_SMALL) {
            dIntStU = ( dStU1 + dStU2) / 2 ;
            dIntEnU = dIntStU ;
            return ITTT_VERT_VERT ;
         }
         else
            return ITTT_NO ;
      }
      else if ( nRes2 == ILTT_VERT) {
         if ( dStU2 < dStU1 - EPS_SMALL)
            return ITTT_NO ;
         else if ( dStU2 < dStU1 + EPS_SMALL) {
            dIntStU = ( dStU1 + dStU2) / 2 ;
            dIntEnU = dIntStU ;
            return ITTT_VERT_VERT ;
         }
         else
            return ITTT_NO ;
      }
      else
         return ITTT_NO ;
      break ;

   case ILTT_EDGE :
      LOG_ERROR( GetEGkLogger(), "FindTriaTriaIntersType : ILTT_EDGE should never happen")
      break ;
   }

   return ITTT_NO ;
}