//----------------------------------------------------------------------------
//  EgalTech 2020-2020
//----------------------------------------------------------------------------
// File :      CDeSurfTmSurfTm.h      Data : 13.11.20    Versione : 
// Contenuto : Implementazione funzione verifica collisione tra
//             SurfTm e SurfTm.
//
// Modifiche : 13.11.20 LM Creazione modulo.
//
//
//----------------------------------------------------------------------------

#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"
#include <unordered_set>
#include <utility>

using namespace std ;

//----------------------------------------------------------------------------
// La funzione restituisce true in caso di collisone, false in caso di assenza
// di collisione o inconsistenza dei parametri di input.
// 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 i segmenti per asse e il triangolo
// originale traslato di una costante pari alla distanza di sicurezza lungo la
// sua normale.
bool
CDeClosedSurfTmClosedSurfTm( const SurfTriMesh& SurfA, const SurfTriMesh& SurfB, double dSafeDist)
{
  // Se le superfici non sono valide o non sono chiuse, non ha senso proseguire.
   if ( ! ( SurfA.IsValid()  &&  SurfB.IsValid())  ||  ! ( SurfA.IsClosed()  &&  SurfB.IsClosed()))
      return false ;
  // Se i box delle superfici non si intersecano, ho finito.
   BBox3d b3BoxA, b3BoxB ;
   SurfA.GetLocalBBox( b3BoxA) ;
   SurfB.GetLocalBBox( b3BoxB) ;
  // Se è necessario, espando il box di una costante additiva pari alla distanza di sicurezza.
   if ( dSafeDist > EPS_SMALL)
      b3BoxA.Expand( dSafeDist) ;
  // Se i box non si sovrappongono, non c'è collisione. Ho finito.
   if ( ! b3BoxA.Overlaps( b3BoxB))
      return false ;
  // Recupero i triangoli di B che interferiscono col box del triangolo di A
   INTVECTOR vTriaIndex ;
   SurfA.GetAllTriaOverlapBox( b3BoxB, vTriaIndex) ;
  // Ciclo sui triangoli della superficie A che cadono nel box della superficie B.
   for ( int nTA : vTriaIndex) {
      Triangle3d trTriaA ;
      if ( ! ( SurfA.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 ;
      SurfB.GetAllTriaOverlapBox( b3BoxTriaA, vNearTria) ;
     // Settare tutti i triangoli come già processati.
     // Al termine della chiamata i TFlags dei triangoli valgono 0.
      SurfB.ResetTempInt() ;
     // 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 ( ! ( SurfB.GetTriangle( nTB, trTriaB)  &&  trTriaB.Validate()))
            continue ;   
        // Se necessario considero l'offset
         if ( dSafeDist > EPS_SMALL) {
            int nAdjTriaId[3] ;
            SurfB.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 ( ! ( SurfB.GetTriangleTempInt( 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) - trTriaB.GetP( ( nVB + 1) % 3) ;
               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: nTFlag = 1
         SurfB.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 ;
   SurfA.GetFirstVertex( ptPointA) ;
   SurfB.GetFirstVertex( ptPointB) ;
   DistPointSurfTm DistPoinASurfB( ptPointA, SurfB) ;
   DistPointSurfTm DistPoinBSurfA( ptPointB, SurfA) ;
   return ( DistPoinASurfB.IsPointInside()  ||  DistPoinBSurfA.IsPointInside()) ;
}