//----------------------------------------------------------------------------
//  EgalTech 2020-2024
//----------------------------------------------------------------------------
// File : CDeConvexTorusTria.cpp      Data : 24.03.24    Versione : 2.6c2
// Contenuto : Implementazione funzione verifica collisione tra
//             toro convesso e SurfTriMesh.
//
// Modifiche : 18.11.20 LM Creazione modulo.
//             24.03.24 DS Aggiunta TestConvexTorusSurfTm.
//
//----------------------------------------------------------------------------

#include "stdafx.h"
#include "CDeConvexTorusTria.h"
#include "/EgtDev/Include/EGkCDeConvexTorusClosedSurfTm.h"
#include "/EgtDev/Include/EGkDistPointSurfTm.h"

using namespace std ;

//----------------------------------------------------------------------------
// Il toro convesso è il solido a disco limitato dalla sola parte esterna del toro.
// Raggio principale R1, raggio secondario R2.
// Il toro è posto nel piano XY del suo riferimento, centrato sull'origine.
// La funzione restituisce :
// - true in caso di collisione o inconsistenza dei parametri di input
// - false in caso di assenza di collisione.
//----------------------------------------------------------------------------
bool
CDeConvexTorusClosedSurfTm( const Frame3d& frTorus, double dRad1, double dRad2,
                            const ISurfTriMesh& Stm, double dSafeDist)
{
  // I raggi devono essere non nulli
   if ( dRad1 < EPS_SMALL  ||  dRad2 < EPS_SMALL)
      return true ;
  // Se superficie non valida o aperta, non ha senso proseguire
   if ( ! Stm.IsValid()  ||  ! Stm.IsClosed())
      return true ;
  // Box della superficie
   BBox3d b3Surf = Stm.GetAllTriaBox() ;
  // Box del toro (sempre completo)
   BBox3d b3TorusL( Point3d( -dRad1 - dRad2, -dRad1 - dRad2, -dRad2),
                    Point3d(  dRad1 + dRad2,  dRad1 + dRad2,  dRad2)) ;
   if ( dSafeDist > EPS_SMALL)
      b3TorusL.Expand( dSafeDist) ;
   BBox3d b3Torus = GetToGlob( b3TorusL, frTorus) ;
  // Se i BBox non interferiscono, non c'è collisione
   if ( ! b3Surf.Overlaps( b3Torus)  ||  ! b3Surf.Overlaps( frTorus, b3TorusL))
      return false ;
  // Recupero i triangoli che interferiscono con il box del toro
   INTVECTOR vT ;
   Stm.GetAllTriaOverlapBox( b3Torus, vT) ;
  // Verifico se il toro interferisce con i triangoli del poliedro presenti nel suo BBox
   for ( int nT : vT) {
      Triangle3d trTria ;
      if ( Stm.GetTriangle( nT, trTria)) {
         if ( CDeConvexTorusTria( frTorus, dRad1, dRad2, CT_TOT, trTria, dSafeDist))
            return true ;
      }
   }
  // Se il BBox del toro non è interno a quello del poliedro e viceversa, non c'è collisione
   if ( ! b3Surf.Encloses( b3Torus)  &&  ! b3Torus.Encloses( b3Surf))
      return false ;
  // Verifico se il toro è dentro la superficie tramite calcolo distanza minima del suo centro
   Point3d ptTorusOrig = frTorus.Orig() ;
   DistPointSurfTm DistConeOrigSurfCalc( ptTorusOrig, Stm) ;
  // Se il toro è interno c'è collisione
   return ( DistConeOrigSurfCalc.IsPointInside()) ;
}

//----------------------------------------------------------------------------
// Verifica l'interferenza tra il toro convesso e la superficie : restituisce true in caso di interferenza
//----------------------------------------------------------------------------
bool
TestConvexTorusSurfTm( const Frame3d& frTorus, double dRad1, double dRad2,
                       const ISurfTriMesh& Stm, double dSafeDist)
{
  // I raggi devono essere non nulli
   if ( dRad1 < EPS_SMALL  ||  dRad2 < EPS_SMALL)
      return true ;
  // Se superficie non valida, non ha senso proseguire
   if ( ! Stm.IsValid())
      return true ;
  // Box della superficie
   BBox3d b3Surf = Stm.GetAllTriaBox() ;
  // Box del toro (sempre completo)
   BBox3d b3TorusL( Point3d( -dRad1 - dRad2, -dRad1 - dRad2, -dRad2),
                    Point3d(  dRad1 + dRad2,  dRad1 + dRad2,  dRad2)) ;
   if ( dSafeDist > EPS_SMALL)
      b3TorusL.Expand( dSafeDist) ;
   BBox3d b3Torus = GetToGlob( b3TorusL, frTorus) ;
  // Se i BBox non interferiscono, non c'è collisione
   if ( ! b3Surf.Overlaps( b3Torus)  ||  ! b3Surf.Overlaps( frTorus, b3TorusL))
      return false ;
  // Recupero i triangoli che interferiscono con il box del toro
   INTVECTOR vT ;
   Stm.GetAllTriaOverlapBox( b3Torus, vT) ;
  // Verifico se il toro interferisce con i triangoli del poliedro presenti nel suo BBox
   for ( int nT : vT) {
      Triangle3d trTria ;
      if ( Stm.GetTriangle( nT, trTria)) {
         if ( CDeConvexTorusTria( frTorus, dRad1, dRad2, CT_TOT, trTria, dSafeDist))
            return true ;
      }
   }
  // Non c'è interferenza
   return false ;
}