//----------------------------------------------------------------------------
//  EgalTech 2020-2024
//----------------------------------------------------------------------------
// File : CDeConeFrustumClosedSurfTm.cpp  Data : 24.031.24  Versione : 2.6c2 
// Contenuto : Implementazione della verifica di collisione tra
//             Cone e Closed SurftriMesh.
//
//
// Modifiche : 09.11.20 LM Creazione modulo.
//             24.03.24 DS Aggiunta TestConeFrustumSurfTm.
//
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "CDeConeFrustumTria.h"
#include "/EgtDev/Include/EGkCDeConeFrustumClosedSurfTm.h"
#include "/EgtDev/Include/EGkDistPointSurfTm.h"

using namespace std ;

//----------------------------------------------------------------------------
// Il sistema di riferimento deve avere l'asse di simmetria del cono come asse Z e origine nel centro della base.
// La distanza di sicurezza ha effetto solo se maggiore di EPS_SMALL, altrimenti è ignorata ed è ininfluente.
// Il sistema di riferimento del cono è riferito a quello della superficie.
// La funzione restituisce :
// - true in caso di collisione o inconsistenza dei parametri di input
// - false in caso di assenza di collisione.
//----------------------------------------------------------------------------
bool
CDeConeFrustumClosedSurfTm( const Frame3d& frCone, double dBaseRad, double dTopRad, double dHeight,
                            const ISurfTriMesh& Stm, double dSafeDist)
{
  // Se il tronco di cono non è ben definito non ha senso proseguire
   if ( max( dBaseRad, dTopRad) < EPS_SMALL  ||  dHeight < EPS_SMALL)
      return true ;
  // Se superficie non valida o aperta, non ha senso proseguire
   if ( ! Stm.IsValid()  ||  ! Stm.IsClosed())
      return true ;
  // Recupero BBox della trimesh
   BBox3d b3Surf = Stm.GetAllTriaBox() ;
  // Calcolo il BBox del tronco di cono
   double dMaxRad = max( dBaseRad, dTopRad) ;
   BBox3d b3ConeL( Point3d( -dMaxRad, -dMaxRad, 0),
                   Point3d( dMaxRad, dMaxRad, dHeight)) ;
   if ( dSafeDist > EPS_SMALL)
      b3ConeL.Expand( dSafeDist) ;
   BBox3d b3Cone = GetToGlob( b3ConeL, frCone) ;
  // Se i BBox non interferiscono, non c'è collisione
   if ( ! b3Surf.Overlaps( b3Cone)  ||  ! b3Surf.Overlaps( frCone, b3ConeL))
      return false ;
  // Recupero i triangoli che interferiscono con il box del cono
   INTVECTOR vT ;
   Stm.GetAllTriaOverlapBox( b3Cone, vT) ;
  // Verifico se il tronco di cono interferisce con i triangoli del poliedro presenti nel suo BBox
   for ( int nT : vT) {
      Triangle3d trTria ;
      if ( Stm.GetTriangle( nT, trTria)) {
         if ( CDeConeFrustumTria( frCone, dBaseRad, dTopRad, dHeight, trTria, dSafeDist))
            return true ;
      }
   }
  // Se il BBox del tronco di cono non è interno a quello del poliedro e viceversa, non c'è collisione
   if ( ! b3Surf.Encloses( b3Cone)  &&  ! b3Cone.Encloses( b3Surf))
      return false ;
  // Verifico se il tronco di cono è dentro la superficie tramite calcolo distanza minima.
   Point3d ptConeCen( 0, 0, dHeight / 2) ;
   ptConeCen.ToGlob( frCone) ;
   DistPointSurfTm DistConeCenSurfCalc( ptConeCen, Stm) ;
  // Se il tronco di cono è interno c'è collisione
   return DistConeCenSurfCalc.IsPointInside() ;
}

//----------------------------------------------------------------------------
// Verifica l'interferenza tra il tronco di cono e la superficie : restituisce true in caso di interferenza.
//----------------------------------------------------------------------------
bool
TestConeFrustumSurfTm( const Frame3d& frCone, double dBaseRad, double dTopRad, double dHeight,
                       const ISurfTriMesh& Stm, double dSafeDist)
{
  // Se il tronco di cono non è ben definito non ha senso proseguire
   if ( max( dBaseRad, dTopRad) < EPS_SMALL  ||  dHeight < EPS_SMALL)
      return true ;
  // Se superficie non valida, non ha senso proseguire
   if ( ! Stm.IsValid())
      return true ;
  // Recupero BBox della trimesh
   BBox3d b3Surf = Stm.GetAllTriaBox() ;
  // Calcolo il BBox del tronco di cono
   double dMaxRad = max( dBaseRad, dTopRad) ;
   BBox3d b3ConeL( Point3d( -dMaxRad, -dMaxRad, 0),
                   Point3d( dMaxRad, dMaxRad, dHeight)) ;
   if ( dSafeDist > EPS_SMALL)
      b3ConeL.Expand( dSafeDist) ;
   BBox3d b3Cone = GetToGlob( b3ConeL, frCone) ;
  // Se i BBox non interferiscono, non c'è collisione
   if ( ! b3Surf.Overlaps( b3Cone)  ||  ! b3Surf.Overlaps( frCone, b3ConeL))
      return false ;
  // Recupero i triangoli che interferiscono con il box del cono
   INTVECTOR vT ;
   Stm.GetAllTriaOverlapBox( b3Cone, vT) ;
  // Verifico se il tronco di cono interferisce con i triangoli del poliedro presenti nel suo BBox
   for ( int nT : vT) {
      Triangle3d trTria ;
      if ( Stm.GetTriangle( nT, trTria)) {
         if ( CDeConeFrustumTria( frCone, dBaseRad, dTopRad, dHeight, trTria, dSafeDist))
            return true ;
      }
   }
  // Non c'è interferenza
   return false ;
}