//----------------------------------------------------------------------------
//  EgalTech 2018-2018
//----------------------------------------------------------------------------
// File : EGkDistPointTria.h     Data : 11.12.18        Versione : 1.9l2
// Contenuto : Implementazione della classe distanza Punto da Trimesh.
//
//
//
// Modifiche : 07.12.18 LM Creazione modulo.
//
//
//----------------------------------------------------------------------------

#include "stdafx.h"
#include "/EgtDev/Include/EGkDistPointTria.h"
#include "/EgtDev/Include/EGkDistPointSurfTm.h"

using namespace std ;

//----------------------------------------------------------------------------
// Calcola la differenza fra i bounding-box A e B.
// L'insieme differenza non è un bounding-box, ma è esprimibile come unione di al più sei bounding-box.
// Se l'insieme differenza fra i box non ha misura nulla viene restituito true, false altrimenti.
// I casi in cui non vengono trovati box di misura positiva sono quelli in cui o il box A è contenuto
// nel box B; uno di questi si verifica se il box A è vuoto.
// Nel vettore vBoxDiff vengono restituiti i box la cui unione costituisce la differenza fra A e B.
bool 
BoundingBoxDifference( const BBox3d& boxA, const BBox3d& boxB, BOXVECTOR& vBoxDiff) 
{
  // Svuoto il risultato
   vBoxDiff.clear() ;
  // Se box A vuoto, risultato vuoto
   if ( boxA.IsEmpty())
      return false ;
  // Se box B vuoto o i box non si intersecano, risultato è ancora A
   BBox3d boxInt ;
   if ( boxB.IsSmall()  ||  ! boxA.FindIntersection( boxB, boxInt)) {
      vBoxDiff.emplace_back( boxA) ;
      return true ;  
   }
  // Recupero i punti estremi dei box A e Intersezione
   Point3d ptMinA, ptMaxA ; boxA.GetMinMax( ptMinA, ptMaxA) ;
   Point3d ptMinInt, ptMaxInt ; boxInt.GetMinMax( ptMinInt, ptMaxInt) ;
  // Sotto
   if ( ptMinInt.z - ptMinA.z > EPS_SMALL) {
      BBox3d boxD( ptMinA, Point3d( ptMaxA.x, ptMaxA.y, ptMinInt.z)) ;
      vBoxDiff.emplace_back( boxD) ;
   }
  // Sopra
   if ( ptMaxA.z - ptMaxInt.z > EPS_SMALL) {
      BBox3d boxD( Point3d( ptMinA.x, ptMinA.y, ptMaxInt.z), ptMaxA) ;
      vBoxDiff.emplace_back( boxD) ;
   }
  // Davanti
   if ( ptMinInt.y - ptMinA.y > EPS_SMALL) {
      BBox3d boxD( Point3d( ptMinA.x, ptMinA.y, ptMinInt.z), Point3d( ptMaxA.x, ptMinInt.y, ptMaxInt.z)) ;
      vBoxDiff.emplace_back( boxD) ;
   }
  // Dietro
   if ( ptMaxA.y - ptMaxInt.y > EPS_SMALL) {
      BBox3d boxD( Point3d( ptMinA.x, ptMaxInt.y, ptMinInt.z), Point3d( ptMaxA.x, ptMaxA.y, ptMaxInt.z)) ;
      vBoxDiff.emplace_back( boxD) ;
   }
  // Sinistra 
   if ( ptMinInt.x - ptMinA.x > EPS_SMALL) {
      BBox3d boxD( Point3d( ptMinA.x, ptMinInt.y, ptMinInt.z), Point3d( ptMinInt.x, ptMaxInt.y, ptMaxInt.z)) ;
      vBoxDiff.emplace_back( boxD) ;
   }
  // Destra
   if ( ptMaxA.y - ptMaxInt.y > EPS_SMALL) {
      BBox3d boxD( Point3d( ptMaxInt.x, ptMinInt.y, ptMinInt.z), Point3d( ptMaxA.x, ptMaxInt.y, ptMaxInt.z)) ;
      vBoxDiff.emplace_back( boxD) ;
   }
  // Risultato
   return ( ! vBoxDiff.empty()) ;
}

//----------------------------------------------------------------------------
DistPointSurfTm::DistPointSurfTm( const Point3d& ptP, const ISurfTriMesh& tmSurf)
{
  // Trimesh non valida 
   if ( &tmSurf == nullptr  ||  ! tmSurf.IsValid()) {
      m_dDist = - 1. ;
      return ;
   }
  // Calcolo la distanza
   Calculate( ptP, tmSurf) ;
}

