//----------------------------------------------------------------------------
//  EgalTech 2020-2024
//----------------------------------------------------------------------------
// File : CDeClosedSurfTmClosedSurfTm.h    Data : 24.03.24  Versione : 2.6c2 
// Contenuto : Implementazione funzione verifica collisione tra
//             SurfTm e SurfTm.
//
// Modifiche : 13.11.20 LM Creazione modulo.
//             24.03.24 DS Aggiunta TestSurfTmSurfTm.
//
//----------------------------------------------------------------------------

#include "stdafx.h"
#include "SurfTriMesh.h"
#include "CDeTriaTria.h"
#include "CDeSpheTria.h"
#include "CDeCylTria.h"
#include "/EgtDev/Include/EGkBBox3d.h"
#include "/EgtDev/Include/EGkCDeClosedSurfTmClosedSurfTm.h"
#include "/EgtDev/Include/EGkDistPointSurfTm.h"

using namespace std ;

//----------------------------------------------------------------------------
// Le due superfici devono essere espresse nel medesimo sistema di riferimento.
// La distanza di sicurezza ha effetto solo se maggiore di EPS_SMALL.
// Se necessario cerco la collisione con un offset della superficie B costituito
// da sfere centrate nei vertici, cilindri con gli spigoli per asse e il triangolo
// originale traslato della distanza di sicurezza lungo la sua normale.
// La funzione restituisce :
// - true in caso di collisione o inconsistenza dei parametri di input
// - false in caso di assenza di collisione.
//----------------------------------------------------------------------------
bool
CDeClosedSurfTmClosedSurfTm( const ISurfTriMesh& SurfA, const ISurfTriMesh& SurfB, double dSafeDist)
{
  // Recupero le superfici base
   const SurfTriMesh* pSrfA = GetBasicSurfTriMesh( &SurfA) ;
   const SurfTriMesh* pSrfB = GetBasicSurfTriMesh( &SurfB) ;
  // Se le superfici non sono valide o non sono chiuse, non ha senso proseguire
   if ( pSrfA == nullptr ||  ! pSrfA->IsValid()  ||  ! pSrfA->IsClosed()  ||
        pSrfB == nullptr ||  ! pSrfB->IsValid()  ||  ! pSrfB->IsClosed())
      return true ;
  // Se i box delle superfici non si intersecano, ho finito.
   BBox3d b3BoxA, b3BoxB ;
   pSrfA->GetLocalBBox( b3BoxA) ;
   pSrfB->GetLocalBBox( b3BoxB) ;
  // Se è necessario, espando il box di B di una costante additiva pari alla distanza di sicurezza.
   if ( dSafeDist > EPS_SMALL)
      b3BoxB.Expand( dSafeDist) ;
  // Se i box non si sovrappongono, non c'è collisione. Ho finito.
   if ( ! b3BoxA.Overlaps( b3BoxB))
      return false ;
  // Recupero i triangoli di A che interferiscono col box di B
   INTVECTOR vTriaIndex ;
   pSrfA->GetAllTriaOverlapBox( b3BoxB, vTriaIndex) ;
  // Ciclo sui triangoli della superficie A che interferiscono col box della superficie B.
   for ( int nTA : vTriaIndex) {
      Triangle3d trTriaA ;
      if ( ! ( pSrfA->GetTriangle( nTA, trTriaA)  &&  trTriaA.Validate()))
         continue ;
      BBox3d b3BoxTriaA ;
      trTriaA.GetLocalBBox( b3BoxTriaA) ;
     // Se è necessario, espando il box di una costante additiva pari alla distanza di sicurezza.
      if ( dSafeDist > EPS_SMALL)
         b3BoxTriaA.Expand( dSafeDist) ;
     // Recupero i triangoli di B che interferiscono col box del triangolo di A
      INTVECTOR vNearTria ;
      pSrfB->GetAllTriaOverlapBox( b3BoxTriaA, vNearTria) ;
     // Settare tutti i triangoli come già processati.
     // Al termine della chiamata i TempInt dei triangoli valgono 0.
      pSrfB->ResetTempInts() ;
     // Ciclo sui triangoli della superficie B che cadono nel box del triangolo corrente della Superficie A.
      for ( int nTB : vNearTria) {
        // Recupero il triangolo corrente della superficie B.
        // Se triangolo non valido salto al successivo.
         Triangle3d trTriaB ;
         if ( ! ( pSrfB->GetTriangle( nTB, trTriaB)  &&  trTriaB.Validate()))
            continue ;   
        // Se necessario considero l'offset
         if ( dSafeDist > EPS_SMALL) {
            int nAdjTriaId[3] ;
            pSrfB->GetTriangleAdjacencies( nTB, nAdjTriaId) ;
           // Ciclo sui vertici del triangolo.
            for ( int nVB = 0 ; nVB < 3 ; ++ nVB) {
              // Se il triangolo adiacente al triangolo corrente su questo edge
              // non è stato processato, processo il vertice e l'edge.
               int nAdjTriaTempFlag ;
               if ( ! ( pSrfB->GetTempInt( nAdjTriaId[nVB], nAdjTriaTempFlag)  ||  nAdjTriaTempFlag == 0))
                  continue ;
              // Processo il vertice: se c'è collisione fra triangolo A e sfera ho finito.
               if ( CDeSimpleSpheTria( trTriaB.GetP( nVB), dSafeDist, trTriaA))
                  return true ;
              // Processo l'edge: se c'è collisione fra triangolo A e cilindro ho finito.
               Vector3d vtEdgeV = trTriaB.GetP( ( nVB + 1) % 3) - trTriaB.GetP( nVB) ;
               double dEdgeLen = vtEdgeV.Len() ;
               vtEdgeV /= dEdgeLen ;
               Frame3d frCyl ;
               frCyl.Set( trTriaB.GetP( nVB), vtEdgeV) ;
               if ( CDeSimpleCylTria( frCyl, dSafeDist, dEdgeLen, trTriaA))
                  return true ;  
            }
           // Traslo il triangolo
            trTriaB.Translate( dSafeDist * trTriaB.GetN()) ;
         }
        // Processo il triangolo: se i due triangoli collidono ho finito.
         if ( CDeTriaTria( trTriaA, trTriaB))
            return true ;
        // Segno il triangolo come processato: nTemp = 1
         pSrfB->SetTempInt( nTB, 1) ;
      }
   }
  // Non ho trovato collisioni fra triangoli delle superfici.
  // Se il BBox della prima superficie non è interno a quello della seconda e viceversa, non c'è collisione
   if ( ! b3BoxA.Encloses( b3BoxB)  &&  ! b3BoxB.Encloses( b3BoxA))
      return false ;
  // La collisione c'è se una superficie è dentro l'altra.
   Point3d ptPointA, ptPointB ;
   pSrfA->GetFirstVertex( ptPointA) ;
   pSrfB->GetFirstVertex( ptPointB) ;
   DistPointSurfTm DistPoinASrfB( ptPointA, *pSrfB) ;
   DistPointSurfTm DistPoinBSrfA( ptPointB, *pSrfA) ;
   return ( DistPoinASrfB.IsPointInside()  ||  DistPoinBSrfA.IsPointInside()) ;
}

