//----------------------------------------------------------------------------
//  EgalTech 2017-2017
//----------------------------------------------------------------------------
// File : IntersPlaneSurfTm.cpp      Data : 16.10.17         Versione : 1.8j4
// Contenuto : Implementazione della intersezione piano/superficie trimesh.
//
//
//
// Modifiche : 16.10.17 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "ProjPlane.h"
#include "CurveLine.h"
#include "/EgtDev/Include/EGkDistPointLine.h"
#include "/EgtDev/Include/EGkIntersPlaneSurfTm.h"
#include "/EgtDev/Include/EGkIntersPlaneTria.h"
#include "/EgtDev/Include/EGkIntersLineSurfTm.h"
#include "/EgtDev/Include/EGkPointGrid3d.h"
#include "/EgtDev/Include/EGkHashGrids3d.h"
#include "/EgtDev/Include/EGkDistPointTria.h"
#include <array>

using namespace std ;

//----------------------------------------------------------------------------
static void 
UpdateIntersPlaneSurfTm( const Plane3d& plPlane, const Triangle3d& Tria, PNTVECTOR& vPnt, BIPNTVECTOR& vBpt, TRIA3DVECTOR& vTria, 
                         PointGrid3d& PtGrid, HashGrids3d& LnGrid, HashGrids3d& TrGrid) 
{ 
  // intersezione tra il piano e il triangolo
   Point3d ptInt, ptInt2 ;
   int nRes = IntersPlaneTria( plPlane, Tria, ptInt, ptInt2) ;
  // se vertice
   if ( nRes == IPTT_VERT) {
     // verifico se punto già inserito
      int nId ;
      if ( ! PtGrid.Find( ptInt, 10 * EPS_SMALL, nId)) {
         vPnt.emplace_back( ptInt) ;
         PtGrid.InsertPoint( ptInt, int( vPnt.size()) - 1) ;
      }
   }
  // se altrimenti segmento
   else if ( nRes == IPTT_EDGE  ||  nRes == IPTT_YES) {
     // se abbastanza lungo
      if ( ! AreSamePointApprox( ptInt, ptInt2)) {
        // verifico se già inserito
         bool bFound = false ;
         BBox3d b3Line( ptInt, ptInt2) ;
         INTVECTOR vnIds ;
         if ( LnGrid.Find( b3Line, vnIds)) {
            for ( int i = 0 ; i < int( vnIds.size()) ; ++ i) {
               int nA = vnIds[i] ;
               const Point3d& ptOth = vBpt[nA].first ;
               const Point3d& ptOth2 = vBpt[nA].second ;
               if ( ( AreSamePointEpsilon( ptInt, ptOth, 10 * EPS_SMALL)  &&
                  AreSamePointEpsilon( ptInt2, ptOth2, 10 * EPS_SMALL))  ||
                  ( AreSamePointEpsilon( ptInt, ptOth2, 10 * EPS_SMALL)  &&
                     AreSamePointEpsilon( ptInt2, ptOth, 10 * EPS_SMALL))) {
                  bFound = true ;    
                  break ;    
               }
            }
         }
        // se non inserito, procedo
         if ( ! bFound) {
            vBpt.emplace_back( ptInt, ptInt2) ;
            LnGrid.Add( int( vBpt.size()) - 1, b3Line) ;
            LnGrid.Update() ;
         }
      }
   }
  // se altrimenti l'intero triangolo
   else if ( nRes == IPTT_OVERLAPS) {
     // verifico se triangolo già inserito
      bool bFound = false ;
      BBox3d b3Tria( Tria.GetP( 0), Tria.GetP( 1)) ;
      b3Tria.Add( Tria.GetP( 2)) ;
      INTVECTOR vnIds ;
      if ( TrGrid.Find( b3Tria, vnIds)) {
         for ( int i = 0 ; i < int( vnIds.size()) ; ++ i) {
            int nA = vnIds[i] ;
            const Triangle3d& trOth = vTria[nA] ;
            array< bool, 3> bOth = { false, false, false} ;
            for ( int j = 0 ; j < 3 ; ++ j) {
               for ( int k = 0 ; k < 3 ; ++ k) {
                  if ( ! bOth[k])
                     bOth[k] = AreSamePointEpsilon( Tria.GetP( j), trOth.GetP( k), 10 * EPS_SMALL) ;
               }
            }
            if ( bOth[0]  &&  bOth[1]  &&  bOth[2]) {
               bFound = true ;    
               break ;    
            }
         }
      }
     // se non inserito, procedo
      if ( ! bFound) {
         vTria.emplace_back( Tria) ;
         TrGrid.Add( int( vTria.size()) - 1, b3Tria) ;
         TrGrid.Update() ;
      }
   }

   return ;
}

