//----------------------------------------------------------------------------
//  EgalTech 2023-2023
//----------------------------------------------------------------------------
// File : IntersLineCone.cpp        Data : 16.05.23          Versione : 2.5e3
// Contenuto : Implementazione della intersezione linea/tronco di cono.
//
//
//
// Modifiche : 16.05.23 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "IntersLineCone.h"
#include "IntersLineCyl.h"
#include "/EgtDev/Include/ENkPolynomialRoots.h"

using namespace std ;

//----------------------------------------------------------------------------
// Linea e tronco di cono sono nel medesimo riferimento.
// Il tronco di cono è centrato sull'asse Z e appoggiato con RMin sul piano XY.
// In caso di intersezione viene restituito true e i parametri in dU1 e dU2.
//----------------------------------------------------------------------------
bool
IntersLineCone( const Point3d& ptL, const Vector3d& vtL,
                double dRadMin, double dRadMax, double dHeight,
                double& dU1, double& dU2)
{
  // Verifico il versore
   if ( vtL.IsSmall())
      return false ;

  // Verifico il tronco di cono
   if ( ( dRadMin < EPS_SMALL  &&  dRadMax < EPS_SMALL)  ||  dHeight < EPS_SMALL)
      return false ;

  // Se è un cilindro, rimando a questo
   if ( abs( dRadMax - dRadMin) < EPS_SMALL)
      return IntersLineCyl( ptL, vtL, ( dRadMin + dRadMax) / 2, dHeight, dU1, dU2) ;

  // Se raggi invertiti, li scambio
   if ( dRadMin > dRadMax)
      swap( dRadMin, dRadMax) ;

   // Tangente dell'angolo di semi-apertura del cono
   double dTanTheta = ( dRadMax - dRadMin) / dHeight ;
   double dSqTanTheta = dTanTheta * dTanTheta ;
   double dSqCosTheta = 1 / ( 1 + dSqTanTheta) ;

   // Determino le eventuali intersezioni con le due basi a quota minima e massima (solo se linea non giace sul cono)
   int nBasInt = 0 ;
   if ( abs( vtL.z) > EPS_ZERO) {
     // le linee tangenti al cono non sono considerate intersecanti
      bool bSameHAng = ( abs( abs( vtL.x) - abs( vtL.y)) < EPS_SMALL  &&  abs( dSqCosTheta - vtL.z * vtL.z) < 2 * abs( vtL.z) * EPS_SMALL) ;
      double EpsRad = ( bSameHAng ? - EPS_SMALL : EPS_SMALL) ;
      Point3d ptInt1 = ptL + ( ( 0 - ptL.z) / vtL.z) * vtL ;
      if ( ptInt1.x * ptInt1.x + ptInt1.y * ptInt1.y < dRadMin * dRadMin + 2 * dRadMin * EpsRad) {
         dU1 = ( ptInt1 - ptL) * vtL ;
         nBasInt += 1 ;
      }
      Point3d ptInt2 = ptL + ( ( dHeight - ptL.z) / vtL.z) * vtL ;  
      if ( ptInt2.x * ptInt2.x + ptInt2.y * ptInt2.y < dRadMax * dRadMax + 2 * dRadMax * EpsRad) {
         dU2 = ( ptInt2 - ptL) * vtL ;
         nBasInt += 2 ;
      }
   }

  // Se la linea interseca entrambe le basi, si sono trovate le due intersezioni
   if ( nBasInt == 3) {
      if ( dU1 > dU2)
         swap( dU1, dU2) ;
     // Trovate intersezioni
      return true ;
   }

  // Posizione del vertice del cono
   double dDeltaH = ( dRadMin < EPS_SMALL ? 0 : dRadMin / dTanTheta) ;
  // Sposto il punto di passaggio della linea di conseguenza
   Point3d ptMyL = ptL + Z_AX * dDeltaH ;
   
  // Determino le intersezioni con la superficie laterale del cono
   DBLVECTOR vdCoeff{ ptMyL.x * ptMyL.x + ptMyL.y * ptMyL.y - ptMyL.z * ptMyL.z * dSqTanTheta,
                      2 * ( ptMyL.x * vtL.x + ptMyL.y * vtL.y - ptMyL.z * vtL.z * dSqTanTheta),
                      vtL.x * vtL.x + vtL.y * vtL.y - vtL.z * vtL.z * dSqTanTheta} ;
   DBLVECTOR vdRoots ;
   int nRoot = PolynomialRoots( 2, vdCoeff, vdRoots) ;

  // Elimino le soluzioni cha danno intersezioni fuori dai limiti in Z del tronco di cono
   if ( nRoot == 2) {
      double dIntZ2 = ptL.z + vdRoots[1] * vtL.z ;
      if ( dIntZ2 < 0 - EPS_SMALL  ||  dIntZ2 > dHeight + EPS_SMALL)
         -- nRoot ;
   }
   if ( nRoot >= 1) {
      double dIntZ1 = ptL.z + vdRoots[0] * vtL.z ;
      if ( dIntZ1 < 0 - EPS_SMALL  ||  dIntZ1 > dHeight + EPS_SMALL) {
         if ( nRoot == 2)
            vdRoots[0] = vdRoots[1] ;
         -- nRoot ;
      }
   }

  // Due soluzioni: la retta interseca due volte la superficie laterale
   if ( nRoot == 2) {
      dU1 = vdRoots[0] ;
      dU2 = vdRoots[1] ;
      if ( dU1 > dU2)
         swap( dU1, dU2) ;
     // Trovate intersezioni
      return true ;
   }
   
  // Una soluzione : la retta interseca la superficie laterale e un piano
   else if ( nRoot == 1) {
     // Se piano superiore
      if ( nBasInt == 2) {
         dU1 = vdRoots[0] ;
      }
     // altrimenti piano inferiore
      else if ( nBasInt == 1) {
         dU2 = vdRoots[0] ;
      }
     // altrimenti niente
      else
         return false ;
      if ( dU1 > dU2)
         swap( dU1, dU2) ;
     // Trovate intersezioni
      return true ;
   }

  // Nessuna soluzione : nessuna intersezione 
   else
      return false ;
}