//----------------------------------------------------------------------------
// Verifica l'interferenza tra le due superfici : restituisce true in caso di interferenza.
//----------------------------------------------------------------------------
bool
TestSurfTmSurfTm( const ISurfTriMesh& SurfA, const ISurfTriMesh& SurfB, double dSafeDist)
{
  // Recupero le superfici base
   const SurfTriMesh* pSrfA = GetBasicSurfTriMesh( &SurfA) ;
   const SurfTriMesh* pSrfB = GetBasicSurfTriMesh( &SurfB) ;
  // Se le superfici non sono valide, non ha senso proseguire
   if ( pSrfA == nullptr ||  ! pSrfA->IsValid()  ||
        pSrfB == nullptr ||  ! pSrfB->IsValid())
      return true ;
  // Se i box delle superfici non si intersecano, ho finito.
   BBox3d b3BoxA, b3BoxB ;
   pSrfA->GetLocalBBox( b3BoxA) ;
   pSrfB->GetLocalBBox( b3BoxB) ;
  // Se è necessario, espando il box di B di una costante additiva pari alla distanza di sicurezza.
   if ( dSafeDist > EPS_SMALL)
      b3BoxB.Expand( dSafeDist) ;
  // Se i box non si sovrappongono, non c'è collisione. Ho finito.
   if ( ! b3BoxA.Overlaps( b3BoxB))
      return false ;
  // Recupero i triangoli di A che interferiscono col box di B
   INTVECTOR vTriaIndex ;
   pSrfA->GetAllTriaOverlapBox( b3BoxB, vTriaIndex) ;
  // Ciclo sui triangoli della superficie A che interferiscono col box della superficie B.
   for ( int nTA : vTriaIndex) {
      Triangle3d trTriaA ;
      if ( ! ( pSrfA->GetTriangle( nTA, trTriaA)  &&  trTriaA.Validate()))
         continue ;
      BBox3d b3BoxTriaA ;
      trTriaA.GetLocalBBox( b3BoxTriaA) ;
     // Se è necessario, espando il box di una costante additiva pari alla distanza di sicurezza.
      if ( dSafeDist > EPS_SMALL)
         b3BoxTriaA.Expand( dSafeDist) ;
     // Recupero i triangoli di B che interferiscono col box del triangolo di A
      INTVECTOR vNearTria ;
      pSrfB->GetAllTriaOverlapBox( b3BoxTriaA, vNearTria) ;
     // Settare tutti i triangoli come già processati.
     // Al termine della chiamata i TempInt dei triangoli valgono 0.
      pSrfB->ResetTempInts() ;
     // Ciclo sui triangoli della superficie B che cadono nel box del triangolo corrente della Superficie A.
      for ( int nTB : vNearTria) {
        // Recupero il triangolo corrente della superficie B.
        // Se triangolo non valido salto al successivo.
         Triangle3d trTriaB ;
         if ( ! ( pSrfB->GetTriangle( nTB, trTriaB)  &&  trTriaB.Validate()))
            continue ;   
        // Se necessario considero l'offset
         if ( dSafeDist > EPS_SMALL) {
            int nAdjTriaId[3] ;
            pSrfB->GetTriangleAdjacencies( nTB, nAdjTriaId) ;
           // Ciclo sui vertici del triangolo.
            for ( int nVB = 0 ; nVB < 3 ; ++ nVB) {
              // Se il triangolo adiacente al triangolo corrente su questo edge
              // non è stato processato, processo il vertice e l'edge.
               int nAdjTriaTempFlag ;
               if ( ! ( pSrfB->GetTempInt( nAdjTriaId[nVB], nAdjTriaTempFlag)  ||  nAdjTriaTempFlag == 0))
                  continue ;
              // Processo il vertice: se c'è collisione fra triangolo A e sfera ho finito.
               if ( CDeSimpleSpheTria( trTriaB.GetP( nVB), dSafeDist, trTriaA))
                  return true ;
              // Processo l'edge: se c'è collisione fra triangolo A e cilindro ho finito.
               Vector3d vtEdgeV = trTriaB.GetP( ( nVB + 1) % 3) - trTriaB.GetP( nVB) ;
               double dEdgeLen = vtEdgeV.Len() ;
               vtEdgeV /= dEdgeLen ;
               Frame3d frCyl ;
               frCyl.Set( trTriaB.GetP( nVB), vtEdgeV) ;
               if ( CDeSimpleCylTria( frCyl, dSafeDist, dEdgeLen, trTriaA))
                  return true ;  
            }
           // Traslo il triangolo
            trTriaB.Translate( dSafeDist * trTriaB.GetN()) ;
         }
        // Processo il triangolo: se i due triangoli collidono ho finito.
         if ( CDeTriaTria( trTriaA, trTriaB))
            return true ;
        // Segno il triangolo come processato: nTemp = 1
         pSrfB->SetTempInt( nTB, 1) ;
      }
   }
  // Non c'è interferenza
   return false ;
}