//----------------------------------------------------------------------------
static void 
AdjustIntersPlaneSurfTm( PNTVECTOR& vPnt, BIPNTVECTOR& vBpt, TRIA3DVECTOR& vTria,
                         const Plane3d& plPlane, const HashGrids3d& LnGrid, const HashGrids3d& TrGrid)
{
  // rimuovo i punti che stanno sui segmenti
   for ( int i = int( vPnt.size()) - 1 ; i >= 0 ; -- i) {
      bool bFound = false ;
      BBox3d b3Pnt( vPnt[i]) ;
      b3Pnt.Expand( 10 * EPS_SMALL) ;
      INTVECTOR vnIds ;
      if ( LnGrid.Find( b3Pnt, vnIds)) {
         for ( int j = 0 ; j < int( vnIds.size()) ; ++ j) {
            int nA = vnIds[j] ;
            if ( DistPointLine( vPnt[i], vBpt[nA].first, vBpt[nA].second).IsEpsilon( 10 * EPS_SMALL)) {
               bFound = true ;
               break ;
            }
         }
      }
      if ( bFound)
         vPnt.erase( vPnt.begin() + i) ;
   }

  // rimuovo i punti che stanno sui triangoli
   for ( int i = int( vPnt.size()) - 1 ; i >= 0 ; -- i) {
      bool bFound = false ;
      BBox3d b3Pnt( vPnt[i]) ;
      b3Pnt.Expand( 10 * EPS_SMALL) ;
      INTVECTOR vnIds ;
      if ( TrGrid.Find( b3Pnt, vnIds)) {
         for ( int j = 0 ; j < int( vnIds.size()) ; ++ j) {
            int nA = vnIds[j] ;
            const Triangle3d& trOth = vTria[nA] ;
            if ( DistPointTriangle( vPnt[i], trOth).IsEpsilon( 10 * EPS_SMALL)) {
               bFound = true ;
               break ;
            }
         }
      }
      if ( bFound)
         vPnt.erase( vPnt.begin() + i) ;
   }

  // rimuovo i segmenti che stanno sui triangoli
   for ( int i = int( vBpt.size()) - 1 ; i >= 0  ; -- i) {
      bool bFound = false ;
      Point3d ptStart = vBpt[i].first ;
      Point3d ptEnd = vBpt[i].second ;
      BBox3d b3Line( ptStart, ptEnd) ;
      INTVECTOR vnIds ;
      if ( TrGrid.Find( b3Line, vnIds)) {
         for ( int j = 0 ; j < int( vnIds.size()) ; ++ j) {
            int nA = vnIds[j] ;
            const Triangle3d& trOth = vTria[nA] ;
            if ( DistPointTriangle( ptStart, trOth).IsEpsilon( 10 * EPS_SMALL)  &&
               DistPointTriangle( ptEnd, trOth).IsEpsilon( 10 * EPS_SMALL)) {
               bFound = true ;
               break ;
            }
         }
      }
      if ( bFound)
         vBpt.erase( vBpt.begin() + i) ;
   }

  // porto i punti esattamente nel piano
   for ( int i = 0 ; i < int( vPnt.size()) ; ++ i) {
      vPnt[i] = ProjectPointOnPlane( vPnt[i], plPlane) ;
   }

  // porto i segmenti esattamente nel piano
   for ( int i = 0 ; i < int( vBpt.size()) ; ++ i) {
      vBpt[i].first = ProjectPointOnPlane( vBpt[i].first, plPlane) ;
      vBpt[i].second = ProjectPointOnPlane( vBpt[i].second, plPlane) ;
   }

  // porto i triangoli esattamente nel piano
   for ( int i = 0 ; i < int( vTria.size()) ; ++ i) {
      Triangle3d& trTria = vTria[i] ;
      trTria.SetP( 0, ProjectPointOnPlane( trTria.GetP( 0), plPlane)) ;
      trTria.SetP( 1, ProjectPointOnPlane( trTria.GetP( 1), plPlane)) ;
      trTria.SetP( 2, ProjectPointOnPlane( trTria.GetP( 2), plPlane)) ;
      trTria.Validate( true) ;
   }

   return ;
}

