//----------------------------------------------------------------------------
//  EgalTech 2020-2024
//----------------------------------------------------------------------------
// File : CDeRectPrismoidClosedSurfTm.h    Data : 24.03.24  Versione : 2.6c2
// Contenuto : Implementazione funzione verifica collisione tra
//             Prismoide a basi rettangolari e Closed SurfTriMesh.
//
// Modifiche : 09.11.20 LM Creazione modulo.
//             24.03.24 DS Aggiunta TestRectPrismoidSurfTm.
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "CDeRectPrismoidTria.h"
#include "/EgtDev/Include/EGkCDeRectPrismoidClosedSurfTm.h"
#include "/EgtDev/Include/EGkDistPointSurfTm.h"

using namespace std ;

//----------------------------------------------------------------------------
// Il sistema di riferimento deve avere l'origine nel centro della Base, asse X lungo
// un segmento della stessa e asse Z ortogonale alle basi e diretta verso la base Top.
// Il sistema di riferimento della piramide deve essere immerso in 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
CDeRectPrismoidClosedSurfTm( const Frame3d& frPrismoid, double dLenghtBaseX, double dLenghtBaseY,
                             double dLenghtTopX, double dLenghtTopY, double dHeight,
                             const ISurfTriMesh& Stm, double dSafeDist)
{
  // Se il tronco di piramide non è definito non ha senso proseguire.
   if ( max( dLenghtBaseX, dLenghtTopX) < EPS_SMALL  ||
        max( dLenghtBaseY, dLenghtTopY) < 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 piramide
   double dMaxLenX = max( dLenghtBaseX, dLenghtTopX) ;
   double dMaxLenY = max( dLenghtBaseY, dLenghtTopY) ;
   BBox3d b3PyrL( Point3d( -dMaxLenX / 2, -dMaxLenY / 2, 0.),
                  Point3d(  dMaxLenX / 2,  dMaxLenY / 2, dHeight)) ;
   if ( dSafeDist > EPS_SMALL)
      b3PyrL.Expand( dSafeDist) ;
   BBox3d b3Pyr = GetToGlob( b3PyrL, frPrismoid) ;
  // Se i BBox non interferiscono, non c'è collisione
   if ( ! b3Surf.Overlaps( b3Pyr)  ||  ! b3Surf.Overlaps( frPrismoid, b3PyrL))
      return false ;
  // Recupero i triangoli che interferiscono con il box del tronco di piramide.
   INTVECTOR vT ;
   Stm.GetAllTriaOverlapBox( b3Pyr, vT) ;
  // Verifico se il tronco di piramide interferisce con i triangoli del poliedro presenti nel suo BBox
   for ( int nT : vT) {
      Triangle3d trTria ; 
      if ( Stm.GetTriangle( nT, trTria)) {
         if ( CDeRectPrismoidTria( frPrismoid, dLenghtBaseX, dLenghtBaseY, dLenghtTopX, dLenghtTopY, dHeight,
                                    trTria, dSafeDist))
            return true ;
      }
   }
  // Se il BBox del tronco di piramide non è interno a quello del poliedro e viceversa, non c'è collisione
   if ( ! b3Surf.Encloses( b3Pyr)  &&  ! b3Pyr.Encloses( b3Surf))
      return false ;
  // Verifico se il tronco di piramide è dentro la superficie tramite calcolo distanza minima del suo centro
   Point3d ptPyrCen( 0, 0, dHeight / 2) ;
   ptPyrCen.ToGlob( frPrismoid) ;
   DistPointSurfTm DistPyrCenSurfCalc( ptPyrCen, Stm) ;
  // C'è collisione se il tronco di piramide è interno
   return ( DistPyrCenSurfCalc.IsPointInside()) ;
}

//----------------------------------------------------------------------------
// Verifica l'interferenza tra il Prismoide a basi rettangolari e la superficie : restituisce true in caso di interferenza.
//----------------------------------------------------------------------------
bool
TestRectPrismoidSurfTm( const Frame3d& frPrismoid, double dLenghtBaseX, double dLenghtBaseY,
                        double dLenghtTopX, double dLenghtTopY, double dHeight,
                        const ISurfTriMesh& Stm, double dSafeDist)
{
  // Se il tronco di piramide non è definito non ha senso proseguire.
   if ( max( dLenghtBaseX, dLenghtTopX) < EPS_SMALL  ||
        max( dLenghtBaseY, dLenghtTopY) < 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 piramide
   double dMaxLenX = max( dLenghtBaseX, dLenghtTopX) ;
   double dMaxLenY = max( dLenghtBaseY, dLenghtTopY) ;
   BBox3d b3PyrL( Point3d( -dMaxLenX / 2, -dMaxLenY / 2, 0.),
                  Point3d(  dMaxLenX / 2,  dMaxLenY / 2, dHeight)) ;
   if ( dSafeDist > EPS_SMALL)
      b3PyrL.Expand( dSafeDist) ;
   BBox3d b3Pyr = GetToGlob( b3PyrL, frPrismoid) ;
  // Se i BBox non interferiscono, non c'è collisione
   if ( ! b3Surf.Overlaps( b3Pyr)  ||  ! b3Surf.Overlaps( frPrismoid, b3PyrL))
      return false ;
  // Recupero i triangoli che interferiscono con il box del tronco di piramide.
   INTVECTOR vT ;
   Stm.GetAllTriaOverlapBox( b3Pyr, vT) ;
  // Verifico se il tronco di piramide interferisce con i triangoli del poliedro presenti nel suo BBox
   for ( int nT : vT) {
      Triangle3d trTria ; 
      if ( Stm.GetTriangle( nT, trTria)) {
         if ( CDeRectPrismoidTria( frPrismoid, dLenghtBaseX, dLenghtBaseY, dLenghtTopX, dLenghtTopY, dHeight,
                                    trTria, dSafeDist))
            return true ;
      }
   }
  // Non c'è interferenza
   return false ;
}