//----------------------------------------------------------------------------
void 
DistPointSurfTm::Calculate( const Point3d& ptP, const ISurfTriMesh& tmSurf)
{
  // Inizializzo distanza non calcolata
   m_dDist = - 1. ;

  // Recupero e verifico il box locale della superficie
   BBox3d b3Stm = tmSurf.GetAllTriaBox() ;
   if ( b3Stm.IsEmpty())
      return ;

  // Determino i triangoli vicini e fra di essi cerco quello di minima distanza.
  // Considero un box centrato nel punto P; finché all'interno del box non trovo un set di triangoli
  // fra cui quello a distanza minima dal punto P ha distanza minore del minimo semi-lato del box,
  // continuo a ingrandire il box. La condizione di terminazione garantisce di trovare il tiangolo di
  // distanza minima della trimesh intera.
   Point3d ptMin, ptMax ; b3Stm.GetMinMax( ptMin, ptMax) ;
   double dDeltaLen = max( min( min( ptMax.x - ptMin.x, ptMax.y - ptMin.y), ptMax.z - ptMin.z) / 40., 10.) ;
   double dBoxHalfLenX = max( max( ptMin.x - ptP.x, ptP.x - ptMax.x), 0.) + dDeltaLen ;
   double dBoxHalfLenY = max( max( ptMin.y - ptP.y, ptP.y - ptMax.y), 0.) + dDeltaLen ;
   double dBoxHalfLenZ = max( max( ptMin.z - ptP.z, ptP.z - ptMax.z), 0.) + dDeltaLen ;
  // Considero anche il box precedente per poter analizzare solo lo spazio differenza tra i due
   BBox3d boxPPrev( ptP) ;
   BBox3d boxP( ptP, dBoxHalfLenX, dBoxHalfLenY, dBoxHalfLenZ) ;
  // Variabili distanza minima, indice del triangolo di distanza minima, punto di distanza minima 
   double dMinSqDist = DBL_MAX ;
   int nMinDistTriaIndex = SVT_NULL ;
   Point3d ptMinDistPoint ;
   bool bContinue = true ;
  // Finché non si verifica la condizione di terminazione ingrandisco il box.
   while ( bContinue) {
     // Calcolo il box differenza con il precedente per non esplorare parti già considerate
      BOXVECTOR vBox ;
      BoundingBoxDifference( boxP, boxPPrev, vBox) ;
     // Ciclo sui box differenza
      bool bCollide = false ;
      for ( const auto& b3Box : vBox) {
        // interseco il box con quello della superficie e ne verifico la distanza minima dal punto
         BBox3d b3Int ;
         if ( ! b3Box.FindIntersection( b3Stm, b3Int)  ||  b3Int.SqDistFromPoint( ptP) > dMinSqDist)
            continue ;
        // ricerca sui triangoli nel box
         bCollide = true ;
         INTVECTOR vnIds ; 
         if ( tmSurf.GetAllTriaOverlapBox( b3Int, vnIds)) { 
           // Ciclo sui triangoli del sotto-box corrente
            for ( auto nT : vnIds) {
               Triangle3d trCurTria ;
               tmSurf.GetTriangle( nT, trCurTria) ;
               DistPointTriangle distPT( ptP, trCurTria) ;
               double dCurSqDist ;
              // Se la distanza del triangolo è valida e minore di quella attuale aggiorno 
               if ( distPT.GetSqDist( dCurSqDist)  &&  dCurSqDist < dMinSqDist) {
                  dMinSqDist = dCurSqDist ;
                  nMinDistTriaIndex = nT ;
                  distPT.GetMinDistPoint( ptMinDistPoint) ;
               }
            }
         }
      }
     // Se si verifica la condizione di terminazione arresto il ciclo altrimenti aggiorno i box 
      if ( ! bCollide  ||  dMinSqDist < EPS_SMALL * EPS_SMALL)
         bContinue = false ;
      else {
        boxPPrev = boxP ;
        boxP.Expand( dDeltaLen) ;
      }
   }

   if ( nMinDistTriaIndex != SVT_NULL) {
      m_dDist = sqrt( max( dMinSqDist, 0.)) ;
      m_nMinDistTriaIndex = nMinDistTriaIndex ;
      m_ptMinDistPoint = ptMinDistPoint ;
   }
}

//----------------------------------------------------------------------------
bool
DistPointSurfTm::GetDist( double& dDist)
{
  // Distanza non valida
   if ( m_dDist < -EPS_ZERO)
      return false ;
  // Distanza valida
   dDist = m_dDist ;
   return true ;
}

//----------------------------------------------------------------------------
bool 
DistPointSurfTm::GetMinDistPoint( Point3d& ptMinDistPoint)
{
  // Distanza non valida
   if ( m_dDist < -EPS_ZERO)
      return false ;
  // Distanza valida
   ptMinDistPoint = m_ptMinDistPoint ;
   return true ;
}

//----------------------------------------------------------------------------
bool
DistPointSurfTm::GetMinDistTriaIndex( int& nMinDistIndex)
{
  // Distanza non valida
   if ( m_dDist < -EPS_ZERO)
      return false ;
  // Distanza valida 
   nMinDistIndex = m_nMinDistTriaIndex ;
   return true ;
}