//----------------------------------------------------------------------------
//  Intersezione di un piano con una superficie TriMesh
//----------------------------------------------------------------------------
bool
IntersPlaneSurfTm( const Plane3d& plPlane, const ISurfTriMesh& Stm,
                   PNTVECTOR& vPnt, BIPNTVECTOR& vBpt, TRIA3DVECTOR& vTria)
{
  // verifico piano
   if ( &plPlane == nullptr  ||  plPlane.GetVersN().IsSmall())
      return false ;
  // verifico superficie
   if ( &Stm == nullptr  ||  ! Stm.IsValid())
      return false ;
  // verifico parametri di ritorno
   if ( &vPnt == nullptr  ||  &vBpt == nullptr  ||  &vTria == nullptr)
      return false ;
   vPnt.clear() ;
   vBpt.clear() ;
   vTria.clear() ;

  // per ricerca veloce di punti ripetuti
   PointGrid3d PtGrid ;
   PtGrid.Init( 100) ;

  // per ricerca veloce di linee ripetute
   HashGrids3d LnGrid ;
   LnGrid.SetActivationGrid( true) ;

  // per ricerca veloce di triangoli ripetuti
   HashGrids3d TrGrid ;
   TrGrid.SetActivationGrid( true) ;

  // cerco i triangoli intersecati dal piano
   Triangle3d Tria ;
   int nT = Stm.GetFirstTriangle( Tria) ;
   while ( nT != SVT_NULL) {
      UpdateIntersPlaneSurfTm( plPlane, Tria, vPnt, vBpt, vTria, PtGrid, LnGrid, TrGrid) ; 
      nT = Stm.GetNextTriangle( nT, Tria) ;
   }

   AdjustIntersPlaneSurfTm( vPnt, vBpt, vTria, plPlane, LnGrid, TrGrid) ;

   return true ;
}

//----------------------------------------------------------------------------
//  Intersezione di molti piani paralleli con una superficie TriMesh
//----------------------------------------------------------------------------
IntersParPlanesSurfTm::IntersParPlanesSurfTm( const Frame3d& frPlanes, const ISurfTriMesh& Stm)
   : m_bOk( false), m_frPlanes( frPlanes), m_pSTm( &Stm)
{
  // verifico esistenza superficie
   if ( m_pSTm == nullptr  ||  ! m_pSTm->IsValid())
      return ;

  // creo HashGrid 1d
   const int LIM_HG_TRIA = 127 ;
   m_HGrids.SetActivationGrid( m_pSTm->GetTriangleCount() > LIM_HG_TRIA) ;

  // riempio HashGrid 
   Triangle3d Tria ;
   int nT = Stm.GetFirstTriangle( Tria) ;
   while ( nT != SVT_NULL) {
     // calcolo il BBox del triangolo nel riferimento dei piani
      Tria.ToLoc( m_frPlanes) ;
      BBox3d b3Tria ;
      b3Tria.Add( Tria.GetP( 0)) ;
      b3Tria.Add( Tria.GetP( 1)) ;
      b3Tria.Add( Tria.GetP( 2)) ;
     // inserisco nella griglia
      if ( ! m_HGrids.Add( nT, b3Tria))
         return ;
     // passo al prossimo triangolo 
      nT = Stm.GetNextTriangle( nT, Tria) ;
   }
  // aggiorno
   m_bOk = m_HGrids.Update() ;
}

//----------------------------------------------------------------------------
bool
IntersParPlanesSurfTm::GetInters( double dDist, PNTVECTOR& vPnt, BIPNTVECTOR& vBpt, TRIA3DVECTOR& vTria) const
{
  // verifico validità
   if ( ! m_bOk)
      return false ;

  // verifico parametri di ritorno
   if ( &vPnt == nullptr  ||  &vBpt == nullptr  ||  &vTria == nullptr)
      return false ;
   vPnt.clear() ;
   vBpt.clear() ;
   vTria.clear() ;

  // per ricerca veloce di punti ripetuti
   PointGrid3d PtGrid ;
   PtGrid.Init( 100) ;

  // per ricerca veloce di linee ripetute
   HashGrids3d LnGrid ;
   LnGrid.SetActivationGrid( true) ;

  // per ricerca veloce di triangoli ripetuti
   HashGrids3d TrGrid ;
   TrGrid.SetActivationGrid( true) ;

  // calcolo il piano ( in globale)
   Point3d ptPl = m_frPlanes.Orig() + dDist * m_frPlanes.VersZ() ;
   Plane3d plPlane ;
   plPlane.Set( ptPl, m_frPlanes.VersZ()) ;

  // calcolo box del piano ( nel riferimento)
   BBox3d b3Plane ;
   b3Plane.Add( Point3d( 0, 0, dDist)) ;
   b3Plane.Expand( INFINITO - 1 , INFINITO - 1, 0) ;

  // recupero indici triangoli che intersecano box 
   INTVECTOR vnIds ;
   if ( m_HGrids.Find( b3Plane, vnIds)) {
      for ( int i = 0 ; i < int( vnIds.size()) ; ++ i) {
         int nT = vnIds[i] ;
         Triangle3d Tria ;
         m_pSTm->GetTriangle( nT, Tria) ;
         UpdateIntersPlaneSurfTm( plPlane, Tria, vPnt, vBpt, vTria, PtGrid, LnGrid, TrGrid) ;         
      }
   }

   AdjustIntersPlaneSurfTm( vPnt, vBpt, vTria, plPlane, LnGrid, TrGrid) ;

   return true ;
}