//----------------------------------------------------------------------------
//  EgalTech 2020-2020
//----------------------------------------------------------------------------
// File : CDeConvexTorusTria.cpp      Data : 18.11.20    Versione : 
// Contenuto : Implementazione funzione verifica collisione tra
//             toro convesso e SurfTriMesh.
//
// Modifiche : 18.11.20 LM Creazione modulo.
//
//
//----------------------------------------------------------------------------

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

using namespace std ;

//----------------------------------------------------------------------------
// Il toro convesso è il 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.
bool
CDeConvexTorusClosedSurfTm( const Frame3d& frTorus, double dRad1, double dRad2,
                            double dSafeDist, const ISurfTriMesh& Stm)
{
  // I raggi devono essere non nulli e la superficie ben definita.
   if ( dRad1 < EPS_SMALL  ||  dRad2 < EPS_SMALL  ||  ! Stm.IsValid())
      return false ;
  // 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, dSafeDist, trTria))
            return true ;
      }
   }
  // Se superficie aperta, non c'è collisione
   if ( ! Stm.IsClosed())
      return false ;
  // 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()) ;
}