//----------------------------------------------------------------------------
//  EgalTech 2020-2020
//----------------------------------------------------------------------------
// File : CDeConTria.cpp   Data : 27.10.20     Versione : 2.2k1
// Contenuto : Implementazione della verifica di collisione tra 
//             Cono e Triangle3d.
//
//
// Modifiche : 27.10.20 LM Creazione modulo.
//
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "IntersLineSurfStd.h"
#include "CDeUtility.h"
#include "EgtDev/Include/EGkCDeConeTria.h"
#include "EgtDev/Include/EGkCDeCylTria.h"
#include "EgtDev/Include/EGkPlane3d.h"
#include "EgtDev/Include/EGkIntersLineTria.h"
#include "EgtDev/Include/EGkIntersPlanePlane.h"
#include "EgtDev/Include/EgtNumUtils.h"

using namespace std ;

//----------------------------------------------------------------------------
// Il sistema di riferimento deve avere l'asse di simmetria del cono come asse Z e origine nel centro della base.
// La distanza di sicurezza ha effetto solo se maggiore di epsilon, altrimenti è ignorata ed è ininfluente.
bool
CDeConeTria( const Frame3d& frCone, double dBaseRad, double dTopRad, double dHeight, double dSafeDist, const Triangle3d& trTria)
{
   // Se è un cilindro chiamo la routine apposita.
   if ( abs( dBaseRad - dTopRad) < EPS_SMALL)
      return CDeCylTria( frCone, dHeight, max( dBaseRad, dTopRad), dSafeDist, trTria) ;

   // Se il tronco di cono o il trinagolo non è ben definito non procedo
   if ( dBaseRad < EPS_SMALL  ||  dTopRad < EPS_SMALL  ||  dHeight < EPS_SMALL  ||  ! trTria.IsValid())
      return false ;

   // Se il raggio di base è maggiore del raggio top cambio il sistema di riferimento.
   double dMinRad = dBaseRad ;
   double dMaxRad = dTopRad ;
   Frame3d frMyFrame = frCone ;
   if ( dBaseRad > dTopRad) {
      swap( dMinRad, dMaxRad) ;
      frMyFrame.Translate( dHeight * frMyFrame.VersZ()) ;
      frMyFrame.Rotate( frMyFrame.Orig(), frMyFrame.VersX(), 180.) ;
   }

   // Se la distanza di sicurezza è maggiore di epsilon aumento le dimensioni del tronco di cono.
   if ( dSafeDist > EPS_SMALL) {
      dBaseRad += dSafeDist ;
      dTopRad += dSafeDist ;
      dHeight += ( 2 * dSafeDist) ;
      frMyFrame.Translate( - dSafeDist * frMyFrame.VersZ()) ;
   }

   // Porto il triangolo nel sistema di riferimento del tronco di cono
   Triangle3d trMyTria = trTria ;
   trMyTria.ToLoc( frMyFrame) ;

   // Se almeno un vertice collide ho finito
   for ( int nV = 0 ; nV < 3 ; ++ nV) {
      Point3d ptVert = trMyTria.GetP( nV) ;
      double dLenZ = ptVert.z ;
      double dLenXY = sqrt( ptVert.x * ptVert.x + ptVert.y * ptVert.y) ;
      double dRadAtHeight = Clamp( dMinRad + ( dMaxRad - dMinRad) * dLenZ / dHeight, dMinRad, dMaxRad) ;
      if ( dLenZ > - EPS_SMALL  &&  dLenZ < dHeight + EPS_SMALL  &&  dLenXY < dRadAtHeight)
         return true ;
   }

   // Se almeno un segmento collide ho finito
   for ( int nS = 0 ; nS < 3 ; ++ nS) {
      Point3d ptSegSt = trMyTria.GetP( nS) ;
      Point3d ptSegEn = trMyTria.GetP( ( nS + 1) % 3) ;
      Vector3d vtDir = ptSegEn - ptSegSt ;
      double dSegLen = vtDir.Len() ;
      vtDir /= dSegLen ;
      double dU1, dU2 ;
      // Collisione con la superficie laterale
      int nIndex = SegmentTruncatedCone( ptSegSt, vtDir, dSegLen, ORIG, Z_AX, dMinRad, dMaxRad, dHeight, dU1, dU2) ;
      if ( nIndex != CC_ERROR_INT  &&  nIndex != CC_NO_INTERS)
         return true ;
      // Collisione con le basi
      nIndex = SegmentDisc( ptSegSt, vtDir, dSegLen, ORIG, -Z_AX, dMinRad, dU1, dU2) ;
      if ( nIndex != D_ERROR_INT  &&  nIndex != D_NO_INTERS)
         return true ;
      nIndex = SegmentDisc( ptSegSt, vtDir, dSegLen, Point3d( 0., 0., dHeight), Z_AX, dMaxRad, dU1, dU2) ;
      if ( nIndex != D_ERROR_INT  &&  nIndex != D_NO_INTERS)
         return true ;
   }

   // Contatti con l'interno del triangolo
   Point3d ptP = trMyTria.GetP( 0) ;
   Vector3d vtN = trMyTria.GetN() ;
   // Se il segmento congiungente i centri delle basi interseca il triangolo ho finito
   if ( abs( vtN.z) > EPS_ZERO) {
      Point3d ptInt( 0., 0., ( ( ptP - ORIG) * vtN) / vtN.z) ;
      if ( ptInt.z > - EPS_SMALL  &&  ptInt.z < dHeight + EPS_SMALL  &&  IsPointInsideTriangle( ptInt, trMyTria, TriangleType::CLOSED))
         return true ;
   }
   // Intersezione basi / interno triangolo
   Point3d ptLine ;
   Vector3d vtLine ;
   Plane3d plPlaneTria ;
   plPlaneTria.Set( ptP, vtN) ;
   // Base minore
   Plane3d plMinBase ;
   plMinBase.Set( ORIG, - Z_AX) ;
   int nMinBaseTriaIndex = IntersPlanePlane( plPlaneTria, plMinBase, ptLine, vtLine) ;
   if ( nMinBaseTriaIndex == IPPT_OVERLAPS) {
      if ( CoplanarDiscTriangleInterference( plMinBase.GetPoint(), dMinRad, trMyTria, TriangleType::CLOSED))
         return true ;
   }
   else if ( nMinBaseTriaIndex == IPPT_YES) {
      double dU1, dU2 ;
      if ( LineDisc( ptLine, vtLine, ORIG, -Z_AX, dMinRad, dU1, dU2) == D_INFINITE_INT_LINE_ON_PLANE) {
         int nLineTriaIndex = IntersLineTria( ptLine + dU1 * vtLine, ptLine + dU2 * vtLine, trMyTria, Point3d(), Point3d()) ;
         if ( nLineTriaIndex != ILTT_NO  &&  nLineTriaIndex != ILTT_IN)
            return true ;
      }
   }
   // Base maggiore
   Plane3d plMaxBase ;
   plMaxBase.Set( Point3d( 0., 0., dHeight), Z_AX) ;
   int nMaxBaseTriaIndex = IntersPlanePlane( plPlaneTria, plMaxBase, ptLine, vtLine) ;
   if ( nMaxBaseTriaIndex == IPPT_OVERLAPS) {
      if ( CoplanarDiscTriangleInterference( plMaxBase.GetPoint(), dMaxRad, trMyTria, TriangleType::CLOSED))
         return true ;
   }
   else if ( nMinBaseTriaIndex == IPPT_YES) {
      double dU1, dU2 ;
      if ( LineDisc( ptLine, vtLine, ORIG, -Z_AX, dMaxRad, dU1, dU2) == D_INFINITE_INT_LINE_ON_PLANE) {
         int nLineTriaIndex = IntersLineTria( ptLine + dU1 * vtLine, ptLine + dU2 * vtLine, trMyTria, Point3d(), Point3d()) ;
         if ( nLineTriaIndex != ILTT_NO  &&  nLineTriaIndex != ILTT_IN)
            return true ;
      }
   }
   // Tronco di cono tangente al triangolo
   Vector3d vtTriaNormXY( vtN.x, vtN.y, 0.) ;
   vtTriaNormXY.Normalize() ;
   Point3d ptContactSt( dMinRad * vtTriaNormXY.x, dMinRad * vtTriaNormXY.y, 0.) ;
   Point3d ptContactEn( dMaxRad * vtTriaNormXY.x, dMaxRad * vtTriaNormXY.y, 0.) ;
   int nContactIndex = IntersLineTria( ptContactSt, ptContactEn, trMyTria, Point3d(), Point3d()) ;
   return ! ( nContactIndex == ILTT_NO  ||  nContactIndex == ILTT_IN) ;
}